CN105659817B - 一种捷联惯性系统全姿态初始对准方法 - Google Patents

Abstract

本发明公开了一种捷联惯性系统全姿态初始对准方法，这种方法首先需要建立捷联惯性系统静基座初始对准方程和求解基于四元数的姿态变换矩阵，然后建立状态变量为四元数的捷联系统静基座初始对准时的二阶非线性方程组，对6个非线性方程组求解q₀、q₁、q₂和q₃四个元素，并根据四元数和旋转矢量之间的关系转换为旋转矢量法定义的三个姿态角θ_x、θ_y、θ_z，这样就可实现捷联系统的初始对准。相比通过以克雷罗夫角法进行初始对准的方法，本发明的初始对准方法具有与旋转顺序无关、可实现全姿态对准的优点。<pb pnum="1" />

Description

一种捷联惯性系统全姿态初始对准方法

技术领域

本发明涉及一种捷联惯性系统全姿态初始对准方法，尤其涉及一种用于捷联惯性系统的间接式初始对准方法。

背景技术

如图1所示，以地理坐标系为导航坐标系的惯导系统中(包括平台式和捷联式)，物理平台和数学平台都是测量加速度的基准，必须准确地跟踪地理坐标系，以避免由于基准误差引起的视加速度测量误差。在惯导系统加电启动后，平台的三轴指向是任意的，既不在水平面内，又没有确定的方位，因此在系统进入导航工作状态前，必须将平台的指向对准，此过程便称为惯导系统的初始对准。

目前，在捷联惯性系统姿态角解算中主要有以下三种：方向余弦法、欧拉-克雷罗夫角法以及四元数法。

基于方向余弦阵的姿态微分方程从上式可以看出，方向余弦法直接对坐标变换矩阵进行解算，而不能直接得到三个姿态角。

用克雷罗夫角表示的坐标变换如图2所示，图2中OX^LY^LZ^L表示导航坐标系，OX^bY^bZ^b为载体坐标系。图2中第一步由OX^LY^LZ^L绕Z^L转动γ角到OX₁Y₁Z₁，第二步由OX₁Y₁Z₁绕X₁转动φ_x角到OX₂Y₂Z₂。第三步由OX₂Y₂Z₂绕Y₂转动φ_y角到OX^bY^bZ^b。基于克雷罗夫角的姿态微分方程：

$\left[\begin{array}{c}{\stackrel{\&CenterDot;}{\mathrm{\&phi;}}}_x\\ {\stackrel{\&CenterDot;}{\mathrm{\&phi;}}}_y\\ \stackrel{\&CenterDot;}{\mathrm{\&gamma;}}\end{array}\right]=\left[\begin{array}{ccc}{\mathrm{cos\&phi;}}_y& 0& {\mathrm{sin\&phi;}}_y\\ {\mathrm{sin\&phi;}}_y{\mathrm{tan\&phi;}}_x& 1& -{\mathrm{cos\&phi;}}_y{\mathrm{tan\&phi;}}_x\\ -{\mathrm{sin\&phi;}}_y{\mathrm{sec\&phi;}}_x& 0& {\mathrm{cos\&phi;}}_y{\mathrm{sec\&phi;}}_x\end{array}\right]{\stackrel{\&RightArrow;}{\mathrm{\&omega;}}}_{ib}^b-\left[\begin{array}{ccc}cos\mathrm{\&gamma;}& sin\mathrm{\&gamma;}& 0\\ -{\mathrm{sin\&gamma;sec\&phi;}}_x& {\mathrm{cos\&gamma;sec\&phi;}}_x& 0\\ {\mathrm{sin\&gamma;tan\&phi;}}_x& -{\mathrm{cos\&gamma;tan\&phi;}}_x& 1\end{array}\right]({\stackrel{\&RightArrow;}{\mathrm{\&omega;}}}_{ie}^L+{\stackrel{\&RightArrow;}{\mathrm{\&omega;}}}_{eL}^L)$

