IEEE Transactions on Robotics

Can Not Touch This: Real-Time, Safe Motion Planning and Control for Manipulators Under Uncertainty

作者：Jonathan Michaux, Patrick Holmes, Bohao Zhang, Che Chen, Baiyue Wang, Shrey Sahgal, Tiancheng Zhang, Sidhartha Dey, Shreyas Kousik, Ram Vasudevan · 机构：Robotics Institute, University of Michigan, Ann Arbor, MI, USA; Agility Robotics, Albany, OR, USA; Mechanical Engineering, Georgia Institute of Technology, Atlanta, GA, USA; Mechanical Engineering, University of Michigan, Ann Arbor, MI, USA

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

路径规划运动规划控制优化操作

精读笔记

Problem Setting

论文标题：Can Not Touch This: Real-Time, Safe Motion Planning and Control for Manipulators Under Uncertainty（IEEE Transactions on Robotics / 2025）。

这篇论文面对的是一个比普通 collision-free motion planning 更窄但更硬的问题：机械臂在未知但有界的惯量/负载参数下，实时生成可执行轨迹，并保证连续时间内不碰撞、不超关节位置/速度限制、不超力矩限制。这里的“安全”不是经验上的低碰撞率，而是给定模型假设下的集合包含关系。

真正困难点在于约束之间耦合得很紧：碰撞约束取决于实际轨迹而不是期望轨迹；实际轨迹取决于控制器和动力学不确定性；力矩约束又取决于轨迹、误差、惯量和控制律。以前很多方法把这些耦合拆开：规划器只看运动学，控制器负责追踪；或者动力学优化只对 nominal model 和离散节点检查；或者 reachability 可以验证但难以实时嵌入优化。ARMOUR 要解决的正是这个接口断裂问题。

关键矛盾是：要有严格安全保证，就需要对连续时间、所有不确定参数、所有可能跟踪误差做 worst-case 包络；但包络一旦太保守，在线优化就不可行，尤其在 clutter/narrow passage 中会直接失去可用性。论文的核心工作就是在“可证明安全”和“不过度保守到无法规划”之间找到一个可实时计算的表示。

Motivation

作者的出发点不是再做一个更强的轨迹优化器，而是指出现有 manipulator planning/control pipeline 缺少一个能被优化器实时使用的 safety certificate。

纯运动学规划的问题是，它默认底层控制器能把轨迹变成真实运动，但在负载不确定、力矩受限时这个假设经常不成立。MPC/trajectory optimization 的问题是，虽然可以包含动力学，但通常依赖准确模型，并且在离散时间点施加约束；连续时间碰撞和中间时刻力矩峰值容易漏掉。可达性方法理论上适合安全保证，但传统表示在机械臂高维空间里保守或计算重，难以成为在线优化内循环。

作者的核心观察是：如果直接优化状态/控制序列，安全约束很难连续时间化；但如果限制到一族解析参数化轨迹，并用集合表示一次性包住整族轨迹的时空占据与所需输入，那么在线阶段只需在低维参数 k 上做 constrained optimization。换言之，缺口不是“没有安全控制器”或“没有碰撞检测”，而是缺少一种能把控制误差、动力学不确定性、连续时间 occupancy 和优化变量对齐到同一 representation 的机制。

Core Idea

ARMOUR 的真正核心是把 planning problem 从“找一条轨迹并希望它能被安全跟踪”改成“在一族轨迹上预先构造闭环可达管，并只选择那些可达管安全的参数”。这个建模方式改变了规划器和控制器之间的信息流：控制器不再是后处理模块，而是先给规划器一个 uniform tracking error contract；规划器在这个 contract 内做连续时间 collision/limit checking；PZRNEA 再把同一个 contract 转换成 torque feasibility constraint。

这带来的 inductive bias 很明确：不是尝试在完整状态-控制空间中自由搜索，而是把搜索限制在低维、解析、可微、可集合传播的轨迹族中。这样牺牲了一部分表达能力，换来可证安全和实时性。相比 prior 的本质差异不在于“用了 zonotope”这类表面选择，而在于它把一整族候选轨迹的安全性预编译成可微集合约束，使优化器不需要每次重新做复杂验证。

为什么理论上可能有效：机械臂 rigid-body dynamics 通过 RNEA 有递推结构，解析轨迹通过 PZ 可以保留参数依赖，passivity controller 可以给出误差管。三者刚好都能被集合运算封装。ARMOUR 的价值就是把这三个结构拼成一个闭环安全证书。

Method

