CN106595711A - 一种基于递推四元数的捷联惯性导航系统粗对准方法 - Google Patents

Abstract

本发明公开了一种基于递推四元数的捷联惯性导航系统粗对准方法，该方法采用递推四元数算法来计算凝固法粗对准中的常值姿态矩阵，进而在凝固法粗对准的基础上完成捷联惯性导航粗对准过程。凝固法粗对准将初始姿态矩阵的求取转化为常值姿态矩阵的计算，通过构造观测矢量及参考矢量，采用递推四元数算法可以求取凝固法中的常值姿态矩阵的最优四元数解，进而完成捷联惯性导航粗对准过程。构造不同的观测矢量，本发明可应用于静基座和摇摆基座下初始对准，同样也适用于动基座下初始对准。本发明将递推四元数算法应用于凝固法粗对准过程中有助于提高对准精度及收敛速度。因此，作为一种摇摆基座或动基座的粗对准方法，本发明具有很好的工程参考和应用价值。

Description

一种基于递推四元数的捷联惯性导航系统粗对准方法

技术领域

本发明属于捷联惯性导航技术领域，涉及初始对准，为一种基于递推四元数的捷联惯性导航系统粗对准方法。

背景技术

惯性导航系统(INS)因其自主性好，隐蔽性强及短时精度优等特点在无人化设备以及自主式机器人中得到广泛应用。捷联惯性导航系统的一项关键技术就是初始对准技术。捷联惯性导航的初始对准就是确定初始时刻的姿态矩阵。捷联惯性导航系统初始对准的本质是通过矢量观测来确定姿态，因此基于矢量观测的姿态确定算法都可用来完成初始对准。而如何实现在最短的时间内达到最高的对准精度，是捷联惯性导航初始对准技术所研究的重点内容。

初始对准技术中精度和快速性是两个重要的指标。为了提高系统的对准精度，要求惯性传感器具有尽可能高的精度和稳定性，并要求系统对外界扰动不敏感。显然，精度和快速性这两方面的要求是互相矛盾的，因此需要合理地进行系统设计，尽可能兼顾这两方面的要求，以期求得满意的效果。如何实现在最短的时间内达到最高的对准精度，是捷联惯性导航初始对准技术所研究的重点内容。

秦永元提出了惯性空间凝固假设，将初始对准问题转化为初始对准起始时刻常值姿态矩阵的双矢量确定问题，可有效解决惯性导航摇摆条件下的初始粗对准问题。Itzhack等提出了递推四元数(REQUEST)算法，该算法利用递推处理矢量观测信息来求解姿态矩阵，克服了经典四元数(QUEST)算法单点进行姿态解算的缺陷。

发明内容

本发明的技术解决问题是：针对捷联惯性导航系统凝固法粗对准中的双矢量定姿的精度低，收敛速度慢等问题，提供一种基于递推四元数的捷联惯性导航系统粗对准方法，该方法采用递推四元数算法来计算凝固法粗对准中的常值姿态矩阵，进而在凝固法粗对准的基础上完成捷联惯性导航粗对准过程。

本发明的技术解决方案为：

一种基于递推四元数的捷联惯性导航系统粗对准方法，分为两步，首先利用凝固法粗对准将姿态矩阵的求取转化为常值姿态矩阵的计算，并采用递推四元数算法求取凝固法中的常值姿态矩阵

(1)利用地球自转角速度可以精确计算，并且在某段精确的时间内，重力加速度在惯性空间旋转过的角度可以精确的计算出来。通过引入惯性凝固假设，将姿态矩阵分解为四个坐标变换矩阵，并将姿态矩阵的求解转换为常值姿态矩阵的计算。

(2)利用观测矢量确定常值姿态矩阵的问题可转化为Wahba姿态确定问题，即求解矩阵A使得表达式L(A)取得最小值，

其中{b_i},{r_i}分别观测矢量及对应的参考矢量，a_i为非负权值。并通过递推四元数算法(REQUEST)求解Wahba问题，从而得到常值姿态矩阵的最优四元数解q^*，进而完成粗对准过程。

利用凝固法将姿态矩阵的求取转化为常值姿态矩阵的计算通过以下几个步骤实现

