付廷強(qiáng)，馬太原，王亞飛，殷承良

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

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

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

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

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

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

構(gòu)建GPS/INS組合導(dǎo)航模型是完成位置解算至關(guān)重要的一步，組合導(dǎo)航的數(shù)學(xué)模型包括狀態(tài)模型和測(cè)量模型.本文在構(gòu)建模型時(shí)采用的導(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)

測(cè)量模型為

y=Hx+V

(2)

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

1.1 狀態(tài)模型

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

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

(3)

式中：

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

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

(4)

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

(5)

(6)

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

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

(7)

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

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

1.2 測(cè)量模型

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

(1)INS與GPS的位置差

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

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

(14)

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

2 延時(shí)量的估計(jì)

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

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

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

(15)

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

(16)

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

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

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

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

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

測(cè)量殘差：

(21)

估計(jì)誤差：

(22)

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

(23)

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

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

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

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

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

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

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

4 試驗(yàn)結(jié)果分析

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

圖3 車輛運(yùn)動(dòng)軌跡及東向速度變化Fig.3 Vehicle trajectory and eastward velocity

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

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

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

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

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

4.2 延時(shí)估計(jì)與補(bǔ)償算法的有效性驗(yàn)證

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

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

表1 延時(shí)估計(jì)值Tab.1 Estimation of the time delay

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

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

5 結(jié)語(yǔ)

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