一种抑制高度和航向角误差累积的行人自主导航方法

本发明属于行人导航，涉及一种足绑式行人导航方法，具体提供一种抑制高度和航向角误差累积的行人自主导航方法。

背景技术：

1、随着城市化进程的加快，人们的活动范围随着生活及工作需求的扩大而逐渐增大，仅依赖大脑记忆或传统纸质地图的导航方式已经无法满足现代社会日益复杂的导航需求，导航定位服务已经逐渐成为人们日常生活中不可或缺的工具。尤其在管道作业、消防救援、电气线路等专业应用领域，工作人员的位置变化需要导航系统进行实时反应，并进行相应的指挥和调度；由此可见，用以在室内外环境中提供连续、精准导航服务的行人导航系统至关重要。

2、目前，基于惯性传感器的自主式行人导航技术得到广泛研究，主流方法包括行人航迹推算与基于ins解算的行人导航算法；前者原理简单，后者鲁棒性好，通常结合零速修正技术（zupt）可以有效抑制惯导漂移；但是，使用zupt需要有精确的零速状态检测技术，能否正确检测出足部的静止状态至关重要；其次，在zupt算法中位置和航向是不可观测的，而在长时间的导航定位过程中，航向误差是造成定位误差的主要原因；因此，如何消除zupt算法中的航向误差成为zupt辅助的行人导航方法的研究重点。

技术实现思路

1、本发明的目的在于提供一种抑制高度和航向角误差累积的行人自主导航方法，用以抑制行人导航系统的高度及航向漂移、缓解导航系统的误差累积、提升定位精度。本发明提出一种自适应零速检测方法，能够准确检测出行人在多种运动模式下（行走、跑步、上下楼）左右脚的零速状态，进而实现对行人静止状态的有效检测；当行人状态处于零速区间时，分别建立左脚与右脚的单足系统，并依次利用椭球约束与zupt对系统误差状态量进行两次修正，有效抑制惯性误差的漂移；再基于uwb测量的双足间距建立双足卡尔曼滤波系统，并利用双足间距的uwb测距约束对系统误差状态量进行再次修正，实现对航向角的修正，最终提升定位精度。

2、为实现上述目的，本发明采用的技术方案为：

3、一种抑制高度和航向角误差累积的行人自主导航方法，包括以下步骤：

4、步骤1、将微惯性测量单元分别安装在行人的左脚与右脚上，测量得到行人惯性数据；

5、步骤2、根据行人惯性数据，采用捷联惯性导航算法计算行人的姿态、速度、位置信息；

6、步骤3、根据行人惯性数据，采用自适应零速检测算法对行人状态进行判定；

7、步骤4、当行人状态为静止状态时，分别建立左脚与右脚的单足卡尔曼滤波系统，并依次利用椭球约束与zupt对系统误差状态量进行两次修正；

8、步骤5、建立双足卡尔曼滤波系统，并利用双足间距的uwb测距约束对系统误差状态量进行再次修正；

9、步骤6、采用修正后误差状态量对行人的姿态、速度、位置信息进行误差修正。

10、进一步的，步骤3具体为：

11、步骤3.1、根据微惯性测量单元测量得到的z轴加速度数据计算单足的步态变化表征值：

12、，

13、其中，k表示时刻，g(k)表示单足在k时刻的步态变化表征值，az(k)表示单足在k时刻的z轴加速度；

14、设定零速检测阈值自适应时间窗为kc，搜索(k-kc)~k内步态变化表征值的最大值gmax与最小值gmin，并将最大值gmax与最小值gmin按固定数值步长进行区间划分，依次表示为(xi,xi+1)，x1=gmin，xi+1=xi+gd，i=1,2,...n，n为区间数，gd为数值步长；统计每个区间内步态变化表征值的频数，以xi为横坐标、频数为纵坐标绘制直方图，并通过曲线拟合得到函数fk(xi)；

15、步骤3.2、根据函数fk(xi)自适应更新零速检测阈值εk：

16、，

17、，

18、，

19、其中，、与分别表示函数fk(xi)的一阶导数、二阶导数与三阶导数，表示符号前后两个条件同时满足；

20、步骤3.3、根据零速检测阈值εk对单足状态进行判定，若单足的z轴加速度小于等于零速检测阈值εk，则判定单足处于运动状态，否则，单足处于零速区间；

21、步骤3.4、针对行人的左脚与右脚分别进行自适应零速检测，当左脚与右脚均处于零速区间时，判定行人处于静止状态；否则，判定行人处于运动状态。

22、进一步的，步骤4具体为：

23、步骤4.1、建立单足卡尔曼滤波系统，其状态方程为：

24、，

25、，

