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