基于ROS無人駕駛車算法優(yōu)化的研究

張偉民 戶肖劍 賈群喜

摘 ?要：借助ROS機(jī)器人操作系統(tǒng)，通過實(shí)驗(yàn)分析并比較三種建圖算法，使用Gmapping算法并結(jié)合RPLIDAR生成二維地圖效果最佳。在原有AMCL算法的基礎(chǔ)上，添加了動態(tài)參數(shù)，使其粒子群收斂速度根據(jù)車體速度的改變而改變，對于無人駕駛車的實(shí)時(shí)定位會更加精確。創(chuàng)新性的使用路徑曲率控制方案，相較與純路徑追蹤控制，方便且更易編程實(shí)現(xiàn)。最后使用Move_base功能包實(shí)時(shí)規(guī)劃出最優(yōu)的全局與局部路徑，使無人駕駛車實(shí)現(xiàn)自主導(dǎo)航與避障的功能。

關(guān)鍵詞：Gmapping算法;AMCL算法;Move_base功能包;自主導(dǎo)航

中圖分類號：TP277 ? ? 文獻(xiàn)標(biāo)識碼：A 文章編號：2096-4706（2020）23-0080-04

Research on Algorithm Optimization of Autonomous Vehicle Based on ROS

ZHANG Weimin，HU Xiaojian，JIA Qunxi

（Luoyang Institute of Science and Technology，Luoyang ?471023，China）

Abstract：With the help of the ROS robot operating system，the three mapping algorithms are analyzed and compared through experiments，and the Gmapping algorithm combined with RPLIDAR to generate a two-dimensional map has the best effect. On the basis of the original AMCL algorithm，the dynamic parameters are added to make the convergence speed of particle swarm change according to the change of vehicle speed，so that the real-time positioning of the autonomous vehicle will be more accurate. The innovative path curvature control scheme is more convenient and easier to program than pure path tracking control. Finally，the Move_base function package is used to plan the optimal global and local paths in real time，so that the autonomous vehicle can realize the function of autonomous navigation and obstacle avoidance.

Keywords：Gmapping algorithm;AMCL algorithm;Move_base function pack;autonomous navigation

0 ?引 ?言

無人駕駛汽車的出現(xiàn)，帶來了新的經(jīng)濟(jì)增長點(diǎn)，而且更加的貼合共享經(jīng)濟(jì)，使殘疾人也能擁有私家車。但是在無人駕駛汽車迅速發(fā)展的同時(shí)，也存在著很多問題：如很多地區(qū)定位不精確，就無法正常導(dǎo)航;而且，部分道路臨時(shí)維護(hù)時(shí)，也不會即時(shí)更新到地圖;部分道路路況復(fù)雜，無人駕駛汽車可能不會順利通過;最重要的是相關(guān)交通事故法規(guī)，也未完全完善。因此作者以參加全國大學(xué)生智能車大賽和機(jī)器人競賽的經(jīng)驗(yàn)為基礎(chǔ)，自主搭建了無人駕駛車車體，利用ROS中開源的功能包，并不斷地創(chuàng)新優(yōu)化算法，使無人駕駛汽車能夠自主行駛。

1 ?ROS無人駕駛車各模塊算法研究

1.1 ?ROS無人駕駛車建圖算法研究

二維環(huán)境地圖是ROS無人駕駛車通過傳感器對周圍環(huán)境進(jìn)行采集分析后形成的數(shù)據(jù)信息，是確保導(dǎo)航定位的標(biāo)準(zhǔn)。ROS無人駕駛車將激光雷達(dá)、姿態(tài)傳感器和里程計(jì)信息相互融合，搭建完善的二維柵格地圖，為無人駕駛提供導(dǎo)航定位相關(guān)的數(shù)據(jù)消息。

1.1.1 ?建圖算法比較

