双基线姿态确定三种算法的比较分析*

刘晓辉 党亚民 王潜心 杨 磊

1)中国测绘科学研究院，北京 100830

2)山东科技大学，青岛266510

1 引言

所谓定姿，就是确定载体坐标系与参考坐标系的相对关系。在航空、航天、航海和陆地导航任务中，都需要准确测定载体的姿态信息。目前用于测定姿态的传感器主要有陀螺仪、星敏感器、太阳敏感器、磁力计和全球定位系统(GNSS)等。

通常将确定三轴姿态的算法分为两类:确定性算法和最优化算法［1］。目前最好的确定性算法是TRIAD 算法［2］，但该算法只能利用两个观测矢量的信息，不是最优算法且有主轴敏感性。黎湧等人［4］于2000年提出一种融合TRIAD 算法，他们利用两个观测矢量的和与差构造正交矢量从而消除了主轴敏感性，并且对姿态角而不是对姿态矩阵进行加权从而省去了矩阵的正交化过程。最优化算法就是寻找使建立的代价函数最小的姿态矩阵，也就是Wahba 问题［5］，这一问题直到QUEST 算法［6］提出才得以完满解决。但是QUEST 方法需要求解特征值和特征向量，计算过程复杂、耗时长，对计算机要求较高。在双基线定姿的实际应用中，有时需要较高的姿态更新率，有时需要程序计算简单，有时需要较高的定姿精度，究竟该选择哪种算法才能满足需要，本文将对这一问题进行研究。

2 双基线定姿算法

2.1 TRIAD 算法

已知参考坐标系下两条非平行单位矢量为W1和W2，它们在载体坐标系下对应的单位矢量为V1和V2。

在参考坐标系下构造三条正交的单位矢量:

在载体坐标系下构造三条正交单位矢量:

利用单位正交矢量构造单位正交阵:

式中A 为姿态矩阵。

式中α 为横滚角，β 为俯仰角，γ 为航向角。

2.2 优化TRIAD 算法

一般情况下，第一个观测矢量和第二个观测矢量测量精度不同。该方法就是分别用第一和第二个观测矢量作为主矢量构造正交阵，得到两个姿态矩阵，然后再对这两个矩阵进行加权平均，最后对得到的矩阵进行正交化。具体解算步骤如下:

以W1和V1作为主矢量求得的姿态矩阵为A1，方差为;以W2和V2为主矢量求得的姿态矩阵为A2，方差为。

式(5)是对A'的正交化［3］。矩阵A 即为最后要得到的姿态矩阵。

2.3 Quest 算法

设代价函数定义为:

最优化算法是寻找最优的正交姿态矩阵Aopt使L(A)最小，Quest 算法把问题转化为寻找最优四元数，然后根据四元数和姿态矩阵的关系求得姿态角，四元数与姿态矩阵的关系为:

定义函数

载体运动后的姿态可以看作是绕旋转轴转动一定的角度得到的，旋转轴所指方向的单位矢量用n表示，转动的角度用θ表示。

用四元数q表示绕n 旋转θ 角的表达式为:

函数g(A)可以表示成关于四元数q 的二次型函数:

3 试验分析

分别用TRIAD 方法、优化TRIAD 方法、QUEST方法对不同的仿真双基线数据进行处理，然后对各种方法的计算精度、计算耗时、代价函数进行分析。

选取载体坐标系下的基线W1和W2如下:

姿态矩阵为:

如果没有误差，则V1和V2为:

由于传感器观测存在误差，假设基线误差服从零均值高斯分布，根据基线精度和姿态角精度的关系［7］，并参考实际测量中姿态角的测量精度，在V1中加入均值为零、方差为0.000 1m2的随机白噪声，根据第二条基线和第一条基线的精度比=40、20、10、5 、1、0.2、0.1、0.05 在V2中分别加入8 种均值

从图1 可以看出，利用优化TRIAD 方法和QUEST 方法计算得到的航向角完全相同。随着的减小，用优化TRIAD 方法和QUEST 方法计算的优势越来越明显(1)。

图1 三种方法得到的航向角偏差Fig.1 Heading angle deviation with the three algorithms

表1 姿态角标准差信息(单位:°)Tab.1 Standard deviation of attitude angles(unit:°)

通过对三种方法进行比较可以看出:优化TRIAD 方法和QUEST 方法计算得到的航向角、俯仰角和翻滚角的精度完全相同，值为0.1 和0.05时利用优化TRIAD 和QUEST 方法得到的姿态角精度比TRIAD 方法高;利用TRIAD 方法得到的姿态角精度随的增大而减小，这是因为主基线的精度不变，而第二条基线的精度不断增高，因此总体上精度不断增高;而优化TRIAD 和QUEST 方法得到的姿态角精度从向两边递减，并且向减小的方向递减的速度明显高于增大的方向的递减速度。

图2 不同精度比下三种方法的代价函数比较Fig.2 Comparison among the three algorithms in different precision ratio

表2 代价函数平均值Tab.2 Mean values of cost function

表3 不同情况下三种方法的计算耗时(单位:s)Tab.3 Computation time in different cases(unit:s)

4 结论

1)利用两条基线进行姿态解算，优化TRIAD 算法和QUEST 算法得到的定姿结果完全相同;

4)姿态角解算精度与代价函数的变化规律不一致。

1 Malcolm D Shuster.Deterministic three-axis attitude determination［J］.The Journal of the Astronautical Science，2004，52(3):405－419.

2 BLACK H D.A passive system for determining the attitude of a satellite［J］.AIAA Journal，1964，2:1 350－1 351.

3 Itzhack Y Bar－Itzhack and Richard R Harman.Optimized TRIAD algorithm for attitude determination［J］.Journal of Guidance，Control and Dynamics(S0731－5090)，1997，20(1):208－211.

4 黎湧，吴宏鑫，刘良栋.融合TRIAD 算法用于GPS 姿态确定［J］.中国空间科学技术，2000，4(2):30－36.(Integration of TRIAD algorithm for GPS attitude determination［J］.Space Science and Technology，2000，4(2):30－36)

5 WAHBA G.Problem 65－1:A Least Squares Estimate of Spacecraft Attitude［J］.SIAM Review，1965，7(3):409.

6 Shuster M D and OH S D.Three-axis attitude determination from vector observations［J］.Journal of Guidance and Control，1981，4(1):70－77.

7 Lu G.Development of a GPS multi-antenna system for attitude determination［D］.University of Calgary，Calgary ，Canada，1995.