基于車輛模型緊耦合的封閉園區(qū)車輛定位方法*

秦曉輝，王哲文，龐 濤，史維清，孫 寧，胡滿江

（汽車車身先進(jìn)設(shè)計(jì)制造國(guó)家重點(diǎn)實(shí)驗(yàn)室，湖南大學(xué)機(jī)械與運(yùn)載工程學(xué)院，長(zhǎng)沙410082）

前言

近年來(lái)隨著自動(dòng)駕駛技術(shù)研究的不斷深入，多種應(yīng)用場(chǎng)景下的自動(dòng)駕駛解決方案應(yīng)運(yùn)而生。諸如港口、工廠園區(qū)等相對(duì)封閉區(qū)域因車輛行駛路線固定、交互環(huán)境簡(jiǎn)單等特點(diǎn)，成為了自動(dòng)駕駛技術(shù)商業(yè)化落地的首選場(chǎng)景。自動(dòng)駕駛技術(shù)包含環(huán)境感知、自主定位、決策規(guī)劃和運(yùn)動(dòng)控制等多項(xiàng)技術(shù)，其中自主定位技術(shù)是自動(dòng)駕駛車輛實(shí)現(xiàn)復(fù)雜環(huán)境下自主運(yùn)動(dòng)的重要基礎(chǔ)。

自主定位技術(shù)的實(shí)現(xiàn)主要依靠自動(dòng)駕駛車輛上安裝的外部傳感器。慣性導(dǎo)航系統(tǒng)（inertial navigation system，INS）包括全球?qū)Ш叫l(wèi)星系統(tǒng)（global navigation satellite system，GNSS）和慣性測(cè)量單元（inertial measurement unit，IMU），在開(kāi)闊環(huán)境中定位精度較高，可為自動(dòng)駕駛車輛提供全天候定位服務(wù)。然而GNSS定位依賴遠(yuǎn)距離在軌衛(wèi)星的授時(shí)信號(hào)，一旦車輛處于無(wú)線電信號(hào)強(qiáng)度不穩(wěn)定或信號(hào)丟失的環(huán)境中，例如道路兩側(cè)堆放大量集裝箱的港口、植被或建筑物密度較高的園區(qū)等，定位精度將無(wú)法保證甚至無(wú)法進(jìn)行定位。且自動(dòng)駕駛車輛常用的低成本IMU只能在極短時(shí)間內(nèi)保證載體定位精度，局限性較大。

對(duì)于GNSS信號(hào)存在遮擋場(chǎng)景中的自動(dòng)駕駛車輛自主定位，國(guó)內(nèi)外學(xué)者進(jìn)行了大量研究。在此工況下，通常有4種位姿估計(jì)方法：基于模型的方法、基于直接傳感器的方法、基于間接傳感器的方法和上述3種方法的融合［1］?；谀Ｐ偷姆椒ㄊ褂密囕v模型在靜態(tài)和動(dòng)態(tài)響應(yīng)中獲得定位結(jié)果，Ren等基于阿克曼原理分析車輛的運(yùn)動(dòng)學(xué)特征，建立考慮誤差的車輛運(yùn)動(dòng)學(xué)模型推算車輛的轉(zhuǎn)向角、航向角和轉(zhuǎn)彎半徑等運(yùn)動(dòng)狀態(tài)［2］。Xiong等結(jié)合基于模型和間接傳感器的方法，通過(guò)基于車輛動(dòng)力學(xué)模型和間接傳感器之一的IMU來(lái)完成位姿和滑移角估計(jì)［3］，但該方法中的IMU累積誤差無(wú)法被很好地消除。Xia等提出一種基于IMU的運(yùn)動(dòng)學(xué)模型，結(jié)合用于補(bǔ)償誤差累積的動(dòng)態(tài)模型并采用開(kāi)環(huán)積分的方法來(lái)估計(jì)車輛的位姿和橫向速度［4］。

