MATLAB动力学建模及Simscape验证实例‌

为方便观看视频的小伙伴，将代码内容复制如下。

（含3个m文件）

PenMain.m

******************************************************************************************************** 网页链接

close all;

clear;clc

%% 说明

% 单摆-均质

% 作者：RobotZhu

% 时间：2021年7月20

%% 参数设置

% 单位 kg m rad s

% 常量

global m l g J

m = 1;

l = 1;

g = 9.8;

J = m*(l^2)/3;          % 转轴转动惯量

J_mc =  m*(l^2)/12;     % 质心转动惯量

% 变量

stopTime = 5;

tspan = [0,stopTime];

x0 = [0;0];             % [theta;dtheta]

options=odeset('AbsTol',1e-6,'InitialStep',1e-7);

[t,x]=ode45(@myDyn,tspan,x0,options);

%% 动画

figure(1);

set(gca,'nextplot','replacechildren');

v = VideoWriter('Pen.avi');

open(v);

x0=0;y0=0;

j=1;

for k=1:200:size(t)

    x1(j)=l*sin(x(k,1));

    y1(j)=-l*cos(x(k,1));

    link1_x=[x0,x1(j)];

    link1_y=[y0,y1(j)];

    line(link1_x,link1_y,'linewidth',2,'color','b')

    

    axis equal

    axis([-1.55 1.55 -1.55 0.55])

    

    title(['Simulation t=',num2str(t(k))]);

    grid on;

    hold on;

    

    plot(x0,y0,'o','linewidth',2,'color','r');

    frame = getframe(gcf);

    writeVideo(v,frame);

    clf;

    j=j+1;

end

close(v);

%% 绘图-状态量随时间变化

figure(2);

grid on;hold on;

plot(t,x(:,1),'r-','linewidth',2)

plot(t,x(:,2),'b-','linewidth',2)

xlabel('{\itt} [s]'); ylabel('{\itx} [rad]  {\itv} [rad/s]')

legend('theta(t)','dtheta(t)')

%% test-1 不同动能表达

figure(3);

grid on;hold on;

T1=0.5*J*x(:,2).*x(:,2);

dx = 0.5*l*cos(x(:,1)).*x(:,2);

dy = -0.5*l*sin(x(:,1)).*x(:,2);

T2=0.5*m*dx.*dx+0.5*m*dy.*dy+0.5*J_mc*x(:,2).*x(:,2);

plot(t,T1,'r-','linewidth',2)

plot(t,T2,'b-','linewidth',2)

legend('T(dtheta)','T(dx,dy)')

********************************************************************************************************

myDyn.m

********************************************************************************************************

function dx=myDyn(t,x)

global m l g

dx=zeros(length(x),1);

theta = x(1);

dtheta = x(2);

tau = PIDController(t,x);

d_q = dtheta;

dd_q = 3*(tau-0.5*m*g*l*sin(theta))/(m*l^2);

dx=[d_q;dd_q];

t %显示时间

end

********************************************************************************************************

PIDController.m

********************************************************************************************************

function tau=PIDController(t,x)

% 目标状态

x1_desire = pi/4;

x2_desire = 0;

% 控制增益

Kp = 1000;

Kd = 500;

% 控制力矩

tau = Kp*(x1_desire-x(1))+Kd*(x2_desire-x(2));

end