CN104977002A - 基于sins/双od的惯性组合导航***及其导航方法 - Google Patents

Abstract

本发明提供一种基于SINS/双OD的惯性组合导航***及其导航方法，方法包括建立SINS误差模型；建立双OD误差模型；对各误差状态量进行估计并修正。通过在基于SINS/双OD的惯性组合导航***中设置双里程计，利用双路里程计的速度和方向角信息可以有效地修正惯导的输出，减小导航参数的误差，获得更精确的轨迹发生器的速度信息和方向角度；通过惯导和双里程计的融合，以惯导和里程计的导航参数的误差量作为估计对象运用卡尔曼滤波技术，可以得到精确可行的卡尔曼滤波算法，进而实现更高精度的导航。

Description

基于SINS/双OD的惯性组合导航***及其导航方法

技术领域

本发明涉及惯性导航领域，特别是涉及一种基于SINS/双OD的惯性组合导航***及其导航方法。

背景技术

惯性导航***通过安装在飞行器或者车体上的惯性测量元件(IMU)输出的旋转角速度和线加速度推算航位姿态、坐标以及速度等信息。IMU主要为陀螺仪和加速度计：陀螺仪测量载体旋转的角速度，用于计算载体的姿态变化，提供了载体和导航坐标系的变换关系；加速度计测量载体在惯导坐标系下运动的线加速度，并用于积分计算载体的速度和位置信息，再通过载体姿态信息变换到导航坐标系中。捷联惯导***(SINS)以其提供导航信息的全面性和完全的自主性，成为了一种广泛应用的惯性导航***，本文所涉及的车辆导航***也在其基础上建立。然而SINS存在随时间累积的导航误差严重影响了***的导航精度，因而控制SINS的误差累积成为捷联惯导***运行的一个关键问题。

利用SINS/GPS组合导航***虽然能够很好地解决误差累积问题，但在该***中，GPS存在着动态响应能力差、易受电子干扰、信号易被遮挡等缺点，使得SINS/GPS组合导航***在GPS信号出现故障时不能正常工作。而GPS信号如果在长时间内不能得倒恢复，***的误差就不可避免地随时间而积累。为了解决上述问题，以提高车辆导航的自主性、稳定性和可靠性为目的，将里程计(OD)和SINS组合起来，将各自收集到的信息融合并获得较高精度的导航数据，构成以SINS为主、里程计为辅的车辆组合导航***。里程计是测量车辆行驶速度和路程的，它不能单独用于车辆定位，但可从捷联惯导中获得姿态和航向信息，进行定位解算且累积误差较小。

目前，将SINS和OD进行组合的导航***多采用单里程计，利用其速度信息修正惯导的导航误差，较为有效地提高了导航精度。但是***采用速度组合方式，将里程计的速度测量值分解到导航坐标系后，与捷联惯导***的速度解算值进行比较，即OD只提供速度量测信息而忽略了方向角的误差，这种做法导致SINS/OD组合导航位置误差会随车辆行驶路程不断地缓慢累积，定位精度有缓慢发散的趋势，因而本文提出利用双路里程计来获得更为精确的速度信息和车的方向角度，从而更加有效地抑制惯导误差的积累，即利用双路里程计和惯导的融合，运用卡尔曼滤波技术，实现更高精度的导航。

发明内容

鉴于以上所述现有技术的缺点，本发明的目的在于提供一种基于SINS/双OD的惯性组合导航***及其导航方法，用于解决现有技术中的由于使用单个里程计与SINS进行组合形成组合导航***而导致的SINS/OD组合导航位置误差会随车辆行驶路程不断地缓慢累积，定位精度有缓慢发散的趋势的问题。

为实现上述目的及其他相关目的，本发明提供一种基于SINS/双OD的惯性组合导航***的导航方法，所述方法包括：

建立SINS误差模型，所述SINS误差模型包括SINS的姿态误差方程、速度误差方程及位置误差方程；

建立双OD误差模型，所述双OD误差模型包括双OD的速度误差方程及方向角误差方程；

对SINS误差模型及双OD误差模型的各误差状态量进行估计并修正。

作为本发明的基于SINS/双OD的惯性组合导航***的导航方法的一种优选方案，本方法的坐标系定义如下：

n系：导航坐标系，即为东-北-天地理坐标系；

b系：惯导坐标系，与轨迹发生器坐标系重合；

m系：里程计坐标系，与轨迹发生器坐标系平行；

其中，b系及m系的方向均为：x轴沿轨迹发生器横轴指向右侧，y轴沿轨迹发生器纵轴指向正前方，z轴垂直于x轴和y轴并构成右手直角坐标系。

作为本发明的基于SINS/双OD的惯性组合导航***的导航方法的一种优选方案，所述SINS的姿态误差方程为：

所述SINS的速度误差方程为：

