石 拓，陳家斌，田曉春，宋春雷,楊黎明，尹靜源

(1.中電54所，石家莊 050081；2.北京理工大學(xué)自動化學(xué)院，北京 100081;.華北光學(xué)儀器有限公司，北京 100053；4.北京機(jī)電工程總體設(shè)計(jì)部，北京 100039)

微慣性/衛(wèi)星組合導(dǎo)航系統(tǒng)算法研究

石 拓1,2，陳家斌2，田曉春2，宋春雷2,楊黎明3，尹靜源4

(1.中電54所，石家莊 050081；2.北京理工大學(xué)自動化學(xué)院，北京 100081;.華北光學(xué)儀器有限公司，北京 100053；4.北京機(jī)電工程總體設(shè)計(jì)部，北京 100039)

微慣導(dǎo)系統(tǒng)(MINS)與衛(wèi)星導(dǎo)航系統(tǒng)(GNSS)的結(jié)合不僅可以充分利用微慣導(dǎo)系統(tǒng)良好的短期性能和GNSS的長期穩(wěn)定性，彌補(bǔ)MINS系統(tǒng)誤差隨時(shí)間積累和GNSS采樣頻率低、動態(tài)性能差的缺陷，而且使得組合導(dǎo)航系統(tǒng)的精度和動態(tài)能力同步提高，保證了系統(tǒng)在衛(wèi)星信號受遮擋或失效的環(huán)境下仍能夠以較高的精度自主運(yùn)行一段時(shí)間，正常工作。采用星網(wǎng)宇達(dá)的產(chǎn)品采集真實(shí)數(shù)據(jù)進(jìn)行了離線仿真，比較了多種濾波算法，實(shí)驗(yàn)證明，采用UKF取得的效果最好，EKF次之，KF最差。

微慣導(dǎo)；組合導(dǎo)航；Kalman濾波

0 引言

隨著微機(jī)電系統(tǒng)(Micro-Electro-Mechanical System，MEMS)技術(shù)的日漸成熟，以及微型飛行器、微型機(jī)器人等微小型設(shè)備的廣泛應(yīng)用，低成本、小型化、低功耗的微慣性器件發(fā)展迅速[1-2]。微機(jī)電慣性導(dǎo)航系統(tǒng)(MEMS Inertial Navigation System,MINS)是指基于MEMS技術(shù)的慣性測量單元，是一種集成化的微型捷聯(lián)慣導(dǎo)系統(tǒng)，與之前常用的慣性器件不同，MINS在保證測量精度不明顯下降的前提下，體積和質(zhì)量更小、成本更低。正是由于MINS系統(tǒng)的這些優(yōu)點(diǎn)，使得MINS/全球?qū)Ш叫l(wèi)星系統(tǒng)(Global Navigation Satellite System，GNSS)組合導(dǎo)航成為近年來研究的熱點(diǎn)。

1 組合導(dǎo)航系統(tǒng)數(shù)學(xué)模型

對于MINS/GNSS組合導(dǎo)航系統(tǒng)，建立準(zhǔn)確的濾波數(shù)學(xué)模型是采用濾波技術(shù)進(jìn)行組合的基礎(chǔ)。根據(jù)姿態(tài)誤差(失準(zhǔn)角)估計(jì)值的大小不同，可以將組合導(dǎo)航系統(tǒng)濾波數(shù)學(xué)模型分為線性模型和非線性模型[3]。在車載環(huán)境下，可將姿態(tài)誤差角視為兩種情況：一種是將三個(gè)姿態(tài)誤差角都看成小角，建立線性模型；另一種是將俯仰角和橫滾角看成小角，而將航向角看成大失準(zhǔn)角，建立非線性模型。

設(shè)組合導(dǎo)航系統(tǒng)的狀態(tài)變量為

