吴思扬1. 系统架构与设计理念
这套自动驾驶规划系统的核心思想源自Apollo EM Planner的经典分层架构，但针对教学和科研场景进行了模块化重构。我在实际开发中发现，将路径规划与速度规划解耦的设计，能够显著降低系统复杂度，特别适合中小型团队进行算法迭代。
系统采用C++14标准开发，整体架构分为三个主要层次：
- 感知接口层：负责接收和预处理传感器数据
- 规划算法层：包含动态规划路径规划、速度规划等核心模块
- 执行控制层：处理轨迹跟踪和车辆控制
关键设计决策：选择动态规划而非A或RRT等算法，主要考虑到DP在结构化道路场景下的两个优势：一是能保证找到全局最优解（或近似最优解），二是计算效率可满足实时性要求（在100ms内完成规划）。
需要模型API调用？ 免费领10W Token，多模型网关一键接入 Claude、DeepSeek 等主流模型。
2. 参考线生成模块实现细节
2.1 Clothoid曲线数学原理
Clothoid曲线（又称欧拉螺旋）的曲率k随弧长s线性变化：
code复制k(s) = k0 + σ·s
其中k0为初始曲率，σ为曲率变化率。这种特性使其特别适合描述车辆转向过程，因为方向盘的转动本质上也是在连续调整曲率。
实际实现时，我们采用分段Clothoid拟合的方法：
- 将原始路点按5米间隔重采样
- 对每两个相邻点计算初始曲率和曲率变化率
- 使用Ipopt求解器优化参数，保证G2连续性
2.2 坐标转换实现技巧
Frenet坐标系转换有个常见陷阱：当参考线曲率较大时，简单的投影法会导致坐标震荡。我们的解决方案是：
cpp复制// 改进的投影算法伪代码
FrenetPoint CartesianToFrenet(const CartesianPoint& p) {
double min_dist = INFINITY;
FrenetPoint result;
// 先在粗粒度上搜索最近点
for (double s = s_min; s <= s_max; s += 5.0) {
CartesianPoint ref = reference_line.interpolate(s);
double dist = distance(p, ref);
if (dist < min_dist) {
min_dist = dist;
result.s = s;
}
}
// 然后在邻域内精细搜索
for (double s = result.s - 5.0; s <= result.s + 5.0; s += 0.1) {
// ...同上...
}
return result;
}
3. 路径规划模块深度解析
3.1 DP路径搜索实现
动态规划的核心是状态转移方程。在我们的SL图中，代价函数设计为：
code复制cost = w1·l^2 + w2·dl^2 + w3·ddl^2 + w4·obs_cost
其中权重系数通过大量仿真实验确定：
- w1=1.0（横向偏移惩罚）
- w2=0.5（横向速度惩罚）
- w3=0.1（横向加速度惩罚）
- w4=10.0（障碍物碰撞惩罚）
实际编码时，采用递推式计算累计代价：
cpp复制for (int level = 1; level < max_level; ++level) {
for (int curr_node : level_nodes[level]) {
double min_cost = INFINITY;
for (int prev_node : level_nodes[level-1]) {
double transition_cost = CalcTransitionCost(prev_node, curr_node);
double total_cost = node_costs[prev_node] + transition_cost;
if (total_cost < min_cost) {
min_cost = total_cost;
back_pointer[curr_node] = prev_node;
}
}
node_costs[curr_node] = min_cost;
}
}
3.2 五次多项式拟合
连接两个节点的轨迹采用五次多项式：
code复制l(s) = a0 + a1·s + a2·s² + a3·s³ + a4·s⁴ + a5·s⁵
需要满足边界条件：
- 起点/终点的l, dl/ds, d²l/ds²
- 最大曲率约束（通常设为0.1 rad/m）
我们使用Eigen库求解线性方程组：
cpp复制Eigen::MatrixXd A(6, 6);
Eigen::VectorXd b(6);
// 填充边界条件到矩阵A和向量b
Eigen::VectorXd coeffs = A.colPivHouseholderQr().solve(b);
4. 速度规划模块关键技术
4.1 ST图障碍物投影
动态障碍物在ST图中的表示需要预测其运动轨迹。我们实现了一个基于运动学的预测器：
cpp复制class ObstacleProjector {
public:
STBoundary Project(const Obstacle& obs, const ReferenceLine& ref) {
STBoundary boundary;
for (double t = 0; t < total_time_; t += dt_) {
CartesianPoint pos = obs.PredictPosition(t);
FrenetPoint frenet = ref.CartesianToFrenet(pos);
boundary.AddPoint(t, frenet.s - obs_length_/2, frenet.s + obs_length_/2);
}
return boundary;
}
};
4.2 速度优化代价函数
速度规划的代价函数包含三个关键项：
code复制cost = w_v·(v - v_ref)² + w_a·a² + w_j·jerk²
其中jerk（加加速度）项对乘坐舒适性影响很大。实测表明，将w_j设为0.5以上能显著减少急加速/急减速情况。
5. 可视化与调试技巧
5.1 Qt可视化工具开发
我们基于QCustomPlot库实现了交互式可视化：
cpp复制void PlanningVisualizer::UpdateSLGraph(const DPPathGraph& graph) {
// 绘制障碍物
for (const auto& obs : graph.obstacles()) {
QCPItemRect* rect = new QCPItemRect(plot_);
rect->setPen(QPen(Qt::red));
rect->setBrush(QBrush(QColor(255,0,0,50)));
// 设置障碍物位置...
}
// 绘制DP节点
for (int level = 0; level < graph.levels(); ++level) {
for (const auto& node : graph.nodes(level)) {
QCPItemEllipse* ellipse = new QCPItemEllipse(plot_);
ellipse->setPen(QPen(Qt::blue));
// 设置节点位置...
}
}
}
5.2 MATLAB仿真验证
我们开发了自动化测试脚本，可以批量运行不同场景：
matlab复制scenarios = {
struct('name', 'straight_road', 'obstacles', [...]),
struct('name', 'curve_road', 'obstacles', [...])
};
for i = 1:length(scenarios)
[path, speed] = RunPlanning(scenarios{i});
AnalyzeResults(path, speed);
SaveAnimation(sprintf('result_%d.gif', i));
end
6. 性能优化实践
6.1 计算热点分析
使用perf工具分析发现，90%的计算时间消耗在：
- 五次多项式系数求解（40%）
- 障碍物碰撞检测（30%）
- 坐标转换（20%）
6.2 关键优化措施
针对上述热点，我们实施了以下优化：
- 多项式求解优化：预计算矩阵分解
cpp复制Eigen::PartialPivLU<Eigen::MatrixXd> lu_decomp(A);
for (auto& node_pair : node_pairs) {
coeffs = lu_decomp.solve(b); // 重用分解结果
}
- 碰撞检测加速：使用轴对齐包围盒（AABB）树
- 坐标转换缓存：对频繁访问的s值缓存插值结果
优化后性能提升：
- 路径规划：从120ms → 65ms
- 速度规划：从80ms → 45ms
7. 典型问题排查指南
7.1 轨迹抖动问题
现象：规划出的轨迹出现高频振荡
排查步骤：
- 检查参考线曲率连续性（G2约束）
- 验证五次多项式边界条件是否严格满足
- 检查代价函数权重配置，适当增加加速度项权重
7.2 规划超时问题
现象：某些场景下规划耗时超过200ms
解决方案：
- 动态调整采样密度：当检测到复杂场景时自动增加采样点
cpp复制int num_samples = base_samples_;
if (complex_scene_detected) {
num_samples = std::min(max_samples_, num_samples * 2);
}
- 实现早期终止机制：当累计代价超过阈值时提前终止搜索
8. 参数调优经验
8.1 横向规划参数
关键参数建议值：
| 参数 | 建议值 | 影响 |
|---|---|---|
| sample_ds | 5.0m | 纵向采样间隔 |
| max_l_num | 7 | 横向采样点数 |
| l_sample_width | 0.5m | 横向采样间隔 |
| w_obs | 10.0 | 障碍物代价权重 |
8.2 速度规划参数
典型配置组合：
yaml复制speed_planning:
max_acc: 2.0 # m/s²
min_acc: -3.0 # m/s²
max_speed: 25.0 # m/s
dt: 0.5 # 时间分辨率
w_speed: 1.0
w_acc: 0.3
w_jerk: 0.5
9. 扩展与改进方向
在实际项目应用中，我们发现以下几个有价值的改进点：
- 混合A*与DP的结合：在复杂场景下，先用A*进行粗搜索，再用DP进行精细优化
- 学习式代价函数：通过收集人类驾驶数据，训练神经网络预测各代价项的权重
- 多车协同规划：引入V2X通信，实现车群间的协同决策
10. 工程实践建议
经过多个项目的验证，总结出以下实践经验：
- 模块化测试：确保每个模块（参考线、路径规划、速度规划）可独立测试
- 可视化优先：在早期就建立可视化工具，大幅降低调试难度
- 参数可配置化：所有算法参数应通过配置文件管理，避免硬编码
- 性能监控：内置实时性能统计，及时发现计算热点
这套系统已在多个高校的自动驾驶课程和科研项目中得到应用，其清晰的架构设计和完整的工具链，特别适合作为算法开发的基础平台。对于希望深入理解自动驾驶规划算法的开发者，建议从修改代价函数开始，逐步尝试不同的规划策略，最终实现自己的创新算法。