CN103217157B - 一种惯导/里程计自主组合导航方法 - Google Patents

Abstract

本发明属于导航技术领域，具体涉及一种惯导/里程计自主组合导航方法。本方法包括以下步骤：(1)惯导系统初始对准，对卡尔曼滤波器参数进行初始化；(2)进行惯性导航计算，得到离散状态转移矩阵和量测矩阵；(3)基于位移误差的求和测量，计算滤波观测量；(4)里程计信息故障检测；(5)卡尔曼滤波；(6)修正。本发明的方法解决了现有惯导/里程计组合导航方法中里程计量化噪声过大，降低组合导航精度的技术问题；采用基于位移积分匹配惯性/里程计组合导航策略，摒弃了传统的利用位移计算速度作为观测量时引入的量化噪声，显著提高了组合导航系统的可靠性。

Description

一种惯导/里程计自主组合导航方法

技术领域

本发明属于导航技术领域，具体涉及一种惯导/里程计自主组合导航方法。

背景技术

在陆用定位定向应用领域，国内外对高精度车载定位定向系统都有着迫切的需求。其中，将惯导系统与GNSS、里程计进行组合导航是目前陆用定位定向系统所普遍采用的方法。然而，由于GNSS信息易受干扰，目前国内外典型的陆用定位定向系统产品所采用的主要工作模式为惯导/里程计组合导航，兼容零速修正技术，获得较高的定位定向精度，并具有完全自主性。

目前，惯导/里程计组合导航普遍采用里程计速度信息与惯导进行组合导航，但由于里程计输出位移增量脉冲信号，且存在刻度系数误差，由位移增量信息进行微分得到的速度信息后，存在较大的速度噪声，且不能保证速度信息的实时性和准确性，从而影响自主导航精度。

发明内容

本发明要解决的技术问题为：现有惯导/里程计组合导航方法中里程计量化噪声过大，降低了组合导航精度。

本发明的技术方案如下所述：

一种惯导/里程计自主组合导航方法，包括以下步骤：

步骤(1)进行惯导系统初始对准，得到惯导系统初始姿态；惯导系统初始对准结束后转入惯导/里程计组合导航状态，对卡尔曼滤波器参数进行初始化；步骤(2)进行惯性导航计算，同时采集里程计脉冲信号，解算惯导速度、位置、姿态信息，里程计姿态阵，并将里程计脉冲信息转换为位移信息，计算里程计航位推算位移增量；同时计算离散状态转移矩阵和量测矩阵；步骤(3)导航计算每到卡尔曼滤波周期时刻，采用基于位移误差的求和测量，计算滤波观测量；步骤(4)进行里程计信息故障检测；步骤(5)进行卡尔曼滤波；步骤(6)对惯导姿态阵、速度、位置误差进行修正，对里程计姿态阵进行修正；对里程计刻度系数进行修正。

本方法的坐标系定义如下：

n：导航坐标系oxyz，北天东地理坐标系，x轴指北，y轴指天，z轴指东；n′：计算导航坐标系o′x′y′z′，x′轴指北，y′轴指天，z′轴指东；b：惯导载体系o″x″y″z″，与陀螺坐标系重合，前上右坐标系，x″轴指向前，y″轴朝上，z″轴指右；m：里程计坐标系o″′x″′y″′z″′，前上右坐标系，x″′轴指向前，y″′轴朝上，z″′轴指右。

步骤(1)中，惯导系统初始对准方法采用静基座对准或动基座对准方法。

步骤(2)中，设里程计位移输出在里程计坐标系m中表示为：

ΔS^m＝[ΔS_x00]^T(1)

则里程计位移在导航坐标系n中表示为：

${\mathrm{\ΔS}}^n=C_m^n{\mathrm{\ΔS}}^m---\left(2\right)$

里程计导航坐标系n位移误差方程：

$\mathrm{\Δ}{\hat{S}}^n={\hat{C}}_m^n\mathrm{\Δ}{\hat{S}}^m=C_b^{n^{\′}}\mathrm{\Δ}{\hat{S}}^m=C_n^{n^{\′}}{C}_m^n{C}_b^m(1+{\mathrm{\δK}}_D){\mathrm{\ΔS}}^m$

(3)

$\≈{\mathrm{\ΔS}}^n-\mathrm{\φ}\×{\mathrm{\ΔS}}^n-C_b^n[{\mathrm{\ΔS}}^m\×]{\mathrm{\φ}}_a+{\mathrm{\δK}}_D{\mathrm{\ΔS}}^n$

可得：

$\mathrm{\δ}{\mathrm{\ΔS}}^n{=\mathrm{\ΔS}}^n\×\mathrm{\φ}-C_b^n[{\mathrm{\ΔS}}^m\×]{\mathrm{\φ}}_a+{\mathrm{\δK}}_D{\mathrm{\ΔS}}^n---\left(4\right)$