其中 $R_b^L=\left[\begin{array}{ccc}{\mathrm{cos\&phi;}}_{y}cos\mathrm{\&gamma;}-{\mathrm{sin\&phi;}}_x{\mathrm{sin\&phi;}}_{y}sin\mathrm{\&gamma;}& -{\mathrm{cos\&phi;}}_{x}sin\mathrm{\&gamma;}& {\mathrm{sin\&phi;}}_{y}cos\mathrm{\&gamma;}+{\mathrm{sin\&phi;}}_x{\mathrm{cos\&phi;}}_{y}sin\mathrm{\&gamma;}\\ {\mathrm{cos\&phi;}}_{y}sin\mathrm{\&gamma;}+{\mathrm{sin\&phi;}}_x{\mathrm{sin\&phi;}}_{y}cos\mathrm{\&gamma;}& {\mathrm{cos\&phi;}}_x\cos \mathrm{\&gamma;}& {\mathrm{sin\&phi;}}_{y}sin\mathrm{\&gamma;}-{\mathrm{sin\&phi;}}_x{\mathrm{cos\&phi;}}_{y}cos\mathrm{\&gamma;}\\ -{\mathrm{cos\&phi;}}_x{\mathrm{sin\&phi;}}_y& {\mathrm{sin\&phi;}}_x& {\mathrm{cos\&phi;}}_x{\mathrm{cos\&phi;}}_y\end{array}\right]$

从上式可以看出，克雷罗夫角法虽然定义了三个姿态角，并对其微分方程进行解算，但该方程不能实现全姿态，并存在着不可交换误差。

c.基于四元数的姿态微分方程其中：

$R_b^L=\left[\begin{array}{ccc}{q_0}^2+{q_1}^2-{q_2}^2-{q_3}^2& 2(q_1{q}_2-q_0{q}_3)& 2(q_1{q}_3+q_0{q}_2)\\ 2(q_1{q}_2+q_0{q}_3)& {q_0}^2-{q_1}^2+{q_2}^2-{q_3}^2& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& {q_0}^2-{q_1}^2-{q_2}^2+{q_3}^2\end{array}\right]$

从上式可以看出，四元数法定义了一个具有四元素的矢量，通过对该微分方程进行解算来得到坐标变换矩阵，但是仍然不能直接得到姿态角。

综上所述，克雷罗夫角法由于不能全姿态解算而在工程中没有得到应用，方向余弦法需要对9个微分方程求解也没有在工程中得到应用。而四元数法由于计算量相对较小，已在工程中得到普遍应用。但在确定初始四元数时，目前的方案是：采用克雷罗夫角定义坐标变换矩阵，首先确定三个克雷罗夫角的初始值φ_x0、φ_y0、γ₀，设克雷罗夫角的转动顺序为γ₀→φ_x0→φ_y0，因此，可得出合成转动四元数，其中：

可以看出，采用克雷罗夫角法进行初始对准时，与转动顺序直接相关。另外，上述方法要求φ_x0必须工作在小角度，使载体不能全姿态工作。

发明内容

本发明的技术解决问题：克服现有技术的不足，提供一种捷联惯性系统全姿态初始对准方法，该方法不存在坐标系的旋转顺序，从而克服了姿态角的不正交误差，能够实现全姿态对准，并且减小了导航解算的计算量。

本发明的技术解决方案：一种捷联惯性系统全姿态初始对准方法，步骤如下

(1)建立捷联惯性系统静基座初始对准方程

其中：L为导航坐标系，b为载体坐标系，i为惯性坐标系，为导航坐标系至载体坐标系的姿态变换矩阵，为载体角速率矢量，ω_x为载体坐标系下X轴陀螺仪的输出角速度，ω_y为载体坐标系下Y轴陀螺仪的输出角速度，ω_z为载体坐标系下Z轴陀螺仪的输出角速度，为导航坐标系相对惯性坐标系在导航坐标系的投影角速率矢量，ω_ie为地球自转角速度，为当地地理纬度，为加速度计输出矢量，a_x为载体坐标系下X轴加速度计的输出，a_y为载体坐标系下Y轴加速度计的输出，a_z为载体坐标系下Z轴加速度计的输出，为当地加速度矢量，

(2)建立基于四元数的姿态变换矩阵并确定三个旋转矢量姿态角与姿态四元数四个元素之间的关系；

$R_b^L=\left[\begin{array}{ccc}{q_0}^2+{q_1}^2-{q_2}^2-{q_3}^2& 2({q_1}_2+q_0{q}_3)& 2(q_1{q}_3-q_0{q}_2)\\ 2(q_1{q}_2-q_0{q}_3)& {q_0}^2-{q_1}^3+{q_2}^2-{q_3}^2& 2(q_2{q}_3+q_0{q}_1)\\ 2(q_1{q}_3+q_0{q}_2)& 2(q_2{q}_3-q_0{q}_1)& {q_0}^2-{q_1}^2-{q_2}^2+{q_3}^2\end{array}\right]$

其中q₀、q₁、q₂、q₃分别为姿态四元数Q的四个元素，q₀、q₁、q₂、q₃为实数；

三个旋转矢量姿态角与姿态四元数四个元素之间的关系为：

