王建彬　張軒望　譚卓　侯明

摘 要：針對(duì)在封閉環(huán)境內(nèi)智能車(chē)如何實(shí)現(xiàn)自動(dòng)駕駛問(wèn)題，本文通過(guò)算法與理論的分析以及在仿真環(huán)境中進(jìn)行運(yùn)動(dòng)規(guī)劃的驗(yàn)證，進(jìn)而得到了一套低成本、簡(jiǎn)單易操控的自動(dòng)駕駛引導(dǎo)方案。經(jīng)實(shí)驗(yàn)結(jié)果證明，基于ROS平臺(tái)設(shè)計(jì)的智能車(chē)，在地圖構(gòu)建（SLAM）、定位、路徑規(guī)劃方面具有較好的穩(wěn)定性、實(shí)時(shí)性、快速性與準(zhǔn)確性。

關(guān)鍵詞：自動(dòng)駕駛 ROS 仿真

1 自動(dòng)駕駛的整體設(shè)計(jì)方案

這套針對(duì)封閉環(huán)境中既定路徑下車(chē)輛自動(dòng)駕駛的控制方案是基于ROS操作平臺(tái)實(shí)現(xiàn)的，ROS提供了大量的工具組合用來(lái)進(jìn)行功能的配置、啟動(dòng)、自檢、調(diào)試以及可視化。在ROS平臺(tái)中使用Gmapping算法針對(duì)激光雷達(dá)和IMU位姿傳感器檢測(cè)和采集的信息對(duì)周?chē)h(huán)境進(jìn)行二維柵格地圖構(gòu)建（SLAM）、采用AMCL算法來(lái)實(shí)現(xiàn)車(chē)輛在地圖中的準(zhǔn)確定位、最后通過(guò)A*算法進(jìn)行全局的路徑規(guī)劃。在路徑規(guī)劃中針對(duì)道路中臨時(shí)出現(xiàn)的障礙通過(guò)TEB算法進(jìn)行判別并進(jìn)行局部的路徑規(guī)劃以實(shí)現(xiàn)對(duì)其合理避讓。整套方案在結(jié)構(gòu)上具有適應(yīng)性廣泛、開(kāi)發(fā)成本低的特點(diǎn)，在不降低效率的前提下實(shí)現(xiàn)了對(duì)于自動(dòng)駕駛的功能。

2 基于Gmapping建圖功能的設(shè)計(jì)

SLAM全稱為Simultaneous Localization and Mapping，及定位的同時(shí)完成建圖。本節(jié)從Gmapping算法功能包開(kāi)始，結(jié)合重要參數(shù)信息展開(kāi)介紹SLAM建圖功能。

Gmapping運(yùn)行的理論基礎(chǔ)為通過(guò)從激光雷達(dá)和里程計(jì)分別獲取數(shù)組觀測(cè)向量信息和里程計(jì)測(cè)量信息，經(jīng)過(guò)TF轉(zhuǎn)換，輸出地圖信息和位姿估計(jì)。

3 定位以及導(dǎo)航算法功能介紹

AMCL全稱為自適應(yīng)蒙特卡洛定位，是在傳統(tǒng)MCL定位算法基礎(chǔ)上，添加KLD采樣實(shí)現(xiàn)功能改進(jìn)。針對(duì)已有的地圖，使用粒子濾波器跟蹤機(jī)器人位置姿態(tài)。AMCL算法解決了機(jī)器人綁架問(wèn)題，有效防止迭代過(guò)程中重要信息粒子的缺失，并極大程度地提高了運(yùn)算效率。

4 全局路徑規(guī)劃

