基于ROS的路徑?jīng)Q策自主導(dǎo)航機器人

張博容，郟冰淋，郭 乾，劉 磊

（南開大學濱海學院，天津 300450）

0 引 言

現(xiàn)如今，隨著我國在電子技術(shù)領(lǐng)域的不斷發(fā)展，智能化設(shè)備正在逐漸進入到人們普遍的生活中，其中自主導(dǎo)航相關(guān)技術(shù)已經(jīng)被廣泛應(yīng)用于各個領(lǐng)域。在自主導(dǎo)航領(lǐng)域中，傳統(tǒng)車輛級的自主導(dǎo)航技術(shù)研究的技術(shù)門檻高、人力消耗與實驗場地、執(zhí)行工作效率低、不適應(yīng)于動態(tài)障礙，本文針對自主導(dǎo)航、路徑規(guī)劃、動態(tài)避障展開研究，其中路徑規(guī)劃技術(shù)的評價指標決定著機器人的工作效率。在眾多路徑規(guī)劃算法中，本文將優(yōu)化傳統(tǒng)路徑算法，使移動機器人在行進過程中的效率問題得到改善，得到一種高效、精準、智能的移動機器人。

在小型自主導(dǎo)航機器人技術(shù)中，關(guān)鍵的問題有兩個，其一是移動機器人的定位與導(dǎo)航，機器人自主導(dǎo)航主要包含以下三方面工作：第一，地圖構(gòu)建； 第二，定位； 第三，路徑規(guī)劃。解決這一問題可以采用代價地圖costmap 來繪制地圖，然后用AMCL 來定位機器人，最后結(jié)合Dijkstra 算法、DWA 算法、TEB 算法來確定機器人的行進路徑。其二對于路徑規(guī)劃問題進行更深入的研究，也就是路徑最優(yōu)問題，當給機器人規(guī)劃了多條路徑后，需要精準高效的完成任務(wù)。所以最優(yōu)路徑算法具有關(guān)鍵作用，路徑規(guī)劃嚴重影響著移動機器人的行進效果，因此對于路徑最優(yōu)問題，在整體的自主導(dǎo)航算法框架中，插入結(jié)合路徑?jīng)Q策算法。本文通過設(shè)置人工給定的多條路徑以及相應(yīng)環(huán)境下節(jié)點權(quán)重，機器人在已知環(huán)境下基于多條路徑中不同環(huán)境的節(jié)點權(quán)重進行加和，以此得出權(quán)重代價和，代價和最高為最優(yōu)路徑，最后機器人以最優(yōu)路徑行進。

綜上，本文基于ROS（Robot Operating System）自主導(dǎo)航系統(tǒng)利用costmap、AMCL 算法、Dijkstra 算法、DWA 算法、TEB 算法等技術(shù)，設(shè)計實現(xiàn)一種能路徑?jīng)Q策的導(dǎo)航算法框架，具體實現(xiàn)一種能路徑?jīng)Q策的自主導(dǎo)航機器人。

1 自主導(dǎo)航移動機器人機械結(jié)構(gòu)設(shè)計

1.1 阿克曼差速結(jié)構(gòu)

自主導(dǎo)航移動機器人采用兩驅(qū)四輪的動力驅(qū)動結(jié)構(gòu)，后輪方向固定，前輪可以轉(zhuǎn)向，從而完成機器人的整體轉(zhuǎn)向，具體涉及阿克曼轉(zhuǎn)向結(jié)構(gòu)，利用阿克曼轉(zhuǎn)向特性原理實現(xiàn)四輪轉(zhuǎn)向，理論分析如下：

阿克曼差速結(jié)構(gòu)如圖1所示。設(shè)機器人的前輪定位角度等于0，行走系統(tǒng)為剛性。在轉(zhuǎn)向時，全部車輪都繞同一個瞬時中心點做圓周滾動，且前內(nèi)輪與前外輪的轉(zhuǎn)角應(yīng)滿足下面關(guān)系式：

