神經(jīng)網(wǎng)絡(luò)修正的速度約束輔助車載SINS定位算法

李正帥，繆玲娟，周志強，吳子昊

(北京理工大學(xué)自動化學(xué)院，北京 100081)

0 引 言

車載全球?qū)Ш叫l(wèi)星系統(tǒng)(GNSS)/捷聯(lián)慣性導(dǎo)航系統(tǒng)(SINS)組合導(dǎo)航系統(tǒng)中的GNSS信號不可用時，僅使用SINS導(dǎo)航定位精度逐漸下降，難以滿足要求。速度約束輔助SINS定位假設(shè)車輛在行駛過程中沒有發(fā)生側(cè)滑和跳躍，利用車輛橫向和豎向速度為零的約束條件修正SINS的誤差，可以在不增加成本的前提下，提高定位精度。付強文等利用速度約束輔助SINS定位，將其作為里程計失效的備選方案，定位精度優(yōu)于僅使用SINS定位。為了進一步提高速度約束輔助SINS的定位精度，胡杰等在速度約束輔助SINS的基礎(chǔ)上，引入模糊推理系統(tǒng)自適應(yīng)調(diào)節(jié)量測噪聲方差陣，通過試驗驗證了速度約束和自適應(yīng)濾波對于提高SINS位置精度的有效性。

車輛長時間朝某一固定方向行駛時，使用速度約束輔助SINS其縱向位置誤差會逐漸累積增大。韓勇強等結(jié)合高階曲線擬合的方法對車輛縱(前)向速度殘差導(dǎo)致的定位誤差進行補償，能夠有效彌補僅使用速度約束的不足，但是該算法需要停車觀測縱向速度殘差才能對縱向位置誤差進行補償，不能在連續(xù)行駛過程中在線修正，這對于一般情況下不能隨意停車的城市隧道場景具有一定的局限性。陳映秋等將微機電系統(tǒng)(Micro-elector-mechanical system, MEMS)慣性測量單元(Inertial measurement unit, IMU)安裝在載體車輪中心，提取出縱向速度信息，相比于僅使用速度約束增加了一維縱向速度約束，提高了自主導(dǎo)航性能，但是考慮到實際載車安裝和布線等因素該方案不易實現(xiàn)。李博文等將車輛瞬時運動看作不同曲率半徑的圓周運動，根據(jù)縱向速度、豎向角速度和比力測量值構(gòu)造向心加速度差值誤差作為新量測量，將其與速度約束一同輔助車載SINS定位，新量測量提高了橫向加速度計偏值的可觀測性，理論上會因此提高定位精度，但是引入的新量測量中的比力沒有考慮桿臂加速度補償，桿臂加速度誤差等效為加速度計測量誤差，動態(tài)情況下角加速度通過角速度微分得到，會引入較大噪聲，對桿臂加速度難以實現(xiàn)精確補償。

隨著人工智能技術(shù)的發(fā)展，大量專家學(xué)者將神經(jīng)網(wǎng)絡(luò)算法用于預(yù)測修正導(dǎo)航誤差。目前常用的神經(jīng)網(wǎng)絡(luò)算法有卷積神經(jīng)網(wǎng)絡(luò)(Convolutional neural networks, CNN)、BP(Back propagation)神經(jīng)網(wǎng)絡(luò)、徑向基函數(shù)(Radial basis function，RBF)神經(jīng)網(wǎng)絡(luò)等。CNN主要用于識別位移、縮放及其他形式扭曲不變的二維圖形，在圖像識別、圖像分類等領(lǐng)域應(yīng)用廣泛；BP神經(jīng)網(wǎng)絡(luò)是一種全局逼近網(wǎng)絡(luò)，結(jié)構(gòu)簡單且適用于非線性函數(shù)逼近，但是易陷入局部極小值，學(xué)習(xí)速度較慢，對于對實時性要求高的應(yīng)用場景具有一定的局限性；RBF神經(jīng)網(wǎng)絡(luò)是一種局部逼近網(wǎng)絡(luò)，相較于全局逼近網(wǎng)絡(luò)(如BP神經(jīng)網(wǎng)絡(luò))，局部逼近可以簡化計算量，并且它只有一個隱含層，因此RBF神經(jīng)網(wǎng)絡(luò)的訓(xùn)練速度更快，能夠滿足對實時性有要求的應(yīng)用場景。RBF神經(jīng)網(wǎng)絡(luò)的突出優(yōu)點是具有很強的非線性映射能力和很快的學(xué)習(xí)收斂速度。研究表明，RBF神經(jīng)網(wǎng)絡(luò)是連續(xù)函數(shù)的最佳逼近。

