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

姜君策 伍錫如

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

關鍵詞：多機器人編隊；路徑規劃； ATB-RRT*算法；領航跟隨；動態窗口法

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

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

開放科學（資源服務）標識碼（OSID） ：

0 引言

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

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

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

1 機器人運動學模型

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

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

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

[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為機器人中心到前側的距離。

2 ATB-RRT*算法

2.1 自適應采樣方法

本文改進采樣函數為采樣密度與障礙物距離相關的自適應采樣，更大概率在障礙物附近產生采樣點。采樣的概率密度函數p（x，x0，）為：

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

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

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

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

設置安全距離函數：

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

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

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

最小安全距離α與編隊中機器人數量呈正相關，單機器人時α值最小。

2.2 目標引力生長機制

生成采樣點之后，進入新節點生長環節。隨機樹從初始點開始，生長新節點Qnew加入樹中。基本RRT*算法新節點Qnew的計算公式為：

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

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

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

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

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

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

2.3 新節點刪除策略

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

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

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

2.4 基于ATB-RRT*算法的全局規劃

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

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

3 編隊路徑規劃

3.1 編隊控制率設計

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

結合機器人系統模型，并對式（2） 求導，可得：

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

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

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

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

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

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

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

結合式（10） ，并對式（12） 求導，可得：

[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）

設置領航者與跟隨者之間的理想相對距離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）

設計控制律：

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

式中，klf反饋比例系數，公式為：

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

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

通過式（17） 的控制律，可以使式（16） 的誤差值收斂為0，達到領航跟隨型多機器人的編隊控制。最終跟隨機器人[Rf]實現跟隨領航者機器人的線速度[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*路徑節點DWA

領航機器人承擔為編隊尋找一條起始點到目標點之間無碰撞路徑的任務。傳統DWA算法規劃路徑由于缺少全局信息，易陷入局部最優。本文利用ATB-RRT*規劃的全局路徑節點引導DWA算法，以解決復雜環境下的路徑規劃問題。

將DWA的目標點由終點變為ATB-RRT*路徑的節點，當機器人運動到當前目標路徑節點的一定范圍內時，目標點變為隨機樹中該路徑節點的子節點。此時DWA的評價函數為：

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

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

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

3.3 改進DWA算法

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

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

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

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

step1： 根據環境信息，獲得環境內障礙物及機器人位置信息，并建立地圖；

step2： 初始化ATB-RRT*算法，為編隊領航者規劃全局地圖；

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

step4： 領航者根據ATB-RRT*路徑節點DWA行進；

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

4 實驗仿真

4.1 ATB-RRT*算法

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

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

在同一環境地圖內重復進行50次實驗，并記錄每次實驗的數據，所有數據平均處理后如表1所示。

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

4.2 編隊路徑規劃

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

領航者機器人利用傳統DWA與ATB-RRT*路徑節點DWA的路徑規劃軌跡如圖8所示。

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

在同一環境下兩種DWA算法的路徑代價及運行時間數據對比如表2所示。

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

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

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

5 結論

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

參考文獻：

[1] 常路，單梁，戴躍偉，等.未知環境下基于改進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] 孟蕾.基于距離約束的單領航者多智能體編隊控制[J].電光與控制，2021，28（7）：48-52.

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

[6] Karaman S，Frazzoli 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*算法與動態窗口法融合的機器人隨機避障方法研究[J].儀器儀表學報，2021，42（3）：132-140.

【通聯編輯：李雅琪】