1. 轨迹族而非任意轨迹：每个 receding-horizon step 只优化参数 k，轨迹是满足起始状态匹配、末端可刹停、加速度连续有界的解析函数。它解决的是实时优化维度和 fail-safe 问题。核心变化是：复杂规划被压缩为低维参数搜索，但也因此表达能力受限。

2. robust passivity-based controller：它解决期望轨迹和真实轨迹在惯量不确定下不一致的问题。控制律由 nominal inverse dynamics/passivity term 加 robust compensation 组成，利用 worst-case disturbance bound 保证 modified tracking error r 在用户指定阈值内。这个模块的必要性在于，没有 uniform error bound，后续 occupancy inflation 就没有理论基础。

3. tracking error buffer：位置和速度误差界被直接转成 configuration/velocity PZ buffer。它解决的是“规划器看到的轨迹必须覆盖真实闭环运动”的问题。核心变化是安全约束不再施加在 desired trajectory 上，而是施加在 desired trajectory + worst-case tracking tube 上。

4. PZ forward occupancy：它把时间区间、轨迹参数和误差 buffer 一起传播到 workspace swept volume。它解决连续时间碰撞检查。PZ 的关键作用是保留参数依赖，避免普通 interval/zonotope 在多次非线性映射后过度膨胀，同时保留可微性。

5. PZRNEA：它把 RNEA 从点值逆动力学扩展成集合逆动力学。它解决的是力矩约束必须对所有时间、所有跟踪误差、所有惯量不确定成立的问题。核心变化是 torque limit 不再是 nominal check，而是 robust reachable input set containment。

6. 在线优化和安全停止：每轮规划时同步执行上一条轨迹，若新优化未在时间预算内返回，则执行上一条轨迹中内置的 braking maneuver。它解决 receding-horizon 中 solver failure 的安全性，而不是性能问题。

Key Insight / Why It Works

最核心的 insight 是：安全规划不一定要直接求解闭环系统的精确 reachable set；只要控制器能给出一个足够简单且可验证的 uniform error tube，规划器就可以在这个 tube 上做 conservative geometry/dynamics checking。ARMOUR 的理论成立依赖这个 contract：controller 保证真实状态落在 tube 内，planner 保证 tube 不碰撞且不超限。因此安全性来自 contract composition，而不是某个单一模块的强大。

最可能的实质贡献有两个。第一是 PZRNEA：它把“整族轨迹 + 不确定惯量 + 跟踪误差”映射到输入集合，使动力学可执行性进入在线规划约束。这比只做 collision reachable set 更关键，因为实际机械臂安全经常被 torque saturation 破坏。第二是 PZ 表示对参数依赖的保留：这直接减少 conservativeness，并允许对约束做解析梯度优化。相比 ARMTD 的 zonotope 版本，这可能是性能提升的主要来源。

robust controller 本身是重要但更像 enabling component。它的 novelty 在于给同样误差界时需要更小 robust input bound，从而降低 torque constraint 的保守性；但从谱系看，它仍属于 passivity/robust control + interval disturbance bounding 的延伸。它真正的价值不是控制性能本身，而是产出规划器可消费的误差界。

哪些可能主要是 engineering/scaling：GPU 并行构造 PZ constraint、CUDA/C++ 实现、IPOPT 加解析梯度、短 horizon 和低维 Bernstein 参数化都对实时性非常关键。这些不是概念上的核心突破，但没有它们 claim 很难成立。文中没有完全拆分“PZ 表示更紧”与“实现更强/并行更多”各自带来的增益。

这不是 data scaling、retrieval 或 implicit memorization 型工作；它的核心是 better inductive bias + test-time constrained optimization + set-based representation alignment。它把机械臂结构、轨迹解析性、刚体动力学递推结构和控制误差界对齐到同一个集合优化框架中。

Relation To Prior Work

最接近的技术谱系是 reachability-based manipulator trajectory design，尤其作者自己的 ARMTD。ARMOUR 与 ARMTD 的本质差异不是名字从 TD 到 OUR，而是从 deterministic kinematic reachable set 走向 uncertain dynamic closed-loop reachable set：它显式考虑惯量不确定、跟踪误差和 torque constraints，并用 PZ 降低 reachable occupancy 的保守性。

和 CHOMP/TrajOpt/STOMP 类优化规划相比，ARMOUR 不是更好的 smoothness/collision cost 设计，而是把安全约束做成 conservative set containment。CHOMP 可以给漂亮路径，但其 collision cost/离散检查不等价于连续时间安全证书；ARMOUR 的优势正是在 claim 的语义上更强。

