基于圖優(yōu)化的智能車輛多傳感器融合定位方法

摘要：為了提升車輛定位系統(tǒng)的精度和魯棒性，針對單一傳感器存在的局限性，提出了一種基于圖優(yōu)化的LIDAR（Light Detection and Ranging，LIDAR）、IMU（Inertial Measurement Unit）和GNSS-RTK（Global Navigation Satellite" System-Real-Time Kinematic）的多傳感器車輛定位方法。首先，使用IMU預(yù)積分模型，通過滑動窗口和掃描匹配的方法構(gòu)建LIDAR里程計因子，加入GNSS-RTK絕對測量值以修正系統(tǒng)的長期漂移;其次，使用因子圖優(yōu)化框架將LIDAR、IMU和GNSS-RTK的測量數(shù)據(jù)進(jìn)行融合，并加入回環(huán)檢測因子，通過求解最大后驗估計以獲取最佳的定位結(jié)果。實驗結(jié)果顯示，所提出方法的相對平移誤差低至0.34m，具有較高的準(zhǔn)確性和魯棒性，彌補(bǔ)了單傳感器的不足，提高了車輛定位系統(tǒng)的定位精度。

關(guān)鍵詞：圖優(yōu)化;多傳感器融合;智能車輛;定位;回環(huán)檢測

中圖分類號：TP242 文獻(xiàn)標(biāo)志碼：A

0 引言（Introduction）

定位與導(dǎo)航是自動駕駛領(lǐng)域的核心技術(shù)之一，也是實現(xiàn)自動駕駛汽車的基礎(chǔ)和前提。定位技術(shù)的準(zhǔn)確性和可靠性，直接影響到自動駕駛汽車的安全性和整體運行性能。

當(dāng)前，有多種可為智能車輛提供定位信息的傳感器，包括全球?qū)Ш叫l(wèi)星系統(tǒng)（Global Navigation Satellite" System，GNSS）[1-2]、實時動態(tài)（Real-Time Kinematic，RTK）定位技術(shù)、基于慣性測量單元（Inertial Measurement" Unit，IMU）的航位推算慣性導(dǎo)航技術(shù)[3-5]、基于激光雷達(dá)（Light Detection and Ranging，LIDAR）點云的定位方法等。每種傳感器都有其獨特的優(yōu)勢，但同時也存在一定的局限性。為了充分發(fā)揮各類傳感器的長處并彌補(bǔ)其不足，采用多傳感器融合定位技術(shù)成為提高系統(tǒng)定位魯棒性的有效途徑[6-7]。

常見的多傳感器融合定位方法主要有卡爾曼濾波、粒子濾波和圖優(yōu)化等。卡爾曼濾波通過預(yù)測和更新兩個步驟遞推地計算系統(tǒng)的最優(yōu)狀態(tài)估計，但是它在處理非線性系統(tǒng)時效果不佳[8]。粒子濾波適用于無法用解析形式表達(dá)的非線性系統(tǒng)，但它需要大量的粒子來確保估計的準(zhǔn)確性，因此計算量較大[9]。圖優(yōu)化通過優(yōu)化一個能量函數(shù)，可以找到滿足所有約束的最優(yōu)狀態(tài)估計[10]。這種方法適用于處理大規(guī)模的傳感器網(wǎng)絡(luò)和多傳感器數(shù)據(jù)融合問題，是一種有效的多傳感器融合定位方法。

因此，本文提出一種基于圖優(yōu)化的LIDAR、IMU和GNSSRTK的多傳感器智能車輛定位方法，該方法融合了多種傳感器的信息，為智能車輛提供了精確的定位結(jié)果。

1 系統(tǒng)整體設(shè)計（Overall system design）

多傳感器融合定位系統(tǒng)框架圖如圖1所示。該系統(tǒng)由GNSS-RTK、IMU、LIDAR3種傳感器提供車輛行駛數(shù)據(jù)。首先，對GNSS-RTK和IMU的數(shù)據(jù)做初步的降噪處理。其次，對IMU數(shù)據(jù)進(jìn)行機(jī)械編排，并構(gòu)建IMU 預(yù)積分因子。IMU 獲得的初始位姿信息被用于LIDAR的初始化。在LIDAR數(shù)據(jù)方面，先進(jìn)行點云去畸變，再進(jìn)行特征提取和關(guān)鍵幀選擇。再次，通過滑動窗口和掃描匹配構(gòu)建LIDAR里程計因子，使用平滑后的GNSS-RTK數(shù)據(jù)計算GNSS-RTK因子殘差。最后，將LIDAR、IMU、GNSS-RTK測量數(shù)據(jù)以及計算結(jié)果通過因子圖優(yōu)化框架融合，實現(xiàn)最大后驗概率（Maximum A Posteriori，MAP）估計，獲取最終估計的定位結(jié)果。

2 因子圖優(yōu)化（Factor graph optimization）

在得到所有的測量量之后，可以使用因子圖優(yōu)化將所有的測量量融合后實現(xiàn)MAP估計，獲取估計的最佳定位結(jié)果。

3 實驗驗證（Experimental verification）

為了驗證多傳感器融合定位算法的有效性，分別采用了公開數(shù)據(jù)集和實車實驗數(shù)據(jù)對所提算法進(jìn)行測試。程序?qū)崿F(xiàn)方面，基于Ubuntu18.04 Robot Operating System（ROS）框架，使用C++語言實現(xiàn)。實驗結(jié)果表明，所提出的方法能夠滿足車輛實時應(yīng)用需求。

