动力学代码，按需修改参数
各关节力矩-关节变量的关系曲线：
%%%%%%%%SCARA机器人仿真模型
l=[0.457 0.325];
L(1)= Link('d',0,'a',l(1),'alpha',0,'standard','qlim',[-130 130]*pi/180);%连杆1
L(2)=Link('d',0,'a',l(2),'alpha',pi,'standard','qlim',[-145 145]*pi/180);%连杆2
L(3)=Link('theta',0,'a',0,'alpha',0,'standard','qlim',[0 0.3]);%连杆3
L(4)= Link('d',0,'a',0,'alpha',0,'standard','qlim',[-360 360]*pi/180);%连杆4
scara=SerialLink(L,'name','SCARA Robot');%建立 SCARA 机器人模型
q0=[0 0 0 0];%初始关节变量
scara.plot(q0);%绘制机器人结构
scara.teach(q0);%驱动机器人%%%%%%%%工作空间绘制
qL=scara.qlim;%关节运动范围
N=10000;%随机数数量
ws_q=zeros(N,4);%定义N个关节变量向量组成的关节变量矩阵q
a=rand(size(ws_q)); %生成元素数值在0~1之间的与q同维度的随机矩阵for j=1:Nfor i=1:4ws_q(j,i)=qL(i,1)+(qL(i,2)-qL(i,1))*a(j,i);%给矩阵q赋q范围内的随机值，以得到足够多的给定范围(矩阵ql)内的机器人的关节变量(矩阵q)值end
end
%求机器人位置坐标
ws_x=l(1)*cos(ws_q(:,1))+l(2)*cos(ws_q(:,1)+ ws_q(:,2));%机器人末端x坐标值
ws_y=l(1)*sin(ws_q(:,1))+l(2)*sin(ws_q(:,1)+ws_q(:,2));%机器人末端了坐标值
ws_z=- ws_q(:,3);%机器人末端z坐标值
figure('Name','Scara机器人工作空间')
plot3(ws_x,ws_y,ws_z,'r.')
grid;
xlabel('x');
ylabel('y');
zlabel('z');
%%%%%%%%轨迹规划
qA=[0 0 0 0];%起始点关节变量值
qB=[0.7 1.2 0.15 2];%终止点关节变量值
t=0:0.05:2;%产生时间向量
[q,qd,qdd]=jtraj(qA,qB,t);%jtraj()为构建轨迹命令，得到各关节的位移、速度和加速度
%%%%%%%%求解关节驱动力矩矢量M（t）
m1=15;m2=10;m3=2.5;m4=2.5;%连杆质量
l1=0.475;l2=0.325;r0=0.01; %连杆参数
g=9.8;%重力加速度[m/s^2]
for i=1:length(t)D11=(m1*l1^2)/3+m2*((l1^2)/3+l2^2+l1*l2*cos(q(i,2)))+(m3+m4)*(l1^2+l2^2+2*l1*l2*cos(q(i,2)))+(m4*r0^2)/2;D22=(m2*l2^2)/3+m3*l2^2+m4*l2^2+(m4*r0^2)/2;D33=m3+m4;D44=(m4*r0^2)/2;D12=(m2*l2^2)/3+(m2*l2*cos(q(i,2)))/2+m3*l2^2+m3*l1*l2*cos(q(i,2))+m4*l2^2+m4*l1*l2*cos(q(i,2))+(m4*r0^2)/2;D13=0; D14=-(m4*r0^2)/2;D21=D12;D23=0;D24=-(m4*r0^2)/2;D31=D13;D32=D23;D34=0;D41=D14;D42=D24;D43=D34;D112=-(m2/2+m3+m4)*l1*l2*sin(q(i,2));D121=D112;D122=-(m2/2+m3+m4)*l1*l2*cos(q(i,2));D211=-D122;tau(:,i)=[D11 D12 D13 D14;D21 D22 D23 D24;D31 D32 D33 D34;D41 D42 D43 D44]*qdd(i,:)'+[2*D112*qd(i,1)*qd(i,2)+D122*qd(i,2)^2;D211*qd(i,2)^2; -(m3+m4)*g;0];
end
figure('Name','SCARA机器人力(矩)-关节变量的关系曲线');
subplot(1,4,1);
plot(q(:,1),tau(1,:));
grid on
xlabel('theta-1(rad)');
ylabel('关节1力矩(N·m)');
subplot(1,4,2);
plot(q(:,2),tau(2,:));
grid on
xlabel('theta-2(rad)');
ylabel('关节2力矩(N·m)');
subplot(1,4,3);
plot(q(:,3),tau(3,:));
grid on
xlabel('d-3(m)');
ylabel('关节3力矩(N·m)');
subplot(1,4,4);
plot(q(:,4),tau(4,:));
grid on
xlabel('theta-4(rad)');
ylabel('关节4力矩(N·m)');