所述SINS的位置误差方程为：

$\left\{\begin{array}{c}\mathrm{\δ}\stackrel{\·}{L}=\frac{{\mathrm{\δv}}_E^n}{R_M+h}-\frac{v_N^n\mathrm{\δ}h}{{(R_M+h)}^2}\\ \mathrm{\δ}\stackrel{\·}{\mathrm{\λ}}=\frac{({\mathrm{\δv}}_N^n\sec L+v_E^n\sec L\tan L)}{R_N+h}-\frac{v_E^n\sec L\mathrm{\δ}h}{{(R_N+h)}^2}\\ \mathrm{\δ}\stackrel{\·}{h}={\mathrm{\δv}}_U^n\end{array}\right.$

式中，为捷联惯导计算的东、北和天向的姿态误差角；为速度误差；δL、δλ、δh为位置误差，分别为纬度、经度和高度误差；为地球自转角速率在导航坐标系上的投影，为导航系相对地球坐标系的角速率在导航系中的投影；fⁿ为导航系中的比力；ε^b＝[ε_x ε_y ε_z]为轨迹发生器系中的陀螺漂移，为轨迹发生器体系中的加速度计零偏；为姿态矩阵，R_M和R_N分别为地球子午圈和卯酉圈半径。

作为本发明的基于SINS/双OD的惯性组合导航***的导航方法的一种优选方案，建立双OD误差模型的具体方法为：

使用双OD测得轨迹发生器在t时刻的位姿信息；

根据所述位姿信息得到轨迹发生器的速度及方向角；

根据所述轨迹发生器的速度及方向角，得到所述双OD的速度误差方程及方向角误差方程。

作为本发明的基于SINS/双OD的惯性组合导航***的导航方法的一种优选方案，所述双OD的速度误差方程为：

所述双OD的方向角误差方程为：

式中，为里程计应得到的轨迹发生器的真实速度在n系中的投影，为里程计量测得到的轨迹发生器的速度在n系中的投影，为姿态角误差，δk为里程计刻度系数误差，为里程计的方向角在n系中的投影，δθ为里程计方向角系数误差。

作为本发明的基于SINS/双OD的惯性组合导航***的导航方法的一种优选方案，对SINS误差模型及双OD误差模型的各误差状态量进行估计并修正的具体方法为：

以SINS的姿态误差、速度误差、位置误差、陀螺常值漂移及加速度计常值零偏作为SINS的状态变量，以刻度系数误差及方向角***误差作为双OD的状态变量建立状态方程；以SINS解算的数据与双OD测得的数据的差值作为量测量建立量测方程；所述状态方程及量测方程构成基于SINS/双OD的惯性组合导航***卡尔曼滤波模型；

根据所述状态方程及量测方程对各误差状态量进行滤波估计并修正。

作为本发明的基于SINS/双OD的惯性组合导航***的导航方法的一种优选方案，所述状态方程为：

$X\left(t\right)=F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)$

式中，X(t)为t时刻***的状态向量，F(t)为***矩阵，G(t)为***噪声转移矩阵，W(t)为***噪声；

t时刻***的状态向量为：

式中，为姿态误差，为东、北、天向速度误差，δL、δλ、δh为纬度、经度及高度方向的位置误差，ε_x、ε_y、ε_z为三个陀螺常值漂移， 为三个水平加速度计常值零偏，δk为里程计刻度系数误差，δθ为里程计方向角系数误差。

作为本发明的基于SINS/双OD的惯性组合导航***的导航方法的一种优选方案，所述量测方程为：

$Z_1\left(t\right)=v_{\sin s}^n-v_{od}^n=H_1\left(t\right)X\left(t\right)+v_1\left(t\right)$

$Z_2\left(t\right)={\mathrm{\θ}}_{\sin s}^n-{\mathrm{\θ}}_{od}^n=H_2\left(t\right)X\left(t\right)+v_2\left(t\right)$

式中，v₁ ^(t)、v₂ ^(t)为量测噪声矢量；H₁ ^(t)、H₂ ^(t)为t时刻的量测矩阵，H₁ ^(t)、H₂ ^(t)为：

$H_1\left(t\right)=\left[\begin{array}{ccccccccc}0& v_{odU}^n& -v_{odN}^n& 1& 0& 0& 0_{1\×9}& -v_{odE}^n& 0\\ -v_{odU}^n& 0& v_{odE}^n& 0& 1& 0& 0_{1\×9}& -v_{odN}^n& 0\\ v_{odN}^n& -v_{odE}^n& 0& 0& 0& 1& 0_{1\×9}& -v_{odU}^n& 0\end{array}\right]$

