CBS多机器人路径规划(Conflict-Based Search)_cbs算法

多机器人路径规划

多智能体路径规划 (Multi-Agent Path Finding, MAPF) 研究多智能体的路径规划算法，为多机系统规划无冲突的最优路径．
CBS(Conflict-Based Search) 是一种基于冲突的 MAPF 算法, CBS 算法给出 MAPF 问题的全局最优结果．

参考文献:

• Conflict-based search for optimal multi-agent path finding

CBS 多机路径规划

CBS是一种中央规划算法(Centralized Planning for Distributed Plans)，什么意思呢？也就是由一台主机作为中央控制器，在全局视角生成每一台机器人的路径并统筹解决冲突．除了中央规划算法，还有分布式规划算法(Distributed Planning for Centralized Plans，Distributed Planning for Distributed Plans)等，具体可以参考这篇综述 Survey of the Multi-Agent Pathfinding Solutions．

CBS 是一族方法．算法的思想主要将多机规划分为两层，底层执行带有约束的单机规划，例如用传统 A* 算法，顶层遍历底层的规划路径，检查路径之间是否有冲突，如果有冲突则施加约束重新进行底层单机规划，直到所有底层路径无冲突为止．

一些术语

先来讲讲CBS定义的一些术语:

• $path$: 一条机器人的路径，也就是我们平时用得最多的单机规划结果
• $solution$: 多机系统中所有机器人的 $path$ 的集合($n$ 条 $path$)，也就是 mapf 算法的全局规划结果
• $conflict$: 冲突．上述的 $solution$ 中，$n$ 条 $path$ 之间可能会有冲突(没冲突当然皆大欢喜了)．具体的描述形式为 $(a_i,a_j,v,t)$ ，表示在时刻 $t$, $a_i$ 和 $a_j$ 同时占据了顶点 $v$. 拿栅格地图来说，就是在时刻 $t$, $a_i$ 和 $a_j$ 同时占据了矩阵的一个格子 $matrix(i,j)$.
• $constraints$: 约束．一个约束 $(a_i,v,t)$ ，表示在时刻 $t$, $a_i$ 不能占据顶点 $v$.

顶层

CBS算法顶层使用约束树(Search the Constraint Tree (CT))数据结构来解决底层冲突，大概长这样：

其实就是一颗树(可以设计成二叉树或者多叉树)，树的每个节点除了有指向子节点的指针，还包括：

• 1. 约束($constraints$) : 约束根据冲突($conflicts$)得到
• 2. 当前计算的全局路径($solution$, 注意这个可能包含冲突，一旦无冲突即为全局最优路径, 算法退出)
• 3. 全局代价 $cost$

生成树

顶层算法中，最核心的一点就是树的生成，也就是说从父节点 $N$ 该如何生成子节点 $N$->$child$

假设现在有一个节点 $N$，节点内包括:

• 当前节点的约束 $constraints$
• 根据当前$constraints$计算出来的全局路径 $solution$ (怎么得到的？通过将约束施加到底层规划算法，例如$A$*，得到一堆 $path$, 把这些 $path$ 合起来就叫 $solution$． 怎么样将约束施加到底层算法呢？在后面来解释)
• 根据 $solution$ 计算出来的全局代价 $cost$
  

生成子节点 $N$->$child$ :

• 第一步: 遍历 $solution$, 搜索 $path$ 之间的冲突 $conflict$
  • 没冲突：就找到全局最优结果，算法退出
  • 有冲突：假设现在找到了一个冲突 $(a_i,a_j,v,t)$ ，表示在时刻 $t$, $a_i$ 和 $a_j$ 同时占据了顶点 $v$
• 第二步：解决冲突
  • $(a_i,a_j,v,t)$ 表示在时刻 $t$, $a_i$ 和 $a_j$ 同时占据了顶点 $v$, 怎么解决？打个比方，两伙人去饭店吃饭，只剩一桌空桌了，怎么办呢，很显然，在当前文明社会，来个石头剪刀布，输了的人等下一桌．虽然字母很抽象，但算法都来源于生活．
  • 好，按照生活常理，在时刻 $t$, $a_i$ 和 $a_j$ 只能有一个占据顶点 $v$，也石头剪刀布？这样不好，计算机里面不是文明社会，谁也不服谁，容易打架．加一桌？不行，店就这么大，会挤到其他人．好在计算机能力强，不行的话我再开一家店吧，就在旁边立马复制一个一模一样的店，一样的味道，一样的人气，一样只空一桌，$a_i$ 去这边， $a_j$去那边，谁也不亏，谁也不占便宜．
  • 注意，就在此时，父节点分叉了(fork)，诞生出了两个子节点，$a_i$ 去的子节点施加约束 $(a_j,v,t)$，$a_j$ 去的子节点施加约束 $(a_i,v,t)$. 一个冲突$(a_i,a_j,v,t)$，拆成了两个约束 $(a_i,v,t)$ 和 $(a_j,v,t)$
