面向群组机器人路径规划的Voronoi-APF算法研究

蔡鑫伟，侯向辉，莫清宇，张美玉，简琤峰

(浙江工业大学 计算机学院 数字媒体技术研究所，杭州 310023) E-mail：hxh@zjut.edu.cn

1 引 言

群组机器人是受启发于群居动物的多机器人系统，该机器人群体由大量能力有限的机器人组成.相对于单机器人，其在成本、灵活性、稳定性上具有巨大优势，并具有聚集、空间覆盖、编队移动、自组装等单机器人所不具备的群体工作能力.在相关的论文中，群组机器人已被运用到移民保护[1]、火灾救援[2,3]、水下工作[4]，除此以外，群组机器人在其他复杂环境下也正发挥着其独特作用.

自组装作为群组机器人的重要应用方向，有着重要的研究意义.为了达成目标配置，路径规划尤为关键.而在复杂的实际应用环境中，保证机器人之间的碰撞避免和躲避地图障碍物问题则越来越受重视.近几年，越来越多的研究工作将人工势场[5]，粒子群优化算法[6]，蚁群算法[7]，遗传算法[8]等算法优化、结合并运用到该领域中，取得了一定成果.Wang W等人利用线性递减惯性权重粒子群算法进行路径规划，同时使用删除冗余结点方法和混沌理论进行优化，使得算法在复杂环境下依旧能有优秀的路径规划表现[9]；Yang J等人提出基于约束粒子群优化的协同搜索算法以应对多机器人在受限环境下的协同搜索任务[10]；Hu L等人将蚁群算法的评价函数引入到D*算法中，并改进了D*算法中的子节点扩展方式和启发函数，提出了一种高效率、高适应性的融合算法[11]；Nazarahari M等人提出以创新人工势场搜索所有可行路径，然后使用增强遗传算法找到最优路径，在多机器人在连续环境中的应用取得显著效果[12]；Pan Z等人提出了一种基于改进人工势场和PID自适应跟踪控制算法的避障方法，该算法利用旋转势场来解决局部极小值和振荡，并且建立了机器人之间的排斥势函数和优先级模型，成功地解决了避障和防撞问题[13]；Matoui F等人采用了人工势场、邻里系统、机器人之间的优先级概念3种技术的混合方法，使得分布式体系中的机器人能有效地躲避静态和动态的障碍物[14].

本文提出的基于Voronoi约束[15]的人工势场算法(VAPF)，首先使用匈牙利算法[16,17]和替换策略为各机器人指定目标点，然后通过群组机器人的实时位置构建Voronoi图，各机器人各自使用人工势场算法进行路径规划，限制其路径在构建的Voronoi图的对应分割空间中，即机器人单次运动到终点或分隔空间的边界便停止.重复以上步骤，直到所有机器人到达指定终点.VAPF算法既保留人工势场算法的优势，又实现了机器人间的碰撞避免，通过matlab仿真对比实验证明了其在时间消耗和路径长度上的改进效果.

2 相关工作

2017年，Sun J等人提出了APF的改进算法(为方便表述，将其称之为OAPF)，该算法在人工势场算法中引入了距离因子，并将自身以外的个体视作障碍物，既解决了人工势场算法容易陷入局部极小值、无法达到终点的问题，又解决了人工势场无法实现碰撞避免的问题[18].

人工势场算法在二维空间下的算法模型如式(1)-式(3)所示:

(1)

(2)

(3)

OAPF算法改进了其中的斥力势公式，如式(4)所示:

(4)

其中，(X-Xtarg)n=|(x-xtarg)n|+|(y-ytarg)n|表示距离因子，保证了斥力势的引入不影响机器人到达终点.

考虑到机器人在单次迭代中相对静止，且安全距离通常大于机器人的体积，所以OAPF算法将其他机器人当作障碍物的假设是可行的.因为机器人间斥力同样存在局部最小问题，所以也引入距离因子，如式(5)所示:

(5)

其中，θ为机器人之间的斥力增益，ρ(ij)表示机器人i,j之间的最小距离.

最终，机器人在3个叠加势力场的影响下运动.

在OAPF算法中，距离因子即指在原人工势场算法的斥力计算公式中添加距离要素，该方法的引入导致机器人路径规划与机器人运动终点高度紧密联系.而在复杂地图中单纯考虑距离要素并不一定达成最优规划.例如外围的机器人优先到达终点将导致内围机器人无法进入.当机器人在越过复杂地形后需要交换目标点时，若不考虑动态指派策略，势必会影响算法的鲁棒性和收敛性.

