基于ATB-RRT*與改進DWA的多機器人編隊路徑規(guī)劃

姜君策 伍錫如

摘要：針對多機器人編隊規(guī)劃算法在多障礙物區(qū)域路徑規(guī)劃與編隊保持能力較弱的問題，提出一種自適應(yīng)采樣目標引力雙向RRT*（Adaptive sampling target gravitational bidirectional RRT*，ATB-RRT*） 與改進動態(tài)窗口法（Dynamic window approach，DWA） 的融合算法。引入自適應(yīng)采樣與目標引力機制改進雙向RRT*算法，改善算法隨機性;引入新節(jié)點刪除策略刪除低質(zhì)量節(jié)點，提高算法效率；設(shè)計領(lǐng)航跟隨型機器人編隊控制器，并利用全局路徑點改進DWA算法，增強編隊在遇到動態(tài)障礙后的編隊保持能力。仿真實驗表明，ATB-RRT*算法相比雙向RRT*算法的效率有明顯提升，應(yīng)用改進DWA算法的多機器人編隊有較強的編隊保持能力。

關(guān)鍵詞：多機器人編隊；路徑規(guī)劃； ATB-RRT*算法；領(lǐng)航跟隨；動態(tài)窗口法

中圖分類號：TP273? ? ? 文獻標識碼：A

文章編號：1009-3044（2023）13-0005-05

開放科學(xué)（資源服務(wù)）標識碼（OSID） ：

0 引言

多機器人編隊控制是機器人領(lǐng)域的熱點問題，由于要考慮障礙物、編隊形成保持等約束，規(guī)劃編隊路徑具有一定的難度[1]。多機器人編隊控制傳統(tǒng)解決方案為通過狀態(tài)觀測器控制編隊機器人的角度與距離實現(xiàn)[2]，文獻[3]通過增加滑膜模糊積分觀測器來應(yīng)對編隊運行中產(chǎn)生的擾動，文獻[4]提出了一種基于距離約束的單領(lǐng)航者編隊控制算法，通過自適應(yīng)控制律預(yù)估領(lǐng)航者速度保持編隊。上述文獻雖對編隊路徑規(guī)劃有一定作用，但控制方式較為復(fù)雜，當需要考慮規(guī)劃及避障時控制效果無法保證。

RRT算法是一種基于采樣的概率完備全局規(guī)劃算法，在機器人運動規(guī)劃領(lǐng)域應(yīng)用廣泛，但該算法在多障礙物環(huán)境下搜索效率較低[5]。針對上述問題，相關(guān)研究者提出多種基于RRT的泛型算法，如具有漸進最優(yōu)性的RRT*、雙樹搜索的Bi-RRT*、利用先驗信息搜索的Informed-RRT*等算法[6-8]，改善了算法在多障礙物環(huán)境的搜索性能。文獻[9]通過改進A*全局算法引導(dǎo)動態(tài)窗口法，改善動態(tài)窗口法對于復(fù)雜障礙場通過能力差的問題。上述研究主要針對單機器人規(guī)劃進行改進，當對象換為多機器人編隊時搜索性能改進程度較低。

針對多機器人編隊路徑規(guī)劃問題，本文提出一種基于ATB-RRT*和改進DWA的多機器人編隊路徑規(guī)劃算法。先后利用ATB-RRT*和DWA規(guī)劃編隊中領(lǐng)航者的路徑軌跡，改進DWA算法回歸編隊路徑。實驗表明，該算法能為編隊規(guī)劃出成本較低的路徑，避障礙后編隊保持能力較強。

1 機器人運動學(xué)模型

本文采用麥克納姆輪全向四輪式機器人，機器人模型及實物如圖1所示。設(shè)輪半徑為R，輪子軸線到機器人中心在x與y軸方向的距離分別為lx和ly ，速度分別為vx與vy，機器人繞z軸的角度為wz，四個輪子的線速度與轉(zhuǎn)速分別為vir、wi（i=1，2，3，4）。則機器人運動學(xué)方程為：

[vxvywz=R411-1lx+ly-111lx+ly-11-1lx+ly111lx+lyw1w2w3w4]? （1）

基于l-φ型的領(lǐng)航跟隨型多機器人編隊模型如圖2所示。圖中Rl、Rf分別為領(lǐng)航與跟隨機器人。隊形通過控制兩者之間的相對角度[φ*lf]與理想相對距離[l*lf]實現(xiàn)，多機器人編隊距離公式為：

