【关于ECBS的多机器人路径规划的整理】

ECBS是对CBS问题的优化，而了解CBS又得先了解A*算法，本文对以上内容进行详细介绍，主要参考文献如下
Suboptimal Variants of the Conflict-Based Search Algorithm for the Multi-Agent Pathfinding Problem

1. A*算法

关于A算法，主要参考了A*算法详解 一看就会 手把手推导 完整代码注释以及启发式搜索（heuristic search）———A*算法
A算法常用于解决最短路问题，其是Dijkstra 算法和贪婪优先算法的结合，对于评估函数f(n)而言：

• Dijkstra: 到起点的距离优先：即从起点A，沿着产生的路径，移动到网格上指定节点的移动耗费。
• 贪婪优先算法：到目标点的距离优先 ：即从网格上那个节点移动到终点B的预估移动耗费
• A*算法：即前两者之和：$f(n)=g(n)+h(n)$，$g(n)$即对应Dijkstra，$h(n)$即对应贪婪优先算法

这里着重强调一下$h(n)$的含义，$h(n)$经常被称为启发式的，这样叫的原因是因为它只是个猜测。我们没办法事先知道路径的长度，因为路上可能存在各种障碍(墙，水，等等)。它计算从当前位置到目的位置之间的距离。很重要的一点，我们忽略了一切障碍物。这是对剩余距离的一个估算，而非实际值。而由于忽略了所有障碍物，所以这个值一定是当前位置到目的位置的最小值。

具体步骤：

新建open列表，open表表示当前的搜索边界，也即为用于下一步搜索的备用节点集合
新建close列表，close表存放从open表中弹出的结点
1、将源点加入open表
2、
while(OPEN!=NULL||到达goal_node)
{
    从OPEN表中取f(n)最小的节点n;
    if(n节点==目标节点)
        break;
    for(当前节点n的每个子节点X)
    {
        计算f(X);
        if(X在open表里)
            if(新的f(X)<OPEN中的f(X))
            {
                把n设置为X的父亲;
                更新OPEN表中的f(n); //不要求记录路径的话可以直接加入open表，旧的X结点是不可能比新的先出队
            }
        if(X在CLOSE表里)
            continue;
        if(X都不在)
        {
            把n设置为X的父亲;
            求f(X);
            并将X插入OPEN表中; 
        }
    }//endfor
    将n节点插入CLOSE表中;
    按照f(n)将OPEN表中的节点排序;//实际上是比较OPEN表内节点f的大小，从最小路径的节点向下进行。
}//endwhile(OPEN!=NULL)

3、保存路径，从目标点出发，按照父节点指针遍历，直到找到起点。

2. $A_{ϵ}^{\ast }$算法

参考文献：Studies in Semi-Admissible Heuristics，主要参考了A*算法及其变种
根据论文介绍$A_{ϵ}^{\ast }$算法是一种有界次优算法。设最优代价为$C_0$，给定参数$ϵ$,使得有界次优算法的结果$C$满足$C\le (1+ϵ)C_0$。

我们知道 A* 算法每次在$OPEN$表里取最小的节点进行下一步搜索。论文提到，搜索算法在搜索过程中事实上会得到许多$cost$相近的解，A 算法每次都取最优比较费时间，多花的时间与得到的优化效果不成比例，俗称 “性价比不高”。

因此$A_{ϵ}^{\ast }$算法在A算法$OPEN$表的基础上，设计了$FOCAL$表，定义如下：
$FOCAL=n^{\ast }:\hat{f}(n^{\ast })\le (1+ϵ)mi{n}_{n\in open}f(n)$
其中$f(n)$与A算法相同，$g(n)$代表当前结点n到起点的距离，$h(n)$代表预测当前节点n到终点的距离（启发式函数）

等同于$FOCAL$表将$OPEN$表中$f(n)$值小于等于$(1+ϵ)$倍最小$f$值的所有结点放在一起，形成一个次优集体，从中再选择出最适合的那个结点。
与A算法不同的是，A算法是通过$minf(n)$的结点作为下一个拓展结点，而$A_{ϵ}^{\ast }$算法是通过新的启发式函数$h_F(n)$作为规则，在$FOCAL$表中进行选择。$h_F$估计完成从结点n开始的搜索所需的计算量。（其具体含义根据后面面临的不同问题可以做出不同的设定）

