基于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.

【通联编辑：李雅琪】