董 海，關甜甜

(沈陽大學a.應用技術學院；b.機械工程學院，沈陽 110044)

0 引言

隨著時代發(fā)展，很多領域開始應用移動機器人技術，尤其是路徑規(guī)劃技術。在生產領域，用于物料搬運的AGV，在快遞分揀中用于無人分揀小車和派送機器人等。按照生產需求，充分利用周圍環(huán)境，以感知、建模、規(guī)劃、執(zhí)行的標準路徑規(guī)劃方法執(zhí)行[1-2]。按照路徑規(guī)劃環(huán)境是否已知，可將其分為全局路徑規(guī)劃和局部路徑規(guī)劃；根據路徑規(guī)劃環(huán)境中是否存在動態(tài)障礙物，可分為靜態(tài)路徑規(guī)劃和動態(tài)路徑規(guī)劃[3-4]。

趙偉等[5]基于雙層優(yōu)化A*算法和動態(tài)窗口法，提出改進A*算法，通過一層和二層全局優(yōu)化降低機器人路徑的轉折次數，驗證了該方法適用于動態(tài)和靜態(tài)環(huán)境，實現有效避障和路徑規(guī)劃；XIONG等[6]針對動態(tài)環(huán)境下蟻群優(yōu)化路徑規(guī)劃中存在的收斂速度慢、全局搜索能力差和未知動態(tài)障礙物等問題，提出了一種基于時間禁忌策略的改進蟻群優(yōu)化算法，改善搜索弱點，應用網格預測模型解決實時避障問題；AJEIL等[7]提出一種修正頻率蝙蝠(MFB)算法，在環(huán)境中沒有障礙物時生成機器人路徑，當檢測到障礙物時開啟第二種模式進行避障，驗證提出的算法可以實現規(guī)劃的路徑更短，且過程中無碰撞； ZHANG等[8]提出了一種動態(tài)環(huán)境下移動機器人的預測路徑規(guī)劃算法，在預先搜索全局近似最優(yōu)路徑的前提下，根據移動機器人自身和動態(tài)障礙物的速度對其進行簡單的運動預測，仿真證明該算法使路徑規(guī)劃速度加快并降低路徑代價和碰撞概率。ELMI等[9]在動態(tài)和未知環(huán)境中，提出了一種基于蚱蜢算法的新型路徑規(guī)劃方法，對比得到該方法在運行時間、穩(wěn)定性和效率等方面具有良好的應用前景。

本文采用改進虛擬彈簧算法對機器人群的行進路線進行規(guī)劃。傳統(tǒng)虛擬彈簧算法，雖然環(huán)境適應性好，但是規(guī)劃出的路線不平滑，易受環(huán)境影響，且環(huán)境越復雜路線越曲折。本文以最短路徑和最短時間為目標，結合動態(tài)環(huán)境的情況，綜合考慮車間可能出現障礙物的不確定情況，有效提高處理障礙物的效率，降低避障時間；同時，在MATLAB軟件進行仿真，驗證其有效解決局部最小值和GNRON問題的能力，及具有良好的路徑規(guī)劃能力和避障功能。

1 模型建立

1.1 多機器人網格的邏輯拓撲

假設每個機器人只接受相鄰機器人的消息，沒有任何通信延遲。其中，n個機器人都通過虛擬彈簧相互連接。圖1為K=4和K=6的拓撲網絡。本文以K=6的拓撲模型為例，領導者機器人為第一層，是機器人(f1～f6)的父節(jié)點，每個機器人都有自己的子節(jié)點。

(a) K=4的拓撲網格 (b) K=6的拓撲網格

假設彈簧的自然長度為l0，實際長度為lr。機器人間的彈簧控制規(guī)則如下：

①當l0≤lr時，機器人f1和f2都將受到Fp的力，且與彈簧變形方向相反；若l0>lr時，機器人f1和f2都將受到力Fs，方向與彈簧變形方向相反；②當機器人f1的節(jié)點消失時，將機器人f1和f2之間的兩個彈簧視為一個被拉伸的彈簧，力的方向Fp與彈簧的變形方向相反。

1.2 多機器人的編隊控制

