一种基于距离和角度的行人协同导航方法

本发明涉及导航定位，尤其涉及一种基于距离和角度的行人协同导航方法。

背景技术：

1、基于导航解算的行人导航算法是近年来行人导航算法的热点；这种方法中，imu需要与人体固联，一般安装在足部，其原理是基于惯导的积分原理，对加速度计、陀螺仪数据进行解算，得到行人的运动信息，如位置、姿态和速度；然而，由于惯性元件精度较差，随着时间的增长期误差在不断累积，不能得到一个可靠的导航结果。foxlin在2006年提出了零速修正(zero velocity update,zupt)算法。在zupt中，考虑行人行走足部运动的周期性，将一个周期内脚部的运动状态划分为步幅阶段和静止阶段，静止阶段就是脚掌支撑在地面上，此时脚部的速度可以看作是零速，然后将零速作为扩展卡尔曼的观测量融合进系统。这种方法在每一个周期结束的时候更新行人的位姿，抑制了惯导漂移。zupt算法的出现使得ins解算的行人导航系统得到广泛应用。基于惯导解算的行人导航系统一般都是zupt辅助的基础上进行工作的。但是由于陀螺仪存在漂移误差，zupt算法中航向角是不可观的，会导致航向角误差得不到修正，从而逐渐积累，发生航向角的漂移。

2、需要说明的是，在上述背景技术部分公开的信息只用于加强对本公开的背景的理解，因此可以包括不构成对本领域普通技术人员已知的现有技术的信息。

技术实现思路

1、本发明的目的在于克服现有技术的缺点，提供了一种基于距离和角度的行人协同导航方法，解决了现有基于zupt的行人导航航向角误差积累发散问题。

2、本发明的目的通过以下技术方案来实现：一种基于距离和角度的行人协同导航方法，所述协同导航方法包括：

3、s1、通过mimu输出每个行人各自的三轴加速度计和三轴陀螺仪数据，并进行捷联惯导解算，再由零速检测得到的状态通过扩展卡尔曼滤波对惯导解算的数据进行零速修正；

4、s2、通过uwb测距测角得到行人之间的相对距离和角度作为观测量，再以n个行人的位置信息和航向角信息为状态量，进行卡尔曼滤波，得到每个行人的位置信息和航向角信息的修正数据；

5、s3、根据每个行人修正后的位置信息和航向角信息进行协同导航。

6、所述通过mimu输出每个行人各自的三轴加速度计和三轴陀螺仪数据，并进行捷联惯导解算，再由零速检测得到的状态通过扩展卡尔曼滤波对惯导解算的数据进行零速修正具体包括以下内容：

7、s101、通过mimu输出三轴加速度计和三轴陀螺仪的数据，分别为fb,ωb；

8、s102、利用加速度计协方差检测法检测出行人的零速状态；

9、s103、对mimu测量数据进行捷联惯导解算，当检测出零速度状态时，采用零速伪观测量通过扩展卡尔曼滤波对惯导解算进行零速修正。

10、所述利用加速度计协方差检测法检测出行人的零速状态包括：假设某段时间内的采样值共有n个，即窗口的大小为n，此时mimu的零速状态为其中，fk为k时刻加速度计的输出，t(fk)为零速检测的检验统计量，l为阈值。

11、所述s103中扩展卡尔曼滤波的状态模型为：

12、

13、其中，δφ,δλ,δh分别表示三维位置(纬度、经度和高度)误差，δve,δvn,δvu分别表示东向、北向和天向的速度误差，δθ,δγ,分别代表惯性导航系统的俯仰角、横滚角、偏航角误差，δωx,δωγ,δωz分别表示陀螺仪的漂移和δfx,δfy,δfz分别表示加速度计的零偏，一共15维误差，f表示系统的动态转移矩阵，g表示噪声分布矢量，w(k)表示高斯白噪声；

14、将15维误差状态方程表示为：

15、

16、其中，frv、frε和fvε子阵代表了位置误差、速度误差和姿态误差之间的扰动关系，为b系到n系的转换矩阵,βf和βω为加速度计和陀螺仪零偏相关的系数；

17、扩展卡尔曼滤波的观测模型为：

18、

19、其中，zk为惯导解算的三维速度，hk为[03×3diag[1 1 1]03×6]的3×15维矩阵，vk代表零均值的测量噪声序列，rk表示噪声协方差矩阵。

20、所述卡尔曼滤波具体包括以下步骤：

21、a1、预测状态方程：通过系统的状态转移矩阵φk,k-1和当前状态估计值预测下一时刻的系统状态

22、a2、预测误差协方差：通过系统的状态转移矩阵φk,k-1、当前状态估计误差协方差矩阵pk-1(+),以及噪声驱动矩阵gk-1和系统噪声协方差矩阵qk-1，预测下一时刻的状态估计误差协方差矩阵

