puma560机器人运动学分析与轨迹规划（程序+报告）

%五次多项式轨迹规划 clear clc %puma560 % theta d a alpha sigma L1=Link([0 0.4 0.025 pi/2 0 ]); L2=Link([pi/2 0 0.56 0 0 ]); L3=Link([0 0 0.035 pi/2 0 ]); L4=Link([0 0.515 0 pi/2 0 ]); L5=Link([pi 0 0 pi/2 0 ]); L6=Link([0 0.08 0 0 0 ]); robot=SerialLink([L1,L2,L3,L4,L5,L6]); robot.display(); %显示建立的机器人的DH参数 robot.teach; %五次多项式轨迹规划 init_ang=[0,0,0,0,0,0];%p1起点 targ_ang=[pi/4,pi/2,pi/3,pi/5,pi/6,pi/7];%p2终点 step=40; %jtraj，已知初始和终止的关节角度，利用五次多项式来规划轨迹。 [q,qd,qdd] = jtraj(init_ang, targ_ang, step); robot.plot(q); hold on for i=1:step q1=q(i,:); M=robot.fkine(q1) X=M(1,4); Y=M(2,4); Z=M(3,4); plot3(X,Y,Z,'.'); end figure(2) n=6; subplot(3,1,1); i=1:n; plot(q(:,i)); legend('自由度1','自由度2','自由度3','自由度4','自由度5','自由度6') title('角度'); grid on; subplot(3,1,2); i=1:n; plot(qd(:,i)); legend('自由度1','自由度2','自由度3','自由度4','自由度5','自由度6') title('速度'); grid on; subplot(3,1,3); i=1:n; plot(qdd(:,i)); legend('自由度1','自由度2','自由度3','自由度4','自由度5','自由度6') title('加速度'); grid on; ings=[]; xx=[]; for i=1:40 q1=q(i,:); M=robot.fkine(q1) for j=1:3 xx(j)=M(j,4); end ings=[ings;xx]; end figure(5) n=3; i=1:n; plot(ings(:,i)); legend('自由度1','自由度2','自由度3','自由度4','自由度5','自由度6') title('位置'); grid on;