基于GNSS/SINS/雙目視覺里程計的車載導(dǎo)航系統(tǒng)分析與設(shè)計

馮黎 郭承軍

（電子科技大學(xué)，電子科學(xué)技術(shù)研究院，成都 611731）

主題詞：多傳感器融合 無人車 雙目視覺里程計 冗余 相關(guān)性未知 協(xié)方差交叉 分散式架構(gòu)

1 前言

捷聯(lián)式慣性導(dǎo)航系統(tǒng)（Strap-down Inertial Navigation System，SINS）具有短期定位精度與數(shù)據(jù)采樣頻率較高的特點，可以與全球?qū)Ш叫l(wèi)星系統(tǒng)（Global Navigation Satellite System，GNSS）形成組合導(dǎo)航系統(tǒng)。但基于GNSS 和慣性測量單元（Inertial Measurement Unit，IMU）的傳感器融合定位仍達不到無人車高精度定位的要求。除了GNSS 和IMU，無人車一般還配備有激光雷達、相機和微波雷達等多個傳感器，通過冗余配置可有效提高導(dǎo)航信息的準(zhǔn)確性和可靠性，如何利用多個傳感器資源，進行無人車多傳感器融合導(dǎo)航定位是目前研究的熱點。文獻[1]實現(xiàn)了基于GPS/航位推算（Dead Reckoning，DR）/視覺的無人車導(dǎo)航，當(dāng)GPS信號受到遮擋時，采用視覺導(dǎo)航，當(dāng)GPS 信號正常時，采用GPS/DR組合導(dǎo)航。Suhr 等基于粒子濾波器融合數(shù)字地圖以及GNSS、IMU、輪速傳感器和單個前置攝像頭的輸出，實現(xiàn)城市環(huán)境下的高精度自定位[2]。

現(xiàn)有的方案沒有考慮多個傳感器數(shù)據(jù)的相關(guān)性，例如，安裝在同一車輛上的一組導(dǎo)航傳感器的觀測噪聲可能由于車輛運動的變化而彼此相關(guān)，但現(xiàn)有研究尚未識別出這種相關(guān)性。在無人車即時定位與地圖構(gòu)建（Simultaneous Localization And Mapping，SLAM）中，車輛估計其環(huán)境中的地標(biāo)位置時，同時使用這些地標(biāo)更新其自身的位置估計，車輛和地標(biāo)的位置估計變得高度相關(guān)[3]。此外，現(xiàn)有的方案也沒有可以擴展多個傳感器的無人車多傳感器融合架構(gòu)，這一架構(gòu)應(yīng)該是分散式的[4]，單個節(jié)點的丟失或錯誤不會妨礙整體運行，并且可以即插即用地配置傳感器節(jié)點。同時，需考慮各節(jié)點的相關(guān)性，基于各節(jié)點的融合結(jié)果應(yīng)具有一致性[5]。

針對上述問題，本文設(shè)計基于SINS、GNSS 和雙目視覺里程計的無人車分散式融合架構(gòu)，并在此基礎(chǔ)上，根據(jù)節(jié)點信息的相關(guān)性，在卡爾曼（kalman）濾波框架下，采用相應(yīng)的協(xié)方差交叉算法融合其他節(jié)點信息，并基于KITTI數(shù)據(jù)集進行仿真分析。

2 無人車分散式融合架構(gòu)算法

2.1 卡爾曼濾波算法與協(xié)方差交叉算法

系統(tǒng)狀態(tài)空間模型為：

式中，Xk為系統(tǒng)在k時刻的狀態(tài)；Zk為對應(yīng)狀態(tài)的觀測值；Wk為輸入的白噪聲；Vk為觀測噪聲；A為狀態(tài)轉(zhuǎn)移矩陣；B為噪聲驅(qū)動矩陣；H為觀測矩陣。

卡爾曼濾波器的操作包括兩個階段：時間更新與測量更新。時間更新方程與測量更新方程分別為[6]：

式中，Q為系統(tǒng)噪聲方差陣；R為測量噪聲方差陣；I為適維單位矩陣；Pk為Xk的協(xié)方差矩陣；X?k為狀態(tài)估計值；Kk為增益矩陣。

