障碍环境下队形可变编队的路径规划方法研究

王 祝，张振鹏，张原宁，焦嵩鸣

（华北电力大学（保定）自动化系，保定 071003）

1 引 言

移动机器人具有机动性强、负载能力高等优点，当前已被应用到探索[1]、侦察[2]、搜救[3]等领域。由于真实环境的复杂性和机器人性能的局限性，仅靠单机器人难以高效完成复杂任务，因而需要机器人编队通过相互配合完成复杂任务[4]。为使机器人编队能快速到达目标点执行任务，需要规划出一条避免碰撞且使特定性能指标最优的可行路径[5-6]。

针对多机器人编队路径规划问题，近年来研究者已经开展了蚁群算法[7]、遗传算法[8]、粒子群算法[9]等研究。文献[10]针对领航跟随编队提出了一种改进的蚁群算法，为领导机器人搜索出一条最短路径，实现了编队避障。文献[11]针对传统遗传算法路径规划时存在的收敛速度慢、路径不平滑问题，在适应度函数中加入了路径平滑度因素，为机器人编队规划出一条安全无碰撞且平滑度较好的最优路径。文献[12]提出了一种基于改进快速搜索随机树的多机器人编队路径规划算法，实现了机器人间的编队队形保持，同时在规划中实时调整队形方向，实现了编队整体避障。文献[13]使用人工势场法进行多个全向机器人路径规划，实现在编队不变的情况下多机器人编队能够躲避障碍物和边界到达目标点。然而，上述研究均假设编队队形保持不变，未能利用队形可变的特点，在障碍比较复杂的情况下，队形保持可能导致不存在可行的编队路径，同时队形保持可能需要绕行较远的路径，以实现避撞，导致编队抵达任务点的时间增加。

在考虑队形变换的编队协同研究方面，文献[14]提出了一种基于Dubins 和粒子群优化的路径规划方法，根据飞行器运动学模型对Dubins 轨迹进行降维处理，采用粒子群优化求解编队重构路径规划。该方法需要在初始队形和目标队形已知情况下，对编队队形实现重构。文献[15]针对协同航迹规划中因队形约束而忽略较窄通道的问题，提出了一种基于自适应分布式模型预测控制的快速粒子群优化方法，在编队控制中根据前方环境自适应进行编队变换。该方法是一种局部的反应式控制方法，并非全局规划方法。文献[16]提出一种改进的人工势场法，引入实际速度、加速度改进势场力表示方法，实现了编队穿越狭窄区域的队形变换。该方法也是在编队控制阶段进行反应式的队形切换，并未在全局路径规划阶段预先考虑队形变化对可行路径的影响。

针对以上问题，本文提出一种考虑队形变化的编队路径规划方法，通过障碍二次膨胀构建规划地图，并引入穿越软障碍路径代价新指标，构建包含路径长度和软障碍违背度的统一化代价函数，然后利用改进A*搜索算法生成一条无碰撞且长度较小的编队参考路径。编队路径规划过程中能够自动调整队形来适应障碍环境，以满足多指标的规划需求。

2 问题建模

本节首先给出了本文所研究的编队路径规划问题描述、机器人编队队形模型，然后构建了编队路径规划中需考虑的障碍约束条件、路径长度指标和队形保持指标。

2.1 编队参考路径规划问题

本文研究的编队参考路径规划目标是在尽可能保证队形不变的前提下，找到一条从起点到目标点的安全路径，作为编队的参考路径，引导机器人编队通过编队控制的方式抵达目标点，完成给定任务。

编队参考路径表征编队几何中心点的变化路径。根据规划得到的中心点参考路径，机器人编队可以采取合适的编队控制方法，例如领航跟随法[17]、虚拟结构法[18]等，跟踪参考路径从起点安全到达目标点。

图1给出了机器人编队路径规划的示意图，其中机器人的期望任务队形为三角编队。若保持三角队形运动，需对下方的两个障碍进行绕行，以避免碰撞，如图1实线路径所示；若允许编队变换，则可以将三角队形变换为纵向队形，从下方的两个障碍间通过，如图1虚线路径所示。

图1 机器人编队路径规划示意图Fig.1 Diagram of robot formation path planning

2.2 机器人编队队形模型

在编队参考路径规划时，需要根据编队的规模和队形计算出编队的长度和宽度用于障碍碰撞检测。然而不同编队不同规模其编队的长度和宽度均不同，例如对于图2给出的机器人编队典型队形：横向编队、三角编队、纵向编队和菱形编队。

