# HRVO 核心原理

## 一句话结论

HRVO 的核心是 **在速度空间里排除未来会发生碰撞的速度，然后选择一个尽量接近期望速度、同时更不容易产生左右振荡的新速度**。

它不是全局寻路算法，也不是推进器控制器，而是一个去中心化的局部避障算法。每个 agent 只根据自己感知到的邻居位置和速度独立决策，不需要中心调度，也不需要 agent 之间通信。

仓库地址：

```text
https://github.com/snape/HRVO
```

## 要解决的问题

多 agent 避障最麻烦的不是单个障碍物，而是 **大家都会动，并且大家都会试图避让**。

如果用简单 steering，常见逻辑是：

```text
朝目标走
靠近别人就加一个排斥力
最后合成速度
```

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

- 只看当前距离，不一定能判断未来是否会撞上。
- 多个 agent 同时避让时，可能互相把对方推乱。
- 对称场景下容易左右摇摆。
- 高密度场景里，排斥力叠加后速度可能非常不稳定。

HRVO 换了一个视角：不直接在位置空间里把 agent 推开，而是在速度空间里判断：

```text
如果我现在选择这个速度，
按照邻居的当前位置和速度预测，
未来一小段时间内会不会撞上？
```

会撞的速度，就是不能选的速度。

## Velocity Obstacle

Velocity Obstacle，简称 VO，表示速度空间中的危险区域。

假设有两个 agent：A 和 B。

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

对 A 来说，可以把 B 膨胀成半径 `rA + rB` 的圆。然后在速度空间中构造一片区域：

```text
如果 A 选择这片区域里的速度，
并且 B 继续保持当前速度，
A 未来会撞到 B。
```

这片速度区域就是 B 对 A 产生的 Velocity Obstacle。

直观理解：

```text
位置空间：哪里有障碍？
速度空间：哪些速度会导致未来撞上障碍？
```

VO 的价值在于，它把动态碰撞预测变成了当前帧的速度选择问题。

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

普通 VO 假设“别人保持当前速度，我来躲”。这在动态 agent 群里会偏保守，因为实际上对方也会躲。

RVO 的思想是 reciprocal：

```text
A 和 B 都是主动体。
如果两者当前速度组合会导致碰撞，
不应该让 A 独自承担全部避让修正。
双方各承担一部分速度修正。
```

这样 agent 的行为会更自然，尤其适合：

- 机器人群；
- 人群模拟；
- 鱼群或艇群；
- 多个同等级 NPC 的局部避让。

## HRVO：减少 reciprocal 避让振荡

RVO 虽然比 VO 更对称，但在某些对称场景中仍然可能出现振荡。

典型例子：

```text
A 和 B 正面对冲
这一帧双方都觉得向左让比较好
下一帧相对状态变化后，双方又都觉得向右让比较好
然后反复左右切换
```

HRVO 的关键改进就是 Hybrid：

```text
它不是完全使用 VO，也不是完全使用 RVO，
而是把两者的几何边界混合起来，
构造一个更偏向稳定避让侧的速度障碍。
```

直观说，HRVO 会让速度障碍的顶点和边界选择更偏向“别来回改主意”，从而减少互相避让时的左右横跳。

所以 HRVO 关注的不只是无碰撞，还包括：

```text
collision-free
oscillation-free
decentralized
```

也就是无碰撞、少振荡、去中心化。

## 每帧流程

HRVO 每帧大致做这些事：

1. 更新每个 agent 的当前位置和当前速度。
2. 为每个 agent 设置期望速度 `preferredVelocity`。
3. 查找附近邻居。
4. 对每个邻居构造一个 HRVO 速度障碍。
5. 在可选速度中寻找一个不落入危险区域、且最接近期望速度的新速度。
6. 用新速度推进 agent。

可以理解成：

```text
目标方向
  -> 期望速度
  -> 邻居预测
  -> 构造速度禁区
  -> 选择安全速度
  -> 推进位置
```

## 和 VO / RVO / ORCA 的区别

| 方法 | 核心假设 | 主要特点 |
| --- | --- | --- |
| VO | 我躲别人，别人保持当前速度 | 简单直接，但对动态群体偏保守 |
| RVO | 双方都会避让，各承担一部分修正 | 更适合多 agent，但对称场景可能振荡 |
| HRVO | 混合 VO 和 RVO 的速度障碍 | 在 reciprocal 基础上减少左右振荡 |
| ORCA | 把避障转成半平面约束和优化问题 | 工程上更常用，求解形式更规整 |

HRVO 可以看作 RVO 家族中的一个改进版本，重点是处理 reciprocal collision avoidance 中的振荡问题。

## 关键参数

HRVO 这类算法通常会关心这些参数：

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

参数影响很直接：

- `neighborDist` 太小：反应太晚，容易贴脸避让。
- `neighborDist` 太大：邻居过多，性能下降，也可能过度保守。
- `timeHorizon` 太短：只处理眼前风险。
- `timeHorizon` 太长：提前避让过多，路径可能绕或慢。
- `maxNeighbors` 太低：高密度时可能漏掉重要邻居。

## 优点

- 去中心化，不需要统一调度所有 agent。
- 对多动态 agent 场景更自然。
- 使用速度空间建模，可解释性比纯排斥力强。
- 相比普通 RVO，更重视减少对称避让中的振荡。
- 适合实时局部避障。

## 局限

- 它主要是二维平面局部避障算法。
- 不负责全局路径规划。
- 不理解地图语义、死胡同、长期收益。
- 对强动力学约束系统需要额外适配。
- 高密度或狭窄通道中仍可能出现局部最优、停滞或拥堵。
- 如果 agent 初始已经严重重叠，算法需要时间把重叠解开。

## 放到 FishROV 中的定位

在 FishROV 里，HRVO 更适合作为一层局部避障参考，而不是完整运动方案。

建议定位：

```text
全局目标 / 路径
  -> 生成期望速度
  -> HRVO 局部避障修正速度
  -> FishROV 自身运动控制 / 刚体控制 / 推进器控制
```

也就是说，HRVO 输出的是“当前更安全的速度建议”，不是最终控制指令。

如果用于 ROV 或鱼群 benchmark，需要特别注意：

- 如果当前运动是 3D，HRVO 原生二维模型需要投影到 XZ 平面，或扩展为 3D 近似。
- 如果存在水流、惯性、最大转向角、加速度限制，需要在 HRVO 输出之后再做动力学约束。
- 如果有地形、洞穴、障碍物、任务点顺序，仍然需要全局或中层规划。
- 如果 agent 高度层不同，不能只看 XZ 重叠，否则会把视觉上错层的对象也当成冲突。

## 对自研避障算法的启发

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

```text
1. 在速度空间里判断未来碰撞，而不是只看当前距离。
2. 假设其他动态体也会避让，而不是全部由自己承担。
3. 对称避让时要显式处理振荡，不然会左右横跳。
```

如果后续继续做 FishROV 自研局部避障，可以把 HRVO 作为参考方向：

- 用预测碰撞时间替代单纯距离惩罚。
- 把候选速度分成安全区、危险区和高风险区。
- 给 reciprocal 避让加入稳定侧偏好，减少频繁换边。
- 把“接近期望速度”和“避碰安全”放进同一个评分或约束求解里。
- 在 3D 场景中明确区分水平面冲突和真实体积冲突。

## 参考

- HRVO 仓库：https://github.com/snape/HRVO
- HRVO 项目页：https://gamma.cs.unc.edu/HRVO/
- Velocity Obstacle 术语说明：https://en.wikipedia.org/wiki/Velocity_obstacle
