一種機(jī)器人路徑規(guī)劃算法的研究

景曉琦，呂艷輝，李發(fā)伯

(沈陽(yáng)理工大學(xué) 信息科學(xué)與工程學(xué)院，沈陽(yáng)110159)

隨著人工智能的迅速發(fā)展，室內(nèi)移動(dòng)機(jī)器人已進(jìn)入人們的生活，移動(dòng)機(jī)器人給人們帶來(lái)了很多便利[1]。路徑規(guī)劃是移動(dòng)機(jī)器人不可缺少的一項(xiàng)技術(shù)[2]，該技術(shù)是在已知機(jī)器人的環(huán)境地圖和當(dāng)前姿態(tài)的情況下，規(guī)劃出一條從指定起點(diǎn)到指定目標(biāo)點(diǎn)的無(wú)碰撞路徑。

A*算法是移動(dòng)機(jī)器人路徑規(guī)劃中的一種重要算法[3]，憑借啟發(fā)式函數(shù)可以獲得一條最優(yōu)路徑。A*算法便于運(yùn)用到其他算法中，并有較強(qiáng)的塑造性，但在應(yīng)用過(guò)程中，A*算法存在著冗余點(diǎn)過(guò)多、計(jì)算量大等問(wèn)題，為此采用刪除冗余點(diǎn)的方法以減少計(jì)算量。但僅憑借A*算法無(wú)法完成遇到動(dòng)態(tài)障礙物時(shí)的路徑規(guī)劃，所以要采用局部路徑規(guī)劃算法來(lái)實(shí)時(shí)避障。本文將改進(jìn)后的A*算法與時(shí)間彈性帶[4](Timed Elastic Band，TEB)局部路徑規(guī)劃算法相結(jié)合，形成新的路徑規(guī)劃算法。

1 全局路徑規(guī)劃

1.1 A*算法介紹

A*算法的思想是依靠全局地圖信息，使用啟發(fā)式搜索法來(lái)選擇下一個(gè)節(jié)點(diǎn)[5]。A*算法的啟發(fā)式函數(shù)可以描述為

f(n)=g(n)+h(n)

(1)

式中：g(n)為起始點(diǎn)到當(dāng)前點(diǎn)的代價(jià)；h(n)為當(dāng)前點(diǎn)到目標(biāo)點(diǎn)的代價(jià)。

A*算法搜索過(guò)程可分為以下幾個(gè)步驟。

(1)開始搜索：首先將起始點(diǎn)放到open 隊(duì)列中，向周圍八向查找能夠通過(guò)的方格并且將其加入到open隊(duì)列中；然后設(shè)置起始點(diǎn)為新加入方格的父節(jié)點(diǎn)并用箭頭標(biāo)注；最后把起始點(diǎn)從open隊(duì)列中移入到close隊(duì)列中(close隊(duì)列中存放的是所有不用再次查看的方格)。

(2)繼續(xù)搜索：選擇open隊(duì)列中具有最小f(n)值的節(jié)點(diǎn)n，并將其放入close隊(duì)列中。

(3)再次搜索：重復(fù)步驟(2)，繼續(xù)擴(kuò)展，直到具有最小f(n)值的點(diǎn)是目標(biāo)點(diǎn)為止。

(4)反向?qū)ぢ罚喊凑漳繕?biāo)點(diǎn)以及箭頭反向?qū)ぢ氛业狡鹗键c(diǎn)，形成一條當(dāng)前柵格地圖[6]中的最短路徑。

1.2 改進(jìn)A*算法

雖然A*算法表現(xiàn)良好，但仍然存在計(jì)算量大[7]、拐點(diǎn)較多、路徑不平滑的缺點(diǎn)。做為室內(nèi)移動(dòng)機(jī)器人的路徑規(guī)劃算法，要考慮到機(jī)器人實(shí)際情況，減少一定的計(jì)算量，防止機(jī)器人因驟然轉(zhuǎn)彎造成的損耗，因此，本文對(duì)A*算法進(jìn)行改進(jìn)。圖1為改進(jìn)后的A*算法刪除冗余點(diǎn)示意圖。

(2)

圖1 改進(jìn)后的A*算法刪除冗余點(diǎn)

當(dāng)一條線段上有多個(gè)open隊(duì)列節(jié)點(diǎn)時(shí)，將線段的兩個(gè)端點(diǎn)保留，刪除該線段上中間其他各點(diǎn)，達(dá)到減少open和close隊(duì)列節(jié)點(diǎn)計(jì)算量的目的。

進(jìn)行上述改進(jìn)后，再進(jìn)行路徑曲線的平滑處理。本文采取三次B樣條曲線算法進(jìn)行路徑的修飾加工，去除折點(diǎn)，變成一條平滑的曲線路徑，以減少機(jī)器人的損耗。采用B樣條曲線是因?yàn)槠渚哂芯植啃?、連續(xù)性及保凸性等優(yōu)點(diǎn)。p(t)為B樣條曲線方程，表達(dá)式為

(3)

式中：pi代表控制曲線的各特征點(diǎn)；Gi.k(t)表示k次B樣條基函數(shù)。

