零速修正在GNSS/INS緊組合導(dǎo)航中的應(yīng)用研究

?！贰≌录t平　高周正,2　丁昱心　張　全

1　武漢大學(xué)衛(wèi)星導(dǎo)航定位技術(shù)研究中心，武漢市珞喻路129號(hào)，430079 2　武漢大學(xué)測(cè)繪學(xué)院，武漢市珞喻路129號(hào)，430079 >3　北京航空航天大學(xué)電子信息工程學(xué)院，北京市學(xué)院路37號(hào)，100191

?

零速修正在GNSS/INS緊組合導(dǎo)航中的應(yīng)用研究

常樂1章紅平1高周正1,2丁昱心3張全1

1武漢大學(xué)衛(wèi)星導(dǎo)航定位技術(shù)研究中心，武漢市珞喻路129號(hào)，430079 2武漢大學(xué)測(cè)繪學(xué)院，武漢市珞喻路129號(hào)，430079 >3北京航空航天大學(xué)電子信息工程學(xué)院，北京市學(xué)院路37號(hào)，100191

為研究零速修正在GNSS/INS緊組合導(dǎo)航中的應(yīng)用，提出綜合利用GNSS/INS緊組合解算速度、IMU原始數(shù)據(jù)和多普勒觀測(cè)數(shù)據(jù)的零速判斷準(zhǔn)則以及零速狀態(tài)下的緊組合數(shù)學(xué)模型，并通過車載測(cè)試進(jìn)行驗(yàn)證。結(jié)果表明，該零速判斷準(zhǔn)則對(duì)載體靜止?fàn)顟B(tài)的判斷與實(shí)際情況吻合良好，零速修正能夠有效抑制載體靜止?fàn)顟B(tài)下的IMU傳感器誤差積累，尤其是在衛(wèi)星觀測(cè)質(zhì)量不好時(shí)。相對(duì)于不使用零速修正，在較短靜止時(shí)間內(nèi)，采用零速修正算法可以使衛(wèi)星信號(hào)正常時(shí)的載體位置、速度精度分別改善27%、35%；衛(wèi)星信號(hào)中斷30～120 s時(shí)，載體位置和速度精度分別改善16%～75%和87%～97%；但衛(wèi)星信號(hào)正常與中斷時(shí)，姿態(tài)改善效果都不明顯。

GNSS/INS緊組合；零速判斷；零速修正；卡爾曼濾波

車載慣性導(dǎo)航中，零速修正(zero velocity update,ZUPT)可有效提高長(zhǎng)時(shí)間導(dǎo)航的精度。其利用載體靜止時(shí)的速度為零作為觀測(cè)量對(duì)其他各項(xiàng)誤差進(jìn)行修正[1-3]，還可用載體運(yùn)動(dòng)時(shí)垂向和橫向速度為零作為約束條件進(jìn)行動(dòng)態(tài)修正[3-5]。零速修正的關(guān)鍵在于正確判斷載體零速狀態(tài)。目前，大部分學(xué)者利用IMU(inertial measurement unit)觀測(cè)數(shù)據(jù)來判斷載體的運(yùn)動(dòng)狀態(tài)[3, 6-7]。但這種判斷方式需要的閾值通常由IMU設(shè)備的性能以及工作環(huán)境來確定，準(zhǔn)確率不太理想。本文提出綜合利用GNSS/INS緊組合解算的速率、經(jīng)誤差補(bǔ)償?shù)腎MU觀測(cè)數(shù)據(jù)以及多普勒數(shù)據(jù)對(duì)載體的運(yùn)動(dòng)狀態(tài)進(jìn)行判斷。實(shí)驗(yàn)結(jié)果表明，本文方法不僅提高了零速狀態(tài)判斷的準(zhǔn)確率，同時(shí)明顯改善了GNSS/INS緊組合導(dǎo)航結(jié)果的精度。

1　GNSS/INS緊組合數(shù)學(xué)模型

