自动驾驶路径规划算法（EMplanner cpp版) 动态规划+二次规划算法 精简EMplanner的cpp实现 实现静态障碍物避障 注意不包括速度规划 带有注释，逻辑清晰 方便初学者入门，小白友好 需配置第三方库: Eigen, OSQP，需安装python2.7（画图使用，cpp调用matplotlibcpp文件实现画图功能）

刚摸到自动驾驶路径规划的时候，总觉得那些算法像黑盒子。今天咱们直接撸代码，用C++实现一个简化版的EMPlanner，手把手看它怎么绕开静态障碍物。这里只用路径规划，暂时不碰速度部分，咱先把路线画明白。

一、动态规划探路

动态规划（DP）在这里的作用是生成粗路径。想象你在迷宫里扔出一把石子，看哪条路能绕开障碍物，我们就用类似的思路：

// DP路径结构体
struct DpPath {
    std::vector<double> s;    // 纵向距离
    std::vector<double> l;    // 横向偏移
    double total_cost = 0.0;  // 路径总成本
};

// 障碍物信息
struct Obstacle {
    double s;  // 纵向位置
    double l;  // 横向位置 
    double radius; // 膨胀半径
};

// DP核心计算
DpPath dynamicProgramming(const std::vector<Obstacle>& obstacles) {
    const int N = 50; // 路径点数量
    const double delta_s = 1.0; // 纵向间隔
    const double delta_l = 0.5; // 横向步长
    
    std::vector<std::vector<double>> cost(N, std::vector<double>(5, INFINITY));
    cost[0][2] = 0.0; // 起点居中
    
    // 状态转移
    for (int i = 0; i < N-1; ++i) {
        for (int dl : {-2, -1, 0, 1, 2}) { // 横向变化量
            double current_l = dl * delta_l;
            
            // 障碍物碰撞检测
            bool collision = false;
            for (const auto& obs : obstacles) {
                if (fabs(i*delta_s - obs.s) < 3.0 && 
                    fabs(current_l - obs.l) < obs.radius) {
                    collision = true;
                    break;
                }
            }
            if (collision) continue;
            
            // 计算转移成本
            for (int next_dl : {-1, 0, 1}) { // 限制横向变化率
                double next_l = current_l + next_dl * delta_l;
                double new_cost = cost[i][dl+2] + 
                                 fabs(next_dl)*0.3 +  // 横向变化惩罚
                                 fabs(next_l)*0.1;    // 偏离中心惩罚
                
                if (new_cost < cost[i+1][next_dl+2]) {
                    cost[i+1][next_dl+2] = new_cost;
                }
            }
        }
    }
    // 回溯路径...（具体实现略）
}

这里横向变化限制为±1步长，避免急打方向。障碍物检测用了膨胀半径，相当于给障碍物加了个安全距离。实际工程中会考虑更多代价项，比如靠近车道线的惩罚。

二、二次规划精修

拿到DP的粗路径后，二次规划（QP）负责把它变成平滑可用的轨迹：

#include <osqp/osqp.h>

void quadraticProgramming(const DpPath& dp_path) {
    // 构建目标函数: min 0.5*x'Px + q'x
    Eigen::MatrixXd P = Eigen::MatrixXd::Identity(3,3) * 0.1;
    Eigen::VectorXd q = Eigen::VectorXd::Zero(3);
    
    // 约束条件 Ax = b
    Eigen::MatrixXd A(2,3); 
    A << 1, dp_path.s[0], dp_path.s[0]*dp_path.s[0],
         1, dp_path.s.back(), dp_path.s.back()*dp_path.s.back();
    
    Eigen::VectorXd b(2);
    b << dp_path.l[0], dp_path.l.back(); // 保持起点终点位置
    
    // 转换为OSQP格式
    OSQPData *data = (OSQPData*)malloc(sizeof(OSQPData));
    data->n = 3; // 变量数
    data->m = 2; // 约束数
    // ...矩阵填充细节
    
    OSQPSettings *settings = (OSQPSettings *)malloc(sizeof(OSQPSettings));
    osqp_set_default_settings(settings);
    
    OSQPWorkspace *work = osqp_setup(data, settings);
    osqp_solve(work);
    
    // 提取结果
    double a = work->solution->x[0];
    double b = work->solution->x[1];
    double c = work->solution->x[2];
    // 生成平滑路径...
}

这里用二次多项式拟合路径，保持起点终点位置不变。实际应用会增加曲率约束等条件，防止轨迹曲率过大。

三、障碍物怎么绕

静态障碍物处理主要在DP阶段：

// 在DP代价计算中加入障碍物距离项
double obstacle_cost = 0.0;
for (const auto& obs : obstacles) {
    double dist = sqrt(pow(s - obs.s, 2) + pow(l - obs.l, 2));
    if (dist < obs.radius) {
        obstacle_cost += 1000.0; // 碰撞惩罚
    } else {
        obstacle_cost += 1.0 / (dist - obs.radius); // 距离越近代价越高
    }
}

这种处理让路径自然远离障碍物，同时保持安全距离。实际项目会采用S-T图等方式处理动态障碍物，这里只处理静态情况。

四、画图配置

用matplotlibcpp做可视化（需要安装python2.7）：

#include "matplotlibcpp.h"
namespace plt = matplotlibcpp;

void plotPath(const DpPath& dp_path, const QpPath& qp_path) {
    std::vector<double> obs_s, obs_l;
    // 填充障碍物坐标...
    
    plt::figure_size(800, 600);
    plt::plot(dp_path.s, dp_path.l, "r--", {{"label", "DP Path"}});
    plt::plot(qp_path.s, qp_path.l, "b-", {{"label", "QP Path"}});
    plt::scatter(obs_s, obs_l, 50, {{"color", "g"}, {"label", "Obstacles"}});
    plt::xlabel("S (m)");
    plt::ylabel("L (m)");
    plt::legend();
    plt::show();
}

这个可视化能直观看到DP的探索过程和QP的平滑效果，适合调试参数。

五、实战要点

1. Eigen矩阵操作注意内存对齐，建议使用Eigen::Map转换原生数组
2. OSQP的稀疏矩阵填充比较麻烦，建议先在小规模问题上测试
3. 横向离散化步长建议0.3-0.5米，太细会大幅增加计算量
4. 障碍物膨胀半径至少设为车宽的一半+安全余量

完整代码需要约800行，这里展示核心逻辑。建议从单障碍物场景开始调试，观察代价函数各分量的影响。路径规划不是数学最优解，而是安全性和舒适性的平衡艺术。