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

一、技术领域

本发明提出基于反向姿态跟踪的自主式惯性导航行进间初始对准方法，它涉及一种惯性导航行进间初始对准方法，属于惯性导航初始对准技术领域。

二、背景技术

惯性导航行进间初始对准是指载体在运动过程中完成惯性导航系统初始对准的技术，因此，它是动基座初始对准技术的一种。惯性导航行进间初始对准技术对于增强载体的机动能力和快速反应能力具有不可估量的意义和作用。因此，如何在载体运动过程中实现初始对准是一个值得研究的课题。

与传统的静基座初始对准环境不同，在载体运动状态下，载体的位置、速度、加速度以及角速度都在不断地发生变化，其对初始对准的影响主要具体表现为：一方面，线运动会使惯性导航基本方程中的对地加速度、哥式加速度等参量时刻变化，因此在运动状态下无法利用加速度计输出数据测得重力加速度的精确信息；另一方面，运动条件下载体振动使得干扰角速度具有很宽的频带，陀螺仪输出信号信噪比较低，无法从陀螺仪输出数据中将地球自转角速度这一对准的有用信息提取出来。

可见，在载体运动条件下，就不能单纯依靠陀螺仪和加速度计的直接测量信息进行初始对准，而需要引入测距或测速信息，以补偿运动过程中有害加速度对初始对准精度的影响。目前，惯性导航行进间对准方法目前主要有捷联罗经法、惯性系对准方法以及最优估计对准方法等。捷联罗经法应用了成熟的经典控制理论方法实现行进间初始对准，原理简单但对准时间较长且对陀螺的低频干扰较为敏感，需要根据运动环境选取合适的控制参数。惯性系对准方法以惯性空间为中间过渡坐标系，隔离载体角运动对初始对准的干扰，但该方法仅对测量误差做简单处理，因而对准精度不高且不能获取载体的位置信息。最优估计对准方法建立起惯性导航误差方程，利用里程计等测速传感器的测速信息作为量测信息进行卡尔曼(Kalman)滤波，估计出平台失准角等关键误差从而实现行进间初始对准。该方法多是基于惯性导航线性化误差模型的，需要先在静止条件下获取粗略的初始姿态矩阵才可进行，因此，在一定程度上削弱了载体的机动性优势。

由此可见，各方法各有其特点和适用性。为了既能在运动过程中获取粗略姿态矩阵，又能实现高精度姿态对准和位置导航，本发明单纯以里程计速度采样数据为对准辅助信息提出一种基于反向姿态跟踪的自主式惯性导航行进间对准方法。该方法在仅知道初始位置的条件下，即可实现高精度的惯性导航行进间初始对准。

三、发明内容

针对现有技术中存在的问题，本发明提出基于反向姿态跟踪的自主式惯性导航行进间初始对准方法。首先在载体运动过程中，利用里程计的测速信息为辅助进行粗对准获取粗略姿态矩阵，同时保存惯性测量元件(包括陀螺仪和加速度计)和里程计的采样数据。然后进行反向姿态跟踪求得初始对准开始时刻的姿态矩阵。最后在此基础上，利用保存的惯性测量元件采样数据和里程计速度采样数据进行Kalman滤波精对准。最终实现高精度的姿态对准和位置导航。

本发明所提出的基于反向姿态跟踪的自主式惯性导航行进间初始对准方法，用于车载惯性导航系统，该系统包括传感器子系统、数据存储模块、粗对准计算模块、反向姿态跟踪计算模块和精对准计算模块。它们之间的关系是：传感器子系统的采样数据分别传递给数据存储模块和粗对准计算模块；粗对准计算模块计算出粗略初始姿态矩阵并将其传递给反向姿态跟踪计算模块；反向姿态跟踪计算模块利用数据存储模块的传感器数据进行反向姿态跟踪，并将其计算结果传递给精对准计算模块。精对准计算模块再利用数据存储模块的数据进行精对准计算获取精确对准姿态矩阵和位置导航结果。

