基于ROS的路徑決策自主導航機器人

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

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

0 引 言

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

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

綜上，本文基于ROS（Robot Operating System）自主導航系統利用costmap、AMCL 算法、Dijkstra 算法、DWA 算法、TEB 算法等技術，設計實現一種能路徑決策的導航算法框架，具體實現一種能路徑決策的自主導航機器人。

1 自主導航移動機器人機械結構設計

1.1 阿克曼差速結構

自主導航移動機器人采用兩驅四輪的動力驅動結構，后輪方向固定，前輪可以轉向，從而完成機器人的整體轉向，具體涉及阿克曼轉向結構，利用阿克曼轉向特性原理實現四輪轉向，理論分析如下：

阿克曼差速結構如圖1所示。設機器人的前輪定位角度等于0，行走系統為剛性。在轉向時，全部車輪都繞同一個瞬時中心點做圓周滾動，且前內輪與前外輪的轉角應滿足下面關系式：

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

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

可得到左、右前輪的轉向角。

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

1.2 阿克曼梯形

上述阿克曼差速結構需要同時分別控制兩個前輪轉動，從而完成轉向。機器人移動在動態環境中，不免會出現惡劣環境，在惡劣環境中行進極有可能導致左、右輪不協調，產生不必要的行進失誤。阿克曼梯形結構如圖2所示。依據阿克曼梯形結構，可以優化上述結構，僅利用唯一一個舵機轉向，便可實現機器人整體轉向，具體涉及理論分析如下：

圖2 阿克曼梯形結構

阿克曼梯形滿足阿克曼理論轉向特性的四連桿機構，由式（4）確定：

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

2 自主導航移動機器人控制系統設計

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

圖3 機器人系統圖

3 定位和導航算法分析和實現

3.1 定位算法分析和實現

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

粒子濾波一般有初始化、計算權重、重采樣和狀態轉移等步驟。機器人在二維地圖使用粒子濾波器推算機器人位置。

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

圖4 機器人位置定位流程

上述流程蒙特卡羅定位方法結合粒子濾波，依據機器人激光雷達反饋的數據在二維地圖中實現定位，對此進行自適應優化解決如下問題：

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

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

3.2 導航算法分析和實現

3.2.1 全局路徑算法分析和實現

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

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

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

圖5 起點終點位置

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

圖6 節點圖（一）

圖7 節點圖（二）

圖8 節點圖（三）

圖9 最終路徑圖

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

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

3.2.2 局部路徑算法分析和實現

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

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

圖11 坐標變化圖

機器人下一個狀態的位置為：

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

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

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

4 路徑決策算法設計和實現

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

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

圖13 層次分析法決策結構

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

表1 比例標度法

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

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

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

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

表2 RI 值

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

當CR<0.10 時，可認為判斷矩陣的一致性是可以接受的；否則，應對判斷矩陣進行適當的修正。

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

5 仿真和實驗測試

5.1 路徑決策算法的仿真實驗

為檢驗路徑決策算法是否可行，利用數學軟件和模擬場景來進行仿真實驗測試，模擬場景地圖如圖14所示。

圖14 模擬場景地圖

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

表3 場景模擬

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

表4 準則層B

將比較因子數據引入算法模塊，在MATLAB 中執行路徑決策算法并驗證。

通過MATLAB 編程，計算最終總排序權重值，得到結果見表6所列。

表6 準則層B 中各因素對于總目標的權重

驗證結果：因為1 號路線的總排序權重值最高，因此決策出1 號路線為目標路徑。雖然1 號路線地形情況最惡劣，但綜合決策，其仍是3 條路徑中綜合行駛時間最短的路徑，符合預期，即：所構建的決策模塊可以完成路徑決策。

5.2 機器人部分運行截圖

運行結果圖如圖15所示。

圖15 運行結果圖

6 結 語

本文以ROS 自主導航機器人為對象，研究了阿克曼轉向結構、機器人定位、路徑規劃、路徑決策等技術。自主導航移動機器人在移動過程中進行自適應蒙特卡羅定位，并在定位過程中利用激光雷達實時構建環境代價地圖，根據建立的代價地圖用戶自定義導航目標，根據用戶定義的導航目標，利用路徑決策算法進行路徑決策，從而決策出最佳路徑。在得到最佳路徑后，調用ROS 系統的自主導航模塊，利用Dijkstra 和DWA 算法，最終抵達目的地。本文在對機器人阿克曼轉向模型和ROS 系統的基礎上利用AMCL 算法、Dijkstra 算法、DWA 算法、層次分析法，實現ROS 自主導航機器人的路徑決策功能，具有較大的實用價值。