IEEE Transactions on Robotics

SICNav: Safe and Interactive Crowd Navigation Using Model Predictive Control and Bilevel Optimization

作者：Sepehr Samavi, James R. Han, Florian Shkurti, Angela P. Schoellig · 机构：University of Toronto Robotics Institute, Vector Institute for Artificial Intelligence, Toronto, ON, Canada; Technical University of Munich, Munich, Germany; University of Toronto, Toronto, ON, Canada; Munich Institute for Robotics and Machine Intelligence (MIRMI), Munich, Germany; Vector Institute for Artificial Intelligence, Toronto, ON, Canada

会议/期刊：IEEE Transactions on Robotics · 发布日期：2025 · 页码：801-818

运动规划控制优化移动机器人安全

精读笔记

Problem Setting

SICNav 处理的是单机器人在多人行人环境中的局部安全导航，但它真正瞄准的不是普通 collision avoidance，而是 predict-then-plan 架构下的结构性错误：人被当成外生动态障碍，机器人计划不会进入人的预测模型。这个错误在稀疏场景中不明显，但在门口、窄走廊、目标被人阻挡等需要相互让行的场景中会直接导致 freezing、绕行过度或振荡。

关键矛盾是：如果只追求安全，机器人会保守到冻结；如果追求交互性，很多方法会把安全放进 reward 或 learned policy，失去明确约束。论文的问题因此可以表述为：如何让机器人在优化自身动作时同时推断“人会如何响应这个动作”，并且仍然把碰撞避免写成显式约束。

以前方法卡在两个方向。一类 open-loop prediction 方法预测得越保守越安全，但也越容易把可协商空间误判为不可通行空间；另一类 RL / game / cooperative 方法能产生交互行为，但安全通常不是 optimization-level hard constraint，且多人行人场景中的可解释性和泛化更弱。

Motivation

作者的核心观察是：crowd navigation 的性能不只取决于 prediction accuracy，而取决于 prediction 是否与 robot plan 同步变化。一个在 open-loop benchmark 上更准的 predictor，不一定让导航更好，因为 planner 真正需要的是 counterfactual prediction：如果机器人这样走，人会怎么变。

这解释了为什么作者选择 ORCA 而不是更强的神经预测器。ORCA 的价值不在于最准确，而在于它提供了一个可优化、可约束、可解释的 best-response 结构。人的未来速度可以被写成一个小型凸 QP 的解，进而被嵌入上层 MPC。换言之，作者缺的不是一个 black-box predictor，而是一个可被 planner 反向“查询”的交互模型。

关键缺口是 closed-loop interaction 与 explicit safety constraints 的结合。现有交互方法往往牺牲硬约束，现有安全 MPC 往往牺牲交互性；SICNav 试图用 bilevel optimization 把两者放进同一个问题。

Core Idea

论文真正核心的思想是：把人看成对机器人计划有反应的优化器，而不是预测器输出的轨迹。每个行人在每个时刻解 relaxed ORCA，目标是接近自己的 preferred velocity，同时满足与其他 agent 和障碍物的速度空间避碰约束；机器人上层 MPC 在选择自身控制时，必须同时满足这些 lower-level ORCA 最优性。于是，人轨迹不再是 planner 的输入，而是 planner 优化变量的一部分，只是这些变量受到“必须是 ORCA 最优响应”的约束。

这个建模改变了信息流。传统方法是先固定人轨迹，再找机器人轨迹；SICNav 是在评估一个机器人轨迹时，同时生成与之相容的人反应轨迹。它引入的 inductive bias 是：人会进行局部 reciprocal collision avoidance，并且会在机器人接近时作出短视让行/避让反应。这个 bias 在局部 crowd navigation 中很强，因为大量真实交互确实可以近似为短期速度空间协调。

和 prior 的本质区别不是“用了 MPC”或“用了 ORCA”，而是把 ORCA 从模拟器/预测器提升为 planner 内部的 lower-level rationality constraint。这样机器人可以主动利用人的避让反应，例如缓慢逼近门口让对方调整，而不是把人当作不可改变的移动障碍。

Method

1. Relaxed ORCA lower level：它解决人的预测问题，但不是输出固定预测，而是定义“给定当前全局状态时，人应该选择什么速度”。加入 slack 的原因是原始 ORCA 在密集场景中可能无解；relaxation 让 lower-level 始终可行，并通过大惩罚保持尽量安全。核心变化是 lower-level 成为强凸 QP，解唯一，后续 KKT reformulation 才站得住。

