一種基于李群描述的深空探測器姿態(tài)估計(jì)方法

徐 浩，裴?？。Y 寧

（1.北京工業(yè)大學(xué) 信息學(xué)部，北京 100124；2.計(jì)算智能與智能系統(tǒng)北京市重點(diǎn)實(shí)驗(yàn)室，北京 100124）

引 言

隨著我國航天事業(yè)的飛速發(fā)展，深空飛行器的自主導(dǎo)航問題成為了研究的熱點(diǎn)。深空飛行器距地距離遠(yuǎn)，工作時(shí)間長，深空環(huán)境未知，傳統(tǒng)的依賴地面與航天器通訊來實(shí)現(xiàn)導(dǎo)航和控制的方法存在實(shí)時(shí)性、成本高、干擾較大等種種限制，很難實(shí)現(xiàn)高精度的深空導(dǎo)航需求。因此，在深空探測領(lǐng)域自主導(dǎo)航技術(shù)成為了保證深空探測任務(wù)完成的關(guān)鍵技術(shù)[1-4]。目前在自主導(dǎo)航的姿態(tài)確定方面，星敏感器是深空探測航天器上常用的姿態(tài)確定儀器，其基本原理是基于星點(diǎn)質(zhì)心定位，實(shí)現(xiàn)星圖識(shí)別和姿態(tài)解算[5-7]，從而實(shí)現(xiàn)探測器姿態(tài)估計(jì)。相比較于太陽敏感器、地平儀、磁力計(jì)等姿態(tài)測量設(shè)備，星敏感器測量精度高，抗干擾能力及自主導(dǎo)航能力強(qiáng)。本文主要就姿態(tài)解算部分做出了研究。

基于星敏感器的航天器姿態(tài)確定是通過星敏感器測量同一坐標(biāo)矢量在慣性坐標(biāo)系下和星敏感器坐標(biāo)系下的不同數(shù)值來確定航天器姿態(tài)參數(shù)的過程。常用的姿態(tài)描述方法有歐拉角、四元數(shù)、羅格里格斯參數(shù)法、修正的羅格里格斯參數(shù)法、旋轉(zhuǎn)矩陣等?，F(xiàn)階段的姿態(tài)解算算法主要分為靜態(tài)確定性算法和動(dòng)態(tài)姿態(tài)估計(jì)兩大類。靜態(tài)確定性算法是根據(jù)一組觀測矢量求解出航天器坐標(biāo)系和慣性坐標(biāo)系之間的姿態(tài)矩陣進(jìn)而求出航天器姿態(tài)角。Davenport提出了q方法，通過四元數(shù)的性質(zhì)構(gòu)造K矩陣，用求取特征值的方法確定姿態(tài)四元數(shù)[8]?；谧钚《嗽淼腟VD方法將矩陣進(jìn)行奇異值分解得到姿態(tài)矩陣[9]。此外較常用的方法還有：基于牛頓迭代的數(shù)值算法、TRIAD方法[10]、FOAM方法[11]等。動(dòng)態(tài)姿態(tài)估計(jì)方法主要是基于四元數(shù)的卡爾曼濾波的改進(jìn)算法，現(xiàn)階段已有的方法有擴(kuò)展卡爾曼濾波（EKF）[12]、乘性卡爾曼濾波（MEKF）[13]、預(yù)測卡爾曼濾波[14]、自適應(yīng)卡爾曼濾波[15]等。動(dòng)態(tài)濾波姿態(tài)估計(jì)方法能夠克服觀測矢量中的不確定誤差，利用更多的觀測信息，實(shí)現(xiàn)最優(yōu)化估計(jì)下的最優(yōu)解，相比較于靜態(tài)確定性算法能夠更好的對姿態(tài)進(jìn)行估計(jì)。但是在采用四元數(shù)建模時(shí)EKF產(chǎn)生的線性化誤差極易引起濾波發(fā)散，有些濾波方法無法實(shí)現(xiàn)使用單一的姿態(tài)敏感儀器完成估計(jì)等問題還未得到較好的解決。