(1)将初始姿态矩阵分解为四个坐标变换矩阵：

其中i₀系是由0时刻的地心地球坐标系凝固而成的惯性坐标系，i_b0系是由0时刻的载体系凝固而成的惯性坐标系；

(2)由步骤(1)得，导航坐标系n系与地心地球坐标系e系之间的坐标转换矩阵为：

(3)由步骤(1)得，地心地球坐标系e系与地心惯性坐标系i₀系与之间的坐标变换矩阵为：

(4)由步骤(1)得，载体坐标系b系与i_b0系与之间的坐标转换矩阵可以利用捷联惯性陀螺仪的输出实时计算，即求解如下的姿态矩阵微分方程：

(5)由步骤(1)(2)(3)(4)得，计算初始姿态矩阵就转化为求解i_b0系和i₀系的姿态转换矩阵

通过递推四元数算法(REQUEST)求解Wahba问题，从而得到常值姿态矩阵的最优四元数解q^*，可由以下几个步骤实现：

(1)将坐标变换矩阵记为矩阵A，求解矩阵A使得表达式L(A)取得最小值

其中{b_i},{r_i}分别观测矢量及对应的参考矢量，a_i为非负权值；

(2)用A(q)表示矩阵A对应的四元数，则步骤(1)中的表达式L(A)可以表示为：

并记

g(q)＝1-J(q) (7)

于是求解最优四元数q使得表达式g(q)达到最大值即可。

(3)由步骤(2)得，g(q)可以表示为：

g(q)＝q^TKq (8)

K矩阵采用迭代方式进行计算：

其中，m_k不必严格等于1，

则可得

δz_k+1＝a_k+1(b_k+1×r_k+1)(19)，

(4)由步骤(3)得，设使得g(q)取得最大值的单位四元数记为q^*，则q^*满足

Kq^*＝λq^* (21)，

其中λ为未知的拉格朗日乘数；

(5)由步骤(3)(4)得，λ为矩阵K的特征值，q^*为矩阵K的对应于特征值λ的特征向量，则表达式g(q^*)满足：

g(q^*)＝λ (22)；

(6)由步骤(5)得，若找到矩阵K的最大特征值λ_max，即使得表达式g(q^*)取得最大值，同时令

y^*＝[(λ_max+σ)I-S]^-1z (23)，

则最优四元数为

得到最优四元数之后就可求解常值姿态矩阵利用凝固法即可计算得到初始时刻姿态矩阵既而完成粗对准过程。

本发明利用凝固法粗对准将初始姿态矩阵的求取转化为常值姿态矩阵的计算，通过构造观测矢量及参考矢量，采用递推四元数算法可以求取凝固法中的常值姿态矩阵的最优四元数解，进而完成捷联惯性导航粗对准过程。构造不同的观测矢量，本发明可应用于静基座和摇摆基座下初始对准，同样也适用于动基座下初始对准。本发明将递推四元数算法应用于凝固法粗对准过程中有助于提高对准精度及收敛速度。

本发明与现有技术相比的优点在于：

(1)本发明通过构造不同的观测矢量及参考矢量，可应用于静基座和摇摆基座下初始对准，同样也适用于动基座下初始对准。

(2)本发明采用递推四元数算法来计算凝固法对准中的常值姿态矩阵，充分利用了初始对准过程中的矢量信息，使得该方法有着更高的对准精度以及更快的收敛速度。

附图说明

图1为本发明捷联惯性导航系统粗对准流程图。

图2为本发明捷联惯性导航系统粗对准原理图。

图3为本发明捷联惯性导航系统粗对准姿态角误差曲线。

图4为图3的局部放大图。

具体实施方式

如图1，本发明的具体实施步骤如下：

1)首先利用凝固法粗对准，将初始姿态矩阵分解为四个矩阵，利用当地纬度信息确定坐标转换矩阵利用地球自转角速度值确定坐标变换矩阵利用陀螺输出数据确定坐标变换矩阵

2)利用加速度计输出数据及重力加速度信息确定观测矢量及参考矢量，单位化处理观测矢量及参考矢量，并确定相应权值；

