一种基于ZIHR的航向角修正算法的MEMS行人导航方法技术

本发明专利技术提供的是一种基于ZIHR的航向角修正算法的MEMS行人导航方法。一：静止时刻利用加速度计和磁力计对MEMS行人导航系统进行初始对准；二：求取MEMS行人导航系统惯导解算方程和误差方程；三：利用陀螺仪和加速度计的输出值进行零速状态检测；四：求解ZIHR修正算法中零速状态时相邻时刻航向角差值和陀螺漂移及航向误差角的关系；五：建立简化的MEMS行人导航UKF滤波模型，进行UKF滤波。本发明专利技术最大限度的使用静止时刻的信息，计算量并不复杂，而且能够很好的抑制航向误差角发散。在零速时刻利用UKF滤波器进行实时的反馈校正能很好地抑制低精度MEMS传感器长时间工作导航参数误差发散的问题，提高行人导航系统的定位精度。

MEMS pedestrian navigation method based on ZIHR's heading angle correction algorithm

The invention provides a MEMS pedestrian navigation method based on ZIHR's heading angle correction algorithm. Firstly, the accelerometer and magnetometer are used to align the MEMS pedestrian navigation system at rest time; secondly, the inertial navigation equation and error equation are obtained; thirdly, the zero-speed state is detected by the output value of gyroscope and accelerometer; fourthly, the adjacent time of zero-speed state in the ZIHR correction algorithm is solved. The relationship between heading angle difference and gyroscope drift and heading error angle. Fifth, a simplified model of UKF filtering based on MEMS for pedestrian navigation is established. The invention maximizes the use of information at rest time, the calculation amount is not complicated, and the divergence of heading error angle can be well suppressed. Real-time feedback correction using UKF filter at zero speed can effectively suppress the error divergence of navigation parameters of low-precision MEMS sensors in long-time operation and improve the positioning accuracy of pedestrian navigation system.

