二级倒立摆MATLAB仿真程序

clear; clc; format short; k12 = 86.69; k13 = -21.62; k17 = 6.64; k22 = -40.31; k23 = 39.45; k27 = -0.088; A = [0 0 0 1 0 0; 0 0 0 0 1 0; 0 0 0 0 0 1; 0 0 0 0 0 0; 0 k12 k13 0 0 0; 0 k22 k23 0 0 0]; B = [0 0 0 1 k17 k27]'; C = [1 0 0 0 0 0; 0 1 0 0 0 0; 0 0 1 0 0 0]; D = [0; 0 ;0]; Q11 = 500; %代表小车位置的权重 Q22 = 500; %代表摆杆 1 角度的权重 Q33 = 500; %代表摆杆 2 角度的权重 Q44 = 0; %小车速度的权重 Q55 = 0; %摆杆 1 角速度的权重 Q66 = 0; %摆杆 2 角速度的权重 Q = [Q11 0 0 0 0 0 ; 0 Q22 0 0 0 0 ; 0 0 Q33 0 0 0 ; 0 0 0 Q44 0 0 ; 0 0 0 0 Q55 0 ; 0 0 0 0 0 Q66]; R = 1; %控制能量的限制 K = lqr(A,B,Q,R) % Wc = -C/(A-B*K)*B % L = 1/Wc(1) Ang1_0 = -3/180*pi; Ang2_0 = 5/180*pi; sim('lip2_lqr.slx',6) ; figure(1) set(gcf,'Units','centimeters','Position',[10 10 5*1.45 4*1.45]);%设置图片大小为 5*1.45cm× 4*1.45cm plot(T, Ang1*180/pi,'b'); hold on plot(T,Ang2*180/pi,'r'); axis([0 4 -14 16]) % hold off legend('Ang1','Ang2') xlabel('t(s)'); ylabel('\theta (°)' ); title('角度变化曲线') grid on; hold on; figure(2) set(gcf,'Units','centimeters','Position',[20 10 5*1.45 4*1.45]);%设置图片大小为 5*1.45cm× 4*1.45cm plot(T, Pos, 'b'); xlabel('t(s)'); ylabel('x (m)'); title('位置变化曲线') grid on; hold on; % Ac = [(A-B*K)]; % % Bc = B; % Cc = [C]; % Dc = [D]; % Cn = [1 0 0 0 0 0]; % % s = size(A,1); % Z = [zeros(1,s) 1]; % % N = inv([A,B;Cn,0])*Z'; % Nx = N(1:s); % Nu = N(1+s); % Nbar = Nu + K*Nx; % % Bcn = [Nbar*B]; % x0 = [0 0 0.05 0 0 0]; % T = 0:0.01:10; % U = zeros(size(T)); % [Y,X] = lsim(Ac,Bcn,Cc,Dc,U,T,x0); % xpos = Y(:,1); % xangle = Y(:,2); % xangle2 = Y(:,3); % % plot(T,xpos,':'); % hold on % plot(T,xangle,'--') % plot(T,xangle2,'-')