基于狀態(tài)矩陣卡爾曼濾波的姿態(tài)估計算法研究

王 冰，隋立芬，吳江飛，張 宇

（1.信息工程大學 地理空間信息學院，河南 鄭州 450052；2.66240部隊，北京 100042）

GPS接收機除用于導航定位、精確著陸之外，還可用于航天、航空、航海以及地面等各種載體的姿態(tài)測定。GPS姿態(tài)測量系統(tǒng)與傳統(tǒng)測量系統(tǒng)相比，具有結構簡單、價格低廉、精度穩(wěn)定、體積小、固體化和可靠性好等優(yōu)點，可將其作為一種新的姿態(tài)測量方法代替或者輔助傳統(tǒng)的姿態(tài)測量［1］。基于GPS估計載體姿態(tài)算法有確定性算法和狀態(tài)估計算法兩大類［2］。確定性算法是只利用GPS的觀測信息進行姿態(tài)估計，狀態(tài)估計算法是結合載體運動模型與觀測信息進行姿態(tài)估計。

眾所周知，載體姿態(tài)確定的精度不僅取決于姿態(tài)測量系統(tǒng)硬件配置的性能與精度，還與所采用的姿態(tài)估計算法密切相關［3］。在姿態(tài)估計算法中，常用的姿態(tài)參數主要有方向余弦矩陣、歐拉角、姿態(tài)四元數、旋轉矢量、羅德里格參數等。而這些參數或者存在奇異、不連續(xù)問題，或者存在冗余性問題［4］。四元數以其計算量小、非奇異性、可全姿態(tài)工作等優(yōu)點，而在實際系統(tǒng)中得到廣泛應用。但四元數存在冗余問題，文獻［5－6］提出的乘性四元EKF算法通過構造乘性誤差四元數來計算四元數協方差，不僅避免了四元數規(guī)范化的限制，而且解決了四元數協方差奇異性問題。然而當系統(tǒng)非線性程度較強時，EKF算法的截斷誤差將大大降低濾波精度甚至會導致濾波發(fā)散，針對此，不少學者已經開展了相應的研究工作，提出了基于四元數的二階EKF算法［7］、無跡濾波［8］（unscented Kalman filter，UKF）、粒子濾波（particle filter，PF）［9］以及非線性預測 濾 波［10］（nonlinear predictive filter，NPF）等方法。

本文給出了一種矩陣卡爾曼濾波算法，該方法選擇方向余弦矩陣作為姿態(tài)參數，以此構造的姿態(tài)系統(tǒng)狀態(tài)方程和測量方程均為線性方程，可以避免基于誤差四元數的EKF濾波中存在的線性誤差。

1 載體姿態(tài)估計濾波器設計

1.1 系統(tǒng)狀態(tài)方程

定義衛(wèi)星本體系相對慣性系的方向余弦矩陣為R，由其表達的姿態(tài)運動學方程為［11］

其中：ω為載體坐標系相對慣性系的角速度在載體坐標系的坐標，

假設歷元間隔Δt＝tk＋1－tk足夠小，對式（1）進行離散化處理［12］，得到

為了推導方便，假設陀螺測量系與本體坐標系重合，陀螺測量模型建立如下：

其中：為陀螺的測量輸出值，ω為載體相對慣性空間的姿態(tài)角速度，ηv為陀螺測量噪聲。

將式（3）代入式（2）得到狀態(tài)方程

其中

由于狀態(tài)模型誤差Wk為3×3維矩陣，建立其協方差陣需要將其向量化

其中，vec［ηv×］可以表示為列向量ηv的線性形式

其中：ej（j＝1，2，3）為3×1維向量，位置j元素為1，其余元素為0。

將式（7）代入式（6），得到

根據誤差協方差傳播定律，建立狀態(tài)模型誤差協方差陣

1.2 GPS觀測方程

假設載體上一短基線同時觀測兩顆衛(wèi)星，雙差載波相位觀測方程描述為

其中：▽Δφ為雙差載波相位觀測值，z為雙差整周模糊度，λ為L1載波波長。由于載體上天線之間的距離很短，可假設同一歷元載體上所有天線接收中心到同一顆衛(wèi)星的視向量相同，記為s12。bE為基線在ECEF（Earth－Centered，Earth－Fixed）坐標系中的向量。ε為雙差載波相位觀測噪聲。

若載體上安裝m＋1個天線，共視衛(wèi)星數為n＋1個，選擇某一天線作為主天線，仰角最高的衛(wèi)星作為參考星（以符號r表示），將基線i的雙差載波相位觀測量表示為向量yi形式

基線i的觀測方程可以表示為

其中，系數陣A，S分別表示如下：

其中：In為n維單位向量；為基線i關于衛(wèi)星j，r的視向量。

根據式（12）將載體上m條基線觀測量組合，得到如下觀測方程：

假設BB表示為BE在載體坐標系中的相應向量，兩者之間的關系以姿態(tài)矩陣表示為RBB＝BE。多基線觀測方程可以表示為姿態(tài)矩陣形式

根據文獻［13］，多基線載波相位觀測量方差－協方差陣表示為

2 基于SMKF濾波的姿態(tài)估計算法

可以看出基于方向余弦矩陣的狀態(tài)方程、觀測方程均為線性方程，無需線性化處理，但其未知參數及觀測量均以矩陣形式出現，若采用kalman濾波對其進行估計，需要對其進行向量化。根據矩陣向量化定理［14］：vec（ABC）＝（CT?A）vecB，由狀態(tài)方程、觀測方程可以得到

由向量化后的系統(tǒng)方程式（17）、式（18）可以看出，其與線性kalman濾波系統(tǒng)方程類似，基于狀態(tài)矩陣的kalman濾波（SMKF）算法步驟歸納如下：

