2. 计算智能与智能系统北京市重点实验室,北京 100124
2. Beijing Key Laboratory of Computational Intelligence and Intelligent System,Beijing 100124,China
Hight lights:
- ● The innovative introduction of Lie group structure describes the rigid body attitude, which overcomes the problems of singularity and non-uniqueness in traditional methods.
- ● Using the measurement information of the star sensor and the dynamic equations of the spacecraft, the system equations described by Lie Group are constructed to estimate the position of the spacecraft in space.
- ● Based on the mapping relationship between Lie group and Lie algebra, the steps of Lie group filtering are deduced and the spacecraft attitude matrix is directly estimated.
- ● Through the simulation and analysis of traditional quaternion filtering algorithm and Lie group filtering algorithm, the feasibility and superiority of the attitude estimation filtering algorithm based on Lie group description is proved.
随着我国航天事业的飞速发展,深空飞行器的自主导航问题成为了研究的热点。深空飞行器距地距离远,工作时间长,深空环境未知,传统的依赖地面与航天器通讯来实现导航和控制的方法存在实时性、成本高、干扰较大等种种限制,很难实现高精度的深空导航需求。因此,在深空探测领域自主导航技术成为了保证深空探测任务完成的关键技术[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. 1The working principle of star sensor |
星敏感器由光学系统和CCD感光装置构成,来自定位恒星的光线穿过光学透镜聚焦到CCD成像平面。X和Y轴在CCD成像平面内呈正交化,Z轴与透镜的光轴重合。
假设星敏感器坐标点
${{l}} = {[{p_x}, {p_y}, f]^{\rm{T}}}$ | (1) |
假设星敏感器坐标系与航天器载体坐标系重合,
${{{l}}_i}(k) = {{X}}(k){{{r}}_i} + {{{v}}_i}$ | (2) |
其中:
$\begin{array}{l} E\{ {{{v}}_i}\} = 0\\ E\{ {{{v}}_i}{{{v}}_i}^{\rm{T}}\} = {\sigma _i}^2{{{I}}_3} \end{array}$ | (3) |
其中,
![]() |
图 2星敏感器测量原理Fig. 2The measurement principle of star sensor |
李群理论是近代数学中的一个重要分支,19世纪由挪威数学家Marius Sophus Lie提出的[16]。李群是群,同时也是微分流形,满足群运算和可逆性。特殊正交群
${{SO}}(n) = \{ {{R}}|{{R}} \in {{{R}} ^{n \times n}},{{R}}{{{R}}^{\rm{T}}} = {{I}},\det ({{R}}) = 1\} $ | (4) |
本文主要考虑的航天器的姿态问题,所以主要讨论三维特殊正交群的相关内容。对等式
$\dot {{R}} = {{RO}}$ | (5) |
其中:
${{Q}}({{k}}){\rm{ = }}\left[ {{{k}} \times } \right] = \left[ {\begin{split} 0& \quad{ - {k_3}}&{{k_2}}\\ {{k_3}}&\quad\;\; 0&{ - {k_1}}\\ { - {k_2}}&\;\;\quad{{k_1}}&0\;\; \end{split}} \right]$ | (6) |
其中,
在参考系下的航天器姿态角速度
${{\omega }} = {[{\omega _x}{\text{,}}{\omega _y}\text{,}{\omega _z}]^{\rm{T}}}$ | (7) |
空间航天器的运动动力学微分方程可以被写作
$\dot {{R}} = {{R}}[{{\omega}} ] \times $ | (8) |
刚体飞行器的动态姿态可以被改写为如下的形式
${{\dot \omega }} = {{{J}}^{ - 1}}\{ {{N}} - {{\omega }} \times {{J\omega }}\} $ | (9) |
其中:
结合公式(2)和(8)构造李群滤波方程,实现对航天器姿态的李群滤波估计。李群滤波器直接作用于基于李群的系统方程,相比于四元数方法省去了转换为姿态矩阵的步骤,保证了滤波器的线性结构,减少非线性的计算误差,李群滤波能够实现快速有效的姿态估计。
3.1 空间飞行器姿态动力学系统模型首先,根据航天器姿态的动力学模型和星敏感器观测模型构建时间连续的系统模型
${{{\dot X}}_t} = {{{X}}_t}[{{{\omega }}_t} + {{{w}}_t}] \times $ | (10) |
${{{l}}_t} = {{{X}}_t}{{{r}}_t} + {{{v}}_t}$ | (11) |
其中:
为了解决传感器的实际测量值为离散量的问题,需要在固定采样时间频率下对模型进行离散化处理。公式(10)中
${{{X}}_{k + 1}} = {{{W}}_k}{{{X}}_k}{{{A}}_k}$ | (12) |
${{{l}}_{k + 1}} = {{{X}}_{k + 1}}{{{r}}_{k + 1}} + {{{v}}_{k + 1}}$ | (13) |
其中:
李群到李代数的映射是指数映射,其为光滑满射,陀螺的测量值为旋转矩阵
为了更好的提出和推导本文所提出的李群滤波方法,本文先分析一种传统卡尔曼滤波通用离散系统模型,式(14)和(15)为传统卡尔曼滤波通用矢量模型
${{{L}}_{k + 1}} = {{{L}}_k} + {{{W}}_k}$ | (14) |
${{{Y}}_k} = {{{H}}_k}{{{L}}_k} + {{{V}}_k}$ | (15) |
其中:
${\hat {{L}}_{k + 1\left| k \right.}} = {\hat {{L}}_k}$ | (16) |
${\hat {{L}}_{k + 1}} = {\hat {{L}}_{k + 1\left| k \right.}} + {{{K}}_{k + 1}}({{{Y}}_{k + 1}} - {{{H}}_{k + 1}}{\hat {{L}}_{k + 1\left| k \right.}})$ | (17) |
使用两个三维矢量
${{{e}}_{k + 1\left| k \right.}} = {{{e}}_{k + 1}} + {{{W}}_n}$ | (18) |
${{{e}}_{k + 1}} = {{{e}}_{k + 1\left| k \right.}} - {{{K}}_{k + 1}}({{{H}}_{k + 1}}{{{e}}_{k + 1\left| k \right.}} + {{{V}}_{k + 1}})$ | (19) |
误差方程可以具有自发性,然后滤波增益
根据线性情况下李群的对称性原理并结合航天器姿态模型式(12),可以将式(16)和(17)转换到李群形式下[17]
${\hat {{X}}_{k + 1\left| k \right.}} = {\hat {{X}}_k}{{{A}}_k}$ | (20) |
${\hat {{X}}_{k + 1}} = {\hat {{X}}_{k + 1\left| k \right.}}{J_{k + 1}}({{{K}}_{k + 1}}({{{l}}_{k + 1}} - {{{X}}_{k + 1\left| k \right.}}{{{r}}_{k + 1}}))$ | (21) |
其中:
${{{\eta}} _{k + 1}} = {{{X}}_{k + 1}}{\hat {{X}}_{k + 1}^{\rm{T}}}$ | (22) |
${{{\eta}} _{k + 1\left| k \right.}} = {{{X}}_{k + 1}}{\hat {{X}}_{k + 1\left| k \right.}^{\rm{T}}}$ | (23) |
根据误差均方差最小原理,李群滤波器设计如下:
状态一步预测方程
${\hat {{X}}_{k + 1\left| k \right.}} = {\hat {{X}}_k}{{{A}}_k}$ | (24) |
一步预测均方差方程
${{{P}} _{k|k - 1}} = {{{A}}_k}{{{P}} _{k - 1}}{{{A}}^{\rm{T}}}_k + {{{Q}} _w}$ | (25) |
其中:
定义新息为
${{{\varepsilon}} _{k + 1}} = \left\{ {\begin{split} {{{{l}}_{k + 1}} - {{{X}}_{k + 1\left| k \right.}}{{{r}}_{k + 1}}} \\ {{{{l}}^{'}}_{k + 1} - {{{X}}_{k + 1\left| k \right.}}{{{r}}^{'}}_{k + 1}} \end{split}} \right\}$ | (26) |
新息的均方差矩阵表示为
${{{S}}_k} = {{{H}}_k}{{{P}}_{k|k - 1}}{{H}}_k^{\rm T} + {{{Q}}_v}$ | (27) |
其中:
滤波增益
${{{K}}_k} = {{{P}} _{k|k - 1}}{{H}}_k^{\rm{T}}{{{S}} _k}_{}^{ - 1}$ | (28) |
估计均方误差方程
${{{P}} _k} = ({{I}} - {{{K}}_k}{{{H}}_k}){{{P}} _{k|k - 1}}{({{I}} - {{{K}}_k}{{{H}}_k})^{\rm{T}}} + {{{Q}} _v}$ | (29) |
状态估值计算方程采用公式(21)的方式,由于李代数到李群的指数映射为光滑满射,使用指数映射能够全面的修正状态量,所以状态估值计算方程为
${\hat {{X}}_{k + 1}} = {\hat {{X}}_{k + 1\left| k \right.}}\exp ({{{K}}_{k + 1}}{{{\varepsilon}} _{k + 1}})$ | (30) |
其中
$\exp (\theta ) = {{I}} + \frac{{\sin (\left\| \theta \right\|)}}{{\left\| \theta \right\|}}\theta \times + \frac{{1 - \cos (\left\| \theta \right\|)}}{{{{\left\| \theta \right\|}^2}}}{(\theta \times )^2}$ | (31) |
李群滤波的过程总结如下
$\left\{ \begin{split} & {{\hat {{X}}}_{k + 1\left| k \right.}} = {{\hat {{X}}}_k}{{{A}}_k} \\ & {{{P}} _{k|k - 1}} = {{{A}}_k}{{{P}} _{k - 1}}{{{A}}_k^{\rm{T}}} + {{{Q}} _w} \\ & {{{S}} _k} = {{{H}}_k}{{{P}} _{k|k - 1}}{{H}}_k^{\rm{T}} + {{{Q}} _v} \\ & {{{K}}_k} = {{{P}} _{k|k - 1}}{{H}}_k^{\rm{T}}{{{S}} _k}^{ - 1} \\ & {{{P}} _k} = ({{I}}- {{{K}}_k}{{{H}}_k}){{{P}} _{k|k - 1}} \\ & {{\hat {{X}}}_{k + 1}} = {{\hat {{X}}}_{k + 1\left| k \right.}}\exp ({{{K}}_{k + 1}}{{{\varepsilon}} _{k + 1}}) \end{split} \right.$ |
相比较与传统的确定性算法,本文所提出的基于李群滤波的动态姿态估计方法,能够加入航天器中的IMU信息,并且对系统噪声和状态噪声进行响应,从而提供估计中的优化解,同时在一定程度上降低了不确定性因素的影响。与四元数滤波的方法相比较,本文的算法直接对姿态矩阵进行估计,避免了对模型进行线性化带来的误差和四元数描述姿态的非唯一性等问题,提高了滤波的稳定性和精度。下面通过一组仿真实验对算法进行验证。
仿真条件:星敏感器安装与在飞行器载体坐标系重合。设定两组目标恒星在惯性参考坐标系下的坐标为
$\begin{split} {{{v}}_1} = {[1,0,0]^{\rm T}}\\ {{{v}}_2} = {[0{\rm{,1,0}}]^{\rm T}} \end{split}$ |
X-Y-Z三轴姿态初值分别为0,–90°,90。
理论上真实的初始姿态矩阵为
${{X}} = \left[ {\begin{split} \; 0 \quad\quad & 1 \quad &0 \;\\ \; 0 \quad\quad & 0 \quad &1 \;\\ \; 1 \quad\quad & 0 \quad &0 \; \end{split}} \right]$ |
星敏感器模拟两个观测矢量的初值为
$\begin{split} {{{w}}_1} = {[0,0,1]^{\rm T}}\\ {{{w}}_2} = {[1{\rm{,0,0}}]^{\rm T}} \end{split}$ |
理论的姿态角速度为
${{\omega }} = {10^{ - 4}} \times {[\cos (0.1t),\cos (0.08t),\cos (0.06t)]^{\rm{T}}}{\rm rad}/{\rm s}$ |
惯性矩阵参数为
${{J}} = \left[ {\begin{split} \;\; {200} \!\!\!\!\! \quad\quad & \quad\quad 0& \quad\quad 0\;\;\;\;\;\;\;\; \\ 0 \quad\quad & \quad\quad\!\!\!\!\! {300}& \quad\quad 0\;\;\;\;\;\;\;\; \\ 0 \quad\quad & \quad\quad 0&{100}\;\;\;\;\;\; \end{split}} \right]$ |
星敏感器采样周期为
$t = 1{\rm s}$ |
滤波器初始参数如下
${{{X}}_0} = \left[ {\begin{align} \; 1 &\quad \quad 0 \quad &0 \;\\ \; 0 &\quad \quad 1 \quad &0 \;\\ \; 0 &\quad \quad 0\quad &1 \; \end{align}} \right]$ |
${{{P}} _0} = \left[ {\begin{split} \; 5 \quad \quad & 0 \quad &0 \;\\ \; 0 \quad \quad & 5 \quad &0 \;\\ \; 0 \quad \quad & 0 \quad &5 \; \end{split}} \right]$ |
${{{Q}} _w} = 5 \times {10^{ - 5}}\left[ {\begin{split} \; 1\quad \quad &0 \quad &0\; \\ \; 0\quad \quad &1 \quad &0 \;\\ \; 0\quad \quad &0 \quad &1 \; \end{split}} \right]$ |
${{{Q}} _v} = {0.125^2}{I _6}$ |
四元数滤波的参数为
如图 3~4所示,李群滤波算法具有较好的收敛性和较高的姿态估计精度,在150次滤波后精度达到0.005°。由于四元数模型的不确定性误差使用EKF四元数滤波导致X轴上的误差随时间发散。仿真实验结果验证了以上的分析,证明本文所提出的算法的稳定性和可靠性。通过比较可以得出李群滤波在对准精度和稳定性方面都优于四元数的方法,李群滤波适用于基于星敏感器的航天器姿态估计问题。
![]() |
图 3李群滤波三轴误差角Fig. 3The angle errors in three axis using Lie group filter |
![]() |
图 4四元数滤波三轴误差角Fig. 4The angle errors in three axis using quaternion filter |
在实际应用中刚体的姿态描述方法,多采用单位四元数,但是基于四元数的描述方法存在着非唯一性和归一化的问题,在滤波过程中四元数模型的非线性问题也影响着滤波的性能。本文通过李群的性质直接使用姿态矩阵描述姿态,避免了实际应用中求解四元数在转化为姿态矩阵的过程。在此基础上结合卡尔曼滤波最优递推估计的思想,提出了一种全新的基于星敏感器航天器姿态的解算算法,实现了航天器姿态矩阵的直接估计。本文只是对李群在滤波方法上的应用做出了初步的探索,证明了该应用有一定的可行性,由于算法未考虑李群形式下的传感器误差,所以滤波收敛速率较慢。下一步将就如何使用李群对传统的误差模型建模进行研究,以提高滤波的估计精度和速度。
[1] |
BRAUN R D, MANNING R M. Mars exploration entry, descent and landing challeges[C]// Aerospace Conference. [S. l.]: IEEE, 2006: 18 .
( ![]() |
[2] |
GARCÍA-LLAMA E, IVANOV M C, WINSKI R G, et al. Mars science laboratory entry guidance improvements study for the Mars 2018 mission[C]// Aerospace Conference. [S. l.]: IEEE, 2012: 1-11.
( ![]() |
[3] |
崔平远,于正湜,朱圣英. 火星进入段自主导航技术研究现状与展望[J]. 宇航学报,2013,34(4):447-456.
CUI P Y,YU Z S,ZHU S Y. Research progress and prospect of autonomous navigation techniques for Mars entry phase[J]. Journal of Astronautics,2013,34(4):447-456. http://d.old.wanfangdata.com.cn/Periodical/yhxb201304001 ( ![]() |
[4] |
于正湜,崔平远. 行星着陆自主导航与制导控制研究现状与趋势[J]. 深空探测学报,2016,3(4):345-355.
YU Z S,CUI P Y. Research status and developing trend of the autonomous navigation,guidance,and control for planetary landing[J]. Journal of Deep Space Exploration,2016,3(4):345-355. http://d.old.wanfangdata.com.cn/Periodical/sktcxb201604006 ( ![]() |
[5] |
刘垒,张路,郑辛,等. 星敏感器技术研究现状及发展趋势[J]. 红外与激光工程,2007,36(s2):529-533.
LIU L,ZHANG L,ZHENG X,et al. Current situation and development trends of star sensor technology[J]. Infrared and Laser Engineering,2007,36(s2):529-533. http://d.old.wanfangdata.com.cn/Periodical/hwyjggc2007z2138 ( ![]() |
[6] |
龚德铸,王艳宝,卢欣. 基于星敏感器的CCD47-20航天应用[J]. 空间控制技术与应用,2006(6):42-50.
( ![]() |
[7] |
YU-FENG L I,HAO Z H. Research of hyper accuracy subpixel subdivision location algorithm for star image[J]. Optical Technique,2005(5):1-9.
http://www.wanfangdata.com.cn/details/detail.do?_type=perio&id=gxjs200505005
( ![]() |
[8] |
SHUSTER M D,S. D. O H Three-axis attitude determination from vector observations[J]. Journal of Guidance Control & Dynamics,2012,4(1):1-9.
http://d.old.wanfangdata.com.cn/NSTLHY/NSTL_HYCC0212632985/
( ![]() |
[9] |
MARKLEY F L. Attitude determination using vector observations and the singular value decomposition[J]. Journal of the Astronautical Sciences,1988,36(3):245-258.
( ![]() |
[10] |
BARITZHACK I Y,HARMAN R R. Optimized TRIAD algorithm for attitude determination[J]. Journal of Guidance Control & Dynamics,2012,20(1):208-211.
http://d.old.wanfangdata.com.cn/NSTLQK/10.2514-2.4025/
( ![]() |
[11] |
MARKLEY F L. Attitude determination from vector observations:a fast optimal matrix algorithm[J]. Journal of the Astronautical Sciences,1993,41(2):261-280.
http://d.old.wanfangdata.com.cn/NSTLQK/NSTL_QKJJ027178328/
( ![]() |
[12] |
LEFFERTS E J,MARKLEY F L,SHUSTER M D. Kalman filtering for spacecraft attitude estimation[J]. Journal of Guidance Control & Dynamics,1982,5(4):536-542.
( ![]() |
[13] |
CHOUKROUN D,WEISS H,BAR-ITZHACK I Y,et al. Kalman filtering for matrix estimation[J]. IEEE Transactions on Aerospace and Electronic Systems,2006,42(1):147-159.
http://d.old.wanfangdata.com.cn/Periodical/xtfzxb201208021
( ![]() |
[14] |
SHUSTER M D. Kalman filtering of spacecraft attitude and the QUEST model[J]. Journal of the Astronautical Sciences,1990,38(3):377-393.
( ![]() |
[15] |
JULIER S J, UHLMANN J K, DURRANT-WHYTE H F. A new approach for filtering nonlinear systems[C]// American Control Conference. [S. l.]: IEEE, 2002.
( ![]() |
[16] |
理查得·摩雷, 李泽湘, 夏恩卡·萨思特里. 机器人操作的数学导论[M]. 北京: 机械工业出版社, 1998.
RICHARD M M, LI Z X, SHANKAR S. A mathematical introduction to robotic manipulation[M]. Beijing: China Machine Press, 1998. ( ![]() |
[17] |
BARRAU A, BONNABEL S. Intrinsic filtering on SO(3) with discrete-time observations[C]// Decision and Control. [S. l.]: IEEE, 2013:3255-3260.
( ![]() |