基于ESKF的GNSS/INS組合導(dǎo)航算法研究

摘 要：介紹了基于ESKF算法的GNSS/INS組合實現(xiàn)流程，針對GNSS和IMU在導(dǎo)航定位時存在各自不足，使用了ESKF算法對GNSS/IMU進(jìn)行融合估計，通過IMU數(shù)據(jù)進(jìn)行系統(tǒng)狀態(tài)預(yù)測，通過GNSS數(shù)據(jù)對校正系統(tǒng)狀態(tài)進(jìn)行校正。為了驗證ESKF算法的可行性，在三種場景下進(jìn)行了實地測試，實驗結(jié)果顯示，使用本算法估計的位置結(jié)果跟GNSS的觀測結(jié)果保持一致，在GNSS無效和起伏路段也有較好的預(yù)測結(jié)果。

關(guān)鍵詞：ESKF；組合導(dǎo)航；松耦合；航跡推算

中圖分類號：TN967.2 文獻(xiàn)標(biāo)識碼：A

Research on GNSS/INS Integrated Navigation

Algorithm Based on ESKF

TANG Yaofang

（School of Information， Guangdong Nanfang Institute of Technology， Jiangmen，Guangdong 529000， China）

Abstract：This paper mainly introduces the combined implementation process of GNSS/INS based on ESKF algorithm. In view of the shortcomings of GNSS and IMU in navigation and positioning， ESKF algorithm is applied to fusion estimation of GNSS/IMU. The system states are predicted by IMU data and corrected by GNSS data. In order to verify the feasibility of ESKF algorithm， field tests were carried out in three scenarios. The experimental results showed that the location results estimated by this algorithm were consistent with the GNSS observation results， and good prediction results were also obtained in invalid GNSS and unrolling road sections.

Key words：ESKF; integrated navigation; loosely coupled; dead reckoning

隨著智能駕駛行業(yè)的快速發(fā)展，人們對運動載體的導(dǎo)航定位精度要求越來越高。導(dǎo)航技術(shù)是確定載體的位置和速度或航線的方法，而定位是確定載體的空間位置坐標(biāo)，是導(dǎo)航的子集。定位技術(shù)可分為兩類基本技術(shù)方法：直接定位與航位推算。直接定位是指通過信號或者周圍的環(huán)境特征直接確定位置，最主要的代表就是GNSS；航位推算是通過元器件監(jiān)測運動載體的車速、轉(zhuǎn)向、線速度、角速度，在已知初始位置的情況下，確定當(dāng)前的位置，最主要的元器件包括里程計、INS等[1]。

GNSS和INS各自有不同的優(yōu)勢。GNSS基本原理是通過GNSS接收機(jī)與四顆以上的衛(wèi)星進(jìn)行通信并計算距離后，通過三角測量得到GNSS的實際位置。INS是基于航跡推算法，利用加速度計和陀螺儀分別測出載體平移與旋轉(zhuǎn)參數(shù)，來實現(xiàn)導(dǎo)航功能。GNSS具有高精度的特性，誤差范圍能達(dá)到厘米級別，但在GNSS受障礙物、建筑物遮擋等不利情況下，定位精度會受影響。INS是全自主的導(dǎo)航系統(tǒng)，不受外部影響，有較高的輸出頻率，但I(xiàn)NS內(nèi)部進(jìn)行積分計算時，會不斷地累計誤差，長時間導(dǎo)航誤差會不斷發(fā)散[2]。

1 GNSS/INS組合

充分利用GNSS和INS能很好地互補(bǔ)兩者的不足，當(dāng)GNSS信號中斷時，可以使用INS的線速度和角速度進(jìn)行位置的計算；GNSS信號有效時，則抑制INS的發(fā)散。基于GNSS和INS互補(bǔ)的特性，通過一些估計算法對兩者進(jìn)行組合，使最終獲取的位置和速度更準(zhǔn)確和平滑，同時也解決了GNSS信號中斷時的定位問題。

