[发明专利]一种里程计辅助车载捷联惯导系统行进间初始对准方法

权利要求书

1.一种里程计辅助车载捷联惯导系统行进间初始对准方法，其特征在于依次包括粗对准模块和精对准模块两部分，

所述的粗对准模块包括以下步骤：

（11）车辆发动机开启，人员上下车辆做准备工作，惯性测量单元实时地同步采集车辆角速率和比力信息；

（12）运行系统程序，开始进行快速粗对准；

（13）粗对准结束后，得到捷联惯导系统初始姿态矩阵，车辆开始启动，里程计实时采集车辆运动信息；

所述的精对准模块包括以下步骤：

（21）捷联惯导系统利用粗对准得到的姿态矩阵进行速度、位置以及姿态导航解算，同时里程计根据捷联惯导系统提供的初始车辆姿态矩阵进行航位推算，计算得到准确的速度、姿态信息；

（22）将里程计航位推算得到的速度、姿态信息作为量测基准，与捷联惯导系统解算得到的速度、姿态相减作差，以所得差值作为卡尔曼滤波的量测值，列写卡尔曼滤波的状态方程和量测方程；

（23）将状态方程和量测方程离散化，根据给定状态初值以及估计误差初值利用导航计算机递推得到任意时刻的状态估计值，反馈修正捷联惯导系统，完成初始对准。

2.根据权利要求1所述的里程计辅助车载捷联惯导系统行进间初始对准方法，其特征在于：所述粗对准模块应用惯性凝固假设，将比力信息投影到载体惯性坐标系中，得到重力加速度相对惯性空间随地球旋转而引起的方向变化信息，对其积分可以克服车辆发动机振动以及人员上下车等引起的随机干扰影响，求解得到粗略姿态矩阵

3.根据权利要求1所述的里程计辅助车载捷联惯导系统行进间初始对准方法，其特征在于：所述精对准模块应用卡尔曼滤波估计系统误差，里程计与捷联惯导系统中的陀螺仪进行航位推算，将航位推算得到的车辆速度和姿态信息作为卡尔曼滤波量测基准，与捷联惯导系统解算得到的速度、姿态相减，误差值作为卡尔曼滤波器输入，估计得到系统误差后反馈修正捷联惯导系统，求解得到精确姿态矩阵

4.根据权利要求1所述的里程计辅助车载捷联惯导系统行进间初始对准方法，其特征在于：所述步骤（22）中的卡尔曼滤波的状态方程为：

X·(t)=F(t)X(t)+W(t)]]>

其中，t为时间变量，F(t)是系统状态转移矩阵，W(t)是系统过程噪声序列，状态向量取为X=δpTδvnTφTϵgbT▿abTδpDTφDTδkDT,]]>状态向量中对应的变量分别为捷联惯导位置误差向量δp、速度误差向量δvⁿ、数学平台误差向量φ、陀螺仪常值漂移向量加速度计常值偏置向量里程计航位推算位置误差向量δp_D、航位推算数学平台误差向量φ_D和里程计刻度系数误差δk_D；

δp·δv·nφ·ϵ·▿·δp·Dφ·Dδk·D=F11F1203×303×303×303×303×303×1F21F22F2303×3F2403×303×303×1F31F32F33F3403×303×303×303×103×303×303×303×303×303×303×303×303×303×303×303×303×303×303×303×303×303×303×303×303×3F41F42F4303×303×303×3F5103×3F52F53F5401×301×301×301×301×301×301×301×1δpδvnφϵ▿δpDφDδkD+03×1wawg03×103×103×1wg01×1]]>

式中，w_a、w_g分别为加速度计和陀螺仪在载体坐标系下的噪声，即是均值为0、方差为Q(t)、呈正态分布的白噪声；

F11=000vEtanLRN+hsecL00000,F12=01RM+h0secLRN+h00001]]>

F21=2ωiecosLvN+vEvNRN+hsec2L+2ωiesinLvu00-(2ωiecosL+vERN+hsec2L)vE00-2ωiesinLvE02gR]]>

F22=vNRM+htanL-vURM+h2ωiesinL+vERN+htanL-(2ωiecosL+vERN+h)-(2ωiesinL+vERN+htanL)-vURM+h-vNRM+h2(ωiecosL+vERN+h)2vNRM+h0]]>

F23=0-fUfNfU0-fE-fNfE0,F24=Cbn]]>