二維地圖的建立算法基本上都是基于概率估計(jì)的，比如Gmapping、Hector-SLAM、谷歌Cartographer算法等。Gmapping算法是研究最普遍的SLAM算法，因此，該算法目前有非常高的成熟度。而且，該算法建圖效果精準(zhǔn)，地圖也無漏隙。Hector-SLAM建圖算法則對激光雷達(dá)的掃描頻率要求特別高，使用此算法必須借助價(jià)格昂貴的雷達(dá)。而且Hector-SLAM算法在ROS無人駕駛車快速轉(zhuǎn)向的時(shí)候容易發(fā)生錯誤匹配，導(dǎo)致搭建的地圖發(fā)生錯位。谷歌Cartographer算法結(jié)合RPLIDAR數(shù)據(jù)、底盤odom數(shù)據(jù)、IMU數(shù)據(jù)，再通過相應(yīng)的建圖優(yōu)化算法，構(gòu)造出一個(gè)幾乎沒有缺陷的二維地圖。

結(jié)合以上分析，Gmapping建圖算法原理相較于其他算法比較簡單，而且使用該算法構(gòu)建出的地圖也非常精確，因而本文研究采用該算法構(gòu)建地圖。

1.1.2 ?Gmapping建圖算法分析

Gmapping建圖其實(shí)使用的是基于2D激光雷達(dá)的RBPF算法完成二維柵格地圖構(gòu)建的SLAM算法?？梢允褂靡粋€(gè)條件聯(lián)合概率分布公式來描述：

p（x1：t，m|u1：t，z1：t） ? ? ? ? ? ? ? ? ? ? ?（1）

其中，p為概率，z1：t為ROS無人駕駛車從初始時(shí)刻到時(shí)間t時(shí)刻通過激光雷達(dá)和IMU傳感器獲得的數(shù)據(jù)。u1：t為ROS無人駕駛車從初始時(shí)刻到時(shí)間t時(shí)刻所走過的位移，m為地圖中的柵格點(diǎn)，x1：t為當(dāng)前ROS無人駕駛車的軌跡狀態(tài)。

利用通用公式分布律：

p（A，B|Z）=p（A|Z）p（B|A，Z） ? ? ? ? ?（2）

其中，A，B，Z為3種不同的隨機(jī)變量，對式（2）進(jìn)行化簡得：

p（x1：t，m|u1：t，z1：t）=p（x1：t|u1：t，z1：t）p（m|x1：t，z1：t）

（3）

則進(jìn)一步可以使用p（x1：t|u1：t，z1：t）表示ROS智能機(jī)器人的路徑狀態(tài)概率，使用p（m|x1：t，z1：t）表示在已知路徑狀態(tài)和傳感器數(shù)據(jù)時(shí)，計(jì)算地圖構(gòu)建概率點(diǎn)。

由于Gmapping建圖算法使用粒子濾波來估計(jì)當(dāng)前ROS無人駕駛車位置，同時(shí)將粒子集合一起組建成柵格地圖，所以每一個(gè)粒子就包含了位置姿態(tài)信息和地圖信息，利用貝葉斯準(zhǔn)則對p（x1：t|u1：t，z1：t）進(jìn)行化簡。

先進(jìn)行轉(zhuǎn)換：

p（x1：t|u1：t，z1：t）=p（x1：t|x1：t|zt，u1：t，z1：t-1） ? （4）

再將zt消掉，并利用貝葉斯公式化簡，最后得到：

（5）

其中，η為歸一化變量，接著就可以使用p（x1：t-1|u1：t-1，z1：t-1）表示上一時(shí)刻的粒子群。每一個(gè)粒子都用運(yùn)動學(xué)模型p（xt|xt-1，ut）進(jìn)行狀態(tài)傳播，這樣就得到每個(gè)粒子對應(yīng)的預(yù)測軌跡。對于每一個(gè)傳播之后的粒子，用觀測模型p（zt|xt）進(jìn)行權(quán)重計(jì)算歸一化處理，這樣就可以得到該時(shí)刻的機(jī)器人軌跡，之后根據(jù)估計(jì)的軌跡以及觀測數(shù)據(jù)進(jìn)行二維地圖構(gòu)建。圖1為利用激光雷達(dá)并借助Gmapping算法建立的地圖。

