机器人轨迹规划（关节空间轨迹规划和笛卡尔空间轨迹规划）

clear; deg = pi/180; L(1)=Link([0 0 0.088 -pi/2 0],'standard'); L(2)=Link([-pi/2 0 0.4 0 0],'standard'); L(3)=Link([0 0 0.04 -pi/2 0],'standard'); L(4)=Link([0 0.405 0 pi/2 0],'standard'); L(5)=Link([0 0 0 pi/2 0],'standard'); L(6)=Link([0 0 0 0 0],'standard'); L(1).m = 0; L(2).m = 17.4; L(3).m = 4.8; L(4).m = 0.82; L(5).m = 0.34; L(6).m = .09; % rx ry rz L(1).r = [0 0 0 ]; L(2).r = [0.068 0.006 -0.016]; L(3).r = [0 -0.070 0.014 ]; L(4).r = [0 0 -0.019]; L(5).r = [0 0 0 ]; L(6).r = [0 0 .032 ]; % Ixx Iyy Izz Ixy Iyz Ixz L(1).I = [0 0 0.35 0 0 0]; L(2).I = [.13 .524 .539 0 0 0]; L(3).I = [.066 .0125 .066 0 0 0]; L(4).I = [1.8e-3 1.8e-3 1.3e-3 0 0 0]; L(5).I = [.3e-3 .3e-3 .4e-3 0 0 0]; L(6).I = [.15e-3 .15e-3 .04e-3 0 0 0]; L(1).Jm = 291e-6; L(2).Jm = 409e-6; L(3).Jm = 299e-6; L(4).Jm = 35e-6; L(5).Jm = 35e-6; L(6).Jm = 35e-6; L(1).G = -62.6111; L(2).G = 107.815; L(3).G = -53.7063; L(4).G = 76.0364; L(5).G = 71.923; L(6).G = 76.686; L(1).qlim =[-170 170]*deg; L(2).qlim =[-65 150]*deg; L(3).qlim=[-138 255]*deg; L(4).qlim=[-190 190]*deg; L(5).qlim=[-135 135]*deg; L(6).qlim=[-360 360]*deg; R=SerialLink(L,'name','Motoman_MH5LS'); q0=[0 0 0 0 0 0 ]; T0=R.fkine(q0); T =[0.7325 0.4619 -0.5000 0.6514;0.1913 -0.8446 -0.5000 0.0787;-0.6533 0.2706 -0.7071 -0.0283;0 0 0 1.0000]; %机器人末端位姿 qn=R.ikine(T); t=0:0.05:10; n=jtraj(0,1,t); TC=ctraj(T0,T,n); q=R.ikine(TC); R.plot(q); figure(2); subplot(3,3,1); plot(t,q(:,1)); xlabel('t/s'); ylabel('关节一角位移/rad'); grid on subplot(3,3,2); t1=0:0.05:9.95; w=diff(q(:,1))./diff(t'); plot(t1,w); xlabel('t/s'); ylabel('关节一角速度/rad/s'); grid on subplot(3,3,3); t2=0:0.05:9.9; v=diff(w)./diff(t1'); plot(t2,v); xlabel('t/s^2'); ylabel('关节一角加速度/rad/s^2'); grid on subplot(3,3,4); plot(t,q(:,2)); xlabel('t/s'); ylabel('关节二角位移/rad'); grid on subplot(3,3,5); w1=diff(q(:,2))./diff(t'); plot(t1,w1); xlabel('t/s'); ylabel('关节二角速度/rad/s'); grid on subplot(3,3,6); v1=diff(w1)./diff(t1'); plot(t2,v1); xlabel('t/s^2'); ylabel('关节二角加速度/rad/s^2'); grid on subplot(3,3,7); plot(t,q(:,3)); xlabel('t/s'); ylabel('关节三角位移/rad'); grid on subplot(3,3,8); w2=diff(q(:,3))./diff(t'); plot(t1,w2); xlabel('t/s'); ylabel('关节三角速度/rad/s'); grid on subplot(3,3,9); v2=diff(w2)./diff(t1'); plot(t2,v2); xlabel('t/s^2'); ylabel('关节三角加速度/rad/s^2'); grid on figure(3); subplot(3,3,1); plot(t,q(:,4)); xlabel('t/s'); ylabel('关节四角位移/rad'); grid on subplot(3,3,2); w3=diff(q(:,4))./diff(t'); plot(t1,w3); xlabel('t/s'); ylabel('关节四角速度/rad/s'); grid on subplot(3,3,3); v3=diff(w3)./diff(t1'); plot(t2,v3); xlabel('t/s^2'); ylabel('关节四角加速度/rad/s^2'); grid on subplot(3,3,4); plot(t,q(:,5)); xlabel('t/s'); ylabel('关节五角位移/rad'); grid on subplot(3,3,5); w4=diff(q(:,5))./diff(t'); plot(t1,w4); xlabel('t/s'); ylabel('关节五角速度/rad/s'); grid on subplot(3,3,6); v4=diff(w4)./diff(t1'); plot(t2,v4); xlabel('t/s^2'); ylabel('关节五角加速度/rad/s^2'); grid on subplot(3,3,7); plot(t,q(:,6)); xlabel('t/s'); ylabel('关节六角位移/rad'); grid on subplot(3,3,8); w5=diff(q(:,6))./diff(t'); plot(t1,w5); xlabel('t/s'); ylabel('关节六角速度/rad/s'); grid on subplot(3,3,9); v5=diff(w5)./diff(t1'); plot(t2,v5); xlabel('t/s^2'); ylabel('关节六角加速度/rad/s^2'); grid on figure(4); x(1,1:201)=TC(1,4,:); y(1,1:201)=TC(2,4,:); z(1,1:201)=TC(3,4,:); plot3(x,y,z,'ko'); grid on