本发明提出基于反向姿态跟踪的自主式惯性导航行进间初始对准方法，具体包括以下步骤：

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

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

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

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

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

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

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

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

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

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

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

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

$>C_b^n=C_{i_0}^n·C_b^{i_0}---\left(1\right)$>

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

a计算矩阵

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

$>C_{i_0}^n=C_e^n·C_{n_0}^e·C_{e_0}^{n_0}·C_{i_0}^{e_0}---\left(2\right)$>

式中，为n系相对于地理坐标系(e系)的转换矩阵；为e系相对于n₀系的转换矩阵；

为n₀系相对于e₀系的转换矩阵；为e₀系相对于i₀系的转换矩阵；

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

$>C_{i_0}^n\approx C_{e_0}^{n_0}·C_{i_0}^{e_0}---\left(3\right)$>

其中，

$>C_{e_0}^{n_0}=\left(\begin{array}{ccc}0& 1& 0\\ -{\mathrm{sinL}}_0& 0& {\mathrm{cosL}}_0\\ {\mathrm{cosL}}_0& 0& {\mathrm{sinL}}_0\end{array}\right)---\left(4\right)$>

$>C_{i_0}^{e_0}=\left(\begin{array}{ccc}cos({\omega }_{ie}·\Delta t)& sin({\omega }_{ie}·\Delta t)& 0\\ -sin({\omega }_{ie}·\Delta t)& cos({\omega }_{ie}·\Delta t)& 0\\ 0& 0& 1\end{array}\right)---\left(5\right)$>

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

$>C_{i_0}^n=\left(\begin{array}{ccc}-\sin ({\omega }_{ie}·\Delta t)& \cos ({\omega }_{ie}·\Delta t)& 0\\ -{\mathrm{sinL}}_0·\cos ({\omega }_{ie}·\Delta t)& -{\mathrm{sinL}}_0·\sin ({\omega }_{ie}·\Delta t)& {\mathrm{cosL}}_0\\ {\mathrm{cosL}}_0·\cos ({\omega }_{ie}·\Delta t)& {\mathrm{cosL}}_0·\sin ({\omega }_{ie}·\Delta t)& {\mathrm{sinL}}_0\end{array}\right)---\left(6\right)$>

b计算矩阵

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

$>C_b^{i_0}=C_{i_{b0}}^{i_0}·C_b^{i_{b0}}---\left(7\right)$>

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

利用陀螺仪的测量角速度通过姿态矩阵微分方程(8)可直接得到矩阵

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

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

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

$>{\stackrel{·}{v}}^n={\stackrel{·}{C}}_b^n·v^b+C_b^n·{\stackrel{·}{v}}^b---\left(9\right)$>

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

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

$>{\stackrel{·}{C}}_b^n·v^b+C_b^n·{\stackrel{·}{v}}^b+({\omega }_{ie}^n+{\omega }_{in}^n)\times v^n-f^n=g^n---\left(10\right)$>

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

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

$>C_b^n·{\omega }_{nb}^b\times v^b+C_b^n·{\stackrel{·}{v}}^b+({\omega }_{ie}^n+{\omega }_{in}^n)\times v^n-f^n=g^n---\left(11\right)$>

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

$>{\stackrel{·}{v}}^b+{\omega }_{ib}^b\times v^b-f^b=g^b---\left(12\right)$>

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

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

$>C_b^{i_{b0}}·g^b=C_{i_0}^{i_{b0}}·C_n^{i_0}·g^n---\left(13\right)$>

式中，地球重力加速度在b系下的分量gⁿ＝[00-g]^T为已知量；矩阵均可由前面的计算得到，因此矩阵的求解只需根据式(13)构造三个不在同一平面的向量即可；

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

$>z_t^{i_{b0}}=C_{i_0}^{i_{b0}}·r_t^{i_0}---\left(14\right)$>

式中，

得到转换矩阵需要通过g^b与gⁿ间的重要关系式

$>C_b^{i_{b0}}·g^b=C_{i_0}^{i_{b0}}·C_n^{i_0}·g^n---\left(15\right)$>

