[发明专利]基于反向姿态跟踪的自主式惯性导航行进间初始对准方法

权利要求书

1.一种基于反向姿态跟踪的自主式惯性导航行进间初始对准方法，其特征在于：它包括以下步骤：

步骤一：建立过渡参考坐标系；

整个对准算法建立的四个重要的过渡参考坐标系为初始时刻惯性坐标系即i₀系、初始时刻地球坐标系即e₀系、初始时刻导航坐标系即n₀系和初始时刻载体坐标系即i_b0系，其定义如下：

(1)初始时刻惯性坐标系即i₀系

初始对准开始时，OX_i0在当地子午面内，与赤道平面平行；OZ_i0指向地球自转轴方向；OY_i0与OX_i0和OZ_i0组成右手螺旋坐标系；初始对准开始后，i₀系三轴相对于惯性空间不变；

(2)初始时刻地球坐标系即e₀系

初始时刻地球坐标系以地球中心为坐标系原点，OX_e0在赤道平面内指向当地子午线方向；OZ_e0指向地球自转轴方向；OY_e0与OX_e0和OZ_e0组成右手螺旋坐标系；该坐标系相对于地球表面静止不动；

(3)初始时刻导航坐标系即n₀系

将初始对准开始时刻的东-北-天导航坐标系作为初始时刻导航坐标系；初始对准开始后，该坐标系相对于地球表面不动；

(4)初始时刻载体坐标系即i_b0系

将初始对准开始时刻的右-前-上载体坐标系作为初始时刻载体坐标系；初始对准开始后，该坐标系相对于惯性空间不动；

步骤二：在载体运动过程中，根据传感器子系统采样数据计算粗略姿态矩阵；

以i₀系为过渡参考坐标系，导航坐标系即n系相对于载体坐标系即b系姿态矩阵的计算分为两个部分，其计算过程如式(1)所示

其中，为n系相对于i₀系的转换矩阵；为i₀系相对于b系的转换矩阵；

a计算矩阵

通过过渡参考坐标系n₀系、e₀系，该矩阵的计算过程又可分为四个部分，即：

式中，为n系相对于地球坐标系即e系的转换矩阵；为e系相对于n₀系的转换矩阵；为n₀系相对于e₀系的转换矩阵；为e₀系相对于i₀系的转换矩阵；

由于在粗对准模块中无法精确获取载体位置，因此，式(2)近似为：

其中，

式中，L₀为初始对准开始时的地球纬度；ω_ie为地球自转角速度；Δt为当前时刻t与初始对准开始时刻t_start的时间差，即Δt＝t-t_start；将式(4)和式(5)代入式(3)中后，得：

b计算矩阵

以i_b0系为过渡参考坐标系，i₀系相对于b系的转换矩阵的计算分为两个部分，即：

其中，为i₀系相对于i_b0系的转换矩阵；为i_b0系相对于b系的转换矩阵；

利用惯性测量元件的测量角速度通过姿态矩阵微分方程(8)直接得到矩阵

式中，由于i_b0系与i₀系均相对于惯性坐标系即i系不动，因此用惯性测量元件的角速度采样数据直接代替另外，在初始对准开始时刻，i_b0系与b系重合，所以矩阵的积分计算初值为单位阵I_3×3；

矩阵的计算需要用到地球重力加速度在惯性系中慢漂的性质；其计算方法如下：

i_b0系和i₀系均相对惯性空间不动，因此易知，矩阵为常值矩阵；对载体速度两边求导得：

其中，为载体加速度在n系中的分量；为载体加速度在b系中的分量；

将其代入惯性导航基本方程中，则惯性导航基本方程可改写如式(10)所示的形式；

其中，gⁿ为地球重力加速度在n系中的分量；为地球自转角速度在n系中的分量；为n系相对于i系的旋转角速度；fⁿ为惯性测量元件的比力采样数据在n系中的分量；

又已知姿态矩阵微分方程其中为b系相对于n系的旋转角速度，将其代入(10)中，得：

式中，忽略不计，上式进一步改写为式(12)；

其中，g^b为地球重力加速度在b系中的分量；f^b为惯性测量元件的比力采样数据；v^b用里程计速度采样数据构成的速度测量矢量代替；而用里程计速度测量矢量微分代替；需要说明的是，由于里程计速度采样数据中含有噪声，直接微分会放大噪声而导致对准精度下降，因此需要使用跟踪微分器对进行微分处理得到噪声干扰较小的

为了获取矩阵需要通过g^b与gⁿ间的重要关系式(13)；

式中，gⁿ＝[0 0 -g]^T为已知量；矩阵均可由前面的计算得到，因此矩阵的求解只需根据式(13)构造三个不在同一平面的向量即可；

为了构造向量，同时削弱噪声，将(13)式两边进行积分处理，得到：

