1 背景

1.1 问题描述

多智能体路径寻优( Multi-Agent Path Finding，MAPF )问题由一个无向无权图G = ( V ，E )和一组k个智能体 ${a_{1},...,a_{k}}$ 组成，其中智能体 $a_{i}$ 有起始点 $s_{i} \in V$ 和目标点 $g_{i} \in V$ 。时间被离散化为时间步。在连续的时间步之间，每个智能体既可以移动到相邻的顶点，也可以在当前顶点处等待。除智能体在其目标顶点处进行最终的等待外，移动和等待动作都具有单位时间成本。 $a_{i}$ 的一条路径是将ai从 $a_{i}$ 引导到 $g_{i}$ 的一系列移动和等待动作。元组 $<a_{i},a_{j},v,t>$ 表示节点冲突，当且仅当 $a_{i}$ 和 $a_{j}$ 在t时刻处于同一个顶点v；元组 $<a_{i},a_{j},u,v,t>$ 表示边冲突，当且仅当 $a_{i}$ 和 $a_{j}$ 在时间步t和t + 1之间沿相反方向遍历同一条边 $(u,v)$ 。研究的目标是找到一组无冲突的路径，使得所有的智能体从起始点移动到目标点，同时最小化这些路径的总时间。

from typing import Tuple
import numpy as np


class Agent:

    def __init__(self, start: Tuple[int, int], goal: Tuple[int, int]):
        self.start = np.array(start)
        self.goal = np.array(goal)

    # Uniquely identify an agent with its start position
    def __hash__(self):
        return int(str(self.start[0]) + str(self.start[1]))

    def __eq__(self, other: 'Agent'):
        return np.array_equal(self.start, other.start) and \
               np.array_equal(self.goal, other.goal)

    def __str__(self):
        return str(self.start.tolist())

    def __repr__(self):
        return self.__str__()

1.2 基于冲突的搜索（CBS）

CBS有两个层次。下层为个体Agent寻找最优路径。如果路径发生冲突，上层通过分裂行动对冲突主体施加约束以避免这些冲突。CBS的上层是通过搜索约束树( CT )来进行的,CT是一棵二叉树，其中每个节点N包含：

(1) 一组约束 N.constraints：约束要么是顶点约束 $<a_{i},v,t>$ 用于禁止智能体 $a_{i}$ 在t时刻处于顶点v，要么是边约束 $<a_{i},u,v,t>$ 用于禁止智能体 $a_{i}$ 在时间步t和t + 1之间从顶点u移动到顶点v；

from typing import Dict, Tuple, Set
from copy import deepcopy

from .agent import Agent

'''
Emulated dictionary of dictionaries
'''
class Constraints:

    def __init__(self):
        #                                   time,         obstacles
        self.agent_constraints: Dict[Agent: Dict[int, Set[Tuple[int, int]]]] = dict()

    '''
    Deepcopy self with additional constraints
    '''
    def fork(self, agent: Agent, obstacle: Tuple[int, int], start: int, end: int) -> 'Constraints':
        agent_constraints_copy = deepcopy(self.agent_constraints)
        for time in range(start, end):
            agent_constraints_copy.setdefault(agent, dict()).setdefault(time, set()).add(obstacle)
        new_constraints = Constraints()
        new_constraints.agent_constraints = agent_constraints_copy
        return new_constraints

    def setdefault(self, key, default):
        return self.agent_constraints.setdefault(key, default)

    def __getitem__(self, agent):
        return self.agent_constraints[agent]

    def __iter__(self):
        for key in self.agent_constraints:
            yield key

    def __str__(self):
        return str(self.agent_constraints)

(2)一个解 N.solution：由k条成本最小路径组成，其中每个智能体有一条，且都满足N.constraints;

(3)成本N.cost：即N.solution中路径的成本之和。

from typing import Dict
import numpy as np

from .agent import Agent
from .constraints import Constraints