隨著技術(shù)的不斷發(fā)展創(chuàng)新，攝像頭和激光雷達(dá)（light detection and ranging，LiDAR）等直接傳感器被廣泛應(yīng)用于自動(dòng)駕駛車輛的定位系統(tǒng)中，以攝像頭為傳感器進(jìn)行定位時(shí)通常對(duì)場(chǎng)景光照要求較高，在具有玻璃、白墻等特征較少的場(chǎng)景魯棒性較低［5］，而且算法構(gòu)建的地圖點(diǎn)較為稀疏，在動(dòng)態(tài)物體較多的場(chǎng)景中容易失效［6］。而LiDAR定位方法在三維紋理特征豐富的環(huán)境中具有光照不變性和較高的定位魯棒性，適用于光照受植被或密集建筑物影響較大的封閉園區(qū)環(huán)境。直接傳感器的廣泛應(yīng)用為自動(dòng)駕駛定位問(wèn)題提供了新的解決方案，融合不同類型傳感器的位姿估計(jì)方法逐漸成為研究熱點(diǎn)。融合方案大致分為兩類：松耦合和緊耦合。松耦合方法融合不同傳感器的數(shù)據(jù)處理結(jié)果以得到最終定位結(jié)果［7-8］。Zhang等提出的激光里程計(jì)使用IMU測(cè)量作為先驗(yàn)并消除點(diǎn)云畸變但并不對(duì)IMU數(shù)據(jù)進(jìn)行優(yōu)化處理，多應(yīng)用于室外自動(dòng)駕駛試驗(yàn)場(chǎng)景，累積誤差較為明顯［9］。Yang等提出了一種考慮系統(tǒng)初始化偏差的因子圖，基于松耦合原理和擴(kuò)展卡爾曼濾波器消除定位累積漂移，并重建全局一致的三維地圖［10］。孫寧等提出一種基于GPS∕radar集成的多車系統(tǒng)定位方法，先通過(guò)GPS和radar對(duì)車輛位姿進(jìn)行粗估計(jì)，再將粗估計(jì)結(jié)果通過(guò)車車通信進(jìn)行融合以得到更精確的位姿估計(jì)結(jié)果［11］。曹立波等提出一種基于GPS∕視覺(jué)融合的自動(dòng)駕駛定位方法，在車道判別的基礎(chǔ)上利用超寬帶通信技術(shù)進(jìn)行單錨點(diǎn)V2I定位［12］。然而該方法僅對(duì)靜態(tài)定位誤差進(jìn)行分析，魯棒性較低。緊耦合方法將不同傳感器測(cè)量集成后共同構(gòu)建運(yùn)動(dòng)方程和觀測(cè)方程，并進(jìn)行聯(lián)合優(yōu)化，有助于提高定位精度［13］。一個(gè)迭代誤差狀態(tài)濾波器被Qin等設(shè)計(jì)用于融合LiDAR和IMU測(cè)量，算法魯棒性較高［14］。Ye等提出了一種緊耦合3D LiDAR-IMU位姿估計(jì)算法，使用IMU進(jìn)行幀間預(yù)測(cè)輔助LiDAR配準(zhǔn)，聯(lián)合優(yōu)化各傳感器代價(jià)函數(shù)以獲得實(shí)時(shí)一致的位姿估計(jì)［15］。然而，上述兩種方法均有局限性，例如在崎嶇道路或植被較多的場(chǎng)景下，位姿的估計(jì)結(jié)果尤其是豎直方向上的平移位姿估計(jì)存在誤差累積的情況。

受上述工作啟發(fā)，本文中提出了一種基于車輛模型緊耦合的封閉園區(qū)車輛定位方法，該定位方法可良好適應(yīng)GNSS信號(hào)受遮擋的封閉園區(qū)場(chǎng)景，且不受園區(qū)中因貨物運(yùn)輸?shù)纫蛩卦斐傻沫h(huán)境變化影響，具有實(shí)時(shí)性和魯棒性。在充分利用LiDAR和IMU測(cè)量的基礎(chǔ)上輔以車輛運(yùn)動(dòng)學(xué)模型約束車輛位姿優(yōu)化方向，采用模塊化設(shè)計(jì)思路對(duì)系統(tǒng)分3個(gè)模塊進(jìn)行建模，并使用緊耦合方法進(jìn)行聯(lián)合優(yōu)化以獲得高一致性的自動(dòng)駕駛車輛位姿估計(jì)。本文主要貢獻(xiàn)如下：

（1）提出一種基于車輛模型緊耦合的LiDARIMU融合定位方法，實(shí)現(xiàn)了自動(dòng)駕駛車輛在弱GNSS信號(hào)環(huán)境下高精度的魯棒定位；

（2）利用IMU測(cè)量和系統(tǒng)狀態(tài)量為L(zhǎng)iDAR幀間配準(zhǔn)提供先驗(yàn)信息，并采用基于局部地圖的方法以提高配準(zhǔn)精度；

