IEEE Transactions on Robotics

Safe and Dynamically Feasible Motion Planning Using Control Lyapunov and Barrier Functions

作者：Pol Mestres, Carlos Nieto-Granda, Jorge Cortés · 机构：Department of Mechanical and Civil Engineering, California Institute of Technology, Pasadena, CA, USA; DEVCOM U.S. Army Research Laboratory (ARL), Adelphi, MA, USA; Department of Mechanical and Aerospace Engineering, University of California San Diego, La Jolla, CA, USA

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

路径规划运动规划控制优化安全

精读笔记

Problem Setting

Safe and Dynamically Feasible Motion Planning Using Control Lyapunov and Barrier Functions（IEEE Transactions on Robotics / 2025）关注的是一个很具体但长期被低估的问题：sampling-based planner 给出的 waypoint path 往往只是几何意义上的可行，而不是控制意义上的可执行。对 CLF-CBF controller 来说，相邻 waypoint 是否能安全稳定连接，不是由两点间是否无碰撞决定的，而是由整个闭环经过区域内 CLF 下降约束与 CBF 安全约束是否同时可行决定。

真正困难点是局部连接的 certificate。传统 kinodynamic RRT 会卡在 BVP 或 forward propagation；CBF-RRT 类方法绕开 BVP，但靠闭环仿真决定新节点，代价高且安全只受数值积分精度影响；几何 RRT 更严重，它生成的边可能在执行时导致 CLF-CBF QP infeasible。本文的关键矛盾是：想保留 RRT 的搜索效率和概率完备性，同时又不接受“先规划、后祈祷 controller 能跟上”的分层假设。

Motivation

作者的核心动机不是提出一个更快的 RRT variant，而是补上 planner 与 CLF-CBF controller 之间的可行性断层。已有路线缺的是一种边级别的、无需 rollout 的反馈可执行性证明。只检查路径几何安全不够，因为 CLF 会把系统拉向 waypoint，CBF 会把系统推离障碍，两者在障碍附近可能相互冲突；只仿真一条闭环轨迹也不够，因为它无法证明 controller 在整个吸引区域内递归可行。

论文的关键观察是：如果候选边对应的 Lyapunov sublevel set 内，CLF-CBF QP 对所有状态都可行，那么这条边天然是安全且动态可执行的。于是 planner 的工作不再是生成一条可跟踪轨迹，而是生成一串 waypoint，每个 waypoint 自带一个局部安全稳定反馈 funnel。这是本文最重要的建模转向。

Core Idea

核心思想可以概括为：把 RRT 的 edge validity 从 collision-free segment 改成 CLF-CBF compatibility over a region。给定候选新节点 x_new，构造以它为目标的 CLF V；用 Θ={x: V(x)≤V(x_near)} 表示从 x_near 在 CLF controller 下会被困住并下降的区域；然后验证在 Θ∩F 中是否存在控制输入同时满足 CLF 下降和所有相关 CBF 约束。若成立，则不需要知道具体轨迹，也能知道从 x_near 出发执行 QP controller 会保持安全并收敛到 x_new。

这与 prior 的本质区别在于，它不是在 state trajectory 上做局部安全检查，而是在一个 Lyapunov-defined invariant-like region 上做 controller feasibility 证明。信息流也被重新组织了：RRT 不再调用 simulator 来产生证据，而是调用 compatibility oracle 来决定边是否存在。这个 oracle 是本文的 inductive bias：只接受能被 CLF-CBF feedback funnel 覆盖的边，因此搜索空间被限制到“可反馈执行”的路径族。

Method

1. CLF-CBF compatible path：它定义了论文真正想输出的对象。路径中的每一段都要求存在一个以后一 waypoint 为目标的 CLF，并且在对应 sublevel set 内 CLF-CBF QP 处处可行。这个定义把动态可行性从单条轨迹性质提升为区域反馈性质，是后续理论能成立的基础。

2. Compatibility verification：本文用 Farkas lemma 分析一组关于 u 的 affine inequalities 的同时可行性。直觉上，如果 CLF 约束法向量与 CBF active constraints 的法向量不发生坏的锥依赖，则可行；如果发生依赖，则需要检查 drift 和 margin 项是否仍允许共同满足。对应到论文中的 ζ1/ζ2：ζ1 检查法向退化，ζ2 检查退化时的常数项裕度。这个机制是核心贡献，不是工程细节。

3. Tractable special cases：一般 compatibility 优化非凸，作者没有回避这一点，而是把可落地性放在线性系统、二次 CLF、多面体/椭球/圆障碍这些结构化情形上。此时问题变成 QCQP 或闭式条件。这里的贡献不是“QCQP 很新”，而是把 CLF-CBF 可行性证书降到可在 RRT loop 中调用的复杂度。

4. C-CLF-CBF-RRT：算法本身基本继承 GEOM-RRT 的采样、nearest、steering，只是把 edge acceptance 改成 COMPATIBILITY。若检查失败，则通过增大 CBF class-K gain、减小 CLF decrease W 来放宽可行域。这个增益调节更像 feasibility heuristic；理论上有单调放宽直觉，但实际性能中有多少来自这个 heuristic，文中未充分说明。

Key Insight / Why It Works

