1. 控制系統(tǒng)

????????這里我們先介紹常用的控制系統(tǒng)邏輯：

??????????????????????????????????????

????????假設(shè)我們現(xiàn)在狀態(tài)是x0，我們有狀態(tài)方程 :?????????????????（u為控制矩陣）

????????特別的，這里我們是對(duì)偏差建立方程，x是偏差的狀態(tài)，優(yōu)化的目的是x=0，針對(duì)我們通過一些假設(shè)可以得到詳細(xì)的方程，這里我直接先給出其中,??

????????詳細(xì)的推導(dǎo)省略，可自行檢索。

????????再假設(shè)有一個(gè)反饋控制器：???????? ?????????這里很重要，可以認(rèn)為是當(dāng)前的控制量是通過當(dāng)前的狀態(tài)量計(jì)算出來的。

????????通過這套方法，我們就能得到一個(gè)穩(wěn)定的系統(tǒng) ：??

????????當(dāng)然這是基本的理論，再進(jìn)一步，我們就會(huì)思考，通過這些控制量作為自變量，再設(shè)計(jì)一個(gè)代價(jià)函數(shù)，來優(yōu)化這些控制量？

2. LQR控制算法

????????講到這里就很自然引出LQR了，首先的問題代價(jià)函數(shù)是什么？一方面我們希望系統(tǒng)達(dá)到穩(wěn)定狀態(tài)，及偏差最小; 另一方面我們希望控制量較小，即付出較小的代價(jià)達(dá)到我們的目的。這里我直接給出：

?????????其中x為狀態(tài)量，u為控制量，Q為狀態(tài)權(quán)重矩陣，R為控制權(quán)重矩陣。

??????????特別的x和u中取值有正有負(fù)，所以需要平方和最小，在矩陣中沒有平方，這里我們采用轉(zhuǎn)置乘以本身的做法模擬矩陣的平方，如x^T*x?。這里狀態(tài)量x和控制量u都是多維向量，上式計(jì)算的結(jié)果是一個(gè)標(biāo)量。

??????????其實(shí)我們可以把看作是的多維擴(kuò)展表達(dá)式，這里我們需要Q為半正定，就是希望Q能起到a≥0的效果，R為正定矩陣就是希望矩陣R能夠起到a>0的效果。

??????????一般的我們認(rèn)為狀態(tài)量x為：橫向偏差、橫向變化率、角度偏差、角度變化率。Q為我們提前標(biāo)定的對(duì)角矩陣，標(biāo)定值對(duì)應(yīng)以上不同維度的權(quán)重，也可以為非對(duì)角矩陣，考慮不同維度之間的相互關(guān)系。

?????????控制向量u為：前輪轉(zhuǎn)角、加速度。同樣的R也為提前標(biāo)定的對(duì)角矩陣。也可以考慮相互關(guān)系。

? ? ? ? ?Q11選取較大的值，會(huì)讓x1很快到0；另外一方面，加大R的值，會(huì)使得對(duì)應(yīng)的控制量減小，控制器執(zhí)行更少的動(dòng)作，意味著系統(tǒng)的狀態(tài)衰減將變慢。所以要綜合看具體的實(shí)際應(yīng)用場(chǎng)景來調(diào)節(jié)，魚和熊掌不可兼得。建議在不同場(chǎng)景下采用不同的參數(shù)。

3. 公式求解

?假設(shè)有一個(gè)線性系統(tǒng)能用狀態(tài)向量的形式表示成：

? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?( 1 )

其中?，初始條件是. 并且假設(shè)這個(gè)系統(tǒng)的所有狀態(tài)變量都是可測(cè)量到的。

??????在介紹LQR前，先簡(jiǎn)單回顧一下現(xiàn)代控制理論中最基本的控制器--全狀態(tài)反饋控制。

??????全狀態(tài)反饋控制系統(tǒng)圖形如下：

我們要設(shè)計(jì)一個(gè)狀態(tài)反饋控制器?????????????