图2 机器人典型编队示意图Fig.2 Diagram of typical robot formations

为方便计算机器人编队尺寸，将编队中机器人沿横向和纵向分别平移映射到一条中心线上，得到编队机器人横向数量NL和纵向数量NW，然后根据式(1)可计算得到编队横向和纵向尺寸。

式中，LF、WF为编队横向和纵向长度，LR、WR为机器人长宽，Lspace和Wspace为编队个体间的长度和宽度。

2.3 障碍规避约束

机器人路径规划需考虑对环境中的障碍进行规避，以保证机器人安全。为了便于路径搜索和地图信息存储，采用栅格地图进行环境建模，如图3所示。由于机器人向前不会与障碍物碰撞，只需考虑机器人两侧与障碍物碰撞，故选取机器人宽度作为障碍物膨胀尺寸，防止机器人两侧撞击到障碍物，建图时以机器人的宽度尺寸对实际障碍进行膨胀，从而构建机器人路径规划的构型空间，在此空间下可以将机器人看作质点进行路径规划。

图3中，黑色区域为实际障碍的占据空间，灰色区域为障碍膨胀后的占据区域。将机器人当作质点进行路径规划时，黑色区域和灰色区域均为不可通行区，白色区域记为可通行区。为保证机器人的运行安全，路径规划时需进行障碍检测，以保证规划的路径不会穿过不可通行区。

图3 机器人编队路径规划的环境栅格图Fig.3 Grid map of environment for robot formation path planning

2.4 路径规划指标

为了使编队在障碍环境下仍具有较高的通过性，考虑队形可变条件下的编队路径规划。考虑到编队任务需求，编队运动过程中应尽可能地提高以任务队形运动的路径长度，同时考虑到尽可能快地抵达目标点，应减小编队的路径总长度。对此，可以建立下述多目标规划指标：

路径长度指标J1：编队路径由一系列离散的路径点构成，记为Path={Pi,i=1,2,...,N}，其中Pi为表示第i个路径点，N为路径点的总数量，则路径长度指标可表示为

其中，Li为第i段路径的长度，即线段PiPi+1的长度|PiPi+1|。

队形保持指标J2：编队运动过程中，应尽可能保持任务队形，即尽可能增大采用期望队形运动的路径长度比例，或者尽可能减小采用纵向队形实现障碍规避的路径长度比例，因此，队形保持指标可表示为

式中，L为路径总长度，LC表示以纵向队形运动的路径长度。

3 队形可变编队的路径规划算法

本节介绍基于 A*框架的队形可变编队路径规划算法（记为Variable-Shape Formation-A*，VSF-A*），包括算法总体流程、构型空间生成和统一化代价函数构建。

3.1 算法总体流程

A*搜索算法作为一种应用广泛的启发式路径规划方法，具有效率高和适应性强等优点，而且在满足可接纳条件下能够保证结果的最优性。在A*的框架下，通过两次膨胀建立包含硬障碍区和软障碍区的构型空间，并构建统一化代价函数，以获得多指标权衡的路径结果。

VSF-A*算法流程如图4所示，节点选择和扩展搜索流程与A*算法相似，具体流程不再赘述，下文主要对改进的构型空间构建方法和节点代价计算方法进行阐述。

图4 VSF-A*算法流程图Fig.4 Flowchart of VSF-A* algorithm

3.2 路径规划构型空间生成

如2.3 节所述，基于真实环境地图直接进行机器人路径规划时，需要考虑机器人的尺寸进行碰撞检测，会降低规划效率。一般地，在环境地图中根据机器人尺寸对障碍区进行膨胀获得新的环境地图，在该环境下可以将机器人直接作为质点进行路径规划，也称为构型空间。

针对队形可变的编队路径规划问题，本文提出通过两次膨胀生成构型空间的方法，如图5所示。图中黑色为实际障碍区域，灰色区域为根据个体尺寸进行膨胀得到的区域（称为硬障碍区），斜线栅格部分为根据机器人编队占据的空间尺寸膨胀得到的区域（称为软障碍区）。硬障碍区表示编队按纵向队形运动也无法通行的区域，软障碍区则为编队按任务队形运动时无法通行的区域，但采用纵向队形是可以通过的。硬障碍膨胀尺寸

图5 考虑障碍二次膨胀的栅格地图Fig.5 Grid map with twice obstacles inflation

如式(5)所示，软障碍膨胀尺寸如式(6)所示，其中，Lobstrue、Wobstrue为实际障碍长宽，Lobshard、Wobshard为硬障碍膨胀长宽，Lobssoft、Wobssoft为软障碍长宽。

