CN104819716A - 一种基于mems的ins/gps组合的室内外个人导航算法 - Google Patents

Abstract

一种基于MEMS的INS/GPS组合的室内外个人导航算法，该方法的实现步骤如下，在INS/GPS松耦合组合导航误差模型中，选取INS的姿态误差、速度误差、位置误差以及陀螺仪和加速度计的常值漂移误差作为组合导航***的状态变量，由于单纯的使用惯性器件进行个人定位，导航误差随着时间的增长也会越来越大。本发明采用GPS测得的相邻有效信息对于GPS测得位置的准确性进行判断以此来决定***是否进入组合模式。判断方法在传统的利用接受卫星数量和精度估计因子的判别基础上，增加了一个判别识别窗口。通过融合室内地图的粒子滤波算法，通过判断粒子推算结果是否符合客观事实，来修正原始算法的航向误差以此对行人轨迹进行优化，能够比较好的提供长时间高精度的室内自主导航。

Description

一种基于MEMS的INS/GPS组合的室内外个人导航算法

技术领域

本发明涉及基于MEMS的导航与定位技术领域，特别是在室内外的个人导航领域具有广泛的应用。

背景技术

个人导航***(Personal Navigation System，PNS)，又称为步行者导航***，可以实时确定行人行进方向、速度、位置等信息，是导航定位领域中的一个重要的新兴分支，主要用于跟踪定位徒步行走人员的实时位置。MEMS器件具有体积小、功耗低、质量轻、灵敏度高、成本低的优点，基于MEMS的个人导航***的研究已经成为一大研究热点。在单兵作战、消防人员的定位以及山区丛林探险者等场景都具有广泛的应用前景。

在空旷区，全球定位***(GPS)的精确度在几米之内，而且定位误差不随时间而积累，但在建筑物密集区，它甚至经常无法正确定位城市街区。问题在于多路径干扰，多路径是GPS在城市和室内的主要干扰源，其他干扰都不会造成如此严重的误差，这是所有基于无线电的定位***的灾难。所以GPS很多时候受到环境的限制，而无法进行导航与定位。

惯性导航***不依赖任何外界***的支持就能独立自主地进行导航，可以连续地提供包括姿态基准在内的全部导航和制导参数，对准后短期时间内具有良好的精度和稳定性。但是INS结构较复杂，导航误差随时间积累而增大，对准时间比较长，因此，不能满足远距离或长时间航行以及高精度导航或制导的要求。

发明内容

为了提高导航定位精度，出现了多种组合导航的方式，即把各具特点的不同类型的导航***匹配组合，使之相互取长补短，从而形成一种更为优良的新型导航***——组合导航***，其中GPS/INS组合导航***最为先进且应用最为广泛。而本发明主要是基于个人的GPS/INS组合导航***。

本发明采用的技术方案为一种基于MEMS的INS/GPS组合的室内外个人导航算法，该方法的实现步骤如下，

(1)多步态零速检测算法

在捷联解算过程中，如果不对误差进行及时的修正，位置误差会以时间的三次方迅速发散，最终使导航功能丧失。人在正常行走过程中每走一步脚部与地面接触时都会有一个相对静止的瞬间，也称作是零速间隔。准确得检测零速间隔，并在零速间隔对误差实时修正可以有效抑制误差的累积效应。本方法采用加速度方差、陀螺比力模值相结合的自调节阈值的零速检测算法。通过一个基于平均加速度的滑动时间窗口，来判断行人运动的步态模式，根据行人的状态两种检测方法自调节选用合适的阈值。检测过程如下：

1)判断运动模式：

此时MTI的x轴与人体前进方向相同，计算x轴的基于时间窗口的平均加速度来判断运动模式，k时刻的x轴平均加速度为：依据平均加速度的大小作为行走状态的判断依据，根据行走的状态加速度方差的阈值(th_σa)和陀螺模值平方和的阈值(th_ω)分别选择不同的值。