$H_2\left(t\right)=\left[\begin{array}{ccccc}1& {\mathrm{\θ}}_{odU}^n& -{\mathrm{\θ}}_{odN}^n& 0_{1\×13}& -{\mathrm{\θ}}_{odE}^n\\ -{\mathrm{\θ}}_{odU}^n& 1& {\mathrm{\θ}}_{odE}^n& 0_{1\×13}& -{\mathrm{\θ}}_{odN}^n\\ {\mathrm{\θ}}_{odN}^n& -{\mathrm{\θ}}_{odE}^n& 1& 0_{1\×13}& -{\mathrm{\θ}}_{odU}^n\end{array}\right]$

式中，分别为加入惯导信息后里程计测得的车体东向、北向、天向速度,为加入惯导信息后里程计测得的姿态角。

作为本发明的基于SINS/双OD的惯性组合导航***的导航方法的一种优选方案，根据所述状态方程及量测方程对各误差状态量进行滤波估计并修正的方法为：将所述状态方程及所述量测方程离散化，离散化后的状态方程及量测方程分别为：

X_k＝φ_k,k-1X_k-1+Γ_k,k-1W_k-1

Z_k1＝H_k1X_k+V_k1

Z_k2＝H_k2X_k+V_k2

式中，X_k为k时刻状态量的估计值，φ_k,k-1为k-1时刻至k时刻的一步转移矩阵，Γ_k,k-1为k-1时刻至k时刻***噪声驱动阵的一步转移矩阵，W_k-1为k-1时刻的***激励噪声序列，H_k1、H_k2为k1时刻及k2时刻的量测矩阵，V_k1、V_k2为k1时刻及k2时刻的量测噪声序列。

本发明还提供一种基于SINS/双OD的惯性组合导航***，所述基于SINS/双OD的惯性组合导航***包括：SINS、双里程计及卡尔曼滤波模块；其中

所述SINS适于得到量测得到惯性量测数据，并通过导航积分计算获得轨迹发生器的航位姿态、坐标及速度信息；

所述双里程计适于量测轨迹发生器的行驶速度，并接收SINS获得的航位姿态、坐标及速度信息进行航位推算得到准确的姿态坐标及速度信息；

所述卡尔曼滤波模块适于对SINS及双里程计的误差状态量进行滤波估计并修正。

作为本发明的基于SINS/双OD的惯性组合导航***的一种优选方案，所述SINS包括惯性量测单元及SINS处理模块；所述惯性量测单元适于输出惯性量测数据，所述SINS处理模块适于接收所述惯性量测单元输出的数据，通过导航积分计算获得轨迹发生器的航位姿态、坐标及速度信息。

作为本发明的基于SINS/双OD的惯性组合导航***的一种优选方案，所述惯性量测单元包括陀螺仪及加速度计。

如上所述，本发明的基于SINS/双OD的惯性组合导航***及其导航方法，具有以下有益效果：通过在基于SINS/双OD的惯性组合导航***中设置双里程计，利用双路里程计的速度和方向角信息可以有效地修正惯导的输出，减小导航参数的误差，获得更精确的轨迹发生器的速度信息和方向角度；通过惯导和双里程计的融合，以惯导和双里程计的导航参数的误差量作为估计对象运用卡尔曼滤波技术，可以得到精确可行的卡尔曼滤波算法，进而实现更高精度的导航。

附图说明

图1显示为本发明的基于SINS/双OD的惯性组合导航***的导航方法的流程图。

图2显示为本发明的基于SINS/双OD的惯性组合导航***的导航方法中建立双OD误差模型过程中车体的位姿示意图。

图3至图5显示为导航效果对比图；其中，图3为SINS***的导航效果示意图，图4为传统SINS/双OD组合导航***的导航效果示意图，图5为本发明的基于SINS/双OD的惯性组合导航***的导航效果示意图。

图6显示为本发明的基于SINS/双OD的惯性组合导航***的结构示意图。

元件标号说明

1      SINS

11     惯性量测单元

111    陀螺仪

112    加速度计

12     SINS处理模块

2      双里程计

3      卡尔曼滤波模块

具体实施方式

以下由特定的具体实施例说明本发明的实施方式，熟悉此技术的人士可由本说明书所揭露的内容轻易地了解本发明的其他优点及功效。

请参阅图1至图4。须知，本说明书所附图示所绘示的结构、比例、大小等，均仅用以配合说明书所揭示的内容，以供熟悉此技术的人士了解与阅读，并非用以限定本发明可实施的限定条件，故不具技术上的实质意义，任何结构的修饰、比例关系的改变或大小的调整，在不影响本发明所能产生的功效及所能达成的目的下，均应仍落在本发明所揭示的技术内容得能涵盖的范围内。同时，本说明书中所引用的如“上”、“下”、“左”、“右”、“中部”及“一”等的用语，亦仅为便于叙述的明了，而非用以限定本发明可实施的范围，其相对关系的改变或调整，在无实质变更技术内容下，当亦视为本发明可实施的范畴。

