機器人行駛路徑分層規劃系統設計

易春求，孔麗麗，易春回，徐朋朋，張靜妮

（1.奧的斯機電電梯有限公司，杭州 310019；2.中國計量大學，杭州 310018；3.桂林智源電力電子有限公司，廣西桂林 541004；4.濱江兄弟信息技術（杭州）有限公司，杭州 310052）

0 引言

在一些工礦、隧道施工等高強度、高粉塵、大噪聲的惡劣環境中，操作人員面臨勞動強度大、易疲勞、易受傷等身體問題。這種特殊的工作環境不便于操作人員直接進入工作現場，采用機器人代替人工操作是解決上述問題的理想方案。路徑規劃是機器人自主行駛的核心問題之一，是指機器人按照某一性能指標搜索一條從起始點到目標點的可行無碰最優路徑[1]。

目前，對移動機器人路徑規劃算法的研究已經取得了大量成果，常用的路徑規劃算法主要有拓撲法、可視圖法、切線圖法、柵格法、懲罰函數法、人工勢場法、啟發式搜索法、RRT（快速隨機擴展樹）算法、A*算法、模糊邏輯算法、遺傳算法、人工神經網絡、模擬退火算法、蟻群優化啟發式算法、粒子群算法等[2-4]。近幾年，還提出了基于滾動窗口的規劃方法、基于滾動的RRT算法、Bi-RRT算法等方法[5-10]。如果起始點到目標點的距離遠且環境復雜，拓撲法、柵格法、啟發式搜索法等存在搜索時間長、效率低等問題，因此這一類方法更適合在較大的、全局范圍內進行一條粗略初始路徑的選擇；另一方面，這些方法僅僅提出了如何規劃一條路徑，對于如何使機器人更好地跟蹤已規劃好的路徑研究較少。

許多路徑規劃算法已能得到令人滿意的效果，但目前的研究多是集中在單一或者某種改進算法的研究與仿真，對路徑規劃系統整體設計的研究相對較少，工程化應用較少。本文以輪式移動機器人為載體，提出了一種用于移動機器人的分層路徑規劃方法。設計了機器人自主行駛規劃系統架構，并根據機器人行駛速度慢、工作環境復雜、環境地圖非結構化等特點，介紹了分層路徑規劃系統中每一層的算法及實現過程，并給出了仿真實驗結果和工程實車實驗結果。

1 分層路徑規劃系統

如圖1 所示，規劃系統按結構可以分為3 層：全局規劃、局部規劃和路徑跟蹤。

全局路徑規劃包括環境地圖建立、全局路徑搜索生成兩個子模塊，是基于已知或部分已知數字地圖的路徑規劃，目的是根據環境模型，采用優越的搜索策略找出從起始點到目標點的可行最優路徑。

局部路徑規劃由兩個子模塊構成：障礙物檢測和路徑重規劃。利用機器人所安裝的雷達、視覺等環境感知傳感器所提供的障礙物的尺寸、形狀、位置信息，進行障礙物規避。在局部規劃中，環境信息是未知的，周圍障礙物信息完全由環境感知傳感器獲取。局部路徑規劃的目的是在具有障礙物的環境中，重規劃一條避障路徑[11-12]。路徑跟蹤實現車輛的縱向、橫向控制，目的是控制車輛能按照已規劃的路徑行駛。

圖1 分層路徑規劃系統架

2 全局路徑規劃

全局路徑規劃的步驟包括：（1）輪式機器人環境地圖建立；（2）柵格地圖劃分；（3）形成柵格地圖所對應的搜索空間；（4）在搜索空間上利用搜索算法進行路徑搜索。

2.1 柵格地圖劃分

采用柵格法進行工作空間劃分。柵格法是用一定尺寸的柵格，將機器人的二維工作空間進行均勻柵格劃分，基本元素位于自由區取值為0，表示該柵格為可行柵格，用白色表示。處在障礙物區為1，表示該柵格為障礙柵格，用黑色表示。在計算機建立的二維柵格地圖如圖2所示。

圖2 柵格地圖模型

2.2 啟發式A＊算法

柵格地圖創建完之后，要在柵格節點之間建立一定的聯系，才能從起始點搜索到目標點的有效路徑。常用的是8 連接，其表示從當前柵格可以達到與之相鄰的8 個柵格節點。如圖3 所示，a 為當前柵格，周圍1～8為與之相鄰的8個柵格。

應用啟發式A算法[13-14]進行相鄰柵格節點搜索，該算法相較于其他搜索算法具有更好的重構性和實時性。具體的搜索過程是：首先構造一個估價函數，在柵格a 周圍的8個柵格中找到一個使得估價函數值最小的柵格b；然后把a 標注為已走過的柵格，畫出從柵格a 到柵格b 之間的路徑；然后再以柵格b為中心柵格，重復上述過程，直至到達終點。構造其估價函數為：

式中：k為a 周圍的8 個柵格中的某一個；g(k)為起始點到k 柵格的路徑長度，定義為實際代價，是已知的；h(k)是k 柵格到目標點的估計路徑長度，定義為啟發函數，是未知的。

圖3 全局路徑節點選取

為了保證搜索路徑的最優性，選用以下公式作為啟發函數，令：

式中：（xk,yk）是k 柵格的中心點坐標；（xend，yend）是目標點坐標。

估價函數最小的判斷條件是：一個柵格與障礙物柵格的距離越大、且與目標點的距離越小，估價函數的值越小，此柵格將更容易被選中。其實現流程如圖4所示。