1.2 ?AMCL定位算法研究

AMCL對MCL算法進(jìn)行了優(yōu)化，借助RPLIDAR傳感器的360度掃描數(shù)據(jù)，實(shí)時(shí)獲取無人駕駛車的位姿，并用粒子不斷地進(jìn)行概率估計(jì)，且可視化成箭頭標(biāo)識。當(dāng)無人駕駛車行駛時(shí)，AMCL位姿粒子的估計(jì)度會逐漸達(dá)到飽和，且粒子也將會逐漸匯聚到一塊，實(shí)時(shí)跟隨無人駕駛車移動。

在二維柵格地圖中，粒子的分布度由初始化條件決定，初始化條件即在調(diào)用功能包時(shí)需要配置的初始參數(shù)，包括雷達(dá)的掃描頻率、最大掃描范圍，及粒子的初始化大小、覆蓋范圍等。

假設(shè)有N個(gè)觀測點(diǎn)，以z表示：

z={z1，z2，…，zN} ? ? ? ? ? ? ? ? ? ? ? ? ? ?（6）

估計(jì)在某定點(diǎn)位置的概率為：

（7）

觀測點(diǎn)z為激光雷達(dá)和IMU獲得的數(shù)據(jù)，它是一個(gè)數(shù)學(xué)上的集合，含有N個(gè)觀測數(shù)據(jù)。m為使用Gmapping建圖算法構(gòu)建的地圖信息。x為當(dāng)前ROS機(jī)器人在二維地圖上的坐標(biāo)。P為估計(jì)在某定點(diǎn)位置的概率值，n為從1開始的整數(shù)。

我們又在該算法的基礎(chǔ)上增加一個(gè)動態(tài)參數(shù)kv，將式（7）改為：

（8）

此時(shí)，粒子收斂的概率將會根據(jù)車體速度的改變而改變。車體速度快時(shí)，粒子收斂速度將會略微降低，以免出現(xiàn)快速下的誤估計(jì);車體速度快慢時(shí)，則相反。將此算法應(yīng)用在ROS無人駕駛車上，可以發(fā)現(xiàn)粒子在初始時(shí)刻聚集度就較高，成功地實(shí)現(xiàn)了算法的優(yōu)化，如圖2中聚集群所示。

1.4 ?輪式里程計(jì)算法研究

輪式里程計(jì)與簡單的左右差速驅(qū)動方法不同，該算法通過實(shí)時(shí)測量的左右輪轉(zhuǎn)速信息推演機(jī)器人的輪式里程計(jì)信息。

在無人駕駛車的底盤上，我們增加編碼器裝置測得車輪實(shí)時(shí)的線速度vt，根據(jù)線速度與角速度關(guān)系，可以得到角速度 ，R為無人駕駛車的轉(zhuǎn)向半徑。

繼而我們建立以下運(yùn)動學(xué)模型，去實(shí)時(shí)推演當(dāng)前ROS無人駕駛車的位置與姿態(tài)：

（9）

θt為前輪轉(zhuǎn)向角，dt為極短的時(shí)間段，則可以使用以上模型去實(shí)時(shí)獲取車體在橫縱兩個(gè)方向的位移xt+1與yt+1，繼而給無人駕駛車提供精確的位姿信息，使其更穩(wěn)定的行駛。

2 ?ROS無人駕駛車控制算法

2.1 ?路徑曲率控制

我們采用外接圓半徑公式來控制速度，假設(shè)在△ABC中，已知三邊a，b，c那么它的外接圓的半徑R的公式為：

（10）

其中，。