（3）利用系統(tǒng)狀態(tài)信息構(gòu)建車輛運(yùn)動(dòng)學(xué)模型以充分利用車輛運(yùn)動(dòng)信息約束位姿優(yōu)化方向，并預(yù)測(cè)自動(dòng)駕駛車輛的6自由度位姿；

（4）通過(guò)不同弱GNSS信號(hào)場(chǎng)景的實(shí)車試驗(yàn)對(duì)該方法進(jìn)行驗(yàn)證，其性能優(yōu)于室外定位導(dǎo)航應(yīng)用較為廣泛的A-LOAM定位方法和僅使用LiDAR-IMU緊耦合的定位方法。

1 系統(tǒng)架構(gòu)

本文中提出的基于車輛模型緊耦合的封閉園區(qū)車輛定位方法系統(tǒng)架構(gòu)如圖1所示。主要包括4個(gè)部分：LiDAR里程計(jì)、IMU里程計(jì)、車輛運(yùn)動(dòng)學(xué)模型預(yù)測(cè)方程和聯(lián)合優(yōu)化。

圖1 考慮車輛運(yùn)動(dòng)學(xué)模型約束的緊耦合車輛定位方法系統(tǒng)架構(gòu)

LiDAR測(cè)量值，即點(diǎn)云，通常會(huì)由于自動(dòng)駕駛車輛的運(yùn)動(dòng)而產(chǎn)生畸變，常用狀態(tài)估計(jì)的方法進(jìn)行糾偏。狀態(tài)估計(jì)指充分利用IMU測(cè)量值、優(yōu)化后的系統(tǒng)狀態(tài)量和IMU坐標(biāo)系與LiDAR坐標(biāo)系之間的外部參數(shù)來(lái)估計(jì)自動(dòng)駕駛車輛的實(shí)時(shí)狀態(tài)。對(duì)糾偏后的點(diǎn)云進(jìn)行濾波以剔除異常值，并從點(diǎn)云中提取特征點(diǎn)，包括邊緣點(diǎn)和平面點(diǎn)，用于配準(zhǔn)。隨后可構(gòu)造基于LiDAR測(cè)量的重投影殘差。

IMU測(cè)量可為L(zhǎng)iDAR里程計(jì)提供較準(zhǔn)確的初始值且可為點(diǎn)云糾偏提供狀態(tài)估計(jì)量，包括來(lái)自加速度計(jì)的加速度和陀螺儀的角速度，會(huì)受到零偏和噪聲的影響，在IMU里程計(jì)殘差構(gòu)建和初始化過(guò)程中，需要考慮這些因素對(duì)自動(dòng)駕駛車輛位姿估計(jì)造成的偏差。

車輛運(yùn)動(dòng)學(xué)模型的輸入來(lái)自先前LiDAR測(cè)量時(shí)刻的聯(lián)合優(yōu)化結(jié)果，包括平移、速度和旋轉(zhuǎn)。本文在自行車模型的基礎(chǔ)上，設(shè)計(jì)了一種改進(jìn)的運(yùn)動(dòng)學(xué)模型，利用3自由度輸入對(duì)自動(dòng)駕駛車輛的6自由度位姿進(jìn)行預(yù)測(cè)，并對(duì)LiDAR測(cè)量時(shí)刻的位姿進(jìn)行預(yù)測(cè)。最后利用預(yù)測(cè)量約束車輛位姿，得到基于車輛運(yùn)動(dòng)學(xué)模型預(yù)測(cè)的殘差。

以上3部分殘差以緊耦合方式被綜合起來(lái)形成代價(jià)函數(shù)，以車輛當(dāng)前位姿為待優(yōu)化變量，使用Ceres求解，得到車輛當(dāng)前位姿的最優(yōu)解。

2 緊耦合定位方法

本節(jié)介紹LiDAR、IMU里程計(jì)的殘差構(gòu)建，改進(jìn)的車輛運(yùn)動(dòng)學(xué)模型構(gòu)建方式和預(yù)測(cè)分析，以及緊耦合定位方法代價(jià)函數(shù)的推導(dǎo)過(guò)程。

2.1 系統(tǒng)狀態(tài)量構(gòu)建

