組合導(dǎo)航系統(tǒng)仿真平臺的設(shè)計與實現(xiàn)

李 寧，丁 薇，張勇剛

（哈爾濱工程大學 自動化學院，黑龍江 哈爾濱150001）

0 引 言

任何單一的導(dǎo)航系統(tǒng)難于滿足軍用運載體的導(dǎo)航需求，因此組合導(dǎo)航成為研究和仿真的主要方向［1，2］。傳統(tǒng)的關(guān)于組合導(dǎo)航系統(tǒng)的仿真大多是基于Matlab進行的某一組合模式下導(dǎo)航濾波算法的仿真性能驗證或比較，文獻［3］中提出了采用H∞濾波算法在模型輔助捷聯(lián)慣導(dǎo)單一的組合模型下通過Matlab軟件對其進行仿真。文獻［4］中提出了采用改進Kalman濾波算法對GPS／DR 組合模型在Matlab環(huán)境下進行仿真。然而在研究的組合模式或濾波方法發(fā)生變化時，仿真需要重新進行，大大降低了研究效率。

基于LabVIEW／Matlab實現(xiàn)的組合導(dǎo)航仿真平臺在保證算法高效運行的同時，為研究者提供了可視的配置界面。通過仿真平臺便捷的配置組合模式及濾波算法，能夠?qū)崿F(xiàn)某一組合模式下不同濾波方法之間的性能比較，比較結(jié)果以誤差曲線的形式在仿真平臺上顯示。并且，仿真平臺所包含的組合模式和組合濾波方法可隨著研究的進行而不斷擴展，增加了系統(tǒng)的擴展性和實用性。因此，本文所研究的組合導(dǎo)航仿真平臺對組合導(dǎo)航方法研究具有一定的意義。

1 組合導(dǎo)航系統(tǒng)模型與算法

在誤差傳播性能上慣性導(dǎo)航系統(tǒng)（inertial navigation system，INS）和全球衛(wèi)星導(dǎo)航系統(tǒng)（global positioning system，GPS）各有優(yōu)缺點，具有良好的互補性，INS／GPS組合導(dǎo)航系統(tǒng)將二者有機地結(jié)合起來，建立以慣導(dǎo)為基礎(chǔ)，GPS為輔助的組合導(dǎo)航系統(tǒng)［5，6］。綜合利用二者的特點，以實現(xiàn)優(yōu)勢的互補，提高系統(tǒng)導(dǎo)航精度、容錯性及可靠性。

1.1 INS／GPS組合導(dǎo)航系統(tǒng)模型

選取系統(tǒng)狀態(tài)向量為

則INS／GPS組合導(dǎo)航系統(tǒng)的狀態(tài)方程為

式中：F——組合導(dǎo)航系統(tǒng)矩陣，G——組合導(dǎo)航系統(tǒng)噪聲分布矩陣。

取加速度計隨機白噪聲ωg和陀螺漂移白噪聲ωa為系統(tǒng)白噪聲

在INS／GPS組合導(dǎo)航系統(tǒng)的量測方程分別選取兩個導(dǎo)航系統(tǒng)的位置信息差值和速度信息差值作為INS／GPS組合導(dǎo)航系統(tǒng)的量測量［7］，方程如下

其中，Zv＝ ［δveδvn］T，Zp＝ ［δφδλ］T為觀測向量，H為系統(tǒng)觀測矩陣，且滿足

在平臺失準角較小的情況下，INS／GPS組合導(dǎo)航系統(tǒng)模型可近似為線性模型，此時常用標準的Kalman濾波算法進行估計。

由于低成本慣性元件引起載體機動性增大，測量精度下降，慣性導(dǎo)航平臺失準角增大，此時組合導(dǎo)航系統(tǒng)模型近似為非線性模型，采用Kalman濾波對該組合模型進行估計濾波精度下降甚至會出現(xiàn)發(fā)散現(xiàn)象，采用非線性濾波方法可以保證濾波精度，抑制濾波發(fā)散。

對偶存在模態(tài)算子

1.2 組合導(dǎo)航濾波算法

由于組合導(dǎo)航模型有線性模型和非線性模型之分，組合導(dǎo)航濾波算法也分為線性濾波算法和非線性濾波算法，其中線性濾波算法僅適用于線性模型，而非線性濾波算法同時適用于線性模型和非線性模型。本文采用的線性濾波算法是卡爾曼濾波和H∞濾波，非線性濾波算法采用的是擴展卡爾曼濾波和無跡卡爾曼濾波。這里需要說明一點，H∞濾波算法也可用于非線性模型，但是在本文中將其用于線性模型，以便和卡爾曼濾波算法進行對比。