F31=000-ωiesinL00(ωiecosL+vERN+hsec2L)00,F32=0-1RM+h01RN+h00tanLRN+h00]]>

F33=0ωiesinL+vERN+htanL-(ωiecosL+vERN+h)-(ωiesinL+vERN+h)0-vNRM+hωiecosL+vERN+hvNRM+h0]]>

F34=-Cbn]]>

F41=000vDEtanLDRN+hsecLD00000,F42=vDURM+hD0-vDERM+hD0-secLDRN+hDvDUsecLDRN+hDvDN-vDNvDE0]]>

F43=vDNRM+hDsecLDRN+hDvDEvDUT]]>

F51=-CDbn,F52=000-ωiesinLD00(ωiecosLD+vDEsec2LDRN+hD)00]]>

F53=[M(vDn×)-(ωinn×)],M=0-1RM+hD01RN+hD00tanLDRN+hD00]]>

F54=-vDNRM+hDvDERN+hDvDERN+hDtanLDT]]>

式中，v_E、v_N、v_U分别表示捷联解算得车辆东向、北向、天向速度值；L表示捷联解算得车辆所在点纬度值；h表示捷联解算得车辆所在点高度值；R_M和R_N分别表示子午面曲率半径和卯酉圈曲率半径；ω_ie表示地球自转角速度；v_DE、v_DN、v_DU分别表示航位推算得车辆东向、北向、天向速度值；L_D表示航位推算得车辆所在点纬度值；h_D表示航位推算得车辆所在点高度值；表示捷联惯导姿态更新矩阵；表示航位推算姿态更新矩阵；

f_E为加速度计所测比力分量在导航坐标系中东向投影分量；

f_N为加速度计所测比力分量在导航坐标系中北向投影分量；

f_U为加速度计所测比力分量在导航坐标系中天向投影分量。

5.根据权利要求1所述的里程计辅助车载捷联惯导系统行进间初始对准方法，其特征在于：所述步骤（22）中的卡尔曼滤波的量测方程的计算过程如下：

利用航位推算获得的车辆速度、姿态辅助捷联惯导系统完成精对准，将捷联惯导输出的速度、姿态与航位推算获得的对应信息相减作为辅助对准的量测信息，即：

Z=[ψ_S-ψ_D,θ_S-θ_D,γ_S-γ_D,v_SE-v_DE,v_SN-v_DN,v_SU-v_DU]^T

式中，ψ_S、θ_S和γ_S分别为捷联惯导输出车辆航向角、俯仰角和横摇角，ψ_D、θ_D和γ_D分别为里程计航位推算输出航向角、俯仰角和横摇角，v_SE、v_SN和v_SU分别为捷联惯导输出车辆速度，v_DE、v_DN和v_DU分别为里程计输出车辆速度；

根据捷联惯导、航位推算的姿态角误差与数学平台失准角之间关系，可以推导获得量测方程为：

Z=δvE-vDNφDU+vDUφDN-vDEδkDδvN+vDEφDU-vDUφDE-vDNδkDδvU-vDEφDN+vDNφDE-vDUδkD-T12T32T122+T222φE-T32T22T122+T222φN+φU+R12R32R122+R222φDE+R32+R22R122+R122φDN-φDU-T221-T322φE+T121-T322φN+R221-R322φDE-R121-R322φDNT21T33-T23T31T312+T332φE+T13T31-T11T33T312+T332φN-R21R33-R23R31R312+R332φDE-R13R31-R11R33R312+R332φDN]]>

结合系统状态X，可将辅助对准的量测方程写成如下形式：

Z(t)=H(t)X(t)+V(t)

H(t)=1000000vDU-vDN-vDE010000-vDU0vDE-vDN001000vDN-vDE0-vDU06×3000-T12T32T122+T222-T32T22T122+T222106×9R12R32R122+R222R32R22R122+R222-10000-T221-T322T121-T3220R221-R322-R121-R32200000T21T33-T23T31T312+T332T13T31-T11T33T312+T3320-R21R33-R23R31R312+R332-R13R31-R11R33R312+R33200]]>

其中，H(t)表示量测矩阵，可以由状态方程和量测方程之间关系直接求得，V(t)为量测噪声向量，其方差为R(t)，记捷联惯导姿态更新矩阵里程计航位推算更新矩阵CDbn=[Rij]3×3.]]>