隨著自動駕駛技術的發展，在未知環境中智能汽車的定位技術成為該領域研究的核心。目前定位技術主要的解決方案是基於全球定位系統（GPS），但是在某些特殊的環境中如下車庫，沒有GPS信號如何解決定位問題就是本文研究的關鍵所在。
本文來源：猛獅無人駕駛實驗室、Kittcamp自動駕駛學院
隨著自動駕駛技術的發展，在未知環境中智能汽車的定位技術成為該領域研究的核心。目前定位技術主要的解決方案是基於全球定位系統（GPS），但是在某些特殊的環境中如下車庫，沒有GPS信號如何解決定位問題就是本文研究的關鍵所在。
近年來，同步定位與地圖構建（SimultaneousLocalizationandMapping，SLAM）技術的日益成熟，配合多傳感器融合解決方案，自動駕駛車輛在未知環境無GPS信號的情況下，完成路徑規劃的自動駕駛任務得以實現。
本文先介紹了自動駕駛系統概述、視覺雷射融合的SLAM理論與算法，又基於ROS框架搭建了自動駕駛汽車的建圖與路徑規劃仿真實驗，最後完成了在地庫中的實車算法驗證實驗，並做了論文結論總結與自動駕駛技術的未來展望。
1.自動駕駛系統概述
自動駕駛汽車即無人駕駛智能汽車，在沒有人為參與的情況下，依靠車內的控制系統與智能算法，通過多重傳感器數據融合控制汽車底層協議完成正常的車輛行駛功能。智能駕駛汽車是一個綜合的集成系統，包括了自動泊車系統、自動駕駛系統、障礙物停障系統等，又分為了感知層、決策層和控制層三個部分如圖1所示。
其中感知層包括各路傳感器的數據採集、處理與融合等，更加精確和全面的感知周圍環境信息。決策層的輸入包括感知層的信息、路徑的規劃以及控制層反饋回來的數據，通過增強學習算法下發決策指令。決策指令包括了循跡、跟車、超車、剎車、轉向、調頭等等；最終通過控制層下發CAN總線下發指令完成智能駕駛汽車的自動駕駛任務，包括油門與剎車的控制、方向盤與擋位的控制等等。
自動駕駛汽車發展與研發中的核心技術是車輛線控技術和車輛精確定位技術。本論文主要分析車輛的精確定位，目前最常用的解決方法就是使用GPS，可以讓汽車實時地得到自身的位置坐標。本文則基於SLAM技術研究了一種新的定位方法。
結合SLAM技術，自動駕駛汽車的傳感器分別由攝像頭、雷射雷達、車載毫米波雷達、車載超聲波雷達、慣導組合、工業控制器等組成，具體的安裝位置如圖2所示。
使用的自動駕駛控制流程圖如圖3所示。
2.SLAM技術
SLAM（SimultaneousLocalizationandMapping），同時定位與地圖構建，主要解決的是自動駕駛汽車在未知環境中從未知位置開始移動，通過傳感器的輸入信號估計自身位姿並構建空間環境地圖進行自身定位，實現自主導航。這裡記自動駕駛汽車在各個時刻的狀態為x1，x2，…，xk，其中k表示離散時間下標。
下面分別用觀測模型和運動模型描述自動駕駛汽車的SLAM問題：
表示觀測量，表示當前環境量，表示當前的位姿狀態，表示當前運動傳感器的讀數，
都是噪聲值。
SLAM技術是一個比較龐大的系統，其中包括前端裡程計、後端優化、迴環檢測、建圖四大部分。本文主要介紹基於視覺雷射融合的SLAM前端裡程計的位姿估計與定位技術。
3.位姿估計算
視覺雷射的數據融合可以讓自動駕駛汽車知道周圍環境的情況，檢測到障礙物的種類和對應的距離深度，假設兩個關鍵幀圖像數據之間已經匹配好的三維對應點：
通過求解歐式變換R，t，使得
，求解上述表達式採用迭代最近鄰點法（IterativeClosestPoint，ICP），具體ICP算法的求解方法有兩種：線性代數法（SVD）和非線性優化法。
4.線性代數法（SVD）
定義兩組點的質心為：
對應的平方項展開並去掉交叉項為零的部分，優化目標函數可以簡化為：
令：
同樣的方法展開只含有R的左側式子的平方和，並消去與R無關的項，最終化簡為需要優化的式子：
先定義矩陣：
W是一個三維矩陣，對其進行SVD分解：
其中U和V為對角陣，當W滿秩時，
求解R後，代回優化目標函數可求出t。
5.非線性優化法
求解ICP用非線性優化的方法，是根據迭代思想尋找最優解，與PnP算法類似。優化過程中為了消除旋轉矩陣的自身約束，使用了李代數表示位姿，構建的目標函數為：
式子中只有一個未知量，所以可以採用李代數的擾動模型求解：
因為ICP算法中特徵點已經匹配好，所以構建的最小二乘方程一定有解析解，就可以不進行迭代優化。為了方便計算，可以採用兩種算法放在一起考慮，例如：如果深度信息未知，可以採用3D-2D的PNP重投影誤差；若深度信息已經被測出，就可以用3D-3D的ICP算法完成建模與求解。
6.基於濾波的定位算法
通過觀測模型解決了自動駕駛汽車的位姿估計和空間運動求解問題，還要解決自動駕駛汽車在構建地圖中的定位問題。本論文主要採用的貝葉斯濾波中一種特殊濾波方式——粒子濾波。
（1）貝葉斯濾波
自動駕駛汽車的當前定位存在觀測值z和位姿真實值x，兩者之間是存在誤差的，從觀測值到位姿真實值是一個概率分布問題，是所有空間環境定位中的可能性事件分布。貝葉斯濾波的流程是：已知狀態量t-1時刻的xt-1概率分布和運動傳感器值ut-1，在給定t時刻的觀測數據zt的情況下，估計出狀態量在t時刻的概率分布。對應的貝葉斯濾波公式為：
化簡可得：
貝葉斯濾波的核心就是通過t-1時刻的後驗概率推導在t時刻的先驗概率，在通過t時刻的先驗概率更新t時刻的後驗概率。
（2）粒子濾波算法
粒子濾波是基於蒙特卡洛的方法，用粒子集表示概率，尋找隨機樣本用來近似的描述概率密度函數，用樣本均值代替積分運算，進而獲得系統狀態的最小方差估計的過程。由於粒子濾波在非高斯、非線性系統和多峰分布表現出來的優勢，可以用來估計帶噪聲的觀測數據中自動駕駛汽車的位姿和運動狀態。
粒子濾波可表示為：
其中表示自動駕駛汽車的位姿，表示當前位姿所處環境跟地圖的匹配度的權重。如圖4所示為粒子近似分布示意圖。
(3)實現流程
本論文中實現自動駕駛系統中粒子濾波算法的流程分為以下三步：
1.用粒子進行狀態傳播：
2.評價每一個粒子的權重：
3.根據權重進行重採樣:
以概率接受，目的是去除低權重的粒子，其中粒子濾波的狀態傳播模型為：
因為在沒有GPS信號的情況下，無法知道自動駕駛汽車位姿的實際發布，從它的預測分布進行定位採樣如圖5所示，並用觀測模型進行計算權重，聯合權重再重新採樣來近似自動駕駛汽車的後驗概率分布。
概率越大，權重越大，定位越準確，重定位需要複製的粒子越多，粒子權重就更符合後驗概率分布如圖6，圖7所示。
7.基於ROS的定位與建圖仿真實驗
本論文中應用以上的SLAM位姿估計算法和粒子濾波定位算法，結合機器人作業系統ROS（RobotOperatingSystem）進行算法驗證與仿真實驗。
ROS是一個適用於自動駕駛汽車開源的元級作業系統。主要的優點是可以為代碼復用提供支持、良好的通訊架構、具備插件仿真工具、有強大的庫且兼容多種程式語言。它提供了作業系統應有的功能，包括硬體抽象、底層設備控制、共用功能執行、進程間消息傳遞，以及功能包管理。
在本仿真實驗中，用URDF機器人統一描述平臺構建了自動駕駛汽車的模型，然後把自動駕駛汽車模型加入Gazebo物理仿真平臺進行仿真地下車庫的環境如圖8所示
通過粒子濾波算法進行模型定位仿真，用gmapping導航功能包在Rviz界面中顯示地圖的構建情況，並在地圖中進行自主定位導航與路徑規劃如圖9所示。
8.實車測試和結果分析
進行了理論分析與實驗算法仿真以後，用自動駕駛車輛進行測試與實驗，選擇的環境為沒有GPS信號的地下車庫如圖10所示。
首先通過自動駕駛車輛的相對位姿估計算法進行求解當前車的空間剛體運動矩陣，得到的位姿關係進行了整個地庫地圖拼接與構建並在Rviz中進行顯示如圖11所示，轉換為PCD格式的點雲地圖如圖12所示。
將構建好的整個車庫地圖選取部分路徑進行定位與導航試驗，通過VL-SLAM算法可以得到自動駕駛的位姿估計以及精度地圖。為了精確定位自動駕駛汽車在精度地圖中的位置關係，將velodyne32線雷射雷達和單目視覺相機採集的數據融合後與精度地圖進行匹配，根據匹配的權重大小確定當前的採集數據是在精度地圖中的哪個位置。
已知自身定位，然後再設定目標位置，結合ROS中的gmapping導航功能包就可以實現地圖環境中的路徑規劃任務。在RVIZ中的實驗結果如圖13所示，用紅色正方形標出自動駕駛車輛正在初始化，加載汽車模型。
如圖14所示，汽車模型加載成功，並且定位準確作為初始位置出發點。
隨後自動駕駛汽車開啟自動駕駛功能，如圖15所示為運動過程中自動駕駛汽車的精確定位。
文章由易車號作者提供