根據(jù)GNSS/INS的組合方式，可分為松耦合、緊耦合和深耦合[3-4]。松耦合是指GNSS和INS各自獨立運行，GNSS將位置、速度等數(shù)據(jù)作為觀測輸入到濾波器，與INS的運算結(jié)果進(jìn)行求差，對INS的誤差結(jié)果進(jìn)行修正后，輸出組合后的位置、速度和姿態(tài)值。緊耦合是利用INS的位置、速度信息來估計GNSS的偽距和偽距率，與GNSS的觀測值進(jìn)行比較，用差值構(gòu)造觀測方程，經(jīng)卡爾曼濾波，獲得精確的導(dǎo)航解。深耦合采用適量跟蹤的思想，將GNSS的信號跟蹤與INS誤差發(fā)散抑制合并為一個組合，深入GNSS接收機(jī)內(nèi)部，完成信號跟蹤領(lǐng)的組合。

2 誤差狀態(tài)卡爾曼濾波

誤差狀態(tài)卡爾曼濾波（ESKF）將系統(tǒng)狀態(tài)量分為真實狀態(tài)、名義狀態(tài)和誤差狀態(tài)，真實狀態(tài)由名義狀態(tài)和誤差狀態(tài)的相加所得[5]。名義狀態(tài)是理想化的系統(tǒng)狀態(tài)，沒有考慮INS的噪聲和擾動，而誤差狀態(tài)則包含了噪聲和擾動。ESKF不對真實狀態(tài)進(jìn)行估計，而是用線性化的方式對誤差狀態(tài)進(jìn)行估計。ESKF定義的系統(tǒng)狀態(tài)量包括位置p、速度v、姿態(tài)q、加速度偏置ab、角速度偏置ωb。

2.1 ESKF測量更新

使用GNSS觀測得到的坐標(biāo)位置校準(zhǔn)ESKF的預(yù)測更新觀測方程：

y=h（x）+v（1）

式中，v為量測噪聲，v～N（0，V），h（x）為測量狀態(tài)函數(shù)，y為GNSS量測得到的位置值。

卡爾曼增益：

K=PHT（HPHT+V）－1（2）

系統(tǒng)狀態(tài)協(xié)方差矩陣更新：

P←（I－KH）P（I－KH）T+KVKT（3）

誤差狀態(tài)更新：

δx←K（y－h(huán)（x））（4）

2.2 ESKF預(yù)測更新

根據(jù)當(dāng)前時刻INS獲取到的am、ωm，更新名義狀態(tài)運動學(xué)方程中的p、v、q。

p←p+vΔt+12（R（am－ab）+g）Δt2（5）

v←v+（R（am－ab）+g）Δt（6）

q←qq（ωm－ωb）Δt（7）

誤差狀態(tài)協(xié)方差矩陣更新：

P←FxPFTx+FiQiFTi（8）

Fx為誤差狀態(tài)轉(zhuǎn)移矩陣，F(xiàn)i和Qi分別為噪聲傳遞矩陣和噪聲協(xié)方差矩陣。

2.3 ESKF流程圖

ESKF的流程圖如圖1所示，若系統(tǒng)接收不到GNSS數(shù)據(jù)，預(yù)測過程將會持續(xù)進(jìn)行系統(tǒng)狀態(tài)和協(xié)方差矩陣的預(yù)測。

圖1 ESKF實現(xiàn)流程圖

3 實驗與結(jié)果分析

在AGV小車上搭建了數(shù)據(jù)采集平臺，該平臺搭載了GNSS模塊和INS模塊，GNSS模塊的數(shù)據(jù)發(fā)布頻率為50 Hz，INS模塊的數(shù)據(jù)發(fā)布頻率為20 Hz。為驗證ESKF算法的有效性，取GNSS的觀測位置與ESKF預(yù)測位置進(jìn)行比較，設(shè)GNSS的首次觀測位置為原點（0，0）。

