FANUC机器人三维模型+正逆运动学分析+轨迹规划matlab代码

clear clc L(1)=Link('d', 0.33, 'a',0 , 'alpha', pi/2,'offset',0); L(2)=Link('d', 0, 'a', 0.26, 'alpha',0,'offset',pi/2); L(3)=Link('d', 0, 'a', 0.02, 'alpha',pi/2,'offset',0); L(4)=Link('d', -0.29, 'a', 0, 'alpha',pi/2,'offset',0); L(5)=Link('d', 0, 'a', 0, 'alpha',pi/2,'offset',0); L(6)=Link('d', 0.07, 'a', 0, 'alpha',0,'offset',0); L(1).qlim=[-170*pi/180 170*pi/180]; L(2).qlim=[-110*pi/180 120*pi/180]; L(3).qlim=[-122*pi/180 280*pi/180]; L(4).qlim=[-190*pi/180 190*pi/180]; L(5).qlim=[-120*pi/180 120*pi/180]; L(6).qlim=[-360*pi/180 360*pi/180]; robot=SerialLink(L); robot.display(); %显示建立的机器人的DH参数 init_ang=[0 0 0 0 0 0 ]; robot.teach; robot.plot(init_ang); init_ang=[0 0 0 0 0 0 ]; targ_ang=[pi/4,pi/2,pi/10,-pi/3,-pi/6,-pi/7]; step=50; %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) subplot(3,1,1); i=1:3; plot(q(:,i)); legend('自由度1','自由度2','自由度3') title('位置'); grid on; subplot(3,1,2); i=1:3; plot(qd(:,i)); legend('自由度1','自由度2','自由度3') title('速度'); grid on; subplot(3,1,3); i=1:3; plot(qdd(:,i)); legend('自由度1','自由度2','自由度3') title('加速度'); grid on; figure(3) subplot(3,1,1); i=4:6; plot(q(:,i)); legend('自由度4','自由度5','自由度6') title('位置'); grid on; subplot(3,1,2); i=4:6; plot(qd(:,i)); legend('自由度4','自由度5','自由度6') title('速度'); grid on; subplot(3,1,3); i=4:6; plot(qdd(:,i)); legend('自由度4','自由度5','自由度6') title('加速度'); grid on;