余文凱，章 政+，金 震，黃衛(wèi)華

(1.武漢科技大學 機器人與智能系統(tǒng)研究院，湖北 武漢 430081；2.武漢科技大學 信息科學與工程學院，湖北 武漢 430081)

0 引 言

移動機器人的多目的地路徑規(guī)劃是指從起點自主移動到兩個或兩個以上目的地的無碰可行路徑[1-3]。對于多目的地路徑規(guī)劃而言，采用現(xiàn)有的單目的地規(guī)劃方式，則需要多次規(guī)劃起點到各目的地的路徑，存在多次路徑的往返，甚至無效路徑等問題，影響機器人的路徑搜索效率。因此，在多目的地環(huán)境下實現(xiàn)一次性的完整路徑規(guī)劃，對于節(jié)約運行時間、提高規(guī)劃效率具有重要的意義。

多目的地的路徑規(guī)劃問題主要涉及如何快速搜索出各兩地點之間的路徑、快速排序目的地點和局部動態(tài)規(guī)劃。文獻[5]針對公路運輸?shù)穆窂揭?guī)劃問題提出一種求解多目的地運輸路線規(guī)劃問題的方法，并根據(jù)不同路段運費及時間等權(quán)重，利用哈密頓圖/半哈密頓圖的構(gòu)造條件選取適當?shù)穆窂綐?gòu)造哈密頓通路，使之在運輸過程中選取合適的路徑不重復地遍歷所有目的地，達到最優(yōu)效果。文獻[6]提出了一種基于利用蟻群算法快速收斂的特性改進粒子群算法的路徑規(guī)劃收斂性的方法，該方法將目標地點的選擇轉(zhuǎn)化為旅行商問題，并利用蟻群算法進行優(yōu)化。文獻[7]提出一種分級粒子群、遺傳和A*算法相結(jié)合的遍歷多任務路徑規(guī)劃方法，解決了粒子群-遺傳算法存在計算成本過高并且單一算法不能解決有障礙物存在的地圖上遍歷多任務目標點的移動機器人避障行走問題。上述研究工作對多目的地的全局靜態(tài)路徑規(guī)劃進行了研究，驗證了多目的地路徑規(guī)劃比多次單目的地路徑規(guī)劃的效率高。然而，多目的地路徑規(guī)劃算法不僅涉及全局的靜態(tài)規(guī)劃，還有機器人在移動中的局部動態(tài)規(guī)劃。

因此，本文結(jié)合改進A*算法、模擬退火算法和改進動態(tài)窗口算法設計了一個結(jié)合靜態(tài)全局路徑規(guī)劃和局部動態(tài)規(guī)劃的多目的地路徑規(guī)劃算法。實驗結(jié)果表明，本文所設計的算法具有搜索效率高、適應性強和運動速度平穩(wěn)性好等特點。

1 多目的地路徑規(guī)劃算法設計

機器人移動到達多個目的地，不僅需要規(guī)劃兩地點之間的路徑，還需要規(guī)劃到各個目的地的先后順序。如圖1所示，其中S為起點，Ti(i=1,2,3,4)為目的地，全局移動路徑的方案有多種選擇(如圖1方案一、方案二)，需要進行全局靜態(tài)規(guī)劃，獲得全局的最優(yōu)路徑。而在移動中，機器人在按照全局靜態(tài)最優(yōu)路徑進行移動時，面臨行程中出現(xiàn)路徑可能會被占據(jù)，目的地位置可能發(fā)生變換等問題。單一的全局靜態(tài)規(guī)劃不能解決移動中的問題。因此，將多目的地路徑搜索分成全局靜態(tài)路徑規(guī)劃和局部動態(tài)規(guī)劃，保證機器人安全、準確抵達每一個目的地。

圖1 多目的地路徑規(guī)劃方案