[lxlf=（xl-xf-dcosθf）cosθl+（yl-yf-dsinθf）sinθllylf=-（xl-xf-dcosθf）sinθl+（yl-yf-dsinθf）cosθl] （2）

式中，d為機器人中心到前側(cè)的距離。

2 ATB-RRT*算法

2.1 自適應(yīng)采樣方法

本文改進采樣函數(shù)為采樣密度與障礙物距離相關(guān)的自適應(yīng)采樣，更大概率在障礙物附近產(chǎn)生采樣點。采樣的概率密度函數(shù)p（x，x0，）為：

[p（x，x0，γ）=1πγ（x-x0）2+γ2]? ? ? ?（3）

式中x0為概率密度峰值位置的位置參數(shù)，γ為概率密度尺度函數(shù)。該函數(shù)在x0處達到峰值，左右兩側(cè)對稱，自適應(yīng)采樣概率密度函數(shù)如圖3所示。

令[l=x-x0]，其中x為采樣點，x0為距該采樣點最近的障礙物，則式（3）可表示為：

[p（l，γ）=1πγl2+γ2]? ? ? ? ? ? （4）

設(shè)置安全距離函數(shù)：

[safety（l）=1? ? ?（l>α）0? ? ?（l<α）]? ? ? ? ?（5）

式中α表示障礙物與采樣點之間的最小安全距離。當[l>α]時，[safety（l）=1]，采樣點安全;當[l<α]時，[safety（l）=0]，采樣點不安全。采樣概率分布函數(shù)為：

[P（l，γ）=safety（l） 1-20+∞p（l，γ） dl]? ?（6）

最小安全距離α與編隊中機器人數(shù)量呈正相關(guān)，單機器人時α值最小。

2.2 目標引力生長機制

生成采樣點之后，進入新節(jié)點生長環(huán)節(jié)。隨機樹從初始點開始，生長新節(jié)點Qnew加入樹中?；綬RT*算法新節(jié)點Qnew的計算公式為：

[Qnew=Qnearest+ρ（Qrand-Qnearest）Qrand-Qnearest]? ? ? ?（7）

式中Qrand為自適應(yīng)采樣的采樣點，Qnearest為隨機樹中距離Qrand最近的節(jié)點，ρ為隨機樹生長的步長。經(jīng)典RRT*算法中的新節(jié)點生長過程如圖4（a）所示。

本文根據(jù)人工勢場法引力思想，改進新節(jié)點生長過程。改進后隨機樹的生長在向采樣點方向生長的基礎(chǔ)上，增加目標方向的引力分量。加入目標引力的RRT*算法新節(jié)點Qnew的計算公式為：

[Qnew=Qnearest+][ρ（Qrand-Qnearest）Qrand-Qnearest+k（Qgoal-Qnearest）Qgoal-Qnearest]? ?（8）

式中k為引力系數(shù)。目標引力RRT*新節(jié)點生長過程如圖4（b）所示，Qinit、Qgoal為起始與目標節(jié)點。

加入目標引力生長機制后RRT*算法節(jié)點都具有目標導(dǎo)向性，搜索的隨機性降低。由于RRT*算法的目標導(dǎo)向性與避障能力呈負相關(guān)，路徑避障能力會隨之下降，通過調(diào)整引力系數(shù)k控制生長過程中引力分量的占比來解決這一問題，遇到障礙物時取[k<ρ]，未遇到障礙物時取[k>ρ]。

2.3 新節(jié)點刪除策略

減少隨機樹中低價值節(jié)點可以增快算法的計算速度。若新節(jié)點Qnew符合公式：

[Qnew-Qinit+Qgoal-Qnew>σbest]? ? （9）

即新節(jié)點到起始點與目標點距離之和大于當前最低路徑成本，因經(jīng)過該新節(jié)點的路徑一定大于等于與起始點與終點的直線距離之和，則刪除該節(jié)點，式中σbest為當前最低路徑代價。

2.4 基于ATB-RRT*算法的全局規(guī)劃

ATB-RRT*算法利用自適應(yīng)采樣替代隨機均勻采樣、引入目標引力生長機制引導(dǎo)新節(jié)點擴展，降低搜索的盲目性；擴展節(jié)點刪除策略，提高計算的速度。完整的ATB-RRT*算法流程如圖5所示。

