一种基于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的航向角修正算法在mems行人导航中的应用，包括以下几个具体步骤：

步骤一：初始对准为导航解算提供初始的姿态信息，是导航定位中一个重要的环节。初始对准又包括粗对准和精对准。由于mems陀螺仪的精度低导致其不能敏感到地球的自转角速度，因此不能实现方位上的自校准功能，只能利用静止时刻加速度计的输出值实现水平上的粗对准。计算出的俯仰角和横滚角的初始值为：

其中，分别为加速度计测量比力值f^b在载体坐标系三个轴上的分量。方位上的粗对准需要利用外部信息获取，比如磁力计。设三轴磁力计在载体坐标系下的量测值为则三轴磁力计在n’系下投影为：

其中n’系和地理坐标系n系之间仅相差绕z轴的旋转矩阵；md为当地的地磁偏角；就是磁力计计算得的航向角。上述得到的还存在很大的误差。因此在进行导航解算之前，保持一段时间静止，利用基于zihr和零速修正的ukf滤波器来得到更加精确的初始姿态角。

步骤二：取地理坐标系(n系)为导航系，人身体的右-前-上为载体坐标系(b系)。则mems行人导航基本方程如下：

其中，为姿态矩阵；为的反对称矩阵；为的反对称矩阵，是地速导致的角速度；vⁿ是载体速度在导航系上的投影；rⁿ为行人位置在导航系上的投影。根据mems惯性器件精度低和人行走的特点，将和两项忽略不计。简化后的mems行人导航基本方程如下：

简化的ukf滤波器模型是基于mems惯性导航系统的误差方程建立的。mems捷联惯导在地理坐标系下的系统误差模型为：

其中，δrⁿ,δvⁿ,分别为位置误差、速度误差、姿态误差、加速度计零偏和陀螺仪常值漂移；为零均值高斯白噪声。由于(12)和(13)式中的含有姿态矩阵和噪声的乘积项，因此不能按平常的加性噪声处理。

步骤三：零速检测技术是为了判断一段时间内行人是否处于零速状态。目前的算法主要有两种，一种是利用mems惯性测量元件输出值检测，主要有隐马尔科夫模型检测、广义似然比检测、加速度方差检测、加速度模值检测和角速度能量检测。另一种是借用外部辅助设备检测，比如高分辨率压力传感器安装在行人脚跟接触地面的地方，根据步态周期内的压力测量值检测零速。磁力计检测根据每个步态周期内永磁体的测量结果来检测零速等。本文使用广义似然比的方法进行零速检测，检测融合了陀螺仪和加速度计的输出值，检测精度大大提高。

将零速检测看成是一个假设性检验问题，假设某段时间内处于零速状态，采样值为n，即滑动窗口为n，如果下式成立则认为假设成立，否则假设不成立，行人处于运动状态。

其中，为陀螺仪和加速度计在tk时刻的输出值；g为地球重力加速度；t(fk,ωk)为零速检测的检验统计量；j为阈值；σf为加速度计输出噪声的标准差；σω为陀螺仪输出噪声的标准差。阈值j与加速度计和陀螺仪的输出噪声统计特性有关，窗口大小n与mems的输出频率和行走速度有关。

步骤四：在零速状态时，如果仅利用零速修正技术进行卡尔曼滤波，对航向误差角不能达到很好的修正效果，因为航向误差角可观测性很差。理论上零速状态时相邻时刻的真实航向角是相同的，由于mems陀螺仪的漂移很大，导致惯导解算出相邻时刻的航向角存在差异。将零速状态时相邻时刻航向角的差值作为量测量，来修正陀螺仪漂移引起的航向角误差，可以起到抑制陀螺漂移的效果。

导航坐标系依次转换φz，φx，φy可得到载体坐标系。载体坐标系相对导航坐标系的角速度向量ωnb写成沿载体坐标系的投影形式。

将上式矩阵求逆之后得到：

其中，带入之后得真实航向角的变化率为：

计算得到的航向角的变化率为：

其中为陀螺仪的常值漂移，为零均值高斯白噪声。因为处于静止状态，所以将上面两式相减得到：

因为则计算得到的前后两个航向角的差值为：

是零均值高斯白噪声，将其作为量测噪声进行处理，简化为下式中v4；为当地纬度；δtk为采样周期。简化之后两个采样时刻航向角的差值为：

