魏喜慶 宋申民

（哈爾濱工業(yè)大學(xué)控制理論與制導(dǎo)技術(shù)研究中心，哈爾濱150001）

1 引言

航天器交會(huì)對(duì)接（Rendezvous and Docking，RVD）過程中，要求兩個(gè)航天器不能發(fā)生碰撞，這是至關(guān)重要的安全要求。為了滿足RVD的高精度和苛刻安全要求，需要大量理論研究和物理仿真。利用氣浮技術(shù)進(jìn)行全物理仿真，是發(fā)展較早且廣泛采用的一種有效手段［1］。四自由度對(duì)接仿真平臺(tái)采用了單軸氣浮軸承的形式，開發(fā)對(duì)接與分離的地面演示系統(tǒng)，用于仿真驗(yàn)證最后逼近階段的制導(dǎo)與控制規(guī)律、衛(wèi)星姿態(tài)控制以及位姿測(cè)量系統(tǒng)的有效性和可靠性。

位姿估計(jì)是計(jì)算機(jī)視覺的一個(gè)重要內(nèi)容，n點(diǎn)透視（Perspective-n-Point，PnP）問題具有常規(guī)的解法［2］，但是快速準(zhǔn)確的求解此類問題，仍一直是學(xué)者努力的方向。Nister給出了求解位姿的五點(diǎn)算法［3］，Lu基于n點(diǎn)透視提出了一種正交迭代方法求解位姿［4］。利用陀螺儀以及星敏感器等裝置的擴(kuò)展卡爾曼濾波（EKF）姿態(tài)確定算法，在20世紀(jì)80年代初成為研究熱點(diǎn)［5-6］，并逐漸發(fā)展為一種成熟方法，成功應(yīng)用在了多種飛行器上［7］。

本文的主要研究利用視覺攝像機(jī)獲得的姿態(tài)角和俯仰與偏航軸陀螺儀輸出的角速度來有效地確定姿態(tài)。由于視覺系統(tǒng)的CCD相機(jī)的視野有限，僅有40°左右，因此一旦主動(dòng)星轉(zhuǎn)動(dòng)使目標(biāo)超出視野，就會(huì)造成視覺定姿失敗；而陀螺儀輸出角速度具有自主性且輸出信號(hào)更新速率快，但是漂移隨時(shí)間累積的特點(diǎn)。為了將兩者優(yōu)勢(shì)結(jié)合起來，更好地獲得主動(dòng)星的姿態(tài)，本文采用了這種成熟的EKF算法，充分利用了姿態(tài)運(yùn)動(dòng)學(xué)方程和陀螺儀模型以及視覺的觀測(cè)數(shù)據(jù)。由于EKF的初始狀態(tài)協(xié)方差陣的選取對(duì)濾波精度影響較大，且本系統(tǒng)的協(xié)方差陣為6維方陣，如果初值選取不當(dāng)會(huì)造成濾波器發(fā)散；又由于求取濾波增益時(shí)需要進(jìn)行求逆運(yùn)算，存在求解計(jì)算量大的問題。為了克服這些困難，本文提出了一種新的姿態(tài)估計(jì)方法，使用四元數(shù)和改進(jìn)的羅德里格參數(shù)（MRPs）相結(jié)合，利用李雅普諾夫 （Lyapunov）函數(shù)推導(dǎo)出一種非線性觀測(cè)器，并證明了觀測(cè)器的全局收斂性。

2 四自由度對(duì)接仿真平臺(tái)

四自由度對(duì)接仿真系統(tǒng)由追蹤星、目標(biāo)星和光滑無摩擦的大理石平臺(tái)組成，用于完成對(duì)接的地面演示試驗(yàn)。追蹤星由支撐系統(tǒng)和星體系統(tǒng)兩部分組成，支撐系統(tǒng)的底座下面安裝有平面氣浮墊，通過噴氣在氣浮墊和大理石平臺(tái)之間形成一層氣膜，支撐起了整個(gè)追蹤星，可以實(shí)現(xiàn)整個(gè)系統(tǒng)與大理石平臺(tái)的無摩擦二維平動(dòng)和繞垂直軸的轉(zhuǎn)動(dòng)；支撐系統(tǒng)和星體系統(tǒng)由單軸氣浮軸連接，既能支撐起星體，又能保證星體繞側(cè)向軸做無摩擦的俯仰運(yùn)動(dòng)。因此系統(tǒng)可模擬空間無摩擦的四自由度運(yùn)動(dòng)。

