CN104515527A - 一种无gps信号环境下的抗粗差组合导航方法 - Google Patents

Abstract

本发明涉及一种无GPS信号环境下的抗粗差组合导航方法，包括以下步骤：SINS初始化；INS导航计算，获得车辆当前时刻的速度、位置和姿态；卡尔曼滤波时间更新，得到系统的状态一步预测值及相应的均方误差；计算滤波新息，并根据滤波新息判断是否存在粗差，若是，则剔除相应的观测值，若否，则保留相应的观测值；将剔除粗差后的观测值作为卡尔漫滤波器的输入进行滤波量测更新，获得状态最优估计值及相应的均方误差；利用状态最优估计值进行反馈校正，获得修正后的车辆位置、速度和姿态矩阵。与现有技术相比，本发明具有低成本、导航稳定、精度高等优点。

Description

一种无GPS信号环境下的抗粗差组合导航方法

技术领域

本发明涉及一种车载导航方法，尤其是涉及一种无GPS信号环境下的抗粗差组合导航方法。

背景技术

室内和地下等无GPS(Global Positioning System,全球定位系统)信号环境下的导航定位技术在我国具有重大需求，可应用于室内导航、室内移动测图系统、地下交通导航定位、地下作业安全等领域，对于地下隧道开挖、矿产资源的勘探、矿山开采和事故应急等应用具有重要意义。

惯性导航系统(Inertial Navigation System，INS)是一种自主导航系统，以牛顿力学定律为工作原理，利用安装在载体上的惯性测量元件(Inertial MeasurementUnit，IMU)来敏感载体的角速度和加速度信息，通过积分运算得到载体的速度、位置和姿态等导航信息，IMU包括加速度计和陀螺。惯导系统按有无实体平台，可以分为两类：平台式惯性导航系统(Gimbaled Inertial Navigation System，GINS)和捷联式惯性导航系统(Strapdown Inertial Navigation System，SINS)。在捷联惯导系统中，惯性测量元件直接安装在载体上，由于没有实体平台，捷联惯导具有结构简单、体积小、成本低等优点，被广泛应用于车辆导航系统中。惯性导航系统在短时间内具有高精度，但是由于积分原理，其误差随时间累积，特别是车载导航系统中，考虑成本因素，通常采用的IMU精度较低，其误差累积更快，长时间工作的精度较差。

为了减少误差累积，要引入外部观测值来对惯导系统进行修正。里程计是测量车辆运行距离／速度的装置，其价格低、使用方便，是车辆导航中经常采用的装置，经过刻度因子的检校和补偿以后，其速度观测值一般可以达到优于1％的精度，因此可以用来修正惯性导航系统的速度误差。电子罗盘通过测量地球磁场在敏感轴上的分量来确定磁航向角，经过地磁偏角的补偿以后，航向角的精度可以达到0.5度以内，将其安装于车辆，可以提供航向信息，用来修正惯性导航系统的姿态误差。因此可以构建捷联惯导／里程计／电子罗盘的组合导航系统，有效利用各自的优点，在低成本的情况下实现无GPS信号环境下的自主导航。

在捷联惯导／里程计／电子罗盘的组合导航系统中，采用卡尔曼滤波进行最优数据融合，将里程计的速度观测值与电子罗盘的速度观测值作为滤波器的部分输入，通过滤波获得系统状态(速度、位置、姿态)的最优估计。

然而，由于车辆运行环境的复杂性，经常会受到各种局部和短暂的干扰。具体情况有：车辆经过松软的沙土或者碰到地面隆起的障碍物，可能出现车轮打滑的现象；车辆运行过程中，可能经过铁磁物质附近，导致电子罗盘的输出值异常。如果出现车轮打滑的情况，则里程计输出的速度观测值将严重偏离车辆的真实速度；如果出现强磁场的干扰，则电子罗盘输出的航向角将严重偏离车辆的真实航向角。以上两种情况下输出的观测值都可以称为“粗差”。粗差观测值如果作为卡尔曼滤波器的输入，将引起滤波器的震荡，甚至导致滤波器发散，这将影响整个系统的精度和稳定性。因此本发明从滤波器新息的角度分析观测值的质量，识别出粗差观测值并剔除。

发明内容

本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种低成本、导航稳定和精度高的无GPS信号环境下的抗粗差组合导航方法。

本发明的目的可以通过以下技术方案来实现：

一种无GPS信号环境下的抗粗差组合导航方法，该方法通过SINS、里程计和电子罗盘的组合对在无GPS信号环境下运作的车辆进行导航，具体包括以下步骤：

1)SINS初始化，获得车辆的初始位置和初始姿态矩阵；

2)根据IMU、里程计、电子罗盘的实时测量数据及步骤1)中的初始位置和初始姿态矩阵，进行INS导航计算，获得车辆当前时刻的速度、位置和姿态；

