基于擴展卡爾曼濾波的動中通低成本姿態(tài)估計

閆林波，賈維敏，姚敏立，沈曉衛(wèi)

(第二炮兵工程大學，西安 710025)

0 引言

動中通(satcom on-the-move，SOTM)作為一種寬帶移動衛(wèi)星通信系統(tǒng)，具有運動中、寬帶及超視距通信的優(yōu)點，因此，在軍、民領域都有著重要的應用意義。動中通是指利用FSS頻段的衛(wèi)星資源，能夠在運動狀態(tài)下實時建立并保持衛(wèi)星和天線之間通信鏈路暢通［1］。為了實現(xiàn)運動中載體與衛(wèi)星之間的通信鏈路暢通，天線波束需做到實時對準衛(wèi)星，也就是說，需要精確的姿態(tài)控制算法對載體的姿態(tài)進行實時估計。

對于動中通來說，為保證控制精度，目前大多數(shù)動中通廠商都采用了穩(wěn)定可靠的高成本姿態(tài)航向參考系統(tǒng)(Attitude and Heading Reference System，AHRS)［2］作為測控方案。為了降低成本、重量及小型化，基于低成本微機械傳感器(Micro Electro Mechanical System，MEMS)姿態(tài)估計融合算法得到了廣泛的研究。但是，MEMS存在噪聲、零偏及安裝誤差，所以姿態(tài)估計精度較差。為了改善姿態(tài)估計的精度和可靠性，兩種典型的傳感器融合算法得到了廣泛的應用，一類是互補濾波算法(Complementary Filter，CF)，它是利用陀螺的高頻特性和加速度計的低頻特性進行優(yōu)勢互補，從而補償陀螺的零偏和低動態(tài)下加速度計的誤差。文獻［3］在基于多軸互補濾波算法的基礎上，融合微機械陀螺、加速度計和磁強針信息對載體的姿態(tài)角進行估計，同時采用自適應算法自動調整交接頻率來校正載體的機動加速度。另一類是卡爾曼濾波算法(KF)，其實質是利用反饋控制的方法估計過程狀態(tài)，即濾波器估計過程某一時刻的狀態(tài)，然后以(含噪聲)測量變量的方式獲得反饋。文獻［4］利用擴展卡爾曼濾波算法(EKF)融合陀螺和加速度計信息進行姿態(tài)估計，同時利用自適應算法去除機動加速度對姿態(tài)估計帶來的影響。但是，上述方法由于陀螺零偏和加速度計測量噪聲的存在，同時磁阻傳感器容易受車上元件及外部物體的電磁干擾，使得姿態(tài)角的估計精度較差。文獻［5-6］在基于四元數(shù)更新算法的基礎上，分別利用CF和EKF算法融合陀螺和單天線GPS信息對陀螺零偏和姿態(tài)進行估計。但單天線GPS應用到動中通中存在兩個問題:一是在載體的運動速度較低時，單天線GPS給出的速度信息誤差比較大［5］，很難達到SOTM指向精度要求;二是當載體轉彎時，估計的偽姿態(tài)角誤差較大。

鑒于此，本文通過EKF算法融合單基線GPS、微機械陀螺和加速度計信息以獲得實時的姿態(tài)估計。同時，為了解決載體機動加速度帶來的影響，首先通過加速度補償法對加速度計進行初步補償;當載體發(fā)生轉彎時，利用側滑角補償法進一步補償。最后，采用實測數(shù)據(jù)進行了仿真驗證。

1 載體姿態(tài)確定

1.1 陀螺姿態(tài)確定

在捷聯(lián)系統(tǒng)中，慣性測量單元(Inertial Measurement Unit，IMU)放在載體上時，通過陀螺可以測量出載體坐標系相對于地理坐標系下的三軸角速率。定義地理坐標系(t系)，x軸指向當?shù)氐恼龞|方向，y軸指向當?shù)氐恼狈较?，z軸垂直于x軸、y軸且構成右手直角坐標系;載體坐標系(b系)，x軸指向載體的右方，y軸指向載體的前方，z軸指向載體的上方。按照ψ→θ→φ的旋轉順序，可以得到載體系相對于地理系3個歐拉角，即航向角ψ，俯仰角θ和橫滾角φ。用歐拉角方法得到其微分方程形式為［7］