XI= [φEφNφUδvEδvNδvUδLδλ

取MINS和GNSS輸出的位置和速度之差作為組合導(dǎo)航系統(tǒng)的觀測量。

1.1 線性系統(tǒng)的數(shù)學(xué)模型

對于線性系統(tǒng)，設(shè)其狀態(tài)方程為

系統(tǒng)噪聲向量由陀螺儀和加速度計(jì)的隨機(jī)誤差組成，表達(dá)式為

其中，F(xiàn)N為對應(yīng)的9維基本導(dǎo)航參數(shù)矩陣，其非零項(xiàng)可由下述誤差方程確定。

由于姿態(tài)誤差角為小角度，即有[4]

則姿態(tài)誤差方程為:

式中，下角標(biāo)E、N、U分別為地理坐標(biāo)系東、北、天3個(gè)方向；RM為當(dāng)?shù)刈游缑鎯?nèi)主曲率半徑；RN為與子午面垂直平面上的主曲率半徑；L為地理緯度；λ為地理經(jīng)度；h為地理高度；ωie為地球自轉(zhuǎn)角速度。

速度誤差方程為:

式中，fE、fN、fU為加速度計(jì)輸出在導(dǎo)航坐標(biāo)系下的投影。

位置誤差方程為:

FS和FM的取值如下

1.2 非線性系統(tǒng)的數(shù)學(xué)模型

對于非線性系統(tǒng)設(shè)其狀態(tài)方程為

車載環(huán)境下，通常俯仰角和橫滾角為小角，航向角為大失準(zhǔn)角，即有

則姿態(tài)誤差方程為:

深化市縣機(jī)構(gòu)改革，夯實(shí)基層基礎(chǔ)。市縣機(jī)構(gòu)改革必須面向人民群眾、符合基層事務(wù)特點(diǎn)，在職能配置上更加突出民生，強(qiáng)化社會管理和公共服務(wù)職能，在機(jī)構(gòu)設(shè)置上不簡單照搬上級機(jī)關(guān)設(shè)置模式，既允許“一對多”，也允許“多對一”，確保上下貫通，執(zhí)行有力。全面落實(shí)鄉(xiāng)鎮(zhèn)、街道相關(guān)改革，統(tǒng)籌推進(jìn)第二批經(jīng)濟(jì)發(fā)達(dá)鎮(zhèn)行政管理體制改革，實(shí)行扁平化和網(wǎng)格化管理，使各類機(jī)構(gòu)、組織在服務(wù)保障群眾需求上有更大作為。

sinφUεE-cosφUεN

位置誤差與慣性器件誤差與線性系統(tǒng)相同，這里不再贅述。

取SINS和GNSS輸出的位置和速度之差作為量測量，其中，速度量測矢量為

式中

位置觀測矢量為

其中

上述各式中，vGE、vGN、vGU為GNSS沿東北天方向上的速度誤差；NGE、NGN、NGU為GNSS沿東北天方向上的位置誤差。

聯(lián)合速度和位置量測方程，組合導(dǎo)航的量測方程為

2 組合導(dǎo)航系統(tǒng)離線仿真

本車載實(shí)驗(yàn)是在北京理工大學(xué)中關(guān)村校區(qū)附近進(jìn)行的，跑車路線是圍繞北京理工大學(xué)中關(guān)村校區(qū)順時(shí)針1周，起點(diǎn)和終點(diǎn)都是北京理工大學(xué)北門。實(shí)際路線如圖1所示。

圖1 實(shí)驗(yàn)跑車路線Fig.1 Experiment path

本實(shí)驗(yàn)儀器選用北京星網(wǎng)宇達(dá)科技股份有限公司所研發(fā)的慣性/衛(wèi)星組合導(dǎo)航系統(tǒng)，根據(jù)該公司提供的軟件，可以分別設(shè)置慣導(dǎo)和衛(wèi)星的數(shù)據(jù)輸出頻率。本實(shí)驗(yàn)設(shè)定全球定位系統(tǒng)(GlobalPositioningSystem，GPS)的采樣頻率為0.5Hz，MINS的采樣頻率為50Hz。

實(shí)驗(yàn)將GPS給出的速度和位置與慣導(dǎo)解算出的速度和位置作差作為觀測量，用Kalman濾波(KF)估計(jì)出系統(tǒng)的誤差，再對慣導(dǎo)系統(tǒng)進(jìn)行補(bǔ)償。

