IEEE Transactions on Robotics

Smooth Distances for Second-Order Kinematic Robot Control

作者：Vinicius Mariano Gonçalves, Anthony Tzes, Farshad Khorrami, Philippe Fraisse · 机构：Center for Artificial Intelligence and Robotics (CAIR), New York University Abu Dhabi, Abu Dhabi, UAE; New York University Abu Dhabi, Electrical Engineering, Abu Dhabi, UAE; New York University, Electrical and Computer Engineering Department, Brooklyn, NY, USA; Laboratoire d'informatique, de robotique et de microelectronique de Montpellier (LIRMM) Montpellier, Montpellier Cedex 5, France

会议/期刊：IEEE Transactions on Robotics · 发布日期：2024 · 页码：2950-2966

控制操作安全

精读笔记

Problem Setting

论文标题：Smooth Distances for Second-Order Kinematic Robot Control（IEEE Transactions on Robotics / 2024）。

这篇论文解决的不是一般 collision checking，也不是更快的最近点查询，而是一个更具体也更尖锐的问题：在加速度级/二阶运动学控制中，避障约束需要距离函数对配置 q 至少二阶可微，但传统欧氏物体距离在 witness points 不唯一时不可微。这个问题在一阶速度控制里已经麻烦，在二阶控制里更致命，因为约束会包含 \dot J_c \dot q 或 Hessian 相关项；一旦距离 Jacobian 跳变，QP 可能直接 infeasible。

关键矛盾是：真实几何距离有明确安全含义，但它的 nonsmoothness 来自几何本身，尤其是平面-平面、圆柱-平面、盒子边/面等常见机器人 collision primitive；而控制器需要的是一个连续、可微、Hessian 不爆炸的 surrogate。以前方法通常在两端妥协：要么改变几何对象使 witness point 唯一，例如严格凸包围体；要么对距离函数做外部 smoothing；要么工程上接受不可微并靠滤波/阻尼兜底。这篇论文试图给一个几何层面的平滑距离定义，让控制器看到的距离本身就是 smooth 的。

Motivation

作者的核心观察是：nondifferentiability 的根源不是距离数值本身，而是 hard argmin / hard projection 在多个最优点之间切换。因此，与其事后平滑 distance(q)，不如从 point-to-set projection 这个最底层算子开始软化。

已有路线不够的地方在于：严格凸包围体会改变几何，且会引入保守性；传统距离算法再快也不能解决 Jacobian 不存在；对配置空间函数做 bundled objective/gradient 之类的 smoothing，需要在 q 附近多次评估原函数，实时控制中代价很高，而且原函数本身的 witness point 切换仍然被隐藏在采样平均里。论文缺的是一个“单点 q 上即可计算、结构上仍像传统 witness-point distance、但光滑性有保证”的距离替代物。

Core Idea

核心思想可以理解为把最近点查询从 hard-min 改成热力学平均：点 p 到集合 A 的 half-squared distance 不再取 \min_a \|p-a\|^2/2，而是对集合内所有点按 \exp(-\|p-a\|^2/(2h^2)) 加权积分，再取 -h^2 log。h 越小越接近 hard min；h > 0 时变成 smooth soft-min。对应的“投影点”不再是单个最近点，而是 Gibbs 分布下的期望点，即 soft witness point。

本质区别在于，它不是在配置空间 q 上做黑盒函数平滑，而是在几何空间里重定义 projection operator。这样带来的 inductive bias 很强：距离梯度始终由一个连续变化的软 witness point 给出，而不是由离散切换的最近点给出。更重要的是，作者没有直接用 \|a^*-b^*\|^2/2 作为 set-to-set distance，而是构造了 cross-distance，使 fixed point 处满足 stationarity；因此对 q 求导时可以像 envelope theorem 一样忽略 witness point 的隐式导数。这是整篇论文最关键的结构设计。

Method

1. Smooth point-to-set distance：用带权 log-integral soft-min 替代真实点到集合距离。它解决的是 hard projection 不可微的问题。h 控制 soft-min 温度，R 是 regularization/weight scale，用于强 log-concavity 和收敛证明。核心变化是：projection 从 argmin 变成概率加权期望。

