matlab控制倒立摆小车并绘制二维动态效果图

Posted 2022-04-17 studyer_domi

clc;close all;clear
A = [0 1 0 0;0 0 -1.176 0;0 0 0 1;0 0 18.293 0];%设置倒立摆小车控制系统参数
B = [0; 1 ;0;-1.667];
C =[1 0 0 0;0 0 1 0];
G = [42.85 1.04;471.83 22.39;0.94 43.15;19.17 464.64];
K = [-9.1841 -10.7148 -63.8735 -15.4258];
sim('CAR.mdl');      %运行倒立摆小车控制系统simulink模型'CAR.mdl'

H_CAR = 0.4;         %小车车体高度，单位：m
H_WHEEL = 0.08;      %小车轮子直径，单位：m
L = 1.2;             %倒立摆杆长度，单位：m
N = length(yout.signals.values);%仿真得到的采样数据点个数
LINEWIDTH = 6.0;     %画倒立摆杆的线粗
CARWIDTH = 3.0;      %画车体的线粗
ARROWWIDTH = 2.5;    %画车体行车方向箭头的线粗
%% 
for i=1:N        %FOR循环画出倒立摆小车控制过程的动画   

    y_line = y+L*sin(theta);
    z_line = L*cos(theta)+H_CAR;

%   plot([(y-0.3),(y-0.3),(y+0.3),(y+0.3),(y-0.3)],[H_WHEEL,H_CAR,H_CAR,H_WHEEL,H_WHEEL],'b','LineWidth',CARWIDTH);%画车体
    plot([(y+0.04),(y+0.3),(y+0.3),(y-0.3),(y-0.3),(y-0.04)],[H_CAR,H_CAR,H_WHEEL,H_WHEEL,H_CAR,H_CAR],'b','LineWidth',CARWIDTH);%画车体
    hold on
    plot([y,y],[H_CAR,H_CAR+L+0.2],'--k','LineWidth',0.5);%画垂直虚线
    hold on
    
    %画车体中间的半圆
    r = 0.04; 
    hseta = 0:0.001:pi; 
    yh = y+r*cos(hseta); 
    zh = H_CAR+r*sin(hseta); 
    plot(yh,zh,'b','LineWidth',CARWIDTH); 
    hold on;
    
    %画两个车轮
    yl_circle = y-0.2;
    yr_circle = y+0.2;
    z_circle = H_WHEEL/2;
    r = H_WHEEL/2;
    seta = 0:0.001:2*pi; 
    yyl = yl_circle+r*cos(seta); 
    yyr = yr_circle+r*cos(seta); 
    zz = z_circle+r*sin(seta); 
    plot(yyl,zz,yyr,zz,'b','LineWidth',CARWIDTH); 
    hold on;
    
    plot([y,y_line],[H_CAR,z_line],'r','LineWidth',LINEWIDTH);%画单摆杆
    hold on
    alpha=0:pi/20:2*pi;    %角度[0,2*pi] 
    R=0.05;                   %半径 
    x1=y_line+R*cos(alpha); 
    y1=z_line+R*sin(alpha); 
    plot(x1,y1,'-') 
    hold on
    fill(x1,y1,'r');         %用红色填充
    %plot([y_line],[z_line],'r','LineWidth',LINEWIDTH);%画单摆杆
    hold on
    if i>1 && i<N   %画出表示行车方向的箭头
        if yout.signals.values(i,1) > yout.signals.values(i-1,1)
          plot([(y-0.08),(y+0.08)],[(H_CAR+H_WHEEL)/2,(H_CAR+H_WHEEL)/2],'m','LineWidth',ARROWWIDTH);%画
          hold on 
          plot([(y+0.08),(y+0.08)-0.05*cos(pi/6)],[(H_CAR+H_WHEEL)/2,(H_CAR+H_WHEEL)/2+0.05*sin(pi/6)],'m','LineWidth',ARROWWIDTH);%画
          hold on 
          plot([(y+0.08),(y+0.08)-0.05*cos(pi/6)],[(H_CAR+H_WHEEL)/2,(H_CAR+H_WHEEL)/2-0.05*sin(pi/6)],'m','LineWidth',ARROWWIDTH);%画
          hold on 
        elseif yout.signals.values(i,1) < yout.signals.values(i-1,1)
          plot([(y-0.08),(y+0.08)],[(H_CAR+H_WHEEL)/2,(H_CAR+H_WHEEL)/2],'m','LineWidth',ARROWWIDTH);%画
          hold on 
          plot([(y-0.08),(y-0.08)+0.05*cos(pi/6)],[(H_CAR+H_WHEEL)/2,(H_CAR+H_WHEEL)/2+0.05*sin(pi/6)],'m','LineWidth',ARROWWIDTH);%画
          hold on 
          plot([(y-0.08),(y-0.08)+0.05*cos(pi/6)],[(H_CAR+H_WHEEL)/2,(H_CAR+H_WHEEL)/2-0.05*sin(pi/6)],'m','LineWidth',ARROWWIDTH);%画
          hold on   
        end
    end
    plot([(y-1.65),(y+1.65)],[0,0],'b','LineWidth',6)
    hold on   
    plot([(y-1.75),(y-1.65)],[0.1,0],'b','LineWidth',6)
    hold on
    plot([(y+1.75),(y+1.65)],[0.1,0],'b','LineWidth',6)
    hold on   
    plot([(y-1.75),(y-0.3)],[(H_CAR+H_WHEEL)/2,(H_CAR+H_WHEEL)/2],'g','LineWidth',14)
    hold on
    plot([(y+1.75),(y+0.3)],[(H_CAR+H_WHEEL)/2,(H_CAR+H_WHEEL)/2],'g','LineWidth',14)
    hold on
    
    if i>1 && i<N   %画出表示行车方向的箭头
        if yout.signals.values(i,1) > yout.signals.values(i-1,1)
            
          plot([(y+1),(y+1.1)-0.2*cos(pi/6)],[(H_CAR+H_WHEEL)/2,(H_CAR+H_WHEEL)/2+0.2*sin(pi/6)],'g','LineWidth',ARROWWIDTH);%画
          hold on 
          plot([(y+1),(y+1.1)-0.2*cos(pi/6)],[(H_CAR+H_WHEEL)/2,(H_CAR+H_WHEEL)/2-0.2*sin(pi/6)],'g','LineWidth',ARROWWIDTH);%画
          hold on 

          plot([(y-1.1),(y-1)-0.2*cos(pi/6)],[(H_CAR+H_WHEEL)/2,(H_CAR+H_WHEEL)/2+0.2*sin(pi/6)],'g','LineWidth',ARROWWIDTH);%画
          hold on 
          plot([(y-1.1),(y-1)-0.2*cos(pi/6)],[(H_CAR+H_WHEEL)/2,(H_CAR+H_WHEEL)/2-0.2*sin(pi/6)],'g','LineWidth',ARROWWIDTH);%画
          hold on   
          
        elseif yout.signals.values(i,1) < yout.signals.values(i-1,1)
            
          plot([(y+1),(y+1.1)+0.2*cos(pi/6)],[(H_CAR+H_WHEEL)/2,(H_CAR+H_WHEEL)/2+0.2*sin(pi/6)],'g','LineWidth',ARROWWIDTH);%画
          hold on 
          plot([(y+1),(y+1.1)+0.2*cos(pi/6)],[(H_CAR+H_WHEEL)/2,(H_CAR+H_WHEEL)/2-0.2*sin(pi/6)],'g','LineWidth',ARROWWIDTH);%画
          hold on   
          
          plot([(y-1),(y-1)+0.2*cos(pi/6)],[(H_CAR+H_WHEEL)/2,(H_CAR+H_WHEEL)/2+0.2*sin(pi/6)],'g','LineWidth',ARROWWIDTH);%画
          hold on 
          plot([(y-1),(y-1)+0.2*cos(pi/6)],[(H_CAR+H_WHEEL)/2,(H_CAR+H_WHEEL)/2-0.2*sin(pi/6)],'g','LineWidth',ARROWWIDTH);%画
          hold on   
        end
    end
    
    
    xlabel('y(t) / m'), ylabel('z(t) / m'),title('倒立摆小车二维动态效果图');
    % ylim([0 2])
    axis equal;  %保持坐标比例协调
    pause(0.5);
   

end