星體的執(zhí)行機(jī)構(gòu)包括噴嘴和飛輪。仿真平臺(tái)上的13個(gè)冷氣噴氣噴嘴，可以實(shí)現(xiàn)二維的平動(dòng)與轉(zhuǎn)動(dòng)。另外采用了兩個(gè)反作用飛輪，與噴氣系統(tǒng)結(jié)合來控制星體的俯仰與偏航運(yùn)動(dòng)，具體的控制實(shí)現(xiàn)方式是采用大范圍的轉(zhuǎn)動(dòng)機(jī)動(dòng)時(shí)使用噴氣驅(qū)動(dòng)，而微調(diào)時(shí)用飛輪控制。

測(cè)量系統(tǒng)包括慣性和視覺測(cè)量系統(tǒng)兩部分。慣性測(cè)量裝置包括兩個(gè)正交放置的陀螺儀，可以輸出偏航和俯仰兩個(gè)方向的角速度。視覺測(cè)量系統(tǒng)是對(duì)接系統(tǒng)的主要測(cè)量裝置，通過目標(biāo)星上安裝的主動(dòng)目標(biāo)器，安裝在追蹤星上的CCD相機(jī)來獲取目標(biāo)器上的發(fā)光點(diǎn)成像，通過圖像處理、特征匹配和利用投影姿態(tài)與定標(biāo)迭代（Pose from Orthography and Scaling with Iterations，POSIT）算法［8］可以得到兩個(gè)星體之間的相對(duì)位姿，從而為氣浮追蹤星提供所需的制導(dǎo)信息，如圖1所示。

圖1 視覺測(cè)量系統(tǒng)原理圖Fig.1 Diagram of vision measurement system

3 姿態(tài)運(yùn)動(dòng)方程

3.1 四元數(shù)姿態(tài)運(yùn)動(dòng)學(xué)方程

四元數(shù)由一個(gè)三維的矢量和一個(gè)標(biāo)量?jī)刹糠纸M成，寫成如下形式［9］：式中q13和q4分別是四元數(shù)矢量和標(biāo)量部分；e為旋轉(zhuǎn)軸；φ為繞旋轉(zhuǎn)軸旋轉(zhuǎn)的角度。由于四元數(shù)四維的向量來表示3個(gè)角度，因此四元數(shù)各個(gè)元素彼此并不獨(dú)立，有‖q‖＝1。四元數(shù)表示的旋轉(zhuǎn)矩陣［6］：

式中Ⅰ3×3為三維單位陣，［q13×］為叉乘矩陣。叉乘矩陣定義為：a×b＝ ［a×］b，其中

式中a1，a2和a3是矢量a的三個(gè)分量。

四元數(shù)乘法的采用Lefferts，Markley和Shuster等人定義［6］：

如果將式（4）中的q定義為原始四元數(shù)，q′定義為增量四元數(shù)，則四元數(shù)q″的含義是在q的基礎(chǔ)上增加了q′，表示為姿態(tài)矩陣的形式［9］：從而可以看出，從A（q）基礎(chǔ)上左乘A（q′）得到關(guān)于q″的姿態(tài)矩陣，與通常使用的姿態(tài)角表示的姿態(tài)矩陣連續(xù)旋轉(zhuǎn)的順序相同。

四元數(shù)的姿態(tài)運(yùn)動(dòng)學(xué)方程［10］為

式中ω是體系相對(duì)于慣性系的角速度在體系下的表示。

3.2 修正的羅德里格參數(shù)

修正的羅德里格參數(shù)定義為p＝q13／（1＋q4）＝etan（φ／4）

修正的羅德里格參數(shù)相對(duì)于歐拉角具有的優(yōu)勢(shì)是只有在360°時(shí)才會(huì)出現(xiàn)奇異值。另定義：

在不致發(fā)生混淆的情況下，為了敘述方便下文將ap也稱為修正的羅德里格參數(shù)。從式（7）的最后一項(xiàng)能夠看出，當(dāng)φ為小角度時(shí)，φ與ap（ap的幅值）近似相等。

反之，由式（7）和‖q‖＝1也可以得到由修正羅德里格參數(shù)表示的四元數(shù)［11］：