為了提高GNSS失效時車載SINS的導(dǎo)航精度，本文在不增加額外傳感器的情況下利用速度約束輔助SINS定位，提出一種限定記憶指數(shù)加權(quán)的自適應(yīng)濾波算法，實時估計量測噪聲，以提高濾波器對新近量測數(shù)據(jù)的利用權(quán)重以及對外界環(huán)境的適應(yīng)能力；在此基礎(chǔ)上，針對車輛長時間朝某一固定方向行駛時僅使用速度約束輔助SINS其縱向位置誤差發(fā)散較嚴(yán)重的問題，提出一種基于神經(jīng)網(wǎng)絡(luò)的縱向位置誤差修正算法，引入RBF神經(jīng)網(wǎng)絡(luò)算法在線預(yù)測縱向位置誤差修正系數(shù)，對定位結(jié)果進一步修正。在人為設(shè)置GNSS失效和真實隧道場景下進行車載試驗，驗證了本文算法的有效性與可行性。

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

1.1 狀態(tài)方程

(1)

=

(2)

濾波器狀態(tài)方程為：

(3)

式中：為SINS誤差方程和安裝誤差角誤差方程構(gòu)造的狀態(tài)轉(zhuǎn)移矩陣；為系統(tǒng)噪聲輸入矩陣；為系統(tǒng)噪聲。

1.2 量測方程

GNSS有效時，將SINS解算的位置、速度信息與GNSS輸出的位置、速度信息分別求差作為濾波器量測值，此時量測方程為：

=+

(4)

式中：表示量測量；表示量測噪聲；表示量測矩陣，表達(dá)式為：

(5)

GNSS失效時，采用速度約束構(gòu)造量測方程。車體坐標(biāo)系系下速度分量為：

(6)

根據(jù)速度約束構(gòu)成量測量：

(7)

對式(6)進行全微分，并考慮式(1)則有：

(8)

式中：δ表示系下速度誤差；[()]表示取的反對稱矩陣。

由式(7)和式(8)可得量測方程：

=+

(9)

式中：為量測矩陣。

(10)

式中：(1,*)表示取的第1行，其余符號含義類似；表示量測噪聲。

2 限定記憶指數(shù)加權(quán)的自適應(yīng)濾波算法

標(biāo)準(zhǔn)卡爾曼濾波要求系統(tǒng)的數(shù)學(xué)模型、系統(tǒng)噪聲和量測噪聲統(tǒng)計特性準(zhǔn)確已知。但是在實際工程應(yīng)用中，量測噪聲受外界環(huán)境因素影響，具有較大的不確定性，卡爾曼濾波器使用不準(zhǔn)確的量測噪聲統(tǒng)計特性會產(chǎn)生較大的狀態(tài)估計誤差，甚至可能使濾波發(fā)散。本文對量測噪聲協(xié)方差陣進行自適應(yīng)調(diào)整。

理想情況下濾波殘差滿足零均值高斯白噪聲分布的條件，其理論協(xié)方差為：

(11)

一般根據(jù)區(qū)間長度為的濾波殘差序列對進行極大似然最優(yōu)估計，對滑動窗口內(nèi)的數(shù)據(jù)求取算術(shù)平均。為了提高濾波殘差協(xié)方差的估計精度和濾波器對新近量測數(shù)據(jù)的利用權(quán)重，本文對滑動窗口內(nèi)的數(shù)據(jù)進行指數(shù)加權(quán)。

時刻的濾波殘差協(xié)方差的估計值為：

(12)

