自动驾驶控制-斯坦利stanely算法C++纯代码实现 代码注释完整，可以自己看明白，也可以付费提供代码和算法原理讲解服务。 通过C++程序实现的路径跟踪仿真，可视化绘图需要安装matplotlibcpp库，已经提前安装好包含在头文件，文件内也含有安装教程。 可以自定义路径进行跟踪，只需有路径的X Y坐标即可，替换下图中框框标出来的地方路径就可以了。 图片是双移线和一些自定义的路线仿真效果。

先搞个底盘再说话。咱们用C++手撕斯坦利算法，先看效果：车能跟着双移线轨迹蛇皮走位，误差控制在0.1米内。这算法的精髓在于——既要抬头看路（航向角匹配）又要脚下不飘（横向误差修正）。

先丢个车辆状态结构体镇楼：

struct State {
    double x;        // 车屁股中点X
    double y;        // 车屁股中点Y
    double yaw;      // 车头朝向（弧度）
    double velocity; // 当前车速
    double steer;    // 前轮转角
    double k;        // 前视增益系数
};

这结构体存着车辆六维状态，注意坐标系得统一用右手系。重点在k参数，这货控制着预瞄距离，后边调参时能玩出花来。

路径处理用了个骚操作——最近点投影。看这段暴力搜索：

int target_idx = 0;
double min_dist = std::numeric_limits<double>::max();
for (int i = 0; i < cx.size(); ++i) {
    double dx = state.x - cx[i];
    double dy = state.y - cy[i];
    double dist = dx*dx + dy*dy; // 平方距离省sqrt运算
    if (dist < min_dist) {
        min_dist = dist;
        target_idx = i;
    }
}

别嫌这方法low，实测在10万点路径下都能实时跑。找到最近点后，计算航向误差和横向误差：

double heading_err = normalize_angle(course[target_idx] - state.yaw);
double cross_err = calc_cross_err(state, cx, cy, target_idx);

航向误差得用[-π, π]规整，否则转向会抽风。横向误差计算注意车体坐标系转换：

double dx = cx[target_idx] - state.x;
double dy = cy[target_idx] - state.y;
double vec_angle = atan2(dy, dx);
double err_angle = normalize_angle(vec_angle - course[target_idx]);
cross_err = -dx * sin(course[target_idx]) + dy * cos(course[target_idx]);

这里负号不是手滑，是坐标转换必须的骚操作。

自动驾驶控制-斯坦利stanely算法C++纯代码实现 代码注释完整，可以自己看明白，也可以付费提供代码和算法原理讲解服务。 通过C++程序实现的路径跟踪仿真，可视化绘图需要安装matplotlibcpp库，已经提前安装好包含在头文件，文件内也含有安装教程。 可以自定义路径进行跟踪，只需有路径的X Y坐标即可，替换下图中框框标出来的地方路径就可以了。 图片是双移线和一些自定义的路线仿真效果。

核心控制算法登场：

double stanley_control(State &state, const vector<double>& cx, 
                      const vector<double>& cy, const vector<double>& course) {
    // 找最近点代码略...
    
    // 航向修正项
    double heading_correction = heading_err;
    
    // 横向误差修正项
    double cross_correction = atan2(state.k * cross_err, state.velocity + 1e-5);
    
    // 前轮转角 = 航向修正 + 横向修正 
    double delta = heading_correction + cross_correction;
    
    return delta;
}

注意分母加了个1e-5防零除。这算法妙在速度越高，横向修正越柔和，避免高速画龙。

调参玄学时刻：k值建议从0.3开始试。车速20km/h以下时，k=0.5能让车贴地飞行；高速场景k=0.1更稳。实测参数组合：

// 不同速度下的k值表
map<double, double> k_table = {
    {5.0, 0.8},   // 低速大增益
    {20.0, 0.3},  // 中速
    {60.0, 0.1}   // 高速
};

动态调k能让车在弯道和直道切换更顺滑。

最后上主循环：

while (sim_time < max_time) {
    State state = move(state, delta); // 车辆运动学模型
    delta = stanley_control(state, cx, cy, course);
    plot_path(cx, cy, state.x, state.y); // matplotlibcpp绘图
}

运动学模型建议用自行车模型，注意前后轴距离参数别设反了。

效果展示部分，用matplotlibcpp画轨迹对比：

plt::plot(cx, cy, "r--"); // 参考路径红色虚线
plt::plot(vehicle_x, vehicle_y, "b-"); // 车辆轨迹蓝色实线
plt::axis("equal");
plt::pause(0.001);

路径替换巨简单，改cx,cy数组就行。比如画个正弦路径：

vector<double> cx, cy;
for (double t=0; t<20; t+=0.1){
    cx.push_back(t);
    cy.push_back(2*sin(t/2.0));
}

调参时若发现车在弯道切内线，把k值调大0.1；若车在直道晃，把k值降0.05。这算法在砂石路面上仿真效果比纯PID强两档，不信跑个急弯对比下横向加速度曲线就知道。