基于罗格里斯参数的航天器递归线性姿态估计算法

罗宇阳 龚德仁 陈筠力 段登平 邵晓巍

1.上海交通大学，上海200240

2.上海卫星工程研究所，上海200240

高精度在轨姿态确定是航天器进行精确姿态控制的基础，对提升航天器完成复杂空间任务的能力有着至关重要的作用。为了提高姿态确定精度，除了采用更精密的传感器外，还需采用先进算法进行数据处理和姿态估计。早在1965年，Wahba［1］就提出了基于多矢量观测的最小二乘姿态确定问题

其中:A 为待确定的姿态矩阵;bi为航天器本体系下的单位观测矢量;ri为惯性坐标系下的单位参考矢量;ξi为第i个传感器数据的相对权重，满足ξi=1;n 为传感器数目。

针对Wahba 问题，国内外学者已经进行了深入研究，形成了两类解决方法，即非递归法和递归法。非递归法仅利用当前观测值进行姿态估计，比较典型的算法包括:Shuster［2－3］提出的Davenport －q 方法，以及在此基础上发展得到的QUEST(Quaternion ESTimator)算法。Markley［4－5］采用矩阵分解提出了SVD(Single Value Decomposition)方法。Mortari［6－8］提出了ESOQ(Estimators of the Optimal Quaternion)，ESOQ2 方法。值得一提的是，Mortari 通过Cayley 变换提出了罗格里斯参数描述的OLAE(Optimal Linear Attitude Estimator)算法［9］，避免了一元四次方程最大根的求解。递归法算法保留了以前的观测信息，通过预测和更新等手段融入新的观测信息，再利用QUEST 算法进行求解，这样的做法提高了姿态估计的精度。1989年Shuster［10］提出了滤波QUEST和平滑QUEST 算法。Bar － Itzhack［11］提出了REQUEST (Recursive QUEST)算法。Choukroun［12－13］提出了最优REQUEST 算法，随后又提出了对向量化K 矩阵进行Kalman 滤波以获得更高姿态确定精度的方法。2009年Shuster［14］分析了滤波QUEST和REQUEST 之间的关系。2011年GONG［15］提出了基于几何关系获得的三种线性姿态估计算法，并对奇异避免和姿态估计协方差进行了分析。

本文以GONG［15］提出的第三种线性姿态估计算法为基础，根据姿态运动学对其进行递推，使得历史测量数据能够融入到当前的姿态估计中，从而提高估计精度。进一步引入遗忘因子，对历史测量数据和当前测量数据进行权衡，获得了加权递归算法。仿真结果表明本文提出的递归算法能够有效地提高姿态估计精度，具有一定的理论意义和工程应用价值。

1 基于转动几何关系的线性姿态估计算法

1.1 转动几何关系

根据欧拉原理，航天器在惯性坐标系中的姿态可以描述成绕固定轴转动一定角度，因此航天器方向余弦矩阵A 可以写成

其中:I 为3 ×3 的单位矩阵;a 为转动轴;θ 为转动角。a×为由矢量a=［a1，a2，a3］T产生的斜对称矩阵，具有以下形式

对于任意的观测对{ri，bi}，有

经过整理后可得

通过式(5)可知，ri，bi，a 和θ 四者之间的空间几何关系如图1 所示。

图1 转动几何关系

根据GONG［15］提出的方法，定义基于观测向量对{ri，bi}的右手坐标系

该坐标系方向的幅值为

根据图1 中的几何关系可得

其中:αi为xi和a 的夹角，βi为xi和bi的夹角。

罗格里斯参数是常见的姿态描述之一，其定义为

将式(8)和式(9)代入式(10)可得

1.2 线性姿态估计算法

考虑到传感器噪声的影响，式(12)并不是总成立。假设传感器输出的观测向量为

其中vi为观测向量误差。选取误差向量为

其中

定义新的优化准则为

其中ξi为相对权重，i=1，2，…，n。此时姿态确定问题就是寻找一个最优的罗格里斯参数g，使得优化准则式(16)达到最小值。将式(14)和式(15)代入式(16)可得

定义

此时优化准则可简写成

式(19)取得极值的一阶条件为

2 递归线性姿态估计算法

采用递归算法可以充分利用历史测量数据来提高姿态估计精度。首先提出时不变递归算法，在此基础上引入角速度，提出时变递归算法。进一步引入遗忘因子，得到加权时变递归算法。

2.1 时不变递归算法

时不变递归算法是针对角速度为0 时的情况。假设在tk时刻，已经处理了mk－1组测量数据，有nk组新的测量数据等待处理。令ξk，i，rk，i和分别为tk时刻的第i 组相对权重、参考向量和观测向量和分别为tk时刻用来计算最优姿态Gibbs 向量的M 矩阵和υ 向量。根据式(18)可得

其中:μk为tk时刻的权重总和

将式(22)写成递归形式可得

2.2 时变递归算法

时变递归算法针对航天器旋转时姿态进行确定，需要利用陀螺的角速度测量进行辅助计算。假设采样时间较短，在［tk－1，tk］时间段内卫星角速度为常值ωk－1，因此在此段时间内，由卫星旋转引起的姿态Gibbs 向量为

其中:Δtk－1=tk－tk－1为采样时间间隔。

