隨著自動駕駛技術(shù)的發(fā)展，在未知環(huán)境中智能汽車的定位技術(shù)成為該領(lǐng)域研究的核心。目前定位技術(shù)主要的解決方案是基于全球定位系統(tǒng)（GPS），但是在某些特殊的環(huán)境中如下車庫，沒有 GPS 信號如何解決定位問題就是本文研究的關(guān)鍵所在。

隨著自動駕駛技術(shù)的發(fā)展，在未知環(huán)境中智能汽車的定位技術(shù)成為該領(lǐng)域研究的核心。目前定位技術(shù)主要的解決方案是基于全球定位系統(tǒng)（GPS），但是在某些特殊的環(huán)境中如下車庫，沒有 GPS 信號如何解決定位問題就是本文研究的關(guān)鍵所在。

近年來，同步定位與地圖構(gòu)建（Simultaneous Localization and Mapping，SLAM）技術(shù)的日益成熟，配合多傳感器融合解決方案，自動駕駛車輛在未知環(huán)境無 GPS 信號的情況下，完成路徑規(guī)劃的自動駕駛?cè)蝿?wù)得以實現(xiàn)。

本文先介紹了自動駕駛系統(tǒng)概述、視覺激光融合的 SLAM 理論與算法，又基于 ROS 框架搭建了自動駕駛汽車的建圖與路徑規(guī)劃仿真實驗，最后完成了在地庫中的實車算法驗證實驗，并做了論文結(jié)論總結(jié)與自動駕駛技術(shù)的未來展望。

1 自動駕駛系統(tǒng)概述

自動駕駛汽車即無人駕駛智能汽車，在沒有人為參與的情況下，依靠車內(nèi)的控制系統(tǒng)與智能算法，通過多重傳感器數(shù)據(jù)融合控制汽車底層協(xié)議完成正常的車輛行駛功能。智能駕駛汽車是一個綜合的集成系統(tǒng)，包括了自動泊車系統(tǒng)、自動駕駛系統(tǒng)、障礙物停障系統(tǒng)等，又分為了感知層、決策層和控制層三個部分如圖 1 所示。

圖片

圖1 自動駕駛系統(tǒng)組成

其中感知層包括各路傳感器的數(shù)據(jù)采集、處理與融合等，更加精確和全面的感知周圍環(huán)境信息。決策層的輸入包括感知層的信息、路徑的規(guī)劃以及控制層反饋回來的數(shù)據(jù)，通過增強學(xué)習(xí)算法下發(fā)決策指令。決策指令包括了循跡、跟車、超車、剎車、轉(zhuǎn)向、調(diào)頭等等；最終通過控制層下發(fā) CAN 總線下發(fā)指令完成智能駕駛汽車的自動駕駛?cè)蝿?wù)，包括油門與剎車的控制、方向盤與擋位的控制等等。

自動駕駛汽車發(fā)展與研發(fā)中的核心技術(shù)是車輛線控技術(shù)和車輛精確定位技術(shù)。本論文主要分析車輛的精確定位，目前最常用的解決方法就是使用 GPS，可以讓汽車實時地得到自身的位置坐標(biāo)。本文則基于 SLAM 技術(shù)研究了一種新的定位方法。

結(jié)合 SLAM 技術(shù)，自動駕駛汽車的傳感器分別由攝像頭、激光雷達、車載毫米波雷達、車載超聲波雷達、慣導(dǎo)組合、工業(yè)控制器等組成，具體的安裝位置如圖 2 所示。

圖片

圖2 傳感器安裝圖

使用的自動駕駛控制流程圖如圖 3 所示。

圖3 控制流程圖

2 SLAM技術(shù)

SLAM（Simultaneous Localization and Mapping），同時定位與地圖構(gòu)建，主要解決的是自動駕駛汽車在未知環(huán)境中從未知位置開始移動，通過傳感器的輸入信號估計自身位姿并構(gòu)建空間環(huán)境地圖進行自身定位，實現(xiàn)自主導(dǎo)航。這里記自動駕駛汽車在各個時刻的狀態(tài)為 x1，x2，…，xk，其中 k 表示離散時間下標(biāo)。

下面分別用觀測模型和運動模型描述自動駕駛汽車的 SLAM 問題：

圖片

表示觀測量，表示當(dāng)前環(huán)境量，表示當(dāng)前的位姿狀態(tài)，表示當(dāng)前運動傳感器的讀數(shù)，都是噪聲值。