2)在k时刻加速度方差和陀螺模值平方和的大小：

       ${\mathrm{\σ}}_a^2=\frac{1}{2s+1}\underset{j=k-s}{\overset{k+1}{\mathrm{\Σ}}}{(a_j-{\stackrel{\‾}{a}}_j)}^2---\left(1\right)$

       ${\mathrm{\ω}}_k=[{\mathrm{\ω}}_k{\left(1\right)+{\mathrm{\ω}}_k\left(2\right)}^2+{\mathrm{\ω}}_k{\left(3\right)}^2]---\left(2\right)$

这里a_q表示平均加速度值；s表示滑动窗口的大小；a_q表示加速度模值，σ_a加速度方差，ω_k为三轴的陀螺平方和。

3)两种检测方法的融合：

当加速度方差σ_a小于th_σa时，加速度的零速检测波形为低电平；同理当陀螺模值的平方和ω_k小于th_ω时，陀螺的零速检测波形也为低电平；然后将两种零速检测结果进行与计算。在两种检测方法相融合时常常会带来一些高频噪声，通过改进的卡尔曼滤波算法剔除高频噪声后进行零速修正。

(2)融合室内地图的粒子滤波

粒子滤波的核心思想是先在状态空间中产生一组随机样本粒子,然后在测量的基础上,通过调节权值的大小和样本的位置从而获得服从实际分布的样本,并以样本的均值作为状态估计值。这里在室内平面地图已知的情况下，采用融合室内地图的粒子滤波算法对导航轨迹进行修正，从而得到最优的解算结果。

●粒子初始化

当k＝0时，根据状态先验分布p(X₀)建立初始状态粒子集其中每一个样本的初始权重

●信息提取

根据零速修正算法，得到人行走过程中的起始时刻和终止时刻，经过卡尔曼滤波算法进行零速修正以后，得到惯性导航解算的位置r_k和航向角ψ_k信息，然后根据运动模型提取出每一步的步长l_k和航向角变化量δψ_k，即：

       $Z_k=\left[\begin{array}{c}{l}_k\\ {\mathrm{\δ\ψ}}_k\end{array}\right]=\left[\begin{array}{c}{(\mathrm{\δ}{\hat{r}}_k{\left(1\right)}^2+\mathrm{\δ}{\hat{r}}_k{\left(2\right)}^2)}^{1/2}\\ {\widehat{\mathrm{\ψ}}}_k-{\widehat{\mathrm{\Ψ}}}_{k-1}\end{array}\right]---\left(3\right)$

●预测