目前，常用的編隊控制[10]方法有3種：領導者-跟隨者[11]、基于行為的方法[12]和虛擬結構法[13]。本文應用領導者-跟隨者編隊法。機器人R1是領導者機器人，可以確定每個跟隨機器人Rfi(i=1,2,…,n)的運動。

空間中兩個機器人fi和fj用彈簧相連，它們之間的伸縮量為d，λ為剛度系數，彈簧受力表達式為：

F=λd

(1)

式中，依據經驗值將λ設置為1000。

在運動過程中，虛擬彈簧狀態(tài)Tij有松弛、壓縮和拉伸3種狀態(tài)，表示為：

(2)

式中，lrij為機器人fi和fj之間彈簧的物理長度，根據胡克定律，機器人fi和fj之間的虛擬彈簧力表示為：

Fij=Tijk0|l0-lrij|

(3)

為避免式(3)中機器人存在碰撞的可能，定義ls為機器人的期望安全距離，則表達式為：

(4)

Fij是矢量，指向彈簧的變形方向，角βij是機器人fi和fj之間的角：

(5)

則每個機器人的控制率可以表示為：

(6)

距離估計函數運用曼哈頓距離，表達式為：

D=abs(xij-xtarget)+abs(yij-ytarget)

(7)

式中，xij和yij分別表示機器人當前位置的橫縱坐標；xtarget和ytarget分別表示機器人目標位置的橫縱坐標。

判斷依據為彈簧的彈性勢能G，G越小，控制效果越好。能量函數可以表示為：

(8)

1.3 目標約束采樣

本文結合目標偏執(zhí)和位置約束兩個方法，以提高采樣的目標導向性[14]。設置目標偏執(zhí)概率ppz為0.1，其次根據均勻概率隨機產生一個概率值p，若該p

(9)

式中，Rr為隨機采樣函數。

若隨機生成采樣點，需要約束其位置。隨機生成的采樣點，對比其在X或Y方向比前一個采樣點更接近目標點作為依據。采樣約束示意圖如圖2所示。

圖2 采樣約束示意圖

1.4 避障控制策略

融合多機器人避障規(guī)劃的姿態(tài)信息和誤差反饋跟蹤調節(jié)方法[15]，定義多機器人避障的表達式為：

(10)

式中，

p(k)=Rn

(11)

式中，H(k)為多機器人避障中的障礙物矩陣；p(k)為多機器人群繞坐標系各軸的慣性協(xié)方差矩陣；若p(k)∈Q2(0,∞)，I、J、L、O為多移動機器人質心分布矩陣參數；n為控制約束的自變量維數，為正整數；ΔI1、ΔJ1為多移動機器人避障隱態(tài)誤差增益。

基于式(10)，多機器人避障規(guī)劃的控制器表達為：

(12)

考慮動態(tài)環(huán)境下的避障功能，引入動態(tài)A*算法中的估計函數：

f(x)=g(x)+h(x)

(13)

式中，f(x)為節(jié)點x的估價函數；g(x)為狀態(tài)空間中從初始節(jié)點到x節(jié)點的實際代價；h(x)為從x節(jié)點到目標節(jié)點最優(yōu)路徑的估計代價。

2 改進虛擬彈簧算法(IVS算法)

當多機器人距離障礙物足夠近時，障礙物產生排斥力Frep，排斥力與多機器人障礙物之間的距離成反比，并指向遠離障礙物的方向。Fa是Fatt和Frep的合力，機器人群將朝著合力的方向運動，直到到達目標。虛擬彈簧受力分析如圖3所示。虛擬彈簧受力分析表達式如式(14)所示。

圖3 作用在機器人群上的虛擬彈簧受力分析圖

(14)

為了避免碰撞，當Frep

(15)

式中，q0為障礙物坐標；f(q-q0)為機器人群與障礙物間的距離，迎面距離為障礙物的最大沖擊距離。

虛擬彈簧中儲存的彈性勢能表達為：

(16)

式中，Gg(q)為機器人群與目標之間虛擬彈簧的勢能；G0(q)為機器人群與障礙物之間虛擬彈簧的勢能。

傳統(tǒng)虛擬彈簧算法的不足是易陷入局部最小值或者GNRON問題，如圖4和圖5所示。

圖4 局部極小值時的路徑圖 圖5 GNRON時的路徑圖

