基于多傳感信息融合的智能車輛定位方法

趙一兵，劉昌華，鄭 震，郭 烈，馬振強，韓治中

(大連理工大學汽車工程學院，遼寧，大連 116024)

高精度的定位系統是智能車輛感知層的重要一環，它解決行駛過程中“我在哪里？”的問題，是實現智能車輛自主導航的基礎[1]。根據智能車輛對周圍環境的感知程度分為絕對定位和相對定位[2]。在進行相對定位時，基于視覺定位的定位方式研究較為廣泛，AZUMA[3]等利用擴展卡爾曼濾波算法將相機數據與IMU 數據相融合，結果表明，融合后的定位效果較好，能有效地減小系統誤差。甘露[4]提出一種基于卷積神經網絡的視覺定位系統，將雙目視覺和卷積神經網絡結合，定位效果穩定，具有較高的實用價值。

由于單傳感器誤差較大，將多個內部傳感器數據相融合[5]的定位方式越來越得到專家學者的青睞。LIU 等[6]提出了一種基于1 個慣性測量單元和2 個3D 激光雷達的適合大型自然地形的實時6DSLAM 的方法，除了將整個地圖劃分為許多由大量基于樹結構的體素組成的子地圖之外，還使用概率方法來表示1 個體素被占用和空余的可能性，實時性得以提升。柯顯信等[7]設計出包括紅外定位系統、聽覺定位系統和視覺定位系統的多傳感器定位系統，提出的加權平均融合算法可使權重隨目標位置和外部環境發生改變，環境適應性更強，有效提高了交互系統的整體定位精度和可靠性。

在靜態環境下，基于地圖匹配的定位方法經常取得不錯的效果，該方法將傳感器在一定范圍內掃描得到的信息與先驗地圖的信息做匹配，得到符合度、一致性最好的匹配結果，從而確定智能車輛的位置，實現定位功能[8-9]。艾國[10]將地圖匹配與激光陀螺慣性導航系統相結合，并提出基于曲線匹配的地圖匹配方法，充分利用激光陀螺慣性導航系統的高精度姿態信息，較現有的點到點、點到線的匹配技術有更高的定位精度。李祎承等[11]提出了一種融合了視覺傳感器和GPS/RTK 數據的視覺道路環境地圖構建方法，在視覺地圖的基礎上，智能車僅需裝載普通的攝像機和GPS 接收器便能實現高精度定位，有效降低了智能車成本。

1 智能車輛建模分析

試驗所用智能車輛是銳趣科技的四輪小車平臺，該平臺基于ROS 操作系統，并采用四輪差速設計，同側驅動輪以相同速度運行。激光雷達采用思嵐A2 雷達，安裝于小車頂部，驅動電機采用霍爾AB 型雙通道的增量式光電編碼器，慣性測量單元采用GY-85 九軸陀螺儀傳感器。

將定位問題分為無地圖定位與基于地圖定位，通過理論與實踐相結合的方式對智能車輛定位問題進行探索。試驗結果表明，航跡推測法具有較明顯的累計誤差問題，不適用于長距離定位，基于地圖的絕對定位方法對累計誤差有一定校正作用，定位效果較為良好，說明了地圖在定位系統中的重要性。

1.1 智能車輛運動學模型

試驗基于室內結構化環境下完成，車輪厚度僅影響重心高度，且只考慮智能車輛車輪發生純滾動情況，忽略車輛打滑，只建立其運動學模型，尋找狀態變量與控制變量之間的函數關系。

將傳感器坐標系與智能車輛坐標系合并，全局坐標系表示智能車輛的運動范圍，局部坐標系建立在小車中心位置，用于確定環境中特征點相對于小車的位置分布。智能車輛的運動學模型如圖1 所示。

圖1 智能車輛運動學模型

在圖1 中，坐標系xCOMy為局部坐標系，Xg0Yg為全局坐標系，用(x,y,θ)T表示智能車輛在全局坐標系中的位置和朝向信息，其中(x,y)表示智能車輛在全局坐標系中的坐標信息，θ表示在全局坐標系中的方向角。左右輪中心距離為2c，車輪的半徑為R，智能車輛左右兩輪的線速度分別為VL和VR，則智能車輛運動學方程可表示為：

且式(2)需要滿足非完整性約束條件：

因此，可以通過調節左右輪的線速度來改變小車的位姿。

1.2 智能車輛傳感器模型

1.2.1 里程計模型

假如在Δt時間段內，智能車輛的光電編碼器的脈沖增量為ΔN，里程計分辨率為re，則小車在這段時間內移動的距離為：

