陳平　張少將　朱強(qiáng)　張慶余　余楚禮

摘 要：結(jié)合自動(dòng)駕駛汽車在無GPS信號(hào)的環(huán)境中如何解決定位問題，本文通過視覺激光傳感器融合的同時(shí)定位與地圖構(gòu)建技術(shù)（VL-SLAM），基于機(jī)器人操作系統(tǒng)（ROS）構(gòu)建了自動(dòng)駕駛汽車的物理仿真平臺(tái)和點(diǎn)云地圖可視化界面。應(yīng)用最近鄰迭代（ICP）相對(duì)位姿估計(jì)算法采集到地庫環(huán)境的高精度點(diǎn)云地圖，又應(yīng)用粒子濾波算法解決了在地圖中的準(zhǔn)確定位問題。最終，在實(shí)際地庫環(huán)境中進(jìn)行實(shí)車測(cè)試，驗(yàn)證該系統(tǒng)的算法及解決方案。

關(guān)鍵詞：視覺激光傳感器融合；SLAM；自動(dòng)駕駛汽車定位；ROS

1 引言

隨著自動(dòng)駕駛技術(shù)的發(fā)展，在未知環(huán)境中智能汽車的定位技術(shù)成為該領(lǐng)域研究的核心。目前定位技術(shù)主要的解決方案是基于全球定位系統(tǒng)（GPS），但是在某些特殊的環(huán)境中如地下車庫，沒有GPS信號(hào)如何解決定位問題就是本文研究的關(guān)鍵所在。近年來，同步定位與地圖構(gòu)建（Simultaneous Localization and Mapping，SLAM）技術(shù)的日益成熟，配合多傳感器融合解決方案，自動(dòng)駕駛車輛在未知環(huán)境無GPS信號(hào)的情況下，完成路徑規(guī)劃的自動(dòng)駕駛?cè)蝿?wù)得以實(shí)現(xiàn)。本文先介紹了自動(dòng)駕駛系統(tǒng)概述、視覺激光融合的SLAM理論與算法，又基于ROS框架搭建了自動(dòng)駕駛汽車的建圖與路徑規(guī)劃仿真實(shí)驗(yàn)，最后完成了在地庫中的實(shí)車算法驗(yàn)證實(shí)驗(yàn)，并做了論文結(jié)論總結(jié)與自動(dòng)駕駛技術(shù)的未來展望。

2 自動(dòng)駕駛系統(tǒng)概述

自動(dòng)駕駛汽車即無人駕駛智能汽車，在沒有人為參與的情況下，依靠車內(nèi)的控制系統(tǒng)與智能算法，通過多重傳感器數(shù)據(jù)融合控制汽車底層協(xié)議完成正常的車輛行駛功能。智能駕駛汽車是一個(gè)綜合的集成系統(tǒng)，包括了自動(dòng)泊車系統(tǒng)、自動(dòng)駕駛系統(tǒng)、障礙物停障系統(tǒng)等，又分為了感知層、決策層和控制層三個(gè)部分如圖1所示。其中感知層包括各路傳感器的數(shù)據(jù)采集、處理與融合等，更加精確和全面的感知周圍環(huán)境信息。決策層的輸入包括感知層的信息、路徑的規(guī)劃以及控制層反饋回來的數(shù)據(jù)，通過增強(qiáng)學(xué)習(xí)算法下發(fā)決策指令。決策指令包括了循跡、跟車、超車、剎車、轉(zhuǎn)向、調(diào)頭等等；最終通過控制層下發(fā)CAN總線下發(fā)指令完成智能駕駛汽車的自動(dòng)駕駛?cè)蝿?wù)，包括油門與剎車的控制、方向盤與擋位的控制等等。

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

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

3 SLAM融合算法

3.1 SLAM技術(shù)介紹

SLAM（Simultaneous Localization and Mapping），同時(shí)定位與地圖構(gòu)建，主要解決的是自動(dòng)駕駛汽車在未知環(huán)境中從未知位置開始移動(dòng)，通過傳感器的輸入信號(hào)估計(jì)自身位姿并構(gòu)建空間環(huán)境地圖進(jìn)行自身定位，實(shí)現(xiàn)自主導(dǎo)航。

這里記自動(dòng)駕駛汽車在各個(gè)時(shí)刻的狀態(tài)為x1，x2，…，xk，其中k表示離散時(shí)間下標(biāo)。下面分別用觀測(cè)模型和運(yùn)動(dòng)模型描述自動(dòng)駕駛汽車的SLAM問題：

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

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