请参阅图1至图2，本实施例提供一种基于SINS/双OD的惯性组合导航***的导航方法，所述方法包括：

S1：建立SINS误差模型，所述SINS误差模型包括SINS的姿态误差方程、速度误差方程及位置误差方程；

S2：建立双OD误差模型，所述双OD误差模型包括双OD的速度误差方程及方向角误差方程；

S3：对SINS误差模型及双OD误差模型的各误差状态量进行估计并修正。

执行步骤S1，请参阅图1中的S1步骤，建立SINS误差模型，所述SINS误差模型包括SINS的姿态误差方程、速度误差方程及位置误差方程。

具体的，首先定义如下坐标系：将东-北-天地理坐标系作为导航坐标系，记为n系：当SINS按一定的方位安装到轨迹发生器上时，认为轨迹发生器坐标系与惯导坐标系重合，记为b系：假设里程计坐标系与轨迹发生器坐标系平行，记为m系：其中，b系及m系的方向均为：x轴沿轨迹发生器横轴指向右侧，y轴沿轨迹发生器纵轴指向正前方，z轴垂直于x轴和y轴并构成右手直角坐标系。

具体的，在定义上述坐标系的前提下，所述SINS误差模型中所述SINS的姿态误差方程为：

所述SINS的速度误差方程为：

所述SINS的位置误差方程为：