以起始點和目標點分別構(gòu)建兩棵搜索樹，在環(huán)境狀態(tài)空間內(nèi)自適應(yīng)采樣獲得隨機節(jié)點Qrand，通過目標引力機制生長出新節(jié)點Qnew，利用新節(jié)點刪除策略判斷該新節(jié)點價值，若是則刪除該節(jié)點重新采樣，若不是則選取Qnew的鄰近節(jié)點Qnear，執(zhí)行重選最優(yōu)父節(jié)點與重布線環(huán)節(jié)，優(yōu)化隨機樹。判斷此次采樣是否達到迭代次數(shù)，達到則結(jié)束算法，未達到判斷當前隨機樹Qnew能否與另一棵樹最近節(jié)點Qother連接，若不能，則繼續(xù)采樣，若能，則比較新連接的路徑成本σnew是否小于當前最優(yōu)路徑成本σbest，若不小于，則交換隨機樹繼續(xù)采樣，若小于，則σnew的值賦予σbest后交換隨機樹繼續(xù)采樣。

3 編隊路徑規(guī)劃

3.1 編隊控制率設(shè)計

基于領(lǐng)航跟隨法設(shè)計多機器人編隊控制律，將編隊系統(tǒng)進行分為領(lǐng)航者與跟隨者兩層，選取一個機器人作為領(lǐng)航者負責整個編隊的路徑規(guī)劃和引導(dǎo)跟隨者，跟隨者以[l-φ]（距離-角度）的方式對領(lǐng)航者進行跟隨，參考坐標為編隊領(lǐng)航者。

結(jié)合機器人系統(tǒng)模型，并對式（2） 求導(dǎo)，可得：

[lxlf=lylfwl-vfcosδlf+dwfsinδlf+vllylf=-lxlfwl-vfsinδlf-dwfcosδlf]? ?（10）

[δlf=θf-θl]? ? ? ? ? ? ? （11）

式中，vl與wl分別是領(lǐng)航機器人的線速度與角速度，vf與wf分別是跟隨機器人的線速度與角速度。

多機器人編隊在領(lǐng)航機器人坐標系下實際相對距離llf與理想相對距離llf*的誤差為elf，誤差公式為：

[elf=exlfeylf=lx*lf-lxlfly*lf-lylf]? ? ? ? ? （12）

[lx*lf=l*lfcosφ*lf]? ? ? ? ? ? （13）

[ly*lf=l*lfsinφ*lf]? ? ? ? ? ? （14）

結(jié)合式（10） ，并對式（12） 求導(dǎo)，可得：

[exlf=l*lfcosφ*lf-l*lfφ*lfsinφ*lf-（l*lfsinφ*lf-eylf）wl? ? ? ?+vfcosδlf-dwfsinδlf-vleylf=l*lfsinφ*lf+l*lfφ*lfcosφ*lf+（l*lfcosφ*lf-exlf）wl? ? ? ?+vfsinδlf+dwfcosδlf] （15）

設(shè)置領(lǐng)航者與跟隨者之間的理想相對距離llf*與理想相對角度φlf*均為一個定值，故把[l*lf=φ*lf=0]代入式（15） ，可得：

[exlf=wleylf+vfcosδlf-dwfsinδlf-l*lfwlsinφ*lf-vleylf=-wlexlf+vfsinδlf+dwfcosδlf+l*lfwlcosφ*lf] （16）

設(shè)計控制律：

[elf=-klfelf]? ? ? ? ? ? ?（17）

式中，klf反饋比例系數(shù)，公式為：

[klf=k1? 00? ?k2]? ? ? ? ? ? （18）

式中，[k1>0]，[k2>0]。

通過式（17） 的控制律，可以使式（16） 的誤差值收斂為0，達到領(lǐng)航跟隨型多機器人的編隊控制。最終跟隨機器人[Rf]實現(xiàn)跟隨領(lǐng)航者機器人的線速度[vf]與角速度[ωf]分別為：

[vf=-k1（l*lfcosφ*lf-lxlf）+lylfwl+vlcosδlf+-k2（l*lfsinφ*lf-lylf）-lxlfwlsinδlf] （19）

[ωf=-k2（l*lfsinφ*lf-lylf）-lxlfwlcosδlfd- -k1（l*lfcosφ*lf-lxlf）+lylfwl+vlsinδlfd]? （20）

3.2 ATB-RRT*路徑節(jié)點DWA

領(lǐng)航機器人承擔為編隊尋找一條起始點到目標點之間無碰撞路徑的任務(wù)。傳統(tǒng)DWA算法規(guī)劃路徑由于缺少全局信息，易陷入局部最優(yōu)。本文利用ATB-RRT*規(guī)劃的全局路徑節(jié)點引導(dǎo)DWA算法，以解決復(fù)雜環(huán)境下的路徑規(guī)劃問題。