使得閉環(huán)系統(tǒng)能夠滿足我們期望的性能。我們把這種控制代入之前的系統(tǒng)狀態(tài)方程得到

???????????????????????????????

? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ( 2 )

對(duì)于(1)式的開環(huán)系統(tǒng)，由現(xiàn)代控制理論我們知道開環(huán)傳遞函數(shù)的極點(diǎn)就是系統(tǒng)矩陣A的特征值。(傳遞函數(shù)的分母是|sI -A|，|·|表示行列式)

現(xiàn)在變成了(2)的閉環(huán)形式，狀態(tài)變換矩陣A變成了(A-BK)。因此通過配置反饋矩陣K，可以使得閉環(huán)系統(tǒng)的極點(diǎn)達(dá)到我們期望的狀態(tài)。注意，這種控制器的設(shè)計(jì)與輸出矩陣C,D沒有關(guān)系。

???????那么，什么樣的極點(diǎn)會(huì)使得系統(tǒng)性能很棒呢？并且，當(dāng)系統(tǒng)變量很多的時(shí)候，即使設(shè)計(jì)好了極點(diǎn)，矩陣K也不好計(jì)算。

???????于是，LQR為我們?cè)O(shè)計(jì)最優(yōu)控制器提供了一種思路。

在設(shè)計(jì)LQR控制器前，我們得設(shè)計(jì)一個(gè)能量函數(shù)，最優(yōu)的控制軌跡應(yīng)該使得該能量函數(shù)最小。一般選取如下形式的能量函數(shù)。

????????????，其中Q是你自己設(shè)計(jì)的半正定矩陣，R為正定矩陣。

可是，為什么能量函數(shù)(或稱系統(tǒng)的目標(biāo)函數(shù))得設(shè)計(jì)成這個(gè)樣子呢？

???????首先假設(shè)狀態(tài)向量x(t)是1維的，那么其實(shí)就是一個(gè)平方項(xiàng) Qx^2 >= 0，同理.?能量函數(shù)J要最小，那么狀態(tài)向量x(t)，u(t)都得小。J最小，那肯定是個(gè)有界的函數(shù)，我們能推斷當(dāng)t趨于無窮時(shí)，狀態(tài)向量x(t)將趨于0，這也保證了閉環(huán)系統(tǒng)的穩(wěn)定性。那輸入u(t)要小是什么意思呢？它意味著我們用最小的控制代價(jià)得到最優(yōu)的控制。譬如控制電機(jī)，輸入PWM小，將節(jié)省能量。

???????再來看看矩陣Q,R的選取，一般來說，Q值選得大意味著，要使得J小，那x(t)需要更小，也就是意味著閉環(huán)系統(tǒng)的矩陣(A-BK)的特征值處于S平面左邊更遠(yuǎn)的地方，這樣狀態(tài)x(t)就以更快的速度衰減到0。

另一方面，大的R表示更加關(guān)注輸入變量u(t),u(t)的減小，意味著狀態(tài)衰減將變慢。同時(shí)，Q為半正定矩陣意味著他的特征值非負(fù)，R為正定矩陣意味著它的特征值為正數(shù)。如果你選擇Q,R都是對(duì)角矩陣的話，那么Q的對(duì)角元素為正數(shù)，允許出現(xiàn)幾個(gè)0.R的對(duì)角元素只能是正數(shù)。

???????注意LQR調(diào)節(jié)器是將狀態(tài)調(diào)節(jié)到0，這與軌跡跟蹤不同，軌跡跟蹤是使得系統(tǒng)誤差為0.

????????知道了背景后，那如何設(shè)計(jì)反饋矩陣K使得能量函數(shù)J最小呢？很多地方都是從最大值原理，Hamilton函數(shù)推導(dǎo)出來。這里用另外一種更容易接受的方式推導(dǎo)。

將u = -Kx 代入之前的能量函數(shù)得到：

????????????????????????( 3 )

為了找到K,我們先不防假設(shè)存在一個(gè)常量矩陣P使得：

????????????????????(4)

代入(3)式得：

???????????????????(5)