三角形外接圓的圓心被稱作三角形的外心，即三角形三邊垂直平分線的交點(diǎn)，而三角形的三個(gè)頂點(diǎn)就在此外接圓上。我們可以得知關(guān)于三角形外接圓外心的性質(zhì)：銳角三角形的外心在三角形內(nèi);直角三角形的外心在斜邊上，與斜邊中點(diǎn)重合;鈍角三角形的外心在三角形外;等邊三角形外心與內(nèi)心為同一點(diǎn)。

我們以最典型的彎道分析該算法，可以看到，在圖3中，采集的三個(gè)點(diǎn)A、B、C，組成了一個(gè)鈍角三角形，在圖4中的三個(gè)點(diǎn)組成了一個(gè)銳角三角形。而經(jīng)過以上的性質(zhì)可知，鈍角三角形的外接圓半徑較銳角三角形的外接圓半徑長。除此之外，可以看到，圖4中的彎道曲率明顯比圖3中的彎道曲率大。

綜合以上分析，就可以根據(jù)采集到的3個(gè)點(diǎn)組成的三角形的外接圓的半徑的大小，來控制速度的大小。即當(dāng)外接圓的半徑值越大，給定的速度值越大，外接圓的半徑值越小，給定的速度值越小。

當(dāng)已知線速度，同時(shí)就可以根據(jù)小車的實(shí)際機(jī)械結(jié)構(gòu)，測量出小車的最小轉(zhuǎn)向半徑R，繼而根據(jù)公式v=ωR，可以得出角速度ω。則角速度ω將可以被用來控制小車的轉(zhuǎn)向。

2.2 ?純路徑追蹤控制

對于阿克曼車體，因?yàn)槠渌俣瓤刂苾H依賴于一個(gè)電機(jī)，轉(zhuǎn)向依賴于一個(gè)舵機(jī)，因此為了簡化控制模型，可以將此阿克曼車體等效為一個(gè)自行車模型，如圖5所示。

則關(guān)于運(yùn)動學(xué)自行車模型中的各個(gè)狀態(tài)量的更新公式為：

X軸方向的位移xt+1：

xt+1=xt+vtcos（ψt+β）·dt ? ? ? ? ? ? ? ? ? ? （11）

Y軸方向的位移yt+1：

yt+1=yt+vtsin（ψt+β）·dt ? ? ? ? ? ? ? ? ? ?（12）

兩輪中心距與Y軸的夾角ψt+1：

（13）

速度變化△v：

△v=vt+1-vt-a·dt ? ? ? ? ? ? ? ? ? ? ? ? ? （14）

結(jié)合式（11）、式（12）與式（13），車輛的轉(zhuǎn)向角β則可以求得 ：

（15）

a為車體當(dāng)前加速度，lf為自行車模型重心點(diǎn)到前輪的距離，lr為自行車模型重心點(diǎn)到前輪的距離。

上述純路徑追蹤方案相較于路徑曲率控制，在編程實(shí)現(xiàn)時(shí)，需要調(diào)節(jié)的參數(shù)過多，而且算法實(shí)現(xiàn)要最理想的效果也很困難。所以，我們選擇路徑曲率方案來實(shí)現(xiàn)ROS無人駕駛車得控制。

3 ?ROS無人駕駛車的功能實(shí)現(xiàn)

3.1 ?ROS無人駕駛車自主導(dǎo)航

利用Gmapping建圖算法，得到一張二維地圖，提供給ROS無人駕駛車運(yùn)行的環(huán)境。然后，結(jié)合AMCL定位算法，使其知道自身在地圖中的位置，即X軸、Y軸坐標(biāo)。同時(shí)，輪式里程計(jì)的使用，將能使其得知自身離起點(diǎn)的位移，使其定位更為精確。

ROS是一個(gè)分布式的進(jìn)程框架，這些進(jìn)程被封裝在易于被分享和發(fā)布的程序包和功能包中。從而我們就可以將我們要使用的功能包聯(lián)系起來。

對于ROS無人駕駛車來說，主要使用3大功能包，如圖6所示。