機器人自身車輪軸距L已知，角由IMU9250 慣性測量模塊測得，則求解如下方程：

可得瞬時轉(zhuǎn)向半徑R。在求得R后，逆解如下方程組：

可得到左、右前輪的轉(zhuǎn)向角。

綜上所述，此過程方法可以利用阿克曼轉(zhuǎn)向特性實現(xiàn)分別控制兩個單輪的單輪轉(zhuǎn)向。

1.2 阿克曼梯形

上述阿克曼差速結(jié)構(gòu)需要同時分別控制兩個前輪轉(zhuǎn)動，從而完成轉(zhuǎn)向。機器人移動在動態(tài)環(huán)境中，不免會出現(xiàn)惡劣環(huán)境，在惡劣環(huán)境中行進極有可能導(dǎo)致左、右輪不協(xié)調(diào)，產(chǎn)生不必要的行進失誤。阿克曼梯形結(jié)構(gòu)如圖2所示。依據(jù)阿克曼梯形結(jié)構(gòu)，可以優(yōu)化上述結(jié)構(gòu)，僅利用唯一一個舵機轉(zhuǎn)向，便可實現(xiàn)機器人整體轉(zhuǎn)向，具體涉及理論分析如下：

圖2 阿克曼梯形結(jié)構(gòu)

阿克曼梯形滿足阿克曼理論轉(zhuǎn)向特性的四連桿機構(gòu)，由式（4）確定：

梯形上底長度AB與兩主銷中心距及兩主銷中心線穿地點之距完全一致，由式（4）可計算出梯形內(nèi)角Qa，設(shè)AD、BC為機器人兩前輪位置，Qa為轉(zhuǎn)向角。則通過控制單一的角Qa變量，即可控制整個梯形的變化，利用阿克曼梯形理論，可以實現(xiàn)同時控制兩個前輪的轉(zhuǎn)向。在梯形機械結(jié)構(gòu)轉(zhuǎn)向過程中，前排的車輪轉(zhuǎn)向角度始終保持一定的關(guān)系，即Qb=Qa+x。這樣便無需考慮對每個前輪的單獨控制，本機器人通過STM32 主控板連接一個舵機來搭建阿克曼梯形實體硬件機械結(jié)構(gòu)，以此通過舵機的的運動改變阿克曼梯形，從而實現(xiàn)機器人的轉(zhuǎn)向過程。

2 自主導(dǎo)航移動機器人控制系統(tǒng)設(shè)計

自主導(dǎo)航移動機器人采用STM32 主控板作為底層結(jié)構(gòu)控制板，驅(qū)動底層結(jié)構(gòu)，通過PWM 控制電機驅(qū)動模塊控制電機，電機反饋回速度與里程數(shù)據(jù)用于算法處理，連接OLED 屏幕用于顯示速度、坐標等基礎(chǔ)信息，連接舵機控制阿克曼梯形轉(zhuǎn)向機械結(jié)構(gòu)完成轉(zhuǎn)向，通過I2C 接口連接IMU9250 慣性測量模塊，實現(xiàn)基坐標系轉(zhuǎn)換，獲取位姿相關(guān)數(shù)據(jù)，用于算法處理；采用JetsonNano 開發(fā)板來搭載ROS系統(tǒng)，通過USB 與STM32 開發(fā)板通信，通過USB 連接激光雷達與深度相機等主要傳感器，在建圖、導(dǎo)航過程中接收數(shù)據(jù)，通過USB 與WiFi 通信模塊連接，實現(xiàn)與虛擬機的通信，通過虛擬機來運行Rviz 等可視化工具，來觀測與調(diào)試機器人的運行結(jié)果，以便調(diào)整與驗證整體實驗效果。機器人系統(tǒng)圖如圖3所示。

圖3 機器人系統(tǒng)圖

3 定位和導(dǎo)航算法分析和實現(xiàn)

3.1 定位算法分析和實現(xiàn)

