多机器人协同运行

多机器人协同运行

   在实际环境中，通常会有多个移动机器人完成一个或多个任务。为了实现这个目标，可以使用多机路径规划加上一个多机器人协同运行控制器。在多机路径规划完成后，就会得到所有机器人运行的路径，且这些路径点都有相对时间属性，通过相对时间就可以确定机器人通过路口的先后顺序。
  为了让机器人在实际环境下运行，运动控制器以时间先后顺序做为约束条件同时对所有机器人做速度规划，完成速度规划后，给所有机器人一个启动信号，让机器人以规划好的速度运行。这样做的缺点非常明显，如果机器人中途遇到障碍物，需要重新进行速度规划。当机器人比较多的时候难以保证启动信号的同步性，特别是实际环境下的通信延时的情况下，因此这种方式是不可取的。比较可行的办法是将时间先后顺序转变成位置约束，也就是说一个机器人能够继续运行的条件是另一个或多个机器人已经到达了对应的位置。

适用条件

1. 多机器人在有向或无向图下进行路径规划（减少CBS的计算量）；
2. 多机器人全局路径规划得到的路径，如CBS；
3. 机器人的体积小于栅格的体积；

实现过程

  以下将通过一个简单的例子介绍这个方法的实现过程，如图1所示。图1为2*5的栅格地图，黑色为不可达区域，1号机器人位于栅格a，其目标位置为栅格e，2号机器人位于栅格b，其目标位置为栅格d。

图1

  从图1中可以看到1号机器人将被2号机器人阻挡，普通的路径规划无法解决这个问题，在使用CBS后，2号机器人将进入栅格f避让1号机器人，从而完成路径规划，得到如下路径，各机器人路径点按照位置先后关系确定时刻点，即时间先后顺序。

相对时刻	0	1	2	3	4
1号机器人的路径	a	b	c	d	e
2号机器人的路径	b	c	f	c	d

  虽然得到了机器人各自的路径，但如果1号机器人先于2号机器人启动，将会发生相撞的情况，而且实际环境下机器人的速度不相同，还存在原地旋转的情况，如果不添加启停约束，就算完成了多机路径规划同样无法保证机器人相撞的情况。通过观察可以知道，如果2号机器人到达c点后，1号机器人从a开始启动运行到b，当2号机器人运行到f后，1号机器人从b开始启动运行到c。依照此方式最终可以使各机器人到达各自目标位置。具体的伪代码参考如下：

去除掉各机器人路径位置重复的路径点，保留时刻点最小的路径点
for i in [0, 1, 2,...,n-2]
    for j in [i+1, i+2,..., n-1]
        for pi in 第i个机器人的路径点:
            for pj in 第j个机器人的路径点:
                if pi和pj的位置相等:
                    if 路径点pi的相对时刻小于路径点pj的相对时刻:
                        添加机器人j在路径点pj-1能够启动的条件是机器人i到达了路径点pi+1
                    else:
                        添加机器人i在路径点pi-1能够启动的条件是机器人j到达了路径点pj+1

  通过上述伪代码的步骤，可以得到各机器人运行的约束，并将这些约束形成一个图结构，如图2所示。

图2

  图2可以通俗的解释为：

1. 1号机器人在a点启动运行的条件为2号机器人到达c点；
2. 1号机器人在b点启动运行的条件为2号机器人到达f点；
3. 2号机器人在f点启动运行的条件为1号机器人到达d点；
4. 2号机器人在c点启动运行的条件为1号机器人到达e点；

  大致的运行效果为：2号机器人缓慢地旋转并朝c移动，当2号机器人到达c点时并向f点移动，1号机器人从a点启动到达b点，当2号机器人到达f点时，1号机器人从b点启动，依次经过从c、d两点，到达目标点e，在这个过程中，当1号机器人到达d点时，2号机器人从f点启动朝c点运行，当1号机器人到达e点时，2号机器人从c点启动朝d点运行，最终到达目标点d。

注意事项

1. 当机器人有障碍物检测的情况下，可以放松机器人的启动运行条件，如“1号机器人在a点启动运行的条件为2号机器人到达b点”，如此可以提高运行效率；
2. 当前的运行约束存在死锁的可能性（实际情况不存在）；
3. 需要一个集中式系统记录各机器人到达了哪个路径点；
4. 可以将各机器人的运行状态传给每一个机器人，让机器人自己决定是否运行，也可以将这一步骤由集中式系统处理，将得到的一部分路径传给机器人执行即可，不需要知道其它机器人的状态。推荐使用后面这种方式，可以大大地减少通信的数据量；