一種結(jié)合H∞濾波的SINS/GPS組合無人機定位算法

王昆侖，陶庭葉，黃祚繼，王春林

(1. 合肥工業(yè)大學(xué)土木與水利工程學(xué)院，安徽 合肥 230009； 2. 安徽省(水利部淮河水利委員會)水利科學(xué)研究院，安徽 合肥 230088)

近年來，低成本、微小型無人機定位技術(shù)在軍用和民用領(lǐng)域得到廣泛使用，其中導(dǎo)航定位技術(shù)是無人機應(yīng)用的關(guān)鍵[1-2]。慣性導(dǎo)航作為一種主要的導(dǎo)航方式，具有自主性強、隱蔽性高、短期輸出的導(dǎo)航參數(shù)(位置、速度、姿態(tài))精度較高等優(yōu)點，缺點為長期精度較低，并且誤差隨時間而累積。GPS是一種高精度的導(dǎo)航方式，它具有定位精度高，同時輸出速度信號和姿態(tài)信號等優(yōu)點，但存在更新率較低、信號易受干擾等不足，無法滿足實時定位的要求[3]。將慣性導(dǎo)航和GPS組合起來，用GPS信息對慣導(dǎo)信息進行修正，取長補短，既提升了單獨使用慣性導(dǎo)航的長期定位精度，又降低使用高精度慣性元件的成本，從而提高定位的精度及系統(tǒng)的穩(wěn)定性。

本文設(shè)計一種運用于SINS/GPS組合式定位系統(tǒng)的H∞濾波技術(shù)，將H∞范數(shù)引入濾波問題，使得干擾信號輸入到濾波誤差輸出的H∞范數(shù)最小[4]。該方法相較于Kalman濾波器在處理系統(tǒng)過程噪聲中的不確定性具有很好的穩(wěn)健性，可以保證組合導(dǎo)航定位的精度、提高系統(tǒng)的可靠性。

1 組合式導(dǎo)航定位設(shè)計

根據(jù)H∞濾波原理，慣性元件的各類誤差作為系統(tǒng)的不確定性誤差，需要得到組合系統(tǒng)的系統(tǒng)噪聲、量測噪聲和初始誤差估計的先驗信息[5-6]。設(shè)計中首先建立組合式導(dǎo)航系統(tǒng)的狀態(tài)方程，其次在系統(tǒng)誤差方程的基礎(chǔ)上建立量測方程，然后進行定位誤差修正，從而減小誤差，進一步提高定位精度的可靠性。

SINS/GPS組合導(dǎo)航方式可以根據(jù)組合深度的不同分為緊密組合和松散組合兩種[7]。松散組合是一種在實際應(yīng)用中較為常見的組合，組合中GPS和慣導(dǎo)各自發(fā)揮自己的作用，互不干擾，用GPS信息輔助慣導(dǎo)，校正慣導(dǎo)累積誤差[8-9]。本文根據(jù)實際情況選擇了工程上易于實現(xiàn)，組合效果較為明顯的位置、速度組合的松散組合方式。將SINS和GPS所有的量測量作為濾波器的量測量，這樣一方面可以實現(xiàn)長時間內(nèi)精度較高的GPS信息對SINS進行校正，另一方面防止一旦慣導(dǎo)失效，GPS信息可以實現(xiàn)補充。組合導(dǎo)航原理設(shè)計如圖1所示。

圖1 組合系統(tǒng)輸出校正原理

2 組合導(dǎo)航模型方程的建立

2.1 SINS/GPS組合導(dǎo)航系統(tǒng)的狀態(tài)方程

導(dǎo)航中誤差量都表現(xiàn)為非線性，但具有一定精度的導(dǎo)航系統(tǒng)的誤差量均可看做小量[10-11]，非線性方程中子誤差量的高階項都可看做高階小量而忽略不計，因此誤差方程可以描述為線性的[12-15]。組合導(dǎo)航系統(tǒng)的狀態(tài)僅取系統(tǒng)導(dǎo)航參數(shù)誤差，此時系統(tǒng)的階次為9階。導(dǎo)航坐標(biāo)系選取為E-N-U坐標(biāo)系，其誤差方程為