根据***状态方程(4)，由k-l时刻的位置和航向角的变化信息得到k时刻的状态值

       $\left\{\begin{array}{c}{x}_k^{\left(i\right)}=x_{k-1}^{\left(i\right)}+l_k^{\left(i\right)}\cos {\mathrm{\ψ}}_{k-1}^{\left(i\right)}\\ y_k^{\left(i\right)}=y_{k-1}^{\left(i\right)}+l_k^{\left(i\right)}\sin {\mathrm{\ψ}}_{k-1}^{\left(i\right)}\\ {\mathrm{\ψ}}_k^{\left(i\right)}={\mathrm{\ψ}}_{k-1}^{\left(i\right)}+{\mathrm{\δ\ψ}}_k^{\left(i\right)}\end{array}\right.---\left(4\right)$

●权值计算

根据观测值与预测值的关系计算每一个粒子的权重即：

       $w_k^{\left(i\right)}=w_{k-1}^{\left(i\right)}*p\left(Z_k^{\left(i\right)}\right|X_k^{\left(i\right)})---\left(5\right)$

结合欧氏相似度与余弦相似度原理(5)式中可按下式表示：

       $p\left(Z_k^{\left(i\right)}\right|X_k^{\left(i\right)})=e^{(-\frac{L^2}{2})}*(1+\frac{\cos \mathrm{\θ}}{2})---\left(6\right)$

其中，L、θ分别为k时刻第i个粒子的位置、航向角与惯性导航解算的位置、航向角的差值。

●融合室内地图

由于人的行走轨迹不可能穿越墙壁，为航位推算的正确性提供了客观依据。这里通过结合地图平面信息，即墙壁信息，对每一个粒子k时刻的预测值与前一时刻的预测值的连接路径进行判定，如果穿越墙壁，权重则置为零，从而对无效粒子进行剔除。

●权值归一化

将优化后的粒子权重进行归一化即：

       ${\stackrel{\‾}{w}}_k^{\left(i\right)}=\frac{w_k^{\left(i\right)}}{\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{w}_k^{\left(i\right)}},i=\mathrm{1,2},...,N---\left(7\right)$

●状态估计输出

       ${\hat{X}}_k=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{\stackrel{\‾}{w}}_k^{\left(i\right)}{X}_k^{\left(i\right)}---\left(8\right)$

●自适应重采样

融合室内地图的粒子滤波算法，使得穿越墙壁粒子权重此时将回溯到上一步从新更新粒子，经若干次尝试后生成的新粒子仍然穿墙就将其标记为无效粒子。当所有粒子全部穿越墙壁或绝大多数粒子穿越墙壁时严重加快了粒子的退化速度，导致权重归一化失败，进而导致粒子重采样失去意义使推算出现严重偏差。为此针对大误差数据采用了一种自适应的重采样方案。设置经过穿墙判别以后权重不为0的粒子为有效粒子，用N_eff表示有效粒子个数，N表示粒子总个数，故有效粒子率对有效粒子率λ进行判断，来决定是否进行重采样。这样就有效避免了有效粒子数过少即粒子严重退化时，重采样而导致的粒子贫化问题。

(3)室外GPS/INS组合导航

本方法采用GPS/INS组合模式，其工作原理是：将GPS输出的位置、速度信息和惯导输出的位置、速度信息进行作差，经过卡尔曼滤波器进行滤波，估计出惯导***的误差，然后对惯导***进行校正来提高导航的精度。

在INS/GPS松耦合组合导航误差模型中，选取INS的姿态误差、速度误差、位置误差以及陀螺仪和加速度计的常值漂移误差作为组合导航***的状态变量，其状态变量为：

      

式中：分别为东向、北向和天向的三个姿态误差角；δv_E,δv_N,δv_U别为东向、北向和天向速度误差；δL,δλ,δh分别为纬度、经度和高度误差；ε_bx,ε_by,ε_bz分别为三轴陀螺误差；▽_ax,▽_ay,▽_az分别为加速度计误差。

由于单纯的使用惯性器件进行个人定位，导航误差随着时间的增长也会越来越大。为了对航向累积误差进行校正，在室外有GPS信号环境下可以釆用融合GPS和航迹推算的方法进行组合定位导航修正航向角，提高个人定位导航的准确性和稳定。

本发明采用GPS测得的相邻有效信息对于GPS测得位置的准确性进行判断以此来决定***是否进入组合模式。判断方法在传统的利用接受卫星数量和精度估计因子的判别基础上，增加了一个判别识别窗口，只有当侧得的位置信息在窗口内连续有效并且位置偏差小于一定值时，***才进入真正的组合模式。

与现有技术相比，本发明方法根据人行走的规律研究了一种基于个人的多种步态模式下的伪自适应阈值零速检测法，来限制惯性导航(INS)误差的增长，对跑、走、台阶等运动模式都具有稳定可靠的检测效果。通过融合室内地图的粒子滤波算法，通过判断粒子推算结果是否符合客观事实，来修正原始算法的航向误差以此对行人轨迹进行优化，能够比较好的提供长时间高精度的室内自主导航。由于航向角不可观测，为了抑制航向的漂移，在室外，当GPS信号良好时，采用GPS/INS航向融合算法对航向角得到及时的修正，明显提高了导航精度。

附图说明

图1为本发明的***结构图；

图2为走时零速检测结果示意图；

图3为跑时零速检测结果示意图；

图4为走时导航轨迹图；

图5台阶导航轨迹图；

图6为跑步导航轨迹图；

图7室内导航轨迹图；

图8为采用融合室内地图的粒子滤波修正算法的效果图；

图9为GPS/INS组合导航流程图；

图10为GPS/INS组合导航轨迹对比图；

图11为GPS/INS组合导航航向修正后的轨迹图；

图12为GPS/INS组合导航室内外行走的轨迹图；

具体实施方式

1、如图2所示，曲线波形为x轴加速度原始变化曲线，矩形波为人正常行走时自调节阈值的零速检测方法获得的零速检测波形；如图3所示，曲线波形为x轴陀螺原始变化曲线，矩形波为人跑时自调节阈值的零速检测方法获得的零速检测波形；采样周期都为100HZ，相同时间内检测出的零速间隔越多，速度越快，从图中可以看出跑时的速度大约是走时的2倍，自调节阈值的零速检测方法对不同步态模式具有很好的检测效果。

2、本文实验采用的基于MEMS的惯性测量单元为荷兰Xsens公司生产的MTI-G-700大量程传感器，使用鞋绑式的实验方法，将IMU捆绑于鞋面。设置输出频率为100Hz，陀螺的量程为±1000°/s，陀螺的零偏为0.2°/s，加速度计的量程为±15g，加速度计的零偏为0.03s/m²。通过大量实验依靠经验法选取加速度方差的阈值自调节范围3＜th_σa＜15，陀螺模值平方和的阈值2500＜th_ω＜160000，融合室内地图的粒子滤波算法中粒子数N＝200，整个实验过程采用MATLAB进行数据处理和导航解算。

为了验证伪自适应阀值的零速检测算法的可靠性和准确性，对走、跑、台阶三种运动模式做了四组实验，其零速检测的准确率如表1所示。

表1多步态零速检测

Tab 1 Zero detection under different gaits

      

图4为校内科学楼十楼，正常行走一圈的惯性导航轨迹图，起始点为(0,0)点，行走距离为118.3m，行走速度为1.06m/s，零速检测的准确率100％，定位误差为0.567％。

图5为人正常上台阶的导航轨迹图，台阶数量为10个。零速检测算法能够检测到每一步的零速时刻，垂直上升高度3m，垂直上升速度为0.31m/s。

图6为人跑时的导航轨迹图，为了验证伪自适应阀值的零速检测算法适用于较宽的速度范围，在校园的操场进行了跑步实验。操场一圈为400m以平均速度为3m/s的速度跑了一圈，定位误差为2.6％。

图7为人在室内行走的导航轨迹图，通过mapinfo对学校经管楼的一层平面地图进行矢量化，对比可看出融合室内地图获得较准确的初始航向角后的轨迹图比磁力计获得初始航向的轨迹图更加准确，初始位置设为(-71，-43)，终点设为(-5，-10)，行走距离约为210m，室内定位误差为2.3m。从图中可以看出通过磁力计测得的初始航向角由于受到磁场的影响往往并不准确，并且由于航向角不可观测，无法使用卡尔曼滤波算法进行修正，轨迹最终发生了穿墙现象。

图8为融合室内地图的粒子滤波修正算法的轨迹图，从图中可以看出此算法准确的修正了穿越墙壁的错误解算结果，对穿门和转弯处都有较好的修正，使定位误差缩减到0.2m。

3、通过计算组合导航的位置偏差，来计算变化角度，加上初始航向角即可得到此时刻的航相角，并且判断角度变化和位置变化情况，将航向角的变化范围与惯导相类似即以地理北向的夹角为航向角范围在[-180°,180°]。

利用周期为1s的滑动时间窗口来判断航向角是否发生变化，当航向角偏差小于一定范围之内，利用航迹推算的航向角校正惯导的航向角。然后将角度转化为弧度，从而改变转换矩阵实现连续的航向修正。

图10为在操场走了一圈进行采样实验获得的导航轨迹对不图，其中实线轨迹为INS的轨迹从图中可以看出随着时间的增长，由于航向角不可观测无法进行修正，使得轨迹偏离了一定航向，方块符轨迹为GPS测得的轨迹。图11中可以看出当惯导航向偏离一定值时且GPS测得较准确的航向时对INS航向角进行了修正使得导航轨迹明显得到了改善，同时提高了导航精度。

图12为从学校经管楼外边的街道走入经管楼然后在出来回到原点处的轨迹，从图中可以看出当进入室内GPS没有信号时，导航***为单独INS导航，当从室内走出来后，GPS获得信号，虽然接收卫星数量大于4颗，但是刚开始GPS获得的信号并不准确，采用滑动时间窗口进行判断后，对于这些点成功的进行了剔除。

Claims (1)

1.一种基于MEMS的INS/GPS组合的室内外个人导航算法，其特征在于：该方法的实现步骤如下，

(1)多步态零速检测算法

在捷联解算过程中，如果不对误差进行及时的修正，位置误差会以时间的三次方迅速发散，最终使导航功能丧失；人在正常行走过程中每走一步脚部与地面接触时都会有一个相对静止的瞬间，也称作是零速间隔；准确得检测零速间隔，并在零速间隔对误差实时修正可以有效抑制误差的累积效应；本方法采用加速度方差、陀螺比力模值相结合的自调节阈值的零速检测算法；通过一个基于平均加速度的滑动时间窗口，来判断行人运动的步态模式，根据行人的状态两种检测方法自调节选用合适的阈值；检测过程如下：

1)判断运动模式：