其中，均可由前面的计算得到；将上式两边积分得

$>z_t^{i_{b0}}=C_{i_0}^{i_{b0}}·r_t^{i_0}---\left(16\right)$>

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

$>C_{i_0}^{i_{b_0}}=\left(\begin{array}{c}{\left(z_{t_1}^{i_{b0}}\right)}^T\\ {(z_{t_1}^{i_{b0}}\times z_{t_2}^{i_{b0}})}^T\\ {(z_{t_1}^{i_{b0}}\times z_{t_2}^{i_{b0}}\times z_{t_1}^{i_{b0}})}^T\end{array}\right)·{\left(\begin{array}{c}{\left(r_{t_1}^{i_0}\right)}^T\\ {(r_{t_1}^{i_0}\times r_{t_2}^{i_0})}^T\\ {(r_{t_1}^{i_0}\times r_{t_2}^{i_0}\times r_{t_1}^{i_0})}^T\end{array}\right)}^{-1}---\left(17\right)$>

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

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

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

$>\left(\begin{array}{c}{\stackrel{·}{x}}_1=x_2\\ {\stackrel{·}{x}}_2=-r·sign(x_1-v(t)+\frac{x_2\left|x_2\right|}{2r})\end{array}\right)---\left(18\right)$>

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

$>\left(\begin{array}{c}{x}_1(k+1)=x_1\left(k\right)+\eta ·x_2\left(k\right)\\ x_2(k+1)=x_2\left(k\right)-\eta ·r·sign\left(x_1\right(k)-v(k+1)+\frac{x_2\left(k\right)\left|x_2\left(k\right)\right|}{2r})\end{array}\right)---\left(19\right)$>

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

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

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

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

$>\stackrel{·}{q}=-\frac{1}{2}·{\Omega }_{nb}^b·q---\left(20\right)$>

其中，

$>{\Omega }_{nb}^b=\left(\begin{array}{cccc}0& -{\omega }_{nbx}^b& -{\omega }_{nby}^b& -{\omega }_{nbz}^b\\ {\omega }_{nbx}^b& 0& {\omega }_{nbz}^b& -{\omega }_{nby}^b\\ {\omega }_{nby}^b& -{\omega }_{nbz}^b& 0& {\omega }_{nbx}^b\\ {\omega }_{nbz}^b& {\omega }_{nby}^b& -{\omega }_{nbx}^b& 0\end{array}\right)---\left(21\right)$>

$>{\omega }_{nb}^b=\left(\begin{array}{c}{\omega }_{nbx}^b\\ {\omega }_{nby}^b\\ {\omega }_{nbz}^b\end{array}\right)={\omega }_{ib}^b-C_n^b.({\omega }_{ie}^n+{\omega }_{en}^n)---\left(22\right)$>

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

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

$>q(k+1)=[I(1-\frac{\Delta \theta {(k+1)}^2}{8})+(\frac{1}{2}-\frac{\Delta \theta {(k+1)}^2}{48})·{\Omega }_{nb}^b(k+1)·T_s]·q\left(k\right)---\left(23\right)$>

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

$>\begin{array}{c}\Delta \theta (k+1)={\omega }_{nb}^b(k+1)·T_s\\ ={\omega }_{ib}^b(k+1)·T_s-C_n^b\left(k\right)·[{\omega }_{ie}^n\left(k\right)+{\omega }_{en}^n\left(k\right)]·T_s\end{array}---\left(24\right)$>

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

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

$>\begin{array}{c}q\left(k\right)={[I(1-\frac{\Delta \theta {(k+1)}^2}{8})+(\frac{1}{2}-\frac{\Delta \theta {(k+1)}^2}{48})·{\Omega }_{nb}^b(k+1)·T_s]}^{-1}·q(k+1)\\ \approx {[I(1-\frac{\Delta \theta {\left(k\right)}^2}{8})+(\frac{1}{2}-\frac{\Delta \theta {\left(k\right)}^2}{48})·{\Omega }_{nb}^b\left(k\right)·T_s]}^{-1}·q(k+1)\end{array}---\left(26\right)$>

