機器人行駛路徑分層規(guī)劃系統(tǒng)設(shè)計

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

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

0 引言

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

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

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

1 分層路徑規(guī)劃系統(tǒng)

如圖1 所示，規(guī)劃系統(tǒng)按結(jié)構(gòu)可以分為3 層：全局規(guī)劃、局部規(guī)劃和路徑跟蹤。

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

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

圖1 分層路徑規(guī)劃系統(tǒng)架

2 全局路徑規(guī)劃

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

2.1 柵格地圖劃分

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

圖2 柵格地圖模型

2.2 啟發(fā)式A＊算法

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

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

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

圖3 全局路徑節(jié)點選取

為了保證搜索路徑的最優(yōu)性，選用以下公式作為啟發(fā)函數(shù)，令：

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

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

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

圖4 啟發(fā)式A*算法流程圖

3 局部路徑規(guī)劃

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

3.1 機器人狀態(tài)空間模型

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

3.2 局部路徑規(guī)劃方法

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

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

圖5 快速隨機擴展樹模型構(gòu)建

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

圖6 RRT算法局部規(guī)劃流程圖

4 路徑跟蹤

4.1 機器人車輛模型

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

圖7 機器人車輛模型

4.2 橫向控制和縱向控制

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

圖8 路徑跟蹤控制系統(tǒng)框圖

5 實驗分析

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

圖9 仿真實驗結(jié)果

實車實驗是將輪式機器人應(yīng)用在實際工程實踐中，如圖10所示。圖10（a）中曲線是全局規(guī)劃路徑。根據(jù)環(huán)境感知傳感器獲取的周圍環(huán)境信息，進行全局路徑規(guī)劃。圖10（b）是實車開始行駛時，對規(guī)劃路徑的跟蹤效果。當檢測到前方有障礙物或動態(tài)障礙物出現(xiàn)時，根據(jù)局部路徑規(guī)劃進行避障規(guī)劃。將機器人以一長方形等效顯示，長方形區(qū)域為實際走過的軌跡?？梢钥闯鰴C器人的實際行駛軌跡對規(guī)劃路徑的跟蹤效果良好。

圖10 工程實驗結(jié)果

6 結(jié)束語

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