GPS/INS延時估計與基于殘差重構(gòu)的延時補償算法

付廷強，馬太原，王亞飛，殷承良

(上海交通大學(xué) 機械與動力工程學(xué)院，上海 200240)

隨著人們對減小交通的事故發(fā)生率，將人類從疲勞的駕駛工作中解放出來的需求發(fā)展，自動駕駛成為了當(dāng)下的研究熱點，而車輛的定位導(dǎo)航則是實現(xiàn)自動駕駛的前提和基礎(chǔ).車輛定位導(dǎo)航的方式有很多種，可以大體分為單傳感器定位如視覺定位[1-2]、衛(wèi)星定位[3-4]以及多傳感器組合定位[5-6]等.其中，全球定位系統(tǒng)(GPS)/慣性導(dǎo)航系統(tǒng)(INS)的組合導(dǎo)航系統(tǒng)比較成熟，由對多種自動駕駛車輛的傳感器配置的統(tǒng)計結(jié)果[7]可以看出，GPS和慣性測量單元(IMU)幾乎是必備的2種傳感器.兩者具有較強的互補特性，組合后既可以通過INS的高采樣速率和自主式導(dǎo)航的特性解決GPS采樣速率低和易失鎖的問題，又可以通過良好環(huán)境下精確的GPS信號去除INS的累積誤差.

但是，多傳感器進(jìn)行信息融合時總是會受到時延上的困擾，在對時延進(jìn)行補償方面，現(xiàn)有研究思路大體可分為以下幾種：把存在延時的狀態(tài)作為擴展量添加到狀態(tài)方程中，擴展維數(shù)與延遲步數(shù)相關(guān)，因此適合于延時不是太大的情況[8]；根據(jù)延遲的觀測量或殘差值構(gòu)造當(dāng)前時刻的偽觀測量或偽殘差值，利用構(gòu)造的量進(jìn)行測量更新[9-10]；利用延時的測量值估計出延時的狀態(tài)量和延時值，然后運用運動學(xué)或動力學(xué)關(guān)系構(gòu)造方程計算現(xiàn)在的狀態(tài)[11-12].GPS/INS組合導(dǎo)航系統(tǒng)面臨相同問題，根據(jù)文獻(xiàn)[11]和[12]對延時問題的研究，GPS接收機的數(shù)據(jù)處理過程給GPS輸出引入了可達(dá)百毫秒級的延時，該問題會影響組合導(dǎo)航的精度.

許多文獻(xiàn)對GPS和INS融合過程中的延時問題進(jìn)行了分析，并提出了一些延時補償?shù)姆椒?文獻(xiàn)[13]基于對測量殘差的觀測，分析了GPS測量值發(fā)生延時的情況下實現(xiàn)組合導(dǎo)航濾波穩(wěn)定的條件.文獻(xiàn)[14]構(gòu)建了時間同步誤差模型，并采用Lagrange插值法獲取對應(yīng)于任意延時值的IMU數(shù)據(jù)，消除延時的影響.文獻(xiàn)[15]提出了高動態(tài)運動條件下軟硬件相結(jié)合的時間同步策略,比較適用于飛行器等大機動運載體.文獻(xiàn)[11]構(gòu)建了非線性觀測器，獲得延時的導(dǎo)航解及延時值，并通過運動學(xué)關(guān)系估計當(dāng)前時刻的導(dǎo)航解.考慮到車輛相對于飛機等運載器的運動強度低得多，不需要精密的硬件時間同步裝置，因此本文以軟件的方式進(jìn)行時間同步.

區(qū)別于以上方法，本文的特色在于直接對卡爾曼濾波器進(jìn)行適應(yīng)性修改，利用殘差重構(gòu)的方式實現(xiàn)延時補償.首先構(gòu)建車輛的GPS/INS松耦合組合導(dǎo)航模型，然后根據(jù)運動學(xué)關(guān)系對模型做了一定的修改并實現(xiàn)延時估計，最后將殘差重構(gòu)算法應(yīng)用到GPS/INS組合導(dǎo)航濾波中實現(xiàn)了延時誤差補償，并通過試驗驗證了所述算法的有效性.

