一种基于李群描述的深空探测器姿态估计方法

徐 浩，裴福俊，蒋 宁

（1.北京工业大学 信息学部，北京 100124；2.计算智能与智能系统北京市重点实验室，北京 100124）

引 言

随着我国航天事业的飞速发展，深空飞行器的自主导航问题成为了研究的热点。深空飞行器距地距离远，工作时间长，深空环境未知，传统的依赖地面与航天器通讯来实现导航和控制的方法存在实时性、成本高、干扰较大等种种限制，很难实现高精度的深空导航需求。因此，在深空探测领域自主导航技术成为了保证深空探测任务完成的关键技术[1-4]。目前在自主导航的姿态确定方面，星敏感器是深空探测航天器上常用的姿态确定仪器，其基本原理是基于星点质心定位，实现星图识别和姿态解算[5-7]，从而实现探测器姿态估计。相比较于太阳敏感器、地平仪、磁力计等姿态测量设备，星敏感器测量精度高，抗干扰能力及自主导航能力强。本文主要就姿态解算部分做出了研究。

基于星敏感器的航天器姿态确定是通过星敏感器测量同一坐标矢量在惯性坐标系下和星敏感器坐标系下的不同数值来确定航天器姿态参数的过程。常用的姿态描述方法有欧拉角、四元数、罗格里格斯参数法、修正的罗格里格斯参数法、旋转矩阵等。现阶段的姿态解算算法主要分为静态确定性算法和动态姿态估计两大类。静态确定性算法是根据一组观测矢量求解出航天器坐标系和惯性坐标系之间的姿态矩阵进而求出航天器姿态角。Davenport提出了q方法，通过四元数的性质构造K矩阵，用求取特征值的方法确定姿态四元数[8]。基于最小二乘原理的SVD方法将矩阵进行奇异值分解得到姿态矩阵[9]。此外较常用的方法还有：基于牛顿迭代的数值算法、TRIAD方法[10]、FOAM方法[11]等。动态姿态估计方法主要是基于四元数的卡尔曼滤波的改进算法，现阶段已有的方法有扩展卡尔曼滤波（EKF）[12]、乘性卡尔曼滤波（MEKF）[13]、预测卡尔曼滤波[14]、自适应卡尔曼滤波[15]等。动态滤波姿态估计方法能够克服观测矢量中的不确定误差，利用更多的观测信息，实现最优化估计下的最优解，相比较于静态确定性算法能够更好的对姿态进行估计。但是在采用四元数建模时EKF产生的线性化误差极易引起滤波发散，有些滤波方法无法实现使用单一的姿态敏感仪器完成估计等问题还未得到较好的解决。

本文针对基于四元数描述的姿态估计方法存在的问题，提出了基于李群描述的深空探测器姿态估计的新方法。该方法基于李群对于旋转矩阵的特定唯一描述特点，采用李群对深空探测器进行姿态描述来解决四元数姿态描述方法中存在的非唯一性等问题。在此基础上，针对四元数描述的姿态估计方法中的模型非线性问题，基于李群微分方程构造了线性的姿态矩阵微分方程作为系统模型，并设计了李群滤波器来实现直接对姿态矩阵进行估计。该方法能够保证系统模型线性化结构，克服传统四元数方法中非线性模型在线性化时导致的模型误差。在估计精度上比静态姿态确定方法、四元数卡尔曼滤波方法具有更好的性能。

1 星敏感器

1.1 星敏感器基本工作原理

星敏感器一般有两种工作模式：全天球识别工作模式和星跟踪工作模式。全天球识别工作模式的工作原理为通过星敏感器的光学镜头在视场角内拍摄星图，经过星图识别、星点质心定位和姿态解算几个过程后输出航天器姿态角；星跟踪模式下星敏感器首先利用先验姿态信息，进入星跟踪模块通过局部的星点质心定位和识别最终解算出当前姿态信息。星敏感器的基本工作原理框图如图 1所示。本文主要针对姿态解算提出了一种新的算法。

图1 星敏感器的基本工作原理Fig.1 The working principle of star sensor

1.2 星敏感器观测模型