class CTNode:

    def __init__(self, constraints: Constraints,
                       solution: Dict[Agent, np.ndarray]):

        self.constraints = constraints
        self.solution = solution
        self.cost = self.sic(solution)

    # Sum-of-Individual-Costs heuristics
    @staticmethod
    def sic(solution):
        return sum(len(sol) for sol in solution.items())

    def __lt__(self, other):
        return self.cost < other.cost

    def __str__(self):
        return str(self.constraints.agent_constraints)

当CBS选择一个CT节点N进行扩展时，它检查N.solution中的冲突。如果没有，CBS终止并返回N.solution。否则，CBS选择其中一个冲突(默认情况下,任意)，通过将N拆分为两个子CT节点来解决，CT树子节点继承父节点的约束。在每个子CT节点中，来自冲突的一个智能体通过增加约束的方式被禁止使用冲突的节点或边，该智能体的路径不再满足CT树子节点的约束，必须通过低层搜索(例如,时空A *搜索[1])重新规划，其他智能体的所有路径保持不变。如果低层搜索无法找到满足约束条件的路径，则该子CT节点没有解，因此进行剪枝。每个冲突有两个子CT节点，CBS通过探索解决每个冲突的两种方式来保证最优性。

时空A*搜索的第三方库：

pip3 install space-time-astar

导入：

from stastar.planner import Planner

1.3 改进CBS

CBS任意选择冲突进行拆分，然而，不好的选择将大幅增加CT的结点数，从而增加运行时间。

1.3.1 ICBS[2]

2015年，Boyarski提出ICBS，通过对每个CT节点处的冲突进行优先级排序来解决这个问题。他将冲突节点分为3种类型:

(1) cardinal 冲突 :对于CT节点N，一个冲突C =〈a1，a2，v，t〉，如果在N中添加由C (〈a1 , v , t〉,〈a2 , v , t〉)导出的两个约束中的任意一个，并调用约束Agent上的低层，其路径成本相对于N中的成本增加，则称之为cardinal冲突。

(2) semi-cardinal 冲突: 如果添加由C导出的两个约束中的一个增加了N . cost，而其他叶子N . cost不变，则冲突C是semi-cardinal 冲突。

(3) non-cardinal 冲突: 如果由C导出的约束都不增加N . cost，则冲突是non-cardinal 冲突。

当选择具有路径成本c节点N进行CBS扩展时，首先检查它的所有冲突。如果遇到cardinal冲突，则立即进行拆分动作。这会产生两个成本> c的子节点。在这种情况下，如果OPEN中的另一个节点N′具有成本c，那么可以立即选择N′进行下一步的扩展，而不需要进一步开发N以下的节点。如果不存在cardinal冲突，下一个偏好是选择semi-cardinal冲突。在这里，我们知道至少一个子节点的成本会立刻增加。如果只存在non-cardinal冲突，则任意选择一个。

1.3.2 CBSH

CBS的上层总是选择具有最小N.cost的节点来扩展CT节点。CBSH通过增加一个可接受的启发式来加速上层搜索，具体思路如下：

如果N.solution包含一个cardinal冲突，那么令h值为1是允许的，因为它的任何一个具有无冲突解的后代CT节点的cost至少为N.cost+1,如果N.solution包含多个基数冲突，则CBSH构建一个冲突图，其顶点为多智能体，边表示N.solution中的cardinal冲突，每个cardinal冲突至少有一个智能体的路径成本至少增加1，于是，冲突图的最小顶点覆盖（minimun vertex cover, MVC）的大小是节点N的容许h值，这种启发式也称之为CG启发式。

文献参考

[1] Silver, D., 2021. Cooperative Pathfinding. Proc. AAAI Conf. Artif. Intell. Interact. Digit. Entertain. 1, 117–122. https://doi.org/10.1609/aiide.v1i1.18726

[2] Boyarski, E., n.d. ICBS: The Improved Conflict-based Search algorithm for Multi-Agent Pathfinding: Extended Abstract.