2025年回看过去这十年智能驾驶圈最内卷、最出成果的模块之一就是规划算法。从早期靠A*和RRT在栅格地图里“找路”到今天端到端模型一把梭直接输出轨迹规划算法的演进几乎就是整个智能驾驶行业的缩影。这篇文章不打算写成教科书我只想把2015到2025这十年里规划算法真正走通的路、踩过的坑、沉淀下来的工程经验按从业者的视角讲清楚。适合正在做智能驾驶研发的工程师、刚入行的算法新人以及想理解“车到底怎么自己开”的朋友。1. 十年整体脉络从“搜索”到“学习”的转型路径1.1 2015-2017搜索与采样的黄金时代2015年前后智能驾驶还处于“能开起来就很牛”的阶段。当时传感器、算力、控制链路都很简陋规划算法最典型的做法是先建栅格地图或道路网络再用A*、Dijkstra这类图搜索算法找一条从起点到终点的无碰撞路径然后用RRT快速随机搜索树在连续空间里做采样看能不能额外避掉一些障碍。DARPA挑战赛出来的Stanley、Junior后来的谷歌无人车、百度Apollo早期版本基本都脱胎于这一套思想。那个阶段的核心特征是说“搜索算法主导一切”规划结果对车辆运动学约束的考虑非常弱。RRT在低速空旷场地表现不错但随机性很强规划出来的轨迹曲率突变极其严重根本没法直接下发给控制模块。所以当年很多团队的实际做法是“搜索平滑”两步走先用搜索得到参考路径再用曲线拟合或数值优化把折线磨平。优点是很鲁棒容易实现适合快速验证闭环缺点则是轨迹质量差稍微复杂的道路结构——比如多车道变道、避障加超车——就变得手忙脚乱。1.2 2018-2020曲线生成与优化的精细时代2018年前后行业开始意识到搜索方法只解决了“有没有路”的问题没有解决“怎么走得好”的问题。于是规划算法的重心从“通路搜索”转向“轨迹生成与优化”。五次多项式曲线和贝塞尔曲线被大规模用于横纵向轨迹生成横向用多项式拟合换道轨迹纵向用多项式规划速度曲线。优化方法也开始占据主导轨迹规划被建模成一个带约束的优化问题目标函数包含平滑性、与参考线的偏移程度、加速度、加加速度等约束则包含动力学极限、道路边界、避障条件。那个阶段的代表性方案是百度Apollo公开的EM Planner。E-step做采样与搜索M-step做二次规划精修本质上是“先离散搜索生成粗糙轨迹再用QP二次规划精细优化”的混合架构。这个阶段最核心的进步是规划结果从“能走”变成了“走得优雅”算法开始系统性地考虑舒适性、能耗、变道效率这些指标。我当时在项目里最大的感受是同样一条变道轨迹2016年的方法和2019年的方法乘坐体验差出一整个量级。1.3 2021-2023学习方法全面介入2021年以后深度学习已经在感知领域站稳脚跟规划算法也开始被“数据驱动”快速渗透。模仿学习IL和逆强化学习IRL成为研究热点大家用大量人工驾驶数据来训练代价函数让规划结果的“品味”接近人类老司机。Dagger算法解决了模仿学习中臭名昭著的数据集偏移问题它要求在闭环交互里不断用专家矫正模型错误而不是静态地把数据喂进去就完事。深度强化学习RL在规划任务上也做了大量尝试泛化能力确实很强但在实车上的部署困难重重——奖励函数设计、样本效率、安全性验证都是硬伤。这个阶段行业对规划算法的要求从“数学最优”转向了“像人一样”。评估指标不再只看轨迹误差而是看是否违反交通规则、乘坐体验好不好、避障策略是否符合人类驾驶习惯。可以说2021-2023这三年规划算法彻底从纯工程问题变成了工程与数据科学的交叉问题。1.4 2024-2025端到端与生成式规划的兴起2024年开始端到端成为行业最热的方向。所谓端到端就是把感知、预测、规划整个链路用一个统一模型串联起来直接输出轨迹或控制量。UniAD、VAD这类代表性工作加上基于Diffusion Policy和世界模型的生成式规划方法把“规划”变成了“生成”问题给定场景上下文直接生成一段可行的轨迹分布。端到端方案的优点非常明显数据驱动、扩展性强、模块间接口少整个系统可以被视为一个可学习的整体。但代价也摆在明面上可解释性差、闭环测试成本高、对边缘场景的覆盖几乎完全依赖数据规模。十年看下来规划算法的方法论发生了根本性变化但工程上没有任何人把老方法完全扔掉。实际量产方案仍然以“多方法混合”为主——优化保底、学习提优、规则安全兜底。这个结论到现在依然成立。2. 先搞清楚规划算法到底在解决什么问题2.1 全局规划与局部规划的分工聊规划算法之前必须先把两个容易被混在一起的概念拆明白全局路径规划路由和局部路径规划运动规划。全局路径规划解决的是“从A到B走哪条路”的问题输入是导航地图和起点终点输出是一条车道级别的参考路径。这一层用的算法仍然以图搜索为主A*、Dijkstra足够用顶多加一些车道级约束和代价权重。局部路径规划解决的是“在当前车道或路口接下来几秒具体怎么走”的问题。输入是全局参考线、自车状态、动静态障碍物信息输出是未来5到8秒的轨迹序列形式上通常是带时间戳的位置点、速度、朝向。这一层才是规划算法的核心战场也是过去十年内卷最厉害的地方。两者的关系是串联的全局给局部提供参考局部负责具体的微观行为。工程实现上全局规划可以低频运行1Hz就够了局部规划必须高频运行10Hz甚至50Hz以上因为实时避障和轨迹跟踪的需要倒逼刷新率。2.2 横纵向解耦为什么主流方案都这么干绝大多数量产规划方案都会把轨迹规划拆解成横向规划和纵向规划分别解决“往哪拐”和“怎么走”两个问题。横向规划在Frenet坐标系下进行把自车投影到参考线上用横向偏移量表示车辆相对参考线的左右位置。因为绝大多数行驶场景中车辆都是沿参考线走的横向偏移通常都控制在小范围内这个强先验让横向规划问题被大幅简化。纵向规划则负责沿参考线方向的速度和加速度处理跟车、停车、红绿灯、汇入等纵向行为。为什么大家统一选择解耦原因有两条。第一问题维度显著降低。横纵向联合优化的参数空间很大非线性约束复杂求解难度成倍上升解耦后可以分别用成熟的一维求解器处理又快又稳。第二工程调试极其友好。实车测试中横向问题压线、换道抖动和纵向问题跟车顿挫、刹车时机错往往是独立出现的解耦后可以精准定位问题模块不用一上来就面对一个黑盒。当然解耦也有代价高速过弯时横纵向强耦合解耦方案很难给出极限工况下的最优轨迹。但量产方案仍然以解耦为主原因很简单足够好用而且经过大规模验证。2.3 代价函数设计规划算法的“价值观”不管用采样、优化还是学习最终评价一条轨迹好不好的几乎都会落到代价函数上。代价函数就是规划算法的“价值观”它用一套可计算的数值告诉系统什么样的轨迹是好的什么样的轨迹是坏的。一套典型的规划代价函数通常包含四个大项。曲率代价项惩罚急打方向保证路径可跟踪且轮胎不尖叫加加速度jerk代价项惩罚加速度变化率避免乘员晕车与参考线偏移代价项惩罚偏离车道中心的横移保证跟线行驶与期望速度差距代价项引导车辆按交通流速度行驶。这里面每一项都有权重权重乘起来就是总代价。我自己做工程最大的心得是权重不是靠数学推导出来的是靠实车标定一遍遍试出来的。平滑性权重太高车会“很肉”变道慢吞吞被后车骂安全性权重太高车会频繁急刹乘坐体验极差。代价函数的调参本质上是系统工程问题不是算法问题。所以每次看到算法论文里漂亮的双目标权衡曲线我都很明白那只是实验室里的结果真正量产项目的权重表往往是厚厚一叠试驾记录和投诉工单堆出来的。3. 关键技术点拆解采样、曲线、优化与学习3.1 采样与图搜索RRT、Hybrid A*与Lattice Planner先说RRT。RRT的玩法是在连续空间里随机撒点从起点不断向采样点扩展树结构直到连上终点。优点是概率完备只要时间足够理论上一定能找到通路缺点是随机性太强生成的是折线路径几乎无法直接用作控制参考需要大量后处理。在高速场景里RRT的实时性和稳定性都不够所以它更多被用在后处理或者低速场景的快速搜索里。Hybrid A是图搜索的连续化变体。传统A在离散栅格上搜索Hybrid A额外考虑了车辆运动学模型每个节点不仅包含位置还包含航向和曲率约束。这样搜出来的路径天然满足车辆可行驶性要求不需要像RRT那样做完再花大功夫磨平。DARPA挑战赛之后Hybrid A成了低速场景规划的事实标准泊车入库的粗路径搜索基本都是它。Lattice Planner则是另一个思路在Frenet坐标系下预先生成一大簇候选轨迹每条轨迹都由一组离散化的端点状态定义然后用代价函数排序挑一条。它的优势是生成轨迹天然可执行、平滑性好而且候选轨迹可以并行计算实时性不错。缺点是全局性弱只能做未来几秒的局部决策没法处理整个路由级规划。实际量产做高速领航辅助经常是“Lattice Planner局部优化”组合全局路由仍然由A*之类的图搜索负责。3.2 曲线生成五次多项式、贝塞尔与B样条怎么选有了参考路径还得把它变成平滑、可跟踪的轨迹这就轮到各种曲线族登场。最常见的三类是五次多项式、贝塞尔曲线和B样条。五次多项式在换道场景用得最多已知起点的位置、速度、加速度终点的位置、速度、加速度一条五次多项式可以唯一确定一条平滑曲线边界条件天然满足。它的优点是公式简单、闭式解、实时性好缺点是一条多项式只能拟合一段曲线复杂场景需要分段拼接分段处如果约束没给够曲率就可能不连续。贝塞尔曲线的特点是控制点的凸包性质生成的曲线一定落在控制点围成的凸包内局部可控性强。但它拟合历史路径时不是精确经过每个点应用起来需要调参。B样条则可以理解成贝塞尔曲线的“升级版”通过控制点和节点向量定义曲线既能精确拟合又保持局部平滑性在泊车和复杂道路结构中特别好用。我个人的工程偏好是能用五次多项式就用五次多项式公式简单意味着调试成本低、出问题好定位。B样条留给泊车和复杂场景贝塞尔则更适合做视觉辅助或简单路径美化。别一听B样条高端就处处用曲线工具的选择永远服从于“够用且可维护”。3.3 优化框架QP与MPC为什么能成为工程首选轨迹规划里的优化问题绝大多数可以建模为二次规划QP。决策变量是轨迹点序列或多项式系数目标函数是二次项加权和约束则是线性化后的运动学、避障和边界条件。QP能成为量产主流靠的是求解器成熟、求解速度快几毫秒就能给出确定性解。对量产车来说“确定性”和“快”比什么都重要因为规划模块计算超时带来的直接后果就是控制模块失去参考车辆进入降级状态。MPC模型预测控制的引入则把规划和控制的边界打通了。它像一个“边走边算”的司机在每个控制周期基于当前状态求解未来N步的优化问题只执行第一步然后滚动往前推。MPC天然支持硬约束、预测信息和控制约束处理横纵向耦合关系也比解耦方案更“全局”。2019年以后大量团队把MPC用到轨迹跟踪甚至直接做轨迹规划。但MPC绝不是什么银弹。非线性约束带来的求解开销大模型失配时性能下降明显调参维度巨大而且一旦陷入局部极小很难快速跳出。量产路线里更常见的配置是“QP保底MPC优化”或者“MPC只做跟踪”把最激进的部分留给仿真环境慢慢磨。3.4 学习方法从模仿学习到端到端学习方法介入规划以后大致有三大方向。模仿学习Behavior Cloning是最直接的思路用专家轨迹做监督学习输入场景信息输出轨迹或策略。但原生模仿学习的问题非常典型数据集偏移和误差累积模型一遇到分布外的输入就会越走越偏。Dagger算法比较好地处理了这一点在闭环交互中当模型犯错时把专家拉回来纠正让模型逐步适应真实分布。逆强化学习IRL走的是另一个方向不直接学习策略而是从人类驾驶数据中反推奖励函数再用这个奖励函数去驱动优化规划。这个思路对自动驾驶天然契合因为我们希望车“像人一样开”但人的真实驾驶目标舒适、高效、守法、安全没法手工写成一套完备的公式。端到端规划是最激进的方向输入传感器原始数据直接输出轨迹。UniAD用统一Transformer串起感知、预测、规划发布后引发了大量关注。但这种方案现阶段最大的软肋还是老三条可解释性差出问题难以定位闭环仿真验证安全边界困难边缘事件覆盖完全依赖数据规模。这也是为什么直到今天学习式规划在量产车里更多是“辅助”角色而不是“主力”。我自己的判断是学习方法的完全落地还差一个大规模闭环仿真基础设施和一个足够大的安全验证集。4. 工程落地经验从论文到可量产代码的必经之路4.1 实时性与算力约束50ms以内要给出新轨迹规划模块的实时性要求有多苛刻拿我参与过的一个量产项目举例规划周期设计为20Hz也就是每隔50ms必须输出一条新的有效轨迹。如果单次计算超过100ms没有有效输出跟踪模块就会失控车辆直接进安全兜底状态。实时性要求一方面来自硬件成本量产车不是学术服务器嵌入式平台的算力有限规划算法跑不了几秒钟一次的暴力搜索另一方面来自安全冗余规划模块一旦过载下游所有模块跟着遭殃。所以工程优化的第一优先级永远是减小最坏情况计算时间也就是WCET而不是平均计算时间。具体手段包括调低网格粒度、障碍物栅格化降采样、简化动力学模型、缓存上周期解作为热启动初值。更关键的一条是必须有明确的降级策略——算不过来时用上一周期轨迹做抑制性修改而不是重新从头规划。这个兜底逻辑往往比算法本身更决定量产成败。4.2 代码实现中的关键细节坐标系、离散化与碰撞检测规划算法的代码写得好不好很大程度取决于工程细节而不是算法选型。第一个大坑是坐标系。全局坐标系经纬度大地高不适合直接做规划数值运算因为距离、曲率的计算都带畸变。主流做法是转到局部平面坐标系作为规划坐标系再到Frenet坐标系下做轨迹规划。坐标转换如果没做好最直观的故障就是规划轨迹在地图边界附近出现突兀跳变实车表现为车辆“突然打一把方向”。第二个坑是离散化精度。轨迹点的时间步长一般取0.1秒到0.2秒空间步长约0.5米到1米。步长太大轨迹稀疏碰撞检测容易漏检步长太小计算量指数上升。这里没有银弹只能根据场景速度动态调整。第三个坑是碰撞检测。最常见的做法是把车辆包络近似为矩形用分离轴定理SAT判断两个矩形是否相交障碍物则用凸多边形或栅格逼近。这类方法的隐藏问题在于碰撞检测必须考虑障碍物的预测误差和不确定性安全距离不能只留零。实际工程中通常在每个轨迹点附近加缓冲距离或者直接把碰撞约束设计成“软约束”允许一定程度的侵入但施加高额代价惩罚。我的经验是写碰撞检测代码时先测量几何库的耗时再定离散化和分辨率顺序反了后面全是性能坑。4.3 一个最小可复现的路径规划算法思路结合“路径规划算法代码”这个关键词我分享一个最简单但完整的实现思路栅格地图上跑A*得到参考路径再做平滑。这个组合虽然算不上先进但能快速验证一整套规划闭环的流程特别适合入门。一段极简的A*核心逻辑代码示意如下import heapq def astar(grid, start, goal): open_set [(0, start)] came_from {} g_score {start: 0} heapq.heapify(open_set) while open_set: _, current heapq.heappop(open_set) if current goal: return reconstruct_path(came_from, current) for neighbor in get_neighbors(grid, current): tentative_g g_score[current] cost(current, neighbor) if tentative_g g_score.get(neighbor, float(inf)): came_from[neighbor] current g_score[neighbor] tentative_g f_score tentative_g heuristic(neighbor, goal) heapq.heappush(open_set, (f_score, neighbor)) return None这段代码的核心就三件事用优先级队列维护待扩展节点、用g_score记录最短代价、用启发式函数引导搜索方向。理解了它后面加车辆运动学约束、加曲线平滑、加代价函数都是在框架上加“肉”的过程。工程注意两点启发式函数选择欧氏距离还是曼哈顿距离直接影响搜索速度和最优性如果规划要考虑车辆航向就得升级到Hybrid A*启发式函数还得加入最小转弯半径约束。别小看这个“入门版”我见过不少量产级规划模块的早期原型就是这么长出来的。5. 场景学科普泊车与无人机里的路径规划算法5.1 泊车路径规划为什么低速场景反而是硬骨头泊车看起来速度低、空间小、路径短但它实际上是规划算法里最难啃的骨头之一。难点在于三个方面泊车空间狭小大概率需要多段倒车车辆低速下的运动学模型有强非线性约束比如最小转弯半径、倒车时方向感难以直观判断感知盲区大随时可能冒出一个推婴儿车的行人。低速并不等同于简单。主流泊车路径规划算法的组合拳是Hybrid A加Reeds-Shepp曲线再加局部优化。第一步用Hybrid A在栅格地图中搜索泊车位内的粗路径第二步用Reeds-Shepp曲线求解精确最短路径专门处理多段倒车动作第三步用数值优化平滑轨迹并加入碰撞检测和代价项。这套流程在量产自动泊车系统里已经非常成熟。实车体会最深的一点是泊车系统最怕“最后一厘米”搜索和优化都正常收敛但车头离墙近到误差被放大方向盘一动就蹭上。所以量产方案里都会在最终轨迹上叠一层“贴近检测安全距离检查”确保任何时刻车辆包络与障碍物距离不小于设定阈值而且阈值要根据车宽、车速、感知噪声自适应调整。看起来是小事但没有这一层泊车系统永远上不了市。5.2 无人机路径规划三维空间的异与同无人机路径规划算法这几年热起来因为它和智能驾驶规划算法在底层思维上高度同源。相同点很多二者都是“从起点到终点在约束条件下找到最优轨迹”都要处理障碍物、动力学约束和实时性要求搜索、采样、优化、学习这四类方法同样适用。我甚至见过团队直接把自动驾驶里的QP轨迹优化改一改就用到无人机航迹避障上。不同点也很明显。无人机的规划空间是真正的三维智能驾驶基本是二维平面加一个朝向无人机的动力学模型有6自由度偏航角受约束但垂直方向自由可解空间比车大得多无人机载荷限制导致感知范围有限常常要在未知地图上“边飞边规划”。因此无人机领域更常用RRT*、D* Lite、PRM这类采样搜索方法辅以B样条轨迹优化。而智能驾驶在同等任务里更依赖高精地图和结构化道路先验。两个领域互相借鉴的潜力非常大——比如无人机航迹规划里的不确定性推理和软约束建模放到自动驾驶的阻塞预测里也完全成立。5.3 通用规划思维换个领域依然吃得开如果剥离具体场景只看搜索、采样、优化、学习这四种范式你会发现它们几乎能覆盖所有路径规划问题机器人、无人机、无人车、物流仓储调度统统适用。我的建议是别只背模型要理解“代价函数、约束、求解器”这三件套。只要手里有一个明确可计算的代价函数有一组可表达的约束再选一个适合问题规模的求解器剩下的工程细节全都可以复用。这套思维才是十年规划算法演进留给我最值钱的东西。算法会过时但“建模求解”的方法论转移到一个新领域时依然能快速落地。6. 常见问题与排查技巧实录6.1 轨迹不平滑、曲率突变怎么办现象是轨迹点与点之间方向跳变大控制模块跟随吃力实车表现为方向盘来回抖。排查顺序很重要第一先检查参考线质量参考线不连续会导致Frenet坐标系下横向偏移跳变第二检查曲线生成阶数两个曲线的连接处曲率不连续第三才轮到优化目标看平滑性权重是不是太低、加加速度项是不是漏了。不少人一上来就调优化器花了两天发现是参考线的问题白白浪费时间。对策是增强平滑性代价项、分段曲线间加曲率连续性约束最后还可以再用样条做一次后处理平滑。6.2 优化为什么不收敛很多刚上手的团队都会碰到求解失败连续几百毫秒没有新轨迹输出。主要原因有三个约束太紧比如碰撞距离设定过小且障碍物密集可行域直接为空初始解质量差优化器困在局部极小数值尺度不一致位置和速度的量纲差太多导致矩阵病态。对策是软约束化把硬碰撞约束改成高代价的软约束增加松弛变量用上一周期轨迹做热启动初值对决策变量做坐标归一化。一个通用口诀先保证可解再谈最优。可解都没有最优就是空话。6.3 碰撞检测为什么漏检现象最吓人规划的轨迹点和障碍物间距明明低于安全阈值系统却毫无反应直到实车急刹。原因通常是栅格分辨率太低小尺寸障碍物恰好落进栅格空隙被忽略时间步长太大两个轨迹点之间直接“穿透”了障碍物还有一个隐蔽原因是忽略障碍物预测误差把运动中的障碍物当成了静止物。对策是把栅格分辨率提升到合理水平通常20厘米以内轨迹点在关键区域加密碰撞检测必须考虑障碍物预测协方差再加一层安全缓冲距离。我见过最离谱的漏检案例是因为栅格地图的“障碍物膨胀系数”设成负数压路机都给贴没了。6.4 学习模型在实车上“失灵”怎么办现象是仿真里表现优秀一上实车就乱来。核心原因永远是分布偏移仿真场景分布和真实道路分布差异太大模型在仿真里见过的东西和实车拍到的完全不是一回事。闭环交互误差累积也是一个重要因素模型每一步的小误差在前方几秒的预测里被放大。解决办法有三个大量搜集实车困难场景数据补进训练集采用人机协同方案让学习模型输出只作参考规则化模块兜底在闭环仿真平台上做大量回归测试确保学习模型不会把车辆带入不可控状态。说白了学习式规划最缺的不是模型能力而是数据覆盖和验证闭环。6.5 常见问题速查表现象主要原因优先排查项对策轨迹不平滑、曲率突变参考线跳变 / 曲线阶数低 / 平滑权重低参考线质量加连续性约束、后处理样条平滑优化不收敛约束过紧 / 初始解差 / 数值尺度不一致热启动与软约束软约束化、上一帧热启动、坐标归一化碰撞检测漏检栅格分辨率低 / 步长过大 / 忽略预测误差栅格分辨率与时间步长提升分辨率、加密轨迹点、加缓冲距离学习模型实车失灵分布偏移 / 累计误差数据覆盖度实车数据补训、规则化兜底、闭环回归测试最后聊一点我自己的体会。回头看这十年规划算法从“找得到路”走到“开得像人”方法论迭代了三四次但真正拉开团队差距的从来不是模型花不花哨而是工程细节异常处理对不对失败降级能不能兜住代价函数权重调得是否合理。我见过不少团队论文发得漂亮实车一跑就露馅。我的建议是如果你刚入行先把A*、五次多项式、QP这一套基础吃透再考虑上学习方案。规划算法最底层的逻辑永远是安全问题先于性能问题这一点无论十年怎么演进都不会变。