以前有寫過MPC的控制算法，主要介紹的也是理論部分，在實際使用過程當中發現C++沒有高效的優化方法，相似python中的cvxpy的庫，因此想繞過去，研究一下LQR控制算法。python
1. PID控制系統算法
這裏並非要介紹PID系統，而是爲了方便理解LQR，首先咱們看PID系統中的P調節方法：函數
假設咱們如今狀態是x0，咱們有狀態方程 : （u爲控制矩陣）優化
再假設有一個反饋控制器： 這裏很重要，能夠認爲是當前的控制量是經過當前的狀態量計算出來的。spa
經過這套方法，咱們就能獲得一個穩定的系統 ： 設計
固然這是基本的理論，再進一步，咱們就會思考，經過這些控制量做爲自變量，再設計一個代價函數，來優化這些控制量？3d
2. LQR控制算法對象
講到這裏就很天然引出LQR了，首先的問題代價函數是什麼？這裏我直接給出：blog
其中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. 將u=−Kx代入代價函數後： ，u=−Kx即咱們認爲當前的控制量能夠經過當前的狀態量計算出來。若是咱們能計算出-K就很簡單了，能夠直接套出控制量。
2. 假設純在一個常量矩陣P使得， ，這裏就直接這麼假設，接着往下看。
3. 把上式代入獲得：
4. 把2中的方程微分展開：
5. 狀態變量x的微分用式表示：
整理獲得：
經過矩陣得知上式要想有解只有中間部分爲零，即：
6. 把 代入上式：
整理獲得：
7. 這裏咱們再令 這裏須要思考一下，爲何能夠這樣假設，湊出結果，//Todo
代入：
整理獲得： ，獲得這一步就很簡單了，式中A、B、Q、R都是已知量，很容易就能計算出P的結果。
8. 計算出P再經過就能夠計算出K，也就是達到了咱們一開始的目標，輕鬆獲得當前的控制量。
4. MPC與LQR比較
首先，LQR的研究對象是現代控制理論中的狀態空間方程給出的線性系統，而MPC的研究對象能夠是線性系統，也能夠是非線性系統。不過如今不少的作法都是將非線性系統線性化，而後進行相關計算，具體要根據本身的工程狀況來肯定哪一種方式比較好，好比以前作MPC的時候，線控車底層速度控制接口就是加速度，那就不必根據IMU再套嵌個一層PID。
其次，既然是優化問題，那就離不開目標函數的設計，LQR的目標函數在上面已經有描述，MPC的目標函數，多數都是多個優化目標乘以不一樣權重而後求和的方式。雖然方式不一樣，不過都是對達到控制目標的代價累計。
最後，工做時域上的不一樣，LQR的計算針對同一工做時域，在一個控制週期內，LQR只計算一次，並將這次計算出的最優解下發給控制器便可；而MPC是滾動優化的，計算將來一段時間內，每一個採樣週期都會通過計算，得出一組控制序列，可是隻將第一個控制值下發給控制器。