全局靜態(tài)路徑規(guī)劃是指機器人在移動前規(guī)劃一條由起點經(jīng)過所有目的地再回到起點的全局路徑。首先，需要多次運算起始地點與目的地(S→T1，S→T2，…)、目的地與目的地(T1→T2，T2→T3，…)兩兩之間的路徑規(guī)劃，并計算兩地點之間規(guī)劃路徑的距離大小；然后，依據(jù)此距離大小對多個目的地進行優(yōu)化排序，選擇一條距離最小的全局路徑。如圖1所示，其路徑為(S→T1→T2→T3→T4→S或S→T2→T1→T3→T4→S…)。運算量雖然增加，但規(guī)劃后的全局路徑一次經(jīng)過所有目的地，移動路徑大大減少。

局部動態(tài)路徑規(guī)劃是指機器人在移動過程中，面對出現(xiàn)的多個目的地確保機器人可以有效進行局部動態(tài)避障的同時，能按照預先規(guī)劃路徑進行移動。設計多目的地局部動態(tài)路徑規(guī)劃時，將預先規(guī)劃的路徑節(jié)點當作當前移動的目標點，每兩個路徑節(jié)點組成一段路徑，通過機器人傳感器獲取障礙物信息，判斷這一段路徑內(nèi)是否有障礙物，存在障礙物進行局部動態(tài)避障，否則依據(jù)規(guī)劃的路徑進行移動。當目的地位置發(fā)生變化時，可將當前節(jié)點設置為新的目的地坐標。即保證了全局路徑最優(yōu)，又可及時動態(tài)規(guī)劃。

針對多目的地路徑規(guī)劃的特點，本文設計了一種結(jié)合全局靜態(tài)路徑規(guī)劃與局部動態(tài)路徑規(guī)劃的組合路徑搜索算法，其結(jié)構(gòu)如圖2所示。

圖2 多目的地路徑規(guī)劃算法結(jié)構(gòu)

2 基于改進A*算法的多目的地全局靜態(tài)路徑規(guī)劃

2.1 改進A*算法兩地點間的路徑規(guī)劃

A*算法在多目的地全局靜態(tài)路徑規(guī)劃中需要多次運算進行兩地點間的路徑規(guī)劃，是解決靜態(tài)路網(wǎng)中最短路徑的最有效的直接搜索方法。

基于柵格法建立地圖模型，采用A*算法規(guī)劃的路徑是由柵格中心節(jié)點組成，如圖3所示，路徑為(S,n1,n2,n3,n4,n5,n6,n7,n8,n9,n10,Ti)；存在許多問題，例如冗余的路徑節(jié)點，多路徑拐點，并且路徑節(jié)點只能位于網(wǎng)格的中心。若直接將A*算法規(guī)劃的路徑節(jié)點代入到動態(tài)規(guī)劃算法中，則導致路徑不是最短且平滑度較差。因此，對A*算法規(guī)劃的路徑節(jié)點進行刪除冗余節(jié)點的優(yōu)化改進。

圖3 路徑平滑優(yōu)化

步驟1 判斷節(jié)點位置是否為一段路徑的中間位置，然后刪除同一方向上的中間節(jié)點，只保留起始的節(jié)點、轉(zhuǎn)彎處的節(jié)點及目標點。如圖3所示，運算后保留的路徑為S→n7→n8→n9→Ti。

步驟2 從路徑的起始點S方向開始運算，在保留的路徑ni、nj之間每隔k步長取一節(jié)點m，并與前一段路徑節(jié)點判斷，判斷兩點之間路徑是否存在障礙物。

(1)若有障礙物，則路徑節(jié)點不變；

(2)若無障礙物，如圖4所示，計算障礙物與兩點間的連線的距離d。

圖4 障礙距離判斷

圖4中，路徑節(jié)點S的坐標為(xa,ya)，n12的坐標為(xb,yb)，障礙點C的坐標為(xc,yc)，計算距離d為點C與路徑AB之間線段的垂直距離，l為障礙點C到節(jié)點S與節(jié)點n12之間線段縱軸的距離CD，節(jié)點AB之間線段與橫軸之間的夾角為?。由S、n12及C三點坐標可求出障礙點到路徑的距離為