1 GPS/INS組合導(dǎo)航模型

構(gòu)建GPS/INS組合導(dǎo)航模型是完成位置解算至關(guān)重要的一步，組合導(dǎo)航的數(shù)學(xué)模型包括狀態(tài)模型和測量模型.本文在構(gòu)建模型時采用的導(dǎo)航坐標(biāo)系(n系)為北東地坐標(biāo)系；采用的載體坐標(biāo)系(b系)x軸指向車輛前方，y軸指向車輛的右側(cè)，z軸向下；下標(biāo)i表示慣性坐標(biāo)系；下標(biāo)e表示地心地固坐標(biāo)系.

狀態(tài)模型為

(1)

測量模型為

y=Hx+V

(2)

式中：x為狀態(tài)矢量；F為系統(tǒng)矩陣；W為估計噪聲；y為觀測量；H為測量矩陣；V為測量噪聲.狀態(tài)模型描述了慣性解算過程中位置、速度、角度、加速度計和陀螺儀誤差狀態(tài)的微分形式，而測量模型則描述了觀測量與誤差狀態(tài)之間的關(guān)系.

1.1 狀態(tài)模型

車輛在平直路面上的行駛過程中，沿著地向的加速度可以視為白噪聲，因此沒有必要估計車輛沿著地向的速度誤差狀態(tài)以及位置誤差狀態(tài).誤差狀態(tài)矢量

(1)平臺失準(zhǔn)角微分方程.平臺失準(zhǔn)角微分方程描述了計算導(dǎo)航坐標(biāo)系相對于實際導(dǎo)航坐標(biāo)系的角度偏差.產(chǎn)生偏差的主要原因是由于車輛在運動過程中所參考的導(dǎo)航坐標(biāo)系在不停移動，這造成了實際導(dǎo)航坐標(biāo)系相對于計算導(dǎo)航坐標(biāo)系發(fā)生偏轉(zhuǎn)，同時陀螺儀的漂移也會為角速度的測量引入誤差.

(3)

式中：

ψ=(ψx,ψy,ψz)

(2)速度誤差微分方程.速度誤差主要是由平臺失準(zhǔn)角和加速度計漂移引起的，平臺失準(zhǔn)角的存在導(dǎo)致比力不能正確變換到導(dǎo)航坐標(biāo)系下.

(4)

(3)位置誤差微分方程.位置誤差主要由速度誤差引起，如果不及時校正，誤差會逐漸積累，最終造成較大的定位誤差.

(5)

(6)

式中：RN為車輛所處地點的子午圈曲率半徑；RE為車輛所處地點的卯酉圈曲率半徑；hb為車輛所處地點的高度.

(4)加速度計和陀螺儀偏差.傳感器的零偏是導(dǎo)致慣性傳感器輸出誤差的重要因素,慣性傳感器的零偏可以建模為一階Gauss-Markov過程.

(7)

式中：τa和τg分別為加速度計和陀螺儀零偏的相關(guān)時間；Wba和Wbg分別為加速度計和陀螺儀零偏的噪聲項.

由以上的討論可見，這些誤差是相互耦合的，只有對這些誤差狀態(tài)全部正確估計，才能得到較好的導(dǎo)航結(jié)果.根據(jù)式(3)～(7)即可完成組合導(dǎo)航狀態(tài)模型即式(1)的建模，具體的建模推導(dǎo)過程可以參考文獻(xiàn)[16].

1.2 測量模型

對于衛(wèi)星接收機而言，接收機的位置和速度易獲取，在松耦合組合導(dǎo)航方程中，選取INS與GPS的位置差及速度差作為松耦合組合導(dǎo)航 Kalman 濾波中的觀測量.

