倒立摆最优设计matlab

以下是倒立摆最优设计的Matlab代码，包括最优控制器设计和动画仿真：

% 倒立摆最优控制器设计和动画仿真
% 系统参数
m = 1; % 摆杆质量
M = 5; % 小车质量
L = 2; % 摆杆长度
g = 9.8; % 重力加速度
d = 1; % 阻尼系数

% 状态空间表达式
A = [0 1 0 0; 0 -d/M -m*g/M 0; 0 0 0 1; 0 -d/(M*L) -(m+M)*g/(M*L) 0];
B = [0; 1/M; 0; 1/(M*L)];
C = [1 0 0 0; 0 0 1 0];
D = [0; 0];

% 系统状态反馈最优控制器设计
Q = diag([1 1 1 1]); % 状态权重矩阵
R = 1; % 控制权重矩阵
[K,~,~] = lqr(A,B,Q,R); % 状态反馈最优控制器

% 系统仿真
tspan = 0:0.01:10; % 时间范围
x0 = [0; 0; pi+0.1; 0]; % 初始状态
[t,x] = ode45(@(t,x)pendcart(x,m,M,L,g,d,-K*(x-[0;0;pi;0])),tspan,x0); % 系统仿真

% 动画演示
for k = 1:length(t)
    drawpendcart(x(k,:),m,M,L);
end

% 绘制倒立摆动画函数
function drawpendcart(x,m,M,L)
    y = 0; % 地面高度
    w = 1; % 小车宽度
    h = 0.5; % 小车高度
    wr = 0.2; % 轮子半径
    mr = 0.3; % 摆杆质心半径
    L1 = L/2; % 摆杆一半长度
    xcart = x(1); % 小车位置
    theta = x(3); % 摆杆角度
    xpend = xcart + L*sin(theta); % 摆杆质心位置
    figure(1); clf; hold on;
    plot([-10 10],[y y],'k','LineWidth',2);
    plot([xcart-w/2 xcart+w/2],[y y],'k','LineWidth',2);
    rectangle('Position',[xcart-wr/2 y-h/2 wr h],'Curvature',0.2,'FaceColor',[0.5 0.5 0.5],'EdgeColor',[0.5 0.5 0.5]);
    plot([xcart xpend],[y y+L],'k','LineWidth',2);
    rectangle('Position',[xpend-mr/2 y+L-mr/2 mr mr],'Curvature',1,'FaceColor',[0.5 0.5 0.5],'EdgeColor',[0.5 0.5 0.5]);
    axis([-5 5 -2 5]);
    axis equal;
    drawnow;
end