其中θ为旋转矢量姿态角，

θ_x为θ在导航坐标系X轴的分量；

θ_y为θ在导航坐标系Y轴的分量；

θ_z为θ在导航坐标系Z轴的分量；

(3)将步骤(2)中的姿态变换矩阵代入步骤(1)中的静基座初始对准方程，得到以姿态四元数表示的捷联惯性系统静基座初始对准方程：

其中：

(4)利用载体坐标系下捷联惯性系统中三个加速度计的输出值a_x、a_y、a_z和三个陀螺仪的输出角速度ω_x、ω_y、ω_z对步骤(3)中的两个方程进行求解，得到姿态四元数q₀、q₁、q₂、q₃；

(5)利用步骤(2)建立的三个旋转矢量姿态角与姿态四元数四个元素之间的关系，根据步骤(4)求解得到的姿态四元数计算得到三个旋转矢量姿态角θ_x、θ_y、θ_z，最终利用求解得到的三个旋转矢量角确定出导航坐

标系与载体坐标系之间的关系，其中，

(6)完成捷联惯性系统全姿态初始对准。

所述步骤(4)求解姿态四元数的方法为：

(1)当q₀≠0，四个姿态四元数为：

(2)当q₀＝0、q₁≠0，四个姿态四元数为：

(3)当q₀＝0、q₁＝0、q₂≠0，四个姿态四元数为：