若在這段時間內，小車左輪移動的距離用ΔdL表示，右輪移動的距離用ΔdR表示，令里程計模型的輸入為u=[ΔD,Δθ]。其中，智能車輛對應的位移變化量ΔD與航向角變化量Δθ可以表示為：

根據智能車輛在行駛過程中航向角Δθ是否發生變化，可以把其里程計模型分為兩種：直線模型和圓弧模型。在試驗小車進行自定位的過程中，盡量增加采樣頻率，縮小初始點與終止點之間的距離，從而盡可能地保證定位過程的準確性。由于會頻繁讀取里程計的信息來獲得智能車輛的位姿變化情況，因此，使用里程計的直線模型為主，在通過數據融合進行航向角計算時，需要使用圓弧模型。假設在t1時刻小車位姿為（x1,y1,θ1）T，在t2時刻小車位姿為（x2,y2,θ2）T，則表達式如下：

1.2.2 激光傳感器模型

試驗中忽略激光傳感器自轉時對不同角度的采樣時間差，二維激光雷達的數據觀測模型可以用條件概率進行表示：

式中：xk表示在k時刻智能車輛的位姿；zk表示激光觀測量。由于激光傳感器旋轉一周會進行多次采樣過程，由此可得：

假設激光傳感器每次掃描得到的點之間相互獨立，則對于每次完成一圈掃描得到的觀測模型概率密度可以表示為：

1.2.3 慣性測量單元模型

慣性測量單元(IMU)具有靈敏、高頻的特點，常作為其他傳感器的輔助傳感器使用，由物理理論可得：

式中：φ為旋轉角度；v為小車速度；Δt為慣性測量單元相鄰數據之間的時間差。

假設智能車輛的初始位置為p0=（0, 0,0）T，初始姿態為φ0=（0,0,0）T，R（φ0）表示初始姿態對應的旋轉矩陣。歐拉角Δφ=（Δφx,Δφy,Δφz）T，歐拉角對應的旋轉矩陣為R（Δφ）。

則進行多次迭代后，可以得出：

式中：ωk表示在k時刻IMU 所測量的角速度值。

利用加速度a和采樣間隔時間Δt，可以得到在三維空間下智能車輛的速度和位移變化情況：

式中：ak表示在k時刻IMU 所測得的加速度值；Rk表示在k時刻時IMU 在世界坐標系下的姿態。

1.3 智能車輛定位問題建模

定位問題可以看作一個坐標系在另一個坐標系下的求解問題。假設智能車輛在全局坐標系中位姿為（x1,y1,θ1）T，在小車局部坐標系下任意一點的坐標為(x1,y1)，經過齊次坐標變換可以通過平移變換矩陣T，旋轉變換矩陣R可轉換至全局坐標系下，其中矩陣T和矩陣R為：

智能車輛的定位經常被稱為位姿估計，而基于概率的狀態估計是最常用手段[12]。概率運動學模型在定位過程中起著狀態變換的作用，這即是條件密度，即：

式中：xt與xt-1表示智能車輛的位姿信息；ut表示運動控制，由里程計數據提供。這個模型表示在xt-1位姿基礎上執行控制ut后，智能車輛取得的運動狀態后驗概率分布。

圖2 智能車輛定位圖例

定位圖例如圖2 所示，陰影節點值(地圖m、測量值z和控制u)已知，時刻t的狀態依賴t-1 時刻狀態和控制ut，測量zt依賴t時刻狀態。

2 智能車輛測量校準分析

智能車輛在硬件裝配過程中會存在各種誤差，從軟件層面來說，系統在一定頻率下接收數據，也會存在一定誤差。因此，需要對智能車輛的IMU、線速度、角速度進行校準。

2.1 IMU 校準

在校準IMU 的過程中需對加速度計和陀螺儀分別進行校準，習慣上先對加速度計進行校準，然后利用校準后的加速度計信息來校準陀螺儀。加速度校準通常需將加速度計保持靜止狀態，根據測量值的二范數等于當地重力加速度來進行校準。陀螺儀校準過程主要包括Allan方差校準零偏噪聲(Bias)和優化方式求解尺度因子及軸偏差[13-14]。

2.2 線速度、角速度校準

本文設計了線速度、角速度校準試驗，在線速度校準中，通過校準文件使智能車輛前進1 m，將實際移動距離與理論移動距離(本文為1 m)的比值作為線速度修正系數，在角速度校準中，通過校準文件使智能車輛旋轉360°，將實際旋轉角度與理論旋轉角度(本文為360°)的比值作為角速度修正系數。最后通過測程法前進返回試驗對校準效果進行驗證，共進行5 次試驗，結果如圖3 和圖4 所示。