IMU采樣率一般大于GNSS接收機(jī)，因此在GNSS觀測(cè)數(shù)據(jù)不可用時(shí)，利用INS力學(xué)編排來更新狀態(tài)參數(shù)中的位置、速度和姿態(tài)信息[8]；若GNSS觀測(cè)可用，則利用GNSS觀測(cè)值與INS推算觀測(cè)量以閉環(huán)修正模式進(jìn)行GNSS/INS緊組合觀測(cè)更新計(jì)算。

GNSS/INS緊組合系統(tǒng)中，狀態(tài)參數(shù)x由INS誤差狀態(tài)和GNSS誤差狀態(tài)共同組成：

(1)

式中，δrn、δvn、ψ分別表示位置誤差、速度誤差、姿態(tài)角誤差，bg、sg分別表示陀螺零偏和比例因子，ba、sa分別表示加速度計(jì)零偏和比例因子，δVtR表示接收機(jī)鐘差誤差，δfR表示接收機(jī)鐘漂誤差。

GNSS/INS緊組合中，通常采用卡爾曼濾波進(jìn)行參數(shù)的時(shí)間更新和觀測(cè)更新[1, 8]。狀態(tài)參數(shù)x的時(shí)間更新可表示為[8]：

(2)

(3)

狀態(tài)參數(shù)觀測(cè)更新可表示為[8]：

(4)

(5)

式中，Kk為卡爾曼濾波增益矩陣；zk為新息向量，由INS推算的GNSS偽距、多普勒值與GNSS接收機(jī)觀測(cè)的偽距、多普勒觀測(cè)值作差得到；Hk為設(shè)計(jì)矩陣；Rk為觀測(cè)信息先驗(yàn)方差矩陣，本文按高度角確定觀測(cè)值先驗(yàn)方差[9]。

2　零速修正

零速修正可有效提高靜止?fàn)顟B(tài)下GNSS/INS組合導(dǎo)航結(jié)果的精度，其中零速狀態(tài)判斷最為關(guān)鍵。本文在前人提出的基于IMU數(shù)據(jù)判斷[3, 6-7]的基礎(chǔ)上，進(jìn)一步提出綜合利用GNSS/INS緊組合解算速度、IMU原始數(shù)據(jù)和多普勒數(shù)據(jù)的零速判斷準(zhǔn)則：在衛(wèi)星觀測(cè)信號(hào)良好的情況下，優(yōu)先使用多普勒觀測(cè)信息判斷載體運(yùn)動(dòng)狀態(tài)；對(duì)于已經(jīng)判斷為零速的時(shí)刻，統(tǒng)計(jì)IMU觀測(cè)數(shù)據(jù)的標(biāo)準(zhǔn)差，為使用IMU觀測(cè)數(shù)據(jù)判斷零速狀態(tài)提供閾值。

2.1零速狀態(tài)判斷

1)GNSS/INS計(jì)算速度判定。使用GNSS/INS組合解算的NED方向速度計(jì)算得到的載體速率進(jìn)行判斷，設(shè)置閾值為偽距、偽距率緊組合的速度誤差，小于閾值認(rèn)為靜止，否則認(rèn)為動(dòng)態(tài)。

2)IMU數(shù)據(jù)判定。當(dāng)載體處于零速狀態(tài)時(shí)，IMU的觀測(cè)數(shù)據(jù)趨于穩(wěn)定，其標(biāo)準(zhǔn)差接近0。利用一個(gè)GNSS采樣間隔內(nèi)經(jīng)過誤差補(bǔ)償之后的線加速度信息和角加速度信息的標(biāo)準(zhǔn)差，當(dāng)加速度計(jì)、陀螺儀3軸觀測(cè)數(shù)據(jù)的標(biāo)準(zhǔn)差都小于對(duì)應(yīng)坐標(biāo)軸設(shè)定的閾值時(shí)認(rèn)為靜止，否則認(rèn)為動(dòng)態(tài)。由于慣導(dǎo)設(shè)備本身性能的多樣性以及工作環(huán)境的復(fù)雜性，閾值一般不為定值，可通過統(tǒng)計(jì)當(dāng)前GNSS采樣與前2個(gè)GNSS采樣都判斷為靜止時(shí)刻的誤差補(bǔ)償之后的IMU觀測(cè)值標(biāo)準(zhǔn)差的RMS得到，閾值設(shè)為該RMS的2倍。