3.3 统一代价函数构建

为了实现队形可变编队的路径规划，综合考虑路径长度最小化和编队保持的需求，构建统一化的代价函数，如式(7)所示。

其中，n代表当前搜索过程中的节点，f(n)为节点代价值；g(n)为累计代价，表示从起点到当前点的路径长度；h(n)为估计代价，表示从当前点到目标点的估计路径长度；e(n)为穿越软障碍代价，表示路径经过软障碍区Osoft的长度；ω1表示路径总长度的权重系数；ω2表示经过软障碍区的路径长度权重系数。h(n)、g(n)和e(n)均使用欧式距离估计，如式（8）～（10）所示，其中{xend,yend}表示目标点坐标，nf表示节点n的父节点，{x,y}和{xf,yf}分别为当前节点和父节点坐标。

4 仿真试验和结果分析

为了验证本文方法的有效性和体现方法的优势，设定两组场景进行不同指标权重下的对比试验，并开展随机障碍环境下的鲁棒性测试，大规模编队不同任务队形下的鲁棒性。

4.1 试验1：队形保持无可行路径的场景1

场景1 如图5所示，机器人运动范围为20 m ×10 m，栅格地图规模为 200 × 100，即分辨率0.1 m，机器人尺寸为0.4 m，起点位于（6，60），终点位于（198，40）。环境中共有5 个障碍，大小分别为1 m × 3 m，1 m × 2 m，1.5 m × 1.5 m，0.5 m × 1.5 m，0.2 m × 0.8 m。任务编队为3 个机器人组成的三角编队，机器人间隔0.2 m，避障是可选择纵向队形。图5中，黑色区域表示根据单个机器人尺寸（0.4 m）膨胀得到的硬障碍区，灰色区域表示根据三角编队尺寸（1.6 m）膨胀得到的软障碍区。

针对场景 1，若采用传统的不允许队形改变的方法，将无法生成可行路径，而采用VSF-A*算法，可以获得经过软障碍但不经过硬障碍的路径结果。图6给出了权重ω2取不同值时的路径规划结果，具体的指标结果如表1所示。由结果可知，当权重ω2较小时，路径通过软障碍区的长度较大，而随着权重ω2的增加，路径经过软障碍区的长度减少，规划结果更倾向于规避软障碍区。但由于该场景下不存在绕开所有软障碍区的可行路径，因而权重很大时，规划路径依然会经过软障碍区。

图6 场景1 不同权重编队路径规划结果Fig.6 Planned formation path for scenario 1

表1 场景1 路径规划结果Table 1 Path planning results for scenario 1

针对场景1，图7在若干离散路径点处给出了机器人编队在环境中的运动情况，其中三角队形的方格表示机器人的实际尺寸。需要说明的是，考虑机器人尺寸的结果图中，机器人方格与真实障碍（黑色区域）无交集，即表明机器人不会与障碍发生碰撞。当规划路径在空白可通行区时，编队可以采用三角任务队形运动，而当规划路径穿越软障碍区时，编队只能以纵向队形运动，否则机器人将与障碍发生碰撞。通过上述仿真结果可知，编队通过改变队形具备了更好的狭小区域通行能力。因此，提出的VSF-A*算法能够提高编队路径规划方法在复杂环境下的适用能力。

图7 针对场景1 规划的编队路径Fig.7 Planned formation path for scenario 1

4.2 试验2：队形保持有可行路径的场景2

场景2 如图8所示，其中栅格地图尺寸、起点、终点和场景1 相同，5 个障碍的大小不变，但位置重新生成，使得从起点到目标点存在不通过软障碍区域的可行路径。

表2和图8给出了VSF-A*取不同权重ω2下的编队路径规划结果。根据结果可知，随着权重ω2的增加，规划路径更倾向于绕行灰色软障碍区，编队经过软障碍区的路径长度减小。当权重ω2足够大时，针对场景二的路径规划结果将完全不经过软障碍区，例如ω2取100 时的结果。另一方面，随着权重ω2的增大，路径的总长度呈增长的趋势，即为了满足队形保持的要求，编队绕行灰色软障碍区导致从起点抵达目标点的所需时间变长。