式中：為窗口大??；為指數(shù)因子，0<<1。歷史時刻的殘差數(shù)據(jù)離當(dāng)前時刻越遠(yuǎn)，在求殘差協(xié)方差的估計值時所占比重越小。

根據(jù)文獻[13]中極大似然法的證明方法，可以證明本文算法提出的式(12)仍是的極大似然最優(yōu)估計。

為了減小計算量和存儲空間，對式(12)進一步化簡，得到遞推公式：

(13)

(14)

(15)

3 基于神經(jīng)網(wǎng)絡(luò)的SINS縱向位置誤差修正算法

RBF神經(jīng)網(wǎng)絡(luò)是一種三層前饋網(wǎng)絡(luò)模型，由輸入層、隱含層和輸出層組成。從輸入層到隱含層的變換是非線性的，從隱含層到輸出層的變換是線性的。網(wǎng)絡(luò)的參數(shù)主要有三種：徑向基函數(shù)的中心、方差和隱含層到輸出層的權(quán)值，對網(wǎng)絡(luò)的訓(xùn)練過程即為求解這三種參數(shù)的過程。

假設(shè)城市環(huán)境中GNSS有效時的訓(xùn)練樣本包括了車輛多種機動狀態(tài)下的數(shù)據(jù)，通過GNSS有效場景下的樣本數(shù)據(jù)訓(xùn)練神經(jīng)網(wǎng)絡(luò)，利用神經(jīng)網(wǎng)絡(luò)的預(yù)測功能估計GNSS失效場景下SINS縱向位置誤差的修正系數(shù)。

車輛的運動狀態(tài)可由速度和姿態(tài)描述，其中姿態(tài)可由四元數(shù)表示。由于該四元數(shù)為規(guī)范化四元數(shù)，所以可以選取四元數(shù)的3個虛部作為神經(jīng)網(wǎng)絡(luò)輸入量的一部分，從而減小計算量。對于神經(jīng)網(wǎng)絡(luò)的訓(xùn)練樣本，本文選取GNSS有效時SINS解算的3個速度、3個四元數(shù)虛部共6個量作為神經(jīng)網(wǎng)絡(luò)的輸入量，輸出量為修正系數(shù)°表示使用速度約束輔助SINS定位濾波周期內(nèi)，縱向行駛的距離修正后與修正前的比值；通過在GNSS有效時的濾波周期內(nèi)進行GNSS/SINS組合導(dǎo)航的同時，并行使用速度約束輔助SINS定位來求取。

輸出量求解過程中車輛的位置示意圖如圖1所示。

圖1 輸出量s求解過程中車輛的位置示意圖Fig.1 Schematic diagram of vehicle position in the process of output s solution

圖1中,-1與分別表示-1時刻與時刻GNSS/SINS組合導(dǎo)航定位的車輛位置；′表示GNSS有效時使用速度約束輔助SINS定位的車輛位置；表示-1與間的距離；表示-1與′間的距離。的大小為：

(16)

在GNSS有效時輸出的導(dǎo)航結(jié)果為GNSS/SINS組合導(dǎo)航的位置，速度約束輔助SINS定位的位置僅用于計算和。

當(dāng)GNSS有效時記錄樣本數(shù)據(jù)(6個輸入量，1個輸出量)，在GNSS失效時的第一時刻利用GNSS失效前記錄的最新的組樣本數(shù)據(jù)進行訓(xùn)練?？稍O(shè)置為500或適當(dāng)調(diào)整。訓(xùn)練過程包括讀取樣本數(shù)據(jù)、初始化神經(jīng)網(wǎng)絡(luò)、訓(xùn)練神經(jīng)網(wǎng)絡(luò)、存儲網(wǎng)絡(luò)參數(shù)。

在GNSS失效的后續(xù)時間里，讀取網(wǎng)絡(luò)參數(shù)，以當(dāng)前時刻慣導(dǎo)解算的3個速度、3個四元數(shù)虛部作為輸入量，預(yù)測修正系數(shù)′,′表示神經(jīng)網(wǎng)絡(luò)預(yù)測的SINS縱向位置誤差修正系數(shù)。