【技术实现步骤摘要】
一种基于ZIHR的航向角修正算法的MEMS行人导航方法
本专利技术涉及的是一种行人导航方法，具体地说是一种惯性导航技术的行人导航方法。
技术介绍
行人导航系统是导航定位
的一个重要的分支，能够为从事消防、抢险等高危工作人员提供可靠的安全保障。传统的行人导航的核心是GPS，但是当GPS位于城市高楼大厦以及植被茂密的丛林中就很难起到作用。而以MEMS惯性传感器为核心的行人定位系统则不需要考虑上述问题。由于MEMS惯性传感器具有体积小，成本低，重量轻等优点，因此对MEMS传感器行人导航的研究已经成为热点。非GPS人员跟踪系统通常使用陀螺仪来估计用户的航向变化。陀螺仪测量旋转速率，需要将它们的信号进行数值积分以产生所需的航向信息。由于陀螺仪的测量信号中存在一个小的，接近恒定的漂移值，尤其是MEMS陀螺仪的精度低，漂移相对较大，使得在信号积分过程中计算出的航向角和真实的航向角之间产生不可忽略的误差。行人导航运动轨迹会随着航向角误差的累积偏离真实的行走路线，如何将航向角的误差降低到合理的范围内成为行人导航研究的一大难题。现阶段常用的方法主要有：(1)利用零角速率修正算法来校正每一个静止时刻陀螺仪的漂移。(2)利用启发式漂移减少的技术来减少航向角的误差。(3)将磁航向易受干扰但是长时间定位精度高与MEMS惯导抗干扰能力强短时定位精度高，长时间误差发散进行优劣互补，对MEMS惯导航向进行校正。(4)利用楼向信息来对航向角进行修正，但是基于建筑物内大部分走廊和路径是直的假设。(5)利用WIFI辅助航迹推算，利用无迹卡尔曼滤波以松组合的形式对航向角进行修正。(6)采用零速修正(ZeroVelocityupdate,ZUPT)方法来校准加速度计和陀螺仪的传感器漂移误差。利用卡尔曼滤波进行零速校正时，尽管能够对速度误差和水平姿态角误差进行抑制，但是航向误差角仍然不能得到很好的修正。通过对卡尔曼滤波方程的可观测性分析，发现航向误差角的可观测性很差，对可观测性差的状态量，卡尔曼滤波是不能精确的将其估计出来。
技术实现思路
本专利技术的目的在于提供一种可以提高导航精度的基于ZIHR的航向角修正算法的MEMS行人导航方法。本专利技术的目的是这样实现的：包括以下步骤：步骤一：静止时刻利用加速度计和磁力计对MEMS行人导航系统进行初始对准；步骤二：求取MEMS行人导航系统惯导解算方程和误差方程；步骤三：利用陀螺仪和加速度计的输出值进行零速状态检测；步骤四：求解ZIHR修正算法中零速状态时相邻时刻航向角差值和陀螺漂移及航向误差角的关系；步骤五：建立简化的MEMS行人导航UKF滤波模型，进行UKF滤波。本专利技术还可以包括：1、步骤四具体包括：导航坐标系依次转换φz，φx，φy得到载体坐标系，载体坐标系相对导航坐标系的角速度向量ωnb写成沿载体坐标系的投影形式，将上式矩阵求逆之后的到：其中，带入之后得真实航向角的变化率为：计算得到的航向角的变化率为：其中为陀螺仪的漂移，为陀螺仪输出噪声，因为处于静止状态，所以将上面两式相减得到：计算得到的前后两个航向角的差值为是零均值高斯白噪声，将其作为量测噪声进行处理，简化为下式中v4；为当地纬度；Δtk为采样周期，简化之后两个采样时刻航向角的差值为：2、步骤五具体包括：选取地理坐标系中的位置误差、速度误差、姿态误差、加速度计零偏和陀螺仪常值漂移构成状态矢量构成系统的状态在零速修正基础上添加ZIHR修正观测，则观测量δv为系统的速度误差，为两个相邻采样时刻航向角的差值，系统状态方程和量测方程为：zk＝Hkxk+vk其中F(tk)、G(tk)均为非线性函数，跟据系统误差方程得到；w为系统噪声，v为量测噪声，w和Hk取值如下：其中，w、v均为零均值的高斯白噪声且E[wk]＝0，E[vk]＝0，δkj为Dirac函数；简化的UKF滤波过程为：(1)初始状态变量及其方差(2)时间更新：(3)量测更新其中，[xk]L为L列的矩阵，每一列都是xk，其他的参数计算如下:式中，L为xk的维数；α用于确定Sigma点在其均值周围的分布状况，是一个很小的数，取10-4≤α≤1；κ＝3-L；β取值与xk的分布形式有关，对于正态分布，β＝2为最优值，P表示误差协方差阵，K表示增益矩阵。为了解决行人导航中航向误差角随时间累积的问题，本专利技术提供了基于ZIHR的航向角修正算法，在零速修正的基础上扩展一维量测，对于系统的非线性特点采用一种简化的UKF滤波模型，并且将估计出的误差值反馈校正可以提高原系统的导航精度。本专利技术在零速修正的基础上，加上零积分航向角速率(ZeroIntegratedHeadingRate，ZIHR)航向角修正算法，利用零状态时相邻两个时刻航向角真值相同，但是惯导系统输出的航向角不同，将相邻两个时刻航向角差值作为量测值，公式推导发现，航向角差值和陀螺的常值漂移以及航向误差角存在确定的联系，实验证明此方法能够很好的抑制航向误差角的发散。卡尔曼滤波模型是针对线性系统而言，而行人导航所用的MEMS惯性器件的精度低，导致系统模型的非线性严重，本专利技术，本专利技术使用严恭敏等人提出的一种简化的无迹卡尔曼滤波(UKF)，将系统的状态方程看作是非线性的，量测方程看成是线性的，避免了重复采样等一系列的复杂算法。跟线性卡尔曼滤波相比，除了使用UT变换进行状态一步预测和一步预测误差协方差以外，其他步骤均是相同的，但是滤波精度会得到很大的提高。本专利技术的优点主要体现在：(1)最大限度的使用静止时刻的信息，利用零速状态相邻两个航向角的差值作为量测量，仅仅扩展了一维量测，计算量并不复杂，而且能够很好的抑制航向误差角发散。(2)在零速时刻利用UKF滤波器进行实时的反馈校正能很好地抑制低精度MEMS传感器长时间工作导航参数误差发散的问题，减小系统的模型误差，提高行人导航系统的定位精度。附图说明图1是基于ZIHR航向角修正的行人导航系统原理方框图；图2是基于ZIHR航向角修正的行人导航系统原理流程图；图3是加速度计和陀螺仪输出值示意图；图4是静态实验中加和不加ZIHR修正的航向角对比图；图5是静态实验中加和不加ZIHR修正的航向角误差标准差对比图；图6是速度和零速状态检测示意图；图7是加ZIHR修正的连续相同两圈的行走轨迹图；图8是不加ZIHR修正的连续相同两圈的行走轨迹图；图9是加ZIHR修正连续两圈行走时的航向角变化示意图。图10是矩形行走加和不加ZIHR修正的轨迹对比图；图11是矩形行走在百度地图上加和不加ZIHR修正的轨迹对比图；图12是矩形行走加ZIHR修正时的航向角变化示意图；图13是加和不加ZIHR修正时的定位误差对比表1；图14是MEMS惯性传感器的各项工作参数表2；具体实施方式下面举例对本专利技术作更详细的描述:如图1所示，当MEMS惯性导航系统初始对准之后，利用与MEMS配套的采数软件采集陀螺仪、加速度计的输出值。一方面利用陀螺仪和加速度计的数据进行惯导解算，另一方面进行零速检测解算。在没有检测到零速时，UKF滤波器只进行时间更新，当检测到零速时刻时会触发ZIHR修正和零速修正进行量测更新。将UKF滤波器估计出的位置误差、速度误差、姿态误差、陀螺常值漂移和加速度计的零偏，通过反馈校正到MEMS惯导解算单元中。本专利技术的基于ZIHR的航向角修正本文档来自技高网...

