LQR控制算法

原理

离散型线性系统的状态空间方程为：

上述公式中：

接下来就需要求解上述代价函数的最优解（参考动态规划的思想）

上述代表的是N-1步到最终第N步的代价。

求偏导数的过程如下：

求导最终的化简结果为：

求导的目的是找到极小值，因此令其等于0，则可以求解最优控制策略：

定义反馈矩阵：

可得：

最终可以归纳出最优控制策略的一般形式为：

其中

同时可以归纳出最优剩余代价的一般形式为：

代码示例

matlab 复制代码

%% 清屏
clear;
close all;
clc;
%% 系统定义
%定义系统矩阵A
A = [0 1;-1 -0.5];
%计算系统矩阵A的维度
n = size(A,1);
%定义输入矩阵B
B = [0 ; 1];
C = [0 0;0 0];
D = [0;0];
% 计算输入矩阵B的维度
p = size(B,2);
%% 系统离散
Ts = 0.1;
%% 连续系统转换成离散系统
sys_d = c2d(ss(A,B,C,D),Ts);
%% 提取离散系统A矩阵
A = sys_d.a;
%% 提取离散系统B矩阵
B = sys_d.b;
% 系统初始化
% 状态初始化
x0 = [1 ; 0];
x = x0;
% 输入初始化
u0 = 2;
u = u0;

% 定义系统运行步数
k_steps = 100;
% 定义x_history零矩阵，用于存储系统状态结果，维度n x k_step
x_history = zeros(n,k_steps);
% 将系统状态初始值赋值到x_history矩阵第一个位置
x_history(:,1) = x;
% 定义u_history零矩阵，用于存储系统输入结果 ，维度 p x k_step
u_history = zeros(p,k_steps);
% 将系统输入初始值赋值到u_history矩阵第一个位置
u_history(:,1) = u;
%%%%%%%%%%%%%%%%%%%%%权重设计%%%%%%%%%%%%%%%%%%%%%
% 设计系统状态权重矩阵Q，维度n x n
Q = [1 0; 0 1];
% 设计系统终值权重矩阵S, 维度n x n
S = [1 0; 0 1];
% 设计系统输入权重矩阵R，维度p x p
R = 0.1;

%定义末端时刻
N = k_steps;
P_k = S;
% 计算反馈增益
for k = 1:N
    % 计算F[N-k]
    F = inv(R+B'*P_k*B)*B'*P_k*A;
    % 计算P[k]
    P_k = (A-B*F)'*P_k*(A-B*F)+(F)'*R*(F)+Q;
    if k == 1
        F_N = F;
    else
        F_N = [F;F_N];
    end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 仿真开始，建立for循环
for k = 1 : k_steps
% 计算系统输入
u = -F_N((k-1)*p+1: k * p, :)*x;
% 系统输入带入系统方程，计算系统响应
x = A * x + B * u;
% 保存系统状态到预先定义矩阵的相应位置
x_history(:,k+1) = x;
end

%%%%%%%%%%%%%%%%%%%结果%%%%%%%%%%%%%%%%%%
%% 结果视图：系统状态vs.运行步数
subplot(2, 1, 1);
for i = 1:n
    plot(x_history(i,:));
    hold;
end

legend(num2str((1:n)', 'x %d'));
xlim([1,k_steps]);
grid on

%% 结果视图：系统输入vs.运行步数
subplot (2, 1, 2);
for i = 1:p
    stairs(u_history(i,:));
    hold;
end
legend(num2str((1:p)', 'u %d'));
xlim([1, k_steps]);
grid on;