在碰撞避免问题上，虽然OAPF算法将机器人自身以外的个体视作障碍物，很好地解决了碰撞问题.但是，这意味着在每一次迭代过程中，每个机器人都需要知道其他所有机器人当前位置以构建斥力场，考虑实际情况下的机器人通讯问题，这个问题极大程度地限制了模型的效率与实际应用.

在OAPF算法中，通过限制机器人单次迭代中运动的步长来避免机器人同时运动时发生碰撞，大大增加算法复杂度，所以导致完成目标配置的时间开销提升同时群组机器人逐渐聚集导致机器人之间的斥力势增大，机器人则越不容易接近目标点，路程方面的开销势必增加.

3 VAPF算法

通过以上分析可知，人工势场改进算法OAPF不适应动态的目标指派策略，在该方面有较大的改进空间，其次，机器人之间的斥力势，虽然解决了机器人间的碰撞问题，但大大降低了算法的性能和实际应用的可行性.针对以上缺陷，本文提出的VAPF算法通过以下方法进行改进优化：

1)引入了动态的目标指派策略.

通过群组机器人实时位置和目标之间的对角距离构建成本矩阵，使用匈牙利算法进行最优指派，针对多最优解的情况，使用替换策略优化目标指派.

2)引入了Voronoi 约束保证机器人间的碰撞避免.

通过群组机器人实时位置构建Voronoi图，限制单机器人的单次迭代运动范围在Voronoi图对应的cell中.该方法既保证了机器人之间的避免，又极大程度提升了机器人的单次迭代运动范围.

3)引入了运动控制方法解决机器人抖动问题.

通过机器人改变运动角度的大小划分运动策略，当角度过大发生抖动现象时，限制运动角度，并缩小运动步长.

VAPF算法流程如下：

1.导入地图.

2.获得目标配置.

3.设置初始参数.

4.匈牙利算法为机器人指派目标点.

5.替换策略优化目标指派.

6.根据机器人位置，构建Voronoi图.

7.机器人在分配活动范围内使用人工势场算法进行路径规划.

8.当所有机器人到达终点，算法结束.若存在机器人未到达终点，当时间大于设定阈值，转4，否则，转6.

在上述过程中匈牙利算法指派目标点、替换策略、限定活动范围内的路径规划等算法细节将在后续详细描述.

3.1 环境建模和障碍物预处理

机器人在2D空间运动，已知：①所有机器人起点；②目标配置要求；③障碍物信息.则机器人自组装环境可以用有限空间存储以及表达，本文采用栅格法进行环境建模.

选取适当的矩形空间，满足空间包含上述①②位置，再将环境划分成u×v个栅格.最后采用直角坐标法表示栅格位置，则任意栅格均可用数据对{x,y}表示坐标.

为了简化问题，本文将机器人视作质点，那么为了保证机器人和障碍之间的碰撞避免，需要对障碍物进行膨胀化处理，即保持障碍物形状不变，障碍物外围轮廓向外偏移一定距离.考虑到障碍物的不可预估性，膨胀化处理有助于机器人与障碍物保持安全距离，能及时规避风险.

其次，障碍物无法跨越，且机器人初始位置不在障碍物中，那么障碍物只有外围轮廓信息是机器人路径规划的必要信息.所以本文对障碍物进行了上述的膨胀化处理后，只保留膨胀后增加部分的障碍物信息，舍弃原障碍物信息.大大减少了环境信息的数据量，有利于机器人进行更高效的路径规划.对障碍物处理示例如图1所示.

图1 障碍物预处理Fig.1 Obstacle pretreatment

基于上述环境建模和障碍物处理，后续工作将基于如下4种假设展开：2D平面除障碍物外均匀光滑；机器人可确定自身坐标与运动方向；机器人有足够能力完成任务；自组装任务可完成.

3.2 目标指派

假设群组机器人中有N个机器人，其位置集合为S={S1,S2,…,SN}，假设目标配置集合为T={T1,T2,…,TN}，为了获得最小代价完成一一对应的目标配置，则问题转化为指派问题.

(6)

(7)

根据康尼格定理：系数矩阵中独立0元素的最多个数等于能覆盖所有0元素的最小直线数，本文采用匈牙利算法计算指派矩阵流程如下：

步骤1.成本矩阵C各行所有元素减去该行最小值，各列所有元素减去该列最小值.

步骤2.若能找到N个独立的0元素，则可以进行指派，0元素对应位置的Xi,j=1，其他位置Xi,j=0.指派结束.