总结：$A_{ϵ}^{\ast }$算法与A*算法的不同之处在于：
1.使用了参数数$ϵ$，从$OPEN$中选择$\hat{f}(n^{\ast })\le (1+ϵ)mi{n}_{n\in open}f(n)$的结点，形成新集合$FOCAL$，从中进行选择
2.使用了新的启发式函数$h_F(n)$，在$FOCAL$中作为评判标准选择拓展节点。$h(n)$用于作为构建$FOCAL$的评判标准，而$h_F(n)$作为从$FOCAL$中选择拓展节点的标准。

3. CBS算法

多智能体路径规划 (Multi-Agent Path Finding, MAPF) 研究多智能体的路径规划算法，为多机系统规划无冲突的最优路径．关于MAPF可以看这里多机器人路径规划(MAPF)综述

CBS(Conflict-Based Search) 是一种基于冲突的 MAPF 算法, CBS 算法给出 MAPF 问题的全局最优结果。参考文献为Conflict-based search for optimal multi-agent pathfinding，主要参考CBS(Conflict-Based Search)多机器人路径规划

CBS路径规划：行为上是一棵树的形式，算法的思想划分为两层，

1. 底层执行各个$agent$的单机最短路径规划，使用传统的A*算法
2. 顶层遍历底层的规划路径，检查路径是否有冲突，如果有冲突，如果有冲突的话就施加约束重新进行底层规划，直到所有底层路径没有冲突为止。

3.1 定义术语：

• $path$:一条机器人的路径，也就是单机规划的结果
• $solution$:所有agent的$path$集合，也就是mapf全局规划的结果
• $conflict$:冲突，在$solution$中，agent之间路径可能会发生冲突，具体描述形式为($a_i$,$a_j$,$v$,$t$),表示在$t$这个timestep，$a_i$和$a_j$同时占据了顶点$v$。
• $constraints$：约束，具体描述形式为($a_i$,$v$,$t$),表示在时刻$t$，$a_i$不能占据顶点$v$。
• $cost$：表示当前$solution$，每个$agent$的f(n)之和

3.2 顶层

CBS算法顶层使用约束树(Search the Constraint Tree (CT))数据结构来解决底层冲突，如下图所示：

其实就是一颗树(可以设计成二叉树或者多叉树)，树的每个节点除了有指向子节点的指针，还包括：
$constraints$：由 $conflict$得到
$solution$：目前结点的全局路径（可能包含冲突）
$cost$：目前结点的总代价

注意：次数的结点不是指A*算法中的agent节点，而是指CT树上面的结点:CT_node

3.3 生成树

生成树的过程就是从父节点生出子节点的过程，如下图所示
两个智能体$a_1$和$a_2$想分别从$S_1$和$S_2$出发到达$G_1$和$G_2$

在初始情况下，约束集为空，此时进行底层A*搜索，生成两个path
$pat{h}_1$:$<S_1,A_1,C,G_1>$
$pat{h}_2$:$<S_2,B_1,C,G_2>$
计算当前的$cost$=3+3=6(起点是不算的)
因此CT树的根节点$root$如图所示：

接下来：

1. 遍历当前结点的$solution$，搜索得到$paths$之间的第一个冲突
2. 解决冲突，两条路径在第2步时都到达了$C$，因此将冲突拆成两个约束$(a_1,C,2)$和$(a_2,C,2)$，分别表示$a_1$在第2步不能占据$C$和$a_2$在第2步不能占据$C$
3. 生成子节点，将结点$root$分为两个子节点，每个子节点都继承父节点原有的约束，并赋予第2步中独有的约束
4. 将子节点的约束施加到底层规划算法中，计算出新的$path$，合并成$solution$，如下图：
   
5. 计算每个子节点的全局$cost$，左右子节点均为7，由于搜索一般从左边开始，因此优先考虑左节点，此时左节点无冲突，则为全局最优路径，算法返回。

