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

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

（汽車車身先進設計制造國家重點實驗室，湖南大學機械與運載工程學院，長沙410082）

前言

近年來隨著自動駕駛技術研究的不斷深入，多種應用場景下的自動駕駛解決方案應運而生。諸如港口、工廠園區等相對封閉區域因車輛行駛路線固定、交互環境簡單等特點，成為了自動駕駛技術商業化落地的首選場景。自動駕駛技術包含環境感知、自主定位、決策規劃和運動控制等多項技術，其中自主定位技術是自動駕駛車輛實現復雜環境下自主運動的重要基礎。

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

對于GNSS信號存在遮擋場景中的自動駕駛車輛自主定位，國內外學者進行了大量研究。在此工況下，通常有4種位姿估計方法：基于模型的方法、基于直接傳感器的方法、基于間接傳感器的方法和上述3種方法的融合［1］。基于模型的方法使用車輛模型在靜態和動態響應中獲得定位結果，Ren等基于阿克曼原理分析車輛的運動學特征，建立考慮誤差的車輛運動學模型推算車輛的轉向角、航向角和轉彎半徑等運動狀態［2］。Xiong等結合基于模型和間接傳感器的方法，通過基于車輛動力學模型和間接傳感器之一的IMU來完成位姿和滑移角估計［3］，但該方法中的IMU累積誤差無法被很好地消除。Xia等提出一種基于IMU的運動學模型，結合用于補償誤差累積的動態模型并采用開環積分的方法來估計車輛的位姿和橫向速度［4］。

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

受上述工作啟發，本文中提出了一種基于車輛模型緊耦合的封閉園區車輛定位方法，該定位方法可良好適應GNSS信號受遮擋的封閉園區場景，且不受園區中因貨物運輸等因素造成的環境變化影響，具有實時性和魯棒性。在充分利用LiDAR和IMU測量的基礎上輔以車輛運動學模型約束車輛位姿優化方向，采用模塊化設計思路對系統分3個模塊進行建模，并使用緊耦合方法進行聯合優化以獲得高一致性的自動駕駛車輛位姿估計。本文主要貢獻如下：

（1）提出一種基于車輛模型緊耦合的LiDARIMU融合定位方法，實現了自動駕駛車輛在弱GNSS信號環境下高精度的魯棒定位；

（2）利用IMU測量和系統狀態量為LiDAR幀間配準提供先驗信息，并采用基于局部地圖的方法以提高配準精度；

（3）利用系統狀態信息構建車輛運動學模型以充分利用車輛運動信息約束位姿優化方向，并預測自動駕駛車輛的6自由度位姿；

（4）通過不同弱GNSS信號場景的實車試驗對該方法進行驗證，其性能優于室外定位導航應用較為廣泛的A-LOAM定位方法和僅使用LiDAR-IMU緊耦合的定位方法。

1 系統架構

本文中提出的基于車輛模型緊耦合的封閉園區車輛定位方法系統架構如圖1所示。主要包括4個部分：LiDAR里程計、IMU里程計、車輛運動學模型預測方程和聯合優化。

圖1 考慮車輛運動學模型約束的緊耦合車輛定位方法系統架構

LiDAR測量值，即點云，通常會由于自動駕駛車輛的運動而產生畸變，常用狀態估計的方法進行糾偏。狀態估計指充分利用IMU測量值、優化后的系統狀態量和IMU坐標系與LiDAR坐標系之間的外部參數來估計自動駕駛車輛的實時狀態。對糾偏后的點云進行濾波以剔除異常值，并從點云中提取特征點，包括邊緣點和平面點，用于配準。隨后可構造基于LiDAR測量的重投影殘差。

IMU測量可為LiDAR里程計提供較準確的初始值且可為點云糾偏提供狀態估計量，包括來自加速度計的加速度和陀螺儀的角速度，會受到零偏和噪聲的影響，在IMU里程計殘差構建和初始化過程中，需要考慮這些因素對自動駕駛車輛位姿估計造成的偏差。

