捷联式惯导系统的姿态计算

3.3.1　捷联式惯导系统的姿态计算

捷联式惯导系统的主要特征是用计算机来完成导航平台的功能，即所谓的“数学平台”。用捷联陀螺测量的载体角速度计算姿态矩阵，从姿态阵的元素中提取载体的姿态和航向信息;并用姿态阵把加速度计的输出从载体坐标系变换到导航坐标系，然后进行导航计算。由于载体的姿态、方位角速率较大，故姿态矩阵的实时计算对计算机提出了更高的要求。姿态方位实时计算也是捷联式系统的关键技术之一。

载体的姿态和航向是载体坐标系和地理坐标系之间的方位关系。确定两个坐标系之间的方位关系问题，可借助力学中的刚体定点转动理论。描述动坐标系相对参考坐标系方位关系的方法有多种，可简单地分作三类，即三参数法、四参数法和九参数法。考虑到转动的不可交换性，有时用等效转动矢量加以辅助。

1.三参数法

三参数法也叫欧拉角法，是欧拉在1776年提出来的。一个动坐标系相对参考坐标系的方位，完全可以由动坐标系依次绕三个不同的轴转动的三个角度来确定。如把载体坐标系Ox_b y_b z_b作为动坐标系，把地理坐标系Ox_g y_g z_g作为参考坐标系，则姿态角θ，γ和航向角ψ即为一组欧拉角。

如用表示载体坐标系相对地理坐标系的角速度在载体坐标系轴向的分量构成的列矢量，则和姿态航向角速度的关系可表示为:

或表示为:

式(3-31)即为欧拉角微分方程。根据角速度可以求解θ，γ，ψ三个角度。

求解欧拉角微分方程个数只有三个，与其他算法相比，需求解的方程个数少。而且用欧拉角法求解得到的姿态矩阵是正交矩阵，用该矩阵进行加速度信息的坐标变换时，变换后的信息中不存在非正交误差，因此用欧拉角法得到的姿态矩阵不需要进行正交变化处理。但在用计算机进行数值积分时要进行超越函数的运算，计算工作量加大。此外，由于每个方程都包含三角函数运算，当θ= 90°时，方程出现“奇点”，方程式退化，故不能全姿态工作。同时该算法漂移误差较大，一般应用在平台式惯性导航计算机软件中，而捷联式姿态运算中则较少运用。

2.四参数法

四参数法又称四元数法，是由Hamilton于1943年提出来的。四元数理论的思想类似于平面问题使用复数解的方式。先求解姿态四元数微分方程，再由姿态四元数确定航向角和姿态角。虽然需要四个微分方程，较欧拉角微分方程多一个方程，但进行数值计算求解时只需要进行加减乘除运算，求解过程的计算量要比欧拉角法减少很多。

四元数是一个由四个元构成的数，其形式为:

式中q₀为标量;q为矢量，i，j，k遵守下列相乘规则:

式中“·”表示四元数相乘。

由理论力学知识可知，绕定点转动的刚体的角位置可以通过依次转过三个欧拉角的三次转动而获得并且通过采用方向余弦法解决刚体的定位，也可以通过绕某一瞬时轴(等效转动轴)转过某个角度θ的一次转动而获得，且采用四元数法来解决定位问题。如果用u表示等效转轴方向的单位向量，则动坐标系的方位可完全由u和θ来确定。用u和θ可构造一个四元数Q，该四元数称作“规范化”四数，或变换四数:

其范数为:

这样就把三维空间和一个四维空间联系起来，从而可用四维空间中四元数的性质和运算规则来研究三维空间中的刚体定点转动问题。

与方向余弦法比较，四元数法计算量小，存储容量少，仅需要进行简单的四元数规范化处理便可以保证姿态矩阵的正交性，因而成为捷联姿态计算中一种普遍采用的方法。但是不可避免地引入了有限转动的不可交换性误差，特别是当运载体姿态变化比较剧烈，或伴有角振动时，应用四元数法会产生严重的姿态漂移误差，所以该法适用于工作环境平缓和变化缓慢的运载体。

3.九参数法

九参数法又称方向余弦法，该法是用矢量的方向余弦来表示姿态矩阵，即绕定点转动的两个坐标系之间的关系可以用方向余弦矩阵来表示。方向余弦矩阵是随时间变化的，其变换规律可用数学微分方程来描述，通过求解微分方程可得到即时值。方向余弦法求解姿态矩阵避免了欧拉角法的方程退化，可以全姿态工作。然而，方向余弦矩阵具有九个元素，所以解算矩阵微分方程时，实际上是解算九个联立微分方程，计算工作量较大。

4.等效转动矢量法

该法是建立在刚体矢量旋转的基础上的，与四元数不同的是，在姿态更新周期内，使用了陀螺的角增量信息。角增量的进一步微小化，使有限转动尽量接近无限转动。与四元数的根本区别是:该方法求解的是姿态变化四元数微分方程，而不是姿态四元数微分方程。

在方向余弦法和四元数法中都用到了角速度矢量的积分

当不是定轴转动时，即ω矢量的方向在空间变化时，上式是不成立的，即角度不是矢量。故采用角速度矢量积分时，使计算产生了误差，称作转动不可交换性误差。只有积分区间很小时，上式才近似成立。显然，采样周期必须很小，否则，计算结果中会有较大的不可交换误差，而采样周期过小，计算机的工作量会增大。为消除不可交换性误差，1971年John E.bortz提出了等效旋转矢量的概念。

为使式(3-36)成立，给ω加一修正量，使下式成立:

则φ称为等效转动矢量。用等效转动矢量φ代替方向余弦法或四元数法中的Δθ，则可避免转动不可交换性误差。修正量的表达式为:

式中，

所以，

实际中，常取前两项:

如果在一个姿态更新周期内，陀螺角速度输出用一次函数近似，即

定义

则可求得:

式中，

在方向余弦或四元数的一阶或二阶算法中，该修正量可略去不计;而在三阶算法中则需加以考虑。

如果在一个迭代周期内，对陀螺仪采样两次，即

Δθ_n1+Δθ_n2=Δθ(n)

则等效旋转矢量二子样算法为:

如果在一个姿态更新周期内，陀螺角速度输出用二次函数近似，即

ω= A+ B t+ C t²

则等效旋转矢量三子样算法为:

式中，

Δθ(n)=Δθ_n1+Δθ_n2+Δθ_n3

为姿态更新周期内陀螺三次等间隔采样的角增量。

类似地，可以得到更高阶的旋转矢量算法。

由于载体角运动有很大任意性，角速度变化是十分复杂的，在姿态更新周期内用某一曲线来拟合角速度这种方法本身就是近似的。载体的角运动越剧烈，用于拟合的曲线阶次应越高，这样才能较真实地反映载体的角运动，所以子样数越高，算法的精度也越高。得到等效旋转矢量后，可以用φ替换四元数解中的Δθ，然后得到实时更新矩阵，也可以用φ=［φ_x，φ_y，φ_z］构造一个变换四元数，进而进行实时姿态矩阵的解算。

等效转动矢量的迭代计算可用较高的频率如100Hz，通常称为快速回路。而用了等效转动矢量后的四元数或方向余弦阵的计算，则可用较低的迭代频率如20Hz，通常称为慢速回路。两种回路的结合构成了捷联姿态阵的计算。

等效转动矢量法在解四元数微分方程时要用到等效旋转矢量的概念，该法的计算量与四元数法计算量接近，但因能够对不可交换性误差做有效补偿，所以算法漂移比四元数小，是先进捷联式惯导系统中应用较为普遍的算法。