定位功能的實現(xiàn)過程中，ROS 含有一個AMCL 的功能包，AMCL（Adaptive Monte Carlo Localization）是移動機器人二維環(huán)境下的概率定位系統(tǒng)，它實現(xiàn)了自適應(yīng)蒙特卡羅定位方法。蒙特卡羅的基本思想是用隨機試驗來逼近得出問題的解，粒子濾波就采用了這一思想，把粒子濾波用在定位上就成蒙特卡羅定位。

粒子濾波一般有初始化、計算權(quán)重、重采樣和狀態(tài)轉(zhuǎn)移等步驟。機器人在二維地圖使用粒子濾波器推算機器人位置。

初始化步驟，將粒子隨機分布在二維地圖上，根據(jù)機器人激光雷達測得的與障礙物的距離值來比較各個粒子與機器人的距離值，從而進行計算權(quán)重步驟，以此得到粒子位置的權(quán)重，進行重采樣，即利用權(quán)重重新分布粒子權(quán)重高的位置分布粒子多一點，重復(fù)這樣的步驟，粒子逐步集中于機器人的位置，即完成定位。當機器人移動時，便進行狀態(tài)轉(zhuǎn)移步驟，即依據(jù)機器人移動后的新位置數(shù)據(jù)，重復(fù)上述步驟，則可定位每次機器人移動后的位置，流程如圖4所示。

圖4 機器人位置定位流程

上述流程蒙特卡羅定位方法結(jié)合粒子濾波，依據(jù)機器人激光雷達反饋的數(shù)據(jù)在二維地圖中實現(xiàn)定位，對此進行自適應(yīng)優(yōu)化解決如下問題：

（1）粒子數(shù)固定的問題。當機器人定位基本完成的時候，比如這些粒子都集中在一塊了，這個時候重采樣的粒子數(shù)可以少一點。

（2）機器人綁架的問題，當粒子的平均分數(shù)突然降低（意味著正確的粒子在某次迭代中被拋棄了），在全局再重新撒一些粒子（“綁架劫持”：機器人在已知自身位姿的情況下，得到了一個錯誤的位姿信息或者外界將其放到另外一個位姿，而里程計信息給出了錯誤的信息）。

3.2 導(dǎo)航算法分析和實現(xiàn)

3.2.1 全局路徑算法分析和實現(xiàn)

全局路徑規(guī)劃模塊使用ROS 導(dǎo)航功能包集中的默認全局路徑規(guī)劃器，此功能包涵蓋于ROS 導(dǎo)航功能包集中的move base 功能包中，其具體原理涉及Dijkstra 算法。

初始化步驟，將整個二維地圖網(wǎng)格化，可以理解為：化為一個個像素點，即柵格法，將每一個像素點作為一個節(jié)點。給每一個節(jié)點定義一個屬性：①可通過；②不可通過。

定義列表集合openlist 集合和closedlist 集合，每一個像素點（節(jié)點）都處在一個狀態(tài)集合openlist 或closedlist，分別屬于兩個集合（屬性為不可通過的像素點默認為closedlist狀態(tài)，該像素點（節(jié)點）就屬于closedlist 集合）。openlist 集合中為待考察節(jié)點，closedlist 集合中為已考察節(jié)點。先以初始位置像素點為父節(jié)點，其狀態(tài)為closedlist，以其為中心的九宮格的其余8 個節(jié)點為子節(jié)點，都附加狀態(tài)openlist，并計算中心點周圍8 個子節(jié)點，取最小值的子節(jié)點并作第二步中心點（父節(jié)點），從openlist 取出，放入closedlist，計算中心點周圍8 個像素點，累計路程。若點已存在openlist，則取路程小的方案。重復(fù)上述步驟，依次選取距離初始點最近的節(jié)點，直至到達目標點，即最終路徑由從起始點到目標點依次選取的最近節(jié)點構(gòu)成。起始點位置如圖5所示，以S 為起始點，Q 為目標點，從起點開始，每次走一格，不能跨格；共有8 個相鄰柵格可以走；若走（上/下/左/右）計路程為1；若走（左上/左下/右下/右上）計路程為2。

