多智能体连续时间内路径规划：AA-SIPP(m) 的 Rust 实现与算法

niyongsheng
0 阅读12分钟

从搜索状态、开放列表、关闭列表到 SIPP 的安全区间，拆解 AA-SIPP(m) 的数据结构与算法主体。从安全区间、any-angle 重连到连续时间碰撞检测，拆解一个优先规划 MAPF 规划器的内部实现。

引言：50 台 AGV，同一张地图

一个 64×64 的仓库，50 台 AGV 共享同一张栅格地图。每台车有自己的半径、平移速度、旋转速度和初始朝向；地图上还有货架、墙体，以及按固定时刻表穿行的叉车。

给每台车单独跑 A*，会得到 50 条各自最优但彼此冲突的路径。多智能体路径规划（MAPF）要保证任意时刻任意两个智能体的身体都不重叠。工程上常见的做法是 prioritized MAPF：按优先级逐个规划，每规划完一个，就把它的轨迹注册成移动障碍物，后面的智能体绕开它。

本项目实现的是 ICAPS 2017 的 AA-SIPP(m)。本文内容包括搜索状态与容器怎么设计，以及单智能体搜索和多智能体循环怎么跑。不展开 XML、可视化和完整验证流程。

A. Andreychuk, K. Yakovlev. Any-Angle Pathfinding for Multiple Agents Based on SIPP Algorithm. ICAPS 2017. arXiv:1703.04159

32 个智能体在 32×32 栅格上的规划轨迹

一、核心数据结构设计

1.1 搜索状态：不是 (x, y, t)，而是 (cell, interval, heading)

传统时间展开 A* 把状态定义成 (cell, t)，t 取离散步长。问题是时间上界未知：智能体可能要等很久才能等到动态障碍物离开；上界取小了漏解，取大了状态爆炸。

SIPP 把时间压缩成安全区间。一个状态是：

(cell, safe_interval, heading)

对应到代码：

pub struct Node {
    pub cell: Cell,          // 所在栅格
    pub g: f64,              // 绝对到达时间，不是距离
    pub f: f64,              // g + h，只用于排序
    pub heading: f64,        // 到达时的朝向
    pub interval: SafeInterval,
    pub parent: Option<NodeId>,
}

pub struct SafeInterval {
    pub begin: f64,
    pub end: f64,
    pub id: u32,             // 该区间在格子内的编号
}

三个细节决定了它和普通 A* 的不同：

  1. g 是时间。平移耗时 距离 / mspeed，转向耗时 最短弧 / (rspeed × 180)；路径代价就是到达时间。
  2. 区间是闭区间，[b, b] 这种零长度区间合法且可用；“恰好相切不算碰撞”的语义就藏在这里。
  3. 目标格必须有一段 end == ∞ 的区间。到达目标还不够，必须能永久停在那里；如果目标格之后会被别的智能体占用，它就不能当终点。

SafeInterval 只是“这个格子什么时候安全”。真正描述“谁在什么时候占着哪里”的是 Section：

pub struct Section {
    pub from: Cell,
    pub to: Cell,
    pub size: f64,   // 对方身体的半径
    pub g1: f64,     // 进入时间
    pub g2: f64,     // 离开时间；∞ 表示永久停驻
    pub mspeed: f64,
}

一段 Section 是一个半径 size 的圆盘，在 [g1, g2] 内从 from 匀速直线运动到 to。已规划智能体的轨迹和动态障碍物都走这套表示；g2 = ∞ 就是“停在这里不走了”，等待无法解决，只能绕路。

1.2 开放列表：按行分桶，而不是小根堆

这是整个实现里最反直觉的数据结构。它没有用 BinaryHeap，而是给每一行栅格维护一个有序 Vec：

pub struct OpenList {
    rows: Vec<Vec<Node>>,   // 一行一个桶
    count: usize,
}

每次从每行的头部取一个候选，再线性扫描所有行选最小：

fn find_min_row(&self) -> Option<usize> {
    let mut best = None;
    for (idx, row) in self.rows.iter().enumerate() {
        let Some(head) = row.first() else { continue };
        match best {
            None => best = Some((idx, head.f, head.g)),
            Some((_, cur_f, cur_g)) => {
                if head.f - EPSILON < cur_f {
                    if (head.f - cur_f).abs() < EPSILON {
                        if cur_g < head.g {
                            best = Some((idx, head.f, head.g));
                        }
                    } else {
                        best = Some((idx, head.f, head.g));
                    }
                }
            }
        }
    }
    best.map(|(idx, _, _)| idx)
}

规则是：f 明显更小直接赢；f 落在 EPSILON 带内，g 更大者赢；否则保留当前最优。

为什么不能换成堆？因为“f 落在 epsilon 带内时选 g 大的”这个关系不是传递的。取 EPSILON = 1e-9：

条目fg
A1.0000000005
B1.000000000610
C1.000000001220

