多移动机器人3阶段解耦路径规划

唐铭伟， 宋栓军

(西安工程大学 机电工程学院， 陕西 西安 710613)

多机器人集群作业系统正在逐渐取代单机器人作业系统[1]。与单机器人相比，多机器人集群作业时，各机器人之间可以协调配合，从而提升工作效率[2-3]。但是在多机器人集群系统中，机器人若在安全时间内到达同一节点，则会发生路径冲突、系统死锁等情况。因此，如何合理地解决这些问题，是当前机器人领域的一个重要研究方向[4]。

NAZARAHARI等[5]通过对遗传算法进行改进，在算法中添加碰撞消除算子，用以消除机器人之间可能发生的碰撞；但是该方法局限于障碍物较少的简单环境中，对于障碍物较多的复杂环境则不适用。张丹露等[6]利用A*算法结合交通规则的方法，解决了机器人间的交通拥堵问题，实现多机器人的协同路径规划；但是其地图局限性较大，地图中的可行道路必须是直行的单向通道。曹其新等[7]提出了一种基于保留区域的多机器人路径规划方法，避免了路径规划时各机器人间路径高度耦合的问题；但是该方法中各机器人需要共享位置信息，导致多机器人集群系统计算量增大。晁永生等[8]利用改进的A*算法，减少了搜索的时间，提高了系统的寻路效率。余娜娜等[9]以工作完成时间最小为目标，制定了各机器人的优先级；但是由于各机器人的独立性较强，使用该方法寻路容易出现局部最优情况。基于此，课题组提出一种3阶段多机器人解耦路径规划法：①利用改进传统蚁群算法，为各机器人在静态环境下快速规划出一条无碰撞初始路径[10]；②对规划出的初始路径进行冲突检查；③利用不同的避碰策略消解冲突，从而在消除冲突的前提下为系统输出一组较优的路径组合。

1 模型构建

1.1 环境建模

为保证环境模型构建的简洁性与连续性，课题组采用栅格法进行环境建模[11-12]。白色栅格表示自由栅格，黑色栅格表示障碍物[13]。建立二维坐标系，对栅格按照从上至下、从左至右的顺序进行编号[14]，同时对机器人的运行环境进行如下处理：

1) 将障碍物轮廓扩大，扩大范围为机器人的半径大小，在移动过程中机器人可以视为1个质点。对障碍物进行模糊化处理时，对于不满1个栅格的障碍物按1个障碍物处理。

2) 环境地图由N*N个栅格构成，设置阈值时间ΔT，规定在该时段内不同的机器人不能到达同一栅格节点。

环境模型如图1所示，各栅格节点在坐标系中都有相对应的序号，取其中心点坐标为该节点的坐标。由图1可知，当栅格边长取值为1，栅格序号与坐标可用公式(1)进行转换：

(1)

式中：ceil为取整函数，mod为取余函数，a为栅格边长，I为栅格序号。

图1 栅格地图Figure 1 Grid map

1.2 模型构建

在多机器人集群路径规划的数学模型中，需满足连续约束、安全约束、避碰约束和终止约束[15]。机器人Ri的路径由一组栅格坐标组成，即Li=[(xi(1),yi(1)),(xi(2),yi(2)),…,(xi(n),yi(n))]。系统工作时间取决于集群中工作时间最长的机器人，即：

(2)

式中：vi为机器人Ri的移动速度，δ为暂停的次数。

1.3 算法设计

当前机器人路径规划算法主要有2类：启发式算法(Dijkstra算法、A*算法等)和仿生算法(遗传算法、粒子群算法等)[16]。采用蚁群算法对机器人进行全局路径规划。针对传统蚁群算法存在收敛速度过慢、随机性较强等问题,课题组提出2项优化措施。

1.3.1参数优化

在传统蚁群算法中，信息启发因子α、期望启发因子β、信息素挥发系数ρ、蚂蚁数量m等都是非常重要的参数，其值通常取为固定值。但是在实际应用过程中，在不同时段，参数对于算法的影响也不同。为了加快算法的收敛速度，提高寻路效率，这里分别对α和β进行动态化处理，如公式(3)～(4)所示：

(3)

(4)

式中：A,B,C,D,E和F为常数，Nc为当前迭代次数，k为总迭代次数。

在蚂蚁寻路过程中，信息素强度Q也起到至关重要的作用。取值越大，算法的收敛速度越快，但是会导致蚂蚁寻路的空间减小，容易陷入局部最优；取值过低，算法前期的正反馈效果不明显，使得寻路效果不佳。因此对参数Q进行自适应处理，即：

(5)

式中：Q0为初始信息素强度，Lb为本次迭代最优路径，LB为之前所有迭代中的最优路径，λ为调整参数。

1.3.2修改启发函数

传统蚁群算法采用从当前节点i到下一节点j之间距离的倒数作为启发函数ηij。但是在栅格环境中，蚂蚁在寻路时，由于正反馈作用不明显，容易陷入局部最优。针对这种情况，引入路径指导函数，如公式(6)所示：

(6)

式中djG为下一可选节点j与目标点G之间的距离。

新的启发函数为

(7)