3)卡尔曼滤波时间更新，得到系统的状态一步预测值及相应的均方误差；

4)根据步骤3)中系统的状态一步预测值，以及里程计和电子罗盘的观测值，计算滤波新息，并根据滤波新息判断是否存在粗差，若是，则剔除相应的观测值，若否，则保留相应的观测值；

5)将剔除粗差后的观测值作为卡尔漫滤波器的输入进行滤波量测更新，获得状态最优估计值及相应的均方误差；

6)利用状态最优估计值进行反馈校正，获得修正后的车辆位置、速度和姿态矩阵。

所述的车辆的初始位置根据环境中的已知参考点获得或通过全站仪测量得到。

所述的车辆的初始姿态矩阵为：

$R_b^L=\left(\begin{array}{ccc}\cos r\cos y-\sin r\sin p& -\sin y\cos p& \sin r\cos y+\cos r\sin y\sin p\\ \cos r\sin y+\sin r\cos y\sin p& \cos y\cos p& \sin r\sin y-\cos r\cos y\sin p\\ -\sin r\cos p& \sin p& \cos r\cos p\end{array}\right)$

式中，车辆坐标系为b系，SINS的导航坐标系选择当地水平坐标系，为L系，y为电子罗盘观测得的航向角，p、r为计算得的车辆俯仰角和翻滚角，且

$\left\{\begin{array}{c}p=\arcsin \left[\frac{g_y^b}{g}\right]\\ r=\arctan \left[\frac{g_x^b}{g_z^b}\right]\end{array}\right.$

其中，g为重力加速度，为车辆静止设定时间内，SINS中的加速度计观测得的比力向量均值。

所述的INS导航计算具体包括以下步骤：

201)姿态更新：利用SINS中的陀螺输出的角速度观测值，采用四元数法进行姿态更新，然后将更新后的四元数转化成姿态矩阵，记为

202)姿态提取：根据更新后的姿态矩阵中元素的值，计算车辆的姿态角(p，r，y)；

203)比力分解：将加速度计输出的比力观测值fb由b系转换到L系：

$f^L=R_b^L\left(k\right)\·f^b;$

204)加速度的修正：对f^L进行哥氏加速度修正和正常重力补偿，得到车辆在地球表面的加速度a：

$f^L-(2{\mathrm{\Ω}}_{\mathrm{ie}}^L+{\mathrm{\Ω}}_{\mathrm{eL}}^L)v^L+g^L$

式中，分别表示地球自转角速度和导航系的转移速率，两者都表达在L系中，v^L为车辆上一时刻的速度，g^L为重力加速度在L系中的分量；

205)积分求取速度：利用加速度进行积分得到速度增量，进而得到k时刻的速度 $v_k^L:$

$v_k^L=v_{k-1}^L+a\·\mathrm{\Δt}$

式中，为k-1时刻的速度，Δt为时间间隔；

206)积分求取位置：利用速度进行积分得到位置增量，进而得到k时刻的位置 $r_k^L:$

$r_k^L=r_{k-1}^L+\frac{1}{2}(v_k^L+v_{k-1}^L)\·\mathrm{\Δt}$

式中，为k-1时刻的位置。

所述的更新卡尔曼滤波时间具体包括以下步骤：

301)设定卡尔曼滤波器的状态向量为

X=(δr^L，δv^L，δε^L，d，b，δe)

式中，X为16阶误差向量，其中δr^L为SINS的三维位置误差，δv^L为SINS的三维速度误差，δε^L为SINS的平台失准角，d为三轴陀螺漂移，b为三轴加速度计零偏，δe为里程计的刻度因子误差；

302)根据误差向量建立SINS的状态方程：

X_k=Φ_k，k-1X_k-1+Г_k，k-1W_k-1

式中，X_k-1和X_k分别表示SINSk-1和k时刻的状态，Φ_k，k-1为状态转移矩阵，W_k-1为动态噪声，Г_k,k-1为噪声输入矩阵；

303)根据状态方程计算SINS的状态一步预测值及相应的均方误差：

$\begin{array}{c}{\hat{X}}_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}\\ P_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k,k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k,k-1}^T.\end{array}$

所述的是否存在粗差具体指：

当速度误差量测值Z_v满足以下关系时，判断相应的观测值为粗差：

${\left|\right|Z_v-H_v{\hat{X}}_{k,k-1}\left|\right|}_2>i\·{\left|\right|V_{\mathrm{ins}}^L\left|\right|}_2+j$

式中，为INS导航计算得到的车辆速度，为里程计在L系中测得的车辆速度，||||₂表示2范数，H_v为速度观测矩阵，i为大于0的比例系数，j为大于0的常数；

当航向角误差量测值Z_a满足以下关系时，判断相应的观测值为粗差：