圖2 KF組合姿態(tài)Fig.2 KF combined attitude

圖3 KF組合速度Fig.3 KF combined speed

圖4 KF組合位置Fig.4 KF combined position

圖2～圖9所示為運(yùn)用標(biāo)準(zhǔn)KF進(jìn)行組合導(dǎo)航得到的結(jié)果。由圖2可以看出，組合后的俯仰角和橫滾角變化在3°左右，明顯小于純慣導(dǎo)解算的結(jié)果。圖4所示為位置解算結(jié)果，紅色代表GPS跑車路線，藍(lán)色代表組合后的跑車路線，可以看出，組合后的位置比單獨(dú)GPS給出的位置平滑很多，如在圖中下部等紅燈階段，GPS數(shù)據(jù)產(chǎn)生了微小的跳變，而組合之后位置曲線很平滑，并且在GPS丟星的時(shí)間里，組合系統(tǒng)運(yùn)用純慣導(dǎo)數(shù)據(jù)進(jìn)行解算，雖然存在一定的誤差，但大致方向正確。圖5所示為濾波估計(jì)出的姿態(tài)誤差，可以看出俯仰角和橫滾角誤差最大在2°左右，但是航向角誤差較大，最大值在5°左右。圖6所示為濾波估計(jì)出的速度誤差，水平方向的最大值在5m/s左右，天向能達(dá)到20m/s。濾波后的位置誤差如圖7所示，在GPS信號良好的情況下，組合導(dǎo)航的位置誤差在5m以內(nèi)；在無法獲得衛(wèi)星信號的大約20s時(shí)間段內(nèi)，系統(tǒng)由微慣導(dǎo)導(dǎo)航，緯度誤差達(dá)到了15m，經(jīng)度誤差約為25m，高度誤差超過200m。另外組合導(dǎo)航系統(tǒng)還經(jīng)過KF估計(jì)出了陀螺的常值漂移為100(°)/h，加速度計(jì)零偏為10mg。

圖5 KF姿態(tài)角誤差Fig.5 KF attitude error

圖6 KF速度誤差Fig.6 KF speed error

圖8 陀螺常值漂移Fig.8 Gyro drift

圖9 加計(jì)零偏Fig.9 Accelerometer bias

論文基于大方位失準(zhǔn)角條件下的非線性誤差方程，分別設(shè)計(jì)了擴(kuò)展Kalman濾波(EKF)和無跡Kalman濾波(UKF)兩種非線性濾波方法，仿真結(jié)果如圖10～圖17所示。

圖10 EKF組合位置Fig.10 EKF combined position

圖11 EKF姿態(tài)誤差Fig.11 EKF attitude error

圖12 EKF速度誤差Fig.12 EKF speed error

圖13 EKF位置誤差Fig.13 EKF position error

圖14 UKF組合位置Fig.14 UKF combined position

圖15 UKF姿態(tài)誤差Fig.15 UKF attitude error

圖16 UKF速度誤差Fig.16 UKF speed error

圖17 UKF位置誤差Fig.17 UKF position error

圖10～圖13所示為采用EKF方法進(jìn)行組合導(dǎo)航的結(jié)果。圖11所示的姿態(tài)角誤差可以看出俯仰角和橫滾角誤差最大在1°左右，小于圖5所示的結(jié)果，航向角也相對較小；水平方向的速度誤差最大值在3m/s左右，天向也減小到15m/s。

圖14～圖17所示為采用UKF方法進(jìn)行組合導(dǎo)航的結(jié)果。雖然姿態(tài)誤差沒有明顯的提升，但速度誤差和位置誤差都有較好的結(jié)果。從圖17所示的位置誤差可以看出，UKF濾波后的緯度誤差在20m左右，經(jīng)度誤差在10m以內(nèi)，高度誤差縮小到了100m以內(nèi)，使得丟星后的定位誤差有較大的提升。

3 結(jié)論

由以上分析可以看出，丟星之前采用KF、EKF和UKF方法進(jìn)行組合導(dǎo)航都可以較好地完成導(dǎo)航任務(wù)，丟星后UKF定位精度最高，EKF次之，KF效果最差。

