在自動(dòng)化控制這個(gè)圈子里摸爬滾打了這么多年我發(fā)現(xiàn)一個(gè)很有意思的現(xiàn)象凡是涉及軌跡跟蹤、路徑規(guī)劃、約束優(yōu)化的項(xiàng)目最后幾乎都會(huì)繞到MPC模型預(yù)測(cè)控制這個(gè)算法上。不管是無(wú)人駕駛車(chē)輛的橫向控制、機(jī)械臂的柔順抓取還是溫控系統(tǒng)的節(jié)能調(diào)度MPC就像是一個(gè)“能預(yù)知未來(lái)的控制器”通過(guò)滾動(dòng)求解一個(gè)有限時(shí)域的優(yōu)化問(wèn)題讓系統(tǒng)在滿足各種硬約束的前提下輸出最優(yōu)的控制動(dòng)作。今天這篇文章我打算把MPC這塊硬骨頭從頭到尾啃一遍從最底層的原理推導(dǎo)到代碼層面的逐步實(shí)現(xiàn)把我自己踩過(guò)的坑和總結(jié)的經(jīng)驗(yàn)全部攤開(kāi)來(lái)講清楚希望能給正在入門(mén)或卡在瓶頸期的你一點(diǎn)實(shí)在的幫助。先說(shuō)下這篇東西是寫(xiě)給誰(shuí)看的如果你剛接觸MPC被滿屏的Hessian矩陣、KKT條件搞得頭大可以在這里找到一條從感性到理性的認(rèn)知路徑如果你已經(jīng)能用現(xiàn)成工具箱跑通Demo但對(duì)“為什么參數(shù)要這么調(diào)”心存疑惑這篇文章會(huì)給你一些實(shí)用的判斷依據(jù)如果你正準(zhǔn)備在嵌入式設(shè)備或者實(shí)時(shí)系統(tǒng)里部署MPC那么第四章的代碼實(shí)現(xiàn)和第六章的工程化建議可能正是你需要的。1. MPC的核心原理與直覺(jué)理解1.1 為什么傳統(tǒng)PID不夠用而MPC更強(qiáng)要理解MPC我覺(jué)得最好的切入點(diǎn)是先搞清楚它解決了什么痛點(diǎn)。傳統(tǒng)的PID控制器是“事后調(diào)節(jié)”它只根據(jù)當(dāng)前的誤差來(lái)決策參考你的目標(biāo)是讓輸出值等于設(shè)定值它只關(guān)心你現(xiàn)在跑偏了多少不關(guān)心未來(lái)會(huì)不會(huì)撞墻。舉個(gè)例子你開(kāi)車(chē)時(shí)如果只看著當(dāng)前車(chē)距來(lái)踩油門(mén)剎車(chē)遇到急轉(zhuǎn)彎或前車(chē)突然減速的情況反應(yīng)往往慢了半拍——因?yàn)槟銢](méi)有“預(yù)測(cè)”未來(lái)很短時(shí)間內(nèi)的趨勢(shì)。而MPC的思想完全是另一個(gè)維度它是“事前規(guī)劃”。它的每一步控制決策都會(huì)基于一個(gè)預(yù)測(cè)模型去推算系統(tǒng)在未來(lái)一段時(shí)間比如未來(lái)2秒的狀態(tài)軌跡然后在這段預(yù)測(cè)的范圍內(nèi)優(yōu)化出一串控制序列比如未來(lái)2秒內(nèi)每0.1秒踩多少油門(mén)并且強(qiáng)制要求每一個(gè)時(shí)刻的狀態(tài)都滿足約束比如速度不能超過(guò)80加速度不能超過(guò)2。執(zhí)行完這一步后系統(tǒng)會(huì)把控制序列的第一段指令發(fā)給執(zhí)行器然后整個(gè)流程滾動(dòng)地重復(fù)。這套機(jī)制帶來(lái)的直接好處有三個(gè)第一系統(tǒng)性約束處理變得非常自然你可以把“前車(chē)距離不能少于5米”這種硬約束直接寫(xiě)進(jìn)優(yōu)化問(wèn)題里而不是像PID那樣靠限幅或分段邏輯去逼近第二控制品質(zhì)顯著提升因?yàn)樗跁r(shí)間軸上統(tǒng)籌了未來(lái)一段時(shí)間的代價(jià)不會(huì)為了眼前的小偏差而犧牲后續(xù)的性能第三它天然適合多輸入多輸出MIMO系統(tǒng)比如四輪獨(dú)立驅(qū)動(dòng)的車(chē)輛四個(gè)輪子的扭矩分配是一個(gè)典型的耦合優(yōu)化問(wèn)題很難用單回路PID去解耦。當(dāng)然MPC也不是沒(méi)有代價(jià)它的代價(jià)是計(jì)算量大。本質(zhì)上你每個(gè)控制周期都要求解一個(gè)帶約束的最優(yōu)化問(wèn)題這在算力有限的單片機(jī)上往往是瓶頸。所以很多工業(yè)現(xiàn)場(chǎng)會(huì)出現(xiàn)“幾十毫秒算一次MPC”的場(chǎng)景這已經(jīng)算是不錯(cuò)的表現(xiàn)了。1.2 三要素預(yù)測(cè)模型、滾動(dòng)優(yōu)化、反饋校正拆開(kāi)“模型預(yù)測(cè)控制”這六個(gè)字核心其實(shí)就三件事預(yù)測(cè)模型、滾動(dòng)優(yōu)化、反饋校正。這三個(gè)要素缺一不可。預(yù)測(cè)模型顧名思義是用來(lái)模擬系統(tǒng)動(dòng)態(tài)行為的數(shù)學(xué)模型。在工程實(shí)現(xiàn)中最常用的是線性狀態(tài)空間模型x(k1) A x(k) B u(k)因?yàn)樗銐蚝?jiǎn)單、有成熟的理論支持而且可以直接套用線性MPC框架。如果系統(tǒng)非線性很強(qiáng)比如機(jī)械臂的動(dòng)力學(xué)模型或者自動(dòng)駕駛中的輪胎摩擦模型我們可以使用線性時(shí)變MPC在每個(gè)采樣時(shí)刻把非線性模型線性化得到一個(gè)局部的線性方程。滾動(dòng)優(yōu)化是整個(gè)MPC的靈魂。它不是說(shuō)“我一次性算好未來(lái)10步的動(dòng)作然后全部執(zhí)行”而是只執(zhí)行當(dāng)前這一步下一時(shí)刻重新測(cè)量狀態(tài)、重新預(yù)測(cè)、重新求解。這樣當(dāng)系統(tǒng)受到新的擾動(dòng)或模型失配時(shí)控制器能及時(shí)修正不會(huì)一條道走到黑。這個(gè)“滾動(dòng)”的動(dòng)作本質(zhì)上是一種帶前瞻的閉環(huán)反饋策略而不是開(kāi)環(huán)的“放電影”。反饋校正則是把模型預(yù)測(cè)和實(shí)際系統(tǒng)之間的差異考慮進(jìn)來(lái)的環(huán)節(jié)。因?yàn)槟Ｐ陀肋h(yuǎn)不可能完美預(yù)測(cè)軌跡和真實(shí)軌跡總會(huì)存在偏差所以MPC的做法是在每次采樣時(shí)用傳感器反饋的實(shí)際測(cè)量值比如當(dāng)前位置、當(dāng)前速度替換掉模型預(yù)測(cè)中的對(duì)應(yīng)狀態(tài)然后重新開(kāi)始新一輪預(yù)測(cè)。這一步保證了系統(tǒng)不會(huì)因?yàn)槟Ｐ驼`差而持續(xù)發(fā)散是MPC能夠落地的關(guān)鍵“安全網(wǎng)”。1.3 從直覺(jué)到數(shù)學(xué)優(yōu)化問(wèn)題長(zhǎng)什么樣如果把MPC的直覺(jué)轉(zhuǎn)化成數(shù)學(xué)語(yǔ)言你會(huì)看到這樣一個(gè)形式化的優(yōu)化問(wèn)題在每個(gè)控制時(shí)刻 k已知當(dāng)前狀態(tài)x(k)我們要求解以下問(wèn)題min J sum_{i0}^{N-1} [ (x_{ki} - x_ref_{ki}) Q (x_{ki} - x_ref_{ki}) u_{ki} R u_{ki} ] (x_{kN} - x_ref) P (x_{kN} - x_ref) s.t. x_{ki1} A x_{ki} B u_{ki} x_{ki} ∈ X_constraint u_{ki} ∈ U_constraint for i 0, 1, ..., N-1這里N是預(yù)測(cè)時(shí)域我們可以看未來(lái)多少步Q和R是對(duì)狀態(tài)誤差和控制量的權(quán)重矩陣P是末端代價(jià)權(quán)重。Q越大說(shuō)明我越在意狀態(tài)跟蹤的精度R越大說(shuō)明我越在意控制動(dòng)作的平穩(wěn)性。X_constraint和U_constraint是狀態(tài)和輸入的上下界約束。求解完這個(gè)帶約束的二次規(guī)劃問(wèn)題QP后我們會(huì)得到一個(gè)最優(yōu)控制序列u(k), u(k1), ..., u(kN-1)然后只提取第一步u(k)應(yīng)用到被控對(duì)象上。等到下一個(gè)采樣周期用新的測(cè)量值作為初始狀態(tài)再次滾動(dòng)求解。這個(gè)流程就是MPC的完整閉環(huán)。值得一提的細(xì)節(jié)是預(yù)測(cè)時(shí)域N的選取。N太小比如N1或2MPC就退化成近似貪婪控制提前量的價(jià)值大打折扣N太大優(yōu)化問(wèn)題的變量數(shù)和約束數(shù)爆炸求解時(shí)間直線上升。實(shí)際工程中N的經(jīng)驗(yàn)取值通常是讓預(yù)測(cè)時(shí)長(zhǎng)覆蓋系統(tǒng)動(dòng)態(tài)的上升時(shí)間比如一個(gè)溫度系統(tǒng)的時(shí)滯是2分鐘上升時(shí)間是5分鐘那么預(yù)測(cè)時(shí)域N至少要讓總預(yù)測(cè)時(shí)長(zhǎng)達(dá)到3~5分鐘以上否則前瞻性不夠。2. 從零搭建MPC數(shù)學(xué)模型與設(shè)計(jì)流程詳解2.1 狀態(tài)空間模型這是MPC的“眼睛”MPC是一次典型的“模型驅(qū)動(dòng)”的控制方法所以建模這一步至關(guān)重要。如果你連對(duì)象的基本動(dòng)態(tài)都描述不準(zhǔn)那后面算出來(lái)的控制序列基本是瞎指揮。對(duì)于很多實(shí)際項(xiàng)目來(lái)說(shuō)我們需要的不是精確到每一個(gè)非線性細(xì)節(jié)的復(fù)雜模型而是一個(gè)能在工作點(diǎn)附近足夠準(zhǔn)確的線性模型——這就是為什么在工業(yè)現(xiàn)場(chǎng)MPC最常用的模型就是離散狀態(tài)空間模型。建模思路通常是兩條路線機(jī)理建模和數(shù)據(jù)辨識(shí)。機(jī)理建模是從物理定律出發(fā)推到運(yùn)動(dòng)方程比如根據(jù)牛頓第二定律推出小車(chē)的速度加速度關(guān)系根據(jù)熱力學(xué)方程推出溫度的升溫冷卻規(guī)律數(shù)據(jù)辨識(shí)則是拿輸入輸出數(shù)據(jù)來(lái)擬合系統(tǒng)參數(shù)常用的方法是子空間辨識(shí)或最小二乘估計(jì)。工程中往往是兩條路結(jié)合用機(jī)理確定結(jié)構(gòu)用數(shù)據(jù)校準(zhǔn)參數(shù)。舉個(gè)非常經(jīng)典的例子假設(shè)我們要控制一輛小車(chē)的縱向速度設(shè)狀態(tài)量為x [位置, 速度]輸入為u [驅(qū)動(dòng)力]。忽略空氣阻力時(shí)離散化后的狀態(tài)空間方程可以寫(xiě)成A [[1, dt], [0, 1]] B [[0], [dt / m]] C [[1, 0]] D [[0]]這個(gè)dt是采樣周期m是車(chē)的質(zhì)量。你可以看到A矩陣描述了“位置會(huì)隨著時(shí)間被速度累積”B矩陣描述了“力通過(guò)加速度影響下一時(shí)刻的速度”。有了A和BMPC就能在時(shí)間軸上推演狀態(tài)了。2.2 MPC設(shè)計(jì)流程七個(gè)步驟一條龍如果你的項(xiàng)目準(zhǔn)備上MPC我建議你按照下面這七步流程走基本不會(huì)跑偏。第一步明確控制目標(biāo)。你是想讓輸出跟蹤一個(gè)參考軌跡還是讓狀態(tài)穩(wěn)定在一個(gè)設(shè)定點(diǎn)或者是在兩個(gè)目標(biāo)之間做最優(yōu)切換目標(biāo)不同代價(jià)函數(shù)定義就不同。第二步確定采樣周期和預(yù)測(cè)時(shí)域。采樣周期上限沒(méi)有特定要求但至少要比系統(tǒng)動(dòng)態(tài)的上升時(shí)間快5到10倍比如目標(biāo)是控制機(jī)械臂末端運(yùn)動(dòng)上升時(shí)間是0.5秒采樣周期建議在10~50毫秒之間。預(yù)測(cè)時(shí)域的預(yù)測(cè)時(shí)長(zhǎng)通常設(shè)置為上升時(shí)間的2~3倍然后轉(zhuǎn)換成步數(shù)N。第三步建立被控對(duì)象的數(shù)學(xué)模型。這個(gè)模型可以是線性的也可以是非線性的。如果模型是非線性的但工作范圍不大盡量在工作點(diǎn)做線性化。第四步構(gòu)造約束條件。把所有物理限制列全執(zhí)行器飽和扭矩限幅、閥門(mén)開(kāi)度限幅、狀態(tài)安全界限溫度上限、位置邊界、變化率限制加速度不能太猛。約束寫(xiě)得越準(zhǔn)確MPC的優(yōu)勢(shì)越明顯。第五步設(shè)計(jì)代價(jià)函數(shù)權(quán)重。Q和R的初值可以按“布里森法則”來(lái)定Q diag(1/max_deviation^2)R diag(1/max_u^2)意思是讓不同量綱的誤差和控制量在歸一化之后具有可比性。這一步往往會(huì)反復(fù)調(diào)整后面我會(huì)專(zhuān)門(mén)講怎么調(diào)權(quán)重。第六步選擇求解器并實(shí)現(xiàn)代碼。在線的QP求解器有很多選擇比如OSQP、qpOASES、CVXGEN后面代碼部分會(huì)詳解。第七步仿真驗(yàn)證與硬件部署。先在Simulink或者Python仿真環(huán)境里跑通再移植到嵌入式平臺(tái)同時(shí)做好數(shù)據(jù)標(biāo)定和在線調(diào)參。2.3 一個(gè)DP問(wèn)題的補(bǔ)充為什么Q和R矩陣要用對(duì)角陣在設(shè)計(jì)代價(jià)函數(shù)時(shí)Q和R矩陣最常用的形式是對(duì)角陣比如Q diag([q1, q2])。這背后的邏輯是我們通常假設(shè)各個(gè)狀態(tài)量之間、各個(gè)控制量之間的代價(jià)可以解耦也就是位置偏差的代價(jià)和速度偏差的代價(jià)是互不干擾的。如果你發(fā)現(xiàn)狀態(tài)之間存在強(qiáng)烈的耦合關(guān)系比如同時(shí)減小位置誤差和速度誤差會(huì)互相矛盾那就需要引入Q的非對(duì)角元素形成一個(gè)全矩陣權(quán)重不過(guò)這會(huì)讓QP問(wèn)題的數(shù)值性變差工程上一般不輕易這么干。R矩陣通常就設(shè)成對(duì)角線上的標(biāo)量代表我們對(duì)控制量的“客氣程度”。R太大控制器會(huì)變得溫吞吞的響應(yīng)慢R太小控制器會(huì)特別激進(jìn)可能頻繁觸達(dá)約束邊界。這個(gè)參數(shù)直接影響閉環(huán)系統(tǒng)的魯棒性和執(zhí)行器壽命調(diào)參時(shí)切忌“猛拉猛放”。Q矩陣中如果你特別在意穩(wěn)態(tài)精度可以把對(duì)應(yīng)狀態(tài)的Q設(shè)大如果你更在意過(guò)程的平穩(wěn)性避免大起大落就應(yīng)該相對(duì)調(diào)大R或調(diào)小Q。3. 完整代碼實(shí)現(xiàn)Python一步步跑通MPC3.1 準(zhǔn)備工作安裝必要的Python庫(kù)在實(shí)際寫(xiě)MPC之前需要確認(rèn)環(huán)境已經(jīng)裝好以下幾樣工具numpy矩陣運(yùn)算和狀態(tài)空間更新的基礎(chǔ)scipy有一些優(yōu)化算法可以作為備選osqp一個(gè)非?？斓膬?nèi)點(diǎn)法QP求解器針對(duì)稀疏矩陣做了大量?jī)?yōu)化和cvxpy配合起來(lái)體驗(yàn)極佳cvxpy一個(gè)建模范式很強(qiáng)的凸優(yōu)化庫(kù)可以在一行內(nèi)描述目標(biāo)函數(shù)和約束適合快速原型驗(yàn)證control這是一個(gè)工具箱可以方便地創(chuàng)建狀態(tài)空間模型、計(jì)算階躍響應(yīng)幫助觀察模型特性安裝命令很簡(jiǎn)單pip install numpy scipy osqp cvxpy control如果你的環(huán)境比較干凈這一套裝下來(lái)通常不會(huì)遇到什么大坑。偶爾可能有cvxpy和osqp的版本兼容問(wèn)題如果發(fā)現(xiàn)導(dǎo)入失敗試試先卸載再重裝pip uninstall osqp cvxpy pip install osqp cvxpy。3.2 雙積分器對(duì)象的MPC示例代碼逐行拆解為了讓你能在一分鐘之內(nèi)看懂MPC的代碼骨架我選了一個(gè)最經(jīng)典的“雙積分器”對(duì)象它的狀態(tài)方程是x1 x2, x2 u物理上可以通俗理解為“位置由速度決定速度由加速度決定”的一維慣性系統(tǒng)。很多伺服電機(jī)的位置環(huán)都可以近似成這個(gè)模型。以下是完整的MPC控制主循環(huán)代碼import numpy as np import cvxpy as cp import matplotlib.pyplot as plt # 系統(tǒng)參數(shù) dt 0.05 # 采樣周期50ms m 1.0 # 質(zhì)量歸一化 N 20 # 預(yù)測(cè)時(shí)域步數(shù) T 10 # 仿真總時(shí)長(zhǎng)秒 steps int(T / dt) # 仿真步數(shù) # 離散狀態(tài)空間模型x(k1) A x(k) B u(k) A np.array([[1, dt], [0, 1]], dtypenp.float64) B np.array([[0], [dt / m]], dtypenp.float64) nx A.shape[0] # 狀態(tài)數(shù) nu B.shape[1] # 控制量數(shù) # 約束定義 u_max 2.0 x_max 5.0 # 權(quán)重矩陣 Q np.diag([2.0, 0.1]) # 更關(guān)心位置誤差速度權(quán)重相對(duì)低 R np.array([[0.1]]) # 控制量代價(jià) # 目標(biāo)參考狀態(tài)期望把位置控制在1.0處穩(wěn)定不動(dòng) x_ref np.array([1.0, 0.0]) # 初始化 x np.array([0.0, 0.0]) # 初始狀態(tài)位置0速度0 u_log [] x_log [x.copy()] # 定義優(yōu)化變量 X cp.Variable((nx, N1)) # 狀態(tài)變量軌跡N1個(gè)時(shí)刻的狀態(tài) U cp.Variable((nu, N)) # 控制變量軌跡N個(gè)時(shí)刻的控制 # MPC主循環(huán) for k in range(steps): # 代價(jià)函數(shù)跟蹤誤差 控制代價(jià) cost 0 constraints [] for i in range(N): cost cp.quad_form(X[:, i] - x_ref, Q) cp.quad_form(U[:, i], R) cost cp.quad_form(X[:, N] - x_ref, Q) # 末端代價(jià)增加穩(wěn)定性 # 動(dòng)態(tài)約束狀態(tài)轉(zhuǎn)移方程 for i in range(N): constraints.append(X[:, i1] A X[:, i] B U[:, i]) # 初始狀態(tài)約束 constraints.append(X[:, 0] x) # 輸入約束和狀態(tài)約束 for i in range(N): constraints.append(cp.abs(U[:, i]) u_max) constraints.append(cp.abs(X[:, i]) x_max) # 求解QP問(wèn)題 prob cp.Problem(cp.Minimize(cost), constraints) prob.solve(solvercp.OSQP, verboseFalse) # 檢查求解是否成功 if prob.status not in [optimal, optimal_inaccurate]: print(fStep {k}: QP solve failed with status {prob.status}) break # 取第一個(gè)控制量應(yīng)用到系統(tǒng) u_applied U[:, 0].value u_log.append(u_applied) # 用真實(shí)系統(tǒng)這里也用同一模型模擬更新?tīng)顟B(tài) x A x (B u_applied).flatten() x_log.append(x.copy()) # 繪圖展示結(jié)果 x_log np.array(x_log) u_log np.array(u_log) plt.figure(figsize(10,6)) plt.subplot(2,1,1) plt.plot(x_log[:,0], labelposition) plt.plot(x_log[:,1], labelvelocity) plt.axhline(yx_ref[0], colorgray, linestyle--, labelref position) plt.ylabel(states) plt.legend() plt.subplot(2,1,2) plt.step(range(len(u_log)), u_log, wherepost, labelcontrol input) plt.ylabel(u) plt.xlabel(step) plt.legend() plt.show()這段代碼是整個(gè)MPC實(shí)現(xiàn)最簡(jiǎn)練的版本你跟著跑一遍應(yīng)該能看到位置從0平滑收斂到1.0速度先升后降、最終歸零控制量始終被鉗在[-2, 2]的范圍內(nèi)。這正是MPC約束處理能力的直觀體現(xiàn)如果換成沒(méi)有約束的LQR控制器在初始位置離參考點(diǎn)很遠(yuǎn)時(shí)控制量會(huì)瞬間暴漲到遠(yuǎn)超執(zhí)行器上限的值。3.3 代碼背后的幾個(gè)坑求解失敗、不可行和數(shù)值病態(tài)跑上面的代碼時(shí)你可能會(huì)遇到幾個(gè)問(wèn)題我先提前打個(gè)預(yù)防針。第一個(gè)坑是QP求解失敗。常見(jiàn)原因包括約束沖突比如兩個(gè)狀態(tài)約束互相擠兌、預(yù)測(cè)時(shí)域過(guò)大導(dǎo)致數(shù)值不穩(wěn)定、或者是初始狀態(tài)正好落在約束邊界上。排查思路是先把約束全部放寬一點(diǎn)看看問(wèn)題是否可解然后逐步收緊找到瓶頸。第二個(gè)坑是代價(jià)函數(shù)權(quán)重差異過(guò)大。如果你把Q設(shè)成diag([1e10, 1e-10])優(yōu)化求解器精度的數(shù)值范圍遠(yuǎn)超合理區(qū)間很可能出現(xiàn)“明明每個(gè)變量都標(biāo)了0.1但優(yōu)化出來(lái)的軌跡卻震蕩得很厲害”這種病態(tài)問(wèn)題。解決辦法是先做量綱歸一化再合理縮放權(quán)重。第三個(gè)坑是用真實(shí)系統(tǒng)模型和控制器模型不一致時(shí)的“模型失配”。仿真里我用同一個(gè)A和B模擬真實(shí)對(duì)象在現(xiàn)實(shí)中這是不可能的。建議測(cè)試時(shí)給真實(shí)模型加一點(diǎn)隨機(jī)擾動(dòng)或參數(shù)漂移再觀察MPC還能不能把系統(tǒng)穩(wěn)住。提示如果你第一次跑仿真發(fā)現(xiàn)控制量在觸達(dá)約束邊界時(shí)出現(xiàn)抖動(dòng)先別急著懷疑MPC的穩(wěn)定性很可能是QP求解器的收斂精度設(shè)置得太緊或太松。試試把OSQP的eps_abs和eps_rel調(diào)到1e-4左右通常能緩解這個(gè)問(wèn)題。3.4 進(jìn)階非線性系統(tǒng)的線性時(shí)變MPC如果被控對(duì)象是非線性的比如倒立擺、四旋翼、車(chē)輛運(yùn)動(dòng)學(xué)模型直接用上面的線性MPC是行不通的。業(yè)界常用的手段是線性時(shí)變MPCLTV-MPC在每個(gè)采樣時(shí)刻把非線性模型在當(dāng)前狀態(tài)和輸入工作點(diǎn)附近做一階泰勒展開(kāi)得到一個(gè)局部的線性模型然后基于這個(gè)局部模型求解MPC問(wèn)題。舉例來(lái)說(shuō)車(chē)輛運(yùn)動(dòng)學(xué)模型x v cos(theta) y v sin(theta) theta v / L tan(delta)其中(x,y)是車(chē)輛位置theta是航向角v是縱向速度delta是前輪轉(zhuǎn)角。在每一時(shí)刻把速度和航向角固定在當(dāng)前值對(duì)狀態(tài)做線性化就能得到隨時(shí)間變化的A和B矩陣。然后把之前代碼中的A和B替換成每個(gè)采樣周期重新計(jì)算的A_k和B_k其余流程不變。這種做法的計(jì)算量會(huì)大一些因?yàn)槊總€(gè)周期都要重新組裝約束矩陣但效果比單點(diǎn)線性化好得多尤其是在大范圍軌跡跟蹤場(chǎng)景中幾乎是指定動(dòng)作。4. MPC的關(guān)鍵參數(shù)調(diào)優(yōu)讓控制器“活”起來(lái)4.1 預(yù)測(cè)時(shí)域N短則激進(jìn)長(zhǎng)則遲鈍預(yù)測(cè)時(shí)域N是MPC里最有手感的超參數(shù)之一。當(dāng)你把N調(diào)得很短比如只有3步每步預(yù)測(cè)總的0.15秒MPC只能看到很近的未來(lái)它的行為會(huì)非常短視——為了盡快消除當(dāng)前誤差會(huì)猛加油門(mén)或猛打方向表現(xiàn)接近貪婪控制。當(dāng)你把N拉得很長(zhǎng)比如100步MPC看得遠(yuǎn)控制動(dòng)作會(huì)變得非常平滑但代價(jià)是計(jì)算量猛增而且太長(zhǎng)的預(yù)測(cè)時(shí)域會(huì)讓較遠(yuǎn)的預(yù)測(cè)軌跡跟實(shí)際跑出的軌跡產(chǎn)生巨大偏差優(yōu)化那些不可靠的遠(yuǎn)視野反而是浪費(fèi)算力。經(jīng)驗(yàn)法則先用階躍響應(yīng)或動(dòng)態(tài)仿真測(cè)出系統(tǒng)從初始狀態(tài)到穩(wěn)定狀態(tài)的大致上升時(shí)間然后把N * dt設(shè)為上升時(shí)間的1.5~3倍。比如系統(tǒng)上升時(shí)間約2秒采樣周期50ms那么N取60~120之間的值。當(dāng)然如果算力有限可以適當(dāng)調(diào)小N并以終端的權(quán)重來(lái)彌補(bǔ)遠(yuǎn)眺能力的缺失。4.2 代價(jià)權(quán)重Q和R從頻域正則化的視角理解Q和R的調(diào)參說(shuō)到底是平衡“跟蹤精度”和“控制能量”之間的折中。這個(gè)折中不只是工程直覺(jué)的問(wèn)題背后還有深刻的控制理論含義。從最優(yōu)控制的角度看Q和R通過(guò)代價(jià)函數(shù)定義了一個(gè)“性能度量”而MPC每次都是在當(dāng)前約束下最小化這個(gè)度量。R提高等價(jià)于在頻域上對(duì)高頻控制動(dòng)作施加懲罰等效于一個(gè)低通濾波器抑制了系統(tǒng)的高頻振蕩Q提高則等效于提高系統(tǒng)的閉環(huán)帶寬讓跟蹤誤差衰減得更快。所以我的調(diào)參策略是這樣的初值采用布里森法則歸一化量綱。先固定R把Q逐步增大到系統(tǒng)出現(xiàn)邊緣振蕩的位置記錄那個(gè)臨界Q值。然后固定Q把R從很小逐步增大直到控制量的高頻波動(dòng)幅度顯著下降同時(shí)關(guān)閉系統(tǒng)振蕩。最后微調(diào)比例關(guān)系仿真驗(yàn)證目標(biāo)是否滿足。每次調(diào)參后都要跑幾組不同幅值、不同初態(tài)的仿真防止“只對(duì)單個(gè)場(chǎng)景有效”的過(guò)擬合調(diào)參。4.3 約束的軟硬處理什么時(shí)候該軟化約束MPC的最大賣(mài)點(diǎn)是硬約束處理但硬約束也有副作用一旦約束太緊或者受擾動(dòng)影響出現(xiàn)不可行解整個(gè)優(yōu)化問(wèn)題就直接失敗輕則控制器凍結(jié)重則系統(tǒng)失控。工程上常用的做法是“軟約束”。也就是在約束條件里加入松弛變量epsilon并修改代價(jià)函數(shù)為min J tracking_cost lambda * sum(epsilon) s.t. x_min - epsilon x x_max epsilon epsilon 0這樣當(dāng)約束滿足不了時(shí)求解器可以選擇輕微越界并付出懲罰而不是直接宣布無(wú)解。lambda的大小決定了“越界被容忍的程度”。軟約束在溫度控制、軌跡跟蹤中尤其常用——寧可讓溫度稍微超調(diào)0.5攝氏度也不要讓整條產(chǎn)線停機(jī)。4.4 采樣周期與實(shí)時(shí)性平衡MPC的實(shí)時(shí)性完全取決于求解一個(gè)QP問(wèn)題的耗時(shí)。采樣周期設(shè)得太短控制器根本來(lái)不及算完下一幀指令就會(huì)產(chǎn)生執(zhí)行延遲設(shè)得太長(zhǎng)則控制的作用頻率太低動(dòng)態(tài)性能下降。理想情況是QP求解耗時(shí)占采樣周期的20%~40%留出足夠余量給傳感器讀取、狀態(tài)估計(jì)和控制輸出。如果求解耗時(shí)太長(zhǎng)可以嘗試以下手段縮短預(yù)測(cè)時(shí)域N、降低系統(tǒng)模型維度比如去掉快動(dòng)態(tài)模態(tài)、使用熱啟動(dòng)上一時(shí)刻的最優(yōu)解作為下一時(shí)刻的初始猜測(cè)、或者換用更快的求解器qpOASES在QP規(guī)模幾百個(gè)變量的情況下通常比OSQP快不少。5. 工程落地中的常見(jiàn)問(wèn)題與調(diào)試技巧5.1 震蕩和發(fā)散從穩(wěn)定性角度排查MPC的穩(wěn)定性理論條件很?chē)?yán)格工程實(shí)現(xiàn)中更容易因?yàn)閷?shí)現(xiàn)細(xì)節(jié)出岔子。最常見(jiàn)的震蕩來(lái)源有三個(gè)模型失配導(dǎo)致的預(yù)測(cè)偏差、采樣周期過(guò)長(zhǎng)導(dǎo)致的控制滯后、權(quán)重設(shè)置不當(dāng)導(dǎo)致的高頻激勵(lì)。排查方法建議先做“模型誤差模擬測(cè)試”在仿真中故意把真實(shí)對(duì)象的增益調(diào)成控制器模型增益的1.2倍看系統(tǒng)還穩(wěn)不穩(wěn)。如果1.2倍就發(fā)散說(shuō)明你的MPC魯棒性太差大概率需要增加R權(quán)重或縮短預(yù)測(cè)時(shí)域。5.2 狀態(tài)估計(jì)的重要性MPC不能直接吃傳感器噪聲MPC的輸入是當(dāng)前狀態(tài)x(k)但傳感器直接測(cè)量的往往不是狀態(tài)而是帶噪聲的觀測(cè)信號(hào)。如果直接把原始測(cè)量值丟給MPC作為初始狀態(tài)噪聲會(huì)被控制器當(dāng)成真實(shí)擾動(dòng)導(dǎo)致控制序列劇烈波動(dòng)。工程上的標(biāo)準(zhǔn)做法是在MPC前面加一個(gè)狀態(tài)估計(jì)器最常用的是卡爾曼濾波器。卡爾曼濾波器通過(guò)系統(tǒng)模型和傳感器的雙通道信息給出一個(gè)對(duì)真實(shí)狀態(tài)的最優(yōu)估計(jì)值再把這個(gè)估計(jì)值作為MPC的輸入。這樣傳感器的高頻噪聲被極大抑制MPC才能發(fā)揮出原有的平滑控制優(yōu)勢(shì)。5.3 實(shí)際部署時(shí)的“熱啟動(dòng)”技巧在嵌入式平臺(tái)上跑MPC每一幀的求解時(shí)間要壓到極致熱啟動(dòng)是繞不開(kāi)的優(yōu)化手段。所謂熱啟動(dòng)就是利用上一時(shí)刻解出來(lái)的最優(yōu)控制序列和狀態(tài)軌跡作為本次求解的初始點(diǎn)。對(duì)OSQP和qpOASES這類(lèi)求解器而言給定一個(gè)接近最優(yōu)的初始點(diǎn)迭代收斂次數(shù)通?？梢詼p少一半以上。我的代碼里雖然沒(méi)有顯式寫(xiě)熱啟動(dòng)cvxpy會(huì)自動(dòng)做一部分但在裸寫(xiě)OSQP接口時(shí)這個(gè)優(yōu)化是少不了的。另外你可以把上一幀的最優(yōu)解序列整體平移一步作為當(dāng)前幀的初始點(diǎn)——這比全部填零要高效得多。5.4 調(diào)試用的“黃金三件套”曲線調(diào)試MPC時(shí)我強(qiáng)烈建議你必看三條曲線參考軌跡和實(shí)際軌跡的對(duì)比、控制量變化曲線、每個(gè)時(shí)刻的關(guān)鍵約束余量比如離約束邊界的距離。如果軌跡跟蹤差先看參考軌跡是不是定得太激進(jìn)如果控制量抖成鋸齒先看R權(quán)重是不是太小如果約束余量長(zhǎng)期為零說(shuō)明系統(tǒng)長(zhǎng)期貼著邊界跑這時(shí)候要警惕硬約束不可行的風(fēng)險(xiǎn)建議改成軟約束。還建議打印每個(gè)控制周期的QP求解狀態(tài)和求解耗時(shí)連續(xù)多個(gè)周期失敗就要立刻停機(jī)防護(hù)這是工程上的底線要求。6. 更進(jìn)一步的擴(kuò)展思路自適應(yīng)MPC、經(jīng)濟(jì)MPC與分布式MPCMPC并不只是“一個(gè)算法”它是一整個(gè)框架。當(dāng)你把基礎(chǔ)的線性MPC跑熟之后完全可以往下面幾個(gè)方向延伸。自適應(yīng)MPCAdaptive MPC是解決模型失配的進(jìn)階方案。它的做法是在線估計(jì)系統(tǒng)參數(shù)然后實(shí)時(shí)更新預(yù)測(cè)模型。比如車(chē)輛的質(zhì)量隨著乘員數(shù)量變化通過(guò)在線辨識(shí)質(zhì)量參數(shù)并更新B矩陣可以讓控制器在所有負(fù)載條件下都保持良好性能。經(jīng)濟(jì)MPCEconomic MPC是流程工業(yè)的熱門(mén)方向。它不再只關(guān)注設(shè)定點(diǎn)跟蹤而是直接優(yōu)化經(jīng)濟(jì)效益相關(guān)的目標(biāo)函數(shù)比如能耗最小、產(chǎn)量最大、溫室氣體排放最少。哲學(xué)上經(jīng)濟(jì)MPC把“控制”和“優(yōu)化”融為一體在保證安全約束的前提下讓系統(tǒng)直接運(yùn)行在最優(yōu)的經(jīng)濟(jì)工作點(diǎn)附近。分布式MPCDistributed MPC面向大規(guī)模互聯(lián)系統(tǒng)。比如多臺(tái)無(wú)人機(jī)協(xié)同編隊(duì)或者一個(gè)工廠里多臺(tái)反應(yīng)釜的聯(lián)動(dòng)控制如果每臺(tái)設(shè)備各自跑獨(dú)立的MPC它們之間的耦合效應(yīng)會(huì)導(dǎo)致整體性能下降分布式MPC通過(guò)交換彼此的未來(lái)軌跡信息實(shí)現(xiàn)局部自治全局協(xié)調(diào)是目前多智能體控制研究的熱門(mén)方向。7. 給初學(xué)者的幾點(diǎn)心法最后從個(gè)人經(jīng)驗(yàn)角度分享一些調(diào)試MPC的心法和習(xí)慣。心法一“先看約束后調(diào)權(quán)重”。很多人一上來(lái)就狂調(diào)Q和R卻忽略了約束邊界的合理設(shè)置。其實(shí)約束的可實(shí)現(xiàn)性決定了MPC的可行性如果約束本身定得太緊或互相沖突再好的權(quán)重也白搭。先把約束邊界用物理理解校準(zhǔn)再去調(diào)權(quán)重。心法二“從仿真到硬件逐步增加復(fù)雜度”。不要一上來(lái)就在真實(shí)設(shè)備上調(diào)試MPC先在Python仿真里把控制器參數(shù)調(diào)好再做硬件在環(huán)測(cè)試最后才上真機(jī)。真機(jī)上出問(wèn)題時(shí)不要急著改算法先確認(rèn)狀態(tài)估計(jì)、執(zhí)行器標(biāo)定、通信延時(shí)就緒。心法三“記錄每一步的調(diào)試數(shù)據(jù)”。MPC的調(diào)試過(guò)程往往是非線性的哪個(gè)版本改了哪些權(quán)重、哪行代碼把這個(gè)約束從硬轉(zhuǎn)軟這些細(xì)節(jié)都是寶貴資產(chǎn)。我自己的習(xí)慣是給每個(gè)實(shí)驗(yàn)版本建一個(gè)日志文件記錄時(shí)間、參數(shù)文件、仿真結(jié)果、問(wèn)題現(xiàn)象這樣三個(gè)月后再回頭調(diào)整參數(shù)時(shí)不至于對(duì)著一堆文件抓瞎。心法四“MPC不是萬(wàn)能的但它依然值得掌握”。MPC的局限在于模型依賴(lài)和計(jì)算開(kāi)銷(xiāo)有時(shí)一個(gè)調(diào)好的PID反而更簡(jiǎn)單可靠。但掌握了MPC你就擁有了一把能同時(shí)處理約束、多變量和非線性問(wèn)題的通用鑰匙在復(fù)雜的控制場(chǎng)景里它帶來(lái)的結(jié)構(gòu)性優(yōu)勢(shì)往往是其他方法給不了的。這些經(jīng)驗(yàn)來(lái)自我在幾個(gè)實(shí)際項(xiàng)目中的反復(fù)摸索希望能幫你少走一些彎路。動(dòng)手跑一次MPC感受一下“預(yù)測(cè)—優(yōu)化—執(zhí)行—再預(yù)測(cè)”這個(gè)閉環(huán)的魅力你會(huì)對(duì)它有一個(gè)比看一百篇文章都更扎實(shí)的理解。