d=lcos?

(1)

判斷障礙物與兩點間的連線距離d與設置的安全距離D的關系；

(1)若d≤D，則路徑節(jié)點不變；

(2)若d>D，則節(jié)點m為新的路徑節(jié)點。

如圖3所示，處理后的路徑為S→n11→Ti。

步驟3 從路徑目的地點T的方向，反向取點判斷(重復步驟2的判斷方式)，輸出優(yōu)化路徑Path，算法結(jié)束。如圖3所示，優(yōu)化后路徑為S→n12→Ti。

2.2 基于模擬退火算法的多目的地排序策略

規(guī)劃完所有兩地點間的路徑后，如圖1移動順序有多種情況如S→T1→T2→T3→T4→S或S→T2→T1→T3→T4→S，……。因此，需要對多目的地優(yōu)化排序，選擇出一條經(jīng)過所有目的地且距離最短的全局路徑。

假設在移動機器人多目的地路徑規(guī)劃中含有n個目的地，則路徑到達順序的組合有(n2+n)/2種。為了滿足移動機器人運動路徑最短要求，機器人需要從(n2+n)/2種組合中選擇一條最短路徑。模擬退火算法[6]參數(shù)少，計算時間短，魯棒性強，適用于本文高效率的要求，多目的地排序過程采用模擬退火算法優(yōu)化排序。

基于模擬退火算法的多目的地排序算法步驟如下：

