基于半實物仿真的組合導航系統抗側風設計*

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

(西安現代控制技術研究所, 西安 710065)

0 引言

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

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

1 組合導航方案設計

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

線性條件下,系統的狀態方程和量測方程的簡化形式為[6]：

(1)

Zk=HkXk+Vk

(2)

式中:Xk、Zk、Hk、Vk、Wk-1分別為tk時刻系統狀態、量測值、量測距陣、量測噪聲序列、系統噪聲序列;Φk/k-1為tk-1時刻至tk時刻的一步狀態轉移矩陣;Γk-1為tk-1時刻系統噪聲驅動矩陣。

(3)

時間更新迭代方程如下：

狀態一步預測[6]：

(4)

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

(5)

式中,Qk-1為系統噪聲協方差陣[7]。

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

ZP,k=LSINS-LSAT

(6)

ZV,k=VSINS-VSAT

(7)

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

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

(8)

其中,R為量測噪聲協方差陣。

狀態估計[6]：

(9)

均方誤差[6]：

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

(10)

1.2 航向量測分析

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

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

航向量測量[6]：

Zyaw,k=βk=Ψtr-Ψ

(11)

航向量測的濾波增益為：

(12)

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

均方誤差[6]：

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

(13)

狀態估計[6]：

(14)

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

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

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

1.3 速度航向H∞濾波

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

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

(15)

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

1.4 側滑角估計

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

平滑后的側滑角為：

βm=Ψtr,m-Ψm

(16)

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

則航向量測量變為：

Zyaw,k=Ψtr,k-βm

(17)

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

2 半實物仿真環境設計

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

圖2 半實物仿真系統框圖

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

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

(18)

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

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

(19)

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

(20)

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

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

導航系比力為：

(21)

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

質心運動的動力學方程為[10]：

(22)

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

彈道坐標系下的速度、位置輸入到衛星信號模擬器中,模擬器可以模擬載體的衛星導航信號,發送給組合導航系統。

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

3 組合導航系統的半實物仿真試驗

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

飛行軌跡如圖3所示。

圖3 飛行軌跡

圖4 姿態角誤差曲線

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

表1 誤差統計表格(RMS)

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

4 結論

應用抗側風側滑角估計方法后,增加了航向角解算中天向陀螺的權重,增加了速度量測的魯棒性,降低了速度方向對航向角修正的作用。所以相對于全程用速度方向修正航向角的方法,側滑角估計方法在無風條件下誤差稍大,但是在有側風條件下,由于航向失準角較小,航向角、俯仰角誤差顯著降低。小型無人機飛行速度慢,在飛行過程中較容易受到側風影響,所以在實際飛行中側滑角估計方法的姿態角誤差更小。