颅内动脉瘤开颅夹闭术后发生硬膜下并发症较为常见，发生率为1%～5.1%，且男性、年龄、颅内动脉瘤部位、脑萎缩及抗血小板药物的使用是动脉瘤开颅夹闭术后硬膜下积液或血肿发生的危险因素[5-9]。本研究结果表明，男性、高龄、动脉瘤部位(大脑中动脉瘤、多发动脉瘤)是动脉瘤开颅夹闭术后硬膜下积液发生的独立危险因素(P<0.05)；硬膜下积液演变为慢性硬膜下血肿则与男性、高龄相关(P<0.05)。本研究结果还表明，硬膜下积液量越大，其演变为慢性硬膜下血肿的比例越高，演变为慢性硬膜下血肿后须行钻孔引流手术治疗的比例也越高。

图8 场景2 不同权重编队路径规划结果Fig.8 Planned formation path for scenario 2

表2 场景2 路径规划结果Table 2 Plath planning results for scenario 2

机器人编队在典型路径点处的位置分布情况如图9所示。由图可知，编队沿权重ω2为0 时的规划路径运动时，因为穿越软障碍区，需要通过队形变换才能避免碰撞的安全通过。而沿权重足够大，即ω2为100 的规划路径运动时，由于规划路径没有经过软障碍区，因此编队可以始终保持期望的三角队形抵达目标点。

图9 针对场景2 规划的编队路径Fig.9 Planned formation path for scenario 2

4.3 试验3：100 次随机场景测试

随机测试实验的100个场景中，栅格地图尺寸、起点、终点、障碍大小均相同，但障碍位置每次随机产生，且使得硬障碍区不包含起点和终点。

针对每个随机产生的场景，利用VSF-A*算法进行路径规划，并设置不同的软障碍路径长度权重进行对比。规划结果的路径总长度、经过软障碍区长度、经过软障碍区长度占总长度比例的统计结果分别如图10～12 所示。

图10 100 次随机场景下路径总长度对比Fig.10 Comparison of total path length for 100 random scenes

图11 100 次随机场景下软障碍穿越长度对比Fig.11 Length comparison of path passing through soft obstacles for 100 random scenes

图12 100 次随机场景下LC/L 值对比Fig.12 Comparison of LC/L for 100 random scenes

对100 次随机测试的结果进行统计，权重ω2为0、5、10、100 时，路径长度均值为20.34 m、20.35 m、20.4 m、21.46 m，穿越软障碍长度均值为10.38 m、8.48 m、7.86 m、3.7 m，穿越软障碍长度占路径总长度的比例均值为 51.03%、41.67%、38.53%、17.24%。随着权重ω2的增加，编队经过软障碍区的路径长度减小，而路径总长度由于绕行软障碍区而增加。针对100 次随机生成的场景，VSF-A*算法都可以快速规划得到编队的可行路径，验证了该方法具有良好的鲁棒性，能够应用于复杂障碍环境下的编队路径规划。

4.4 试验4：大规模编队测试

场景3 如图13所示，机器人运动范围为200 m × 100 m，栅格地图规模为2000×1000，即分辨率0.1 m，机器人尺寸为0.4 m。环境中共有10个障碍，大小分别为1 m × 2 m，1 m × 3 m，4 m ×3 m，5 m × 2 m，5 m × 5 m，5 m × 6 m，6 m × 7 m，7 m × 9 m，10 m × 30 m，20 m × 20 m。编队规模由原先的3 个提升为12 个。

编队首先以三角编队从起点（1698，140）运动到终点1（460，800），再更换成菱形编队从终点1 运动到终点2（1780，800），路径规划时可选择纵向队形通过狭窄区域。其中，硬障碍膨胀大小为机器人宽度（0.4 m），三角编队软障碍膨胀大小为三角编队宽度（7 m），菱形编队软障碍膨胀大小为菱形编队宽度（4 m）。

针对上述需求的编队路径规划结果如图13和图14所示。结果表明，VSF-A*算法针对规模较大的编队协同问题，VSF-A*算法同样可以规划得到编队的可行路径，表明该方法具有良好的适应性。

图13 场景3 中的三角编队路径Fig.13 Planned triangle formation path for scenario 3

图14 场景3 中的菱形编队路径Fig.14 Planned diamond formation path for scenario 3

5 结 论

通过两次障碍碰撞构建包含硬障碍区和软障碍区的移动机器人编队路径规划构型空间，并设计代价函数的计算方法，提出一种适用于队形可变编队路径规划的VSF-A*算法。仿真测试结果表明，VSF-A*算法能够利用队形可变的特点获得复杂障碍环境下的编队可行路径，实际应用中可以根据编队对队形保持和任务时间的偏好设定合适的权重，从而生成多指标权衡的可行编队路径。