CN101476894A - 车载sins/gps组合导航系统性能增强方法 - Google Patents

Abstract

车载SINS/GPS组合导航系统性能增强方法。本发明涉及导航技术领域，它解决了现有车载SINS/GPS组合导航系统所存在的GPS暂时失效时系统精度低与可靠性差的问题，步骤如下：首先判断GPS是否有效；有效，利用GPS提供的位置、速度信息与SINS的位置、速度信息之差作为观测量，通过卡尔曼滤波方法估计SINS的误差，校正；无效，判断是否停车；停车，利用零速校正辅助SINS，校正；行进，利用SINS的姿态角计算导航坐标系至车体坐标系的坐标变换矩阵C_n ^b，利用C_n ^b将导航坐标系下的速度转换为车体坐标系下的速度，利用速度约束构造车辆运动约束量测方程；根据车辆运动情况简化该方程；利用车辆运动约束辅助SINS，将SINS与车辆运动约束进行速度组合，校正。它用于提高车载SINS/GPS组合导航系统的精度与可靠性。

Description

车载SINS/GPS组合导航系统性能增强方法

技术领域

本发明涉及导航技术领域，具体涉及一种车载SINS/GPS组合导航系统性能增强方法。

背景技术

捷联惯性导航系统(SINS，Strapdown Inertial Navigation System)，简称惯导，是一种完全自主的导航系统，它可以连续提供载体的位置、速度、方位和姿态等多种导航参数信息，能跟踪载体的快速机动运动，短时间内稳定性好。但惯导的误差随时间而累积，不能单独长时间工作，必须不断加以校准。

全球定位系统(GPS，Global Positioning System)是当前应用最为广泛的卫星导航定位系统，其定位和测速精度高，且长时间工作稳定性好，使用方便，成本低廉。

但GPS在许多特殊环境下(如城市环境、林荫道、隧道、立交桥下等区域)，常常存在多路径效应干扰、GPS卫星信号被遮挡或失锁的问题，可见卫星会少于4颗，GPS接收机无法定位或者定位精度很差，无法用于导航。

将多种导航系统组合形成组合导航系统是导航技术发展的一种必然趋势。目前应用较为广泛的是由GPS和SINS构成的组合导航系统，利用卡尔曼滤波技术进行导航信息估计，SINS/GPS组合导航系统能够充分发挥这两个系统的优点。

但是SINS/GPS组合导航系统长时间运行的精度仍然更多地依赖于GPS，当GPS受到干扰导致精度下降或GPS卫星信号不能覆盖时，由于SINS误差随时间积累且得不到校正，系统精度将会迅速降低。在GPS不可用时，利用其它方式得到的车辆导航信息辅助SINS是提高车载SINS/GPS运行精度的有效方法。如增加里程计，通过里程计的速度测量辅助SINS；或增加磁罗盘，利用磁罗盘的角度信息辅助SINS，但这些方法需要增加里程计或磁罗盘等硬件，需对原有SINS/GPS系统进行较大改动。地图匹配和零速校正只需通过改变原有SINS/GPS系统的软件模块，即可以用来辅助SINS提高车辆定位精度。但地图匹配需要高精度的道路电子地图，零速校正只在停车时才可用。车辆运动约束是车辆在路面上行驶时的固有约束，由于其不是完整性约束，无法单独使用，故多与里程计组合使用辅助SINS系统。

发明内容

本发明为了解决现有车载SINS/GPS组合导航系统所存在的GPS暂时失效时系统精度低与可靠性差的问题，而提出了一种车载SINS/GPS组合导航系统性能增强方法。

本发明的步骤如下：

步骤一：判断GPS是否有效；是，则进入步骤二，否，则进入步骤三；

步骤二：利用GPS提供的位置、速度信息与SINS的位置、速度信息之差作为观测量，通过卡尔曼滤波方法估计SINS的误差，对SINS的位置、速度和姿态角进行反馈校正，由SINS向用户输出导航信息；

步骤三：判断是否停车；是，则进入步骤四，否，则进入步骤五；

步骤四：利用零速校正辅助SINS，通过卡尔曼滤波方法估计SINS的误差，对SINS的速度和姿态角进行反馈校正，由SINS向用户输出导航信息；

步骤五：利用SINS的姿态角计算导航坐标系OX_nY_nZ_n至车体坐标系OX_bY_bZ_b的坐标变换矩阵，利用将导航坐标系OX_nY_nZ_n下的速度转换为车体坐标系OX_bY_bZ_b的速度，利用速度约束构造车辆运动约束量测方程；

