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