陳 灃,楊鵬翔,劉夢焱,劉 琴,梅春波,郭昊昌

(西安現(xiàn)代控制技術(shù)研究所, 西安 710065)

0 引言

隨著小型無人機體積、成本的下降,基于MEMS慣性器件的捷聯(lián)慣性導航系統(tǒng)(SINS)、衛(wèi)星導航系統(tǒng)(GNSS)進行的組合導航得到了廣泛應(yīng)用。但是在無人機平直飛行過程中,SINS/GNSS航向可觀測性較差[1],低精度MEMS慣導無法保證長時間航向測量精度。文獻[2]提出了一種磁強計組合方法,但是適用于小型無人機體積的磁強計大多需要標定,且容易受其他電子部件干擾,很難達到規(guī)定精度。文獻[3]討論了一種通過動力學模型以及慣性導航輸出量估計攻角及側(cè)滑角的方法,估計精度較高,但是需要精確的模型以及氣動力的輸入,在缺乏大氣測量器件的情況下不可行。文獻[4]、文獻[5]提出了用GNSS位置、速度推算航向角的方法,在無風理想條件下取得了良好的效果,但是在有側(cè)風條件,由于側(cè)滑角的影響,航向角估計精度變差,而且會影響俯仰角精度。

在速度修正航向的基礎(chǔ)上,文中以MEMS SINS/GNSS組合導航系統(tǒng)[5]為平臺,設(shè)計了一種側(cè)滑角在線估計與補償方法。利用半實物仿真進行驗證,結(jié)果表明,在側(cè)風條件下,算法可有效提高組合導航系統(tǒng)的姿態(tài)角精度;無風條件下,姿態(tài)角精度亦不損失。

1 組合導航方案設(shè)計

1.1 序貫卡爾曼濾波方程建立

線性條件下,系統(tǒng)的狀態(tài)方程和量測方程的簡化形式為[6]：

(1)

Zk=HkXk+Vk

(2)

式中:Xk、Zk、Hk、Vk、Wk-1分別為tk時刻系統(tǒng)狀態(tài)、量測值、量測距陣、量測噪聲序列、系統(tǒng)噪聲序列;Φk/k-1為tk-1時刻至tk時刻的一步狀態(tài)轉(zhuǎn)移矩陣;Γk-1為tk-1時刻系統(tǒng)噪聲驅(qū)動矩陣。

(3)

時間更新迭代方程如下：

狀態(tài)一步預測[6]：

(4)

一步預測均方誤差[6]：

(5)

式中,Qk-1為系統(tǒng)噪聲協(xié)方差陣[7]。

量測更新采用序貫方式處理。量測更新方程為：

ZP,k=LSINS-LSAT

(6)

ZV,k=VSINS-VSAT

(7)

式中：ZP,k、ZV,k分別為tk時刻位置、速度量測值;LSINS、VSINS分別為慣性導航解算得到的位置、速度;LSAT、VSAT分別為衛(wèi)星導航輸出的位置、速度。

位置、速度量測的濾波增益均可表示為[6]：

(8)

其中,R為量測噪聲協(xié)方差陣。

狀態(tài)估計[6]：

(9)

均方誤差[6]：

Pk/k=(I-KkHk)Pk/k-1

(10)

1.2 航向量測分析

在無水平機動的情況下,航向角觀測性較差,故組合導航在平直運動時利用衛(wèi)星導航相對地面的速度航向糾正航向角。

俯仰、滾轉(zhuǎn)角較小時,近似認為速度矢量在水平面投影方向Ψtr與航向角Ψ誤差為側(cè)滑角β。

航向量測量[6]：

Zyaw,k=βk=Ψtr-Ψ

(11)

航向量測的濾波增益為：

(12)

式中:航向量測陣Hyaw=[0 1 001×12];Ryaw為量測噪聲協(xié)方差陣,可認為航向噪聲幅度為2°～6°。

均方誤差[6]：

Pk/k=(I-Kyaw,kHyaw,k)Pk/k-1

(13)

狀態(tài)估計[6]：

(14)

圖1 速度增加量失準角示意圖

在速度量測更新過程中水平速度誤差會傳遞到俯仰、滾轉(zhuǎn)誤差角中,造成錯誤的姿態(tài)角修正,位置量測更新中也會出現(xiàn)類似現(xiàn)象。

由于存在以上誤差,組合導航系統(tǒng)需要盡量減少航向失準角,同時減少航向失準角對于速度、位置量測的影響。

1.3 速度航向H∞濾波

由于速度量測對于姿態(tài)角影響較為顯著,采用魯棒性更好的H∞濾波進行速度量測,位置量測仍然采用標準卡爾曼濾波量測更新。

在隨機線性離散系統(tǒng)中,H∞濾波算法遞推公式可以表示為[8]：

(15)

其中,Lk為線性矩陣,這里取為單位陣。為調(diào)節(jié)魯棒性和精度的因子,當γ→∞時H∞濾波就退化為Kalman 濾波[9]。γ越大系統(tǒng)精度越高,但魯棒性降低,這里取為30。

1.4 側(cè)滑角估計

由于轉(zhuǎn)彎過程中航向角誤差可觀測性增加,初始航向誤差會逐漸減小。所以載體轉(zhuǎn)彎過程中,停止航向量測更新。轉(zhuǎn)彎結(jié)束,滾轉(zhuǎn)角及俯仰角均小于10°后,可以認為航向誤差收斂到最小,且載體縱向?qū)ΨQ平面近似垂直于地面。根據(jù)載體航跡特性,等待一定時間,并進行平滑濾波。

