大家好，又见面了，我是你们的朋友全栈君。
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
发布者：全栈程序员栈长，转载请注明出处：https://javaforall.cn/171031.html原文链接：https://javaforall.cn