此时MTI的x轴与人体前进方向相同，计算x轴的基于时间窗口的平均加速度来判断运动模式，k时刻的x轴平均加速度为：依据平均加速度的大小作为行走状态的判断依据，根据行走的状态加速度方差的阈值(th_σa)和陀螺模值平方和的阈值(th_ω)分别选择不同的值；

2)在k时刻加速度方差和陀螺模值平方和的大小：

${\mathrm{\σ}}_a^2=\frac{1}{2s+1}\underset{j=k-s}{\overset{k+s}{\mathrm{\Σ}}}{(a_j-{\stackrel{\‾}{a}}_j)}^2---\left(1\right)$

ω_k＝[ω_k(1)²+ω_k(2)²+ω_k(3)²]      (2)

这里表示平均加速度值；s表示滑动窗口的大小；a_q表示加速度模值，σ_a加速度方差，ω_k为三轴的陀螺平方和；

3)两种检测方法的融合：

当加速度方差σ_a小于th_σa时，加速度的零速检测波形为低电平；同理当陀螺模值的平方和ω_k小于th_ω时，陀螺的零速检测波形也为低电平；然后将两种零速检测结果进行与计算；在两种检测方法相融合时常常会带来一些高频噪声，通过改进的卡尔曼滤波算法剔除高频噪声后进行零速修正；

