前面几篇分别讲了轨迹平滑、时间参数化、碰撞检测、运动学约束。今天把这些问题合在一起——怎么在一个统一的框架里同时处理所有约束。答案是两个经典的轨迹优化框架CHOMP和TrajOpt。CHOMPCovariant Hamiltonian Optimization for Motion Planning和TrajOptTrajectory Optimization是CMU和Stanford分别在2009-2013年间提出的后来都集成到了MoveIt中。两者都是基于梯度的轨迹优化方法但处理约束的策略不同——CHOMP把约束融入目标函数TrajOpt用信赖域方法处理约束。一、CHOMP——基于协方差梯度CHOMP的核心思想把轨迹表示为一系列waypoint的序列定义目标函数为平滑度碰撞代价然后用梯度下降优化。目标函数的两项平滑度项轨迹的加速度平方积分——衡量轨迹的抖动程度。可以写成waypoint的二次型θ^T A θ其中A是一个固定的三对角矩阵。碰撞代价项对轨迹上的每个点计算到最近障碍物的距离d如果d小于安全距离d_safe代价为(d_safe - d)²。距离用ESDF欧几里得符号距离场计算——预处理时把整个空间的距离场算好查询时O(1)。ESDF的计算对3D栅格地图做距离变换Distance Transform每个栅格存储到最近障碍物的欧几里得距离。正号表示在障碍物外部负号表示在障碍物内部。3D栅格分辨率0.05m时一张典型办公桌场景的ESDF约200×200×200个栅格计算时间约20ms。ESDF只需计算一次——场景不变就不用更新。如果障碍物移动了比如有人走进来需要增量更新ESDF——只重算受影响区域的距离值。# CHOMP的目标函数 def chomp_cost(trajectory, esdf_map, d_safe0.1): smoothness trajectory.T A trajectory # 二次型 collision 0 for point in trajectory: d esdf_map.query(point) if d d_safe: collision (d_safe - d) ** 2 return smoothness lambda_ * collisionCHOMP的协方差梯度是个关键技巧普通梯度下降在高维空间中收敛慢CHOMP用轨迹的协方差矩阵C A^(-1)来预条件化梯度——相当于在梯度方向上做了一个线性变换让下降方向更合理。这个技巧让收敛速度提升了5-10倍。二、TrajOpt——基于序列凸优化TrajOpt由Stanford的John Schulman在2013年提出。和CHOMP不同TrajOpt把碰撞约束作为硬约束处理不是软代价用序列凸优化SCO, Sequential Convex Optimization求解。TrajOpt的流程初始化一条轨迹通常是从起点到终点的直线插值在当前轨迹处线性化碰撞约束——用一阶泰勒展开把非凸的碰撞约束近似为线性约束求解一个凸优化问题QP或SOCP得到新轨迹用信赖域限制——新轨迹不能离当前轨迹太远防止线性化失效重复2-4直到收敛# TrajOpt的核心迭代 def trajopt(initial_traj, max_iter50): traj initial_traj for i in range(max_iter): # 在当前轨迹处线性化约束 constraints linearize_collision(traj) # 信赖域半径 trust_radius compute_trust_radius(traj) # 求解QP new_traj solve_qp(smoothness_cost, constraints, trust_radius) # 接受或拒绝 if actual_reduction predicted_reduction * 0.1: traj new_traj else: trust_radius * 0.5 # 缩小信赖域 return traj信赖域Trust Region是TrajOpt的关键机制它限制了每次迭代轨迹可以变化的幅度。如果信赖域太大线性化不准优化可能跑偏如果太小收敛太慢。TrajOpt自适应调整信赖域大小——优化步成功就放大失败就缩小。三、CHOMP vs TrajOpt对比维度CHOMPTrajOpt约束处理软约束代价函数硬约束信赖域线性化优化方法协方差梯度下降序列凸优化SQP变体碰撞检测ESDF距离场直接几何碰撞检测收敛速度较慢100-500次迭代较快10-50次迭代约束满足近似满足取决于权重严格满足在信赖域内对初始解的依赖强容易陷入局部最优中等信赖域有帮助计算时间50-200ms30-100ms经验法则如果障碍物简单大平面、规则形状CHOMP够用且实现简单。如果障碍物复杂多物体、窄通道TrajOpt更好——硬约束能严格保证不碰撞。讲真两者在实际项目中经常组合使用——先用CHOMP做粗优化不需要好的初始解再用TrajOpt做精优化在CHOMP结果附近精细调整。MoveIt的默认流程就是这么做的。四、和采样方法的结合CHOMP和TrajOpt都是局部优化方法——需要一个还不错的初始解。如果初始解太差比如起点到终点的直线穿过障碍物优化可能收敛到局部最优甚至找不到可行解。工程上的标准做法先用采样方法RRT、RRT-Connect找到一条无碰撞路径作为初始解再用CHOMP/TrajOpt优化。RRT负责找到路解决可行性CHOMP/TrajOpt负责优化路解决最优性。这个组合在MoveIt2中体现为Planning Pipeline的概念先选一个plannerOMPL的RRT-Connect再选一个optimizerCHOMP或TrajOpt流水线式执行。五、面试实战QCHOMP和TrajOpt的核心区别是什么ACHOMP把碰撞作为软约束加入目标函数用梯度下降优化。TrajOpt把碰撞作为硬约束用序列凸优化信赖域求解。CHOMP实现简单但约束不严格满足TrajOpt约束严格但实现更复杂。QCHOMP的协方差梯度是什么意思A普通梯度下降在高维空间中可能走Z字形收敛慢。CHOMP用轨迹平滑度矩阵的逆A^(-1)来预条件化梯度——等价于在每个维度上自适应调整学习率。平滑度矩阵的逆就是轨迹的协方差矩阵所以叫协方差梯度。Q你在项目中用过CHOMP或TrajOpt吗A用过TrajOpt。做机械臂的避障轨迹优化。RRT-Connect给初始路径TrajOpt优化20次迭代收敛。优化后轨迹的平滑度提升约40%碰撞检测通过。单次优化约50ms在100ms的规划周期内够用。Q局部最优问题怎么处理A三个方法。一是好的初始解——RRT给出的初始解质量越好局部最优问题越小。二是多起点优化——用不同的随机种子跑多次RRT得到多条初始路径分别优化取最好的。三是扰动策略——优化卡住时对轨迹加随机扰动打破局部最优。小结CHOMP和TrajOpt是轨迹优化的两个经典框架。CHOMP用软约束协方差梯度实现简单。TrajOpt用硬约束信赖域约束满足更严格。两者都需要好的初始解——通常由RRT等采样方法提供。面试中被问到CHOMP/TrajOpt核心说清楚约束处理方式的区别、优化方法的差异、以及对初始解的依赖。能讲清楚信赖域机制和协方差梯度的直觉是加分项。下一篇是运动规划面试高频题——面试官最爱问的15个规划问题。如果这篇文章对你有帮助欢迎点赞、在看、转发三连。 你的支持是我持续更新的最大动力。「机器人软件开发面试·从入门到精通」连载系列上一篇第232篇 运动学约束规划——速度/加速度/加加速度限制的处理下一篇预告第234篇 运动规划面试高频题——面试官最爱问的15个规划问题有任何问题欢迎评论区留言我会尽量回复。