073｜TrajOpt：用局部凸问题修复整条碰撞轨迹

系列首页 · 时间线总览

论文：John Schulman、Jonathan Ho、Alex Lee、Ibrahim Awwal、Henry Bradlow、Pieter Abbeel，Finding Locally Optimal, Collision-Free Trajectories with Sequential Convex Optimization，RSS 2013。正式论文；全文 PDF。依据正文十页的算法、碰撞推导、基准与实机示例；未独立复现。这里讨论原始 RSS 版本。

先接住前作的问题

CHOMP和STOMP已在连续轨迹上做局部改进，本篇以逐次凸化和连续碰撞处理重新组织求解。它可组合GJK类距离查询，收敛后仍需验证原约束。

五个前置概念

五个前置概念：配置空间把机器人姿态表示为一个点；顺序凸优化反复求局部凸近似；信赖域限制近似可信的更新范围；铰链损失只惩罚违反约束的一侧；扫掠体是物体在一段运动中占据空间的并集。

一、问题设定：碰撞初始轨迹怎样经局部凸化变成可行路径

把机械臂起点和终点直线连接，往往会穿过桌子。TrajOpt 不必先得到一条完美路径，而是从可能碰撞的轨迹出发，反复构建一个容易求解的局部凸近似，把整条轨迹推向无碰撞且较短的解。

问题设定与最相关前作

优化变量是轨迹上的关节配置 $\theta_1,\ldots,\theta_T$，起终点可固定，也能加入末端位姿和方向约束。本文主体是运动学规划，并未把完整动力学、力矩和接触切换统一纳入主基准。Atlas 示例保持静态稳定并预设支撑阶段，不能当作动态跑步或自动接触序列发现。

CHOMP 同样优化轨迹，但主要使用距离场和预条件梯度下降。TrajOpt 使用凸几何碰撞查询、局部凸子问题和可调整的精确惩罚。两者同时改变了多个组件，所以整体结果并不能单独归因为“二阶优化胜过一阶优化”。

对于采样式规划，失败可能来自搜索预算不足；对于局部优化，失败可能来自初始路径所在的局部结构。多初始化为局部优化提供更多机会，但也增加成本。正确比较应把所有初始化支出计入，而不是只保留最快成功的一次。

二、分开凸子问题、信赖域与外层惩罚

方法：三层循环各管什么

论文用相邻配置变化的平方和作为平滑目标：

$$f(\theta)=\sum_{t=1}^{T-1}\|\theta_{t+1}-\theta_t\|^2.$$

它鼓励较短且均匀的关节空间运动，但不是严格等于连续曲线长度；轨迹离散化和关节单位也影响其含义。将一般不等式写为 $g_i(\theta)\le0$、等式写为 $h_j(\theta)=0$，惩罚目标为

$$F_\mu=f+\mu\sum_i\max(0,g_i)+\mu\sum_j|h_j|.$$

铰链在满足约束后归零，避免已经安全的轨迹仍被无限推离障碍。足够大的 $\mu$ 在适当正则条件下具有精确惩罚性质；这不是说任意非凸问题都能靠加大系数自动求到可行全局最优。

外层调整惩罚，中层重新线性化约束，内层调整信赖域。局部模型预测改善很多但真实目标没有改善，说明步子可能太大，应缩小信赖域重新求解，而不是盲目接受一个看似漂亮的凸解。

不光滑罚项为什么仍能放进一个QP

令 $\delta$为当前轨迹的局部增量，$a$为碰撞违反量的线性化梯度，$b$为当前违反量。线性近似为 $a^T\delta+b$时，hinge罚项 $\mu\max(0,a^T\delta+b)$中 $\mu>0$是罚系数，可引入松弛变量 $s$，改写为目标加 $\mu s$，约束 $s\ge0,s\ge a^T\delta+b$。在最优点，$s$恰取两者最大值；目标与约束仍是QP可处理形式。

教学一步：当前间隙0.02米，要求0.05米，沿法向的位移为 $\delta$，则违反量为 $0.03-\delta$。若信赖域只许 $|\delta|\le0.01$，此轮至少还留0.02米违规。QP最优并不意味着碰撞约束已经满足，外层必须更新模型、扩大可接受移动或明确失败。

本篇与CHOMP、STOMP是平行轨迹优化方法；调用GJK距离类几何查询作为模块。原文第III节算法1区分罚系数、凸化和信赖域三层循环。实验成功率应依据原非线性约束最终检查，不能把子问题收敛或代价下降当作连续路径安全。

碰撞：位置安全不等于运动安全

设两个凸体的有符号距离为 $d(\theta)$，分离时为正、穿透时为负。安全约束是 $d\ge d_{safe}$，对应 $\max(0,d_{safe}-d)$。在当前配置附近，最近点与分离法向给出距离变化方向，再通过机器人 Jacobian 传回关节空间。