其中，

$>\begin{array}{c}\Delta \theta \left(k\right)={\omega }_{nb}^b\left(k\right)·T_s\\ ={\omega }_{ib}^b\left(k\right)·T_s-C_n^b(k+1)·[{\omega }_{ie}^n(k+1)+{\omega }_{en}^n(k+1)]·T_s\end{array}---\left(27\right)$>

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

$>{\omega }_{en}^n(k+1)={\left(\begin{array}{ccc}-\frac{v_{DN}^n\left(k\right)}{R_e}& \frac{v_{DE}^n\left(k\right)}{R_e}& \frac{v_{DE}^n\left(k\right)·{\mathrm{tanL}}_0}{R_e}\end{array}\right)}^T---\left(28\right)$>

其中，

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

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

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

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

b.构建滤波状态模型

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

式中，为惯性导航平台失准角；δvⁿ为惯性导航速度误差；δ_p为惯性导航位置误差；ε为惯性导航陀螺仪的零漂；为惯性导航加速度计的零偏；δp_D为里程计推算位置误差；δK_D为里程计的刻度因数误差；已知地球赤道半径R_e，矩阵M₁到M₆分别为：

$>M_1=\left(\begin{array}{ccc}0& -\frac{1}{R_e}& 0\\ \frac{1}{R_e}& 0& 0\\ \frac{\tan L}{R_e}& 0& 0\end{array}\right)---\left(30\right)$>

$>M^{\prime }=\left(\begin{array}{ccc}0& 0& 0\\ -{\omega }_{ie}\sin L& 0& 0\\ {\omega }_{ie}\cos L& 0& 0\end{array}\right);M^{\prime \prime }=\left(\begin{array}{ccc}0& 0& \frac{v_N^n}{{R_e}^2}\\ 0& 0& -\frac{v_E^n}{{R_e}^2}\\ \frac{v_E^n{\sec }^{2}L}{R_e}& 0& -\frac{v_E^n\tan L}{{R_e}^2}\end{array}\right)---\left(31\right)$>

M₂＝M′+M″(32)

$>M_3=(v^n\times )·M_1-\left(\right(2{\omega }_{ie}^n+{\omega }_{en}^n)\times )---\left(33\right)$>

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

$>M_5=\left(\begin{array}{ccc}0& \frac{1}{R_e}& 0\\ \frac{\sec L}{R_e}& 0& 0\\ 0& 0& 1\end{array}\right)---\left(35\right)$>

$>M_6=\left(\begin{array}{ccc}0& 0& \frac{v_N^n}{{R_e}^2}\\ \frac{v_E^n\tan L\sec L}{R_e}& 0& -\frac{v_E^n\sec L}{{R_e}^2}\\ 0& 0& 0\end{array}\right)---\left(36\right)$>

滤波状态模型简写为

$>\stackrel{·}{X}=F·X+G·W---\left(37\right)$>

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

$>F=\left(\begin{array}{ccccccc}-{\omega }_{in}^n\times & M_1& M_2& -C_b^n& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 1}\\ f^n\times & M_3& M_4& 0_{3\times 3}& C_b^n& 0_{3\times 3}& 0_{3\times 1}\\ 0_{3\times 3}& M_5& M_6& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 1}\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 1}\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 1}\\ M_5·(v_D^n\times )& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& M_6& v_D^n\\ 0_{1\times 3}& 0_{1\times 3}& 0_{1\times 3}& 0_{1\times 3}& 0_{1\times 3}& 0_{1\times 3}& 0_{1\times 1}\end{array}\right)---\left(38\right)$>

c.构建量测模型

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

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

式中，H为量测转移矩阵，H＝[0_3×6I_3×30_3×6-I_3×30_3×1]；V为位置量测噪声；量测向量Z＝[L-L_Dλ-λ_Dh-h_D]^T；

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

