CN104677358A - 一种微捷联航姿系统全姿态控制器 - Google Patents

Abstract

本发明提供的是一种微捷联航姿系统全姿态控制器。其实现方式是：（1）选取姿态角的定义域；（2）确定俯仰角；（3）确定非奇异点的航向角；（4）确定非奇异点的滚动角；（5）确定奇异点的航向角与滚动角。本发明提供的全姿态算法有效解决了姿态角的计算过程可能存在误判与溢出问题。

Description

一种微捷联航姿系统全姿态控制器

技术领域

本发明涉及的是一种导航系统姿态解算控制器，特别涉及到一种微捷联航姿系统全姿态控制器。

背景技术

在捷联惯性导航系统中，姿态解算是捷联惯性导航系统算法中的关键技术，捷联惯性导航的姿态解算就是在导航计算机中实时计算出捷联矩阵，实现载体坐标系到导航坐标系的转换，建立数学解析平台，同时计算出载体的姿态、速度和位置等信息，其解算精度对捷联惯导系统的影响很大。因此，捷联惯性导航系统中姿态算法的研究就具有十分重要的意义。

微惯性测量组合体由三轴惯性传感器组成，是测量物体三轴姿态角(或角速率)以及加速度的装置，在飞行器的捷联惯导系统中常采用欧拉角法、四元数法和方向余旋法作为姿态更新算法，其中采用四元数这个数学工具，可以弥补欧拉角参数在设计系统时的不足之处，而且算法简单，计算量小，但是在利用四元数法进行姿态更新时，姿态角则仅局限于±90度之间，而且俯仰角在计算±90度时有奇异点，因而不能实现全角度的四元数与欧拉角之间的转换，为了解决奇异点问题需要研究实现全姿态计算问题。

发明内容

本发明的目的在于提供一种微捷联航姿系统全姿态控制器。

本发明的目的是这样实现的：

具体包括以下几个步骤：

（1）选取姿态角的定义域；

（2）确定俯仰角；

（3）确定非奇异点的航向角；

（4）确定非奇异点的滚动角

（5）确定奇异点的航向角与滚动角。

附图说明

图1为微捷联航姿系统全姿态控制器流程图；

图2只有俯仰角变化时，各个姿态角变化规律；

图3只有滚动角变化时，各个姿态角变化规律；

图4只有航向角变化时，各个姿态角变化规律；

图5载体做全姿态运动，各个姿态角变化规律。

本发明的技术方案的实质是：合理定义姿态角定义域基础，用反正切函数替代反正弦函数并进行符号判断的方法求解俯仰角，用方向余弦矩阵的元素确定真值并进行象限判断的方法求解航向角与滚动角，从而实现全姿态的解算。

该技术方案的优点在于：

实现全角度的四元数与欧拉角之间的转换，算法简单，计算量小。

具体实施方式

步骤1、选取姿态角的定义域：

姿态的含义表述为：（1）无论在哪种情况下，都能求解出正确的俯仰角；（2）无论载体是否处在垂直状态，都能正确给出航向角和滚动角的值。

为了保证载体姿态角的单值性，三个姿态角不能都定义在（0，360）的区间内，否则载体的一个姿态将对应多组的姿态参数。为了能够更好的运用方向余旋中的元素来求解姿态角，姿态角的定义域选取为：俯仰角θ[-90，90]、滚动角γ（-180，180]、航向角

3、根据权利要求1所述的微捷联航姿系统全姿态控制器，其特征是所述的确定俯仰角：

若俯仰角定义在（-90°，+90°）之间时，可由θ=-sin^-1（T₃₁）便可求得其值，俯仰角定义在[-90°,+90°]之间时则可根据反正切函数来求解。这时

$\mathrm{\θ}=\arctan \frac{{-T}_{31}}{\sqrt{1-T_{31}^2}}---\left(1\right)$

但当θ趋近于90°时，T₃₁趋近于1，分母就会趋于零，从而导致计算溢出。所以可以选取一个小量ε，使分母非常小但不为零，然后就可以根据的符号而直接得出，即：

