多移动机器人队形初始化目标点分配算法研究

李海婷， 张鹏超， 罗朝阳， 刘亚恒， 徐鹏飞

(1.陕西理工大学 电气工程学院，陕西 汉中 723000；2.陕西理工大学 机械工程学院, 陕西 汉中 723000；3.陕西理工大学 陕西省工业自动化重点实验室，陕西 汉中 723000)

近年来，多移动机器人编队控制技术[1]已经广泛应用于工业自动化生产、高危作业操作、协助军事行动以及灾后危险区域搜索与营救[2-4]等，引起了众多学者的关注，成为研究的热点。目前机器人的编队研究着重于避碰避障和路径规划，很少有人关注队形的形成。队形的形成[5]作为多移动机器人编队控制的基础是不可忽略的，关键解决两个问题：面对多个目标点位置时如何确定机器人的目标位置点；如何保证每个机器人无碰撞到达目标位置点。每个机器人只有确定并精确快速地运动到自己的目标点，才能基于该初始队形完成既定任务。

针对以上两个问题研究者们提出了几种解决方法。Ma等[6]在传统方法[7]的基础上提出一种动态角色分配算法完成队形的形成并实现了避障，但计算复杂，路径长度代价待优化；陈梅等[8]提出一种改进目标点优化分配法，考虑了机器人角度方面的影响，但需要进行多次整体调整分配，增加了决策时间，适用于极少数机器人编队;郭丽丽[9]针对目标点选择问题提出一种人工社会职位法，利用规则和目标点等级划分以及综合人工力矩控制完成多个机器人队形形成。但对信息传感要求高且队形形成过程中没有考虑碰撞问题;Han Ming-ming 等[10]根据最大距离值分配原则出了一种在障碍物环境中的任意队形形成算法，但总路径长度未必最短。

针对现有的目标点分配算法存在的计算复杂、路径消耗量大、效率低且未考虑机器人之间碰撞问题，本文提出一种在二维静态障碍物环境中的目标点分配算法，通过筛选出预测矩阵并求解实时路径方程，确定目标点分配矩阵，使每个机器人选择互不冲突的目标位置并避障避碰到达目标点，实现队形初始化。

1 目标点分配算法

1.1 目标点分配矩阵的产生

设参与编队的机器人数目为i，目标点个数为j，i≤j=1,2,3,…。机器人初始位置点坐标记为(xi0,yi0)，目标点位置坐标记为(xjm,yjm)，算法如下：

(1)计算每一个机器人初始位置点到各个目标点的距离l。

初始位置点与各个目标点的连线不经过障碍物时：

(1)

初始位置点与各个目标点的连线经过障碍物时，为方便计算，设机器人转弯路径为半圆弧，障碍物的影响半径为r，则路径长度为

(2)

(2)由步骤(1)可得每个机器人路径长度集合为Li={li1,li2,li3,…,lij}，在每个路径集合Li中不重复的按序抽取一个路径元素组成多个分配矩阵Ak，其中k={1,2,3,…,n!}且随机抽取的路径元素对应目标点位置不重合。

(3)计算每个分配矩阵A的F范数，即‖Ak‖F，并将‖Ak‖F从小到大排列，最小F范数对应的矩阵为机器人到达目标点路径总和最短矩阵，并选该矩阵为当前预测最优目标点分配矩阵。

(4)判断当前选定的预测最优矩阵是否存在机器人之间碰撞情况。

根据选定的最短路径矩阵列出各机器人队形初始化过程中运动轨迹实时坐标方程组：

(3)

其中机器人i在t时刻的位置坐标为(xit,yit)，θi是机器人i的初始坐标点到其对应目标点连线与坐标系x轴正方向的夹角，θi=arctan((yjm-yi0)/(xjm-xi0))，v为机器人恒定运行速度。

1.2 目标点分配算法流程