4 擴(kuò)展卡爾曼濾波

4.1 狀態(tài)方程

定義真實(shí)四元數(shù)表示為四元數(shù)相乘［12］：

式中qref（t）代表一個(gè)參考四元數(shù)，δq（ap（t））代表qref（t）與真實(shí)四元數(shù)q（t）之間的誤差四元數(shù)。易知有修正的羅德里格參數(shù)ap（t）與δq（ap（t））互為對(duì)應(yīng)。

由于qref（t）也是單位四元數(shù)，同樣遵守式（6）的運(yùn)動(dòng)學(xué)方程

式中ωref是參考姿態(tài)的參考角速度，為簡(jiǎn)便起見，從公式（9）以后略去公式中變量關(guān)于時(shí)間的表達(dá)。

對(duì)式（8）進(jìn)行求導(dǎo)，并將式（6）和（9）代入可得［12］：

式中 角速度ω可以由陀螺輸出ωout來表示

式中b為陀螺漂移；ηv和ηu為不相關(guān)零均值白噪聲，其協(xié)方差陣分別為σvⅠ3×3和σuⅠ3×3。當(dāng)角度偏差δq（ap）很小時(shí)，由式（10）易知參考角速度ωref近似于估計(jì)角速度，從而參考角速度可以寫為

將誤差四元數(shù)寫為修正的羅德里格參數(shù)形式，得ap＝4δq13／（1＋δq4），將其代入式（10）得

由公式（11）和（12）可得：

式中 Δb＝b-，將式（14）帶入到式（13）并忽略關(guān)于ap的高階項(xiàng)，可得

所以系統(tǒng)的誤差狀態(tài)方程可以得出

高斯白噪聲w均值為零，方差陣為

4.2 測(cè)量方程

追蹤星通過CCD像機(jī)觀測(cè)目標(biāo)星上的特征點(diǎn)靶標(biāo)，可以測(cè)量追蹤星和目標(biāo)星的相對(duì)姿態(tài)，目標(biāo)星固定不動(dòng)，因此可以設(shè)目標(biāo)星的特征點(diǎn)坐標(biāo)系為參考坐標(biāo)系。假設(shè)追蹤星星體系和參考系之間的姿態(tài)角四元數(shù)測(cè)量值為qobs，觀測(cè)值和姿態(tài)預(yù)測(cè)四元數(shù)的偏差為δq（aobs）：

式中aobs為與δq（aobs）相對(duì)應(yīng)的修正的羅德里格參數(shù)。

因此系統(tǒng)狀態(tài)偏差離散測(cè)量模型可以簡(jiǎn)化為

其中量測(cè)噪聲v（k）的數(shù)學(xué)期望值和協(xié)方差陣分別為：

因此由式（16）可知系統(tǒng)的量測(cè)矩陣：

4.3 EKF姿態(tài)確定流程

由姿態(tài)確定的EKF工作流程可以看出，濾波器經(jīng)過初值設(shè)置后，主要步驟是預(yù)測(cè)和更新，與普通EKF類似。但由于本文是關(guān)于偏差的EKF，所以在預(yù)測(cè)和更新之后增加了偏差量對(duì)預(yù)測(cè)值的補(bǔ)償。

5 非線性觀測(cè)器設(shè)計(jì)

5.1 非線性觀測(cè)器

為了克服EKF存在的初值調(diào)節(jié)困難和避免濾波更新中矩陣求逆導(dǎo)致的計(jì)算復(fù)雜性，下文基于Lyapunov方法設(shè)計(jì)了一種新型的非線性觀測(cè)器。觀測(cè)器的設(shè)計(jì)過程同樣利用了四元數(shù)和修正的羅德里格參數(shù)，并且充分利用了系統(tǒng)的姿態(tài)運(yùn)動(dòng)方程，證明了觀測(cè)器的漸進(jìn)穩(wěn)定性，從而在具有較大初值的情況下保證了觀測(cè)器的收斂。

定義李雅普諾夫函數(shù)：

其中，α＞0，β＞0為常值系數(shù)。對(duì)V進(jìn)行求導(dǎo)：

將式（12）中參考角速度ωref重新定義為

其中，ε為修正量，從而

將其帶入式（13），忽略ap高階項(xiàng)和ηv，類似于式（15）得

當(dāng)選擇：

