簡介這是基于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 的橫向誤差。本文還有配套的精品資源點擊獲取