自动驾驶控制-斯坦利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_limitsdouble::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 vectordouble cx, const vectordouble cy, const vectordouble 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以下时k0.5能让车贴地飞行高速场景k0.1更稳。实测参数组合// 不同速度下的k值表 mapdouble, 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数组就行。比如画个正弦路径vectordouble cx, cy; for (double t0; t20; t0.1){ cx.push_back(t); cy.push_back(2*sin(t/2.0)); }调参时若发现车在弯道切内线把k值调大0.1若车在直道晃把k值降0.05。这算法在砂石路面上仿真效果比纯PID强两档不信跑个急弯对比下横向加速度曲线就知道。