式中λ、γ為系數(shù)，λ＞0，γ＞0，則式（19）可變?yōu)?/p>

易知（ap，Δb）是負(fù)定的，因此式（18），（20）和（21）是全局漸進(jìn)穩(wěn)定的。

5.2 非線性觀測(cè)器流程

由非線性觀測(cè)器的工作流程看出其與EKF的流程類似，但是形式相對(duì)簡(jiǎn)單。相比較矩陣運(yùn)算較少，沒有關(guān)于矩陣求逆的運(yùn)算，易于實(shí)現(xiàn)，計(jì)算量也較小。

6 試驗(yàn)結(jié)果與分析

這部分通過四自由度對(duì)接仿真平臺(tái)試驗(yàn)來驗(yàn)證擴(kuò)展卡爾曼濾波器和非線性觀測(cè)器的正確性和有效性。試驗(yàn)采用的陀螺儀的更新頻率達(dá)到100Hz，采用的陀螺儀測(cè)量噪聲標(biāo)準(zhǔn)差和陀螺漂移噪聲標(biāo)準(zhǔn)差分別為而視覺采樣周期為10Hz，采用的CCD相機(jī)由于光線變化和圖像處理等原因，具有σ＝0.01°標(biāo)準(zhǔn)差的噪聲影響。假設(shè)初始陀螺漂移的估計(jì)值為初始姿態(tài)四元數(shù)估計(jì)q0＝［0 0 1 0］T。設(shè)姿態(tài)角偏差的初始協(xié)方差為0.01（°）2，陀螺漂移協(xié)方差為3×10-9［（°）／s］2。

試驗(yàn)時(shí)間為500s，圖2中by、bz分別表示俯仰和偏航方向的陀螺漂移估計(jì)誤差隨時(shí)間變化曲線，圖3為俯仰和偏航方向姿態(tài)角估計(jì)誤差隨時(shí)間變化曲線。從圖2、圖3可看出陀螺儀漂移和姿態(tài)角估計(jì)很快收斂，但是由于CCD相機(jī)量測(cè)更新頻率只能達(dá)到10Hz，估計(jì)后的姿態(tài)角精度不高，姿態(tài)角誤差精度略低于0.1°。從圖2中可以看到，對(duì)于陀螺漂移的估計(jì)EKF和非線性觀測(cè)器的效果接近，但是對(duì)于姿態(tài)角的估計(jì)，EKF濾波過程中間偶爾出現(xiàn)誤差較大的情況，非線性觀測(cè)器估計(jì)精度要優(yōu)于EKF。

圖2 陀螺漂移估計(jì)誤差Fig.2 Gyros bia estimate error

圖3 姿態(tài)角估計(jì)誤差Fig.3 Attitude estimate error

其中主要的一個(gè)因素是EKF初始協(xié)方差陣僅對(duì)角線元素就有6個(gè)參數(shù)，其選取對(duì)EKF精度有較大影響，因此選取的參數(shù)很難選到一個(gè)最優(yōu)的參數(shù)，難以發(fā)揮EKF的最佳性能，導(dǎo)致非線性觀測(cè)器性能表現(xiàn)優(yōu)于EKF。而所用到的非線性觀測(cè)器只用到了α／β，γ和λ共3個(gè)參數(shù)，調(diào)整起來相對(duì)容易。由于試驗(yàn)表明這3個(gè)參數(shù)對(duì)姿態(tài)角的估計(jì)影響不大，只對(duì)陀螺儀漂移估計(jì)具有較大影響。圖4～圖6給出了非線性觀測(cè)器3個(gè)參數(shù)對(duì)估計(jì)精度影響結(jié)果（為簡(jiǎn)化只給出非線性觀測(cè)器的偏航方向陀螺漂移誤差隨時(shí)間變化曲線）。

由圖4中by和bz隨時(shí)間變化的曲線分析，在其他兩個(gè)參數(shù)γ和λ固定的情況下只調(diào)整α／β，by受參數(shù)變化影響較小，而bz受影響較大，隨α／β增大非線性觀測(cè)器陀螺漂移估計(jì)初始誤差隨之增加，因此α／β不宜取較大值，取α／β＝0.01即可。

由圖5可知，當(dāng)其他兩個(gè)參數(shù)固定只改變?chǔ)?，?dāng)γ取值較小時(shí)收斂較慢濾波器收斂緩慢，因此γ取10可以保證濾波器的快速收斂。