3)利用步骤2)中所得到的观测矢量及参考矢量来计算当前时刻的K_k矩阵及新息矩阵δK_k+1；

4)利用步骤3)中计算的当前时刻K_k矩阵及新息矩阵δK_k+1来迭代计算下一时刻的K_k+1矩阵；

5)利用步骤4)计算的K_k+1矩阵的基础上，求解下一时刻K_k+1矩阵的最大特征值，并计算最大特征值所对应的特征向量，单位化处理得到最优四元数解；

6)利用步骤5)计算的最优四元数解求解常值姿态矩阵并利用凝固法计算初始姿态矩阵得到精确的姿态角，完成捷联惯性导航系统粗对准。

本发明捷联惯性导航系统粗对准的原理图如图2，步骤2)至步骤6)完成一个姿态解算周期，循环周期，最终实现粗对准过程。

本发明所述的凝固法粗对准按照以下方法进行：

利用凝固法将姿态矩阵的求取转化为常值姿态矩阵的计算通过以下几个步骤实现

将初始姿态矩阵分解为四个坐标变换矩阵：

其中i₀系是由0时刻的地心地球坐标系凝固而成的惯性坐标系，i_b0系是由0时刻的载体系凝固而成的惯性坐标系；

导航坐标系n系与地心地球坐标系e系之间的坐标转换矩阵为：

地心地球坐标系e系与地心惯性坐标系i₀系与之间的坐标变换矩阵为：

载体坐标系b系与i_b0系与之间的坐标转换矩阵可以利用捷联惯性陀螺仪的输出实时计算，即求解如下的姿态矩阵微分方程：

计算初始姿态矩阵就转化为求解i_b0系和i₀系的姿态转换矩阵

本发明的重要特征在于所述的递推四元数算法求解过程，具体如下：

通过递推四元数算法(REQUEST)求解Wahba问题，从而得到常值姿态矩阵的最优四元数解q^*，可由以下几个步骤实现：

将坐标变换矩阵记为矩阵A，求解矩阵A使得表达式L(A)取得最小值

其中{b_i},{r_i}分别观测矢量及对应的参考矢量，a_i为非负权值；

用A(q)表示矩阵A对应的四元数，则步骤(1)中的表达式L(A)可以表示为：

并记

g(q)＝1-J(q) (7)

于是求解最优四元数q使得表达式g(q)达到最大值即可。

g(q)可以表示为：

g(q)＝q^TKq (8)

K矩阵采用迭代方式进行计算：

其中，m_k不必严格等于1，

则可得

δz_k+1＝a_k+1(b_k+1×r_k+1) (19)，

设使得g(q)取得最大值的单位四元数记为q^*，则q^*满足

Kq^*＝λq^* (21)，

其中λ为未知的拉格朗日乘数；

λ为矩阵K的特征值，q^*为矩阵K的对应于特征值λ的特征向量，则表达式g(q^*)满足：

g(q^*)＝λ (22)；

若找到矩阵K的最大特征值λ_max，即使得表达式g(q^*)取得最大值，同时令

y^*＝[(λ_max+σ)I-S]^-1z (23)，

则最优四元数为

得到最优四元数之后就可求解常值姿态矩阵利用凝固法即可计算得到初始时刻姿态矩阵既而完成粗对准过程。

本发明采用一种基于递推四元数的捷联惯性导航系统粗对准方法，下面实验室三轴转台条件下验证该发明的有益效果。

惯性传感器的性能指标如下：陀螺常值漂移：0.04°/h；陀螺随机漂移：0.04°/h；加速度计常值偏置：50μg；加速度计随机偏置：50μg。初始失准角：纵摇失准角0.15°、横摇失准角0.15°、航向失准角0.7°。

实验中，三轴摇摆的运动方式为：内框摆幅为3°，频率为0.15Hz；中框摆幅为3°，频率为0.2Hz；外框摆幅为2°，频率为0.125Hz。三轴同时进行摇摆运动，以此运动状态模拟舰船实际的应用环境，每组试验进行300s。

图3显示了基于递推四元数估计算法的改进型凝固法粗对准姿态角误差曲线。实验结果表明基于递推四元数算法的改进型凝固法粗对准能够提高对准精度，加快收敛速度。