2. Bilevel MPC upper level：机器人优化自身轨迹、控制输入、预测的人动作，同时约束机器人动力学、输入范围、输入变化、机器人-人/障碍距离。这里的关键不是 MPC cost，而是 upper-level 的安全约束与 lower-level 人响应约束共存。安全不靠 policy 学出来，而是直接写进 feasible set。

3. KKT reformulation：每个人每个时间步的 ORCA QP 被 stationarity、primal feasibility、dual feasibility、complementarity 替代，形成单层 MPCC。它解决的是 bilevel 不易直接求解的问题。理论上，lower-level 满足 Slater 且凸，KKT 对 lower-level 是必要充分；但整个问题仍非凸，局部等价依赖 LICQ。

4. Feasible warm start：用 ORCA rollout 初始化机器人和人轨迹，并为机器人加入保守的伪约束以近似非完整动力学限制。它解决的是 MPCC 在局部求解、LICQ 退化、实时运行中的脆弱性。这个部分更像 engineering glue：没有它方法可能不稳定，但它不是交互建模的核心贡献。

Key Insight / Why It Works

SICNav 有效的主要原因不是 ORCA 预测本身很强，而是 ORCA 的反应结构被放进了 test-time optimization。它利用的是 test-time compute + structured inductive bias：每次 MPC 求解都在做一个局部 Stackelberg-like counterfactual search——机器人选择动作，人按 ORCA best response 反应。相比 open-loop predictor，这直接对导航任务所需的 counterfactual 做建模。

最可能的核心贡献是“将可优化的人类反应模型嵌入安全 MPC”，而不是 KKT 推导本身。KKT reformulation 是标准 bilevel-to-MPCC 技术，实质创新在于选了一个刚好具备强凸、可解、行为上还算合理的 lower-level 模型，并把它用在 crowd navigation 的闭环规划里。

为什么能减少 freezing？因为机器人不再假设挡路的人会保持当前速度或固定轨迹，而是可以预测对方会为避免碰撞调整。于是一些在 CVMM 下不可行的路径，在 ORCA-response 下变成可行。这个机制尤其适合门口、走廊、交叉路径等局部协调场景。

哪些可能只是辅助：ORCA forecasting benchmark 主要是在为模型假设背书，但它不是导航性能的充分解释；warm start、slack、Acados/CasADi、horizon 设置等更多是让系统跑起来。仿真中 SICNav 对 ORCA agents 表现好并不意外，因为模型匹配；SFM 实验缓解了这一点，但仍是简化人群模型。真机实验规模较小，不能证明大规模真实 crowd 的泛化。

需要警惕的是，所谓“influence human motion”在论文中更像是在模型中预测人会响应，并在仿真/受控实验中观察到类似效果；它不是对真实人类心理或社会规范的显式建模。若人类不合作，系统依赖 receding horizon 反复后退来维持短期安全，这说明长期交互推理并没有真正形成。

Relation To Prior Work

SICNav 最接近三条谱系：安全 MPC / constrained planning、ORCA/RVO 类 reciprocal collision avoidance、以及 Stackelberg/game-theoretic interactive planning。它不是纯 prediction paper，也不是 RL navigation paper，更像是把 ORCA best response 作为下层 game model 的 constrained MPC。

相对 open-loop prediction + planning，真正差异是预测不再外生。CVMM、Social-GAN、Trajectron++ 这类预测器即使更准确，也通常难以在 planner 内部回答“如果机器人换一种走法，人会怎么变”。SICNav 的新增信息是 action-conditioned human response。

相对 cooperative Gaussian-process 或 learned social navigation 方法，SICNav 的优势是安全约束清晰，代价是人类模型更刚性。它没有学习复杂 social convention，而是押注局部速度避碰足够覆盖主要交互。

相对 Stackelberg driving work，SICNav 把双车/结构化道路中的 best-response 思路迁移到多人、非结构化行人场景；但它用 ORCA 简化了人的 reward 和 equilibrium 求解。严格说，博弈思想不是新思想，KKT reformulation 也不是新工具；实质创新是把一个经典 decentralized collision avoidance QP 重新组织成 MPC 内部的人类反应约束，并在真机上展示了可运行性。