由圖6可知，當(dāng)其他兩個(gè)參數(shù)固定只改變?chǔ)舜笮?，發(fā)現(xiàn)λ值對(duì)濾波器具有較明顯的影響。隨著λ增大，濾波器誤差會(huì)隨之增大，甚至?xí)斐刹环€(wěn)定的現(xiàn)象，因此取λ＝1即可。

圖4 α／β值對(duì)觀測(cè)器估計(jì)值影響Fig.4 Effect ofα／βon the observer estimate

圖5 γ值對(duì)觀測(cè)器估計(jì)值影響Fig.5 Effect ofγon the observer estimate

圖6 λ值對(duì)觀測(cè)器估計(jì)值影響Fig.6 Effect ofλon the observer estimate

7 結(jié)束語(yǔ)

本文首先介紹了包含兩軸陀螺儀和一個(gè)視覺測(cè)量系統(tǒng)的四自由度對(duì)接仿真平臺(tái)的姿態(tài)確定系統(tǒng)。為了充分利用兩種測(cè)量裝置進(jìn)行姿態(tài)確定，首先設(shè)計(jì)了采用四元數(shù)和MRPs結(jié)合的經(jīng)典EKF算法。為了克服EKF算法的不足，本文提出了一種非線性觀測(cè)器的姿態(tài)確定方法并用Lyapunov方法證明其收斂性。通過對(duì)傳統(tǒng)EKF和新設(shè)計(jì)的非線性觀測(cè)器分別進(jìn)行試驗(yàn)對(duì)比，所設(shè)計(jì)的非線性觀測(cè)器用于本系統(tǒng)不僅精度高于EKF算法，且具有需要調(diào)節(jié)參數(shù)少，簡(jiǎn)單易行的優(yōu)勢(shì)。

［1］SCHWARTZ J，PECK M，HALL C.Historical review of air-bearing spacecraft simulators［J］.Journal of Guidance，Control and Dynamics，2003，26 （4）：513-522.

［2］FISCHLER M，BOLLES R.Random sample consensus：aparadigm for model fitting with applications to image analysis and automated cartography［J］.Communications of the ACM，1981，24 （6）：381-395.

［3］NISTER D.An efficient solution to the five-point relative pose problem［C］.IEEE Computer Society Conference on Computer Vision and Pattern Recognition，Madison，WI，United states，2003.

［4］LU C P，HAGER G D，MJOLSNESS E.Fast and globally convergent pose estimation from video images［J］.IEEE Transactions on Pattern Analysis and Machine Intelligence，2000，22：610-622.

［5］MURRELL J.Precision attitude determination for multimission spacecraft［C］／／Proceedings of the AIAA Guidance，Navigation，and Control Conference，Palo，CA，United States，1978.

［6］LEFFERTS E J，MARKLEY F L，SHUSTER M D.Kalman filtering for spacecraft attitude estimation［J］.Journal of Guidance，Control，and Dynamics，1982，5（5）：417-429.

［7］ANDREWS S F，BILANOW S.Recent flight results of the TRMM Kalman filter［C］.AIAA Guidance，Navigation，and Control Conference and Exhibit，Monterey，CA，United States，2002.

［8］DEMENTHON D F，DAVIS L S.Model-based object pose in 25lines of code［J］.International Journal of Computer Vision，1995，15 （1-2）：123-141.

［9］SHUSTER M.A survey of attitude representations［J］.Journal of the Astronautical Sciences，1993，41 （4）：439-517.

［10］KIM SG，CRASSIDIS J L，CHENG Y，et al.Kalman filtering for relative spacecraft attitude and position estimation［J］.Journal of Guidance，Control，and Dynamics，2007，30（1）：133-143.

［11］CRASSIDIS J L，MARKLEY F L.Attitude estimation using modified rodrigues parameters［C］／／Proceedings of the American Astronautical Society Greenbelt，Maryland，United States，1996.

［12］MARKLEY F L.Attitude error representations for Kalman filtering［J］.Journal of Guidance，Control，and Dynamics，2003，26（2）：311-317.

［13］CRASSIDIS J，JUNKINS J.Optimal estimation of dynamic systems［M］.Boca Raton：Chapman ＆ Hall，2004：419-433.