2.1 基于IVS算法的多機器人路徑規(guī)劃

由于機器人群在轉向上的時間能量耗費比直線行駛要多，所以在傳統(tǒng)虛擬彈簧算法基礎上加入轉向代價，估價函數表達為：

f′(x)=g(x)+h(x)+ε0

(17)

式中，ε0為轉向代價，其值為：

(18)

假設空間中的目標與障礙物分布圖如圖6所示。機器人群連接起點O和目標點A形成OA，當OA不穿過任何障礙物，機器人群路徑規(guī)劃圖如圖7所示。

圖6 目標與障礙物之間分布圖 圖7 機器人群路徑規(guī)劃圖

當OA穿過障礙物時，機器人群以當前位置為中心旋轉，直到OA不穿過任何障礙物為止。旋轉后的矢量記為OB，軸向旋轉的轉角為γ，搜索點為T1，該點可滿足以下方程：

BT1·OB=0

(19)

|BT1|=d

(20)

(21)

根據式(19)～式(21)可得T1唯一的位置，視為虛擬目標，OT1為第一子路徑，當機器人群到達T1，若T1A通過障礙物，按照得到T1的方式，可得其它虛擬目標T2,T3,…,Tn，直到TnA不跨越任何障礙物，若機器人群當前位置O與所有虛擬目標點連接，最終目標與有向線段連接，即為機器人群從局部極小點到最終目標的期望最優(yōu)路徑。本文基于MATLAB進行仿真，結果如圖8所示。為評估算法的可靠性，計算彈性勢能Eg(q)如圖9所示。

圖8 機器人實際移動路徑圖 圖9 機器人間彈性勢能Eg(q)圖

從機器人群軌跡可看出，實際路徑與圖7規(guī)劃的路徑非常接近，成功到達目標。圖9顯示了機器人群到達各個虛擬目標和最終目標的彈性勢能情況，該圖可匹配圖8中的軌跡。

2.2 多機器人編隊路徑規(guī)劃

在多機器人系統(tǒng)中，機器人群的目標還包括編隊[16]。在動態(tài)環(huán)境下，機器人群實時更新附近障礙物情況以此判斷哪個方向位置為最佳。采用IVS算法的多機器人動態(tài)路徑規(guī)劃的流程圖如圖10所示。

圖10 多機器人動態(tài)路徑規(guī)劃流程圖

3 仿真分析

為評估所提的策略和算法的性能，設置機器人群步長為0.06，仿真中所有參數均視為無量綱數。

3.1 多機器人編隊路徑規(guī)劃

機器人群之間虛擬彈簧的松弛長度l0=3，剛度因子k0=300。障礙物位置均為隨機分布，障礙物的虛擬彈簧松弛長度β=1000，剛度因子kr=600，領導者機器人的目標位置坐標為(68,68)，剛度因子kg=6。6個機器人穿越并避開隨機障礙物的路線和彈性勢能圖如圖11所示。

由圖11a中可得，機器人群路徑規(guī)劃過程中沒有發(fā)生任何碰撞。如圖11b中的彈性勢能走向可得，該圖與軌跡匹配，驗證底層控制效果良好。

(a) 6個機器人穿越并避開隨機障礙物路線圖 (b) 6個機器人之間的彈性勢能圖

領導者機器人和跟隨者機器人穿越并避開隨機障礙物的初始狀態(tài)如表1所示。領導者機器人和跟隨者機器人穿越并避開固定障礙物的初始狀態(tài)如表2所示。

表1 領導者機器人和跟隨者機器人穿越并避開隨機障礙物的初始狀態(tài)

表2 領導者機器人和跟隨者機器人穿過并避開固定障礙物的初始狀態(tài)

設置l0、k0、β=1000、kr同上。領導者機器人的目標位置坐標為(30,65)，剛度因子kg=6。6個機器人穿越并避開固定障礙物的路線和彈性勢能圖如圖12所示。

(a) 6個機器人穿越并避開固定障礙物路線圖 (b) 6個機器人之間的彈性勢能圖

從圖12a中可以明顯看出，機器人群保持原有隊形，成功到達目標，且無碰撞。圖12b驗證了多機器人在編隊控制方面表現出了良好的性能。

3.2 算法比較

3.2.1 路徑平滑比較