假设根据tk－1时刻及以前的测量数据得到的tk－1时刻姿态估计为，预测tk时刻的姿态为，则有

tk－1时刻的估计姿态通过最小化如下性能函数得到

式(29)达到最小值的一阶条件为

其中

整理后可得

定义

根据式(33)可得

根据时不变递归算法可知，时变递归算法为

显然，当角速度ωk－1=0 时，时变递归算法等效为时不变递归算法。

2.3 加权时变递归算法

对于长期姿态估计过程来说，历史测量数据会逐渐增多，从而导致新的测量数据权重变小，无法反应当前的测量状态。为了改变这种情况，在递归算法中引入遗忘因子ρ，ρ 越大表示遗忘速度越快，姿态估计值中新的测量数据占的比重也就越大。此时，矩阵和向量的递归式变成

其中:0≤ρk≤1，μk=(1 －ρk)μk－1+ρkδμk。当ρk=0时，仅利用了历史测量数据;当ρk=0.5 时，新旧数据权重相同，就是普通的递归线性估计算法;当ρk=1时，仅仅利用了当前测量数据，就是普通的线性估计算法。因此，调节ρk就可以获得不同的姿态估计性能。

3 仿真示例

采用蒙特卡罗方法进行数值仿真，并对本文提出的递归线性姿态估计算法(Recursive linear attitude estimator，简称ReLAE)和QUEST 算法、OLAE算法进行比较和分析。假设星敏感器有3个，在航天器本体系中的安装方位分别为［1，0，0］T，［0，1，0］T和［0，0，1］T。星敏感器噪声为零均值高斯白噪声，其标准差为10－3(rad)，校正后的陀螺测量误差标准为 10－6(rad/s)，航天器角速度为ω=0.001s－1。

定义姿态估计误差向量为

其中q=［qT，q4］T为真实姿态四元数。姿态估计误差幅值为

仿真结果如图2 所示，分析仿真数据可以得到如下几个结论:

图2 姿态估计误差

1)递归线性姿态估计算法的精度明显高于QUEST 和OLAE 算法;

2)随着遗忘因子的减小，递推姿态估计算法的精度越来越高。当ρ=0.005 时，稳定后的姿态估计精度比非递推算法提高了近20 倍。

4 结论

本文对线性姿态估计的递归算法进行了研究。通过分析航天器姿态运动学，将历史测量数据引入到当前的姿态估计中，提高估计精度。并引入遗忘因子，得到加权的递归线性姿态估计算法。仿真结果表明:

1)递归算法的姿态估计精度比非递归算法有了显著的提高;

2)遗忘因子对姿态估计精度有着较大的影响。遗忘因子越小，姿态估计精度越高，但精度收敛的时间越长。

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

［2］Shuster M D. Approximate Algorithms for Fast Optimal Attitude Computation［C］. AIAA 78-1249，Proceedings of AIAA Guidance and Control Conference，Palo Alto，California，August，1978:88-95.

［3］Shuster M D，OH S D.Three-Axis Attitude Determination from Vector Observations［J］. Journal of Guidance and Control，1981，4(1):70-77.

［4］Markley F L. Attitude Determination Using Vector Observations and the Singular Value Decomposition［J］.The Journal of the Astronautical Sciences，1988，36(3):245-258.

［5］Markley F L，Mortari D.Quaternion Attitude Estimation Using Vector Measurements［J］. The Journal of the Astronautical Sciences，2000，48(2＆3):359-380.

［6］Mortari D.ESOQ:A Closed-Form Solution to the Wahba Problem［J］. The Journal of the Astronautical Sciences，1977，45(2):195-204.

［7］Mortari D.ESOQ2:Single-Point Algorithm for Fast Optimal Attitude Determination［J］. Advances in the Astronautical Sciences，1977，97(2):803-816.

［8］Mortari D. Second Estimator for the Optimal Quaternion［J］.Journal of Guidance，Control and Dynamics，2000，23(4):885-888.

［9］Mortari D，Markley F L，Singla P.Optimal Linear Attitude Estimator［J］. Journal of Guidance，Control and Dynamics，2007，30(6):1619-1627.

［10］Shuster M D.A Simple Kalman Filter and Smoother for Spacecraft Attitude［J］.Journal of the Astronautical Sciences，1989，37(1):89-106.

［11］Bar-Itzhack I Y.REQUEST:A Recursive QUEST Algorithm for Sequential Attitude Determination［J］. Journal of Guidance，Control and Dynamics，1996，19(5):1034-1038.

［12］Choukroun D，Bar-Itzhack I Y，Oshman Y.Optimal REQUEST Algorithm for Attitude Estimation［J］.Journal of Guidance，Control，and Dynamics，2004，27(3):418-425.

［13］Choukroun D. Quaternion Estimation Using Kalman Filtering of the Vectorized K-matrix［C］. AIAA Guidance Navigation，and Control Conference，10-13 August，2009，Chicago，Illinois.

［14］Shuster M D.Filter QUEST or REQUEST［J］.Journal of Guidance，Control and Dynamics，2009，32(2):643-645.

［15］Gong D，Shao X，Wang X，et al.Optimal Linear Attitude Estimators viar Geometric Analysis［J］. Journal of Zhejiang University-SCIENCE A (Applied Physics ＆ Engineering)，2011，12(11):873-883.