★ FEATURED ARTICLE 1. 拆解“规划部分”到底在解决什么问题聊到自动驾驶系统很多人第一反应是感知——摄像头、激光雷达、毫米波雷达怎么融合障碍物怎么检测。但真正让一辆车“会开车”的其实是规划模块。感知告诉车“周围有什么”规划告诉车“接下来该怎么走”。这个“怎么走”三个字背后是一整套从全局路由到局部轨迹生成的复杂决策链条。我接触这套规划框架有几年时间了从最早看文档一头雾水到后来自己跑仿真、调参数、复现论文里的算法踩过的坑不算少。这篇文章不打算复述官方文档而是从一个实际使用者的角度把规划部分的核心架构、关键算法、实操配置和常见问题掰开揉碎讲清楚。无论你是刚入门自动驾驶的学生还是想从感知转规划的工程师或者只是好奇“自动驾驶到底怎么决定变道”的技术爱好者应该都能从中拿到一些能直接用的东西。规划模块的本质是在一个动态、不确定的环境里持续求解一个带约束的优化问题。约束来自交通规则、车辆动力学、障碍物位置、乘客舒适度优化目标则是安全、高效、平滑。听起来简单但真正落地时你会发现每一个约束的建模方式、每一个求解器的参数选择都会直接影响最终行为。这也是为什么规划部分往往是自动驾驶系统里最“玄学”的模块——同样的场景参数差一点行为可能完全不同。2. 规划模块的整体架构与设计思路2.1 从全局到局部分层规划的必然选择规划部分最核心的设计思想是分层。为什么不直接用一个端到端的模型搞定因为计算复杂度和可解释性不允许。全局路径规划要考虑整个路网局部轨迹规划要在毫秒级响应两者的时间尺度和空间尺度差了好几个数量级。所以主流框架都采用分层架构顶层是路由规划中间是行为决策底层是运动规划。路由规划解决的是“从A到B走哪条路”的问题。输入是地图数据和起终点输出是一条由车道段组成的全局路径。这一层通常用图搜索算法比如Dijkstra或A*在地图的路网拓扑上找最优路径。代价函数一般考虑距离、通行时间、道路等级等因素。实际使用中这一层的更新频率很低只在车辆偏离全局路径或收到新的导航指令时才重新计算。行为决策层是很多人容易忽略但极其关键的一环。它要回答的是“在当前场景下我应该跟车、变道、超车还是让行”。这一层的输出不是具体轨迹而是高层意图。常见做法是用有限状态机或者行为树来建模。比如跟车状态、变道状态、路口通行状态每个状态有明确的触发条件和退出条件。状态机的好处是可解释性强调试时能清楚知道车辆为什么做出某个决策。缺点是状态爆炸场景一复杂状态转移图就变得难以维护。运动规划层是最终生成轨迹的地方。它接收行为决策的意图结合障碍物预测信息输出一条带时间戳的轨迹点序列。这条轨迹要满足车辆动力学约束比如最大曲率、最大加速度、最大方向盘转角。同时要保证安全不能和任何障碍物的预测轨迹相交。这一层的算法选择最多后面会详细展开。2.2 为什么选择这种架构而不是端到端端到端规划这几年很火输入感知结果直接输出控制指令。但实际项目中纯端到端方案很难通过安全验证。原因很简单不可解释。当车辆做出一个奇怪决策时你无法定位是感知错了、预测错了还是规划逻辑有问题。分层架构虽然工程量大但每一层都有明确的输入输出定义可以独立测试和验证。这在功能安全要求极高的场景下是必须的。另一个考虑是模块复用。路由规划可以独立于车辆平台行为决策可以针对不同地区交通规则做适配运动规划可以替换不同算法。这种灵活性在长期迭代中价值巨大。我见过一些团队一开始为了快速出Demo用了端到端后来要过安全评审时不得不推倒重来代价很高。2.3 核心数据流与模块间接口规划模块的数据流可以概括为地图定位感知预测 → 规划 → 控制。具体来说路由规划从地图模块获取路网信息从定位模块获取当前车辆位姿从导航模块获取目的地。行为决策和运动规划则额外需要感知模块的障碍物列表和预测模块的轨迹预测结果。接口设计上有几个关键点。第一是坐标系要统一通常用世界坐标系做规划最后再转到车辆坐标系给控制。第二是时间戳要对齐感知和预测的结果可能有延迟规划必须使用同一时刻的快照数据。第三是障碍物信息要包含足够的字段位置、速度、加速度、朝向、尺寸、类型、预测轨迹、置信度。缺少任何一个都可能影响规划质量。注意很多新手在调试时会忽略时间戳对齐问题导致规划轨迹和实际障碍物位置有偏差。建议在规划模块入口处加一个数据同步检查如果各输入的时间戳差异超过阈值就报警。3. 核心算法细节与实操要点3.1 路由规划A*算法的工程化改造路由规划最常用的是A算法但直接拿教科书上的A来用会有一堆问题。首先是地图表示路网不是规则的网格而是由车道段和连接点组成的有向图。每个节点是一个车道段每条边是车道之间的连接关系。启发函数通常用欧氏距离除以最大限速这样能保证可采纳性。工程化改造主要有几个方向。一是分层搜索先在大粒度路网上找粗略路径再在小粒度上细化。二是动态代价调整根据实时交通信息修改边的权重。三是多路径备选同时保留几条候选路径当主路径不可用时快速切换。我实际用下来A*在几千个节点的路网上单次搜索耗时可以控制在10毫秒以内完全满足实时性要求。有一个容易踩的坑是死循环。如果启发函数设计不当或者地图数据有环A*可能陷入无限循环。解决办法是加一个访问节点集合已经扩展过的节点不再重复扩展。另外要注意浮点数比较的精度问题两个节点的代价可能因为浮点误差被判定为不相等导致重复扩展。3.2 行为决策有限状态机的状态设计行为决策的状态机设计直接决定了车辆的行为风格。一个典型的跟车场景状态机包含这几个状态巡航、跟车、接近、停车。巡航状态下车辆按期望速度行驶当检测到前方有慢车且距离小于阈值时进入跟车状态跟车过程中如果距离持续缩小则进入接近状态准备减速如果前车停止则进入停车状态。状态转移条件的设计需要大量调参。比如跟车距离阈值设得太小会导致频繁切入切出设得太大又会被加塞。我的经验是采用动态阈值跟车速相关低速时距离小高速时距离大。具体公式可以用安全时间间隔乘以自车速度再加上一个最小距离。安全时间间隔通常取1.5到2.5秒具体值取决于你对舒适性和效率的权衡。变道决策更复杂需要考虑的因素包括当前车道前方是否有慢车、目标车道是否有足够间隙、变道收益是否大于成本、是否满足交通规则。我见过一些实现用简单的规则判断比如“前方车辆速度低于自车期望速度的80%且持续3秒以上就考虑变道”。这种规则在简单场景下能用但复杂场景下容易做出不合理决策。更好的做法是引入效用函数对每个候选行为打分选最高分执行。3.3 运动规划从采样到优化的算法选型运动规划算法大致分三类采样类、优化类、学习类。采样类以RRT和Lattice Planner为代表优点是实现简单、能处理复杂约束缺点是轨迹质量不稳定、可能不光滑。优化类以EM Planner和MPC为代表优点是轨迹平滑、能显式处理动力学约束缺点是求解可能失败、计算量大。学习类目前还在研究中实际落地较少。Lattice Planner是我推荐新手入门的算法。它的核心思想是预先采样一组轨迹然后用代价函数评估每条轨迹选代价最小的。采样维度包括横向偏移和纵向速度。横向偏移可以取-2米到2米间隔0.5米纵向速度可以取0到最大速度间隔2米每秒。这样组合起来大概几十条候选轨迹每条用五次多项式生成保证位置、速度、加速度连续。代价函数的设计是Lattice Planner的灵魂。通常包含这几项偏离车道中心线的惩罚、速度偏离期望速度的惩罚、加速度和加加速度的惩罚、与障碍物碰撞的惩罚。各项权重需要仔细调节。我的经验是碰撞惩罚要设得极大确保任何有碰撞风险的轨迹都被排除舒适性惩罚适中避免急加速急刹车效率惩罚相对较小因为安全永远优先。EM Planner是另一种主流方案它把规划问题分解成路径规划和速度规划两个子问题交替迭代求解。路径规划在Frenet坐标系下进行用二次规划求解最优路径速度规划用动态规划或二次规划求解最优速度曲线。这种分解方式降低了问题复杂度但可能陷入局部最优。实际使用中EM Planner生成的轨迹通常比Lattice Planner更平滑但计算时间也更长。3.4 Frenet坐标系规划问题的降维利器Frenet坐标系是规划部分必须掌握的概念。简单说它用纵向距离s和横向偏移l来描述车辆相对于参考线的位置。参考线通常是车道中心线。在Frenet坐标系下车辆的运动可以解耦成纵向和横向两个一维问题大大简化了规划难度。从笛卡尔坐标到Frenet坐标的转换需要参考线信息。给定参考线上一点可以计算车辆在该点的投影得到s值车辆到参考线的法向距离就是l值。转换过程中要注意参考线的曲率曲率大时投影可能不唯一。实际实现中通常用迭代法找最近点迭代几次就能收敛。Frenet坐标系下的轨迹生成很直观。横向用五次多项式连接初始状态和期望状态纵向用四次多项式或梯形速度曲线。五次多项式能保证位置、速度、加速度连续适合横向纵向更关注速度规划用四次多项式保证加加速度连续即可。生成轨迹后再转回笛卡尔坐标给控制模块。提示Frenet坐标系在参考线曲率过大时会失效比如急弯或路口。这时候需要切换到笛卡尔坐标系规划或者用分段参考线。我遇到过在环岛场景下Frenet规划直接崩溃的情况后来加了曲率检测超过阈值就切换规划模式。4. 完整实操流程与关键环节实现4.1 环境搭建与依赖准备假设你已经有一套自动驾驶仿真环境规划模块通常以独立进程或节点形式运行。依赖主要包括地图数据OpenDRIVE格式或自定义格式、定位数据、感知障碍物列表、预测轨迹。开发语言以C为主部分算法原型可以用Python验证。编译规划模块前先确认几个关键库的版本。Eigen用于矩阵运算版本建议3.3以上OSQP或IPOPT用于二次规划求解Boost用于图搜索和几何计算。这些库的版本兼容性很重要我遇到过Eigen版本不匹配导致段错误的情况排查了很久。配置文件通常包含这几类参数车辆参数轴距、最大曲率、最大加速度、规划参数采样间隔、代价权重、安全距离、地图参数参考线采样密度、车道宽度。建议用YAML或JSON格式方便修改和版本管理。4.2 参考线生成与平滑参考线是规划的基础。它通常从地图的车道中心线提取但原始地图数据往往不够平滑直接使用会导致规划轨迹抖动。所以第一步是对参考线做平滑处理。平滑方法有几种。最简单的是移动平均滤波对每个点取前后若干点的平均值。这种方法计算快但会改变参考线形状弯道处可能偏离车道。更好的是用样条曲线拟合比如B样条或三次样条。拟合时以原始点为控制点生成光滑曲线。我通常用三次样条因为计算简单且能保证二阶连续。平滑后的参考线要重新采样采样间隔一般取0.5米到1米。间隔太小会增加计算量太大则轨迹精度不够。采样时记录每个点的位置、切向、曲率、累计弧长。这些信息在Frenet转换时会用到。4.3 障碍物处理与决策逻辑障碍物列表来自感知模块但感知输出往往有噪声和误检。规划模块需要做一层过滤和分类。过滤规则包括置信度低于阈值的丢弃、距离过远的忽略、不在规划范围内的排除。分类则区分静态障碍物和动态障碍物动态障碍物还需要预测轨迹。动态障碍物的预测轨迹通常由预测模块提供包含未来几秒内每个时间步的位置和速度。规划时要把这些预测轨迹当作时空中的障碍区域规划出的轨迹不能与这些区域相交。实际实现中为了降低计算量通常只考虑未来3到5秒的预测更远的未来用保守假设处理。决策逻辑的核心是判断当前场景属于哪类。我通常按这几个维度分类是否有前车、前车是否慢、是否有变道需求、是否在路口、是否有行人或非机动车。每个维度组合对应一个决策状态。比如“有前车前车慢有变道需求目标车道空闲”对应变道超车状态。4.4 轨迹生成与优化求解以Lattice Planner为例轨迹生成分横向和纵向两步。横向采样一组偏移量对每个偏移量用五次多项式生成从当前横向状态到目标横向状态的曲线。纵向采样一组速度用四次多项式生成速度曲线。两者组合得到候选轨迹集合。每条候选轨迹计算代价。代价函数通常写成加权和形式cost w_lat * lateral_cost w_lon * longitudinal_cost w_acc * accel_cost w_jerk * jerk_cost w_collision * collision_cost权重需要根据实际场景调节。我的一般原则是碰撞代价权重设为1000以上确保有碰撞的轨迹直接被排除舒适性代价权重在1到10之间效率代价权重在0.1到1之间。具体值要在仿真里反复测试。选出最优轨迹后还要做一步后处理。包括轨迹平滑用样条拟合离散点、速度限制检查确保不超过限速和曲率限制、碰撞复查用更精确的碰撞检测算法验证。后处理能显著提升轨迹质量但也会增加计算时间需要权衡。4.5 仿真验证与参数调优规划模块开发完成后必须在仿真里大量测试。仿真场景要覆盖直道跟车、弯道行驶、变道超车、路口通行、行人避让、紧急制动。每个场景测试几百次统计成功率、平均速度、最大加速度、最大加加速度等指标。参数调优是个体力活。我通常用网格搜索加人工经验结合的方式。先固定大部分参数调一两个关键参数观察指标变化。比如调跟车距离从1.5秒开始每次增加0.1秒看成功率和舒适性怎么变。找到拐点后再调下一个参数。仿真里表现好不代表实车没问题。实车测试要循序渐进先封闭场地低速测试再封闭场地高速测试最后开放道路测试。每次测试都要记录规划轨迹和实际轨迹的偏差偏差大说明模型或参数有问题。5. 常见问题与排查技巧实录5.1 规划轨迹抖动或震荡这是最常见的问题。表现是规划出的轨迹在相邻帧之间跳变导致控制模块输出抖动。原因通常有几个参考线不平滑、代价函数权重不合理、障碍物信息噪声大。排查步骤先看参考线把参考线画出来如果本身就有锯齿那问题在参考线生成。再看代价函数如果两条候选轨迹代价非常接近最优解会在两者之间跳变。解决办法是加一个代价平滑项惩罚与上一帧轨迹的偏差。最后检查障碍物如果障碍物位置每帧都在跳规划轨迹自然跟着跳。这时候需要在感知和规划之间加滤波器。5.2 规划求解失败或无解优化类算法可能求解失败采样类算法可能所有候选轨迹都不可行。原因包括约束太紧、初始状态不可行、障碍物完全堵死。对于优化类先检查约束是否矛盾。比如最大曲率约束和横向偏移约束可能冲突。放宽约束或增加松弛变量。对于采样类检查采样范围是否覆盖了可行区域。如果障碍物完全堵死那确实无解这时候应该触发紧急停车或请求远程协助。注意规划无解时不要直接输出上一帧轨迹因为环境可能已经变了。正确的做法是输出一条安全的停车轨迹同时上报故障。5.3 变道决策过于保守或激进保守表现为该变道时不变导致通行效率低激进表现为频繁变道或危险变道。根因在决策逻辑的阈值和代价函数。调保守问题降低变道收益阈值让更多场景触发变道增大目标车道间隙的容忍度。调激进问题提高变道收益阈值只在收益明显时变道减小间隙容忍度要求更大的安全距离。我个人的经验是变道决策宁保守勿激进。因为激进变道一旦出问题就是大问题保守变道最多被后车鸣笛。安全永远是第一位的。5.4 弯道轨迹偏离车道弯道是规划容易出问题的地方。表现是轨迹切内弯或甩外弯。原因通常是参考线曲率估计不准或者Frenet转换在弯道处误差大。解决办法提高参考线采样密度弯道处加密到0.2米用更精确的曲率计算方法比如用相邻三点的外接圆曲率在Frenet转换时考虑曲率修正项。如果弯道特别急考虑切换到笛卡尔坐标系规划。5.5 常见问题速查表问题现象可能原因排查方法解决措施轨迹抖动参考线不平滑可视化参考线样条平滑加密采样轨迹抖动代价函数权重不当打印候选轨迹代价调整权重加平滑项求解失败约束矛盾检查约束边界放宽约束加松弛变量求解失败障碍物堵死可视化障碍物触发停车上报故障变道保守收益阈值过高打印变道收益值降低阈值增大间隙容忍变道激进收益阈值过低打印变道收益值提高阈值减小间隙容忍弯道偏离曲率估计不准对比参考线和实际轨迹加密采样曲率修正弯道偏离Frenet转换误差检查转换前后坐标切换笛卡尔规划5.6 几个容易被忽略的细节第一个是时间一致性。规划模块每帧的输出应该和上一帧有连续性不能突变。实现上可以加一个轨迹平滑滤波器或者把上一帧轨迹作为当前帧的参考。第二个是数值精度。规划涉及大量浮点运算累积误差可能导致意外结果。关键计算用双精度比较时加容差。我遇到过因为浮点误差导致碰撞检测误判的情况后来把所有比较都改成带容差的。第三个是异常处理。规划模块要能处理各种异常输入空障碍物列表、无效定位、地图数据缺失。每种异常都要有明确的处理策略不能直接崩溃。第四个是日志记录。规划模块的日志要足够详细能复现问题。建议记录每帧的输入快照、候选轨迹、代价、最终选择。出问题时用日志回放比在实车上调试效率高得多。6. 规划部分的扩展方向与个人体会规划模块做完基础功能后有几个方向可以继续深入。一是引入学习型方法用神经网络学习人类驾驶行为作为规则方法的补充。二是做多模态规划同时输出多条候选轨迹让下游模块根据置信度选择。三是做交互式规划考虑其他交通参与者的反应而不是把他们当作静态障碍物。我个人在实际操作中的体会是规划部分最难的不是算法本身而是场景覆盖和参数调优。算法可以从论文里抄但参数必须自己一点点试。我建议新手先从Lattice Planner入手把整个流程跑通再逐步替换成更复杂的算法。不要一上来就搞MPC或强化学习容易陷入细节出不来。另外规划模块的测试用例要尽早积累。每遇到一个bad case就存下来形成回归测试集。这个习惯能帮你避免很多重复问题。我见过一些团队没有回归测试改了一个bug又引入两个新bug效率极低。最后分享一个小技巧规划模块的可视化工具非常重要。把参考线、障碍物、候选轨迹、最终轨迹都画出来一眼就能看出问题在哪。花时间做一个好用的可视化工具绝对值得。
阅读完成 · 觉得有帮助？