全局路徑規(guī)劃代價(jià)地圖包含地圖層和膨脹層。并基于A*算法配置、設(shè)計(jì)全局路徑規(guī)劃器，通過(guò)全局代價(jià)題圖獲取單元格代價(jià)信息，通過(guò)計(jì)算尋找起點(diǎn)與終點(diǎn)之間最小代價(jià)路徑。本節(jié)將具體介紹A*算法的基本原理以及實(shí)現(xiàn)過(guò)程。A*算法第一部是將區(qū)域作方格化簡(jiǎn)化操作，將二維區(qū)域地圖簡(jiǎn)化為2維數(shù)組的形式，數(shù)組的每個(gè)元素對(duì)應(yīng)一個(gè)方格，便于計(jì)算操作。在簡(jiǎn)化搜索區(qū)域完成后，需標(biāo)注起始點(diǎn)、終點(diǎn)和障礙的位置信息。準(zhǔn)備工作完成后，將起始點(diǎn)放入開(kāi)放列表中，對(duì)其周?chē)陌藗€(gè)方格，計(jì)算F值，F(xiàn)值由G值和H值相加所得。G值對(duì)應(yīng)從起始點(diǎn)移動(dòng)到指定方格的移動(dòng)代價(jià)，H值為指定方格移動(dòng)到終點(diǎn)的估算成本。尋找F值最小的方格放入開(kāi)放列表中，將該方格作為新的起始點(diǎn)，重復(fù)上述操作，直到起始點(diǎn)周?chē)礁裰邪K點(diǎn)，則此時(shí)成功找到合適路徑。

5 局部路徑規(guī)劃

局部路徑規(guī)劃代價(jià)地圖包含障礙地圖層和膨脹層。該功能用于小車(chē)行進(jìn)過(guò)程中，對(duì)地圖中未記錄的障礙信息，能夠及時(shí)改變路徑避開(kāi)障礙。局部路徑規(guī)劃器是基于TEB算法實(shí)現(xiàn)的。

TEB算法核心圍繞位姿和記錄時(shí)間序列展開(kāi)。位姿及為機(jī)器人在地圖中的位置、姿態(tài)信息記錄不同時(shí)間段的位姿信息，經(jīng)過(guò)整合形成位姿序列。時(shí)間序列則由相鄰位姿狀態(tài)變化所經(jīng)歷的時(shí)間整合而成。將位姿序列和時(shí)間序列進(jìn)行合并，以全局路徑允許的最大路徑作為約束條件，通過(guò)加權(quán)目標(biāo)優(yōu)化獲取最優(yōu)的路徑點(diǎn)。

6 智能車(chē)在Gazebo中的仿真

實(shí)現(xiàn)基于ROS操作系統(tǒng)的智能車(chē)仿真，需要一個(gè)穩(wěn)定且功能強(qiáng)大的仿真的環(huán)境。ROS 官方專門(mén)提供了Gazebo物理仿真軟件。Gazebo仿真環(huán)境可以實(shí)現(xiàn)機(jī)器人在三維環(huán)境中完成復(fù)雜動(dòng)作。Gazebo不僅提供各種可二次開(kāi)發(fā)的數(shù)據(jù)庫(kù)，還可以與ROS系統(tǒng)中的其他平臺(tái)進(jìn)行通訊，不僅提高了開(kāi)發(fā)人員的工作效率也為ROS 平臺(tái)的開(kāi)發(fā)提供了更多的可能性。仿真具體過(guò)程的流程如下：

6.1 車(chē)模與運(yùn)動(dòng)控制器的建立

在ROS操作系統(tǒng)里面，一個(gè)完整的機(jī)器人仿真模型由一系列的link和joint組成，斑紋采用acrox描述文件來(lái)構(gòu)建車(chē)模的link（部件）與joint（連接件）的基本屬性。Link設(shè)置模型的各個(gè)部件的特性與名稱，link的三個(gè)屬性分別為，conllision，inertial分別與外觀，碰撞屬性和慣性特性相對(duì)應(yīng)。Joint則是連接兩個(gè)link的關(guān)節(jié)，將parent link與child link進(jìn)行連接，由于parent link與child link的運(yùn)動(dòng)關(guān)系不同，使得不同joint之間的連接類型不同，下表為仿真模型用到的主要joint類型參數(shù)?？刂破鞯呐渲迷诤艽蟪潭壬蠜Q定了車(chē)模的屬性。

智能車(chē)模型的運(yùn)動(dòng)控制器配置過(guò)程：

1）在配置模型屬性的過(guò)程中，模型的各部分joint已經(jīng)添加了gazebo的標(biāo)簽并且添加了相應(yīng)的控制器插件，接下來(lái)通過(guò)一個(gè)yaml文件，聲明仿真需要的controller以及相對(duì)應(yīng)的參數(shù)，并通過(guò)joint_state_controller來(lái)發(fā)布每個(gè)關(guān)節(jié)的實(shí)時(shí)狀態(tài)。

2）生成啟動(dòng)文件，在文件目錄下加載需要加載的controller