$\mathrm{\&theta;}=\left\{\begin{array}{cc}\arctan \frac{{-T}_{31}}{\sqrt{1-T_{31}^2}}& ,\left|T_{31}\right|>1-\mathrm{\&epsiv;}\\ -\frac{\mathrm{\&pi;}}{2}& ,\left|T_{31}\right|<1-\mathrm{\&epsiv;},T_{31}>0\\ \frac{\mathrm{\&pi;}}{2}& ,\left|T_{31}\right|<1-\mathrm{\&epsiv;},T_{31}<0\end{array}\right.---\left(2\right)$

式中， $T=\left[\begin{array}{ccc}0& -\sin \mathrm{\&alpha;}& \cos \mathrm{\&alpha;}\\ 0& \cos \mathrm{\&alpha;}& -\sin \mathrm{\&alpha;}\\ A& 0& 0\end{array}\right]$

当θ=90时，A=1；θ=-90时，A=-1。

4、根据权利要求1所述的微捷联航姿系统全姿态控制器，其特征是所述的确定非奇异点的航向角：

5、根据权利要求1所述的微捷联航姿系统全姿态控制器，其特征是所述的确定非奇异点的滚动角：

当航向角定义在（-90°，+90°）时有：

其中

因为在θ属于（-90°，+90°）时，cosθ>0。在定义域为[0°,+360)时，上式会有分母趋于零的情况，并且需要根据分子、分母的符号来确定航向角的象限。

当T₁₁→0即||T₁₁||<ε（ε为某一确定小量）时，则此时的值为90°或270°，可根据T₂₁的符号进一步判断；

当||T₁₁||>ε，T₁₁>0且||T₂₁||<ε时，显然

当||T₁₁||>ε，T₁₁>0且||T₂₁>ε，T₂₁>0时，属于第一象限，此时

当||T₁₁||>ε，T₁₁>0且||T₂₁||>ε，T₂₁<0时，在第四象限，

当||T₁₁||>ε，T₁₁<0时，属于第二或三象限，此时

综合以上所述可得：

$\mathrm{\&gamma;}=\left\{\begin{array}{cc}\arctan \left(\frac{T_{32}}{T_{33}}\right)& ,\left|\right|T_{33}\left|\right|>\mathrm{\&epsiv;},T_{33}>0\\ \arctan \left(\frac{T_{32}}{T_{33}}\right)+\mathrm{\&pi;}& ,\left|\right|T_{33}\left|\right|>\mathrm{\&epsiv;},T_{33}<0,\left|\right|T_{32}\left|\right|>\mathrm{\&epsiv;},T_{32}>0\\ \arctan \left(\frac{T_{32}}{T_{33}}\right)-\mathrm{\&pi;}& ,\left|\right|T_{33}\left|\right|>\mathrm{\&epsiv;},T_{33}<0,\left|\right|T_{32}\left|\right|>\mathrm{\&epsiv;},T_{32}<0\\ \frac{\mathrm{\&pi;}}{2}& ,\left|\right|T_{33}\left|\right|<\mathrm{\&epsiv;},T_{32}>0\\ -\frac{\mathrm{\&pi;}}{2}& ,\left|\right|T_{33}\left|\right|<\mathrm{\&epsiv;},T_{32}<0\\ \mathrm{\&pi;}& ,\left|\right|T_{33}\left|\right|>\mathrm{\&epsiv;},T_{33}<0,\left|\right|T_{32}\left|\right|<\mathrm{\&epsiv;}\end{array}\right.---\left(5\right)$

6、根据权利要求1所述的微捷联航姿系统全姿态控制器，其特征是所述的确定奇异点的航向角与滚动角：

当θ=±90时，载体纵轴在水平面投影为一个点，载体纵向铅垂面变成了载体纵向对称面中的一条直线，只能得到这航向角与滚动角的和或者差的值。载体姿态角有三个自由度退化为两个自由度，此时只能用俯仰角θ与航向角与滚动角的和或者差的值来描述载体的姿态，这里选取航向角与俯仰角作为载体两个自由度的姿态角，即显然此时γ=0，载体姿态角一个自由度由θ=±90确定，另一个由航向角确定。

当θ=±90时，γ=0时，得姿态矩阵为：

当航向角定义在（-90°，+90°）时有：

在定义域为[0°,+360)时，上式会有分母趋于零的情况，并且需要根据分子、分母的符号来确定航向角的象限。

当||T₂₂||<ε时，则此时的值为90°或270°，可根据T₁₂的符号进一步判断；

当||T₂₂||>ε，T₂₂<0时，属于第二或第三象限，此时

当||T₂₂||>ε，T₂₂>0且||T₁₂||<ε时，

当||T₂₂||>ε，T₂₂>0且||T₁₂||>ε，T₁₂<0时，

当||T₂₂||>ε，T₂₂>0且||T₁₂||>ε，T₁₂>0时，

Claims (2)

1.一种微捷联航姿系统全姿态控制器，其特征在于微捷联航姿系统全姿态控制器，其特征是所述的选取姿态角的定义域：姿态角的定义域选取为：俯仰角θ[-90，90]、滚动角γ（-180，180]、航向角）。

2.根据权利要求1所述的微捷联航姿系统全姿态控制器，其特征是所述的确定俯仰角：

$\mathrm{\&theta;}=\left\{\begin{array}{cc}\arctan \frac{{-T}_{31}}{\sqrt{1-T_{31}^2}}& ,\left|T_{31}\right|>1-\mathrm{\&epsiv;}\\ -\frac{\mathrm{\&pi;}}{2}& ,\left|T_{31}\right|<1-\mathrm{\&epsiv;},T_{31}>0\\ \frac{\mathrm{\&pi;}}{2}& ,\left|T_{31}\right|<1-\mathrm{\&epsiv;},T_{31}<0\end{array}\right.---\left(1\right)$

式中， $T=\left[\begin{array}{ccc}0& -\sin \mathrm{\&alpha;}& \cos \mathrm{\&alpha;}\\ 0& \cos \mathrm{\&alpha;}& -\sin \mathrm{\&alpha;}\\ A& 0& 0\end{array}\right]$

当θ=90时，A=1；θ=-90时，A=-1。