由圖3 可知，校準后小車距離初始點的距離更近，其運動精度得到明顯提高，通過對試驗過程的觀察以及對圖4 中的數據結果進行分析可知，造成小車運動偏離的主要原因是旋轉角度誤差較大，通過校準后該問題得到顯著改善。

圖3 校準前后小車距原點距離對比

圖4 校準前后小車航向角對比

3 無地圖環境下智能車輛定位方法

3.1 擴展卡爾曼濾波

擴展卡爾曼濾波[15]是借助線性濾波理論求解非線性問題的算法，如果被用作多傳感器的數據融合，可以將其中一個傳感器數據當作測量量，另一傳感器數據當作估計值進行融合。擴展卡爾曼濾波近似的主要思想就是線性化，最常通過泰勒展開進行線性化，然后對系統狀態進行估計。擴展卡爾曼濾波分為時間更新方程和測量更新方程兩部分，如圖5 所示。

圖5 擴展卡爾曼濾波算法流程

3.2 定位評價標準

定位評價標準是衡量一種定位算法好壞的指標[16]，本研究選用定位誤差、定位成功率作為評價指標，其計算方式如下。

3.2.1 定位誤差

定位誤差指智能車的真實位置與估計位置之間的位置偏差，其表達式為：

式中：(xt,yt,zt)代表智能車輛通過定位算法估算出的位置；(xe,ye,ze)代表智能車輛在運動過程中的實際位置，由于智能車輛在室內結構化環境下運動，這里zt=ze=0，在實際計算時還要考慮小車航偏角偏差：

式中：θt代表定位算法估計出的航偏角；θe代表真實位置的航偏角。

3.2.2 定位成功率

在某一精度范圍內，成功次數與總次數的比值，其表達式為：

3.2.3 定位實時性

指定位系統能否在規定時間內完成相應的邏輯處理并得到正確結果，實時性的高低是評價定位系統性能的重要指標。其影響因素包括硬件和軟件兩個層面。

3.3 多傳感器融合定位試驗

本文在robot_localization 功能包[17]的基礎上進行改進，對里程計及IMU 進行數據融合，該功能包是所有狀態估計節點的集合，每個狀態估計節點通過其使用的傳感器對小車在三維空間中的運動狀態進行估計。在原功能包的基礎上對其做了一些改進：

（1）修改了功能包中原有的參數配置讀取功能，將里程計、IMU 等傳感器相關模型及參數統一放置在參數配置文件中。根據小車實際的傳感器參數進行配置，保證了功能包中的傳感器模型與小車實際所用傳感器的一致性。

（2）根據小車實際參數(傳感器間外部參數、小車軸距、論據等參數)對小車進行了物理建模并將模型導入robot_localization 功能包，保證了小車實際尺寸與功能包中物理模型的一致性。

在robot_localization 功能包中，主要使用ekf_localization_node，該節點可以接收IMU 所測量的翻滾角、俯仰角、偏航角及速度和加速度，以及來自車輪編碼器的偏航速度等。在ekf_localization_node 狀態估計節點中，通常在只融合連續的位置數據(里程計、IMU)時，把世界坐標系和傳感器坐標系設置為一個，此時世界坐標系為odom。多傳感器融合下智能車輛TF 樹如圖6 所示。

圖6 多傳感器信息融合下的TF 樹

定位測試試驗中經常使用的有兩種方法：第1種是在實際環境中選取需要的定位點，讓智能車輛重復到達定位點，比較智能車輛在定位點的輸出值；第2 種是在實際環境下選取需要的定位點，讓智能車輛重復到達定位點，比較小車定位點實際值與輸出值。在無地圖情況下定位試驗采用第2 種測試方法。在該試驗中共進行了直線軌跡與曲線軌跡兩種試驗，為降低由操作失誤等不可控因素對試驗結果的干擾，本文對試驗結果根據格拉布斯(Grubbs)準則[18]進行了野值檢測與剔除，處理后的結果如圖7～10 所示。

圖7 直線軌跡距離誤差

圖8 直線軌跡航向角誤差

圖9 曲線軌跡距離誤差

圖10 曲線軌跡航向角誤差

由圖7 和圖9 可知，在無地圖情況下通過航跡推測法進行定位存在較為明顯的累積誤差并會不斷增加，不能通過外部傳感器或先驗地圖信息進行修正，因此，在無地圖情況下不做定位成功率評價。

在圖8 和圖10 中，角度誤差均值分別為1.15°與3.25°，該結果可以反映在試驗過程中小車按照既定軌跡行駛。