采集了三種場景下的原始數(shù)據(jù)進(jìn)行分析，場景一：平地，全程能觀測到GNSS數(shù)據(jù)（圖2）；場景二：平地，GNSS數(shù)據(jù)中途缺失（圖3）；場景三：泥石路，全程能觀測到GNSS數(shù)據(jù)（圖4）。

在圖2～圖4中，上半部分子圖表示GNSS軌跡，下半部分子圖表示ESKF預(yù)測軌跡，實線和虛線對應(yīng)位置x和y的值，橫坐標(biāo)為數(shù)據(jù)的時長（單位：s），縱坐標(biāo)為（x，y）相對原點的偏移距離（單位：m）。

從圖2可見，ESKF預(yù)測的車體位置與GNSS軌跡基本保持吻合。

在場景二，為了模擬GNSS的信號丟失，從時刻128 s開始，到時刻154 s為止，ESKF僅以INS數(shù)據(jù)進(jìn)行預(yù)測，GNSS數(shù)據(jù)未有進(jìn)行校正。通過比對可見，ESKF預(yù)測的車體位置與GNSS軌跡基本保持吻合。

場景三為有地勢起伏的泥石路，通過比對發(fā)現(xiàn)，ESKF預(yù)測結(jié)果與GNSS軌跡大致接近，但是GNSS的觀測結(jié)果較為平滑，ESKF受INS的噪聲和累計誤差影響，預(yù)測結(jié)果與GNSS軌跡相比有輕微的跳變。

為了能更好地評價ESKF預(yù)測位置與GNSS的參考軌跡的誤差，使用單向距離[6]（One Way Distance）進(jìn)行評價。

設(shè)p、q分別為軌跡r1、r2上的點，|r1|為軌跡r1上點的個數(shù)，dE（p、q）表示p、q的歐幾里得距離。

D（r1，r2）=1r1∫p∈rid（p，r2）dp（9）

d（p，r2）=min q∈r2dE（p，q）（10）

分別求取D（r1、r2）和D（r2、r1），然后取平均值，可得軌跡r1、r2的單向距離。

dOWD（r1，r2）=12×D（r1，r2）+D（r2，r1）（11）

表1為三個場景與GNSS軌跡（參考軌跡）的單向距離，通過比對發(fā)現(xiàn)，ESKF在三個場景的估計誤差都維持在厘米級別，其中場景一表現(xiàn)最佳，在GNSS丟失情況下，ESKF在場景二相比場景一誤差提升了23%。場景三表現(xiàn)最差，但ESKF預(yù)測結(jié)果與參考軌跡始終保持一致，沒有出現(xiàn)發(fā)散的情況。

4 結(jié) 論

本文選用了ESKF對GNSS/INS進(jìn)行融合估計，并分別在GNSS正常、GNSS異常、起伏路段三種場景下進(jìn)行ESKF的估計分析。綜合來看，ESKF估計的結(jié)果與真實軌跡依然會存在厘米級別的偏差，并且在起伏路段，ESKF的估計結(jié)果會有輕微的跳變，但ESKF算法在保證實時性的基礎(chǔ)上保證了一定的精度，在實時性有要求的系統(tǒng)中會有較好的使用場景。

參考文獻(xiàn)

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

[2] 于德新， 楊兆升， 劉雪杰. 基于卡爾曼濾波的 GPS/DR 導(dǎo)航信息融合方法[J]. 交通運輸工程學(xué)報， 2006， 6（2）： 65-69.

[3] 趙俊梅. GPS/SINS 組合導(dǎo)航系統(tǒng)濾波算法研究[D]. 太原： 中北大學(xué)， 2006.

[4] GROVES P D. Principles of GNSS， inertial， and multisensor integrated navigation systems[J]. IEEE Aerospace and Electronic Systems Magazine， 2015， 30（2）： 26-27.

[5] J SOLA J. Quaternion kinematics for the error state Kalman filter[J]. arxiv preprint arxiv：1711.02508， 2017.

[6] LIN B， SU J. One way distance： for shape based similarity search of moving object trajectories[J]. GeoInformatica， 2008， 12： 117-142.