Claims (4)

1.一种基于递推四元数的捷联惯性导航系统粗对准方法，其特征在于：分为两步：

a、通过引入惯性凝固假设，将姿态矩阵分解为四个坐标变换矩阵，并将姿态矩阵的求解转换为常值姿态矩阵的计算；

b、采用递推四元数算法求取凝固法中的常值姿态矩阵即求解矩阵A使得表达式L(A)取得最小值，

$L\left(A\right)=\frac{1}{2}\underset{i=1}{\overset{k}{\Σ}}{a}_i|b_i-{\mathrm{Ar}}_i{|}^2---\left(1\right)$

其中{b_i},{r_i}分别观测矢量及对应的参考矢量，a_i为非负权值；通过递推四元数算法求解，从而得到常值姿态矩阵的最优四元数解q^*，完成粗对准过程。

2.根据权利要求1所述的一种基于递推四元数的捷联惯性导航系统粗对准方法，其特征在于：包括以下步骤：

1)利用凝固法粗对准，将初始姿态矩阵分解为四个矩阵，利用当地纬度信息确定坐标转换矩阵利用地球自转角速度值确定坐标变换矩阵利用陀螺输出数据确定坐标变换矩阵

2)利用加速度计输出数据及重力加速度信息确定观测矢量及参考矢量，单位化处理观测矢量及参考矢量，并确定相应权值；

3)利用步骤2)中所得到的观测矢量及参考矢量来计算当前时刻的K_k矩阵及新息矩阵δK_k+1；

4)利用步骤3)中计算的当前时刻K_k矩阵及新息矩阵δK_k+1来迭代计算下一时刻的K_k+1矩阵；

5)利用步骤4)计算的K_k+1矩阵的基础上，求解下一时刻K_k+1矩阵的最大特征值，并计算最大特征值所对应的特征向量，单位化处理得到最优四元数解；

6)利用步骤5)计算的最优四元数解求解常值姿态矩阵并利用凝固法计算初始姿态矩阵得到精确的姿态角，完成捷联惯性导航系统粗对准；

步骤2)至步骤6)完成一个姿态解算周期，循环周期，实现粗对准过程。

3.根据权利要求1所述的一种基于递推四元数的捷联惯性导航系统粗对准方法，其特征在于：所述步骤a包括以下步骤：

a1、将初始姿态矩阵分解为四个坐标变换矩阵：

$C_b^n=C_e^n{C}_{i_0}^e{C}_{i_{b0}}^{i_0}{C}_b^{i_{b0}}---\left(2\right)$

其中i₀系是由0时刻的地心地球坐标系凝固而成的惯性坐标系，i_b0系是由0时刻的载体系凝固而成的惯性坐标系；

a2、由步骤a1得，导航坐标系n系与地心地球坐标系e系之间的坐标转换矩阵为：

$C_e^n=\left[\begin{array}{ccc}0& 1& 0\\ -\sin L& 0& \cos L\\ \cos L& 0& \sin L\end{array}\right]---\left(3\right)$

a3、由步骤a2得，地心地球坐标系e系与地心惯性坐标系i₀系与之间的坐标变换矩阵为：

$C_{i_0}^e=\left[\begin{array}{ccc}{\mathrm{cos\ω}}_{ie}(t-t_0)& {\mathrm{sin\ω}}_{ie}(t-t_0)& 0\\ -{\mathrm{sin\ω}}_{ie}(t-t_0)& {\mathrm{cos\ω}}_{ie}(t-t_0)& 0\\ 0& 0& 1\end{array}\right]---\left(4\right)$

a4、由步骤a1得，载体坐标系b系与i_b0系与之间的坐标转换矩阵可以利用捷联惯性陀螺仪的输出实时计算，即求解如下的姿态矩阵微分方程：