考慮一類非常普遍的數(shù)據(jù)融合問題：將2條數(shù)據(jù)信息A和B融合，得到輸出C，并且A和B真實統(tǒng)計數(shù)據(jù)及互協(xié)方差未知，唯一可用的信息是A和B的均值和協(xié)方差的估計。對此，Simon Julier和Jeffery Uhlmann 提出了避免計算未知的互協(xié)方差的協(xié)方差交叉（Covariance Intersection，CI）算法。協(xié)方差交叉融合給出融合估計實際方差的一個上界，即保證融合估計比經(jīng)典Kalman方法更具有一致性和魯棒性，提高了融合的精度，避免融合估值器的發(fā)散[7]。

相關(guān)性已知的相關(guān)估計量最優(yōu)融合為[8]：

相關(guān)性未知的相關(guān)估計量最優(yōu)融合是協(xié)方差的凸組合：

式中，a、b和c分別為A、B和C的均值；Paa、Pbb和Pcc分別為A、B和C的協(xié)方差；Pab和Pba為A、B的互協(xié)方差；ω∈[0,1]。

2.2 無人車分散式融合架構(gòu)算法

以SINS為參考系統(tǒng)，分別與GNSS和視覺里程計構(gòu)成節(jié)點，節(jié)點間通過通信鏈路相連，每個節(jié)點設(shè)計1個局部濾波器和主濾波器，具體結(jié)構(gòu)如圖1所示。局部卡爾曼濾波器1采用SINS與GNSS的位置和速度之差作為觀測值，局部卡爾曼濾波器2采用SINS與視覺里程計解算的位置和速度作為觀測值，使用標(biāo)準(zhǔn)的卡爾曼濾波器得到估計值并傳播到其他節(jié)點。對于每個節(jié)點的主濾波器，在卡爾曼濾波框架下，根據(jù)信息的相關(guān)性進行CI融合，再將融合結(jié)果進行觀測更新，進而求得最優(yōu)估計。

圖1 多傳感器融合架構(gòu)

考慮節(jié)點1從k時刻到(k+1)時刻的融合算法，其融合過程為：節(jié)點2主濾波器得到k時刻節(jié)點2的狀態(tài)估計和協(xié)方差；節(jié)點2的局部濾波器更新得到(k+1)時刻的狀態(tài)估計和協(xié)方差，將其傳播至節(jié)點1；節(jié)點1主濾波器使用卡爾曼濾波器預(yù)測方程得到狀態(tài)估計和協(xié)方差；由于上2個步驟的狀態(tài)量使用同一個SINS的輸出值，具有相關(guān)性，選用相關(guān)性未知的CI算法融合上2個步驟的狀態(tài)估計和協(xié)方差（否則選用相關(guān)性已知的CI算法）；根據(jù)節(jié)點1的GNSS的觀測值使用卡爾曼更新方程對上一步的結(jié)果進行更新，得到節(jié)點1在(k+1)時刻的狀態(tài)估計。

由上述過程可知，每個節(jié)點先將其他節(jié)點信息與本地預(yù)測進行CI融合，再對融合結(jié)果進行觀測更新，這樣保證了每個節(jié)點都利用了全局的信息，由此得到全局的估計。因此，本文設(shè)計的多傳感器分散式融合架構(gòu)中，主濾波器1和主濾波器2均為系統(tǒng)的濾波輸出，這種濾波結(jié)構(gòu)相比于集中式濾波有更高的靈活性和容錯能力。

3 狀態(tài)模型與觀測模型

3.1 雙目視覺里程計導(dǎo)航解算

傳統(tǒng)的里程計在不光滑的表面運動時，結(jié)果往往不可靠，且誤差隨時間的累積逐漸增大。視覺里程計通過對圖像特征點提取、匹配與跟蹤，進而估計出運載體的位姿信息的過程不受道路表面的影響，并且具有更高的靈活性和精度。視覺里程計可以分為單目視覺里程計、雙目視覺里程計和多目視覺里程計。單目視覺里程計計算相對簡單，但其缺少絕對尺度信息。雙目視覺里程計利用不同位置的2個攝像機模擬人類的雙眼，可以計算出圖像中物體的三維信息。普遍采用的雙目視覺里程計計算流程如圖2所示。