星敏感器由光学系统和CCD感光装置构成，来自定位恒星的光线穿过光学透镜聚焦到CCD成像平面。X和Y轴在CCD成像平面内呈正交化，Z轴与透镜的光轴重合。px和py两坐标点为目标恒星在CCD成像平面成像的质心坐标，f是光学系统的焦距。星敏感器的测量原理如图 2所示。

假设星敏感器坐标点p=[px，py]T表示在星敏感器坐标系下目标恒星的坐标，完整的目标恒星惯性坐标系下矢量ri在星敏感器坐标系下的投影可表示为

假设星敏感器坐标系与航天器载体坐标系重合，tk时刻在航天器载体坐标系下的目标恒星的观测方程可以写成

其中：li(k)是i颗目标恒星的观测矢量；X(k)是惯性系到航天器载体坐标系的姿态转换矩阵；ri是第i颗目标恒星在惯性系下的单位矢量；vi是星敏感器的观测噪声，通常被视为均值为零的随机白噪声并满足以下性质

图2 星敏感器测量原理Fig.2 The measurement principle of star sensor

2 基于李群的航天器姿态模型

李群理论是近代数学中的一个重要分支，19世纪由挪威数学家Marius Sophus Lie提出的[16]。李群是群，同时也是微分流形，满足群运算和可逆性。特殊正交群SO(n)是一般线性群GL(n)的子群，是李群中的一类群，定义为如下形式

本文主要考虑的航天器的姿态问题，所以主要讨论三维特殊正交群的相关内容。对等式RRT=I两边求导并移项得=-()T，为反对称阵，整理后有下式

其中：R为SO(3)阵；Q为3×3反对称矩阵，取向量k∈R3

其中， [·]×是反对称矩阵的简化数学表示。

在参考系下的航天器姿态角速度ω 可以表示为

空间航天器的运动动力学微分方程可以被写作

刚体飞行器的动态姿态可以被改写为如下的形式

其中：J为惯性矩阵；N是总共的外部转矩。

3 姿态估计李群滤波算法

结合公式（2）和（8）构造李群滤波方程，实现对航天器姿态的李群滤波估计。李群滤波器直接作用于基于李群的系统方程，相比于四元数方法省去了转换为姿态矩阵的步骤，保证了滤波器的线性结构，减少非线性的计算误差，李群滤波能够实现快速有效的姿态估计。

3.1 空间飞行器姿态动力学系统模型

首先，根据航天器姿态的动力学模型和星敏感器观测模型构建时间连续的系统模型

其中：Xt表示从航天器的姿态矩阵；ωt表示航天器姿态旋转矢量；wt表示一个随机的连续白噪声；lt表示目标恒星在航天器载体坐标下的连续观测矢量；rt代表了目标恒星在惯性坐标系下的坐标矢量；vt为星敏感器的连续观测噪声，本文中视为一个连续的随机白噪声。

为了解决传感器的实际测量值为离散量的问题，需要在固定采样时间频率下对模型进行离散化处理。公式（10）中Xt和ωt为一对李群和李代数，根据李代数到李群的映射关系，可在离散情况下将 ωt投影到李群空间对姿态矩阵Xt进行更新，该过程也是求解差分方程的过程，得系统离散化模型为

其中：Xk+1表示在tk+1时刻的航天器载体姿态矩阵；Wk是在tk+1时刻下均值白噪声在转动形式下的表示，传播矩阵为 σ12I3；Ak为tk+1时刻的转动增量，即对ωt进行指数展开。式（13）中，lk+1rk+1分别表示tk+1时刻目标恒星在航天器载体坐标系和惯性坐标系下的坐标矢量；vk+1为tk+1时刻的离散观测噪声，其协方差矩阵为σ22I3。

李群到李代数的映射是指数映射，其为光滑满射，陀螺的测量值为旋转矩阵X的李代数，所以使用式（12）的离散化模型能够保证姿态动力学系统的性质。

3.2 离散时间的李群滤波

为了更好的提出和推导本文所提出的李群滤波方法，本文先分析一种传统卡尔曼滤波通用离散系统模型，式（14）和（15）为传统卡尔曼滤波通用矢量模型

