1. 控制系統

????????這里我們先介紹常用的控制系統邏輯：

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

????????假設我們現在狀態是x0，我們有狀態方程 :?????????????????（u為控制矩陣）

????????特別的，這里我們是對偏差建立方程，x是偏差的狀態，優化的目的是x=0，針對我們通過一些假設可以得到詳細的方程，這里我直接先給出其中,??

????????詳細的推導省略，可自行檢索。

????????再假設有一個反饋控制器：???????? ?????????這里很重要，可以認為是當前的控制量是通過當前的狀態量計算出來的。

????????通過這套方法，我們就能得到一個穩定的系統 ：??

????????當然這是基本的理論，再進一步，我們就會思考，通過這些控制量作為自變量，再設計一個代價函數，來優化這些控制量？

2. LQR控制算法

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

?????????其中x為狀態量，u為控制量，Q為狀態權重矩陣，R為控制權重矩陣。

??????????特別的x和u中取值有正有負，所以需要平方和最小，在矩陣中沒有平方，這里我們采用轉置乘以本身的做法模擬矩陣的平方，如x^T*x?。這里狀態量x和控制量u都是多維向量，上式計算的結果是一個標量。

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

??????????一般的我們認為狀態量x為：橫向偏差、橫向變化率、角度偏差、角度變化率。Q為我們提前標定的對角矩陣，標定值對應以上不同維度的權重，也可以為非對角矩陣，考慮不同維度之間的相互關系。

?????????控制向量u為：前輪轉角、加速度。同樣的R也為提前標定的對角矩陣。也可以考慮相互關系。

? ? ? ? ?Q11選取較大的值，會讓x1很快到0；另外一方面，加大R的值，會使得對應的控制量減小，控制器執行更少的動作，意味著系統的狀態衰減將變慢。所以要綜合看具體的實際應用場景來調節，魚和熊掌不可兼得。建議在不同場景下采用不同的參數。

3. 公式求解

?假設有一個線性系統能用狀態向量的形式表示成：

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

其中?，初始條件是. 并且假設這個系統的所有狀態變量都是可測量到的。

??????在介紹LQR前，先簡單回顧一下現代控制理論中最基本的控制器--全狀態反饋控制。

??????全狀態反饋控制系統圖形如下：

我們要設計一個狀態反饋控制器?????????????

使得閉環系統能夠滿足我們期望的性能。我們把這種控制代入之前的系統狀態方程得到

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

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

對于(1)式的開環系統，由現代控制理論我們知道開環傳遞函數的極點就是系統矩陣A的特征值。(傳遞函數的分母是|sI -A|，|·|表示行列式)

現在變成了(2)的閉環形式，狀態變換矩陣A變成了(A-BK)。因此通過配置反饋矩陣K，可以使得閉環系統的極點達到我們期望的狀態。注意，這種控制器的設計與輸出矩陣C,D沒有關系。

???????那么，什么樣的極點會使得系統性能很棒呢？并且，當系統變量很多的時候，即使設計好了極點，矩陣K也不好計算。

???????于是，LQR為我們設計最優控制器提供了一種思路。

在設計LQR控制器前，我們得設計一個能量函數，最優的控制軌跡應該使得該能量函數最小。一般選取如下形式的能量函數。

????????????，其中Q是你自己設計的半正定矩陣，R為正定矩陣。

可是，為什么能量函數(或稱系統的目標函數)得設計成這個樣子呢？

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

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

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

???????注意LQR調節器是將狀態調節到0，這與軌跡跟蹤不同，軌跡跟蹤是使得系統誤差為0.

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

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

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

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

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

代入(3)式得：

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

注意，我們已經假設閉環系統是穩定的，也就是t趨于無窮時，x(t)趨于0.

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

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

這個式子要始終成立的話，括號里的項必須恒等于0.

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

這是一個關于K的二次型等式，當然這個二次型是我們不愿看到的，因為計算復雜。現在只要這個等式成立，我們何必不選擇K使得兩個二次項正好約掉了呢？這樣既符合了要求，又簡化了計算。

取 ???代入上式得：

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

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

現在回過頭總結下LQR控制器是怎么計算反饋矩陣K的：

???????1.選擇參數矩陣Q,R

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

???????3.計算

再看看LQR的結構圖：

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

實際應用中，為了消除穩態誤差，LQR中還需要加入前饋控制。詳細內容可查看：智能駕駛車輛橫向控制算法_ChenGuiGan的博客-CSDN博客_自動駕駛橫向控制

4.?MPC與LQR比較

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

??????????其次，既然是優化問題，那就離不開目標函數的設計，LQR的目標函數在上面已經有描述，MPC的目標函數，多數都是多個優化目標乘以不同權重然后求和的方式。雖然方式不同，不過都是對達到控制目標的代價累計。

??????????最后，工作時域上的不同，LQR的計算針對同一工作時域，在一個控制周期內，LQR只計算一次，并將此次計算出的最優解下發給控制器即可；而MPC是滾動優化的，計算未來一段時間內，每個采樣周期都會經過計算，得出一組控制序列，但是只將第一個控制值下發給控制器。
————————————————
版權聲明：本文為CSDN博主「一實相印」的原創文章，遵循CC 4.0 BY-SA版權協議，轉載請附上原文出處鏈接及本聲明。
原文鏈接：https://blog.csdn.net/zhouyy858/article/details/107606500

總結

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

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。