(1)

其中

(2)

W=[ωgxωgyωgzωaxωayωaz]T

(3)

2.2 SINS/GPS組合式導(dǎo)航量測方程

SINS/GPS組合方式有很多種，本文使用的是H∞濾波進行位置、速度的組合，將GPS輸出的位置和速度信息與SINS輸出的相應(yīng)信息相減得到量測方程為

(4)

(5)

式中，λS、LS為SINS輸出的經(jīng)度和緯度信息；λG、LG為GPS輸出的經(jīng)度和緯度信息；VS為SINS輸出的速度信息；VG為GPS輸出的速度信息；δλ為經(jīng)度中誤差；δL為緯度中誤差；H1為量測矩陣；VGPS為GPS的量測白噪聲。

2.3 離散型H∞濾波算法

將系統(tǒng)方程(1)及量測方程(5)進行離散化處理，可得

(6)

使用H∞濾波算法對式(6)進行處理，表達式為

(7)

式中，k=0,1,2,…,其中θ需滿足

(8)

3 SINS/GPS組合導(dǎo)航系統(tǒng)H∞濾波仿真試驗

3.1 飛行軌跡的設(shè)計

仿真需要對飛行軌跡進行設(shè)計，首先建立Simulink仿真環(huán)境。使用Aerosim工具箱中的飛行器模塊編寫軌跡發(fā)生器，生成飛機軌跡參數(shù)，再以該參數(shù)作為基礎(chǔ)，通過仿真子系統(tǒng)仿真產(chǎn)生SINS系統(tǒng)所需要的比力、角速率慣導(dǎo)數(shù)據(jù)，獲取飛機的飛行航跡信息[16-18]。

本文載體飛行狀態(tài)包括快速拉升、8字形、S形、轉(zhuǎn)彎、巡航等過程。飛行時間設(shè)置為300 s，初始位置為[31.82°，117.17°]。根據(jù)SINS算法中加速度、速度、位置、姿態(tài)角的變化規(guī)律及耦合關(guān)系所設(shè)計的飛行軌跡仿真如圖2所示。

圖2 無人機軌跡仿真結(jié)果

3.2 仿真試驗

針對文中SINS/GPS組合導(dǎo)航系統(tǒng)性能進行研究，設(shè)計了仿真環(huán)境，參數(shù)見表1。

表1 仿真參數(shù)設(shè)置

3.3 試驗分析

根據(jù)設(shè)計，進行多次無人機試驗，驗證得出統(tǒng)計結(jié)果見表2。

表2 H∞濾波SINS/GPS組合導(dǎo)航系統(tǒng)導(dǎo)航誤差

(1) 根據(jù)圖3和圖4可以看出在300 s時間內(nèi)，純SINS各項誤差隨時間的累積不斷增大，可得出純SINS各項參數(shù)隨時間的累積而快速發(fā)散，因此無法滿足高精度導(dǎo)航要求。

(2) 根據(jù)圖5和圖6仿真結(jié)果可以看出，當(dāng)GPS的位置和速度信息被引入以后，再經(jīng)過H∞濾波進行修正，相比于純SINS慣性導(dǎo)航，隨時間的增加，其位置和速度誤差得到了明顯的收斂。表明H∞濾波可以有效地克服純SINS隨時間發(fā)散的現(xiàn)象。

圖3 速度誤差曲線

圖4 位置誤差曲線

圖5 速度誤差曲線

圖6 位置誤差曲線

(3) 根據(jù)表2試驗統(tǒng)計結(jié)果可以得出，水平速度誤差均值達到0.01 m/s，位置誤差均值優(yōu)于1 m。表明采用H∞濾波設(shè)計組合導(dǎo)航定位方式能夠明顯提高動態(tài)飛行時的精度。

4 結(jié) 論