[1] 閆鑫. 基于抗差估計(jì)的GPS/MIMU組合導(dǎo)航濾波算法研究[D].哈爾濱: 哈爾濱工程大學(xué),2010.

[2] 黃麗斌.MINS/GPS組合導(dǎo)航系統(tǒng)算法研究[D].南京:東南大學(xué), 2004.

[3] 全偉, 劉百奇, 宮曉琳, 等. 慣性/天文/衛(wèi)星組合導(dǎo)航技術(shù)[M].北京: 國防工業(yè)出版社, 2011.

[4] 秦永元. 慣性導(dǎo)航(第二版)[M].北京：科學(xué)出版社, 2014.

[5] 付夢印,鄧志紅, 閆麗萍.Kalman濾波理論及其在導(dǎo)航系統(tǒng)中的應(yīng)用(第二版)[M].北京:科學(xué)出版社,2010.

[6] 嚴(yán)恭敏.捷聯(lián)慣導(dǎo)算法及車載組合導(dǎo)航系統(tǒng)研究[D].西安: 西北工業(yè)大學(xué), 2004.

[7] 謝鋼.GPS原理與接收機(jī)設(shè)計(jì)[M].北京: 電子工業(yè)出版社, 2009.

[8] 王君帥, 王新龍.SINS/GPS緊組合與松組合導(dǎo)航系統(tǒng)性能仿真分析[J], 航空兵器, 2013(2): 14-19.

[9]GodhaS,CannonME.GPS/MEMSINSintegratedsystemfornavigationinurbanareas[J].GPSSolutions, 2007, 11(3):193-203.

[10]SolovievA.TightcouplingofGPS,INS,andlaserforurbannavigation[J].IEEETransactionsonAerospaceandElectronicSystems, 2010, 46(4): 1731-1746.

[11] 應(yīng)士君, 李金金, 劉衛(wèi), 等.BD-2/GPS組合導(dǎo)航系統(tǒng)選星算法及定位分析[J].上海海事大學(xué)學(xué)報(bào), 2013, 34(2): 1-4.

[12]BrennerM.IntegratedGPS/inertialfaultdetectionavailability[C]//ProceedingsofIONGPS,PalmSprings,CA, 1995.

[13] 劉華, 劉彤, 張繼偉. 陸地車輛GNSS/MEMS慣性組合導(dǎo)航機(jī)體系約束算法研究[J].北京理工大學(xué)學(xué)報(bào), 2013, 33(5): 500-515.

[14] 張光理. 北斗/SINS組合導(dǎo)航系統(tǒng)研究[D].哈爾濱工程大學(xué), 2012.

Algorithm Research on MINS/GNSS Integrated Navigation System

SHI Tuo1,2, CHEN Jia-bin2, TIAN Xiao-chun2,SONG Chun-lei2, YANG Li-ming3, YIN Jing-yuan4

(1.CETC 54, Shijiazhuang 050081, China;2.School of Automation, Beijing Institute of Technology, Beijing 100081, China;3.North China Optical Instrument Co., Ltd.,Beijing 100053, China;4. Beijing Electro-mechanical Engineering System Design Department, Beijing 100039, China)

Micro inertial navigation system(MINS) with the combination of the Global navigation satellite system (GNSS) can make full use of the short-term performance of MINS and the long-term stability of the GNSS.It can make up for the defects of error accumulated over time of MINS, and for the low sampling frequency and poor dynamic performance of GNSS. Thus, it can improve the precision and dynamic capabilities of integrated navigation system, and it can also ensure the system to operate a period of time autonomously at high precision when the satellite signal is shielded or failure. We use StarNeto products to collect real data and compare a variety of filtering algorithms. Experimental results show that the effect achieved by UKF is best, EKF followed, KF worst.

MINS; Intergrated navigation; Kalman filter

10.19306/j.cnki.2095-8110.2016.06.006

2015-09-19；

2015-10-29。

石拓(1989 - )，男，碩士，主要從事衛(wèi)星與慣性組合導(dǎo)航技術(shù)方面的研究。E-mail: shituo5858@126.com

U666.11

A

2095-8110(2016)06-0026-07