簡介這是基于C實(shí)現(xiàn)MPC算法控制車輛運(yùn)動(dòng)軌跡的完整工程資源適合學(xué)習(xí)機(jī)器人控制、自動(dòng)駕駛或智能車路徑跟蹤的學(xué)生與開發(fā)者也可用于畢業(yè)設(shè)計(jì)、課程作業(yè)或工程實(shí)訓(xùn)。項(xiàng)目代碼結(jié)構(gòu)清晰可直接編譯運(yùn)行包含CMake構(gòu)建腳本、MPC求解器、車輛運(yùn)動(dòng)學(xué)模型和Eigen數(shù)值計(jì)算依賴可幫助讀者從零搭建可復(fù)現(xiàn)的軌跡跟蹤控制流程。壓縮包共1650個(gè)文件以714個(gè)cpp源文件、469個(gè)h頭文件為主體另有txt說明、CMake配置、腳本及文檔包體僅5.81MB輕量但內(nèi)容完整。已有732人瀏覽學(xué)習(xí)其核心價(jià)值在于將理論MPC算法真正落地為C可執(zhí)行實(shí)現(xiàn)讀者可依據(jù)源碼逐步理解預(yù)測模型構(gòu)建、優(yōu)化求解過程與反饋控制閉環(huán)并結(jié)合實(shí)驗(yàn)場景驗(yàn)證算法效果對深入掌握現(xiàn)代控制方法具有實(shí)際參考意義。1. 為什么車輛軌跡控制要選 C 與 MPC 這個(gè)組合低速園區(qū)配送車進(jìn)出庫、過窄彎的時(shí)候純跟蹤算法容易卡在轉(zhuǎn)向角約束上速度越低橫向剛度越弱前饋比例越難調(diào)一旦把最大轉(zhuǎn)向角寫進(jìn)邏輯車身姿態(tài)又開始抖。MPC也就是模型預(yù)測控制處理這類問題的思路完全不同——它把參考路徑、車輛運(yùn)動(dòng)學(xué)模型、控制量上下限放進(jìn)同一個(gè)優(yōu)化問題每個(gè)控制周期求出一段未來控制序列但只執(zhí)行第一步。約束不再是試出來的限幅而是顯式進(jìn)入了目標(biāo)函數(shù)。C 出現(xiàn)在這里的理由很直接。底盤控制器或路徑跟蹤節(jié)點(diǎn)通常跑在 Linux 工控機(jī)上采樣周期要到 20 ms 到 100 ms 量級同一周期內(nèi)還要完成狀態(tài)讀取、模型更新和優(yōu)化求解。C 的確定性與低內(nèi)存開銷比 Python 更適合這種實(shí)時(shí)性要求。下面按建模、實(shí)現(xiàn)、調(diào)參到工程化的順序把基于 C 的 MPC 車輛軌跡控制完整走一遍。2. 建模把車輛運(yùn)動(dòng)學(xué)變成 C 可離散的線性狀態(tài)方程2.1 車輛運(yùn)動(dòng)學(xué)模型與狀態(tài)量設(shè)定做軌跡跟蹤時(shí)橫向控制最常用的模型是自行車模型。它把前后輪各等效成一個(gè)輪子忽略輪胎側(cè)偏角認(rèn)為車輛繞瞬時(shí)轉(zhuǎn)動(dòng)中心做剛體運(yùn)動(dòng)。狀態(tài)量一般取四個(gè)大地坐標(biāo)系下的位置 x、y航向角 theta縱向速度 v。控制量取加速度 a 和前輪轉(zhuǎn)角 delta。連續(xù)時(shí)間狀態(tài)方程如下dx/dt v * cos(theta) dy/dt v * sin(theta) dtheta/dt v * tan(delta) / L dv/dt aL 是軸距。這個(gè)模型的優(yōu)點(diǎn)是參數(shù)少、計(jì)算快適合作為 MPC 預(yù)測模型代價(jià)是低速或大轉(zhuǎn)角時(shí)精度有限不能表達(dá)輪胎力飽和。對軌跡跟蹤場景尤其是園區(qū)低速車它已經(jīng)是工程上常用的第一版模型。符號(hào)含義單位代碼變量x, y車輛在大地坐標(biāo)系下的位置mstate[0], state[1]theta航向角radstate[2]v縱向車速m/sstate[3]a縱向加速度m/s^2u[0]delta前輪轉(zhuǎn)角radu[1]L軸距mwheelbase2.2 在參考軌跡上做線性化得到偏差模型MPC 的預(yù)測模型最好是線性的這樣每個(gè)周期求解的是二次規(guī)劃耗時(shí)可控。軌跡跟蹤一般不是控制絕對坐標(biāo)而是控制相對參考軌跡的偏差。設(shè)參考狀態(tài)為 x_r, y_r, theta_r參考速度為 v_r參考轉(zhuǎn)角為 delta_r。定義狀態(tài)偏差x_tilde [x - x_r; y - y_r; theta - theta_r; v - v_r]在這個(gè)參考點(diǎn)上做一階泰勒展開得到線性偏差模型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]這里第一列對應(yīng)加速度 a第二列對應(yīng)前輪轉(zhuǎn)角 delta。可以看到航向角偏差對位置的影響通過 -v_r * sin(theta_r) 和 v_r * cos(theta_r) 體現(xiàn)這些系數(shù)在每個(gè)周期都會(huì)隨參考點(diǎn)變化所以線性化要每拍重算。2.3 用歐拉離散把連續(xù)狀態(tài)方程轉(zhuǎn)換為 A_d/B_d連續(xù)方程不能直接寫進(jìn) MPC需要離散化。工程上常用一階歐拉A_d I dt * A_cB_d dt * B_c。dt 是采樣周期。下面用 Eigen 寫一個(gè)離散化函數(shù)輸入?yún)⒖紶顟B(tài)和控制量輸出 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, // 參考狀態(tài) [x_r, y_r, theta_r, v_r] double delta_r, // 參考轉(zhuǎn)角單位 rad Eigen::Matrix4d Ad, // 輸出離散狀態(tài)矩陣 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 的偏導(dǎo)v_r / (L * cos(delta_r)^2)。參考點(diǎn)轉(zhuǎn)角 delta_r 如果是零這個(gè)系數(shù)退化為 v_r / L如果車輛在低速且轉(zhuǎn)角大比如原地掉頭場景這個(gè)值會(huì)明顯放大控制序列容易在數(shù)值上跳變。實(shí)際調(diào)試時(shí)若第一版代碼在彎道位置出現(xiàn)控制量震蕩先檢查這里有沒有除以 cos^2(delta_r)。3. 實(shí)現(xiàn)最小可用的 C MPC 求解器3.1 預(yù)測模型與目標(biāo)函數(shù)先寫清楚離散化之后預(yù)測模型寫成x(ki1) A_d * x(ki) B_d * u(ki)其中 i 從 0 到 N-1N 是預(yù)測時(shí)域步數(shù)。MPC 在每個(gè)控制周期求解的目標(biāo)函數(shù)是J sum_{i1..N} ( x(ki) Q x(ki) u(ki-1) R u(ki-1) ) x(kN) P x(kN)前兩項(xiàng)分別懲罰狀態(tài)偏差和控制量最后一項(xiàng)是終端代價(jià)用來兜底預(yù)測時(shí)域末尾的殘余誤差。Q 是 4x4 狀態(tài)權(quán)重R 是 2x2 控制權(quán)重。約束直接加在控制量上a_min a a_max delta_min delta delta_max這是 MPC 比 PID、純跟蹤強(qiáng)的核心能力轉(zhuǎn)向角、加速度上下限是優(yōu)化問題的邊界條件而不是掛在控制器后面的 limiter。限幅器會(huì)在每一拍咬掉控制量導(dǎo)致姿態(tài)突變MPC 會(huì)在滾動(dòng)優(yōu)化時(shí)就把邊界考慮進(jìn)去得到的是可行性范圍內(nèi)的最優(yōu)序列。3.2 不依賴第三方庫的簡化求解梯度下降加約束裁剪完整 MPC 求解通常通過 OSQP 等 QP 求解器完成。這里為了先把閉環(huán)跑通給一個(gè)不依賴任何第三方庫的教學(xué)級求解器在預(yù)測時(shí)域內(nèi)對控制序列做梯度下降每步更新后直接 clamp 到約束范圍。生產(chǎn)代碼請把它替換成 QP 求解器。#include Eigen/Dense #include algorithm struct MpcConfig { int N{10}; // 預(yù)測步數(shù) 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}; // 前輪轉(zhuǎn)角約束rad double lr{0.05}; // 梯度下降步長 int iter{30}; // 每周期迭代次數(shù) MpcConfig() { Q Eigen::Matrix4d::Zero(); Q(0,0) 1.0; // 橫向偏差權(quán)重 Q(1,1) 1.0; // 縱向偏差權(quán)重 Q(2,2) 0.1; // 航向角偏差權(quán)重 Q(3,3) 0.01; // 速度差權(quán)重 R Eigen::Matrix2d::Zero(); R(0,0) 0.01; // 加速度權(quán)重 R(1,1) 0.05; // 轉(zhuǎn)角權(quán)重 } }; 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; }要特別說明嚴(yán)格的 MPC 梯度應(yīng)該通過伴隨狀態(tài)從最后一個(gè)預(yù)測步逐級回代因?yàn)?u(i) 會(huì)影響后面所有狀態(tài)。上面的grad只取了“本步狀態(tài)對控制量”的局部導(dǎo)數(shù)適合把流程跑通但控制效果在長時(shí)域下會(huì)打折。實(shí)際項(xiàng)目里這段函數(shù)內(nèi)的循環(huán)應(yīng)整體替換為一次 QP 求解調(diào)用接口保持一致上層邏輯不用動(dòng)。參數(shù)方面lr和iter決定求解收斂程度。lr太大會(huì)在目標(biāo)函數(shù)附近震蕩iter太少則控制序列不收斂表現(xiàn)為車輛在參考軌跡兩側(cè)來回?cái)[動(dòng)。首次調(diào)試建議 lr 取 0.02 到 0.05iter 固定 30然后用第 4 章的評估指標(biāo)回看。3.3 滾動(dòng)執(zhí)行控制只把第一個(gè)控制量交給車輛MPC 的關(guān)鍵動(dòng)作是滾動(dòng)優(yōu)化而不是把 N 步控制序列全部執(zhí)行。常見做法是每個(gè)采樣周期只取U.segment2(0)作為這一拍的控制量車輛執(zhí)行后采集新狀態(tài)下一拍重新求解。Eigen::Vector2d rollingControl( const KinematicModel model, const Eigen::Vector4d xCur, // 車輛當(dāng)前狀態(tài) const Eigen::Vector4d xRef, // 當(dāng)前參考狀態(tài) double deltaRef, // 參考轉(zhuǎn)角 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是狀態(tài)偏差MPC 基于偏差模型求解所以目標(biāo)函數(shù)里的 Q 直接作用在偏差量上。deltaRef一般由參考路徑上的曲率推算v_r 大、彎道半徑小的地方deltaRef 是主要前饋MPC 只負(fù)責(zé)修正偏差。這樣分工會(huì)比讓 MPC 從零開始完全靠誤差收斂更穩(wěn)。4. 在 vscode 里跑通仿真并調(diào) N、Q、R 三個(gè)參數(shù)4.1 vscode 配置 C 環(huán)境與最小仿真工程桌面調(diào)試時(shí)可以用 vscode 配 C 環(huán)境。裝好 C/C 擴(kuò)展確保 g 可用再用 tasks.json 調(diào)用編譯命令。一個(gè)最小工程目錄只需要兩個(gè)文件mpc_main.cpp和上面兩段代碼。Eigen 只需下載頭文件目錄不需要編譯所以 includePath 指過去即可。編譯命令g -stdc17 -O2 -I/path/to/eigen mpc_main.cpp -o mpc_demo-O2對 MPC 這類循環(huán)密集代碼影響很大同一個(gè)求解循環(huán)開不開優(yōu)化速度差好幾倍。調(diào)試時(shí)先用-O0 -g確認(rèn)邏輯后再切到-O2看實(shí)時(shí)性。主循環(huán)框架for (int k 0; k 500; k) { Eigen::Vector4d xRef referenceAt(k * dt); // 參考狀態(tài) Eigen::Vector2d u rollingControl(model, xCur, xRef, 0.0, cfg); // 用真實(shí)運(yùn)動(dòng)學(xué)模型推進(jìn) 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在真實(shí)工程里來自全局路徑規(guī)劃器的輸出這里先用正弦曲線或折線接圓弧的軌跡替代。注意參考軌跡要提供 theta_r 和 v_r不能只給 xy 坐標(biāo)否則上面的離散化矩陣會(huì)退化成不確定值。4.2 MPC 三個(gè)參數(shù) N、Q、R 到底怎么調(diào)調(diào)參順序建議固定為先定 N再調(diào) R最后調(diào) Q。N 乘以 dt 是預(yù)測距離它應(yīng)該覆蓋車輛從當(dāng)前速度剎停所需的距離。速度 5 m/s、剎停加速度 2 m/s^2 時(shí)制動(dòng)距離約 6.25 mdt0.1 時(shí) N 至少要取 10取 15 到 20 更穩(wěn)。參數(shù)影響調(diào)整方向N預(yù)測距離 N*dt太小抑制不住超調(diào)太大會(huì)讓控制序列遲鈍求解變慢R 對角線控制量懲罰太小輸出抖動(dòng)太大收斂慢誤差消不了Q(0,0), Q(1,1)橫向、縱向偏差懲罰先保持 1.0觀察穩(wěn)態(tài)誤差再上調(diào)Q(2,2)航向角偏差懲罰彎道切彎嚴(yán)重時(shí)加大但過大會(huì)把車掰出平滑弧線調(diào)參時(shí)看兩個(gè)現(xiàn)象。第一起步階段控制量是否頻繁上下沖如果是先加大 R(1,1)也就是懲罰前輪轉(zhuǎn)角速率這比單純限制 delta 幅度更能抑制抖振。第二彎道里是否出現(xiàn)穩(wěn)態(tài)切彎誤差也就是車始終貼著彎內(nèi)側(cè)走這說明 Q(2,2) 太小航向偏差沒被有效壓住。提示R 矩陣不是越大越好。R 過大會(huì)讓 MPC 認(rèn)為“少打方向”比“貼近軌跡”更重要結(jié)果是車在彎道里走出一條外切直線橫向誤差完全不收斂。每次改 R 后都要重新看 RMSE而不是只看軌跡形狀。4.3 用橫向誤差和 RMSE 判斷 MPC 是否真的在跟蹤軌跡跟蹤效果不能靠肉眼看曲線形狀要量化。兩個(gè)最直接的指標(biāo)橫向誤差絕對值和縱向位置誤差的 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 只反映整體誤差。還要單看橫向誤差的最大值它決定了車輛會(huì)不會(huì)蹭到車道邊界。工程上常見的目標(biāo)低速園區(qū)場景橫向誤差 RMSE 小于 0.1 m最大值小于 0.2 m。如果你的結(jié)果遠(yuǎn)大于這個(gè)數(shù)不要急著調(diào) Q先打印出每一步參考點(diǎn)和控制量檢查是不是參考軌跡的 theta_r 跳變太大比如直線接圓弧時(shí)沒有做平滑過渡導(dǎo)致 MPC 每一拍都在追趕一個(gè)不連續(xù)的參考航向。5. 從演示到實(shí)車三個(gè)工程化技巧5.1 把 QP 求解器藏在接口后面上面的教學(xué)代碼用的是梯度下降近似真要放到工程里第一件事是把求解器替換為 OSQP 或內(nèi)點(diǎn)法實(shí)現(xiàn)。為了避免算法主邏輯跟著求解器綁死可以把求解過程抽象成一個(gè)接口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 版本和教學(xué)版本都繼承這個(gè)接口。這樣車輛上層邏輯不變換求解器不影響控制代碼也方便在仿真和實(shí)車之間切換。5.2 別讓 C 繼承隱藏坑掉你的模型多態(tài)這個(gè)接口看起來簡單但 C 的覆蓋和隱藏是面試?？鸵彩枪こ汤锶菀妆缓雎缘牡胤健Ｈ绻宇惖膕olve參數(shù)少一個(gè)const或者把Eigen::Matrix4d寫成了Eigen::MatrixXd編譯器不會(huì)報(bào)錯(cuò)它只會(huì)安靜地認(rèn)為這是一個(gè)新函數(shù)把基類虛函數(shù)隱藏掉。結(jié)果就是指針調(diào)用時(shí)永遠(yuǎn)走基類版本。手動(dòng)加override關(guān)鍵字是最直接的防線簽名不匹配時(shí)編譯直接失敗。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 用狀態(tài)估計(jì)與延遲補(bǔ)償填補(bǔ)模型與現(xiàn)實(shí)差仿真里狀態(tài)直接給真值實(shí)車不行。GNSS 更新率低IMU 積分會(huì)飄常見做法是卡爾曼濾波融合把位置、速度、航向作為狀態(tài)量進(jìn)行估計(jì)。另外 MPC 求解本身有耗時(shí)假設(shè)單次求解花 10 ms那么在 k 時(shí)刻發(fā)出的控制量實(shí)際作用在 k1 時(shí)刻。這會(huì)導(dǎo)致控制滯后一拍彎道里表現(xiàn)為橫向誤差有規(guī)律地偏向一側(cè)。先估算單拍耗時(shí)在 MPC 里用預(yù)測狀態(tài)而不是當(dāng)前測量值作為初值計(jì)算出的控制量就更接近車輛實(shí)際執(zhí)行時(shí)的狀態(tài)。這一步做完通常能再壓掉 1 到 2 cm 的橫向誤差。本文還有配套的精品資源點(diǎn)擊獲取