将所有方程组(3)按照排列组合顺序抽取2个组成新方程组，通过求解t判断新方程组对应的两个机器人之间在到达目前选定的目标点路径过程中是否会发生碰撞。若所有的新方程组均不存在t使方程组成立，则所有机器人之间不会发生碰撞。此时当前选定的矩阵为最终目标点分配矩阵，机器人队形初始化的目标点分配完成；若存在t使方程组成立，则该新方程组有解，机器人之间会发生碰撞，舍去当前选择的矩阵，按照范数的大小值排列顺序选取下一范数对应的队形矩阵作为新的最佳预测矩阵，并列出相应方程组再次判断是否发生机器人之间的碰撞，以此类推，选出不发生碰撞的最优目标点分配矩阵，直至机器人目标点分配完成，算法流程如图1所示。

图1 目标点分配算法流程图

2 路径轨迹的确定

目标点分配完成后需要解决的问题是机器人怎样避障到达目标点，因设定障碍物为静态障碍物，对机器人需进行全局路径规划。为缩短运动路径，提高运动效率，采用改进快速搜索算法(RRT*)。机器人由初始位置到各自目标点行进的路线以直线为基础，通过单向搜索以目标点所在方向为目标路径，直到遇到障碍物或者相交变为双向搜索进行避障，在此过程中对路径代价进行优化，得到最优路径，算法如下。

改进RRT*算法：

1.V′←V;E′←E;qnearst←Nearst(G,q);qnew←Steer(qnearst,q);

2.if ObstacleFree(qnearst,qnew)then

3.V′←V′∪(qnew);qmin←qnearst;Cnear←Near(G,qnew,|V|);

4. for allqnear∈Cneardo

5. if ObstacleFree(qnearst,qnew)then

6.c′←cos(qnear)+pc(Line(qnear,qnew));

7. ifc′

8.qmin←qnear;E′←E∪{(qmin,qnear)} ;

9. for allqnear∈Ccear{qmin} do

10. if ObstacleFree(qnearst,qnew)and cost(qnear)>cost(qnear)+c(Line(qnearst,qnew))then

11.qparent←Parent(qnear);E′←E′{(qparent,qnear)};E′∪E′{(qnew,qnear)};

12.returnG′=(V′,E′)。

图2 转向示意图

本文中的改进RRT*算法即路径渐进最优，将得到的原始路径的所有节点都进行记录，并记为有效路径点集q。从整条路径的起点开始，依次对下一个路径点进行连接，如果与相邻路径点的连线不与障碍物空间发生干涉，就可以剔除这条连线转而连接下一个路径点，直到连线与障碍物产生交集，停止本次连接，以该节点所在的上一路径点作为连接点产生改进后的新路径；当第一条新的路径产生，就以该路径末端为起点重复上述操作，如此递归进行，完成对整条原始路径的多余点剔除操作，将经过多余点剔除的新路径的有效路径点进行记录，就得到一条有效避障且路径长度优化的运动轨迹。当节点足够多，迭代次数足够大时，可收敛到局部最优。

针对实验室turtlebot2两轮驱动差分机器人，原地转向需要考虑转弯半径，设转向函数为

将机器人运动的当前位置点与目标点连线，其中θ为机器人运行的直线轨迹与障碍物最大影响圆区域的两交点与圆心连线的角度，如图2所示。

3 实验验证

为验证所提目标点分配算法的有效性，做了两组仿真实验，第一组用于验证该算法的有效性，第二组用于和其他算法作对比，验证该算法的高效性。

仿真实验1：取i=3，即3个机器人参与队形初始化，将每个机器人分别编号1、2、3,取初始位置依次为(0,0)、(1,3)、(2,1.5),目标点位置坐标为(8,7)、(7,8)、(9,8)，两个障碍物坐标为(4.3,3.6)、(4,5.5)，障碍物影响半径r=0.04 m,机器人运行速度v=0.5 m/s。由式(1)、(2)可得机器人到达各个目标点路径长度矩阵A(mi为目标点位置，Ri为机器人，i=1,2,3。)为

m1m2m3

在矩阵A中排序抽取位置不重合元素构成6个预测目标点分配矩阵，即

通过计算得‖A5‖F<‖A2‖F<‖A3‖F<‖A4‖F<‖A1‖F<‖A6‖F，取‖A5‖F对应的矩阵A5为当前最优目标点分配矩阵。即(0,0)→(7,8),(1,3)→(9,8),(2,1.5)→(8,7)。由式(3)可得方程组(4)、(5)、(6)并求解，判断该矩阵产生的路径是否存在机器人之间的碰撞：

