[发明专利]一种基于MEMS的INS/GPS组合的室内外个人导航算法

摘要

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

主权项

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