注意，我們已經(jīng)假設(shè)閉環(huán)系統(tǒng)是穩(wěn)定的，也就是t趨于無窮時(shí)，x(t)趨于0.

現(xiàn)在把(4)式左邊的微分展開，并把狀態(tài)變量x的微分用(2)式替代得到：

? ? ? ? ? ? ? ? ??????????????????

這個(gè)式子要始終成立的話，括號(hào)里的項(xiàng)必須恒等于0.

????????????????

這是一個(gè)關(guān)于K的二次型等式，當(dāng)然這個(gè)二次型是我們不愿看到的，因?yàn)橛?jì)算復(fù)雜。現(xiàn)在只要這個(gè)等式成立，我們何必不選擇K使得兩個(gè)二次項(xiàng)正好約掉了呢？這樣既符合了要求，又簡(jiǎn)化了計(jì)算。

取 ???代入上式得：

?????????????(6)

K的二次項(xiàng)沒有了，可K的取值和P有關(guān)，而P是我們假設(shè)的一個(gè)量，P只要使得的(6)式成立就行了。而(6)式在現(xiàn)代控制理論中極其重要，它就是有名的Riccati 方程。

現(xiàn)在回過頭總結(jié)下LQR控制器是怎么計(jì)算反饋矩陣K的：

???????1.選擇參數(shù)矩陣Q,R

???????2.求解Riccati 方程得到矩陣P

???????3.計(jì)算

再看看LQR的結(jié)構(gòu)圖：

????????? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ???????

實(shí)際應(yīng)用中，為了消除穩(wěn)態(tài)誤差，LQR中還需要加入前饋控制。詳細(xì)內(nèi)容可查看：智能駕駛車輛橫向控制算法_ChenGuiGan的博客-CSDN博客_自動(dòng)駕駛橫向控制

4.?MPC與LQR比較

??????????首先，LQR的研究對(duì)象是現(xiàn)代控制理論中的狀態(tài)空間方程給出的線性系統(tǒng)，而MPC的研究對(duì)象可以是線性系統(tǒng)，也可以是非線性系統(tǒng)。不過現(xiàn)在很多的做法都是將非線性系統(tǒng)線性化，然后進(jìn)行相關(guān)計(jì)算，具體要根據(jù)自己的工程情況來確定哪種方式比較好，比如之前做MPC的時(shí)候，線控車底層速度控制接口就是加速度，那就沒必要根據(jù)IMU再套嵌個(gè)一層PID。

??????????其次，既然是優(yōu)化問題，那就離不開目標(biāo)函數(shù)的設(shè)計(jì)，LQR的目標(biāo)函數(shù)在上面已經(jīng)有描述，MPC的目標(biāo)函數(shù)，多數(shù)都是多個(gè)優(yōu)化目標(biāo)乘以不同權(quán)重然后求和的方式。雖然方式不同，不過都是對(duì)達(dá)到控制目標(biāo)的代價(jià)累計(jì)。

??????????最后，工作時(shí)域上的不同，LQR的計(jì)算針對(duì)同一工作時(shí)域，在一個(gè)控制周期內(nèi)，LQR只計(jì)算一次，并將此次計(jì)算出的最優(yōu)解下發(fā)給控制器即可；而MPC是滾動(dòng)優(yōu)化的，計(jì)算未來一段時(shí)間內(nèi)，每個(gè)采樣周期都會(huì)經(jīng)過計(jì)算，得出一組控制序列，但是只將第一個(gè)控制值下發(fā)給控制器。
————————————————
版權(quán)聲明：本文為CSDN博主「一實(shí)相印」的原創(chuàng)文章，遵循CC 4.0 BY-SA版權(quán)協(xié)議，轉(zhuǎn)載請(qǐng)附上原文出處鏈接及本聲明。
原文鏈接：https://blog.csdn.net/zhouyy858/article/details/107606500

總結(jié)

以上是生活随笔為你收集整理的智能驾驶LQR横向控制算法的全部?jī)?nèi)容，希望文章能夠幫你解決所遇到的問題。

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