深入浅出：LQR控制算法原理与实现详解
深入浅出：LQR控制算法原理与实现详解
LQR就像一个聪明的司机，时刻调整方向盘（控制输入），让车子（系统）沿着直线（目标状态）行驶。聪明的司机不仅要让车子尽快回到直线，还要尽量少动方向盘（节省能量）。
为什么叫做LQR? LQR中的三个字母L、Q、R分别代表以下含义：
- L：Linear，即线性。这表明LQR算法适用于线性系统。LQR控制算法应用于线性系统，其状态空间模型为：
线性模型
其中，x是状态向量，描述系统的状态。 u是控制输入向量。 A和 B分别是系统矩阵和输入矩阵，描述系统的动态特性。
- Q：Quadratic，即二次型。LQR算法的性能指标是一个二次型函数，通常表示为：
二次型性能函数
其中，x是系统状态向量，u是控制输入向量，Q和 R是权重矩阵。这个性能指标包含了状态误差和控制输入的加权和，通过最小化该指标可以优化系统性能。
- R：Regulator，即调节器。LQR是一种调节器，它通过反馈控制来调节系统状态，使其达到期望的平衡点。LQR控制器计算出的控制输入是系统状态的线性函数，用以调节系统行为。
我们用一个具体的例子来理解算法思想——弹簧阻尼系统。
什么是弹簧阻尼系统？
弹簧阻尼系统是一个经典的二阶系统，由质量块m、弹簧和阻尼器组成。我们的目标是通过控制系统输入力u，使得质量块的位置和速度达到理想状态。
弹簧阻尼系统示意图
系统建模
1. 系统参数
- m ：质量（kg）
- k ：弹簧系数（N/m）
- c ：阻尼系数（Ns/m）
- u : 输入力F (N)
2. 状态向量
定义系统的状态变量：
- s ：质量块的位置（m）
- v ：质量块的速度（m/s）
因此，系统的状态向量可以表示为：
状态向量
3. 动力学方程
根据牛顿第二定律，系统的动力学方程为：
系统的动力学方程
将其转换为状态空间形式：
状态空间形式
4. 状态空间模型
用矩阵形式表示状态空间模型：
状态空间模型
5.具体数值代入后的状态空间模型
假设系统参数为：
- m =1.0kg
- k =20.0 N/m
- c =1.0 Ns/m
将这些参数代入状态空间模型：
参数代入状态空间模型矩阵
所以完整的状态空间模型为：
完整的状态空间模型
LQR控制器设计
性能指标
LQR算法会计算一个性能指标，这个指标综合考虑了系统状态的误差和控制输入的能量。我们用两个矩阵Q和R来定义这个性能指标的权重：
性能指标公式如下：
二次型性能函数
- Q 是状态权重矩阵，用于权衡系统状态偏离目标值的这部分代价，用于衡量系统状态的误差（比如质量块m偏离所给定的目标位置的程度）。
- R 是控制权重矩阵，用于权衡控制输入的代价，衡量控制输入的能量（比如施加在质量块M上的力的大小）。
这个公式表示，在整个时间范围内，系统状态的误差和控制输入的能量的加权和。
控制律
LQR控制算法会计算一个反馈增益矩阵K ，然后通过以下控制律来调整控制输入：
控制律
其中，u是控制输入，x是系统状态。所以LQR其实也是一个全状态反馈控制，闭环控制，如下图：
框图
反馈增益矩阵K通过求解Riccati方程得到：
Riccati方程
其中，P是对称正定矩阵，求解后可得到反馈增益矩阵K：
反馈增益矩阵
求解过程
总结一下，LQR控制器的设计步骤如下：
求解过程
MATLAB代码实现LQR控制器
我们来用MATLAB实现上述步骤，设计弹簧阻尼系统的LQR控制器。
clear all;close all;clc;
% 加载control包，如使用Matlab，可删除本行
pkg load control;
% 系统参数
m = 1.0; % 质量（kg）
k = 20.0; % 弹簧系数（N/m）
c = 1.0; % 阻尼系数（Ns/m）
% 连续时间状态空间模型的矩阵
A = [0, 1; -k/m, -c/m];
B = [0; 1/m];
C = eye(2);
D = zeros(2, 1);
% 状态反馈权重矩阵 Q 和控制输入权重矩阵 R
Q = [1, 0; 0, 1];
R = 10;
% 采样时间
T_s = 0.01;
% 定义连续时间状态空间系统
sys_c = ss(A, B, C, D);
% 使用c2d函数进行离散化
sys_d = c2d(sys_c, T_s, 'zoh');
% 提取离散化后的状态矩阵和输入矩阵
A_d = sys_d.A;
B_d = sys_d.B;
% 计算离散时间LQR反馈增益矩阵 K_d
[K_d, S, e] = dlqr(A_d, B_d, Q, R);
% 初始状态
x0 = [0.1; 0]; % 初始位置偏离0.1米，初始速度为0
x = x0;
% 仿真时间
t = 0:T_s:10;
% 初始化状态存储矩阵
x_history = zeros(length(t), length(x0));
x_history(1, :) = x0';
% 初始化控制输入存储矩阵
u_history = zeros(length(t), 1);
% 仿真LQR控制系统
for k = 2:length(t)
% 计算控制输入
u = -K_d * x;
% 状态更新
x = A_d * x + B_d * u;
% 存储状态和控制输入
x_history(k, :) = x';
u_history(k) = u;
end
% 绘制仿真结果
figure;
subplot(3,1,1);
plot(t, x_history(:,1));
xlabel('time (s)');
ylabel('position s (m)');
title('position with time');
grid on;
subplot(3,1,2);
plot(t, x_history(:,2));
xlabel('time (s)');
ylabel('speed v (m/s)');
title('speed with time');
grid on;
subplot(3,1,3);
plot(t, u_history);
xlabel('time (s)');
ylabel('input u(N)');
title('input with time');
grid on;
代码解释
系统参数：
- m 是质量。
- k 是弹簧系数。
- c 是阻尼系数。
连续时间状态空间模型的矩阵：
- A 是状态矩阵。
- B 是输入矩阵。
- C 和 D 是输出矩阵和直接传输矩阵（这里分别为单位矩阵和零矩阵）。
状态反馈权重矩阵 Q 和控制输入权重矩阵 R
Q = [10, 0;0, 1];
R = 1;
这里Q矩阵是状态误差就这权重，位置误差的权重设为10，速度误差权重设置1。R矩阵是输入能量权重设置为1。我们可以看到，位置误差权重设为10，大于其他权重值，这就是表示让LQR算法更关注位置误差，也就是让算法尽量保证位置误差最小化。
离散化参数：
- T_s 是采样时间。
定义连续时间状态空间系统：
- sys_c 使用 ss 函数定义连续时间状态空间系统。
使用 c2d 函数进行离散化：
- sys_d 使用 c2d 函数将连续时间系统转换为离散时间系统。
提取离散化后的状态矩阵 A_d 和输入矩阵 B_d。
关于连续时间到离散时间的转换：
首先，连续时间系统的状态空间模型如下：
连续模型
为了将其转换为离散时间系统，我们使用零阶保持法（ZOH），离散时间系统模型如下：
离散模型
在MATLAB代码中我们使用c2d函数自动完成。然后我们可以在离散时间系统上设计LQR控制器。
LQR控制器设计：
使用 dlqr 函数计算离散时间的反馈增益矩阵 K_d。
如上面讲到的，离散时间的LQR问题可以描述为寻找控制输入 []以最小化以下代价函数：
离散型代价函数
使用离散时间的状态空间模型，LQR控制器的反馈增益矩阵 可以通过求解离散时间代数黎卡提方程（DARE）得到。MATLAB代码中的dlqr函数可以直接完成这一过程。
初始状态：
- x0 是初始状态向量，初始位置为0.1m处，初始速度是0。
仿真LQR控制系统：
使用 for 循环遍历每个时间步，计算控制输入 u，并更新状态 x。
存储每个时间步的状态到 x_history 和控制输入到 u_history。
绘制仿真结果：
使用 subplot 绘制位置、速度和控制输入随时间变化的曲线。
运行结果分析
仿真结果图
通过仿真结果，可以观察到在LQR控制下，系统从初始状态0.1m位置逐渐趋于稳定。位置和速度逐渐趋于零，系统的控制输入也在动态调整以达到稳定状态。
修改权重矩阵QR对比
我们修改Q和R权重参数，将仿真初始参数（=[10,0;0,1], =1）修改为=[1,0;0,1], =10。将两组仿真结果绘制在同一张图上。
权重修改
图中红色为R=10，Q=1的情况曲线，蓝色为R=1,Q=10的情况曲线。
通过仿真结果，我们可以对比两组不同参数下的系统行为：
位置 (s) 随时间变化：
权重矩阵 Q=[10,0;0,1]和 R=1的情况下，系统趋于稳定的速度较快。
权重矩阵 Q=[1,0;0,1]和 R=10 的情况下，系统趋于稳定的速度较慢。
速度 (v) 随时间变化：
权重矩阵Q =[10,0;0,1] 和R =1的情况下，系统的速度变化更快。
权重矩阵 Q=[1,0;0,1]和 R=10的情况下，系统的速度变化较慢。
控制输入 (u) 随时间变化：
权重矩阵Q =[10,0;0,1]和 R=1的情况下，控制输入较大，说明控制器更积极地调整系统。
权重矩阵Q =[1,0;0,1] 和 R=10的情况下，控制输入较小，说明控制器调整系统较为保守。
总结一下就是，R由于是输入能量权重矩阵，当R较大时，系统会为了节能，尽量进行较小的输入，同时Q较小，这样就会导致系统状态误差收敛较慢。