3.4 算法伪代码流程：

可以看出，在CBS的顶层也使用了$open$表，存放着CT树里的拓展结点，并且通过CT_node的$cost$值作为判断标准选择CT_node进行拓展。也就是伪代码里的$p$，然后对$p$进行冲突检测，如果$p$中没有冲突，则返回结果。否则记录下$p$中的第一个冲突，记录为$C$，对于冲突对中的每一个智能体进行增加$constraints$，并且赋给子节点$A$，随后$A$中对每一个agent进行A*算法求解$path$，最后得到$A$的$solution$并且计算$A$的$cost$，最后将A插入顶层的$open$表中

3.5 总结：

在CBS中，顶层设计采用的CT树的结构，并且也是通过设计$open$将拓展结点装入，并且通过CT_node的$cost$选择新拓展结点，与A*算法一致。

在底层中，通过新拓展节点$P$的$first\_conflict$给其子节点$A$添加分别对应的$constrain$，随后子节点再分别对每个$agent$进行路径规划，并且计算出$A$的$cost$。随后将$A$加入$open$中

4. ECBS算法

参考文献Suboptimal Variants of the Conflict-Based Search Algorithm for the Multi-Agent Pathfinding Problem主要参考ECBS多机器人路径规划

ECBS其实结构与CBS是一致的，CBS在顶层设计和底层设计中使用的是A*的策略，而ECBS在顶层和底层中使用的是$A_{ϵ}^{\ast }$的策略。

4.1 底层设计：

对于ECBS的底层设计，为$agent$ $a_i$搜索路径使用的$OPEN$表为$OPE{N}_i$，则$OPE{N}_i$中的最小$f$值$f_{min}$为t$a_i$最优路径搜索代价$cos{t}_i$的下界(Lower Bound)。对于一个CT树节点CT_node,设其为$n$，则$n$的$cost$的最优解$LB(n)=\sum _{i=1}^k{f}_{min}(i)$，其中$k$为$agent$的个数

在底端对$a_i$使用$A_{ϵ}^{\ast }$算法，设置参数$ϵ$，将满足$\hat{f}(n^{\ast })\le (1+ϵ)mi{n}_{n\in open}f(n)$的路径$pat{h}_{n^{\ast }}$装入$FOCA{L}_i$，随后设置$h_c$，其表示返回与先前分配的其他代理冲突最少的路径。

例如为$a_1$分配了路径$(S_1,A,G)$，那么为$a_2$分配道路时就要优先考虑避免第1步中位置$A$以外的其他位置。

4.2 顶层设计：

在ECBS的顶层同样采取$A_{ϵ}^{\ast }$的算法，每个CT_node都返回两个值给顶层算法

• $n.cost$
• $LB(n)$

其中$n.cost$为CT树节点$n$的代价$cost$，$LB(n)$为$n$的理想状态的代价下界
设$LB=min(LB(n)|n\in OPEN)$，此时的$OPEN$为顶层搜索的$OPEN$表，则$LB$显然是全局最优代价$C^{\ast }$的下界。因此顶层的$FOCAL$表的设计为
F O C A L = { n ∣ n ∈ O P E N , n . c o s t ≤ L B ⋅ w } FOCAL=\lbrace n|n\in OPEN,n.cost\leq LB\cdot w \rbrace FOCAL={n∣n∈OPEN,n.cost≤LB⋅w}

而顶层设计也同样拥有$h_c$，其含义为当前分支节点中至少有一个冲突的$agent$对的数量(即$agent\_pairs$)，例如当前结点$n$存在$conflict$ $(a_1$,$a_2$,$1$,$1)$和$(a_2$,$a_3$,$3$,$2)$，则存在冲突对为$(a_1$,$a_2)$,$(a_2$,$a_3)$,$h_c(n)=2$

其余算法逻辑与CBS一致。

4.3 总结：

顶层和底层都使用了$A_{ϵ}^{\ast }$算法，其中底层的$h_c$表示返回与先前分配的其他代理冲突最少的路径。顶层的$h_c$表示当前分支节点中至少有一个冲突的$agent$对的数量