• 第三步：生成子节点
  • 对于一个冲突 $(a_i,a_j,v,t)$, 给予 $a_i$ 和 $a_j$ 同等机会, 节点 $N$ 分叉成两个子节点, 每个子节点都继承父节点原有的约束，并施加每个子节点独有的约束，如下图：
  • 
  • 将每个子节点的约束施加到底层规划算法，计算出一堆 $path$, 把这些 $path$ 合起来变成$solution$, 计算每个子节点的全局 $cost$, 然后在整个约束树找 $cost$ 最小的节点 $N$, 回到第一步继续查找冲突, 如果有冲突，每个子节点继续分叉

底层

CBS的底层是单机搜索，理论上，可以用任意一种搜索算法，比如经典的$A$* . 只不过这个 $A$* 除了空间维度，还需要搜索时间维度．顶层施加约束在底层的逻辑处理其实就是将约束当成一个虚拟障碍物，比如约束$(a_i,v,t)$在 $A$* 算法中，只是一个空间 x 时间的立方体里的一个障碍物方块，与二维里的障碍物没有本质的区别．

关于时空 A*(space-time A*)，可以参考这篇文章

例子

这是论文中的例子.

如图，有两只小老鼠 $a_1$ 和 $a_2$ 想要吃蛋糕, 分别从 $S_1$ 和 $S_2$ 出发，想要到达 $G_1$ 和 $G_2$

我们按照步骤来生成顶层搜索约束树:

现在约束树暂时没有节点，先生成一个根节点，约束集为空, 在空约束集的约束下调用底层搜索，生成两个 $path$:
$a_1$: $<S_1,A_1,C,G_1>$
$a_2$: $<S_2,B_1,C,G_2>$
计算一下所有 $path$ (也就是 $solution$) 的 $cost$: 3 + 3 = 6(起点不算)

现在的 $root$ 约束树如图：

好，接下来正式按步骤：

• 第一步：遍历 $solution$, 搜索 $path$ 之间的冲突 $conflict$
  • 很明显有冲突：一眼找到两条 $path$ 中的第 2 步(起点不算)都到 $C$, 生成冲突$(a_1,a_2,C,2)$ ，表示在时刻 $2$, $a_1$ 和 $a_2$ 同时占据了顶点 $C$
• 第二步：解决冲突
  • 一个冲突$(a_1,a_2,C,2)$，拆成两个约束 $(a_1,C,2)$ 和 $(a_2,C,2)$
• 第三步：生成子节点
  • 对于冲突 $(a_1,a_2,C,2)$, 给予 $a_1$ 和 $a_2$ 同等机会, 节点 $root$ 分叉成两个子节点, 每个子节点都继承父节点原有的约束，并施加每个子节点独有的约束
  • 将每个子节点的约束 $(a_1,C,2)$ 或 $(a_2,C,2)$ 施加到底层规划算法，计算出一堆 $path$, 把这些 $path$ 合起来变成$solution$, 如下图：
    
  • 计算每个子节点的全局 $cost$，发现都是 $7$，由于搜索的时候一般从左边开始, 所以先考察左子节点， 回到第一步，遍历左子节点的 $solution$, 发现无冲突，说明找到了全局最优路径，算法返回.

总结

到这里，可以发现，CBS就是一个算法框架，顶层解决冲突，底层进行搜索．

优化 CBS 算法，就是考虑最优性(Optimal)和完全性(Complete), 来优化顶层约束树的生成方式和底层搜索算法.

例如 ECBS, 就是一种有界次优的 CBS 算法, 通过优化约束树进行节点分叉的启发函数和底层搜索算法给出有界次优结果，在可接受的情况下大大降低时间复杂度，有兴趣可以看看．

• Suboptimal Variants of the Conflict-Based Search Algorithm for the Multi-Agent Pathfinding Problem

多机器人路径规划(Multi-Agent Path Finding, MAPF) 这里展示了 CBS 算法在 ros 上的实现效果，有兴趣可以看看.