姜君策 伍錫如

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

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

中圖分類號(hào)：TP273? ? ? 文獻(xiàn)標(biāo)識(shí)碼：A

文章編號(hào)：1009-3044（2023）13-0005-05

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

0 引言

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

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

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

1 機(jī)器人運(yùn)動(dòng)學(xué)模型

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

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

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

[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為機(jī)器人中心到前側(cè)的距離。

2 ATB-RRT*算法

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

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

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

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

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

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

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

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

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

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

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

2.2 目標(biāo)引力生長(zhǎng)機(jī)制

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

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

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

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

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

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

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

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

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

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

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

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

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

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

3 編隊(duì)路徑規(guī)劃

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

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

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

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

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

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

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

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

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

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

結(jié)合式（10） ，并對(duì)式（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)航者與跟隨者之間的理想相對(duì)距離llf*與理想相對(duì)角度φlf*均為一個(gè)定值，故把[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è)計(jì)控制律：

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

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

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

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

通過式（17） 的控制律，可以使式（16） 的誤差值收斂為0，達(dá)到領(lǐng)航跟隨型多機(jī)器人的編隊(duì)控制。最終跟隨機(jī)器人[Rf]實(shí)現(xiàn)跟隨領(lǐng)航者機(jī)器人的線速度[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é)點(diǎn)DWA

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

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

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

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

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

3.3 改進(jìn)DWA算法

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

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

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

3.4 基于ATB-RRT*與改進(jìn)DWA算法流程

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

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

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

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

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

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

4.1 ATB-RRT*算法

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

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

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

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

4.2 編隊(duì)路徑規(guī)劃

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

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

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

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

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

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

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

5 結(jié)論

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

參考文獻(xiàn)：

[1] 常路，單梁，戴躍偉，等.未知環(huán)境下基于改進(jìn)DWA的多機(jī)器人編隊(duì)控制[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)航者多智能體編隊(duì)控制[J].電光與控制，2021，28（7）：48-52.

[5] 王樂樂，眭澤智，蒲志強(qiáng)，等.一種改進(jìn)RRT的多機(jī)器人編隊(duì)路徑規(guī)劃算法[J].電子學(xué)報(bào)，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] 遲旭，李花，費(fèi)繼友.基于改進(jìn)A*算法與動(dòng)態(tài)窗口法融合的機(jī)器人隨機(jī)避障方法研究[J].儀器儀表學(xué)報(bào)，2021，42（3）：132-140.

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