式中，ωx，ωy，ωz是陀螺輸出的三軸角速率。由于陀螺零偏隨著時間的推移不斷積累，所以僅僅依靠陀螺只適用于短時的姿態(tài)角估計。

1.2 加速度計姿態(tài)確定

在捷聯(lián)系統(tǒng)中，加速度計測量的是載體坐標系加速度和重力加速度的迭加。假定載體不受重力以外的加速度作用，則三軸加速度計的輸出即為重力矢量g在載體坐標系的分量，重力矢量g在地理坐標系的坐標為(0，0，-g)T。則俯仰角和橫滾角可由如下等式得到［8］

式中，fx，fy，fz為加速度計輸出的三軸加速度。當載體受到機動加速度影響時，姿態(tài)角估計精度較差，這時就需要對機動加速度進行去除，以改善估計效果。

1.3 單基線GPS姿態(tài)確定

GPS可以給出精確的位置和速度信息，所以它在姿態(tài)估計中有著非常重要的作用。單基線GPS不僅能夠提供準確的航向角，還可以得到速度信息。利用GPS的速度信息可以導出速率航向ψV［6］為

式中，ve，vn分別是地理坐標系下的東向速度和北向速度。

當載體直線運動時，速率航向與載體航向一致;當載體發(fā)生轉彎時，載體航向和速率航向之間就會產生一個夾角，即側滑角。文獻［9］給出了一種計算側滑角的方法，利用單基線GPS提供載體航向角ψ和速度信息導出速率航向角ψV，進而得到側滑角β為

2 機動加速度補償

從上面的分析可以看出，載體的機動加速度對加速度計的估計精度影響很大，同時側滑角的存在影響載體的機動加速度，所以，側滑角的估計和機動加速度的去除對于載體的姿態(tài)估計顯得十分必要。

2.1 加速度補償

加速度補償是指對因載體機動而產生的非重力加速度進行去除。利用GPS測得的速度信息可以很容易對其進行補償。由載體的三向速度導出載體的速度V為

式中:vt是地理坐標系下的天向速度;則線性加速度為ay=d V/d t;徑向加速度為ax=ωz×V。

線性加速度和徑向加速度是載體機動時影響加速度計估計精度的主要干擾。結合式(2)得到加速度補償后姿態(tài)角為［10］

2.2 側滑角補償

側滑角是指載體進行轉彎運動時，因載體航向角和速率航向角不同而產生的夾角［9］。載體轉彎產生側滑角如圖1所示。

圖1 載體轉彎產生側滑角示意圖Fig.1 Sideslip angle resulted from vehicle turning

圖1中:彎曲的曲線為載體的運行軌跡;O為轉彎的中心點;aI為載體轉彎時產生的向心加速度，垂直于速率航向。

載體進行直線運動時，速率航向和載體航向基本一致;在載體發(fā)生轉彎時，從圖中可以看出，速度方向與載體方向出現(xiàn)夾角，此時速率航向與載體航向之間形成側滑角β，進而影響加速度補償效果，所以，在加速度計姿態(tài)確定過程中，需要進一步校正。由圖可以得出載體坐標系x軸和y軸的機動加速度ax、ay為

式中，V·β是側滑角引起的向心加速度。利用式(7)進一步對機動加速度進行補償，可以更好地提高加速度計的姿態(tài)估計精度。

3 EKF融合算法

載體的姿態(tài)估計是一個典型的非線性問題，為了解決這一問題，必須對卡爾曼濾波進行改進。常用的非線性濾波算法有擴展卡爾曼濾波器和無跡卡爾曼濾波器(UKF)兩種。但在實際工程應用中，由于低成本MEMS傳感器各種不確定因素的影響和運算量的原因，使UKF很難滿足動中通中對姿態(tài)角估計的實時性要求。所以本文選取EKF算法融合低成本MEMS傳感器和單基線GPS信息進行姿態(tài)估計。

3.1 四元數(shù)姿態(tài)解算

利用陀螺信息可以得到三軸角速率信息，進而利用式(1)可以得到載體的姿態(tài)角。雖然利用歐拉角解算方法理解分析簡單，但其運算復雜且抽象，還會出現(xiàn)奇點。而四元數(shù)運算線性化且不會出現(xiàn)奇點，所以本文建立在四元數(shù)更新算法的基礎上進行姿態(tài)估計。

四元數(shù)定義為

四元數(shù)隨時間更新等式為

