1.Introduction
LQR (外文名linear quadratic regulator)即線性二次型調(diào)節(jié)器，LQR可得到狀態(tài)線性反饋的最優(yōu)控制規(guī)律，易于構(gòu)成閉環(huán)最優(yōu)控制。LQR最優(yōu)控制利用廉價成本可以使原系統(tǒng)達到較好的性能指標(biāo)(事實也可以對不穩(wěn)定的系統(tǒng)進行整定) ，而且方法簡單便于實現(xiàn) ，同時利用 Matlab 強大的功能體系容易對系統(tǒng)實現(xiàn)仿真。
之前已經(jīng)在自動駕駛汽車上工程落地，參見自動駕駛——最優(yōu)控制算法(LQR）工程化總結(jié)。
與上次不同的是，此次被控對象是移動機器人，兩輪差速模型，差速模型原理參見另一篇機器人控制算法——兩輪差速驅(qū)動運動模型
2. Implment LQR Algorithm for Differential Drive Motion
車輛運動學(xué)模型是根據(jù)車輛的幾何關(guān)系建立的，不考慮影響車輛運動的力。以車輛后軸中點作為研究對象，在慣性坐標(biāo)系OXY下，車輛運動狀態(tài)可用車輛坐標(biāo)(x,y)和車身航向角。v為車輛縱向行駛速度，如下圖所示。
則車輛運動學(xué)模型為：
其中，u1和u2分別是車輛的縱向和橫向控制輸入。
設(shè)車輛行駛的目標(biāo)點位姿為 
，則在車輛目標(biāo)點處建立局部相對坐標(biāo)系，如圖2所示的坐標(biāo)系變換示意圖，則車輛當(dāng)前位置相對目標(biāo)位置的橫縱向偏差和航向角偏差滿足下列幾何關(guān)系：
其中，
分別為車輛的縱向偏差、橫向偏差和航向角偏差。
根據(jù)公式(1)和公式(2)可得車輛在相對坐標(biāo)系下的運動學(xué)模型為：
公式（3）中包含了縱向控制u1（v）和橫向控制u2（w），但是本文我們只考慮橫向控制。我們需要把橫向控制和縱向控制解耦合。
根據(jù)(3)可得解耦后的車輛橫向運動學(xué)模型為：
我們需要進一步將公式16進行線性化處理，利用高等數(shù)學(xué)極限知識，當(dāng)x->0,sin(x)~x,故有：
至此我們得到了機器人兩輪差速動力學(xué)模型。
3. LQR Implement
在本部分，我們將使用LQR控制算法計算最優(yōu)控制規(guī)律。
- 對矩陣A和B離散化處理
B_D = T * B
A_D = (I+0.5AT)(I-0.5A*T)^(-1)
matrix_A_D = (matrix_I - 0.5 * ts * matrix_A)^(-1) * (matrix_I + 0.5 * ts * matrix_A);
matrix_B_D = matrix_B * ts;
2.設(shè)定Q和R矩陣，Q應(yīng)是2X2矩陣，R應(yīng)是1X1維。
3.Riccati 方程求解：
My Related Works
2. 自動駕駛-LQR工程實現(xiàn)（調(diào)研）
3. 自動駕駛-動力學(xué)模型-狀態(tài)空間法A矩陣----c語言實現(xiàn)【附Github源碼鏈接】
4. 自動駕駛——最優(yōu)控制算法(LQR）工程化總結(jié)
Reference
1.基于車輛運動學(xué)模型的路徑跟蹤仿真及程序文章來源：http://www.zghlxwxcb.cn/news/detail-766980.html
如果覺得寫得還不錯，可以關(guān)注一下我的微信公眾號，最新動態(tài)會分享在此。
文章來源地址http://www.zghlxwxcb.cn/news/detail-766980.html
到了這里，關(guān)于機器人控制算法——移動機器人橫向控制最優(yōu)控制LQR算法的文章就介紹完了。如果您還想了解更多內(nèi)容，請在右上角搜索TOY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關(guān)文章，希望大家以后多多支持TOY模板網(wǎng)！
![[足式機器人]Part2 Dr. CAN學(xué)習(xí)筆記- 最優(yōu)控制Optimal Control Ch07-1最優(yōu)控制問題與性能指標(biāo)](https://imgs.yssmx.com/Uploads/2024/01/818750-1.png)
![[足式機器人]Part2 Dr. CAN學(xué)習(xí)筆記- 最優(yōu)控制Optimal Control Ch07](https://imgs.yssmx.com/Uploads/2024/01/820876-1.png)