圖5 起點終點位置

起點為S 點，計算周圍點，父節(jié)點為S，取一個最小值的點作第二步中心點重復(fù)步驟，節(jié)點圖如圖6～圖8所示，得到的路徑如圖9所示（綠色路徑）。

圖6 節(jié)點圖（一）

圖7 節(jié)點圖（二）

圖8 節(jié)點圖（三）

圖9 最終路徑圖

基于Dijkstra 算法的全局路徑規(guī)劃流程如圖10所示。

圖10 基于Dijkstra 算法的全局路徑規(guī)劃流程

3.2.2 局部路徑算法分析和實現(xiàn)

局部路徑規(guī)劃模塊使用ROS 功能包集中的默認局部路徑規(guī)劃器，此功能包涵蓋于ROS 功能包集中的move base 功能包中，其具體原理涉及DWA（Dynamic Window Approach）算法。

基于DWA 速度采樣算法，完成局部路徑規(guī)劃，首先通過機器人的硬件傳感器IMU9250 慣性測量模塊反饋的數(shù)據(jù)，利用機器人的線性速度Vx、Vy和角速度ω，通過已獲得的Vx、Vy和ω結(jié)合微積分的思想建立小車的運動模型，當機器人相鄰時刻的間距內(nèi)（ms 級），當間隔趨向無窮小時，兩時刻點之間的運動軌跡可以看作直線，由y方向速度產(chǎn)生機器人的x、y坐標的變化。坐標變化圖如圖11所示。

圖11 坐標變化圖

機器人下一個狀態(tài)的位置為：

建立機器人運動模型后，根據(jù)硬件傳感器IMU9250 慣性測量模塊反饋的速度數(shù)據(jù)就可以推算出軌跡，得到軌跡后，利用方位角評價函數(shù)、距離評價函數(shù)和速度評價函數(shù)來判斷軌跡的好與壞。這三項函數(shù)每一項分別除以每一項的總和來計算三個部分的權(quán)重，從而影響小車選取路徑，使小車達到避開障礙物、朝著目標、以較快的速度行進的目標。

基于DWA 算法的局部路徑規(guī)劃流程，如圖12所示。

圖12 基于DWA 算法的局部路徑規(guī)劃流程

4 路徑?jīng)Q策算法設(shè)計和實現(xiàn)

路徑?jīng)Q策問題需要采用適當?shù)臄?shù)學模型完成選項的選擇，基于層次分析法可以實現(xiàn)這一目的，根據(jù)層次分析法將起始點到終點的過程看作一個決策問題，在起始點到目的地的路徑中可以引入多個參考的影響因素，通過對影響路徑的各個因素進行層次分析，得到多條傳輸路徑優(yōu)劣的排序，從而選擇相對較好的傳輸路徑。

將問題分為3 個層次：目標層A、準則層B、方案層C，將各層元素之間的關(guān)系用直線連接。本路徑?jīng)Q策模塊以路徑長度、擁擠狀況、地面情況作為路徑影響因素，基于層次分析法構(gòu)建的決策結(jié)構(gòu)如圖13所示。

圖13 層次分析法決策結(jié)構(gòu)

路徑選擇過程：對目標層，將準則層的3 個因素，即路徑長度（B1）、擁擠情況（B2）、地形情況（B3）的相對重要性構(gòu)成判斷矩陣。具體構(gòu)成方法是將元素之間的重要性進行兩兩對比，確定準則層對于目標的權(quán)重值；對比采用1～9比例標度法，見表1所列。

表1 比例標度法

對于比較因子而言，認為一樣重要為1 ∶1，強烈重要為9 ∶1，也可以取中間值6 ∶1 等，兩兩比較，將所得數(shù)值填入并排列形成判斷矩陣B，再構(gòu)造所有相對于不同準則的方案層判斷矩陣C。