$\left\{\begin{array}{c}|Z_a-H_a{\hat{X}}_{k,k-1}|>b_1\\ |\mathrm{\δ}{y}_{\mathrm{ins}}-\mathrm{\δ}{y}_{\mathrm{mag}}|>b_2\end{array}\right.$

式中，Z_a＝y_ins-y_mag，y_ins为INS导航计算得到的车辆航向角，y_mag为电子罗盘测得的车辆航向角，H_a为航向角观测矩阵，δy_ins是一定时间间隔内INS导航计算得到的航向角变化值，δy_mag是一定时间间隔内电子罗盘输出的航向角变化值，b₁、b₂为设定的阈值。

所述的步骤6)中的反馈校正具体为：

位置、速度、姿态直接通过以下公式修正：

$\left\{\begin{array}{c}{\hat{r}}^L=r^L-{\mathrm{\δr}}^L\\ {\hat{v}}^L=v^L-{\mathrm{\δv}}^L\\ {\hat{R}}_b^L=R_b^L(I-({\mathrm{\δ\ϵ}}^L\×))\end{array}\right.$

式中，分别为修正后的位置、速度和姿态矩阵，是INS导航计算得到的位置、速度和姿态矩阵，(δε^L×)为三维反对称阵；

里程计的刻度因子通过下式进行修正：

$\hat{e}=e+\mathrm{\δe}$

式中，是经过修正后的里程计刻度因子，e是修正前的值；

陀螺漂移和加速度计零偏反馈到下一个周期的INS导航计算中。

该方法还包括：在导航开始之前的检校步骤，该检校步骤包括里程计刻度因子检校和电子罗盘检校。

所述的里程计刻度因子e的检校公式为：

$\begin{array}{c}{e}_i=\frac{l}{m_i}(i=\mathrm{1,2},...,n)\\ e=\frac{1}{n}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{e}_i\end{array}$

l为设定的地面直线的长度，m为安装有里程计的车辆沿地面直线运行时输出的脉冲数，n为实验次数。

本发明的导航方法及系统可用于室内和地下等无GPS信号环境下的导航，与现有技术相比，具有以下优点：

1、提出一种捷联惯导／里程计／电子罗盘构成的车载组合导航方法，无需发射或接收信号，属于低成本的，可实现自主导航。

2、利用加速度计与电子罗盘完成系统的姿态初始化。由于车载导航系统采用的IMU精度较低，其陀螺仪无法敏感地球自转，导致无法确定初始航向角。本发明采用加速度计静止时的输出数据计算车辆的俯仰角和翻滚角，采用电子罗盘获取车辆的航向角，然后一起构建车辆的初始姿态矩阵，完成姿态的初始化，提高了导航的精确度。

3、通过滤波新息判断观测值质量，有效剔除粗差，保持系统长时间的稳定性和高精度。通常，车辆打滑带来的速度误差，以及磁场干扰带来的航向角误差，都将引起滤波器的震荡，甚至导致滤波器发散。本发明在深入理解卡尔曼滤波原理的基础上，提出通过对滤波新息的分析来判断速度观测值和航向角观测值的质量，从而有效的剔除观测值粗差，保持系统长时间的稳定性和高精度。

附图说明

图1为本发明方法的流程示意图；

图2为本发明中INS导航计算原理与流程示意图；

图3为实施例中里程计输出的车辆前进速度曲线；

图4为实施例中电子罗盘输出的车辆航向角曲线；

图5为实施例中卡尔曼滤波过程中的速度新息曲线；

图6为实施例中卡尔曼滤波过程中的航向角新息曲线；

图7为实施例中车辆运行轨迹示意图。

具体实施方式

下面结合附图和具体实施例对本发明进行详细说明。本实施例以本发明技术方案为前提进行实施，给出了详细的实施方式和具体的操作过程，但本发明的保护范围不限于下述的实施例。

一种无GPS信号环境下的抗粗差组合导航方法，该方法通过SINS(捷联式惯性导航系统)、里程计和电子罗盘的组合对在无GPS信号环境下运作的车辆进行导航，采用卡尔曼滤波作为数据融合手段，实现系统状态的最优估计，并且能有效的剔除观测值粗差，保持系统长时间的稳定性和高精度。

如图1所示，该方法具体实现步骤如下。

1、系统检校

在导航开始之前的检校步骤，该检校步骤包括里程计刻度因子检校和电子罗盘检校。

刻度因子是指里程计输出的一个脉冲信号所代表的实际地面距离，这个值需要定期检校，并加以补偿。里程计刻度因子e的检校公式为：

$e_i=\frac{l}{m_i}(i=\mathrm{1,2},...,n)---\left(1\right)$

$\frac{1}{n}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{e}_i---\left(2\right)$

l为设定的地面直线的长度(本实施例中取l=200m)，m为安装有里程计的车辆沿地面直线运行时输出的脉冲数，n为实验次数(本实施例中取n＝5)。

经过补偿后，里程计的测速精度可以达到1％以内。但是由于温度、轮胎气压、地面状况等因素的影响，刻度因子会发生变化，与离线检校获得的值出现差异，这个差异值称为刻度因子误差，记为δe，通常考虑成随机常数，加入到系统状态向量，通过卡尔曼滤波实时估计。

相比于里程计的检校，电子罗盘的检校过程要简单很多。如可采用下述方法：安装有电子罗盘的车辆在水平面内绕原地转一圈，内置于电子罗盘的算法，将根据转动过程中记录的两个水平方向的磁场矢量大小，完成检校。

2、初始化

系统初始化的目的是获得车辆的初始位置和初始姿态矩阵。考虑到在室内和地下环境通常都有一些已知参考点，因此位置的初始化采用与已知参考点对准的方法，或者采用全站仪测量。

姿态初始化通常采用静止对准的方法，依靠加速度计敏感地球重力和陀螺敏感地球自转来实现。然而地球自转角速度是一个微弱的量(约0.004°／s)，由于组合系统中采用的IMU精度比较低，无法敏感到地球自转，也就无法计算车辆的航向角，需要借助电子罗盘来提供航向。因此采用加速度计与电子罗盘结合的方式完成初始对准。步骤如下：

(1)坐标系定义。车辆本身的坐标系记为b系，其三轴指向为“右、前、上”，导航坐标系选择当地水平坐标系，记为L系，其三轴指向为“东、北、天”。b系到L系的旋转矩阵记为其元素由姿态角的值计算。

(2)车辆俯仰角与翻滚角计算。让车辆保持静止一段时间(本例中取5分钟)，加速度计输出的均值记为gb，

$g^b={\left(g_x^b{g}_y^b{g}_z^b\right)}^T---\left(3\right)$

重力加速度g在L系的分量为gL，这是个已知量

g^L=[00-g]^T  (4)

g^L与g^b之间的关系满足

$g^b=R_L^b\·g^L---\left(5\right)$

根据姿态矩阵的定义，将上式展开得

$\left\{\begin{array}{c}{g}_x^b=-\sin r\cos p\·s\\ g_y^b=\sin p\·g\\ g_z^b=\cos r\cos p\·g\end{array}\right.---\left(6\right)$

式中，p、r为计算得的车辆俯仰角和翻滚角，由此可以得到p和r的计算公式

$\left\{\begin{array}{c}p=\arcsin \left[\frac{g_y^b}{g}\right]\\ r=\arctan \left[\frac{g_x^b}{g_z^b}\right]\end{array}\right.---\left(7\right)$

(3)初始姿态矩阵计算。结合电子罗盘输出的航向角y，可以计算出车辆的姿态矩阵

$R_b^L=\left(\begin{array}{ccc}\cos r\cos y-\sin r\sin p& -\sin y\cos p& \sin r\cos y+\cos r\sin y\sin p\\ \cos r\sin y+\sin r\cos y\sin p& \cos y\cos p& \sin r\sin y-\cos r\cos y\sin p\\ -\sin r\cos p& \sin p& \cos r\cos p\end{array}\right)---\left(8\right)$

3、INS导航计算

根据IMU、里程计、电子罗盘的实时测量数据及步骤1)中的初始位置和初始姿态矩阵，进行INS导航计算，获得车辆当前时刻的速度和位置，其中，IMU包括加速度计和陀螺。

如图2所示，

INS导航计算具体包括以下步骤：

(1)姿态更新：利用SINS中的陀螺输出的角速度观测值，采用四元数法进行姿态更新，然后将更新后的四元数转化成姿态矩阵，记为

(2)姿态提取：根据更新后的姿态矩阵中元素的值，计算车辆的姿态角(p，r，y)；

(3)比力分解：将加速度计输出的比力观测值f^b由b系转换到L系：

$f^L=R_b^L\left(k\right)\·f^b---\left(9\right)$

(4)加速度的修正：对f^L进行哥氏加速度修正和正常重力补偿，得到车辆在地球表面的加速度a：

$f^L-(2{\mathrm{\Ω}}_{\mathrm{ie}}^L+{\mathrm{\Ω}}_{\mathrm{eL}}^L)v^L+g^L---\left(10\right)$

式中，分别表示地球自转角速度和导航系的转移速率，两者都表达在L系中，v^L为车辆上一时刻的速度，g^L为重力加速度在L系中的分量；

(5)积分求取速度：利用加速度进行积分得到速度增量，进而得到k时刻的速度 $v_k^L:$

$v_k^L=v_{k-1}^L+a\·\mathrm{\Δt}---\left(11\right)$

式中，为k-1时刻的速度，Δt为时间间隔；

(6)积分求取位置：利用速度进行积分得到位置增量，进而得到k时刻的位置 $r_k^L:$

$r_k^L=r_{k-1}^L+\frac{1}{2}(v_k^L+v_{k-1}^L)\·\mathrm{\Δt}---\left(12\right)$

式中，为k-1时刻的位置。

4、卡尔曼滤波时间更新

采用扩展卡尔曼滤波，时间更新包括状态一步预测和预测均方误差的计算，实现过程如下：

设定卡尔曼滤波器的状态向量为

X=(δrL，δv^L，δε^L，d，b，δe)  (13)

式中，X为16阶误差向量，其中δr^L为SINS的三维位置误差，δv^L为SINS的三维速度误差，δε^L为SINS的平台失准角，d为三轴陀螺漂移，b为三轴加速度计零偏，δe为里程计的刻度因子误差。

根据误差向量建立SINS的状态方程，状态方程以INS的误差方程为基础，并加入里程计的刻度因子误差过程，其中INS的误差方程是根据其导航方程加一阶扰动推导而来，里程计刻度因子误差考虑成随机常数。状态方程具体为：

X_k=Φ_k，k-1X_k-1+Г_k，k-1W_k-1  (14)

式中，X_k-1和X_k分别表示SINS k-1和k时刻的状态，Φ_k，k-1为状态转移矩阵，W_k-1为动态噪声，Г_k，k-1为噪声输入矩阵；

根据状态方程计算SINS的状态一步预测值及相应的均方误差：

${\hat{X}}_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}\left(15\right)$

$P_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k,k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k,k-1}^T---\left(16\right)$