3)多普勒數(shù)據(jù)判定。衛(wèi)星的多普勒觀測(cè)數(shù)據(jù)具有比較高的精度，在衛(wèi)星觀測(cè)信號(hào)良好的情況下，可以比較準(zhǔn)確地判斷載體的運(yùn)動(dòng)狀態(tài)。

當(dāng)衛(wèi)星數(shù)目大于某一數(shù)值時(shí)，對(duì)于每個(gè)衛(wèi)星按式(6)計(jì)算載體徑向速率vel：

(6)

若載體處于靜止?fàn)顟B(tài)，各個(gè)衛(wèi)星計(jì)算的速率接近0，統(tǒng)計(jì)其RMS。當(dāng)RMS小于閾值時(shí)，判定為靜止，否則為動(dòng)態(tài)。衛(wèi)星的徑向速度精度為0.1cm/s[10]，多普勒測(cè)速的精度為0.1m/s[11-12]，衛(wèi)星的徑向速度誤差相比于多普勒觀測(cè)值誤差可以忽略不計(jì)，故將統(tǒng)計(jì)速率RMS的閾值設(shè)為0.1m/s。當(dāng)衛(wèi)星數(shù)目不足時(shí)，認(rèn)為多普勒數(shù)據(jù)不可靠，不使用其進(jìn)行零速判斷。

圖1為本文提出的靜止?fàn)顟B(tài)判定方法。

圖1　零速判斷流程Fig.1　Flow chart of determining the zero velocity

2.2零速修正數(shù)學(xué)模型

理論上，載體靜止時(shí)導(dǎo)航計(jì)算出來的速度應(yīng)該為零，但因觀測(cè)噪聲等誤差的影響，載體的位置和速度會(huì)出現(xiàn)漂移。因此當(dāng)判斷為零速狀態(tài)時(shí)，在有衛(wèi)星信號(hào)的情況下，在偽距、偽距率觀測(cè)方程基礎(chǔ)上增加零速約束方程(7)，形成零速狀態(tài)觀測(cè)方程；在沒有衛(wèi)星信號(hào)的情況下，直接使用零速約束方程，形成零速狀態(tài)觀測(cè)方程。零速約束方程精度極高，通過給定較小的先驗(yàn)方差，實(shí)現(xiàn)利用零速附加方程卡爾曼濾波參數(shù)解算進(jìn)行強(qiáng)約束，達(dá)到對(duì)靜止?fàn)顟B(tài)誤差累計(jì)進(jìn)行修正的目的。

(7)

3　實(shí)例分析

為驗(yàn)證零速修正在GNSS/INS緊組合導(dǎo)航中的作用，本文采用在武漢市大花嶺采集的一組車載數(shù)據(jù)進(jìn)行分析處理(測(cè)試軌跡見圖2)。測(cè)試時(shí)長(zhǎng)3 650s，測(cè)試環(huán)境為開闊天空，共有4段4min左右的靜止，其中，后2段中有2min去掉天線，以仿真GNSS信號(hào)中斷。使用TrimbleNetR9作為靜止參考站，在測(cè)試車中搭載SickeEncoderDFS60里程計(jì)以及中精度光纖陀螺定位定姿系統(tǒng)MP-POS310，其GNSS板卡型號(hào)為TrimbleBD982。IMU的主要指標(biāo)見表1，其中里程計(jì)觀測(cè)數(shù)據(jù)作為零速判斷的參考真值。另外，采用武漢邁普時(shí)空導(dǎo)航公司開發(fā)的GINS軟件提供的PPK/INS松組合平滑結(jié)果作為位置、速度和姿態(tài)的參考真值。分別使用零速修正的GNSS/INS緊組合解算和不采用零速修正的GNSS/INS緊組合解算進(jìn)行數(shù)據(jù)處理。

圖2　測(cè)試軌跡Fig.2　Testing track

