陳 灃,楊鵬翔,劉夢(mèng)焱,劉 琴,梅春波,郭昊昌

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

0 引言

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

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

1 組合導(dǎo)航方案設(shè)計(jì)

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

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

(1)

Zk=HkXk+Vk

(2)

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

(3)

時(shí)間更新迭代方程如下：

狀態(tài)一步預(yù)測(cè)[6]：

(4)

一步預(yù)測(cè)均方誤差[6]：

(5)

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

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

ZP,k=LSINS-LSAT

(6)

ZV,k=VSINS-VSAT

(7)

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

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

(8)

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

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

(9)

均方誤差[6]：

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

(10)

1.2 航向量測(cè)分析

在無(wú)水平機(jī)動(dòng)的情況下,航向角觀測(cè)性較差,故組合導(dǎo)航在平直運(yùn)動(dòng)時(shí)利用衛(wèi)星導(dǎo)航相對(duì)地面的速度航向糾正航向角。

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

航向量測(cè)量[6]：

Zyaw,k=βk=Ψtr-Ψ

(11)

航向量測(cè)的濾波增益為：

(12)

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

均方誤差[6]：

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

(13)

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

(14)

圖1 速度增加量失準(zhǔn)角示意圖

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

由于存在以上誤差,組合導(dǎo)航系統(tǒng)需要盡量減少航向失準(zhǔn)角,同時(shí)減少航向失準(zhǔn)角對(duì)于速度、位置量測(cè)的影響。

1.3 速度航向H∞濾波

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

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

(15)

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

1.4 側(cè)滑角估計(jì)

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

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

βm=Ψtr,m-Ψm

(16)

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

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

Zyaw,k=Ψtr,k-βm

(17)

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

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

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

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

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

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

(18)

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

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

(19)

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

(20)

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

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

導(dǎo)航系比力為：

(21)

進(jìn)而得到載體系下比力fb。fb由仿真機(jī)注入到組合導(dǎo)航系統(tǒng)中,以模擬組合導(dǎo)航裝置的線(xiàn)運(yùn)動(dòng)。

質(zhì)心運(yùn)動(dòng)的動(dòng)力學(xué)方程為[10]：

(22)

其中,x、y、z為彈道坐標(biāo)系下位置。

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

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

3 組合導(dǎo)航系統(tǒng)的半實(shí)物仿真試驗(yàn)

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

飛行軌跡如圖3所示。

圖3 飛行軌跡

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

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

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

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

4 結(jié)論

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