，

(青海民族大學(xué)計算機(jī)學(xué)院，青海 西寧 810008)

0 引言

導(dǎo)航是智能機(jī)器人非常重要的功能，也是機(jī)器人領(lǐng)域的一個研究重點。路徑規(guī)劃技術(shù)作為導(dǎo)航的核心，其主要任務(wù)是通過機(jī)體搭載的傳感器獲取周圍環(huán)境信息，根據(jù)已知信息規(guī)劃出一條從起始點到目標(biāo)點的最優(yōu)路徑[1]。近些年來，針對機(jī)器人路徑規(guī)劃，諸多學(xué)者通過展開大量研究，提出各種面向不同場景的路徑規(guī)劃技術(shù)。其中比較著名的有快速搜索隨機(jī)樹[2-3]，概率路圖法[4]，也有基于節(jié)點搜索的A*算法和D*算法[5-6]，基于啟發(fā)式的遺傳算法，粒子群算法和蟻群算法等[7-9]，還有應(yīng)用于局部路徑規(guī)劃的動態(tài)窗口法和人工勢場法等[10-12]。全局路徑規(guī)劃的前提是機(jī)器人需要有完整地圖信息，而局部路徑規(guī)劃中機(jī)器人只需要知道局部信息，即通過傳感器獲得的自身周邊障礙物信息。

在靜態(tài)環(huán)境中A*算法是搜索最短路徑最有效的算法，因此被廣泛應(yīng)用于實際路徑規(guī)劃中。由于該算法本身搜索原理的限制，規(guī)劃出來的路徑雖然效率高，但是轉(zhuǎn)折點較多，對于移動機(jī)器人來說不利于直接執(zhí)行。文獻(xiàn)[13]提出一種平滑A*算法，旨在解決柵格環(huán)境下移動機(jī)器人A*算法規(guī)劃出的路徑折線多，轉(zhuǎn)折次數(shù)多和轉(zhuǎn)折角大的問題，最終獲得了較優(yōu)路徑，該改進(jìn)算法只是優(yōu)化了路徑，并沒有減少路徑長度，機(jī)器人執(zhí)行時所付出的代價相差不大，同時也不具備很好的動態(tài)避障能力。文獻(xiàn)[14]將人工勢場法和蟻群算法進(jìn)行融合，在柵格環(huán)境中，以人工勢場法的規(guī)則信息作為蟻群算法尋優(yōu)的基礎(chǔ)，引入勢場合力作為螞蟻搜索路徑點的部分啟發(fā)信息，該方法解決了人工勢場法容易陷入極值點的缺點，但是計算復(fù)雜度太大。動態(tài)窗口法實時性好，具有良好的躲避障礙物能力，但是該方法只是一種局部路徑規(guī)劃，并沒有考慮到機(jī)器人的全局路徑規(guī)劃信息，使得最后規(guī)劃出來的全局路徑并不是最優(yōu)。

基于以上各方法的特點，本文提出一種A*算法的改進(jìn)方法，對鄰域進(jìn)行擴(kuò)展，并且將啟發(fā)式函數(shù)進(jìn)行修改以適應(yīng)具體環(huán)境需求，使傳統(tǒng)A*算法能夠更加靈活，規(guī)劃出來的路徑轉(zhuǎn)折點少，同時全局路徑更加平滑。將改進(jìn)A*算法和動態(tài)窗口法進(jìn)行結(jié)合，將兩種算法的優(yōu)點結(jié)合起來，既能進(jìn)行全局最優(yōu)路徑的搜索，也讓算法具備良好的避障能力，躲避障礙物。

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

1.1 傳統(tǒng)A*算法介紹

傳統(tǒng)A*算法是一種標(biāo)準(zhǔn)的啟發(fā)式函數(shù)，是靜態(tài)環(huán)境中尋找最優(yōu)路徑的有效方法，其核心在于一個估價函數(shù)。從初始節(jié)點開始，按照啟發(fā)函數(shù)對周圍的節(jié)點開始搜索，選取周圍最優(yōu)的一個節(jié)點進(jìn)行擴(kuò)展，重復(fù)這一過程，直到搜索到目標(biāo)點，然后由目標(biāo)點回溯到起始點形成一條全局規(guī)劃軌跡。其估價函數(shù)為：

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

(1)