2. Soft projection and covariance Hessian：梯度满足 \nabla_p D_{h,R}^A(p)=p-\Pi_{h,R}^A(p)，其中 \Pi_{h,R}^A 是 soft projection。其 Jacobian 等于加权分布协方差除以 h^2。这给了一个很有用的解释：局部曲率来自 witness distribution 的扩散程度；h 小时分布尖锐，曲率可能变大，接近真实 nonsmooth limit。

3. Modified alternating projection：把 von Neumann alternating projection 中的真实投影换成 soft projection。对凸标准集合，借助 strong convexity / contraction 证明 fixed point 唯一且迭代收敛。这一步解决 set-to-set witness point 非唯一的问题。

4. Cross-distance construction：定义 D_h^A(b)+D_g^B(a)-\|a-b\|^2/2，并在 soft alternating projection 的 fixed point 处取值作为 set-to-set smooth distance。它的必要性在于让 fixed point 成为 stationary point，从而在 moving sets 下计算 q-gradient 时消掉 \partial a^*/\partial q 和 \partial b^*/\partial q。

5. Rigid-body Jacobian formula：对于机器人链接这种刚体变换集合，最终距离 Jacobian 形式几乎与传统最近点距离一致，只是 witness points 换成 smooth witness points。这一点对工程落地很重要：它没有迫使控制框架重写几何微分结构。

6. Second-order QP controller：控制器本身是加速度级 QP + 二阶 CBF 约束。它的作用主要是展示 smooth distance 的必要性，而不是论文最深的创新。这里 smooth distance 解决的是 \dot J_c \dot q 需要二阶可微的问题。

Key Insight / Why It Works

最核心的 insight 是：不可微性来自 hard selection，而不是来自“距离”这个标量本身。只要把最近点选择变成一个有温度的分布平均，witness point 的跳变就变成连续迁移。这个结构比在 q-space 做后处理 smoothing 更干净，因为它保持了几何梯度的 witness-point 形式。

这篇最可能的实质贡献不是“提出一个平滑函数”——log-sum-exp soft-min 是老思想；真正贡献是把 soft-min、soft projection、alternating projection fixed point 和机器人刚体 Jacobian 组织成一个闭环，使 set-to-set distance 的导数仍然像传统距离那样可写。这种 envelope-theorem-like cross-distance 设计是关键，否则 smooth witness points 本身对 q 的隐式导数会让控制实现复杂很多。

有效性的另一部分来自 contraction。R 的引入看起来有点技术性，但它服务于证明 strong convexity 和 fixed point 唯一性。直观上，soft projection 不再贴着集合边界做硬切换，而是被核平均拉回集合内部/凸包内，因而组合映射更容易成为 contraction。

哪些可能只是辅助：二阶 QP 框架、joint limit/velocity limit 约束、数值差分 \dot J、primitive 近似公式等更多是应用包装。论文的控制实验说明 smooth distance 有用，但并不证明该 QP 框架本身优于其他 acceleration-level CBF 控制器。

是否是 scaling？不是。主要增益不是来自更大数据或更多算力，而是更合适的 inductive bias：用概率化 soft witness distribution 替代 hard closest-point combinatorics。是否是 test-time compute？部分是，因为 smooth witness points 通过迭代 fixed point 求解，且点云版本 O(m)；但相比 bundled smoothing，它把 test-time compute 从 q-space 多次采样转移到 object-space 一次积分/求和。这个转移是有意义的。

需要注意，smoothness 是有代价的：h 越大越平滑但越偏离真实距离；h 越小越准确但曲率和数值问题回归。论文没有消灭这个 tradeoff，只是把它参数化并使其可控。

Relation To Prior Work

最接近的谱系有三条：第一是 convex geometry / alternating projection / witness point distance；第二是 log-sum-exp / entropy-regularized min / softmax smoothing；第三是机器人 CBF/QP 避障控制。

相对严格凸包围体方法，本文不是通过改变物体形状来保证最近点唯一，而是改变距离算子本身。这一区别很重要：严格凸化通常带来几何保守性，并且对象表示受限；本文保留原始凸集合，只是把点到集合的 projection 软化。不过它也不是无代价，因为 smooth distance 不是真正 metric，碰撞时不保证为零，仍需安全 margin。

相对 Suh et al. 的 bundled objective/gradient 这类外部 smoothing，本文的本质差异是 smoothing 发生在 object space 而非 configuration space。前者需要在 q 附近评估多个原始距离/梯度，后者在当前 q 上通过 soft witness 直接得到 smooth distance。这在实时控制里是明显更合适的信息流。