圖2 雙目視覺里程計計算流程

基于雙目視覺里程計[9]解算位置和速度的流程為：

a.獲取圖像對Ik、Ik+1，并對圖像進行畸變校準(zhǔn)。

b.計算時刻k和(k+1)的視差圖。

c.使用加速分割測試特征（Features from Accelerated Segment Test，F(xiàn)AST）算法檢測圖像的特征并進行特征匹配。

d.選取使得所有匹配均互相兼容的點云數(shù)據(jù)（上一步中匹配特征點的3D坐標(biāo)值）。

e.為了確定旋轉(zhuǎn)矩陣R和平移向量t，使用列文伯格-馬夸爾特（Levenberg-Marquardt，L-M）非線性優(yōu)化方法最小化重投影誤差ε：

式中，fk、fk+1分別為k時刻和(k+1)時刻左圖像中的特征；jk、jk+1分別為fk、fk+1的二維齊次坐標(biāo)；wk、wk+1分別為fk、fk+1的三維齊次坐標(biāo)；P為3×4左攝像機投影矩陣；T為4×4齊次變換矩陣；ε為重投影誤差。

f.雙目視覺姿態(tài)估計、位置估計、速度估計分別可以表示為[10]：

3.2 SINS、GNSS及雙目視覺里程計多源融合模型

慣性導(dǎo)航系統(tǒng)誤差方程由平臺誤差角方程、位置和速度誤差方程及陀螺儀和加速度計誤差方程組成。綜合這些誤差方程，可以得到SINS的狀態(tài)方程：

式中，W=[wgx wgy wgz wbx wby wbz wax way waz]T為噪聲矢量；wgx、wgy、wgz為陀螺儀白噪聲；wbx、wby、wbz為陀螺儀隨機常值漂移噪聲；wax、way、waz為加速度計隨機噪聲；F為狀態(tài)轉(zhuǎn)移矩陣，其選取參考文獻[11]；G為誤差系數(shù)矩陣，其選取參考文獻[11]；ΨE、ΨN、ΨU分別為平臺東向、北向、天向的誤差角；ζvE、ζvN、ζvU分別為載體在東向、北向、天向的速度誤差；ζL、ζλ、ζη分別為緯度、經(jīng)度和高度誤差；εbx、εby、εbz分別為陀螺儀的隨機常數(shù)漂移；εrx、εry、εrz為馬爾科夫漂移；Δx、Δy、Δz為加速度一階馬爾科夫漂移。

取SINS 與GNSS 輸出的速度和位置之差作為量測量，得到SINS與GNSS組合的量測方程：

式中，V1(t)為GNSS量測的位置和速度誤差；Zp1為位置量測矢量；Zv1為速度量測矢量；觀測矩陣H1為：

式中，RM為量測點沿北方向的曲率半徑；RN為量測點沿東西方向的曲率半徑；L為量測點的緯度。

取視覺里程計解算位置、速度與SINS 輸出的位置和速度之差作為量測量，量測方程為：

式中，V2(t)為雙目視覺里程計量測的位置和速度誤差；Zp2為位置量測矢量；Zv2為速度量測矢量；觀測矩陣H2為：

4 仿真驗證分析

KITTI 數(shù)據(jù)集由德國卡爾斯魯厄理工學(xué)院和豐田美國技術(shù)研究院聯(lián)合創(chuàng)辦，是目前國際上最大的自動駕駛場景下的計算機視覺算法評測數(shù)據(jù)集。KITTI 的原始數(shù)據(jù)（Raw Data）提供以10 Hz 同步的GPS、IMU 數(shù)據(jù)（位置、速度、加速度等）和雙目圖像序列，各傳感器坐標(biāo)系如圖3所示。

圖3 KITTI車載傳感器安裝示意[12]

本文選用數(shù)據(jù)集住宅集中道路中的一段數(shù)據(jù)進行驗證，該組數(shù)據(jù)中含有803幀圖像，分辨率為1242像素×375 像素。以KITTI 數(shù)據(jù)集提供的GPS 經(jīng)緯度為真值，分別在經(jīng)度和緯度的真值上加入標(biāo)準(zhǔn)差為5 m的白噪聲作為GPS量測位置值。在KITTI數(shù)據(jù)集提供的東向速度和北向速度各加入標(biāo)準(zhǔn)差為0.1 m/s 的白噪聲作為GPS量測的速度值。圖4所示為該組數(shù)據(jù)的真實軌跡。

