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

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

0 引言

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

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

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

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

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

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

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

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

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

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

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

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

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

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

2 機(jī)動加速度補(bǔ)償

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

2.1 加速度補(bǔ)償

加速度補(bǔ)償是指對因載體機(jī)動而產(chǎn)生的非重力加速度進(jìn)行去除。利用GPS測得的速度信息可以很容易對其進(jìn)行補(bǔ)償。由載體的三向速度導(dǎo)出載體的速度V為

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

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

2.2 側(cè)滑角補(bǔ)償

側(cè)滑角是指載體進(jìn)行轉(zhuǎn)彎運(yùn)動時，因載體航向角和速率航向角不同而產(chǎn)生的夾角［9］。載體轉(zhuǎn)彎產(chǎn)生側(cè)滑角如圖1所示。

圖1 載體轉(zhuǎn)彎產(chǎn)生側(cè)滑角示意圖Fig.1 Sideslip angle resulted from vehicle turning

圖1中:彎曲的曲線為載體的運(yùn)行軌跡;O為轉(zhuǎn)彎的中心點(diǎn);aI為載體轉(zhuǎn)彎時產(chǎn)生的向心加速度，垂直于速率航向。

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

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

3 EKF融合算法

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

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

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

四元數(shù)定義為

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

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

3.2 融合算法

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

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

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

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

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

EKF算法流程如圖2所示。

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

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

4 實(shí)驗(yàn)結(jié)果和討論

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

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

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

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

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

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

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

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

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

圖5 補(bǔ)償角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誤差角結(jié)果Table 1 Attitude error statistics from the test

5 結(jié)論

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

［1］李耐和.“動中通”衛(wèi)星通信技術(shù)［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)，姚敏立，趙鵬.基于互補(bǔ)濾波的動中通天線姿態(tài)估計(jì)［J］.中國慣性技術(shù)學(xué)報，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ì)［J］.宇航學(xué)報，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)估計(jì)算法研究［D］.西安:第二炮兵工程大學(xué)，2011.