相对传统 GJK/EPA 或 primitive-specific closest point 算法，本文不是更快地求真实最近点，而是求一个带温度的 surrogate 最近点。它牺牲了精确几何语义，换取可微性和更稳定的二阶控制。

看似新的部分中，soft-min 本身不是新思想；用 sampled points 得到 LogSumExp 形式也不新。实质创新在于：给出 continuous convex set 上的 smooth projection 定义、收敛 fixed point 证明、以及与刚体运动距离 Jacobian 的结构兼容性。

Dataset / Evaluation

Evaluation 更像 targeted validation，而不是广泛 benchmark。实验包括随机 primitive 对的收敛/计算时间测试，以及一个 7-DoF Kuka 机械臂穿过带孔墙的真机实验。它验证了论文最关心的场景：传统距离在多个 witness points 时会导致 \dot J_c \dot q 巨大、QP infeasible，而 smooth distance 能让同一类加速度级约束工作。

真机实验是加分项，因为它直接覆盖了二阶控制、实时 QP、多 primitive 障碍约束、狭窄通道等组合难点。但实验范围仍然窄：一个机器人、一个主要环境、primitive-based convex collision model。它没有系统比较不同 h/R/margin 的安全-可达性 tradeoff，也没有展示复杂非凸 mesh、动态障碍、大规模多体碰撞场景。

benchmark 是否支持 claim：支持“smooth distance 能解决传统距离在二阶控制中不可微导致的不稳定/不可行”这个 claim；不充分支持“方法在一般机器人场景中更 scalable”这个更广泛 claim。点云版本的 evaluation 主要是时间复杂度展示，不是理论 guarantee 的替代。

Limitation

1. Convexity 和 standard set 假设很关键。理论最强的部分依赖标准凸集、非零体积、有限 R/h。非凸物体需要分解成凸 primitive 或点云近似；此时全局几何语义和理论保证都会弱化。

2. 点云版本缺最关键保证。作者明确说离散近似下 Proposition 3 和 6 不保留，也就是 strong convexity 和 alternating projection 收敛证明缺失。实际工程中很多复杂形状会走点云路线，这使理论与部署之间有缺口。

3. 它不是真实距离。smooth distance 不保证碰撞时为零，且通常大于真实距离。安全依赖 margin δ，而 δ 如何与 h,g,R,S、物体尺寸、控制频率、执行误差联合选择，文中未充分说明。这是 deployment 中最实质的 tuning risk。

4. h 的 tradeoff 没有被消除。h 小时更接近真实距离，但 Hessian 可能变大、数值 underflow 更严重、收敛界变差；h 大时控制更平滑但几何 bias 更大，可能影响狭窄通道可行性。论文展示了可调，但没有给系统设计准则。

5. 二阶控制只给 Lyapunov stability，不保证 asymptotic convergence。作者也承认可能停在 spurious equilibrium。实验中到达目标不等于理论上保证任务完成。

6. Feasibility 条件偏强。加速度约束、障碍 CBF、joint limit 叠加时，QP feasibility 是核心问题；smooth distance 只解决 Jacobian/Hessian 不连续导致的 infeasibility，不解决本质动力学/几何不可行。

7. 对动态模型的抽象较粗。控制假设是 double integrator \ddot q=u，真机通过位置接口和数值积分实现。真实 torque limits、delay、tracking error 对 safety margin 的影响没有系统分析。

8. 增益归因清楚但范围有限。相对 h=0 的失败案例很有说服力，但没有与其他 smooth collision proxy、signed distance field smoothing、CHOMP/STOMP-style cost、differentiable collision libraries 做全面对比。

Takeaway

  • 1. 最值得迁移的不是具体公式，而是“在几何原语层面软化 argmin，而不是在控制/优化外层修补 nonsmoothness”。
  • 这对 differentiable collision checking、trajectory optimization、contact-aware planning 都有价值。
  • 2. Smooth witness point 是一个很好的抽象：它保留了最近点方法的可解释性，同时避免 hard witness switching。
  • 很多几何优化问题都可以用类似 Gibbs witness distribution 替代 discrete closest feature selection。

一句话总结

这篇论文把机器人避障中的最近点距离从 hard geometric projection 推进到 entropy-smoothed soft witness projection，并通过 fixed-point 与 envelope-style 距离定义使其成为适合二阶 QP/CBF 控制的可微几何原语。