時刻神經(jīng)網(wǎng)絡(luò)修正位置示意圖如圖2所示。

圖2 位置修正示意圖Fig.2 Schematic diagram of position correction

時刻經(jīng)過神經(jīng)網(wǎng)絡(luò)修正后的車輛緯度、經(jīng)度分別為：

(17)

本文算法流程圖如圖3所示，實現(xiàn)步驟如下：

圖3 本文算法流程圖Fig.3 Flow chart of the proposed algorithm

1)首先建立狀態(tài)方程，當(dāng)GNSS有效時，進行GNSS/SINS組合導(dǎo)航，并存儲用于RBF神經(jīng)網(wǎng)絡(luò)訓(xùn)練的樣本數(shù)據(jù)。

2)當(dāng)GNSS失效時，采用速度約束和自適應(yīng)濾波輔助SINS，在GNSS失效的第一時刻對神經(jīng)網(wǎng)絡(luò)進行初始化，通過已存儲的樣本數(shù)據(jù)對神經(jīng)網(wǎng)絡(luò)進行訓(xùn)練，并存儲網(wǎng)絡(luò)參數(shù)。

3)在GNSS失效的后續(xù)時間里，將SINS解算的速度、四元數(shù)信息作為神經(jīng)網(wǎng)絡(luò)的輸入量預(yù)測縱向位置誤差修正系數(shù)′。

4)根據(jù)式(17)對速度約束輔助SINS定位后的位置進行修正。

4 試驗結(jié)果與分析

基于所設(shè)計的神經(jīng)網(wǎng)絡(luò)修正的速度約束輔助車載SINS定位算法，在人為設(shè)置GNSS失效和真實隧道場景下進行了車載試驗，對采集的原始MEMS慣導(dǎo)數(shù)據(jù)和GNSS數(shù)據(jù)進行離線仿真，驗證了算法的有效性與可用性。

4.1 試驗條件

車載試驗通過LaneTo-3低成本GNSS/MEMS SINS組合導(dǎo)航設(shè)備實現(xiàn)對MEMS慣導(dǎo)數(shù)據(jù)和衛(wèi)星數(shù)據(jù)的采集。測試的同時搭載Mars-D高精度GNSS/SINS組合導(dǎo)航定位系統(tǒng)，該系統(tǒng)采用緊組合的方式將慣導(dǎo)與雙天線差分GNSS定位進行組合，開放天空下定位精度可至厘米級；結(jié)合地圖數(shù)據(jù)庫信息和反向平滑技術(shù)，衛(wèi)星信號中斷120 s位置精度(1)小于1 m。將Mars-D處理后的導(dǎo)航結(jié)果作為基準(zhǔn)用于統(tǒng)計本文算法的誤差。

車載試驗系統(tǒng)如圖4～6所示。

圖4 天線安裝位置Fig.4 Position of antenna installation

圖5 LaneTo-3組合導(dǎo)航設(shè)備Fig.5 LaneTo-3 integrated navigation device

圖6 Mars-D高精度組合導(dǎo)航定位系統(tǒng)Fig.6 Mars-D high precision integrated navigation positioning system

用于采集試驗數(shù)據(jù)的LaneTo-3組合導(dǎo)航設(shè)備中陀螺儀和加速度計的基本參數(shù)如表1所示。

表1 陀螺儀和加速度計基本參數(shù)Table 1 Basic parameters of the gyroscope and accelerometer

試驗場景主要測試直線行駛和隧道情況。試驗路線如圖7～9所示，選取路段1～3：

圖7 路段1與路段3車輛軌跡Fig.7 Vehicle tracks on Route 1 and Route 3

圖8 路段2車輛軌跡Fig.8 Vehicle track on Route 2

圖9 路段3車輛軌跡Fig.9 Vehicle track on Route 3

1)路段1如圖7所示，選在北京市牡丹園站所在的南北直線路段，將GNSS數(shù)據(jù)斷開60 s，模擬直線隧道場景，衛(wèi)星斷開時間內(nèi)載車直線行駛約600 m，車輛先加速再減速，速度范圍是14.4～50.4 km/h。