式中，

取两个时刻t₁和t₂的积分值和则由式(15)计算得到；

至此，在载体运动过程中，利用式(6)、(8)和(15)计算得到初始对准结束时刻t_end的粗略姿态矩阵但该姿态矩阵精度不高，且此时不能获取载体位置，因此需要将传感器采样数据保存以做进一步处理；

步骤三：跟踪微分器处理里程计速度微分；

令则有下列非线性跟踪微分器

式中，r为跟踪微分器参数，v(t)为含有噪声的待跟踪信号，在此为里程计速度采样数据；假设η为采样周期，则离散化计算方法如下所示；

至此，得到受干扰噪声污染程度较小的里程计速度微分；

步骤四：利用存储的传感器子系统采样数据，反向姿态跟踪计算初始对准开始时刻姿态矩阵；

反向姿态跟踪算法是为了将粗对准得到的姿态矩阵反向推算回初始对准开始时刻，得到姿态矩阵以便为后面的精对准和位置确定打下基础；

以四元数q表示载体姿态，则姿态的解算由四元数微分方程(18)得到；

其中，

式中，为b系相对于n系的角速度在b系中的分量；为地球自转角速度在n系中的分量；为n系相对于地球坐标系即e系的角速度在n系中的分量；

在姿态的正向解算中，是利用上一时刻k的姿态四元数q(k)和这一时刻k+1的惯性测量元件采样数据去求解k+1时刻的姿态四元数q(k+1)；这一计算过程采用毕卡求解法；毕卡求解法是用角增量来求解姿态四元数的常用方法，其三阶近似计算式为：

式中，T_s为姿态更新周期；

Δθ(k+1)＝||Δθ(k+1)|| (23)

而在反向姿态跟踪计算中，是已知k+1时刻的姿态四元数q(k+1)和k时刻的惯性测量元件角速度采样数据去求解k时刻的姿态四元数q(k)；因此，将式(21)改写为式(24)；

其中，

考虑到粗对准后，载体的位置信息不可知，因此近似取值为同时为了提高Δθ(k)的计算精度，使用里程计速度采样数据来计算其计算过程如下：

其中，

至此，通过式(24)、(25)和(26)将姿态矩阵跟踪回初始对准开始时刻，得到从而为接下来的精对准打好基础；

步骤五：利用存储的传感器子系统采样数据进行惯性导航解算，建立Kalman滤波器进行精对准，获取精确的姿态矩阵和载体位置信息；

a.惯性导航解算与里程计位置推算

以初始对准开始时刻的纬度L₀、经度λ₀、高度h₀以及反向姿态跟踪结果为解算初值，使用存储的传感器采样数据进行姿态、速度和位置解算，得到每个采样点处的惯性导航解算位置p＝[L λ h]^T、惯性导航解算速度惯性导航解算姿态矩阵和里程计推算位置p_D＝[L_D λ_D h_D]^T、推算速度

b.构建滤波状态模型

以n系作为惯性导航的参考坐标系，滤波状态模型如下所示：

式中，为惯性导航平台失准角；δvⁿ为惯性导航速度误差；δp为惯性导航位置误差；ε为导航惯性测量元件的零漂；为惯性导航加速度计的零偏；

δp_D为里程计推算位置误差；δK_D为里程计的刻度因数误差；已知地球赤道半径R_e，矩阵M₁到M₆分别为：

M₂＝M′+M″ (30)

M₄＝(vⁿ×)·(2M′+M″) (32)

滤波状态模型简写为

其中，系统状态转移矩阵F∈R^19×19，其矩阵具体如式(36)所示；G为系统噪声转移阵；W为系统噪声；

c.构建量测模型

将惯性导航解算位置p和里程计推算位置p_D之差作为量测向量Z，将里程计测量噪声导致的推算位置误差建立为量测模型的量测噪声；

Z＝p-p_D＝δp-δp_D+V＝H·X+V (37)

式中，H为量测转移矩阵，H＝[0_3×6 I_3×3 0_3×6 -I_3×3 0_3×1]；V为位置量测噪声；

量测向量Z＝[L-L_D λ-λ_D h-h_D]^T；

最后用滤波器估计出来的惯性导航误差修正解算的惯性导航姿态矩阵得到精确的惯性导航姿态矩阵同时，还得到里程计的精确推算位置p_D；实现了惯性导航精确的姿态对准和位置导航。

2.根据权利要求1所述的一种基于反向姿态跟踪的自主式惯性导航行进间初始对准方法，其特征在于：在步骤二中所述的传感器子系统,它包括惯性测量元件和里程计；惯性测量元件测得载体相对于惯性空间的角速度和比力，得到角速度和比力采样数据；里程计测量载体速度，得到载体速度采样数据。