$\left\{\begin{array}{c}{q}_0=0\\ q_1=0\\ q_2=\sqrt{\frac{1}{2}(1-\frac{a_z}{g})}\\ q_3=\frac{a_y}{2q_{2}g}\end{array}\right.;$

(4)当q₀＝0、q₁＝0、q₂＝0，四个姿态四元数为：

$\left\{\begin{array}{c}{q}_0=0\\ q_1=0\\ q_2=0\\ q_3=\sqrt{\frac{1}{2}(1+\frac{a_z}{g})}\end{array}\right..$

本发明与现有技术相比的优点如下：

(1)由于克雷罗夫角法定义的坐标变换矩阵与旋转顺序有关，求解的姿态角存在着不正交误差，而本发明首先求解姿态四元数然后根据四元数与旋转矢量之间的关系间接得到旋转矢量三个姿态角，三个姿态角物理意义非常明确，分别代表了刚体旋转轴与导航坐标系三个轴的夹角，因此其角度值具有唯一性，与旋转顺序不相关，从而克服了姿态角的不正交误差。

(2)现有方向余弦法和四元数法也可实现全姿态，但并不能直接得到三个姿态角，并且方向余弦法需要求解9个微分方程，四元数法需要求解4个微分方程，在根据坐标变换矩阵求解姿态角的过程中需要大量的三角函数运算，申请人在另一项发明专利“基于旋转矢量法的捷联惯性系统全姿态初始对准方法”中需要求解3个微分方程，并且需要求解两个三角函数即sinθ和cosθ，而本发明采用的间接旋转矢量法在求解过程中不涉及求解三角函数，因此大大减小了导航解算的计算量。相比现有方法，由于在求解过程无奇点，使得本发明的方法可以实现捷联惯性系统全姿态对准。

附图说明

图1为导航坐标系定义示意图；

图2为用克雷罗夫角表示的坐标变换示意图；

图3为本发明初始对准实现流程图；

图4为用旋转矢量法表示的坐标变换示意图。

具体实施方式

本发明的实现原理是：首先需要建立捷联惯性系统静基座初始对准方程和基于四元数的姿态变换矩阵，并确定三个旋转矢量姿态角与姿态四元数四个元素之间的关系，然后建立状态变量为四元数的捷联系统静基座初始对准时的二阶非线性方程组，对6个非线性方程组求解q₀、q₁、q₂和q₃四个元素，并根据四元数和旋转矢量姿态角之间的关系转换为旋转矢量法定义的三个姿态角θ_x、θ_y、θ_z，这样就可实现捷联系统的初始对准。相比通过以克雷罗夫角法进行初始对准的方法，本发明的初始对准方法具有与旋转顺序无关、可实现全姿态对准的优点。

下面结合图3介绍全姿态捷联惯性系统初始对准方法：

(1)建立捷联惯性系统静基座初始对准方程

其中：L为导航坐标系，b为载体坐标系，i为惯性坐标系，为导航坐标系至载体坐标系的变换矩阵，为载体角速率矢量，ω_x为载体坐标系下X轴陀螺仪的输出角速度，ω_y为载体坐标系下Y轴陀螺仪的输出角速度，ω_z为载体坐标系下Z轴陀螺仪的输出角速度，为导航坐标系相对惯性坐标系在导航坐标系的投影角速率矢量，ω_ie为地球自转角速度，为当地地理纬度，为加速度计输出矢量，a_x为载体坐标系下X轴加速度计的输出，a_y为载体坐标系下Y轴加速度计的输出，a_z为载体坐标系下Z轴加速度计的输出，为当地加速度矢量，

(2)建立基于四元数的姿态变换矩阵并确定三个旋转矢量姿态角与姿态四元数四个元素之间的关系；

$R_b^L=\left[\begin{array}{ccc}{q_0}^2+{q_1}^2-{q_2}^2-{q_3}^2& 2(q_1{q}_2+q_0{q}_3)& 2(q_1{q}_3-q_0{q}_2)\\ 2(q_1{q}_2-q_0{q}_3)& {q_0}^2-{q_1}^2+{q_2}^2-{q_3}^2& 2(q_2{q}_3+q_0{q}_1)\\ 2(q_1{q}_3+q_0{q}_2)& 2(q_2{q}_3-q_0{q}_1)& {q_0}^2-{q_1}^2-{q_2}^2+{q_3}^2\end{array}\right]$

其中q₀、q₁、q₂、q₃分别为姿态四元数Q的四个元素，q₀、q₁、q₂、q₃为实数；

三个旋转矢量姿态角与姿态四元数四个元素之间的关系为：

其中θ为旋转矢量姿态角，

θ_x为θ在导航坐标系X轴的分量；

θ_y为θ在导航坐标系Y轴的分量；

θ_z为θ在导航坐标系Z轴的分量；

图4中，OX₀Y₀Z₀表示导航坐标系，OX_tY_tZ_t表示载体坐标系。OX₀Y₀Z₀绕矢量经一次旋转到达OX_tY_tZ_t，转过的角矢量为

(3)将步骤(2)中的姿态变换矩阵代入步骤(1)中的静基座初始对准方程，得到以姿态四元数表示的捷联惯性系统静基座初始对准方程：

其中：

(4)利用载体坐标系下捷联惯性系统中三个加速度计的输出值a_x、a_y、a_z和三个陀螺仪的输出角速度ω_x、ω_y、ω_z对步骤(3)中的两个输出方程进行求解，得到姿态四元数q₀、q₁、q₂、q₃；

有

对以上两式求解：如果q₀＝0，则有对于该式，有三种可能：

(1)q₁＝0时，有

(2)q₁＞0时，有

(3)q₁＜0时，有

这三种情况都有多个解，但每个解的物理意义和结果相同，因此，只需取一组数即可。

如果q₀≠0，则有设q₀＞0，有

求解姿态四元数的方法为：

(1)当q₀≠0，四个姿态四元数为：

(2)当q₀＝0、q₁≠0，四个姿态四元数为：

(3)当q₀＝0、q₁＝0、q₂≠0，四个姿态四元数为：

$\left\{\begin{array}{c}{q}_0=0\\ q_1=0\\ q_2=\sqrt{\frac{1}{2}(1-\frac{a_z}{g})}\\ q_3=\frac{a_y}{2q_{2}g}\end{array}\right.$

(4)当q₀＝0、q₁＝0、q₂＝0，四个姿态四元数为：

(5)利用步骤(2)建立的三个旋转矢量姿态角与姿态四元数四个元素之间的关系，根据步骤(4)求解得到的姿态四元数计算得到三个旋转矢量姿态角θ_x、θ_y、θ_z，最后利用三个旋转矢量角确定出导航坐标系与载体坐标系之间的关系，其中，

(6)完成捷联惯性系统全姿态初始对准。

利用本发明的初始对准方法与克雷罗夫角对准方法进行了数据仿真计算，表1给出克雷罗夫角与本发明的仿真计算结果。

表1克雷罗夫角与本发明的仿真计算结果

从表1可以看出，用克雷罗夫角描述的刚体姿态角与真实值之间相差最大值可达90度以上，而本发明初始对准更接近于真实值。

实施例1：为验证发明方法的实用性和正确性，进行了转台测试试验，表2给出了设定初始姿态角时某激光捷联惯组的输出，表3给出了三种方法的对准结果。

表2某激光捷联惯性系统静基座输出结果

从表2中的数据可以看出在不同的位置条件下，实际陀螺仪和加速度计输出完全与捷联惯性系统中陀螺仪和加速度计输出相符，从而为初始对准解算提供正确的陀螺仪和加速度计数据。

表3某激光捷联惯性系统静基座初始对准结果

从表3的对准结果可以看出，在姿态角为小角度时，四种对准方法都能很好的完成初始对准。从表3的第四组数据可以看出，当φ_x＝90°，克雷洛夫角法不能分别求解出真实的三个姿态角，但可以求出φ_y+γ，并且解出φ_y+γ≈0，这与理论上φ_y+γ＝0相符，这样就证明了当φ_x＝90°时，克雷洛夫角法不能完成初始对准。由于四元数法是基于克雷洛夫角法的，因此，四元数法也不能完成初始对准。而直接法和本发明的间接旋转矢量法都能很好的完成初始对准，从而也就证明了旋转矢量算法能够全姿态解算。

Claims (2)

1.一种捷联惯性系统全姿态初始对准方法，其特征在于步骤如下

(1)建立捷联惯性系统静基座初始对准方程

其中：L为导航坐标系，b为载体坐标系，i为惯性坐标系，为导航坐标系至载体坐标系的姿态变换矩阵，为载体角速率矢量，ω_x为载体坐标系下X轴陀螺仪的输出角速度，ω_y为载体坐标系下Y轴陀螺仪的输出角速度，ω_z为载体坐标系下Z轴陀螺仪的输出角速度，为导航坐标系相对惯性坐标系在导航坐标系的投影角速率矢量，ω_ie为地球自转角速度，为当地地理纬度，为加速度计输出矢量，a_x为载体坐标系下X轴加速度计的输出，a_y为载体坐标系下Y轴加速度计的输出，a_z为载体坐标系下Z轴加速度计的输出，为当地加速度矢量，

(2)建立基于四元数的姿态变换矩阵并确定三个旋转矢量姿态角与姿态四元数四个元素之间的关系；

$R_b^L=\left[\begin{array}{ccc}{q_0}^2+{q_1}^2-{q_2}^2-{q_3}^2& 2(q_1{q}_2+q_0{q}_3)& 2(q_1{q}_3-q_0{q}_2)\\ 2(q_1{q}_2-q_0{q}_3)& {q_0}^2-{q_1}^2+{q_2}^2-{q_3}^2& 2(q_2{q}_3+q_0{q}_1)\\ 2(q_1{q}_3+q_0{q}_2)& 2(q_2{q}_3-q_0{q}_1)& {q_0}^2-{q_1}^2-{q_2}^2+{q_3}^2\end{array}\right]$

其中q₀、q₁、q₂、q₃分别为姿态四元数Q的四个元素，q₀、q₁、q₂、q₃为实数；

三个旋转矢量姿态角与姿态四元数四个元素之间的关系为：

其中θ为旋转矢量姿态角，

θ_x为θ在导航坐标系X轴的分量；

θ_y为θ在导航坐标系Y轴的分量；

θ_z为θ在导航坐标系Z轴的分量；

(3)将步骤(2)中的姿态变换矩阵代入步骤(1)中的静基座初始对准方程，得到以姿态四元数表示的捷联惯性系统静基座初始对准方程：

其中：

(4)利用载体坐标系下捷联惯性系统中三个加速度计的输出值a_x、a_y、a_z和三个陀螺仪的输出角速度ω_x、ω_y、ω_z对步骤(3)中的两个方程进行求解，得到姿态四元数q₀、q₁、q₂、q₃；

(5)利用步骤(2)建立的三个旋转矢量姿态角与姿态四元数四个元素之间的关系，根据步骤(4)求解得到的姿态四元数计算得到三个旋转矢量姿态角θ_x、θ_y、θ_z，最终利用求解得到的三个旋转矢量角确定出导航坐标系与载体坐标系之间的关系，其中，

(6)完成捷联惯性系统全姿态初始对准。

2.根据权利要求1所述的一种捷联惯性系统全姿态初始对准方法，其特征在于：所述步骤(4)求解姿态四元数的方法为：

(1)当q₀≠0，四个姿态四元数为：

(2)当q₀＝0、q₁≠0，四个姿态四元数为：

(3)当q₀＝0、q₁＝0、q₂≠0，四个姿态四元数为：

$\left\{\begin{array}{c}{q}_0=0\\ q_1=0\\ q_2=\sqrt{\frac{1}{2}(1-\frac{a_z}{g})}\\ q_3=\frac{a_y}{2q_{2}g}\end{array}\right.;$

(4)当q₀＝0、q₁＝0、q₂＝0，四个姿态四元数为：

$\left\{\begin{array}{c}{q}_0=0\\ q_1=0\\ q_2=0\\ q_3=\sqrt{\frac{1}{2}(1+\frac{a_z}{g})}\end{array}\right..$