Android 省市区三级联动城市选择器 CityPicker 完整集成指南
2026/8/22 13:53:28
前面几篇分别讲了轨迹平滑、时间参数化、碰撞检测、运动学约束。今天把这些问题合在一起——怎么在一个统一的框架里同时处理所有约束。答案是两个经典的轨迹优化框架：CHOMP和TrajOpt。
CHOMP（Covariant Hamiltonian Optimization for Motion Planning）和TrajOpt（Trajectory 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_safe=0.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_iter=50): 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对比
| 维度 | CHOMP | TrajOpt |
|---|---|---|
| 约束处理 | 软约束（代价函数） | 硬约束（信赖域+线性化） |
| 优化方法 | 协方差梯度下降 | 序列凸优化（SQP变体） |
| 碰撞检测 | ESDF距离场 | 直接几何碰撞检测 |
| 收敛速度 | 较慢（100-500次迭代） | 较快（10-50次迭代） |
| 约束满足 | 近似满足（取决于权重） | 严格满足（在信赖域内） |
| 对初始解的依赖 | 强（容易陷入局部最优） | 中等（信赖域有帮助） |
| 计算时间 | 50-200ms | 30-100ms |
经验法则：如果障碍物简单（大平面、规则形状），CHOMP够用且实现简单。如果障碍物复杂（多物体、窄通道），TrajOpt更好——硬约束能严格保证不碰撞。
讲真，两者在实际项目中经常组合使用——先用CHOMP做粗优化（不需要好的初始解），再用TrajOpt做精优化（在CHOMP结果附近精细调整）。MoveIt的默认流程就是这么做的。
四、和采样方法的结合
CHOMP和TrajOpt都是局部优化方法——需要一个还不错的初始解。如果初始解太差（比如起点到终点的直线穿过障碍物），优化可能收敛到局部最优甚至找不到可行解。
工程上的标准做法：先用采样方法（RRT、RRT-Connect）找到一条无碰撞路径作为初始解，再用CHOMP/TrajOpt优化。RRT负责"找到路"（解决可行性），CHOMP/TrajOpt负责"优化路"（解决最优性）。
这个组合在MoveIt2中体现为"Planning Pipeline"的概念：先选一个planner（OMPL的RRT-Connect），再选一个optimizer（CHOMP或TrajOpt），流水线式执行。
五、面试实战
Q：CHOMP和TrajOpt的核心区别是什么？A：CHOMP把碰撞作为软约束加入目标函数，用梯度下降优化。TrajOpt把碰撞作为硬约束，用序列凸优化+信赖域求解。CHOMP实现简单但约束不严格满足，TrajOpt约束严格但实现更复杂。
Q：CHOMP的协方差梯度是什么意思？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个规划问题
有任何问题欢迎评论区留言，我会尽量回复。