(4)

(5)

(6)

其中ta、tb分别为经过障碍物区域的起始时间和末端时间。分别联立式(4)和(5)、式(5)和(6)、式(4)和(6)可得方程组均无解。就是说A5对应的分配路径机器人之间不会发生碰撞，所以选定A5为最终目标点分配矩阵，即(0,0)→(7,8)、(1,3)→(9,8)、(2,1.5)→(8,7)为最终分配结果,仿真结果如图3、图4和表1。

由图3可看出矩阵A5对应的3个机器人分别由各自初始位置C1、C2、C3点到达各自目标点，可知C1(0,0)→Ta2(7,8)，C2(1,3)→Ta3(9,8)，C3(2,1.5)→Ta1(8,7)，有效完成了目标点分配，图4为利用改进RRT*算法为机器人到达各自目标点的路径轨迹规划，可看出有效避碰避障。表1中参数S为每个目标点分配矩阵Ai对应的分配路径长度总和，maxL为指定Ai情况下机器人距目标点最长路径，即决定队形初始化消耗的时间t。6种可选择的方案中，矩阵A5对应的总路径长度最小，时间消耗最少且不发生碰撞，即选A5对应的方案为最佳目标点分配方案。

图3 目标点分配图 图4 路径规划

表1 各个分配路径集合Ai对比

仿真实验2：取9个机器人初始位置点依次为(2.5,12.5)、(5.5,9.5)、(-2.5,6.8)、(-3.8,3)、(0.5,5)、(0.2,0.2)、(4.8,3)、(10.5,3)、(10.2,0.2),目标点分别为(-1,11)、(3.5,11)、(8,11)、(-1,6.5)、(3.5,6.5)、(8,6.5)、(-1,2)、(3.5,2)、(8,2),机器人速度v=0.5 m/s。分别采用人工社会职位法、最大距离分配算法、目标点分配算法(本文算法)得到的仿真结果如图5和表2所示。

(a)人工社会职位法 (b)最大距离分配算法 (c)本文算法

表2 机器人R与目标点m的对应关系

图5是3种算法为9个机器人与各目标点分配的结果。图5(a)采用郭丽丽[9]在传统方法上改进的人工社会职位法；图5(b)为Han等[10]进一步改进的最大距离分配算法；图5(c)采用本文提出的目标点分配算法。Ri对应的实心圆点为机器人的初始位置，其余圆点为目标点，分别散落于图中正方形上。表2为各算法结果显示的机器人与目标点对应关系。由表3实验结果分析对比可知：在路径总消耗上，本文算法比人工社会职位法减少23%，比最大距离分配算法减少22%；在时间消耗方面，比人工社会职位法减少46%，比最大距离分配算法减少44%；在算法复杂程度上即计算步数，比人工社会职位法简单，但稍劣于最大距离分配算法，原因是本文考虑了机器人向目标点运行过程中可能发生碰撞的情况，对于室内无障碍区无疑提高了决策效率。对于有障碍区，利用改进RRT*算法实现路径规划，使机器人安全高效到达目标点。

表3 3种算法仿真结果对比

由两组实验可得本文提出的算法有效完成了目标点分配，而且避免了机器人之间、机器人与障碍物之间的碰撞，精确的到达目标点。与其他算法相比，本算法简单，便于操作，且易数学分析，能够有效避碰避障，此外，还可根据机器人允许的速度范围之内灵活调节队形初始化的运行时间。充分证明了所提出算法的有效性与高效性。

4 结论

本文针对多移动机器人队形初始化，提出了一种通过利用筛选法得到预测目标点分配矩阵。根据当前选定的目标矩阵求解方程组判断是否发生机器人之间碰撞，通过循环筛选求解，直到选出避碰的最优目标点分配路径。在MATLAB中对该算法的有效性进行验证可知，与已有方法相比，该算法简单、可实施性强，在有障碍物的环境中，能够实现在避障避碰的同时实现时间和路径消耗最少的高效率队形初始化，以达到最优目标点分配，提高移动机器人使用寿命的同时为多移动机器人协作高效率的完成任务奠定良好基础。