5、粗差识别

根据状态一步预测值，以及里程计和电子罗盘的观测值，计算滤波新息，并根据滤波新息判断是否存在粗差，若是，则剔除相应的观测值，若否，则保留相应的观测值。

根据滤波新息分析观测值的质量，识别出观测值粗差。新息的计算方式如下：

$\mathrm{In}=Z-H{\hat{X}}_{k,k-1}---\left(17\right)$

式中，In为新息，Z为观测值，H为观测矩阵。

本方法的观测值包括两部分：速度误差观测值和航向角误差观测值。

其中速度误差量测值为

$Z_v=V_{\mathrm{ins}}^L-V_{\mathrm{od}}^L---\left(18\right)$

式中为INS导航计算得到的车辆速度，为里程计测得的车辆速度，两者都表达在L系中。

$V_{\mathrm{od}}^L=R_b^L{V}_{\mathrm{od}}^b---\left(19\right)$

式中为里程计测得的车辆速度在载体坐标系中的表达。基于速度误差的量测方程为

Z_v=H_vX+V_v  (20)

速度观测矩阵H_v阵的具体形式如下

$H_v=\left(\underset{3\×3}{O}\underset{3\×3}{I}\underset{3\×3}{M}\underset{3\×6}{O}\underset{3\×1}{N}\right)---\left(21\right)$