6.2 建圖：

首先，打開(kāi) gazebo和rviz的launch啟動(dòng)文件，此時(shí)將加載仿真車(chē)模及其仿真環(huán)境，以及啟動(dòng)gampping的功能文件。接著gazebo結(jié)點(diǎn)會(huì)自動(dòng)訂閱車(chē)模坐標(biāo)控制信息、里程計(jì)坐標(biāo)信息、激光雷達(dá)坐標(biāo)信息，結(jié)合AMCL算法實(shí)現(xiàn)小車(chē)自定位，為建圖做好準(zhǔn)備。然后，通過(guò)采用設(shè)計(jì)好的控制工具包提供的滑塊控制車(chē)模在環(huán)境中運(yùn)動(dòng)（如圖2所示），通過(guò)rviz可以觀察車(chē)模運(yùn)動(dòng)過(guò)程中地圖的掃描情況。在車(chē)模運(yùn)動(dòng)過(guò)程中，gampping算法將車(chē)模的坐標(biāo)控制信息、里程計(jì)坐標(biāo)信息、激光雷達(dá)坐標(biāo)信息進(jìn)行處理運(yùn)算實(shí)現(xiàn)二維地圖的構(gòu)建。完成建圖工作后使用 map_server 命令行工具將掃描好的地圖進(jìn)行保存。

6.3 導(dǎo)航：

首先，開(kāi)啟gazebo和rviz的仿真環(huán)境，在gazebo仿真環(huán)境中增設(shè)障礙物錐桶，以模擬真實(shí)環(huán)境中的路況變化。Rviz能夠加載顯示出新建好的二維柵格地圖。其次，分別開(kāi)啟全局路徑規(guī)劃、局部路徑規(guī)劃、AMCL定位的功能文件，自動(dòng)引導(dǎo)啟動(dòng)與定位和導(dǎo)航相關(guān)的多個(gè)節(jié)點(diǎn)。通過(guò)在rviz上使用2D-nav goal指定目標(biāo)點(diǎn)的位姿矢量，之后move_base節(jié)點(diǎn)自動(dòng)訂閱車(chē)模坐標(biāo)信息進(jìn)行定位，并由此計(jì)算出最佳路徑，由控制器發(fā)送指令控制車(chē)模沿路徑方向移動(dòng)至終點(diǎn)。

7 仿真結(jié)果的分析

經(jīng)過(guò)測(cè)試本實(shí)驗(yàn)設(shè)計(jì)的智能車(chē)可以在仿真環(huán)境中完成SLAM導(dǎo)航，Gmapping建立柵格地圖，躲避障礙物等功能，實(shí)現(xiàn)了Gzaebo在仿真環(huán)境中的研究。

為了測(cè)試智能車(chē)在仿真環(huán)境中的性能。通過(guò)激光雷達(dá)建立柵格地圖與環(huán)境模型進(jìn)行對(duì)比，驗(yàn)證實(shí)驗(yàn)結(jié)果的準(zhǔn)確性。將柵格地圖與環(huán)境模型進(jìn)行比較，可知智能車(chē)在環(huán)境建圖測(cè)試中表現(xiàn)良好，擁有著較為精準(zhǔn)的導(dǎo)航與建圖功能。圖4為測(cè)試結(jié)果。

8 結(jié)論

基于該方案設(shè)計(jì)的自主駕駛方案，經(jīng)過(guò)基于ROS平臺(tái)的 Gezabo 仿真實(shí)驗(yàn)，得到了準(zhǔn)確度較高的柵格地圖，經(jīng)與仿真環(huán)境中的賽道進(jìn)行對(duì)比，證實(shí)了該方案在校園清潔、 醫(yī)院消毒、封閉環(huán)境中的自動(dòng)駕駛與路徑規(guī)劃等應(yīng)用場(chǎng)景的可行性。

參考文獻(xiàn)：

[1]恩里克費(fèi)爾南德斯.ROS 機(jī)器人程序設(shè)計(jì)[M]. 機(jī)械工業(yè)出版社.2016.

[2]李鑫. 基于激光雷達(dá)的同時(shí)定位與建圖方法研究[D].長(zhǎng)沙.

[3]高翔，張濤.視覺(jué)SLAM十四講：從理論到實(shí)踐[M].北京：電子工業(yè)出版社，2017.