系統(tǒng)采用圖2所示幀間匹配方式，以LiDAR測(cè)量時(shí)刻為時(shí)間基準(zhǔn)，以IMU坐標(biāo)系為測(cè)量基準(zhǔn)融合LiDAR幀間傳感器測(cè)量值和車輛模型預(yù)測(cè)值構(gòu)建局部地圖。對(duì)于車輛運(yùn)動(dòng)學(xué)模型更新模塊，其輸入量來(lái)自當(dāng)前LiDAR幀時(shí)刻和前兩個(gè)LiDAR幀時(shí)刻的系統(tǒng)狀態(tài)量，通過(guò)阿克曼轉(zhuǎn)向原理和車輛運(yùn)動(dòng)方程構(gòu)建車輛運(yùn)動(dòng)學(xué)模型并預(yù)測(cè)下一時(shí)刻自動(dòng)駕駛車輛運(yùn)動(dòng)狀態(tài)。對(duì)于IMU測(cè)量更新模塊，高頻IMU測(cè)量通過(guò)線性插值和預(yù)積分得到相鄰LiDAR測(cè)量時(shí)刻的IMU預(yù)積分量以構(gòu)建約束。對(duì)于LiDAR測(cè)量更新模塊，定義局部地圖LiDAR關(guān)鍵幀，目的是通過(guò)將關(guān)鍵幀后的普通LiDAR幀投影到關(guān)鍵幀并利用特征點(diǎn)配準(zhǔn)方法構(gòu)建穩(wěn)定性較強(qiáng)的約束。相鄰LiDAR測(cè)量時(shí)刻間的狀態(tài)量定義如下：

圖2 系統(tǒng)中不同數(shù)據(jù)的幀間匹配方式

式中：w表示世界坐標(biāo)系，重力的方向與世界坐標(biāo)系的豎直方向?qū)R；b表示IMU坐標(biāo)系；i表示LiDAR測(cè)量時(shí)刻；X為系統(tǒng)狀態(tài)量，且均以IMU坐標(biāo)系為參考；Xbk為k（k∈[0，i]）時(shí)刻IMU狀態(tài)量，包括相對(duì)于世界坐標(biāo)系的平移、速度和旋轉(zhuǎn)，分別以p、v和四元數(shù)q表示；ba和bg分別為加速度計(jì)和陀螺儀的零偏。

2.2 LiDAR測(cè)量殘差

經(jīng)IMU測(cè)量值和優(yōu)化后的系統(tǒng)狀態(tài)量糾偏后的點(diǎn)云L被用來(lái)構(gòu)造LiDAR測(cè)量殘差。首先用體素網(wǎng)格濾波器對(duì)點(diǎn)云進(jìn)行降采樣以提高點(diǎn)云質(zhì)量。然后提取點(diǎn)云特征點(diǎn)，并構(gòu)造特征線和特征面，同時(shí)根據(jù)曲率大小將特征點(diǎn)分為邊緣點(diǎn)和平面點(diǎn)。之后根據(jù)點(diǎn)云中邊緣點(diǎn)和平面點(diǎn)到特征線和特征面的距離構(gòu)造LiDAR測(cè)量殘差。

對(duì)于點(diǎn)云幀中的邊緣點(diǎn)，在前一幀點(diǎn)云中提取其對(duì)應(yīng)點(diǎn)，并利用KD樹(shù)搜索與該點(diǎn)距離最近的兩點(diǎn)以構(gòu)建特征線，同時(shí)計(jì)算該點(diǎn)與特征線間的距離以構(gòu)建LiDAR測(cè)量的一部分殘差。對(duì)于點(diǎn)云幀中的平面點(diǎn)，與上述方式類似，在前一幀點(diǎn)云中提取其對(duì)應(yīng)點(diǎn)并利用KD樹(shù)搜索與該點(diǎn)距離最近的3點(diǎn)以構(gòu)建特征面，同時(shí)計(jì)算該點(diǎn)與特征面間的距離以構(gòu)建LiDAR測(cè)量的另一部分殘差。綜上，LiDAR測(cè)量殘差可表示為

式中：l表示LiDAR坐標(biāo)系；e和s分別表示邊緣點(diǎn)和平面點(diǎn)；和分別表示由邊緣點(diǎn)和平面點(diǎn)構(gòu)造的LiDAR殘差；R為旋轉(zhuǎn)矩陣，雖然四元數(shù)主要在狀態(tài)量的表示中被使用，但旋轉(zhuǎn)矩陣也用于簡(jiǎn)化旋轉(zhuǎn)和偏導(dǎo)數(shù)等的計(jì)算；m為特征線的方向向量和為L(zhǎng)i+1到Li的轉(zhuǎn)換；n為特征面法向量；D為特征面一般方程中的常數(shù)項(xiàng)。