指標(biāo)數(shù)值陀螺零偏0.5(°)/h角度隨機(jī)游走0.05(°)/h1/2加速度零偏25mGal速度隨機(jī)游走0.1m/(s·h1/2)

3.1零速判斷

分別使用綜合準(zhǔn)則(GNSS/INS計(jì)算速度判定閾值設(shè)為0.5 m/s，多普勒數(shù)據(jù)判定方法的衛(wèi)星數(shù)目閾值設(shè)為4顆)以及IMU數(shù)據(jù)(加速度計(jì)數(shù)據(jù)閾值設(shè)為0.001 25 m/s，陀螺儀閾值設(shè)為0.000 035 rad)進(jìn)行載體零速狀態(tài)判斷，將結(jié)果與里程計(jì)速率判斷的參考真值對(duì)比。圖3、圖4中，縱坐標(biāo)為1表示里程計(jì)判斷為靜止而判斷準(zhǔn)則判斷為動(dòng)態(tài)；-1表示里程計(jì)判斷為動(dòng)態(tài)而判斷準(zhǔn)則判斷為靜止；0表示判斷結(jié)果與里程計(jì)一致。載體在測(cè)試階段，里程計(jì)判斷有1 055個(gè)GPS歷元處于靜止?fàn)顟B(tài)，對(duì)于這些靜止?fàn)顟B(tài)，綜合準(zhǔn)則判斷正確率為99.905%，IMU數(shù)據(jù)判斷正確率僅為69.95%；測(cè)試階段載體一共經(jīng)歷了3 650個(gè)GPS歷元，綜合準(zhǔn)則總體判斷正確率為99.945%，IMU數(shù)據(jù)總體判斷正確率為91.15%。可以看出，相對(duì)于IMU數(shù)據(jù)判斷，綜合準(zhǔn)則很好地判斷出了載體的運(yùn)動(dòng)狀態(tài)，為進(jìn)一步的零速修正提供了基礎(chǔ)。

圖3　IMU數(shù)據(jù)零速判斷結(jié)果Fig.3　Results of determining the zero veclocity by IMU data

圖4　綜合判斷零速結(jié)果Fig.4　Results of determining the zero veclocity by comprehensive method

3.2零速修正對(duì)GNSS/INS緊組合導(dǎo)航結(jié)果的影響分析

兩種方法解算的位置、速度、姿態(tài)誤差見圖5。從表2、3可以看出，在靜止時(shí)段，使用零速修正后的位置、速度的精度明顯改善。其中，在信號(hào)良好時(shí)，北向、東向、地向位置誤差RMS由不使用零速修正的2.045 m、1.422 m、3.161 m分別減少為1.648 m、0.982 m、2.209 m；北向、東向、地向速度誤差RMS由不使用零速修正的0.014 m/s、0.014 m/s、0.032 m/s分別減少為0.009 m/s、0.009 m/s、0.021 m/s。在信號(hào)中斷時(shí)，隨中斷時(shí)間的增大，位置、速度誤差逐步增大，姿態(tài)誤差變化不大。中斷120 s時(shí)，北向、東向、地向位置誤差RMS由不使用零速修正的7.328 m、8.029 m、2.726 m分別減少為1.510 m、1.181 m、2.100 m；北向、東向、地向速度誤差RMS由不使用零速修正的0.087 m/s、0.135 m/s、0.015 m/s分別減少為0.003 m/s、0.003 m/s、0.003 m/s。統(tǒng)計(jì)結(jié)果表明，衛(wèi)星信號(hào)正常與中斷時(shí)，零速修正對(duì)姿態(tài)誤差的改善效果都不明顯。這說明零速修正可有效地抑制載體靜止?fàn)顟B(tài)下的位置、速度誤差累計(jì)，提高定位測(cè)速精度，但對(duì)姿態(tài)幾乎無影響。

圖5　兩種方法解算的位置誤差、速度誤差、姿態(tài)誤差Fig.5　Position errors,velocity errors and attitude errors with two methods