3.2 位姿估計(jì)算法

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

p=﹛p1…pn﹜，p′=﹛p1′，…pn′﹜

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

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

定義第i對(duì)點(diǎn)的誤差項(xiàng)為；ei=pi-（Rpi′+t），構(gòu)建最小二乘函數(shù)，使得誤差的平方和達(dá)到極小值的R，t：J=

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

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

令：

qi=pi-p，qi′=pi′-p′

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

先定義矩陣：

W是一個(gè)三維矩陣，對(duì)其進(jìn)行SVD分解：

W=UXVT

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

R=UVT

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

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

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

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

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

3.2.3 基于濾波的定位算法

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

（1）貝葉斯濾波

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

化簡(jiǎn)可得：

p（xt|z1：t，u1：t）=ηp（z1|x1）∫p（xt|xt-1，ut）p（xt-1|z1：t-1，u1：t-1） dxt-1

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

（2）粒子濾波算法

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

粒子濾波可表示為：X={（xti，wti）[i=1，…，n]}，其中xti表示自動(dòng)駕駛汽車的位姿，wti表示當(dāng)前位姿所處環(huán)境跟地圖的匹配度的權(quán)重。如圖4所示為粒子近似分布示意圖。

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

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

xti～p（xt|ut，xt-1i ）

2.評(píng)價(jià)每一個(gè)粒子的權(quán)重：

wti=ηp（zt|xt ）

3.根據(jù)權(quán)重進(jìn)行重采樣，以wti的概率接受xti，目的是去除低權(quán)重的粒子。

其中粒子濾波的狀態(tài)傳播模型為：

bel'（xt）=∫p（xt|xt-1，ut）p（xt-1|z1：t-1，u1：t-1）dxt-1

因?yàn)樵跊]有GPS信號(hào)的情況下，無法知道自動(dòng)駕駛汽車位姿的實(shí)際發(fā)布，從它的預(yù)測(cè)分布進(jìn)行定位采樣如圖5所示，并用觀測(cè)模型進(jìn)行計(jì)算權(quán)重，聯(lián)合權(quán)重再重新采樣來近似自動(dòng)駕駛汽車的后驗(yàn)概率分布。概率越大，權(quán)重越大，定位越準(zhǔn)確，重定位需要復(fù)制的粒子越多，粒子權(quán)重就更符合后驗(yàn)概率分布如圖6，圖7所示。

4 基于ROS的定位與建圖仿真實(shí)驗(yàn)

本論文中應(yīng)用以上的SLAM位姿估計(jì)算法和粒子濾波定位算法，結(jié)合機(jī)器人操作系統(tǒng)ROS（Robot Operating System）進(jìn)行算法驗(yàn)證與仿真實(shí)驗(yàn)。ROS是一個(gè)適用于自動(dòng)駕駛汽車開源的元級(jí)操作系統(tǒng)。主要的優(yōu)點(diǎn)是可以為代碼復(fù)用提供支持、良好的通訊架構(gòu)、具備插件仿真工具、有強(qiáng)大的庫且兼容多種編程語言。它提供了操作系統(tǒng)應(yīng)有的功能，包括硬件抽象、底層設(shè)備控制、共用功能執(zhí)行、進(jìn)程間消息傳遞，以及功能包管理。

在本仿真實(shí)驗(yàn)中，用URDF機(jī)器人統(tǒng)一描述平臺(tái)構(gòu)建了自動(dòng)駕駛汽車的模型，然后把自動(dòng)駕駛汽車模型加入Gazebo物理仿真平真平臺(tái)進(jìn)行仿真地下車庫的環(huán)境如圖8所示，通過粒子濾波算法進(jìn)行模型定位仿真，用gmapping導(dǎo)航功能包在Rviz界面中顯示地圖的構(gòu)建情況，并在地圖中進(jìn)行自主定位導(dǎo)航與路徑規(guī)劃如圖9所示。

5 實(shí)車測(cè)試與結(jié)果分析

進(jìn)行了理論分析與實(shí)驗(yàn)算法仿真以后，用自動(dòng)駕駛車輛進(jìn)行測(cè)試與實(shí)驗(yàn)，選擇的環(huán)境為沒有GPS信號(hào)的地下車庫如圖10圖11所示。首先通過自動(dòng)駕駛車輛的相對(duì)位姿估計(jì)算法進(jìn)行求解當(dāng)前車的空間剛體運(yùn)動(dòng)矩陣，得到的位姿關(guān)系進(jìn)行了整個(gè)地庫地圖拼接與構(gòu)建并在Rviz中進(jìn)行顯示如圖12所示，轉(zhuǎn)換為PCD格式的點(diǎn)云地圖如圖13所示。