步骤3.对于K(K

考虑到代价矩阵无法在复杂环境下精准得出，本文算法设定了更新目标指派的时间阈值Δt，若当前时间距离上次目标指派时间超过阈值Δt，则重新进行目标指派.其次，传统的代价矩阵一般采用欧式距离或曼哈顿距离，如式(8)、式(9)所示：

(8)

hm=dx+dy

(9)

其中，ho代表欧式距离，hm代表曼哈顿距离，dx,dy分别代表起点与终点的横坐标、纵坐标之差.欧式距离表示两点之间的直线距离，未考虑障碍物的影响，且平方根运算增大了计算量.曼哈顿距离则只考虑了4个方向的运动，未考虑对角运动，脱离实际，不利于目标指派.为了更为贴近实际情况，同时保证运算的简单高效，本文采用对角距离，如式(10)所示：

hd=D(dx+dy)+(D2-2×D)×min(dx,dy)

(10)

其中，D表示机器人进行平行移动的移动代价，D2表示机器人进行对角移动的移动代价.

图2 目标指派的特殊情况Fig.2 Special case of target assignment

若选择指派矩阵X2，机器人S2将会阻挡机器人S1.考虑自组装的实际需求，应当选择指派矩阵X1，即S1→T1，S2→T2.类似情况下，匈牙利算法生成结果可能有多个相同最优解结果，所以本文提出了替换策略.

当匈牙利算法运行结束后，若存在两对目标指派Sα→Ty,Sb→T2，同时满足式(11)、式(12)，则交换两机器人的目标点，即修改指派矩阵.

Ca,y+Cb,z=Ca,z+Cy,b

(11)

abs(Ca,z-Cy,b)

(12)

通过以上策略，可以有效地均衡各机器人的运行距离，并解决上述特殊情况产生的问题.

3.3 运动空间划分与路径规划

通过机器人当前位置集合S，使用Delaunay三角剖分法构建Voronoi图，将全空间U划分为N个单元V={V1,V2,…，Vn}，其与集合S元素一一对应，满足式(13)：

Vi={p∈U|d(p,Xi)≤d(p,Xj)for allj≠i}

(13)

对于每个单元Vi，其均为凸多边形.因为单元Vi为凸多边形，所以可以用一组离散结点p={p1，p2，…，pk}表示.假设该离散点集合按顺时针排序，对于单元内的任意一点Q，恒满足式(14)，则该点位于Vi中.

(14)

将群组中单个机器人看作节点，那基于机器人节点位置构建Voronoi图具有以下性质：Voronoi单元中任意一点到该单元机器人节点的距离均小于或等于该点到相邻单元机器人节点的距离，同时基于单个机器人节点的位置对空间平面进行剖分，所以生成Voronoi单元中有且仅有一个机器人节点.因此可以利用Voronoi图中各个节点关系或者单元之间的几何特征来约束群组机器人路径规划中碰撞避免.

每个机器人在对应单元中，使用人工势场算法进行路径规划，当机器人到达对应的终点或超出运动范围时，则机器人单次迭代运动结束.人工势场算法通过式(1)-式(3)计算可得,算法如下所示：

算法:机器人Xi单次迭代运动伪代码if(已经到达指派目标) returnendif通过人工势场算法计算下一个运动点Xnextwhile(XnextinVi)do 把Xnext加入路径 更新当前位置 if(到达指派目标) break endif 通过人工势场算法计算下一个运动点Xnextendwhile

需要注意的是，机器人同时向Voronoi图中的边界运动时，有极小概率在边界上发生碰撞，此时机器人可采取少量回退操作，避免碰撞发生.

3.4 群组机器人的运动控制

在完成碰撞避免和障碍躲避后，本文对群组机器人的运动控制进行了研究.人工势场算法中，当机器人受到力的方向和运动方向一直成钝角时，会发生抖动现象，尤其是在机器人与目标点之间存在障碍物的情况下.在实际应用中，机器人期望有一条相对平滑的路径，抖动现象并不利于机器人运动.本文提出如式(15)、式(16)所示运动控制方法以解决抖动问题.

(15)

(16)

其中，x(t),y(t)表示机器人现在所在位置，x(t+1),y(t+1)表示机器人所要到达的位置；θ(t)表示机器人当前的运动方向，Δθ表示机器人到达下一步位置所要改变多少运动方向，l表示机器人的运动距离；当机器人改变运动方向为锐角时，不改变运动规则，当机器人改变运动方向为钝角时，限制改变方向至多为直角；τ表示调整参数，当机器人可能发生抖动时，限制机器人运动步长，可以有效地缓解抖动现象.

基于上述运动规则，机器人采用两步法进行运动：先通过旋转满足方向要求，再通过直线运动满足位移要求.采用此方法进行控制，不仅能有效降低机器人控制难度，还能控制单个机器人成本.

4 仿真实验

4.1 VAPF算法测试

为了测试VAPF算法的性能，本文在matlab平台上进行了仿真实验.实验保证算法运行环境不变，且设置了良好的参数.表1展示了VAPF算法的参数列表.

表1 参数列表Table 1 List of parameters

机器人自组装环境设定为500×500的栅格图，图中不规则地分布着若干个圆形代表地图上的障碍物.机器人的初始位置为随机分布，但保证机器人不在障碍物内部，且目标点数量与机器人数量相同.

本实验采用自组装问题中的直线型、H型和不规则型进行对比实验.其中，直线型和H型是最为简单常规的模型，能有效测试算法正确性、稳定性与性能，且这类简单模型的复合可以模仿动物的运动方式，有广泛的应用前景.本实验设计的直线型和H型模型分布均匀，而不规则型的目标点分布不均，但在一定程度上为包围型自组装配置，能有效考验算法在目标指派问题上的性能.上述3种模型实验如图3-图5所示.

图3 VAPF算法实现直线型自组装Fig.3 VAPF algorithm in linear self-assembly

图4 VAPF算法实现H型自组装Fig.4 VAPF algorithm in H-type self-assembly

图5 VAPF算法实现不规则型自组装Fig.5 VAPF algorithm in irregular self-assembly

其中，黑色的圆圈代表预处理后的障碍物；黑色实线代表机器人各自规划的路径；实线终点的标记代表实验设定的目标点；不同的色块由Voronoi图构建，其代表机器人各自的运动范围，能有效地帮助机器人实现碰撞避免.

通过上述结果可见，使用VAPF算法，机器人在不同自组装模型下均能高效、安全地到达各自的目标点.目标指派策略使得机器人各自到达目标点的消耗实现了最大程度上的均衡；运动空间划分能有效避免碰撞；运动控制方法能有效地抑制机器人在进行路径规划时发生抖动现象.

4.2 对比实验分析

针对上述实验模型，本实验通过不断增加机器人数量来对比VAPF算法和OAPF算法在时间和路程方面的表现.实验结果数据如图6和图7所示.

图6 算法时间消耗对比图Fig.6 Algorithm time consumption comparison chart

图7 算法路程消耗对比图Fig.7 Algorithmic distance consumption comparison chart

从多组对比实验可以看出，时间消耗方面：两种算法的时间消耗均随着机器人数量的增加而增加，OAPF算法时间消耗呈近似指数增长趋势，而VAPF算法的时间消耗增速远低于OAPF算法.在机器人数量相对较少时，两种算法的性能差别尤为明显.VAPF通过Voronoi图划分运动空间，机器人数量相对较少意味着其分布也相对稀疏，则每个机器人在单次迭代中有更大的独立运动空间.高效的迭代性能使得少量群组机器人的加入对VAPF算法性能影响极小，所以在图6中，当机器人数量不足20个时，时间消耗基本没有变化.而OAPF算法每次迭代要考虑机器人之间斥力，且单次迭代运动空间极小，即使是少量群组机器人的加入也会严重加剧算法的时间消耗.H型自组装相对于直线型自组装更为复杂，不规则型自组装更甚之，复杂的自组装要求会大大增加OAPF算法的时间消耗，而VAPF算法在不同自组装要求下性能稳定，且远优于OAPF算法.平均路程长度方面：机器人的分布对结果有着相对重要的影响，随着机器人数量的增加，随机且均匀的分布会使得机器人平均路程愈发稳定.由于OAPF算法中机器人之间存在斥力，OAPF算法的平均路程长度均多于VAPF算法，在复杂类型的自组装实验中尤为明显.

通过上述实验对比分析，VAPF算法无论是在时间消耗方面还是路程长度方面都有着显著的优势，且对不同的自组装要求以及群组机器人规模均有着良好的适应性.总的来说，VAPF算法达成了自组装过程中的碰撞避免和障碍躲避要求，在性能方面有着显著的优越性，有着一定的应用前景.

5 总 结

本文提出的基于Voronoi约束的人工势场算法，并将其应用到群组机器人的自组装路径规划方面，通过matlab仿真实验证明了该算法在碰撞避免和障碍躲避方面的可靠性、安全性.在该算法中，群组机器人均为无差异个体，且能自主运动，只要通过有限且少量次数的信息交互，就能达成自组装配置，具有良好的通用性和稳定性.其中，匈牙利算法和替换策略，为机器人提供理想的目标规划；单机器人在Voronoi图的单元中运动，保证了机器人间的碰撞避免；机器人在单元中使用人工势场进行路径规划，既使得机器人可以自主避障，又去除了其他机器人的干扰，机器人能在自己的cell中最大程度地移动，大大减少了算法迭代次数和机器人之间的信息交互，实现了时间和路程的双赢.

除此以外，本文提出的算法及仿真实验都建立在2D空间中，考察方面有限.未来工作将考虑运动学约束，并将该算法运用到3D空间中，与真实自然环境结合，在实际应用方向作扩展研究.