(1)INS與GPS的位置差

式中：LI和LG為INS和GPS輸出的緯度值；λI和λG為INS和GPS輸出的經(jīng)度值；Nx和Ny為GPS接收機沿著導(dǎo)航坐標(biāo)系x和y方向的距離測量誤差.由于RN和RE隨著車輛位置不斷變化，為了使觀測噪聲不受車輛位置變化影響，式(8)和(9)可以改為

(LI-LG)(RN+hb)=δLb(RN+hb)+Nx

(10)

(λI-λG)(RE+hb)cosLb=

δλb(RE+hb)cosLb+Ny

(11)

(2)INS與GPS的速度差

由式(10)～(13)可獲得測量矩陣

根據(jù)以上方程構(gòu)建模型后，對狀態(tài)模型和測量模型進(jìn)行離散化,以實現(xiàn)由k-1步的狀態(tài)值估計第k步的狀態(tài)值.將式(1)和(2)離散化之后得

(14)

式中：φk-1=eFτ(τ為IMU采樣時間間隔)；Hk=H；Q為系統(tǒng)噪聲協(xié)方差矩陣，由文獻(xiàn)[16]所述方法計算得到.

2 延時量的估計

圖1簡要描述了GPS/INS組合導(dǎo)航系統(tǒng)中傳感器采樣過程，Tl為GPS采樣時間間隔，Ts為INS輸出導(dǎo)航時間間隔.根據(jù)GPS接收機延時問題的研究[12]，接收機輸出數(shù)據(jù)的延時大約為數(shù)百毫秒，文獻(xiàn)[11]測試了u-blox LEA-6T接收機，延時值為150 ms左右，造成的影響不容忽視.

圖1 考慮時延的數(shù)據(jù)融合Fig.1 Data fusion considering the delay

文獻(xiàn)[14]從運動學(xué)角度推導(dǎo)了GPS的位置測量值與延時量之間的關(guān)系，并考慮GPS延時影響，對GPS/INS松耦合組合導(dǎo)航的狀態(tài)模型和測量模型進(jìn)行相應(yīng)修改.本文在文獻(xiàn)[14]的基礎(chǔ)上，繼續(xù)推導(dǎo)了GPS速度測量值與GPS延時量之間的關(guān)系.

(15)

將δTd作為擴展?fàn)顟B(tài)進(jìn)行估計后，式(14)中的狀態(tài)方程應(yīng)改寫為

(16)

進(jìn)行誤差狀態(tài)矢量擴展之后，相應(yīng)測量方程的測量矩陣也應(yīng)發(fā)生改變.時延會對GPS速度和位置測量值的準(zhǔn)確性產(chǎn)生影響，如圖1所示，GPS在t時刻的采樣實際對應(yīng)著t-Td時刻的GPS測量值.假設(shè)車輛在t時刻的真實速度函數(shù)和位置函數(shù)為v(t)和p(t)(為了表達(dá)簡潔，這里省去了上下標(biāo))，則t-Td時刻對應(yīng)的真實速度和位置為v(t-Td)和p(t-Td).t時刻GPS對速度和位置的測量值為vG(t)和pG(t)，測量誤差分別為ξ和N.由式(15)及以上表述，t時刻GPS的測量值可以表示為

在測量方程中，將INS與GPS的位置差及速度差作為觀測量，根據(jù)式(17)～(20)對延時狀態(tài)下INS和GPS觀測量的表達(dá)，繼續(xù)將速度差和位置差作為觀測量，可以得到擴展?fàn)顟B(tài)后的測量矩陣應(yīng)為

3 基于殘差重構(gòu)的延時補償

計算偽觀測值是進(jìn)行延時補償?shù)囊环N重要方法，在Kalman濾波過程中，根據(jù)測量值計算得到的殘差進(jìn)行測量更新，因此偽觀測值可用偽殘差值來代替.本文根據(jù)文獻(xiàn)[10]對延時問題和殘差重構(gòu)方法的研究，設(shè)計了針對GPS/INS組合導(dǎo)航系統(tǒng)的延時補償算法，利用偽殘差值來彌補延遲的GPS測量值.

