單片機(jī)中使用 LQR 控制算法入門(mén)教程從 PID 到現(xiàn)代控制 很多同學(xué)在學(xué)習(xí)單片機(jī)控制時(shí)最先接觸的是 PID。PID 簡(jiǎn)單、好用、容易上手但當(dāng)控制對(duì)象變得復(fù)雜時(shí)比如兩輪平衡車(chē)、倒立擺、機(jī)器人底盤(pán)、云臺(tái)、電機(jī)姿態(tài)控制等單純依靠 PID 調(diào)參會(huì)越來(lái)越困難。這時(shí)就可以學(xué)習(xí)一種更系統(tǒng)化的控制方法LQR 控制算法。一、LQR 是什么LQR 的全稱是Linear Quadratic Regulator 線性二次型調(diào)節(jié)器text簡(jiǎn)單來(lái)說(shuō)LQR 是一種現(xiàn)代控制算法。它的目標(biāo)可以理解為在讓系統(tǒng)盡快穩(wěn)定的同時(shí)盡量不要讓控制輸出太大。舉個(gè)例子假設(shè)我們要控制一輛小車(chē)回到目標(biāo)位置。系統(tǒng)需要考慮小車(chē)離目標(biāo)位置有多遠(yuǎn)小車(chē)當(dāng)前速度有多快控制電機(jī)時(shí)輸出不能太猛最終希望系統(tǒng)又快又穩(wěn)地回到目標(biāo)狀態(tài)。LQR 就是根據(jù)這些狀態(tài)自動(dòng)計(jì)算一個(gè)比較合理的控制輸出。二、LQR 和 PID 有什么區(qū)別對(duì)比項(xiàng)PID 控制LQR 控制控制思想根據(jù)誤差調(diào)節(jié)輸出根據(jù)系統(tǒng)狀態(tài)整體調(diào)節(jié)輸出調(diào)參對(duì)象Kp、Ki、KdQ 矩陣、R 矩陣是否依賴模型依賴較少需要狀態(tài)空間模型適合對(duì)象單輸入單輸出系統(tǒng)多狀態(tài)耦合系統(tǒng)單片機(jī)實(shí)現(xiàn)難度簡(jiǎn)單中等控制效果容易上手更適合復(fù)雜系統(tǒng)一句話總結(jié)PID 更像是“看到誤差就修正”LQR 更像是“根據(jù)整個(gè)系統(tǒng)狀態(tài)做最優(yōu)決策”。三、LQR 的核心公式LQR 最核心的控制公式其實(shí)非常簡(jiǎn)單u -Kx其中符號(hào)含義u控制輸出例如 PWM、電壓、電流指令KLQR 增益矩陣x系統(tǒng)狀態(tài)變量-負(fù)反饋?zhàn)屜到y(tǒng)趨于穩(wěn)定也就是說(shuō)單片機(jī)真正運(yùn)行 LQR 時(shí)本質(zhì)上就是做一次乘加運(yùn)算。四、狀態(tài)變量是什么LQR 不是只看一個(gè)誤差而是會(huì)綜合多個(gè)狀態(tài)進(jìn)行控制。例如兩輪平衡車(chē)可以選擇以下?tīng)顟B(tài)變量x [角度, 角速度, 位置, 速度]也就是狀態(tài)變量含義angle車(chē)身傾角gyro車(chē)身角速度position小車(chē)位置speed小車(chē)速度對(duì)應(yīng)的 LQR 控制律可以寫(xiě)成u -(k1 * angle k2 * gyro k3 * position k4 * speed)這就非常適合在單片機(jī)中實(shí)現(xiàn)。五、單片機(jī)中使用 LQR 的基本思路很多人一開(kāi)始會(huì)誤以為L(zhǎng)QR 是不是要在單片機(jī)里面實(shí)時(shí)求矩陣其實(shí)一般不是。在實(shí)際 項(xiàng)目 中LQR 通常分為兩個(gè)階段。1. 上位機(jī)離線計(jì)算使用 MATLAB、Python 或其他工具計(jì)算出 LQR 增益矩陣 K。這一步會(huì)涉及矩陣計(jì)算、Riccati 方程求解計(jì)算量比較大一般不放在普通單片機(jī)中實(shí)時(shí)運(yùn)行。2. 單片機(jī)實(shí)時(shí)運(yùn)行單片機(jī)只需要執(zhí)行u -Kx也就是幾個(gè)乘法和加法。例如u -(k1 * angle k2 * gyro k3 * position k4 * speed);所以LQR 在單片機(jī)中的實(shí)時(shí)計(jì)算量其實(shí)很小。六、LQR 控制系統(tǒng) 整體流程采集傳感器數(shù)據(jù)計(jì)算系統(tǒng)狀態(tài) x代入 LQR 控制律 u -Kx限制控制輸出范圍輸出 PWM 控制電機(jī)系統(tǒng)狀態(tài)變化七、LQR 需要的 數(shù)學(xué)模型LQR 需要把系統(tǒng)寫(xiě)成狀態(tài)空間形式。連續(xù)系統(tǒng)一般寫(xiě)成x_dot A x B u離散系統(tǒng)一般寫(xiě)成x(k1) A_d x(k) B_d u(k)其中矩陣或變量含義x系統(tǒng)狀態(tài)變量u控制輸入A系統(tǒng)自身狀態(tài)變化關(guān)系B控制輸入對(duì)系統(tǒng)的影響A_d離散化后的 A 矩陣B_d離散化后的 B 矩陣對(duì)于單片機(jī)來(lái)說(shuō)通常最終使用的是離散模型因?yàn)閱纹瑱C(jī)控制是按固定周期執(zhí)行的。例如每 5ms 執(zhí)行一次控制算法 每 10ms 執(zhí)行一次控制算法八、Q 矩陣和 R 矩陣怎么理解LQR 調(diào)參主要調(diào)兩個(gè)東西Q 矩陣 R 矩陣通俗理解矩陣作用Q你有多在意狀態(tài)誤差R你有多在意控制輸出大小1. Q 矩陣Q 矩陣用來(lái)表示你對(duì)不同狀態(tài)的重視程度。例如平衡車(chē)狀態(tài)為x [角度, 角速度, 位置, 速度]如果你特別希望角度保持穩(wěn)定就可以把角度對(duì)應(yīng)的 Q 值調(diào)大。例如Q diag([100, 1, 10, 1])含義如下?tīng)顟B(tài)權(quán)重角度100最重要角速度1普通位置10比較重要速度1普通2. R 矩陣R 矩陣用來(lái)限制控制輸出。可以這樣理解R 越大控制越溫和R 越小控制越激進(jìn)。例如R 10表示不希望電機(jī)輸出太猛系統(tǒng)響應(yīng)會(huì)更柔和。R 0.1表示允許電機(jī)輸出更積極系統(tǒng)響應(yīng)會(huì)更快但也可能更容易抖動(dòng)。九、MATLAB 中計(jì)算 LQR 增益 K假設(shè)有一個(gè)簡(jiǎn)單系統(tǒng)A [0 1; 0 -2]; B [0; 1];matlab運(yùn)行設(shè)置 Q 和 RQ [100 0; 0 1]; R 1;matlab運(yùn)行計(jì)算 LQR 增益K lqr(A, B, Q, R);matlab運(yùn)行如果輸出結(jié)果為K [10.0000 3.5826]matlab運(yùn)行那么控制律就是u -10.0000 * x1 - 3.5826 * x2十、Python 中計(jì)算 LQR 增益 K如果沒(méi)有 MATLAB也可以使用 Python 計(jì)算 LQR。需要安裝 scipy pip install numpy scipyPython 示例代碼import numpy as np from scipy.linalg import solve_continuous_are A np.array([ [0, 1], [0, -2] ]) B np.array([ [0], [1] ]) Q np.array([ [100, 0], [0, 1] ]) R np.array([ [1] ]) P solve_continuous_are(A, B, Q, R) K np.linalg.inv(R) B.T P print(K , K)python運(yùn)行輸出的 K 就可以寫(xiě)入單片機(jī)程序中。十一、單片機(jī)中如何寫(xiě) LQR假設(shè)我們有 4 個(gè)狀態(tài)angle 角度 gyro 角速度 position 位置 speed 速度LQR 增益為K [k1, k2, k3, k4]那么控制公式為u -(k1 * angle k2 * gyro k3 * position k4 * speed)十二、LQR 基礎(chǔ)代碼實(shí)現(xiàn)1. 定義 LQR 參數(shù)float k1 35.0f; float k2 2.5f; float k3 0.8f; float k4 1.2f;2. 定義狀態(tài)變量float angle 0.0f; float gyro 0.0f; float position 0.0f; float speed 0.0f;3. 編寫(xiě) LQR 控制函數(shù)float LQR_Controller(float angle, float gyro, float position, float speed) { float u; u -(k1 * angle k2 * gyro k3 * position k4 * speed); return u; }c運(yùn)行4. 添加限幅函數(shù)實(shí)際電機(jī) PWM 不可能無(wú)限大所以一定要限幅。float Limit(float value, float min, float max) { if (value max) { value max; } else if (value min) { value min; } return value; }c運(yùn)行5. 在控制循環(huán)中調(diào)用void Control_Loop(void) { float pwm; angle Get_Angle(); gyro Get_Gyro(); position Get_Position(); speed Get_Speed(); pwm LQR_Controller(angle, gyro, position, speed); pwm Limit(pwm, -1000.0f, 1000.0f); Motor_SetPWM(pwm); }c運(yùn)行十三、完整 LQR 單片機(jī)模板下面是一個(gè)更通用的 LQR 模板適合后期擴(kuò)展。#include math.h #define PWM_MAX 1000.0f #define PWM_MIN -1000.0f float K[4] { 35.0f, 2.5f, 0.8f, 1.2f }; float Limit(float value, float min, float max) { if (value max) { value max; } else if (value min) { value min; } return value; } float LQR_Calculate(float *x) { float u 0.0f; for (int i 0; i 4; i) { u K[i] * x[i]; } return -u; } void Control_Loop(void) { float x[4]; float pwm; x[0] Get_Angle(); x[1] Get_Gyro(); x[2] Get_Position(); x[3] Get_Speed(); pwm LQR_Calculate(x); pwm Limit(pwm, PWM_MIN, PWM_MAX); Motor_SetPWM(pwm); }c運(yùn)行十四、LQR 在單片機(jī)中的應(yīng)用場(chǎng)景應(yīng)用場(chǎng)景狀態(tài)變量示例兩輪自平衡小車(chē)角度、角速度、位置、速度倒立擺擺桿角度、擺桿角速度、小車(chē)位置、小車(chē)速度云臺(tái)控制角度誤差、角速度機(jī)器人底盤(pán)位置、速度、航向角電機(jī)控制電流、速度、位置四旋翼姿態(tài)控制姿態(tài)角、角速度十五、LQR 調(diào)參經(jīng)驗(yàn)LQR 的調(diào)參重點(diǎn)是 Q 和 R。1. 系統(tǒng)響應(yīng)太慢可以嘗試增大關(guān)鍵狀態(tài)對(duì)應(yīng)的 Q 或者減小 R例如角度回正太慢可以增大角度對(duì)應(yīng)的 Q。2. 電機(jī)輸出太猛可以嘗試增大 RR 增大后系統(tǒng)會(huì)更保守電機(jī)輸出會(huì)更加溫和。3. 系統(tǒng)容易抖動(dòng)可以嘗試增大 R 減小某些 Q 增加傳感器濾波 檢查控制周期是否穩(wěn)定 檢查狀態(tài)變量單位是否一致4. 輸出經(jīng)常達(dá)到最大值可能原因有K 參數(shù)過(guò)大 R 設(shè)置太小 狀態(tài)變量單位不統(tǒng)一 傳感器數(shù)據(jù)突變 PWM 限幅范圍太小 模型和實(shí)際系統(tǒng)差別較大十六、單片機(jī)使用 LQR 的注意事項(xiàng)1. 不建議在單片機(jī)中實(shí)時(shí)求 KLQR 的 K 矩陣一般提前在電腦上算好。單片機(jī)中只負(fù)責(zé)運(yùn)行u -Kx;這樣效率更高也更穩(wěn)定。2. 控制周期要固定如果你在建模時(shí)使用的控制周期是Ts 0.005s也就是 5ms 控制一次。那么單片機(jī)中也應(yīng)該盡量保持 5ms 執(zhí)行一次控制函數(shù)。控制周期不穩(wěn)定會(huì)影響控制效果。3. 狀態(tài)變量單位要一致比如角度單位有兩種角度制degree 弧度制rad如果 MATLAB 中使用的是弧度那么單片機(jī)中也應(yīng)該使用弧度。不要 MATLAB 里用弧度單片機(jī)中卻用角度否則 K 的效果會(huì)完全不對(duì)。4. 傳感器 數(shù)據(jù)要濾波LQR 對(duì)狀態(tài)變量比較敏感。如果角度、角速度、速度等數(shù)據(jù)噪聲很大控制輸出也會(huì)抖動(dòng)。常用濾波方式有一階低通濾波 互補(bǔ)濾波 卡爾曼濾波 滑動(dòng)平均濾波5. 必須做輸出限幅電機(jī)、電壓、電流、PWM 都有最大范圍。所以一定要做限幅處理pwm Limit(pwm, -1000.0f, 1000.0f);否則控制輸出可能異常嚴(yán)重時(shí)會(huì)損壞硬件。6. 上電初期要做保護(hù)以平衡車(chē)為例如果剛上電時(shí)車(chē)身傾角太大LQR 會(huì)立刻輸出很大的控制量。可以加入保護(hù)邏輯if (fabs(angle) 30.0f) { Motor_SetPWM(0); return; }這樣可以避免電機(jī)突然高速轉(zhuǎn)動(dòng)。十七、LQR 可以和 PID 一起用嗎可以。LQR 和 PID 不是完全對(duì)立的。在實(shí)際工程中經(jīng)常會(huì)組合使用。組合方式說(shuō)明LQR 控制姿態(tài)PID 控制速度常見(jiàn)于平衡車(chē)PID 做外環(huán)LQR 做內(nèi)環(huán)適合多級(jí)控制系統(tǒng)PID 做基礎(chǔ)控制LQR 做優(yōu)化控制適合從 PID 逐步升級(jí)LQR 狀態(tài)觀測(cè)器適合狀態(tài)無(wú)法全部測(cè)量的系統(tǒng)例如平衡車(chē)可以這樣設(shè)計(jì)速度 PID根據(jù)目標(biāo)速度計(jì)算目標(biāo)角度 姿態(tài) LQR根據(jù)角度、角速度、位置、速度控制電機(jī)十八、LQR 的優(yōu)點(diǎn)和缺點(diǎn)優(yōu)點(diǎn)1. 控制效果整體較好 2. 適合多狀態(tài)系統(tǒng) 3. 理論基礎(chǔ)清晰 4. 單片機(jī)實(shí)時(shí)運(yùn)算量小 5. 適合平衡車(chē)、倒立擺、機(jī)器人等項(xiàng)目缺點(diǎn)1. 需要建立系統(tǒng)模型 2. 需要一定矩陣和控制理論基礎(chǔ) 3. 對(duì)狀態(tài)變量質(zhì)量要求較高 4. 對(duì)模型誤差比較敏感 5. 普通 LQR 不適合強(qiáng)非線性、大范圍工況變化的系統(tǒng)十九、LQR 不適合哪些情況LQR 不是萬(wàn)能的。如果系統(tǒng)存在以下情況普通 LQR 效果可能不好系統(tǒng)強(qiáng)非線性 摩擦和死區(qū)明顯 負(fù)載變化很大 傳感器噪聲嚴(yán)重 模型和真實(shí)系統(tǒng)差別很大 控制對(duì)象變化范圍很大這時(shí)可以考慮分段 LQR 增益調(diào)度 LQR LQI MPC 滑模控制 自適應(yīng)控制對(duì)于入門(mén)階段來(lái)說(shuō)先掌握普通 LQR 就已經(jīng)足夠了。二十、LQR 入門(mén)學(xué)習(xí)路線推薦按照下面的順序?qū)W習(xí)1. 先理解 PID 控制 2. 學(xué)習(xí)狀態(tài)變量的概念 3. 學(xué)習(xí)狀態(tài)空間模型 4. 理解 x_dot Ax Bu 5. 學(xué)習(xí) Q 和 R 的意義 6. 用 MATLAB 或 Python 計(jì)算 K 7. 在單片機(jī)中實(shí)現(xiàn) u -Kx 8. 加入限幅、濾波和保護(hù)邏輯 9. 在真實(shí)系統(tǒng)中調(diào)試二十一、一個(gè)完整的 LQR 開(kāi)發(fā)流程第一步確定控制對(duì)象 例如電機(jī)、平衡車(chē)、倒立擺、云臺(tái)、機(jī)器人底盤(pán)。 第二步確定狀態(tài)變量 例如角度、角速度、位置、速度。 第三步建立狀態(tài)空間模型 寫(xiě)出 A 矩陣和 B 矩陣。 第四步選擇 Q 和 R Q 用來(lái)控制狀態(tài)誤差懲罰R 用來(lái)控制輸出懲罰。 第五步使用 MATLAB 或 Python 計(jì)算 K 得到 LQR 增益矩陣。 第六步將 K 寫(xiě)入單片機(jī)程序 單片機(jī)中只需要執(zhí)行 u -Kx。 第七步加入限幅、濾波和安全保護(hù) 避免電機(jī)輸出過(guò)大或系統(tǒng)失控。 第八步進(jìn)行實(shí)物調(diào)試 根據(jù)實(shí)際效果繼續(xù)調(diào)整 Q、R 或 K。二十二、最小可用 LQR 控制代碼如果只是想快速移植可以使用下面這個(gè)最小模板float LQR_Control(float *x, float *K, int n) { float u 0.0f; for (int i 0; i n; i) { u K[i] * x[i]; } return -u; }c運(yùn)行調(diào)用示例float x[4] { angle, gyro, position, speed }; float K[4] { 35.0f, 2.5f, 0.8f, 1.2f }; float pwm LQR_Control(x, K, 4); pwm Limit(pwm, -1000.0f, 1000.0f); Motor_SetPWM(pwm);二十三、總結(jié)LQR 在單片機(jī)中的實(shí)現(xiàn)并沒(méi)有想象中復(fù)雜。它的核心就是u -Kx真正復(fù)雜的部分在于建立模型 計(jì)算 K 矩陣 選擇 Q 和 R 實(shí)物調(diào)試而單片機(jī)實(shí)時(shí)運(yùn)行時(shí)只需要做簡(jiǎn)單的乘法和加法。可以這樣理解PID 是根據(jù)誤差進(jìn)行控制。LQR 是根據(jù)多個(gè)狀態(tài)綜合控制。PID 更適合簡(jiǎn)單系統(tǒng)。LQR 更適合狀態(tài)較多、耦合較強(qiáng)的系統(tǒng)。對(duì)于平衡車(chē)、倒立擺、機(jī)器人底盤(pán)、云臺(tái)控制、電機(jī)控制等項(xiàng)目來(lái)說(shuō)LQR 是一個(gè)非常值得學(xué)習(xí)的控制算法。最后記住一句話上位機(jī)負(fù)責(zé)計(jì)算 K 單片機(jī)負(fù)責(zé)執(zhí)行 u -Kx掌握這個(gè)思路就可以把 LQR 真正應(yīng)用到嵌入式控制項(xiàng)目中了。單片機(jī)中使用 LQR 控制算法入門(mén)教程從 PID 到現(xiàn)代控制-CSDN博客