C++自动驾驶控制算法解析：Pure Pursuit、Stanley与MPC调参实践

简介这是一套面向自动驾驶控制方向学习者与开发者的C源码合集集成PID、Pure pursuit、Stanley、MPC与LQR等常见控制算法代码完整且功能验证通过尤其适合毕业设计、课程设计及算法入门实践也可作为相关课程教学与企业项目演示的参考。资源共82个文件压缩包大小仅2.37MB以cpp/h源码文件为主配合png示意图、md说明文档、json配置、xml工程配置及zbak备份文件并包含catkin_workspace环境标记便于在ROS工作区中组织与编译目前已有67人学习/下载。压缩包内提供基于C的工程目录与编译脚本按Purepursuit、MPC、Stanley、LQR、CANBUS等模块划分可对照说明文档与示意图快速定位算法实现附带的QP_Smooth_referenceline参考线平滑模块和附赠内容为路径跟踪、约束优化等进阶调试提供可直接运行的参考。使用时注意将解压后的项目置于英文路径下可避免编译解析错误。 拿到这份 C 控制算法包的第一天我先翻的不是 MPC而是 Pure pursuit 和 Stanley 两个目录——同一工程里同时放两套几何追踪算法说明作者很清楚没有一种算法能通吃所有工况。这份源码适合做毕业设计、课程设计的学生也适合刚转自动驾驶控制方向的工程师。解压后 src 下是独立的 Purepursuit、Stanley、MPC、LQR、CANBUS 模块外加 QP_Smooth_referenceline 参考线平滑大致串成一条横向加纵向控制链路。真正要消化的不是代码本身而是每种算法的适用边界和调参手感。2. Pure pursuit 与 Stanley几何追踪在弯道上的取舍2.1 前视距离Pure pursuit 的弯道性能边界Pure pursuit 把车辆简化为自行车模型核心假设是车辆能沿一段圆弧到达参考线前方的预瞄点。这个假设成立的前提下预瞄距离 ld 和路径曲率之间的关系直接决定弯道表现ld 偏大车辆切弯明显过弯半径被拉大车道内侧偏移持续存在ld 偏小转向对路径噪声过度敏感直线段也会出现高频摆动。工程上普遍把 ld 拆成两项一项正比于车速一项是基础偏移这项基础偏移就是代码里 lookahead_distance 参数存在的意义。低速园区场景基础偏移取 1.0~1.5 米就够高速道路场景要按车速的 0.3~0.5 倍叠加。同一个工程里同时保留 Purepursuit 和 Stanley一个重要原因是这两个模块在低速和高速区间的表现差异明显用速度条件切换比单算法硬扛更实际。最近点查找的索引推进参考线通常按 0.5~1.0 米间距离散成路点而控制周期是 20~50ms。每个周期都从零开始全表扫描路点找最近邻在几千个路点里做欧氏距离遍历性能和实时性都不够稳。这个包里的 Purepursuit 模块用了一个很通用的优化以上一周期最近点索引为起点向后搜索。// src/Purepursuit/pure_pursuit.cpp int findNearestIndex(double x, double y, const std::vectorPathPoint ref_path, int last_nearest_idx) { int nearest_idx last_nearest_idx; double min_dist std::numeric_limitsdouble::max(); // 从上次索引往前回退 5 个点开始向后扫覆盖车辆前进场景 int search_start std::max(0, last_nearest_idx - 5); for (int i search_start; i ref_path.size(); i) { double dx ref_path[i].x - x; double dy ref_path[i].y - y; double d dx * dx dy * dy; // 用平方距离比较省一次 sqrt if (d min_dist) { min_dist d; nearest_idx i; } } return nearest_idx; }这里 search_start 回退 5 个点是按高速工况给的经验窗口因为相邻两个控制周期车辆沿参考线前进的距离通常不会超过几个路点间距。低速大转角或倒车场景这个窗口可能不够最近点索引会跳变我的做法是把 5 改成 10并加一个“若新的最近点索引小于上一周期索引则全表扫描兜底”的条件。2.2 Pure pursuit 转角计算从圆弧几何到代码找到预瞄点后Pure pursuit 的转角计算只有几行// src/Purepursuit/pure_pursuit.cpp double PurePursuit::calcTargetSteer(const VehicleState state, const PathPoint target) { double dx target.x - state.x; double dy target.y - state.y; double alpha atan2(dy, dx) - state.yaw; // 车辆航向与目标方向的夹角 // 圆弧几何推导结果前轮转角正比于 2 * 轴距 * sin(alpha) / 预瞄距离 double delta atan2(2.0 * wheelbase_ * sin(alpha), lookahead_dist_); return clampAngle(delta); }alpha 是预瞄点在车辆坐标系下的方向角与当前航向的差sin(alpha) 的符号决定左转还是右转。公式本身来自车辆沿圆弧到达预瞄点的几何约束C 实现里最容易出错的是角度单位。路径点的 yaw 如果是从 GNSS/IMU 组合导航拿的弧度制atan2 结果也是弧度直接相减没问题但有的传感器厂商会给角度制航向混用时一个 0.5 弧度的名义转角会被放大 57 倍车会直接画龙。我在调试时会在 return 前打印 alpha 与 delta 两个值确认量级和符号连续。2.3 Stanley 的合成转角与低速抗抖处理Stanley 不追踪前方的预瞄点而是找参考线上的最近投影点把航向误差和前轴中心到投影点的横向误差合成为一个转角。它的控制律更直接直线上比 Pure pursuit 稳定但在连续 S 弯上因为缺少前视投影点切换时横向误差跳变会引发振荡。// src/Stanley/stanley.cpp double Stanley::calcTargetSteer(const VehicleState state, const PathPoint nearest_point) { double heading_error normalizeAngle(nearest_point.yaw - state.yaw); double e_y calcCrossTrackError(state, nearest_point); double v std::max(state.v, 0.1); // 车速下限保护防止低速分母爆炸 double delta heading_error atan2(k_ * e_y, v); return clampAngle(delta); }k_ 是横向误差增益atan2 把横向误差和速度的比值映射到角度。v 接近 0 时如果不做下限保护atan2(k_ * e_y, v) 会趋向正负 90 度前轮直接打死。我一般会把 k_ 拆成低速挡和高速挡两套值低速用 2.0 左右保证停车入库能追上参考线高速降到 1.0 附近防止蛇行。对比维度Pure pursuitStanley核心误差量预瞄点方向角误差航向误差 前轴横向误差参考线平滑度要求敏感预瞄点跳变直接引起转角突变相对不敏感低速大曲率表现需调小 ld否则明显切弯天然好用但 e_y 突变时抖动高速稳定性加大 ld 可抑制噪声必须减小 k_否则车道内振荡典型场景高速换道、缓曲率公路低速园区、泊车对比维度Pure pursuitStanley核心误差量预瞄点方向角误差航向误差 前轴横向误差参考线平滑度要求敏感预瞄点跳变直接引起转角突变相对不敏感低速大曲率表现需调小 ld否则明显切弯天然好用但 e_y 突变时抖动高速稳定性加大 ld 可抑制噪声必须减小 k_否则蛇行典型场景高速换道、缓曲率公路低速园区、泊车这套对比不是我臆想的而是把两个模块在相同参考线下用固定速度跑出来的效果。实际做切换时不要生硬地按某个车速阈值直接跳变而应该做一段速度区间内的权重过渡比如 6~9 m/s 之间按车速线性混合两个模块的输出转角。3. PID 与 LQR纵向速度控制从偏差调节到状态反馈3.1 增量式 PID 为什么比位置式更适合油门/刹车切换纵向控制里被控对象是车速执行器是油门和刹车。位置式 PID 直接输出油门开度绝对值车速接近目标时如果误差波动输出会有明显跳变车上乘员能感知到一顿一顿的冲击。增量式 PID 输出的是本次控制量相对上次的变化量天然带缓冲作用这也是为什么源码包里纵向控制器用的是增量式实现。// src/LQR/pid_controller.cpp double IncrementalPID::calculate(double target, double actual, double dt) { double err target - actual; double out kp_ * (err - err_last_) ki_ * err * dt kd_ * ((err - 2.0 * err_last_ err_last2_) / dt); err_last2_ err_last_; err_last_ err; return out; // 返回值是控制量的增量不是绝对值 }误差差分项对应位置式 PID 里的比例作用误差本身对应积分作用三帧差分除以 dt 是微分作用。这样输出叠加到上次油门刹车命令上后即使目标速度突变执行器变化也是逐步逼近的。工程上要留意 dt 不是固定 20ms线程调度抖动会造成微分项异常放大我的做法是每次计算前对 dt 做 10ms~50ms 的限幅。增量式和位置式在整车控制器里还有一个重要差异增量式天然支持故障保护。通讯中断时执行器保持上一帧值不会因为 PID 输出突然跳回 0 或冲到限幅值。对实车调试来说这是安全兜底。3.2 PID 参数整定从临界比例法到经验修正拿到这份代码包后不建议直接猜参数。先做一步开环测试手动给固定的油门/刹车命令记录车速从 0 加速到 10m/s 的阶跃响应曲线量出纯滞后时间和最大斜率再按工程上常用的经验公式初定 kp、Ti、Td。场景kpTi积分时间Td微分时间匀速巡航5~150.5~1.0s0.05~0.1s起步/停车10~200.2~0.5s0.1~0.2s换道加速3~81.0~2.0s0.02~0.05s注意这里 Ti、Td 和代码里 ki、kd 的换算关系是 ki kp / Tikd kp * Td。从 Simulink 转 C 时容易把 Ti 直接当 ki 填积分作用会被放大几倍车速会在目标值附近低频振荡听声音能明显感觉到油门周期性松紧。出现这种情况优先查单位换算而不是急着降低 kp。3.3 LQR 状态反馈矩阵组装与黎卡提方程求解LQR 和 PID 的差异在于它是模型驱动的状态反馈方法。状态向量里同时包含纵向误差、误差积分、横向误差、航向误差代价函数 J xᵀQx uᵀRu 里 Q 惩罚状态偏差R 惩罚控制量。要算出最优反馈增益 K核心是求解代数黎卡提方程。// src/LQR/lqr_controller.cpp void LQRController::solveRiccatiIterative(const Eigen::MatrixXd A, const Eigen::MatrixXd B, const Eigen::MatrixXd Q, const Eigen::MatrixXd R, Eigen::MatrixXd* P) { Eigen::MatrixXd P_prev Q; for (int i 0; i 200; i) { Eigen::MatrixXd tmp (R B.transpose() * P_prev * B).inverse(); Eigen::MatrixXd P_next A.transpose() * P_prev * A - A.transpose() * P_prev * B * tmp * B.transpose() * P_prev * A Q; if ((P_next - P_prev).norm() 1e-6) { // 收敛判定 *P P_next; return; } P_prev P_next; } *P P_prev; }200 次迭代对 4 维状态矩阵每周期求解一般能在微秒级完成但实际项目里更常见的做法是在参数切换时离线求解一次再把 K 矩阵查表使用。这个代码里最容易踩的坑是 Q 矩阵各单位不一致横向误差单位是米航向误差单位是弧度直接设同样权重时响应速度会被数值量级更小的那个状态主导。做毕设时可能看不出问题实车场景就会表现为航向修正有力、横向纠偏迟钝。我的做法是先做归一化把横向误差按车道宽度 3.5m 除一下航向误差按最大前轮转角 0.3rad 除一下再乘权重系数进 Q。3.4 Q 与 R 的物理意义和标定顺序Q 矩阵对角线元素越大的状态量被约束得越紧R 越大控制量变化越平缓。对纵向速度控制来说如果只关心车速收敛把速度误差对应的 Q 项设成其他项的 3~5 倍即可如果同时关注舒适性就要把加速度相关的 R 项提高到让急加速被压制。我把这样的标定顺序当作默认路径先固定 R 为单位矩阵Q 全设 1跑一段匀速工况记录响应随后只调速度误差的 Q 项观察超调量变化最后调 R 看控制量波动。这套顺序避免 Q 和 R 同时动导致定位不准。LQR 适合作为高速巡航的主控但在大曲率紧急避障场景里它没有显式的约束处理能力前轮转角限幅只能靠输出饱和裁剪这时就该切到下一章的方案。4. MPC 模型预测控制约束滚动优化与 QP 求解4.1 预测模型与代价函数构造MPC 比 LQR 多出来的核心能力是显式约束。它在每个控制周期内求解一个有限时域最优控制问题给定当前状态预测未来 N 步的系统状态在满足控制量和控制增量约束的前提下最小化与参考轨迹的偏差。预测模型可以是非线性单车模型也可以是线性化后的误差模型代码包里选择的是后者把纵向速度、横向偏差、航向偏差组成线性状态方程显著降低求解负担。// src/MPC/mpc_controller.cpp // 预测状态 X 由初始状态 x0 和控制序列 U 线性表示 // X A_d * x0 B_d * U // 代价函数 J (X - Xref)^T * Q_diag * (X - Xref) U^T * R_diag * U // 展开成关于 U 的二次型J 0.5 * U^T * H * U g^T * U const void MPCController::formulateCost(const Eigen::VectorXd x0, const Eigen::VectorXd x_ref_stack, Eigen::SparseMatrixdouble* H, Eigen::VectorXd* g) { // H B_d^T * Q_diag * B_d R_diag这里用稀疏矩阵 // g 由 x_ref_stack 与 A_d*x0 的偏差项组合而来 }H 矩阵规模随预测时域 N 平方增长所以必须用稀疏矩阵和稀疏求解器。常见的做法是接 OSQP 这类开源 QP 求解器也可以用带约束的梯度下降自己做求解器但收敛速度和数值稳定性需要额外验证。源码包里如果没有默认求解器通常会看到用户自己在 CMakeLists 里链接第三方库的痕迹。4.2 约束进入 QP 的工程处理MPC 的约束一般分三类控制量限幅、控制增量限幅、状态量限幅。前轮转角限幅是控制量约束对应执行器物理极限控制增量约束对应转向电机响应速度状态量限幅可以用来防止横向偏差超出车道边界。做碰撞避免的完整场景还会加入障碍物不等式约束但那属于轨迹规划范畴源码包里没把障碍物约束和控制耦合这是一个合理的模块切分。参数参考范围对控制表现的影响预测时域 N15~30N 越大越有前瞻性但计算量增长且对模型误差更敏感控制时域 M3~8M 太小提前松弛M 太大控制量高频波动Q状态权重对角取值 1~10越大轨迹追踪越紧但对噪声越敏感R控制权重0.1~1越大控制越平缓响应变慢调参遇到低频振荡时优先把 Q 中速度误差权重降下来高频抖动用增大 R 中控制增量惩罚来解决。这两类参数对应的是两个不同矩阵位置不要混在一起调。4.3 参考线平滑与 QP_Smooth_referenceline 的角色包里的 QP_Smooth_referenceline 是经常被忽视但很关键的模块。MPC 的代价函数把参考轨迹当作理想目标如果参考线本身有折点或高频毛刺跟踪误差无法收敛到零控制量还会被折点持续激励出现周期性抖动。参考线平滑的实质是另一个小型二次规划目标函数是平滑后点列与原始点列偏差的平方和加上相邻点差分的光滑性惩罚。这个平滑模块既可以离线生成路径时用也可以在每个控制周期对局部窗口在线执行。处理这类问题时要记住控制算法表现不稳定先怀疑参考线质量再怀疑控制器参数。5. CANBUS 对接、Vofa 曲线与调参路径验证5.1 CANBUS 回灌与仿真对接源码里的 CANBUS 目录承担的是控制命令下发和车辆状态回读。调测时我建议先回灌固定半径圆形参考路径而不是直接上完整地图。以恒定速度跑一圈记录参考航向和实际航向的差值能快速判断横向控制是发散还是存在稳态误差。CANBUS 的 DBC 解析要重点确认信号字节序和缩放系数车速信号常见的坑是低位在前还是高位在前定义不一致导致控制量方向正确但数值差 256 倍。5.2 Vofa 查看误差收敛曲线Vofa 是调试控制算法常用的上位机工具支持 CSV 格式推流。我在 C 控制器里加一个调试回调把时间戳、目标速度、实际速度、前轮转角、横向误差按固定周期打包输出Vofa 里直接画曲线能直观看到 PID 过冲、Stanley 低速抖动这类问题。// src/CANBUS/debug_publisher.cpp void publishDebug(const VehicleState s, double delta, double e_y) { // 格式时间戳, 目标速度, 实际速度, 前轮转角, 横向误差 printf(%.3f,%.2f,%.2f,%.3f,%.3f\n, s.timestamp, s.speed_ref, s.speed_act, delta, e_y); }注意控制周期到底是 20ms 还是 50msVofa 的横轴时间戳必须单调递增且带时间单位否则曲线抖动大看不出是控制问题还是采样时钟错位。我排查时还会故意发一个单位阶跃的目标速度观察控制器跟随的延迟和超调量这个数据比稳态误差更能反映参数是否合理。5.3 推荐调参顺序拿到这种包含全部算法且目录刻意独立的源码包不要直接改参数按算法复杂度从低到高验证先跑 Pure pursuit固定速度 2m/s预瞄距离从 1m 调到 5m观察弯道超调量变化理解 ld 的作用域。再切 Stanley同一段路k 从 0.5 调到 2.0记录横向误差收敛速度。PID 负责纵向速度稳定后再介入 LQR 做航向闭环避免两个控制器互相拉扯。MPC 最后上N 初始取 20Q 对角取 1R 取 0.1约束先放宽到极限值的 1.5 倍确认预测轨迹始终贴着参考线后再逐步收紧到执行器实际限幅。这样从头到尾验证一遍五个模块就都完成了从读到改的闭环。最后提醒一个细节解压后项目路径和文件名尽量全英文CMakeLists.txt 里通常不处理中文路径的编码问题这在 Windows 上用 vscode 打开这份源码的 c_cpp_properties.json 时很容易触发解析错误。还有如果 CMakeLists 用文件通配方式收集源文件新增 .cpp 后需要重新执行 cmake 配置否则新文件不会被编译进工程。本文还有配套的精品资源点击获取