測量殘差方程和估計誤差方程是推導(dǎo)殘差傳播方程的兩個基本方程，

測量殘差：

(21)

估計誤差：

(22)

(I-Kk+jH)φk+j-1ek+j-1

(23)

式中：Kk+j為Kalman濾波中的增益矩陣；I為計算過程中產(chǎn)生的單位矩陣.測量殘差可以由估計誤差得到

(24)

將式(23)不斷循環(huán)代入式(24)，即可獲得殘差傳播方程

εk+j=Hφk+j-1ek+j-1=

Hφk+j-1(I-Kk+j-1H)φk+j-2…

φk(I-KkH)φk-1ek-1=

Hφk+j-1(I-Kk+j-1H)φk+j-2…

φk(I-KkH)H-1Hφk-1ek-1=

Hφk+j-1(I-Kk+j-1H)φk+j-2…

φk(I-KkH)H-1εk=

(25)

式中：

Ak+m=Hφm(I-KmH)HT(HHT)-1

若由慣性導(dǎo)航方程計算并輸出導(dǎo)航解的周期為Ts，而GPS輸出測量值的周期為Tl，則由延時引起的采樣步數(shù)誤差為kd=Td/Ts.假設(shè)當(dāng)INS運行到第k步時，GPS接收機輸出了測量值，則殘差重構(gòu)算法可以按如下方式運行.

(1)將GPS輸出的測量值與存儲的第k-kd步INS數(shù)據(jù)進(jìn)行融合，并計算獲得第k-kd步的殘差：

(2)計算殘差傳播方程，將第k-kd步的殘差向第k步傳播，獲得當(dāng)前時刻的偽殘差值：

(3)計算當(dāng)前時刻的Kalman增益，根據(jù)計算得到的測量殘差，通過測量更新獲得誤差狀態(tài)矢量，將誤差狀態(tài)反饋得到當(dāng)前時刻導(dǎo)航解.當(dāng)GPS再次輸出測量值時返回第(1)步.

運行完第(1)和第(2)步后完成了偽殘差值的重構(gòu)，運行完第(3)步后完成基于偽殘差值的更新，從而完成了整個延時補償過程.具有延時估計和補償功能的GPS/INS組合導(dǎo)航卡爾曼濾波器設(shè)計可以參考圖2.

圖2 具有延時補償功能的組合導(dǎo)航系統(tǒng)設(shè)計Fig.2 Design of integrated navigation system with delay compensation

4 試驗結(jié)果分析

數(shù)據(jù)來源為西班牙馬拉加大學(xué)MAPIR實驗室采集的數(shù)據(jù)集，該數(shù)據(jù)集主要用于研究車輛運行中的多種方式定位導(dǎo)航，具體的數(shù)據(jù)說明文檔可以參考文獻(xiàn)[17].該數(shù)據(jù)庫不僅提供了完整的采集數(shù)據(jù)，還為開發(fā)者提供了軌跡真值，因此常被用于定位研究.本次實驗選取的IMU數(shù)據(jù)來源于Xsens公司生產(chǎn)的MTi系列慣性測量單元，采樣頻率為100 Hz，GPS數(shù)據(jù)由一個消費級的GPS接收機采集，采樣頻率為1 Hz，所有采集數(shù)據(jù)均經(jīng)過精確時間同步.車輛在馬加拉大學(xué)校園內(nèi)進(jìn)行數(shù)據(jù)采集工作，圖3展示了車輛的運行軌跡.

圖3 車輛運動軌跡及東向速度變化Fig.3 Vehicle trajectory and eastward velocity

