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

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

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

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

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

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

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

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

定義衛(wèi)星本體系相對(duì)慣性系的方向余弦矩陣為R，由其表達(dá)的姿態(tài)運(yùn)動(dòng)學(xué)方程為［11］

其中：ω為載體坐標(biāo)系相對(duì)慣性系的角速度在載體坐標(biāo)系的坐標(biāo)，

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

為了推導(dǎo)方便，假設(shè)陀螺測(cè)量系與本體坐標(biāo)系重合，陀螺測(cè)量模型建立如下：

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

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

其中

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

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

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

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

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

1.2 GPS觀測(cè)方程

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

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

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

基線i的觀測(cè)方程可以表示為

其中，系數(shù)陣A，S分別表示如下：

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

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

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

根據(jù)文獻(xiàn)［13］，多基線載波相位觀測(cè)量方差－協(xié)方差陣表示為

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

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

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

1）對(duì)狀態(tài)參數(shù)及其協(xié)方差陣進(jìn)行初始化

2）計(jì)算預(yù)測(cè)狀態(tài)向量

3）計(jì)算預(yù)測(cè)狀態(tài)協(xié)方差陣

4）計(jì)算信息向量及其協(xié)方差陣

5）計(jì)算增益矩陣

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

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

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

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

3 仿真實(shí)驗(yàn)

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

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

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

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

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

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

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

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

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

4 結(jié)束語

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

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

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

［3］牟忠凱.衛(wèi)星姿態(tài)確定的非線性濾波方法研究［D］.鄭州：信息工程大學(xué)，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估計(jì)航天器姿態(tài)的IUKF算法及其改進(jìn)［J］.測(cè)繪科學(xué)技術(shù)學(xué)報(bào)，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)動(dòng)力學(xué)與控制［M］.北京：北京航空航天大學(xué)出版社，1998：147－148.

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

［13］許國昌.GPS理論、算法與應(yīng)用［M］.李強(qiáng)，譯.北京：清華大學(xué)出版社，2011.

［14］張賢達(dá).矩陣分析與應(yīng)用［M］.北京：清華大學(xué)出版社，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.