步骤六：根据车辆运动情况简化步骤五中车辆运动约束量测方程；

步骤七：利用车辆运动约束辅助SINS，将SINS与车辆运动约束进行速度组合，经卡尔曼滤波方法估计SINS误差，对SINS的速度和姿态角进行反馈校正，由SINS向用户输出导航信息。

本发明可用于提高车载SINS/GPS组合导航系统的精度与可靠性，特别适用于车载SINS/GPS组合导航系统因GPS信号被遮挡等原因导致GPS暂时失效的情况。本发明与现有技术相比的优点在于：利用零速约束和车辆运动约束辅助车载SINS/GPS组合导航系统，无需任何外部输入，也无需增加任何额外传感器，仅通过对原有SINS/GPS系统增加相应软件模块即可实现，所以该方法可以在不增加原有SINS/GPS系统硬件成本情况下，提高原有SINS/GPS组合导航系统在GPS无效时的定位精度，是一种简单可行的方法。

附图说明

图1为本发明的流程图；图2为本发明的二维情况下的车辆运动约束示意图，图中的坐标N表示北方，坐标E表示东方；图3为SINS/GPS组合导航系统未采用和采用性能增强方法的经度位置误差对比曲线；图4为SINS/GPS组合导航系统未采用和采用性能增强方法的纬度位置误差对比曲线；图3和图4中的—为SINS/GPS组合导航系统未采用性能增强方法的位置误差曲线，—为SINS/GPS组合导航系统采用性能增强方法的位置误差曲线。

具体实施方式

具体实施方式一：结合图1说明本实施方式，本实施方式步骤如下：

步骤一：判断GPS是否有效；是，则进入步骤二，否，则进入步骤三；

步骤二：利用GPS提供的位置、速度信息与SINS的位置、速度信息之差作为观测量，通过卡尔曼滤波方法估计SINS的误差，对SINS的位置、速度和姿态角进行反馈校正，由SINS向用户输出导航信息；

步骤三：判断是否停车；是，则进入步骤四，否，则进入步骤五；

步骤四：利用零速校正辅助SINS，通过卡尔曼滤波方法估计SINS的误差，对SINS的速度和姿态角进行反馈校正，由SINS向用户输出导航信息；

步骤五：利用SINS的姿态角计算导航坐标系OX_nY_nZ_n至车体坐标系OX_bY_bZ_b的坐标变换矩阵

利用

将导航坐标系OX_nY_nz_n下的速度转换为车体坐标系OX_bY_bZ_b的速度，利用速度约束构造车辆运动约束量测方程；

步骤六：根据车辆运动情况简化步骤五中车辆运动约束量测方程；

步骤七：利用车辆运动约束辅助SINS，将SINS与车辆运动约束进行速度组合，经卡尔曼滤波方法估计SINS误差，对SINS的速度和姿态角进行反馈校正，由SINS向用户输出导航信息。

具体实施方式二：本实施方式与具体实施方式一的不同点在于步骤二为GPS有效时的导航过程；

首先，建立SINS/GPS组合导航的数学模型，包括系统方程和量测方程。

系统方程为忽略高度通道的SINS误差方程，如式(1)所示：

${\stackrel{.}{X}}_1\left(t\right)=F_1\left(t\right)X_1\left(t\right)+w_1\left(t\right)---\left(1\right)$

X₁为系统方程状态矢量，

其中

和为东向、北向和天向平台失准角，δV_E和δV_N为东向和北向速度误差，δφ和δλ为纬度和经度误差，ε_E、ε_N和ε_U为东向、北向和天向陀螺随机常值漂移，

和

为东向和北向加速度计零偏分量。F₁为系统转移矩阵，w₁为系统噪声矢量。

利用SINS系统给出的位置和速度信息与GPS系统相应的量测信息之差作为观测量，将观测误差归入观测噪声，可得量测方程为：

Z₁(t)＝H₁(t)X₁(t)+v₁(t)          (2)

式中，Z₁为观测矢量，z₁＝[φ_SINS-φ_GPS λ_SINS-λ_GPS V_ESINS-V_EGPS V_NSINS-V_NGPS]^T，其总φ表示纬度、λ表示经度、V_E表示东向速度、V_N表示北向速度，下标SINS、GPS分别表示是惯导和GPS的测量值。H₁为观测矩阵，v(t)为系统观测噪声。

观测矩阵H1具体如下：