其中：Lk，Yk∈R3，分别表示所考虑模型的离散状态量和观测量；Wk和Vk∈R3为高斯白噪声，Hk∈ R3×3为量测矩阵。这种模型下的最常用的卡尔曼滤波的状态一步预测方程和状态估值计算方程为

使用两个三维矢量ek+1|k=Lk+1-和ek+1=Lk+1-预测状态误差和纠正状态误差，可以得到

误差方程可以具有自发性，然后滤波增益Kk+1可以根据误差协方差矩阵进行更新。滤波的收敛性可以根据可观可控性原理来确定，卡尔曼滤波实际上是一种基于方差最小的最优化估计方法。故根据式（12）和（13）的形式，将卡尔曼滤波方法与李群相结合，使用乘法原理替换该模型中的加法原理。下面对一个相似的方法进行具体的描述。

根据线性情况下李群的对称性原理并结合航天器姿态模型式（12），可以将式（16）和（17）转换到李群形式下[17]

根据误差均方差最小原理，李群滤波器设计如下：

状态一步预测方程

一步预测均方差方程

其中：Qw为系统噪声协方差矩阵，这里取σ12I3。

定义新息为 εk+1，新息为测量和估计量的一步误差，这里使用两个参考坐标

新息的均方差矩阵表示为

其中：Qv表示量测噪声协方{差矩阵，其}值取为σ22I6；Hk表示系统量测矩阵，

滤波增益

估计均方误差方程

状态估值计算方程采用公式（21）的方式，由于李代数到李群的指数映射为光滑满射，使用指数映射能够全面的修正状态量，所以状态估值计算方程为

其中 exp(·)表示指数展开，本文采用罗格里斯方程进行运算

李群滤波的过程总结如下

4 仿真结果与分析

相比较与传统的确定性算法，本文所提出的基于李群滤波的动态姿态估计方法，能够加入航天器中的IMU信息，并且对系统噪声和状态噪声进行响应，从而提供估计中的优化解，同时在一定程度上降低了不确定性因素的影响。与四元数滤波的方法相比较，本文的算法直接对姿态矩阵进行估计，避免了对模型进行线性化带来的误差和四元数描述姿态的非唯一性等问题，提高了滤波的稳定性和精度。下面通过一组仿真实验对算法进行验证。

仿真条件：星敏感器安装与在飞行器载体坐标系重合。设定两组目标恒星在惯性参考坐标系下的坐标为

X-Y-Z三轴姿态初值分别为0，-90°，90。

理论上真实的初始姿态矩阵为

星敏感器模拟两个观测矢量的初值为

理论的姿态角速度为

惯性矩阵参数为

星敏感器采样周期为

滤波器初始参数如下

四元数滤波的参数为q0=[1 0 0 0]，P0=I4，Q=1×10-5I4，R=0.01I4。

如图 3～4所示，李群滤波算法具有较好的收敛性和较高的姿态估计精度，在150次滤波后精度达到0.005°。由于四元数模型的不确定性误差使用EKF四元数滤波导致X轴上的误差随时间发散。仿真实验结果验证了以上的分析，证明本文所提出的算法的稳定性和可靠性。通过比较可以得出李群滤波在对准精度和稳定性方面都优于四元数的方法，李群滤波适用于基于星敏感器的航天器姿态估计问题。

图3 李群滤波三轴误差角Fig.3 The angle errors in three axis using Lie group filter

图4 四元数滤波三轴误差角Fig.4 The angle errors in three axis using quaternion filter

5 结 论

在实际应用中刚体的姿态描述方法，多采用单位四元数，但是基于四元数的描述方法存在着非唯一性和归一化的问题，在滤波过程中四元数模型的非线性问题也影响着滤波的性能。本文通过李群的性质直接使用姿态矩阵描述姿态，避免了实际应用中求解四元数在转化为姿态矩阵的过程。在此基础上结合卡尔曼滤波最优递推估计的思想，提出了一种全新的基于星敏感器航天器姿态的解算算法，实现了航天器姿态矩阵的直接估计。本文只是对李群在滤波方法上的应用做出了初步的探索，证明了该应用有一定的可行性，由于算法未考虑李群形式下的传感器误差，所以滤波收敛速率较慢。下一步将就如何使用李群对传统的误差模型建模进行研究，以提高滤波的估计精度和速度。