$\left\{\begin{array}{c}{\stackrel{\·}{C}}_b^{i_{b0}}=C_b^{i_{b0}}\[{\mathrm{\ω}}_{ib}^b\×\]\\ C_b^n\left(t_0\right)=I\end{array}\right.---\left(5\right);$

a5、由步骤a1、a2、a3、a4得，计算初始姿态矩阵就转化为求解i_b0系和i₀系的姿态转换矩阵

4.根据权利要求1中所述的一种基于递推四元数的捷联惯性导航系统粗对准方法，其特征在于：所述步骤b包括以下步骤：

(b1)将坐标变换矩阵记为矩阵A，求解矩阵A使得表达式L(A)取得最小值

$L\left(A\right)=\frac{1}{2}\underset{i=1}{\overset{k}{\Σ}}{a}_i|b_i-{\mathrm{Ar}}_i{|}^2---\left(1\right)$

其中{b_i},{r_i}分别观测矢量及对应的参考矢量，a_i为非负权值；

(b2)用A(q)表示矩阵A对应的四元数，则步骤(b1)中的表达式L(A)可以表示为：

$J\left(q\right)=\frac{1}{2}\underset{i=1}{\overset{n}{\Σ}}{a}_i|b_i-A\left(q\right)r_i{|}^2---\left(6\right)$

并记

g(q)＝1-J(q) (7)

于是求解最优四元数q使得表达式g(q)达到最大值即可；

(b3)由步骤(b2)得，g(q)可以表示为：

g(q)＝q^TKq (8)

K矩阵采用迭代方式进行计算：

$K_{k+1}=\frac{m_k}{m_{k+1}}{K}_k+\frac{1}{m_{k+1}}{\mathrm{\δK}}_{k+1}---\left(9\right)$

其中，

${\mathrm{\σ}}_k=\frac{1}{m_k}\underset{i=1}{\overset{k}{\Σ}}{a}_i{b}_i^T{r}_i---\left(11\right),$

$B_k=\frac{1}{m_k}\underset{i=1}{\overset{k}{\Σ}}{a}_i{b}_i{r}_i^T---\left(12\right),$

$S_k=B_k+B_k^T---\left(13\right),$

$z_k=\frac{1}{m_k}\underset{i=1}{\overset{k}{\Σ}}{a}_i(b_i\×r_i)---\left(14\right),$

则可得

$K_k=\left[\begin{array}{cc}{S}_k-{\mathrm{\σ}}_{k}I& z_k\\ z_k^T& {\mathrm{\σ}}_k\end{array}\right]---\left(15\right);$

${\mathrm{\δ\σ}}_{k+1}=a_{k+1}{b}_{k+1}^T{r}_{k+1}---\left(16\right),$

${\mathrm{\δB}}_{k+1}=a_{k+1}{b}_{k+1}{r}_{k+1}^T---\left(17\right),$

${\mathrm{\δB}}_{k+1}=a_{k+1}{b}_{k+1}{r}_{k+1}^T---\left(18\right),$

δz_k+1＝a_k+1(b_k+1×r_k+1) (19)，

${\mathrm{\δK}}_{k+1}=\left[\begin{array}{cc}{\mathrm{\δS}}_{k+1}-{\mathrm{\δ\σ}}_{k+1}I& {\mathrm{\δz}}_{k+1}\\ {\mathrm{\δz}}_{k+1}^T& {\mathrm{\δ\σ}}_{k+1}\end{array}\right]---\left(20\right);$

(b4)由步骤(b3)得，设使得g(q)取得最大值的单位四元数记为q^*，则q^*满足

Kq^*＝λq^* (21)，

其中λ为未知的拉格朗日乘数；

(b5)由步骤(b3)(b4)得，λ为矩阵K的特征值，q^*为矩阵K的对应于特征值λ的特征向量，则表达式g(q^*)满足：

g(q^*)＝λ (22)；

(b6)由步骤(b5)得，若找到矩阵K的最大特征值λ_max，即使得表达式g(q^*)取得最大值，同时令

y^*＝[(λ_max+σ)I-S]^-1z (23)，

则最优四元数为

$q^{*}=\frac{1}{\sqrt{1+|y^{*}{|}^2}}\left[\begin{array}{c}{y}^{*}\\ 1\end{array}\right]---\left(24\right)$

得到最优四元数之后就可求解常值姿态矩阵利用凝固法即可计算得到初始时刻姿态矩阵既而完成粗对准过程。