利用特征值法求權(quán)重并進行一次性檢驗，為了能利用它的對應(yīng)特征根的特征向量為被比較因素的權(quán)向量，要限定一個容許的不一致范圍。

n為問題決策層次的數(shù)量，以構(gòu)建的決策結(jié)構(gòu)為例，含有目標層A、準則層B、方案層C，則問題決策層次數(shù)量n的值為3，每個矩陣已知，則可得出每個矩陣的最大特征值λmax，結(jié)合層數(shù)n。根據(jù)下列公式，可以計算出一致性指標CI 為：

已知隨機性一般指標RI，見表2所列。

表2 RI 值

根據(jù)下列公式，可以計算出一致性比例CR。

當CR<0.10 時，可認為判斷矩陣的一致性是可以接受的；否則，應(yīng)對判斷矩陣進行適當?shù)男拚?/p>

同理，對于第二層次的路徑長度、擁擠情況、地形情況，3 個判斷矩陣的一致性檢驗均通過。最后得到層次總排序權(quán)重值，通過比較總排序權(quán)重值，即可決策出符合預(yù)期的最佳目標路徑。

5 仿真和實驗測試

5.1 路徑?jīng)Q策算法的仿真實驗

為檢驗路徑?jīng)Q策算法是否可行，利用數(shù)學軟件和模擬場景來進行仿真實驗測試，模擬場景地圖如圖14所示。

圖14 模擬場景地圖

在場景模擬過程中，從起點A 到終點B，有3 條路徑，每條路徑由三個因素影響：路徑長度、擁擠情況、地形情況，見表3所列。

表3 場景模擬

三個因素又由三種特定情況（+，0，-）而決定，按照需要對路徑的需求進行權(quán)重分配。在本場景模擬中，預(yù)期的路徑是綜合行駛時間最短的路徑，根據(jù)圖14描述的各個路徑的影響因素情況，設(shè)定具體的比較因子數(shù)值用于決策算法，見表4、表5所列。

表4 準則層B

將比較因子數(shù)據(jù)引入算法模塊，在MATLAB 中執(zhí)行路徑?jīng)Q策算法并驗證。

通過MATLAB 編程，計算最終總排序權(quán)重值，得到結(jié)果見表6所列。

表6 準則層B 中各因素對于總目標的權(quán)重

驗證結(jié)果：因為1 號路線的總排序權(quán)重值最高，因此決策出1 號路線為目標路徑。雖然1 號路線地形情況最惡劣，但綜合決策，其仍是3 條路徑中綜合行駛時間最短的路徑，符合預(yù)期，即：所構(gòu)建的決策模塊可以完成路徑?jīng)Q策。

5.2 機器人部分運行截圖

運行結(jié)果圖如圖15所示。

圖15 運行結(jié)果圖

6 結(jié) 語

本文以ROS 自主導(dǎo)航機器人為對象，研究了阿克曼轉(zhuǎn)向結(jié)構(gòu)、機器人定位、路徑規(guī)劃、路徑?jīng)Q策等技術(shù)。自主導(dǎo)航移動機器人在移動過程中進行自適應(yīng)蒙特卡羅定位，并在定位過程中利用激光雷達實時構(gòu)建環(huán)境代價地圖，根據(jù)建立的代價地圖用戶自定義導(dǎo)航目標，根據(jù)用戶定義的導(dǎo)航目標，利用路徑?jīng)Q策算法進行路徑?jīng)Q策，從而決策出最佳路徑。在得到最佳路徑后，調(diào)用ROS 系統(tǒng)的自主導(dǎo)航模塊，利用Dijkstra 和DWA 算法，最終抵達目的地。本文在對機器人阿克曼轉(zhuǎn)向模型和ROS 系統(tǒng)的基礎(chǔ)上利用AMCL 算法、Dijkstra 算法、DWA 算法、層次分析法，實現(xiàn)ROS 自主導(dǎo)航機器人的路徑?jīng)Q策功能，具有較大的實用價值。