一种基于高度自观测算法的室内个人惯性导航高程精度提升方法与流程

本发明属于行人导航
技术领域：
，涉及惯性导航，具体是指一种基于高度自观测算法的室内个人惯性导航高程精度提升方法。
背景技术：
：近年来，人们对基于位置的服务的需求日益增长。个人组合导航系统因能提供较为准确的定位，实时监测行人的位置变化，为行人提供可视导航信息，从而可以有效地指导人们在陌生的环境下的活动，已成为目前导航领域中研究的热点之一。惯性导航虽然有较强的自主性，但微惯性器件的性能相对传统惯性器件仍有一定的差距，惯性器件还受到环境温度等不确定因素的影响，使微惯性系统无法完成较长时间的精确导航。目前基于ins的个人导航精度已得到了很大的提升，但是由于ins原理本身问题，在高度方向上解算效果较差，一般需要引入硬件级器件，如气压高度计，对惯导系统的高度通道进行阻尼。但是低成本的气压高度计存在精度低，对温度非常敏感的问题，效果不是很理想。参考文献[1]：基于手持设备的室内行人pdr/ins/wifi集成导航[j].微型机械,2015,6(6):793-812.提出了ins、pdr、wifi的复合定位系统，实验结果表明，其提出的pdr/ins/wifi的平均准确度整合算法在室内达到4.5米，相比于单纯依靠ins进行导航定位精度得到了很大的提升，但仅限于平面，尚未在高度通道上进行进一步的论证。参考文献[2]：使用脚踏式惯性传感器和激光雷达的行人导航[j].传感器,2016,16(1):120.提出了一种使用距离传感器(lidar)和惯性测量单元(imu)的方法，通过实验表明精度得到显著提高，但其测算装置较为复杂，技术实用性大打折扣。技术实现要素：为了解决上述背景提出的技术问题，本发明提出了一种基于高度自观测算法的室内个人惯性导航高程精度提升方法，无需增加额外的硬件且不依赖于其他外部信息就能提高导航的精度。具体步骤如下：步骤一、在行人脚部安装惯性测量单元实时测量脚部的角速度和加速度，导航系统初始化；惯性测量单元由三轴正交的mems加速度计和陀螺仪构成；步骤二、将脚部的加速度和角速度信息分别输入到模糊步态识别模块，惯性导航模块和零速检测模块；模糊步态识别模块根据输入的加速度和角速度识别出行人的行走状态，包括站立、行走、转向以及上下楼梯等共10种状态；具体过程如下：首先，从脚部运动信息中提取各轴加速度的特征值；定义mems加速度计的x正半轴指向人体前进方向，y正半轴指向人体左方，z正半轴指向天向；提取的特征值如下：加速度计z轴输出的均值μaz与标准差σaz、加速度计x轴输出的均值μax与标准差σax、加速度计y轴输出均值μay与标准差σay、陀螺仪z轴输出均值绝对值μwz，公式分别为：其中n表示从ts到tk时间段内数据的个数；ts表示行走一步过程中脚部抬起的时刻，tk表示行走一步过程中脚部落地的时刻，az(ti)代表ti时刻加速度计z轴除去重力加速度后的输出；ax(ti)代表ti时刻加速度计x轴除去重力加速度后的输出；ay(ti)代表ti时刻加速度计y轴除去重力加速度后的输出，wz(ti)代表ti时刻陀螺仪z轴滤除陀螺零偏后的角速度输出。然后，根据隶属度函数对提取出的各特征值进行模糊化，并利用制定的模糊规则对模糊化后的特征值进行模糊运算，得到每种步态的相似度参数；每一条规则对应一个步态相似度参数foi，与步态类别的对应关系如下所示：正常走跨步踏步小步后退左移右移上楼梯下楼梯转向fo1fo2fo3fo4fo5fo6fo7fo8fo9fo10将上楼梯和下楼梯这两种步态模式定义为立体运动，即高程发生变化；将正常走等剩余八种步态模式定义为平面运动，即高程没有发生变化。最后，取相似度参数最大的步态模式，即为行人运动状态的识别结果。惯性导航模块对输入的加速度和角速度进行计算，得到行人的水平位置和高程；零速检测模块根据输入的加速度和角速度，判断行人脚部是否处于静止状态，静止时置标志位zupt＝1，否则zupt＝0；零速判别特征包括：(1)对于角速度，任意时刻角速度的模为：wx代表陀螺仪x轴滤除陀螺零偏后的角速度输出；wy代表陀螺仪y轴滤除陀螺零偏后的角速度输出；wz代表陀螺仪z轴滤除陀螺零偏后的角速度输出设置阈值thw＝20°/s，角速度数据判定零速有效标志：(2)对于加速度，任意时刻加速度的模为：az代表加速度计z轴除去重力加速度后的输出；ax代表加速度计x轴除去重力加速度后的输出；ay代表加速度计y轴除去重力加速度后的输出；设置阈值tha＝0.1g，加速度数据判定零速有效标志：当flag_g和flag_a均为1时，标志位zupt＝1，否则zupt＝0。(3)零速判别的观测方程及量测矩阵为：zv＝hvx+vvhv＝[03×3i303×9]x＝[φnδvnδpnεb▽b]t其中vv表示静止测量的噪声矩阵，并被假设成高斯白噪声；i3表示三阶单位矩阵。表示俯仰/横滚/航向姿态误差，表示东北天三向速度误差，表示东北天三向位置误差，ε表示陀螺的零偏；▽表示加速度计的零偏。步骤三、利用高度约束自观测算法，根据模糊步态识别模块得到的行走状态生成伪高度信息；高度约束自观测算法的观测方程及量测矩阵为：zh＝hhx+vh＝hght1-hght0；hh＝[01×600101×6]；x＝[φnδvnδpnεb▽b]t其中vh表示高度测量的噪声矩阵，并被假设成高斯白噪声。hght1表示tk时刻得到的导航的高程，hght0表示ts时刻得到的导航的高程。步骤四、将伪高度信息，零速检测模块的信息与惯性导航模块得到的水平位置和高程的计算结果输入到卡尔曼滤波器，对惯性导航的结果进行修正，修正后的结果作为最终的导航结果。卡尔曼滤波器工作过程为：tk时刻时，若检测为平面运动，则卡尔曼滤波器做伪高度量测更新和零速修正；若检测为立体运动，则卡尔曼滤波器只做零速修正，更新完毕后生成修正的水平位置和高程并将其作为最终的导航结果。本发明的有益效果在于：(1)一种基于高度自观测算法的室内个人惯性导航高程精度提升方法，通用性强，可适用于大多数基于步态识别的个人组合导航系统的误差修正过程。(2)一种基于高度自观测算法的室内个人惯性导航高程精度提升方法，方法简便，无需对程序进行过多修正，适应性强。(3)一种基于高度自观测算法的室内个人惯性导航高程精度提升方法，效果显著，该方法能够有效的降低高度上的量测误差，从而提高了导航精度。附图说明图1是本发明一种基于高度自观测算法的室内个人惯性导航高程精度提升方法的流程图；图2是本发明正交的mems加速度计xyz三轴选取方向的示意图；图3是本发明制定的隶属度函数示意图；图4是本发明卡尔曼滤波器的工作流程图；图5是本发明的实验结果图。具体实施方式下面将结合附图对本发明作进一步的详细说明。本发明公开了一种在室内个人惯性导航系统中通过高度自观测算法减小高度方向导航误差的方法，如图1所示，利用安装在脚部的惯性测量单元检测脚的角速度和加速度，通过模糊推理的方法对行人的行走步态进行识别，然后根据步态的类型，利用高度约束自观测算法，生成伪高度判定信息。该判定信息用于修正惯性导航高度方向的误差。实验结果表明，本方法与通常的零速修正相结合的算法可以显著降低惯性导航在高度方向上的误差。如图1所示，具体步骤如下：步骤一、在行人脚部安装惯性测量单元实时测量脚部的角速度和加速度，导航系统初始化；惯性测量单元由三轴正交的mems加速度计和陀螺仪构成；步骤二、将脚部的加速度和角速度信息分别输入到模糊步态识别模块，惯性导航模块和零速检测模块；模糊步态识别模块根据输入的加速度和角速度识别出行人的行走状态，包括站立、行走、转向、上下楼梯等共10种状态；采用的模糊步态识别技术处理过程如下：首先，从脚部运动信息中提取各轴加速度的特征值；定义正交三轴mems加速度计的x正半轴指向人体前进方向，y正半轴指向人体左方，z正半轴指向天向；如图2所示；提取的特征值如下：加速度计z轴输出的均值μaz与标准差σaz、加速度计x轴输出的均值μax与标准差σax、加速度计y轴输出均值μay与标准差σay、陀螺仪z轴输出均值绝对值μwz，公式分别为：其中n表示从ts到tk时间段内数据的个数；ts表示行走一步过程中脚部抬起的时刻，tk表示行走一步过程中脚部落地的时刻，az(ti)代表ti时刻加速度计z轴除去重力加速度后的输出；ax(ti)代表ti时刻加速度计x轴除去重力加速度后的输出；ay(ti)代表ti时刻加速度计y轴除去重力加速度后的输出，wz(ti)代表ti时刻陀螺仪z轴滤除陀螺零偏后的角速度输出。然后，根据隶属度函数对提取出的各特征值进行模糊化，并利用制定的模糊规则对模糊化后的特征值进行模糊运算，得到每种步态的相似度参数；七个特征值隶属度函数如图3所示；模糊规则如表1所示；表1每一条规则对应一个步态相似度参数foi，i＝1,...,10；与步态类别的对应关系如下所示：正常走跨步踏步小步后退左移右移上楼梯下楼梯转向fo1fo2fo3fo4fo5fo6fo7fo8fo9fo10将上楼梯和下楼梯这两种步态模式定义为立体运动，即高程发生变化；将正常走等剩余八种步态模式定义为平面运动，即高程没有发生变化。最后，取相似度参数最大的步态模式，即为行人运动状态的识别结果。惯性导航模块对输入的加速度和角速度进行计算，得到行人的水平位置和高程；零速检测模块根据输入的加速度和角速度，判断行人脚部是否处于静止状态，静止时置标志位zupt＝1，否则zupt＝0；零速判别特征包括：(1)对于角速度，任意时刻角速度的模为：wx代表陀螺仪x轴滤除陀螺零偏后的角速度输出；wy代表陀螺仪y轴滤除陀螺零偏后的角速度输出；wz代表陀螺仪z轴滤除陀螺零偏后的角速度输出设置阈值thw＝20°/s，角速度数据判定零速有效标志：(2)对于加速度，任意时刻加速度的模为：az代表加速度计z轴除去重力加速度后的输出；ax代表加速度计x轴除去重力加速度后的输出；ay代表加速度计y轴除去重力加速度后的输出；设置阈值tha＝0.1g，加速度数据判定零速有效标志：当flag_g和flag_a均为1时，标志位zupt＝1，否则zupt＝0。(3)零速判别的观测方程及量测矩阵为：zv＝hvx+vvhv＝[03×3i303×9]x＝[φnδvnδpnεb▽b]t其中vv表示静止测量的噪声矩阵，并被假设成高斯白噪声；i3表示三阶单位矩阵。表示俯仰/横滚/航向姿态误差，表示东北天三向速度误差，表示东北天三向位置误差，ε表示陀螺的零偏；▽表示加速度计的零偏。步骤三、利用高度约束自观测算法，根据模糊步态识别模块所得到的行走状态生成伪高度信息；高度约束自观测算法的观测方程及量测矩阵为：zh＝hhx+vh＝hght1-hght0；hh＝[01×600101×6]；x＝[φnδvnδpnεb▽b]t其中vh表示高度测量的噪声矩阵，并被假设成高斯白噪声。hght1表示tk时刻得到的导航的高程，hght0表示ts时刻得到的导航的高程。步骤四、将伪高度信息，零速检测模块的信息与惯性导航模块得到的水平位置和.高程的计算结果输入到卡尔曼滤波器，对惯性导航的结果进行修正，修正后的结果作为最终的导航结果。如图4所示，卡尔曼滤波器工作过程为：tk时刻时，若检测为平面运动，则卡尔曼滤波器做伪高度量测更新和零速修正；若检测为立体运动，则卡尔曼滤波器只做零速修正，更新完毕后生成修正的水平位置和高程并将其作为最终的导航结果。本发明对个人组合导航装置进行对比试验，一组采用只采用零速修正(zupt)，另一组采用零速修正加高度约束自观测算法(zupt&hsso)，试验结果如附图5所示，两条折线中处于上方的折线只是采用了零速修正(zupt)，处于下方的折线采用零速修正加高度约束自观测算法(zupt&hsso)，对图中的两种算法进行对比可以发现，由于zupt&hsso在高度方向上约束，上下楼梯实验轨迹重合较好，高度上的误差累积集中在上下楼梯动作中。在平面运动时，高度基本不变，特别在最后测量踏步的过程中，有效的抑制了高度方向上的发散，很好的重构了实验时的运动轨迹。最终zupt导航高度误差由1.57m减小为zupt&hsso的0.16m,很好的说明了该方法可减小高度方面的量测误差。当前第1页12