简介这是基于C实现MPC算法控制车辆运动轨迹的完整工程资源适合学习机器人控制、自动驾驶或智能车路径跟踪的学生与开发者也可用于毕业设计、课程作业或工程实训。项目代码结构清晰可直接编译运行包含CMake构建脚本、MPC求解器、车辆运动学模型和Eigen数值计算依赖可帮助读者从零搭建可复现的轨迹跟踪控制流程。压缩包共1650个文件以714个cpp源文件、469个h头文件为主体另有txt说明、CMake配置、脚本及文档包体仅5.81MB轻量但内容完整。已有732人浏览学习其核心价值在于将理论MPC算法真正落地为C可执行实现读者可依据源码逐步理解预测模型构建、优化求解过程与反馈控制闭环并结合实验场景验证算法效果对深入掌握现代控制方法具有实际参考意义。1. 为什么车辆轨迹控制要选 C 与 MPC 这个组合低速园区配送车进出库、过窄弯的时候纯跟踪算法容易卡在转向角约束上速度越低横向刚度越弱前馈比例越难调一旦把最大转向角写进逻辑车身姿态又开始抖。MPC也就是模型预测控制处理这类问题的思路完全不同——它把参考路径、车辆运动学模型、控制量上下限放进同一个优化问题每个控制周期求出一段未来控制序列但只执行第一步。约束不再是试出来的限幅而是显式进入了目标函数。C 出现在这里的理由很直接。底盘控制器或路径跟踪节点通常跑在 Linux 工控机上采样周期要到 20 ms 到 100 ms 量级同一周期内还要完成状态读取、模型更新和优化求解。C 的确定性与低内存开销比 Python 更适合这种实时性要求。下面按建模、实现、调参到工程化的顺序把基于 C 的 MPC 车辆轨迹控制完整走一遍。2. 建模把车辆运动学变成 C 可离散的线性状态方程2.1 车辆运动学模型与状态量设定做轨迹跟踪时横向控制最常用的模型是自行车模型。它把前后轮各等效成一个轮子忽略轮胎侧偏角认为车辆绕瞬时转动中心做刚体运动。状态量一般取四个大地坐标系下的位置 x、y航向角 theta纵向速度 v。控制量取加速度 a 和前轮转角 delta。连续时间状态方程如下dx/dt v * cos(theta) dy/dt v * sin(theta) dtheta/dt v * tan(delta) / L dv/dt aL 是轴距。这个模型的优点是参数少、计算快适合作为 MPC 预测模型代价是低速或大转角时精度有限不能表达轮胎力饱和。对轨迹跟踪场景尤其是园区低速车它已经是工程上常用的第一版模型。符号含义单位代码变量x, y车辆在大地坐标系下的位置mstate[0], state[1]theta航向角radstate[2]v纵向车速m/sstate[3]a纵向加速度m/s^2u[0]delta前轮转角radu[1]L轴距mwheelbase2.2 在参考轨迹上做线性化得到偏差模型MPC 的预测模型最好是线性的这样每个周期求解的是二次规划耗时可控。轨迹跟踪一般不是控制绝对坐标而是控制相对参考轨迹的偏差。设参考状态为 x_r, y_r, theta_r参考速度为 v_r参考转角为 delta_r。定义状态偏差x_tilde [x - x_r; y - y_r; theta - theta_r; v - v_r]在这个参考点上做一阶泰勒展开得到线性偏差模型d(x_tilde)/dt A_c * x_tilde B_c * u_tilde。A_c 是 4x4 矩阵B_c 是 4x2 矩阵A_c [0, 0, -v_r * sin(theta_r), cos(theta_r)] [0, 0, v_r * cos(theta_r), sin(theta_r)] [0, 0, 0, tan(delta_r) / L] [0, 0, 0, 0] B_c [0, 0] [0, 0] [0, v_r / (L * cos(delta_r)^2)] [1, 0]这里第一列对应加速度 a第二列对应前轮转角 delta。可以看到航向角偏差对位置的影响通过 -v_r * sin(theta_r) 和 v_r * cos(theta_r) 体现这些系数在每个周期都会随参考点变化所以线性化要每拍重算。2.3 用欧拉离散把连续状态方程转换为 A_d/B_d连续方程不能直接写进 MPC需要离散化。工程上常用一阶欧拉A_d I dt * A_cB_d dt * B_c。dt 是采样周期。下面用 Eigen 写一个离散化函数输入参考状态和控制量输出 A_d 和 B_d#include Eigen/Dense #include cmath struct KinematicModel { double L{2.8}; // 轴距单位 m double dt{0.1}; // 采样周期单位 s void buildDiscreteModel( const Eigen::Vector4d xr, // 参考状态 [x_r, y_r, theta_r, v_r] double delta_r, // 参考转角单位 rad Eigen::Matrix4d Ad, // 输出离散状态矩阵 Eigen::Matrixdouble,4,2 Bd) const { double s std::sin(xr[2]); double c std::cos(xr[2]); double t std::tan(delta_r); double ct std::cos(delta_r); Eigen::Matrix4d Ac; Ac 0, 0, -xr[3] * s, c, 0, 0, xr[3] * c, s, 0, 0, 0, t / L, 0, 0, 0, 0; Eigen::Matrixdouble,4,2 Bc; Bc 0, 0, 0, 0, 0, xr[3] / (L * ct * ct), 1, 0; Ad Eigen::Matrix4d::Identity() Ac * dt; Bd Bc * dt; } };buildDiscreteModel里最需要注意的是第三行对 delta 的偏导v_r / (L * cos(delta_r)^2)。参考点转角 delta_r 如果是零这个系数退化为 v_r / L如果车辆在低速且转角大比如原地掉头场景这个值会明显放大控制序列容易在数值上跳变。实际调试时若第一版代码在弯道位置出现控制量震荡先检查这里有没有除以 cos^2(delta_r)。3. 实现最小可用的 C MPC 求解器3.1 预测模型与目标函数先写清楚离散化之后预测模型写成x(ki1) A_d * x(ki) B_d * u(ki)其中 i 从 0 到 N-1N 是预测时域步数。MPC 在每个控制周期求解的目标函数是J sum_{i1..N} ( x(ki) Q x(ki) u(ki-1) R u(ki-1) ) x(kN) P x(kN)前两项分别惩罚状态偏差和控制量最后一项是终端代价用来兜底预测时域末尾的残余误差。Q 是 4x4 状态权重R 是 2x2 控制权重。约束直接加在控制量上a_min a a_max delta_min delta delta_max这是 MPC 比 PID、纯跟踪强的核心能力转向角、加速度上下限是优化问题的边界条件而不是挂在控制器后面的 limiter。限幅器会在每一拍咬掉控制量导致姿态突变MPC 会在滚动优化时就把边界考虑进去得到的是可行性范围内的最优序列。3.2 不依赖第三方库的简化求解梯度下降加约束裁剪完整 MPC 求解通常通过 OSQP 等 QP 求解器完成。这里为了先把闭环跑通给一个不依赖任何第三方库的教学级求解器在预测时域内对控制序列做梯度下降每步更新后直接 clamp 到约束范围。生产代码请把它替换成 QP 求解器。#include Eigen/Dense #include algorithm struct MpcConfig { int N{10}; // 预测步数 double dt{0.1}; // 采样周期 Eigen::Matrix4d Q; Eigen::Matrix2d R; double aMin{-3.0}, aMax{2.0}; // 加速度约束m/s^2 double dMin{-0.5}, dMax{0.5}; // 前轮转角约束rad double lr{0.05}; // 梯度下降步长 int iter{30}; // 每周期迭代次数 MpcConfig() { Q Eigen::Matrix4d::Zero(); Q(0,0) 1.0; // 横向偏差权重 Q(1,1) 1.0; // 纵向偏差权重 Q(2,2) 0.1; // 航向角偏差权重 Q(3,3) 0.01; // 速度差权重 R Eigen::Matrix2d::Zero(); R(0,0) 0.01; // 加速度权重 R(1,1) 0.05; // 转角权重 } }; Eigen::VectorXd solveMpc( const Eigen::Matrix4d Ad, const Eigen::Matrixdouble,4,2 Bd, const Eigen::Vector4d x0, const MpcConfig c) { int nU 2 * c.N; Eigen::VectorXd U Eigen::VectorXd::Zero(nU); for (int it 0; it c.iter; it) { Eigen::Vector4d xk x0; for (int i 0; i c.N; i) { Eigen::Vector2d u U.segment2(2 * i); Eigen::Vector4d xn Ad * xk Bd * u; // 简化梯度对控制量的单步惩罚梯度 Eigen::Vector2d grad 2.0 * Bd.transpose() * c.Q * xn 2.0 * c.R * u; Eigen::Vector2d un u - c.lr * grad; un[0] std::clamp(un[0], c.aMin, c.aMax); un[1] std::clamp(un[1], c.dMin, c.dMax); U.segment2(2 * i) un; xk xn; } } return U; }要特别说明严格的 MPC 梯度应该通过伴随状态从最后一个预测步逐级回代因为 u(i) 会影响后面所有状态。上面的grad只取了“本步状态对控制量”的局部导数适合把流程跑通但控制效果在长时域下会打折。实际项目里这段函数内的循环应整体替换为一次 QP 求解调用接口保持一致上层逻辑不用动。参数方面lr和iter决定求解收敛程度。lr太大会在目标函数附近震荡iter太少则控制序列不收敛表现为车辆在参考轨迹两侧来回摆动。首次调试建议 lr 取 0.02 到 0.05iter 固定 30然后用第 4 章的评估指标回看。3.3 滚动执行控制只把第一个控制量交给车辆MPC 的关键动作是滚动优化而不是把 N 步控制序列全部执行。常见做法是每个采样周期只取U.segment2(0)作为这一拍的控制量车辆执行后采集新状态下一拍重新求解。Eigen::Vector2d rollingControl( const KinematicModel model, const Eigen::Vector4d xCur, // 车辆当前状态 const Eigen::Vector4d xRef, // 当前参考状态 double deltaRef, // 参考转角 const MpcConfig cfg) { Eigen::Matrix4d Ad; Eigen::Matrixdouble,4,2 Bd; model.buildDiscreteModel(xRef, deltaRef, Ad, Bd); Eigen::Vector4d xErr xCur - xRef; Eigen::VectorXd U solveMpc(Ad, Bd, xErr, cfg); return U.segment2(0); }这里传入的xErr是状态偏差MPC 基于偏差模型求解所以目标函数里的 Q 直接作用在偏差量上。deltaRef一般由参考路径上的曲率推算v_r 大、弯道半径小的地方deltaRef 是主要前馈MPC 只负责修正偏差。这样分工会比让 MPC 从零开始完全靠误差收敛更稳。4. 在 vscode 里跑通仿真并调 N、Q、R 三个参数4.1 vscode 配置 C 环境与最小仿真工程桌面调试时可以用 vscode 配 C 环境。装好 C/C 扩展确保 g 可用再用 tasks.json 调用编译命令。一个最小工程目录只需要两个文件mpc_main.cpp和上面两段代码。Eigen 只需下载头文件目录不需要编译所以 includePath 指过去即可。编译命令g -stdc17 -O2 -I/path/to/eigen mpc_main.cpp -o mpc_demo-O2对 MPC 这类循环密集代码影响很大同一个求解循环开不开优化速度差好几倍。调试时先用-O0 -g确认逻辑后再切到-O2看实时性。主循环框架for (int k 0; k 500; k) { Eigen::Vector4d xRef referenceAt(k * dt); // 参考状态 Eigen::Vector2d u rollingControl(model, xCur, xRef, 0.0, cfg); // 用真实运动学模型推进 xCur[0] xCur[3] * std::cos(xCur[2]) * dt; xCur[1] xCur[3] * std::sin(xCur[2]) * dt; xCur[2] xCur[3] * std::tan(u[1]) / model.L * dt; xCur[3] u[0] * dt; log.push_back({xRef[0], xRef[1], xCur[0], xCur[1]}); }referenceAt在真实工程里来自全局路径规划器的输出这里先用正弦曲线或折线接圆弧的轨迹替代。注意参考轨迹要提供 theta_r 和 v_r不能只给 xy 坐标否则上面的离散化矩阵会退化成不确定值。4.2 MPC 三个参数 N、Q、R 到底怎么调调参顺序建议固定为先定 N再调 R最后调 Q。N 乘以 dt 是预测距离它应该覆盖车辆从当前速度刹停所需的距离。速度 5 m/s、刹停加速度 2 m/s^2 时制动距离约 6.25 mdt0.1 时 N 至少要取 10取 15 到 20 更稳。参数影响调整方向N预测距离 N*dt太小抑制不住超调太大会让控制序列迟钝求解变慢R 对角线控制量惩罚太小输出抖动太大收敛慢误差消不了Q(0,0), Q(1,1)横向、纵向偏差惩罚先保持 1.0观察稳态误差再上调Q(2,2)航向角偏差惩罚弯道切弯严重时加大但过大会把车掰出平滑弧线调参时看两个现象。第一起步阶段控制量是否频繁上下冲如果是先加大 R(1,1)也就是惩罚前轮转角速率这比单纯限制 delta 幅度更能抑制抖振。第二弯道里是否出现稳态切弯误差也就是车始终贴着弯内侧走这说明 Q(2,2) 太小航向偏差没被有效压住。提示R 矩阵不是越大越好。R 过大会让 MPC 认为“少打方向”比“贴近轨迹”更重要结果是车在弯道里走出一条外切直线横向误差完全不收敛。每次改 R 后都要重新看 RMSE而不是只看轨迹形状。4.3 用横向误差和 RMSE 判断 MPC 是否真的在跟踪轨迹跟踪效果不能靠肉眼看曲线形状要量化。两个最直接的指标横向误差绝对值和纵向位置误差的 RMSE。下面这段代码从仿真日志里算 RMSEdouble rmse 0.0; int n 0; for (const auto e : log) { double err std::hypot(e.refX - e.curX, e.refY - e.curY); rmse err * err; n; } rmse std::sqrt(rmse / n);RMSE 只反映整体误差。还要单看横向误差的最大值它决定了车辆会不会蹭到车道边界。工程上常见的目标低速园区场景横向误差 RMSE 小于 0.1 m最大值小于 0.2 m。如果你的结果远大于这个数不要急着调 Q先打印出每一步参考点和控制量检查是不是参考轨迹的 theta_r 跳变太大比如直线接圆弧时没有做平滑过渡导致 MPC 每一拍都在追赶一个不连续的参考航向。5. 从演示到实车三个工程化技巧5.1 把 QP 求解器藏在接口后面上面的教学代码用的是梯度下降近似真要放到工程里第一件事是把求解器替换为 OSQP 或内点法实现。为了避免算法主逻辑跟着求解器绑死可以把求解过程抽象成一个接口class MpcSolver { public: virtual ~MpcSolver() default; virtual Eigen::VectorXd solve( const Eigen::Matrix4d Ad, const Eigen::Matrixdouble,4,2 Bd, const Eigen::Vector4d x0) 0; };控制器持有MpcSolver*OSQP 版本和教学版本都继承这个接口。这样车辆上层逻辑不变换求解器不影响控制代码也方便在仿真和实车之间切换。5.2 别让 C 继承隐藏坑掉你的模型多态这个接口看起来简单但 C 的覆盖和隐藏是面试常客也是工程里容易被忽略的地方。如果子类的solve参数少一个const或者把Eigen::Matrix4d写成了Eigen::MatrixXd编译器不会报错它只会安静地认为这是一个新函数把基类虚函数隐藏掉。结果就是指针调用时永远走基类版本。手动加override关键字是最直接的防线签名不匹配时编译直接失败。class OsqpSolver : public MpcSolver { public: Eigen::VectorXd solve( const Eigen::Matrix4d Ad, const Eigen::Matrixdouble,4,2 Bd, const Eigen::Vector4d x0) override; };5.3 用状态估计与延迟补偿填补模型与现实差仿真里状态直接给真值实车不行。GNSS 更新率低IMU 积分会飘常见做法是卡尔曼滤波融合把位置、速度、航向作为状态量进行估计。另外 MPC 求解本身有耗时假设单次求解花 10 ms那么在 k 时刻发出的控制量实际作用在 k1 时刻。这会导致控制滞后一拍弯道里表现为横向误差有规律地偏向一侧。先估算单拍耗时在 MPC 里用预测状态而不是当前测量值作为初值计算出的控制量就更接近车辆实际执行时的状态。这一步做完通常能再压掉 1 到 2 cm 的横向误差。本文还有配套的精品资源点击获取 C++与MPC的车辆轨迹控制：建模、求解与工程实践
简介这是基于C实现MPC算法控制车辆运动轨迹的完整工程资源适合学习机器人控制、自动驾驶或智能车路径跟踪的学生与开发者也可用于毕业设计、课程作业或工程实训。项目代码结构清晰可直接编译运行包含CMake构建脚本、MPC求解器、车辆运动学模型和Eigen数值计算依赖可帮助读者从零搭建可复现的轨迹跟踪控制流程。压缩包共1650个文件以714个cpp源文件、469个h头文件为主体另有txt说明、CMake配置、脚本及文档包体仅5.81MB轻量但内容完整。已有732人浏览学习其核心价值在于将理论MPC算法真正落地为C可执行实现读者可依据源码逐步理解预测模型构建、优化求解过程与反馈控制闭环并结合实验场景验证算法效果对深入掌握现代控制方法具有实际参考意义。1. 为什么车辆轨迹控制要选 C 与 MPC 这个组合低速园区配送车进出库、过窄弯的时候纯跟踪算法容易卡在转向角约束上速度越低横向刚度越弱前馈比例越难调一旦把最大转向角写进逻辑车身姿态又开始抖。MPC也就是模型预测控制处理这类问题的思路完全不同——它把参考路径、车辆运动学模型、控制量上下限放进同一个优化问题每个控制周期求出一段未来控制序列但只执行第一步。约束不再是试出来的限幅而是显式进入了目标函数。C 出现在这里的理由很直接。底盘控制器或路径跟踪节点通常跑在 Linux 工控机上采样周期要到 20 ms 到 100 ms 量级同一周期内还要完成状态读取、模型更新和优化求解。C 的确定性与低内存开销比 Python 更适合这种实时性要求。下面按建模、实现、调参到工程化的顺序把基于 C 的 MPC 车辆轨迹控制完整走一遍。2. 建模把车辆运动学变成 C 可离散的线性状态方程2.1 车辆运动学模型与状态量设定做轨迹跟踪时横向控制最常用的模型是自行车模型。它把前后轮各等效成一个轮子忽略轮胎侧偏角认为车辆绕瞬时转动中心做刚体运动。状态量一般取四个大地坐标系下的位置 x、y航向角 theta纵向速度 v。控制量取加速度 a 和前轮转角 delta。连续时间状态方程如下dx/dt v * cos(theta) dy/dt v * sin(theta) dtheta/dt v * tan(delta) / L dv/dt aL 是轴距。这个模型的优点是参数少、计算快适合作为 MPC 预测模型代价是低速或大转角时精度有限不能表达轮胎力饱和。对轨迹跟踪场景尤其是园区低速车它已经是工程上常用的第一版模型。符号含义单位代码变量x, y车辆在大地坐标系下的位置mstate[0], state[1]theta航向角radstate[2]v纵向车速m/sstate[3]a纵向加速度m/s^2u[0]delta前轮转角radu[1]L轴距mwheelbase2.2 在参考轨迹上做线性化得到偏差模型MPC 的预测模型最好是线性的这样每个周期求解的是二次规划耗时可控。轨迹跟踪一般不是控制绝对坐标而是控制相对参考轨迹的偏差。设参考状态为 x_r, y_r, theta_r参考速度为 v_r参考转角为 delta_r。定义状态偏差x_tilde [x - x_r; y - y_r; theta - theta_r; v - v_r]在这个参考点上做一阶泰勒展开得到线性偏差模型d(x_tilde)/dt A_c * x_tilde B_c * u_tilde。A_c 是 4x4 矩阵B_c 是 4x2 矩阵A_c [0, 0, -v_r * sin(theta_r), cos(theta_r)] [0, 0, v_r * cos(theta_r), sin(theta_r)] [0, 0, 0, tan(delta_r) / L] [0, 0, 0, 0] B_c [0, 0] [0, 0] [0, v_r / (L * cos(delta_r)^2)] [1, 0]这里第一列对应加速度 a第二列对应前轮转角 delta。可以看到航向角偏差对位置的影响通过 -v_r * sin(theta_r) 和 v_r * cos(theta_r) 体现这些系数在每个周期都会随参考点变化所以线性化要每拍重算。2.3 用欧拉离散把连续状态方程转换为 A_d/B_d连续方程不能直接写进 MPC需要离散化。工程上常用一阶欧拉A_d I dt * A_cB_d dt * B_c。dt 是采样周期。下面用 Eigen 写一个离散化函数输入参考状态和控制量输出 A_d 和 B_d#include Eigen/Dense #include cmath struct KinematicModel { double L{2.8}; // 轴距单位 m double dt{0.1}; // 采样周期单位 s void buildDiscreteModel( const Eigen::Vector4d xr, // 参考状态 [x_r, y_r, theta_r, v_r] double delta_r, // 参考转角单位 rad Eigen::Matrix4d Ad, // 输出离散状态矩阵 Eigen::Matrixdouble,4,2 Bd) const { double s std::sin(xr[2]); double c std::cos(xr[2]); double t std::tan(delta_r); double ct std::cos(delta_r); Eigen::Matrix4d Ac; Ac 0, 0, -xr[3] * s, c, 0, 0, xr[3] * c, s, 0, 0, 0, t / L, 0, 0, 0, 0; Eigen::Matrixdouble,4,2 Bc; Bc 0, 0, 0, 0, 0, xr[3] / (L * ct * ct), 1, 0; Ad Eigen::Matrix4d::Identity() Ac * dt; Bd Bc * dt; } };buildDiscreteModel里最需要注意的是第三行对 delta 的偏导v_r / (L * cos(delta_r)^2)。参考点转角 delta_r 如果是零这个系数退化为 v_r / L如果车辆在低速且转角大比如原地掉头场景这个值会明显放大控制序列容易在数值上跳变。实际调试时若第一版代码在弯道位置出现控制量震荡先检查这里有没有除以 cos^2(delta_r)。3. 实现最小可用的 C MPC 求解器3.1 预测模型与目标函数先写清楚离散化之后预测模型写成x(ki1) A_d * x(ki) B_d * u(ki)其中 i 从 0 到 N-1N 是预测时域步数。MPC 在每个控制周期求解的目标函数是J sum_{i1..N} ( x(ki) Q x(ki) u(ki-1) R u(ki-1) ) x(kN) P x(kN)前两项分别惩罚状态偏差和控制量最后一项是终端代价用来兜底预测时域末尾的残余误差。Q 是 4x4 状态权重R 是 2x2 控制权重。约束直接加在控制量上a_min a a_max delta_min delta delta_max这是 MPC 比 PID、纯跟踪强的核心能力转向角、加速度上下限是优化问题的边界条件而不是挂在控制器后面的 limiter。限幅器会在每一拍咬掉控制量导致姿态突变MPC 会在滚动优化时就把边界考虑进去得到的是可行性范围内的最优序列。3.2 不依赖第三方库的简化求解梯度下降加约束裁剪完整 MPC 求解通常通过 OSQP 等 QP 求解器完成。这里为了先把闭环跑通给一个不依赖任何第三方库的教学级求解器在预测时域内对控制序列做梯度下降每步更新后直接 clamp 到约束范围。生产代码请把它替换成 QP 求解器。#include Eigen/Dense #include algorithm struct MpcConfig { int N{10}; // 预测步数 double dt{0.1}; // 采样周期 Eigen::Matrix4d Q; Eigen::Matrix2d R; double aMin{-3.0}, aMax{2.0}; // 加速度约束m/s^2 double dMin{-0.5}, dMax{0.5}; // 前轮转角约束rad double lr{0.05}; // 梯度下降步长 int iter{30}; // 每周期迭代次数 MpcConfig() { Q Eigen::Matrix4d::Zero(); Q(0,0) 1.0; // 横向偏差权重 Q(1,1) 1.0; // 纵向偏差权重 Q(2,2) 0.1; // 航向角偏差权重 Q(3,3) 0.01; // 速度差权重 R Eigen::Matrix2d::Zero(); R(0,0) 0.01; // 加速度权重 R(1,1) 0.05; // 转角权重 } }; Eigen::VectorXd solveMpc( const Eigen::Matrix4d Ad, const Eigen::Matrixdouble,4,2 Bd, const Eigen::Vector4d x0, const MpcConfig c) { int nU 2 * c.N; Eigen::VectorXd U Eigen::VectorXd::Zero(nU); for (int it 0; it c.iter; it) { Eigen::Vector4d xk x0; for (int i 0; i c.N; i) { Eigen::Vector2d u U.segment2(2 * i); Eigen::Vector4d xn Ad * xk Bd * u; // 简化梯度对控制量的单步惩罚梯度 Eigen::Vector2d grad 2.0 * Bd.transpose() * c.Q * xn 2.0 * c.R * u; Eigen::Vector2d un u - c.lr * grad; un[0] std::clamp(un[0], c.aMin, c.aMax); un[1] std::clamp(un[1], c.dMin, c.dMax); U.segment2(2 * i) un; xk xn; } } return U; }要特别说明严格的 MPC 梯度应该通过伴随状态从最后一个预测步逐级回代因为 u(i) 会影响后面所有状态。上面的grad只取了“本步状态对控制量”的局部导数适合把流程跑通但控制效果在长时域下会打折。实际项目里这段函数内的循环应整体替换为一次 QP 求解调用接口保持一致上层逻辑不用动。参数方面lr和iter决定求解收敛程度。lr太大会在目标函数附近震荡iter太少则控制序列不收敛表现为车辆在参考轨迹两侧来回摆动。首次调试建议 lr 取 0.02 到 0.05iter 固定 30然后用第 4 章的评估指标回看。3.3 滚动执行控制只把第一个控制量交给车辆MPC 的关键动作是滚动优化而不是把 N 步控制序列全部执行。常见做法是每个采样周期只取U.segment2(0)作为这一拍的控制量车辆执行后采集新状态下一拍重新求解。Eigen::Vector2d rollingControl( const KinematicModel model, const Eigen::Vector4d xCur, // 车辆当前状态 const Eigen::Vector4d xRef, // 当前参考状态 double deltaRef, // 参考转角 const MpcConfig cfg) { Eigen::Matrix4d Ad; Eigen::Matrixdouble,4,2 Bd; model.buildDiscreteModel(xRef, deltaRef, Ad, Bd); Eigen::Vector4d xErr xCur - xRef; Eigen::VectorXd U solveMpc(Ad, Bd, xErr, cfg); return U.segment2(0); }这里传入的xErr是状态偏差MPC 基于偏差模型求解所以目标函数里的 Q 直接作用在偏差量上。deltaRef一般由参考路径上的曲率推算v_r 大、弯道半径小的地方deltaRef 是主要前馈MPC 只负责修正偏差。这样分工会比让 MPC 从零开始完全靠误差收敛更稳。4. 在 vscode 里跑通仿真并调 N、Q、R 三个参数4.1 vscode 配置 C 环境与最小仿真工程桌面调试时可以用 vscode 配 C 环境。装好 C/C 扩展确保 g 可用再用 tasks.json 调用编译命令。一个最小工程目录只需要两个文件mpc_main.cpp和上面两段代码。Eigen 只需下载头文件目录不需要编译所以 includePath 指过去即可。编译命令g -stdc17 -O2 -I/path/to/eigen mpc_main.cpp -o mpc_demo-O2对 MPC 这类循环密集代码影响很大同一个求解循环开不开优化速度差好几倍。调试时先用-O0 -g确认逻辑后再切到-O2看实时性。主循环框架for (int k 0; k 500; k) { Eigen::Vector4d xRef referenceAt(k * dt); // 参考状态 Eigen::Vector2d u rollingControl(model, xCur, xRef, 0.0, cfg); // 用真实运动学模型推进 xCur[0] xCur[3] * std::cos(xCur[2]) * dt; xCur[1] xCur[3] * std::sin(xCur[2]) * dt; xCur[2] xCur[3] * std::tan(u[1]) / model.L * dt; xCur[3] u[0] * dt; log.push_back({xRef[0], xRef[1], xCur[0], xCur[1]}); }referenceAt在真实工程里来自全局路径规划器的输出这里先用正弦曲线或折线接圆弧的轨迹替代。注意参考轨迹要提供 theta_r 和 v_r不能只给 xy 坐标否则上面的离散化矩阵会退化成不确定值。4.2 MPC 三个参数 N、Q、R 到底怎么调调参顺序建议固定为先定 N再调 R最后调 Q。N 乘以 dt 是预测距离它应该覆盖车辆从当前速度刹停所需的距离。速度 5 m/s、刹停加速度 2 m/s^2 时制动距离约 6.25 mdt0.1 时 N 至少要取 10取 15 到 20 更稳。参数影响调整方向N预测距离 N*dt太小抑制不住超调太大会让控制序列迟钝求解变慢R 对角线控制量惩罚太小输出抖动太大收敛慢误差消不了Q(0,0), Q(1,1)横向、纵向偏差惩罚先保持 1.0观察稳态误差再上调Q(2,2)航向角偏差惩罚弯道切弯严重时加大但过大会把车掰出平滑弧线调参时看两个现象。第一起步阶段控制量是否频繁上下冲如果是先加大 R(1,1)也就是惩罚前轮转角速率这比单纯限制 delta 幅度更能抑制抖振。第二弯道里是否出现稳态切弯误差也就是车始终贴着弯内侧走这说明 Q(2,2) 太小航向偏差没被有效压住。提示R 矩阵不是越大越好。R 过大会让 MPC 认为“少打方向”比“贴近轨迹”更重要结果是车在弯道里走出一条外切直线横向误差完全不收敛。每次改 R 后都要重新看 RMSE而不是只看轨迹形状。4.3 用横向误差和 RMSE 判断 MPC 是否真的在跟踪轨迹跟踪效果不能靠肉眼看曲线形状要量化。两个最直接的指标横向误差绝对值和纵向位置误差的 RMSE。下面这段代码从仿真日志里算 RMSEdouble rmse 0.0; int n 0; for (const auto e : log) { double err std::hypot(e.refX - e.curX, e.refY - e.curY); rmse err * err; n; } rmse std::sqrt(rmse / n);RMSE 只反映整体误差。还要单看横向误差的最大值它决定了车辆会不会蹭到车道边界。工程上常见的目标低速园区场景横向误差 RMSE 小于 0.1 m最大值小于 0.2 m。如果你的结果远大于这个数不要急着调 Q先打印出每一步参考点和控制量检查是不是参考轨迹的 theta_r 跳变太大比如直线接圆弧时没有做平滑过渡导致 MPC 每一拍都在追赶一个不连续的参考航向。5. 从演示到实车三个工程化技巧5.1 把 QP 求解器藏在接口后面上面的教学代码用的是梯度下降近似真要放到工程里第一件事是把求解器替换为 OSQP 或内点法实现。为了避免算法主逻辑跟着求解器绑死可以把求解过程抽象成一个接口class MpcSolver { public: virtual ~MpcSolver() default; virtual Eigen::VectorXd solve( const Eigen::Matrix4d Ad, const Eigen::Matrixdouble,4,2 Bd, const Eigen::Vector4d x0) 0; };控制器持有MpcSolver*OSQP 版本和教学版本都继承这个接口。这样车辆上层逻辑不变换求解器不影响控制代码也方便在仿真和实车之间切换。5.2 别让 C 继承隐藏坑掉你的模型多态这个接口看起来简单但 C 的覆盖和隐藏是面试常客也是工程里容易被忽略的地方。如果子类的solve参数少一个const或者把Eigen::Matrix4d写成了Eigen::MatrixXd编译器不会报错它只会安静地认为这是一个新函数把基类虚函数隐藏掉。结果就是指针调用时永远走基类版本。手动加override关键字是最直接的防线签名不匹配时编译直接失败。class OsqpSolver : public MpcSolver { public: Eigen::VectorXd solve( const Eigen::Matrix4d Ad, const Eigen::Matrixdouble,4,2 Bd, const Eigen::Vector4d x0) override; };5.3 用状态估计与延迟补偿填补模型与现实差仿真里状态直接给真值实车不行。GNSS 更新率低IMU 积分会飘常见做法是卡尔曼滤波融合把位置、速度、航向作为状态量进行估计。另外 MPC 求解本身有耗时假设单次求解花 10 ms那么在 k 时刻发出的控制量实际作用在 k1 时刻。这会导致控制滞后一拍弯道里表现为横向误差有规律地偏向一侧。先估算单拍耗时在 MPC 里用预测状态而不是当前测量值作为初值计算出的控制量就更接近车辆实际执行时的状态。这一步做完通常能再压掉 1 到 2 cm 的横向误差。本文还有配套的精品资源点击获取