# RVO_Py_MAS 核心原理

## 一句话结论

`RVO_Py_MAS` 的核心是 **在速度空间里排除未来会碰撞的速度，然后选择一个最接近期望速度的安全速度**。

它不是全局寻路系统，也不负责生成完整路径。它更像一个局部避障控制器：上层系统先给每个 agent 一个期望速度，RVO 再根据周围动态 agent 和静态障碍，把这个速度修正成更安全的实际速度。

典型接入方式是：

```text
目标/任务控制器
  -> 计算期望速度 V_desired
  -> RVO_Py_MAS 根据邻居和障碍修正速度
  -> 输出实际速度 V
  -> 推进位置
```

## 它要解决的问题

普通 steering 或分离力算法通常在位置空间工作：

```text
离目标近一点
离邻居远一点
离障碍远一点
最后把多个力合成一个速度
```

这类方法简单，但容易出现几个问题：

- 只看当前距离，不一定能判断未来是否会撞上。
- 多个 agent 同时互相躲避时，可能左右摇摆。
- 高密度场景下，分离力叠加后速度容易变乱。
- 很难回答“这个速度在未来一小段时间内是否安全”。

RVO 的视角不同。它不直接问“我现在离别人多近”，而是问：

```text
如果我选择这个速度，
在未来一段时间内会不会撞到别人？
```

## Velocity Obstacle

对两个 agent A 和 B：

- A 的位置是 `pA`，速度是 `vA`，半径是 `rA`。
- B 的位置是 `pB`，速度是 `vB`，半径是 `rB`。
- 如果未来某个时间点，两者中心距离小于 `rA + rB`，就会发生碰撞。

Velocity Obstacle 的意思是：

```text
对 A 来说，有一片速度区域是危险的。
如果 A 选择这片区域内的速度，
并且 B 按当前趋势运动，
A 未来就可能撞到 B。
```

这片危险区域可以理解成速度空间里的一个锥形区域。锥的方向由 A 指向 B 的相对位置决定，锥的张角由双方半径和距离决定。

## RVO：双方共同承担避让

普通 VO 假设“对方保持原样，我单方面避让”。如果所有 agent 都这么算，就容易过度避让或振荡。

RVO 的关键变化是：

```text
A 和 B 都是主动体。
如果未来可能碰撞，不应该只让 A 负责全部避让。
双方都假设对方也会做类似避让，因此各自承担一部分速度修正。
```

在 `RVO_Py_MAS` 代码里，RVO 版本的速度障碍平移点使用了双方当前速度的平均项：

```python
transl_vB_vA = [
    pA[0] + 0.5 * (vB[0] + vA[0]),
    pA[1] + 0.5 * (vB[1] + vA[1]),
]
```

这就是 reciprocal 的工程化表达：不是单边躲避，而是假设双方都会动一点。

## 每个 agent 的计算流程

`RVO_update` 对每个 agent 做下面几步：

1. 读取当前 agent 的位置 `pA` 和当前速度 `vA`。
2. 遍历其他 agent。
3. 为每个邻居构造一个 RVO 速度障碍。
4. 遍历静态圆形障碍，也构造速度障碍。
5. 调用 `intersect`，从候选速度里选择最合适的速度。
6. 把结果写回输出速度列表。

可以简化成：

```text
对每条鱼：
  收集周围会影响它的动态体和静态障碍
  把这些对象都转成速度空间中的禁区
  找一个不在禁区里、又最接近期望速度的速度
```

## 动态 agent 如何形成禁区

对每个邻居 B，代码会计算：

- `dist_BA`：A 和 B 的距离。
- `theta_BA`：A 指向 B 的方向角。
- `theta_BAort`：碰撞锥的半角。
- `bound_left` / `bound_right`：碰撞锥左右边界。

然后把这个速度障碍存成：

```python
RVO_BA = [
    transl_vB_vA,
    bound_left,
    bound_right,
    dist_BA,
    2 * ROB_RAD,
]
```

其中 `2 * ROB_RAD` 表示两个相同半径 agent 的安全距离。代码里还给机器人半径额外加了 `0.1`，相当于保守安全边距。

## 静态障碍如何处理

静态障碍被当成速度为 0 的对象：

```python
vB = [0, 0]
```

圆形障碍直接用自身半径加 agent 半径形成安全距离。代码里还有一个 `OVER_APPROX_C2S = 1.5`，用于把某些障碍做更保守的圆形近似。

所以动态 agent 和静态障碍最终都被转成同一种东西：

```text
速度空间里的危险区域
```

这让算法主体可以统一处理。

## 候选速度选择

`intersect` 函数不是做严格的数学优化，而是在速度空间里采样候选速度：

```python
for theta in numpy.arange(0, 2 * PI, 0.1):
    for rad in numpy.arange(0.02, norm_v + 0.02, norm_v / 5.0):
        new_v = [rad * cos(theta), rad * sin(theta)]
```