式中：ΔS^m：理想里程计坐标系m输出位移；ΔS_x：理想里程计输出载体前向位移；ΔSⁿ：理想里程计坐标系m输出位移在导航坐标系n中的表示； $\mathrm{\Δ}{\hat{S}}^n={\left[\begin{array}{ccc}{\mathrm{\ΔS}}_N^{\mathrm{ODO}}& {\mathrm{\ΔS}}_U^{\mathrm{ODO}}& {\mathrm{\ΔS}}_E^{\mathrm{ODO}}\end{array}\right]}^T:$ 实际里程计坐标系m输出位移在导航坐标系n投影；惯导载体系b到计算导航坐标系n′的计算姿态转移矩阵；实际里程计坐标系m输出位移；理想导航坐标系n到计算导航坐标系n′的姿态转移矩阵；里程计坐标系m到理想导航坐标系n的姿态转移矩阵；计算里程计坐标系m到计算导航坐标系n′的姿态转移矩阵；惯导载体系b到里程计坐标系m的姿态转移矩阵；δK_D：里程计刻度系数误差；φ＝[φ_Nφ_Uφ_E]^T：惯导失准角；里程计坐标系m位移误差在导航坐标系n中的投影分量；φ_a＝[φ_axφ_ayφ_az]^T：里程计坐标系m与惯导载体系b之间x轴安装误差角，y轴安装误差角，z轴安装偏差角；

设计如下惯导/里程计组合导航系统误差模型：

将以上连续系统误差模型进行离散化得到式(6)所示模型：