式中O为零矩阵，I,M,N的形式如下：

$\left(\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right)---\left(22\right)$

$\left(\begin{array}{ccc}0& -v_{\mathrm{od}}^u& v_{\mathrm{od}}^n\\ v_{\mathrm{od}}^u& 0& -v_{\mathrm{od}}^e\\ -v_{\mathrm{od}}^n& v_{\mathrm{od}}^e& 0\end{array}\right)---\left(23\right)$

$V_{\mathrm{od}}^L---\left(24\right)$

航向角误差量测值为

Z_a=y_ins-y_mag  (25)

式中y_ins为INS导航计算得到的车辆航向角，y_mag为电子罗盘测得的车辆航向角。基于航向角误差的量测方程为

Z_a=H_aX+V_a  (26)

式中，H_a为航向角观测矩阵，

$H_a=\left(\begin{array}{ccccc}\underset{1\×6}{0}& \frac{\sin y\sin p}{\cos p}& -\frac{\cos y\sin p}{\cos p}& 1& \underset{1\×7}{0}\end{array}\right)---\left(27\right)$

因此新息也包括两部分：速度误差新息和航向角误差新息。通常，在车轮出现打滑的时候，卡尔曼滤波得到的速度新息会出现异常。更确切的讲，这时候里程计输出的速度会远远大于卡尔曼滤波预测的得到的车辆速度值，利用这一点可以设置相应的阈值判断是否出现车轮打滑。

当速度误差量测值Z_v满足以下函数关系时，认为车轮打滑，判断相应的观测值为粗差

${\left|\right|Z_v-H_v{\hat{X}}_{k,k-1}\left|\right|}_2>i\·{\left|\right|V_{\mathrm{ins}}^L\left|\right|}_2+j---\left(29\right)$

式中，||||₂表示2范数，i为大于0的比例系数，j为大于0的常数。i、j的取值根据多次实验来确定经验值。