2.3 IMU預(yù)積分和殘差構(gòu)建

本文在Shen等的工作基礎(chǔ)上［16］，參考Forster等的方法［17］，考慮了IMU零偏和噪聲對(duì)自動(dòng)駕駛車輛位姿估計(jì)造成的偏差。

對(duì)于原始IMU測(cè)量，采用線性插值使其時(shí)間戳與LiDAR時(shí)間戳對(duì)齊。隨后對(duì)測(cè)量值進(jìn)行積分，對(duì)上一IMU測(cè)量時(shí)刻的IMU測(cè)量值進(jìn)行積分可得到當(dāng)前IMU測(cè)量時(shí)刻的平移、速度和旋轉(zhuǎn)，結(jié)合IMU坐標(biāo)系與世界坐標(biāo)系間的旋轉(zhuǎn)變換可將相對(duì)于世界坐標(biāo)系的積分量轉(zhuǎn)換為相對(duì)于IMU測(cè)量時(shí)刻的積分量，即為預(yù)積分量。預(yù)積分量只與IMU測(cè)量值相關(guān)，表示一段時(shí)間內(nèi)的IMU測(cè)量，它的引入使得IMU坐標(biāo)系相對(duì)于世界坐標(biāo)系的旋轉(zhuǎn)優(yōu)化更新后不需要對(duì)IMU測(cè)量重新進(jìn)行積分，大大減少了運(yùn)算量，提高了系統(tǒng)運(yùn)行效率。預(yù)積分量被用作測(cè)量值對(duì)狀態(tài)量進(jìn)行約束并構(gòu)建IMU測(cè)量殘差：

式中：pbibi+1、qbi+1bi、vbibi+1分別為平移、旋轉(zhuǎn)和速度在相鄰LiDAR幀間的預(yù)積分量；gw為世界坐標(biāo)系下的重力加速度；[·]xyz為取四元數(shù)虛部的運(yùn)算；?表示四元數(shù)的乘法運(yùn)算。

2.4 車輛運(yùn)動(dòng)學(xué)模型測(cè)量殘差

基于車輛運(yùn)動(dòng)學(xué)模型的位姿預(yù)測(cè)方法充分利用之前LiDAR測(cè)量時(shí)刻的狀態(tài)量（包括平移、速度和旋轉(zhuǎn)）來(lái)估計(jì)自動(dòng)駕駛車輛的6自由度位姿，不添加額外傳感器以避免由于新增傳感器而導(dǎo)致的額外誤差。同時(shí)提出平面假設(shè)，即相鄰兩LiDAR測(cè)量時(shí)刻間車輛在同一平面上運(yùn)動(dòng)。根據(jù)阿克曼轉(zhuǎn)向原理和運(yùn)動(dòng)學(xué)方程等預(yù)測(cè)相鄰LiDAR測(cè)量時(shí)刻間自動(dòng)駕駛車輛的位姿變化。

改進(jìn)的車輛運(yùn)動(dòng)學(xué)模型可利用二維車輛狀態(tài)信息對(duì)車輛相對(duì)于世界坐標(biāo)系的三維位姿進(jìn)行預(yù)測(cè)，具體建模方法如圖3所示，圖中車輛坐標(biāo)系以車輛后軸為中心，該圖基于自行車模型原理對(duì)3個(gè)相鄰LiDAR測(cè)量時(shí)刻的車輛參數(shù)進(jìn)行描述。圖中XwOwYw為世界坐標(biāo)系，XkvOkY kv為k時(shí)刻（k=i-1、i、i+1）以自動(dòng)駕駛車輛后軸中心為原點(diǎn)的車輛坐標(biāo)系。OkMk表示k時(shí)刻自動(dòng)駕駛車輛所在位置，此時(shí)刻前輪轉(zhuǎn)角為δk，相對(duì)于前一時(shí)刻車輛的橫擺角變化量為ψk。自動(dòng)駕駛車輛軸距為L(zhǎng)，速度vk方向始終與車頭朝向平行，即沿Xkv方向。根據(jù)阿克曼轉(zhuǎn)向原理，假設(shè)自動(dòng)駕駛車輛在兩相鄰LiDAR測(cè)量時(shí)刻間以相同半徑R作圓周運(yùn)動(dòng)，點(diǎn)C為軌跡的圓心。R可通過(guò)相鄰LiDAR測(cè)量時(shí)刻自動(dòng)駕駛車輛在世界坐標(biāo)系中的位置坐標(biāo)和空間中圓的一般方程來(lái)計(jì)算?；趫D中的幾何關(guān)系，可得i和i+1時(shí)刻的前輪轉(zhuǎn)角：