圖4 數(shù)據(jù)集中自主車的真實運動軌跡

4.1 雙目視覺里程計驗證

首先進行雙目視覺里程計的解算，圖像序列左、右2個相機各有803幀圖像。圖5給出了雙目視覺里程計得到的內(nèi)點數(shù)的分布圖，由圖5可知，絕大多數(shù)分圖像均能得到50個以上內(nèi)點，雙目視覺里程計具有良好的性能。

4.2 無人車分散式融合架構(gòu)算法驗證

為了驗證本文設(shè)計的基于GNSS、SINS 和雙目視覺里程計的無人車多傳感器融合架構(gòu)算法，本文將該算法與慣性導(dǎo)航解算算法和視覺里程計解算算法得到的位置和速度進行比較。慣性導(dǎo)航解算算法將前一時刻的導(dǎo)航參數(shù)（姿態(tài)、速度和位置）作為初值，利用前一時刻至當(dāng)前時刻的慣性器件采樣輸出，解算當(dāng)前時刻的導(dǎo)航參數(shù)，作為下一時刻慣導(dǎo)解算的初值，如此反復(fù)不斷。本文的融合算法由于采用分散式的架構(gòu)，節(jié)點1（GNSS與SINS融合）與節(jié)點2（視覺里程計與SINS融合）均會得到全局的融合結(jié)果，這里取節(jié)點1的結(jié)果進行對比。圖6～圖8所示分別為真實軌跡的經(jīng)緯度、真實的速度，圖9～圖10為各種解算方式的東向和北向的位置和速度誤差。

圖5 雙目視覺里程計得到的內(nèi)點數(shù)的分布結(jié)果

圖6 運動軌跡

圖7 真實東向運動速度

由圖9可以看出，基于慣性導(dǎo)航解算和雙目視覺里程計解算的軌跡在運動起始的一段時間內(nèi)偏差較小，但是隨著運動的繼續(xù)，偏差逐漸增大。由圖10可以看出，在有一定數(shù)量的特征點時，雙目視覺里程計解算的速度具有較高的精度，在第53 s左右由于特征點不足產(chǎn)生了較大誤差，這是視覺里程計普遍存在的問題。因此，單一的導(dǎo)航系統(tǒng)難以為無人車提供穩(wěn)定、高精度的導(dǎo)航。

圖8 真實北向運動速度

圖9 位置誤差

圖10 速度誤差

根據(jù)圖9 和圖10 的誤差曲線，表1 統(tǒng)計了SINS 解算、雙目視覺里程計解算和本文融合算法解算的最大誤差對比。表2給出了節(jié)點1和節(jié)點2的位置和速度的均方根誤差，可見2 個節(jié)點都具有良好的性能，能得到相似的全局估計，也驗證了分散式融合架構(gòu)的有效性。

從上述仿真結(jié)果可以看出，基于GNSS、SINS 和雙目視覺里程計的無人車多傳感器融合架構(gòu)算法解算的導(dǎo)航信息更加精確，并具有更好的魯棒性。

表1 各種解算方法最大誤差對比

表2 節(jié)點估計均方根誤差

5 結(jié)束語

本文實現(xiàn)了雙目視覺里程計的位置和速度解算，利用雙目視覺里程計和GNSS量測的位置和速度，推導(dǎo)了SINS、GNSS和雙目視覺里程計的多源融合模型，并設(shè)計了分散式融合架構(gòu)算法。同時，針對分散式融合節(jié)點信息的相關(guān)性未知的問題，在卡爾曼濾波框架下，采用協(xié)方差交叉算法融合其他節(jié)點信息，提升導(dǎo)航系統(tǒng)的魯棒性。在基于KITTI數(shù)據(jù)集上的驗證結(jié)果表明，本文設(shè)計的分散式融合架構(gòu)算法具有更高的導(dǎo)航精度和魯棒性。