当出现磁场干扰的时候，卡尔曼滤波得到的航向新息会出现异常。更确切的讲，这时候经卡尔曼滤波预测得到的航向角与电子罗盘输出的航向角之间会出现较大的差异，且一定时间间隔内航向的变化值也会有较大差异。因此用以下条件判断电子罗盘观测值的质量：

$\left\{\begin{array}{c}|Z_a-H_a{\hat{X}}_{k,k-1}|>b_1\\ |\mathrm{\δ}{y}_{\mathrm{ins}}-\mathrm{\δ}{y}_{\mathrm{mag}}|>b_2\end{array}\right.---\left(29\right)$

式中，δy_ins是一定时间间隔内INS导航计算得到的航向角变化值，δY_mag是一定时间间隔内电子罗盘输出的航向角变化值，b₁、b₂为设定的阈值。

6、卡尔曼滤波量测更新

将剔除粗差后的观测值作为卡尔漫滤波器的输入进行滤波量测更新，获得状态最优估计值及相应的均方误差。过程如下：

(1)计算滤波增益矩阵

$K_k=P_{k,k-1}{H}_k^T{(H_k{P}_{k,k-1}{H}_k^T+R_k)}^{-1}---\left(30\right)$

式中，K_k为滤波增益矩阵，P_k，k-1为系统一步预测的均方误差，H_k为系统量测矩阵，R_k为量测噪声矩阵。

(2)计算状态最优估计值

${\hat{X}}_k={\hat{X}}_{k,k-1}+K_k(Z_k-H_k{\hat{X}}_{k,k-1})---\left(31\right)$

(3)计算估计量的均方误差

$P_k=(I-K_k{H}_k)P_{k,k-1}{(I-K_k{H}_k)}^T+K_k{R}_k{K}_k^T---\left(32\right)$

7、反馈校正

利用状态最优估计值进行反馈校正，获得修正后的车辆位置、速度和姿态矩阵。其中，位置、速度、姿态直接通过以下公式修正：

$\left\{\begin{array}{c}{\hat{r}}^L=r^L-{\mathrm{\δr}}^L\\ {\hat{v}}^L=v^L-{\mathrm{\δv}}^L\\ {\hat{R}}_b^L=R_b^L(I-({\mathrm{\δ\ϵ}}^L\×))\end{array}\right.$

式中，分别为修正后的位置、速度和姿态矩阵，是INS导航计算得到的位置、速度和姿态矩阵，(δε^L×)为三维反对称阵；

里程计的刻度因子通过下式进行修正：

$\hat{e}=e+\mathrm{\δe}$

式中，是经过修正后的里程计刻度因子，e是修正前的值；

陀螺漂移和加速度计零偏反馈到下一个周期的INS导航计算中。

8、实验过程与结果

基于本发明提出的组合导航方法，用C++实现了算法代码，进行了多次实验和数据分析。通过多次实验发现：几乎在每次实验中都有磁场干扰情况的发生，但车轮打滑的现象不是常有的，比如在室内环境中就很少出现打滑的现象，只有在环境比较恶劣的地下环境容易出现车轮打滑。以下介绍一个比较有代表性的实验，实验所采用的车辆上装有IMU、里程计和电子罗盘。实验地点位于北京某高校的一个地下实验场地，在实验前已经完成了里程计和电子罗盘的检校。系统利用已知参考点进行位置初始化，让车辆静止5分钟，利用加速度计与电子罗盘的观测数据完成姿态的初始化。初始化完成之后，车辆在实验区采集IMU数据、里程计数据和电子罗盘数据。IMU采用光纤陀螺和MEMS加速度计，安装于车厢底部，其陀螺漂移为0.007°／s，加速度计零偏为10mg，输出频率为100Hz，里程计采用光栅编码器，安装于车轮轴承，车轮转动一圈对应200个脉冲信号，输出频率为10Hz，电子罗盘型号为HMR3000，标称精度为0.5°，输出频率为10Hz。组合系统的卡尔曼滤波周期为ls。

实验车辆使用遥控器控制，设计的最大行驶速度为1.5m／s，整个实验过程中车辆行驶轨迹为一个近似矩形的闭合圈，实验时长约26min，车辆累计行驶约1600m。

图3描绘了里程计输出的车辆前进速度值，是由脉冲数据经转换后得出的。从图中可以看出，车辆运行速度约为1m／s，且比较均匀，由于车辆设计时所限定的最大运行速度是1.5m／s，显然图4中明显大于1.5m／s的速度值尖峰是由车轮打滑导致的。

图4为电子罗盘输出的车辆航向角(电子罗盘原始输出为磁航向角，经磁偏角补偿后，得到真实航向角)。由于车辆运行一个近似矩形闭合圈，在实验过程中，只在矩形的角点处转弯，显然图5中有一些磁场干扰的情况，导致电子罗盘输出的航向角与车辆前进方向出现明显差异。

图5表示卡尔曼滤波的速度新息的2范数。从图中可以看出，速度新息一般在±0.3m／s以内，说明滤波是很平稳的，个别情况下出现速度新息大于1m／s的情况，结合图4可以看出，这主要是由于里程计输出的速度异常导致的。

图6为卡尔曼滤波的航向角新息。从图中可以看出，航向新息一般在±10°以内，说明滤波是很平稳的，个别情况下出现航向新息很大的情况，结合图5可以看出，这主要是由于电子罗盘输出的航向角异常导致的。

从图3-6可以看出，速度观测值异常和航向角观测值异常都可以通过卡尔曼滤波新息反应出来，因此通过对新息的分析，识别出粗差观测值并将其剔除，可以提高组合系统的精度和稳定性。

图7画出了不同方法计算得到的车辆运行轨迹。其中参考轨迹是利用全站仪测量确定的，车辆运行过程中在地面留下痕迹，并放置标志物，事后通过全站仪测量。抗粗差组合的轨迹即采用本发明所提出的方法计算出的轨迹，经过约1600m的运行后，闭合差为17.8m，相对精度为1.1％。常规组合的轨迹即采用卡尔曼滤波但不顾及粗差识别所得出的轨迹，经过约1600m的运行后，闭合差为89.5m，相对精度为5.6％。由此可见，本发明方法能显著提高车辆导航的精度和稳定性。

Claims (9)

1.一种无GPS信号环境下的抗粗差组合导航方法，其特征在于，该方法通过SINS、里程计和电子罗盘的组合对在无GPS信号环境下运作的车辆进行导航，具体包括以下步骤：

1)SINS初始化，获得车辆的初始位置和初始姿态矩阵；

2)根据IMU、里程计、电子罗盘的实时测量数据及步骤1)中的初始位置和初始姿态矩阵，进行INS导航计算，获得车辆当前时刻的速度、位置和姿态；