和 MPC/differential dynamic programming 类方法相比，ARMOUR 的不同点在于它不依赖准确动力学，也不只在节点上检查约束。代价是控制/轨迹空间更受限，且约束更保守。RAPTOR/Aligator 这类 baseline 更像“强优化器 + nominal/true model”，而 ARMOUR 是“受限轨迹族 + robust set certificate”。

和 adaptive/sliding-mode/robust control 文献相比，ARMOUR 不是单纯改控制律；控制律只是生成闭环误差管。真正新增的信息是把这个误差管接入在线 motion planning，并通过 PZRNEA 使输入限制也成为 robust planning constraint。

很多思想并非全新：receding horizon、fail-safe braking、passivity-based robust control、zonotope/PZ reachability、RNEA 都已有基础。实质创新在于组合方式：把它们组织成一个可以实时优化、连续时间安全、考虑动力学不确定和输入约束的统一 pipeline。

Dataset / Evaluation

评估覆盖了随机 clutter、thin obstacles、不同 high-level planner，以及 Kinova Gen3 真机携带 dumbbell 的场景。它基本验证了论文最重要的 claim：在已知有界惯量不确定和静态障碍下，ARMOUR 可以实时生成安全运动，并且比只做运动学/离散/nominal 动力学的 baseline 更不容易出现碰撞或不可执行轨迹。

最有价值的是 thin obstacles 和真机负载实验。thin obstacles 检验连续时间 occupancy 的必要性；dumbbell 实验检验惯量不确定下控制-规划闭环是否能落地。随机障碍实验更多是在说明在线速度和一般 clutter 可用性。

但 evaluation 也有明显边界。任务主要是关节空间 start-goal reaching，不是复杂接触操作或长时序 manipulation。障碍静态、几何外包络已知，感知不确定基本不在范围内。真实世界实验展示了可运行性，但规模有限，不能证明对广泛 household/industrial manipulation 的泛化。baseline 对比总体合理，但不同方法的模型权限、时间预算、约束语义差别很大，因此不应把结果解读为“ARMOUR 优化性能全面更强”，而应解读为“在强安全语义下，ARMOUR 更稳”。

Limitation

核心限制是保守性和表达能力的交换。ARMOUR 的安全性来自 overapproximation；只要速度高、负载不确定大、障碍间隙窄、link geometry 外包络粗，reachable set 就会膨胀，导致优化不可行或只能选择慢速运动。论文也承认 narrow passage 中速度越高越难通过。这个不是简单调参能消除的问题，而是 set-based worst-case safety 的结构性代价。

第二，方法严重依赖已知有界不确定集。如果真实负载、摩擦、柔性、接触、关节延迟或控制接口误差超出建模范围，保证就不再成立。文中对 zero-order hold、控制延迟、实际低层伺服器动态的处理不充分，理论假设和机器人软件栈之间仍有 gap。

第三，scalability 有上限。PZ 在非线性映射、三角函数、递推动力学中会产生 generator/order 增长，论文通过固定时间片、有限阶近似、并行计算和低维参数化控制复杂度。更高 DOF、更复杂 link mesh、更长 horizon、更密障碍场景下是否仍实时，文中未充分说明。

第四，长期规划能力被转移给 high-level planner。ARMOUR 本身只是在局部轨迹族内选择安全参数；如果 waypoint 不好，方法会安全停止而不是自动发现复杂全局策略。所谓 receding-horizon planning 并不等于形成了强长期状态建模。

第五，增益归因不完全清晰。相对 ARMTD 的提升可能来自 PZ 更紧，也可能来自不同实现/参数/优化细节；相对 CHOMP/MPC 的优势部分来自安全语义更强，而不是同一问题定义下更高效。文中对这些因素的 ablation 不够彻底。

Takeaway

  • 1. 值得迁移的核心思想是“controller 先给 planner 一个可证明 tracking contract，planner 只优化 contract 内安全的轨迹族”。
  • 这比传统 planner-controller 分层更适合安全关键机器人。
  • 2. PZRNEA 代表了一类有前景的方向：把经典机器人递推算法从点值计算提升为集合计算，使动力学约束可以直接进入 robust planning。
  • 类似思想可迁移到接触力、摩擦锥、执行器热约束等问题。

一句话总结

ARMOUR 是一篇把 reachability、robust control 和 inverse dynamics 集合化后接入实时轨迹优化的工作，真正贡献在于为不确定动力学机械臂提供了一个闭环一致的连续时间安全规划证书，而不是单纯提出一个更快的局部规划器。