為了便于理解，通常將解算出的四元數(shù)轉化為歐拉角，四元數(shù)與歐拉角的關系為

3.2 融合算法

由于陀螺零偏隨著時間的推移不斷積累，所以必須進行實時校正。狀態(tài)變量定義為

式中，bx、by、bz是三軸陀螺零偏。結合式(9)可得非線性狀態(tài)方程為

式中:b=［bxbybz］T，是三軸陀螺零偏;η是過程噪聲向量，得到狀態(tài)噪聲協(xié)方差Q。

觀測變量定義為y=［ψ θ φ］T，則非線性觀測方程為

式中:ψ、θ、φ為加速度計和單基線GPS輸出的姿態(tài)角構成的觀測向量;n為觀測噪聲向量，得到觀測噪聲協(xié)方差R;nψ、nθ、nφ為加速度計和單基線GPS導出的歐拉角所得觀測噪聲。

EKF算法流程如圖2所示。

圖2 姿態(tài)估計算法流程圖Fig.2 Flow chart of the proposed sensor fusion algorithm

姿態(tài)估計算法的整體流程:在單基線GPS可以提供航向信息時，以GPS測得的航向角作為航向角觀測變量，同時利用速度信息對加速度計產生的非重力加速度進行初步補償;在載體進行轉彎運動時，速率航向角與載體航向角偏離較大，由式(7)可知，需要利用側滑角補償法進行進一步校正。根據(jù)式(6)，利用校正后的加速度信息導出俯仰和橫滾角作為俯仰和橫滾角觀測變量，進而通過EKF算法融合陀螺的角速率信息對載體的姿態(tài)角和陀螺零偏進行實時估計，最后將陀螺零偏經平滑后用于實時校正陀螺。

4 實驗結果和討論

測量組件采用ADU5220微機械IMU模塊，其中IMU采樣頻率為50 Hz;同時采集光纖航姿系統(tǒng)(輸出結果曲線表示為Reference，陀螺漂移0.5(°)/h，姿態(tài)精度±0.2°，航向精度±1°)的姿態(tài)信息作為參考值。通過與參考航姿結果進行比較，驗證本文算法的有效性。

為了驗證EKF姿態(tài)估計算法，進行了綜合行車實驗。跑車的機動狀態(tài)包括:起步加速，4次轉彎，直線加、減速和停止減速。

實驗中，采用北京星網宇達科技公司研制的AHRS(XW-ADU7612)作為高精度慣性測量基準，它以100 Hz的數(shù)據(jù)更新速率給出載體的姿態(tài)角(方位、俯仰和橫滾角)，其方位角精度為0.1°，俯仰角和橫滾角的精度靜態(tài)時達0.05°，動態(tài)時可達0.1°。在車體的前端和后端分別安裝GPS(XW-ADU3601)天線，基線長度為1.615 m，兩天線的連線與載體坐標系的y軸平行，以10 Hz的速率更新數(shù)據(jù)。IMU采用北京星網宇達科技公司研制的慣性測量單元XW-IMU5220，它含有3個低成本微機械陀螺和3個加速度計，分別與載體坐標系的三軸平行，可實現(xiàn)對姿態(tài)運動、線運動的實時測量，以100 Hz的數(shù)據(jù)更新速率為載體提供穩(wěn)定的加速度和角速率值。圖3分別給出了AHRS、陀螺、加速度計和GPS姿態(tài)角估計圖。

圖3 陀螺、加速度計和GPS與AHRS輸出角Fig.3 The attitude estimated by different methods

從圖中可以看出，陀螺姿態(tài)角估計具有短時精度高的特點，由于陀螺零偏隨著時間的推移不斷積累，所以估計的姿態(tài)角誤差不斷增大;在常規(guī)路面上行駛時，俯仰角和橫滾角變化很小，根據(jù)式(1)可知歐拉角之間的耦合對航向角影響較小，但陀螺估計的俯仰角和橫滾角受載體轉彎影響較大且航向角誤差不斷增大。加速度計姿態(tài)估計具有長時穩(wěn)定性，但由于振動噪聲和載體機動加速度的影響，估計的俯仰角在載體加、減速時誤差較大，橫滾角在載體轉彎時誤差較大。所以，對陀螺的短時精確性和加速度計的長時穩(wěn)定性進行融合就顯得非常必要。

