# RVO2 / ORCA 核心原理

## 一句话结论

RVO2 的核心是 **在速度空间里选择一个尽量接近期望速度、同时在未来一段时间内不会和邻居相撞的速度**。它不是先规划一条全局路径，而是每帧根据周围动态体的位置、速度、半径，实时修正当前速度。

在 FishROV benchmark 中，`Rvo2Adapter` 直接调用 `RVO.Simulator`，把 Unity 的 `x/z` 平面映射到 RVO2 的 `x/y` 平面。

## 要解决的问题

普通 steering 常见做法是：

```text
朝目标走
遇到近邻就加一个远离力
最后合成速度
```

这种方法容易出现几个问题：

- 只看当前距离，不看未来是否会碰撞。
- 多个 agent 同时互相躲避时，可能左右摇摆。
- 高密度时排斥力叠加会让速度很乱。
- 很难证明“这个速度在未来一小段时间内安全”。

RVO2/ORCA 换了一个视角：不直接在位置空间里推开物体，而是在速度空间里判断哪些速度会导致未来碰撞。

## Velocity Obstacle

假设有两个 agent：A 和 B。

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

Velocity Obstacle 的意思是：

```text
对 A 来说，有一片速度区域是危险的。
如果 A 选择这片区域内的速度，
在假设 B 继续按当前速度运动的情况下，
A 会在未来撞到 B。
```

这片危险区域通常可以理解成速度空间中的一个锥形区域。

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

普通 VO 假设“我来躲别人”。如果所有 agent 都这样算，容易过度避让。

RVO 的思路是：

```text
A 和 B 都是主动体。
发生潜在碰撞时，不应该让 A 单独承担全部避让。
双方各承担一部分速度修正。
```

这让多 agent 场景更自然，尤其适合鱼群、机器人群、行人群这类对等个体。

## ORCA：把避障变成线性约束

ORCA 是 RVO 的改进形式。它会为每个邻居构造一个半平面约束：

```text
为了避免和这个邻居碰撞，
当前 agent 的新速度必须落在这条线的一侧。
```

多个邻居会产生多个约束。最终问题变成：

```text
在所有安全半平面约束内，
找一个距离期望速度最近的速度。
```

这就是 RVO2 高效的关键：每帧不是做复杂全局搜索，而是在低维速度空间里做局部约束求解。

## 每帧流程

RVO2 的典型帧流程是：

1. 更新每个 agent 的当前位置。
2. 给每个 agent 设置期望速度。
3. 使用空间结构查找邻居。
4. 为每个邻居构造 ORCA 约束。
5. 求出新的安全速度。
6. 用新速度推进 agent。

在当前 benchmark 中，推进 Transform 由统一的 `BenchmarkAgent.Integrate` 负责，RVO2 adapter 只负责把期望速度改成避障后的速度。

## 关键参数

- `radius`：agent 半径，直接决定碰撞距离。
- `neighborDist`：考虑多远范围内的邻居。
- `maxNeighbors`：最多考虑多少邻居。
- `timeHorizon`：预测未来多久内的碰撞。
- `maxSpeed`：最大速度。
- `timeStep`：仿真步长。

这些参数会明显影响表现：

- `neighborDist` 太小：反应太晚，容易碰撞。
- `neighborDist` 太大：邻居过多，性能下降，也可能过度保守。
- `timeHorizon` 太短：贴脸才躲。
- `timeHorizon` 太长：提前过度避让，可能绕不开或变慢。
- `maxNeighbors` 太低：高密度时会漏掉重要邻居。

## 优点

- 对多动态 agent 很合适。
- 速度空间建模清晰，可解释。
- 不需要全局路径。
- 高密度局部避障比简单排斥力稳定。
- 理论上比纯启发式 steering 更有安全约束感。

## 局限

- RVO2-CS 是 2D 算法，当前项目的 3D 模式会投影到 XZ 平面。
- 它不负责全局规划，遇到死胡同或复杂障碍仍可能局部最优。
- 只考虑局部未来，不能理解长远路线收益。
- 参数不合适时仍会碰撞、停滞或过度保守。
- 如果 benchmark 初始位置已经重叠，它需要时间把重叠解开。

## 在 FishROV 中的观察重点

测试 RVO2 时重点看：

- `XZ重叠对` 是否能稳定下降。
- `3D重叠对` 是否比 XZ 少，判断 2.5D 高度层是否只是视觉重叠。
- 高密度场景中 `模拟耗时` 是否随数量近似线性或亚二次增长。
- 是否出现互相卡住、绕圈或过早减速。

## 对自研算法的启发

RVO2 最值得吸收的不是具体代码，而是这个思想：

```text
不要只问“障碍在哪里”，
还要问“我现在可选的速度里，哪些会在未来导致碰撞”。
```

后续自研算法可以借鉴：

- 用预测碰撞时间代替单纯距离。
- 在速度空间做候选速度打分。
- 对不同邻居按风险程度加权。
- 把“接近期望速度”和“避碰安全”放到同一个优化目标里。
