ORCA这个算法，做机器人避障的几乎没人不知道。但知道名字和真正搞懂底层逻辑，是两回事。这篇论文我前前后后读过三遍，每次重读都能挖出点新东西。Reciprocal n-body Collision Avoidance，2011年ICRA上由Jur van den Berg、Jamie Snape等人发表，后来以RVO2库的形式开源，被用在无人机编队、游戏AI、仓储机器人、人群仿真各种场景里。我最早接触它是因为四足机器人集群实验，想找一种能让多台机器人在狭窄空间里"礼貌地"互相让路的速度规划方案，试了一圈之后发现ORCA的鲁棒性和计算效率确实能打。这篇就当是我整理的一份论文阅读笔记，把ORCA从VO到RVO再到ORCA的来龙去脉、核心公式的几何直觉、复现时的关键参数，以及我踩过的那些坑，完整地捋一遍。
1. 为什么需要ORCA：从VO到RVO再到ORCA的演进路线
1.1 避碰问题的"速度空间"视角
多智能体避碰，最直观的做法是在位置空间里做路径规划，比如A*、RRT*，规划出一条从起点到终点的无碰路径。但路径规划只解决"几何上不重叠"的问题，一旦环境里还有其他正在运动的智能体，路径是动态变化的，规划一次根本不够。于是学界把目光转向了速度空间——与其规划一条路径，不如直接给每个智能体一个"此刻该用什么速度"的决策。
这个思路的奠基之作就是速度障碍（Velocity Obstacle，VO）。给定两个圆盘形智能体A和B，如果A以某个速度运动，B以某个速度运动，在未来一段时间内两者会碰撞，这些"会导致碰撞的速度集合"就叫VO。A只需要把自己的速度移出VO，就能保证在预设时间窗口内不会撞上B。
VO在单个智能体面对静态或匀速运动障碍物时非常好用，这也是1998年Fiorini和Shiller原始论文的主要场景。但我拿它做多智能体对称场景时出了个问题：如果每个智能体都把自己当"主角"，把别人当"障碍物"，两个迎面走来的智能体会同时往同一侧让，然后又同时往回让，形成来回摆动的振荡。这在机器人集群里就是尴尬的"对跳"。
1.2 RVO：对称避让的第一次修正
2008年van den Berg提出了RVO（Reciprocal Velocity Obstacles），核心思想是假设其他智能体也会主动避让，而不是一动不动。直观理解，两个迎面走过来的人，不会都停下来等对方先走，而是各自往旁边偏一点，这事儿就过去了。RVO在数学上就是把VO的判断条件改一下：以两个智能体的速度中点为参考系来判定哪些速度会导致碰撞。
RVO解决了"完全把对方当障碍物"的过度保守问题，但带来了一个新毛病：震荡。因为每个智能体都在假设对方会承担一半避让责任，但实际决策时各自独立计算，没有交流，速度更新过程中容易在边界上来回反复。我自己第一次跑RVO仿真时，看到两个智能体在目标点附近划圈，一度以为是代码写错了，调试了半天才发现这是算法的固有特性。
1.3 ORCA要解决的三个问题
ORCA（Optimal Reciprocal Collision Avoidance，最优相互避碰）就是冲着RVO的震荡问题来的。它带来的三个关键改进，我觉得都可以在论文里对应上：
第一，把"避让义务"从算法层面固定下来。每个智能体对两两之间的碰撞负一半责任，这个"一半"不是靠运气，而是通过构造半平面（half-plane）来精确表达。每个相邻智能体给对方施加一个"你必须在半平面里选速度"的约束，所有约束取交集，问题就变成了一个带线性不等式约束的最优化问题。
第二，保证无振荡。论文里证明了ORCA在多智能体环境下，如果所有智能体同时使用同一个策略，两两之间的速度选择不会出现RVO那种来回抖动的死循环。这在多机器人实际部署里太重要了，速度振荡意味着电机的持续抖动、能量的浪费，甚至会让机身上的传感器数据变得不可用。
第三，把问题规约成2D线性规划，实时性有保障。线性规划（LP）的求解是多项式复杂度，而且RVO2库里用的是专门针对二维情况优化的求解器，在只处理少数几个相邻智能体时，计算量小到可以塞进IMU板载处理器。
2. 论文核心机制：半平面约束是怎么画出来的
2.1 碰撞锥与速度障碍的精确定义
先把论文里的形式化定义说清楚。假设A和B都是圆盘，位置分别是p_A、p_B，速度分别是v_A、v_B，半径分别是r_A、r_B。A相对于B的速度是v_A - v_B，位置差是p_A - p_B。
A的VO是这样一个速度集合：
VO_A|B^τ = { v | ∃t∈[0,τ]，t(v−v_B) ∈ D(p_B −p_A, r_A + r_B) }
什么意思呢？D(p, r)表示以p为圆心、r为半径的圆盘。这个公式是说，如果在未来τ时间内存在某个时刻t，A以速度v运动、B以速度v_B运动，两个圆盘会重叠，那么v就属于VO。
几何上，以B为参考系，把A看成一个点，B的圆盘半径变成r_A + r_B。从A的位置向扩大的圆盘做两条切线，切线之间的锥形区域就是"碰撞速度方向"。沿这些方向运动，不管速度大小，只要时间足够，一定会撞上。如果速度太小，在时间窗口τ内还没走到碰撞点，那也不算碰撞风险，所以VO不是无限锥，而是截断了靠近原点的一部分。这个"截断"保证低速情况下不会误报碰撞——毕竟如果两个人都静止，永远不会撞上。
2.2 从VO边界到ORCA半平面的三步转换
ORCA的数学推导是这篇论文最漂亮的部分，也是理解整个算法钥匙。我想用一个实际例子来讲。
假设A当前的最优速度v_A^opt = (0.5, 0.3)，B的最优速度v_B^opt = (-0.3, 0.4)，两个智能体沿会碰撞的方向接近。第一步，算出相对速度v_rel = v_A^opt - v_B^opt = (0.8, -0.1)，然后判断这个相对速度是否落在VO_A|B内部。如果在，说明按当前速度继续走会撞，需要修正；如果不在，理论上不需要避让，但出于鲁棒性，论文仍然会构造半平面，只是半平面完全包含期望速度，不影响解。
第二步，如果v_rel落在VO内部，就找v_rel到VO边界（截断圆弧和切线段）的最近点u。这个u是"避免碰撞需要的最小相对速度改变量"，它的方向和大小直接决定了避让的力度。
第三步，把u赋予两个智能体：A负责改变u的一半，B也负责改变u的一半。这就是Reciprocal这个词的落地。于是ORCA半平面定义为：
ORCA_A|B^τ = { v | ( v − (v_A^opt + ½u) ) · n ≥ 0 }
其中n是u的单位方向向量。这个半平面的几何意义是：A可以选择任何速度，只要投影到n方向上不低于v_A^opt + ½u这半个修正量。B同理。两个智能体各自承担一半，避让责任对半分，不会出现"我让你、你也让我"然后谁都没动的僵局。
2.3 线性规划：如何在约束交集里选最优速度
当A周围有多个智能体B₁、B₂……Bₙ时，每个智能体都给A施加一个半平面约束：
n₁·v ≥ b₁
n₂·v ≥ b₂
...
nₙ·v ≥ bₙ
再加上速度上限约束 |v| ≤ v_max，这些约束共同围出一个凸多边形区域（可能是空集）。A的最优速度就是在这个凸多边形里选一个离期望速度v_A^opt最近的向量。
v_A^new = argmin_v ∈ S || v − v_A^opt ||²
S = { v ∈ R² | ∀i: n_i·v ≥ b_i, |v| ≤ v_max }
这是一个二维线性规划问题，标准解法是Simply算法或者更快的线性时间算法。RVO2库用的是一种增量式的方法：先随机选一个可行点，然后逐个添加约束，如果某个约束把当前最优解排除掉了，就在这条直线上去求新的最优。二维情况下这个操作非常快，一般十几微秒就能完成。
这里我想补充一个论文里没有细说的细节：如果约束集合是空集怎么办？ORCA的处理是放宽约束——把这组约束里最"苛刻"的一条暂时去掉，然后再解。如果还不行，就再放开一条。实在不行就输出一个全方向里离期望速度最近的可行速度。这个退化处理在实际高密度场景里一定会触发，我后面还会细说。
3. 论文里没细说但你必须知道的边界与假设
3.1 圆盘模型与匀速直线运动的隐性假设
论文里所有推导都假设智能体是圆盘，且在时间窗口τ内速度保持不变。这两个假设是ORCA的基石，也是它的阿喀琉斯之踵。
圆盘模型意味着智能体的轮廓被简化为一个圆。对于四足机器人、双足机器人、无人机，这个近似通常可以接受——底盘的转向能力本来就弱，转不转得开跟轮廓形状关系不大。但对于细长形的移动机器人，比如AGV叉车，用圆盘就会过度保守，整个巷道都会变成禁行区，效率损失严重。
匀速直线运动的假设更微妙。ORCA在每一个控制周期（比如100ms）都重新求解一次，用当前速度作为v_A^opt的起点。这个假设在低速场景下问题不大，但在高速场景下，比如无人车高速汇入车流，τ时间内走过3~5米的距离，速度一直不变就会在避让动作上慢半拍。
实操上怎么补救？我在无人机编队里是把ORCA输出的速度作为期望速度喂给位置环PI控制器，控制器会做平滑处理。这个做法使得实际速度曲线是连续变化的，相当于把"匀速假设"的破坏降到了最低。
3.2 timeHorizon：时间窗口是效率与安全的唯一旋钮
ORCA有两个时间窗口参数：timeHorizon（用于智能体之间的避碰）和timeHorizonObst（用于静态障碍物）。这两个参数对行为的影响是决定性的。
timeHorizon取无穷大时，ORCA会避免任何速度方向上可能存在的碰撞，行为极保守，智能体转圈绕远路，根本接近不了目标。timeHorizon取很小时（比如0.1s），只有当碰撞"迫在眉睫"时才触发避让动作，全局效率高，但稍有延迟就撞上。
我调试时的经验是，把timeHorizon设为2~3倍的"响应时间"。响应时间包括感知延迟、决策延迟、执行器延迟，比如用板载相机做感知，帧率30fps，加上电机响应，整体延迟约80ms，那timeHorizon取0.15~0.25s比较合理。如果取太大，会看到智能体在还很远的地方就开始绕路，白白浪费动作空间。
3.3 避让责任均分在不对称环境下的隐患
ORCA承诺"无振荡"是有严格前提的：所有智能体都使用相同的ORCA策略，并且对彼此的速度有一定的感知。这个前提在仿真里成立，在真实环境里几乎不成立。
举个例子：A是有完备定位的机器人，B是行人。行人的运动模型完全不是ORCA，他不知道也不会遵守"避让责任对半分"。按ORCA算出来的半平面会让A承担一半避让义务，给A生成一个偏转速度，但B继续直走，结果还是撞上。换句话说，ORCA要求"对方讲理"。
我处理混合环境时常用的策略是加一个"社会力加成"：对非合作障碍物（行人、车辆），把它们的ORCA权重提高，让A承担接近全部的避让责任。换句话说，遇到不遵守规则的agent，就用回VO的单方面避让。
4. 复现思路与关键参数调优实战
4.1 一套可以直接跑通的最小实现
如果你想亲手验证ORCA的效果，我建议直接基于RVO2库起步，再把核心逻辑抽出来改一版最小实现。RVO2是C++写的，依赖少，但代码结构偏工程化，读起来有点吃力。
最小实现的核心流程可以拆成下面几步：
- 维护每个智能体的状态：位置p、速度v、期望速度v_pref、半径r、最大速度v_max。
- 遍历所有其他智能体，用邻居距离neighborDist筛选出"需要考虑的邻居"。
- 对每个邻居计算VO（跳过共享速度空间的细节），再构造ORCA半平面。
- 把所有半平面约束聚合成一个线性规划问题。
- 求解LP，得到新速度。
- 用新速度更新位置：p += v_new * Δt。
LP求解器是最大坑。RVO2库里用的是自己实现的2D LP求解器，核心逻辑约200行，看起来简单，但边界条件极多（退化约束、线性相关、平行线等）。我试过直接用第三方LP库（比如GLPK）替换，结果慢了一个数量级，而且数值稳定性反而不如RVO2自带的那个。所以我的建议是：复现时直接保留RVO2的LP求解器，只重写算法调度部分。
4.2 参数表：每个参数到底在影响什么
| 参数 | 含义 | 典型值（仿真） | 调高/调低的影响 |
|---|---|---|---|
| neighborDist | 邻居搜索半径 | 2~5m | 调高大场景下避险意识强，计算量大；调小可能漏掉近距离智能体 |
| maxNeighbors | 参与计算的最大邻居数 | 5~10 | 调高更全局但计算量大；调小性能好但可能顾此失彼 |
| timeHorizon | 智能体间避让时间窗口 | 0.5~2s | 调大更保守、绕路多；调小更激进、碰撞风险高 |
| timeHorizonObst | 静态障碍避让时间窗口 | 0.1~0.5s | 调大对静态障碍避让更提前；调小容易贴墙走 |
| radius | 智能体碰撞半径 | 按实际尺寸 | 调大避让距离远、通行效率低 |
| maxSpeed | 最大速度限制 | 按执行器上限 | 调大可能超出执行器极限；调小限制通行效率 |
这套参数没有标准答案，跟具体的机器人动力学强相关。我建议按"先定radius和maxSpeed，再调timeHorizon，最后才动neighborDist"的顺序来收敛。因为前两个是物理硬约束，timeHorizon对应控制周期，neighborDist是优化项。顺序反了很容易陷入局部调参。
4.3 多智能体扩展时的实现细节
ORCA论文里的"n-body"不是指一次性把所有智能体放进一个全局优化，而是每对相邻智能体两两计算，再各自求解。这个分布式特性意味着计算量不会随智能体数量指数爆炸，但也意味着每个智能体的"视野"有限，全局最优性无从谈起。
多智能体部署时，邻居管理要用空间哈希或KD树做邻居查询，不要直接O(n²)遍历。我在100个智能体的仿真里测试过，用KD树做邻居查询，单帧计算从12ms降到了1.4ms。RVO2库里用的是KD树，但没有用最新式的AABB层次体系，你如果自己实现，推荐flann或者nanoflann这类轻量库。
还有一点，当智能体数量超过邻居截断上限maxNeighbors时，ORCA会直接忽略超出上限的邻居。这在高密度场景里特别危险——周围的智能体明明很多，却只考虑最近的几个，很容易把远处的碰撞风险完全漏掉。我的处理方法是把maxNeighbors调大，同时把timeHorizon调小，这样即使计算量上升，也不会因为漏算而出事故。
5. 实测中遇到的意外情况与解决思路
5.1 半平面交集为空时的退化处理
第一次跑10个智能体双向通行场景时，我撞上了ONCA的退化分支：多个半平面的交集为空。视觉表现就是智能体在通道中间僵住不动，或者低速原地抖动。
原因很好理解：A被B要求往左上让，被C要求往右下让，被D要求往左边偏，这些约束可能就围出了一个空集。尤其在狭窄通道、对面来车的高密度场景，这种情况几乎必然发生。
ORCA的默认策略是逐个放开约束，直到LP有解。但这个"逐个放开"的顺序对结果影响很大。RVO2库里是按线性规划求解过程中碰到不可行约束的先后顺序来放开的，这个顺序本质上是一个贪心策略，有时候放开的恰好是最重要的避让约束，导致后续速度选择离期望速度很远，甚至偏向新的碰撞方向。
我在工程里做了一层增强：如果放开约束后得到的速度仍然很偏，就对同一边的两个约束取平均方向，生成一个"折中约束"，再重新求解。这样虽然轻微破坏了ORCA的对称性，但在密集队列中能有效减少僵持时间。
5.2 对称死锁：ORCA也会有走不出来的局面
ORCA虽然保证无振荡，但死锁是另一回事。两个智能体在过道两端相向而行，它们的期望速度方向完全相反，ORCA算出来的最优速度会导致它们在同一个位置附近徘徊。
这个死锁有一点像TCP的拥塞控制退化：双方都愿意让，但让到哪儿算"合适"却没法协调。我从实验中得到的数据是，对称死锁只出现在完全对称的场景，比如两个相同的机器人、相同的拓扑、相同的期望速度幅值。只要稍微打破对称性——比如让其中一个的期望速度大一点、或者目标位置偏一点——死锁就自动消失了。
所以我后来做应用的时候，故意往期望速度里加了一点周期性扰动（正弦波，幅度约为最大速度的2%）。效果很好，完全避免了对称死锁，而且对路径质量的影响几乎可以忽略。这个方法属于工程补丁，论文里完全没有，但很实用。
5.3 动态障碍物与传感器噪声带来的不对称性
你如果直接把激光雷达或相机的检测结果喂进ORCA，大概率会看到忽快忽慢的抖动。原因是ORCA假设智能体对邻居的速度有准确估计，而传感器对动态物体的速度估计通常噪声很大。邻居速度估计抖动，ORCA半平面边界跟着抖，最终输出的最优速度就来回跳。
我解决这个问题的办法有两步。第一步，在速度估计上加一个低通滤波，截止频率大约1~2Hz。第二步，在ORCA输出后加一个限幅环节，速度变化率不超过预设值。这两步做完，机器人的动作就平滑很多。
需要注意，滤波会引入延迟，延迟会削弱ORCA对突发障碍物的反应速度。所以滤波截止频率不能设太低，否则一个横穿的行人可能来不及避让。我这个经验是在一次室内演示翻车后总结出来的——行人从左前侧快速横穿，机器人愣在原地0.5秒才动，吓得我赶紧把滤波器的截止频率调高了。
5.4 与控制器的对接：ORCA在差速轮与阿克曼底盘上的落地
ORCA输出的是全向速度向量，而大多数移动机器人底盘是差速轮或阿克曼转向，不能直接往这个方向走。如果直接把ORCA输出的期望速度分解到左右轮，会有严重的偏移误差——想象中的"斜着走"执行成"先转个角再直走"。
差速轮底盘的常见做法是：把ORCA输出的期望速度转换成一个线速度和一个角速度，线速度取期望速度在车头方向的投影，角速度用期望速度方向与车头方向的夹角乘以比例系数。再配合一个轨迹跟踪控制器（比如纯跟踪或模型预测控制）去实时平滑这个参考输入。
阿克曼底盘更麻烦，因为转弯半径是恒定的，ORCA输出的大角度转向必须被平滑地裁剪成一条可行的曲线。我试过在ORCA后面接一个Dubins曲线控制器，效果还可以，但实时性会受影响。如果做高速场景，建议直接用ORCA生成参考轨迹点，再用MPC跟踪。
5.5 静态障碍物的处理：ORCA与全局路径规划的协同
ORCA本质上是一个局部避让算法，没有全局视野，在静态障碍物构成的地形里很容易被困在局部极小值。比如"U"形障碍物中间有个凹槽，ORCA会绕着凹槽边缘反复尝试，永远出不来。
我的经验是，把全局路径规划（比如A*、RRT*）和ORCA串起来：全局规划器每1秒更新一次，生成一系列路径点；ORCA以路径点为"临时目标"进行局部避让；到达一个路径点后切换到下一个。这样既保留全局路径的省时特性，又能利用ORCA的实时避障能力。
更进一步的配合是，让全局路径规划器输出的轨迹通过一个轨迹平滑器（比如时间弹性带算法TEB或均匀B样条），再把平滑后的轨迹点作为ORCA的期望速度方向。这样ORCA只会在这条轨迹附近做微调，不会大幅偏离全局路径，整体效率会提高很多。
最后再分享一个小技巧：ORCA的LP求解器对输入约束的数值范围很敏感，工程实现时一定要把坐标和速度归一化到合理的尺度，不然一次普通的乘法溢出就可能导致求解器崩溃。我自己就因为这个原因排查了整整一个下午，最后发现是某个智能体的位置坐标在渲染坐标系里没除1000，导致半平面的法向量计算出现了NaN。把数据单位统一，很多诡异的bug直接就消失了。这套算法的核心思路不复杂，但从论文到工程落地之间，确实还有不小的距离。希望这篇笔记能帮你少走几段弯路。