步骤五：选取地理坐标系中的位置误差、速度误差、姿态误差、加速度计零偏和陀螺仪常值漂移构成状态矢量，即系统的状态在零速时刻，如果只是用零速修正观测量不能抑制航向误差角发散，为了提高航向角的精度，在零速修正基础上增加zihr修正观测，则观测量δvⁿ为系统的速度误差，为两个相邻采样时刻航向角的差值。系统状态方程和量测方程为：

zk＝hkxk+vk(31)

其中f(tk)，g(tk)均为非线性函数，可以据系统误差方程得到，w为系统噪声，v为量测噪声，w和hk取值如下：

其中，w、v均为零均值的高斯白噪声且e[wk]＝0，e[vk]＝0，δkj为dirac函数，qk为系统噪声序列方差阵，rk为量测噪声序列的方差阵。

简化的ukf滤波过程：

(3)初始状态变量及其方差：

(4)时间更新：

(3)量测更新：

其中，[xk]l为l列的矩阵，每一列都是xk，其他的参数计算如下:

式中，l为xk的维数；α用于确定sigma点在其均值周围的分布状况，是一个很小的数，可取10^-4≤α≤1；κ＝3-l；β取值与xk的分布形式有关，对于正态分布，β＝2为最优值；p表示误差协方差阵；k表示增益矩阵。

验证实例：

本发明实验采用mimu是由xsens公司生产的mti-g系列的mems。其最大的特点就在于体积小、重量轻、工作稳定并能保证一定的精度，完全符合单兵导航的要求。mti-g型mems中包括算法中使用到的三轴陀螺仪、三轴加速度计和三轴的磁力计。图14的表2中列出了mems惯性传感器各项工作参数。

实验验证和结果分析，设定加速度计输出噪声标准偏差为σf＝0.02m/s、陀螺仪输出噪声的标准偏差为σw＝0.1*π/180rad/s，采样频率设置为100hz，滑动窗口大小n设置为3，广义似然比判定运动和静止的阈值为0.3*10⁵。状态初始值x0均取值为0，初始的预测误差协方差矩阵系统噪声方差阵量测噪声方差阵

(1)第一组实验：在隔震台采集14min的数据。图4中刚开始时加zihr修正的航向角会比不加zihr修正的航向误差角发散的快，但是随后不再增大，反而会慢慢的恢复到初始值，因为zihr修正算法在制约着航向误差角的发散。而不加zihr修正的航向角会以一定的斜率直线增加，跟初始值差来越大。图5是卡尔曼滤波得出的加和不加zihr修正的航向角误差标准差对比图,不加zihr修正的航向误差角标准差约为10°，加zihr修正的航向误差角标准差约为5°，约是两倍的关系。

(2)第二组实验：连续行走两圈，共行走70米，70步，行走时间85秒。图6是广义似然比零速检测结果，从图中能够明显的在速度为零时，零速检测值为1，说明广义似然比的零速检测模型十分准确。图7是加上zihr修正的连续相同两圈行走轨迹图，可以看出两圈行走轨迹基本重合，并且起始点和终止点之间也是基本重合的，两点之间水平距离仅相差0.19米。图8是不加zihr修正的连续相同两圈行走轨迹图，可以看出轨迹图变得粗糙，是因为航向角的小的变动引起的，而且前后两圈轨迹刚开始基本重合，接着因为航向角的变化相差很大的距离。并且起始点和终止点之间的距离也很远，相差1.65米。图9是加zihr修正时的连续相同两圈的航向角的变化图，从图中可以看出，航向角变化很规律，每转一个弯都会有90度的变化。

(3)第三组实验：某建筑物行走一个闭合的矩形，行走距离为415米，其中红色线是加zihr修正轨迹图，蓝色线是不加zihr修正的轨迹图，从图10中可以明显的看出加上zihr修正之后在最后一段距离能够将航向角及时的校正。在图11中用百度地图来作为一个真实轨迹的参考，发现解算出来的轨迹和该建筑物在百度地图中的位置十分吻合，表明解算出的轨迹没有发生整体的偏移。图12是zihr修正之后航向角变化示意图，发现航向角的变化很规律，每转一个弯都会有90度的变化。

结论：

本发明在基于零速修正的ukf滤波器的基础上提出了zihr算法来修正航向误差角，在一定程度上解决了低精度mems惯导算法中航向误差角易发散的问题。从仿真结果中看出，利用了人行走的规律，准确的检测出零速时刻，在零速期间，利用简化的ukf滤波进行zihr修正和零速修正，此算法能够校准水平姿态误差和速度误差中的绝大部分，相对于只使用零速修正算法，航向误差角和位置误差得到了很好的抑制效果，从而提高mems惯导系统的定位精度。