$\left\{\begin{array}{c}\mathrm{\δ}\stackrel{\·}{L}=\frac{{\mathrm{\δv}}_E^n}{R_M+h}-\frac{v_N^n\mathrm{\δ}h}{{(R_M+h)}^2}\\ \mathrm{\δ}\stackrel{\·}{\mathrm{\λ}}=\frac{({\mathrm{\δv}}_N^n\sec L+v_E^n\sec L\tan L)}{R_N+h}-\frac{v_E^n\sec L\mathrm{\δ}h}{{(R_N+h)}^2}\\ \mathrm{\δ}\stackrel{\·}{h}={\mathrm{\δv}}_U^n\end{array}\right.$

式中，为捷联惯导计算的东、北和天向的姿态误差角；为速度误差；δL、δλ、δh为位置误差，分别为纬度、经度和高度误差；为地球自转角速率在导航坐标系上的投影，为导航系相对地球坐标系的角速率在导航系中的投影；fⁿ为导航系中的比力；ε^b＝[ε_x ε_y ε_z]为轨迹发生器系中的陀螺漂移，为轨迹发生器体系中的加速度计零偏；为姿态矩阵，R_M和R_N分别为地球子午圈和卯酉圈半径。公式中一些字母上加点表示对该字母进行求导。

执行步骤S2，请参阅图1中的S2步骤及图2，建立双OD误差模型，所述双OD误差模型包括双OD的速度误差方程及方向角误差方程。

具体的，与传统的单里程计导航***不同，本发明中的里程计利用轨迹发生器的位姿信息推出速度，提高了模型的精确度。所述轨迹发生器以汽车为例，本发明中的双里程计分别设置于所述车体的左右两侧，设t时刻，基于SINS/双OD的惯性组合导航***通过左右里程计测得的车轮的角速度分别为ω_od1和ω_od2，如图2所示，图中所示的坐标系与b系的xoy面平行，定义θ为车的方向角，车的质心C点的坐标为(x，y)，用一个三维向量(x，y，θ)描述车的位姿，则相应的位姿方程为：

$\left[\begin{array}{c}\stackrel{\·}{x}\\ \stackrel{\·}{y}\\ \stackrel{\·}{\mathrm{\θ}}\end{array}\right]=\frac{r}{2}\left[\begin{array}{cc}\frac{\cos (\mathrm{\θ}-\mathrm{\γ})}{\cos \mathrm{\γ}}& \frac{\cos (\mathrm{\θ}+\mathrm{\γ})}{\cos \mathrm{\γ}}\\ \frac{\sin (\mathrm{\θ}-\mathrm{\γ})}{\cos \mathrm{\γ}}& \frac{\sin (\mathrm{\θ}+\mathrm{\γ})}{\cos \mathrm{\γ}}\\ -\frac{1}{l\cos \mathrm{\γ}}& \frac{1}{l\cos \mathrm{\γ}}\end{array}\right]\left[\begin{array}{c}{\mathrm{\ω}}_{od1}\\ {\mathrm{\ω}}_{od2}\end{array}\right]$

式中，r为车轮半径；B点为车轮中心，γ为BC与两轮水平轴的夹角，l为BC的长度。公式中一些字母上加点表示对该字母进行求导。

由此，得到里程计所获得的车体速度为：

$v_{od}=\sqrt{{(\stackrel{\·}{x}cos\mathrm{\θ}+\stackrel{\·}{y}sin\mathrm{\θ})}^2+{(-\stackrel{\·}{x}sin\mathrm{\θ}+\stackrel{\·}{y}cos\mathrm{\θ})}^2}$

该速度v_od的方向指向车体的正前方，且v_od在b系的投影的矢量形式为：

$v_{od}^b={\left[\begin{array}{ccc}0& v_{od}& 0\end{array}\right]}^T$

在n系上的投影为：

$v_{od}^n=C_b^n{v}_{od}^b$

又双里程计的速度误差主要由里程计刻度系数误差δk和引入的姿态角误差造成，其中，δk由车轮气压和路面状况等因素决定。因此，考虑δk及之后的里程计速度的实际计算公式可表示为：

式中，表示向量的反对称矩阵。

将上式展开，并忽略δk和的高阶小量，得到如下公式：

进而得到里程计的速度误差方程为：

由里程计所获得的车体速度v_od可得到里程计的方向角在b系中的矢量形式为：

${\mathrm{\θ}}_{od}^b={\[\[\begin{array}{ccc}\mathrm{\θ}& 0& 0\end{array}\]\]}^T$

在n系上的投影为：

${\mathrm{\θ}}_{od}^n=C_b^n{\mathrm{\θ}}_{od}^b$

又双里程计的方向角误差主要由里程计方向角系数误差δθ和引入的姿态角误差造成。将里程计方向角的实际计算公式展开并忽略δθ和的高阶小量，得到如下公式：

进而得到里程计的方向角的误差方程为：

执行步骤S3，请参阅图1中的S3步骤，对SINS误差模型及双OD误差模型的各误差状态量进行估计并修正。

具体的，对SINS误差模型及双OD误差模型的各误差状态量进行估计并修正的具体方法为：

以SINS的姿态误差、速度误差、位置误差、陀螺常值漂移及加速度计常值零偏作为SINS的状态变量，以刻度系数误差及方向角***误差作为双OD的状态变量建立状态方程；以SINS解算的数据与双OD测得的数据的差值作为量测量建立量测方程；所述状态方程及量测方程构成基于SINS/双OD的惯性组合导航***卡尔曼滤波模型；

根据所述状态方程及量测方程对各误差状态量进行滤波估计并修正。

具体的，本发明适用于轨迹发生器的GPS无法正常工作时，本发明的基于SINS/双OD的惯性组合导航***采用双里程计的速度信息来辅助SINS，构成组合导航。在轨迹发生器起步阶段，只要轨迹发生器有位移，则SINS利用卡尔曼滤波器对双里程计的误差进行估计，然后利用双里程计来修正SINS的误差累积现象。设计所述基于SINS/双OD的惯性组合导航***的卡尔曼滤波模型，首先需建立以SINS误差方程及双里程计误差方程为基础的***状态方程，其次在基于SINS/双OD的惯性组合导航***的误差方程的基础上建立***的量测方程。

首先，在分析SINS和双OD的性能和误差的基础上，选取误差状态的姿态误差、东、北、天向速度误差、纬度、经度和高度误差、三个陀螺常值漂移、三个水平加速度计常值零偏作为SINS的状态变量，刻度系数误差和方向角系数误差作为里程计的状态变量，则基于SINS/双OD的惯性组合导航***t时刻的状态变量为：

式中，为姿态误差，为东、北、天向速度误差，δL、δλ、δh为纬度、经度及高度方向的位置误差，ε_x、ε_y、ε_z为三个陀螺常值漂移， 为三个水平加速度计常值零偏，δk为里程计刻度系数误差，δθ为里程计方向角系数误差。

基于SINS/双OD的惯性组合导航***的状态方程为：

$X\left(t\right)=F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)$

式中，X(t)为t时刻***的状态向量，F(t)为***矩阵，G(t)为***噪声转移矩阵，W(t)为***噪声。

其次，以SINS解算的数据与双OD测得的数据的差值作为卡尔曼滤波器的量测，则量测方程为：

$Z_1\left(t\right)=v_{\sin s}^n-v_{od}^n=H_1\left(t\right)X\left(t\right)+v_1\left(t\right)$

$Z_2\left(t\right)={\mathrm{\θ}}_{\sin s}^n-{\mathrm{\θ}}_{od}^n=H_2\left(t\right)X\left(t\right)+v_2\left(t\right)$

式中，v₁(t)、v₂(t)为量测噪声矢量，H₁ ^(t)、H₂ ^(t)为t时刻的量测矩阵。

结合所述里程计的速度误差及方向角误差，SINS解算的数据与双OD测得的数据之差可表示为：

式中，和分别为捷联惯导的速度输出和其误差，为SINS的方向角输出，即其姿态角。

由上述SINS解算的数据与双OD测得的数据之差即可得到H₁ ^(t)、H₂ ^(t)，所述H₁ ^(t)、H₂ ^(t)具体为：

