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

摘要

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

主权项

一种惯导/里程计自主组合导航方法，其特征在于：包括以下步骤：步骤(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_x 0 0]^T     (1)则里程计位移在导航坐标系n中表示为：$\Delta S^n=C_m^n\Delta S^m---\left(2\right)$里程计导航坐标系n位移误差方程：$\begin{array}{c}\Delta {\hat{S}}^n={\hat{C}}_m^n\Delta {\hat{S}}^m=C_b^{n^{\prime }}\Delta {\hat{S}}^m=C_n^{n^{\prime }}{C}_m^n{C}_b^m(1+\delta K_D)\Delta S^m\\ \approx \Delta S^n-\phi \times \Delta S^n-C_b^n[\Delta S^m\times ]{\phi }_a+\delta K_D\Delta S^n\end{array}---\left(3\right)$可得：$\mathrm{\delta \Delta }{S}^n=\Delta S^n\times \phi -C_b^n[\Delta S^m\times ]{\phi }_a+\delta K_D\Delta S^n---\left(4\right)$式中：ΔS^m：理想里程计坐标系m输出位移；ΔS_x：理想里程计输出载体前向位移；ΔSⁿ：理想里程计坐标系m输出位移在导航坐标系n中的表示；$\Delta {\hat{S}}^n={\left[\begin{array}{ccc}\Delta S_N^{\mathrm{ODO}}& \Delta S_U^{\mathrm{ODO}}& \Delta 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)=\Phi (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)$其中，离散状态转移矩阵为：$\Phi (k+n,k)=I_{15\times 15}+\underset{i=k}{\overset{k+n}{\Sigma }}{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}{\Sigma }}{\left[\begin{array}{ccccc}\begin{array}{ccc}{T}_s& 0& 0\\ 0& T_s& 0\\ 0& 0& T_s\end{array}& -[\left(\Delta {\hat{S}}^n(i-1,i)\right)\times ]& 0_{3\times 3}& -\Delta {\hat{S}}^n(i-1,i)& \begin{array}{c}\left(C_b^n\left(i\right)\right[\left(\Delta {\hat{S}}^m(i-1,i)\right)\times \left]\right)\left(\mathrm{1,2}\right)\\ \left(C_b^n\left(i\right)\right[\left(\Delta {\hat{S}}^m(i-1,i)\right)\times ]\left(\mathrm{2,2}\right)\\ \left(C_b^n\left(i\right)\right[\left(\Delta {\hat{S}}^m(i-1,i)\right)\times \left]\right)\left(\mathrm{3,2}\right)\end{array}\end{array}\right]}^T\Phi (i-1,i)\right\}{\Phi }^{-1}(k-1,k)---\left(8\right)$其中，T_s为里程计采样周期。