選擇估價函數最小的柵格，將其存入到close 列表中，當擴展搜索到目標點時，即將目標點存入到close列表中時，結束搜索。在列表中，搜索到最小f ( k )的柵格節點可以形成一條從起始點到目標點的最優路徑。當close 為空時，說明路徑不存在。全局規劃的結果是一系列離散軌跡點。

圖4 啟發式A*算法流程圖

3 局部路徑規劃

局部路徑規劃重點解決：當環境感知傳感器檢測到新的障礙物或動態障礙物時，需要改變原路徑，重新規劃一條供機器人行駛的無障礙路徑，以實現規避障礙物。環境感知傳感器檢測障礙物的尺寸、形狀和位置。

3.1 機器人狀態空間模型

輪式機器人行駛需滿足非完整運動學約束，在狀態空間中車輛狀態X=(x,y,u)，其中(x,y)為車輛位置坐標，u為輸入。θ為車體航向角，L為軸距，Φ 為前輪轉角，車輛微分約束方程表示為則其狀態轉移方程為：

3.2 局部路徑規劃方法

局部規劃算法有很多種，快速隨機擴展樹算法（RRT 算法），因其具有快速高效的特點，在機器人路徑規劃中得到廣泛應用。而且RRT 算法還綜合考慮了車輛非完整運動學約束等約束條件，該算法更適合于機器人非結構化工作環境下的局部路徑規劃[15]。

在局部范圍內定義機器人的狀態空間C，自由空間Cfree，起始狀態Xstart，目標狀態Xgoal，Xgoal∈Cfree。如圖5所示，其以狀態空間C中的起始狀態Xstart為根節點構建隨機搜索樹T，通過迭代隨機采樣方式選擇狀態節點Xrand，遍歷T 找出距Xrand最近的節點Xnear。然后通過控制輸入u，驅動機器人沿著Xnear到Xrand。對式（2）在一定時間Δt上進行積分便可得到Xnew，并將其作為新的葉節點添加到隨機擴展樹上。重復上述過程，直至到達目標狀態，隨機樹構建結束。

圖5 快速隨機擴展樹模型構建

從起始狀態點出發，構建隨機搜索樹，直至到達目標狀態點。這樣就規劃出了機器人從起始狀態點到達目標狀態點滿足微分約束的路徑，以及在每一時刻的控制輸入參數，為下一步的路徑跟蹤提供依據。因為在搜索過程中充分考慮了機器人客觀存在的微分約束方程，因而RRT 算法規劃出來的路徑具有很強的合理性。圖6 所示為RRT 算法局部規劃流程圖。

圖6 RRT算法局部規劃流程圖

4 路徑跟蹤

4.1 機器人車輛模型

輪式機器人等效成圖7 所示模型。后軸驅動，其行駛速度為v，前輪轉角為Φ。后驅動軸安裝編碼器用于采集當前速度值，前輪安裝轉角傳感器負責采集方向盤轉角值。

圖7 機器人車輛模型

4.2 橫向控制和縱向控制

輪式機器人對已規劃路徑的跟蹤效果由機器人控制層決定。輪式機器人的控制包括兩個方面：速度控制和方向控制。將路徑跟蹤分為橫向控制和縱向控制。橫向控制指方向控制，縱向控制指速度控制。如圖8所示，機器人的路徑規劃層負責計算出期望速度和期望方向，傳感器獲取機器人當前的速度和方向信息。設計縱向控制器，能根據車輛期望速度和當前速度的偏差Δv計算出油門和剎車的控制量。設計橫向控制器，能根據車輛期望方向和當前方向的偏差ΔΦ 計算出方向盤轉角的控制量。控制輪式機器人的行駛速度和方向始終與期望值偏差為0，使機器人能按照規劃的路徑行駛。

圖8 路徑跟蹤控制系統框圖

5 實驗分析

基于上述的路徑規劃系統架構及路徑跟蹤控制器的設計，實現了機器人的規劃系統。仿真實驗時，根據機器人的最大長寬尺寸，將其等效為一個矩形。隨機生成若干障礙物，設定起始點和目標點，進行路徑規劃。共設置了20 20柵格地圖，在地圖中隨機設置障礙物，圖9給出了兩種隨機放置障礙物的路徑搜索結果。

圖9 仿真實驗結果

實車實驗是將輪式機器人應用在實際工程實踐中，如圖10所示。圖10（a）中曲線是全局規劃路徑。根據環境感知傳感器獲取的周圍環境信息，進行全局路徑規劃。圖10（b）是實車開始行駛時，對規劃路徑的跟蹤效果。當檢測到前方有障礙物或動態障礙物出現時，根據局部路徑規劃進行避障規劃。將機器人以一長方形等效顯示，長方形區域為實際走過的軌跡。可以看出機器人的實際行駛軌跡對規劃路徑的跟蹤效果良好。

圖10 工程實驗結果

6 結束語

本文針對輪式機器人的路徑規劃問題，設計并實現了一種分層路徑規劃系統。在全局規劃中采用柵格法創建地圖信息，采用啟發式A*算法搜索到一條全局路徑點，為下一步的局部規劃提供導向約束。在局部規劃中建立機器人車輛的微分約束方程，選用快速RRT 算法進行局部路徑規劃。路徑跟蹤時，設計橫向控制器和縱向控制器分別實現機器人車輛的橫向控制和縱向控制。仿真實驗和實車試驗表明：這種分層的路徑規劃系統可以全面地考慮各種因素的影響，層次架構清晰。算法的搜索速度適宜，完全滿足實驗中所采用的慢速機器人的行駛速度要求。不同層次采用不同的規劃算法，使得規劃的路徑合理、平順、穩定，實際可執行性強。這種系統級的研究相比較單一算法的理論研究，更具有工程實際應用價值，而且這種分層路徑規劃架構通用性強，適用范圍廣。