其中，在步骤二中，所述的“传感器子系统”包括惯性测量元件和里程计。惯性测量元件测得载体相对于惯性空间角速度和比力，得到角速度和比力采样数据；里程计测量载体速度，得到载体速度采样数据。

通过以上步骤，本发明所提出的基于反向姿态跟踪的自主式惯性导航行进间初始对准方法，充分利用了传感器采样数据，在仅知道载体初始位置的条件下，即可实现行进间的精确对准姿态阵获取和位置导航，大大提高了载体机动能力，是一种行之有效的行进间初始对准方法。

本发明的优点在于：

(1)本发明提出基于反向姿态跟踪的自主式惯性导航行进间初始对准方法，通过反向姿态跟踪，将粗对准与精对准进行有机结合，充分利用传感器子系统采样数据，能够实现精确对准姿态矩阵的获取和位置导航；

(2)本发明提出基于反向姿态跟踪的自主式惯性导航行进间初始对准方法，利用Kalman滤波精对准，对惯性测量元件误差(加速度计零偏、陀螺仪零漂)以及里程计标度因数误差进行最优估计，能够为之后的导航计算提供补偿参数；

(3)本发明提出基于反向姿态跟踪的自主式惯性导航行进间初始对准方法，在仅知道载体初始对准开始时刻位置的条件下，即可实现姿态对准和位置导航，大大提高了载体的机动性能，具有良好的应用前景；

四、附图说明

图1为本发明提出的基于反向姿态跟踪的自主式惯性导航行进间初始对准方法结构示意图。

图2为本发明所述方法流程框图。

图中序号、符号、代号说明如下：

1—传感器子系统2—数据存储模块3—粗对准计算模块

4—反向姿态跟踪计算模块5—精对准计算模块

101—惯性测量元件102—里程计

201—数据存储单元

301—粗对准计算单元

401—反向姿态跟踪计算单元

501—惯性导航解算单元502—里程计位置推算单元503—Kalman滤波单元

—角速度f^b—比力—里程计测得的载体速度

—粗对准计算模块所确定的姿态矩阵

—反向姿态跟踪计算模块所确定的姿态矩阵

p—惯性导航解算位置p_D—里程计推算位置—精对准所确定的对准姿态矩阵

五、具体实施方式

下面将结合附图对本发明作进一步的详细说明。

本发明提出基于反向姿态跟踪的自主式惯性导航行进间初始对准方法。首先在载体运动过程中，利用里程计的测速信息为辅助进行粗对准获取粗略姿态矩阵同时保存惯性测量元件和里程计的采样数据。然后进行反向姿态跟踪求得初始对准开始时刻的姿态矩阵最后在此基础上，利用保存的惯性测量元件采样数据和里程计速度采样数据进行Kalman滤波精对准。最终，获取高精度的对准姿态矩阵并实现载体位置导航。

见图1，本发明提出基于反向姿态跟踪的自主式惯性导航行进间初始对准方法，包括传感器子系统1、数据存储模块2、粗对准计算模块3、反向姿态跟踪计算模块4和精对准计算模块5。

传感器子系统系统的采样数据分别传递给数据存储模块2和粗对准计算模块3；粗对准计算模块3计算出粗略初始姿态矩阵并将其传递给反向姿态跟踪计算模块4；反向姿态跟踪计算模块4利用数据存储模块2的传感器数据进行反向姿态跟踪，并将其计算结果传递给精对准计算模块5。精对准计算模块5再利用数据存储模块2的数据进行精对准计算获取精确对准姿态矩阵和位置导航结果。

见图2，本发明提出基于反向姿态跟踪的自主式惯性导航行进间初始对准方法，具体包括以下步骤：

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

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

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

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

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

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

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

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

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

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

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

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

$>C_b^n=C_{i_0}^n·C_b^{i_0}---\left(1\right)$>

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

a计算矩阵

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

$>C_{i_0}^n=C_e^n·C_{n_0}^e·C_{e_0}^{n_0}·C_{i_0}^{e_0}---\left(2\right)$>

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

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

$>C_{i_0}^n\approx C_{e_0}^{n_0}·C_{i_0}^{e_0}---\left(3\right)$>

