1. 先看十年大趋势从“能开”到“敢坐”2015年我刚开始接触智能驾驶规划算法时业内谈得最多的是“这车能不能自己拐弯”。到了2025年大家讨论的已经是“这车能不能在晚高峰的窄路上自己找车位”。同样叫规划算法这十年的内涵变化用“天翻地覆”来形容一点不过分。规划算法在智能驾驶系统里的位置一句话概括就是感知告诉车“周围有什么”规划告诉车“接下来怎么走”。它往下衔接控制往上承接决策是整个系统的“大脑思维层”。2015年那会儿主流方案还在用A*、RRT这类源自机器人领域的经典搜索算法处理的是结构化道路上的简单场景到2025年端到端模型、Occupancy Network、VLA模型直接冲击传统管线规划算法的技术栈和思维模式都发生了根本性变化。这篇文章我想以这十年亲历者的视角把规划算法的演进脉络、核心方案背后的取舍逻辑、工程落地的坑点以及“端到端时代传统规划何去何从”这个热门话题一次性说透。无论你是刚入行的学生还是想转行做规控的工程师这篇文章都能给你一张相对完整的地图。提示文章里涉及的具体算法参数和实现细节我会结合多年来在实车上跑过的经验来讲不会只停留在纸面推导。2. 核心算法逐个数每个方案背后的取舍逻辑2.1 搜索派的开山鼻祖A与Hybrid A先聊A*。这个算法学机器人或AI的人都不陌生——栅格地图上从起点到终点维护一个open list和一个closed list每次取f值最小的节点扩展直到找到终点。它的本质是BFS的“带方向优化版”用启发式函数比如曼哈顿距离或欧氏距离指引搜索方向避免盲目扩展。在2015年左右的智能驾驶项目里A主要用在低速场景比如园区通勤车、封闭场地的摆渡车。原因很简单低速车辆可以近似看成质点航向角约束不那么致命栅格分辨率取0.5米甚至1米也够用。但放到开放道路的高速场景A就露怯了——它规划出来的路径是“折线”车辆在高速行驶时根本没法直接跟踪因为曲率不连续、方向突变的点会让方向盘在瞬间打满乘坐体验和安全性都无法接受。于是出现了Hybrid A*这可以看成是“带运动学约束的A*”。它不再把车当成质点而是把状态扩展成 (x, y, yaw)用转向角、轴距来模拟车辆的转弯半径。斯坦福在2007年DARPA Urban Challenge夺冠时用的就是这套思路2010年前后发表的论文直接引爆了这个方向。Hybrid A*的核心代价函数设计是门手艺活我当时调参时印象很深[ f(n) g(n) h(n) \alpha \cdot \text{转向惩罚} \beta \cdot \text{挡位切换惩罚} ]g(n)是已经走的路径长度h(n)是用Reeds-Shepp曲线算出来的无障碍最短路径长度转向惩罚让搜索倾向于“少打方向盘”的路径挡位切换惩罚则避免路径上频繁前进后退。这个公式看起来简单但三个权重系数的标定直接决定轨迹质量。我记得很清楚的例子转向惩罚系数调太大车辆会走“绕大圈”的路径在窄路里直接把自己堵死调太小路径上全是锯齿形的小幅转向控制端抖得不行。A*的代码实现思路其实很固定核心骨架如下def astar(grid, start, goal): open_list PriorityQueue() open_list.put((0, start)) came_from {} g_score {start: 0} while not open_list.empty(): _, current open_list.get() if current goal: return reconstruct_path(came_from, current) for neighbor in get_neighbors(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 tentative_g heuristic(neighbor, goal) open_list.put((f, neighbor)) return None而Hybrid A*扩展节点时不是往四邻域或八邻域扩而是按一组离散的转向角比如-30°、-15°、0°、15°、30°积分出一段段圆弧每段弧的长度由车辆速度和步长时间决定。这样扩展出来的节点天然满足车辆运动学约束这就是“Hybrid”的含义——混合了“栅格离散”和“连续运动学”。2.2 采样派代表RRT与它的变体们RRTRapidly-exploring Random Tree是另一个从机器人领域引入的流派。它的思路很反直觉在地图上随机撒点把树向随机点方向生长直到树连接到目标点。因为不依赖精确的栅格划分RRT在高维空间比如机械臂的7自由度关节空间表现很好理论完备性方面也有概率保证。但把RRT原封不动搬到智能驾驶上问题很多。最典型的是路径“过于随机”——它找得到路但找的往往不是一条人类愿意走的、平滑的路。想象一下一个新手司机在停车场里反复倒车、打轮最后也停进去了但过程让人晕车——RRT生成的轨迹有时就是这种感觉。另一个问题是随机采样在狭窄通道场景下效率极低树要“挤”过门缝一样的空隙可能要采样几万个点才碰巧成功。后来出现的RRT在渐进最优性上做了改进每次找到新节点后会检查它周围一定半径内的已有节点看是否存在代价更小的连接方式把这个节点“重新接线”rewire。这个操作让树的拓扑结构不断优化理论上能收敛到最优解。在智能驾驶领域RRT最成功的应用场景是泊车——尤其是有障碍物遮挡、通道狭窄的复杂车位搜索类方法容易陷入组合爆炸RRT*反而能用概率优势快速找出一条可行通道。我跟朋友开玩笑说A和RRT的选择有点像“严谨的公务员”和“天马行空的艺术家”——A老老实实地遍历所有可能路径保证找到最优RRT则靠随机采样碰运气速度快但结果不稳定。实际工程里很多泊车系统会先用RRT*粗搜一条可行通道再用参数化曲线把它平滑成可执行的轨迹两者配合效率更高。2.3 老牌劲旅参数化曲线与Lattice Planner搜索和采样解决的是“有没有路”的问题但工程上还需要解决“这条路好不好开”的问题。于是有了Lattice Planner——百度Apollo早期版本的核心规划器也是我职业生涯早期花时间最多的一个模块。Lattice Planner的思路可以这样理解车在Frenet坐标系纵向距离s横向偏差l下运动规划器在s方向和时间方向上采样一系列目标状态然后用五次多项式或三次多项式把这些目标状态连成候选轨迹再从中挑选一条代价最低的。Frenet坐标系的妙处在于道路的曲率被“拉直”了横向偏差和纵向速度可以分开优化这让轨迹的语义非常清晰——哪条轨迹更贴中心线、哪条轨迹加速更快一眼就能看出来。我记得当时采样参数是这样设置的纵向采样间隔每1.0秒一个点总共采样3~5秒的时间范围覆盖30~80米的前方距离横向偏移采样以当前车道中心为基准向左右各采样-3.5米到3.5米间隔0.5米末端速度采样从当前速度到当前速度10 m/s间隔2 m/s每次规划周期大概会生成几百条候选轨迹。代价函数包含跟车距离、偏离中心线的程度、加速度冲击度jerk、与障碍物的距离等多维项。选最小代价轨迹时还要做碰撞检查——候选轨迹先经过“碰撞”筛选剩下的才进入代价评比。Lattice Planner最大的优点是“生成轨迹本身就是平滑的”不需要额外的后处理最大的缺点是“横向采样粒度有限”在弯道多、曲率变化大的场景下0.5米的横向采样间隔可能不足以覆盖所有安全空间。比如一个急弯处安全走廊可能只有——我举个例子——道路边界内20厘米的余量这时候0.5米的采样步长就可能“漏掉”最优解。Apollo后来引入了EM PlannerExpectation Maximization Planner本质上还是在Frenet坐标系下做DPQP两段式优化。DP动态规划粗搜出一条参考线把问题空间缩小QP二次规划在这个凸空间内求解最优轨迹。EM的思路可以简单概括为“先找到大概对的路再精确优化”这种递进式策略大幅提升了复杂场景的求解成功率。2.4 路径规划算法代码一个可运行的样例说了这么多理论给一个可以直接跑起来感受一下的简化例子。我用一个简化的Frenet坐标系Lattice采样逻辑来展示核心思路配置好环境就能跑通import numpy as np def generate_candidate_trajectory(s0, s_dot0, s_ddot0, target_s, target_s_dot, T): # 五次多项式s(t) a0 a1*t a2*t^2 a3*t^3 a4*t^4 a5*t^5 a0 s0 a1 s_dot0 a2 s_ddot0 / 2.0 A np.array([ [T**3, T**4, T**5], [3*T**2, 4*T**3, 5*T**4], [6*T, 12*T**2, 20*T**3] ]) b np.array([ target_s - (a0 a1*T a2*T**2), target_s_dot - (a1 2*a2*T), 0 # 末端加速度设为0让轨迹更平滑 ]) x np.linalg.solve(A, b) a3, a4, a5 x return [a0, a1, a2, a3, a4, a5] # 示例参数 s0, s_dot0, s_ddot0 0.0, 10.0, 0.0 target_s, target_s_dot, T 30.0, 12.0, 2.0 coeffs generate_candidate_trajectory(s0, s_dot0, s_ddot0, target_s, target_s_dot, T) print(五次多项式系数:, coeffs) # 在t1.0秒时计算纵向位置 t 1.0 s_t 0 for i, c in enumerate(coeffs): s_t c * (t ** i) print(ft{t}s时纵向位置: {s_t:.2f}m)运行这段代码你能直观看到五次多项式如何保证“起点状态”和“终点状态”都被约束住中间过程平滑过渡。真实工程里还会对横向偏移做同样的多项式拟合然后把纵向和横向合成到笛卡尔坐标系下得到最终的XY路径点序列。3. 算法落地的工程真相从仿真到真车的gap3.1 第一坑坐标系与坐标变换规划算法在纸面上跑得再好上车之后第一个暴击往往来自坐标系。感知模块输出的障碍物一般在自车坐标系原点在后轴中心x向前y向左地图模块给出的是绝对坐标系下的道路信息比如UTM坐标而规划算法内部运算又经常用Frenet坐标系。这三者之间的转换一旦出问题轻则轨迹抖动重则撞上真实障碍物。我最深的一次教训是处理“时延”问题。感知输出的障碍物位置是“过去某一时刻”的位置而规划算法计算时用的是“当前时刻”的位置。在高速场景下比如100 km/h100毫秒的延迟意味着车辆往前跑了2.8米。如果不做运动补偿——也就是用障碍物的历史速度和加速度外推它的当前位置——规划出的轨迹可能已经穿过“实际上的障碍物”了。这个细节很多仿真环境里根本不会暴露因为仿真里的感知是“理想同步”的但实车一定会暴露。坐标变换的代码核心其实很简单但方向反了就会出现完全相反的结果import math def global_to_vehicle(global_point, vehicle_pose): # vehicle_pose: (x, y, yaw) dx global_point[0] - vehicle_pose[0] dy global_point[1] - vehicle_pose[1] cos_yaw math.cos(vehicle_pose[2]) sin_yaw math.sin(vehicle_pose[2]) local_x dx * cos_yaw dy * sin_yaw local_y -dx * sin_yaw dy * cos_yaw return (local_x, local_y)很多新手不注意“坐标轴方向”和“航向角参考系”写完转换之后直接在仿真里通过一上车就发现轨迹偏移。建议的做法是写单元测试输入一组已知坐标手动推导期望输出再加一个反向变换做往返一致性校验这样能拦截掉绝大多数基础错误。3.2 第二坑轨迹平滑与曲率连续性规划算法输出的轨迹控制模块是要直接去跟踪的。如果轨迹上的曲率不连续控制模块的反馈会非常痛苦——方向盘会一直抖车辆会画龙。我曾经接过一个项目规划算法用的是纯几何样条曲线拼接看起来挺优美但曲率在拼接点处出现了突变上车之后方向盘在拼接点处“咔哒”一下猛打把测试工程师吓出一身冷汗。解决曲率连续业界最常用的做法是“曲率平滑后处理”。具体可以分为两步第一步用三次样条或五次样条对原始路径点做插值保证位置、一阶导数航向、二阶导数曲率连续第二步用非线性优化对轨迹进行“推优化”目标函数约束最大曲率、最大曲率变化率我习惯用的一个简化版曲率约束优化核心思想是把相邻三个路径点确定的圆弧曲率作为状态量限制其变化率[ \text{minimize} \quad \sum_{i1}^{n-1} \left( \kappa_{i1} - \kappa_i \right)^2 \lambda \sum_{i1}^{n} \kappa_i^2 ]第一项让曲率变化尽量平缓第二项防止曲率本身过大(\lambda)是正则化系数控制两项的权重。这个优化问题规模不大用OSQP或自定义的高斯牛顿都能在几毫秒内解完。需要注意的是平滑后的轨迹必须要重新做碰撞检测因为平滑过程可能让轨迹“侵占”到障碍物一侧。注意轨迹平滑和碰撞检测是一个闭环迭代过程。平滑让轨迹更舒适但可能破坏安全性碰撞检测发现问题后要重新在局部生成替代轨迹。实际工程里这两个模块往往要循环执行2~3轮才能得到既安全又平滑的结果。3.3 第三坑规划与控制的时间同步规划模块的输出并不只是“一条路径”而是一条带时间戳的轨迹——每个路径点都对应一个期望到达时刻。控制模块跟踪这条轨迹时如果规划周期比如100ms和控制周期比如10ms之间不同步控制模块就会一直在跟踪“过期”的轨迹点。我记得有一次实车测试车辆在过弯时出现明显的“一卡一卡”现象。排查了半天问题出在规划模块输出的轨迹点时间戳用的是“规划计算完成时刻”而控制模块读取时用的是“读取时刻”两者之间差了将近一个规划周期的时间偏移。结果控制模块总是跟踪一个落后于实际的点导致每周期都产生约0.3米的跟踪误差在弯道中被放大成明显的顿挫感。解决方式也很直接规划模块每次输出轨迹时不是从“当前时刻”开始而是从一个“未来时刻”通常是下一次控制周期开始的时间开始。比如规划周期100ms、控制周期10ms规划模块可以预瞄到“当前时刻110ms”的状态作为轨迹起点保证控制模块跟踪时看到的是“未来”的轨迹而不是“过去”的轨迹。这个偏移量的标定需要在实车上反复试不同的传感器延迟特性、不同的控制模块响应速度都会影响最优值。4. 泊车与无人机两个场景的规划算法变体4.1 泊车为什么是“低速但高难”的典型泊车路径规划在热词里出现频率很高这确实是个独特的场景。说它低速是因为泊车过程中车速通常不超过5 km/h说它高难是因为空间极其狭小、需要反复前进后退操作对规划的“几何可行性和多段操作”能力要求很高。解决泊车问题主流方案有两种路线。第一种是“搜索平滑”用Hybrid A*搜索一条满足车辆运动学约束的粗糙路径再用优化方法平滑。2010年前后Stanford团队的论文已经证明这条路可行但问题是窄小车位里搜出来的路径经常带有大量的“挡位切换”每条小弧段长度可能只有几十厘米平滑之后依然可能碰壁。第二种是“数值优化”路线把泊车问题建模成非凸优化[ \min_{x} \sum_{k0}^{N} | \mathbf{x}k - \mathbf{x}{ref,k} |^2_Q | \mathbf{u}_k |^2_R ]约束条件包括运动学模型、转向角限制比如±35°、障碍物距离限制车辆轮廓到障碍物的最小距离大于安全阈值、路径曲率限制等。这个优化问题是非凸的不能直接求全局最优一般用SQP序列二次规划或罚函数法迭代求解。工程实战中通常会先用Hybrid A*生成一条初始可行解作为优化起点再用优化方法把路径“磨”得更加平滑、贴边。这叫“热启动”——好的初始解能大幅提升优化收敛速度和结果质量。泊车场景的另一个特点是“终点姿态约束严格”——最终车辆必须精确停入车位正中且航向角要与车位方向一致。这个约束在优化问题里必须作为硬约束处理否则车辆可能停在歪斜的位置虽然也能入库但用户体验极差。4.2 无人机路径规划同一个家族的不同分支无人机路径规划算法和智能驾驶规划算法经常并列出现在讨论里很多人以为两者是同一套技术。“它们同宗但不同路”我来具体说说。两者的底层技术栈确实有交集——A*、RRT、优化算法在无人机上也很常见但约束条件差异巨大维度差异无人机是三维空间运动智能驾驶基本是二维平面运动忽略坡道时。三维空间的状态空间大得多搜索算法的“组合爆炸”问题更严重运动学差异无人机尤其是多旋翼可以悬停、垂直起降、任意方向飞行没有非完整约束——它不需要像汽车那样考虑转弯半径和倒车但无人机有动力学约束加速度、速度、姿态角速率限制轨迹规划必须考虑“能不能飞出来”安全空间差异智能驾驶的安全距离是厘米级的无人机在城市峡谷等场景的安全距离通常是米级的无人机领域最常用的规划算法是“混合A*的采样 多项式轨迹优化”——先粗搜一条平滑轨迹再用Minimum Snap最小加加速度优化得到动力学可行的轨迹。Minimum Snap的思想是让四旋翼在航点之间飞行时四个电机推力的变化率尽量小这样飞得稳、不抖。它的本质是一个二次规划问题约束条件是航点的位置、速度、加速度目标函数是最小化加加速度的平方积分[ \int_0^T \left( \frac{d^3 \mathbf{r}}{dt^3} \right)^2 dt ]这个目标函数在数学上的好处是它是凸的求解快而且得到的轨迹非常平滑。但缺点是需要预先知道航点的位置和时间分配——如果时间分配不合理轨迹会很“别扭”比如某段飞行时间过长导致速度过低另一段又太短导致加速度过大。工程上通常会在轨迹求解后检查速度和加速度是否超限如果超限就增大对应航段的时间重新求解。这个过程叫“时间重分配”是无人机轨迹规划里一个重要但常被忽略的环节。5. 端到端的冲击传统规划算法会被淘汰吗5.1 端到端到底改变了什么2023年以来端到端End-to-End成为智能驾驶领域最热的话题。端到端模型把“感知-预测-规划”压缩成一个神经网络输入多摄像头图像有时加激光雷达点云直接输出控制指令方向盘转角、油门、刹车或轨迹点。这个思路的本质是“用数据驱动替代规则驱动”——传统的规划算法本质上是一堆人为设定的规则和公式端到端模型则是从海量驾驶数据中“学习”出规划策略。端到端对大算力的需求非常夸张。一个典型的端到端模型比如基于Transformer架构的驾驶模型参数量通常在数亿到数十亿级别训练需要上千张A100/H100 GPU。训练数据则是数十万小时的真实驾驶视频和对应的控制指令。这种规模让很多中小团队望而却步也让“数据闭环”成为行业竞争的真正壁垒。但端到端不是银弹。它最大的问题是“可解释性差”——模型在某种场景下做了一个奇怪的决策工程师很难说清楚是哪里出了问题。这带来了两个工程层面的困难一是安全验证变得困难传统规划算法可以用形式化方法穷举验证端到端模型则只能依赖大量测试二是“长尾问题”处理困难——模型在训练数据稀疏的场景比如极端天气、非常规障碍物下行为不可预测。5.2 传统规划工程师的生存之道我身边不少做传统规划的同行在2024年那波端到端热潮中感到焦虑担心自己积累多年的A*、Lattice、QP优化经验一夜之间变成废纸。但其实“传统规划技术不会消失只会换一种形态存在”。原因很简单端到端模型输出的是轨迹或控制指令但系统的其他模块仍然需要大量“传统”技术。首先模型本身需要监督信号和数据标注——标注数据时通常需要参考传统规划器的输出。其次端到端模型输出的轨迹往往不够平滑需要后处理模块用样条插值、曲率约束优化进行修正——这些正是传统规划的核心技术。最后安全兜底机制不可或缺——当端到端模型输出异常时系统需要一个“安全护栏”比如RTKReal-Time Kinematic边界限制、紧急制动策略这些也都依赖传统规则算法。从更宏观的视角看端到端解决的是“感知信息如何变成驾驶行为”的映射问题而规划算法解决的是“在约束条件下如何生成最优轨迹”的优化问题。后者本质上是一个数学问题跟感知用什么模型没有直接关系。哪怕感知端完全变成端到端规划端的优化框架依然成立。我给同行们的建议是不要丢掉传统规划的基本功但要主动学习端到端的思维模式。一个既能写QP优化又能理解Transformer注意力机制的工程师在未来两三年内会非常抢手。6. 给想入行的人的一点建议很多读者问我2025年了想入行智能驾驶规划算法应该从哪里入手。我的建议是先动手把经典算法的代码写一遍不要停留在“看懂了”的层面。“看懂了”和“能上手调参”之间隔着无数个细节。A的open list用什么数据结构Hybrid A的Reeds-Shepp曲线怎么生成Frenet坐标和笛卡尔坐标的转换公式怎么推导这些不看代码自己实现一遍永远不会真正理解。第二个建议是“把仿真当玩具把实车当真考验”。再好的仿真环境也模拟不了真实的传感器噪声、通信延迟、机械执行误差。如果条件允许尽量参加实车测试——哪怕只是坐在副驾观察问题也比只看仿真日志收获大得多。我在实车上看到的一次“轨迹突然向左猛拐”比读一百篇论文更能理解安全冗余的重要性。第三点是“保持对数学的敬畏”。规划算法表面上是代码问题本质上是个优化问题——不论是图搜索里的代价函数设计还是轨迹优化里的约束建模背后都是数学。线性代数、凸优化、概率论这三门课值得反复研读。第四点在前面提过现在值得强调一下不要拒绝端到端。“不要只做传统规划也不要只相信端到端”两条腿走路才能在行业震荡期站稳脚跟。这十年的经验告诉我这个行业最稳定的人往往是那些“什么都懂一点又有一项深耕到极致”的人。最后回到开头的那个比喻——规划算法这十年从解决“能不能开”的问题走到了解决“敢不敢坐”的阶段。技术的迭代不会停止但底下那些“在约束条件下找最优解”的核心逻辑会一直延续下去。
智能驾驶规划算法十年演进：从A*到端到端，传统优化何去何从？
1. 先看十年大趋势从“能开”到“敢坐”2015年我刚开始接触智能驾驶规划算法时业内谈得最多的是“这车能不能自己拐弯”。到了2025年大家讨论的已经是“这车能不能在晚高峰的窄路上自己找车位”。同样叫规划算法这十年的内涵变化用“天翻地覆”来形容一点不过分。规划算法在智能驾驶系统里的位置一句话概括就是感知告诉车“周围有什么”规划告诉车“接下来怎么走”。它往下衔接控制往上承接决策是整个系统的“大脑思维层”。2015年那会儿主流方案还在用A*、RRT这类源自机器人领域的经典搜索算法处理的是结构化道路上的简单场景到2025年端到端模型、Occupancy Network、VLA模型直接冲击传统管线规划算法的技术栈和思维模式都发生了根本性变化。这篇文章我想以这十年亲历者的视角把规划算法的演进脉络、核心方案背后的取舍逻辑、工程落地的坑点以及“端到端时代传统规划何去何从”这个热门话题一次性说透。无论你是刚入行的学生还是想转行做规控的工程师这篇文章都能给你一张相对完整的地图。提示文章里涉及的具体算法参数和实现细节我会结合多年来在实车上跑过的经验来讲不会只停留在纸面推导。2. 核心算法逐个数每个方案背后的取舍逻辑2.1 搜索派的开山鼻祖A与Hybrid A先聊A*。这个算法学机器人或AI的人都不陌生——栅格地图上从起点到终点维护一个open list和一个closed list每次取f值最小的节点扩展直到找到终点。它的本质是BFS的“带方向优化版”用启发式函数比如曼哈顿距离或欧氏距离指引搜索方向避免盲目扩展。在2015年左右的智能驾驶项目里A主要用在低速场景比如园区通勤车、封闭场地的摆渡车。原因很简单低速车辆可以近似看成质点航向角约束不那么致命栅格分辨率取0.5米甚至1米也够用。但放到开放道路的高速场景A就露怯了——它规划出来的路径是“折线”车辆在高速行驶时根本没法直接跟踪因为曲率不连续、方向突变的点会让方向盘在瞬间打满乘坐体验和安全性都无法接受。于是出现了Hybrid A*这可以看成是“带运动学约束的A*”。它不再把车当成质点而是把状态扩展成 (x, y, yaw)用转向角、轴距来模拟车辆的转弯半径。斯坦福在2007年DARPA Urban Challenge夺冠时用的就是这套思路2010年前后发表的论文直接引爆了这个方向。Hybrid A*的核心代价函数设计是门手艺活我当时调参时印象很深[ f(n) g(n) h(n) \alpha \cdot \text{转向惩罚} \beta \cdot \text{挡位切换惩罚} ]g(n)是已经走的路径长度h(n)是用Reeds-Shepp曲线算出来的无障碍最短路径长度转向惩罚让搜索倾向于“少打方向盘”的路径挡位切换惩罚则避免路径上频繁前进后退。这个公式看起来简单但三个权重系数的标定直接决定轨迹质量。我记得很清楚的例子转向惩罚系数调太大车辆会走“绕大圈”的路径在窄路里直接把自己堵死调太小路径上全是锯齿形的小幅转向控制端抖得不行。A*的代码实现思路其实很固定核心骨架如下def astar(grid, start, goal): open_list PriorityQueue() open_list.put((0, start)) came_from {} g_score {start: 0} while not open_list.empty(): _, current open_list.get() if current goal: return reconstruct_path(came_from, current) for neighbor in get_neighbors(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 tentative_g heuristic(neighbor, goal) open_list.put((f, neighbor)) return None而Hybrid A*扩展节点时不是往四邻域或八邻域扩而是按一组离散的转向角比如-30°、-15°、0°、15°、30°积分出一段段圆弧每段弧的长度由车辆速度和步长时间决定。这样扩展出来的节点天然满足车辆运动学约束这就是“Hybrid”的含义——混合了“栅格离散”和“连续运动学”。2.2 采样派代表RRT与它的变体们RRTRapidly-exploring Random Tree是另一个从机器人领域引入的流派。它的思路很反直觉在地图上随机撒点把树向随机点方向生长直到树连接到目标点。因为不依赖精确的栅格划分RRT在高维空间比如机械臂的7自由度关节空间表现很好理论完备性方面也有概率保证。但把RRT原封不动搬到智能驾驶上问题很多。最典型的是路径“过于随机”——它找得到路但找的往往不是一条人类愿意走的、平滑的路。想象一下一个新手司机在停车场里反复倒车、打轮最后也停进去了但过程让人晕车——RRT生成的轨迹有时就是这种感觉。另一个问题是随机采样在狭窄通道场景下效率极低树要“挤”过门缝一样的空隙可能要采样几万个点才碰巧成功。后来出现的RRT在渐进最优性上做了改进每次找到新节点后会检查它周围一定半径内的已有节点看是否存在代价更小的连接方式把这个节点“重新接线”rewire。这个操作让树的拓扑结构不断优化理论上能收敛到最优解。在智能驾驶领域RRT最成功的应用场景是泊车——尤其是有障碍物遮挡、通道狭窄的复杂车位搜索类方法容易陷入组合爆炸RRT*反而能用概率优势快速找出一条可行通道。我跟朋友开玩笑说A和RRT的选择有点像“严谨的公务员”和“天马行空的艺术家”——A老老实实地遍历所有可能路径保证找到最优RRT则靠随机采样碰运气速度快但结果不稳定。实际工程里很多泊车系统会先用RRT*粗搜一条可行通道再用参数化曲线把它平滑成可执行的轨迹两者配合效率更高。2.3 老牌劲旅参数化曲线与Lattice Planner搜索和采样解决的是“有没有路”的问题但工程上还需要解决“这条路好不好开”的问题。于是有了Lattice Planner——百度Apollo早期版本的核心规划器也是我职业生涯早期花时间最多的一个模块。Lattice Planner的思路可以这样理解车在Frenet坐标系纵向距离s横向偏差l下运动规划器在s方向和时间方向上采样一系列目标状态然后用五次多项式或三次多项式把这些目标状态连成候选轨迹再从中挑选一条代价最低的。Frenet坐标系的妙处在于道路的曲率被“拉直”了横向偏差和纵向速度可以分开优化这让轨迹的语义非常清晰——哪条轨迹更贴中心线、哪条轨迹加速更快一眼就能看出来。我记得当时采样参数是这样设置的纵向采样间隔每1.0秒一个点总共采样3~5秒的时间范围覆盖30~80米的前方距离横向偏移采样以当前车道中心为基准向左右各采样-3.5米到3.5米间隔0.5米末端速度采样从当前速度到当前速度10 m/s间隔2 m/s每次规划周期大概会生成几百条候选轨迹。代价函数包含跟车距离、偏离中心线的程度、加速度冲击度jerk、与障碍物的距离等多维项。选最小代价轨迹时还要做碰撞检查——候选轨迹先经过“碰撞”筛选剩下的才进入代价评比。Lattice Planner最大的优点是“生成轨迹本身就是平滑的”不需要额外的后处理最大的缺点是“横向采样粒度有限”在弯道多、曲率变化大的场景下0.5米的横向采样间隔可能不足以覆盖所有安全空间。比如一个急弯处安全走廊可能只有——我举个例子——道路边界内20厘米的余量这时候0.5米的采样步长就可能“漏掉”最优解。Apollo后来引入了EM PlannerExpectation Maximization Planner本质上还是在Frenet坐标系下做DPQP两段式优化。DP动态规划粗搜出一条参考线把问题空间缩小QP二次规划在这个凸空间内求解最优轨迹。EM的思路可以简单概括为“先找到大概对的路再精确优化”这种递进式策略大幅提升了复杂场景的求解成功率。2.4 路径规划算法代码一个可运行的样例说了这么多理论给一个可以直接跑起来感受一下的简化例子。我用一个简化的Frenet坐标系Lattice采样逻辑来展示核心思路配置好环境就能跑通import numpy as np def generate_candidate_trajectory(s0, s_dot0, s_ddot0, target_s, target_s_dot, T): # 五次多项式s(t) a0 a1*t a2*t^2 a3*t^3 a4*t^4 a5*t^5 a0 s0 a1 s_dot0 a2 s_ddot0 / 2.0 A np.array([ [T**3, T**4, T**5], [3*T**2, 4*T**3, 5*T**4], [6*T, 12*T**2, 20*T**3] ]) b np.array([ target_s - (a0 a1*T a2*T**2), target_s_dot - (a1 2*a2*T), 0 # 末端加速度设为0让轨迹更平滑 ]) x np.linalg.solve(A, b) a3, a4, a5 x return [a0, a1, a2, a3, a4, a5] # 示例参数 s0, s_dot0, s_ddot0 0.0, 10.0, 0.0 target_s, target_s_dot, T 30.0, 12.0, 2.0 coeffs generate_candidate_trajectory(s0, s_dot0, s_ddot0, target_s, target_s_dot, T) print(五次多项式系数:, coeffs) # 在t1.0秒时计算纵向位置 t 1.0 s_t 0 for i, c in enumerate(coeffs): s_t c * (t ** i) print(ft{t}s时纵向位置: {s_t:.2f}m)运行这段代码你能直观看到五次多项式如何保证“起点状态”和“终点状态”都被约束住中间过程平滑过渡。真实工程里还会对横向偏移做同样的多项式拟合然后把纵向和横向合成到笛卡尔坐标系下得到最终的XY路径点序列。3. 算法落地的工程真相从仿真到真车的gap3.1 第一坑坐标系与坐标变换规划算法在纸面上跑得再好上车之后第一个暴击往往来自坐标系。感知模块输出的障碍物一般在自车坐标系原点在后轴中心x向前y向左地图模块给出的是绝对坐标系下的道路信息比如UTM坐标而规划算法内部运算又经常用Frenet坐标系。这三者之间的转换一旦出问题轻则轨迹抖动重则撞上真实障碍物。我最深的一次教训是处理“时延”问题。感知输出的障碍物位置是“过去某一时刻”的位置而规划算法计算时用的是“当前时刻”的位置。在高速场景下比如100 km/h100毫秒的延迟意味着车辆往前跑了2.8米。如果不做运动补偿——也就是用障碍物的历史速度和加速度外推它的当前位置——规划出的轨迹可能已经穿过“实际上的障碍物”了。这个细节很多仿真环境里根本不会暴露因为仿真里的感知是“理想同步”的但实车一定会暴露。坐标变换的代码核心其实很简单但方向反了就会出现完全相反的结果import math def global_to_vehicle(global_point, vehicle_pose): # vehicle_pose: (x, y, yaw) dx global_point[0] - vehicle_pose[0] dy global_point[1] - vehicle_pose[1] cos_yaw math.cos(vehicle_pose[2]) sin_yaw math.sin(vehicle_pose[2]) local_x dx * cos_yaw dy * sin_yaw local_y -dx * sin_yaw dy * cos_yaw return (local_x, local_y)很多新手不注意“坐标轴方向”和“航向角参考系”写完转换之后直接在仿真里通过一上车就发现轨迹偏移。建议的做法是写单元测试输入一组已知坐标手动推导期望输出再加一个反向变换做往返一致性校验这样能拦截掉绝大多数基础错误。3.2 第二坑轨迹平滑与曲率连续性规划算法输出的轨迹控制模块是要直接去跟踪的。如果轨迹上的曲率不连续控制模块的反馈会非常痛苦——方向盘会一直抖车辆会画龙。我曾经接过一个项目规划算法用的是纯几何样条曲线拼接看起来挺优美但曲率在拼接点处出现了突变上车之后方向盘在拼接点处“咔哒”一下猛打把测试工程师吓出一身冷汗。解决曲率连续业界最常用的做法是“曲率平滑后处理”。具体可以分为两步第一步用三次样条或五次样条对原始路径点做插值保证位置、一阶导数航向、二阶导数曲率连续第二步用非线性优化对轨迹进行“推优化”目标函数约束最大曲率、最大曲率变化率我习惯用的一个简化版曲率约束优化核心思想是把相邻三个路径点确定的圆弧曲率作为状态量限制其变化率[ \text{minimize} \quad \sum_{i1}^{n-1} \left( \kappa_{i1} - \kappa_i \right)^2 \lambda \sum_{i1}^{n} \kappa_i^2 ]第一项让曲率变化尽量平缓第二项防止曲率本身过大(\lambda)是正则化系数控制两项的权重。这个优化问题规模不大用OSQP或自定义的高斯牛顿都能在几毫秒内解完。需要注意的是平滑后的轨迹必须要重新做碰撞检测因为平滑过程可能让轨迹“侵占”到障碍物一侧。注意轨迹平滑和碰撞检测是一个闭环迭代过程。平滑让轨迹更舒适但可能破坏安全性碰撞检测发现问题后要重新在局部生成替代轨迹。实际工程里这两个模块往往要循环执行2~3轮才能得到既安全又平滑的结果。3.3 第三坑规划与控制的时间同步规划模块的输出并不只是“一条路径”而是一条带时间戳的轨迹——每个路径点都对应一个期望到达时刻。控制模块跟踪这条轨迹时如果规划周期比如100ms和控制周期比如10ms之间不同步控制模块就会一直在跟踪“过期”的轨迹点。我记得有一次实车测试车辆在过弯时出现明显的“一卡一卡”现象。排查了半天问题出在规划模块输出的轨迹点时间戳用的是“规划计算完成时刻”而控制模块读取时用的是“读取时刻”两者之间差了将近一个规划周期的时间偏移。结果控制模块总是跟踪一个落后于实际的点导致每周期都产生约0.3米的跟踪误差在弯道中被放大成明显的顿挫感。解决方式也很直接规划模块每次输出轨迹时不是从“当前时刻”开始而是从一个“未来时刻”通常是下一次控制周期开始的时间开始。比如规划周期100ms、控制周期10ms规划模块可以预瞄到“当前时刻110ms”的状态作为轨迹起点保证控制模块跟踪时看到的是“未来”的轨迹而不是“过去”的轨迹。这个偏移量的标定需要在实车上反复试不同的传感器延迟特性、不同的控制模块响应速度都会影响最优值。4. 泊车与无人机两个场景的规划算法变体4.1 泊车为什么是“低速但高难”的典型泊车路径规划在热词里出现频率很高这确实是个独特的场景。说它低速是因为泊车过程中车速通常不超过5 km/h说它高难是因为空间极其狭小、需要反复前进后退操作对规划的“几何可行性和多段操作”能力要求很高。解决泊车问题主流方案有两种路线。第一种是“搜索平滑”用Hybrid A*搜索一条满足车辆运动学约束的粗糙路径再用优化方法平滑。2010年前后Stanford团队的论文已经证明这条路可行但问题是窄小车位里搜出来的路径经常带有大量的“挡位切换”每条小弧段长度可能只有几十厘米平滑之后依然可能碰壁。第二种是“数值优化”路线把泊车问题建模成非凸优化[ \min_{x} \sum_{k0}^{N} | \mathbf{x}k - \mathbf{x}{ref,k} |^2_Q | \mathbf{u}_k |^2_R ]约束条件包括运动学模型、转向角限制比如±35°、障碍物距离限制车辆轮廓到障碍物的最小距离大于安全阈值、路径曲率限制等。这个优化问题是非凸的不能直接求全局最优一般用SQP序列二次规划或罚函数法迭代求解。工程实战中通常会先用Hybrid A*生成一条初始可行解作为优化起点再用优化方法把路径“磨”得更加平滑、贴边。这叫“热启动”——好的初始解能大幅提升优化收敛速度和结果质量。泊车场景的另一个特点是“终点姿态约束严格”——最终车辆必须精确停入车位正中且航向角要与车位方向一致。这个约束在优化问题里必须作为硬约束处理否则车辆可能停在歪斜的位置虽然也能入库但用户体验极差。4.2 无人机路径规划同一个家族的不同分支无人机路径规划算法和智能驾驶规划算法经常并列出现在讨论里很多人以为两者是同一套技术。“它们同宗但不同路”我来具体说说。两者的底层技术栈确实有交集——A*、RRT、优化算法在无人机上也很常见但约束条件差异巨大维度差异无人机是三维空间运动智能驾驶基本是二维平面运动忽略坡道时。三维空间的状态空间大得多搜索算法的“组合爆炸”问题更严重运动学差异无人机尤其是多旋翼可以悬停、垂直起降、任意方向飞行没有非完整约束——它不需要像汽车那样考虑转弯半径和倒车但无人机有动力学约束加速度、速度、姿态角速率限制轨迹规划必须考虑“能不能飞出来”安全空间差异智能驾驶的安全距离是厘米级的无人机在城市峡谷等场景的安全距离通常是米级的无人机领域最常用的规划算法是“混合A*的采样 多项式轨迹优化”——先粗搜一条平滑轨迹再用Minimum Snap最小加加速度优化得到动力学可行的轨迹。Minimum Snap的思想是让四旋翼在航点之间飞行时四个电机推力的变化率尽量小这样飞得稳、不抖。它的本质是一个二次规划问题约束条件是航点的位置、速度、加速度目标函数是最小化加加速度的平方积分[ \int_0^T \left( \frac{d^3 \mathbf{r}}{dt^3} \right)^2 dt ]这个目标函数在数学上的好处是它是凸的求解快而且得到的轨迹非常平滑。但缺点是需要预先知道航点的位置和时间分配——如果时间分配不合理轨迹会很“别扭”比如某段飞行时间过长导致速度过低另一段又太短导致加速度过大。工程上通常会在轨迹求解后检查速度和加速度是否超限如果超限就增大对应航段的时间重新求解。这个过程叫“时间重分配”是无人机轨迹规划里一个重要但常被忽略的环节。5. 端到端的冲击传统规划算法会被淘汰吗5.1 端到端到底改变了什么2023年以来端到端End-to-End成为智能驾驶领域最热的话题。端到端模型把“感知-预测-规划”压缩成一个神经网络输入多摄像头图像有时加激光雷达点云直接输出控制指令方向盘转角、油门、刹车或轨迹点。这个思路的本质是“用数据驱动替代规则驱动”——传统的规划算法本质上是一堆人为设定的规则和公式端到端模型则是从海量驾驶数据中“学习”出规划策略。端到端对大算力的需求非常夸张。一个典型的端到端模型比如基于Transformer架构的驾驶模型参数量通常在数亿到数十亿级别训练需要上千张A100/H100 GPU。训练数据则是数十万小时的真实驾驶视频和对应的控制指令。这种规模让很多中小团队望而却步也让“数据闭环”成为行业竞争的真正壁垒。但端到端不是银弹。它最大的问题是“可解释性差”——模型在某种场景下做了一个奇怪的决策工程师很难说清楚是哪里出了问题。这带来了两个工程层面的困难一是安全验证变得困难传统规划算法可以用形式化方法穷举验证端到端模型则只能依赖大量测试二是“长尾问题”处理困难——模型在训练数据稀疏的场景比如极端天气、非常规障碍物下行为不可预测。5.2 传统规划工程师的生存之道我身边不少做传统规划的同行在2024年那波端到端热潮中感到焦虑担心自己积累多年的A*、Lattice、QP优化经验一夜之间变成废纸。但其实“传统规划技术不会消失只会换一种形态存在”。原因很简单端到端模型输出的是轨迹或控制指令但系统的其他模块仍然需要大量“传统”技术。首先模型本身需要监督信号和数据标注——标注数据时通常需要参考传统规划器的输出。其次端到端模型输出的轨迹往往不够平滑需要后处理模块用样条插值、曲率约束优化进行修正——这些正是传统规划的核心技术。最后安全兜底机制不可或缺——当端到端模型输出异常时系统需要一个“安全护栏”比如RTKReal-Time Kinematic边界限制、紧急制动策略这些也都依赖传统规则算法。从更宏观的视角看端到端解决的是“感知信息如何变成驾驶行为”的映射问题而规划算法解决的是“在约束条件下如何生成最优轨迹”的优化问题。后者本质上是一个数学问题跟感知用什么模型没有直接关系。哪怕感知端完全变成端到端规划端的优化框架依然成立。我给同行们的建议是不要丢掉传统规划的基本功但要主动学习端到端的思维模式。一个既能写QP优化又能理解Transformer注意力机制的工程师在未来两三年内会非常抢手。6. 给想入行的人的一点建议很多读者问我2025年了想入行智能驾驶规划算法应该从哪里入手。我的建议是先动手把经典算法的代码写一遍不要停留在“看懂了”的层面。“看懂了”和“能上手调参”之间隔着无数个细节。A的open list用什么数据结构Hybrid A的Reeds-Shepp曲线怎么生成Frenet坐标和笛卡尔坐标的转换公式怎么推导这些不看代码自己实现一遍永远不会真正理解。第二个建议是“把仿真当玩具把实车当真考验”。再好的仿真环境也模拟不了真实的传感器噪声、通信延迟、机械执行误差。如果条件允许尽量参加实车测试——哪怕只是坐在副驾观察问题也比只看仿真日志收获大得多。我在实车上看到的一次“轨迹突然向左猛拐”比读一百篇论文更能理解安全冗余的重要性。第三点是“保持对数学的敬畏”。规划算法表面上是代码问题本质上是个优化问题——不论是图搜索里的代价函数设计还是轨迹优化里的约束建模背后都是数学。线性代数、凸优化、概率论这三门课值得反复研读。第四点在前面提过现在值得强调一下不要拒绝端到端。“不要只做传统规划也不要只相信端到端”两条腿走路才能在行业震荡期站稳脚跟。这十年的经验告诉我这个行业最稳定的人往往是那些“什么都懂一点又有一项深耕到极致”的人。最后回到开头的那个比喻——规划算法这十年从解决“能不能开”的问题走到了解决“敢不敢坐”的阶段。技术的迭代不会停止但底下那些“在约束条件下找最优解”的核心逻辑会一直延续下去。