車輛運動學模型的輸入來自先前LiDAR測量時刻的聯合優化結果，包括平移、速度和旋轉。本文在自行車模型的基礎上，設計了一種改進的運動學模型，利用3自由度輸入對自動駕駛車輛的6自由度位姿進行預測，并對LiDAR測量時刻的位姿進行預測。最后利用預測量約束車輛位姿，得到基于車輛運動學模型預測的殘差。

以上3部分殘差以緊耦合方式被綜合起來形成代價函數，以車輛當前位姿為待優化變量，使用Ceres求解，得到車輛當前位姿的最優解。

2 緊耦合定位方法

本節介紹LiDAR、IMU里程計的殘差構建，改進的車輛運動學模型構建方式和預測分析，以及緊耦合定位方法代價函數的推導過程。

2.1 系統狀態量構建

系統采用圖2所示幀間匹配方式，以LiDAR測量時刻為時間基準，以IMU坐標系為測量基準融合LiDAR幀間傳感器測量值和車輛模型預測值構建局部地圖。對于車輛運動學模型更新模塊，其輸入量來自當前LiDAR幀時刻和前兩個LiDAR幀時刻的系統狀態量，通過阿克曼轉向原理和車輛運動方程構建車輛運動學模型并預測下一時刻自動駕駛車輛運動狀態。對于IMU測量更新模塊，高頻IMU測量通過線性插值和預積分得到相鄰LiDAR測量時刻的IMU預積分量以構建約束。對于LiDAR測量更新模塊，定義局部地圖LiDAR關鍵幀，目的是通過將關鍵幀后的普通LiDAR幀投影到關鍵幀并利用特征點配準方法構建穩定性較強的約束。相鄰LiDAR測量時刻間的狀態量定義如下：

圖2 系統中不同數據的幀間匹配方式

式中：w表示世界坐標系，重力的方向與世界坐標系的豎直方向對齊；b表示IMU坐標系；i表示LiDAR測量時刻；X為系統狀態量，且均以IMU坐標系為參考；Xbk為k（k∈[0，i]）時刻IMU狀態量，包括相對于世界坐標系的平移、速度和旋轉，分別以p、v和四元數q表示；ba和bg分別為加速度計和陀螺儀的零偏。

2.2 LiDAR測量殘差

經IMU測量值和優化后的系統狀態量糾偏后的點云L被用來構造LiDAR測量殘差。首先用體素網格濾波器對點云進行降采樣以提高點云質量。然后提取點云特征點，并構造特征線和特征面，同時根據曲率大小將特征點分為邊緣點和平面點。之后根據點云中邊緣點和平面點到特征線和特征面的距離構造LiDAR測量殘差。

對于點云幀中的邊緣點，在前一幀點云中提取其對應點，并利用KD樹搜索與該點距離最近的兩點以構建特征線，同時計算該點與特征線間的距離以構建LiDAR測量的一部分殘差。對于點云幀中的平面點，與上述方式類似，在前一幀點云中提取其對應點并利用KD樹搜索與該點距離最近的3點以構建特征面，同時計算該點與特征面間的距離以構建LiDAR測量的另一部分殘差。綜上，LiDAR測量殘差可表示為

式中：l表示LiDAR坐標系；e和s分別表示邊緣點和平面點；和分別表示由邊緣點和平面點構造的LiDAR殘差；R為旋轉矩陣，雖然四元數主要在狀態量的表示中被使用，但旋轉矩陣也用于簡化旋轉和偏導數等的計算；m為特征線的方向向量和為Li+1到Li的轉換；n為特征面法向量；D為特征面一般方程中的常數項。

2.3 IMU預積分和殘差構建

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

對于原始IMU測量，采用線性插值使其時間戳與LiDAR時間戳對齊。隨后對測量值進行積分，對上一IMU測量時刻的IMU測量值進行積分可得到當前IMU測量時刻的平移、速度和旋轉，結合IMU坐標系與世界坐標系間的旋轉變換可將相對于世界坐標系的積分量轉換為相對于IMU測量時刻的積分量，即為預積分量。預積分量只與IMU測量值相關，表示一段時間內的IMU測量，它的引入使得IMU坐標系相對于世界坐標系的旋轉優化更新后不需要對IMU測量重新進行積分，大大減少了運算量，提高了系統運行效率。預積分量被用作測量值對狀態量進行約束并構建IMU測量殘差：

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

