GPS/INS延時估計與基于殘差重構的延時補償算法

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

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

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

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

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

區別于以上方法，本文的特色在于直接對卡爾曼濾波器進行適應性修改，利用殘差重構的方式實現延時補償.首先構建車輛的GPS/INS松耦合組合導航模型，然后根據運動學關系對模型做了一定的修改并實現延時估計，最后將殘差重構算法應用到GPS/INS組合導航濾波中實現了延時誤差補償，并通過試驗驗證了所述算法的有效性.

1 GPS/INS組合導航模型

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

狀態模型為

(1)

測量模型為

y=Hx+V

(2)

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

1.1 狀態模型

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

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

(3)

式中：

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

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

(4)

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

(5)

(6)

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

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

(7)

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

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

1.2 測量模型

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

(1)INS與GPS的位置差

式中：LI和LG為INS和GPS輸出的緯度值；λI和λG為INS和GPS輸出的經度值；Nx和Ny為GPS接收機沿著導航坐標系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)可獲得測量矩陣

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

(14)

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

2 延時量的估計

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

圖1 考慮時延的數據融合Fig.1 Data fusion considering the delay

文獻[14]從運動學角度推導了GPS的位置測量值與延時量之間的關系，并考慮GPS延時影響，對GPS/INS松耦合組合導航的狀態模型和測量模型進行相應修改.本文在文獻[14]的基礎上，繼續推導了GPS速度測量值與GPS延時量之間的關系.

(15)

將δTd作為擴展狀態進行估計后，式(14)中的狀態方程應改寫為

(16)

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

在測量方程中，將INS與GPS的位置差及速度差作為觀測量，根據式(17)～(20)對延時狀態下INS和GPS觀測量的表達，繼續將速度差和位置差作為觀測量，可以得到擴展狀態后的測量矩陣應為

3 基于殘差重構的延時補償

計算偽觀測值是進行延時補償的一種重要方法，在Kalman濾波過程中，根據測量值計算得到的殘差進行測量更新，因此偽觀測值可用偽殘差值來代替.本文根據文獻[10]對延時問題和殘差重構方法的研究，設計了針對GPS/INS組合導航系統的延時補償算法，利用偽殘差值來彌補延遲的GPS測量值.

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

測量殘差：

(21)

估計誤差：

(22)

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

(23)

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

(24)

將式(23)不斷循環代入式(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

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

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

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

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

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

圖2 具有延時補償功能的組合導航系統設計Fig.2 Design of integrated navigation system with delay compensation

4 試驗結果分析

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

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

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

4.1 GPS延時產生的影響分析

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

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

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

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

為了驗證延時誤差估計算法的準確性，應用該算法對上述加有延時偏差的GPS數據進行延時估計，對于不同延時值的估計結果如表1所示，估計誤差均在5 ms內，因此延時估計算法可以較好地估計出GPS數據的延時值.

為了驗證殘差重構算法能否較好地實現延時補償，當具有200 ms延時偏差的GPS數據與INS數據進行融合時，利用延時估計和殘差重構算法進行延時估計和補償.作為對比，對于具有100，300 ms延時誤差的GPS數據不作任何處理.

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

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

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

5 結語

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