圖3 改進(jìn)的車輛運(yùn)動(dòng)學(xué)模型原理圖

式中：L為車輛軸距；R為轉(zhuǎn)向半徑。

對(duì)于i時(shí)刻速度狀態(tài)量vwbi，將其轉(zhuǎn)換到車輛坐標(biāo)系并提取沿X軸的速度分量：

式中：qvb為IMU坐標(biāo)系到車輛坐標(biāo)系的旋轉(zhuǎn)外參；R11為提取沿X軸方向速度的變換矩陣。

綜上所述，構(gòu)造自動(dòng)駕駛車輛相對(duì)于上一LiDAR測(cè)量時(shí)刻的位姿增量預(yù)測(cè)方程：

式中：ψvivt和pvivt為t時(shí)刻自動(dòng)駕駛車輛相對(duì)于i時(shí)刻車輛坐標(biāo)系的橫擺角和平移增量；R12為提取沿Y軸方向速度的變換矩陣；R13為提取繞豎直方向旋轉(zhuǎn)的變換矩陣。

對(duì)于橫擺角增量ψvivt，采用羅德里格斯公式并結(jié)合自動(dòng)駕駛車輛運(yùn)動(dòng)平面的法向量nv計(jì)算相鄰LiDAR測(cè)量時(shí)刻間的旋轉(zhuǎn)矩陣：

式中：I3×3為3階單位矩陣；?為反對(duì)稱符號(hào)。

從而可得與旋轉(zhuǎn)矩陣Rvivi+1相對(duì)應(yīng)的四元數(shù)qvivi+1，結(jié)合i和i+1時(shí)刻的旋轉(zhuǎn)狀態(tài)量qwbi和qwbi+1及IMU坐標(biāo)系與車輛坐標(biāo)系的旋轉(zhuǎn)外參qbv可得車輛運(yùn)動(dòng)學(xué)模型旋轉(zhuǎn)預(yù)測(cè)殘差：

式中*為四元數(shù)的共軛運(yùn)算。

對(duì)于平移增量pvivi+1，轉(zhuǎn)換到世界坐標(biāo)系：

式中：Rwbi為旋轉(zhuǎn)狀態(tài)量qwbi對(duì)應(yīng)的旋轉(zhuǎn)向量；pwbi為平移狀態(tài)量；Rbv和pbv為IMU坐標(biāo)系和車輛坐標(biāo)系的旋轉(zhuǎn)和平移外參。

從而可得車輛運(yùn)動(dòng)學(xué)模型平移預(yù)測(cè)殘差為

式中Rwvi+1為i+1時(shí)刻世界坐標(biāo)系下的旋轉(zhuǎn)。

綜上可得車輛運(yùn)動(dòng)學(xué)模型殘差為

2.5 系統(tǒng)優(yōu)化求解

對(duì)于由LiDAR、IMU和車輛運(yùn)動(dòng)學(xué)模型得到的位姿估計(jì)殘差，采用緊耦合方法進(jìn)行聯(lián)合優(yōu)化。通過(guò)最小化所有殘差的先驗(yàn)和馬氏范數(shù)之和獲得最大后驗(yàn)估計(jì)：

3 試驗(yàn)與分析