本文僅選取了一種線性組合模型和一種非線性組合模型，以及基于線性模型實現(xiàn)了卡爾曼濾波和H∞濾波，基于非線性模型實現(xiàn)了擴展卡爾曼濾波和無跡卡爾曼濾波。在實際研究中可根據(jù)研究情況對組合模型及濾波方法模塊進行擴展。

2 組合導(dǎo)航仿真平臺設(shè)計

2.1 組合導(dǎo)航仿真平臺工作原理

組合導(dǎo)航仿真平臺的目的是通過仿真的形式實現(xiàn)不同組合模式下及不同濾波算法下組合導(dǎo)航性能的橫向比較。

在進行組合導(dǎo)航之前，我們首先需要獲得導(dǎo)航子系統(tǒng)的導(dǎo)航數(shù)據(jù)，這是利用軌跡發(fā)生器來產(chǎn)生模擬的傳感器數(shù)據(jù)得到的，然后經(jīng)過組合濾波算法對系統(tǒng)進行輸出校正。如圖1所示，組合導(dǎo)航系統(tǒng)仿真包括：軌跡發(fā)生器、模擬慣性器件輸出、模擬GPS輸出及組合導(dǎo)航濾波算法，互相獨立但構(gòu)成一個完整的，缺一不可的整體。

圖1 組合導(dǎo)航仿真平臺工作原理框架

2.1.1 軌跡發(fā)生器

軌跡數(shù)據(jù)是測試驗證組合導(dǎo)航仿真平臺的必要輸入信息。設(shè)計仿真軌跡，從軌跡信息中解算載體的位置、姿態(tài)和速度等信息，設(shè)定運動參數(shù)，任意設(shè)置其初始值。為組合導(dǎo)航系統(tǒng)參數(shù)提供理論基準，對慣性導(dǎo)航系統(tǒng)輸出的仿真提供基礎(chǔ)。

組合導(dǎo)航系統(tǒng)仿真需要慣性導(dǎo)航的位置、速度數(shù)據(jù)，慣性導(dǎo)航系統(tǒng)位置、速度信息通過慣性測量元件陀螺儀和加速度計分別輸出的角速度和比力解算得到，輸出方程如下

經(jīng)過慣導(dǎo)解算可以得到慣性導(dǎo)航系統(tǒng)輸出的位置、速度和姿態(tài)信息。

2.1.3 模擬GPS輸出

根據(jù)無誤差的速度和位置信息，再參考GPS輸出參數(shù)的精度，在其基礎(chǔ)上加上相應(yīng)的測量誤差，于是就得到模擬GPS輸出的位置信息和速度信息［8］。

2.2 組合導(dǎo)航仿真平臺總體設(shè)計

按照組合導(dǎo)航系統(tǒng)的工作流程，首先須選定組合導(dǎo)航子系統(tǒng)。目前的組合導(dǎo)航方式多以慣性導(dǎo)航為主系統(tǒng)［9－11］，其它導(dǎo)航系統(tǒng)為輔，因此在仿真初始需要對導(dǎo)航系統(tǒng)的參數(shù)尤其是慣性系統(tǒng)的參數(shù)進行設(shè)置，同時需對載體運動的初始狀態(tài)參數(shù)進行設(shè)置。選定組合導(dǎo)航子系統(tǒng)的構(gòu)成后，可以進行組合模型設(shè)置，這里是指設(shè)置為速度組合、位置組合或者速度／位置組合。然后需要選定組合濾波的算法，即需要進行性能比較的算法。這樣在組合導(dǎo)航仿真平臺的輸出顯示部分將會實現(xiàn)在特定的組合模型下幾種組合濾波方法的橫向比較。因此，組合導(dǎo)航仿真平臺的總體設(shè)計如圖2所示。

圖2 組合導(dǎo)航仿真平臺的總體設(shè)計

根據(jù)組合導(dǎo)航仿真平臺的工作流程，對仿真平臺的整體結(jié)構(gòu)框架進行設(shè)計。將組合導(dǎo)航仿真平臺分成如下模塊：器件參數(shù)設(shè)置模塊、初始狀態(tài)設(shè)置模塊、組合方式及系統(tǒng)模型設(shè)置模塊、組合濾波算法設(shè)置模塊、平臺輸出顯示模塊，通過仿真平臺能夠?qū)崿F(xiàn)在某種組合方式下不同的濾波算法結(jié)果的清晰比較。

3 基于Matlab／LabVIEW 的組合導(dǎo)航系統(tǒng)仿真平臺實現(xiàn)