A 和 B 的 f 差 0.6e-9 < EPSILON，B 的 g 更大，B 胜过 A；B 和 C 同理，C 胜过 B；但 A 和 C 的 f 差 1.2e-9 > EPSILON，A 的 f 明显更小，A 胜过 C。也就是 A > C > B > A，形成环。BinaryHeap 需要全序的 Ord，无法表达这种带容差的偏好；硬编码成 (f, -g) 的字典序，会选出参考规则下不会被选中的节点，搜索顺序、平局和最终路径都会变。

同一个格子、同一段区间内，插入时还有支配检查：如果已有条目的 g 更小，新条目被丢弃；如果新条目更优，替换旧条目。没有 decrease-key，也没有在弹出时做惰性删除。

1.3 关闭列表：arena + NodeId

关闭列表不用哈希表，而是 arena：

pub struct SearchTree {
    records: Vec<NodeRecord>,
    by_cell: Vec<Vec<NodeId>>,  // 每个格子上的节点 id
    width: i32,
}

pub struct NodeId(pub u32);

insert 把节点放进 records，返回它的 NodeId；后继的 parent 存这个 id，而不是指针或哈希键。by_cell 记录每个格子上出现过的节点 id，用于“这个格子已经被哪些状态关闭过”的支配检查。

这样做的好处是确定性：同一个输入在同一台机器上给出同一条路径，跨平台也一致。哈希表 + 裸指针的写法会遇到“同键取任意元素”和“父指针短暂指向栈上局部变量”的问题；arena 把父子关系变成精确的索引，不存在这些歧义。

1.4 约束表：sections 累积，安全区间惰性派生

Constraints 维护两份按格索引的数据：

pub struct Constraints {
    sections: Vec<Vec<Section>>,             // 每格的运动段
    cell_states: Vec<Option<Vec<SafeInterval>>>, // 每格的安全区间
    // ...
}
  • sections 随智能体累积：每规划完一个智能体，它的轨迹就被注册到扫过的每一个格子；一次尝试内不删除（起点预留是例外）。
  • 安全区间每个 agent、每个格子最多推导一次。这不是缓存技巧：区间有 id，重新推导会重新编号，而开放列表里排队的节点还拿着旧 id，状态身份会乱。
  • 推导时，同一段 Section 可能被 push 到多个格子的列表里；去重依赖 Section 的粗粒度相等——只比较 from 和 g1。改成全字段比较会让副本无法折叠，同一个阻塞窗口被减多次。

Rust 侧没有照搬嵌入式 A* 的固定 Node 内存池。它的内存策略是：Planner 跨智能体复用 OpenList 和 SearchTree，clear 保留容量；Constraints 每次尝试重建，但按格预分配 Vec；核心算法不使用 unsafe，也不依赖第三方 crate。对服务器/桌面场景，这比固定池更简单；对 MCU 场景，固定池仍然有它的价值，但那是另一个取舍。

复杂度上，开放列表的选择是 O(高度) 的线性扫描，插入/删除是 O(桶长度)；安全区间每个 agent 每格最多推导一次；关闭集支配检查只扫描该格上已关闭的节点。真正的成本大头在 find_intervals：每个候选区间、每个等待步都要重新检查走廊上的所有 section。

二、算法主体

2.1 两层循环：优先规划 + 单智能体 SIPP

高层 Solver::solve：

order = 初始优先级（FIFO / 最短优先 / 最长优先 / 随机）
loop:
    constraints = 动态障碍物 + 所有智能体的起点预留
    for id in order:
        移除 id 自己的起点预留
        result = 单智能体 AA-SIPP(id, constraints)
        if not result.found: 记录失败者，break
        把 result 的轨迹注册成新的 Section
    if 所有智能体都成功: return result
    if 无法重排 or 超过重试上限: return failure
    按规则调整 order

高层只负责“排序”和“把已解轨迹变成约束”，不做冲突消解。某个智能体失败，整次尝试作废，重新建约束、重新规划所有智能体。重排规则有 no、rulebased、random；重试上限 100_000。

两个容易忽略的约束：

  • 起点预留：每次尝试开始时，先给所有智能体的起点 footprint 注册 [0, start_safe_interval] 的静止 section；轮到某个智能体规划时，再移除它自己的预留，否则会挡住自己。
  • 停车约束：每个智能体到达终点后留下 g2 = ∞ 的停车 section，后来的智能体不能把别人的终点当路过格，只能绕开。

低层 Planner::find_path：

current = start 节点（g=0, interval=start 格的第一段安全区间）
open = { current }
goal = None
loop:
    if open 为空: break
    if current 在目标格 且 current.interval.end == ∞:
        记录 goal（若需要终点朝向，加上转向成本）
    if goal 已存在 且 goal.f - EPSILON < current.f: break
    current = open.pop_min()
    closed.insert(current)
    for 每个后继: open.add(后继)

注意：AA 是 any-angle，不是 anytime。没有“找到解后继续改进”的机制，也没有次优界；返回第一次可行解，只有失败才重启。

2.2 启发函数：欧氏、曼哈顿、octile