$H_1\left(t\right)=\left[\begin{array}{ccccccccc}0& v_{odU}^n& -v_{odN}^n& 1& 0& 0& 0_{1\×9}& -v_{odE}^n& 0\\ -v_{odU}^n& 0& v_{odE}^n& 0& 1& 0& 0_{1\×9}& -v_{odN}^n& 0\\ v_{odN}^n& -v_{odE}^n& 0& 0& 0& 1& 0_{1\×9}& -v_{odU}^n& 0\end{array}\right]$

$H_2\left(t\right)=\left[\begin{array}{ccccc}1& {\mathrm{\θ}}_{odU}^n& -{\mathrm{\θ}}_{odN}^n& 0_{1\×13}& -{\mathrm{\θ}}_{odE}^n\\ -{\mathrm{\θ}}_{odU}^n& 1& {\mathrm{\θ}}_{odE}^n& 0_{1\×13}& -{\mathrm{\θ}}_{odN}^n\\ {\mathrm{\θ}}_{odN}^n& -{\mathrm{\θ}}_{odE}^n& 1& 0_{1\×13}& -{\mathrm{\θ}}_{odU}^n\end{array}\right]$

式中，分别为加入惯导信息后里程计测得的车体东向、北向、天向速度,为加入惯导信息后里程计测得的姿态角。

具体的，根据所述状态方程及量测方程对各误差状态量进行滤波估计并修正的方法为：将所述状态方程及所述量测方程离散化，离散化后的状态方程及量测方程分别为：

X_k＝φ_k,k-1X_k-1+Γ_k,k-1W_k-1

Z_k1＝H_k1X_k+V_k1

Z_k2＝H_k2X_k+V_k2

式中，X_k为k时刻状态量的估计值，φ_k,k-1为k-1时刻至k时刻的一步转移矩阵，Γ_k,k-1为k-1时刻至k时刻***噪声驱动阵的一步转移矩阵，W_k-1为k-1时刻的***激励噪声序列，H_k1、H_k2为k1时刻及k2时刻的量测矩阵，V_k1、V_k2为k1时刻及k2时刻的量测噪声序列。

以上即采用经典的Kalman滤波算法作为SINS/双OD组合导航***的滤波算法，对各误差状态进行估计并进行修正，以有效地减小导航误差的累积。

仿真结果：以车载惯导与双里程计的误差参数如下为例：陀螺随机常值漂移0.02/h，噪声0.01/h；加速度计随机常值零漂0.15mg，噪声0.05mg；双里程计刻度系数误差为0.2％，方向角系数误差0.3％；捷联惯导初始对准水平姿态误差角1′，航向误差角10′。

实验设计了一条环形路径进行验证，如图3至5所示，其中，图3为SINS***的导航效果示意图，图4为传统SINS/双OD组合导航***的导航效果示意图，图5为本发明的基于SINS/双OD的惯性组合导航***的导航效果示意图。图3至图5中实线为预设轨迹路径，虚线为SINS***、传统SINS/双OD组合导航***和本发明的基于SINS/双OD的惯性组合导航***解算出的载车运动轨迹。通过比较SINS***、传统的SINS/双OD组合导航***和基于SINS/双OD的惯性组合导航***解算出的载车运动轨迹，发现单纯SINS导航位置误差随着时间的增长而快速发散；本发明的的***较为稳定且与真实轨迹重合更高，与单里程计的组合***相比，提高了定位精度，说明了本发明的基于SINS/双OD的惯性组合导航***及其导航方法的有效性和优越性。

请参阅图6，本发明还提供一种基于SINS/双OD的惯性组合导航***，所述基于SINS/双OD的惯性组合导航***包括：SINS1、双里程计2及卡尔曼滤波模块3；其中，

所述SINS1适于得到量测得到惯性量测数据，并通过导航积分计算获得轨迹发生器的航位姿态、坐标及速度信息；

所述双里程计2适于量测轨迹发生器的行驶速度，并接收SINS1获得的航位姿态、坐标及速度信息进行航位推算得到准确的姿态坐标及速度信息；

所述卡尔曼滤波模块3适于对SINS1及双里程计2的误差状态量进行滤波估计并修正。

具体的，所述SINS1包括惯性量测单元11及SINS处理模块12；所述惯性量测单元11适于输出惯性量测数据，所述SINS处理模块12适于接收所述惯性量测单元11输出的数据，通过导航积分计算获得轨迹发生器的航位姿态、坐标及速度信息。

具体的，所述惯性量测单元11包括陀螺仪111及加速度计112。