SLAM 技術(shù)是一個比較龐大的系統(tǒng)，其中包括前端里程計、后端優(yōu)化、回環(huán)檢測、建圖四大部分。本文主要介紹基于視覺激光融合的 SLAM 前端里程計的位姿估計與定位技術(shù)。

3 位姿估計算

視覺激光的數(shù)據(jù)融合可以讓自動駕駛汽車知道周圍環(huán)境的情況，檢測到障礙物的種類和對應(yīng)的距離深度，假設(shè)兩個關(guān)鍵幀圖像數(shù)據(jù)之間已經(jīng)匹配好的三維對應(yīng)點：

通過求解歐式變換 R，t， 使得，求解上述表達式采用迭代最近鄰點法（Iterative  Closest  Point，ICP）， 具體 ICP 算法的求解方法有兩種：線性代數(shù)法（SVD）和非線性優(yōu)化法。

4 線性代數(shù)法（SVD）

定義兩組點的質(zhì)心為：

對應(yīng)的平方項展開并去掉交叉項為零的部分，優(yōu)化目標(biāo)函數(shù)可以簡化為：

令：

同樣的方法展開只含有 R 的左側(cè)式子的平方和，并消去與 R 無關(guān)的項，最終化簡為需要優(yōu)化的式子：

先定義矩陣：

W 是一個三維矩陣，對其進行 SVD 分解：

其中 U 和 V 為對角陣，當(dāng) W 滿秩時，

求解 R 后，代回優(yōu)化目標(biāo)函數(shù)可求出 t。

5 非線性優(yōu)化法

求解 ICP 用非線性優(yōu)化的方法，是根據(jù)迭代思想尋找最優(yōu)解，與 PnP 算法類似。優(yōu)化過程中為了消除旋轉(zhuǎn)矩陣的自身約束，使用了李代數(shù)表示位姿，構(gòu)建的目標(biāo)函數(shù)為：

式子中只有一個未知量，所以可以采用李代數(shù)的擾動模型求解：

圖片

因為 ICP 算法中特征點已經(jīng)匹配好，所以構(gòu)建的最小二乘方程一定有解析解，就可以不進行迭代優(yōu)化。為了方便計算，可以采用兩種算法放在一起考慮，例如：如果深度信息未知，可以采用 3D-2D 的 PNP 重投影誤差；若深度信息已經(jīng)被測出，就可以用3D-3D 的 ICP 算法完成建模與求解。

6 基于濾波的定位算法

通過觀測模型解決了自動駕駛汽車的位姿估計和空間運動求解問題，還要解決自動駕駛汽車在構(gòu)建地圖中的定位問題。本論文主要采用的貝葉斯濾波中一種特殊濾波方式——粒子濾波。

貝葉斯濾波

自動駕駛汽車的當(dāng)前定位存在觀測值 z和位姿真實值 x，兩者之間是存在誤差的，從觀測值到位姿真實值是一個概率分布問題，是所有空間環(huán)境定位中的可能性事件分布。貝葉斯濾波的流程是：已知狀態(tài)量 t-1時刻的 xt-1概率分布和運動傳感器值 ut-1，在給定 t 時刻的觀測數(shù)據(jù) zt的情況下，估計出狀態(tài)量在 t 時刻的概率分布。對應(yīng)的貝葉斯濾波公式為：

化簡可得：

貝葉斯濾波的核心就是通過 t-1 時刻的后驗概率推導(dǎo)在 t 時刻的先驗概率，在通過 t 時刻的先驗概率更新 t 時刻的后驗概率。

粒子濾波算法

粒子濾波是基于蒙特卡洛的方法，用粒子集表示概率，尋找隨機樣本用來近似的描述概率密度函數(shù)，用樣本均值代替積分運算，進而獲得系統(tǒng)狀態(tài)的最小方差估計的過程。由于粒子濾波在非高斯、非線性系統(tǒng)和多峰分布表現(xiàn)出來的優(yōu)勢，可以用來估計帶噪聲的觀測數(shù)據(jù)中自動駕駛汽車的位姿和運動狀態(tài)。

粒子濾波可表示為：

其中表示自動駕駛汽車的位姿，表示當(dāng)前位姿所處環(huán)境跟地圖的匹配度的權(quán)重。如圖 4 所示為粒子近似分布示意圖。

圖片

圖4  粒子近似分布示意圖

實現(xiàn)流程