本文針對基于四元數(shù)描述的姿態(tài)估計(jì)方法存在的問題，提出了基于李群描述的深空探測器姿態(tài)估計(jì)的新方法。該方法基于李群對于旋轉(zhuǎn)矩陣的特定唯一描述特點(diǎn)，采用李群對深空探測器進(jìn)行姿態(tài)描述來解決四元數(shù)姿態(tài)描述方法中存在的非唯一性等問題。在此基礎(chǔ)上，針對四元數(shù)描述的姿態(tài)估計(jì)方法中的模型非線性問題，基于李群微分方程構(gòu)造了線性的姿態(tài)矩陣微分方程作為系統(tǒng)模型，并設(shè)計(jì)了李群濾波器來實(shí)現(xiàn)直接對姿態(tài)矩陣進(jìn)行估計(jì)。該方法能夠保證系統(tǒng)模型線性化結(jié)構(gòu)，克服傳統(tǒng)四元數(shù)方法中非線性模型在線性化時(shí)導(dǎo)致的模型誤差。在估計(jì)精度上比靜態(tài)姿態(tài)確定方法、四元數(shù)卡爾曼濾波方法具有更好的性能。

1 星敏感器

1.1 星敏感器基本工作原理

星敏感器一般有兩種工作模式：全天球識(shí)別工作模式和星跟蹤工作模式。全天球識(shí)別工作模式的工作原理為通過星敏感器的光學(xué)鏡頭在視場角內(nèi)拍攝星圖，經(jīng)過星圖識(shí)別、星點(diǎn)質(zhì)心定位和姿態(tài)解算幾個(gè)過程后輸出航天器姿態(tài)角；星跟蹤模式下星敏感器首先利用先驗(yàn)姿態(tài)信息，進(jìn)入星跟蹤模塊通過局部的星點(diǎn)質(zhì)心定位和識(shí)別最終解算出當(dāng)前姿態(tài)信息。星敏感器的基本工作原理框圖如圖 1所示。本文主要針對姿態(tài)解算提出了一種新的算法。

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

1.2 星敏感器觀測模型

星敏感器由光學(xué)系統(tǒng)和CCD感光裝置構(gòu)成，來自定位恒星的光線穿過光學(xué)透鏡聚焦到CCD成像平面。X和Y軸在CCD成像平面內(nèi)呈正交化，Z軸與透鏡的光軸重合。px和py兩坐標(biāo)點(diǎn)為目標(biāo)恒星在CCD成像平面成像的質(zhì)心坐標(biāo)，f是光學(xué)系統(tǒng)的焦距。星敏感器的測量原理如圖 2所示。

假設(shè)星敏感器坐標(biāo)點(diǎn)p=[px，py]T表示在星敏感器坐標(biāo)系下目標(biāo)恒星的坐標(biāo)，完整的目標(biāo)恒星慣性坐標(biāo)系下矢量ri在星敏感器坐標(biāo)系下的投影可表示為

假設(shè)星敏感器坐標(biāo)系與航天器載體坐標(biāo)系重合，tk時(shí)刻在航天器載體坐標(biāo)系下的目標(biāo)恒星的觀測方程可以寫成

其中：li(k)是i顆目標(biāo)恒星的觀測矢量；X(k)是慣性系到航天器載體坐標(biāo)系的姿態(tài)轉(zhuǎn)換矩陣；ri是第i顆目標(biāo)恒星在慣性系下的單位矢量；vi是星敏感器的觀測噪聲，通常被視為均值為零的隨機(jī)白噪聲并滿足以下性質(zhì)

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

2 基于李群的航天器姿態(tài)模型

李群理論是近代數(shù)學(xué)中的一個(gè)重要分支，19世紀(jì)由挪威數(shù)學(xué)家Marius Sophus Lie提出的[16]。李群是群，同時(shí)也是微分流形，滿足群運(yùn)算和可逆性。特殊正交群SO(n)是一般線性群GL(n)的子群，是李群中的一類群，定義為如下形式

本文主要考慮的航天器的姿態(tài)問題，所以主要討論三維特殊正交群的相關(guān)內(nèi)容。對等式RRT=I兩邊求導(dǎo)并移項(xiàng)得=-()T，為反對稱陣，整理后有下式

其中：R為SO(3)陣；Q為3×3反對稱矩陣，取向量k∈R3

其中， [·]×是反對稱矩陣的簡化數(shù)學(xué)表示。

在參考系下的航天器姿態(tài)角速度ω 可以表示為

空間航天器的運(yùn)動(dòng)動(dòng)力學(xué)微分方程可以被寫作

剛體飛行器的動(dòng)態(tài)姿態(tài)可以被改寫為如下的形式

其中：J為慣性矩陣；N是總共的外部轉(zhuǎn)矩。

3 姿態(tài)估計(jì)李群濾波算法