其中，

$>C_{e_0}^{n_0}=\left(\begin{array}{ccc}0& 1& 0\\ -{\mathrm{sinL}}_0& 0& {\mathrm{cosL}}_0\\ {\mathrm{cosL}}_0& 0& {\mathrm{sinL}}_0\end{array}\right)---\left(4\right)$>

$>C_{i_0}^{e_0}=\left(\begin{array}{ccc}cos({\omega }_{ie}·\Delta t)& sin({\omega }_{ie}·\Delta t)& 0\\ -sin({\omega }_{ie}·\Delta t)& cos({\omega }_{ie}·\Delta t)& 0\\ 0& 0& 1\end{array}\right)---\left(5\right)$>

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

$>C_{i_0}^n=\left(\begin{array}{ccc}-\sin ({\omega }_{ie}·\Delta t)& \cos ({\omega }_{ie}·\Delta t)& 0\\ -{\mathrm{sinL}}_0·\cos ({\omega }_{ie}·\Delta t)& -{\mathrm{sinL}}_0·\sin ({\omega }_{ie}·\Delta t)& {\mathrm{cosL}}_0\\ {\mathrm{cosL}}_0·\cos ({\omega }_{ie}·\Delta t)& {\mathrm{cosL}}_0·\sin ({\omega }_{ie}·\Delta t)& {\mathrm{sinL}}_0\end{array}\right)---\left(6\right)$>

b计算矩阵

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

$>C_b^{i_0}=C_{i_{b0}}^{i_0}·C_b^{i_{b0}}---\left(7\right)$>

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

利用陀螺仪的测量角速度通过姿态矩阵微分方程(8)可直接得到矩阵

$>{\stackrel{·}{C}}_b^{i_{b0}}=C_b^{i_{b0}}·[{\omega }_{i_{b0}b}^b\times ]=C_b^{i_{b0}}·[{\omega }_{ib}^b\times ]---\left(8\right)$>

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

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

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

$>{\stackrel{·}{v}}^n={\stackrel{·}{C}}_b^n·v^b+C_b^n·{\stackrel{·}{v}}^b---\left(9\right)$>

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

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

$>{\stackrel{·}{C}}_b^n·v^b+C_b^n·{\stackrel{·}{v}}^b+({\omega }_{ie}^n+{\omega }_{in}^n)\times v^n-f^n=g^n---\left(10\right)$>

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

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

$>C_b^n·{\omega }_{nb}^b\times v^b+C_b^n·{\stackrel{·}{v}}^b+({\omega }_{ie}^n+{\omega }_{in}^n)\times v^n-f^n=g^n---\left(11\right)$>

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

$>{\stackrel{·}{v}}^b+{\omega }_{ib}^b\times v^b-f^b=g^b---\left(12\right)$>

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

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

$>C_b^{i_{b0}}·g^b=C_{i_0}^{i_{b0}}·C_n^{i_0}·g^n---\left(13\right)$>

式中，地球重力加速度在b系下的分量gⁿ＝[00-g]^T为已知量；矩阵均可由前面的计算得到，因此矩阵的求解只需根据式(13)构造三个不在同一平面的向量即可；

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

$>z_t^{i_{b0}}=C_{i_0}^{i_{b0}}·r_t^{i_0}---\left(14\right)$>

式中，

得到转换矩阵需要通过g^b与gⁿ间的重要关系式

$>C_b^{i_{b0}}·g^b=C_{i_0}^{i_{b0}}·C_n^{i_0}·g^n---\left(15\right)$>

其中，均可由前面的计算得到；将上式两边积分得

$>z_t^{i_{b0}}=C_{i_0}^{i_{b0}}·r_t^{i_0}---\left(16\right)$>

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