其中:f(n)是估計函數(shù)，表示的是從起始點到目標(biāo)點的評價估計量，g(n)表示的是初始點到當(dāng)前節(jié)點的實際代價值，而h(n)則表示當(dāng)前節(jié)點到目標(biāo)點的估計代價值。若將啟發(fā)式函數(shù)設(shè)置成零，即h(n)=0，估價函數(shù)就完全由代價函數(shù)g(n)決定，此時A*算法就退化為基于貪心策略的Dijkstra算法，搜索效率大打折扣。h(n)啟發(fā)函數(shù)直接決定了A*算法是否高效，h(n)中包含越多的啟發(fā)式信息，A*算法效率就高，在搜索較少節(jié)點的情況下就可以找到最優(yōu)全局路徑，反之，若啟發(fā)式信息量越少，規(guī)劃出的軌跡就離最優(yōu)軌跡相差越遠(yuǎn)。

1.2 鄰域擴(kuò)展

傳統(tǒng)的A*算法只能向節(jié)點周圍8鄰域進(jìn)行擴(kuò)展，這種情況下機(jī)器人只能沿著八個方向運動，每個方向之間至少有四十五度的夾角，這就限制了機(jī)器人的運動，直接導(dǎo)致最終規(guī)劃出的全局路徑轉(zhuǎn)折節(jié)點多，軌跡不夠平滑。如圖1所示，本文將8鄰域搜索擴(kuò)展到24鄰域，機(jī)器人能夠以更小的角度行進(jìn)，從而使軌跡更加平滑。

圖中中心黑色圓點表示目前機(jī)器人所在位置，傳統(tǒng)方法中，機(jī)器人只能向8個方向擴(kuò)展，即擴(kuò)展到圖中小圓點所在位置，擴(kuò)展后一共可以向16個方向進(jìn)行搜索，擴(kuò)展點為圖中24個箭頭所在位置。傳統(tǒng)A*算法中，由于搜索的節(jié)點數(shù)目較少，很可能傳統(tǒng)A*算法中，由于搜索的節(jié)點數(shù)目較少，在前期就將更優(yōu)秀的節(jié)點從列表中刪除，從而只能找到次優(yōu)路徑，擴(kuò)展后的算法由于搜索的節(jié)點更多，在很大程度上可以避免這種情況的發(fā)生，進(jìn)一步優(yōu)化了路徑。

圖1 24鄰域擴(kuò)展節(jié)點示意圖

1.3 啟發(fā)式函數(shù)的優(yōu)化

在A*算法中，傳統(tǒng)的啟發(fā)式函數(shù)采用曼哈頓距離或者歐式距離。曼哈頓距離是指兩個坐標(biāo)點之間橫軸和豎軸絕對值之和，假設(shè)每個相鄰單位之間的路徑代價為C，n表示當(dāng)前點，goal表示目標(biāo)點，可以得到基于曼哈頓距離的啟發(fā)函數(shù)為：

h(n)=C*(abs(nx-goalx)+abs(ny-goaly))

(2)

若機(jī)器人單位可以進(jìn)行任意方向的移動，我們就可以用歐式距離來表示對應(yīng)的啟發(fā)函數(shù)。歐氏距離指的是兩點之間的直線距離，假設(shè)機(jī)器人經(jīng)過單位長度的路徑的代價為C，則歐式距離的啟發(fā)函數(shù)為：

(3)

但是由于A*算法基于柵格地圖，機(jī)器人不能全向移動，所以在實際應(yīng)用中，這種啟發(fā)式函數(shù)需要進(jìn)行轉(zhuǎn)化，速度較曼哈頓慢，但是路徑更短。

基于上述24鄰域優(yōu)化，本文提出一種新的啟發(fā)函數(shù)，更真實的描述節(jié)點擴(kuò)展之后的代價，假設(shè)單位節(jié)點代價函數(shù)還是C，改進(jìn)后的啟發(fā)式函數(shù)如下。

若:|ny-goaly|≥|nx-goalx|:

(4)

若:|ny-goaly|<|nx-goalx|:

(5)

1.4 軌跡平滑

采用全局路徑規(guī)劃算法得到的路徑有時候會歪歪扭扭，不利于機(jī)器人的執(zhí)行，因此需要對路徑進(jìn)行處理，例如本文中對路徑中某些冗余節(jié)點進(jìn)行刪除，并將更優(yōu)的路徑選擇出來。軌跡平滑示意圖如圖2所示。