3)卡尔曼滤波时间更新，得到系统的状态一步预测值及相应的均方误差；

4)根据步骤3)中系统的状态一步预测值，以及里程计和电子罗盘的观测值，计算滤波新息，并根据滤波新息判断是否存在粗差，若是，则剔除相应的观测值，若否，则保留相应的观测值；

5)将剔除粗差后的观测值作为卡尔漫滤波器的输入进行滤波量测更新，获得状态最优估计值及相应的均方误差；

6)利用状态最优估计值进行反馈校正，获得修正后的车辆位置、速度和姿态矩阵。

2.根据权利要求1所述的一种无GPS信号环境下的抗粗差组合导航方法，其特征在于，所述的车辆的初始位置根据环境中的已知参考点获得或通过全站仪测量得到。

3.根据权利要求1所述的一种无GPS信号环境下的抗粗差组合导航方法，其特征在于，所述的车辆的初始姿态矩阵为：

$R_b^L=\left(\begin{array}{ccc}\cos r\cos y-\sin r\sin p& -\sin y\cos p& \sin r\cos y+\cos r\sin y\sin p\\ \cos r\sin y+\sin r\cos y\sin p& \cos y\cos p& \sin r\sin y-\cos r\cos y\sin p\\ -\sin r\cos p& \sin p& \cos r\cos p\end{array}\right)$

式中，车辆坐标系为b系，SINS的导航坐标系选择当地水平坐标系，为L系，y为电子罗盘观测得的航向角，p、r为计算得的车辆俯仰角和翻滚角，且

$\left\{\begin{array}{c}p=\arcsin \left[\frac{g_y^b}{g}\right]\\ r=\arctan \left[\frac{g_x^b}{g_z^b}\right]\end{array}\right.$

其中，g为重力加速度，为车辆静止设定时间内，SINS中的加速度计观测得的比力向量均值。

4.根据权利要求3所述的一种无GPS信号环境下的抗粗差组合导航方法，其特征在于，所述的INS导航计算具体包括以下步骤：

201)姿态更新：利用SINS中的陀螺输出的角速度观测值，采用四元数法进行姿态更新，然后将更新后的四元数转化成姿态矩阵，记为

202)姿态提取：根据更新后的姿态矩阵中元素的值，计算车辆的姿态角(p，r，y)；

203)比力分解：将加速度计输出的比力观测值f^b由b系转换到L系：

$f^L=R_b^L\left(k\right)\·f^b;$

204)加速度的修正：对f^L进行哥氏加速度修正和正常重力补偿，得到车辆在地球表面的加速度a：

$f^L-(2{\mathrm{\Ω}}_{\mathrm{ie}}^L+{\mathrm{\Ω}}_{\mathrm{eL}}^L)v^L+g^L$