為評(píng)估本文中提出的基于車輛模型緊耦合的封閉園區(qū)車輛定位方法，在不同封閉園區(qū)場(chǎng)景進(jìn)行了試驗(yàn)，并與室外定位導(dǎo)航應(yīng)用較為廣泛的A-LOAM定位方法進(jìn)行對(duì)比。A-LOAM定位方法結(jié)合高頻定位和低頻建圖兩個(gè)線程實(shí)現(xiàn)三維位姿估計(jì)，其以低漂移、低計(jì)算量和較高精度等特點(diǎn)成為激光SLAM中的主流方法之一［18］。由林肯MKZ轎車改裝的自動(dòng)駕駛平臺(tái)進(jìn)行相關(guān)試驗(yàn)，該平臺(tái)配備了VelodyneVLP-32C LiDAR和北斗星通npos220慣性導(dǎo)航系統(tǒng)（包括GNSS和IMU），如圖4所示。所有試驗(yàn)定位方法均在配備Intel i5 CPU、2.3GHz四核、16GB RAM和Linux系統(tǒng)的計(jì)算機(jī)上運(yùn)行。在自動(dòng)駕駛車輛行駛時(shí)實(shí)時(shí)采集LiDAR、IMU和GNSS數(shù)據(jù)，其中GNSS數(shù)據(jù)作為真實(shí)值以評(píng)估定位方法精度。

圖4 自動(dòng)駕駛試驗(yàn)平臺(tái)車

圖5所示為封閉園區(qū)試驗(yàn)場(chǎng)景的點(diǎn)云地圖和試驗(yàn)車行駛軌跡，其中場(chǎng)景a的軌跡長(zhǎng)度為965.065 m，場(chǎng)景b的軌跡長(zhǎng)度為903.528 m。軌跡起點(diǎn)和終點(diǎn)以及試驗(yàn)車行駛方向均在圖中標(biāo)示。在以上兩個(gè)場(chǎng)景對(duì)A-LOAM、本文提出的基于多傳感器緊耦合的LiDAR-IMU定位方法和基于車輛模型緊耦合的LiDAR-IMU-VM（Vehicle Model）定位方法分別進(jìn)行試驗(yàn)，并對(duì)試驗(yàn)結(jié)果的相對(duì)誤差、絕對(duì)誤差和豎直方向誤差進(jìn)行對(duì)比。

圖5 兩個(gè)不同封閉園區(qū)試驗(yàn)場(chǎng)景的點(diǎn)云地圖和自動(dòng)駕駛車輛行駛軌跡

表1為3種定位方法的相對(duì)定位誤差對(duì)比。其中：A-LOAM在兩個(gè)試驗(yàn)場(chǎng)景中的平移與旋轉(zhuǎn)精度均較差，本文中提出的LiDAR-IMU定位方法相對(duì)于A-LOAM在兩個(gè)試驗(yàn)場(chǎng)景中的平移與旋轉(zhuǎn)誤差均有較大幅度下降，加上車輛運(yùn)動(dòng)學(xué)模型的LiDARIMU-VM定位方法的平移與旋轉(zhuǎn)精度表現(xiàn)最好，在試驗(yàn)場(chǎng)景b中的平移與旋轉(zhuǎn)誤差分別降至0.514 8%和0.004 0（°）∕m，在試驗(yàn)場(chǎng)景a中效果更好，只有0.372 0%和0.002 1（°）∕m。

表1 不同定位方法在不同場(chǎng)景的相對(duì)定位誤差

圖6所示為3種定位方法在兩個(gè)不同場(chǎng)景的絕對(duì)定位誤差對(duì)比。圖中所示在兩個(gè)不同試驗(yàn)場(chǎng)景中LiDAR-IMU-VM定位方法的絕對(duì)誤差均值均為最小，分別為1.412 6和1.223 9 m，對(duì)于A-LOAM的絕對(duì)誤差均 值2.760 9和2.437 9 m及LiDAR-IMU定位方法的絕對(duì)誤差均值2.072 4和1.866 8 m，定位精度有較大幅度提升。特別的，在場(chǎng)景a下，ALOAM算法在試驗(yàn)終斷的誤差累積雖然有所收斂，但其絕對(duì)誤差仍較大，超過(guò)了4 m，LiDAR-IMU定位方法的誤差累積有所減小，LiDAR-IMU-VM定位方法的誤差累積最小，且終段收斂明顯。在場(chǎng)景b下，A-LOAM的誤差持續(xù)累積并逐漸發(fā)散，LiDAR-IMU定位方法誤差在試驗(yàn)終段存在收斂趨勢(shì)，而LiDARIMU-VM定位方法在試驗(yàn)前中段相對(duì)另外兩種方法表現(xiàn)較好，且誤差累積較小，試驗(yàn)終段誤差雖有增加趨勢(shì)，但仍可收斂到較低水平。

圖6 3種定位方法在試驗(yàn)場(chǎng)景a、b下的絕對(duì)定位誤差