圖4給出了AHRS、EKF和經加速度補償(Acceleration Compensation，AC)后的EKF算法姿態(tài)估計對比圖。從圖中可以看出，EKF濾波算法可以很好地濾除不同傳感器的測量噪聲，同時經過加速度補償后可以得到較好的載體姿態(tài)角估計。由于航向角不受加速度的影響，所以估計精度保持較好。從圖中可以看出，在初始和最后十幾秒中，俯仰角受載體加、減速引起的線加速度的影響變化較大，經加速度補償后，估計的姿態(tài)角與參考值趨于一致。但是，通過進一步觀察可以發(fā)現(xiàn)，當載體轉彎時，經過補償后的姿態(tài)角估計誤差超過±0.5°，不能滿足動中通的應用要求。這時就需要利用側滑角補償法進行進一步補償。

圖4 EKF、EKF-AC輸出角與AHRS輸出角Fig.4 The attitude estimated by EKF with and without acceleration compensation

圖5給出了通過單基線GPS導出的偏流角估計圖。從圖中可以看出當載體轉彎時，偏流角較大。由于經加速度補償和側滑角補償(Sideslip Compensation，SC)的EKF算法姿態(tài)角估計值比較接近，所以采用兩者的誤差圖進行如比較，如圖6所示。從圖6可以看出，經過側滑角補償法校正后的EKF融合算法估計的姿態(tài)角精度達到了±0.3°以內。所以，利用側滑角補償法對提高載體姿態(tài)角的估計精度有著非常重要的意義。

EKF、EKF-AC與EKF-SC算法所得姿態(tài)角誤差均值和方差如表1所示。從表1中可以看出，利用側滑角補償法進行EKF算法融合可以得到最好的姿態(tài)角估計。

圖5 補償角Fig.5 Compensation angle

圖6 EKF-AC誤差與EKF-SC誤差Fig.6 The sideslip angle effect on attitude estimation with acceleration compensation

表1 EKF、EKF-AC與EKF-SC誤差角結果Table 1 Attitude error statistics from the test

5 結論

本文采用EKF算法融合微機械陀螺、加速度計和單基線GPS信息構成動中通低成本姿態(tài)估計系統(tǒng)。該算法成功融合了陀螺的短時精確性、加速度計的長時穩(wěn)定性和GPS精確的測速和定位功能，同時通過GPS速度信息對加速度計的非重力加速度分量進行補償，有效地抑制了機動加速度對姿態(tài)估計的影響。實驗結果表明，在單基線GPS能夠提供航向角信息時，姿態(tài)角估計誤差控制在±0.5°以內，滿足了動中通的應用要求。

［1］李耐和.“動中通”衛(wèi)星通信技術［J］.現(xiàn)代軍事，2007(7):60-63.

［2］JAMESD.Control system for mobile satcom antennas［J］.IEEE Control Systems Magazine，2008，28(1):86-101.

［3］沈曉衛(wèi)，姚敏立，趙鵬.基于互補濾波的動中通天線姿態(tài)估計［J］.中國慣性技術學報，2011，19(2):194-197.

［4］KANG C W，PARK C G.Attitude estimation with accelerometers and gyros using fuzzy tuned Kalman filter［C］//Proceedings of the European Control Conference，Budapest，Hungary，August 23-26，2009:3713-3718.

［5］LAI Y C，JAN SS.Attitude estimation based on fusion of gyroscopes and single antenna GPSfor small UAVs under the influence of vibration［J］.GPS Solut，2011(15):67-77.

［6］TENN H K，JAN SS，HSIAO F B.Pitch and roll attitude estimation of a small-scaled helicopter using single antenna GPSwith gyroscopes［J］.GPSSolutions，2009，13(3):209-220.

［7］朱豐超，姚敏立，賈維敏.基于低成本陀螺和傾角儀的姿態(tài)估計［J］.宇航學報，2011，32(8):1728-1733.

［8］GEBRE-EGZIABHER D，HAYWARD R C，POWELL JD.Design of multi-sensor attitude determination systems［J］.IEEE Trans Aerosp Electron Syst，2004，40(2):627-649.

［9］RYU J，GERDESJC.Integrating inertial sensors with GPS for vehicle dynamics control［J］.Journal of Dynamics，Systems，Measurement，and Control，2004，126(2):243-254.

［10］劉啟超.動中通低成本航向姿態(tài)估計算法研究［D］.西安:第二炮兵工程大學，2011.