本文分析了SINS和GPS各自的優(yōu)缺點，研究了SINS/GPS組合式慣性導(dǎo)航系統(tǒng)，建立了無人機高精度目標(biāo)定位的仿真平臺，通過引入GPS信息修正了系統(tǒng)的速度和位置信息，并使用H∞濾波算法對設(shè)計進行仿真測試，降低了系統(tǒng)累積誤差，提高了定位的精度和可靠性，能夠滿足一般低成本無人機定位精度要求。經(jīng)過仿真模擬驗證了該方法的有效性，是一種無人機項目中可以實現(xiàn)的設(shè)計方法。

參考文獻：

[1] 秦永元.慣性導(dǎo)航[M].北京：北京科技出版社,2006：99-107.

[2] 朱家海.慣性導(dǎo)航[M].北京：國防工業(yè)出版社,2008：35-38.

[3] 牛歡,廉保旺.一種基于改進 UKF 濾波的 GPS+PDR 組合定位方法[J].測繪通報，2017(7):5-9.

[4] 嚴(yán)恭敏,翁浚.捷聯(lián)慣導(dǎo)算法與組合導(dǎo)航原理[M].西安：西北工業(yè)大學(xué)出版社,2016:6-7.

[5] 王崢,周劍.基于Kalman濾波的大規(guī)模GNSS網(wǎng)參數(shù)估計方法[J].測繪通報,2017 (1):18-21.

[6] 胡高歌,劉逸涵,高社生.改進的強跟蹤UKF算法及其在 INS/GPS組合導(dǎo)航中的應(yīng)用[J].中國慣性技術(shù)學(xué)報， 2014，10(5)： 12-17.

[7] 楊元喜,陸明泉,韓春好.GNSS 互操作若干問題[J]．測繪學(xué)報,2016，45(3): 253-259.

[8] 程傳奇,郝向陽,張振杰.魯棒性地形匹配/慣性組合導(dǎo)航算法[J].中國慣性技術(shù)學(xué)報,2016，24(2):14-16.

[9] XIONG Z，LENG X F，LIU J Y.Research of SAR/INS Integrated Navigation System Aided by BeiDou Double Star Position[J].Journal of Astronautics，2007 28(1):88-93.

[10] BESER J,ALEXANDER S,CRANA R,et al.A Low-cost Guidance/Navigation Unit Integrating A SAASM Based GPS and in a Deeply Coupled Mechanization[C]∥Proceedings of ION GNSS 2004.Fairfax VA:Institute of Navigation,2002.

[11] LI W J.Effective Adaptive Kalman Filter for MEMS-IMU/Mangetometers Integrated Attitude and Heading Reference Systems[J].Journal of Navigation，2012，1(1):1-15.

[12] 錢華明,雷艷敏,謝常鎖.組合導(dǎo)航系統(tǒng)幾種濾波方法的分析比較及其性能評價[J].中國慣性技術(shù)學(xué)報,2006，14(6):95-99.

[13] XUE M，LING H B.Robust Visual Tracking and Vehicle Classification via Sparse Representation[J].IEEE Trans-actions on Pattern Analysis and Machine Inteligence,2011，33(11):2259-2272.

[14] WANG D,LU H C,YANG M S.Online Object Tracking with Sparse Prototypes[J].IEEE Transactions on Image Processing，2013 22(9):314-326.

[15] BABU R,WANG J.Ultra-Tight GPS/INS/PL Integr-ation:a System Concept and Performance Analysis [J].GPS Solutions，2009，13(1):75-82.

[16] 陳敏,安艷輝，李曉華.捷聯(lián)慣導(dǎo)系統(tǒng)仿真器的設(shè)計與實現(xiàn)[J].現(xiàn)代測繪,2012，35(1): 10-11.

[17] 王新龍,李亞峰.SINS/GPS 組合導(dǎo)航技術(shù)[M].北京：北京航空航天大學(xué)出版社,2014:2-3.

[18] 秦永元,張洪鉞,汪叔華.卡爾曼濾波與組合導(dǎo)航原理[M].西安：西北工業(yè)大學(xué)出版社,2015：288-292.