【技术保护点】
1.一种基于ZIHR的航向角修正算法的MEMS行人导航方法，其特征是：步骤一：静止时刻利用加速度计和磁力计对MEMS行人导航系统进行初始对准；步骤二：求取MEMS行人导航系统惯导解算方程和误差方程；步骤三：利用陀螺仪和加速度计的输出值进行零速状态检测；步骤四：求解ZIHR修正算法中零速状态时相邻时刻航向角差值和陀螺漂移及航向误差角的关系；步骤五：建立简化的MEMS行人导航UKF滤波模型，进行UKF滤波。

【技术特征摘要】
1.一种基于ZIHR的航向角修正算法的MEMS行人导航方法，其特征是：步骤一：静止时刻利用加速度计和磁力计对MEMS行人导航系统进行初始对准；步骤二：求取MEMS行人导航系统惯导解算方程和误差方程；步骤三：利用陀螺仪和加速度计的输出值进行零速状态检测；步骤四：求解ZIHR修正算法中零速状态时相邻时刻航向角差值和陀螺漂移及航向误差角的关系；步骤五：建立简化的MEMS行人导航UKF滤波模型，进行UKF滤波。2.根据权利要求1所述的基于ZIHR的航向角修正算法的MEMS行人导航方法，其特征是步骤四具体包括：导航坐标系依次转换φz，φx，φy得到载体坐标系，载体坐标系相对导航坐标系的角速度向量ωnb写成沿载体坐标系的投影形式，将上式矩阵求逆之后的到：其中，带入之后得真实航向角的变化率为：计算得到的航向角的变化率为：其中为陀螺仪的漂移，为陀螺仪输出噪声，因为处于静止状态，所以将上面两式相减得到：计算得到的前后两个航向角的差值为是零均值高斯白噪声，将其作为量测噪声进行处理，简化为下式中v4；为当地纬度；Δtk为采样周期，简化之后两个...

【专利技术属性】
技术研发人员：周广涛，王晴晴，蒋弘威，高远，刘远恒，张滋，孟佳帅，刘新，徐庚，
申请(专利权)人：哈尔滨工程大学，
类型：发明
国别省市：黑龙江,23