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

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

中圖分類號(hào)：TN967.2 文獻(xiàn)標(biāo)識(shí)碼：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ā)展，人們對(duì)運(yùn)動(dòng)載體的導(dǎo)航定位精度要求越來越高。導(dǎo)航技術(shù)是確定載體的位置和速度或航線的方法，而定位是確定載體的空間位置坐標(biāo)，是導(dǎo)航的子集。定位技術(shù)可分為兩類基本技術(shù)方法：直接定位與航位推算。直接定位是指通過信號(hào)或者周圍的環(huán)境特征直接確定位置，最主要的代表就是GNSS；航位推算是通過元器件監(jiān)測(cè)運(yùn)動(dòng)載體的車速、轉(zhuǎn)向、線速度、角速度，在已知初始位置的情況下，確定當(dāng)前的位置，最主要的元器件包括里程計(jì)、INS等[1]。

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

1 GNSS/INS組合

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

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

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

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

2.1 ESKF測(cè)量更新

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

y=h（x）+v（1）

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

卡爾曼增益：

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ù)測(cè)更新

根據(jù)當(dāng)前時(shí)刻INS獲取到的am、ωm，更新名義狀態(tài)運(yùn)動(dòng)學(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ù)測(cè)過程將會(huì)持續(xù)進(jìn)行系統(tǒng)狀態(tài)和協(xié)方差矩陣的預(yù)測(cè)。

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

3 實(shí)驗(yàn)與結(jié)果分析

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

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

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

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

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

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

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

設(shè)p、q分別為軌跡r1、r2上的點(diǎn)，|r1|為軌跡r1上點(diǎn)的個(gè)數(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為三個(gè)場(chǎng)景與GNSS軌跡（參考軌跡）的單向距離，通過比對(duì)發(fā)現(xiàn)，ESKF在三個(gè)場(chǎng)景的估計(jì)誤差都維持在厘米級(jí)別，其中場(chǎng)景一表現(xiàn)最佳，在GNSS丟失情況下，ESKF在場(chǎng)景二相比場(chǎng)景一誤差提升了23%。場(chǎng)景三表現(xiàn)最差，但ESKF預(yù)測(cè)結(jié)果與參考軌跡始終保持一致，沒有出現(xiàn)發(fā)散的情況。

4 結(jié) 論

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

參考文獻(xiàn)

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

[2] 于德新， 楊兆升， 劉雪杰. 基于卡爾曼濾波的 GPS/DR 導(dǎo)航信息融合方法[J]. 交通運(yùn)輸工程學(xué)報(bào)， 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.