基于ROS無人駕駛車算法優化的研究

張偉民 戶肖劍 賈群喜

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

關鍵詞：Gmapping算法;AMCL算法;Move_base功能包;自主導航

中圖分類號：TP277 ? ? 文獻標識碼：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 ?引 ?言

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

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

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

二維環境地圖是ROS無人駕駛車通過傳感器對周圍環境進行采集分析后形成的數據信息，是確保導航定位的標準。ROS無人駕駛車將激光雷達、姿態傳感器和里程計信息相互融合，搭建完善的二維柵格地圖，為無人駕駛提供導航定位相關的數據消息。

1.1.1 ?建圖算法比較

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

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

1.1.2 ?Gmapping建圖算法分析

Gmapping建圖其實使用的是基于2D激光雷達的RBPF算法完成二維柵格地圖構建的SLAM算法。可以使用一個條件聯合概率分布公式來描述：

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

其中，p為概率，z1：t為ROS無人駕駛車從初始時刻到時間t時刻通過激光雷達和IMU傳感器獲得的數據。u1：t為ROS無人駕駛車從初始時刻到時間t時刻所走過的位移，m為地圖中的柵格點，x1：t為當前ROS無人駕駛車的軌跡狀態。

利用通用公式分布律：

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

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

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

（3）

則進一步可以使用p（x1：t|u1：t，z1：t）表示ROS智能機器人的路徑狀態概率，使用p（m|x1：t，z1：t）表示在已知路徑狀態和傳感器數據時，計算地圖構建概率點。

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

先進行轉換：

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）表示上一時刻的粒子群。每一個粒子都用運動學模型p（xt|xt-1，ut）進行狀態傳播，這樣就得到每個粒子對應的預測軌跡。對于每一個傳播之后的粒子，用觀測模型p（zt|xt）進行權重計算歸一化處理，這樣就可以得到該時刻的機器人軌跡，之后根據估計的軌跡以及觀測數據進行二維地圖構建。圖1為利用激光雷達并借助Gmapping算法建立的地圖。

1.2 ?AMCL定位算法研究

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

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

假設有N個觀測點，以z表示：

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

估計在某定點位置的概率為：

（7）

觀測點z為激光雷達和IMU獲得的數據，它是一個數學上的集合，含有N個觀測數據。m為使用Gmapping建圖算法構建的地圖信息。x為當前ROS機器人在二維地圖上的坐標。P為估計在某定點位置的概率值，n為從1開始的整數。

我們又在該算法的基礎上增加一個動態參數kv，將式（7）改為：

（8）

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

1.4 ?輪式里程計算法研究

輪式里程計與簡單的左右差速驅動方法不同，該算法通過實時測量的左右輪轉速信息推演機器人的輪式里程計信息。

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

繼而我們建立以下運動學模型，去實時推演當前ROS無人駕駛車的位置與姿態：

（9）

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

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

2.1 ?路徑曲率控制

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

（10）

其中，。

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

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

綜合以上分析，就可以根據采集到的3個點組成的三角形的外接圓的半徑的大小，來控制速度的大小。即當外接圓的半徑值越大，給定的速度值越大，外接圓的半徑值越小，給定的速度值越小。

當已知線速度，同時就可以根據小車的實際機械結構，測量出小車的最小轉向半徑R，繼而根據公式v=ωR，可以得出角速度ω。則角速度ω將可以被用來控制小車的轉向。

2.2 ?純路徑追蹤控制

對于阿克曼車體，因為其速度控制僅依賴于一個電機，轉向依賴于一個舵機，因此為了簡化控制模型，可以將此阿克曼車體等效為一個自行車模型，如圖5所示。

則關于運動學自行車模型中的各個狀態量的更新公式為：

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）

結合式（11）、式（12）與式（13），車輛的轉向角β則可以求得 ：

（15）

a為車體當前加速度，lf為自行車模型重心點到前輪的距離，lr為自行車模型重心點到前輪的距離。

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

3 ?ROS無人駕駛車的功能實現

3.1 ?ROS無人駕駛車自主導航

利用Gmapping建圖算法，得到一張二維地圖，提供給ROS無人駕駛車運行的環境。然后，結合AMCL定位算法，使其知道自身在地圖中的位置，即X軸、Y軸坐標。同時，輪式里程計的使用，將能使其得知自身離起點的位移，使其定位更為精確。

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

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

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

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

Move_base包主要利用ROS中的話題機制，并規劃出兩條路徑，一條是由起點到終點的最優路徑，另一條則是根據當前車體姿態規劃出的局部路徑。將這三者聯系在一起，即可實現無人駕駛車的自主導航。

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

3.2 ?ROS無人駕駛車避障

避障對于ROS無人駕駛車是一個主要的功能，主要分為靜態避障，動態避障：

（1）靜態避障：指智能機器人根據構建的二維柵格地圖，并借助各個傳感器數據，當障礙物出現在地圖中，能夠實時檢測到，并順利的規避過去。

（2）動態避障：對于智能機器人檢測范圍內正在運動著的物體或者行人，也能夠進行有效避障。即能夠實時規劃出安全快捷的路徑，且能夠實時跟隨。

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

4 ?結 ?論

本文利用ROS機器人操作系統，結合Gmapping建圖功能包、AMCL定位功能包和Move_base路徑規劃功能包，實現了無人駕駛車的自主導航與避障功能。并分析了兩種控制方案，本文最終了編程易實現的方案，實現了直道加速與彎道減速的控制。

參考文獻：

[1] 李業謙，陳春苗.基于ROS和激光雷達的移動機器人自動導航系統設計 [J].現代電子技術， 2020，43（10）：176-178+183.

[2] 杜邊境，李富強，劉勇，等.基于激光SLAM的移動機器人實驗室巡檢 [J].電子測試，2020（22）：52-53.

[3] 許曉梅.汽車轉向系統典型模型的對比研究 [D].長春：吉林大學，2012.

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

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

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