結(jié)合公式（2）和（8）構(gòu)造李群濾波方程，實(shí)現(xiàn)對航天器姿態(tài)的李群濾波估計(jì)。李群濾波器直接作用于基于李群的系統(tǒng)方程，相比于四元數(shù)方法省去了轉(zhuǎn)換為姿態(tài)矩陣的步驟，保證了濾波器的線性結(jié)構(gòu)，減少非線性的計(jì)算誤差，李群濾波能夠?qū)崿F(xiàn)快速有效的姿態(tài)估計(jì)。

3.1 空間飛行器姿態(tài)動(dòng)力學(xué)系統(tǒng)模型

首先，根據(jù)航天器姿態(tài)的動(dòng)力學(xué)模型和星敏感器觀測模型構(gòu)建時(shí)間連續(xù)的系統(tǒng)模型

其中：Xt表示從航天器的姿態(tài)矩陣；ωt表示航天器姿態(tài)旋轉(zhuǎn)矢量；wt表示一個(gè)隨機(jī)的連續(xù)白噪聲；lt表示目標(biāo)恒星在航天器載體坐標(biāo)下的連續(xù)觀測矢量；rt代表了目標(biāo)恒星在慣性坐標(biāo)系下的坐標(biāo)矢量；vt為星敏感器的連續(xù)觀測噪聲，本文中視為一個(gè)連續(xù)的隨機(jī)白噪聲。

為了解決傳感器的實(shí)際測量值為離散量的問題，需要在固定采樣時(shí)間頻率下對模型進(jìn)行離散化處理。公式（10）中Xt和ωt為一對李群和李代數(shù)，根據(jù)李代數(shù)到李群的映射關(guān)系，可在離散情況下將 ωt投影到李群空間對姿態(tài)矩陣Xt進(jìn)行更新，該過程也是求解差分方程的過程，得系統(tǒng)離散化模型為

其中：Xk+1表示在tk+1時(shí)刻的航天器載體姿態(tài)矩陣；Wk是在tk+1時(shí)刻下均值白噪聲在轉(zhuǎn)動(dòng)形式下的表示，傳播矩陣為 σ12I3；Ak為tk+1時(shí)刻的轉(zhuǎn)動(dòng)增量，即對ωt進(jìn)行指數(shù)展開。式（13）中，lk+1rk+1分別表示tk+1時(shí)刻目標(biāo)恒星在航天器載體坐標(biāo)系和慣性坐標(biāo)系下的坐標(biāo)矢量；vk+1為tk+1時(shí)刻的離散觀測噪聲，其協(xié)方差矩陣為σ22I3。

李群到李代數(shù)的映射是指數(shù)映射，其為光滑滿射，陀螺的測量值為旋轉(zhuǎn)矩陣X的李代數(shù)，所以使用式（12）的離散化模型能夠保證姿態(tài)動(dòng)力學(xué)系統(tǒng)的性質(zhì)。

3.2 離散時(shí)間的李群濾波

為了更好的提出和推導(dǎo)本文所提出的李群濾波方法，本文先分析一種傳統(tǒng)卡爾曼濾波通用離散系統(tǒng)模型，式（14）和（15）為傳統(tǒng)卡爾曼濾波通用矢量模型

其中：Lk，Yk∈R3，分別表示所考慮模型的離散狀態(tài)量和觀測量；Wk和Vk∈R3為高斯白噪聲，Hk∈ R3×3為量測矩陣。這種模型下的最常用的卡爾曼濾波的狀態(tài)一步預(yù)測方程和狀態(tài)估值計(jì)算方程為

使用兩個(gè)三維矢量ek+1|k=Lk+1-和ek+1=Lk+1-預(yù)測狀態(tài)誤差和糾正狀態(tài)誤差，可以得到

誤差方程可以具有自發(fā)性，然后濾波增益Kk+1可以根據(jù)誤差協(xié)方差矩陣進(jìn)行更新。濾波的收斂性可以根據(jù)可觀可控性原理來確定，卡爾曼濾波實(shí)際上是一種基于方差最小的最優(yōu)化估計(jì)方法。故根據(jù)式（12）和（13）的形式，將卡爾曼濾波方法與李群相結(jié)合，使用乘法原理替換該模型中的加法原理。下面對一個(gè)相似的方法進(jìn)行具體的描述。

根據(jù)線性情況下李群的對稱性原理并結(jié)合航天器姿態(tài)模型式（12），可以將式（16）和（17）轉(zhuǎn)換到李群形式下[17]

根據(jù)誤差均方差最小原理，李群濾波器設(shè)計(jì)如下：

狀態(tài)一步預(yù)測方程

一步預(yù)測均方差方程