这篇论文最有效的地方不是 RRT，而是对 CLF-CBF QP infeasibility 的几何刻画。CLF 和 CBF 约束都是控制输入空间中的半空间；多约束不可行只有在这些半空间法向形成某种对抗结构时发生。Farkas lemma 正好把这种对抗结构变成乘子证书。ζ1/ζ2 的设计本质上是在搜索“是否存在一个状态，使 CLF 下降方向正好被 CBF 安全方向锥阻塞”。这比沿轨迹采样检查更强，因为它覆盖的是区域而不是点。

最可能的核心贡献是 compatibility oracle，而不是 C-CLF-CBF-RRT 这个外壳。RRT 只提供探索机制；真正新增的信息是：每条边必须有一个 CLF-CBF feedback certificate。这个 certificate 让算法避免 BVP、避免闭环 rollout，也避免了只在离散点检查 CBF 的安全漏洞。

哪些部分可能只是辅助：增益更新 α↑、W↓ 是实用上重要但理论上比较粗糙的 feasibility enlargement；它可能显著影响成功率，但更像 engineering knob。实验中 differential-drive robot 被转成单积分器，障碍为圆/多边形，这正好落在本文最 tractable 的设置内，因此性能增益很可能主要来自问题结构与 oracle 便宜，而不是算法在一般非线性系统上的天然 scalability。

从机制归因看，这不是 scaling/data/retrieval 类工作，而是 stronger inductive bias：规划器只搜索可由局部 Lyapunov funnel 串接的路径。它牺牲了一部分路径自由度，换来 controller feasibility 的可证明性。这个 bias 对安全关键机器人很合理，但也意味着可达路径族可能比真实动态可行路径更保守。

Relation To Prior Work

最接近的谱系有三条：kinodynamic RRT / RRT*，CBF-RRT 类安全采样规划，以及 CLF-CBF-QP 控制。本文的差异不是把这些模块简单拼起来，而是把 RRT 的 local planner 替换为 CLF-CBF compatibility verifier。

相对 kinodynamic RRT，它避免求解 BVP 或 forward propagation，但代价是需要 CLF 和区域可行性优化。相对 CBF-RRT，它不通过仿真轨迹来扩展树，因此不会把安全性寄托在积分步长上。相对 LQR-CBF-RRT*，它不是在 reference trajectory 的有限点上检查 CBF，而是在 Lyapunov sublevel set 上验证 QP 可行性，因此理论安全语义更强。

看似新的部分中，RRT 框架、CLF-CBF-QP、HOCBF 都是已有工具；实质创新是 compatibility checking 作为 edge certificate，以及把这个 certificate 嵌入 RRT 后证明概率完备。换句话说，本文不是提出新的 controller，也不是新的 sampling strategy，而是提出了一种 planner-controller interface。

Dataset / Evaluation

实验覆盖仿真和真机，使用 differential-drive robot，并通过坐标变换把控制点动力学化为单积分器。场景包含圆形和多边形障碍，正好匹配文中闭式/QCQP compatibility 的结构化假设。真机实验说明该方法不是纯离线理论，但系统复杂度仍较低。

评价基本支持两个核心 claim：第一，GEOM-RRT 生成的几何路径可能导致执行时 CLF-CBF QP infeasible；第二，避免 rollout 的 compatibility check 相比 CBF-RRT 有明显计算优势，并相比有限点 CBF 检查有更强安全语义。不过实验没有充分证明一般控制仿射非线性系统、高维系统、多机器人或强输入约束下的可扩展性。benchmark 主要验证了“在适配结构下方法很好用”，而不是验证“general nonlinear motion planning 被解决”。

Limitation

最大限制是问题被转移了：从 BVP/trajectory rollout 转移到 CLF 构造和区域 compatibility 优化。对一般非线性系统，FIND_CLF 本身就是难题；compatibility 优化也可能非凸且高维，若只用局部解，理论保证会断。论文在理论上要求全局求解 ζ1/ζ2，这在复杂机器人上并不现实。

第二，概率完备性假设很强：需要存在 CLF-CBF compatible path，而不是任意动态可行路径；需要 waypoint 附近有可采样正测度邻域；需要 FIND_CLF 返回正确连续的 CLF family；需要 COMPATIBILITY 找到全局解；还需要 steering radius 覆盖这些邻域。这是合理的数学定理，但不是无条件的 RRT-style completeness。

第三，方法可能保守。CLF sublevel set 必须整体通过 CBF compatibility 检查，这比实际闭环轨迹安全更强；很多真实可行边会因为 funnel 太大或 CLF 选择不佳而被拒绝。增大 α、减小 W 可以缓解，但会降低 CLF 收敛强度，甚至导致 waypoint 附近慢速收敛。文中提到这一现象，但没有从规划层面根治。

第四，输入约束、不确定性、感知误差、障碍动态变化基本没有实质解决。输入约束可通过 Farkas lemma 扩展，但会进一步加重 compatibility 检查；模型误差下 CBF/CLF certificate 的鲁棒裕度文中未充分说明。

Takeaway

  • 1. 最值得迁移的 insight 是：sampling-based planning 的边可以不是几何对象或仿真轨迹，而是一个 feedback feasibility certificate。
  • 这个思想可推广到 MPC feasibility funnels、contraction funnels、reachability tubes 等。
  • 2. CLF-CBF 组合的核心问题不是 controller QP 怎么写，而是 QP 在相关区域是否递归可行。
  • 本文把这个问题前置到 planner 中，是 planner-controller co-design 的正确方向。

一句话总结

这篇论文把 RRT 的局部连接从轨迹生成问题改写为 CLF-CBF 区域兼容性验证问题，是一类用反馈可行证书重构采样式安全运动规划的代表性工作。