2)路段2如圖8所示，選在北京市北四環(huán)中路的東西直線路段，將GNSS數(shù)據(jù)斷開60 s，模擬直線隧道場景，衛(wèi)星斷開時間內(nèi)載車直線行駛約1020 m，車輛先加速再減速，再加速，速度范圍是47.5～70.6 km/h。由于車輛長時間朝某一固定方向行駛時使用速度約束輔助SINS其縱向位置誤差發(fā)散較嚴(yán)重，因此路段1和路段2用于測試本文算法在直線路段的性能。

3)路段3如圖7、圖9所示，選在北京市慧忠路隧道附近，該隧道衛(wèi)星失效路段單向全長約1.2 km，由小角度弧形彎道和直線路段組成，用于測試本文算法在真實隧道場景下的整體性能。行駛軌跡為自西向東進入隧道，調(diào)頭自東向西第二次進入隧道后再次調(diào)頭自西向東進入隧道，即進入隧道共三次。車輛機動主要是航向轉(zhuǎn)彎和直線行駛等動作，車速約45 km/h。

圖7～9中兩條虛線中間的路段為衛(wèi)星失效路段，箭頭表示行駛方向，圖9中的序號表示箭頭的先后次序。

4.2 算法校驗

本文算法使用提出的限定記憶指數(shù)加權(quán)的自適應(yīng)濾波實現(xiàn)速度約束輔助SINS定位，并通過RBF神經(jīng)網(wǎng)絡(luò)預(yù)測系數(shù)對濾波后的位置進一步修正。

為了檢驗本文算法中提出的自適應(yīng)濾波算法及神經(jīng)網(wǎng)絡(luò)修正的效果，將使用標(biāo)準(zhǔn)卡爾曼濾波的速度約束輔助SINS的算法(簡記為“KF-VC”)、只使用本文提出的自適應(yīng)濾波的速度約束輔助SINS算法(簡記為“AKF-VC”)與完整的本文算法(簡記為“RBF-AKF-VC”)進行了對比。

KF-VC算法、AKF-VC算法與RBF-AKF-VC在處理三個路段的試驗數(shù)據(jù)后的位置誤差、速度誤差對比如圖10～15所示，三個路段均為GNSS失效路段。

圖10 路段1位置誤差對比Fig.10 Comparison of position errors on Route 1

圖11 路段1速度誤差對比Fig.11 Comparison of velocity errors on Route 1

圖12 路段2位置誤差對比Fig.12 Comparison of position errors on Route 2

圖13 路段2速度誤差對比Fig.13 Comparison of velocity errors on Route 2

圖14 路段3位置誤差對比Fig.14 Comparison of position errors on Route 3

圖15 路段3速度誤差對比Fig.15 Comparison of velocity errors on Route 3

圖10～15中實線表示RBF-AKF-VC算法處理試驗數(shù)據(jù)后的位置誤差、速度誤差，虛線、點線分別為KF-VC算法、AKF-VC算法對應(yīng)的誤差曲線。從圖10～15中可以看出，對于GNSS失效的場景，RBF-AKF-VC算法的導(dǎo)航精度最高；AKF-VC算法相對于KF-VC算法，位置誤差與速度誤差均減?。粡膱D10、圖12和圖14可以看出，利用神經(jīng)網(wǎng)絡(luò)修正位置誤差，能夠進一步提高SINS的位置精度。

為了定量表示本文算法中提出的自適應(yīng)濾波算法與神經(jīng)網(wǎng)絡(luò)修正的效果，分別統(tǒng)計KF-VC算法、AKF-VC算法與RBF-AKF-VC算法在三個路段上的均方根誤差(RMSE)，路段3先、后三次進入隧道依次統(tǒng)計。結(jié)果如表2～表4所示。

表2 路段1三種算法的均方根誤差對比Table 2 RMSE comparison of the three algorithms of Route 1

表3 路段2三種算法的均方根誤差對比Table 3 RMSE comparison of the three algorithms of Route 2

表4 路段3隧道場景三種算法的均方根誤差對比Table 4 RMSE comparison of the three algorithms in tunnel scene of route 3