圖2 剔除冗余節(jié)點示意圖

圖中黑色部分表示障礙物，白色部分為可通行空白區(qū)域，黑實線表示未經(jīng)優(yōu)化的路徑，X1到X8代表路徑上的不同節(jié)點，虛線表示的是機(jī)器人可經(jīng)過的路徑。以X2節(jié)點為例，按照節(jié)點優(yōu)化策略，X2節(jié)點跳過相鄰節(jié)點，連接至X4，比較X2X4之間是否經(jīng)過障礙物，即機(jī)器人能否順利通過，若可以經(jīng)過，再檢查距離和之前相比是否變短，路徑確實變短后，檢測能否被機(jī)器人執(zhí)行，能否執(zhí)行主要查看24擴(kuò)展鄰域搜索范圍，若在24擴(kuò)展鄰域范圍內(nèi)，則可以順利進(jìn)行節(jié)點優(yōu)化并將該路徑加入到候選表中。檢查完X4節(jié)點，我們繼續(xù)檢查X5節(jié)點，重復(fù)上述流程，若X5也符合上述檢測標(biāo)準(zhǔn)，則將X4和X5兩個節(jié)點進(jìn)行對比，由分析可知，X2X5路線所優(yōu)化的路程長度大于X2X4，因此將X5替換掉候選表中的X4。由于機(jī)器人搜索區(qū)域的限制(24鄰域)，上圖中最佳的路線為X2X6。從頭到尾重復(fù)上述流程，最后將選出一條更為平滑的路徑。

1.5 改進(jìn)A*算法實驗結(jié)果

本文選用60*60的柵格對A*算法和改進(jìn)A*算法分別進(jìn)行仿真驗證，實驗結(jié)果如下圖所示。

圖3 原始A*算法

圖4 改進(jìn)A*算法

圖中位于坐標(biāo)(10,10)處的紅點為機(jī)器人起始點，位于坐標(biāo)(50,50)處的點為機(jī)器人要到達(dá)的目標(biāo)點，實心點表示的是障礙物，白色區(qū)域為空白地帶，淺色區(qū)域為算法搜索過的點，曲線表示最終規(guī)劃出的去全局路徑。由上述兩個圖可以直觀的看出，改進(jìn)后的A*算法能夠以更少的代價到達(dá)目標(biāo)點，而且軌跡更加平滑，更加理想。

2 動態(tài)窗口法

動態(tài)窗口法的基本思路是：在速度空間中，根據(jù)自身運動模型，對多組速度進(jìn)行采樣，分析出在各組不同速度下，一段時間內(nèi)機(jī)器人的運動軌跡，采用一定的評價函數(shù)對該組軌跡進(jìn)行評價，選擇出評價最高的一組來執(zhí)行，直到下一執(zhí)行時間的到來。動態(tài)窗口法示意圖如圖5所示。

圖5 動態(tài)窗口法示意圖

如圖5所示，矩形表示機(jī)器人，物體表示障礙物，虛線表示的是動態(tài)窗口法規(guī)劃出來的下一時刻內(nèi)的多組軌跡，由圖可知，有三組軌跡將碰到障礙物，因此將這三條軌跡舍棄，然后將剩下的軌跡通過其他評價函數(shù)進(jìn)行評分，最終選擇出最適合的一條軌跡執(zhí)行。

2.1 機(jī)器人運動模型

在動態(tài)窗口法中，對機(jī)器人建立運動模型是最基本的步驟。

假設(shè)機(jī)器人在間隔時間內(nèi)沿著直線運動。若機(jī)器人是非完整約束的，即不能進(jìn)行全向移動，只能進(jìn)行前進(jìn)和旋轉(zhuǎn)。我們先計算機(jī)器人在相鄰時刻的軌跡。 假設(shè)機(jī)器人在該相鄰時刻之間是勻速運動的，由于相鄰時間間隔很短，我們將其進(jìn)行近似處理成一段直線。此時可以得到機(jī)器人在世界坐標(biāo)系中的位移：

Δx=υΔtcos(θt)

(6)

Δy=υΔtsin(θt)

(7)

由上述位移公式我們可以得到機(jī)器人在一段時間內(nèi)的軌跡：

(8)

