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

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

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

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

1 全局路徑規(guī)劃

1.1 A*算法介紹

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

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

(1)

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

A*算法搜索過程可分為以下幾個步驟。

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

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

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

(2)

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

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

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

(3)

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

(4)

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

(5)

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

(6)

2 局部路徑規(guī)劃

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

本文采用阿克曼機(jī)器人[8]模型進(jìn)行實(shí)驗(yàn)，該模型可以解決小車在轉(zhuǎn)彎時，內(nèi)外轉(zhuǎn)向輪路徑指向圓心不統(tǒng)一的問題。圖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成正比。模型中四個輪子路徑的圓心交會于后軸延長線上的瞬時轉(zhuǎn)向中心，因此，基于阿克曼模型的移動機(jī)器人轉(zhuǎn)彎更加順滑。

2.2 TEB算法

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

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

(11)

E*=argEminf(E)

(12)

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

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

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

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

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

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

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

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

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

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

(2)使用Gmapping算法[11]通過激光雷達(dá)獲取點(diǎn)云信息，利用鍵盤控制機(jī)器人在地圖中運(yùn)動一圈，構(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ī)劃的時間Ti和路徑的長度Di，兩種算法的實(shí)驗(yàn)平均數(shù)據(jù)如表1所示。

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

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

表1 算法性能對比

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

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

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

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

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

5 結(jié)論

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