$H_1\left(t\right)={\left[\begin{array}{ccc}{0}_{2\×5}& I_{2\×2}& 0_{2\×5}\\ 0_{2\×3}& I_{2\×2}& 0_{2\×7}\end{array}\right]}_{4\×12}---\left(3\right)$

上式中I表示单位阵。

其次，进行卡尔曼滤波编排，对于SINS/GPS组合导航系统，线性化和离散化后系统数学模型如下：

$\left\{\begin{array}{c}{X}_k={\mathrm{\Φ}}_{k,k-1}{X}_{k-1}+{\mathrm{\Γ}}_{k-1}{W}_{k-1}\\ Z_k=H_k{X}_k+V_k\end{array}\right.---\left(4\right)$

式中，X_k为系统状态向量，Φ_k，k-1为状态转移矩阵，z_k为量测向量，H_k为量测矩阵，Γ_k-1为系统噪声矩阵，W_k-1为系统噪声向量，V_k为量测噪声向量。W_k-1、V_k是不相关的高斯白噪声序列，均值为0、协方差阵分别为Q_k和R_k；

卡尔曼滤波方法可由下述方程描述：

时间更新方程：

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

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

量测更新方程：

$K_k=P_{k/k-1}{H}_k^T{(H_k{P}_{k/k-1}{H}_k^T+R)}^{-1}---\left(7\right)$

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

P_k＝(I-K_kH_k)P_k/k-1        (9)

这里

为对系统状态向量X_k的估计，P_k为状态估计误差的协方差阵，K_k为滤波增益。

最后，对SINS进行反馈校正，其中位置和速度可以直接校正，即：

φ_SINS＝φ_SINS-δφ

λ_SINS＝λ_SINS-δλ

V_ESINS＝V_ESINS-δV_E

V_NSINS＝V_NSINS-δV_N

姿态角校正利用估计的平台误差角间接校正，使用估计出的平台失准角

和

对原有姿态矩阵进行修正，得到校正后的姿态矩阵，再通过校正后的

计算俯仰角p、横滚角r和航向角y。设校正后的为：

$C_n^b=\left[\begin{array}{ccc}{C}_{11}& C_{12}& C_{13}\\ C_{21}& C_{22}& C_{23}\\ C_{31}& C_{32}& C_{33}\end{array}\right]$

则p、r和y计算如下：

p＝sin^-1(C₂₃)

$r={\tan }^{-1}(-\frac{C_{13}}{C_{33}})$

$y={\tan }^{-1}\left(\frac{C_{21}}{C_{22}}\right)$

其它步骤与具体实施方式一相同。

具体实施方式三：本实施方式与具体实施方式二不同点在于步骤四为GPS无效，并处于停车状态时的导航过程；

首先建立零速校正辅助SINS的数学模型，其中系统方程同(1)式，量测方程为：

Z₂＝H₂X₁+v₂             (10)

v₂为观测噪声，观测矩阵H₂为

H₂＝[0_2×3 I_2×2 0_2×7]_2×12         (11)

观测量Z₂为

Z₂＝[V_ESINS-V_E V_NSINS-V_N]^T＝[V_ESINS V_NSINS]^T     (12)

最终通过卡尔曼滤波方法估计SINS的误差，对SINS的速度和姿态角进行反馈校正。其它步骤与具体实施方式二相同。

具体实施方式四：本实施方式与具体实施方式一或三不同点在于步骤五为GPS无效，车辆处于运动状态时的导航过程；

导航坐标系OX_nY_nZ_n至车体坐标系OX_bY_bZ_b的坐标变换矩阵为：

$C_n^b=\left[\begin{array}{ccc}\cos r\cos y+\sin r\sin p\sin y& -\cos r\sin y+\sin p\sin r\cos y& -\cos p\sin r\\ \cos p\sin y& \cos p\cos y& \sin p\\ \sin r\cos y-\sin p\cos r\sin y& -\sin r\sin y-\sin p\cos r\cos y& \cos p\cos r\end{array}\right]---\left(13\right)$

其中，p为俯仰角，r为横滚角，y为航向角，由SINS输出。导航坐标系OX_nY_nZ_n定义为东北天坐标系，车体坐标系OX_bY_bZ_b的原点是车体后轮连线的中心o，Y_b轴沿车体纵轴指向前进方向，X_b轴沿车体横轴向右，Z_b轴垂直于X_b轴和Y_b轴，并构成右手直角坐标系。

构造车辆运动约束量测方程具体步骤为：

首先，通过全微分的方法得到车体坐标系下速度误差表示形式。设导航坐标系下速度表示为Vⁿ，车体坐标系下速度表示为V^b，则

$V^b=C_n^b{V}^n$

对其进行全微分，得

$\mathrm{\δ}{V}^b=C_n^b\mathrm{\δ}{V}^n+\mathrm{\δ}{C}_n^b{V}^n=C_n^b\mathrm{\δ}{V}^n+{\mathrm{EC}}_n^b{V}^n=C_n^b\mathrm{\δ}{V}^n+{\mathrm{EV}}^b$

上式中，考虑 $\mathrm{\δ}{C}_n^b={\mathrm{EC}}_n^b,$ E为平台失准角

的反对称阵，

则有

写为分量形式，为

$\left[\begin{array}{c}\mathrm{\δ}{V}_X^b\\ \mathrm{\δ}{V}_Y^b\\ \mathrm{\δ}{V}_Z^b\end{array}\right]=\left[\begin{array}{ccc}\cos r\cos y+\sin r\sin p\sin y& -\cos r\sin y+\sin p\sin r\cos y& -\cos p\sin r\\ \cos p\sin y& \cos p\cos y& \sin p\\ \sin r\cos y-\sin p\cos r\sin y& -\sin r\sin y-\sin p\cos r\cos y& \cos p\cos r\end{array}\right]\left[\begin{array}{c}\mathrm{\δ}{V}_E\\ \mathrm{\δ}{V}_N\\ \mathrm{\δ}{V}_U\end{array}\right]$

式中惯导的速度在体坐标系下得分量

由惯导输出速度(V_E，V_N，V_U)和姿态角(p，r，y)计算，公式如下：

$\left[\begin{array}{c}{V}_X^b\\ V_Y^b\\ V_Z^b\end{array}\right]=\left[\begin{array}{ccc}\cos r\cos y+\sin r\sin p\sin y& -\cos r\sin y+\sin p\sin r\cos y& -\cos p\sin r\\ \cos p\sin y& \cos p\cos y& \sin p\\ \sin r\cos y-\sin p\cos r\sin y& -\sin r\sin y-\sin p\cos r\cos y& \cos p\cos r\end{array}\right]\left[\begin{array}{c}{V}_E\\ V_N\\ V_U\end{array}\right]---\left(14\right)$

其次，利用车体横轴方向的速度约束构造量测方程。车辆平稳行驶时，在侧滑方向速度理论值为0，即车体坐标系X轴方向上速度理论值为0，此运动约束可看作一个横向速度虚拟传感器对X轴方向的速度测量。根据实际情况，设真实侧滑速度为

可考虑为零均值高斯白噪声v₃，即

$V_{X,\mathrm{Virtual}}^b=0=V_{X,\mathrm{Ture}}^b+v_3$

惯导速度测量在车体坐标系X轴上的分量为：

$V_{X,\mathrm{INS}}^b=(\cos r\cos y+\sin r\sin p\sin y)V_E+(-\cos r\sin y+\sin p\sin r\cos y)V_N$

$-\left(\cos p\sin r\right)V_U$

设惯导速度测量在车体坐标系X轴上的分量的误差为

可得

$V_{X,\mathrm{INS}}^b=V_{X,\mathrm{Ture}}^b+\mathrm{\δ}{V}_X^b=V_{X,\mathrm{Ture}}^b+$

$\left[\begin{array}{ccc}\cos r\cos y+\sin r\sin p\sin y& -\cos r\sin y+\sin p\sin r\cos y& -\cos p\sin r\end{array}\right]\left[\begin{array}{c}\mathrm{\δ}{V}_E\\ \mathrm{\δ}{V}_N\\ \mathrm{\δ}{V}_U\end{array}\right]$

则惯导速度测量在车体坐标系X轴上的分量与横向速度虚拟传感器测量速度之差为：

$V_{X,\mathrm{INS}}^b-V_{X,\mathrm{Virtual}}^b=V_{X,\mathrm{INS}}^b=\mathrm{\δ}{V}_X^b-v_3$

忽略惯导的高度通道，可以构造车辆横向速度约束辅助惯导系统的量测方程为：

$Z_3=\left[\begin{array}{cc}\cos r\cos y+\sin r\sin p\sin y& -\cos r\sin y+\sin p\sin r\cos y\end{array}\right]\left[\begin{array}{c}\mathrm{\δ}{V}_E\\ \mathrm{\δ}{V}_N\end{array}\right]$

整理为

观测矩阵H₃为：

$H_3=\left[\begin{array}{ccccc}0& V_Z^b& -V_Y^b& \cos r\cos y+\sin r\sin p\sin y& -\cos r\sin y+\sin p\sin r\cos y\end{array}\right]$

观测量Z₃为：

Z₃＝(cos r cos y+sin r sin p siny)V_E+(-cos r sin y+sin p sin r cos y)V_N       (16)

其中，V_E和V_N为导航坐标系下的速度，为SINS的输出量；

和为车体坐标系下Y轴和Z轴的速度；v_x为观测噪声。其它步骤与具体实施方式一或三相同。

具体实施方式五：结合图2说明本实施方式，本实施方式与具体实施方式四不同点在于进入步骤六根据车辆运动情况，对步骤五中的车辆运动约束方程进行简化，

当车辆行驶道路较好，惯导测量的俯仰角和横滚角较小的情况下，可将系统考虑为二维情况，此时惯导的速度在体坐标系下得分量为：

$V_X^b=V_E\cos y-V_N\sin y$

$V_Y^b=V_E\sin y+V_N\cos y$                (17)

车辆运动约束量测方程为：

上式中设X₂为

观测矩阵H₃简化为

$H_3=\left[\begin{array}{ccccc}0& 0& V_Y^b& \cos y& -\sin y\end{array}\right]$

观测量Z₃为

Z₃＝V_E cos y-V_N sin y              (19)

由式(17)计算得到，v₃为观测噪声。其它步骤与具体实施方式四相同。

具体实施方式六：本实施方式与具体实施方式五不同点在于进入步骤七；

首先建立车辆运动约束辅助SINS的数学模型，包括系统方程和量测方程。

系统方程为简化的SINS误差方程：

${\stackrel{.}{X}}_2\left(t\right)=F_2\left(t\right)X_2\left(t\right)+w_2\left(t\right)---\left(20\right)$

误差方程状态矢量为其中

和为东向、北向和天向平台失准角，δV_E和δV_N为东向和北向速度误差。F₂为系统转移矩阵，w₂为系统噪声矢量。量测方程为式(15)或式(18)，观测量由式(16)或式(19)计算。

最后经卡尔曼滤波方法估计SINS误差，对SINS的速度和姿态角进行反馈校正。其它步骤与具体实施方式五相同。

本发明内容不仅限于上述各实施方式的内容，其中一个或几个具体实施方式的组合同样也可以实现发明的目的。

本发明的原理是：GPS可用时，将SINS与GPS进行位置、速度组合，经卡尔曼滤波估计出SINS的位置、速度和姿态误差，对SINS进行反馈校正。当GPS由于被遮挡等原因导致不可用时，把零速约束和车辆运动约束相结合辅助SINS/GPS组合导航系统。若车辆处于停车状态，则可以直接使用零速约束辅助SINS/GPS组合导航系统，并对SINS进行反馈校正；否则，若车辆正常行进，则理想情况下侧滑为零，即车辆在其横轴方向上的速度等于零，将SINS输出的东向和北向速度经坐标变换，转换至车辆横轴方向，与该约束条件构成观测量，辅助SINS/GPS组合导航系统，并对SINS进行反馈校正。车辆的位置、速度和姿态信息由校正后的SINS输出。

为检验车载SINS/GPS组合导航系统性能增强方法的有效性，尤其验证GPS暂时失效时系统的精度，进行了仿真验证。仿真中GPS精度10米，输出频率1Hz；SINS中陀螺常值漂移1°/h，随机漂移0.1°/h，加速度计零偏10^-4g，SINS输出频率50Hz。

仿真轨迹包括匀速、加速、转弯、停车等多个动作。假设在600秒到1200秒、1800秒到2400秒、3000秒到3600秒三个时间段GPS失效。图3和图4中直线内的部分表示此时间段内GPS失效，从图中可以看出，在GPS暂时失效时，SINS/GPS性能增强方法可以减缓系统位置误差的发散，提高了原有SINS/GPS组合导航系统的定位精度。

Claims (6)

1、车载SINS/GPS组合导航系统性能增强方法，其特征在于它的步骤如下：

步骤一：判断GPS是否有效；是，则进入步骤二，否，则进入步骤三；

步骤二：利用GPS提供的位置、速度信息与SINS的位置、速度信息之差作为观测量，通过卡尔曼滤波方法估计SINS的误差，对SINS的位置、速度和姿态角进行反馈校正，由SINS向用户输出导航信息；

步骤三：判断是否停车；是，则进入步骤四，否，则进入步骤五；

步骤四：利用零速校正辅助SINS，通过卡尔曼滤波方法估计SINS的误差，对SINS的速度和姿态角进行反馈校正，由SINS向用户输出导航信息；

步骤五：利用SINS的姿态角计算导航坐标系OX_nY_nZ_n至车体坐标系OX_bY_bZ_b的坐标变换矩阵

利用将导航坐标系OX_nY_nZ_n下的速度转换为车体坐标系OX_bY_bZ_b的速度，利用速度约束构造车辆运动约束量测方程；

步骤六：根据车辆运动情况简化步骤五中车辆运动约束量测方程；

步骤七：利用车辆运动约束辅助SINS，将SINS与车辆运动约束进行速度组合，经卡尔曼滤波方法估计SINS误差，对SINS的速度和姿态角进行反馈校正，由SINS向用户输出导航信息。

2、根据权利要求1所述的车载SINS/GPS组合导航系统性能增强方法，其特征在于步骤二；

首先，建立SINS/GPS组合导航的数学模型，包括系统方程和量测方程；

系统方程为忽略高度通道的SINS误差方程，如式(1)所示：

${\stackrel{.}{X}}_1\left(t\right)=F_1\left(t\right)X_1\left(t\right)+w_1\left(t\right)---\left(1\right)$

X₁为系统方程状态矢量，

其中

和

为东向、北向和天向平台失准角，δV_E和δV_N为东向和北向速度误差，δφ和δλ为纬度和经度误差，ε_E、ε_N和ε_U为东向、北向和天向陀螺随机常值漂移，和

为东向和北向加速度计零偏分量；F₁为系统转移矩阵，w₁为系统噪声矢量；

利用SINS系统给出的位置和速度信息与GPS系统相应的量测信息之差作为观测量，将观测误差归入观测噪声，可得量测方程为：

Z₁(t)＝H₁(t)X₁(t)+v₁(t)               (2)

式中，Z₁为观测矢量，Z₁＝[φ_SINS-φ_GPS λ_SINS-λ_GPS V_ESINS-V_EGPS V_NSINS-V_NGPS]^T，其总φ表示纬度、λ表示经度、V_E表示东向速度、V_N表示北向速度，下标SINS、GPS分别表示是惯导和GPS的测量值；H₁为观测矩阵，v(t)为系统观测噪声；

观测矩阵H₁具体如下：

$H_1\left(t\right)={\left[\begin{array}{ccc}{0}_{2\×5}& I_{2\×2}& 0_{2\×5}\\ 0_{2\×3}& I_{2\×2}& 0_{2\×7}\end{array}\right]}_{4\×12}---\left(2\right)$

上式中I表示单位阵；

其次，进行卡尔曼滤波编排，对于SINS/GPS组合导航系统，线性化和离散化后系统数学模型如下：

$\left\{\begin{array}{c}{X}_k={\mathrm{\Φ}}_{k,k-1}{X}_{k-1}+{\mathrm{\Γ}}_{k-1}{W}_{k-1}\\ Z_k=H_k{X}_k+V_k\end{array}\right.---\left(4\right)$

式中，X_k为系统状态向量，Φ_k，k-1为状态转移矩阵，Z_k为量测向量，H_k为量测矩阵，Γ_k-1为系统噪声矩阵，W_k-1为系统噪声向量，V_k为量测噪声向量；W_k-1、V_k是不相关的高斯白噪声序列，均值为0、协方差阵分别为Q_k和R_k；

卡尔曼滤波方法可由下述方程描述：

时间更新方程：

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

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

量测更新方程：

$K_k=P_{k/k-1}{H}_k^T{(H_k{P}_{k/k-1}{H}_k^T+R)}^{-1}---\left(7\right)$

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

P_k＝(I-K_kH_k)P_k/k-1        (9)

这里

为对系统状态向量X_k的估计，P_k为状态估计误差的协方差阵，K_k为滤波增益；

最后，对SINS进行反馈校正，其中位置和速度可以直接校正，即：

φ_SINS＝φ_SINS-δφ

λ_SINS＝λ_SINS-δλ

V_ESINS＝V_ESINS-δV_E

V_NSINS＝V_NSINS-δV_N

姿态角校正利用估计的平台误差角间接校正，使用估计出的平台失准角

和

对原有姿态矩阵进行修正，得到校正后的姿态矩阵，再通过校正后的

计算俯仰角p、横滚角r和航向角y；设校正后的

为：

$C_n^b=\left[\begin{array}{ccc}{C}_{11}& C_{12}& C_{13}\\ C_{21}& C_{22}& C_{23}\\ C_{31}& C_{32}& C_{33}\end{array}\right]$

则p、r和y计算如下：

p＝sin^-1(C₂₃)

$r={\tan }^{-1}(-\frac{C_{13}}{C_{33}})$

$y={\tan }^{-1}\left(\frac{C_{21}}{C_{22}}\right).$

3、根据权利要求2所述的车载SINS/GPS组合导航系统性能增强方法，其特征在于步骤四，

首先建立零速校正辅助SINS的数学模型，其中系统方程同(1)式，量测方程为：

Z₂＝H₂X₁+v₂            (10)

v₂为观测噪声，观测矩阵H₂为

H₂＝[0_2×3 I_2×2 0_2×7]_2×12     (11)

观测量Z₂为

Z₂＝[V_ESINS-V_E V_NSINS-V_N]^T＝[V_ESINS V_NSINS]^T           (12)

最终通过卡尔曼滤波方法估计SINS的误差，对SINS的速度和姿态角进行反馈校正。

4、根据权利要求1或3所述的车载SINS/GPS组合导航系统性能增强方法，其特征在于步骤五，

导航坐标系OX_nY_nZ_n至车体坐标系OX_bY_bZ_b的坐标变换矩阵

为：

$C_n^b=\left[\begin{array}{ccc}\cos r\cos y+\sin r\sin p\sin y& -\cos r\sin y+\sin p\sin r\cos y& -\cos p\sin r\\ \cos p\sin y& \cos p\cos y& \sin p\\ \sin r\cos y-\sin p\cos r\sin y& -\sin r\sin y-\sin p\cos r\cos y& \cos p\cos r\end{array}\right]---\left(13\right)$

其中，p为俯仰角，r为横滚角，y为航向角，由SINS输出。导航坐标系OX_nY_nZ_n定义为东北天坐标系，车体坐标系OX_bY_bZ_b的原点是车体后轮连线的中心o，Y_b轴沿车体纵轴指向前进方向，X_b轴沿车体横轴向右，Z_b轴垂直于X_b轴和Y_b轴，并构成右手直角坐标系。

构造车辆运动约束量测方程具体步骤为：

首先，通过全微分的方法得到车体坐标系下速度误差表示形式。设导航坐标系下速度表示为Vⁿ，车体坐标系下速度表示为V^b，则

$V^b=C_n^b{V}^n$

对其进行全微分，得

$\mathrm{\δ}{V}^b=C_n^b\mathrm{\δ}{V}^n+\mathrm{\δ}{C}_n^b{V}^n=C_n^b\mathrm{\δ}{V}^n+{\mathrm{EC}}_n^b{V}^n=C_n^b\mathrm{\δ}{V}^n+{\mathrm{EV}}^b$

上式中，考虑 $\mathrm{\δ}{C}_n^b={\mathrm{EC}}_n^b,$ E为平台失准角的反对称阵，

则有

写为分量形式，为

$\left[\begin{array}{c}\mathrm{\δ}{V}_X^b\\ \mathrm{\δ}{V}_Y^b\\ \mathrm{\δ}{V}_Z^b\end{array}\right]=\left[\begin{array}{ccc}\cos r\cos y+\sin r\sin p\sin y& -\cos r\sin y+\sin p\sin r\cos y& -\cos p\sin r\\ \cos p\sin y& \cos p\cos y& \sin p\\ \sin r\cos y-\sin p\cos r\sin y& -\sin r\sin y-\sin p\cos r\cos y& \cos p\cos r\end{array}\right]\left[\begin{array}{c}\mathrm{\δ}{V}_E\\ \mathrm{\δ}{V}_N\\ \mathrm{\δ}{V}_U\end{array}\right]$

式中惯导的速度在体坐标系下得分量由惯导输出速度(V_E，V_N，V_U)和姿态角(p，r，y)计算，公式如下：

$\left[\begin{array}{c}{V}_X^b\\ V_Y^b\\ V_Z^b\end{array}\right]=\left[\begin{array}{ccc}\cos r\cos y+\sin r\sin p\sin y& -\cos r\sin y+\sin p\sin r\cos y& -\cos p\sin r\\ \cos p\sin y& \cos p\cos y& \sin p\\ \sin r\cos y-\sin p\cos r\sin y& -\sin r\sin y-\sin p\cos r\cos y& \cos p\cos r\end{array}\right]\left[\begin{array}{c}{V}_E\\ V_N\\ V_U\end{array}\right]---\left(14\right)$

其次，利用车体横轴方向的速度约束构造量测方程。车辆平稳行驶时，在侧滑方向速度理论值为0，即车体坐标系X轴方向上速度理论值为0，此运动约束可看作一个横向速度虚拟传感器对X轴方向的速度测量。根据实际情况，设真实侧滑速度为

可考虑为零均值高斯白噪声v₃，即

$V_{X,\mathrm{Virtual}}^b=0=V_{X,\mathrm{Ture}}^b+v_3$

惯导速度测量在车体坐标系X轴上的分量为：

$V_{X,\mathrm{INS}}^b=(\cos r\cos y+\sin r\sin p\sin y)V_E+(-\cos r\sin y+\sin p\sin r\cos y)V_N$

    $-\left(\cos p\sin r\right)V_U$

设惯导速度测量在车体坐标系X轴上的分量的误差为

可得

$V_{X,\mathrm{INS}}^b=V_{X,\mathrm{Ture}}^b+\mathrm{\δ}{V}_X^b=V_{X,\mathrm{Ture}}^b+$

$\left[\begin{array}{ccc}\cos r\cos y+\sin r\sin p\sin y& -\cos r\sin y+\sin p\sin r\cos y& -\cos p\sin r\end{array}\right]\left[\begin{array}{c}\mathrm{\δ}{V}_E\\ \mathrm{\δ}{V}_N\\ \mathrm{\δ}{V}_U\end{array}\right]$

则惯导速度测量在车体坐标系X轴上的分量与横向速度虚拟传感器测量速度之差为：

$V_{X,\mathrm{INS}}^b-V_{X,\mathrm{Virtual}}^b=V_{X,\mathrm{INS}}^b=\mathrm{\δ}{V}_X^b-v_3$

忽略惯导的高度通道，可以构造车辆横向速度约束辅助惯导系统的量测方程为：

$Z_3=\left[\begin{array}{cc}\cos r\cos y+\sin r\sin p\sin y& -\cos r\sin y+\sin p\sin r\cos y\end{array}\right]\left[\begin{array}{c}\mathrm{\δ}{V}_E\\ \mathrm{\δ}{V}_N\end{array}\right]$

整理为

观测矩阵H₃为：

$H_3=\left[\begin{array}{ccccc}0& V_Z^b& -V_Y^b& \cos r\cos y+\sin r\sin p\sin y& -\cos r\sin y+\sin p\sin r\cos y\end{array}\right]$

观测量Z₃为：

Z₃＝(cosrcosy+sinrsinpsiny)V_E+(-cosrsiny+sinpsinrcosy)V_N     (16)

其中，V_E和V_N为导航坐标系下的速度，为SINS的输出量；和

为车体坐标系下Y轴和Z轴的速度；v_X为观测噪声。

5、根据权利要求4所述的车载SINS/GPS组合导航系统性能增强方法，其特征在于步骤六，对步骤五中的车辆运动约束方程进行简化，

当车辆行驶道路较好，惯导测量的俯仰角和横滚角较小的情况下，可将系统考虑为二维情况，此时惯导的速度在体坐标系下得分量为：

$V_X^b=V_E\cos y-V_N\sin y$

$V_Y^b=V_E\sin y+V_N\cos y$                                  (17)

车辆运动约束量测方程为：

上式中设X₂为

观测矩阵H₃简化为

$H_3=\left[\begin{array}{ccccc}0& 0& V_Y^b& \cos y& -\sin y\end{array}\right]$

观测量Z₃为

Z₃＝V_E cos y-V_N sin y                     (19)

由式(17)计算得到，v₃为观测噪声。

6、根据权利要求5所述的车载SINS/GPS组合导航系统性能增强方法，其特征在于步骤七，

首先建立车辆运动约束辅助SINS的数学模型，包括系统方程和量测方程。

系统方程为简化的SINS误差方程：

${\stackrel{.}{X}}_2\left(t\right)=F_2\left(t\right)X_2\left(t\right)+w_2\left(t\right)---\left(20\right)$

误差方程状态矢量为

其中

和

为东向、北向和天向平台失准角，δV_E和δV_N为东向和北向速度误差。F₂为系统转移矩阵，w₂为系统噪声矢量。量测方程为式(15)或式(18)，观测量由式(16)或式(19)计算。

最后经卡尔曼滤波方法估计SINS误差，对SINS的速度和姿态角进行反馈校正。

Images