共同努力学习学习1. 项目概述：基于动态规划的自动驾驶轨迹规划系统
在自动驾驶技术栈中，轨迹规划模块扮演着承上启下的关键角色。它需要将感知模块识别的环境信息和决策模块输出的高级指令，转化为车辆可以执行的具体运动轨迹。这个看似简单的任务背后，隐藏着复杂的数学建模和优化问题。
我最近完整实现了一套基于动态规划（Dynamic Programming）的路径与速度规划系统，参考了百度Apollo平台中EM Planner的核心思想。这个系统最大的特点是采用"分而治之"的策略，将完整的轨迹规划问题拆解为两个相对独立的子问题：路径规划和速度规划。这种架构设计不仅降低了问题复杂度，还提高了系统的可维护性和扩展性。
需要模型API调用？ 免费领10W Token，多模型网关一键接入 Claude、DeepSeek 等主流模型。
2. 系统架构设计
2.1 路径-速度解耦策略
传统的一体化轨迹规划方法需要同时处理空间和时间维度，导致计算复杂度呈指数级增长。我们的系统采用了更聪明的做法：
-
路径规划：在Frenet坐标系下进行，只关心车辆相对于参考线的横向偏移(l)随纵向距离(s)的变化关系。这相当于规划车辆的"行驶路线"。
-
速度规划：在ST坐标系下进行，关注纵向距离(s)随时间(t)的变化关系。这相当于规划车辆沿既定路线行驶的"时间表"。
这种解耦带来的好处是显而易见的：原本的6维规划问题（x,y,z,速度,加速度,时间）被简化为两个3维问题（s,l和s,t），计算量大幅降低。
2.2 分层规划思想
系统采用"粗糙规划+精细优化"的两阶段策略：
- 动态规划阶段：快速生成满足基本约束的候选轨迹。这个阶段的特点是计算高效，能保证实时性。
- 优化阶段（可选）：在DP结果基础上进行二次优化，提升轨迹的平滑性和舒适性。这个阶段计算量较大，但可以离线进行。
这种分层设计既保证了系统的实时响应能力，又确保了轨迹质量。在实际应用中，我们可以根据计算资源情况灵活调整两个阶段的比例。
3. 路径规划模块实现细节
3.1 Frenet坐标系与图构建
Frenet坐标系是路径规划的基础，它将复杂的全局坐标系问题转化为相对简单的局部坐标系问题。具体构建过程如下：
- 参考线生成：使用Clothoid曲线（回旋曲线）拟合高精度地图提供的道路中心线，确保参考线二阶连续可导。
- 坐标系转换：将全局(x,y)坐标转换为(s,l)坐标，其中s为沿参考线的纵向距离，l为横向偏移。
- 网格化采样：
- 纵向(s方向)：将规划范围（如100米）均匀划分为N层（通常5-10层）
- 横向(l方向)：每层在道路边界内均匀采样M个点（通常7-11个）
cpp复制// C++示例：Frenet坐标系下的路径点采样
vector<FrenetPoint> samplePathPoints(const ReferenceLine& refLine) {
vector<FrenetPoint> samples;
double s_step = refLine.length() / (LAYERS_COUNT - 1);
for(int i=0; i<LAYERS_COUNT; ++i) {
double s = i * s_step;
double l_min = -road_width/2 + vehicle_width/2;
double l_max = road_width/2 - vehicle_width/2;
double l_step = (l_max - l_min) / (POINTS_PER_LAYER - 1);
for(int j=0; j<POINTS_PER_LAYER; ++j) {
double l = l_min + j * l_step;
samples.emplace_back(s, l);
}
}
return samples;
}
3.2 五次多项式连接与代价计算
相邻层之间的路径点采用五次多项式连接，确保轨迹的二阶平滑性。五次多项式的一般形式为：
l(s) = a₅s⁵ + a₄s⁴ + a₃s³ + a₂s² + a₁s + a₀
通过边界条件（起点和终点的l, dl/ds, d²l/ds²）可以唯一确定多项式系数。这种连接方式保证了轨迹在几何上的连续性。
代价函数设计是路径规划的核心，我们综合考虑了多个因素：
-
平滑性代价：惩罚横向偏移的高阶导数
math复制J_{smooth} = w_l·l² + w_{dl}·(dl/ds)² + w_{ddl}·(d²l/ds²)² -
障碍物代价：基于S-T签名的精确碰撞检测
cpp复制bool checkCollision(const VehicleState& state, const Obstacle& obs) { Polygon ego_poly = getVehiclePolygon(state); Polygon obs_poly = obs.getPolygon(); return boost::geometry::intersects(ego_poly, obs_poly); } -
参考线偏离代价：鼓励车辆保持在车道中心
math复制J_{ref} = w_{ref}·l²
3.3 动态规划求解过程
DP求解采用经典的逆向代价计算、正向路径回溯策略：
- 初始化：第一层所有节点的代价为其偏离参考线的代价
- 递推计算：对于第k层的每个节点，遍历第k-1层的所有节点：
- 计算连接代价（多项式拟合代价+平滑性代价）
- 如果连接有效（无碰撞），更新当前节点的最小累计代价
- 路径回溯：从最后一层最小代价节点开始，根据记录的父节点索引反向追踪
cpp复制// DP路径规划核心算法
vector<FrenetPoint> DPPathPlanner::plan() {
// 初始化
for(auto& node : layers[0]) {
node.cost = calcReferenceCost(node);
}
// 动态规划递推
for(int k=1; k<layers.size(); ++k) {
for(auto& curr_node : layers[k]) {
double min_cost = INFINITY;
for(auto& prev_node : layers[k-1]) {
double edge_cost = calcEdgeCost(prev_node, curr_node);
double total_cost = prev_node.cost + edge_cost;
if(total_cost < min_cost) {
min_cost = total_cost;
curr_node.best_prev = &prev_node;
}
}
curr_node.cost = min_cost;
}
}
// 回溯最优路径
vector<FrenetPoint> path;
DPNode* node = findMinCostNode(layers.back());
while(node) {
path.push_back(node->point);
node = node->best_prev;
}
reverse(path.begin(), path.end());
return path;
}
4. 速度规划模块实现
4.1 障碍物的时空映射
速度规划的核心是将动态障碍物投影到ST图上，形成时空禁行区。这个过程需要考虑：
- 障碍物预测：基于感知模块的输出，预测障碍物未来轨迹
- 坐标转换：将障碍物在全局坐标系的轨迹转换到Frenet坐标系
- 禁行区生成：根据决策指令（跟车/超车）确定禁行区域
cpp复制STArea generateObstacleSTArea(const Obstacle& obs, Decision decision) {
STArea area;
for(const auto& point : obs.predicted_trajectory) {
FrenetPoint frenet_point = convertToFrenet(point);
if(decision == FOLLOW) {
area.addTimeRange(point.time, 0, frenet_point.s - SAFETY_DISTANCE);
} else if(decision == OVERTAKE) {
area.addTimeRange(point.time, frenet_point.s + SAFETY_DISTANCE, INFINITY);
}
}
return area;
}
4.2 ST图构建与状态转移
ST图的构建需要考虑车辆动力学约束：
- 时间分层：将规划时域（如8秒）离散为若干时间步（如Δt=0.1秒）
- 纵向距离采样：基于当前速度和加速度限制计算可行范围
math复制s_{min} = s_0 + v_0·Δt - 0.5·a_{max}·Δt² s_{max} = s_0 + v_0·Δt + 0.5·a_{max}·Δt² - 状态转移约束：确保相邻状态间的加速度在合理范围内
4.3 速度规划代价函数
速度规划的代价函数设计需要考虑：
- 效率代价：鼓励保持期望速度
math复制J_{speed} = w_{speed}·(v - v_{desired})² - 舒适性代价：惩罚加速度变化
math复制J_{comfort} = w_{acc}·a² + w_{jerk}·(Δa/Δt)² - 障碍物代价：避免进入禁行区
5. 系统集成与优化
5.1 路径与速度轨迹融合
得到路径和速度剖面后，需要将它们融合为完整的时空轨迹：
- 路径重采样：根据速度规划结果调整路径点密度
- 时空同步：确保路径上的每个点都有对应的时间戳
- 坐标系转换：将Frenet坐标转换回全局坐标系
5.2 基于优化的精细化处理
DP结果可以作为初值输入到优化器中进一步优化。常用的优化方法包括：
- 二次规划(QP)：将问题建模为二次代价函数+线性约束
- 非线性优化：使用IPOPT等求解器处理复杂约束
- 样条平滑：使用B样条或Bezier曲线提升轨迹平滑性
cpp复制void optimizeTrajectory(Trajectory& traj) {
// 构建优化问题
Problem problem;
// 添加优化变量
for(auto& point : traj) {
problem.addVariable(point.s);
problem.addVariable(point.l);
}
// 添加约束（边界约束、动力学约束等）
addConstraints(problem);
// 设置代价函数
problem.setCostFunction(calcSmoothnessCost);
// 调用求解器
IPOPT_Solver solver;
solver.solve(problem);
// 更新轨迹
updateTrajectory(traj, problem.getSolution());
}
6. 实际应用中的经验分享
6.1 参数调优技巧
- 代价函数权重：需要反复测试调整。建议先确定障碍物代价权重，再调整平滑性权重
- 网格分辨率：平衡计算精度和效率。实践中发现5-10层路径网格+0.1s时间步长是个不错的起点
- 动力学参数：根据车辆实际性能设置，保留10%-20%的安全余量
6.2 常见问题排查
-
轨迹抖动问题：
- 检查五次多项式连接是否满足边界条件
- 增加平滑性代价权重
- 考虑使用更高阶的多项式连接
-
规划失败问题：
- 验证障碍物投影是否正确
- 检查动态规划初始化是否合理
- 确认代价函数没有过度约束
-
实时性问题：
- 优化碰撞检测算法（如使用AABB包围盒预筛选）
- 减少网格分辨率或规划时域
- 考虑并行计算各层DP
6.3 性能优化建议
-
算法层面：
- 采用双向DP搜索
- 实现剪枝策略（如beam search）
- 使用启发式代价引导搜索
-
工程实现：
- 内存预分配避免动态申请
- 使用SIMD指令加速向量运算
- 实现热启动机制利用上一帧结果
-
系统架构：
- 将路径和速度规划分配到不同线程
- 实现异步规划机制
- 考虑多分辨率分层规划
这套基于动态规划的轨迹规划系统在实际测试中表现出色，在普通计算平台上能达到50-100ms的规划周期，完全满足自动驾驶的实时性要求。通过合理调整参数和优化实现，它能够处理城市道路、高速公路等多种复杂场景。