数据结构与算法 - RVO

2026-05-21

全文 ≈ 1.7k字 | 预计阅读时长 6分钟

RVO（Reciprocal Velocity Obstacles，互惠速度障碍）是一种多人动态避障算法。

「数据结构与算法 - JPS 」兄弟篇

前言

如果 A*、JPS 等寻路算法解决的是“如何到达目的地”，那么 RVO 解决的就是“在移动过程中，如何避免与动态目标发生碰撞”。常用于游戏中多单位在移动过程中实现自动避让，比如：红警、星际争霸等RTS游戏。

可以通俗的理解为：每个移动的单位都会主动互相让一点，避免路径撞车。

这类动态避障算法的核心思想是：“按照当前速度继续移动，未来会不会发生碰撞？”，也就是 Velocity Obstacle（速度障碍）。

设想以下场景，

张三和路人甲正在骑行道上相向而行，不出意外的话，待会可能就要出意外了。

不过当对方进入可视范围后，张三会下意识的考虑是否需要减速或稍微改变既定方向进行避让，从而避免意外。

这是大脑做出的预测行为，而 VO 就是对这种未来碰撞预测的数学模型，RVO、ORCA则是基于它的具体算法实现。

Velocity Obstacle

顾名思义，VO 中的“障碍”指的不是当前位置上的障碍物，而是会导致未来发生碰撞的速度集合。

通俗地说，VO 关心的不是现在离得近不近，而是：“如果双方都按照当前速度继续运动，未来是否会发生碰撞？”。

举个例子，

如果张三和路人甲并排但朝着同一方向行驶，即使当前位置很接近，在未来一段时间也未必会发生碰撞。
相反，
张三和路人相向行驶，即使当前距离很远，但如果双方正以相对速度高速接近，那么在未来一段时间内就存在碰撞风险。

所以，VO 关注的其实是：当前速度是否会把自己带入未来的碰撞轨迹。

从这个角度来看，刚好可以把速度分为两类：

有撞车风险的速度 -> 危险速度

无撞车风险的速度 -> 安全速度

而 VO 就是把速度空间中未来会发生碰撞的速度圈定，从几何图形上来看这是一个锥形区域，也称为：Velocity Obstacle Cone。

再举个例子，

张三在左边，路人甲在右边。为了判断张三会不会撞上路人甲，可以把两者的半径合成更大的圆，即：

R = r_A + r_B

以张三的视角来看，他只要进入这个“膨胀后的圆”，就会发生碰撞。从张三的位置向这个圆的两侧切线，会形成一个锥形区域。

如果张三的相对速度落在这个锥形内部，他最终会穿过这个“膨胀后的圆”，也就是发生碰撞。所以可以得出结论：

锥形内部 -> 危险速度

锥形外部 -> 安全速度

既然已经知道相对速度落在锥体内就会碰撞，那么接下来的问题就是：如何在速度空间中构造出这个锥形区域，并判断哪些速度属于危险速度？

直接上代码：

// agent.rs
use crate::vec2::Vec2;

#[derive(Clone, Debug)]
pub struct Agent {
    pub id: usize,
    // 当前坐标
    pub pos: Vec2,
    // 当前速度
    pub vel: Vec2,
    // 目标点
    pub goal: Vec2,
    // 半径，可以理解为角色碰撞圆半径
    pub radius: f64,
    // 最大速度
    pub max_speed: f64,
    // 多远以内的其他 Agent 会被认为是邻居
    pub neighbor_dist: f64,
    // 预测未来多少秒内是否会撞
    pub time_horizon: f64,
}

// main.rs
fn will_collide(a: &Agent, b: &Agent, candidate_vel: Vec2) -> bool {
    // 张三相对路人甲的位置，也就是 p_B - p_A
    let relative_pos = b.pos.sub(a.pos);

    // 膨胀后的圆，半径等于两人的半径之和
    let combined_radius = a.radius + b.radius;

    // 相对速度 = 张三的候选速度 - 路人甲当前速度
    let relative_vel = candidate_vel.sub(b.vel);

    // 判断相对运动轨迹是否会进入膨胀圆，
    // 可以转化为一个关于时间 t 的二次方程
    let a_coef = relative_vel.dot(relative_vel);
    let b_coef = 2.0 * relative_pos.dot(relative_vel);
    let c_coef =
        relative_pos.dot(relative_pos)
        - combined_radius * combined_radius;

    // 相对速度几乎为 0，说明张三和路人甲几乎没有相对运动
    if a_coef < 1e-6 {
        return false;
    }

    // 判别式，用来判断相对运动轨迹是否会和膨胀圆相交
    let discriminant =
        b_coef * b_coef
        - 4.0 * a_coef * c_coef;

    // 判别式小于 0，说明轨迹和膨胀圆没有交点，因此不会碰撞
    if discriminant < 0.0 {
        return false;
    }

    let sqrt_d = discriminant.sqrt();

    // t1 和 t2 分别表示进入和离开膨胀圆的时间
    let t1 =
        (-b_coef - sqrt_d)
        / (2.0 * a_coef);

    let t2 =
        (-b_coef + sqrt_d)
        / (2.0 * a_coef);

    // 只要在预测时间内发生碰撞，
    // 就认为这个速度危险
    (t1 >= 0.0 && t1 <= a.time_horizon)
        || (t2 >= 0.0 && t2 <= a.time_horizon)
}

现在张三已经会自动避让了，但在一些场景下会出现抖动，甚至是反复横跳。

这是因为 VO 的避障决策本质上是单边的：张三在选择新速度时，会假设路人甲保持当前速度不变，然后由自己单方面完成避让。

但真实情况是，路人甲往往也在同时避让。这样一来，双方都可能按照“由我来避开对方”的思路做出决策，例如同时向左避让，又同时改为向右避让，结果就是运动轨迹抖动。

要避免这种情况，就不能只让一方单方面避让，而是要让双方共同承担避让责任。这也正是 RVO 的核心思想。

Reciprocal Velocity Obstacle

RVO 的核心思想并不复杂，可以概括成一句话：

VO 认为“由我来避开对方”，而 RVO 认为“我们各自避让一步”。

代码上的逻辑实现也很简单，

// main.rs
fn will_collide(a: &Agent, b: &Agent, candidate_vel: Vec2) -> bool {
    // ...

    // 相对速度 = 2 * (张三的候选速度 - 双方当前速度的中点)
    let relative_vel = candidate_vel.mul(2.0).sub(a.vel).sub(b.vel);

    // ...

    (t1 >= 0.0 && t1 <= a.time_horizon)
        || (t2 >= 0.0 && t2 <= a.time_horizon)
}

现在张三和路人甲在自动避让时，与 VO 相比已经顺滑了不少，左右反复横跳的情况也明显减少了。

最后

RVO 也不是最终方案，它虽然把“单方面避让”改成了“双方各让一步”，但本质上依然是在假设对方会做出与自己一致的配合。在多人场景下，这种假设并不总是稳定。

因此还需要更进一步的改进，这也就是 ORCA 要解决的问题（挖个坑）。