$>C_{i_0}^{i_{b_0}}=\left(\begin{array}{c}{\left(z_{t_1}^{i_{b0}}\right)}^T\\ {(z_{t_1}^{i_{b0}}\times z_{t_2}^{i_{b0}})}^T\\ {(z_{t_1}^{i_{b0}}\times z_{t_2}^{i_{b0}}\times z_{t_1}^{i_{b0}})}^T\end{array}\right)·{\left(\begin{array}{c}{\left(r_{t_1}^{i_0}\right)}^T\\ {(r_{t_1}^{i_0}\times r_{t_2}^{i_0})}^T\\ {(r_{t_1}^{i_0}\times r_{t_2}^{i_0}\times r_{t_1}^{i_0})}^T\end{array}\right)}^{-1}---\left(17\right)$>

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

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

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

$>\left(\begin{array}{c}{\stackrel{·}{x}}_1=x_2\\ {\stackrel{·}{x}}_2=-r·sign(x_1-v(t)+\frac{x_2\left|x_2\right|}{2r})\end{array}\right)---\left(18\right)$>

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

$>\left(\begin{array}{c}{x}_1(k+1)=x_1\left(k\right)+\eta ·x_2\left(k\right)\\ x_2(k+1)=x_2\left(k\right)-\eta ·r·sign\left(x_1\right(k)-v(k+1)+\frac{x_2\left(k\right)\left|x_2\left(k\right)\right|}{2r})\end{array}\right)---\left(19\right)$>

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

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

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

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

$>\stackrel{·}{q}=-\frac{1}{2}·{\Omega }_{nb}^b·q---\left(20\right)$>

其中，

$>{\Omega }_{nb}^b=\left(\begin{array}{cccc}0& -{\omega }_{nbx}^b& -{\omega }_{nby}^b& -{\omega }_{nbz}^b\\ {\omega }_{nbx}^b& 0& {\omega }_{nbz}^b& -{\omega }_{nby}^b\\ {\omega }_{nby}^b& -{\omega }_{nbz}^b& 0& {\omega }_{nbx}^b\\ {\omega }_{nbz}^b& {\omega }_{nby}^b& -{\omega }_{nbx}^b& 0\end{array}\right)---\left(21\right)$>

$>{\omega }_{nb}^b=\left(\begin{array}{c}{\omega }_{nbx}^b\\ {\omega }_{nby}^b\\ {\omega }_{nbz}^b\end{array}\right)={\omega }_{ib}^b-C_n^b·({\omega }_{ie}^n+{\omega }_{en}^n)---\left(22\right)$>

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

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

$>q(k+1)=[I(1-\frac{\Delta \theta {(k+1)}^2}{8})+(\frac{1}{2}-\frac{\Delta \theta {(k+1)}^2}{48})·{\Omega }_{nb}^b(k+1)·T_s]·q\left(k\right)---\left(23\right)$>

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

$>\begin{array}{c}\Delta \theta (k+1)={\omega }_{nb}^b(k+1)·T_s\\ ={\omega }_{ib}^b(k+1)·T_s-C_n^b\left(k\right)·[{\omega }_{ie}^n\left(k\right)+{\omega }_{en}^n\left(k\right)]·T_s\end{array}---\left(24\right)$>

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

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

$>\begin{array}{c}q\left(k\right)={[I(1-\frac{\Delta \theta {(k+1)}^2}{8})+(\frac{1}{2}-\frac{\Delta \theta {(k+1)}^2}{48})·{\Omega }_{nb}^b(k+1)·T_s]}^{-1}·q(k+1)\\ \approx {[I(1-\frac{\Delta \theta {\left(k\right)}^2}{8})+(\frac{1}{2}-\frac{\Delta \theta {\left(k\right)}^2}{48})·{\Omega }_{nb}^b\left(k\right)·T_s]}^{-1}·q(k+1)\end{array}---\left(26\right)$>

其中，

$>\begin{array}{c}\Delta \theta \left(k\right)={\omega }_{nb}^b\left(k\right)·T_s\\ ={\omega }_{ib}^b\left(k\right)·T_s-C_n^b(k+1)·[{\omega }_{ie}^n(k+1)+{\omega }_{en}^n(k+1)]·T_s\end{array}---\left(27\right)$>

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

