基于ESKF的GNSS/INS組合導航算法研究

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

關鍵詞：ESKF；組合導航；松耦合；航跡推算

中圖分類號：TN967.2 文獻標識碼：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

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

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

1 GNSS/INS組合

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

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

2 誤差狀態卡爾曼濾波

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

2.1 ESKF測量更新

使用GNSS觀測得到的坐標位置校準ESKF的預測更新觀測方程：

y=h（x）+v（1）

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

卡爾曼增益：

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

系統狀態協方差矩陣更新：

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

誤差狀態更新：

δx←K（y－h（x））（4）

2.2 ESKF預測更新

根據當前時刻INS獲取到的am、ωm，更新名義狀態運動學方程中的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）

誤差狀態協方差矩陣更新：

P←FxPFTx+FiQiFTi（8）

Fx為誤差狀態轉移矩陣，Fi和Qi分別為噪聲傳遞矩陣和噪聲協方差矩陣。

2.3 ESKF流程圖

ESKF的流程圖如圖1所示，若系統接收不到GNSS數據，預測過程將會持續進行系統狀態和協方差矩陣的預測。

圖1 ESKF實現流程圖

3 實驗與結果分析

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

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

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

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

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

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

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

設p、q分別為軌跡r1、r2上的點，|r1|為軌跡r1上點的個數，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軌跡（參考軌跡）的單向距離，通過比對發現，ESKF在三個場景的估計誤差都維持在厘米級別，其中場景一表現最佳，在GNSS丟失情況下，ESKF在場景二相比場景一誤差提升了23%。場景三表現最差，但ESKF預測結果與參考軌跡始終保持一致，沒有出現發散的情況。

4 結 論

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

參考文獻

［１］ 嚴恭敏. 捷聯慣導算法及車載組合導航系統研究[D]. 西安： 西北工業大學， 2004.

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

[3] 趙俊梅. GPS/SINS 組合導航系統濾波算法研究[D]. 太原： 中北大學， 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.