1. 项目概述从一道赛题看自动驾驶的“基本功”2021年MathorCup A题题目叫“自动驾驶中的车辆调头轨迹规划”。乍一看这好像就是个数学题给几个方程算条路径。但真正干过自动驾驶算法研发的同行都知道这道题戳中的恰恰是行业里最基础、也最考验功力的环节如何在复杂约束下让车安全、舒适、高效地完成一个看似简单的动作。调头尤其是无中间隔离带的路口调头是城市自动驾驶场景中的经典难题它融合了路径规划、运动控制、车辆动力学和交通规则理解是一个典型的“麻雀虽小五脏俱全”的课题。这道题的价值在于它剥离了感知、定位等上游模块的复杂性直接聚焦于“规划与控制”这个核心决策层。给定车辆参数、道路边界和障碍物位置让你设计一条从起点到终点的连续轨迹。这听起来像是机器人学里的经典问题但加上“自动驾驶车辆”这个前缀约束条件就变得极其严苛。轨迹不仅要无碰撞、可执行还必须满足车辆的运动学约束比如前轮转角不能打满、动力学约束比如加速度不能太大导致乘客不适以及最重要的——要像“老司机”开出来的不能画蛇添足也不能过于保守。当时很多参赛队伍可能只把它当作一个优化问题来解用个遗传算法、粒子群优化出几条路径就交差了。但如果你真的在产业界做过就会明白这道题里埋着无数个坑。比如如何定义“舒适度”是加个jerk加加速度的惩罚项那么简单吗再比如障碍物是静态的但实际路况中对向车道来车的预测和交互怎么办题目给的简化模型正是我们进行算法原型验证和性能基准测试的常用方法。通过解这道题你实际上是在亲手搭建一个自动驾驶规划模块的简化版“仿真测试平台”。接下来我就结合这几年在轨迹规划上的实战经验把这套“基本功”的里里外外拆解清楚不仅告诉你题目怎么解更告诉你工业界是怎么想、怎么做的。2. 核心需求与问题拆解把“调头”翻译成数学语言面对“车辆调头轨迹规划”这个问题第一步不是急着找算法而是要把模糊的自然语言描述转化成精确的、可计算的数学问题。这个过程我们称之为“问题建模”。题目通常会给出车辆初始位姿位置和航向角、目标位姿、道路边界可能是几条直线或曲线、以及几个静态障碍物的位置。我们的核心需求可以分解为以下几点2.1 安全性需求绝对的红线这是所有需求的基石一票否决。在数学上它转化为两条硬约束空间避障约束规划出的轨迹上的每一个点车辆的外接矩形或更精确的多边形不能与道路边界多边形、障碍物多边形发生任何重叠。这需要计算几何的知识比如判断多边形是否相交。运动可行性约束轨迹必须符合车辆的运动学模型。你不能规划出一条需要车辆原地旋转或侧向平移的路径。对于普通的阿克曼转向车辆其运动学约束可以用一个简化的“自行车模型”来描述核心在于路径的曲率必须连续且不能超过车辆的最小转弯半径所对应的最大曲率。2.2 舒适性与拟人化需求像“人”一样开车安全是底线但坐起来像“摇船”或者“画龙”的轨迹乘客会骂娘也容易被其他交通参与者误判。这转化为一系列优化目标软约束轨迹平滑度轨迹的一阶导速度、二阶导加速度、三阶导加加速度Jerk应尽可能小。剧烈的加速度变化高Jerk是导致晕车的主要原因。在优化问题中我们通常最小化这些导数的平方积分。行驶效率在满足上述条件的前提下总行程时间应尽可能短或者轨迹总长度尽可能短。但这不能以牺牲平滑度为代价需要权衡。行为可预测性轨迹应尽可能符合人类驾驶员的习惯。例如在调头时通常会尽量利用道路宽度走一个近似“U”形或“灯泡”形的路径而不是贴着内侧拐一个急弯。这有时可以通过给轨迹的曲率分布增加一些启发式规则来实现。2.3 题目特有的建模挑战MathorCup这类赛题往往会在基础模型上增加一些“花样”来区分参赛队伍的水平。例如车辆模型选择是用简单的质点模型还是考虑车长车宽的矩形模型后者对于窄路调头至关重要因为车头和车尾的扫掠区域不同。障碍物建模障碍物是点、圆形、还是多边形不同的模型避障约束的数学表达式复杂度差异巨大。道路约束道路边界是严格的“墙”还是允许轮胎压线但车身不越界这对应不同的约束函数。注意在实际工业界我们还会考虑“不确定性”。例如感知给出的障碍物位置有误差车辆控制存在跟踪误差。因此成熟的规划器会引入“安全边际”或“缓冲带”规划出的轨迹会与障碍物保持一个额外的安全距离。这在赛题中通常被简化但却是实际落地中避免事故的关键思想。3. 技术方案选型与思路解析条条大路通罗马但哪条路最近明确了数学问题接下来就是选择求解工具。轨迹规划领域方法众多主要可以分为三大类基于几何的方法、基于搜索的方法和基于优化的方法。这道题最适合用后两者或者将其结合。3.1 基于优化的方法主流工业界的选择这是目前自动驾驶公司最青睐的方案因为它能最自然地表达舒适性、平滑度等优化目标。其核心思想是将车辆轨迹参数化例如用一系列离散的位姿点表示然后构造一个目标函数如最小化加速度和Jerk和一系列约束函数如避障、运动学限制最后调用数值优化求解器如IPOPT、SNOPT来求解这个通常是非线性优化问题。常用框架学术界和工业界常用的有Apollo的EM Planner虽然其内部复杂但思想可借鉴、Google的车辆轨迹优化方法以及基于样条曲线如B样条的优化方法。应用于本题的思路轨迹参数化我们可以用一条参数化曲线来表示轨迹比如五次多项式曲线。为什么是五次因为要同时约束起终点的位置、航向角、曲率对应速度、加速度五次多项式是满足边界条件的最低阶数阶数越高越灵活但也越容易振荡。构造优化问题目标函数Minimize: ∫( jerk(t)^2 ) dt即最小化加加速度的平方和这是舒适度的核心指标。约束条件等式约束轨迹的起点和终点必须匹配题目给定的位姿位置、航向角有时还包括曲率。不等式约束路径点曲率 车辆最大曲率1/最小转弯半径。车辆轮廓多边形在所有路径点处与障碍物/边界的距离 0。速度、加速度在合理范围内如果题目考虑了动力学。求解将连续问题离散化在轨迹上采样N个点将约束在这些点上进行校验然后使用非线性规划求解器求解。这个过程对初值敏感如果初始猜测的轨迹离可行解太远求解器可能失败。3.2 基于搜索的方法简单可靠的备选当环境非常复杂、障碍物众多时基于优化的方法可能因初值不好而陷入局部最优或无法求解。此时基于搜索的方法如A*算法、Hybrid A*可以作为生成一条粗略、可行但可能不最优的“种子路径”的工具为优化方法提供良好的初值。应用于本题的思路将车辆的状态空间x, y, θ进行离散化。定义可行的动作如以某个固定前轮转角行驶一小段距离。使用A*算法从起点搜索到终点代价函数可以结合距离代价和朝向代价。搜索出的路径是一系列离散的状态点通常比较“折线化”不够平滑。关键步骤路径后处理。得到A*的粗糙路径后必须进行平滑。常用方法包括梯度下降平滑或用样条曲线拟合。平滑后的路径可以作为优化方法的初始猜测进行进一步的精修。3.3 混合方案A 优化强强联合* 这是最实用、最稳健的思路也是很多实际系统的简化版流程。第一阶段全局路径搜索。使用Hybrid A*考虑了车辆朝向的A*变种在状态空间中进行搜索得到一条保证无碰撞、满足运动学约束的可行路径。这条路径可能不光滑但它是安全的“骨架”。第二阶段局部轨迹优化。以上述搜索路径为参考在其周围构造一个“走廊”或“通道”。然后在这个通道内用优化方法如用B样条参数化生成一条平滑、舒适、且严格在通道内从而保证安全的轨迹。这种方法结合了搜索的全局可行性和优化的局部最优性。方法优点缺点适用场景纯优化方法轨迹质量高平滑舒适数学形式优雅便于加各种约束。对初值敏感可能陷入局部最优复杂环境求解可能失败或耗时。环境相对简单或有较好初始猜测时。纯搜索方法完备性强只要存在解就一定能找到不依赖初值。轨迹粗糙需要后处理离散化可能导致“维度灾难”。复杂狭窄环境用于寻找可行解。混合方法兼具两者优点鲁棒性强工业界主流。实现复杂度较高需要衔接两个模块。绝大多数实际自动驾驶场景。对于MathorCup这道题如果追求论文的完整性和展示的算法深度混合方法是最佳选择。如果追求快速实现和求解的稳定性基于样条优化的方法也是一个非常出色的选择。接下来我将以混合方法为主线详细拆解实操步骤。4. 实操过程从理论到代码的完整实现这里我以一个简化的仿真环境为例演示如何用Python实现一个A* 优化的调头轨迹规划器。我们假设道路是双向四车道中间无隔离带车辆需要完成180度调头。4.1 环境与车辆建模首先我们需要用代码定义世界。import numpy as np import matplotlib.pyplot as plt from scipy.interpolate import splprep, splev import cvxopt # 用于二次规划可选 # 1. 定义道路和障碍物 road_width 3.5 # 单车道宽度米 num_lanes 2 total_width road_width * num_lanes * 2 # 双向道路总宽 # 道路边界简单矩形 road_boundary { left: -total_width / 2, right: total_width / 2, bottom: -20, # 纵向范围 top: 20 } # 静态障碍物 (x, y, radius) obstacles [ (5, 0, 1.5), # 对向车道上的一个障碍 (-3, -5, 1.0) ] # 2. 定义车辆参数 class VehicleModel: def __init__(self): self.wheelbase 2.8 # 轴距米 self.width 1.8 self.length 4.5 self.max_steer np.deg2rad(30) # 最大前轮转角 self.min_turning_radius self.wheelbase / np.tan(self.max_steer) # 最小转弯半径 def check_collision(self, x, y, yaw): 给定车辆中心位姿检查其矩形轮廓是否与障碍物碰撞 # 计算车辆四个角点的坐标简化模型 # ... 具体实现涉及坐标变换和几何碰撞检测 pass4.2 Hybrid A路径搜索* Hybrid A* 是A*在连续状态空间x, y, θ的扩展。我们需要定义状态节点、动作空间和启发式函数。class Node: def __init__(self, x, y, yaw, parentNone, steer0.0, cost0.0): self.x x self.y y self.yaw yaw # 航向角弧度 self.parent parent self.steer steer self.cost cost # 从起点到当前节点的实际代价 def hybrid_a_star(start, goal, vehicle, obstacles, grid_resolution(0.5, 0.5, np.deg2rad(5))): Hybrid A* 搜索主函数 start/goal: (x, y, yaw) grid_resolution: (xy_res, xy_res, yaw_res) 状态离散化分辨率 open_set {} closed_set {} start_node Node(*start) goal_node Node(*goal) # 使用离散化的键来管理节点避免重复访问相似状态 def get_index(node): x_idx round(node.x / grid_resolution[0]) y_idx round(node.y / grid_resolution[1]) yaw_idx round(node.yaw / grid_resolution[2]) return (x_idx, y_idx, yaw_idx) open_set[get_index(start_node)] start_node while open_set: # 选择启发代价最小的节点 current_key min(open_set, keylambda k: open_set[k].cost heuristic(open_set[k], goal_node)) current_node open_set.pop(current_key) # 判断是否到达目标容忍度内 if calc_distance(current_node, goal_node) 1.0 and abs(current_node.yaw - goal_node.yaw) np.deg2rad(10): # 回溯路径 return backtrack_path(current_node) closed_set[get_index(current_node)] current_node # 扩展子节点模拟车辆以几种不同前轮转角行驶一段固定距离 for steer in np.linspace(-vehicle.max_steer, vehicle.max_steer, 5): # 5个离散转向动作 # 基于自行车模型进行运动模拟得到子节点状态 child_state simulate_kinematic_model(current_node, steer, step_length1.0, vehicle.wheelbase) child_node Node(*child_state, parentcurrent_node, steersteer, costcurrent_node.cost step_length) child_key get_index(child_node) # 检查碰撞和边界 if not is_collision_free(child_node, vehicle, obstacles, road_boundary): continue if child_key in closed_set: continue # 计算启发代价这里可以用Reeds-Shepp曲线的最短距离作为启发函数更高效 child_node.cost heuristic(child_node, goal_node) if child_key not in open_set or child_node.cost open_set[child_key].cost: open_set[child_key] child_node return None # 搜索失败 def heuristic(node, goal): 启发函数欧几里得距离 航向角差惩罚 dx goal.x - node.x dy goal.y - node.y dist np.hypot(dx, dy) angle_diff abs(goal.yaw - node.yaw) angle_diff min(angle_diff, 2*np.pi - angle_diff) # 处理角度环绕 return dist 0.5 * angle_diff # 权重可调实操心得Hybrid A* 的性能和效果极度依赖于启发式函数的设计。单纯的欧氏距离会导致在狭窄空间扩展大量无用节点。使用Reeds-Shepp曲线或Dubins曲线计算从当前状态到目标状态的理论最短路径长度作为启发值可以极大提升搜索效率这是高级实现的关键。此外离散化的分辨率需要权衡太粗可能找不到解太细则计算爆炸。4.3 路径平滑与优化假设我们通过Hybrid A*得到了一系列离散的路径点path_points [(x1,y1), (x2,y2), ...]。这条路径是锯齿状的需要平滑。def smooth_path(path_points, weight_data0.1, weight_smooth0.5, tolerance0.00001): 使用梯度下降法进行路径平滑。 path_points: 原始路径点列表 weight_data: 保持接近原始点的权重 weight_smooth: 平滑项的权重 new_points np.copy(path_points).astype(float) change tolerance while change tolerance: change 0.0 for i in range(1, len(new_points)-1): for dim in range(2): # x和y维度 original path_points[i][dim] current new_points[i][dim] # 平滑项倾向于与相邻点平均值一致 smoothed (new_points[i-1][dim] new_points[i1][dim]) / 2.0 # 梯度下降更新 new_points[i][dim] weight_data * (original - current) weight_smooth * (smoothed - current) change abs(current - new_points[i][dim]) return new_points # 或者使用样条插值获得参数化曲线 def fit_spline_path(path_points): 使用B样条拟合路径得到平滑的参数化曲线 path_array np.array(path_points).T # 转置为(2, N) # 寻找样条表示 (smoothing spline) tck, u splprep(path_array, s2.0) # s是平滑因子越大越平滑 # 在更多点上评估样条得到平滑路径 u_new np.linspace(0, 1, 200) smooth_array splev(u_new, tck) smooth_path np.stack(smooth_array, axis1) return smooth_path, tck # 返回路径点和样条参数平滑后的路径已经可以用于控制模块的跟踪了。但如果想进一步优化舒适度如最小化Jerk我们需要进行轨迹优化。这里展示一个更高级的思路将平滑后的路径转化为一个时空轨迹。4.4 轨迹优化速度规划路径是空间的轨迹是带时间信息的。我们需要给路径上的每个点分配一个时间和速度。def optimize_velocity_profile(path, max_accel2.0, max_decel-3.0, max_speed5.0, dt0.1): 沿给定路径优化速度剖面使其满足加速度和速度约束。 使用简单的前向积分和后向积分类似Apollo中的ST图规划思想。 path: 平滑后的路径点数组 (N, 2) N len(path) # 计算路径上每段的弧长 distances np.zeros(N) for i in range(1, N): distances[i] distances[i-1] np.linalg.norm(path[i] - path[i-1]) # 前向积分从起点开始以最大加速度加速 v_forward np.zeros(N) for i in range(1, N): ds distances[i] - distances[i-1] # 根据当前速度、最大加速度和剩余距离计算能达到的最大速度 v_desired np.sqrt(v_forward[i-1]**2 2 * max_accel * ds) v_forward[i] min(v_desired, max_speed) # 后向积分从终点开始以最大减速度减速 v_backward np.zeros(N) v_backward[-1] 0.0 # 终点速度设为0 for i in range(N-2, -1, -1): ds distances[i1] - distances[i] v_desired np.sqrt(v_backward[i1]**2 2 * (-max_decel) * ds) v_backward[i] v_desired # 注意这里不减因为是从后往前算的“所需初速度” # 取两者最小值得到满足双向约束的速度上限 v_profile np.minimum(v_forward, v_backward) # 生成时间戳 time np.zeros(N) for i in range(1, N): ds distances[i] - distances[i-1] avg_v (v_profile[i-1] v_profile[i]) / 2.0 if avg_v 1e-3: time[i] time[i-1] ds / avg_v else: time[i] time[i-1] dt # 避免除零 # 现在我们有了一组 (x, y, time) 的轨迹点 trajectory np.column_stack((path, time, v_profile)) # (N, 4): x, y, t, v return trajectory至此我们得到了一个时空轨迹trajectory它包含了位置、时间和速度信息。这个轨迹已经满足了基本的运动学、动力学加速度约束并且是平滑的。5. 常见问题与调试技巧实录在实际编码和调试这个规划器的过程中你会遇到各种各样的问题。下面是我踩过的一些坑和解决方法这些在论文或教科书里很少会提。5.1 Hybrid A搜索效率低下或找不到路径*症状程序运行很久不出结果或者在不该失败的地方返回“无路径”。排查与解决检查启发函数这是首要怀疑对象。如果启发函数低估了真实代价即不满足“可采纳性”A*虽然能保证找到最优解但搜索范围会变大。如果高估了则可能找不到最优解甚至找不到解。对于车辆Reeds-Shepp启发式是黄金标准。可以先用欧氏距离乘以一个系数如0.5作为简单启发值测试。调整离散化分辨率状态网格(x, y, θ)的分辨率太细会导致节点数爆炸太粗则会丢失可行解。通常(0.5m, 0.5m, 5度)是一个不错的起点。可以尝试动态分辨率在开阔地粗在狭窄处细。检查碰撞检测函数这是最常见的Bug来源。确保你的车辆轮廓多边形计算正确并且与障碍物的碰撞检测逻辑严密。建议可视化在搜索过程中把扩展的节点和碰撞点都画出来一眼就能看出问题。动作空间设计np.linspace(-max_steer, max_steer, 5)只用了5个转向动作可能在非常狭窄的空间不够用。可以增加到7或9个。同时步长step_length也很关键通常取车辆长度的一半左右如2米。5.2 优化后的轨迹不平滑或抖动症状路径看起来光滑但车辆跟踪时方向盘或加速度指令抖动。排查与解决检查样条平滑或梯度下降的参数平滑因子s在splprep中或weight_smooth太小会导致拟合不足太大则会使路径偏离原始安全路径太多可能撞上障碍物。需要反复调试可视化是关键。把原始路径、平滑路径、障碍物画在同一张图上观察。曲率连续性问题即使路径点平滑其曲率也可能不连续。曲率κ dθ/ds方向角变化除以弧长。计算并绘制整条路径的曲率曲线应该是一条连续变化、没有突跳的线。如果有突跳说明路径的二阶几何连续性G2不满足车辆需要瞬间改变前轮转角这是不可能的。此时需要考虑使用曲率约束的优化器或者使用Clothoid回旋曲线等曲率线性变化的曲线进行连接。速度规划的影响即使路径完美糟糕的速度规划也会导致纵向加速度抖动。检查optimize_velocity_profile函数生成的速度曲线v_profile和加速度曲线a dv/dt。加速度曲线应该是连续、无阶跃的。如果出现锯齿可能是因为路径弧长计算不准确或者速度更新逻辑有误。5.3 轨迹在障碍物附近“擦边”或轻微碰撞症状规划出的轨迹理论上没碰撞但仿真或实际控制中由于车辆轮廓或控制误差发生了碰撞。排查与解决引入安全边际在碰撞检测中不要用车辆的实际轮廓而是用一个“膨胀”后的轮廓。例如将障碍物的半径或车辆的宽度增加一个安全余量如0.2-0.5米。这是工程实践中的必须步骤。考虑控制误差规划器生成的轨迹控制器不可能完美跟踪。因此规划时需要考虑一个“跟踪误差包络”。一种简单方法是在优化目标中加入一项使轨迹尽量远离障碍物而不仅仅是满足无碰撞。使用更精确的车辆模型在优化中如果你只约束了路径点那么点与点之间的车辆姿态可能发生碰撞。确保你的优化问题约束了整个连续轨迹上的所有点都无碰撞这通常通过采样多个点并施加约束来实现计算量会增大。5.4 调头轨迹不自然不像人类驾驶症状轨迹能完成调头但拐弯很急或者走线很奇怪。排查与解决在目标函数中加入“舒适度”代价这是根本方法。除了最小化Jerk还可以最小化曲率变化率。目标函数可以是∫(κ(s))² ds其中κ是曲率s是弧长。这会使车辆转向更平顺。参考人类驾驶数据如果你有人类驾驶员调头的轨迹数据可以计算其平均曲率剖面。然后在你的优化问题中增加一个项使规划轨迹的曲率分布尽量接近这个参考分布。调整Hybrid A*的代价函数在搜索阶段除了距离给转向变化steer的改变量也增加一个代价。这样搜索会更倾向于选择转向更平缓的路径。调试这类算法可视化工具是你的最佳伙伴。不要只盯着最终结果图要把中间变量都画出来搜索树、碰撞点、曲率图、速度加速度曲线。一边单步调试一边看图很多问题会迎刃而解。最后这道赛题的价值不仅在于解出答案更在于让你系统性地思考自动驾驶规划中的一个完整链条问题定义、模型选择、算法实现、调试优化。这套方法论适用于任何轨迹规划问题。