圖7所示為豎直方向上3種定位方法的平移位姿估計(jì)與Ground Truth平移位姿間差值的絕對(duì)值對(duì)比。在試驗(yàn)場(chǎng)景a下，A-LOAM的豎直方向誤差絕對(duì)值最大達(dá)到5.075 9 m，誤差均值為2.222 5 m。LiDAR-IMU定位方法的最大絕對(duì)誤差值降至2.382 4 m，誤差均值降至1.245 1 m，相較于ALOAM有較大提升，而LiDAR-IMU-VM定位方法的最大豎直方向誤差則降至1.493 1 m且誤差均值僅為0.443 1 m。在試驗(yàn)場(chǎng)景b下，3種定位方法的誤差絕對(duì)值均值分別為3.076 6、2.237 2和0.400 6 m。且LiDAR-IMU-VM定位方法將最大豎直方向誤差控制在1.524 1 m以內(nèi)，相較于A-LOAM和LiDARIMU定位方法的最大豎直方向誤差5.871 5和8.255 0 m，優(yōu)勢(shì)明顯。

圖7 試驗(yàn)場(chǎng)景a、b下豎直方向3種定位方法位姿估計(jì)結(jié)果與真實(shí)位姿間差值的絕對(duì)值對(duì)比

表2為本文LiDAR-IMU-VM定位方法的分模塊耗時(shí)對(duì)比，其中場(chǎng)景a和場(chǎng)景b下的激光里程計(jì)耗時(shí)分別為68.402 3和56.823 1 ms。IMU模型位姿估計(jì)模塊和車輛運(yùn)動(dòng)學(xué)模型位姿預(yù)測(cè)模塊由于在優(yōu)化基礎(chǔ)上進(jìn)行構(gòu)建，其耗時(shí)相對(duì)較少，僅為0.044 2和0.040 1 ms。而建圖模塊的耗時(shí)最多，在場(chǎng)景a和場(chǎng)景b下分別為278.332 4和256.250 4 ms。由于場(chǎng)景a的試驗(yàn)里程較場(chǎng)景b長(zhǎng)60余米，故場(chǎng)景a下的算法總耗時(shí)346.778 9 ms略高于場(chǎng)景b下的算法總耗時(shí)313.113 6 ms。綜上，本文提出的LiDAR-IMUVM定位方法在兩封閉園區(qū)場(chǎng)景下的算法耗時(shí)均可滿足自動(dòng)駕駛車輛定位的實(shí)時(shí)性要求。

表2 不同定位方法在不同場(chǎng)景的定位耗時(shí)對(duì)比

從上述試驗(yàn)中可以看出，本文中提出的基于車輛模型緊耦合的封閉園區(qū)自動(dòng)駕駛定位方法在相對(duì)定位誤差、絕對(duì)定位誤差和豎直方向誤差這3個(gè)定位精度評(píng)價(jià)標(biāo)準(zhǔn)下，相較于A-LOAM和單純的LiDAR-IMU定位方法均表現(xiàn)更好，且具有較好的實(shí)時(shí)性、穩(wěn)定性和魯棒性。為GNSS信號(hào)差或無(wú)GNSS信號(hào)的封閉園區(qū)自動(dòng)駕駛定位解決方案提供了可靠的新思路。

4 結(jié)論

本文中提出了基于車輛模型緊耦合的封閉園區(qū)車輛定位方法，其借助改進(jìn)的車輛運(yùn)動(dòng)學(xué)模型，可在封閉園區(qū)環(huán)境中為自動(dòng)駕駛車輛提供一致性較高的定位結(jié)果。該定位方法引入了車輛運(yùn)動(dòng)學(xué)預(yù)測(cè)模塊以提高定位精度，但未使用除LiDAR和IMU以外的傳感器。試驗(yàn)結(jié)果表明，在建筑物密度較高的封閉園區(qū)環(huán)境中，所提出的定位方法的定位精度和魯棒性較高。

未來(lái)將改進(jìn)點(diǎn)云的特征提取方式，并增加回環(huán)檢測(cè)模塊，以在該自動(dòng)駕駛車輛定位方法的基礎(chǔ)上提高其在園區(qū)復(fù)雜場(chǎng)景例如樹(shù)木較多的道路、特征較少的彎道和連續(xù)上下坡道路等定位可靠性的同時(shí)改善定位誤差累積現(xiàn)象。