也就是：

```text
按不同方向 theta 采样
按不同速度大小 rad 采样
组合成一批候选速度
```

每个候选速度都会拿去检查：它是否落在任意一个 RVO 禁区里。

- 没落入禁区：放入 `suitable_V`。
- 落入禁区：放入 `unsuitable_V`。

## 如果有安全速度

如果存在 `suitable_V`，算法选一个最接近期望速度 `vA` 的速度：

```python
vA_post = min(suitable_V, key=lambda v: distance(v, vA))
```

这体现了 README 里说的“minimal modification of desired velocity”：

```text
能不改就不改。
必须避让时，也尽量少改。
```

## 如果没有安全速度

高密度或被包围时，可能没有任何候选速度完全安全。

这时算法会从 `unsuitable_V` 里选一个折中速度。它会估计每个不安全速度的碰撞时间 `tc`，然后用下面的代价选择：

```python
(WT / tc) + distance(v, vA)
```

含义是：

```text
尽量晚碰撞
同时尽量接近期望速度
```

这不是严格安全保证，而是“没有好选择时选一个相对不坏的选择”。

## VO / RVO / HRVO 的区别

这个仓库的代码里保留了三种思路的切换痕迹：

| 方法 | 核心假设 | 工程表现 |
| --- | --- | --- |
| VO | 对方保持当前速度，我单方避让 | 简单，但多主动体容易过度避让 |
| RVO | 双方都会避让，各承担一部分修正 | 更适合多 agent 对等场景 |
| HRVO | 混合 VO 和 RVO，减少振荡和死锁 | 理论上更稳，但实现略复杂 |

当前代码默认使用 RVO。普通 VO 和 HRVO 的部分在代码里以注释形式保留。

## 优点

- 思路直观，代码量小，适合学习和改造。
- 不需要复杂全局地图，只依赖局部位置、速度、半径。
- 上层控制目标可以保持独立，只需要提供期望速度。
- 对多 agent 场景比单纯分离力更有“未来碰撞预测”意识。
- 动态 agent 和静态障碍可以统一成速度空间约束。

## 局限

- 这是局部避障，不是全局寻路。
- 候选速度靠采样，可能错过真正更优的速度。
- 高密度场景可能没有完全安全速度，只能选折中。
- 代码没有空间索引，agent 数量大时两两遍历会变贵。
- 它不是 ORCA/RVO2 那种半平面约束加线性规划求解。
- 参数、步长、最大速度不合适时仍可能碰撞或卡住。

## 和 RVO2 / ORCA 的区别

| 维度 | RVO_Py_MAS | RVO2 / ORCA |
| --- | --- | --- |
| 核心方式 | 采样候选速度，过滤禁区 | 构造半平面约束，求最近安全速度 |
| 实现复杂度 | 低 | 更高 |
| 可读性 | 很直观 | 数学和工程结构更复杂 |
| 理论性质 | 较弱 | 更强 |
| 性能 | 小规模足够，规模大需优化 | 更成熟，适合大量 agent |
| 学习价值 | 很高，适合理解速度障碍 | 更适合工程使用和高性能仿真 |

## 对 FishROV 的启发

如果 FishROV 后续要做自研鱼群避障，可以吸收它的核心结构：

```text
目标速度
  -> 构造邻居/障碍的未来碰撞风险
  -> 在速度空间选择替代速度
  -> 优先保持目标意图
  -> 只有必要时才避让
```

更适合 FishROV 的工程版本可以考虑：

- 加空间哈希或网格邻居查询，避免全量两两比较。
- 把 3D 鱼群投影到 XZ 平面做水平避障，再单独处理高度层。
- 在候选速度里加入最大转向角、最大加速度、速度平滑约束。
- 对不同鱼群状态加权，例如巡游、逃逸、聚集、追逐。
- 增加右侧/左侧通行偏置，减少对称场景下的互相卡住。
- 把“预计碰撞时间”和“当前重叠深度”作为调试指标输出。

## 结论

`RVO_Py_MAS` 最值得学习的不是它的具体 Python 代码，而是它的建模方式：

```text
不要只在位置空间推开物体。
要在速度空间判断：哪些速度会导致未来碰撞。
```

它适合作为理解 RVO 思想和自研局部避障算法的入门参考；如果目标是直接上生产级大规模 agent 避障，RVO2 / ORCA 或项目内针对鱼群特征优化后的采样避障会更合适。

## 参考

- [MengGuo/RVO_Py_MAS](https://github.com/MengGuo/RVO_Py_MAS)
- [RVO_Py_MAS/RVO.py](https://github.com/MengGuo/RVO_Py_MAS/blob/master/RVO.py)
- [Reciprocal Velocity Obstacles for Real-Time Multi-Agent Navigation](https://gamma.cs.unc.edu/RVO/)