模式位置誤差/m速度誤差/m·s-1姿態(tài)誤差/(°)北向東向地向北向東向地向橫滾俯仰航向使用ZUPTMAX2.1931.5083.0380.0670.0440.1950.0200.0180.212RMS1.6480.9822.2090.0090.0090.0210.0040.0110.130不使用ZUPTMAX9.1268.5755.9140.0810.0580.2210.0200.0160.211RMS2.0451.4223.1610.0140.0140.0320.0050.0110.128

表3　GNSS信號(hào)中斷時(shí)載體靜止時(shí)段誤差統(tǒng)計(jì)

4　結(jié)　語

本文詳細(xì)分析了零速修正在GNSS/INS緊組合導(dǎo)航中的作用，并給出了一種綜合利用解算速度、IMU觀測(cè)數(shù)據(jù)以及多普勒觀測(cè)數(shù)據(jù)的零速判斷準(zhǔn)則。實(shí)驗(yàn)結(jié)果表明，本文的零速判斷準(zhǔn)則可準(zhǔn)確判斷載體的運(yùn)動(dòng)狀態(tài)，通過在卡爾曼濾波中使用零速修正模型，可有效抑制零速狀態(tài)下的位置、速度誤差的累計(jì)，提高導(dǎo)航結(jié)果的精度。

[1]劉川川. 基于偽距/多普勒的GPS/INS緊組合算法實(shí)現(xiàn)與抗差性能研究[D]. 武漢: 武漢大學(xué), 2012(Liu Chuanchuan. Research on Robust Performance and Algorithm Implementation in Tightly Coupled GPS/INS Integration Based on Pseudo Range and Doppler Observations[D]. Wuhan: Wuhan University, 2012)

[2]高鐘毓，王進(jìn)，董景新，等.慣性測(cè)量系統(tǒng)零速修正的幾種估計(jì)方法[J]. 中國(guó)慣性技術(shù)學(xué)報(bào)，1995，3(2): 24-29(Gao Zhongyu, Wang Jin, Dong Jingxin, et al. A Comparison of ZUPT Estimation Methods for Inertial Survey Systems[J]. Journal of Chinese Inertial Technology, 1995,3(2): 24-29)

[3]馮慶奇. 激光陀螺捷聯(lián)慣性導(dǎo)航系統(tǒng)組合導(dǎo)航及零速修正技術(shù)研究[D]. 長(zhǎng)沙: 國(guó)防科學(xué)技術(shù)大學(xué), 2009(Feng Qingqi. Research on Integrated Navigation and Zero Velocity Update Technology of Strapdown Inertial Navigation System with Ring Laser Gyroscope[D]. Changsha: National University of Defense Technology, 2009)

[4]趙玉，趙忠，范毅. 零速修正技術(shù)在車載慣性導(dǎo)航中的應(yīng)用研究[J]. 壓電與聲光, 2012，34(6): 843-847(Zhao Yu, Zhao Zhong, Fan Yi. Study on Application of Zero Velocity Update Technology to Inertial Navigation System[J]. Piezoelectrics and Acoustooptics, 2012，34(6): 843-847)

[5]方靖，顧啟泰，丁天懷. 車載慣性導(dǎo)航的動(dòng)態(tài)零速修正技術(shù)[J]. 中國(guó)慣性技術(shù)學(xué)報(bào), 2008，16(3): 265-268(Fang Jing, Gu Qitai, Ding Tianhuai. Dynamic Zero Velocity Update for Vehicle Inertial Navigation System [J]. Journal of Chinese Inertial Technology, 2008，16(3): 265-268)

[6]高宗余，李德勝，王躍宗. 結(jié)合零速檢測(cè)的微慣性系統(tǒng)混合濾波[J]. 光學(xué)精密工程, 2010，18(2): 377-383(Gao Zongyu, Li Desheng, Wang Yuezong. Design of Hybrid Unscented Particle Filtering Combining with ZUPT for Vehicle MINS[J]. Optics and Precision Engineering, 2010，18(2): 377-383)