若機(jī)器人能夠進(jìn)行全向運動，我們需要另外將機(jī)器人在縱軸移動的距離投影到世界坐標(biāo)系。此時我們推導(dǎo)：

(9)

一段相鄰時間內(nèi)假設(shè)機(jī)器人的軌跡是直線是不準(zhǔn)確的，若要得到更精確的結(jié)果，我們需要將軌跡假設(shè)為曲線，最終軌跡是由許多段圓弧構(gòu)成。假設(shè)機(jī)器人不能進(jìn)行全向運動，那么軌跡中圓弧的半徑為：

γ=υ/ω

(10)

機(jī)器人坐標(biāo)為：

(11)

為了使最后結(jié)果更加精確，本文使用的是第二種模型，即假設(shè)機(jī)器人在相鄰時間內(nèi)的軌跡是圓弧。

2.2 機(jī)器人速度采樣

得到了機(jī)器人運動模型之后，下一步就是確定機(jī)器人的速度，這一步是動態(tài)窗口法的核心，只有得到了機(jī)器人速度才能對機(jī)器人的軌跡進(jìn)行預(yù)測。在速度空間內(nèi)，機(jī)器人可以在當(dāng)前時刻理論上可以由無窮多組，但是由于各種條件限制，可以將速度限制在某些范圍內(nèi)。

機(jī)器人受到自身極限速度限制：

Vm={υ∈[υmin,υmax],ω∈[ωmin,ωmax]}

(12)

機(jī)器人受到電機(jī)性能制約：由于不同電機(jī)具有不同的性能，提供給機(jī)器人的最大加減速也不一樣，因此在一個動態(tài)窗口內(nèi)顯示的速度就是機(jī)器人能夠?qū)嶋H達(dá)到的速度，設(shè)υc和ωc分別為機(jī)器人當(dāng)前速度和角速度，可得機(jī)器人在電機(jī)性能約束下的速度范圍：

(13)

機(jī)器人受障礙物約束：進(jìn)行局部軌跡規(guī)劃最重要的一點就是讓機(jī)器人能夠避開障礙物，動態(tài)窗口法剛開始并不知道障礙物的位置，在進(jìn)行速度的不斷選擇和軌跡評價后，若有軌跡碰到障礙物，此時就可以計算出機(jī)器人到障礙物的距離，然后根據(jù)當(dāng)前機(jī)器人本身性能，看是否可以以最大減速度在障礙物之前停下，如果計算出無法在障礙物之前停下，則將該軌跡刪除。該條件下速度范圍可以由下面的公式來進(jìn)行約束：

(14)

2.3 評價函數(shù)

動態(tài)窗口法最后一步就是對預(yù)測的軌跡進(jìn)行評價，我們采用三個不同的方面共同對軌跡進(jìn)行評價，客觀的得到最優(yōu)路徑。

第一個評價函數(shù)的方位角評價函數(shù)，方位角指的是機(jī)器人到達(dá)規(guī)劃出的軌跡末端時，當(dāng)前朝向和目標(biāo)點朝向的角度差。在這里用180°-θ來表示，即角度差越小，評價函數(shù)值越大，接受程度越高，方位角示意圖如圖6所示。

圖6 動態(tài)窗口法方位角示意圖

第二個評價函數(shù)為距離評價函數(shù)，也就是軌跡末端距離障礙物的遠(yuǎn)近程度，若軌跡與障礙物相交，則將次軌跡舍棄， 將其設(shè)定為一個常數(shù)。

第三個評價函數(shù)為速度評價函數(shù)，用來評價當(dāng)前機(jī)器人速度大小，速度間接影響到機(jī)器人距離函數(shù)。

由上述三個評價函數(shù)得到的總評價函數(shù)為：

G(υ,ω)=σ(α·heading(υ,ω)+β·

dist(υ,ω)+γ·velocity(υ,ω))

(14)

上述公式中heading(υ,ω)，dist(υ,ω)，velocity(υ,ω)分別表示機(jī)器人的方位角評價函數(shù)，距離評價函數(shù)和速度評價函數(shù)。為了使軌跡更加平滑，要對評價函數(shù)進(jìn)行歸一化處理，以方位角評價函數(shù)為例，歸一化指的就是每一項除以每一項的總和：

(15)

同理，其他兩個評價函數(shù)也可用相同方法進(jìn)行歸一化平滑處理。

