第276篇多机器人路径协调——冲突避免和死锁处理

郑伟强dev

113人浏览 · 2026-08-27 06:52:18

郑伟强dev · 2026-08-27 06:52:18 发布

上篇讲了多机器人系统架构的三个维度——通信、协调、任务分配。这篇深入聊路径协调，这是多机器人系统里最容易出问题的环节。

几十台机器人在同一空间运动，路径冲突是必然的。两台机器人同时要到同一个路口，谁先走？四台机器人在十字路口互相堵住怎么办？一台机器人坏了停在路中间，其他机器人怎么绕行？

这些问题不解决，多机器人系统就只是一堆各自为政的单机机器人。路径协调的质量直接决定了系统的整体效率。

一、路径冲突的类型

多机器人路径冲突主要有三种：

顶点冲突——两台机器人在同一时刻到达同一个位置。最直接的碰撞。

边冲突——两台机器人在同一时刻沿同一条边相向而行。头对头撞上。

追尾冲突——后面的机器人速度比前面快，在同一时刻到达同一位置。

def check_conflict(path_a, path_b, time_step):
    """检查两条路径是否有冲突"""
    for t in range(max(len(path_a), len(path_b))):
        pos_a = path_a[min(t, len(path_a)-1)]
        pos_b = path_b[min(t, len(path_b)-1)]
        
        # 顶点冲突
        if pos_a == pos_b:
            return True, f"t={t}: 顶点冲突 at {pos_a}"
        
        # 边冲突（交换位置）
        if t > 0:
            prev_a = path_a[min(t-1, len(path_a)-1)]
            prev_b = path_b[min(t-1, len(path_b)-1)]
            if pos_a == prev_b and pos_b == prev_a:
                return True, f"t={t}: 边冲突"
    
    return False, "无冲突"

二、集中式路径规划：CBS算法

Conflict-Based Search（CBS）是集中式多机器人路径规划的经典算法。

思路是：先给每台机器人独立规划最短路径，检查有没有冲突。如果有冲突，加约束重新规划——"机器人A在t=5时不能出现在位置X"或者"机器人B在t=5时不能从X走到Y"。

def cbs(robots, map):
    # 1. 独立规划
    paths = {}
    for robot in robots:
        paths[robot.id] = a_star(robot.start, robot.goal, map)
    
    # 2. 检测冲突
    root = Node(paths, constraints=[])
    open_list = [root]
    
    while open_list:
        node = pop_best(open_list)
        conflict = detect_conflict(node.paths)
        
        if not conflict:
            return node.paths  # 找到无冲突解
        
        # 3. 分支：给冲突的两台机器人各加一条约束
        robot_a, robot_b, time, location = conflict
        
        # 分支1：约束机器人A
        child1 = copy(node)
        child1.add_constraint(robot_a, location, time)
        child1.replan(robot_a)
        open_list.append(child1)
        
        # 分支2：约束机器人B
        child2 = copy(node)
        child2.add_constraint(robot_b, location, time)
        child2.replan(robot_b)
        open_list.append(child2)

CBS保证找到全局最优解（总路径代价最小），但计算量随机器人数量指数增长。适合小规模（10-20台）的场景。

三、分布式路径协调：优先级规划

大规模场景下，集中式算法算不动。分布式方案更实用——给每台机器人分配一个优先级，高优先级的机器人先规划路径，低优先级的把高优先级的路径当作动态障碍物来避让。

def priority_planning(robots, map):
    # 按优先级排序
    robots.sort(key=lambda r: r.priority, reverse=True)
    planned_paths = {}
    
    for robot in robots:
        # 已规划的路径作为动态障碍
        dynamic_obstacles = list(planned_paths.values())
        path = a_star_with_avoidance(
            robot.start, robot.goal, map, 
            dynamic_obstacles
        )
        planned_paths[robot.id] = path
    
    return planned_paths

这种方案计算快，但不保证最优——低优先级的机器人可能绕很大的弯。而且优先级分配会影响结果质量。

工程上的折中方案：把机器人分成几组，组内用集中式（CBS），组间用优先级。这样计算量和解质量都能接受。

四、死锁检测与解除

死锁是多机器人系统的噩梦。最简单的死锁：两台机器人在窄走廊两端面对面，谁也不让谁。

死锁检测——如果多台机器人在连续N个时间步内都没有移动（或者移动距离极小），就判定为死锁。

死锁解除——检测到死锁后，选一台机器人让它退让（倒车到最近的避让点），让其他机器人先通过。

def detect_deadlock(robots, threshold=10):
    """检测死锁：连续N步没有移动的机器人"""
    stuck = []
    for robot in robots:
        if robot.steps_without_moving >= threshold:
            stuck.append(robot)
    
    if len(stuck) >= 2:
        # 检查是否互相阻塞
        for i in range(len(stuck)):
            for j in range(i+1, len(stuck)):
                if are_blocking(stuck[i], stuck[j]):
                    return True, (stuck[i], stuck[j])
    return False, None

def resolve_deadlock(robot_a, robot_b):
    """解除死锁：优先级低的退让"""
    if robot_a.priority < robot_b.priority:
        robot_b.reverse_to_waiting_spot()
    else:
        robot_a.reverse_to_waiting_spot()

更复杂的死锁（比如4台机器人在十字路口互相堵住）需要更智能的解除策略——可能需要多台机器人同时退让，或者重新规划所有涉及的路径。

有些系统会设置一个"死锁解除专员"角色——专门的进程负责监控全局死锁状态，一旦检测到死锁就强制介入。它有权暂停某些机器人的运动、重新分配路径优先级、甚至临时修改地图（标记某些通道为单向通行）。这种集中式的死锁管理在实际项目中效果很好。