对相邻时刻，论文考虑两端形状的凸包来近似扫掠体。纯平移下，这是容易理解的包络；发生旋转时，仅两端凸包可能漏掉中间伸出的尖角。§V 给出扩张量 $d_{arc}=r\varphi^2/8$，$r$ 是距旋转轴的最大半径，$\varphi$ 是旋转角。

理论条件与实验实现必须分开。原文实验认为修正小于一厘米而忽略 $d_{arc}$；三维扫掠距离梯度也使用近似，未保证一般三维精确；§VI 说明连续碰撞实现没有处理自碰撞，自碰撞仅在离散点惩罚。因此不能把“论文讨论连续安全”简写成“所有实现轨迹都有严格连续安全证明”。

局部凸解还要回到原几何约束中验证，路点之间的扫掠尤其重要。下面分开三层循环，再比较单初值与多初值的总预算。

三、迭代求解并验证扫掠体，检查路点之间

输入：起终点、几何模型、初始轨迹、约束
初始化惩罚系数mu和信赖域半径s
while 未满足约束且未超预算:
    在当前轨迹计算距离、接触法向和Jacobian
    构造凸目标近似与仿射约束近似
    repeat:
        求解信赖域内的凸子问题
        用原始非凸目标检查实际改善
        若实际/预测改善足够：接受；适当扩大信赖域
        否则：缩小信赖域并重求
    until 接受或停滞
    若局部收敛仍不满足约束：增大mu
对最终轨迹做独立碰撞检查
输出可行局部轨迹或明确失败

四、读多初始化比较与计算预算

读图：上下分开看手臂与全身任务；每一类中再同时比较成功比例、时间和路径长度。多初始化改善成功率，但增加预算，高自由度结果不能沿用手臂的亚秒结论。

实验设计

优化没有使用示范轨迹训练，但多初始化包含人为选择结构。实机地图来自感知与预处理，几何简化时间和感知误差也影响完整系统，不能只拿优化求解时间代表从传感到执行的总延迟。

结果与证据强度

证据 1｜表 I：多初始化改善成功率，也增加计算。手臂 TrajOpt 单次为 0.84、0.20 秒，多初始化为 0.99、0.32 秒；RRTConnect 为 0.97、1.2 秒。这里支持该问题集上的性能优势，但单次优化成功率并非优于采样器。归一化路径长度分别约 1.2 与 1.6，不是以米计的绝对长度。

证据 2｜表 II：高自由度结果没有“一律亚秒”。全身多初始化成功率 0.84、平均 7.6 秒；单次 0.63、2.1 秒；RRTConnect 0.53、18.0 秒。它说明局部优化在该套全身问题中有价值，也纠正把简单手臂的快速数字推广到全部问题的误读。

证据 3｜图 5、§V–VI：连续碰撞机制有明确近似边界。旋转扫掠体与端点凸包不同，原文专门给图解释。能够指出这个差异，比只记住“连续碰撞检测”更接近真正理解。未补旋转界或密集复检时，不能赋予额外安全保证。

读图：中间姿态的扫掠体有助于检查两端都无碰撞但中途穿障的情况。

证据 4｜§VIII：实机可执行性的案例证据。PR2 完成垃圾搬运、开门和通过障碍等全身场景。它支持与真实感知集成的可行性，没有给出足以估计部署事故概率的大规模试验。数值与定位见原文 PDF。

五、保留局部解、几何近似与原约束检查

自构例子，未运行。细杆从门框一侧移动到另一侧，两端姿态都不碰门框，但中间旋转扫过门沿。离散点代价可能为零，扫掠体代价却能发现问题。若把杆当作两端形状凸包而忽略旋转弧，仍可能漏检；加入保守修正或减小相邻旋转角才更接近论证条件。

值得追问的实验包括：固定碰撞模型只换优化器、固定初始化数只换代价、在细障碍和大旋转场景检查连续失败率。这些干预可以区分数值方法、几何表示与初始化各自的贡献。

六、追问：多初始化版本成功率更高，怎样避免把额外预算误算成单次求解器优势？

七、连接距离查询与连续时间轨迹表示

• 模块组合：GJK：不枚举所有表面，也能求物体距离。距离查询提供局部碰撞信息，扫掠及原约束验证另行负责。
• 平行路线：GPMP2：把轨迹规划写成因子图推断。用连续时间先验和插值构造另一种稀疏轨迹优化。

八、自测与参考答案

1. 每次都解凸问题，为什么最后仍可能失败？

凸性只属于当前局部近似。原始障碍空间非凸，局部迭代可能停在坏的区域，或者信赖域内没有足够好的方向；多个初始化只能增加机会，不能保证找到全局解。

2. 一条路径的所有路标无碰撞，为什么还不够？

机器人在路标之间连续运动，中间可能穿过薄障碍或发生旋转扫掠碰撞。必须检查区间运动，而不仅是离散姿态。

3. 原始实验是否证明全部轨迹严格连续无碰撞？

不能这样概括。实现忽略了较小的旋转修正，连续代价没有覆盖自碰撞。应依据实现条件与独立复检判断，不能把理论构造的条件省略。