程傳奇、王殿君等[17-18]基于傳統(tǒng)A*算法提出動態(tài)A*算法，適用于典型的動態(tài)路徑規(guī)劃。本文模擬車間的動態(tài)環(huán)境，將IVS算法與傳統(tǒng)的虛擬彈簧算法和動態(tài)A*算法進行比較。

參數θ、§、γ的選取為平滑處理路徑節(jié)點的重要環(huán)節(jié)。經過多次仿真實驗的結果得出，θ=125°，§=56°，γ=2時路徑平滑處理效果最好。路徑平滑處理的參考標準有3種：累計轉角、平均轉角和路徑長度。采用3種算法生成的路徑圖如圖13所示。不同算法路徑平滑處理對比表如表3所示。

圖13 3種算法生成的路徑對比圖

表3 不同算法路徑平滑處理對比表

由表3可得，IVS算法相比于傳統(tǒng)VS算法和動態(tài)A*算法，累計轉角分別降低了79.07%和25.63%，平均轉角分別降低了41.83%和29.14%，路徑長度分別降低了61.76%和18.69%，采用IVS 算法規(guī)劃處的路徑更加平滑。

3.2.2 最短時間和最短路徑比較

為適應更多障礙物環(huán)境，本文覆蓋率分別在8.6%、17.2%和28.4%的環(huán)境下仿真比較，如表4所示。

表4 不同算法下機器人路徑規(guī)劃的路徑長度與時間對比表

由表4可得，IVS算法相比于傳統(tǒng)VS算法和動態(tài)A*算法，在障礙物覆蓋率為8.6%時，路徑規(guī)劃的時間和路徑長度分別減少了38.1%和18.75%；在障礙物覆蓋率為17.2%時，路徑規(guī)劃的時間和路徑長度分別減少了33.33%和18.18%；在障礙物覆蓋率為28.4%時，路徑規(guī)劃的時間和路徑長度分別減少了23.43%和10.91%。

3種算法的路徑規(guī)劃時間和路徑長度對比圖如圖14和圖15所示。

圖14 3種算法的路徑規(guī)劃時間隨環(huán)境變化折線圖 圖15 3種算法的路徑規(guī)劃長度隨環(huán)境變化折線圖

由圖14得出，隨著環(huán)境大小的增大，應用本文算法使得時間增長的幅度明顯低于其他兩種算法?？勺C，本文提出的算法更優(yōu)。

圖15可得，隨著環(huán)境大小的增加，應用本文算法使得路徑長度增長的幅度更小，可證，IVS算法在機器人行走路程上短于采用其他兩種算法的多機器人行走路程，驗證本文算法在路徑規(guī)劃上的搜索效率高和收斂速度快的特點。

3.3 動態(tài)環(huán)境仿真

在仿真環(huán)境下，以(4,0)為起點，(35,45)為目標點進行動態(tài)環(huán)境下得路徑規(guī)劃能力驗證，在路徑中選取兩個局部子目標點，設置為P1，P2，如圖16所示。本文設定障礙物的行駛速度小于機器人群的行駛速度。

圖16 局部子目標點位置圖

圖16中，路徑被分為目標點-P1；P1-P2和P2-目標點。機器人群在P1-P2路徑內，檢測到前方有障礙物出現(黑色方塊)，若按原路線行駛，將會在灰色方塊位置發(fā)生碰撞，如圖17所示。由于障礙物和機器人群的行動軌跡交于一點，需要對P1-P2段路徑進行局部規(guī)劃，如圖18所示。

圖17 機器人位于P1點 圖18 機器人位于P2點

P1-P2段原路徑用虛線表示，其他兩段路徑用實線表示。由圖17和圖18可證，應用IVS算法可實現動態(tài)環(huán)境下的避障，且避障能力充足。

4 結束語

(1)提出IVS算法，可規(guī)劃出最優(yōu)路徑，且具有搜索效率高、避障能力強的特點。

(2)采用IVS算法可以有效解決局部最小值和GNRON問題，且實現實時避障。

(3)將IVS算法、傳統(tǒng)VS算法與動態(tài) A*算法對比，驗證應用IVS算法得到的路徑更加平滑，能夠適應不同障礙物密度環(huán)境，且有效避開動態(tài)障礙物。