1）對狀態(tài)參數及其協方差陣進行初始化

2）計算預測狀態(tài)向量

3）計算預測狀態(tài)協方差陣

4）計算信息向量及其協方差陣

5）計算增益矩陣

6）求解新的狀態(tài)估計值

7）計算狀態(tài)估計的協方差矩陣k

考慮到方向余弦矩陣的正交化性質，在得到濾波的估計值＾Rk及其協方差陣Σ＾Rk后，需要對其進行正交化處理，即解算最優(yōu)化問題

文獻［15］詳細介紹了基于拉格朗日的正交矩陣最優(yōu)化算法，本文在此不作贅述。

3 仿真實驗

根據陀螺角速度、測量噪聲仿真得出陀螺觀測量及載體每個歷元的姿態(tài)。這里假設載體角速度及 初 值 為ω＝［cos（10Ω0t），cos（8Ω0t），cos（5.7Ω0t）］rad/s，Ω0＝0.001rad/s。載體上安裝3條基線，其在載體坐標系中的坐標為：b1＝［2 ，0，0］T，b2＝［1 ，1.7，0］T，b3＝［1，0.6，1.6 ］T，設定載波相位觀測噪聲為0.006m，利用VISUAL軟件［16］，基于2008真實的GPS衛(wèi)星星座分布、當前歷元的姿態(tài)及GPS載波相位觀測噪聲仿真得到GPS觀測值。

為了驗證MKF姿態(tài)估計算法的有效性，采用兩種方案對表1中4組仿真數據進行解算。方案如下：

方案1：基于乘性四元數卡爾曼濾波。

方案2：基于方向余弦矩陣的卡爾曼濾波。

表1給出不同仿真條件下兩種方案姿態(tài)解算統(tǒng)計結果。圖1為第4組數據兩種方案的參數估計誤差曲線圖（其中虛線為方案1結果，實線為方案2結果）。

表1 各方案結果統(tǒng)計分析

圖1 姿態(tài)角估計誤差曲線

1）由表1可以看出，不同條件數據下，基于方向余弦矩陣的kalman濾波較基于誤差四元數的EKF算法的解算精度均有所提高，提高的幅度不等。在初始誤差為5°，采樣頻率為1Hz的情況下，橫滾角的解算精度提高了近4倍。基于誤差四元數的EKF算法的線性化誤差受采樣頻率、初始值誤差的影響較大，而基于方向余弦矩陣的kalman濾波無需進行線性化處理。

2）由圖1可以看出，兩種方案均在20歷元附近得到收斂，20歷元后的解算結果基本相同，而20歷元內的解算結果方案2要優(yōu)于方案1，這也從一定程度上說明基于狀態(tài)矩陣kalman濾波的姿態(tài)估計算法對初始姿態(tài)誤差具有較好的魯棒性。

4 結束語

本文用方向余弦矩陣描述姿態(tài)，提出了基于狀態(tài)矩陣kalman濾波的姿態(tài)估計算法，以方向余弦矩陣表示的姿態(tài)系統(tǒng)狀態(tài)方程及觀測方程均為線性方程，避免了線性化誤差。仿真結果表明：新算法對初始姿態(tài)誤差更具有較好的魯棒性，較基于誤差四元數的EKF濾波算法的精度高，穩(wěn)定性強。

［1］王磊，梁開龍.四元數在GPS船姿測量中的應用［J］.測繪工程，2004，13（3）：32－34.

［2］周朝陽.基于陀螺和星敏感器的衛(wèi)星姿態(tài)確定研究［D］.哈爾濱：哈爾濱工業(yè)大學，2008：4－7.

［3］牟忠凱.衛(wèi)星姿態(tài)確定的非線性濾波方法研究［D］.鄭州：信息工程大學，2010.

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

［5］MARKLEY F L.Attitude Error Representations for Kalman Filtering［J］.Journal of Guidance，Control，and Dynamics，2003，26，（2）：311－317.

［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］VATHSAL S.Spacecraft attitude determination using a second－order nonlinear filter［J］.Journal of Guidance，Control，and Dynamics，1987，10（5）：559－566.

［8］牟忠凱，隋立芬，范澎湃，等.基于MRPs估計航天器姿態(tài)的IUKF算法及其改進［J］.測繪科學技術學報，2009，26（4）：250－253.

［9］OSHMAN Y，CARMI A.Attitude Estimation from Vector Observations Using Genetic Algorithm Embedded Quaternion Particle Filter［J］，Journal of Guidance，Control，and Dynamics，2006，29（4）：879－891.

［10］CRASSIDIS J L，MARKLEY F L.Predictive filter attitude estimation without rate sensors［J］.Journal of Guidance，Control and Dynamics，1997，20：522－527.

［11］章仁為.衛(wèi)星軌道姿態(tài)動力學與控制［M］.北京：北京航空航天大學出版社，1998：147－148.

［12］付夢印，鄧志紅，閆莉萍.Kalman濾波理論及其在導航系統(tǒng)中的應用［M］.北京：科學出版社，2010：13－17.

［13］許國昌.GPS理論、算法與應用［M］.李強，譯.北京：清華大學出版社，2011.

［14］張賢達.矩陣分析與應用［M］.北京：清華大學出版社，2004：638－642.

［15］ALAN EDELMAN，ARIAS T A，STEVEN T S.The Geometry of algorithms with orthogonality constraints［J］.SIAM Journal on Matrix Analysis and Applications，1998，20（2）：303－353.

［16］VERHAGEN S.Visualization of GNSS－related design parameters：Manual for the Matlab user interface VISUAL［S］.2006.