(4)

當(dāng)k=3時(shí)，則有三次均勻B樣條曲線基函數(shù)方程式，為

(5)

那么三次B樣條曲線段p(t)為

(6)

2 局部路徑規(guī)劃

2.1 阿克曼機(jī)器人模型

本文采用阿克曼機(jī)器人[8]模型進(jìn)行實(shí)驗(yàn)，該模型可以解決小車在轉(zhuǎn)彎時(shí)，內(nèi)外轉(zhuǎn)向輪路徑指向圓心不統(tǒng)一的問(wèn)題。圖2所示為阿克曼機(jī)器人轉(zhuǎn)向幾何示意圖。

圖2 阿克曼機(jī)器人轉(zhuǎn)向幾何示意圖

圖2中ω0代表外側(cè)前輪偏轉(zhuǎn)角度；ωi表示內(nèi)側(cè)前輪偏轉(zhuǎn)角度；x為前后輪距離；y為兩軸間距離；R為轉(zhuǎn)向半徑。

內(nèi)、外側(cè)輪胎偏轉(zhuǎn)角度表達(dá)式為

(7)

(8)

前輪的平均偏轉(zhuǎn)角度可表示為

(9)

前輪內(nèi)外偏轉(zhuǎn)角度的差值表示為

(10)

由式(10)可知，Δω與ω2成正比。模型中四個(gè)輪子路徑的圓心交會(huì)于后軸延長(zhǎng)線上的瞬時(shí)轉(zhuǎn)向中心，因此，基于阿克曼模型的移動(dòng)機(jī)器人轉(zhuǎn)彎更加順滑。

2.2 TEB算法

經(jīng)典的彈性帶算法[9](Elastic Band，EB)會(huì)使全局規(guī)劃生成的路徑對(duì)最短路徑發(fā)生變形，且不考慮底層機(jī)器人的運(yùn)動(dòng)約束，故存在著一定缺陷。因此，引入TEB算法，本算法考慮到機(jī)器人在運(yùn)動(dòng)時(shí)間方面的動(dòng)態(tài)約束，采用加權(quán)多目標(biāo)優(yōu)化框架，求出當(dāng)前時(shí)刻的最優(yōu)局部路徑。

TEB算法的關(guān)鍵在于通過(guò)改變機(jī)器人當(dāng)前位姿和時(shí)間間隔，利用局部位姿、速度、加速度等權(quán)重來(lái)優(yōu)化局部路徑規(guī)劃算法。目標(biāo)函數(shù)f(E)和優(yōu)化后的TEB結(jié)果E*可分別表示為

(11)

E*=argEminf(E)

(12)

式中：γk為權(quán)重系數(shù)；fk為各個(gè)目標(biāo)函數(shù)的權(quán)值。式(12)表示當(dāng)f(E)最小時(shí)，E的取值。

3 新的路徑規(guī)劃算法

改進(jìn)后的A*算法可進(jìn)行靜態(tài)環(huán)境下的路徑規(guī)劃，使得機(jī)器人在全局地圖上有一個(gè)整體認(rèn)知與規(guī)劃，但在局部出現(xiàn)動(dòng)態(tài)物時(shí)，A*算法無(wú)法進(jìn)行避讓。

現(xiàn)有動(dòng)態(tài)窗口算法[10](Dynamic Window Approach，DWA)、TEB算法等局部路徑規(guī)劃算法。DWA動(dòng)態(tài)窗口算法是使機(jī)器人先到達(dá)目標(biāo)點(diǎn)再進(jìn)行旋轉(zhuǎn)直到朝向目標(biāo)方向；若將該算法與本實(shí)驗(yàn)采用的阿克曼模型相結(jié)合，在路徑規(guī)劃過(guò)程中便會(huì)因?yàn)樵撃Ｐ筒豢稍剞D(zhuǎn)向的運(yùn)動(dòng)特性而使規(guī)劃路徑失敗，造成導(dǎo)航過(guò)程中機(jī)器人無(wú)法繼續(xù)行走。TEB算法是機(jī)器人移動(dòng)過(guò)程中不斷調(diào)整位姿和朝向，到達(dá)目標(biāo)點(diǎn)時(shí)無(wú)需再進(jìn)行調(diào)整。故本文采用TEB算法，其可以在遇到動(dòng)態(tài)障礙物時(shí)刪除舊的機(jī)器人位姿并添加新的機(jī)器人位姿，這樣每次迭代都可生成一條新的路徑，在進(jìn)行不斷迭代過(guò)程中得到優(yōu)化的路徑。

本文基于A*算法和TEB算法，提出一種新的路徑規(guī)劃算法，該算法可使移動(dòng)機(jī)器人在實(shí)時(shí)避開動(dòng)態(tài)障礙物的情況下，得到一條耗時(shí)短、無(wú)差錯(cuò)且安全的路徑，且提升其平滑度。圖3所示為新的路徑規(guī)劃算法框圖。