2.4 車輛運動學模型測量殘差

基于車輛運動學模型的位姿預測方法充分利用之前LiDAR測量時刻的狀態量（包括平移、速度和旋轉）來估計自動駕駛車輛的6自由度位姿，不添加額外傳感器以避免由于新增傳感器而導致的額外誤差。同時提出平面假設，即相鄰兩LiDAR測量時刻間車輛在同一平面上運動。根據阿克曼轉向原理和運動學方程等預測相鄰LiDAR測量時刻間自動駕駛車輛的位姿變化。

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

圖3 改進的車輛運動學模型原理圖

式中：L為車輛軸距；R為轉向半徑。

對于i時刻速度狀態量vwbi，將其轉換到車輛坐標系并提取沿X軸的速度分量：

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

綜上所述，構造自動駕駛車輛相對于上一LiDAR測量時刻的位姿增量預測方程：

式中：ψvivt和pvivt為t時刻自動駕駛車輛相對于i時刻車輛坐標系的橫擺角和平移增量；R12為提取沿Y軸方向速度的變換矩陣；R13為提取繞豎直方向旋轉的變換矩陣。

對于橫擺角增量ψvivt，采用羅德里格斯公式并結合自動駕駛車輛運動平面的法向量nv計算相鄰LiDAR測量時刻間的旋轉矩陣：

式中：I3×3為3階單位矩陣；?為反對稱符號。

從而可得與旋轉矩陣Rvivi+1相對應的四元數qvivi+1，結合i和i+1時刻的旋轉狀態量qwbi和qwbi+1及IMU坐標系與車輛坐標系的旋轉外參qbv可得車輛運動學模型旋轉預測殘差：

式中*為四元數的共軛運算。

對于平移增量pvivi+1，轉換到世界坐標系：

式中：Rwbi為旋轉狀態量qwbi對應的旋轉向量；pwbi為平移狀態量；Rbv和pbv為IMU坐標系和車輛坐標系的旋轉和平移外參。

從而可得車輛運動學模型平移預測殘差為

式中Rwvi+1為i+1時刻世界坐標系下的旋轉。

綜上可得車輛運動學模型殘差為

2.5 系統優化求解

對于由LiDAR、IMU和車輛運動學模型得到的位姿估計殘差，采用緊耦合方法進行聯合優化。通過最小化所有殘差的先驗和馬氏范數之和獲得最大后驗估計：

3 試驗與分析

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

圖4 自動駕駛試驗平臺車

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

圖5 兩個不同封閉園區試驗場景的點云地圖和自動駕駛車輛行駛軌跡

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

表1 不同定位方法在不同場景的相對定位誤差

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

圖6 3種定位方法在試驗場景a、b下的絕對定位誤差

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

圖7 試驗場景a、b下豎直方向3種定位方法位姿估計結果與真實位姿間差值的絕對值對比

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

表2 不同定位方法在不同場景的定位耗時對比

從上述試驗中可以看出，本文中提出的基于車輛模型緊耦合的封閉園區自動駕駛定位方法在相對定位誤差、絕對定位誤差和豎直方向誤差這3個定位精度評價標準下，相較于A-LOAM和單純的LiDAR-IMU定位方法均表現更好，且具有較好的實時性、穩定性和魯棒性。為GNSS信號差或無GNSS信號的封閉園區自動駕駛定位解決方案提供了可靠的新思路。

4 結論

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

未來將改進點云的特征提取方式，并增加回環檢測模塊，以在該自動駕駛車輛定位方法的基礎上提高其在園區復雜場景例如樹木較多的道路、特征較少的彎道和連續上下坡道路等定位可靠性的同時改善定位誤差累積現象。