列分布式MPC控制算法：Matlab源碼實(shí)現(xiàn)與避坑指南)
簡介這份資源面向自動(dòng)駕駛、智能交通與車輛控制方向的研究人員、工程師及學(xué)生提供一套多車隊(duì)列控制的分布式模型預(yù)測控制MPC實(shí)現(xiàn)方案重點(diǎn)解決三輛車在編隊(duì)行駛中的協(xié)同與穩(wěn)定性問題。壓縮包內(nèi)共2個(gè)文件包含1個(gè)m源碼文件與1個(gè)txt說明文件整體約2KB體量輕巧便于快速閱讀與二次開發(fā)。源碼圍繞分布式MPC的預(yù)測模型、滾動(dòng)優(yōu)化與車輛間協(xié)同約束展開可用于理解隊(duì)列一致性、間距保持與安全行駛的實(shí)現(xiàn)思路適合作為算法教學(xué)、課程設(shè)計(jì)或研發(fā)原型的參考。目前已有365人學(xué)習(xí)下載說明該方向具備一定關(guān)注度。讀者可從中獲取可運(yùn)行的MATLAB代碼框架與配套說明用于復(fù)現(xiàn)三車隊(duì)列控制效果、對比集中式與分布式策略差異并在此基礎(chǔ)上擴(kuò)展至更多車輛或復(fù)雜交通場景提升編隊(duì)穩(wěn)定性與協(xié)同性。1. 多車隊(duì)列控制算法三輛車分布式 MPC 到底在算什么高速公路上三輛車保持固定間距巡航前車一腳剎車后車如果只靠自己的雷達(dá)做 PID很容易出現(xiàn)「前車點(diǎn)頭、后車鞠躬」的隊(duì)列振蕩。多車隊(duì)列控制算法要解決的就是這件事讓每輛車在只拿到鄰居狀態(tài)的前提下仍然把車間距誤差收斂到零同時(shí)不違反加速度和通信帶寬約束。分布式模型預(yù)測控制Distributed MPC是當(dāng)前落地最穩(wěn)的一條路線——每輛車本地求解一個(gè)有限時(shí)域優(yōu)化問題只交換預(yù)測軌跡或狀態(tài)量而不是把全局信息塞給一個(gè)中央控制器。這套 matlab 源碼實(shí)現(xiàn)的就是三輛車的分布式 MPC 隊(duì)列適合做車輛編隊(duì)、AGV 協(xié)同、多智能體一致性研究的從業(yè)者拿來改參數(shù)、換拓?fù)?、?yàn)證自己的通信策略。它不追求花哨追求的是你能在本地跑通、看懂每一行代價(jià)函數(shù)在懲罰什么。2. 分布式 MPC 隊(duì)列建模從車輛動(dòng)力學(xué)到鄰居耦合項(xiàng)2.1 三輛車縱向動(dòng)力學(xué)為什么選離散線性模型真實(shí)車輛是強(qiáng)非線性系統(tǒng)但隊(duì)列控制里如果一上來就寫非線性 MPC求解時(shí)間會直接吃掉你的實(shí)時(shí)裕度。常見做法是在工作點(diǎn)附近做線性化再離散化。源碼里用的是三階狀態(tài)位置 $p$、速度 $v$、加速度 $a$輸入是加速度增量 $j$jerk。這樣狀態(tài)轉(zhuǎn)移矩陣可以寫成% 三車隊(duì)列離散線性模型參數(shù) Ts 0.1; % 采樣周期 100ms兼顧實(shí)時(shí)性與精度 tau 0.5; % 車輛動(dòng)力學(xué)的慣性時(shí)間常數(shù) A_c [0 1 0; 0 0 1; 0 0 -1/tau]; B_c [0; 0; 1/tau]; sys_c ss(A_c, B_c, [], []); sys_d c2d(sys_c, Ts, zoh); % 零階保持離散化 Ad sys_d.A; Bd sys_d.B;這段代碼的邏輯是先把連續(xù)時(shí)間狀態(tài)空間建出來再用零階保持做離散化。Ts0.1不是隨便選的隊(duì)列控制的通信周期通常在 50ms 到 200ms 之間太小會讓優(yōu)化問題頻繁求解、算力吃緊太大則離散誤差累積、間距控制發(fā)飄。tau0.5決定了加速度響應(yīng)的快慢改小會讓車更「賊」改大會讓車更「肉」這個(gè)參數(shù)直接決定你后面 MPC 的預(yù)測時(shí)域該取多長。注意c2d的zoh選項(xiàng)對應(yīng)的是執(zhí)行器在一個(gè)周期內(nèi)保持輸入不變?nèi)绻愕牡讓涌刂破魇侵苯咏o加速度指令而不是 jerk那 B 矩陣要重新推。2.2 隊(duì)列耦合間距誤差怎么進(jìn)代價(jià)函數(shù)分布式 MPC 的核心在于每輛車的優(yōu)化問題里只出現(xiàn)自己和前車的信息。源碼里第 $i$ 輛車的間距誤差定義為$$e_i(k) p_{i-1}(k) - p_i(k) - d_{des} - L$$其中 $d_{des}$ 是期望車間距$L$ 是車長。這個(gè)誤差進(jìn)入代價(jià)函數(shù)的方式?jīng)Q定了隊(duì)列的收斂行為。常見寫法是% 第 i 輛車的 MPC 代價(jià)函數(shù)預(yù)測時(shí)域 Np Np 20; Nc 5; % 預(yù)測20步控制5步 Q diag([10, 1, 0.1]); % 位置誤差權(quán)重大速度次之加速度最小 R 0.5; % 控制增量懲罰抑制抖動(dòng) % 構(gòu)造預(yù)測方程 [Phi, Gamma] predmat(Ad, Bd, Np, Nc); % 參考軌跡前車預(yù)測位置減去期望間距 ref pre_pos_pred - d_des - L; % 二次規(guī)劃求解 H Gamma * kron(eye(Np), Q) * Gamma R * eye(Nc); f Gamma * kron(eye(Np), Q) * (Phi * x0 - ref); U quadprog(H, f, [], [], [], [], u_min, u_max);Q矩陣?yán)镂恢谜`差權(quán)重給到 10是因?yàn)殛?duì)列控制最不能忍的就是間距偏差速度權(quán)重 1 是讓速度跟隨別太激進(jìn)加速度權(quán)重 0.1 是給舒適性留余地。R0.5是控制增量的懲罰這個(gè)值太小會讓加速度指令抖得像心電圖太大會讓響應(yīng)變慢、跟車距離拉大。Np20對應(yīng) 2 秒預(yù)測窗口Nc5是控制時(shí)域通常取預(yù)測時(shí)域的 1/4 到 1/3。quadprog是 matlab 優(yōu)化工具箱里的二次規(guī)劃求解器如果你沒有這個(gè)工具箱可以用mpc對象或者自己寫內(nèi)點(diǎn)法但源碼里默認(rèn)用的是quadprog。2.3 分布式迭代每輛車怎么用鄰居的預(yù)測軌跡分布式 MPC 不是每輛車各算各的就完事那樣會退化成獨(dú)立控制。源碼里用的是「預(yù)測軌跡交換 迭代修正」的框架第 $i$ 輛車在第 $m$ 次迭代時(shí)用第 $i-1$ 輛車上一輪廣播的預(yù)測位置序列作為參考求解自己的優(yōu)化問題然后把新的預(yù)測軌跡廣播給第 $i1$ 輛車。迭代兩到三次就能收斂到納什均衡附近。% 分布式迭代主循環(huán)簡化版 max_iter 3; for iter 1:max_iter for i 2:3 % 頭車不參與間距調(diào)節(jié) % 取前車上一輪預(yù)測位置 pre_pred neighbor_pred{i-1}; % 本地求解 MPC [u_opt, x_pred] solve_mpc(x0{i}, pre_pred, params); % 更新自己的預(yù)測軌跡 my_pred{i} x_pred; end % 交換預(yù)測軌跡 neighbor_pred my_pred; endmax_iter3是實(shí)測下來收斂和算力的折中迭代 1 次隊(duì)列誤差收斂慢迭代 5 次以上收益很小但計(jì)算量線性增長。注意頭車不參與間距調(diào)節(jié)它的軌跡是給定參考這是隊(duì)列控制的常見設(shè)定。如果你要做無頭車或者動(dòng)態(tài)領(lǐng)航這段邏輯要改。neighbor_pred的交換在仿真里是直接內(nèi)存?zhèn)鬟f實(shí)車部署時(shí)要換成通信中間件這時(shí)候通信延遲和丟包就會變成新的坑。3. 在 matlab 里跑通三車隊(duì)列從初始化到出圖3.1 仿真初始化狀態(tài)、參考軌跡和參數(shù)結(jié)構(gòu)體拿到源碼后第一件事不是直接 run而是把參數(shù)結(jié)構(gòu)體看清楚。源碼里通常用一個(gè)params結(jié)構(gòu)體把所有可調(diào)參數(shù)集中管理這樣你改車間距、改權(quán)重、改預(yù)測時(shí)域都不用翻遍腳本。% 初始化三車隊(duì)列仿真參數(shù) params.N 3; % 車輛數(shù) params.d_des 20; % 期望車間距 20m params.L 4.5; % 車長 4.5m params.Ts 0.1; % 采樣周期 params.Np 20; % 預(yù)測時(shí)域 params.Nc 5; % 控制時(shí)域 params.Q diag([10, 1, 0.1]); params.R 0.5; params.u_min -3; % jerk 下限 params.u_max 3; % jerk 上限 params.v_max 30; % 速度上限 30m/s % 初始狀態(tài)三輛車位置、速度、加速度 x0 [0 20 40; % 位置 20 20 20; % 速度 0 0 0]; % 加速度 % 頭車參考軌跡先勻速再減速再加速 t 0:params.Ts:30; v_ref 20 * ones(size(t)); v_ref(t10 t15) 15; % 減速段 v_ref(t20 t25) 25; % 加速段d_des20是車間距這個(gè)值要結(jié)合你的通信延遲和制動(dòng)能力來定20m 在 20m/s 下對應(yīng) 1 秒時(shí)距屬于比較保守的設(shè)定。u_min和u_max是 jerk 約束±3 m/s3 是乘用車舒適性邊界再大乘客會暈。v_ref構(gòu)造了一個(gè)包含減速和加速的工況用來驗(yàn)證隊(duì)列在動(dòng)態(tài)過程中的間距保持能力。注意初始位置是 0、20、40正好對應(yīng)期望間距這樣起步時(shí)誤差為零方便你觀察擾動(dòng)后的收斂過程。3.2 主循環(huán)每一步求解、施加控制、更新狀態(tài)仿真主循環(huán)的結(jié)構(gòu)很固定對每個(gè)時(shí)刻先更新頭車狀態(tài)再讓每輛車依次求解 MPC施加控制量最后更新狀態(tài)并存儲數(shù)據(jù)。% 仿真主循環(huán) Nsim length(t); x_hist zeros(3, params.N, Nsim); % 位置歷史 u_hist zeros(1, params.N, Nsim); % 控制歷史 x_hist(:,:,1) x0; for k 1:Nsim-1 % 頭車跟蹤參考速度 x_hist(2,1,k1) v_ref(k1); x_hist(1,1,k1) x_hist(1,1,k) v_ref(k1)*params.Ts; % 跟車分布式 MPC for i 2:params.N x_cur x_hist(:,i,k); pre_pos x_hist(1,i-1,k1); % 前車當(dāng)前位置 [u_opt, x_pred] solve_mpc(x_cur, pre_pos, params); u_hist(:,i,k) u_opt(1); % 用第一組控制量更新狀態(tài) x_hist(:,i,k1) Ad * x_cur Bd * u_opt(1); end end這段代碼里有個(gè)容易翻車的地方頭車狀態(tài)更新和跟車 MPC 求解的順序。源碼里頭車先更新跟車再用前車的新位置做參考這對應(yīng)「前車信息即時(shí)可用」的假設(shè)。實(shí)車中通信有延遲前車位置往往是上一周期的這時(shí)候要么在 MPC 里做延遲補(bǔ)償要么把參考軌跡往后推一拍。u_opt(1)只取第一個(gè)控制量施加這是滾動(dòng)時(shí)域的標(biāo)準(zhǔn)做法后面預(yù)測的控制量丟掉不用。Ad和Bd是前面離散化得到的矩陣這里直接做狀態(tài)更新沒有再走一遍quadprog。3.3 出圖與驗(yàn)證間距誤差曲線怎么看跑完仿真后最關(guān)鍵的驗(yàn)證圖是間距誤差隨時(shí)間的變化。源碼里一般會畫三張圖位置軌跡、速度軌跡、間距誤差。% 繪制間距誤差 figure; for i 2:params.N err x_hist(1,i-1,:) - x_hist(1,i,:) - params.d_des - params.L; plot(t, squeeze(err), LineWidth, 1.5); hold on; end xlabel(時(shí)間 (s)); ylabel(間距誤差 (m)); legend(車2-車1, 車3-車2); grid on; % 繪制控制量 figure; for i 2:params.N plot(t, squeeze(u_hist(:,i,:)), LineWidth, 1.5); hold on; end xlabel(時(shí)間 (s)); ylabel(jerk (m/s^3)); legend(車2, 車3); grid on;看間距誤差曲線時(shí)關(guān)注三點(diǎn)一是擾動(dòng)后誤差是否回到零二是收斂過程有沒有超調(diào)三是車 3 的誤差是否比車 2 大。如果車 3 誤差明顯放大說明隊(duì)列的「弦穩(wěn)定性」沒滿足需要調(diào)大Q里的位置權(quán)重或者增加預(yù)測時(shí)域?？刂屏繄D看的是 jerk 有沒有頻繁觸邊界如果一直在 ±3 附近跳說明R太小或者Nc太短。注意squeeze是用來去掉單維度因?yàn)閤_hist是三維數(shù)組直接 plot 會報(bào)維度錯(cuò)誤這是 matlab 里很常見的坑。4. 避坑與排查三車隊(duì)列 MPC 最容易翻車的五個(gè)地方4.1 現(xiàn)象間距誤差不收斂反而發(fā)散原因通常是分布式迭代的參考軌跡用錯(cuò)了。如果每輛車都用前車「當(dāng)前時(shí)刻」的位置而不是「預(yù)測軌跡」那 MPC 就退化成了單步跟隨預(yù)測時(shí)域白設(shè)了。解決方法是檢查neighbor_pred的傳遞邏輯確保第 $i$ 輛車拿到的是第 $i-1$ 輛車上一輪迭代的完整預(yù)測序列而不是只拿一個(gè)點(diǎn)。另外檢查Q矩陣?yán)镂恢谜`差的符號如果寫成 $p_i - p_{i-1}$ 而不是 $p_{i-1} - p_i$誤差方向反了控制器會往相反方向調(diào)。4.2 現(xiàn)象quadprog 報(bào)錯(cuò)「H 矩陣非正定」原因一般是H Gamma * kron(eye(Np), Q) * Gamma R * eye(Nc)里R給成了負(fù)數(shù)或者零。R是控制增量懲罰必須正定否則二次規(guī)劃沒有唯一解。解決方法是檢查R的賦值確保它是正標(biāo)量。另一個(gè)可能是Gamma矩陣構(gòu)造時(shí)Np和Nc搞混了導(dǎo)致維度不匹配kron出來的矩陣不是方陣。建議在predmat函數(shù)里加一行assert(size(Gamma,2) Nc)做防御。4.3 現(xiàn)象仿真跑得動(dòng)但實(shí)車抖動(dòng)厲害原因是Ts和Np的匹配問題。Ts0.1、Np20對應(yīng) 2 秒預(yù)測窗口但如果你的底層執(zhí)行器響應(yīng)時(shí)間超過 100msMPC 算出來的控制量還沒執(zhí)行完新的控制量就來了兩個(gè)指令疊加就會抖。解決方法是把Ts調(diào)到和執(zhí)行器周期一致或者加一個(gè)低通濾波器在控制量輸出前。另一個(gè)常見原因是u_min和u_max給得太寬jerk 允許到 ±10車當(dāng)然會竄。先收到 ±3 試試。4.4 現(xiàn)象車 3 的間距誤差始終比車 2 大這是隊(duì)列控制的「誤差傳播」問題不一定是 bug。如果車 3 誤差是車 2 的 1.5 倍以內(nèi)屬于正常范圍如果超過 3 倍說明弦穩(wěn)定性不滿足。解決方法是增大Np或者調(diào)大Q里的位置權(quán)重。源碼里Qdiag([10,1,0.1])是一個(gè)比較溫和的設(shè)定你可以試diag([50,5,0.5])但注意權(quán)重太大會讓控制量更容易觸邊界反而引起振蕩。另一個(gè)辦法是給車 3 單獨(dú)設(shè)一組參數(shù)但這樣就不是「分布式」了屬于工程折中。4.5 現(xiàn)象換車間距后仿真結(jié)果完全不對原因是d_des改了但初始狀態(tài)沒改。如果初始位置還是 0、20、40而d_des改成 30那起步時(shí)誤差就是 -10MPC 會猛加速去追可能觸發(fā)v_max或者u_max。解決方法是改d_des的同時(shí)把初始位置按新間距重排或者給一個(gè)過渡過程讓隊(duì)列先收斂再開始工況。另外L是車長d_des是凈間距還是中心距要統(tǒng)一源碼里用的是「前車位置 - 后車位置 - d_des - L」意味著d_des是凈間距如果你按中心距理解就會差一個(gè)車長。5. 進(jìn)階技巧把三車隊(duì)列擴(kuò)展到更多車和通信受限場景三車跑通之后下一步通常是擴(kuò)到五車、十車或者加入通信延遲和丟包。擴(kuò)展車輛數(shù)時(shí)最直接的問題是quadprog的求解時(shí)間隨車輛數(shù)線性增長十輛車在Ts0.1下可能來不及。我一般會先把Np從 20 降到 10Nc從 5 降到 3犧牲一點(diǎn)預(yù)測精度換實(shí)時(shí)性。另一個(gè)技巧是把Q矩陣做成對角線上位置權(quán)重隨車輛序號遞減頭車附近權(quán)重高、尾部權(quán)重低這樣能抑制誤差放大。通信受限場景下如果鄰居的預(yù)測軌跡延遲了一拍可以在 MPC 里把參考軌跡往后推一個(gè)Ts相當(dāng)于用「舊信息」做預(yù)測。更穩(wěn)的做法是在代價(jià)函數(shù)里加一項(xiàng)對通信延遲的魯棒懲罰但這會顯著增加推導(dǎo)復(fù)雜度。我的習(xí)慣是先在仿真里加一個(gè)delay_steps參數(shù)從 0 試到 3看間距誤差退化到什么程度還能接受再決定要不要上魯棒 MPC。驗(yàn)證擴(kuò)展后的隊(duì)列是否穩(wěn)定除了看間距誤差還要看「弦穩(wěn)定性」指標(biāo)定義誤差傳遞比 $\gamma \max |e_{i1}| / \max |e_i|$如果 $\gamma 1$說明誤差沿隊(duì)列衰減隊(duì)列是弦穩(wěn)定的。這個(gè)指標(biāo)比單看某輛車的誤差曲線更能說明問題。我一般會在仿真腳本最后加一段自動(dòng)計(jì)算 $\gamma$ 的代碼每次改參數(shù)后直接看這個(gè)數(shù)比一張張翻圖快得多。最后說一個(gè)我踩過的坑不要一上來就追求「完全分布式」三輛車用分布式 MPC 的收益其實(shí)有限集中式 MPC 在車輛數(shù)少的時(shí)候更簡單也更穩(wěn)。分布式 MPC 的價(jià)值在車輛數(shù)多、通信拓?fù)湎∈琛⒅醒牍?jié)點(diǎn)不可靠的場景才真正體現(xiàn)。如果你只是做三車驗(yàn)證先把集中式跑通再改成分布式對比兩者的控制效果和計(jì)算時(shí)間這樣你對「分布式到底帶來了什么」會有更具體的判斷。希望幫到你。本文還有配套的精品資源點(diǎn)擊獲取 三車隊(duì)列分布式MPC控制算法：Matlab源碼實(shí)現(xiàn)與避坑指南
簡介這份資源面向自動(dòng)駕駛、智能交通與車輛控制方向的研究人員、工程師及學(xué)生提供一套多車隊(duì)列控制的分布式模型預(yù)測控制MPC實(shí)現(xiàn)方案重點(diǎn)解決三輛車在編隊(duì)行駛中的協(xié)同與穩(wěn)定性問題。壓縮包內(nèi)共2個(gè)文件包含1個(gè)m源碼文件與1個(gè)txt說明文件整體約2KB體量輕巧便于快速閱讀與二次開發(fā)。源碼圍繞分布式MPC的預(yù)測模型、滾動(dòng)優(yōu)化與車輛間協(xié)同約束展開可用于理解隊(duì)列一致性、間距保持與安全行駛的實(shí)現(xiàn)思路適合作為算法教學(xué)、課程設(shè)計(jì)或研發(fā)原型的參考。目前已有365人學(xué)習(xí)下載說明該方向具備一定關(guān)注度。讀者可從中獲取可運(yùn)行的MATLAB代碼框架與配套說明用于復(fù)現(xiàn)三車隊(duì)列控制效果、對比集中式與分布式策略差異并在此基礎(chǔ)上擴(kuò)展至更多車輛或復(fù)雜交通場景提升編隊(duì)穩(wěn)定性與協(xié)同性。1. 多車隊(duì)列控制算法三輛車分布式 MPC 到底在算什么高速公路上三輛車保持固定間距巡航前車一腳剎車后車如果只靠自己的雷達(dá)做 PID很容易出現(xiàn)「前車點(diǎn)頭、后車鞠躬」的隊(duì)列振蕩。多車隊(duì)列控制算法要解決的就是這件事讓每輛車在只拿到鄰居狀態(tài)的前提下仍然把車間距誤差收斂到零同時(shí)不違反加速度和通信帶寬約束。分布式模型預(yù)測控制Distributed MPC是當(dāng)前落地最穩(wěn)的一條路線——每輛車本地求解一個(gè)有限時(shí)域優(yōu)化問題只交換預(yù)測軌跡或狀態(tài)量而不是把全局信息塞給一個(gè)中央控制器。這套 matlab 源碼實(shí)現(xiàn)的就是三輛車的分布式 MPC 隊(duì)列適合做車輛編隊(duì)、AGV 協(xié)同、多智能體一致性研究的從業(yè)者拿來改參數(shù)、換拓?fù)?、?yàn)證自己的通信策略。它不追求花哨追求的是你能在本地跑通、看懂每一行代價(jià)函數(shù)在懲罰什么。2. 分布式 MPC 隊(duì)列建模從車輛動(dòng)力學(xué)到鄰居耦合項(xiàng)2.1 三輛車縱向動(dòng)力學(xué)為什么選離散線性模型真實(shí)車輛是強(qiáng)非線性系統(tǒng)但隊(duì)列控制里如果一上來就寫非線性 MPC求解時(shí)間會直接吃掉你的實(shí)時(shí)裕度。常見做法是在工作點(diǎn)附近做線性化再離散化。源碼里用的是三階狀態(tài)位置 $p$、速度 $v$、加速度 $a$輸入是加速度增量 $j$jerk。這樣狀態(tài)轉(zhuǎn)移矩陣可以寫成% 三車隊(duì)列離散線性模型參數(shù) Ts 0.1; % 采樣周期 100ms兼顧實(shí)時(shí)性與精度 tau 0.5; % 車輛動(dòng)力學(xué)的慣性時(shí)間常數(shù) A_c [0 1 0; 0 0 1; 0 0 -1/tau]; B_c [0; 0; 1/tau]; sys_c ss(A_c, B_c, [], []); sys_d c2d(sys_c, Ts, zoh); % 零階保持離散化 Ad sys_d.A; Bd sys_d.B;這段代碼的邏輯是先把連續(xù)時(shí)間狀態(tài)空間建出來再用零階保持做離散化。Ts0.1不是隨便選的隊(duì)列控制的通信周期通常在 50ms 到 200ms 之間太小會讓優(yōu)化問題頻繁求解、算力吃緊太大則離散誤差累積、間距控制發(fā)飄。tau0.5決定了加速度響應(yīng)的快慢改小會讓車更「賊」改大會讓車更「肉」這個(gè)參數(shù)直接決定你后面 MPC 的預(yù)測時(shí)域該取多長。注意c2d的zoh選項(xiàng)對應(yīng)的是執(zhí)行器在一個(gè)周期內(nèi)保持輸入不變?nèi)绻愕牡讓涌刂破魇侵苯咏o加速度指令而不是 jerk那 B 矩陣要重新推。2.2 隊(duì)列耦合間距誤差怎么進(jìn)代價(jià)函數(shù)分布式 MPC 的核心在于每輛車的優(yōu)化問題里只出現(xiàn)自己和前車的信息。源碼里第 $i$ 輛車的間距誤差定義為$$e_i(k) p_{i-1}(k) - p_i(k) - d_{des} - L$$其中 $d_{des}$ 是期望車間距$L$ 是車長。這個(gè)誤差進(jìn)入代價(jià)函數(shù)的方式?jīng)Q定了隊(duì)列的收斂行為。常見寫法是% 第 i 輛車的 MPC 代價(jià)函數(shù)預(yù)測時(shí)域 Np Np 20; Nc 5; % 預(yù)測20步控制5步 Q diag([10, 1, 0.1]); % 位置誤差權(quán)重大速度次之加速度最小 R 0.5; % 控制增量懲罰抑制抖動(dòng) % 構(gòu)造預(yù)測方程 [Phi, Gamma] predmat(Ad, Bd, Np, Nc); % 參考軌跡前車預(yù)測位置減去期望間距 ref pre_pos_pred - d_des - L; % 二次規(guī)劃求解 H Gamma * kron(eye(Np), Q) * Gamma R * eye(Nc); f Gamma * kron(eye(Np), Q) * (Phi * x0 - ref); U quadprog(H, f, [], [], [], [], u_min, u_max);Q矩陣?yán)镂恢谜`差權(quán)重給到 10是因?yàn)殛?duì)列控制最不能忍的就是間距偏差速度權(quán)重 1 是讓速度跟隨別太激進(jìn)加速度權(quán)重 0.1 是給舒適性留余地。R0.5是控制增量的懲罰這個(gè)值太小會讓加速度指令抖得像心電圖太大會讓響應(yīng)變慢、跟車距離拉大。Np20對應(yīng) 2 秒預(yù)測窗口Nc5是控制時(shí)域通常取預(yù)測時(shí)域的 1/4 到 1/3。quadprog是 matlab 優(yōu)化工具箱里的二次規(guī)劃求解器如果你沒有這個(gè)工具箱可以用mpc對象或者自己寫內(nèi)點(diǎn)法但源碼里默認(rèn)用的是quadprog。2.3 分布式迭代每輛車怎么用鄰居的預(yù)測軌跡分布式 MPC 不是每輛車各算各的就完事那樣會退化成獨(dú)立控制。源碼里用的是「預(yù)測軌跡交換 迭代修正」的框架第 $i$ 輛車在第 $m$ 次迭代時(shí)用第 $i-1$ 輛車上一輪廣播的預(yù)測位置序列作為參考求解自己的優(yōu)化問題然后把新的預(yù)測軌跡廣播給第 $i1$ 輛車。迭代兩到三次就能收斂到納什均衡附近。% 分布式迭代主循環(huán)簡化版 max_iter 3; for iter 1:max_iter for i 2:3 % 頭車不參與間距調(diào)節(jié) % 取前車上一輪預(yù)測位置 pre_pred neighbor_pred{i-1}; % 本地求解 MPC [u_opt, x_pred] solve_mpc(x0{i}, pre_pred, params); % 更新自己的預(yù)測軌跡 my_pred{i} x_pred; end % 交換預(yù)測軌跡 neighbor_pred my_pred; endmax_iter3是實(shí)測下來收斂和算力的折中迭代 1 次隊(duì)列誤差收斂慢迭代 5 次以上收益很小但計(jì)算量線性增長。注意頭車不參與間距調(diào)節(jié)它的軌跡是給定參考這是隊(duì)列控制的常見設(shè)定。如果你要做無頭車或者動(dòng)態(tài)領(lǐng)航這段邏輯要改。neighbor_pred的交換在仿真里是直接內(nèi)存?zhèn)鬟f實(shí)車部署時(shí)要換成通信中間件這時(shí)候通信延遲和丟包就會變成新的坑。3. 在 matlab 里跑通三車隊(duì)列從初始化到出圖3.1 仿真初始化狀態(tài)、參考軌跡和參數(shù)結(jié)構(gòu)體拿到源碼后第一件事不是直接 run而是把參數(shù)結(jié)構(gòu)體看清楚。源碼里通常用一個(gè)params結(jié)構(gòu)體把所有可調(diào)參數(shù)集中管理這樣你改車間距、改權(quán)重、改預(yù)測時(shí)域都不用翻遍腳本。% 初始化三車隊(duì)列仿真參數(shù) params.N 3; % 車輛數(shù) params.d_des 20; % 期望車間距 20m params.L 4.5; % 車長 4.5m params.Ts 0.1; % 采樣周期 params.Np 20; % 預(yù)測時(shí)域 params.Nc 5; % 控制時(shí)域 params.Q diag([10, 1, 0.1]); params.R 0.5; params.u_min -3; % jerk 下限 params.u_max 3; % jerk 上限 params.v_max 30; % 速度上限 30m/s % 初始狀態(tài)三輛車位置、速度、加速度 x0 [0 20 40; % 位置 20 20 20; % 速度 0 0 0]; % 加速度 % 頭車參考軌跡先勻速再減速再加速 t 0:params.Ts:30; v_ref 20 * ones(size(t)); v_ref(t10 t15) 15; % 減速段 v_ref(t20 t25) 25; % 加速段d_des20是車間距這個(gè)值要結(jié)合你的通信延遲和制動(dòng)能力來定20m 在 20m/s 下對應(yīng) 1 秒時(shí)距屬于比較保守的設(shè)定。u_min和u_max是 jerk 約束±3 m/s3 是乘用車舒適性邊界再大乘客會暈。v_ref構(gòu)造了一個(gè)包含減速和加速的工況用來驗(yàn)證隊(duì)列在動(dòng)態(tài)過程中的間距保持能力。注意初始位置是 0、20、40正好對應(yīng)期望間距這樣起步時(shí)誤差為零方便你觀察擾動(dòng)后的收斂過程。3.2 主循環(huán)每一步求解、施加控制、更新狀態(tài)仿真主循環(huán)的結(jié)構(gòu)很固定對每個(gè)時(shí)刻先更新頭車狀態(tài)再讓每輛車依次求解 MPC施加控制量最后更新狀態(tài)并存儲數(shù)據(jù)。% 仿真主循環(huán) Nsim length(t); x_hist zeros(3, params.N, Nsim); % 位置歷史 u_hist zeros(1, params.N, Nsim); % 控制歷史 x_hist(:,:,1) x0; for k 1:Nsim-1 % 頭車跟蹤參考速度 x_hist(2,1,k1) v_ref(k1); x_hist(1,1,k1) x_hist(1,1,k) v_ref(k1)*params.Ts; % 跟車分布式 MPC for i 2:params.N x_cur x_hist(:,i,k); pre_pos x_hist(1,i-1,k1); % 前車當(dāng)前位置 [u_opt, x_pred] solve_mpc(x_cur, pre_pos, params); u_hist(:,i,k) u_opt(1); % 用第一組控制量更新狀態(tài) x_hist(:,i,k1) Ad * x_cur Bd * u_opt(1); end end這段代碼里有個(gè)容易翻車的地方頭車狀態(tài)更新和跟車 MPC 求解的順序。源碼里頭車先更新跟車再用前車的新位置做參考這對應(yīng)「前車信息即時(shí)可用」的假設(shè)。實(shí)車中通信有延遲前車位置往往是上一周期的這時(shí)候要么在 MPC 里做延遲補(bǔ)償要么把參考軌跡往后推一拍。u_opt(1)只取第一個(gè)控制量施加這是滾動(dòng)時(shí)域的標(biāo)準(zhǔn)做法后面預(yù)測的控制量丟掉不用。Ad和Bd是前面離散化得到的矩陣這里直接做狀態(tài)更新沒有再走一遍quadprog。3.3 出圖與驗(yàn)證間距誤差曲線怎么看跑完仿真后最關(guān)鍵的驗(yàn)證圖是間距誤差隨時(shí)間的變化。源碼里一般會畫三張圖位置軌跡、速度軌跡、間距誤差。% 繪制間距誤差 figure; for i 2:params.N err x_hist(1,i-1,:) - x_hist(1,i,:) - params.d_des - params.L; plot(t, squeeze(err), LineWidth, 1.5); hold on; end xlabel(時(shí)間 (s)); ylabel(間距誤差 (m)); legend(車2-車1, 車3-車2); grid on; % 繪制控制量 figure; for i 2:params.N plot(t, squeeze(u_hist(:,i,:)), LineWidth, 1.5); hold on; end xlabel(時(shí)間 (s)); ylabel(jerk (m/s^3)); legend(車2, 車3); grid on;看間距誤差曲線時(shí)關(guān)注三點(diǎn)一是擾動(dòng)后誤差是否回到零二是收斂過程有沒有超調(diào)三是車 3 的誤差是否比車 2 大。如果車 3 誤差明顯放大說明隊(duì)列的「弦穩(wěn)定性」沒滿足需要調(diào)大Q里的位置權(quán)重或者增加預(yù)測時(shí)域?？刂屏繄D看的是 jerk 有沒有頻繁觸邊界如果一直在 ±3 附近跳說明R太小或者Nc太短。注意squeeze是用來去掉單維度因?yàn)閤_hist是三維數(shù)組直接 plot 會報(bào)維度錯(cuò)誤這是 matlab 里很常見的坑。4. 避坑與排查三車隊(duì)列 MPC 最容易翻車的五個(gè)地方4.1 現(xiàn)象間距誤差不收斂反而發(fā)散原因通常是分布式迭代的參考軌跡用錯(cuò)了。如果每輛車都用前車「當(dāng)前時(shí)刻」的位置而不是「預(yù)測軌跡」那 MPC 就退化成了單步跟隨預(yù)測時(shí)域白設(shè)了。解決方法是檢查neighbor_pred的傳遞邏輯確保第 $i$ 輛車拿到的是第 $i-1$ 輛車上一輪迭代的完整預(yù)測序列而不是只拿一個(gè)點(diǎn)。另外檢查Q矩陣?yán)镂恢谜`差的符號如果寫成 $p_i - p_{i-1}$ 而不是 $p_{i-1} - p_i$誤差方向反了控制器會往相反方向調(diào)。4.2 現(xiàn)象quadprog 報(bào)錯(cuò)「H 矩陣非正定」原因一般是H Gamma * kron(eye(Np), Q) * Gamma R * eye(Nc)里R給成了負(fù)數(shù)或者零。R是控制增量懲罰必須正定否則二次規(guī)劃沒有唯一解。解決方法是檢查R的賦值確保它是正標(biāo)量。另一個(gè)可能是Gamma矩陣構(gòu)造時(shí)Np和Nc搞混了導(dǎo)致維度不匹配kron出來的矩陣不是方陣。建議在predmat函數(shù)里加一行assert(size(Gamma,2) Nc)做防御。4.3 現(xiàn)象仿真跑得動(dòng)但實(shí)車抖動(dòng)厲害原因是Ts和Np的匹配問題。Ts0.1、Np20對應(yīng) 2 秒預(yù)測窗口但如果你的底層執(zhí)行器響應(yīng)時(shí)間超過 100msMPC 算出來的控制量還沒執(zhí)行完新的控制量就來了兩個(gè)指令疊加就會抖。解決方法是把Ts調(diào)到和執(zhí)行器周期一致或者加一個(gè)低通濾波器在控制量輸出前。另一個(gè)常見原因是u_min和u_max給得太寬jerk 允許到 ±10車當(dāng)然會竄。先收到 ±3 試試。4.4 現(xiàn)象車 3 的間距誤差始終比車 2 大這是隊(duì)列控制的「誤差傳播」問題不一定是 bug。如果車 3 誤差是車 2 的 1.5 倍以內(nèi)屬于正常范圍如果超過 3 倍說明弦穩(wěn)定性不滿足。解決方法是增大Np或者調(diào)大Q里的位置權(quán)重。源碼里Qdiag([10,1,0.1])是一個(gè)比較溫和的設(shè)定你可以試diag([50,5,0.5])但注意權(quán)重太大會讓控制量更容易觸邊界反而引起振蕩。另一個(gè)辦法是給車 3 單獨(dú)設(shè)一組參數(shù)但這樣就不是「分布式」了屬于工程折中。4.5 現(xiàn)象換車間距后仿真結(jié)果完全不對原因是d_des改了但初始狀態(tài)沒改。如果初始位置還是 0、20、40而d_des改成 30那起步時(shí)誤差就是 -10MPC 會猛加速去追可能觸發(fā)v_max或者u_max。解決方法是改d_des的同時(shí)把初始位置按新間距重排或者給一個(gè)過渡過程讓隊(duì)列先收斂再開始工況。另外L是車長d_des是凈間距還是中心距要統(tǒng)一源碼里用的是「前車位置 - 后車位置 - d_des - L」意味著d_des是凈間距如果你按中心距理解就會差一個(gè)車長。5. 進(jìn)階技巧把三車隊(duì)列擴(kuò)展到更多車和通信受限場景三車跑通之后下一步通常是擴(kuò)到五車、十車或者加入通信延遲和丟包。擴(kuò)展車輛數(shù)時(shí)最直接的問題是quadprog的求解時(shí)間隨車輛數(shù)線性增長十輛車在Ts0.1下可能來不及。我一般會先把Np從 20 降到 10Nc從 5 降到 3犧牲一點(diǎn)預(yù)測精度換實(shí)時(shí)性。另一個(gè)技巧是把Q矩陣做成對角線上位置權(quán)重隨車輛序號遞減頭車附近權(quán)重高、尾部權(quán)重低這樣能抑制誤差放大。通信受限場景下如果鄰居的預(yù)測軌跡延遲了一拍可以在 MPC 里把參考軌跡往后推一個(gè)Ts相當(dāng)于用「舊信息」做預(yù)測。更穩(wěn)的做法是在代價(jià)函數(shù)里加一項(xiàng)對通信延遲的魯棒懲罰但這會顯著增加推導(dǎo)復(fù)雜度。我的習(xí)慣是先在仿真里加一個(gè)delay_steps參數(shù)從 0 試到 3看間距誤差退化到什么程度還能接受再決定要不要上魯棒 MPC。驗(yàn)證擴(kuò)展后的隊(duì)列是否穩(wěn)定除了看間距誤差還要看「弦穩(wěn)定性」指標(biāo)定義誤差傳遞比 $\gamma \max |e_{i1}| / \max |e_i|$如果 $\gamma 1$說明誤差沿隊(duì)列衰減隊(duì)列是弦穩(wěn)定的。這個(gè)指標(biāo)比單看某輛車的誤差曲線更能說明問題。我一般會在仿真腳本最后加一段自動(dòng)計(jì)算 $\gamma$ 的代碼每次改參數(shù)后直接看這個(gè)數(shù)比一張張翻圖快得多。最后說一個(gè)我踩過的坑不要一上來就追求「完全分布式」三輛車用分布式 MPC 的收益其實(shí)有限集中式 MPC 在車輛數(shù)少的時(shí)候更簡單也更穩(wěn)。分布式 MPC 的價(jià)值在車輛數(shù)多、通信拓?fù)湎∈琛⒅醒牍?jié)點(diǎn)不可靠的場景才真正體現(xiàn)。如果你只是做三車驗(yàn)證先把集中式跑通再改成分布式對比兩者的控制效果和計(jì)算時(shí)間這樣你對「分布式到底帶來了什么」會有更具體的判斷。希望幫到你。本文還有配套的精品資源點(diǎn)擊獲取