1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126

from heapq import heappush, heappop
from typing import Dict, List, Set, Tuple
class Constraint:
    """约束类"""
    def __init__(self, agent: int, location: Tuple, time: int):
        self.agent = agent
        self.location = location
        self.time = time
class Conflict:
    """冲突类"""
    def __init__(self, agent1: int, agent2: int, location: Tuple, time: int):
        self.agent1 = agent1
        self.agent2 = agent2
        self.location = location
        self.time = int
class CTNode:
    """约束树节点"""
    def __init__(self):
        self.constraints: Set[Constraint] = set()
        self.solution: Dict[int, List] = {}  # agent -> path
        self.cost: float = float('inf')
        def __lt__(self, other):
        return self.cost < other.cost
def cbs(agents: List, starts: Dict, goals: Dict, graph) -> Dict:
    """CBS 主算法"""
    # 1. 初始化根节点
    root = CTNode()
    for agent in agents:
        root.solution[agent] = astar_constrained(
            starts[agent], goals[agent], graph, root.constraints, agent
        )
    root.cost = compute_sic(root.solution)
        # 2. 初始化 OPEN 列表
    open_list = []
    heappush(open_list, root)
        # 3. 主循环
    while open_list:
        node = heappop(open_list)
                # 4. 检测冲突
        conflict = find_first_conflict(node.solution)
                if conflict is None:
            return node.solution  # 找到无冲突解
                # 5. 分裂节点
        for agent in [conflict.agent1, conflict.agent2]:
            # 创建新约束
            new_constraint = Constraint(
                agent, conflict.location, conflict.time
            )
                        # 创建子节点
            child = CTNode()
            child.constraints = node.constraints | {new_constraint}
            child.solution = node.solution.copy()
                        # 重新规划受影响智能体的路径
            new_path = astar_constrained(
                starts[agent], goals[agent], graph, 
                child.constraints, agent
            )
                        if new_path is not None:
                child.solution[agent] = new_path
                child.cost = compute_sic(child.solution)
                heappush(open_list, child)
        return None  # 无解
def astar_constrained(start, goal, graph, constraints, agent):
    """带约束的 A* 搜索"""
    open_list = [(heuristic(start, goal), 0, start, [])]
    closed = set()
        while open_list:
        f, g, current, path = heappop(open_list)
                if current == goal:
            return path + [current]
                state = (current, len(path))
        if state in closed:
            continue
        closed.add(state)
                for neighbor in graph.neighbors(current):
            # 检查约束
            if is_constrained(agent, neighbor, len(path) + 1, constraints):
                continue
                        new_g = g + graph.cost(current, neighbor)
            new_f = new_g + heuristic(neighbor, goal)
            heappush(open_list, (new_f, new_g, neighbor, path + [current]))
        return None
def find_first_conflict(solution: Dict) -> Conflict:
    """检测第一个冲突"""
    # 获取最大路径长度
    max_time = max(len(path) for path in solution.values())
        for t in range(max_time):
        # 检测顶点冲突
        positions = {}
        for agent, path in solution.items():
            pos = path[min(t, len(path) - 1)]  # 终点等待
            if pos in positions:
                return Conflict(agent, positions[pos], pos, t)
            positions[pos] = agent
                # 检测边冲突
        # ... (略)
        return None
def compute_sic(solution: Dict) -> float:
    """计算 Sum of Individual Costs"""
    return sum(len(path) - 1 for path in solution.values())