3.1 公共數(shù)據(jù)集驗證

為了驗證所提出方法的準(zhǔn)確性和魯棒性，本文與使用迭代誤差狀態(tài)卡爾曼濾波算法的LINS（Lidar-Inertial State" Estimator）方法進(jìn)行對比實驗。實驗使用KITTI（Karlsruhe Institute of Technology and Toyota Technological" Institute）數(shù)據(jù)集[11]評估所提出方法的性能。數(shù)據(jù)采集車輛配備的傳感器包括Velodyne64線激光雷達(dá)，OXTSRT3003組合導(dǎo)航系統(tǒng)，包含GNSS-RTK和IMU傳感器。采用odometry 08序列進(jìn)行評估，該數(shù)據(jù)集軌跡長度為3222.795m，用時為422s。公共數(shù)據(jù)集點云圖如圖2所示，該數(shù)據(jù)集由車輛在復(fù)雜的道路環(huán)境中采集得到，平均車速為7.6m/s。由于雙天線的GNSS-RTK 信號全程可用且信號質(zhì)量良好，因此將GNSS-RTK數(shù)據(jù)作為地面真值，計算LINS和所提算法的絕對軌跡誤差（Absolute Trajectory Error，ATE）和每隔一定距離的相對平移誤差（Relative Translation Error，RTE），公共數(shù)據(jù)集上各種方法的定位軌跡如圖3所示，兩種方法在公共數(shù)據(jù)集上獲得的絕對軌跡誤差和相對平移誤差如表1所示。觀察圖3和表1發(fā)現(xiàn)，本文提出的方法在車速較高的復(fù)雜環(huán)境下依舊表現(xiàn)出不錯的性能，其ATE和RTE分別為7.77m和2.52m，LINS具有較為明顯的漂移，其ATE和RTE分別為9.55m和3.43m。可見，本文提出的方法在長期行駛過程中仍能保持較高的魯棒性和準(zhǔn)確性。

3.2 實車驗證

為了進(jìn)一步驗證所提出方法的實際使用性能，使用智能小車進(jìn)行實驗驗證，所有的方法均在實驗車輛上實時運行，實驗車輛的配置如圖4 所示，包括Velodyne16 線激光雷達(dá)、FDISYSTMES DETA100工業(yè)級IMU和FDISYSTMES雙天線GNSS-RTK，其中IMU已通過Allan方差測量白噪聲和隨機(jī)游走噪聲，采用車載計算機(jī)（Nuvo-8108GC）實時記錄和計算傳感器數(shù)據(jù)并生成定位數(shù)據(jù)，車輛的平均速度為2m/s。

實驗選取場景在具有較大的高度差的教學(xué)樓附近進(jìn)行，該區(qū)域緊鄰主干道，因此場景中的行人及過往的車輛，增加了實驗的復(fù)雜性。本次實驗軌跡全長627m，耗時219s，對整個定位系統(tǒng)提出了嚴(yán)峻的挑戰(zhàn)。實車實驗點云圖如圖5所示，地面的點云灰度變化（由深變淺）顯示了該實驗場景較大的海拔高度差，同時該實驗場景包含多處地面減速帶。實車實驗各種方法的定位軌跡如圖6所示，兩種方法相對于GNSS-RTK 的ATE和RTE值以及在返回起點時的相對平移誤差如表2所示。本文所提出的方法在這種具有挑戰(zhàn)性的場景中依舊表現(xiàn)出較高的魯棒性，LINS的漂移較高，ATE為4.34m，RTE為1.08m，在車輛返回起點時相對平移誤差ENDRTE為3.76m，并且由于實驗場景存在較大的高度差值，LINS最終沒能在縱向回到起點，相比于LINS，本文所提出方法的ATE和RTE都較小，并且在車輛返回起點時能夠產(chǎn)生相對較小的相對平移誤差。

4 結(jié)論（Conclusion）

本文提出了一種使用LIDAR、IMU和GNSS-RTK傳感器的融合定位方法。該方法對IMU 數(shù)據(jù)進(jìn)行機(jī)械編排構(gòu)建IMU預(yù)積分因子;通過滑動窗口和掃描匹配，構(gòu)建LIDAR里程計因子;使用GNSS-RTK提供的絕對測量，修正系統(tǒng)的長期漂移;使用因子圖優(yōu)化框架將LIDAR、IMU和GNSS-RTK采集的數(shù)據(jù)進(jìn)行融合，并加入回環(huán)檢測因子，以提高系統(tǒng)的準(zhǔn)確性和魯棒性。公共數(shù)據(jù)集和實車實驗驗證了所提出的方法在各種具有挑戰(zhàn)性的環(huán)境下的定位性能。實驗結(jié)果表明，所提出的方法在多種環(huán)境中均具有較高的準(zhǔn)確性和魯棒性，可以為實際車輛提供定位、導(dǎo)航幫助。

作者簡介：張 偉（1986-），男（漢族），常州，高級工程師，碩士。研究領(lǐng)域：場車環(huán)境感知，機(jī)電類特種設(shè)備智能化與安全。

李旭東（1998-），男（漢族），徐州，本科。研究領(lǐng)域：智能汽車環(huán)境感知，導(dǎo)航與定位。

曹偉（1992-），男（漢族），蘇州，工程師，本科。研究領(lǐng)域：場車環(huán)境感知，機(jī)電類特種設(shè)備智能化與安全。

趙奉奎（1986-），男（漢族），濟(jì)寧，講師，博士。研究領(lǐng)域：智能汽車環(huán)境感知。