Dataset / Evaluation

evaluation 分三层：ORCA 作为 open-loop trajectory forecasting 的合理性验证、仿真 crowd navigation、真机室内多人交互。这个结构基本覆盖了论文 claim 的必要证据链：先说明 ORCA 不是离谱的人模型，再说明嵌入 planner 后有交互收益，最后说明系统能在真实机器人上实时运行。

ETH/UCY 预测实验的价值有限但必要。ORCA 没有达到 SOTA，但 competitive；这足以支持“可作为可优化预测模型”，不支持“ORCA 是优秀人类行为模型”。而且该预测实验依赖 goal sampling，和在线导航中的短期 goal estimation 并不完全一致。

仿真实验最能支持核心机制，尤其是与 MPC-CVMM 的对比，因为二者共享 MPC 框架，差异主要在 closed-loop ORCA response vs fixed velocity prediction。SFM agents 的实验也重要，因为它测试了模型错配；结果显示不严重退化，但 SFM 仍是低复杂度模拟器，不等价于真实人群。

真机实验是强项但范围窄：最多 3 人、室内 VICON、预设交叉路径、受控参与者。它验证了实时性和局部交互可行性，但没有验证开放场景、密集人群、遮挡感知、非合作人群和长期导航。论文 claim 中“safe”在真实世界层面仍应理解为 empirical safety under controlled perception，而不是 formal safety under arbitrary human behavior。

Limitation

最核心的前提是人会近似遵循 ORCA：知道目标、按 preferred velocity 前进、用 reciprocal collision avoidance 避让。如果人群行为受社交关系、注意力、意图欺骗、群体结构、规则偏好影响，ORCA lower-level 会系统性错配。SICNav-np 用当前速度外推目标只是临时补丁，文中未充分说明目标估计错误在复杂场景下如何传播到 MPC 决策。

安全保证有明显边界。upper-level 确实有显式距离约束，但这些约束作用于预测的人轨迹；当真实人不按 ORCA 走时，安全依赖高频 replanning 和机器人短期可逃逸空间。论文中 adversarial human 实验显示机器人会后退，这说明鲁棒性来自 receding horizon 纠错，而不是模型本身具备对抗安全保证。

scalability 是硬上限。lower-level 约束随 agent-agent pair 增长，KKT 变量和互补约束随 N 和 horizon 膨胀；真机只展示到 3 人，作者也承认需要只选择 3–4 个重要人建模。这样一来，大规模 crowd 中所谓 joint interactive planning 会退化为局部子集选择问题，增益来源可能受到 agent selection heuristic 强烈影响。

优化层面也不干净。KKT reformulation 得到的是非凸 MPCC，局部最优与原 bilevel 的局部最优不总等价，LICQ 失败时可能出现 dual 不唯一或无界。warm start 能缓解但不能消除。这里存在一个方法学风险：理论部分证明了 lower-level KKT 的合理性，但实际系统性能很大程度取决于数值求解、初始化和 slack 设计。

此外，增益归因并不完全清楚。与 MPC-CVMM 的对比相对干净，但与 RL baselines 的比较可能混入训练、reward shaping、环境分布、实现成熟度等因素。仿真中瓶颈/门口场景天然偏向能预测 reciprocal yielding 的方法；这正是 SICNav 的优势场景，但也意味着 benchmark 对核心假设友好。

Takeaway

  • 1. 这篇论文最值得迁移的 insight 是：在安全关键交互规划中，预测器不一定要最准确，但必须能回答 action-conditioned response；可嵌入优化的结构化模型比 black-box predictor 更有规划价值。
  • 2. Bilevel / KKT reformulation 是把“人是反应者”放进 MPC 的有效模板。
  • 未来可替换 lower-level：ORCA 可以换成 learned differentiable QP、inverse-RL reward、social norm constraints 或 distributional best response。
  • 3. 对 crowd navigation 来说，减少 freezing 的关键不是更保守的 uncertainty，而是允许机器人合理利用他人的避让反应。

一句话总结

SICNav 是一篇把 ORCA 式人类局部避让模型内生为 bilevel MPC 约束的工作，真正贡献在于用可优化的 action-conditioned human response 同时获得交互性和显式安全约束，属于从 predict-then-plan 向 constrained interactive planning 演化的代表性方法。