前面几篇讲了混合A*、Reed-Shepp曲线、泊车应用——这些解决的是路径存在性问题即找到一条能从A到B的路。从今天开始进入新话题轨迹优化。说白了路径规划回答走哪条路轨迹优化回答怎么走这条路。路径只是一串空间点(x,y)没有时间信息。轨迹是路径加上时间参数化——不仅要知道走哪里还要知道什么时候走到哪里、走多快。一条好的轨迹需要同时满足多个要求不碰撞、平滑、满足运动学约束、执行时间短、舒适度高。一、路径与轨迹的区别这个问题面试经常被问到很多人分不清。路径Path空间中的一条曲线描述从起点到终点经过的位置。路径没有时间概念——你知道车要经过哪些点但不知道什么时候到哪个点。轨迹Trajectory路径加上时间参数化。每个路径点对应一个时间戳由此可以推导出速度、加速度、加加速度jerk。举个例子从家到公司路径只有一条走哪条路但轨迹可以有很多——8点出发开快一点和8点半出发开慢一点路径相同但轨迹不同。# 路径 vs 轨迹 path [(x1,y1), (x2,y2), ..., (xn,yn)] # 空间点序列 trajectory [(x1,y1,t1), (x2,y2,t2), ..., (xn,yn,tn)] # 时空点序列 # 轨迹可以求导v dx/dt, a dv/dt, jerk da/dt二、好的轨迹有哪些标准这个很关键——面试时如果问你怎么评价一条轨迹的质量你得能说出具体指标。无碰撞Collision-free这是底线轨迹上的每个点都不能与障碍物相交。平滑性Smoothness轨迹不能有尖角或突变。曲率连续、速度连续是最基本要求。不平滑的轨迹会导致跟踪误差大、乘坐舒适度差。运动学可行Kinematically feasible轨迹上的速度、加速度、转向角不能超过车辆的物理极限。比如最大速度10m/s、最大加速度2m/s²、最大前轮转角30度。时间效率Time efficiency在满足约束的前提下轨迹执行时间尽量短。没人喜欢车开得慢吞吞。舒适度Comfort加加速度jerk要小。jerk大意味着加速度突变——急刹车、猛加速就是jerk大的表现。人类对jerk非常敏感自动驾驶车辆的jerk通常控制在2m/s³以内。离障碍物距离Clearance轨迹不能擦着障碍物走。保持一定安全距离——通常至少20cm。这六个标准之间存在矛盾时间短往往意味着速度高、加速度大但舒适度和安全性要求速度平缓、加速度小。轨迹优化的本质就是在这些矛盾中找平衡。一个容易被忽视的指标——可跟踪性Trackability轨迹不仅要好还得能被控制器准确跟踪。如果轨迹的曲率变化太快MPC或Pure Pursuit控制器可能跟不上实际轨迹偏离规划轨迹。这个指标在高速场景中尤其重要——100km/h时曲率突变意味着横向加速度跳变车辆可能失控。我做自动驾驶项目时有个教训轨迹优化出来的路径很平滑、很短但曲率在某个点从0突然跳到0.1对应方向盘急转控制器来不及响应车辆偏离了0.5m。后来加了曲率变化率的约束才解决。三、轨迹优化的数学框架轨迹优化可以形式化为一个优化问题# 轨迹优化问题的一般形式 minimize J(trajectory) # 目标函数 subject to collision_constraint # 无碰撞约束 kinematic_constraint # 运动学约束 dynamic_constraint # 动力学约束 boundary_constraint # 起止点约束目标函数J通常是多个代价项的加权和J w1 * 路径长度 w2 * 执行时间 w3 * 平滑度 w4 * 舒适度 w5 * 离障碍物距离权重w1-w5需要根据场景调节。高速公路场景时间效率权重高停车场场景安全距离权重高。没有万能权重只有适合当前场景的权重。约束处理是难点目标函数通常容易处理连续可微但约束不好处理——碰撞约束是非凸的动力学约束可能是非线性的。处理约束的常见技巧把硬约束转化为软约束加到目标函数中作为惩罚项或者用障碍函数Barrier Function让优化自然避开不可行区域。优化变量可以是路径上的控制点坐标、速度曲线的时间分配、或者整条轨迹的参数化系数——具体取决于用什么方法表示轨迹B样条、多项式、离散点等后面几篇会详细讲。四、轨迹优化的求解方法分类轨迹优化的求解方法大致分为三类解析方法对简化后的问题求出闭式解。比如最小化Jerk轨迹——在无约束情况下最优轨迹是五次多项式系数可以直接算出来。速度快但只适用于简单场景。数值优化方法把连续问题离散化后用非线性规划NLP求解器求解。代表方法有SQP序列二次规划、IPOPT。能处理复杂约束但计算量大。采样/搜索方法在轨迹空间做随机采样或搜索找近似最优解。比如RRT*的变种、格点搜索。计算快但不保证最优。实际工程中很少只用一种方法。典型做法是先用采样方法得到初始解再用数值优化方法精修。初始解质量好的话优化收敛快且不容易陷入局部最优。热启动Warm Start的重要性数值优化方法的收敛速度和初始解质量强相关。如果初始解离最优解远优化可能要迭代几百次甚至收敛到局部最优。工程上的标准做法用上一帧的优化结果作为当前帧的初始猜测——因为相邻两帧之间车辆移动很小上一帧的最优解在当前帧仍然是一个不错的近似。这个技巧能让优化迭代次数从100次降到10次以内。五、面试实战Q路径和轨迹有什么区别A路径是空间曲线只有位置信息没有时间信息。轨迹是路径加上时间参数化可以求导得到速度和加速度。路径规划解决走哪条路轨迹优化解决怎么走。Q轨迹优化有哪些评价标准A六个核心标准——无碰撞、平滑、运动学可行、时间效率、舒适度、离障碍物距离。这些标准之间存在矛盾轨迹优化就是找平衡。权重需要根据场景调节。Q你用过什么轨迹优化方法A用过两种。一种是B样条参数化SQP优化——把轨迹表示为B样条曲线控制点作为优化变量目标函数是平滑度离障碍物距离约束是速度和加速度限制。另一种是最小化Jerk的多项式方法——在waypoint之间拟合五次多项式边界条件保证位置和速度连续。Q轨迹优化和路径规划是什么关系A路径规划是轨迹优化的前置步骤。先用A*/RRT/混合A*找到一条粗略路径再用轨迹优化把路径打磨成可执行的轨迹。有些方法把两者合在一起做比如CHOMP但大多数工程系统是分开做的——分开做更容易调试和维护。Q轨迹优化的计算量一般多少A取决于方法和轨迹长度。最小化Jerk的解析方法几乎不花时间微秒级。B样条SQP优化通常在10-100ms。CHOMP和TrajOpt这类梯度方法在50-200ms。对于自动驾驶的实时规划来说100ms是可接受的上限。Q离线优化和在线优化有什么区别A离线优化在任务开始前一次性算好整条轨迹比如机械臂的点到点运动计算时间不敏感可以用重型优化器。在线优化在运行过程中实时计算比如自动驾驶车辆的局部规划每100ms要更新一次对计算速度要求极高。在线优化通常用轻量级方法B样条少量控制点离线优化可以用更复杂的方法直接配点法IPOPT。小结路径是空间曲线轨迹是路径加时间。好的轨迹需要同时满足无碰撞、平滑、运动学可行、时间高效、舒适、安全距离六个标准。轨迹优化的本质是在这些矛盾标准之间寻找平衡。理解了这个框架之后后面几篇会具体讲不同的轨迹优化方法——最小化Jerk、B样条、时间参数化、运动学约束处理。每种方法解决的是这个框架中的一个子问题。下一篇讲最小化Jerk轨迹——让运动丝滑的数学方法。如果这篇文章对你有帮助欢迎点赞、在看、转发三连。 你的支持是我持续更新的最大动力。「机器人软件开发面试·从入门到精通」连载系列上一篇第225篇 Hybrid A*在自动泊车中的实际应用下一篇预告第227篇 最小化Jerk轨迹——让运动丝滑的数学方法有任何问题欢迎评论区留言我会尽量回复。