综上所述，本发明提供一种基于SINS/双OD的惯性组合导航***及其导航方法，所述基于SINS/双OD的惯性组合导航***及其导航方法通过在基于SINS/双OD的惯性组合导航***中设置双里程计，利用双路里程计的速度和方向角信息可以有效地修正惯导的输出，减小导航参数的误差，获得更精确的轨迹发生器的速度信息和方向角度；通过惯导和双里程计的融合，以惯导和双里程计的导航参数的误差量作为估计对象运用卡尔曼滤波技术，可以得到精确可行的卡尔曼滤波算法，进而实现更高精度的导航。

上述实施例仅例示性说明本发明的原理及其功效，而非用于限制本发明。任何熟悉此技术的人士皆可在不违背本发明的精神及范畴下，对上述实施例进行修饰或改变。因此，举凡所属技术领域中具有通常知识者在未脱离本发明所揭示的精神与技术思想下所完成的一切等效修饰或改变，仍应由本发明的权利要求所涵盖。

Claims (12)

1.一种基于SINS/双OD的惯性组合导航***的导航方法，其特征在于，所述方法包括：

建立SINS误差模型，所述SINS误差模型包括SINS的姿态误差方程、速度误差方程及位置误差方程；

建立双OD误差模型，所述双OD误差模型包括双OD的速度误差方程及方向角误差方程；

对SINS误差模型及双OD误差模型的各误差状态量进行估计并修正。

2.根据权利要求1所述的基于SINS/双OD的惯性组合导航***的导航方法，其特征在于：本方法的坐标系定义如下：

n系：导航坐标系，即为东-北-天地理坐标系；

b系：惯导坐标系，与轨迹发生器坐标系重合；

m系：里程计坐标系，与轨迹发生器坐标系平行；

其中，b系及m系的方向均为：x轴沿轨迹发生器横轴指向右侧，y轴沿轨迹发生器纵轴指向正前方，z轴垂直于x轴和y轴并构成右手直角坐标系。

3.根据权利要求2所述的基于SINS/双OD的惯性组合导航***的导航方法，其特征在于：所述SINS的姿态误差方程为：

所述SINS的速度误差方程为：

所述SINS的位置误差方程为：

$\left\{\begin{array}{c}\mathrm{\δ}\stackrel{\·}{L}=\frac{{\mathrm{\δv}}_E^n}{R_M+h}-\frac{v_N^n\mathrm{\δ}h}{{(R_M+h)}^2}\\ \mathrm{\δ}\stackrel{\·}{\mathrm{\λ}}=\frac{\left({\mathrm{\δv}}_N^n\sec L+v_E^n\sec L\tan L\right)}{R_N+h}-\frac{v_E^n\sec L\mathrm{\δ}h}{{(R_N+h)}^2}\\ \mathrm{\δ}\stackrel{\·}{h}={\mathrm{\δv}}_U^n\end{array}\right.$

式中，为捷联惯导计算的东、北和天向的姿态误差角；δvⁿ＝[δv_Eδv_Nδv_U]为速度误差；δL、δλ、δh为位置误差，分别为纬度、经度和高度误差；为地球自转角速率在导航坐标系上的投影，为导航系相对地球坐标系的角速率在导航系中的投影；fⁿ为导航系中的比力；ε^b＝[ε_xε_yε_z]为轨迹发生器系中的陀螺漂移，为轨迹发生器体系中的加速度计零偏；为姿态矩阵，R_M和R_N分别为地球子午圈和卯酉圈半径。

4.根据权利要求1所述的基于SINS/双OD的惯性组合导航***的导航方法，其特征在于：建立双OD误差模型的具体方法为：

使用双OD测得轨迹发生器在t时刻的位姿信息；

根据所述位姿信息得到轨迹发生器的速度及方向角；

根据所述轨迹发生器的速度及方向角，得到所述双OD的速度误差方程及方向角误差方程。

5.根据权利要求2所述的基于SINS/双OD的惯性组合导航***的导航方法，其特征在于：所述双OD的速度误差方程为：

所述双OD的方向角误差方程为：

式中，为里程计应得到的轨迹发生器的真实速度在n系中的投影，为里程计量测得到的轨迹发生器的速度在n系中的投影，为姿态角误差，δk为里程计刻度系数误差，为里程计的方向角在n系中的投影，δθ为里程计方向角系数误差。

6.根据权利要求1所述的基于SINS/双OD的惯性组合导航***的导航方法，其特征在于：对SINS误差模型及双OD误差模型的各误差状态量进行估计并修正的具体方法为：

以SINS的姿态误差、速度误差、位置误差、陀螺常值漂移及加速度计常值零偏作为SINS的状态变量，以刻度系数误差及方向角***误差作为双OD的状态变量建立状态方程；以SINS解算的数据与双OD测得的数据的差值作为量测量建立量测方程；所述状态方程及量测方程构成基于SINS/双OD的惯性组合导航***卡尔曼滤波模型；

根据所述状态方程及量测方程对各误差状态量进行滤波估计并修正。

7.根据权利要求6所述的基于SINS/双OD的惯性组合导航***的导航方法，其特征在于：

所述状态方程为：

$X\left(t\right)=F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)$