本文對組合導(dǎo)航仿真的整體框架進行分析，將組合模式及組合濾波方法進行模塊化處理，并基于Matlab／Lab－VIEW 實現(xiàn)可視化的仿真平臺。LabVIEW 是一種用圖標代替文本創(chuàng)建應(yīng)用程序的圖像化編程語言，與Matlab兼容性較好，不必通過調(diào)用封裝后的dll文件，而是可以通過Matlab Script直接調(diào)用Matlab 語言程序。LabVIEW 將分立元件模塊化，有利于降低程序開發(fā)的復(fù)雜程度，增強系統(tǒng)的功能和靈活性。

如圖3所示，是LabVIEW 的前面板。前面板主要是完成仿真平臺的器件參數(shù)及初始狀態(tài)設(shè)置和仿真結(jié)果呈現(xiàn)（誤差曲線）。采用Matlab實現(xiàn)模擬慣導(dǎo)數(shù)據(jù)產(chǎn)生、模擬慣導(dǎo)解算和組合濾波算法，初始狀態(tài)和器件參數(shù)通過仿真平臺界面輸入，最終仿真輸出誤差曲線通過平臺前面板呈現(xiàn)。

圖3 組合導(dǎo)航仿真平臺前面板

4 組合導(dǎo)航系統(tǒng)仿真平臺的結(jié)果驗證

以INS／GPS組合導(dǎo)航系統(tǒng)的線性模型利用卡爾曼濾波和H∞濾波進行狀態(tài)估計，以及大失準角下的INS／GPS組合導(dǎo)航系統(tǒng)非線性模型，利用EKF 和UKF 進行估計，驗證組合導(dǎo)航仿真平臺的各項基本功能。

4.1 線性模型下不同濾波算法的對比

設(shè)載體從北緯45.7796°東經(jīng)126.6705°，航向為45°，以10m／s的速度勻速運動，取陀螺常值漂移為0.01°／h，加速度計零偏為10－5g0，仿真時間為600s。

（1）在300s時施加一個外部擾動，組合導(dǎo)航系

統(tǒng)在Kalman濾波和H∞濾波后的誤差曲線如下。利用Kalman濾波和H∞濾波對組合導(dǎo)航系統(tǒng)進行狀態(tài)估計，Kalman濾波和H∞濾波后的位置誤差單位為度，速度誤差單位為m／s，仿真結(jié)果如圖4、圖5所示。

圖4 在較大擾動下的Kalman和H∞位置誤差曲線

圖5 在較大擾動下的Kalman和H∞速度誤差曲線

從圖4、圖5中可以看出，在0－300s內(nèi)Kalman的濾波效果比H∞要好，但是在300s以后，由于外界擾動的作用Kalman濾波開始發(fā)散，而H∞濾波的濾波效果幾乎沒有變化，這說明Kalman濾波的穩(wěn)態(tài)濾波精度較H∞高，但是魯棒性較H∞濾波差，而H∞濾波的穩(wěn)態(tài)精度較Kalman濾波低，但是魯棒性較好。

（2）較小失準角條件下EKF和UKF對比

仿真條件為：初始平臺誤差角α、β、γ分別為0.01°、0.01°、0.01°。位置誤差單位為度，速度誤差單位為m／s，仿真結(jié)果如圖6、圖7所示。

圖6 在較小失準角下EKF和UKF位置誤差曲線

圖7 在較小失準角下EKF和UKF速度誤差曲線

從圖6、圖7 中可以看出，在較小失準角下EKF 和UKF的濾波精度相差不大，但是UKF略好于EKF。

4.2 非線性模型下不同濾波算法的對比

設(shè)載體從北緯45.7796°東經(jīng)126.6705°，航向為45°，以10m／s的速度勻速運動，取陀螺常值漂移為0.01°／h，加速度計零偏為109－5g0，仿真時間為600s。仿真條件為：初始平臺誤差角α、β、γ分別為0.5°、2.5°、0.5°。其中位置誤差單位為度，速度誤差單位為m／s，仿真結(jié)果如圖8、圖9所示。

從圖8、圖9中可以看出，在較大失準角下UKF 的濾波效果明顯好于EKF。由于EKF濾波將組合導(dǎo)航系統(tǒng)的非線性模型進行一階泰勒近似（線性化），舍棄了高階項，而UKF沒有將模型線性化而是利用UT 變換，同時沒有忽略高階項，保證了狀態(tài)估計的精度。

圖8 在較大失準角下EKF和UKF位置誤差曲線

圖9 在較大失準角下EKF和UKF速度誤差曲線

通過以上仿真的形式對不同組合模型及不同濾波算法的組合導(dǎo)航性能進行了橫向比較，驗證了組合導(dǎo)航仿真平臺的功能。