在路径指导函数的作用下，可以加快算法的收敛速度，为机器人快速地规划出较优的全局路径。

2 多机器人冲突预判与消解

2.1 冲突检查

多机器人系统R为各机器人规划出初始路径组LC=[Lc1,Lc2,…,Lcn]，每条路径均为一组节点坐标的集合。若Ri的初始路径Lci与其他机器人的初始路径节点集合的交集为空，则机器人Ri可以按照该初始路径安全移动；若该路径的坐标集合与其他机器人的初始路径的坐标集合的交集不为空，表示该初始路径与其他机器人的初始路径存在交叉点，此时需要对其进行安全判断，判断方法如下：

(8)

式中dSiWi，j(k)为Ri沿初始路径从起始点Si至交叉点Wi,j(k)的距离。

若满足该公式，表示二者沿着初始路径移动时，不会在安全时间内到达该交叉点Wi，j，不会发生路径冲突，称该交叉点为伪冲突点Mi，j。若不满足该公式，表示二者将会发生路径冲突，需要进行路径协调。

机器人Ri与其他机器人的初始路径的交叉点集合为Wi=Wi,1∪Wi,2∪…∪Wi,n，依次对Wi中的交叉节点进行安全判断;判断完成之后，将不会发生路径冲突的伪冲突节点集合Mi从Wi中去除，得到机器人Ri与其他机器人的路径冲突节点集合Zi=(Zi,1,Zi,2,…,Zi,n)。

2.2 冲突消解

设机器人Ri与机器人Rj之间存在冲突点Zi,j(k)，比较二者到达冲突节点所需的时间，采取“先到先行，后到协调”的原则确定需要进行路径协调的机器人。若二者同时到达冲突节点，则随机选择一个机器人进行路径协调。文中采用2种协调策略：

1) 暂停策略

对机器人Ri进行新的安全判定，判断公式为

(9)

若满足公式(9)，表示机器人Ri采用该策略可以消解冲突，且不会产生新的冲突点。则令Ri在移动之前，在起始点处暂停ΔT后，再沿初始路径移动；若不满足该公式，则采用策略2)。

2) 更新策略

机器人Ri将冲突节点Zi视为障碍物栅格，系统重新为其规划出一条从起始点至目标点的路径，再与其他机器人的路径进行安全判断。若不会产生新的冲突节点，则令Ri沿着新规划的路径移动；若产生新的冲突节点，则将新的冲突点视为障碍物栅格，重新对Ri进行全局路径规划。

分别使用上述2种方法进行路径协调，比较二者所需时间，选择时耗较少的方法进行路径协调。

3 实验结果与分析

3.1 改进蚁群算法的实验与分析

为了验证改进蚁群算法的高效性，使用MATLAB软件进行实验仿真。在相同的环境下，分别使用改进的蚁群算法和传统蚁群算法(ACO)进行路径规划。参数选择如表1所示，寻得最优路径如图2所示(图中S表示起始点，G表示目标点)，算法收敛曲线如图3所示，实验结果数据如表2所示。

图2 最优路径对比Figure 2 Comparison of optimal paths

图3 收敛曲线对比Figure 3 Comparison of convergence curves

表1 算法参数选择

表2 仿真结果数据

由表2可知，使用改进后的算法，在收敛速度和路径长度方面，较传统蚁群算法都有了显著的提升，能够较快地获得算法的最优解，证明了改进算法的可靠性，为多机器人集群系统的路径规划打下了良好的基础。

3.2 多机器人路径规划

为验证先前所提出的路径协调方法的有效性，使用MATLAB软件进行实验仿真，设栅格边长为1 m，各机器人移动速度均为1 m/s，安全时间ΔT=1.5 s，移动机器人数量为3个，分别进行2组不同的实验。

在实验1中，机器人集群的初始路径和协调路径分别如图4～5所示，实验结果数据如表3所示。

图4 20*20环境下初始路径规划Figure 4 Initial path planning in 20*20 environment

图5 20*20环境下协调路径规划Figure 5 Coordinated path planning in 20*20 environment

表3 实验1数据结果

在实验2中，机器人集群的初始路径和协调路径分别如图6～7所示，实验结果数据如表4所示。

图7 25*25环境下协调路径规划Figure 7 Coordinated path planning in 25*25 environment

表4 实验2数据结果

4 结论

针对多机器人的路径规划问题，课题组提出一种具有前瞻性的3阶段解耦路径规划法：首先，利用改进传统蚁群算法在静态环境下快速规划出一条无碰撞初始路径；然后，对初始路径进行冲突检查；最后，利用不同的避碰策略消解冲突从而输出一组较优的路径组合。利用MATLAB软件进行了仿真实验，实验结果表明：

1) 改进后的蚁群算法大大提高了算法的收敛速度，可以快速地为机器人规划出一条较优的无碰路径。

2) 课题组所提出的路径协调策略可以有效地检测和消解多机器人之间的路径冲突问题，减少了机器人绕行距离和等待时间，保证了多机器人集群系统的运行效率和可靠性。

课题组所提出的路径规划方法还存在一定的局现性，优化目标较为单一，今后将考虑多个目标进行优化，同时考虑工作环境中存在动态障碍物的情况，提高机器人工作的安全性。