简介面向自动驾驶研究人员、高校相关专业师生及智能交通系统科研工作者此份复现资料围绕基于时空风险场的道路自动驾驶车辆预测轨迹规划方法展开。文档采用轨迹生成与风险评估解耦的双模块结构通过类人工势场量化空间风险结合交互多模型(IMM)预测周围车辆行为概率并利用精心设计的成本函数从候选轨迹中选出最优路径。资源为单个docx文档压缩包约60KB内容覆盖算法概述、完整Python代码、规划空间管理、预测模块、参考线构建与曲率计算、RRT*采样与优化基轨迹平滑等还总结了系统性能优化建议有助于读者理解原理并快速实现。目前已有143人学习下载适合具备一定路径规划与概率基础、希望深入掌握预测轨迹规划实际代码细节的技术人员。1. 基于时空风险场的自动驾驶轨迹规划到底在解决什么问题在一段双车道快速路上自车以 18 m/s 巡航前车突然从 15 m/s 减速到 8 m/s左侧车道后方有一辆正在加速的货车。传统做法是先预测前车轨迹再把预测结果当成“未来必定如此”的一条线交给规划器。问题在于预测线的误差范围没有显式建模轨迹贴着预测线走一旦前车实际减速比预测更狠换道窗口就没了。时空风险场把每个预测目标按概率密度扩散成一片随未来时间变化的风险场规划器不去盯一条轨迹而是选一条累计风险最低的轨迹。它解决的是预测与规划之间“误差怎么传递”这个连接问题适合正在复现论文但卡在公式到代码映射的读者也适合想从规则式或采样式规划转向风险场优化框架的工程师。2. 先建场静态风险场与动态风险场的数学模型风险场看似是四维数据工程落地上可以把它看成一组按时间索引的二维切片堆叠。先不急着写规划器第一步把场的结构与坐标约定敲定否则后面所有查表都会对不上。2.1 栅格化坐标系与时空风险场的数据结构以自车当前位置为原点建立局部笛卡尔坐标系X 轴沿自车航向向前为正Y 轴向左为正。栅格范围不需要覆盖整个地图前向 60 m、横向 ±20 m 足够覆盖高速工况下的紧急换道场景。数据结构就是一个三维数组risk_grid[t][iy][ix]第一维是时间层后两维是空间栅格。import numpy as np GRID_X_MIN, GRID_X_MAX 0.0, 60.0 # 纵向范围前向为正 GRID_Y_MIN, GRID_Y_MAX -20.0, 20.0 # 横向范围左正右负 RESOLUTION 0.5 # 单个栅格尺寸单位米 DT 0.2 # 预测/规划时间步长秒 T_HORIZON 3.0 # 预测时域秒 N_STEPS int(T_HORIZON / DT) # 时间切片数量 x_edges np.arange(GRID_X_MIN, GRID_X_MAX RESOLUTION, RESOLUTION) y_edges np.arange(GRID_Y_MIN, GRID_Y_MAX RESOLUTION, RESOLUTION) nx, ny len(x_edges), len(y_edges) # 时空风险场N_STEPS 个时间片每个时间片是一张二维风险图 risk_grid np.zeros((N_STEPS, ny, nx))这里最容易出错的是索引顺序。数组维度是(层, y, x)物理坐标(x, y)对应risk_grid[t, iy, ix]其中iy由 y 坐标换算、ix由 x 坐标换算。如果统一用(x_idx, y_idx)取索引后面可视化时会发现场被转置。一个比较稳的习惯是把所有栅格坐标换算封装成两个小函数配置文件和代价函数都走同一套接口。2.2 静态风险场用距离变换代替逐栅格遍历静态风险源包括道路边界、护栏、静止障碍物。逐个栅格遍历求最近障碍物距离在 120×80 栅格下勉强能跑但论文里通常要对比不同栅格分辨率遍历法会拖慢整个验证循环。更聪明的做法是先把障碍物画进占位栅格再用scipy.ndimage.distance_transform_edt一次算出每个栅格到最近障碍物的距离from scipy.ndimage import distance_transform_edt occ np.zeros((ny, nx)) for obs_x, obs_y, obs_r in static_obstacles: ix int((obs_x - GRID_X_MIN) / RESOLUTION) iy int((obs_y - GRID_Y_MIN) / RESOLUTION) if 0 ix nx and 0 iy ny: occ[iy, ix] 1.0 # 被障碍物占据的栅格记为 1 dist distance_transform_edt(1 - occ) # 每个栅格到最近障碍物的距离 MU_STATIC, SIGMA_S 1.0, 1.5 static_field MU_STATIC * np.exp(-(dist ** 2) / (2 * SIGMA_S ** 2)) static_field[occ 0] 1.0 # 障碍物本身风险取最大值distance_transform_edt输入的是背景为 0 的数组所以传入1 - occ让障碍物位置变成最近的 0 点。SIGMA_S是风险衰减宽度取值 1.5 m 表示车辆离障碍物 1.5 m 左右时风险开始明显抬升太大会让整个车道的风险都偏高轨迹不敢靠近路边太小则车辆贴着墙走。理论公式常见的是这种指数型势函数也有论文用分段线性函数复现阶段先固定指数形式跑通后再替换。2.3 动态风险注入高斯分布映射到每个时间切片动态目标比静态多出速度、朝向和类型三个信息。预测模块给出目标在未来某时刻的位置均值mu和协方差矩阵cov后把二维高斯分布的概率密度乘以一个最大风险系数加到对应时间切片上def add_gaussian_risk(grid_slice, mu, cov, max_risk, res, x_min, y_min): ny, nx grid_slice.shape x x_min res * (np.arange(nx) 0.5) y y_min res * (np.arange(ny) 0.5) gx, gy np.meshgrid(x, y) dx gx - mu[0] dy gy - mu[1] inv np.linalg.inv(cov) # 马氏距离平方 dist2 inv[0, 0] * dx**2 2.0 * inv[0, 1] * dx * dy inv[1, 1] * dy**2 grid_slice max_risk * np.exp(-0.5 * dist2)max_risk建议与静态场的MU_STATIC保持同一量纲比如都取 1.0这样动态和静态风险在代价函数里可直接相加。cov对角线的开方表示预测位置标准差高速公路上纵向不确定度通常大于横向所以cov[0,0]比cov[1,1]大画出来是一个沿道路方向拉长的椭圆斑。横向不确定度受车道约束不能给太大否则隔壁车道的风险也被过量抬高轨迹会过度保守。2.4 时间维语义轨迹点只查对应时间的切片不是查全量场这是时空风险场区别于普通势场的关键。某个轨迹点(x_i, y_i, t_i)的风险值只应该查t_i对应那一层的risk_grid[layer, iy, ix]而不是把这 3 秒所有的层的值取平均或取最大。时间上错位会导致“前车已经过去了但轨迹还以为它堵在那里”。def query_risk(traj_x, traj_y, traj_t, risk_grid, dt, x_min, y_min, res): risks [] for x_i, y_i, t_i in zip(traj_x, traj_y, traj_t): layer min(int(round(t_i / dt)), risk_grid.shape[0] - 1) ix int((x_i - x_min) / res) iy int((y_i - y_min) / res) if 0 ix risk_grid.shape[2] and 0 iy risk_grid.shape[1]: risks.append(risk_grid[layer, iy, ix]) else: risks.append(1.0) # 越界按最高风险处理 return np.array(risks)这段查询逻辑是后面代价函数的地基。t 必须在 0 到 T_HORIZON 之间超出部分被钳到最后一层。越界返回 1.0 是安全兜底但只在正常范围内有效如果轨迹经常越界说明栅格范围设小了或者代价函数的边界约束没起作用。3. 预测怎么跟风险场耦合从不确定性传播到多模态注入有些论文把预测模块写得非常重但时空风险场框架下预测的价值不是轨迹点有多准而是不确定性的形状有多合理。复现阶段完全可以从最简单的运动学预测起步。3.1 没有代码怎么复现论文先搞清预测输出如何拼进风险场很多人在复现时第一反应是去找作者的开源代码找不到就卡住。实际上时空风险场这系列论文的核心创新在场与规划的结合方式不在预测网络本身。先写一个恒速CV预测器输出均值和协方差跑通风险场与优化器的闭环再去替换成学习型预测网络这样每一步都能定位问题。没有源码时把公式拆成“输入状态、输出分布、注入栅格”三个可单测的小模块是最快的破解路径。3.2 CV 预测器与协方差传播写一个能跑起来的预测类恒速模型假设目标速度不变位置随时间线性外推。位置误差随时间累积协方差矩阵做增量膨胀class CVPredictor: def __init__(self, q_pos0.8): self.q_pos q_pos def predict(self, obj, horizon, dt): means, covs [], [] cov np.diag([1.5, 1.5]) # 初始位置不确定度 for k in range(1, horizon 1): t k * dt mean np.array([obj[x] obj[vx] * t, obj[y] obj[vy] * t]) cov cov self.q_pos * dt * np.eye(2) diag np.minimum(np.diag(cov), [12.0, 8.0]) # 限制协方差上限 cov np.diag(diag) means.append(mean) covs.append(cov.copy()) return means, covsq_pos是过程噪声强度控制协方差增长速度。取 0.8 时3 秒后纵向标准差大致在 2~3 m适合高速路况取到 1.5 会过于保守轨迹会提前很远就开始避让。限制协方差上限很重要CVR 长时间外推误差不可能无限增长给一个纵向 12、横向 8 的上限保证 3 秒后风险场不是一团均匀迷雾。3.3 蒙特卡洛采样式风险注入多模态预测的兜底做法高斯协方差表达的是单峰不确定性。如果目标有变道意图单峰高斯表达不了“要么直行、要么左变道”这种双模态分布。复现论文时先不引入复杂的意图估计用蒙特卡洛采样做一个近似的多模态风险场对速度加随机扰动采样多条轨迹每条轨迹都做一次高斯风险注入最后叠加。def sample_trajectories(obj, n_samples50, horizon10, dt0.2): trajs [] t np.arange(1, horizon 1) * dt for _ in range(n_samples): vx obj[vx] np.random.normal(0.0, 0.3) vy obj[vy] np.random.normal(0.0, 0.2) xs obj[x] vx * t ys obj[y] vy * t trajs.append(np.stack([xs, ys], axis1)) return trajs采样数 50 已经足够稳定每条轨迹权重取 1/50。速度扰动里纵向噪声大于横向噪声对应真实驾驶中纵向速度更容易变化。多模态场叠加后车道上会出现几条明显的“风险带”规划器自然选择绕开这些带。注意采样方法只用于效果对标最终论文图还是建议用解析高斯形式否则会引入随机性两次运行结果不一致。3.4 预测步长不同步时怎么对层预测时域要和规划时域一致这个“一致”包含总时长和步长两层含义。预测只有 2 s、规划要看 3 s超过第 2 s 的部分没有风险数据。解决方法是把最后一层复制并插值同时对超时部分的风险值做乘性放大逼迫轨迹尽量在可预测的时域内完成动作。反过来预测层比规划层密时轨迹点按就近取层。工程上建议直接统一DT和N_STEPS避免两种精度在代码里混用。4. 轨迹规划把风险场变成代价函数的一部分场建好后规划问题变成在状态空间里找一条从当前状态到目标状态、且时空代价最小的轨迹。代价函数的构成直接决定规划结果偏安全还是偏激进。4.1 轨迹参数化横向纵向解耦比笛卡尔坐标直接优化更适合高速场景高速场景下道路走势由参考线决定直接优化笛卡尔坐标点需要处理车辆航向与道路对齐的强约束非线性程度高。常见做法是把轨迹分解到 Frenet 坐标系用纵向位移s(t)和横向偏移d(t)分别表示。每个方向用五次多项式参数化六个系数正好能约束初始和末端的位姿、速度、加速度def evaluate_poly(coeffs, dt, horizon): t np.arange(0, horizon) * dt s_vals np.polyval(coeffs[:6], t) d_vals np.polyval(coeffs[6:], t) return s_vals, d_vals这里的coeffs是 12 维向量前 6 个为纵向多项式系数后 6 个为横向系数。多项式系数从高次到低次排列恰好对应np.polyval的约定。五次多项式的好处是加速度曲线连续不存在分段点处的跳变代价函数里不需要额外处理拼接点比样条插值少一层麻烦。4.2 代价函数设计风险、纵向加速度、横向抖动与参考线权重怎么配总代价分为四项风险累计、纵向加速度平方和、加加速度平方和、横向偏移平方和。def trajectory_cost(coeffs, risk_grid, ref_line, dt, weights): s_vals, d_vals evaluate_poly(coeffs, dt, risk_grid.shape[0]) total 0.0 for i, (s, d) in enumerate(zip(s_vals, d_vals)): x, y frenet_to_cartesian(ref_line, s, d) layer min(i, risk_grid.shape[0] - 1) ix int((x - GRID_X_MIN) / RESOLUTION) iy int((y - GRID_Y_MIN) / RESOLUTION) if 0 ix risk_grid.shape[2] and 0 iy risk_grid.shape[1]: total weights[risk] * risk_grid[layer, iy, ix] else: total weights[risk] * 1.0 total weights[ref] * d ** 2 s_acc np.diff(np.diff(s_vals) / dt) / dt d_acc np.diff(np.diff(d_vals) / dt) / dt s_jerk np.diff(s_acc) / dt d_jerk np.diff(d_acc) / dt total weights[acc] * (np.sum(s_acc**2) np.sum(d_acc**2)) total weights[jerk] * (np.sum(s_jerk**2) np.sum(d_jerk**2)) return totalweights[risk]是唯一直接连到风险场的通道。论文里权重的绝对值没有参考价值关键是各项量级要先归一。风险项通常在 0.1~1 之间加速度项可能到几十所以需要先跑一次只含单项目标的优化统计量级再定比例。经验初始值如下权重项含义建议初值调参方向risk风险场累计代价8.0过大时轨迹整体偏移远离障碍acc纵横向加速度平方和0.4过大时加减速太慢跟车距离拉长jerk加加速度平方和0.8过大时轨迹过度平滑换道变肉ref横向偏差平方和1.0过大时不敢换道始终贴参考线4.3 求解器选择用 scipy.optimize.minimize 做序列二次规划虽然是高维非线性优化问题但多项式系数带来的决策变量只有 12 个约束是初末状态等式和横向加速度不等式。SLSQP对这种规模的问题收敛稳定实现成本低from scipy.optimize import minimize def plan_optimal_trajectory(init_coeffs, target_state, risk_grid, ref_line, dt, weights): def obj(c): return trajectory_cost(c, risk_grid, ref_line, dt, weights) def state_constraint(c): s0, d0 evaluate_poly(c, dt, 1) # 取第一个轨迹点作为当前状态 return np.concatenate([s0 - ego_s0, d0 - ego_d0]) cons [ {type: eq, fun: state_constraint}, {type: ineq, fun: lambda c: lateral_acc_limit(c, ref_line, dt)} ] res minimize(obj, init_coeffs, methodSLSQP, constraintscons, options{maxiter: 300, ftol: 1e-8}) return res不要一开始就上 MPC 或强化学习。MPC 需要实时求解 QP其中约束构造和求解器接口调通就要一两天强化学习需要离线训练环境和奖励设计无法快速验证风险场逻辑是否正确。SLSQP 有现成的数值梯度对 12 维问题足够快。init_coeffs必须给一个好的初值从当前状态到目标状态用匀速插值生成参考轨迹再拟合多项式系数。初值太差时 SLSQP 会收敛到局部极小轨迹直接冲出栅格边界。4.4 最小闭环预测、风险场、优化器如何拼成一条可运行主循环前面的模块要串起来。主循环按“预测目标 - 构建场 - 生成候选轨迹 - 选最优”顺序执行。def main_loop(): ego {x: 0.0, y: 0.0, vx: 18.0, vy: 0.0} lead {x: 25.0, y: 0.0, vx: 10.0, vy: 0.0} predictor CVPredictor(q_pos0.8) means, covs predictor.predict(lead, N_STEPS, DT) risk np.repeat(static_field[None, :, :], N_STEPS, axis0) for t, (mu, cov) in enumerate(zip(means, covs)): add_gaussian_risk(risk[t], mu, cov, max_risk1.0, resRESOLUTION, x_minGRID_X_MIN, y_minGRID_Y_MIN) candidates [] for lateral_offset in [-3.5, 0.0, 3.5]: target_state (lead[x] - 5.0, lateral_offset) # 目标点在前车后方5米 init_coeffs generate_init_coeffs(ego, target_state) res plan_optimal_trajectory(init_coeffs, target_state, risk, ref_line, DT, weights) if res.success: candidates.append((res.fun, res.x)) best_coeffs min(candidates, keylambda item: item[0])[1] return best_coeffs这里三个横向偏移对应“保持车道、向左换道、向右换道”三个候选。目标点取前车后方 5 m是为了给换道后的跟车留出安全余量。generate_init_coeffs用直线插值生成初值不做任何避障交给优化器去修正。整个闭环跑通后验证的重点转向“轨迹是否真的避开了风险场的高亮区域”而不是盯着代价函数数值看。5. 验证与调优用可视化反查风险场是否真正起作用代码闭环之后第一件事不是调权重而是确认风险场本身没有建歪。可视化切片是最直接的验证工具。import matplotlib.pyplot as plt def visualize_layer(t, trajectory, risk_grid): plt.figure(figsize(8, 5)) plt.imshow(risk_grid[t], originlower, extent[GRID_X_MIN, GRID_X_MAX, GRID_Y_MIN, GRID_Y_MAX], cmaphot_r, vmin0.0, vmax1.0) xs, ys trajectory[:, 0], trajectory[:, 1] plt.plot(xs, ys, c-, linewidth2, labeltrajectory) plt.scatter(xs[0], ys[0], cb, labelstart) plt.colorbar(labelrisk) plt.legend() plt.show()originlower保证 y 轴方向与数组行索引一致extent对应真实物理范围。网格上叠加轨迹后轨迹应该从低风险区域穿过而不是横切红色高亮带。这一步能发现栅格转置、坐标系偏移、时间层错位等问题。调参可以先看下表参数含义建议初值调参方向RESOLUTION栅格分辨率0.5 m调大提速但风险细节被糊掉q_pos预测过程噪声0.8高速大车流可到 1.5SIGMA_S静态场衰减宽度1.5 m窄路收缩到 1.0weights[risk]风险权重8.0过大时轨迹整体躲让候选横向偏移换道目标集[-3.5, 0, 3.5]按车道宽度修改复现论文时最常遇到的三个问题第一代价函数里轨迹点时间戳与预测层错位轨迹查了错误的层导致风险值异常第二SLSQP 初值给得太差轨迹越过栅格边界“逃逸”横向约束没写到优化器里第三可视化时imshow的坐标轴没有和栅格索引对齐导致场画对了但看着像转置越查越乱。前两个问题分别通过query_risk单元测试和横向边界约束解决第三个问题统一用第 5 章的可视化函数排查。验证是否正确闭环看三条标准无风险场景下轨迹与参考线偏差小于 0.3 m 且加速度平滑有前车减速时换道轨迹的风险累计值明显低于保持车道轨迹把轨迹绘制在对应时间切片上轨迹不穿过高亮区。三条都满足时空风险场从建模到规划这条路就走通了。本文还有配套的精品资源点击获取 基于时空风险场的自动驾驶轨迹规划实战：从预测耦合到SLSQP优化
简介面向自动驾驶研究人员、高校相关专业师生及智能交通系统科研工作者此份复现资料围绕基于时空风险场的道路自动驾驶车辆预测轨迹规划方法展开。文档采用轨迹生成与风险评估解耦的双模块结构通过类人工势场量化空间风险结合交互多模型(IMM)预测周围车辆行为概率并利用精心设计的成本函数从候选轨迹中选出最优路径。资源为单个docx文档压缩包约60KB内容覆盖算法概述、完整Python代码、规划空间管理、预测模块、参考线构建与曲率计算、RRT*采样与优化基轨迹平滑等还总结了系统性能优化建议有助于读者理解原理并快速实现。目前已有143人学习下载适合具备一定路径规划与概率基础、希望深入掌握预测轨迹规划实际代码细节的技术人员。1. 基于时空风险场的自动驾驶轨迹规划到底在解决什么问题在一段双车道快速路上自车以 18 m/s 巡航前车突然从 15 m/s 减速到 8 m/s左侧车道后方有一辆正在加速的货车。传统做法是先预测前车轨迹再把预测结果当成“未来必定如此”的一条线交给规划器。问题在于预测线的误差范围没有显式建模轨迹贴着预测线走一旦前车实际减速比预测更狠换道窗口就没了。时空风险场把每个预测目标按概率密度扩散成一片随未来时间变化的风险场规划器不去盯一条轨迹而是选一条累计风险最低的轨迹。它解决的是预测与规划之间“误差怎么传递”这个连接问题适合正在复现论文但卡在公式到代码映射的读者也适合想从规则式或采样式规划转向风险场优化框架的工程师。2. 先建场静态风险场与动态风险场的数学模型风险场看似是四维数据工程落地上可以把它看成一组按时间索引的二维切片堆叠。先不急着写规划器第一步把场的结构与坐标约定敲定否则后面所有查表都会对不上。2.1 栅格化坐标系与时空风险场的数据结构以自车当前位置为原点建立局部笛卡尔坐标系X 轴沿自车航向向前为正Y 轴向左为正。栅格范围不需要覆盖整个地图前向 60 m、横向 ±20 m 足够覆盖高速工况下的紧急换道场景。数据结构就是一个三维数组risk_grid[t][iy][ix]第一维是时间层后两维是空间栅格。import numpy as np GRID_X_MIN, GRID_X_MAX 0.0, 60.0 # 纵向范围前向为正 GRID_Y_MIN, GRID_Y_MAX -20.0, 20.0 # 横向范围左正右负 RESOLUTION 0.5 # 单个栅格尺寸单位米 DT 0.2 # 预测/规划时间步长秒 T_HORIZON 3.0 # 预测时域秒 N_STEPS int(T_HORIZON / DT) # 时间切片数量 x_edges np.arange(GRID_X_MIN, GRID_X_MAX RESOLUTION, RESOLUTION) y_edges np.arange(GRID_Y_MIN, GRID_Y_MAX RESOLUTION, RESOLUTION) nx, ny len(x_edges), len(y_edges) # 时空风险场N_STEPS 个时间片每个时间片是一张二维风险图 risk_grid np.zeros((N_STEPS, ny, nx))这里最容易出错的是索引顺序。数组维度是(层, y, x)物理坐标(x, y)对应risk_grid[t, iy, ix]其中iy由 y 坐标换算、ix由 x 坐标换算。如果统一用(x_idx, y_idx)取索引后面可视化时会发现场被转置。一个比较稳的习惯是把所有栅格坐标换算封装成两个小函数配置文件和代价函数都走同一套接口。2.2 静态风险场用距离变换代替逐栅格遍历静态风险源包括道路边界、护栏、静止障碍物。逐个栅格遍历求最近障碍物距离在 120×80 栅格下勉强能跑但论文里通常要对比不同栅格分辨率遍历法会拖慢整个验证循环。更聪明的做法是先把障碍物画进占位栅格再用scipy.ndimage.distance_transform_edt一次算出每个栅格到最近障碍物的距离from scipy.ndimage import distance_transform_edt occ np.zeros((ny, nx)) for obs_x, obs_y, obs_r in static_obstacles: ix int((obs_x - GRID_X_MIN) / RESOLUTION) iy int((obs_y - GRID_Y_MIN) / RESOLUTION) if 0 ix nx and 0 iy ny: occ[iy, ix] 1.0 # 被障碍物占据的栅格记为 1 dist distance_transform_edt(1 - occ) # 每个栅格到最近障碍物的距离 MU_STATIC, SIGMA_S 1.0, 1.5 static_field MU_STATIC * np.exp(-(dist ** 2) / (2 * SIGMA_S ** 2)) static_field[occ 0] 1.0 # 障碍物本身风险取最大值distance_transform_edt输入的是背景为 0 的数组所以传入1 - occ让障碍物位置变成最近的 0 点。SIGMA_S是风险衰减宽度取值 1.5 m 表示车辆离障碍物 1.5 m 左右时风险开始明显抬升太大会让整个车道的风险都偏高轨迹不敢靠近路边太小则车辆贴着墙走。理论公式常见的是这种指数型势函数也有论文用分段线性函数复现阶段先固定指数形式跑通后再替换。2.3 动态风险注入高斯分布映射到每个时间切片动态目标比静态多出速度、朝向和类型三个信息。预测模块给出目标在未来某时刻的位置均值mu和协方差矩阵cov后把二维高斯分布的概率密度乘以一个最大风险系数加到对应时间切片上def add_gaussian_risk(grid_slice, mu, cov, max_risk, res, x_min, y_min): ny, nx grid_slice.shape x x_min res * (np.arange(nx) 0.5) y y_min res * (np.arange(ny) 0.5) gx, gy np.meshgrid(x, y) dx gx - mu[0] dy gy - mu[1] inv np.linalg.inv(cov) # 马氏距离平方 dist2 inv[0, 0] * dx**2 2.0 * inv[0, 1] * dx * dy inv[1, 1] * dy**2 grid_slice max_risk * np.exp(-0.5 * dist2)max_risk建议与静态场的MU_STATIC保持同一量纲比如都取 1.0这样动态和静态风险在代价函数里可直接相加。cov对角线的开方表示预测位置标准差高速公路上纵向不确定度通常大于横向所以cov[0,0]比cov[1,1]大画出来是一个沿道路方向拉长的椭圆斑。横向不确定度受车道约束不能给太大否则隔壁车道的风险也被过量抬高轨迹会过度保守。2.4 时间维语义轨迹点只查对应时间的切片不是查全量场这是时空风险场区别于普通势场的关键。某个轨迹点(x_i, y_i, t_i)的风险值只应该查t_i对应那一层的risk_grid[layer, iy, ix]而不是把这 3 秒所有的层的值取平均或取最大。时间上错位会导致“前车已经过去了但轨迹还以为它堵在那里”。def query_risk(traj_x, traj_y, traj_t, risk_grid, dt, x_min, y_min, res): risks [] for x_i, y_i, t_i in zip(traj_x, traj_y, traj_t): layer min(int(round(t_i / dt)), risk_grid.shape[0] - 1) ix int((x_i - x_min) / res) iy int((y_i - y_min) / res) if 0 ix risk_grid.shape[2] and 0 iy risk_grid.shape[1]: risks.append(risk_grid[layer, iy, ix]) else: risks.append(1.0) # 越界按最高风险处理 return np.array(risks)这段查询逻辑是后面代价函数的地基。t 必须在 0 到 T_HORIZON 之间超出部分被钳到最后一层。越界返回 1.0 是安全兜底但只在正常范围内有效如果轨迹经常越界说明栅格范围设小了或者代价函数的边界约束没起作用。3. 预测怎么跟风险场耦合从不确定性传播到多模态注入有些论文把预测模块写得非常重但时空风险场框架下预测的价值不是轨迹点有多准而是不确定性的形状有多合理。复现阶段完全可以从最简单的运动学预测起步。3.1 没有代码怎么复现论文先搞清预测输出如何拼进风险场很多人在复现时第一反应是去找作者的开源代码找不到就卡住。实际上时空风险场这系列论文的核心创新在场与规划的结合方式不在预测网络本身。先写一个恒速CV预测器输出均值和协方差跑通风险场与优化器的闭环再去替换成学习型预测网络这样每一步都能定位问题。没有源码时把公式拆成“输入状态、输出分布、注入栅格”三个可单测的小模块是最快的破解路径。3.2 CV 预测器与协方差传播写一个能跑起来的预测类恒速模型假设目标速度不变位置随时间线性外推。位置误差随时间累积协方差矩阵做增量膨胀class CVPredictor: def __init__(self, q_pos0.8): self.q_pos q_pos def predict(self, obj, horizon, dt): means, covs [], [] cov np.diag([1.5, 1.5]) # 初始位置不确定度 for k in range(1, horizon 1): t k * dt mean np.array([obj[x] obj[vx] * t, obj[y] obj[vy] * t]) cov cov self.q_pos * dt * np.eye(2) diag np.minimum(np.diag(cov), [12.0, 8.0]) # 限制协方差上限 cov np.diag(diag) means.append(mean) covs.append(cov.copy()) return means, covsq_pos是过程噪声强度控制协方差增长速度。取 0.8 时3 秒后纵向标准差大致在 2~3 m适合高速路况取到 1.5 会过于保守轨迹会提前很远就开始避让。限制协方差上限很重要CVR 长时间外推误差不可能无限增长给一个纵向 12、横向 8 的上限保证 3 秒后风险场不是一团均匀迷雾。3.3 蒙特卡洛采样式风险注入多模态预测的兜底做法高斯协方差表达的是单峰不确定性。如果目标有变道意图单峰高斯表达不了“要么直行、要么左变道”这种双模态分布。复现论文时先不引入复杂的意图估计用蒙特卡洛采样做一个近似的多模态风险场对速度加随机扰动采样多条轨迹每条轨迹都做一次高斯风险注入最后叠加。def sample_trajectories(obj, n_samples50, horizon10, dt0.2): trajs [] t np.arange(1, horizon 1) * dt for _ in range(n_samples): vx obj[vx] np.random.normal(0.0, 0.3) vy obj[vy] np.random.normal(0.0, 0.2) xs obj[x] vx * t ys obj[y] vy * t trajs.append(np.stack([xs, ys], axis1)) return trajs采样数 50 已经足够稳定每条轨迹权重取 1/50。速度扰动里纵向噪声大于横向噪声对应真实驾驶中纵向速度更容易变化。多模态场叠加后车道上会出现几条明显的“风险带”规划器自然选择绕开这些带。注意采样方法只用于效果对标最终论文图还是建议用解析高斯形式否则会引入随机性两次运行结果不一致。3.4 预测步长不同步时怎么对层预测时域要和规划时域一致这个“一致”包含总时长和步长两层含义。预测只有 2 s、规划要看 3 s超过第 2 s 的部分没有风险数据。解决方法是把最后一层复制并插值同时对超时部分的风险值做乘性放大逼迫轨迹尽量在可预测的时域内完成动作。反过来预测层比规划层密时轨迹点按就近取层。工程上建议直接统一DT和N_STEPS避免两种精度在代码里混用。4. 轨迹规划把风险场变成代价函数的一部分场建好后规划问题变成在状态空间里找一条从当前状态到目标状态、且时空代价最小的轨迹。代价函数的构成直接决定规划结果偏安全还是偏激进。4.1 轨迹参数化横向纵向解耦比笛卡尔坐标直接优化更适合高速场景高速场景下道路走势由参考线决定直接优化笛卡尔坐标点需要处理车辆航向与道路对齐的强约束非线性程度高。常见做法是把轨迹分解到 Frenet 坐标系用纵向位移s(t)和横向偏移d(t)分别表示。每个方向用五次多项式参数化六个系数正好能约束初始和末端的位姿、速度、加速度def evaluate_poly(coeffs, dt, horizon): t np.arange(0, horizon) * dt s_vals np.polyval(coeffs[:6], t) d_vals np.polyval(coeffs[6:], t) return s_vals, d_vals这里的coeffs是 12 维向量前 6 个为纵向多项式系数后 6 个为横向系数。多项式系数从高次到低次排列恰好对应np.polyval的约定。五次多项式的好处是加速度曲线连续不存在分段点处的跳变代价函数里不需要额外处理拼接点比样条插值少一层麻烦。4.2 代价函数设计风险、纵向加速度、横向抖动与参考线权重怎么配总代价分为四项风险累计、纵向加速度平方和、加加速度平方和、横向偏移平方和。def trajectory_cost(coeffs, risk_grid, ref_line, dt, weights): s_vals, d_vals evaluate_poly(coeffs, dt, risk_grid.shape[0]) total 0.0 for i, (s, d) in enumerate(zip(s_vals, d_vals)): x, y frenet_to_cartesian(ref_line, s, d) layer min(i, risk_grid.shape[0] - 1) ix int((x - GRID_X_MIN) / RESOLUTION) iy int((y - GRID_Y_MIN) / RESOLUTION) if 0 ix risk_grid.shape[2] and 0 iy risk_grid.shape[1]: total weights[risk] * risk_grid[layer, iy, ix] else: total weights[risk] * 1.0 total weights[ref] * d ** 2 s_acc np.diff(np.diff(s_vals) / dt) / dt d_acc np.diff(np.diff(d_vals) / dt) / dt s_jerk np.diff(s_acc) / dt d_jerk np.diff(d_acc) / dt total weights[acc] * (np.sum(s_acc**2) np.sum(d_acc**2)) total weights[jerk] * (np.sum(s_jerk**2) np.sum(d_jerk**2)) return totalweights[risk]是唯一直接连到风险场的通道。论文里权重的绝对值没有参考价值关键是各项量级要先归一。风险项通常在 0.1~1 之间加速度项可能到几十所以需要先跑一次只含单项目标的优化统计量级再定比例。经验初始值如下权重项含义建议初值调参方向risk风险场累计代价8.0过大时轨迹整体偏移远离障碍acc纵横向加速度平方和0.4过大时加减速太慢跟车距离拉长jerk加加速度平方和0.8过大时轨迹过度平滑换道变肉ref横向偏差平方和1.0过大时不敢换道始终贴参考线4.3 求解器选择用 scipy.optimize.minimize 做序列二次规划虽然是高维非线性优化问题但多项式系数带来的决策变量只有 12 个约束是初末状态等式和横向加速度不等式。SLSQP对这种规模的问题收敛稳定实现成本低from scipy.optimize import minimize def plan_optimal_trajectory(init_coeffs, target_state, risk_grid, ref_line, dt, weights): def obj(c): return trajectory_cost(c, risk_grid, ref_line, dt, weights) def state_constraint(c): s0, d0 evaluate_poly(c, dt, 1) # 取第一个轨迹点作为当前状态 return np.concatenate([s0 - ego_s0, d0 - ego_d0]) cons [ {type: eq, fun: state_constraint}, {type: ineq, fun: lambda c: lateral_acc_limit(c, ref_line, dt)} ] res minimize(obj, init_coeffs, methodSLSQP, constraintscons, options{maxiter: 300, ftol: 1e-8}) return res不要一开始就上 MPC 或强化学习。MPC 需要实时求解 QP其中约束构造和求解器接口调通就要一两天强化学习需要离线训练环境和奖励设计无法快速验证风险场逻辑是否正确。SLSQP 有现成的数值梯度对 12 维问题足够快。init_coeffs必须给一个好的初值从当前状态到目标状态用匀速插值生成参考轨迹再拟合多项式系数。初值太差时 SLSQP 会收敛到局部极小轨迹直接冲出栅格边界。4.4 最小闭环预测、风险场、优化器如何拼成一条可运行主循环前面的模块要串起来。主循环按“预测目标 - 构建场 - 生成候选轨迹 - 选最优”顺序执行。def main_loop(): ego {x: 0.0, y: 0.0, vx: 18.0, vy: 0.0} lead {x: 25.0, y: 0.0, vx: 10.0, vy: 0.0} predictor CVPredictor(q_pos0.8) means, covs predictor.predict(lead, N_STEPS, DT) risk np.repeat(static_field[None, :, :], N_STEPS, axis0) for t, (mu, cov) in enumerate(zip(means, covs)): add_gaussian_risk(risk[t], mu, cov, max_risk1.0, resRESOLUTION, x_minGRID_X_MIN, y_minGRID_Y_MIN) candidates [] for lateral_offset in [-3.5, 0.0, 3.5]: target_state (lead[x] - 5.0, lateral_offset) # 目标点在前车后方5米 init_coeffs generate_init_coeffs(ego, target_state) res plan_optimal_trajectory(init_coeffs, target_state, risk, ref_line, DT, weights) if res.success: candidates.append((res.fun, res.x)) best_coeffs min(candidates, keylambda item: item[0])[1] return best_coeffs这里三个横向偏移对应“保持车道、向左换道、向右换道”三个候选。目标点取前车后方 5 m是为了给换道后的跟车留出安全余量。generate_init_coeffs用直线插值生成初值不做任何避障交给优化器去修正。整个闭环跑通后验证的重点转向“轨迹是否真的避开了风险场的高亮区域”而不是盯着代价函数数值看。5. 验证与调优用可视化反查风险场是否真正起作用代码闭环之后第一件事不是调权重而是确认风险场本身没有建歪。可视化切片是最直接的验证工具。import matplotlib.pyplot as plt def visualize_layer(t, trajectory, risk_grid): plt.figure(figsize(8, 5)) plt.imshow(risk_grid[t], originlower, extent[GRID_X_MIN, GRID_X_MAX, GRID_Y_MIN, GRID_Y_MAX], cmaphot_r, vmin0.0, vmax1.0) xs, ys trajectory[:, 0], trajectory[:, 1] plt.plot(xs, ys, c-, linewidth2, labeltrajectory) plt.scatter(xs[0], ys[0], cb, labelstart) plt.colorbar(labelrisk) plt.legend() plt.show()originlower保证 y 轴方向与数组行索引一致extent对应真实物理范围。网格上叠加轨迹后轨迹应该从低风险区域穿过而不是横切红色高亮带。这一步能发现栅格转置、坐标系偏移、时间层错位等问题。调参可以先看下表参数含义建议初值调参方向RESOLUTION栅格分辨率0.5 m调大提速但风险细节被糊掉q_pos预测过程噪声0.8高速大车流可到 1.5SIGMA_S静态场衰减宽度1.5 m窄路收缩到 1.0weights[risk]风险权重8.0过大时轨迹整体躲让候选横向偏移换道目标集[-3.5, 0, 3.5]按车道宽度修改复现论文时最常遇到的三个问题第一代价函数里轨迹点时间戳与预测层错位轨迹查了错误的层导致风险值异常第二SLSQP 初值给得太差轨迹越过栅格边界“逃逸”横向约束没写到优化器里第三可视化时imshow的坐标轴没有和栅格索引对齐导致场画对了但看着像转置越查越乱。前两个问题分别通过query_risk单元测试和横向边界约束解决第三个问题统一用第 5 章的可视化函数排查。验证是否正确闭环看三条标准无风险场景下轨迹与参考线偏差小于 0.3 m 且加速度平滑有前车减速时换道轨迹的风险累计值明显低于保持车道轨迹把轨迹绘制在对应时间切片上轨迹不穿过高亮区。三条都满足时空风险场从建模到规划这条路就走通了。本文还有配套的精品资源点击获取