式中，X(t)为t时刻***的状态向量，F(t)为***矩阵，G(t)为***噪声转移矩阵，W(t)为***噪声；

t时刻***的状态向量为：

式中，为姿态误差，δv_E、δv_N、δv_U为东、北、天向速度误差，δL、δλ、δh为纬度、经度及高度方向的位置误差，ε_x、ε_y、ε_z为三个陀螺常值漂移， 为三个水平加速度计常值零偏，δk为里程计刻度系数误差，δθ为里程计方向角系数误差。

8.根据权利要求7所述的基于SINS/双OD的惯性组合导航***的导航方法，其特征在于：所述量测方程为：

$Z_1\left(t\right)=v_{\sin s}^n-v_{od}^n=H_1\left(t\right)X\left(t\right)+v_1\left(t\right)$

$Z_2\left(t\right)={\mathrm{\θ}}_{\sin s}^n-{\mathrm{\θ}}_{od}^n=H_2\left(t\right)X\left(t\right)+v_2\left(t\right)$

式中，v₁(t)、v₂(t)为量测噪声矢量；H₁(t)、H₂(t)为t时刻的量测矩阵，H₁(t)、H₂(t)为：

$H_1\left(t\right)=\left[\begin{array}{ccccccccc}0& v_{odU}^n& -v_{odN}^n& 1& 0& 0& 0_{1\×9}& -v_{odE}^n& 0\\ -v_{odU}^n& 0& v_{odE}^n& 0& 1& 0& 0_{1\×9}& -v_{odN}^n& 0\\ v_{odN}^n& -v_{odE}^n& 0& 0& 0& 1& 0_{1\×9}& -v_{odU}^n& 0\end{array}\right]$

$H_2\left(t\right)=\left[\begin{array}{ccccc}1& {\mathrm{\θ}}_{odU}^n& -{\mathrm{\θ}}_{odN}^n& {\mathrm{\θ}}_{1\×13}& -{\mathrm{\θ}}_{odE}^n\\ -{\mathrm{\θ}}_{odU}^n& 1& {\mathrm{\θ}}_{odE}^n& {\mathrm{\θ}}_{1\×13}& -{\mathrm{\θ}}_{odN}^n\\ {\mathrm{\θ}}_{odN}^n& -{\mathrm{\θ}}_{odE}^n& 1& {\mathrm{\θ}}_{1\×13}& -{\mathrm{\θ}}_{odU}^n\end{array}\right]$

式中，分别为加入惯导信息后里程计测得的车体东向、北向、天向速度,为加入惯导信息后里程计测得的姿态角。

9.根据权利要求8所述的基于SINS/双OD的惯性组合导航***的导航方法，其特征在于：根据所述状态方程及量测方程对各误差状态量进行滤波估计并修正的方法为：将所述状态方程及所述量测方程离散化，离散化后的状态方程及量测方程分别为：

X_k＝φ_k,k-1X_k-1+Γ_k,k-1W_k-1

Z_k1＝H_k1X_k+V_k1

Z_k2＝H_k2X_k+V_k2

式中，X_k为k时刻状态量的估计值，φ_k,k-1为k-1时刻至k时刻的一步转移矩阵，Γ_k,k-1为k-1时刻至k时刻***噪声驱动阵的一步转移矩阵，W_k-1为k-1时刻的***激励噪声序列，H_k1、H_k2为k1时刻及k2时刻的量测矩阵，V_k1、V_k2为k1时刻及k2时刻的量测噪声序列。

10.一种基于SINS/双OD的惯性组合导航***，其特征在于，所述基于SINS/双OD的惯性组合导航***包括：SINS、双里程计及卡尔曼滤波模块；其中

所述SINS适于得到量测得到惯性量测数据，并通过导航积分计算获得轨迹发生器的航位姿态、坐标及速度信息；

所述双里程计适于量测轨迹发生器的行驶速度，并接收SINS获得的航位姿态、坐标及速度信息进行航位推算得到准确的姿态坐标及速度信息；

所述卡尔曼滤波模块适于对SINS及双里程计的误差状态量进行滤波估计并修正。

11.根据权利要求10所述的基于SINS/双OD的惯性组合导航***，其特征在于：所述SINS包括惯性量测单元及SINS处理模块；所述惯性量测单元适于输出惯性量测数据，所述SINS处理模块适于接收所述惯性量测单元输出的数据，通过导航积分计算获得轨迹发生器的航位姿态、坐标及速度信息。

12.根据权利要求11所述的基于SINS/双OD的惯性组合导航***，其特征在于：所述惯性量测单元包括陀螺仪及加速度计。