f = g + h，g 是时间，h 是剩余时间的下界。启发式按配置切换：

  • any-angle 开启，或 connectedness > 3：欧氏距离 / mspeed；
  • connectedness == 2：曼哈顿距离 / mspeed；
  • connectedness == 3：octile 距离 / mspeed。
欧氏：sqrt(di² + dj²)
曼哈顿：|di| + |dj|
octile：(max - min) + sqrt(2) × min

八方向移动的对角代价是 sqrt(2)，所以这里用 octile，而不是切比雪夫。切比雪夫假设对角和直行同价，会高估剩余代价，破坏可采纳性。

启发式忽略转向和等待。两者成本都非负，忽略只会低估剩余代价，所以仍然是可采纳的；如果为了“更准”把转向估进去，就可能高估，A* 的最优性不成立。

2.3 后继生成与 any-angle

移动表由 connectedness 决定：

connectedness偏移数说明
24上下左右
38加四条对角
416加“马步”偏移
532再加“骆驼步”偏移

普通后继的代价顺序是：先在父格原地转向，再平移。

heading  = 从 current.cell 到 target 的方位角
pivot.g  = current.g + rotation(current.heading, heading) + additional_wait
child.g  = pivot.g + move.length / mspeed

child.g 只是解析到达时间；真正的到达时间由 find_intervals 决定，可能会因为等待而往后推。最终返回的 EAT 覆盖解析的 g。

any-angle 的唯一机制是祖父重连。展开 current 时，取祖父 gp（current.parent 的父节点）；对每个候选目标 target，如果：

  1. target != gp.cell；
  2. gp.cell != current.cell；
  3. check_line(gp.cell, target) 通过；

就从 gp 直接连一条边到 target，把 target 的父节点设为 gp。

普通路径:   gp ── current ── target
any-angle:  gp ─────────────── target

这不是任意角度采样，也不是后处理平滑；动作集仍然是移动表里的 8/16/32 个偏移，只是边的起点从父节点换成了祖父节点。

后继列表在返回前会反转一次，模拟参考实现的 push_front。顺序很重要：一条直线上的格子既可以逐格走，也可以被 any-angle 折叠成一条长边，两者总时间往往完全相同；谁先进入开放列表，谁就决定最终路径的形状。

2.4 安全区间推导与连续时间碰撞检测

对格子 c 和一段 Section，先算格心到线段的距离 d。如果 d >= r（r = 智能体半径 + section 半径），这段 section 永远碰不到这个格子。否则，圆盘覆盖格心的时间窗由半弦决定：

half = sqrt(r² - d²)
进入时间 = 圆心到达投影点的时间 - half / speed
离开时间 = 圆心到达投影点的时间 + half / speed

把每个 section 产生的阻塞窗口从 [0, ∞] 里减掉，剩下的就是安全区间。窗口盖住开头就裁掉开头，盖住结尾就裁掉结尾，落在中间就把区间劈成两段。阻塞窗口正好从区间起点开始时，代码会保留一个退化的 [b, b] 区间：可以在阻塞窗口开启的瞬间到达，因为开圆盘“恰好相切”不算碰撞。

边是否真的能走，还要做连续时间碰撞检测。两个匀速圆盘的相对位置是 w - v·t，临界条件是：

|w - v·t|² = r²
(v·v)t² - 2(w·v)t + (w·w - r²) = 0

判别式 dscr = (w·v)² - (v·v)(w·w - r²)；首次接触时间 t = ((w·v) - sqrt(dscr)) / (v·v)。代码先判断 c = |w|² - r² < 0（已经重叠），再判断 dscr <= 0（永远不会接触），最后看首次接触时间是否落在两段轨迹的时间重叠窗口内。

碰撞分两类：Transient（g2 有限，等待可以解决）和 AtGoal（g2 = ∞，等待无效，只能换路）。等待只在父格进行，按 1.0 个时间单位步进；这也是“连续时间”里唯一被量化的部分——运动与转向连续，等待不是。

2.5 路径重建

搜索树只记录到达时刻；日志需要的是 <section> 列表。重建时：

  1. 从 goal 沿父链回溯到起点；
  2. 如果终点朝向重要，先把目标判定时加上的转向成本减掉，再补一个“只转向、不移动”的节点；
  3. 对每对相邻节点，如果时间差大于纯移动时间，就在父节点位置插入一个合成的静止 waypoint，时间设为 child.g - 移动时间。

这样，等待和原地转向都会变成独立的 <section>，各段 duration 加起来才对得上总时间。

另外还有一条用于渲染的 secondary path：沿父链逐段做 Bresenham，收集经过的格子。它不包含每段的终点，所以栅格图永远不标记目标格。这是从参考实现继承的可见行为，修掉会改变日志。

50 个智能体在 64×64 栅格上的规划轨迹

三、仓库

项目地址：github.com/niyongsheng…
在线报告：niyongsheng.github.io/SmartPather…

未发布到 crates.io，GitHub 引入：

[dependencies]
smart-pather = { git = "https://github.com/niyongsheng/SmartPather" }
avatar
文章
阅读
粉丝