ORCA这个算法做机器人避障的几乎没人不知道。但知道名字和真正搞懂底层逻辑是两回事。这篇论文我前前后后读过三遍每次重读都能挖出点新东西。Reciprocal n-body Collision Avoidance2011年ICRA上由Jur van den Berg、Jamie Snape等人发表后来以RVO2库的形式开源被用在无人机编队、游戏AI、仓储机器人、人群仿真各种场景里。我最早接触它是因为四足机器人集群实验想找一种能让多台机器人在狭窄空间里礼貌地互相让路的速度规划方案试了一圈之后发现ORCA的鲁棒性和计算效率确实能打。这篇就当是我整理的一份论文阅读笔记把ORCA从VO到RVO再到ORCA的来龙去脉、核心公式的几何直觉、复现时的关键参数以及我踩过的那些坑完整地捋一遍。1. 为什么需要ORCA从VO到RVO再到ORCA的演进路线1.1 避碰问题的速度空间视角多智能体避碰最直观的做法是在位置空间里做路径规划比如A*、RRT*规划出一条从起点到终点的无碰路径。但路径规划只解决几何上不重叠的问题一旦环境里还有其他正在运动的智能体路径是动态变化的规划一次根本不够。于是学界把目光转向了速度空间——与其规划一条路径不如直接给每个智能体一个此刻该用什么速度的决策。这个思路的奠基之作就是速度障碍Velocity ObstacleVO。给定两个圆盘形智能体A和B如果A以某个速度运动B以某个速度运动在未来一段时间内两者会碰撞这些会导致碰撞的速度集合就叫VO。A只需要把自己的速度移出VO就能保证在预设时间窗口内不会撞上B。VO在单个智能体面对静态或匀速运动障碍物时非常好用这也是1998年Fiorini和Shiller原始论文的主要场景。但我拿它做多智能体对称场景时出了个问题如果每个智能体都把自己当主角把别人当障碍物两个迎面走来的智能体会同时往同一侧让然后又同时往回让形成来回摆动的振荡。这在机器人集群里就是尴尬的对跳。1.2 RVO对称避让的第一次修正2008年van den Berg提出了RVOReciprocal Velocity Obstacles核心思想是假设其他智能体也会主动避让而不是一动不动。直观理解两个迎面走过来的人不会都停下来等对方先走而是各自往旁边偏一点这事儿就过去了。RVO在数学上就是把VO的判断条件改一下以两个智能体的速度中点为参考系来判定哪些速度会导致碰撞。RVO解决了完全把对方当障碍物的过度保守问题但带来了一个新毛病震荡。因为每个智能体都在假设对方会承担一半避让责任但实际决策时各自独立计算没有交流速度更新过程中容易在边界上来回反复。我自己第一次跑RVO仿真时看到两个智能体在目标点附近划圈一度以为是代码写错了调试了半天才发现这是算法的固有特性。1.3 ORCA要解决的三个问题ORCAOptimal 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为半径的圆盘。这个公式是说如果在未来τ时间内存在某个时刻tA以速度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直接就消失了。这套算法的核心思路不复杂但从论文到工程落地之间确实还有不小的距离。希望这篇笔记能帮你少走几段弯路。