单片机 LQR 进阶实战教程：从建模、离散化到嵌入式代码落地
·
单片机 LQR 进阶实战教程：从建模、离散化到嵌入式代码落地 🚀
前面我们已经知道，LQR 的核心公式其实很简单：
u = -Kx但是在真实单片机项目中，难点并不只是写出这行代码，而是：
- 状态量怎么选？
- 控制周期怎么定？
- K 矩阵怎么计算？
- 传感器数据怎么处理？
- PWM 输出怎么限幅？
- 电机抖动、输出打满、系统发散怎么排查？
这篇文章就以两轮自平衡小车 / 倒立摆类系统为例，讲一套比较完整的 LQR 工程落地流程。
一、LQR 实战项目目标
本文假设我们要做一个类似两轮平衡车的控制系统。
控制目标：
让小车保持直立，同时尽量稳定在目标位置附近。
单片机需要完成：
1. 读取 IMU 姿态角度
2. 读取陀螺仪角速度
3. 读取编码器位置和速度
4. 使用 LQR 计算控制输出
5. 输出 PWM 控制电机
6. 做限幅、保护和异常检测
二、LQR 工程实现整体架构
LQR 在单片机中的工程流程一般是：
系统建模
↓
线性化处理
↓
离散化模型
↓
选择 Q 和 R
↓
MATLAB / Python 计算 K
↓
将 K 写入单片机
↓
定时器周期调用 LQR
↓
PWM 控制电机
↓
实物调参
如果用流程图表示：
三、状态变量怎么选？
LQR 的核心不是只看一个误差，而是看整个系统状态。
以两轮平衡车为例，常见状态变量可以选成：
x = [角度误差, 角速度, 位置误差, 速度]
对应变量为：
| 状态变量 | 代码变量 | 含义 |
|---|---|---|
| x1 | angle_error | 车身角度误差 |
| x2 | gyro | 车身角速度 |
| x3 | position_error | 小车位置误差 |
| x4 | speed | 小车速度 |
所以状态向量可以写成：
x = [
angle_error,
gyro,
position_error,
speed
]
LQR 控制律为：
u = -(k1 * angle_error + k2 * gyro + k3 * position_error + k4 * speed)
也就是：
u = -(K[0] * angle_error
+ K[1] * gyro
+ K[2] * position_error
+ K[3] * speed);
四、为什么要使用“误差状态”？
在实际控制中，我们通常不是直接使用原始状态，而是使用目标值和当前值的差。
例如：
angle_error = angle - target_angle
position_error = position - target_position
如果目标是让车身保持直立：
target_angle = 0
如果目标是让小车停在当前位置：
target_position = current_position_when_start
所以实际状态量应该是：
x1 = angle - target_angle
x2 = gyro - target_gyro
x3 = position - target_position
x4 = speed - target_speed
一般情况下：
target_gyro = 0
target_speed = 0
五、连续 LQR 和离散 LQR 的区别
很多资料直接写：
x_dot = A x + B u
这是连续系统模型。
但是单片机不是连续运行的，而是每隔固定时间运行一次控制算法。
例如：
每 5ms 控制一次
每 10ms 控制一次
所以单片机更适合使用离散系统：
x(k+1) = A_d x(k) + B_d u(k)
其中：
| 符号 | 含义 |
|---|---|
| A | 连续系统矩阵 |
| B | 连续输入矩阵 |
| A_d | 离散系统矩阵 |
| B_d | 离散输入矩阵 |
| Ts | 采样周期 |
实际工程中建议：
先建立连续模型
再根据控制周期 Ts 离散化
最后使用离散 LQR 计算 K
六、控制周期 Ts 怎么选？
控制周期非常关键。
对于单片机控制项目，一般可以参考：
| 控制对象 | 推荐控制周期 |
|---|---|
| 普通直流电机速度环 | 5ms ~ 20ms |
| 平衡车姿态控制 | 2ms ~ 5ms |
| 云台姿态控制 | 1ms ~ 5ms |
| 四旋翼姿态控制 | 1ms ~ 2ms |
| 慢速温控系统 | 100ms 以上 |
对于平衡车类项目，常用：
Ts = 0.005s
也就是：
5ms 执行一次 LQR 控制
注意：
MATLAB / Python 里计算 K 时用的 Ts，必须和单片机实际控制周期一致。
如果上位机用 5ms 算 K，单片机却 20ms 才运行一次，控制效果很可能变差甚至发散。
七、建立一个简化状态空间模型
以平衡车为例，真实模型比较复杂，涉及电机、电池、电机反电动势、轮子半径、车身质量、转动惯量等参数。
为了入门实战，可以先使用简化模型理解流程。
假设系统状态为：
x = [theta, theta_dot, position, velocity]
其中：
| 状态 | 含义 |
|---|---|
| theta | 车身倾角 |
| theta_dot | 倾角角速度 |
| position | 轮子位置 |
| velocity | 轮子速度 |
连续模型可以写成：
x_dot = A x + B u
示例 A、B 矩阵如下：
A = [0 1 0 0;
20 0 0 0;
0 0 0 1;
-1 0 0 0];
B = [0;
1;
0;
1];
注意：
这里的 A、B 只是为了演示完整流程，不是所有平衡车都能直接用。
真正项目中需要根据自己的机械结构、电机参数、车体质量和采样周期重新建模或辨识。
八、使用 MATLAB 计算离散 LQR
1. 定义连续模型
A = [0 1 0 0;
20 0 0 0;
0 0 0 1;
-1 0 0 0];
B = [0;
1;
0;
1];
C = eye(4);
D = zeros(4, 1);
2. 设置采样周期
Ts = 0.005;
表示 5ms 控制一次。
3. 连续系统离散化
sys_c = ss(A, B, C, D);
sys_d = c2d(sys_c, Ts);
Ad = sys_d.A;
Bd = sys_d.B;
4. 设置 Q 和 R
Q = diag([300, 10, 20, 5]);
R = 1;
含义：
| 权重 | 说明 |
|---|---|
| 300 | 非常重视角度误差 |
| 10 | 比较重视角速度 |
| 20 | 适当重视位置 |
| 5 | 适当重视速度 |
| R = 1 | 控制输出强度适中 |
5. 计算离散 LQR 增益
K = dlqr(Ad, Bd, Q, R)
输出可能类似：
K = [45.2312 8.7421 3.1254 4.8832]
然后单片机中就可以使用：
float K[4] = {
45.2312f,
8.7421f,
3.1254f,
4.8832f
};
九、使用 Python 计算离散 LQR
如果没有 MATLAB，也可以用 Python。
安装依赖：
pip install numpy scipy
完整代码：
import numpy as np
from scipy.signal import cont2discrete
from scipy.linalg import solve_discrete_are
A = np.array([
[0, 1, 0, 0],
[20, 0, 0, 0],
[0, 0, 0, 1],
[-1, 0, 0, 0]
])
B = np.array([
[0],
[1],
[0],
[1]
])
C = np.eye(4)
D = np.zeros((4, 1))
Ts = 0.005
Ad, Bd, Cd, Dd, _ = cont2discrete((A, B, C, D), Ts)
Q = np.diag([300, 10, 20, 5])
R = np.array([[1]])
P = solve_discrete_are(Ad, Bd, Q, R)
K = np.linalg.inv(Bd.T @ P @ Bd + R) @ (Bd.T @ P @ Ad)
print("Ad =")
print(Ad)
print("Bd =")
print(Bd)
print("K =")
print(K)
输出的 K 写入单片机即可。
十、单片机工程代码结构建议
建议把 LQR 控制相关代码单独封装。
例如：
User
├── control
│ ├── lqr.h
│ ├── lqr.c
│ ├── motor.h
│ ├── motor.c
│ ├── encoder.h
│ ├── encoder.c
│ ├── imu.h
│ └── imu.c
这样结构会比较清晰：
| 文件 | 作用 |
|---|---|
| lqr.c / lqr.h | LQR 控制算法 |
| motor.c / motor.h | 电机 PWM 输出 |
| encoder.c / encoder.h | 编码器测速和位置计算 |
| imu.c / imu.h | 姿态角和角速度获取 |
| timer.c | 定时器周期调用控制函数 |
十一、lqr.h 文件
#ifndef __LQR_H
#define __LQR_H
typedef struct
{
float angle;
float gyro;
float position;
float speed;
} LQR_State_t;
typedef struct
{
float target_angle;
float target_position;
float target_speed;
} LQR_Target_t;
void LQR_Init(void);
float LQR_Calculate(LQR_State_t state, LQR_Target_t target);
float LQR_Limit(float value, float min, float max);
#endif
十二、lqr.c 文件
#include "lqr.h"
#include "math.h"
#define PWM_MAX 1000.0f
#define PWM_MIN -1000.0f
static float K[4] = {
45.2312f,
8.7421f,
3.1254f,
4.8832f
};
void LQR_Init(void)
{
/*
* 如果后期需要从 Flash、EEPROM 或串口加载 K 参数，
* 可以在这里进行初始化。
*/
}
float LQR_Limit(float value, float min, float max)
{
if (value > max)
{
value = max;
}
else if (value < min)
{
value = min;
}
return value;
}
float LQR_Calculate(LQR_State_t state, LQR_Target_t target)
{
float x[4];
float u;
x[0] = state.angle - target.target_angle;
x[1] = state.gyro;
x[2] = state.position - target.target_position;
x[3] = state.speed - target.target_speed;
u = -(K[0] * x[0]
+ K[1] * x[1]
+ K[2] * x[2]
+ K[3] * x[3]);
u = LQR_Limit(u, PWM_MIN, PWM_MAX);
return u;
}
十三、控制循环代码
LQR 一般放在定时器中断或者固定周期任务里运行。
例如 5ms 调用一次：
void Control_Task_5ms(void)
{
LQR_State_t state;
LQR_Target_t target;
float pwm;
state.angle = IMU_GetAngle();
state.gyro = IMU_GetGyro();
state.position = Encoder_GetPosition();
state.speed = Encoder_GetSpeed();
target.target_angle = 0.0f;
target.target_position = 0.0f;
target.target_speed = 0.0f;
pwm = LQR_Calculate(state, target);
Motor_SetPWM(pwm);
}
十四、定时器中断调用示例
以 STM32 HAL 库为例：
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
if (htim->Instance == TIM2)
{
Control_Task_5ms();
}
}
但是工程上要注意：
不建议在中断里面写太多耗时操作。
如果 IMU 读取、串口打印、滤波计算很重，可以在主循环中设置标志位，让主循环执行控制任务。
更稳妥的方式：
volatile uint8_t control_flag = 0;
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
if (htim->Instance == TIM2)
{
control_flag = 1;
}
}
int main(void)
{
while (1)
{
if (control_flag)
{
control_flag = 0;
Control_Task_5ms();
}
}
}
十五、传感器状态获取
1. 角度 angle
角度通常来自 IMU 姿态解算。
可以使用：
MPU6050
MPU6500
ICM20602
BMI088
常见姿态解算方法：
互补滤波
卡尔曼滤波
Mahony 滤波
Madgwick 滤波
对于入门项目，互补滤波就够用。
简单形式：
angle = alpha * (angle + gyro * dt) + (1.0f - alpha) * acc_angle;
其中：
| 参数 | 含义 |
|---|---|
| angle | 融合后的角度 |
| gyro | 陀螺仪角速度 |
| dt | 采样周期 |
| acc_angle | 加速度计计算的角度 |
| alpha | 融合系数，一般 0.95 ~ 0.99 |
2. 角速度 gyro
角速度一般直接来自陀螺仪。
注意单位要统一：
deg/s
rad/s
如果上位机建模时使用的是弧度，则单片机中也应该转成弧度：
gyro_rad = gyro_deg * 3.1415926f / 180.0f;
3. 位置 position
位置可以通过编码器累计得到：
position += encoder_delta;
如果需要换算成实际距离：
distance = pulse_count / pulses_per_round * wheel_perimeter;
4. 速度 speed
速度可以通过单位时间内编码器变化量计算：
speed = encoder_delta / dt;
也可以做低通滤波：
speed_filter = 0.8f * speed_filter + 0.2f * speed_raw;
十六、完整控制主循环示例
下面给一个更贴近实际工程的示例：
#include "lqr.h"
#include "imu.h"
#include "encoder.h"
#include "motor.h"
#define CONTROL_DT 0.005f
static LQR_Target_t target;
void Balance_Control_Init(void)
{
LQR_Init();
Encoder_Reset();
target.target_angle = 0.0f;
target.target_position = 0.0f;
target.target_speed = 0.0f;
}
void Balance_Control_Task(void)
{
LQR_State_t state;
float pwm;
state.angle = IMU_GetAngleRad();
state.gyro = IMU_GetGyroRad();
state.position = Encoder_GetPosition();
state.speed = Encoder_GetSpeed();
if (fabsf(state.angle) > 0.5f)
{
Motor_SetPWM(0);
Encoder_Reset();
return;
}
pwm = LQR_Calculate(state, target);
Motor_SetPWM(pwm);
}
这里的保护条件：
if (fabsf(state.angle) > 0.5f)
表示当车身倾角大于 0.5 弧度时关闭电机。
0.5 弧度大约等于：
28.6°
十七、LQR 输出为什么要限幅？
LQR 算出来的 u 是理论控制量。
但是真实电机有最大 PWM。
例如：
PWM 范围：-1000 ~ 1000
如果 LQR 输出：
u = 3000
真实电机也只能输出 1000。
所以必须做限幅：
if (u > 1000)
{
u = 1000;
}
else if (u < -1000)
{
u = -1000;
}
限幅的意义：
1. 防止电机输出过大
2. 防止系统失控
3. 保护驱动板
4. 保护电池和机械结构
十八、LQR 输出打满怎么办？
如果你发现 PWM 经常变成：
1000 或 -1000
说明控制量经常打满。
常见原因：
| 原因 | 解决方法 |
|---|---|
| K 太大 | 增大 R 或减小 Q |
| R 太小 | 增大 R |
| 角度单位不对 | 检查 degree 和 rad |
| 传感器数据异常 | 检查 IMU 和编码器 |
| 机械重心不合理 | 调整结构 |
| 电机能力不足 | 换更大扭矩电机 |
| 控制周期不对 | 确保 Ts 一致 |
十九、Q 和 R 的实战调参方法
1. 先只控制角度和角速度
初期不要一上来就加入位置和速度。
可以先设置：
Q = diag([300, 10, 0, 0])
R = 1
此时重点让车先能站起来。
2. 再慢慢加入位置控制
当姿态稳定后，再加入位置：
Q = diag([300, 10, 10, 2])
R = 1
如果位置控制太强，小车可能会来回冲。
可以降低 position 对应的 Q。
3. 电机太猛就增大 R
例如：
R = 1
太猛，可以改成：
R = 5
甚至：
R = 10
R 越大，输出越保守。
4. 响应太慢就减小 R
如果车反应很慢，可以尝试：
R = 0.5
或者：
R = 0.1
但是 R 太小容易抖动，甚至输出打满。
5. 一个实用调参顺序
推荐顺序：
第一步：只调角度 Q
第二步：加入角速度 Q
第三步：调整 R 控制输出强度
第四步：加入速度状态
第五步：加入位置状态
第六步：实物微调 K 或重新计算 K
二十、LQR 和 PID 外环结合
在工程中，LQR 经常和 PID 一起使用。
例如平衡车：
外环：速度 PID
内环：姿态 LQR
结构如下：
目标速度
↓
速度 PID
↓
目标角度 target_angle
↓
LQR 姿态控制
↓
PWM 输出
↓
电机
这样做的好处是：
1. 速度控制更加直观
2. LQR 专注姿态稳定
3. 外环 PID 调参更容易
4. 工程实现更灵活
速度 PID + LQR 示例
float Speed_PID_Calculate(float target_speed, float current_speed)
{
static float integral = 0.0f;
float error;
float output;
error = target_speed - current_speed;
integral += error;
if (integral > 100.0f)
{
integral = 100.0f;
}
else if (integral < -100.0f)
{
integral = -100.0f;
}
output = 0.5f * error + 0.01f * integral;
return output;
}
在控制任务中：
void Balance_Control_Task(void)
{
LQR_State_t state;
LQR_Target_t target;
float target_angle;
float pwm;
state.angle = IMU_GetAngleRad();
state.gyro = IMU_GetGyroRad();
state.position = Encoder_GetPosition();
state.speed = Encoder_GetSpeed();
target_angle = Speed_PID_Calculate(0.0f, state.speed);
target.target_angle = target_angle;
target.target_position = 0.0f;
target.target_speed = 0.0f;
pwm = LQR_Calculate(state, target);
Motor_SetPWM(pwm);
}
注意：
外环 PID 输出的是 target_angle。
LQR 根据 target_angle 去控制姿态。
target_angle 一定要限幅，比如限制在 ±10° 以内。
二十一、目标角度限幅
如果速度 PID 输出的目标角度太大，平衡车可能会直接冲出去。
所以要限制目标角度。
target_angle = LQR_Limit(target_angle, -0.17f, 0.17f);
其中：
0.17 rad ≈ 9.7°
这表示目标倾角限制在大约 ±10°。
二十二、LQI：带积分的 LQR
普通 LQR 没有积分项，所以可能存在稳态误差。
例如：
小车虽然能站稳，但位置总是偏一点。
电机速度能跟随，但存在一点静差。
这时可以使用 LQI。
LQI 可以理解为：
LQR + 积分误差
例如把位置误差积分加入状态：
x = [角度, 角速度, 位置, 速度, 位置误差积分]
控制律变成：
u = -Kx
只是 K 变成 5 个参数：
K = [k1, k2, k3, k4, k5]
单片机代码：
static float position_integral = 0.0f;
position_integral += (state.position - target.target_position) * CONTROL_DT;
position_integral = LQR_Limit(position_integral, -100.0f, 100.0f);
u = -(K[0] * angle_error
+ K[1] * gyro
+ K[2] * position_error
+ K[3] * speed
+ K[4] * position_integral);
注意：
积分项一定要限幅，否则会出现积分饱和。
二十三、LQR 状态量不全怎么办？
LQR 默认需要知道所有状态。
但是实际中，有些状态不好直接测量。
例如：
位置可以测
速度可以通过编码器差分得到
角度可以测
角速度可以测
这些还比较容易。
但有些系统中，某些状态不能直接测量，这时可以使用：
状态观测器
Luenberger Observer
Kalman Filter
扩展卡尔曼滤波 EKF
对于单片机入门实战来说，建议先尽量选择能直接测量或简单计算的状态。
例如：
| 状态 | 获取方式 |
|---|---|
| 角度 | IMU 姿态解算 |
| 角速度 | 陀螺仪 |
| 位置 | 编码器累计 |
| 速度 | 编码器差分 |
二十四、速度差分为什么容易抖？
速度通常这样算：
speed = encoder_delta / dt;
但是编码器脉冲是离散的，所以速度会有抖动。
可以加入一阶低通滤波：
float LowPass_Filter(float input)
{
static float output = 0.0f;
float alpha = 0.2f;
output = alpha * input + (1.0f - alpha) * output;
return output;
}
使用：
speed_raw = Encoder_GetSpeedRaw();
speed = LowPass_Filter(speed_raw);
滤波太弱，速度抖动大。
滤波太强，速度会滞后。
一般可以从：
alpha = 0.1 ~ 0.3
开始尝试。
二十五、左右电机如何分配 PWM？
LQR 计算出来的是平衡控制输出。
对于两轮平衡车，左右轮还需要考虑转向控制。
可以设计：
balance_pwm = LQR 输出
turn_pwm = 转向控制输出
最终：
left_pwm = balance_pwm - turn_pwm
right_pwm = balance_pwm + turn_pwm
代码：
void Motor_SetBalancePWM(float balance_pwm, float turn_pwm)
{
float left_pwm;
float right_pwm;
left_pwm = balance_pwm - turn_pwm;
right_pwm = balance_pwm + turn_pwm;
left_pwm = LQR_Limit(left_pwm, -1000.0f, 1000.0f);
right_pwm = LQR_Limit(right_pwm, -1000.0f, 1000.0f);
Motor_SetLeftPWM(left_pwm);
Motor_SetRightPWM(right_pwm);
}
二十六、加入转向控制
转向可以使用简单 P 控制：
float Turn_Control(float target_yaw_rate, float current_yaw_rate)
{
float error;
float turn_pwm;
error = target_yaw_rate - current_yaw_rate;
turn_pwm = 20.0f * error;
turn_pwm = LQR_Limit(turn_pwm, -300.0f, 300.0f);
return turn_pwm;
}
完整调用：
balance_pwm = LQR_Calculate(state, target);
turn_pwm = Turn_Control(target_yaw_rate, current_yaw_rate);
Motor_SetBalancePWM(balance_pwm, turn_pwm);
二十七、单片机运行效率优化
LQR 实时计算量很小，但是工程中仍然要注意效率。
建议：
1. 不要在控制中断里 printf
2. 不要在控制中断里做复杂浮点打印
3. 尽量保持固定控制周期
4. K 参数使用 float 即可
5. 状态量数量不多时，不需要复杂矩阵库
6. 尽量避免动态内存 malloc
对于 Cortex-M4 / M7：
float 运算一般可以接受
对于低端 8 位单片机：
可以考虑定点数实现
二十八、定点数 LQR 思路
如果单片机没有 FPU，浮点运算较慢，可以使用定点数。
例如放大 1000 倍：
真实值 1.234 → 存储为 1234
计算时：
int32_t u = 0;
u += K1_fixed * x1_fixed / SCALE;
u += K2_fixed * x2_fixed / SCALE;
u += K3_fixed * x3_fixed / SCALE;
u += K4_fixed * x4_fixed / SCALE;
u = -u;
示例：
#define SCALE 1000
int32_t K_fixed[4] = {
45231,
8742,
3125,
4883
};
int32_t LQR_Fixed(int32_t *x)
{
int64_t sum = 0;
for (int i = 0; i < 4; i++)
{
sum += (int64_t)K_fixed[i] * x[i];
}
sum = sum / SCALE;
return (int32_t)(-sum);
}
注意：
定点数要特别注意溢出问题，所以中间变量建议使用 int64_t。
二十九、调试时必须打印哪些数据？
LQR 调试时，不要只看车动不动。
建议通过串口或上位机实时观察：
angle
gyro
position
speed
pwm
target_angle
battery_voltage
推荐打印格式：
angle:0.02 gyro:-0.15 pos:120 speed:3.5 pwm:230
这样方便画波形。
如果使用上位机画图，可以观察：
1. 角度是否快速回到 0
2. 速度是否震荡
3. PWM 是否经常打满
4. 状态量是否存在明显噪声
5. 控制输出和角度变化方向是否相反
三十、控制方向判断
这是新手最容易出错的地方。
LQR 输出方向如果反了，系统会直接发散。
判断方法：
手动让车向前倾
观察控制输出是否让轮子向前追
如果车身向前倒，轮子应该向前转，去追车身重心。
如果方向反了，可以尝试：
1. 反转电机 PWM 方向
2. 反转编码器方向
3. 反转角度符号
4. 检查 K 前面的负号
常见控制公式：
u = -(K[0] * x[0] + K[1] * x[1] + K[2] * x[2] + K[3] * x[3]);
如果方向不对，不要盲目乱改 K。
先检查：
角度正方向
电机正方向
编码器正方向
陀螺仪正方向
三十一、LQR 实物调试安全流程
实物调试时一定要注意安全。
推荐流程：
1. 先不装轮子，测试电机方向
2. 悬空测试编码器方向
3. 手动倾斜车体，看 PWM 输出方向
4. 限制最大 PWM，例如先限制到 ±300
5. 手扶测试是否有回正趋势
6. 再逐步提高 PWM 限幅
7. 最后放地面测试
不要一上来就：
满电
满 PWM
直接放地上跑
这样很容易摔车、烧驱动或者伤人。
三十二、常见问题排查表
| 现象 | 可能原因 | 解决方法 |
|---|---|---|
| 一上电电机疯狂转 | 方向反了 / K 过大 | 检查符号，降低 PWM 限幅 |
| 车向一边倒 | 角度零点不准 | 做 IMU 零偏校准 |
| 能站但一直抖 | R 太小 / 噪声太大 | 增大 R，增加滤波 |
| 能站但反应慢 | R 太大 / Q 太小 | 减小 R，增大角度 Q |
| PWM 经常满 | K 太大 / 模型不准 | 增大 R，检查单位 |
| 速度波形很乱 | 编码器差分噪声 | 加低通滤波 |
| 位置漂移严重 | 编码器方向或积分问题 | 检查编码器和位置零点 |
| 控制效果忽好忽坏 | 控制周期不稳定 | 使用定时器固定周期 |
| 手扶有力，放地就倒 | 电机扭矩不足 / 重心不对 | 检查机械结构和电池电压 |
三十三、一个完整的工程级控制框架
最终可以形成这样的控制框架：
void Robot_Control_Task(void)
{
LQR_State_t state;
LQR_Target_t target;
float balance_pwm;
float turn_pwm;
state.angle = IMU_GetAngleRad();
state.gyro = IMU_GetGyroRad();
state.position = Encoder_GetPosition();
state.speed = Encoder_GetSpeed();
if (Robot_IsUnsafe(state))
{
Motor_SetPWM(0);
return;
}
target.target_angle = Speed_OuterLoop();
target.target_position = 0.0f;
target.target_speed = 0.0f;
target.target_angle = LQR_Limit(target.target_angle, -0.17f, 0.17f);
balance_pwm = LQR_Calculate(state, target);
turn_pwm = Turn_Control();
Motor_SetBalancePWM(balance_pwm, turn_pwm);
}
安全判断函数：
int Robot_IsUnsafe(LQR_State_t state)
{
if (fabsf(state.angle) > 0.5f)
{
return 1;
}
if (Battery_GetVoltage() < 10.5f)
{
return 1;
}
return 0;
}
三十四、LQR 参数在线调整
为了方便调试，可以通过串口在线修改 K 参数。
例如接收命令：
K0=45.2
K1=8.7
K2=3.1
K3=4.8
然后更新数组：
void LQR_SetK(int index, float value)
{
if (index >= 0 && index < 4)
{
K[index] = value;
}
}
这样不用每次都重新下载程序。
调试完成后，再把最终参数固化到代码或 Flash 中。
三十五、LQR 参数保存到 Flash 的思路
实际项目中，可以把 K 参数保存到 Flash 或 EEPROM。
流程：
上位机串口发送 K 参数
↓
单片机接收并更新 K
↓
保存到 Flash
↓
下次开机自动读取
这样方便现场调参。
伪代码：
void LQR_SaveParam(void)
{
Flash_WriteFloatArray(K, 4);
}
void LQR_LoadParam(void)
{
Flash_ReadFloatArray(K, 4);
}
不同单片机 Flash 操作方式不同，需要根据具体芯片实现。
三十六、LQR 和传统三环 PID 的关系
在电机控制或机器人控制中，经常会听到：
电流环
速度环
位置环
这属于典型 PID 多环结构。
而 LQR 更像是：
把多个状态统一放进一个状态向量里综合控制
简单理解：
| 控制方式 | 思想 |
|---|---|
| 三环 PID | 一层一层控制 |
| LQR | 多个状态一起优化 |
| PID | 调 Kp Ki Kd |
| LQR | 调 Q 和 R |
并不是说 LQR 一定替代 PID。
实际工程中常见做法是：
底层电流环仍然用 PI
上层姿态或位置控制用 LQR
这样更符合工程实际。
三十七、什么时候不建议直接上 LQR？
虽然 LQR 很强，但不是所有场景都适合。
以下情况不建议一开始就用 LQR：
1. 系统模型完全不了解
2. 传感器数据很差
3. 电机驱动方向还没调通
4. 编码器方向还不确定
5. 控制周期不稳定
6. 机械结构本身问题很大
7. 只是简单速度控制，PID 已经够用
建议先保证：
电机能正常转
编码器方向正确
IMU 数据稳定
PWM 输出正常
控制周期固定
再开始上 LQR。
三十八、进阶优化方向
掌握普通 LQR 后，可以继续学习：
1. LQI：带积分的 LQR
2. LQG：LQR + 卡尔曼滤波
3. 增益调度 LQR：不同工况使用不同 K
4. 非线性系统分段线性化
5. MPC：模型预测控制
6. 状态观测器设计
7. 系统辨识与参数估计
推荐路线：
普通 LQR
↓
离散 LQR
↓
LQI
↓
Kalman Filter
↓
LQG
↓
MPC
三十九、完整实战总结
单片机中使用 LQR，真正重要的是工程流程，而不是只会公式。
核心流程可以总结为：
1. 确定控制对象
2. 选择状态变量
3. 建立状态空间模型
4. 选择控制周期 Ts
5. 离散化 A、B 矩阵
6. 设置 Q、R
7. 计算 K
8. 单片机周期运行 u = -Kx
9. 加入限幅、滤波、保护
10. 实物调试和参数优化
LQR 真正落地时，单片机只需要做：
u = -(K[0] * x[0]
+ K[1] * x[1]
+ K[2] * x[2]
+ K[3] * x[3]);
所以不要被 LQR 的数学推导吓到。
真正难的是：
状态量是否准确
单位是否统一
控制周期是否稳定
方向是否正确
Q 和 R 是否合适
模型和实物是否接近
四十、最终核心代码模板
最后给一个可以直接移植的核心模板：
typedef struct
{
float angle;
float gyro;
float position;
float speed;
} LQR_State_t;
typedef struct
{
float target_angle;
float target_position;
float target_speed;
} LQR_Target_t;
float K[4] = {
45.2312f,
8.7421f,
3.1254f,
4.8832f
};
float Limit(float value, float min, float max)
{
if (value > max)
{
return max;
}
if (value < min)
{
return min;
}
return value;
}
float LQR_Control(LQR_State_t state, LQR_Target_t target)
{
float x0;
float x1;
float x2;
float x3;
float u;
x0 = state.angle - target.target_angle;
x1 = state.gyro;
x2 = state.position - target.target_position;
x3 = state.speed - target.target_speed;
u = -(K[0] * x0
+ K[1] * x1
+ K[2] * x2
+ K[3] * x3);
u = Limit(u, -1000.0f, 1000.0f);
return u;
}
调用：
void Control_Task(void)
{
LQR_State_t state;
LQR_Target_t target;
float pwm;
state.angle = IMU_GetAngleRad();
state.gyro = IMU_GetGyroRad();
state.position = Encoder_GetPosition();
state.speed = Encoder_GetSpeed();
target.target_angle = 0.0f;
target.target_position = 0.0f;
target.target_speed = 0.0f;
pwm = LQR_Control(state, target);
Motor_SetPWM(pwm);
}
结语
LQR 从理论上看是现代控制算法，但从单片机实现角度看，它的运行形式非常简单：
采集状态 x
乘以增益 K
得到控制量 u
输出到电机
可以记住一句话：
MATLAB / Python 负责复杂计算，单片机负责稳定实时执行。
当你掌握了：
状态变量
离散化
Q 和 R 调参
控制周期
限幅保护
传感器滤波
实物排错
就已经具备了把 LQR 真正用到嵌入式控制项目中的能力。
对于平衡车、倒立摆、云台、电机控制、机器人底盘来说，LQR 都是一个非常值得深入学习的控制算法。
AtomGit 是由开放原子开源基金会联合 CSDN 等生态伙伴共同推出的新一代开源与人工智能协作平台。平台坚持“开放、中立、公益”的理念，把代码托管、模型共享、数据集托管、智能体开发体验和算力服务整合在一起，为开发者提供从开发、训练到部署的一站式体验。
更多推荐
14
0- 0
已为社区贡献4条内容
所有评论(0)