由于整個過程中車輛大致向東運動，因此對車輛的東向速度進(jìn)行分析比北向更具代表性和價值.由圖3中東向速度可看出車輛是由靜止開始啟動的，中間出現(xiàn)數(shù)次加減速和一次大的轉(zhuǎn)向，多種工況的存在滿足了實驗所需條件.

4.1 GPS延時產(chǎn)生的影響分析

為研究GPS延時對導(dǎo)航效果的影響，本實驗特意給原本經(jīng)過校準(zhǔn)的GPS數(shù)據(jù)分別添加了100，200，300 ms的延時偏差.圖4為加入時延偏差后GPS/INS組合導(dǎo)航系統(tǒng)測得東向速度誤差(Δx)和東向位置誤差(ΔL)及相應(yīng)方均根誤差eΔx和eΔL的變化情況.

圖4 不同延時條件下的定速定位誤差Fig.4 Velocity and positioning error with different time delays

由圖4可見，在速度變化劇烈處(即110 s左右)，速度誤差及均方根誤差均表現(xiàn)出明顯增加，而位置誤差表現(xiàn)出明顯的速度相關(guān)性，車輛速度越大，相應(yīng)的位置誤差也會越大，位置均方根誤差隨著時間的增加逐漸趨于平穩(wěn)；隨著延時時間的增加，車輛的東向速度、位置誤差和均方根誤差均出現(xiàn)了明顯增加.

4.2 延時估計與補償算法的有效性驗證

為了驗證延時誤差估計算法的準(zhǔn)確性，應(yīng)用該算法對上述加有延時偏差的GPS數(shù)據(jù)進(jìn)行延時估計，對于不同延時值的估計結(jié)果如表1所示，估計誤差均在5 ms內(nèi)，因此延時估計算法可以較好地估計出GPS數(shù)據(jù)的延時值.

為了驗證殘差重構(gòu)算法能否較好地實現(xiàn)延時補償，當(dāng)具有200 ms延時偏差的GPS數(shù)據(jù)與INS數(shù)據(jù)進(jìn)行融合時，利用延時估計和殘差重構(gòu)算法進(jìn)行延時估計和補償.作為對比，對于具有100，300 ms延時誤差的GPS數(shù)據(jù)不作任何處理.

表1 延時估計值Tab.1 Estimation of the time delay

圖5描述了對200 ms延時誤差進(jìn)行補償后的速度和位置誤差及均方根誤差的變化.對比圖4和5，有200 ms延時的GPS數(shù)據(jù)與INS數(shù)據(jù)融合時，用殘差重構(gòu)的方式進(jìn)行了延時補償后，其位置、速度的誤差和均方根誤差有了明顯減小.由圖5可見，對200 ms延時量數(shù)據(jù)進(jìn)行延時補償后，其定位定速誤差甚至比未進(jìn)行補償?shù)?00 ms延時量數(shù)據(jù)表現(xiàn)得更好.其中，速度誤差及均方根誤差降低明顯，已接近0，位置誤差及均方根誤差在50 s后也逐漸減小并趨于平穩(wěn).因此，該算法對GPS/INS組合導(dǎo)航的延時估計和補償有著顯著效果.

圖5 對200 ms延時進(jìn)行補償后的定速定位誤差Fig.5 Velocity and positioning errors after delay compensation in 200 ms delay case

5 結(jié)語

數(shù)據(jù)處理過程會給GPS數(shù)據(jù)引入較大時延，這對導(dǎo)航精度造成不利影響，本文通過分析GPS時延給組合導(dǎo)航系統(tǒng)定位和定速引入的誤差，提出了一種延時估計和補償算法，并對傳統(tǒng)的GPS/INS松耦合組合導(dǎo)航濾波器進(jìn)行修改.結(jié)果表明，該算法可以較好地估計GPS延時值，并有效地減小組合導(dǎo)航中由時延引入的誤差.本文算法對實際工程中處理GPS/INS組合導(dǎo)航系統(tǒng)或其他多傳感器融合的延時問題具有指導(dǎo)意義.