將DWA的目標點由終點變?yōu)锳TB-RRT*路徑的節(jié)點，當機器人運動到當前目標路徑節(jié)點的一定范圍內(nèi)時，目標點變?yōu)殡S機樹中該路徑節(jié)點的子節(jié)點。此時DWA的評價函數(shù)為：

[G（v，m）=μη?headingtree（v，w）+β?distance（v，w）+??velocity（v，w）] （21）

式中，headingtree（v，w）表示模擬軌跡的末端與當前目標點間的方位角度差。若當前子目標點被障礙物遮擋，可把孫節(jié)點作為下一目標點。編隊領(lǐng)航者路徑規(guī)劃的過程如圖6所示。

圖6（a）中，q1、q2、q3、q4、q5為路徑節(jié)點樹的節(jié)點，q1為根節(jié)點。圖6（b）中橙色三角形Rl為領(lǐng)航者，q2為DWA算法的目標節(jié)點。圖6（c）中當機器人運行至q2節(jié)點的一定范圍內(nèi)，DWA算法的目標點變?yōu)閝2的子節(jié)點q3，以此方式進行運動，直至到達最終節(jié)點q5的路徑軌跡為圖6（d）中藍色軌跡。

3.3 改進DWA算法

傳統(tǒng)DWA算法沒有考慮多機器人編隊運行時隊形保持的問題，在編隊跟隨者遇到動態(tài)障礙物進行避障動作后，難以保持編隊隊形。本文在DWA的評價函數(shù)中增加路徑適應(yīng)子函數(shù)fit（v，w），以理想編隊路徑來評價軌跡的適應(yīng)程度，改進評價函數(shù)為：

[G（v，m）=μη?headingtree（v，w）+β?distance（v，w）+??velocity（v，w）+κ?fit（v，w）] （22）

式中，[κ]為評價子函數(shù)fit（v，w）的加權(quán)系數(shù)，fit（v，w）與理想路徑完全重合時數(shù)值最高。

3.4 基于ATB-RRT*與改進DWA算法流程

step1： 根據(jù)環(huán)境信息，獲得環(huán)境內(nèi)障礙物及機器人位置信息，并建立地圖；

step2： 初始化ATB-RRT*算法，為編隊領(lǐng)航者規(guī)劃全局地圖；

step3： 利用編隊控制律，控制跟隨者的位姿與速度，形成多機器人編隊；

step4： 領(lǐng)航者根據(jù)ATB-RRT*路徑節(jié)點DWA行進；

step5： 如遇到動態(tài)障礙，利用改進DWA算法進行避障，避障后回歸編隊路徑，實現(xiàn)編隊保持。

4 實驗仿真

4.1 ATB-RRT*算法

在環(huán)境內(nèi)路起始點坐標為（0，0），目標點坐標為（25，25），設(shè)置多個圓形障礙物。仿真參數(shù)設(shè)置采樣尺度參數(shù)[γ=1]，最小安全距離[α=0.6]，步長[ρ=3.5]，最大迭代次數(shù)為500。

在上述環(huán)境中對雙向RRT*與本文所提出的ATB-RRT*算法進行對比實驗，驗證改進算法的有效性。全局路徑規(guī)劃如圖7所示。

在同一環(huán)境地圖內(nèi)重復(fù)進行50次實驗，并記錄每次實驗的數(shù)據(jù)，所有數(shù)據(jù)平均處理后如表1所示。

由圖7與表1分析得到雙向RRT*與ATB-RRT*都可以找到路徑代價較低的無障礙路徑。ATB-RRT*算法通過自適應(yīng)采樣與目標引力生長機制，獲得的采樣點更貼近障礙物，每一段路徑生長的過程中都含有目標引力，生成路徑的代價更小。由于引入了新節(jié)點刪除策略，算法的運行時間大幅度降低。相比于雙向RRT*算法，本文提出的ATB-RRT*算法路徑代價減少了6.0%，采樣節(jié)點數(shù)減少了32.9%，運行時間降低了42.1%。

4.2 編隊路徑規(guī)劃

在復(fù)雜障礙環(huán)境驗證領(lǐng)航者機器人利用ATB-RRT*路徑節(jié)點的DWA算法規(guī)劃的有效性。起始點坐標為（0，0），目標點坐標為（25，25），機器人的最大線速度為1m/s，最大角速度為20°/s，線加速度為0.2m/s，角加速度為60°/s2，速度分辨率為0.04m/s，角速度分辨率為1°/s，時間分辨率為0.15s，軌跡預(yù)測時間為3s，障礙物安全閾值為0.9m。