Gmapping功能包主要利用上述Gmapping建圖算法，在提供里程計(jì)信息后，再結(jié)合激光雷達(dá)建立一個(gè)二維柵格地圖。此二維柵格地圖可利用rviz插件可視化。

AMCL功能包利用蒙特卡羅概率統(tǒng)計(jì)定位方法，針對已有的二維地圖，并使用粒子濾波器跟蹤ROS無人駕駛車的姿態(tài)。

Move_base包主要利用ROS中的話題機(jī)制，并規(guī)劃出兩條路徑，一條是由起點(diǎn)到終點(diǎn)的最優(yōu)路徑，另一條則是根據(jù)當(dāng)前車體姿態(tài)規(guī)劃出的局部路徑。將這三者聯(lián)系在一起，即可實(shí)現(xiàn)無人駕駛車的自主導(dǎo)航。

如圖7所示，二維地圖中的虛線線條即是Move_base功能包規(guī)劃出的全局路徑。

3.2 ?ROS無人駕駛車避障

避障對于ROS無人駕駛車是一個(gè)主要的功能，主要分為靜態(tài)避障，動態(tài)避障：

（1）靜態(tài)避障：指智能機(jī)器人根據(jù)構(gòu)建的二維柵格地圖，并借助各個(gè)傳感器數(shù)據(jù)，當(dāng)障礙物出現(xiàn)在地圖中，能夠?qū)崟r(shí)檢測到，并順利的規(guī)避過去。

（2）動態(tài)避障：對于智能機(jī)器人檢測范圍內(nèi)正在運(yùn)動著的物體或者行人，也能夠進(jìn)行有效避障。即能夠?qū)崟r(shí)規(guī)劃出安全快捷的路徑，且能夠?qū)崟r(shí)跟隨。

對于ROS無人駕駛車，當(dāng)基于Gmapping算法建立的二維地圖實(shí)際環(huán)境中存在障礙物，比如錐桶、行人等，無人駕駛智能車首先通過雷達(dá)傳感器進(jìn)行探測，探測到障礙物時(shí)，將數(shù)據(jù)傳送到車體上的處理器中，然后，再利用ROS之間的話題機(jī)制，發(fā)送到Move_base功能包中，實(shí)時(shí)規(guī)劃出最優(yōu)避障全局路徑與局部路徑，并發(fā)出速度與轉(zhuǎn)向話題，使無人駕駛車有效避開障礙物，如圖8所示。

4 ?結(jié) ?論

本文利用ROS機(jī)器人操作系統(tǒng)，結(jié)合Gmapping建圖功能包、AMCL定位功能包和Move_base路徑規(guī)劃功能包，實(shí)現(xiàn)了無人駕駛車的自主導(dǎo)航與避障功能。并分析了兩種控制方案，本文最終了編程易實(shí)現(xiàn)的方案，實(shí)現(xiàn)了直道加速與彎道減速的控制。

參考文獻(xiàn)：

[1] 李業(yè)謙，陳春苗.基于ROS和激光雷達(dá)的移動機(jī)器人自動導(dǎo)航系統(tǒng)設(shè)計(jì) [J].現(xiàn)代電子技術(shù)， 2020，43（10）：176-178+183.

[2] 杜邊境，李富強(qiáng)，劉勇，等.基于激光SLAM的移動機(jī)器人實(shí)驗(yàn)室巡檢 [J].電子測試，2020（22）：52-53.

[3] 許曉梅.汽車轉(zhuǎn)向系統(tǒng)典型模型的對比研究 [D].長春：吉林大學(xué)，2012.

[4] 任春華，陳燦，王滿喜，等.一種慣性測量與距離組合的定位修正方法研究 [J].儀器儀表學(xué)報(bào)，2013，34（11）：2513-2519.

[5] 李博.GP-SLAM：基于激光雷達(dá)的新型同時(shí)定位與建圖算法 [D].杭州：浙江大學(xué)，2020.

作者簡介：張偉民（1978.12-），男，漢族，河南洛陽人，講師，碩士研究生，研究方向：自動化。