首先輸入柵格地圖并初始化起始點(diǎn)、目標(biāo)點(diǎn)及初始速度，為路徑規(guī)劃提供初始條件。然后通過(guò)改進(jìn)后的A*算法進(jìn)行全局路徑規(guī)劃，如果在環(huán)境中出現(xiàn)動(dòng)態(tài)障礙物，采用TEB算法進(jìn)行局部的路徑規(guī)劃，實(shí)時(shí)避開障礙物，并立即更新地圖，把動(dòng)態(tài)障礙物轉(zhuǎn)化為靜態(tài)帶障礙物的柵格地圖；再次使用改進(jìn)后的A*算法進(jìn)行全局路徑規(guī)劃，直到?jīng)]有動(dòng)態(tài)障礙物出現(xiàn)為止。最后到達(dá)目標(biāo)節(jié)點(diǎn)，形成一條平滑且安全無(wú)碰撞的路徑。

圖3 新的路徑規(guī)劃算法框圖

4 實(shí)驗(yàn)

本實(shí)驗(yàn)在PC端完成。系統(tǒng)環(huán)境是ubuntu18.04，ROS的版本是Kinetic。Gazebo適用于機(jī)器人室內(nèi)外環(huán)境的仿真，方便建模并可直觀地對(duì)機(jī)器人移動(dòng)進(jìn)行觀察。Rviz工具可以對(duì)ROS話題及消息進(jìn)行編輯，實(shí)驗(yàn)在Rviz中進(jìn)行可視化顯示。實(shí)驗(yàn)步驟如下。

(1)在Gazebo物理仿真平臺(tái)上，使用Building Editor工具繪制地圖；

(2)使用Gmapping算法[11]通過(guò)激光雷達(dá)獲取點(diǎn)云信息，利用鍵盤控制機(jī)器人在地圖中運(yùn)動(dòng)一圈，構(gòu)建好柵格地圖；

(3)分別采用傳統(tǒng)A*算法和新的路徑規(guī)劃算法進(jìn)行路徑規(guī)劃，初始化起始點(diǎn)和目標(biāo)點(diǎn)。實(shí)驗(yàn)結(jié)果如圖4和圖5所示。重復(fù)實(shí)驗(yàn)100次，記錄每次路徑規(guī)劃的時(shí)間Ti和路徑的長(zhǎng)度Di，兩種算法的實(shí)驗(yàn)平均數(shù)據(jù)如表1所示。

圖4 傳統(tǒng)A*算法實(shí)驗(yàn)圖

圖5 新的路徑規(guī)劃算法實(shí)驗(yàn)圖

表1 算法性能對(duì)比

由圖4、圖5以及表1可以看出，使用傳統(tǒng)A*算法尋路轉(zhuǎn)角過(guò)陡，對(duì)機(jī)器人的損傷很大，平均路徑長(zhǎng)度和平均規(guī)劃時(shí)間較長(zhǎng)。本文提出的算法能夠規(guī)劃出一條平滑、路徑較短的全局路徑，并且算法規(guī)劃時(shí)間縮短，效率提高。

(4)當(dāng)在地圖中放入障礙物時(shí)，采用新的路徑規(guī)劃算法，初始化起始點(diǎn)和目標(biāo)點(diǎn)。實(shí)驗(yàn)結(jié)果如圖6所示。

圖6 放入障礙物后新的路徑規(guī)劃算法實(shí)驗(yàn)圖

由圖6可以看出，當(dāng)采用新的路徑規(guī)劃算法時(shí)，移動(dòng)機(jī)器人可以識(shí)別出現(xiàn)的障礙物的點(diǎn)云信息，并及時(shí)采用局部路徑規(guī)劃算法，實(shí)時(shí)避障，圖中虛線部分即為局部路徑規(guī)劃路線；當(dāng)移動(dòng)到避開障礙物時(shí)，回歸到全局路徑規(guī)劃的路線，達(dá)到整體路徑規(guī)劃的效果。

若只使用TEB算法，移動(dòng)機(jī)器人能夠在有障礙物出現(xiàn)時(shí)實(shí)時(shí)避障，達(dá)到安全有序的局部路徑規(guī)劃；但TEB算法只考慮到周圍小面積的路徑規(guī)劃，對(duì)地圖沒(méi)有全局意識(shí)，易導(dǎo)致移動(dòng)機(jī)器人陷入死角。而通過(guò)采用本文提出的算法，既可以在全局上規(guī)劃最優(yōu)路徑，又可以實(shí)時(shí)避開障礙物。

5 結(jié)論

基于改進(jìn)后的A*算法，結(jié)合TEB算法，提出一種新的機(jī)器人路徑規(guī)劃算法。本文算法在保證移動(dòng)機(jī)器人安全的前提下，可以實(shí)現(xiàn)在已知機(jī)器人位姿及環(huán)境地圖條件下的自我導(dǎo)航功能；通過(guò)實(shí)驗(yàn)可知該算法規(guī)劃出來(lái)的路徑效果更佳，在轉(zhuǎn)彎時(shí)路徑曲線化，避免了移動(dòng)機(jī)器人由于轉(zhuǎn)角過(guò)大造成易側(cè)翻的問(wèn)題，更加符合機(jī)器人的實(shí)際運(yùn)動(dòng)需求，相比于原兩種算法均有所提升。