交會對接氣浮仿真平臺視覺輔助姿態確定

魏喜慶 宋申民

（哈爾濱工業大學控制理論與制導技術研究中心，哈爾濱150001）

1 引言

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

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

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

2 四自由度對接仿真平臺

四自由度對接仿真系統由追蹤星、目標星和光滑無摩擦的大理石平臺組成，用于完成對接的地面演示試驗。追蹤星由支撐系統和星體系統兩部分組成，支撐系統的底座下面安裝有平面氣浮墊，通過噴氣在氣浮墊和大理石平臺之間形成一層氣膜，支撐起了整個追蹤星，可以實現整個系統與大理石平臺的無摩擦二維平動和繞垂直軸的轉動；支撐系統和星體系統由單軸氣浮軸連接，既能支撐起星體，又能保證星體繞側向軸做無摩擦的俯仰運動。因此系統可模擬空間無摩擦的四自由度運動。

星體的執行機構包括噴嘴和飛輪。仿真平臺上的13個冷氣噴氣噴嘴，可以實現二維的平動與轉動。另外采用了兩個反作用飛輪，與噴氣系統結合來控制星體的俯仰與偏航運動，具體的控制實現方式是采用大范圍的轉動機動時使用噴氣驅動，而微調時用飛輪控制。

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

圖1 視覺測量系統原理圖Fig.1 Diagram of vision measurement system

3 姿態運動方程

3.1 四元數姿態運動學方程

四元數由一個三維的矢量和一個標量兩部分組成，寫成如下形式［9］：式中q13和q4分別是四元數矢量和標量部分；e為旋轉軸；φ為繞旋轉軸旋轉的角度。由于四元數四維的向量來表示3個角度，因此四元數各個元素彼此并不獨立，有‖q‖＝1。四元數表示的旋轉矩陣［6］：

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

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

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

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

四元數的姿態運動學方程［10］為

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

3.2 修正的羅德里格參數

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

修正的羅德里格參數相對于歐拉角具有的優勢是只有在360°時才會出現奇異值。另定義：

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

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

4 擴展卡爾曼濾波

4.1 狀態方程

定義真實四元數表示為四元數相乘［12］：

式中qref（t）代表一個參考四元數，δq（ap（t））代表qref（t）與真實四元數q（t）之間的誤差四元數。易知有修正的羅德里格參數ap（t）與δq（ap（t））互為對應。

由于qref（t）也是單位四元數，同樣遵守式（6）的運動學方程

式中ωref是參考姿態的參考角速度，為簡便起見，從公式（9）以后略去公式中變量關于時間的表達。

對式（8）進行求導，并將式（6）和（9）代入可得［12］：

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

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

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

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

式中 Δb＝b-，將式（14）帶入到式（13）并忽略關于ap的高階項，可得

所以系統的誤差狀態方程可以得出

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

4.2 測量方程

追蹤星通過CCD像機觀測目標星上的特征點靶標，可以測量追蹤星和目標星的相對姿態，目標星固定不動，因此可以設目標星的特征點坐標系為參考坐標系。假設追蹤星星體系和參考系之間的姿態角四元數測量值為qobs，觀測值和姿態預測四元數的偏差為δq（aobs）：

式中aobs為與δq（aobs）相對應的修正的羅德里格參數。

因此系統狀態偏差離散測量模型可以簡化為

其中量測噪聲v（k）的數學期望值和協方差陣分別為：

因此由式（16）可知系統的量測矩陣：

4.3 EKF姿態確定流程

由姿態確定的EKF工作流程可以看出，濾波器經過初值設置后，主要步驟是預測和更新，與普通EKF類似。但由于本文是關于偏差的EKF，所以在預測和更新之后增加了偏差量對預測值的補償。

5 非線性觀測器設計

5.1 非線性觀測器

為了克服EKF存在的初值調節困難和避免濾波更新中矩陣求逆導致的計算復雜性，下文基于Lyapunov方法設計了一種新型的非線性觀測器。觀測器的設計過程同樣利用了四元數和修正的羅德里格參數，并且充分利用了系統的姿態運動方程，證明了觀測器的漸進穩定性，從而在具有較大初值的情況下保證了觀測器的收斂。

定義李雅普諾夫函數：

其中，α＞0，β＞0為常值系數。對V進行求導：

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

其中，ε為修正量，從而

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

當選擇：

式中λ、γ為系數，λ＞0，γ＞0，則式（19）可變為

易知（ap，Δb）是負定的，因此式（18），（20）和（21）是全局漸進穩定的。

5.2 非線性觀測器流程

由非線性觀測器的工作流程看出其與EKF的流程類似，但是形式相對簡單。相比較矩陣運算較少，沒有關于矩陣求逆的運算，易于實現，計算量也較小。

6 試驗結果與分析

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

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

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

圖3 姿態角估計誤差Fig.3 Attitude estimate error

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

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

由圖5可知，當其他兩個參數固定只改變γ，當γ取值較小時收斂較慢濾波器收斂緩慢，因此γ取10可以保證濾波器的快速收斂。

由圖6可知，當其他兩個參數固定只改變λ大小，發現λ值對濾波器具有較明顯的影響。隨著λ增大，濾波器誤差會隨之增大，甚至會造成不穩定的現象，因此取λ＝1即可。

圖4 α／β值對觀測器估計值影響Fig.4 Effect ofα／βon the observer estimate

圖5 γ值對觀測器估計值影響Fig.5 Effect ofγon the observer estimate

圖6 λ值對觀測器估計值影響Fig.6 Effect ofλon the observer estimate

7 結束語

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

［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.