平滑后的側(cè)滑角為：

βm=Ψtr,m-Ψm

(16)

其中,Ψtr,m為轉(zhuǎn)彎結(jié)束后tm時刻速度矢量在水平面投影,Ψm為轉(zhuǎn)彎結(jié)束后tm時刻航向角。

則航向量測量變?yōu)椋?/p>

Zyaw,k=Ψtr,k-βm

(17)

其中,Ψtr,k為下次轉(zhuǎn)彎開始前的tk時刻速度矢量在水平面投影方向。速度、位置量測更新后進行類似式(8)～式(10)的航向量測更新,以修正平直飛行過程中天向陀螺造成的航向漂移。每次轉(zhuǎn)彎結(jié)束后,更新一次βm。

2 半實物仿真環(huán)境設(shè)計

為了驗證組合導航算法在物理平臺中的性能,搭建了半實物仿真環(huán)境進行驗證。仿真系統(tǒng)框圖如圖2所示。仿真機控制轉(zhuǎn)臺運行,根據(jù)載體數(shù)學模型生成載體的加速度以及比力,注入到組合導航系統(tǒng)中。組合導航系統(tǒng)安裝在三軸轉(zhuǎn)臺上,敏感轉(zhuǎn)臺轉(zhuǎn)動角速率,同時接收衛(wèi)星模擬器發(fā)射的衛(wèi)星信號以及仿真機注入的比力數(shù)據(jù),進行組合導航解算。組合導航系統(tǒng)輸出的姿態(tài)角、速度、位置信息又送回到仿真機參與控制模型解算與彈道動力學方程解算。

圖2 半實物仿真系統(tǒng)框圖

控制模型接收導航輸入的姿態(tài)角、速度、位置,進行爬升控制、航線控制、高度控制等中制導控制策略,輸出舵偏角δ。

仿真機由舵偏角δ可以得出滾動、偏航、俯仰力矩Mx、My、Mz,并由質(zhì)心轉(zhuǎn)動方程解出載體系下轉(zhuǎn)動角速率[10]：

(18)

其中,Jx、Jy、Jz為載體系下轉(zhuǎn)動慣量,ωf為仿真機計算的載體系下角速率。

仿真機可以由式(19)解出姿態(tài)角[10]：

(19)

位置、速度的計算,仿真機轉(zhuǎn)入彈道坐標系處理。載體的質(zhì)心動力學方程為[10]:

(20)

其中,Fd為彈道坐標系下載體受到的外力,可以由推力、載體氣動參數(shù)等得出。V、θv、Ψv分別為仿真機計算載體的速度、彈道傾角、彈道偏角。

Fd轉(zhuǎn)到載體坐標系下為Fb。在載體質(zhì)量m已知的情況下,可計算出載體系下加速度ab,轉(zhuǎn)換到導航系下為an。

導航系比力為：

(21)

進而得到載體系下比力fb。fb由仿真機注入到組合導航系統(tǒng)中,以模擬組合導航裝置的線運動。

質(zhì)心運動的動力學方程為[10]：

(22)

其中,x、y、z為彈道坐標系下位置。

彈道坐標系下的速度、位置輸入到衛(wèi)星信號模擬器中,模擬器可以模擬載體的衛(wèi)星導航信號,發(fā)送給組合導航系統(tǒng)。

組合導航輸出的姿態(tài)、速度、位置,與仿真機輸出姿態(tài)、速度、位置轉(zhuǎn)到同一坐標系下比較,即可得出組合導航誤差。

3 組合導航系統(tǒng)的半實物仿真試驗

仿真系統(tǒng)選用的低精度MEMS SINS,陀螺零位偏差約為300°/h,刻度系數(shù)重復性小于5%。衛(wèi)星接收機定位誤差小于10 m,速度誤差小于0.5 m/s。安裝誤差角小于0.3°。

飛行軌跡如圖3所示。

圖3 飛行軌跡

圖4 姿態(tài)角誤差曲線

風力條件側(cè)向10 m/s,方向由西向東時,側(cè)滑角補償前后誤差曲線如圖4所示。分別比較不同側(cè)風條件下,兩種方法姿態(tài)角誤差,如表1所示。姿態(tài)角誤差為組合導航系統(tǒng)輸出姿態(tài)角與轉(zhuǎn)臺姿態(tài)角作差得到。位置、速度誤差為組合導航輸出與仿真機輸出求差得到,具體數(shù)值見表1。

表1 誤差統(tǒng)計表格(RMS)

如表1所示,側(cè)滑角補償方法在3種條件下誤差分布較為平均：在無風條件下比未補償方法誤差偏大;但是在6 m/s側(cè)風及10 m/s側(cè)風條件下,航向角誤差有顯著改善,且俯仰角誤差改善也較為明顯。同時,速度、位置誤差沒有顯著增加。

4 結(jié)論

應(yīng)用抗側(cè)風側(cè)滑角估計方法后,增加了航向角解算中天向陀螺的權(quán)重,增加了速度量測的魯棒性,降低了速度方向?qū)较蚪切拚淖饔?。所以相對于全程用速度方向修正航向角的方?側(cè)滑角估計方法在無風條件下誤差稍大,但是在有側(cè)風條件下,由于航向失準角較小,航向角、俯仰角誤差顯著降低。小型無人機飛行速度慢,在飛行過程中較容易受到側(cè)風影響,所以在實際飛行中側(cè)滑角估計方法的姿態(tài)角誤差更小。