步驟1 確定初始溫度Temax、終止溫度Temin及降溫速率r(0

步驟2 將(S,T1,T2,…,Tn,S)組成n！個排列組合的解空間Ι={(S,T1,T2,…,Tn,S)、(S,T2,T1,…,Tn,S)…}，并隨機生成一個初始解Ιo(Ιo∈Ι)；

步驟3 通過二變換法產(chǎn)生新解，任選序號m，k，交換解元素第m個和第k個之間的訪問順序，若交換的前路徑解為Ιi=(S,T1,T2,…,Tm,…Tk,…Tn,S)，交換后的路徑為新路徑，如式(2)所示

Ι′i=(S,T1,…,Tm-1,Tk,Tm+1,…,Tk-1,Tm,Tk+1,…,Tn,S)

(2)

步驟4 設置距離長度為目標函數(shù)，再由式(3)、式(4)計算變換前的解Ιo和變換后的解Ι′i的目標函數(shù)的差值Δf

(3)

Δf=f(Ι′i)-f(Ιi)

(4)

步驟5 由Metropolis接受準則式(5)計算接收概率p，選出下一代新解；若Δf<0，表示變換后的解代價小于變換前的解，新解為Ι′i；若Δf>0，表示變換后的解代價大于變換前的解，則通過接受當前路徑作為新的路徑概率p判斷新解，概率p越大接受可能性就越大

(5)

步驟6 更新溫度Te′，由降溫函數(shù)式(6)降溫

Te′=Te×r

(6)

步驟7 判斷降溫后的溫度Te′是否到達終止溫度Temin，滿足條件，則輸出最優(yōu)解，算法結(jié)束；否則，執(zhí)行步驟3；

3 基于改進DWA算法的多目的地局部動態(tài)路徑規(guī)劃

DWA(dynamic window approach)算法[11]作多目的地的局部動態(tài)路徑規(guī)劃算法可以將預先規(guī)劃的路徑節(jié)點與局部動態(tài)規(guī)劃有效結(jié)合，讓機器人按全局規(guī)劃的最優(yōu)路徑移動的同時，能安全、準確地到達目的地。

移動中DWA算法時刻模擬運動軌跡，選擇評價函數(shù)最高的一組為下一時刻的運行線速度和角速度進行移動。其評價函數(shù)中有表示模擬軌跡與目的地之間方位角偏差的方位角評價函數(shù)，可將目的地改成全局規(guī)劃的路徑節(jié)點，依據(jù)移動位置不斷改變路徑節(jié)點，直到變成目的地。DWA算法可有效結(jié)合全局規(guī)劃的路徑，實時性好，動態(tài)避障能力強，滿足高效率的要求。

3.1 機器人運動模型

基于動態(tài)窗口算法的局部動態(tài)規(guī)劃預測機器人的運動軌跡，選擇最優(yōu)路徑。首先，建立機器人的運動模型。設機器人的線速度為vt和角速度為wt。當前機器人的位姿表示為(xo,yo,θo)，假設機器人在時間間隔Δt內(nèi)作勻速直線運動，則下一時刻位姿為(xt,yt,θt)，運動模型如式(7)所示，為

(7)

3.2 機器人速度約束及采樣

動態(tài)規(guī)劃中預測軌跡是由機器人可能產(chǎn)生的速度組合(vt,wt)來采樣模擬，其速度組的大小范圍不僅有機器人自身約束，即機器人速度存在最大、最小速度的約束如式(8)所示，還有緊急剎車的安全距離約束，如式(9)所示。因此，在機器人自身限制及環(huán)境因素約束條件下，速度采樣范圍為V，如式(10)所示

V1={(v,w)|v∈[vmin,vmax]∩w∈[wmin,wmax]}

(8)

(9)

V=V1∩V2

(10)

在速度采樣范圍V下，可生成多組不同速度組(v,w)的模擬軌跡，如圖5所示，選擇評價函數(shù)最高的一組(v,w)進行移動。

圖5 動態(tài)窗口采樣

3.3 改進采樣速度的評價函數(shù)

動態(tài)規(guī)劃中，計算生成模擬軌跡每組(v,w)的評價函數(shù)，選擇最優(yōu)(v,w)進行移動，傳統(tǒng)動態(tài)窗口的評價函數(shù)為

G(v,w)=α·head(v,w)+β·dist(v,w)+γ·vel(v,w)

(11)

傳統(tǒng)DWA算法進行規(guī)劃時，缺少全局靜態(tài)規(guī)劃路徑的指引，評價函數(shù)dist(v,w)使移動機器人軌跡遠離障礙物，易生成局部最優(yōu)軌跡，增大了全局的移動距離。若直接減少其加權(quán)系數(shù)β，則對動態(tài)障礙物避障時，不能及時避障，導致移動機器人與動態(tài)障礙物發(fā)生碰撞。因此，本文將靜態(tài)全局規(guī)劃的路徑節(jié)點與DWA算法的評價函數(shù)相結(jié)合。改進了動態(tài)窗口的評價函數(shù)，方位角的評價函數(shù)和靠近障礙物距離的評價函數(shù)，如式(12)所示

G(v,w)=α·heading(v,w)+β·dist_sta(v,w)+
γ·vel(v,w)+λ·dist_dyna(v,w)

(12)

式中：heading(v,w)為當前目標點方位角評價函數(shù)，即下一個路徑的移動節(jié)點作為當前的目標點，在當前速度下，模擬軌跡的位置與當前目標點的方位角偏差；此改進有利于移動機器人沿著全局規(guī)劃的最優(yōu)路徑移動。dist_sta(v,w)為速度對應軌跡上離全局靜態(tài)已知障礙物最近距離的評價函數(shù)，dist_dyna(v,w)為速度對應軌跡上離局部動態(tài)未知障礙物最近距離的評價函數(shù)；對障礙物區(qū)別后，可分別調(diào)節(jié)其加權(quán)系數(shù)β和λ，既可以減少已知障礙物對規(guī)劃軌跡的干擾，又保證了對未知障礙物及時避障。

3.4 初始位姿狀態(tài)預處理

多目的地路徑規(guī)劃中，移動機器人初始位姿方向和每到達一個目的地的位姿方向與靜態(tài)規(guī)劃路徑方向并不相同，其初始位姿與路徑目標節(jié)點的方位角偏差Δθo越大，生成運動軌跡的轉(zhuǎn)彎幅度越大，導致移動距離增大，如圖5移動軌跡所示。

針對上述問題，本文設計了姿態(tài)調(diào)整函數(shù)，在起始點移動前調(diào)整機器人姿態(tài)的方向角θo，將其姿態(tài)與預先規(guī)劃下一個路徑節(jié)點的方位角偏差Δθo消除

(13)

4 多目的地路徑規(guī)劃算法流程

本文基于多目的地的路徑規(guī)劃設計的算法結(jié)構(gòu)如圖6所示；其算法步驟如下：

步驟1 初始化柵格地圖模型，確認n個目的地坐標；

步驟2 生成(n2+n)個規(guī)劃組；

步驟3 基于A*算法對其所有組進行路徑規(guī)劃，并進行路徑節(jié)點雙向平滑度優(yōu)化；

步驟4 基于模擬退火算法對所有路徑進行優(yōu)化排序，得到完整最優(yōu)路徑；

步驟5 得到開始移動指令；

步驟6 判斷初始位置與下一個路徑節(jié)點的方向角偏差Δθo，若Δθo為0，則開始移動；否則，姿態(tài)調(diào)整消除方向角偏差Δθo；

步驟7 獲取信息，若目的地位置發(fā)生變化，則將當前路徑節(jié)點設置為目的地坐標；否則，按全局規(guī)劃路徑設置當前路徑節(jié)點。

步驟8 依據(jù)當前路徑節(jié)點，采用改進后的DWA算法的評價函數(shù)實時動態(tài)規(guī)劃，直到到達當前目的地；

步驟9 判斷當前目的地是否為出發(fā)點，滿足條件，則運動結(jié)束；否則準備移動下一個目的地點，執(zhí)行步驟5。

圖6 算法流程

5 多目的地仿真實驗

為驗證本文改進算法的可行性和有效性，設計了多目的地路徑規(guī)劃仿真實驗；為方便對比研究，假定移動機器人工作環(huán)境柵格邊長為單位1 m，地圖模型為40×40，其改進A*中的參數(shù)取點步長k=0.1,設安全距離D=0.8 m。

(1)多目的地靜態(tài)全局路徑規(guī)劃仿真實驗

為驗證多目的地路徑規(guī)劃算法能提高搬運機器人的運輸效率，設置了8組仿真實驗，每組實驗的目的地個數(shù)逐漸增加。實驗得到移動路徑長度如圖7所示；其中8個目的地的規(guī)劃路徑對比如圖8、圖9所示。圖中S點表示機器人的搬運地點，Ti點(i=1,2,3,4,5,6,7,8)表示搬運的目的地，黑色柵格區(qū)域表示障礙物，實線表示規(guī)劃的路徑。

圖7 路徑長度對比

圖8 8次單目的地路徑規(guī)劃

圖9 多目的地路徑規(guī)劃

由圖8可知單目的地路徑規(guī)劃導致移動機器人需要多次回到起始點，降低了機器人的運輸效率；圖9中本文多目的地規(guī)劃的路徑一次規(guī)劃可到達所有目的地，減少了運算路徑的距離。由圖7路徑長度對比圖可知，隨著目的地個數(shù)增加，單目的地路徑規(guī)劃算法規(guī)劃的路徑長度增長很快，而本文設計的多目的地路徑規(guī)劃算法規(guī)劃的路徑長度增長平緩，與單目的地路徑規(guī)劃算法相比，本文平均長度減少了44.2%的長度，提高了移動機器人的工作效率。

(2)多目的地動態(tài)路徑規(guī)劃仿真實驗

為驗證多目的地局部動態(tài)規(guī)劃的可行性及有效性，本文設計了在基于改進A*算法和姿態(tài)調(diào)整等其它相同的情況下，對比傳統(tǒng)DWA算法和本文改進DWA算法的不同評價函數(shù)下局部動態(tài)避障能力。其中實驗參數(shù)設置如下：最大速度為1.0 m/s，最大角速度為20.0°/s，最大線加速度為0.2 m/s2，最大角加速度為50.0°/s2，預測周期為3.0 s;對比評價函數(shù)參數(shù)α=0.3，β=0.4，γ=0.3，本文改進的評價函數(shù)參數(shù)α=0.3，β=0.1，γ=0.3，λ=0.4。

在40×40的地圖模型中，設置兩個目的地，仿真實驗結(jié)果如圖10、圖11所示，圖中S點表示機器人的搬運地點，Ti點(i=1,2)表示目的地，T’2點表示中途目的地發(fā)生變化后的位置；黑色柵格為已知的障礙物，灰色為未知的障礙物；虛線為靜態(tài)全局規(guī)劃的路徑，實線為移動中局部動態(tài)規(guī)劃的路徑，移動順序為S→T2→T1→S。仿真實驗結(jié)果如圖12、圖13所示，圖中實線表示線速度隨控制節(jié)點變化情況，虛線表示角速度隨控制節(jié)點變化情況。

圖10 未改進DWA局部避障規(guī)劃

圖11 改進DWA局部避障規(guī)劃

由圖10和圖11移動的路徑對比可知，在全局靜態(tài)規(guī)劃相同的情況下，實際移動中傳統(tǒng)的DWA算法目的地位置發(fā)生變化后不能及時重新規(guī)劃，到達原目的地后才重新規(guī)劃到達新的目的地，導致移動路徑增大；且易受靜態(tài)障礙物的影響，移動路徑轉(zhuǎn)彎幅度大，沒有充分利用全局規(guī)劃的直線路徑，增大了移動距離。而本文改進后的DWA算法按照全局規(guī)劃的路徑移動時，移動路徑轉(zhuǎn)彎幅度小。且及時動態(tài)規(guī)劃移動到新的目的地，減少了不必要的移動路徑。

圖12 傳統(tǒng)DWA線/角速度變化

圖13 改進DWA線/角速度變化

由圖12和圖13移動中線速度變化情況可知，到達目的地的過程中，傳統(tǒng)動態(tài)窗口的線速度震蕩次數(shù)多、最大震幅變化是穩(wěn)定線速度的71.2%；其角速度一直變化中，其最大值達到了運動限定的最大角速度。而本文改進后的動態(tài)窗口法的線速度震蕩次數(shù)少，且振幅變化的最大值只有穩(wěn)定線速度20.5%；且角速度變化都在限定的速度內(nèi)，振幅次數(shù)減少；沒有產(chǎn)生不必要的路徑，機器人的移動時間減少了22.6%。實驗結(jié)果表明，本文設計的局部動態(tài)規(guī)劃使移動機器人安全、準確的到達了每個目的地。運動過程沒有頻繁加減速，線速度穩(wěn)定值高，提高了移動機器人的工作效率。

6 結(jié)束語

本文針對多目的地路徑規(guī)劃場景，設計一種基于改進A*、模擬退火策略和改進動態(tài)窗口的多目的地路徑規(guī)劃算法。首先，采用A*算法進行兩點間的路徑規(guī)劃，并對路徑節(jié)點進行雙向平滑度優(yōu)化。然后，引用模擬退火策略對所有規(guī)劃完的路徑進行優(yōu)化排序，得到一條全局最優(yōu)路徑。最后，對動態(tài)窗口算法評價函數(shù)進行改進，并將優(yōu)化后的路徑節(jié)點和目的地位置動態(tài)設置為當前路徑節(jié)點，使移動機器人穩(wěn)定的按照全局路徑移動，同時能及時避礙和準確移動到目的地。仿真結(jié)果表明，本文設計的多目的地路徑規(guī)劃算法在全局規(guī)劃中的路徑長度比單目的地路徑規(guī)劃算法平均長度減少了44.2%，提高了移動機器人的工作效率。局部動態(tài)規(guī)劃中比傳統(tǒng)動態(tài)避障的線速度震蕩次數(shù)平均減少了50%，運動時間減少了22.6%；有效提高了移動機器人移動速度的平穩(wěn)性和工作的效率。