將構(gòu)建好的整個(gè)車庫地圖選取部分路徑進(jìn)行定位與導(dǎo)航試驗(yàn)，通過VL-SLAM算法可以得到自動(dòng)駕駛的位姿估計(jì)以及精度地圖。為了精確定位自動(dòng)駕駛汽車在精度地圖中的位置關(guān)系，將velodyne32線激光雷達(dá)和單目視覺相機(jī)采集的數(shù)據(jù)融合后與精度地圖進(jìn)行匹配，根據(jù)匹配的權(quán)重大小確定當(dāng)前的采集數(shù)據(jù)是在精度地圖中的哪個(gè)位置。已知自身定位，然后再設(shè)定目標(biāo)位置，結(jié)合ROS中的gmapping導(dǎo)航功能包就可以實(shí)現(xiàn)地圖環(huán)境中的路徑規(guī)劃任務(wù)。在RVIZ中的實(shí)驗(yàn)結(jié)果如圖14所示，用紅色正方形標(biāo)出自動(dòng)駕駛車輛正在初始化，加載汽車模型。如圖15所示，汽車模型加載成功，并且定位準(zhǔn)確作為初始位置出發(fā)點(diǎn)。隨后自動(dòng)駕駛汽車開啟自動(dòng)駕駛功能，如圖16所示為運(yùn)動(dòng)過程中自動(dòng)駕駛汽車的精確定位。

5.1 論文總結(jié)與未來展望

本論文主要研究的是在無GPS信號(hào)的特殊環(huán)境下，基于單目視覺與velodyne32線激光傳感器相互融合的自動(dòng)駕駛系統(tǒng)定位的解決方案。本論文概述了自動(dòng)駕駛的系統(tǒng)架構(gòu)，又分析了VL-SLAM的相對(duì)位姿估計(jì)求解算法和粒子濾波定位求解算法。在Linux操作系統(tǒng)安裝ROS環(huán)境搭建了自動(dòng)駕駛的物理仿真平臺(tái)和地圖可視化界面，最終在無GPS的地庫環(huán)境中進(jìn)行實(shí)車測(cè)試，完成了相對(duì)位姿估計(jì)、精度地圖構(gòu)建與定位導(dǎo)航的功能。

自動(dòng)駕駛系統(tǒng)是未來汽車行業(yè)的發(fā)展趨勢(shì)，比如傳感器數(shù)據(jù)融合、結(jié)合深度學(xué)習(xí)的自動(dòng)駕駛決策系統(tǒng)、以及高精度地圖的開發(fā)、基于大數(shù)據(jù)的智能網(wǎng)聯(lián)汽車等等都是未來的研究方向。

參考文獻(xiàn)：

[1]高翔.視覺SLAM十四講.北京：電子工業(yè)出版社，2017，130～178.

[2]沈彤.立體視覺的目標(biāo)測(cè)距系統(tǒng)[J].電子測(cè)量技術(shù)，2015，第38卷第4期.

[3]權(quán)美香.視覺SLAM綜述[J].智能系統(tǒng)學(xué)報(bào).2016，第11卷第6期.

[4]管敘軍.視覺導(dǎo)航技術(shù)發(fā)展綜述[J].航空兵器，2014，第5期.

[5]路研研.在軌航天器雙目視覺圖像采集與處理系統(tǒng)研制[M].黑龍江：哈爾濱工業(yè)大學(xué)，2015.

[6]胡春旭.ROS機(jī)器人開發(fā)實(shí)踐.北京：機(jī)械工程出版社，2017，214～284.

[7]段華旭.無人車基于雙目視覺的同時(shí)定位與地圖構(gòu)建[J].華中科技大學(xué)學(xué)報(bào).2015，第43卷增刊.

[8]Strasdat et.Visual SLAM：Why filter？，Image and Vision Computing，2012.

[9]Paz Lina M et al.Divide and Conquer：EKF SLAM，IEEE Transaction on Robotics，2008.

[10]Triggs B et.El.Bundle adjustment - a Modern Synthesis，Springer，2000.

[11]Kummerle Rainer，et.al.g2o：A General Framework for Graph Optimization，ICRA，2011.

[12]Endres et al.3-D Mapping with an RGB-D Camera，IEEE Transaction on Robotics，2014.