23、a3、计算卡尔曼增益：通过状态估计误差协方差矩阵pk和量测噪声协方差矩阵rk的值来确定卡尔曼滤波增益

24、a4、更新状态方程：通过预测状态和测量更新后的状态估计值,得到当前时刻的最优状态估计值

25、a5、更新误差协方差：通过预测状态估计误差协方差矩阵和测量更新后的状态估计误差协方差矩阵,计算当前时刻的最优状态估计误差协方差矩阵pk(+)＝(i-kkhk)pk(-)。

26、所述通过uwb测距测角得到行人之间的相对距离和角度作为观测量，再以n个行人的位置信息和航向角信息为状态量，进行卡尔曼滤波，得到每个行人的位置信息和航向角信息的修正数据具体包括以下内容：

27、s201、通过公式来基于uwb测距得到行人之间的相对距离，其中，表示t时刻行人i和行人j之间的距离测量，表示距离测量噪声，表示第i个行人在第s步的位置矢量，表示第j个行人在第s步的位置矢量，通过公式来基于uwb测距得到行人之间的相对角度，其中，表示t时刻行人i和行人j之间的角度测量，和分别表示第i个行人在第s步的x轴方向上的位置分量和y轴方向上的位置分量；

28、s202、以多个行人的位置信息和航向角信息为状态量，以行人每步的位移增量和航向增量为系统输入，组建运动模型，并根据扩展卡尔曼滤波的状态模型得到单个行人的状态模型为：

29、

30、其中，xi(k)和yi(k)为k时刻行人i的位置信息在水平面上的分量，ψi(k)为航向角状态量，δψi(k)为航向角增量，δxi(k)和δyi(k)为k时刻行人i的位置变化量在水平面上的分量，进而得到多个行人的状态模型为

31、将得到的状态量从导航坐标系转换为载体坐标系，通过公式和得到行人之间的量测模型为：

32、

33、其中，r为导航坐标系到载体坐标系的旋转矩阵，lij为行人i和行人j之间的观测矩阵h；

34、s203、进行卡尔曼滤波：在行人每一步结束后，进行卡尔曼滤波，得到每个行人的位置信息和航向角信息的修正数据。

35、本发明具有以下优点：一种基于距离和角度的行人协同导航方法，将距离和角度作为观测量来来修正zupt辅助后的行人导航，解决了目前行人导航航向角漂移和集中式滤波计算量大的问题。

技术特征：

1.一种基于距离和角度的行人协同导航方法，其特征在于：所述协同导航方法包括：

2.根据权利要求1所述的一种基于距离和角度的行人协同导航方法，其特征在于：所述通过mimu输出每个行人各自的三轴加速度计和三轴陀螺仪数据，并进行捷联惯导解算，再由零速检测得到的状态通过扩展卡尔曼滤波对惯导解算的数据进行零速修正具体包括以下内容：

3.根据权利要求2所述的一种基于距离和角度的行人协同导航方法，其特征在于：所述利用加速度计协方差检测法检测出行人的零速状态包括：假设某段时间内的采样值共有n个，即窗口的大小为n，此时mimu的零速状态为其中，fk为k时刻加速度计的输出，t(fk)为零速检测的检验统计量，l为阈值。

4.根据权利要求2所述的一种基于距离和角度的行人协同导航方法，其特征在于：所述s103中扩展卡尔曼滤波的状态模型为：

5.根据权利要求4所述的一种基于距离和角度的行人协同导航方法，其特征在于：所述卡尔曼滤波具体包括以下步骤：

6.根据权利要求4所述的一种基于距离和角度的行人协同导航方法，其特征在于：所述通过uwb测距测角得到行人之间的相对距离和角度作为观测量，再以n个行人的位置信息和航向角信息为状态量，进行卡尔曼滤波，得到每个行人的位置信息和航向角信息的修正数据具体包括以下内容：

技术总结
本发明涉及一种基于距离和角度的行人协同导航方法，所述协同导航方法包括：S1、通过MIMU输出每个行人各自的三轴加速度计和三轴陀螺仪数据，并进行捷联惯导解算，再由零速检测得到的状态通过扩展卡尔曼滤波对惯导解算的数据进行零速修正；S2、通过UWB测距测角得到行人之间的相对距离和角度作为观测量，再以N个行人的位置信息和航向角信息为状态量，进行卡尔曼滤波，得到每个行人的位置信息和航向角信息的修正数据；S3、根据每个行人修正后的位置信息和航向角信息进行协同导航。本发明将距离和角度作为观测量来来修正ZUPT辅助后的行人导航，解决了目前行人导航航向角漂移和集中式滤波计算量大的问题。

技术研发人员：邓斯琪,杜爽,赵鑫
受保护的技术使用者：电子科技大学
技术研发日：
技术公布日：2024/1/12