单片机中使用 LQR 控制算法入门教程：从 PID 到现代控制 🚀

很多同学在学习单片机控制时，最先接触的是 PID。
PID 简单、好用、容易上手，但当控制对象变得复杂时，比如两轮平衡车、倒立摆、机器人底盘、云台、电机姿态控制等，单纯依靠 PID 调参会越来越困难。

这时，就可以学习一种更系统化的控制方法：LQR 控制算法。

---

一、LQR 是什么？

LQR 的全称是：

Linear Quadratic Regulator
线性二次型调节器

简单来说，LQR 是一种现代控制算法。

它的目标可以理解为：

在让系统尽快稳定的同时，尽量不要让控制输出太大。

举个例子：

假设我们要控制一辆小车回到目标位置。

系统需要考虑：

• 小车离目标位置有多远；
• 小车当前速度有多快；
• 控制电机时输出不能太猛；
• 最终希望系统又快又稳地回到目标状态。

LQR 就是根据这些状态，自动计算一个比较合理的控制输出。

---

二、LQR 和 PID 有什么区别？

对比项	PID 控制	LQR 控制
控制思想	根据误差调节输出	根据系统状态整体调节输出
调参对象	Kp、Ki、Kd	Q 矩阵、R 矩阵
是否依赖模型	依赖较少	需要状态空间模型
适合对象	单输入单输出系统	多状态耦合系统
单片机实现难度	简单	中等
控制效果	容易上手	更适合复杂系统

一句话总结：

PID 更像是“看到误差就修正”，LQR 更像是“根据整个系统状态做最优决策”。

---

三、LQR 的核心公式

LQR 最核心的控制公式其实非常简单：

u = -Kx

其中：

符号	含义
u	控制输出，例如 PWM、电压、电流指令
K	LQR 增益矩阵
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 控制系统整体流程

七、LQR 需要的数学模型

LQR 需要把系统写成状态空间形式。

连续系统一般写成：

x_dot = A x + B u

离散系统一般写成：

x(k+1) = 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];

设置 Q 和 R：

Q = [100 0;
     0   1];

R = 1;

计算 LQR 增益：

K = lqr(A, B, Q, R);

如果输出结果为：

K = [10.0000 3.5826]

那么控制律就是：

u = -10.0000 * x1 - 3.5826 * x2

---

十、Python 中计算 LQR 增益 K

如果没有 MATLAB，也可以使用 Python 计算 LQR。

需要安装 scipy：

pip install numpy scipy

Python 示例代码：

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)

输出的 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;
}

---

4. 添加限幅函数

实际电机 PWM 不可能无限大，所以一定要限幅。

float Limit(float value, float min, float max)
{
    if (value > max)
    {
        value = max;
    }
    else if (value < min)
    {
        value = min;
    }

    return value;
}

---

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);
}

---

十三、完整 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);
}

---

十四、LQR 在单片机中的应用场景

应用场景	状态变量示例
两轮自平衡小车	角度、角速度、位置、速度
倒立摆	摆杆角度、摆杆角速度、小车位置、小车速度
云台控制	角度误差、角速度
机器人底盘	位置、速度、航向角
电机控制	电流、速度、位置
四旋翼姿态控制	姿态角、角速度

---

十五、LQR 调参经验

LQR 的调参重点是 Q 和 R。

---

1. 系统响应太慢

可以尝试：

增大关键状态对应的 Q
或者减小 R

例如角度回正太慢，可以增大角度对应的 Q。

---

2. 电机输出太猛

可以尝试：

增大 R

R 增大后，系统会更保守，电机输出会更加温和。

---

3. 系统容易抖动

可以尝试：

增大 R
减小某些 Q
增加传感器滤波
检查控制周期是否稳定
检查状态变量单位是否一致

---

4. 输出经常达到最大值

可能原因有：

K 参数过大
R 设置太小
状态变量单位不统一
传感器数据突变
PWM 限幅范围太小
模型和实际系统差别较大

---

十六、单片机使用 LQR 的注意事项

1. 不建议在单片机中实时求 K

LQR 的 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;
}

调用示例：

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 真正应用到嵌入式控制项目中了。