$>{\omega }_{en}^n(k+1)={\left(\begin{array}{ccc}-\frac{v_{DN}^n\left(k\right)}{R_e}& \frac{v_{DE}^n\left(k\right)}{R_e}& \frac{v_{DE}^n\left(k\right)·{\mathrm{tanL}}_0}{R_e}\end{array}\right)}^T---\left(28\right)$>

其中，

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

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

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

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

b.构建滤波状态模型

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

式中，为惯性导航平台失准角；δvⁿ为惯性导航速度误差；δp为惯性导航位置误差；ε为惯性导航陀螺仪的零漂；为惯性导航加速度计的零偏；δp_D为里程计推算位置误差；δK_D为里程计的刻度因数误差；已知地球赤道半径R_e，矩阵M₁到M₆分别为：

$>M_1=\left(\begin{array}{ccc}0& -\frac{1}{R_e}& 0\\ \frac{1}{R_e}& 0& 0\\ \frac{\tan L}{R_e}& 0& 0\end{array}\right)---\left(30\right)$>

$>M^{\prime }=\left(\begin{array}{ccc}0& 0& 0\\ -{\omega }_{ie}\sin L& 0& 0\\ {\omega }_{ie}\cos L& 0& 0\end{array}\right);M^{\prime \prime }=\left(\begin{array}{ccc}0& 0& \frac{v_N^n}{{R_e}^2}\\ 0& 0& -\frac{v_E^n}{{R_e}^2}\\ \frac{v_E^n{\sec }^{2}L}{R_e}& 0& -\frac{v_E^n\tan L}{{R_e}^2}\end{array}\right)---\left(31\right)$>

M₂＝M′+M″(32)

$>M_3=(v^n\times )·M_1-\left(\right(2{\omega }_{ie}^n+{\omega }_{en}^n)\times )---\left(33\right)$>

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

$>M_5=\left(\begin{array}{ccc}0& \frac{1}{R_e}& 0\\ \frac{\sec L}{R_e}& 0& 0\\ 0& 0& 1\end{array}\right)---\left(35\right)$>

$>M_6=\left(\begin{array}{ccc}0& 0& \frac{v_N^n}{{R_e}^2}\\ \frac{v_E^n\tan L\sec L}{R_e}& 0& -\frac{v_E^n\sec L}{{R_e}^2}\\ 0& 0& 0\end{array}\right)---\left(36\right)$>

滤波状态模型简写为

$>\stackrel{·}{X}=F·X+G·W---\left(37\right)$>

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

$>F=\left(\begin{array}{ccccccc}-{\omega }_{in}^n\times & M_1& M_2& -C_b^n& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 1}\\ f^n\times & M_3& M_4& 0_{3\times 3}& C_b^n& 0_{3\times 3}& 0_{3\times 1}\\ 0_{3\times 3}& M_5& M_6& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 1}\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 1}\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 1}\\ M_5·(v_D^n\times )& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& M_6& v_D^n\\ 0_{1\times 3}& 0_{1\times 3}& 0_{1\times 3}& 0_{1\times 3}& 0_{1\times 3}& 0_{1\times 3}& 0_{1\times 1}\end{array}\right)---\left(38\right)$>

c.构建量测模型

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

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

式中，H为量测转移矩阵，H＝[0_3×6I_3×30_3×6-I_3×30_3×1]；V为位置量测噪声；量测向量Z＝[L-L_Dλ-λ_Dh-h_D]^T；

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

其中，在步骤二中，所述的“传感器子系统”包括惯性测量元件和里程计。惯性测量元件测得载体相对于惯性空间的角速度和比力，得到角速度和比力采样数据；里程计测量载体速度，得到载体速度采样数据。

通过以上步骤，本发明所提出的基于反向姿态跟踪的自主式惯性导航行进间初始对准方法，充分利用了传感器采样数据，在仅知道载体初始位置的条件下，即可实现行进间的精确对准姿态阵获取和位置导航，大大提高了载体机动能力，是一种行之有效的行进间初始对准方法。