本論文中實現(xiàn)自動駕駛系統(tǒng)中粒子濾波算法的流程分為以下三步：

1. 用粒子進行狀態(tài)傳播：

2. 評價每一個粒子的權(quán)重：

3. 根據(jù)權(quán)重進行重采樣:

以概率接受，目的是去除低權(quán)重的粒子，其中粒子濾波的狀態(tài)傳播模型為：

因為在沒有 GPS 信號的情況下，無法知道自動駕駛汽車位姿的實際發(fā)布，從它的預(yù)測分布進行定位采樣如圖 5 所示，并用觀測模型進行計算權(quán)重，聯(lián)合權(quán)重再重新采樣來近似自動駕駛汽車的后驗概率分布。

圖片

圖5 定位采樣原理

概率越大，權(quán)重越大，定位越準(zhǔn)確，重定位需要復(fù)制的粒子越多，粒子權(quán)重就更符合后驗概率分布如圖 6，圖 7 所示。

圖6 權(quán)重評估

圖7 重采樣后的粒子分布

7 基于ROS的定位與建圖仿真實驗

本論文中應(yīng)用以上的 SLAM 位姿估計算法和粒子濾波定位算法，結(jié)合機器人操作系統(tǒng) ROS（Robot Operating System）進行算法驗證與仿真實驗。

ROS 是一個適用于自動駕駛汽車開源的元級操作系統(tǒng)。主要的優(yōu)點是可以為代碼復(fù)用提供支持、良好的通訊架構(gòu)、具備插件仿真工具、有強大的庫且兼容多種編程語言。它提供了操作系統(tǒng)應(yīng)有的功能，包括硬件抽象、底層設(shè)備控制、共用功能執(zhí)行、進程間消息傳遞，以及功能包管理。

在本仿真實驗中，用 URDF 機器人統(tǒng)一描述平臺構(gòu)建了自動駕駛汽車的模型，然后把自動駕駛汽車模型加入 Gazebo 物理仿真平臺進行仿真地下車庫的環(huán)境如圖 8 所示。

圖片

圖8 Gazebo物理環(huán)境仿真

通過粒子濾波算法進行模型定位仿真，用 gmapping 導(dǎo)航功能包在 Rviz 界面中顯示地圖的構(gòu)建情況，并在地圖中進行自主定位導(dǎo)航與路徑規(guī)劃如圖 9所示。

圖片

圖9 Rviz地圖構(gòu)建與自主導(dǎo)航

8 實車測試和結(jié)果分析

進行了理論分析與實驗算法仿真以后，用自動駕駛車輛進行測試與實驗，選擇的環(huán)境為沒有 GPS 信號的地下車庫如圖 10所示。

圖10 試車測試驗證圖

首先通過自動駕駛車輛的相對位姿估計算法進行求解當(dāng)前車的空間剛體運動矩陣，得到的位姿關(guān)系進行了整個地庫地圖拼接與構(gòu)建并在 Rviz 中進行顯示如圖 11 所示，轉(zhuǎn)換為 PCD 格式的點云地圖如圖 12 所示。

圖11 地庫圖RVIZ顯示

圖12 地庫點云地圖

將構(gòu)建好的整個車庫地圖選取部分路徑進行定位與導(dǎo)航試驗，通過 VL-SLAM 算法可以得到自動駕駛的位姿估計以及精度地圖。為了精確定位自動駕駛汽車在精度地圖中的位置關(guān)系，將 velodyne32 線激光雷達和單目視覺相機采集的數(shù)據(jù)融合后與精度地圖進行匹配，根據(jù)匹配的權(quán)重大小確定當(dāng)前的采集數(shù)據(jù)是在精度地圖中的哪個位置。

已知自身定位，然后再設(shè)定目標(biāo)位置，結(jié)合 ROS 中的gmapping 導(dǎo)航功能包就可以實現(xiàn)地圖環(huán)境中的路徑規(guī)劃任務(wù)。在 RVIZ 中的實驗結(jié)果如圖 13 所示，用紅色正方形標(biāo)出自動駕駛車輛正在初始化，加載汽車模型。

圖13 定位初始化

如圖 14 所示，汽車模型加載成功，并且定位準(zhǔn)確作為初始位置出發(fā)點。

圖14 確定初始位置

隨后自動駕駛汽車開啟自動駕駛功能，如圖 15所示為運動過程中自動駕駛汽車的精確定位。

圖15 在運動中精確定位