其中：Qw為系統(tǒng)噪聲協(xié)方差矩陣，這里取σ12I3。

定義新息為 εk+1，新息為測量和估計(jì)量的一步誤差，這里使用兩個(gè)參考坐標(biāo)

新息的均方差矩陣表示為

其中：Qv表示量測噪聲協(xié)方{差矩陣，其}值取為σ22I6；Hk表示系統(tǒng)量測矩陣，

濾波增益

估計(jì)均方誤差方程

狀態(tài)估值計(jì)算方程采用公式（21）的方式，由于李代數(shù)到李群的指數(shù)映射為光滑滿射，使用指數(shù)映射能夠全面的修正狀態(tài)量，所以狀態(tài)估值計(jì)算方程為

其中 exp(·)表示指數(shù)展開，本文采用羅格里斯方程進(jìn)行運(yùn)算

李群濾波的過程總結(jié)如下

4 仿真結(jié)果與分析

相比較與傳統(tǒng)的確定性算法，本文所提出的基于李群濾波的動(dòng)態(tài)姿態(tài)估計(jì)方法，能夠加入航天器中的IMU信息，并且對系統(tǒng)噪聲和狀態(tài)噪聲進(jìn)行響應(yīng)，從而提供估計(jì)中的優(yōu)化解，同時(shí)在一定程度上降低了不確定性因素的影響。與四元數(shù)濾波的方法相比較，本文的算法直接對姿態(tài)矩陣進(jìn)行估計(jì)，避免了對模型進(jìn)行線性化帶來的誤差和四元數(shù)描述姿態(tài)的非唯一性等問題，提高了濾波的穩(wěn)定性和精度。下面通過一組仿真實(shí)驗(yàn)對算法進(jìn)行驗(yàn)證。

仿真條件：星敏感器安裝與在飛行器載體坐標(biāo)系重合。設(shè)定兩組目標(biāo)恒星在慣性參考坐標(biāo)系下的坐標(biāo)為

X-Y-Z三軸姿態(tài)初值分別為0，-90°，90。

理論上真實(shí)的初始姿態(tài)矩陣為

星敏感器模擬兩個(gè)觀測矢量的初值為

理論的姿態(tài)角速度為

慣性矩陣參數(shù)為

星敏感器采樣周期為

濾波器初始參數(shù)如下

四元數(shù)濾波的參數(shù)為q0=[1 0 0 0]，P0=I4，Q=1×10-5I4，R=0.01I4。

如圖 3～4所示，李群濾波算法具有較好的收斂性和較高的姿態(tài)估計(jì)精度，在150次濾波后精度達(dá)到0.005°。由于四元數(shù)模型的不確定性誤差使用EKF四元數(shù)濾波導(dǎo)致X軸上的誤差隨時(shí)間發(fā)散。仿真實(shí)驗(yàn)結(jié)果驗(yàn)證了以上的分析，證明本文所提出的算法的穩(wěn)定性和可靠性。通過比較可以得出李群濾波在對準(zhǔn)精度和穩(wěn)定性方面都優(yōu)于四元數(shù)的方法，李群濾波適用于基于星敏感器的航天器姿態(tài)估計(jì)問題。

圖3 李群濾波三軸誤差角Fig.3 The angle errors in three axis using Lie group filter

圖4 四元數(shù)濾波三軸誤差角Fig.4 The angle errors in three axis using quaternion filter

5 結(jié) 論

在實(shí)際應(yīng)用中剛體的姿態(tài)描述方法，多采用單位四元數(shù)，但是基于四元數(shù)的描述方法存在著非唯一性和歸一化的問題，在濾波過程中四元數(shù)模型的非線性問題也影響著濾波的性能。本文通過李群的性質(zhì)直接使用姿態(tài)矩陣描述姿態(tài)，避免了實(shí)際應(yīng)用中求解四元數(shù)在轉(zhuǎn)化為姿態(tài)矩陣的過程。在此基礎(chǔ)上結(jié)合卡爾曼濾波最優(yōu)遞推估計(jì)的思想，提出了一種全新的基于星敏感器航天器姿態(tài)的解算算法，實(shí)現(xiàn)了航天器姿態(tài)矩陣的直接估計(jì)。本文只是對李群在濾波方法上的應(yīng)用做出了初步的探索，證明了該應(yīng)用有一定的可行性，由于算法未考慮李群形式下的傳感器誤差，所以濾波收斂速率較慢。下一步將就如何使用李群對傳統(tǒng)的誤差模型建模進(jìn)行研究，以提高濾波的估計(jì)精度和速度。