式中，分别表示地球自转角速度和导航系的转移速率，两者都表达在L系中，v^L为车辆上一时刻的速度，g^L为重力加速度在L系中的分量；

205)积分求取速度：利用加速度进行积分得到速度增量，进而得到k时刻的速度 $v_k^L:$

$v_k^L=v_{k-1}^L+a\·\mathrm{\Δt}$

式中，为k-1时刻的速度，Δt为时间间隔；

206)积分求取位置：利用速度进行积分得到位置增量，进而得到k时刻的位置 $r_k^L:$

$r_k^L=r_{k-1}^L+\frac{1}{2}(v_k^L+v_{k-1}^L)\·\mathrm{\Δt}$

式中，为k-1时刻的位置。

5.根据权利要求4所述的一种无GPS信号环境下的抗粗差组合导航方法，其特征在于，所述的卡尔曼滤波时间更新具体包括以下步骤：

301)设定卡尔曼滤波器的状态向量为

X=(δr^L，δv^L，δεL，d，b，δe)

式中，X为16阶误差向量，其中δr^L为SINS的三维位置误差，δv^L为SINS的三维速度误差，δε^L为SINS的平台失准角，d为三轴陀螺漂移，b为三轴加速度计零偏，δe为里程计的刻度因子误差；

302)根据误差向量建立SINS的状态方程：

X_k=Φ_k，k-1X_k-1+Г_k，k-1W_k-1

式中，X_k-1和X_k分别表示SINSk-1和k时刻的状态，Φ_k，k-1为状态转移矩阵，W_k-1为动态噪声，Г_k，k-1为噪声输入矩阵：

303)根据状态方程计算SINS的状态一步预测值及相应的均方误差：

$\begin{array}{c}{\hat{X}}_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}\\ P_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k,k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k,k-1}^T.\end{array}$

6.根据权利要求5所述的一种无GPS信号环境下的抗粗差组合导航方法，其特征在于，所述的是否存在粗差具体指：

当速度误差量测值Z_v满足以下关系时，判断相应的观测值为粗差：

${\left|\right|Z_v-H_v{\hat{X}}_{k,k-1}\left|\right|}_2>i\·{\left|\right|V_{\mathrm{ins}}^L\left|\right|}_2+j$

式中，为INS导航计算得到的车辆速度，为里程计在L系中测得的车辆速度，||||₂表示2范数，H_v为速度观测矩阵，i为大于0的比例系数，j为大于0的常数；

当航向角误差量测值Z_a满足以下关系时，判断相应的观测值为粗差：

$\left\{\begin{array}{c}|Z_a-H_a{\hat{X}}_{k,k-1}|>b_1\\ |\mathrm{\δ}{y}_{\mathrm{ins}}-\mathrm{\δ}{y}_{\mathrm{mag}}|>b_2\end{array}\right.$

式中，Z_a=y_ins-y_mag，y_ins为INS导航计算得到的车辆航向角，y_mag为电子罗盘测得的车辆航向角，H_a为航向角观测矩阵，δy_ins是一定时间间隔内INS导航计算得到的航向角变化值，δy_mag是一定时间间隔内电子罗盘输出的航向角变化值，b₁、b₂为设定的阈值。

7.根据权利要求5所述的一种无GPS信号环境下的抗粗差组合导航方法，其特征在于，所述的步骤6)中的反馈校正具体为：

位置、速度、姿态直接通过以下公式修正：

$\left\{\begin{array}{c}{\hat{r}}^L=r^L-{\mathrm{\δr}}^L\\ {\hat{v}}^L=v^L-{\mathrm{\δv}}^L\\ {\hat{R}}_b^L=R_b^L(I-({\mathrm{\δ\ϵ}}^L\×))\end{array}\right.$

式中，分别为修正后的位置、速度和姿态矩阵，是INS导航计算得到的位置、速度和姿态矩阵，(δε^L×)为三维反对称阵；

里程计的刻度因子通过下式进行修正：

$\hat{e}=e+\mathrm{\δe}$

式中，是经过修正后的里程计刻度因子，e是修正前的值；

陀螺漂移和加速度计零偏反馈到下一个周期的INS导航计算中。

8.根据权利要求1所述的一种无GPS信号环境下的抗粗差组合导航方法，其特征在于，该方法还包括：在导航开始之前的检校步骤，该检校步骤包括里程计刻度因子检校和电子罗盘检校。

9.根据权利要求8所述的一种无GPS信号环境下的抗粗差组合导航方法，其特征在于，所述的里程计刻度因子e的检校公式为：

$e_i=\frac{l}{m_i}(i=\mathrm{1,2},...,n)$

$\frac{1}{n}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{e}_i$

l为设定的地面直线的长度，m为安装有里程计的车辆沿地面直线运行时输出的脉冲数，n为实验次数。