[7]徐海鑫，孫付平，劉帥，等. 零速修正在GNSS/INS組合導(dǎo)航中的應(yīng)用[J]. 全球定位系統(tǒng), 2014，39(4): 42-45(Xu Haixin, Sun Fuping, Liu Shuai, et al. The Application of Zero-Velocity Update in the GNSS/INS Integrated Navigation[J]. GNSS Word of China, 2014，39(4): 42-45)

[8]Shin E H. Estimation Techniques for Low-Cost Inertial Navigation[D]. Calgary: University of Calgary, 2005

[9]Jin X X, De-Jong C D. Relationship between Satellite Elevation and Precision of GPS Code Observations[J]. Journal of Navigation,1996, 49(2): 253-265

[10]劉偉平，郝金明，李作虎. 由廣播星歷解算衛(wèi)星位置、速度及精度分析[J]. 大地測(cè)量與地球動(dòng)力學(xué), 2010，30(2): 144-147(Liu Weiping, Hao Jinming, Li Zuohu. Solution Method and Accuracy Analysis of Satellite Position and Velocity from Broadcast Ephemeris[J]. Journal of Geodesy and Geodynamics, 2010，30(2): 144-147)

[11]劉朝英，陳國(guó)，趙齊樂，等. BDS單點(diǎn)測(cè)速原理及精度分析[J]. 大地測(cè)量與地球動(dòng)力學(xué), 2014，34(6): 114-118(Liu Chaoying, Chen Guo, Zhao Qile, et al. Principle and Precision Analysis of BDS Absolute Velocity Determination[J]. Journal of Geodesy and Geodynamics, 2014，34(6): 114-118)

[12]單瑞，趙鐵虎，于得水，等. 單點(diǎn)GPS多普勒測(cè)速模型比較與精度分析[J]. 測(cè)繪通報(bào), 2013(3): 7-9(Shan Rui, Zhao Tiehu, Yu Deshui, et al. Model Comparison and Accuracy Analysis of Single-stationed GPS Velocity Estimation Using GPS Doppler Measurements[J]. Bulletin of Surveying and Mapping, 2013(3): 7-9)

About the first author:CHANG Le, postgraduate, majors in GNSS/INS integrated navigation, E-mail: changlesgg@whu.edu.cn.

The Application of Zero Velocity Update in GNSS/INS Tightly Coupled Integration

CHANGLe1ZHANGHongping1GAOZhouzheng1,2DINGYuxin3ZHANGQuan1

1GNSS Research Center,Wuhan University, 129 Luoyu Road, Wuhan 430079,China 2School of Geodesy and Geomatics,Wuhan University,129 Luoyu Road, Wuhan 430079,China 3School of Electronic Information Engineering, Beihang University,37 Xueyuan Road,Beijing 100191,China

In this paper, the performance of zero velocity update (ZUPT) in the GNSS/INS tightly coupled integration is researched. First, we propose a zero velocity judgment method by the utilization of GNSS/INS tightly coupled solver velocity solutions, IMU raw data and doppler data. Then, the mathematical model for the ZUPT added GNSS/INS tightly coupled integration is introduced in detail, and it is verified by vehicle testing. The statistics show that the new zero velocity judgment method is very effective in detecting the static state of the platform in comparison with real situations. By using this ZUPT mode, the IMU sensor error accumulation while stationary can be constrained visibly, especially in poor observation environmental conditions. Compared to the result without ZUPT, in a short time in the stationary state, the carrier position and velocity accuracy can be improved by 27% and 35% respectively in the quality of satellite observations, and 16%-75% and 87%-97% respectively in the case of 30-120 s satellite signal interruption. However, the improvements of altitude accuracy are not obvious whether the satellite signal is normal or interrupted when the stationary time is not so long.

GNSS/INS tightly coupled integration;zero veclocity determination;zero veclocity update;Kalman filter

National High Technology Research and Development Program of China,No.2015AA124002.

2015-09-22

常樂，碩士生，研究方向?yàn)镚NSS/INS組合導(dǎo)航，E-mail：changlesgg@whu.edu.cn。

10.14075/j.jgg.2016.10.008

1671-5942(2016)010-0879-05

P228

A

項(xiàng)目來源：國(guó)家863計(jì)劃(2015AA124002)。