26、其中，k表示当前时刻，k-1表示上一时刻；表示单足系统的转移矩阵，表示单足系统的系统驱动矩阵，表示单足系统的系统噪声向量；表示右脚在k时刻的误差状态量，表示左脚在k时刻的误差状态量；

27、步骤4.2、建立单足卡尔曼滤波系统的量测方程：

28、，

29、，

30、其中，、分别表示右脚、左脚在k时刻的量测值，表示量测噪声向量，、分别表示右脚、左脚在k时刻的观测矩阵；

31、步骤4.3、建立椭球约束的量测方程与观测矩阵，并分别对左脚、右脚的误差状态量进行第一次修正；

32、椭球约束的量测方程为：

33、，

34、，

35、其中，、分别表示左脚、右脚的高度，、分别表示左脚、右脚在椭圆约束下的量测值；

36、椭球约束的观测矩阵为：

37、，

38、，

39、其中，表示、表示、表示、表示、表示、表示；为地球偏心率，为横向曲率半径，表示行人所在地点的海拔高度，表示维度为1×6的零矩阵；表示地球系下右脚的坐标，表示地球系下左脚的坐标；、分别表示右脚、左脚在椭球约束下的观测矩阵；

40、将椭圆约束的量测值与量测矩阵带入单足卡尔曼滤波系统的状态方程中，计算得到第一次修正后的左脚与右脚的误差状态量；

41、步骤4.4、建立zupt的量测方程与观测矩阵，并分别对左脚、右脚的误差状态量进行第二次修改；

42、zupt的量测方程为：

43、，

44、，

45、其中，、分别表示左脚、右脚的惯性解算速度值，、分别表示左脚、右脚在zupt下的量测值；

46、zupt的观测矩阵为：

47、，

48、，

49、其中，表示3阶单位矩阵，、分别表示左脚、右脚在zupt下的量测矩阵；

50、在第一次修正的基础上，将zupt的量测值与量测矩阵带入单足卡尔曼滤波系统的状态方程中，计算得到第二次修正后的左脚与右脚的误差状态量。

51、进一步的，步骤5具体为：

52、步骤5.1、建立双足卡尔曼滤波系统，其状态方程与量测方程：

53、，

54、，

55、其中，k表示当前时刻，k-1表示上一时刻；表示双足系统的转移矩阵、表示双足系统的系统驱动矩阵、表示双足系统的系统噪声向量，表示双足系统的量测噪声向量；表示双足系统在k时刻的量测值，表示双足系统在k时刻的观测矩阵；表示双足系统在k时刻的误差状态量，，表示右脚在k时刻的误差状态量，表示左脚在k时刻的误差状态量；

56、步骤5.2、建立uwb测距约束的量测方程与观测矩阵，并对双足系统的误差状态量进行修正；

57、uwb测距约束的量测方程为：

58、，

59、其中，表示双足系统在uwb测距约束下的量测值，为uwb的测量值，为双足间距；

60、uwb测距约束的观测矩阵为：

61、，

62、其中，表示，表示，表示，表示，表示，表示，表示双足系统在uwb测距约束下的观测矩阵，、分别表示左脚、右脚对应的载体系到地球系的旋转矩阵，、分别表示左脚、右脚上的uwb与微惯性测量单元之间的杆臂，表示杆臂误差，、分别表示维度为1×3、1×6的零矩阵，表示取模运算，表示矩阵的反对称矩阵；

63、将uwb测距约束的量测值与量测矩阵带入双足卡尔曼滤波系统的状态方程中，计算得到修正后双足系统的误差状态量。

64、更进一步的，步骤5.2中，双足间距具体为：

65、，

66、，

67、其中，、分别表示左脚、右脚在地球系下的位置坐标，表示距离量测噪声。

68、基于上述技术方案，本发明的有益效果在于：

69、本发明提供一种抑制高度和航向角误差累积的行人自主导航方法，首先，设计自适应零速检测方法，准确判断行人在行进过程中左右脚的零速状态，当检测到零速状态后，建立左右脚单足行人导航系统，分别利用椭球约束与zupt进行误差修正；然后，利用uwb测距约束将左右脚单足系统融合构成双足系统，并进行再次误差修正；最后，根据修正后姿态、速度、位置信息实现行人自主导航。

70、综上，本发明提出自适应零速检测方法，能够准确检测出行人在多种运动模式下（行走、跑步、上下楼）左右脚的零速状态，进而实现对行人静止状态的有效检测；在此基础上，提出左右脚单足行人导航系统融合构成的双足系统，并利用椭球约束、zupt以及uwb测距约束有效抑制高度与航向的误差累积，有效提升定位精度，提高导航系统的工作效率。