尧图网页设计 
单片机中使用 LQR 控制算法入门教程从 PID 到现代控制 很多同学在学习单片机控制时最先接触的是 PID。PID 简单、好用、容易上手但当控制对象变得复杂时比如两轮平衡车、倒立摆、机器人底盘、云台、电机姿态控制等单纯依靠 PID 调参会越来越困难。这时就可以学习一种更系统化的控制方法LQR 控制算法。一、LQR 是什么LQR 的全称是Linear Quadratic Regulator 线性二次型调节器text简单来说LQR 是一种现代控制算法。它的目标可以理解为在让系统尽快稳定的同时尽量不要让控制输出太大。举个例子假设我们要控制一辆小车回到目标位置。系统需要考虑小车离目标位置有多远小车当前速度有多快控制电机时输出不能太猛最终希望系统又快又稳地回到目标状态。LQR 就是根据这些状态自动计算一个比较合理的控制输出。二、LQR 和 PID 有什么区别对比项PID 控制LQR 控制控制思想根据误差调节输出根据系统状态整体调节输出调参对象Kp、Ki、KdQ 矩阵、R 矩阵是否依赖模型依赖较少需要状态空间模型适合对象单输入单输出系统多状态耦合系统单片机实现难度简单中等控制效果容易上手更适合复杂系统一句话总结PID 更像是“看到误差就修正”LQR 更像是“根据整个系统状态做最优决策”。三、LQR 的核心公式LQR 最核心的控制公式其实非常简单u -Kx其中符号含义u控制输出例如 PWM、电压、电流指令KLQR 增益矩阵x系统状态变量-负反馈让系统趋于稳定也就是说单片机真正运行 LQR 时本质上就是做一次乘加运算。四、状态变量是什么LQR 不是只看一个误差而是会综合多个状态进行控制。例如两轮平衡车可以选择以下状态变量x [角度, 角速度, 位置, 速度]也就是状态变量含义angle车身倾角gyro车身角速度position小车位置speed小车速度对应的 LQR 控制律可以写成u -(k1 * angle k2 * gyro k3 * position k4 * speed)这就非常适合在单片机中实现。五、单片机中使用 LQR 的基本思路很多人一开始会误以为LQR 是不是要在单片机里面实时求矩阵其实一般不是。在实际 项目 中LQR 通常分为两个阶段。1. 上位机离线计算使用 MATLAB、Python 或其他工具计算出 LQR 增益矩阵 K。这一步会涉及矩阵计算、Riccati 方程求解计算量比较大一般不放在普通单片机中实时运行。2. 单片机实时运行单片机只需要执行u -Kx也就是几个乘法和加法。例如u -(k1 * angle k2 * gyro k3 * position k4 * speed);所以LQR 在单片机中的实时计算量其实很小。六、LQR 控制系统 整体流程采集传感器数据计算系统状态 x代入 LQR 控制律 u -Kx限制控制输出范围输出 PWM 控制电机系统状态变化七、LQR 需要的 数学模型LQR 需要把系统写成状态空间形式。连续系统一般写成x_dot A x B u离散系统一般写成x(k1) A_d x(k) B_d u(k)其中矩阵或变量含义x系统状态变量u控制输入A系统自身状态变化关系B控制输入对系统的影响A_d离散化后的 A 矩阵B_d离散化后的 B 矩阵对于单片机来说通常最终使用的是离散模型因为单片机控制是按固定周期执行的。例如每 5ms 执行一次控制算法 每 10ms 执行一次控制算法八、Q 矩阵和 R 矩阵怎么理解LQR 调参主要调两个东西Q 矩阵 R 矩阵通俗理解矩阵作用Q你有多在意状态误差R你有多在意控制输出大小1. Q 矩阵Q 矩阵用来表示你对不同状态的重视程度。例如平衡车状态为x [角度, 角速度, 位置, 速度]如果你特别希望角度保持稳定就可以把角度对应的 Q 值调大。例如Q diag([100, 1, 10, 1])含义如下状态权重角度100最重要角速度1普通位置10比较重要速度1普通2. R 矩阵R 矩阵用来限制控制输出。可以这样理解R 越大控制越温和R 越小控制越激进。例如R 10表示不希望电机输出太猛系统响应会更柔和。R 0.1表示允许电机输出更积极系统响应会更快但也可能更容易抖动。九、MATLAB 中计算 LQR 增益 K假设有一个简单系统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 个状态angle 角度 gyro 角速度 position 位置 speed 速度LQR 增益为K [k1, k2, k3, k4]那么控制公式为u -(k1 * angle k2 * gyro k3 * position k4 * speed)十二、LQR 基础代码实现1. 定义 LQR 参数float k1 35.0f; float k2 2.5f; float k3 0.8f; float k4 1.2f;2. 定义状态变量float angle 0.0f; float gyro 0.0f; float position 0.0f; float speed 0.0f;3. 编写 LQR 控制函数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. 添加限幅函数实际电机 PWM 不可能无限大所以一定要限幅。float Limit(float value, float min, float max) { if (value max) { value max; } else if (value min) { value min; } return value; }c运行5. 在控制循环中调用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 在单片机中的应用场景应用场景状态变量示例两轮自平衡小车角度、角速度、位置、速度倒立摆摆杆角度、摆杆角速度、小车位置、小车速度云台控制角度误差、角速度机器人底盘位置、速度、航向角电机控制电流、速度、位置四旋翼姿态控制姿态角、角速度十五、LQR 调参经验LQR 的调参重点是 Q 和 R。1. 系统响应太慢可以尝试增大关键状态对应的 Q 或者减小 R例如角度回正太慢可以增大角度对应的 Q。2. 电机输出太猛可以尝试增大 RR 增大后系统会更保守电机输出会更加温和。3. 系统容易抖动可以尝试增大 R 减小某些 Q 增加传感器滤波 检查控制周期是否稳定 检查状态变量单位是否一致4. 输出经常达到最大值可能原因有K 参数过大 R 设置太小 状态变量单位不统一 传感器数据突变 PWM 限幅范围太小 模型和实际系统差别较大十六、单片机使用 LQR 的注意事项1. 不建议在单片机中实时求 KLQR 的 K 矩阵一般提前在电脑上算好。单片机中只负责运行u -Kx;这样效率更高也更稳定。2. 控制周期要固定如果你在建模时使用的控制周期是Ts 0.005s也就是 5ms 控制一次。那么单片机中也应该尽量保持 5ms 执行一次控制函数。控制周期不稳定会影响控制效果。3. 状态变量单位要一致比如角度单位有两种角度制degree 弧度制rad如果 MATLAB 中使用的是弧度那么单片机中也应该使用弧度。不要 MATLAB 里用弧度单片机中却用角度否则 K 的效果会完全不对。4. 传感器 数据要滤波LQR 对状态变量比较敏感。如果角度、角速度、速度等数据噪声很大控制输出也会抖动。常用滤波方式有一阶低通滤波 互补滤波 卡尔曼滤波 滑动平均滤波5. 必须做输出限幅电机、电压、电流、PWM 都有最大范围。所以一定要做限幅处理pwm Limit(pwm, -1000.0f, 1000.0f);否则控制输出可能异常严重时会损坏硬件。6. 上电初期要做保护以平衡车为例如果刚上电时车身倾角太大LQR 会立刻输出很大的控制量。可以加入保护逻辑if (fabs(angle) 30.0f) { Motor_SetPWM(0); return; }这样可以避免电机突然高速转动。十七、LQR 可以和 PID 一起用吗可以。LQR 和 PID 不是完全对立的。在实际工程中经常会组合使用。组合方式说明LQR 控制姿态PID 控制速度常见于平衡车PID 做外环LQR 做内环适合多级控制系统PID 做基础控制LQR 做优化控制适合从 PID 逐步升级LQR 状态观测器适合状态无法全部测量的系统例如平衡车可以这样设计速度 PID根据目标速度计算目标角度 姿态 LQR根据角度、角速度、位置、速度控制电机十八、LQR 的优点和缺点优点1. 控制效果整体较好 2. 适合多状态系统 3. 理论基础清晰 4. 单片机实时运算量小 5. 适合平衡车、倒立摆、机器人等项目缺点1. 需要建立系统模型 2. 需要一定矩阵和控制理论基础 3. 对状态变量质量要求较高 4. 对模型误差比较敏感 5. 普通 LQR 不适合强非线性、大范围工况变化的系统十九、LQR 不适合哪些情况LQR 不是万能的。如果系统存在以下情况普通 LQR 效果可能不好系统强非线性 摩擦和死区明显 负载变化很大 传感器噪声严重 模型和真实系统差别很大 控制对象变化范围很大这时可以考虑分段 LQR 增益调度 LQR LQI MPC 滑模控制 自适应控制对于入门阶段来说先掌握普通 LQR 就已经足够了。二十、LQR 入门学习路线推荐按照下面的顺序学习1. 先理解 PID 控制 2. 学习状态变量的概念 3. 学习状态空间模型 4. 理解 x_dot Ax Bu 5. 学习 Q 和 R 的意义 6. 用 MATLAB 或 Python 计算 K 7. 在单片机中实现 u -Kx 8. 加入限幅、滤波和保护逻辑 9. 在真实系统中调试二十一、一个完整的 LQR 开发流程第一步确定控制对象 例如电机、平衡车、倒立摆、云台、机器人底盘。 第二步确定状态变量 例如角度、角速度、位置、速度。 第三步建立状态空间模型 写出 A 矩阵和 B 矩阵。 第四步选择 Q 和 R Q 用来控制状态误差惩罚R 用来控制输出惩罚。 第五步使用 MATLAB 或 Python 计算 K 得到 LQR 增益矩阵。 第六步将 K 写入单片机程序 单片机中只需要执行 u -Kx。 第七步加入限幅、滤波和安全保护 避免电机输出过大或系统失控。 第八步进行实物调试 根据实际效果继续调整 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 在单片机中的实现并没有想象中复杂。它的核心就是u -Kx真正复杂的部分在于建立模型 计算 K 矩阵 选择 Q 和 R 实物调试而单片机实时运行时只需要做简单的乘法和加法。可以这样理解PID 是根据误差进行控制。LQR 是根据多个状态综合控制。PID 更适合简单系统。LQR 更适合状态较多、耦合较强的系统。对于平衡车、倒立摆、机器人底盘、云台控制、电机控制等项目来说LQR 是一个非常值得学习的控制算法。最后记住一句话上位机负责计算 K 单片机负责执行 u -Kx掌握这个思路就可以把 LQR 真正应用到嵌入式控制项目中了。单片机中使用 LQR 控制算法入门教程从 PID 到现代控制-CSDN博客 单片机中使用 LQR 控制算法入门教程：从 PID 到现代控制
单片机中使用 LQR 控制算法入门教程从 PID 到现代控制 很多同学在学习单片机控制时最先接触的是 PID。PID 简单、好用、容易上手但当控制对象变得复杂时比如两轮平衡车、倒立摆、机器人底盘、云台、电机姿态控制等单纯依靠 PID 调参会越来越困难。这时就可以学习一种更系统化的控制方法LQR 控制算法。一、LQR 是什么LQR 的全称是Linear Quadratic Regulator 线性二次型调节器text简单来说LQR 是一种现代控制算法。它的目标可以理解为在让系统尽快稳定的同时尽量不要让控制输出太大。举个例子假设我们要控制一辆小车回到目标位置。系统需要考虑小车离目标位置有多远小车当前速度有多快控制电机时输出不能太猛最终希望系统又快又稳地回到目标状态。LQR 就是根据这些状态自动计算一个比较合理的控制输出。二、LQR 和 PID 有什么区别对比项PID 控制LQR 控制控制思想根据误差调节输出根据系统状态整体调节输出调参对象Kp、Ki、KdQ 矩阵、R 矩阵是否依赖模型依赖较少需要状态空间模型适合对象单输入单输出系统多状态耦合系统单片机实现难度简单中等控制效果容易上手更适合复杂系统一句话总结PID 更像是“看到误差就修正”LQR 更像是“根据整个系统状态做最优决策”。三、LQR 的核心公式LQR 最核心的控制公式其实非常简单u -Kx其中符号含义u控制输出例如 PWM、电压、电流指令KLQR 增益矩阵x系统状态变量-负反馈让系统趋于稳定也就是说单片机真正运行 LQR 时本质上就是做一次乘加运算。四、状态变量是什么LQR 不是只看一个误差而是会综合多个状态进行控制。例如两轮平衡车可以选择以下状态变量x [角度, 角速度, 位置, 速度]也就是状态变量含义angle车身倾角gyro车身角速度position小车位置speed小车速度对应的 LQR 控制律可以写成u -(k1 * angle k2 * gyro k3 * position k4 * speed)这就非常适合在单片机中实现。五、单片机中使用 LQR 的基本思路很多人一开始会误以为LQR 是不是要在单片机里面实时求矩阵其实一般不是。在实际 项目 中LQR 通常分为两个阶段。1. 上位机离线计算使用 MATLAB、Python 或其他工具计算出 LQR 增益矩阵 K。这一步会涉及矩阵计算、Riccati 方程求解计算量比较大一般不放在普通单片机中实时运行。2. 单片机实时运行单片机只需要执行u -Kx也就是几个乘法和加法。例如u -(k1 * angle k2 * gyro k3 * position k4 * speed);所以LQR 在单片机中的实时计算量其实很小。六、LQR 控制系统 整体流程采集传感器数据计算系统状态 x代入 LQR 控制律 u -Kx限制控制输出范围输出 PWM 控制电机系统状态变化七、LQR 需要的 数学模型LQR 需要把系统写成状态空间形式。连续系统一般写成x_dot A x B u离散系统一般写成x(k1) A_d x(k) B_d u(k)其中矩阵或变量含义x系统状态变量u控制输入A系统自身状态变化关系B控制输入对系统的影响A_d离散化后的 A 矩阵B_d离散化后的 B 矩阵对于单片机来说通常最终使用的是离散模型因为单片机控制是按固定周期执行的。例如每 5ms 执行一次控制算法 每 10ms 执行一次控制算法八、Q 矩阵和 R 矩阵怎么理解LQR 调参主要调两个东西Q 矩阵 R 矩阵通俗理解矩阵作用Q你有多在意状态误差R你有多在意控制输出大小1. Q 矩阵Q 矩阵用来表示你对不同状态的重视程度。例如平衡车状态为x [角度, 角速度, 位置, 速度]如果你特别希望角度保持稳定就可以把角度对应的 Q 值调大。例如Q diag([100, 1, 10, 1])含义如下状态权重角度100最重要角速度1普通位置10比较重要速度1普通2. R 矩阵R 矩阵用来限制控制输出。可以这样理解R 越大控制越温和R 越小控制越激进。例如R 10表示不希望电机输出太猛系统响应会更柔和。R 0.1表示允许电机输出更积极系统响应会更快但也可能更容易抖动。九、MATLAB 中计算 LQR 增益 K假设有一个简单系统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 个状态angle 角度 gyro 角速度 position 位置 speed 速度LQR 增益为K [k1, k2, k3, k4]那么控制公式为u -(k1 * angle k2 * gyro k3 * position k4 * speed)十二、LQR 基础代码实现1. 定义 LQR 参数float k1 35.0f; float k2 2.5f; float k3 0.8f; float k4 1.2f;2. 定义状态变量float angle 0.0f; float gyro 0.0f; float position 0.0f; float speed 0.0f;3. 编写 LQR 控制函数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. 添加限幅函数实际电机 PWM 不可能无限大所以一定要限幅。float Limit(float value, float min, float max) { if (value max) { value max; } else if (value min) { value min; } return value; }c运行5. 在控制循环中调用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 在单片机中的应用场景应用场景状态变量示例两轮自平衡小车角度、角速度、位置、速度倒立摆摆杆角度、摆杆角速度、小车位置、小车速度云台控制角度误差、角速度机器人底盘位置、速度、航向角电机控制电流、速度、位置四旋翼姿态控制姿态角、角速度十五、LQR 调参经验LQR 的调参重点是 Q 和 R。1. 系统响应太慢可以尝试增大关键状态对应的 Q 或者减小 R例如角度回正太慢可以增大角度对应的 Q。2. 电机输出太猛可以尝试增大 RR 增大后系统会更保守电机输出会更加温和。3. 系统容易抖动可以尝试增大 R 减小某些 Q 增加传感器滤波 检查控制周期是否稳定 检查状态变量单位是否一致4. 输出经常达到最大值可能原因有K 参数过大 R 设置太小 状态变量单位不统一 传感器数据突变 PWM 限幅范围太小 模型和实际系统差别较大十六、单片机使用 LQR 的注意事项1. 不建议在单片机中实时求 KLQR 的 K 矩阵一般提前在电脑上算好。单片机中只负责运行u -Kx;这样效率更高也更稳定。2. 控制周期要固定如果你在建模时使用的控制周期是Ts 0.005s也就是 5ms 控制一次。那么单片机中也应该尽量保持 5ms 执行一次控制函数。控制周期不稳定会影响控制效果。3. 状态变量单位要一致比如角度单位有两种角度制degree 弧度制rad如果 MATLAB 中使用的是弧度那么单片机中也应该使用弧度。不要 MATLAB 里用弧度单片机中却用角度否则 K 的效果会完全不对。4. 传感器 数据要滤波LQR 对状态变量比较敏感。如果角度、角速度、速度等数据噪声很大控制输出也会抖动。常用滤波方式有一阶低通滤波 互补滤波 卡尔曼滤波 滑动平均滤波5. 必须做输出限幅电机、电压、电流、PWM 都有最大范围。所以一定要做限幅处理pwm Limit(pwm, -1000.0f, 1000.0f);否则控制输出可能异常严重时会损坏硬件。6. 上电初期要做保护以平衡车为例如果刚上电时车身倾角太大LQR 会立刻输出很大的控制量。可以加入保护逻辑if (fabs(angle) 30.0f) { Motor_SetPWM(0); return; }这样可以避免电机突然高速转动。十七、LQR 可以和 PID 一起用吗可以。LQR 和 PID 不是完全对立的。在实际工程中经常会组合使用。组合方式说明LQR 控制姿态PID 控制速度常见于平衡车PID 做外环LQR 做内环适合多级控制系统PID 做基础控制LQR 做优化控制适合从 PID 逐步升级LQR 状态观测器适合状态无法全部测量的系统例如平衡车可以这样设计速度 PID根据目标速度计算目标角度 姿态 LQR根据角度、角速度、位置、速度控制电机十八、LQR 的优点和缺点优点1. 控制效果整体较好 2. 适合多状态系统 3. 理论基础清晰 4. 单片机实时运算量小 5. 适合平衡车、倒立摆、机器人等项目缺点1. 需要建立系统模型 2. 需要一定矩阵和控制理论基础 3. 对状态变量质量要求较高 4. 对模型误差比较敏感 5. 普通 LQR 不适合强非线性、大范围工况变化的系统十九、LQR 不适合哪些情况LQR 不是万能的。如果系统存在以下情况普通 LQR 效果可能不好系统强非线性 摩擦和死区明显 负载变化很大 传感器噪声严重 模型和真实系统差别很大 控制对象变化范围很大这时可以考虑分段 LQR 增益调度 LQR LQI MPC 滑模控制 自适应控制对于入门阶段来说先掌握普通 LQR 就已经足够了。二十、LQR 入门学习路线推荐按照下面的顺序学习1. 先理解 PID 控制 2. 学习状态变量的概念 3. 学习状态空间模型 4. 理解 x_dot Ax Bu 5. 学习 Q 和 R 的意义 6. 用 MATLAB 或 Python 计算 K 7. 在单片机中实现 u -Kx 8. 加入限幅、滤波和保护逻辑 9. 在真实系统中调试二十一、一个完整的 LQR 开发流程第一步确定控制对象 例如电机、平衡车、倒立摆、云台、机器人底盘。 第二步确定状态变量 例如角度、角速度、位置、速度。 第三步建立状态空间模型 写出 A 矩阵和 B 矩阵。 第四步选择 Q 和 R Q 用来控制状态误差惩罚R 用来控制输出惩罚。 第五步使用 MATLAB 或 Python 计算 K 得到 LQR 增益矩阵。 第六步将 K 写入单片机程序 单片机中只需要执行 u -Kx。 第七步加入限幅、滤波和安全保护 避免电机输出过大或系统失控。 第八步进行实物调试 根据实际效果继续调整 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 在单片机中的实现并没有想象中复杂。它的核心就是u -Kx真正复杂的部分在于建立模型 计算 K 矩阵 选择 Q 和 R 实物调试而单片机实时运行时只需要做简单的乘法和加法。可以这样理解PID 是根据误差进行控制。LQR 是根据多个状态综合控制。PID 更适合简单系统。LQR 更适合状态较多、耦合较强的系统。对于平衡车、倒立摆、机器人底盘、云台控制、电机控制等项目来说LQR 是一个非常值得学习的控制算法。最后记住一句话上位机负责计算 K 单片机负责执行 u -Kx掌握这个思路就可以把 LQR 真正应用到嵌入式控制项目中了。单片机中使用 LQR 控制算法入门教程从 PID 到现代控制-CSDN博客