$\left\{\begin{array}{c}X\left(k\right)=\mathrm{\Φ}(k,k-1)X(k-1)+Q(k-1)\\ z\left(k\right)=H\left(k\right)x\left(k\right)+R\left(k\right)\end{array}\right.---\left(6\right)$

其中，离散状态转移矩阵为：

$\mathrm{\Φ}(k+n,k)=I_{15\×15}+\underset{i=k}{\overset{k+n}{\mathrm{\Σ}}}{A}_i{T}_n---\left(7\right)$

I_15×15为15维单位阵，T_n为离散周期；

ω_ie：地球自转角速率；

V_N，V_U，V_E：惯导北向、天向、东向速度；

f_N，f_U，f_E：惯导北向、天向、东向加速度；

λ，L，h：惯导经度、纬度、高度；

R_M，R_N：地球子午面、卯酉面半径；

δV_N，δV_U，δV_E：惯导北向、天向、东向速度误差；

φ_N，φ_U，φ_E：惯导北向、天向、东向失准角；

惯导经度、高度、纬度误差；

加速度计零位误差；

ε_x，ε_y，ε_z：陀螺常值漂移；

φ_ay：里程计载体系与惯导载体系之间y轴安装误差角；

量测矩阵为：

$H\left(k\right)=\left\{\underset{i=1}{\overset{k}{\mathrm{\Σ}}}{\left[\begin{array}{ccccc}\begin{array}{ccc}{T}_s& 0& 0\\ 0& T_s& 0\\ 0& 0& T_s\end{array}& -[\left(\mathrm{\Δ}{\hat{S}}^n(i-1,i)\right)\×]& 0_{3\×3}& -\mathrm{\Δ}{\hat{S}}^n(i-1,i)& \begin{array}{c}\left(C_b^n\left(i\right)\right[\left(\mathrm{\Δ}{\hat{S}}^m(i-1,i)\right)\×\left]\right)\left(\mathrm{1,2}\right)\\ \left(C_b^n\left(i\right)\right[\left(\mathrm{\Δ}{\hat{S}}^m(i-1,i)\right)\×\left]\right)\left(\mathrm{2,2}\right)\\ \left(C_b^n\left(i\right)\right[\left(\mathrm{\Δ}{\hat{S}}^m(i-1,i)\right)\×\left]\right)\left(\mathrm{3,2}\right)\end{array}\end{array}\right]}^T\mathrm{\Φ}(i-1,i)\right\}{\mathrm{\Φ}}^{-1}(k-1,k)---\left(8\right)$

其中，T_s为里程计采样周期。

步骤(3)中，所述滤波观测量通过下式计算：

$Z\left(k\right)=\left[\begin{array}{c}{\mathrm{\δ\ΔS}}_N^n(k,k-1)\\ {\mathrm{\δ\ΔS}}_N^n(k,k-1)\\ {\mathrm{\δ\ΔS}}_N^n(k,k-1)\end{array}\right]=\left[\begin{array}{c}{\mathrm{\ΔS}}_N^{\mathrm{INS}}(k,k-1)-{\mathrm{\ΔS}}_N^{\mathrm{ODO}}(k,k-1)\\ {\mathrm{\ΔS}}_U^{\mathrm{INS}}(k,k-1)-{\mathrm{\ΔS}}_U^{\mathrm{ODO}}(k,k-1)\\ {\mathrm{\ΔS}}_E^{\mathrm{INS}}(k,k-1)-{\mathrm{\ΔS}}_E^{\mathrm{ODO}}(k,k-1)\end{array}\right]---\left(9\right)$

其中：

$\left[\begin{array}{ccc}{\mathrm{\ΔS}}_N^{\mathrm{INS}}& {\mathrm{\ΔS}}_U^{\mathrm{INS}}& {\mathrm{\ΔS}}_E^{\mathrm{INS}}\end{array}\right]$ 为滤波周期内惯导位移增量；

$\left[\begin{array}{ccc}{\mathrm{\ΔS}}_N^{\mathrm{ODO}}& {\mathrm{\ΔS}}_U^{\mathrm{ODO}}& {\mathrm{\ΔS}}_E^{\mathrm{ODO}}\end{array}\right]$ 为滤波周期内里程计航位推算位移增量。

步骤(3)中，所述滤波周期为1s。

步骤(4)中，采用量测序列检测器隔离里程计故障信息；

定义残差：其中Z_k为观测量，H_k为量测矩阵，为一步预测状态阵；

当无故障发生时，卡尔曼滤波器的残差r_k是零均值白噪声，其方差为：

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

其中，P_k/k-1为卡尔曼滤波一步预测协方差矩阵；R为量测噪声协方差阵。

而当量测发生故障时，残差r_k的均值不再为零，通过对残差r_k的检验确定系统是否发生故障；

故障监测函数为：

${\mathrm{\λ}}_k=r_k{A}_k^{-1}{r}_k---\left(11\right)$

其中，λ_k服从χ²分布；

故障判定准则为：

步骤(4)中，T_D＝0.2。

步骤(5)中，卡尔曼滤波方程如下所示：

$\left\{\begin{array}{c}{P}_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k.k-1}^T+Q\\ K_k=P_{k/k-1}{H}_k^T{(H_k{P}_{k/k-1}{H}_k^T+R)}^{-1}\\ {\hat{X}}_k={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}+K_k(Z_k-H_k{\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1})\\ P_k=(I-K_k{H}_k)P_{k/k-1}{(I-K_k{H}_k)}^T+K_k{\mathrm{RK}}_k^T\end{array}\right.---\left(13\right)$

式中：Q为系统噪声的协方差矩阵；R为量测噪声协方差阵；为最优估计矩阵；K_k为滤波增益矩阵；P_k为最优估计协方差矩阵。

步骤(6)中，

对里程计姿态阵进行修正，修正方法如下式所示：

${\hat{C}}_m^n={\hat{C}}_m^n{C}^{\′},C^{\′}=\left[\begin{array}{ccc}1& 0& -\hat{X}\left(17\right)\\ 0& 1& 0\\ \hat{X}\left(17\right)& 0& 1\end{array}\right]---\left(14\right)$

式中，为卡尔曼滤波最优估计中的第17个状态量；

对里程计刻度系数K_D进行修正，修正方法如下式所示：

$K_D=K_D/(1+\hat{X}\left(16\right))---\left(15\right)$

式中，为卡尔曼滤波最优估计中的第16个状态量。

本发明的有益效果为：

本发明的惯导/里程计自主组合导航方法采用基于位移积分匹配惯性/里程计组合导航策略，设计过程中充分利用了车载里程计输出位移增量的特性，摒弃了传统的利用位移计算速度作为观测量时引入的量化噪声，并通过里程计位移信息高频采样，实时充分利用里程计信息，实现了量测信息的平滑压缩，提高了量测信息信噪比，有效提高了组合导航精度；同时，采用了量测序列故障检测，对里程计故障信息进行有效隔离，显著提高了组合导航系统的可靠性。

附图说明

图1为本发明的惯导/里程计自主组合导航方法流程图；

图2为本发明的惯导/里程计自主组合导航方法试验轨迹示意图；

图3为本发明的惯导/里程计自主组合导航方法定位误差示意图。

具体实施方式

下面结合附图和实施例对本发明的惯导/里程计自主组合导航方法进行详细说明。

本发明的惯导/里程计自主组合导航方法采用的坐标系定义如下：

n：导航坐标系(oxyz)，北天东地理坐标系，x轴指北，y轴指天，z轴指东；

n′：计算导航坐标系(o′x′y′z′)，x′轴指北，y′轴指天，z′轴指东；

b：惯导载体系(o″x″y″z″)，与陀螺坐标系重合，前上右坐标系，x″轴指向前，y″轴朝上，z″轴指右；

m：里程计坐标系(o″′x″′y″′z″′)，前上右坐标系，x″′轴指向前，y″′轴朝上，z″′轴指右。

如图1所示，本发明的惯导/里程计自主组合导航方法包括以下步骤：

步骤(1)

进行惯导系统初始对准，惯导系统初始对准方法可采用常规的静基座对准或动基座对准方法，得到惯导系统初始姿态；惯导系统初始对准结束后转入惯导/里程计组合导航状态，对卡尔曼滤波器参数进行初始化，卡尔曼滤波器参数初始化方法为本领域技术人员公知常识。

步骤(2)

进行惯性导航计算，同时采集里程计脉冲信号，解算惯导速度、位置、姿态信息，里程计姿态阵并将里程计脉冲信息转换为位移信息，计算里程计航位推算位移增量同时利用公式(7)计算离散状态转移矩阵Φ_k，k-1，利用公式(8)量测矩阵H(k)。原理说明如下：

设里程计位移输出在里程计坐标系m中表示为：

ΔS^m＝[ΔS_x00]^T(1)

则里程计位移在导航坐标系n中表示为：

${\mathrm{\ΔS}}^n=C_m^n{\mathrm{\ΔS}}^m---\left(2\right)$

里程计导航坐标系n位移误差方程：

$\mathrm{\Δ}{\hat{S}}^n={\hat{C}}_m^n\mathrm{\Δ}{\hat{S}}^m=C_b^{n^{\′}}\mathrm{\Δ}{\hat{S}}^m=C_n^{n^{\′}}{C}_m^n{C}_b^m(1+{\mathrm{\δK}}_D){\mathrm{\ΔS}}^m$

(3)

$\≈{\mathrm{\ΔS}}^n-\mathrm{\φ}\×{\mathrm{\ΔS}}^n-C_b^n[{\mathrm{\ΔS}}^m\×]{\mathrm{\φ}}_a+{\mathrm{\δK}}_D{\mathrm{\ΔS}}^n$

可得：

$\mathrm{\δ}{\mathrm{\ΔS}}^n{=\mathrm{\ΔS}}^n\×\mathrm{\φ}-C_b^n[{\mathrm{\ΔS}}^m\×]{\mathrm{\φ}}_a+{\mathrm{\δK}}_D{\mathrm{\ΔS}}^n---\left(4\right)$

式中：ΔS^m：理想里程计坐标系m输出位移；ΔS_x：理想里程计输出载体前向位移；ΔSⁿ：理想里程计坐标系m输出位移在导航坐标系n中的表示； $\mathrm{\Δ}{\hat{S}}^n={\left[\begin{array}{ccc}{\mathrm{\ΔS}}_N^{\mathrm{ODO}}& {\mathrm{\ΔS}}_U^{\mathrm{ODO}}& {\mathrm{\ΔS}}_E^{\mathrm{ODO}}\end{array}\right]}^T:$ 实际里程计坐标系m输出位移在导航坐标系n投影；惯导载体系b到计算导航坐标系n′的计算姿态转移矩阵；实际里程计坐标系m输出位移；理想导航坐标系n到计算导航坐标系n′的姿态转移矩阵；里程计坐标系m到理想导航坐标系n的姿态转移矩阵；计算里程计坐标系m到计算导航坐标系n′的姿态转移矩阵；惯导载体系b到里程计坐标系m的姿态转移矩阵；δK_D：里程计刻度系数误差；φ＝[φ_Nφ_Uφ_E]^T：惯导失准角；里程计坐标系m位移误差在导航坐标系n中的投影分量；φ_a＝[φ_axφ_ayφ_az]^T：里程计坐标系m与惯导载体系b之间x轴安装误差角，y轴安装误差角，z轴安装偏差角。

设计如下惯导/里程计组合导航系统误差模型：

将以上连续系统误差模型进行离散化得到式(6)所示模型：

$\left\{\begin{array}{c}X\left(k\right)=\mathrm{\Φ}(k,k-1)X(k-1)+Q(k-1)\\ z\left(k\right)=H\left(k\right)x\left(k\right)+R\left(k\right)\end{array}\right.---\left(6\right)$

其中：

$\mathrm{\Φ}(k+n,k)=I_{15\×15}+\underset{i=k}{\overset{k+n}{\mathrm{\Σ}}}{A}_i{T}_n---\left(7\right)$

I_15×15为15维单位阵，T_n为离散周期；

ω_ie：地球自转角速率；

V_N，V_U，V_E：惯导北向、天向、东向速度；

f_N，f_U，f_E：惯导北向、天向、东向加速度；

λ，L，h：惯导经度、纬度、高度；

R_M，R_N：地球子午面、卯酉面半径；

δV_N，δV_U，δV_E：惯导北向、天向、东向速度误差；

φ_N，φ_U，φ_E：惯导北向、天向、东向失准角；

惯导经度、高度、纬度误差；

加速度计零位误差；

ε_x，ε_y，ε_z：陀螺常值漂移；

φ_ay：里程计载体系与惯导载体系之间y轴安装误差角。

量测矩阵为：

$H\left(k\right)=\left\{\underset{i=1}{\overset{k}{\mathrm{\Σ}}}{\left[\begin{array}{ccccc}\begin{array}{ccc}{T}_s& 0& 0\\ 0& T_s& 0\\ 0& 0& T_s\end{array}& -[\left(\mathrm{\Δ}{\hat{S}}^n(i-1,i)\right)\×]& 0_{3\×3}& -\mathrm{\Δ}{\hat{S}}^n(i-1,i)& \begin{array}{c}\left(C_b^n\left(i\right)\right[\left(\mathrm{\Δ}{\hat{S}}^m(i-1,i)\right)\×\left]\right)\left(\mathrm{1,2}\right)\\ \left(C_b^n\left(i\right)\right[\left(\mathrm{\Δ}{\hat{S}}^m(i-1,i)\right)\×\left]\right)\left(\mathrm{2,2}\right)\\ \left(C_b^n\left(i\right)\right[\left(\mathrm{\Δ}{\hat{S}}^m(i-1,i)\right)\×\left]\right)\left(\mathrm{3,2}\right)\end{array}\end{array}\right]}^T\mathrm{\Φ}(i-1,i)\right\}{\mathrm{\Φ}}^{-1}(k-1,k)---\left(8\right)$

其中，T_s为里程计采样周期。

步骤(3)

导航计算每到卡尔曼滤波周期时刻，本实施例中，卡尔曼滤波周期设为1s，利用公式(9)计算滤波观测量，在滤波观测量计算过程中，采用了基于位移误差的求和测量，采用位移求和测量后，可通过提高里程计位移采样频率，实时充分利用每个采样点的位移信息，不存在计算里程计速度带来的额外量化噪声，滤波观测量为：

$Z\left(k\right)=\left[\begin{array}{c}{\mathrm{\δ\ΔS}}_N^n(k,k-1)\\ {\mathrm{\δ\ΔS}}_N^n(k,k-1)\\ {\mathrm{\δ\ΔS}}_N^n(k,k-1)\end{array}\right]=\left[\begin{array}{c}{\mathrm{\ΔS}}_N^{\mathrm{INS}}(k,k-1)-{\mathrm{\ΔS}}_N^{\mathrm{ODO}}(k,k-1)\\ {\mathrm{\ΔS}}_U^{\mathrm{INS}}(k,k-1)-{\mathrm{\ΔS}}_U^{\mathrm{ODO}}(k,k-1)\\ {\mathrm{\ΔS}}_E^{\mathrm{INS}}(k,k-1)-{\mathrm{\ΔS}}_E^{\mathrm{ODO}}(k,k-1)\end{array}\right]---\left(9\right)$

其中：

$\left[\begin{array}{ccc}{\mathrm{\ΔS}}_N^{\mathrm{INS}}& {\mathrm{\ΔS}}_U^{\mathrm{INS}}& {\mathrm{\ΔS}}_E^{\mathrm{INS}}\end{array}\right]$ 为滤波周期内惯导位移增量；

$\left[\begin{array}{ccc}{\mathrm{\ΔS}}_N^{\mathrm{ODO}}& {\mathrm{\ΔS}}_U^{\mathrm{ODO}}& {\mathrm{\ΔS}}_E^{\mathrm{ODO}}\end{array}\right]$ 为滤波周期内里程计航位推算位移增量。

步骤(4)

完成滤波观测向量计算后，进行里程计信息故障检测，方法如下：

由于车体效应、车轮打滑等现象的存在，里程计测量的轴向位移与车体轴向位移之间存在误差，需要对其故障进行检测及隔离，本发明采用量测序列检测器隔离里程计故障信息，故障检测如式(10)、(11)、(12)。

定义残差：其中Z_k为观测量，H_k为量测矩阵，为一步预测状态阵。

当无故障发生时，卡尔曼滤波器的残差r_k是零均值白噪声，其方差为：

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

其中，P_k/k-1为卡尔曼滤波一步预测协方差矩阵；R为量测噪声协方差阵。

而当量测发生故障时，残差r_k的均值就不再为零了，可通过对残差r_k的检验确定系统是否发生故障。

故障监测函数为：

${\mathrm{\λ}}_k=r_k{A}_k^{-1}{r}_k---\left(11\right)$

其中λ_k服从χ²分布。故障判定准则为：

其中T_D是预先设置的门限，它决定了故障监测的性能。本实施例中，T_D＝0.2。

步骤(5)

完成里程计故障检测后，利用公式(13)进行卡尔曼滤波计算。

采用卡尔曼滤波器对组合导航各项误差进行滤波估计并补偿，即可实现基于位移求和测量的惯导/里程计组合导航。卡尔曼滤波方程如下式。

$\left\{\begin{array}{c}{P}_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k.k-1}^T+Q\\ K_k=P_{k/k-1}{H}_k^T{(H_k{P}_{k/k-1}{H}_k^T+R)}^{-1}\\ {\hat{X}}_k={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}+K_k(Z_k-H_k{\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1})\\ P_k=(I-K_k{H}_k)P_{k/k-1}{(I-K_k{H}_k)}^T+K_k{\mathrm{RK}}_k^T\end{array}\right.---\left(13\right)$

式中：Q为系统噪声的协方差矩阵；R为量测噪声协方差阵；为最优估计矩阵；K_k为滤波增益矩阵；P_k为最优估计协方差矩阵。

步骤(6)

最后利用滤波结果对惯导姿态阵、速度、位置误差进行修正，此修正过程为本领域技术人员公知常识；

对里程计姿态阵进行修正，修正方法如下式所示：

${\hat{C}}_m^n={\hat{C}}_m^n{C}^{\′},C^{\′}=\left[\begin{array}{ccc}1& 0& -\hat{X}\left(17\right)\\ 0& 1& 0\\ \hat{X}\left(17\right)& 0& 1\end{array}\right]---\left(14\right)$

式中，为卡尔曼滤波最优估计中的第17个状态量；

对里程计刻度系数K_D进行修正，修正方法如下式所示：

$K_D=K_D/(1+\hat{X}\left(16\right))---\left(15\right)$

式中，为卡尔曼滤波最优估计中的第16个状态量。

利用本发明的方法在北京六环高速公路进行车载试验。采用陀螺精度优于0.01°/h的激光惯导系统，共进行了12个条次的惯导/里程计组合导航试验，停车状态下，系统准备时间为5min，对准结束后车辆行驶，转入组合导航状态，试验结果如表1所示，图2、3图为其中一条试验曲线图。定位精度即为惯性\里程计自主组合导航结果与GPS的差值。

表1惯导/里程计组合导航试验结果统计

本实施例利用中高精度的激光惯导系统和车载里程仪进行组合，利用GPS信息与自主组合导航结果进行比较，从12个条次的惯导/里程计组合导航试验统计结果来看，相对常规的惯导/里程计速度匹配方法，位移积分匹配组合导航方法的精度更高，所有条次的组合导航定位精度达到0.06％里程(CEP)。

Claims (7)

1.一种惯导/里程计自主组合导航方法，其特征在于：包括以下步骤：

步骤(1)

进行惯导系统初始对准，得到惯导系统初始姿态；惯导系统初始对准结束后转入惯导/里程计组合导航状态，对卡尔曼滤波器参数进行初始化；

步骤(2)

进行惯性导航计算，同时采集里程计脉冲信号，解算惯导速度、位置、姿态信息，里程计姿态阵，并将里程计脉冲信息转换为位移信息，计算里程计航位推算位移增量；同时计算离散状态转移矩阵和量测矩阵；

步骤(3)

导航计算每到卡尔曼滤波周期时刻，采用基于位移误差的求和测量，计算滤波观测量；

步骤(4)

进行里程计信息故障检测；

步骤(5)

进行卡尔曼滤波；

步骤(6)

对惯导姿态阵、速度、位置误差进行修正，对里程计姿态阵进行修正；对里程计刻度系数进行修正；

本方法的坐标系定义如下：

n：导航坐标系oxyz，北天东地理坐标系，x轴指北，y轴指天，z轴指东；

n′：计算导航坐标系o′x′y′z′，x′轴指北，y′轴指天，z′轴指东；

b：惯导载体系o″x″y″z″，与陀螺坐标系重合，前上右坐标系，x″轴指向前，y″轴朝上，z″轴指右；

m：里程计坐标系o″′x″′y″′z″′，前上右坐标系，x″′轴指向前，y″′轴朝上，z″′轴指右；

步骤(1)中，惯导系统初始对准方法采用静基座对准或动基座对准方法；

步骤(2)中，

设里程计位移输出在里程计坐标系m中表示为：

ΔS^m＝[ΔS_x00]^T(1)

则里程计位移在导航坐标系n中表示为：

$\mathrm{\Δ}{S}^n=C_m^n\mathrm{\Δ}{S}^m---\left(2\right)$

里程计导航坐标系n位移误差方程：

$\begin{array}{c}\mathrm{\Δ}{\hat{S}}^n={\hat{C}}_m^n\mathrm{\Δ}{\hat{S}}^m=C_b^{n^{\′}}\mathrm{\Δ}{\hat{S}}^m=C_n^{n^{\′}}{C}_m^n{C}_b^m(1+\mathrm{\δ}{K}_D)\mathrm{\Δ}{S}^m\\ \≈\mathrm{\Δ}{S}^n-\mathrm{\φ}\×\mathrm{\Δ}{S}^n-C_b^n[\mathrm{\Δ}{S}^m\×]{\mathrm{\φ}}_a+\mathrm{\δ}{K}_D\mathrm{\Δ}{S}^n\end{array}---\left(3\right)$

可得：

$\mathrm{\δ\Δ}{S}^n=\mathrm{\Δ}{S}^n\×\mathrm{\φ}-C_b^n[\mathrm{\Δ}{S}^m\×]{\mathrm{\φ}}_a+\mathrm{\δ}{K}_D\mathrm{\Δ}{S}^n---\left(4\right)$

式中：ΔS^m：理想里程计坐标系m输出位移；ΔS_x：理想里程计输出载体前向位移；ΔSⁿ：理想里程计坐标系m输出位移在导航坐标系n中的表示； $\mathrm{\Δ}{\hat{S}}^n={\left[\begin{array}{ccc}\mathrm{\Δ}{S}_N^{\mathrm{ODO}}& \mathrm{\Δ}{S}_U^{\mathrm{ODO}}& \mathrm{\Δ}{S}_E^{\mathrm{ODO}}\end{array}\right]}^T:$ 实际里程计坐标系m输出位移在导航坐标系n投影；惯导载体系b到计算导航坐标系n′的计算姿态转移矩阵；实际里程计坐标系m输出位移；理想导航坐标系n到计算导航坐标系n′的姿态转移矩阵；里程计坐标系m到理想导航坐标系n的姿态转移矩阵；计算里程计坐标系m到计算导航坐标系n′的姿态转移矩阵；惯导载体系b到里程计坐标系m的姿态转移矩阵；δK_D：里程计刻度系数误差；φ＝[φ_Nφ_Uφ_E]^T：惯导失准角；里程计坐标系m位移误差在导航坐标系n中的投影分量；φ_a＝[φ_axφ_ayφ_az]^T：里程计坐标系m与惯导载体系b之间x轴安装误差角，y轴安装误差角，z轴安装偏差角；

设计如下惯导/里程计组合导航系统误差模型：

将以上惯导/里程计组合导航系统误差模型进行离散化得到式(6)所示模型：

$\left\{\begin{array}{c}X\left(k\right)=\mathrm{\Φ}(k,k-1)X(k-1)+Q(k-1)\\ z\left(k\right)=H\left(k\right)x\left(k\right)+R\left(k\right)\end{array}\right.---\left(6\right)$

其中，离散状态转移矩阵为：

$\mathrm{\Φ}(k+n,k)=I_{15\×15}+\underset{i=k}{\overset{k+n}{\mathrm{\Σ}}}{A}_i{T}_n---\left(7\right)$

I_15×15为15维单位阵，T_n为离散周期；

ω_ie：地球自转角速率；

V_N,V_U,V_E：惯导北向、天向、东向速度；

f_N,f_U,f_E：惯导北向、天向、东向加速度；

λ,L,h：惯导经度、纬度、高度；

R_M，R_N：地球子午面、卯酉面半径；

δV_N,δV_U,δV_E：惯导北向、天向、东向速度误差；

φ_N,φ_U,φ_E：惯导北向、天向、东向失准角；

惯导经度、高度、纬度误差；

加速度计零位误差；

ε_x,ε_y,ε_z：陀螺常值漂移；

φ_ay：里程计载体系与惯导载体系之间y轴安装误差角；

量测矩阵为：

$H\left(k\right)=\left\{\underset{i=1}{\overset{k}{\mathrm{\Σ}}}{\left[\begin{array}{ccccc}\begin{array}{ccc}{T}_s& 0& 0\\ 0& T_s& 0\\ 0& 0& T_s\end{array}& -[\left(\mathrm{\Δ}{\hat{S}}^n(i-1,i)\right)\×]& 0_{3\×3}& -\mathrm{\Δ}{\hat{S}}^n(i-1,i)& \begin{array}{c}\left(C_b^n\left(i\right)\right[\left(\mathrm{\Δ}{\hat{S}}^m(i-1,i)\right)\×\left]\right)\left(\mathrm{1,2}\right)\\ \left(C_b^n\left(i\right)\right[\left(\mathrm{\Δ}{\hat{S}}^m(i-1,i)\right)\×]\left(\mathrm{2,2}\right)\\ \left(C_b^n\left(i\right)\right[\left(\mathrm{\Δ}{\hat{S}}^m(i-1,i)\right)\×\left]\right)\left(\mathrm{3,2}\right)\end{array}\end{array}\right]}^T\mathrm{\Φ}(i-1,i)\right\}{\mathrm{\Φ}}^{-1}(k-1,k)---\left(8\right)$

其中，T_s为里程计采样周期。

2.根据权利要求1所述的惯导/里程计自主组合导航方法，其特征在于：步骤(3)中，所述滤波观测量通过下式计算：

$Z\left(k\right)=\left[\begin{array}{c}\mathrm{\δ\Δ}{S}_N^n(k,k-1)\\ \mathrm{\δ\Δ}{S}_N^n(k,k-1)\\ \mathrm{\δ\Δ}{S}_N^n(k,k-1)\end{array}\right]=\left[\begin{array}{c}\mathrm{\Δ}{S}_N^{\mathrm{INS}}(k,k-1)-\mathrm{\Δ}{S}_N^{\mathrm{ODO}}(k,k-1)\\ \mathrm{\Δ}{S}_U^{\mathrm{INS}}(k,k-1)-\mathrm{\Δ}{S}_U^{\mathrm{ODO}}(k,k-1)\\ \mathrm{\Δ}{S}_E^{\mathrm{INS}}(k,k-1)-\mathrm{\Δ}{S}_E^{\mathrm{ODO}}(k,k-1)\end{array}\right]---\left(9\right)$

其中：

$\left[\begin{array}{ccc}\mathrm{\Δ}{S}_N^{\mathrm{INS}}& \mathrm{\Δ}{S}_U^{\mathrm{INS}}& \mathrm{\Δ}{S}_E^{\mathrm{INS}}\end{array}\right]$ 为滤波周期内惯导位移增量；

$\left[\begin{array}{ccc}\mathrm{\Δ}{S}_N^{\mathrm{ODO}}& \mathrm{\Δ}{S}_U^{\mathrm{ODO}}& \mathrm{\Δ}{S}_E^{\mathrm{ODO}}\end{array}\right]$ 为滤波周期内里程计航位推算位移增量。

3.根据权利要求2所述的惯导/里程计自主组合导航方法，其特征在于：步骤(3)中，所述滤波周期为1s。

4.根据权利要求2所述的惯导/里程计自主组合导航方法，其特征在于：步骤(4)中，采用量测序列检测器隔离里程计故障信息；

定义残差：其中Z_k为观测量，H_k为量测矩阵，为一步预测状态阵；

当无故障发生时，卡尔曼滤波器的残差r_k是零均值白噪声，其方差为：

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

其中，P_k/k-1为卡尔曼滤波一步预测协方差矩阵；R为量测噪声协方差阵；

而当量测发生故障时，残差r_k的均值不再为零，通过对残差r_k的检验确定系统是否发生故障；

故障监测函数为：

${\mathrm{\λ}}_k=r_k{A}_k^{-1}{r}_k---\left(11\right)$

其中，λ_k服从χ²分布；

故障判定准则为：

5.根据权利要求4所述的惯导/里程计自主组合导航方法，其特征在于：步骤(4)中，T_D＝0.2。

6.根据权利要求4所述的惯导/里程计自主组合导航方法，其特征在于：步骤(5)中，卡尔曼滤波方程如下所示：

$\left\{\begin{array}{c}{P}_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+Q\\ K_k=P_{k/k-1}{H}_k^T{(H_k{P}_{k/k-1}{H}_k^T+R)}^{-1}\\ {\hat{X}}_k={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}+K_k(Z_k-H_k{\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1})\\ P_k=(I-K_k{H}_k)P_{k/k-1}{(I-K_k{H}_k)}^T+K_k{\mathrm{RK}}_k^T\end{array}\right.---\left(13\right)$

式中：Q为系统噪声的协方差矩阵；R为量测噪声协方差阵；为最优估计矩阵；K_k为滤波增益矩阵；P_k为最优估计协方差矩阵。

7.根据权利要求6所述的惯导/里程计自主组合导航方法，其特征在于：步骤(6)中，

对里程计姿态阵进行修正，修正方法如下式所示：

${\hat{C}}_m^n={\hat{C}}_m^n{C}^{\′},C^{\′}=\left[\begin{array}{ccc}1& 0& -\hat{X}\left(17\right)\\ 0& 1& 0\\ \hat{X}\left(17\right)& 0& 1\end{array}\right]---\left(14\right)$

式中，为卡尔曼滤波最优估计中的第17个状态量；

对里程计刻度系数K_D进行修正，修正方法如下式所示：

$K_D=K_D/(1+\hat{X}\left(16\right))---\left(15\right)$

式中，为卡尔曼滤波最优估计中的第16个状态量。