两个智能体各自找到最短路，合起来仍可能撞在同一格或沿同一条边迎面交换。教学小实例可以把二人的位置合成一个状态，再用广度优先搜索。下面采用 4×4 无障碍离散网格，允许等待，禁止占同格和对向交换。
先确定规划假设
时间是统一的整数步，两人每步最多移动一个上下左右格，也可以等待。目标是找到双方同时位于各自终点的最少联合步数；不优化每个人路径长度之和。到达一个人的目标后仍允许该人暂时离开，只把双方同时到达判为完成。冲突种类和目标后行为会影响答案，必须先写明。
起点为 ((0,0),(0,1))，终点为 ((0,1),(0,0))，二人希望交换位置。直接一步交换被禁止，网格中需要绕行。Stern 等人的 MAPF 术语综述区分了占格冲突、交换冲突和不同优化目标；本例按上述特定假设实现。
完整联合状态搜索
保存 joint_bfs.py，运行 python joint_bfs.py。每个队列元素是两个人的位置元组；父节点字典用于还原路径。若换成有障碍网格，需要在 moves 中加入障碍检查，不能只画障碍图而不改转移。
from collections import deque
from itertools import product
N = 4
start = ((0, 0), (0, 1))
goal = ((0, 1), (0, 0))
def moves(pos):
x, y = pos
return [(x+dx, y+dy) for dx, dy in [(0,0),(1,0),(-1,0),(0,1),(0,-1)]
if 0 <= x+dx < N and 0 <= y+dy < N]
queue = deque([start])
parent = {start: None}
while queue and goal not in parent:
old = queue.popleft()
for nxt in product(moves(old[0]), moves(old[1])):
if nxt[0] == nxt[1] or (nxt[0] == old[1] and nxt[1] == old[0]):
continue
if nxt not in parent:
parent[nxt] = old
queue.append(nxt)
if goal not in parent:
raise RuntimeError("no joint path")
path, state = [], goal
while state is not None:
path.append(state)
state = parent[state]
path.reverse()
for t, state in enumerate(path):
print(t, state)
for old, nxt in zip(path, path[1:]):
assert nxt[0] != nxt[1]
assert not (nxt[0] == old[1] and nxt[1] == old[0])
assert all(abs(a[0]-b[0])+abs(a[1]-b[1]) <= 1 for a,b in zip(old,nxt))
assert path[0] == start and path[-1] == goal
print("joint steps:", len(path)-1)核对输出与使用边界
输出每行是一个时间点的联合位置。首尾必须与设定一致，连续行每个人只移动一格或等待，二人不能占同格也不能交换原位置。BFS 在这个有限、等步长联合图上找到最少联合步数；队列选择顺序可能得到不同的同长路径。
联合状态空间随人数快速增长，本例不能当大规模仓储调度器。它也没有车辆尺寸、加速度、通信延迟或现实安全距离。若规划失败，应检查是否禁掉所有通路、起终点是否合法，以及目标停留规则，而不是删除冲突检查。下一步可添加一个明确障碍集合，用已知可解和不可解的网格验证搜索。
资料与核对依据
Ai菜鸟网。发布者：AI小管家，转载请注明出处：https://www.alyyhw.com/33012.html
