lqr代码

参数不确定+高阶滤波+严格反馈+全驱系统

clc
clear all
close all
tf=50;
dt=0.001;
% 1. 系统参数设置
J = 1.625e-3; % kg·m²
m = 0.506; % kg
M0 = 0.434; % kg
L0 = 0.305; % m
R0 = 0.023; % m
B0 = 16.25e-3; % N·m·s/rad
L = 25e-3; % H
R = 5; % Ω
K_tau = 0.9; % N·m/A
K_B=0.9;
g = 9.8; % m/s²

% Calculate M, B, and N
M = (J + (m * L0^2) / 3 + M0 * L0^2 + (2 * M0 * R0^2) / 5) / K_tau;
B = B0 / K_tau;
N = ((m * L0 * g) / 2 + M0 * L0 * g) / K_tau;

%B、N、K_B、R  unknown
g1=1/M;
g2=1/L;
theta11=B/M;
theta12=N/M;
theta21=K_B/L;
theta22=R/L;

theta1p1=0.2;
theta1p2=35;
theta2p1=35;
theta2p2=200;

zeta=0.5;

pi1=0.1;
pi2=0.2;
omega2=50;

% 定义矩阵 P1
P1 = [273558/5, 31118/91; 31118/91, 26091/58];
P1F= [273558/5; 31118/91];
P1L= [31118/91; 26091/58];

% 定义矩阵 A1_0_1
A1_0_1 = [120, 22];
A1=[0 1;120 22];
u=0;
% 定义参数 r1, r2, P2, A2
r1 = 1;
r2 = 1;
P2 = 1;
A2 = 20;

% 初始条件
x1= pi/6; % x1(0)
x1_dot = 0; % x1_dot(0)
x2= 0; % x2(0)
xi1 = 0; % xi1(0)
xi1_dot= 0; % xi1_dot(0)
xi2= 0; % xi2(0)
x2c = 10; % x2c(0)
hat_theta11 = 0.2; % theta_hat_11(0)
hat_theta12 = 30; % theta_hat_12(0)
hat_theta21 = 30; % theta_hat_21(0)
hat_theta22 = 100; % theta_hat_22(0)
theta1_p1 = 0.2; % theta1_p,1
theta1_p2 = 35; % theta1_p,2
theta2_p1 = 35; % theta2_p,1
theta2_p2 = 200; % theta2_p,2
z22=0;
z21=10;
alpha1=0;

% 初始化数据记录数组
time = 0:dt:tf;
x1_data = zeros(size(time));
x1_dot_data = zeros(size(time));
x2_data = zeros(size(time));
xi1_data = zeros(size(time));
xi1_dot_data = zeros(size(time));
xi2_data = zeros(size(time));
x2c_data = zeros(size(time));
hat_theta11_data = zeros(size(time));
hat_theta12_data = zeros(size(time));
hat_theta21_data = zeros(size(time));
hat_theta22_data = zeros(size(time));
z21_data = zeros(size(time));
z22_data = zeros(size(time));
alpha1_data = zeros(size(time));
u_data = zeros(size(time));

% 初始化索引
index = 1;

% 5. 仿真过程
for t= 0:dt:tf
    % 更新状态
    f11=-x1_dot;
    f12=-sin(x1);
    f21=-x1_dot;
    f22=-x2;
    x1_ddot = f11'*theta11+f12'*theta12+g1*x2;
    x1_dot=x1_dot+x1_ddot*dt;
    x1=x1+x1_dot*dt;
    x2_dot=f21*theta21+f22*theta22+g2*u;
    x2=x2+x2_dot*dt;
    %参考轨迹
    x1c=pi/2*(1-exp(-0.1*t^2))*sin(t);
    dx1c = 0.1 * pi * t .* exp(-0.1 * t.^2) .* sin(t) + (pi/2) * (1 - exp(-0.1 * t.^2)) .* cos(t);
    ddx1c= 0.1 * pi * exp(-0.1 * t.^2) * (1 - 0.2 * t.^2) .* sin(t) + 0.2 * pi * t .* exp(-0.1 * t.^2) .* cos(t) - (pi/2) * (1 - exp(-0.1 * t.^2)) .* sin(t);

    dz22=-2*zeta*omega2*z22-omega2^2*(z21-alpha1);
    z22=z22+dz22*dt;
    dz21=z22;
    z21=z21+dz21*dt;

    x2c=z21;

    tilde_x1=x1-x1c;
    tilde_x2=x2-x2c;

    xi1_ddot=-A1_0_1*[xi1;xi1_dot;]+g1*xi2+g1*(x2c-alpha1);
    xi1_dot=xi1_dot+xi1_ddot*dt;

    xi2_dot=-A2*xi2;

    xi1=xi1+xi1_dot*dt;
    xi2=xi2+xi2_dot*dt;

    v1=tilde_x1-xi1;
    v2=tilde_x2-xi2;
    tilde_x1_dot=x1_dot-dx1c;
    dv1=tilde_x1_dot-xi1_dot;

    dhat_theta11=r1*f11*P1L'*[v1;dv1]+pi1*(theta1_p1-hat_theta11);
    dhat_theta12=r1*f12*P1L'*[v1;dv1]+pi1*(theta1_p2-hat_theta12);
    dhat_theta21=r2*f21*P2'*v2+pi2*(theta2_p1-hat_theta21);
    dhat_theta22=r2*f22*P2*v2+pi2*(theta2_p2-hat_theta22);

    hat_theta11=hat_theta11+dhat_theta11*dt;
    hat_theta12=hat_theta12+dhat_theta12*dt;
    hat_theta21=hat_theta21+dhat_theta21*dt;
    hat_theta22=hat_theta22+dhat_theta22*dt;


    %alpha1=mpc_solver(A1, B1, Q1, R1, X1, Xd1, N1, dt);
    A1=[0 1;0 0;];
     B1=[0;1];
    Q1=10*eye(2);
    R1=10;
    K1 = lqr(A1,B1,Q1,R1);
    alpha2=-K1*[tilde_x1;tilde_x1_dot];
    alpha1=-1/g1*(A1_0_1*[tilde_x1;tilde_x1_dot]+ f11'*hat_theta11+f12'*hat_theta12-ddx1c+B1(2)*alpha2);


    B2=1;
    Q2=10;
    R2=10;

    K2 = lqr(-A2,B2,Q2,R2);
    u2=-K2*(tilde_x2);
    u=-1/g2*(f11'*hat_theta11+f12'*hat_theta12-dz21+A2*tilde_x2+B2*u2);

    % 记录数据
    x1c_data(index) = x1c;
    x1_data(index) = x1;
    x1_dot_data(index) = x1_dot;
    x2_data(index) = x2;
    xi1_data(index) = xi1;
    xi1_dot_data(index) = xi1_dot;
    xi2_data(index) = xi2;
    x2c_data(index) = x2c;
    hat_theta11_data(index) = hat_theta11;
    hat_theta12_data(index) = hat_theta12;
    hat_theta21_data(index) = hat_theta21;
    hat_theta22_data(index) = hat_theta22;
    theta11_data(index) = theta11;
    theta12_data(index) = theta12;
    theta21_data(index) = theta21;
    theta22_data(index) = theta22;
    z21_data(index) = z21;
    z22_data(index) = z22;
    index=index+1;
end
%%
figure(1)
plot(x1_data,'DisplayName','x1_data');hold on;plot(x1c_data,'DisplayName','x1c_data');hold off;