robot_localization 功能包是基于擴展卡爾曼濾波進行數據融合的，定位算法復雜度較低，實時性較好。在計算機上運行測試時，算法運行一次的時間穩定在10 ms 左右并且受計算機系統硬件因素影響。提高硬件系統配置可減少算法運行時間。

4 基于地圖的智能車輛定位方法

4.1 環境地圖構建

本文使用了ROS 開源社區中SLAM 算法——gmapping 功能包，該功能包訂閱智能車輛深度信息、里程計信息和IMU 信息，同時需要完成相應參數配置，然后即可創建該環境下的二維柵格地圖，如圖11所示。

圖11 試驗環境地圖

4.2 蒙特卡羅定位試驗

蒙特卡羅算法[19]通過使用一系列帶有權重的粒子模擬被估計狀態的后驗概率密度函數，又被稱為粒子濾波[20]定位。粒子濾波器是貝葉斯估計器的一種實現方式，在處理非線性情況時具有天然優勢，同時又能處理多峰分布情況。本文應用蒙特卡羅定位，定位方案如圖12 所示。

圖12 智能車輛定位方案

在定位過程中，智能車輛先從已知位置出發，首先利用內部傳感器(里程計、IMU)獲得自身狀態信息，然后利用擴展卡爾曼濾波算法融合里程計和IMU 的信息，對小車的位姿進行一次粗略估計，再使用外部傳感器(激光傳感器)掃描周圍環境，建立局部地圖后，利用粒子濾波算法對小車位姿進行估計，從而獲得智能車輛在全局中的位姿信息，其定位效果如圖13 和圖14 所示，圖中綠色的點代表對智能車輛的位置估計，從圖中可以看出粒子濾波能夠有效地對智能車輛進行定位。

圖13 粒子濾波前截圖

圖14 粒子濾波后截圖

由于傳感器精度有限，為了避免建圖誤差干擾，本次試驗選取第1 種定位測試方式，同樣進行了直線軌跡與曲線軌跡試驗，并對試驗結果進行野值檢測與剔除，處理后的數據結果如圖15～20 所示。

圖15 直線軌跡距離誤差

圖16 直線軌跡航向角誤差

圖17 曲線軌跡距離誤差

圖18 曲線軌跡航向角誤差

圖19 直線軌跡位置誤差結果

圖20 直線軌跡航線角誤差結果

圖15 和圖17 可知，定位誤差存在不斷累積增加的現象，但對比圖7 和9 來看，距離誤差有所減小，說明基于地圖的定位效果比無地圖定位有所提高。圖16 和圖20 中，航向角誤差均值分別為2.12°和3.15°，對比圖18 與圖20 來看，航向角誤差并無明顯改善。

4.3 定位結果分析

根據定位評價標準對該試驗結果進行分析。由圖19 和圖20 可知，在4 組測量共40 次定位結果中，出現了13 次不符合距離誤差指標的定位結果，成功27 次，出現了4 次不符合角度誤差指標的定位結果，成功36 次，可得直線軌跡定位成功率為：

由圖21 和圖22 可知，在4 組測量共50 次定位結果中，出現15 次不符合距離誤差指標的定位結果，成功35 次，出現了13 次不符合角度誤差指標的定位結果，成功37 次，可得曲線軌跡定位成功率為：

圖21 曲線軌跡位置誤差結果

圖22 曲線軌跡航向角誤差結果

在定位的實時性方面，基于地圖的定位采用蒙特卡羅定位算法，其本質是通過算法的迭代來逐步增加正確定位粒子的權重，忽略誤定位粒子，進而實現準確定位。因此，在有地圖的情況下，定位精度會隨著小車運行時間增長而有所提高。經過上述試驗，如圖13 和圖14 所示，小車在穩定行駛3 m后即可獲得成功率較高的定位結果。算法運行一次所用時間穩定在40 ms 左右，并且受計算機系統硬件影響。

5 結論

本文對實驗室內的智能車輛傳感器系統進行了參數校正，理論與實踐相結合，對于無地圖情況下的擴展卡爾曼濾波的航位推測定位技術、地圖構建以及有地圖情況下的蒙特卡羅定位進行了理論分析與試驗驗證的系統研究。試驗結果表明，航跡推測定位不適用于遠距離定位，存在較為明顯的累積誤差現象，基于地圖的定位會對累積誤差進行一定的校正，在該情況下位置定位成功率可達70%左右，角度估計成功率在直線軌跡情況下高達90%，曲線軌跡達到74%，表明了地圖的重要性。仍存在一定的改善空間，后續研究可以考慮使用Kinect 深度攝像機與激光雷達相結合，繼續提高定位精度。