【无人机控制】基于线性二次型调节器 LQR实现无人机控制附matlab代码

% 定义系统的状态空间方程
A = [0 1 0; 0 0 1; 0 0 0]; % 状态转移矩阵
B = [0; 0; 1]; % 输入矩阵
C = eye(3); % 输出矩阵
D = zeros(3,1); % 直接传递矩阵

% 定义系统的权重矩阵
Q = diag([1, 1, 1]); % 状态权重矩阵
R = 1; % 输入权重矩阵

% 计算LQR控制器增益矩阵
K = lqr(A, B, Q, R);

% 定义初始状态和目标状态
x0 = [0; 0; 0]; % 初始状态
x_target = [1; 0; 0]; % 目标状态

% 模拟无人机控制
T = 5; % 模拟时间
dt = 0.01; % 时间步长
num_steps = T / dt; % 总步数

x = x0; % 当前状态
u = zeros(num_steps, 1); % 控制输入
x_history = zeros(3, num_steps); % 状态记录

for k = 1:num_steps
% 计算控制输入
u(k) = -K * (x - x_target);

% 更新状态
x_dot = A * x + B * u(k);
x = x + x_dot * dt;

% 记录状态
x_history(:, k) = x;

end

% 绘制状态随时间的变化
t = linspace(0, T, num_steps);
figure;
plot(t, x_history(1, 😃, ‘r-’, ‘LineWidth’, 2);
hold on;
plot(t, x_history(2, 😃, ‘g-’, ‘LineWidth’, 2);
plot(t, x_history(3, 😃, ‘b-’, ‘LineWidth’, 2);
xlabel(‘时间（秒）’);
ylabel(‘状态’);
legend(‘位置’, ‘速度’, ‘加速度’);
title(‘无人机状态随时间变化’);
grid on;