續(xù)表4

由表2、表3可知，由于路段1主要是南北向直線行駛路段，路段2主要是東西向直線行駛路段，缺少轉(zhuǎn)彎機動過程，僅有橫向和豎向的速度約束，縱向速度誤差逐漸增大。表2中，北向速度誤差明顯大于東向速度誤差，導(dǎo)致路段1的北向位置誤差明顯比東向位置誤差大；而表3中，東向速度誤差明顯大于北向速度誤差，進而導(dǎo)致路段2的東向位置誤差明顯比北向位置誤差大。相比于KF-VC算法，RBF-AKF-VC算法在路段1上的東向位置誤差減小了35.66%，北向位置誤差減小了74.20%，高度誤差減小了7.34%，三個方向的速度誤差均減小，北向速度誤差減小得最多(減小了0.42 m/s)；在路段2上的東向位置誤差減小了59.14%，北向位置誤差減小了66.47%，高度誤差減小了15.21%，三個方向的速度誤差均減小，東向速度誤差減小得最多(減小了0.81 m/s)。AKF-VC算法提高了濾波器對新近量測數(shù)據(jù)的利用權(quán)重與濾波精度，使定位精度得到了一定的提升；RBF-AKF-VC算法使用神經(jīng)網(wǎng)絡(luò)預(yù)測系數(shù)修正了SINS縱向位置誤差，相比于AKF-VC算法，在路段1上東向位置誤差進一步減小了16.27%，北向位置誤差進一步減小了61.96%；在路段2上東向位置誤差進一步減小了43.21%，北向位置誤差進一步減小了46.15%。

表4中三次的試驗結(jié)果中，相對于北向位置誤差，東向位置誤差較大，這是由于車輛在路段3上的整體行駛軌跡是東西方向的，采用速度約束缺少對縱向速度的量測更新，長時間在一個方向行駛該方向位置誤差會逐漸積累。其中第2次的行駛方向與第1、3次相反，轉(zhuǎn)彎方向也相反。相比于KF-VC算法，RBF-AKF-VC算法在路段3先、后三次往返進入隧道的車輛定位精度均得到了提高，東向位置誤差平均減小了40.11%，北向位置誤差平均減小了31.28%，高度誤差平均減小了23.11%，三個方向的速度誤差均減小，東向速度誤差減小得最多(平均減小了0.12 m/s)。RBF-AKF-VC算法使用神經(jīng)網(wǎng)絡(luò)預(yù)測修正系數(shù)對SINS縱向位置誤差進行修正后，相比于AKF-VC算法，位置誤差可進一步減小，尤其是東向位置誤差減小了30.35%。

5 結(jié) 論

車載GNSS/SINS組合導(dǎo)航在GNSS失效SINS單獨工作時誤差會逐漸增大，本文在不增加額外傳感器的前提下利用速度約束輔助SINS定位。提出一種限定記憶指數(shù)加權(quán)的自適應(yīng)濾波算法實時估計量測噪聲，提高了濾波器的適應(yīng)能力；與此同時，針對僅使用速度約束輔助SINS其縱向位置誤差會逐漸發(fā)散的問題，提出一種基于RBF神經(jīng)網(wǎng)絡(luò)的縱向位置誤差修正算法，通過神經(jīng)網(wǎng)絡(luò)訓(xùn)練預(yù)測系數(shù)進一步修正濾波后的定位結(jié)果。在人為設(shè)置GNSS失效和真實隧道場景下進行車載試驗，結(jié)果表明，本文算法對于位置誤差的修正效果優(yōu)于標(biāo)準(zhǔn)卡爾曼濾波和速度約束輔助SINS相結(jié)合的常規(guī)算法，具有較高的定位精度；其中RBF神經(jīng)網(wǎng)絡(luò)預(yù)測的修正系數(shù)對于縱向位置誤差具有較好的修正效果，為解決速度約束輔助SINS定位其縱向位置誤差發(fā)散的問題提供了一種新思路和新方法。本文算法不增加硬件成本，對提高低成本車載導(dǎo)航在衛(wèi)星信號丟失時的定位精度具有重要意義。