2.4 動態(tài)窗口法驗證

如圖7所示，為了驗證動態(tài)窗口法的性能，我們對該算法進(jìn)行仿真驗證，機(jī)器人需要繞過障礙物到達(dá)目標(biāo)點。一連串線條表示動態(tài)窗口法所評價的軌跡，在這些軌跡中選擇一條最優(yōu)軌跡執(zhí)行。

圖7 動態(tài)窗口法的MATLAB仿真結(jié)果

3 系統(tǒng)綜合實驗

3.1 實驗仿真平臺介紹

本文仿真采用的是Ubuntu系統(tǒng)下的機(jī)器人操作系統(tǒng)(ROS，Robotic Operating System)，該操作系統(tǒng)于2007年誕生于斯坦福大學(xué)人工智能實驗室，雖然被稱為操作系統(tǒng)，但它只是借鑒了操作系統(tǒng)的精華，提供給用戶一系列方便的服務(wù)。作為一個開源操作系統(tǒng)，ROS為廣大研發(fā)人員提供了大量接口，支持多種編程語言，模塊化編程，大大提高開發(fā)效率。

ROS還為我們提供了大量軟件接口，例如廣泛使用的Gazebo物理仿真平臺，Rviz數(shù)據(jù)可視化工具等，我們可以用這些工具很方便的進(jìn)行開發(fā)。

3.2 實驗結(jié)果

本實驗在Gazebo平臺下搭建了輪式機(jī)器人以及具體的3D仿真環(huán)境，同主題將Gazebo與Rviz連接起來，這樣我們就可以將機(jī)器人在仿真環(huán)境中將具體的機(jī)器人采集到的數(shù)據(jù)進(jìn)行直觀展示。物理仿真結(jié)果如圖8所示。

圖8 Gazebo物理仿真環(huán)境搭建

在Rviz中設(shè)定機(jī)器人初始點和目標(biāo)點，橙色曲線表示為改進(jìn)A*算法規(guī)劃出的全局路徑，紅色箭頭表示設(shè)定的機(jī)器人最終位姿，機(jī)器人前方一系列綠色箭頭指的就是利用動態(tài)窗口法計算出的諸多預(yù)測軌跡，在其中找到最優(yōu)的一條執(zhí)行。為驗證機(jī)器人避障性能，設(shè)置機(jī)器人最大線速度為2 m/s,最大線加速度為0.8 m/s2,評價函數(shù)參數(shù)α=0.1，β=0.3，γ=0.2。從仿真結(jié)果可以看出機(jī)器人能夠很順利的從初始點沿著全局路徑前進(jìn)，并按照自身機(jī)械結(jié)構(gòu)限制進(jìn)行路徑的改良。

圖9 Rviz數(shù)據(jù)可視化結(jié)果

通過統(tǒng)計仿真過程中路徑節(jié)點，運行時間等數(shù)據(jù)，我們可以更加直觀的看出改進(jìn)A*算法和傳統(tǒng)A*算法之間的差別，統(tǒng)計表如表1所示。

表1 兩種算法對比

從表中可以看出，改進(jìn)后的A*算法搜索的節(jié)點數(shù)目更少，軌跡也更短，直接導(dǎo)致效率的增加，機(jī)器人行動速度變快。

4 結(jié)論

本文針對移動機(jī)器人路徑規(guī)劃，提出一種改進(jìn)A*算法，將算法搜索區(qū)域擴(kuò)展到24鄰域，克服傳統(tǒng)A*算法路徑轉(zhuǎn)折點多，不夠平滑，不易被機(jī)器人執(zhí)行的缺點，并設(shè)計了一種節(jié)點優(yōu)化策略，遍歷規(guī)劃好的路徑節(jié)點，剔除冗余節(jié)點，使路徑更優(yōu)。最后加入動態(tài)窗口法，極大提高了機(jī)器人避障能力，在保證路徑全局最優(yōu)的同時，可使機(jī)器人平滑的到達(dá)目標(biāo)點。

本文設(shè)計的改進(jìn)A*算法在小型地圖上效果顯著，但是在大規(guī)模地圖中，由于計算量過大，導(dǎo)致機(jī)器人實時性能低下，效果不如傳統(tǒng)算法，需要進(jìn)行進(jìn)一步優(yōu)化。