(2)融合室内地图的粒子滤波

粒子滤波的核心思想是先在状态空间中产生一组随机样本粒子,然后在测量的基础上,通过调节权值的大小和样本的位置从而获得服从实际分布的样本,并以样本的均值作为状态估计值；这里在室内平面地图已知的情况下，采用融合室内地图的粒子滤波算法对导航轨迹进行修正，从而得到最优的解算结果；

●粒子初始化

当k＝0时，根据状态先验分布p(X₀)建立初始状态粒子集其中每一个样本的初始权重

●信息提取

根据零速修正算法，得到人行走过程中的起始时刻和终止时刻，经过卡尔曼滤波算法进行零速修正以后，得到惯性导航解算的位置r_k和航向角ψ_k信息，然后根据运动模型提取出每一步的步长l_k和航向角变化量δψ_k，即：

$Z_k=\left[\begin{array}{c}{l}_k\\ {\mathrm{\δ\ψ}}_k\end{array}\right]=\left[\begin{array}{c}{(\mathrm{\δ}{\hat{r}}_k{\left(1\right)}^2+\mathrm{\δ}{\hat{r}}_k{\left(2\right)}^2)}^{1/2}\\ {\widehat{\mathrm{\ψ}}}_k-{\widehat{\mathrm{\ψ}}}_{k-1}\end{array}\right]---\left(3\right)$

●预测

根据***状态方程(4)，由k-l时刻的位置和航向角的变化信息得到k时刻的状态值