5 結(jié)束語

基于組合導(dǎo)航系統(tǒng)的工作流程設(shè)計組合導(dǎo)航仿真平臺的功能模塊，并基于Matlab／LabVIEW 對組合導(dǎo)航仿真平臺各模塊進行具體實現(xiàn)，達到了仿真平臺可視化界面配置的功能要求。以INS／GPS組合模型為例，通過仿真平臺仿真對比Kalman和H∞濾波、EKF和UKF濾波方法在組合導(dǎo)航系統(tǒng)中性能，驗證組合導(dǎo)航仿真平臺的實用性。說明該仿真平臺能夠?qū)崿F(xiàn)不同濾波算法的導(dǎo)航性能之間的性能比較，達到了預(yù)期效果。在實際應(yīng)用中，根據(jù)研究的組合系統(tǒng)模型及組合濾波方法可對該仿真平臺進行擴展。

［1］Jacques Georgy，Aboelmagd Noureldin，Michael J Korenberg，et al.Modeling the stochastic drift of a mens－based gyroscope in gyro／odometer／GPS integrated navigation ［J］.IEEE Transactions on Intelligent Transportation Systems，2010，11 （4）：856－872.

［2］Sul Gee Park，Ho Cheol Jeong，Jeong Won Kim，et al.Magnetic compass fault detection method for GPS／INS／magnetic／compass integrated navigation system ［J］.International Journal of Control，Automation，and Systems，2011，9 （2）：276－284.

［3］LI Ning，DING Wei.Model－aided strapdown inertial navigation integrated method for AUV based on H∞filtering ［C］／／ShiYan：International Conference on Computational and Information Sciences，2013：1089－1093

［4］YOU Shengyu，JIANG Lin，LI Xiang.GPS／DR data fusion based on fading adaptive Kalman filter ［J］.Computer Engineering and Design，2010，31 （21）：4720－4723 （in Chinese）.［游勝玉，姜林，李祥.基于漸消記憶自適應(yīng)Kalman 濾波的GPS／DR 數(shù)據(jù)融合 ［J］.計算機工程與設(shè)計，2010，31 （21）：4720－4723.］

［5］WEI Wei，QIN Yongyuan，ZHANG Xiaodong，et al.Comparative analysis of h－infinity filtering in INS／GPS integrated navigation application ［J］.Journal of Xi’an Technological University，2012，32 （11）：874－885 （in Chinese）.［魏偉，秦永元，張曉冬，等.H∞濾波在慣導(dǎo)／GPS組合導(dǎo)航應(yīng)用的比較分析 ［J］.西安工業(yè)大學學報，2012，32 （11）：874－885.］

［6］Jae Hyung Kim，Joon Lyou，Hwy Kuen Kwak.Vision coupled GPS／INS scheme for helicopter navigation ［J］.Journal of Mechanical Science and Technology，2010，24 （2）：489－496.

［7］LI Yang，HU Baiqing.Research on integrated navigation algorithm based on improved fading－memory filter［J］.Computer Simulation，2012，29 （3）：117－119 （in Chinese）.［李楊，胡柏青.基于改進衰減記憶濾波的組合導(dǎo)航算法研究 ［J］.計算機仿真，2012，29 （3）：117－119.］

［8］HAN Lu，JING Zhanrong，DUAN Zhemin.Simulation of SINS／GPS integrated navigation system ［J］.Computer Simulation，2009，26 （9）：32－36 （in Chinese）. ［韓璐，景占榮，段哲民.SINS／GPS組合導(dǎo)航系統(tǒng)仿真研究 ［J］.計算機仿真，2009，26 （9）：32－36.］

［9］LI Peijuan，XU Xiaosu，ZHANG Tao.Application of information fusion to integrated navigation system of underwater vehicle［J］.Journal of Chinese Inertial Technology，2009，17（3）：344－349 （in Chinese）.［李佩娟，徐曉蘇，張濤.信息融合技術(shù)在水下組合導(dǎo)航系統(tǒng)中的應(yīng)用 ［J］.中國慣性技術(shù)學報，2009，17 （3）：344－349.］

［10］Abuhashim TS，Abdel－Hafes MF，Al－Jarrah MA.Building a robust integrity monitoring algorithm for a low cost GPS－aided－INS system ［J］.International Journal of Control，Automation，and Systems，2010，8 （5）：1108－1122.

［11］Kim S B，Bazin J C，Lee H K，et al.Ground vehicle navigation in harsh urban conditions by integrating inertial navigation system，global position system，odometer and vision data［J］.Radar，Sonar and Navigation，2011，5 （8）：814－823.