領(lǐng)航者機器人利用傳統(tǒng)DWA與ATB-RRT*路徑節(jié)點DWA的路徑規(guī)劃軌跡如圖8所示。

圖8（a）中，機器人選擇從障礙物下方繞過復(fù)雜障礙區(qū)域。圖8（b）中，機器人在地圖中依次以ATB-RRT*路徑隨機樹中的節(jié)點作為DWA的目標點，被全局路徑節(jié)點引導(dǎo)從復(fù)雜障礙區(qū)域穿過。

在同一環(huán)境下兩種DWA算法的路徑代價及運行時間數(shù)據(jù)對比如表2所示。

由圖8與表2分析得到，ATB-RRT*路徑節(jié)點DWA算法利用全局路徑信息，使DWA的規(guī)劃過程不易被環(huán)境中的障礙物稠密區(qū)域干擾，提高機器人穿越較多障礙物區(qū)域的能力，降低規(guī)劃路徑的代價與運行時間。相比傳統(tǒng)DWA算法，本文的ATB-RRT*路徑節(jié)點DWA路徑代價改進了11.6%，運行時間改進了15.5%。

在無障礙地圖中設(shè)置圓形障礙，驗證改進DWA算法，仿真結(jié)果如圖9所示。由圖9（a）可知，機器人在進行避障后可以到達目標點，但避障后的軌跡偏離無障礙時的理想軌跡較遠。圖9（b）為改進DWA的軌跡，機器人避障后可以回到無障礙時的理想軌跡上繼續(xù)向目標點前進。

以由3個機器人形成的編隊仿真如圖10所示。領(lǐng)航機器人初始航向角為45°，編隊機器人相對距離為2m，相對角度為60°。圖10中，跟隨者機器人與領(lǐng)航者機器人快速形成編隊，跟隨領(lǐng)航者規(guī)劃的無路徑前進，在遇到動態(tài)障礙時，利用改進DWA避障后回歸編隊理想路徑，實現(xiàn)了編隊保持。

5 結(jié)論

多機器人編隊路徑規(guī)劃是機器人領(lǐng)域的熱點問題之一，本文提出了一種ATB-RRT*與改進DWA的多機器人編隊路徑規(guī)劃算法，通過改變采樣方法與路徑生長方式降低路徑代價與提升計算速度，刪除低價值節(jié)點減少資源占用。利用距離角度領(lǐng)航跟隨法形成機器人編隊，并結(jié)合理想編隊路徑改進DWA，實現(xiàn)避障后的編隊保持。仿真結(jié)果證明了本文提出的多機器人編隊路徑規(guī)劃算法的有效性。

參考文獻：

[1] 常路，單梁，戴躍偉，等.未知環(huán)境下基于改進DWA的多機器人編隊控制[J].控制與決策，2022，37（10）：2524-2534.

[2] Zhang T R，Xu J N，Wu B K.Hybrid path planning model for multiple robots considering obstacle avoidance[J].IEEE Access，2022（10）：71914-71935.

[3] Boo J，Chwa D.Fuzzy integral sliding mode observer-based formation control of mobile robots with kinematic disturbance and unknown leader and follower velocities[J].IEEE Access，2022，10：76926-76938.

[4] 孟蕾.基于距離約束的單領(lǐng)航者多智能體編隊控制[J].電光與控制，2021，28（7）：48-52.

[5] 王樂樂，眭澤智，蒲志強，等.一種改進RRT的多機器人編隊路徑規(guī)劃算法[J].電子學(xué)報，2020，48（11）：2138-2145.

[6] Karaman S，F(xiàn)razzoli E.Sampling-based algorithms for optimal motion planning[J].The International Journal of Robotics Research，2011，30（7）：846-894.

[7] Jiankun，Wang.Kinematic Constrained Bi-directional RRT with Efficient Branch Pruning for robot path planning[J].Expert Systems With Applications，2021（170）：114541.

[8] Mashayekhi R，Idris M Y I，Anisi M H，et al.Informed RRT*-connect：an asymptotically optimal single-query path planning method[J].IEEE Access，2020（8）：19842-19852.

[9] 遲旭，李花，費繼友.基于改進A*算法與動態(tài)窗口法融合的機器人隨機避障方法研究[J].儀器儀表學(xué)報，2021，42（3）：132-140.

【通聯(lián)編輯：李雅琪】