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

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

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

0 引言

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

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

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

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

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

2 全局路徑規(guī)劃

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

2.1 柵格地圖劃分

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

圖2 柵格地圖模型

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

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

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

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

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

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

式中：（xk,yk）是k 柵格的中心點(diǎn)坐標(biāo)；（xend，yend）是目標(biāo)點(diǎn)坐標(biāo)。

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

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

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

3 局部路徑規(guī)劃

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

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

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

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

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

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

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

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

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

4 路徑跟蹤

4.1 機(jī)器人車輛模型

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

圖7 機(jī)器人車輛模型

4.2 橫向控制和縱向控制

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

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

5 實(shí)驗(yàn)分析

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

圖9 仿真實(shí)驗(yàn)結(jié)果

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

圖10 工程實(shí)驗(yàn)結(jié)果

6 結(jié)束語

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