$\left\{\begin{array}{c}{x}_k^{\left(i\right)}=x_{k-1}^{\left(i\right)}+l_k^{\left(i\right)}\cos {\mathrm{\ψ}}_{k-1}^{\left(i\right)}\\ y_k^{\left(i\right)}=y_{k-1}^{\left(i\right)}+l_k^{\left(i\right)}\sin {\mathrm{\ψ}}_{k-1}^{\left(i\right)}\\ {\mathrm{\ψ}}_k^{\left(i\right)}={\mathrm{\ψ}}_{k-1}^{\left(i\right)}+{\mathrm{\δ\ψ}}_k^{\left(i\right)}\end{array}\right.---\left(4\right)$

●权值计算

根据观测值与预测值的关系计算每一个粒子的权重即：

$w_k^{\left(i\right)}=w_{k-1}^{\left(i\right)}*p\left(Z_k^{\left(i\right)}\right|X_k^{\left(i\right)})---\left(5\right)$

结合欧氏相似度与余弦相似度原理(5)式中可按下式表示：

$p\left(Z_k^{\left(i\right)}\right|X_k^{\left(i\right)})=e^{(-\frac{L^2}{2})}*(1+\frac{\cos \mathrm{\θ}}{2})---\left(6\right)$

其中，L、θ分别为k时刻第i个粒子的位置、航向角与惯性导航解算的位置、航向角的差值；

●融合室内地图

由于人的行走轨迹不可能穿越墙壁，为航位推算的正确性提供了客观依据；这里通过结合地图平面信息，即墙壁信息，对每一个粒子k时刻的预测值与前一时刻的预测值的连接路径进行判定，如果穿越墙壁，权重则置为零，从而对无效粒子进行剔除；

●权值归一化

将优化后的粒子权重进行归一化即：

${\stackrel{\‾}{w}}_k^{\left(i\right)}=\frac{w_k^{\left(i\right)}}{\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{w}_k^{\left(i\right)}},i=\mathrm{1,2},...,N---\left(7\right)$

●状态估计输出

${\hat{X}}_k=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{\stackrel{\‾}{w}}_k^{\left(i\right)}{X}_k^{\left(i\right)}---\left(8\right)$

●自适应重采样

融合室内地图的粒子滤波算法，使得穿越墙壁粒子权重此时将回溯到上一步从新更新粒子，经若干次尝试后生成的新粒子仍然穿墙就将其标记为无效粒子；当所有粒子全部穿越墙壁或绝大多数粒子穿越墙壁时严重加快了粒子的退化速度，导致权重归一化失败，进而导致粒子重采样失去意义使推算出现严重偏差；为此针对大误差数据采用了一种自适应的重采样方案；设置经过穿墙判别以后权重不为0的粒子为有效粒子，用N_eff表示有效粒子个数，N表示粒子总个数，故有效粒子率对有效粒子率λ进行判断，来决定是否进行重采样；这样就有效避免了有效粒子数过少即粒子严重退化时，重采样而导致的粒子贫化问题；

(3)室外GPS/INS组合导航

本方法采用GPS/INS组合模式，其工作原理是：将GPS输出的位置、速度信息和惯导输出的位置、速度信息进行作差，经过卡尔曼滤波器进行滤波，估计出惯导***的误差，然后对惯导***进行校正来提高导航的精度；

在INS/GPS松耦合组合导航误差模型中，选取INS的姿态误差、速度误差、位置误差以及陀螺仪和加速度计的常值漂移误差作为组合导航***的状态变量，其状态变量为：

式中：分别为东向、北向和天向的三个姿态误差角；δv_E,δv_N,δv_U别为东向、北向和天向速度误差；δL,δλ,δh分别为纬度、经度和高度误差；ε_bx,ε_by,ε_bz分别为三轴陀螺误差；▽_ax,▽_ay,▽_az分别为加速度计误差；

由于单纯的使用惯性器件进行个人定位，导航误差随着时间的增长也会越来越大；为了对航向累积误差进行校正，在室外有GPS信号环境下釆用融合GPS和航迹推算的方法进行组合定位导航修正航向角，提高个人定位导航的准确性和稳定；

本方法采用GPS测得的相邻有效信息对于GPS测得位置的准确性进行判断以此来决定***是否进入组合模式；判断方法在传统的利用接受卫星数量和精度估计因子的判别基础上，增加了一个判别识别窗口，只有当侧得的位置信息在窗口内连续有效并且位置偏差小于一定值时，***才进入真正的组合模式。