MPC是模型預(yù)測控制算法，在車輛運(yùn)動跟蹤軌跡的控制中發(fā)揮很大的優(yōu)勢

基礎(chǔ)的不多說，下面記錄我對LQR/MPC/二次規(guī)劃問題的理解

我們從LQR來引出MPC

LQR的能量函數(shù)，目的是求函數(shù)J最小，即用最小的代價得到最好的控制效果，理論上只要系統(tǒng)是可控的，LQR就能求出較優(yōu)的解

的解是 u= -K_lqr * x，這是一個線性解，用這個算法僅需要對能量函數(shù)求解一次，線下求解的u的表達(dá)式，線上不斷重復(fù)計(jì)算u即可

MPC算法和LQR的公式非常接近，其表達(dá)式是離散的，函數(shù)是有邊界的（算法中稱之為約束條件），現(xiàn)實(shí)生活中，求出的解必須要對其進(jìn)行限制（那車速舉例，你不可能讓車子跑出500km/h，這就是約束條件）

要知道，系統(tǒng)接下來的狀態(tài)量x是根據(jù)系統(tǒng)當(dāng)前控制量u得到的，因此需算出N+1的狀態(tài)

約束條件 ：

為什么會存在多組狀態(tài)？

MPC的算法和LQR算法的計(jì)算量根本不是一個數(shù)量級的，因?yàn)槊總€時刻t=1，2，3……都需要重新求解一遍J，而MPC是根據(jù)當(dāng)前狀態(tài)給出接下來的控制量和狀態(tài)量的序列（相比LQR僅算出一次u的表達(dá)式，MPC要計(jì)算N次，這里N指步長）

u（1），x..., u（2），x...,u（3）……u（N），然后選取第一個u（1）作為當(dāng)前控制量，等到下一時刻，所有的控制量u和狀態(tài)量x的序列又重新計(jì)算，再次取新的u（1）作為現(xiàn)在時刻的控制量，或許有人會疑問，這樣不是浪費(fèi)計(jì)算量么？ 我們要搞清楚，MPC的原理，他是為了接下來的預(yù)測出來的控制量控制出來的假想出的狀態(tài)來判斷，來給出當(dāng)前最優(yōu)的控制量，和大多數(shù)控制算法只關(guān)注當(dāng)前時刻的狀態(tài)是不同的。這也是為什么MPC是目前自動駕駛車輛運(yùn)動控制算法的首選。

二次規(guī)劃問題

MPC的離散公式其實(shí)就是求關(guān)于x（i）和u（i）的函數(shù)的積分，假設(shè)該函數(shù)為f（x(i),u(i)），那么二次優(yōu)化就是求出函數(shù)f的優(yōu)化形式，由于f的計(jì)算涉及到大量矩陣，因此所選用的優(yōu)化方法主要就是減少運(yùn)算次數(shù)，縮短MPC的計(jì)算時間，以達(dá)到車輛高速運(yùn)動時的快速響應(yīng)。

Matlab為我們提供了一個函數(shù)quadprog（），其簡單調(diào)用形式為quadprog(H, f, A, b, Aeq, beq, lb, ub, x0, options)

H： 二次規(guī)劃中的二次項(xiàng)矩陣

f： 二次規(guī)劃中的一次項(xiàng)矩陣

Aeq： 線性不等約束的系數(shù)矩陣

beq： 線性不等約束的右端向量

lb/ub： 自變量下/上限（有些理想問題沒有上下限，可不做考慮）

x0： 熱啟動項(xiàng)

（我們認(rèn)為狀態(tài)量不會突然變化，因此k+1時刻的最優(yōu)解將會在k時刻最優(yōu)解的附近，這個思路能夠節(jié)約大量時間，很抱歉之前寫成了初始狀態(tài)，具體怎么設(shè)置我們后續(xù)更新，初學(xué)者可先設(shè)為空或0）

options:解二次規(guī)劃問題的具體算法（quadprog之是提供了借口，解二次規(guī)劃問題需要options指定具體算法）

客官莫急，稍后會講怎么求得以上參數(shù)

二次規(guī)劃問題基本公式：

約束條件：

前面我們已經(jīng)知道，二次規(guī)劃是關(guān)于x1和x2的函數(shù)，（x = [x1; x2]）套用上面的基本公式就能得到quadprog函數(shù)的參數(shù)

現(xiàn)在是結(jié)合mpc的二次規(guī)劃問題，目的也是求出quadprog函數(shù)的參數(shù)：

這是一個離散系統(tǒng)：

能量函數(shù)（cost function）：

我們看到了LQR的影子，但是這個P是什么呢？官方的解釋終于來了，LQR的條件是狀態(tài)k（此時是離散的）是趨近于無窮的，而MPC的預(yù)測步數(shù)N是有限的，為了保證系統(tǒng)的最終穩(wěn)定性，P要盡量等于步長N到無窮時的狀態(tài)，讓它更貼近于LQR，但是P不可能完全等于LQR中的無窮狀態(tài)，（因此我們在這寫成了盡量等于），為了使MPC更像LQR，在quadprog函數(shù)加入上下邊界lb和ub，P與quadprog函數(shù)的上下限一起保證了MPC穩(wěn)定性。

那么P怎么求得？我們后續(xù)更新o(^▽^)o

quadprog函數(shù)的參數(shù)一般有兩種求法——稀疏和稠密法，現(xiàn)在是稀疏法：

新的二次規(guī)劃問題：

（編輯/粘貼公式真的煩）

X是x的可行域，U是u的可行域

以上公式被簡化成這樣：

其中

事實(shí)上我們在不斷更新Beq中的第一項(xiàng)x(k+0)(即當(dāng)前的狀態(tài)量)，通過反饋把從系統(tǒng)中讀取的x(k)送到B處，不斷更新的u在作用于系統(tǒng)，將新得到的x(k+1)再更新到Beq，系統(tǒng)就可以這樣進(jìn)行下去了

所有的Umin都是相同的，不會因時刻不同而變化，Umax也一樣

quadprog函數(shù)的lb/ub僅需要z_min和z_max，寫成這樣完全是因?yàn)楹芏嗨惴ㄐ枰@樣的形式，在quadprog內(nèi)部也會將z_min/z_max轉(zhuǎn)換成這種形式。

MPC流程

調(diào)用quadprog函數(shù)得到的z就是控制量u和狀態(tài)量x的序列，我們?nèi)〉谝粋€u作為當(dāng)前控制量，等到下一時刻如法炮制出新的u。

由此可見mpc的計(jì)算量有多大

后續(xù)更新中將會講到options參數(shù)中的優(yōu)化算法

一個重要猜想：本文的矩陣設(shè)計(jì)實(shí)例依據(jù)倒擺系統(tǒng)，因此在Beq矩陣中，除了初始狀態(tài)X(0)，其余的狀態(tài)我們都希望是零，而接下來我們要加以改進(jìn)應(yīng)用到車輛軌跡跟蹤上，因此我覺得Beq和Aeq需要調(diào)整。這個猜想后續(xù)需要驗(yàn)證，到時驗(yàn)證結(jié)果會更新在此！

總結(jié)

以上是生活随笔為你收集整理的车辆运动控制算法——MPC的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯，歡迎將生活随笔推薦給好友。