位姿1分析（由最初始状态到折叠状态，图中粉色线表示）1、运动学正解，求齐次变换矩阵（Matlab 编程）syms a1a2a3b1b2b3%各关节变量变化量a1=—28*pi/180; a2=28*pi/180; a3=0*pi/180;%各z轴间夹角b1=0;b2=0;b3=-pi/2;%求齐次变换矩阵由公式1iiT-= [ cos(a) -sin(a) 0 c ;sin(a)*cos(b) cos(a)*cos(b) -sin(b) -d*sin(b);sin(a)*sin(b) cos(a)*sin(b) cos(b) d*cos(b);0 0 0 1 ] 03T=01T*12T*23T=2*33由此可求出各其次变换矩阵T=[ 0.8829 0.4695 0 01-0.4695 0.8829 0 00 0 1.0000 00 0 0 1.0000]1T= [ 0.8829 -0.4695 0 245.000020.4695 0.8829 0 00 0 1.0000 00 0 0 1.0000]2T=[ 1.0000 0 0 030 0.0000 1.0000 204.00000 -1.0000 0.0000 0.00000 0 0 1.0000]T=[ 1.0000 0 0 216.3222 30 0.0000 1.0000 88.97950 -1.0000 0.0000 0.00000 0 0 1.0000]T= [ 1.0000 0 0 216.3222; 20 1.0000 0 -115.0205;0 0 1.0000 0;0 0 0 1.0000]1T=[ 0.8829 -0.0000 -0.4695 149.2278;30.4695 0.0000 0.8829 180.1213;0 -1.0000 0.0000 0.0000;0 0 0 1.0000]2、求雅克比矩阵由公式：z1 =[T10(1,3);T10(2,3);T10(3,3)];z2 =[T20(1,3);T20(2,3);T20(3,3)];z3 =[T30(1,3);T30(2,3);T30(3,3)];p1=[T31(1,4);T31(2,4);T31(3,4)];p2=[T32(1,4);T32(2,4);T32(3,4)];r1=[T10(1,1) T10(1,2) T10(1,3);T10(2,1) T10(2,2) T10(2,3);T10(3,1) T10(3,2) T10(3,3)];p11=cross(z1,r1*p1);p22=cross(z2,r2*p2);j=[p11(1,1) p22(1,1)p11(2,1) p22(2,1)p11(3,1) p22(3,1)z1(1,1) z2(1,1)z1(2,1) z2(2,1)z1(3,1) z2(3,1) ]J=[ -88.9795 -204.0000216.3222 00 00 00 01.0000 1.0000]3、运动学反解由程序：x=216.3222;y=88.9795;f1=x-T30(1,4);f2=y-T30(2,4);[a1,a2]=solve(f1,f2,a1,a2);a11=vpa(a1/pi*180,6);a22=vpa(a2/pi*180,6);可得出：a11= -28.000172.7177a22= 27.9999152.0004、操作速度分析syms q1q2v1v2v=[v1;v2;0;0;0;0];q=[q1;q2];je=j*q;f3=v(1,1)-je(1,1);f4=v(2,1)-je(2,1);[q1,q2]=solve(f3,f4,q1,q2)q11=vpa(q1,3)；q22=vpa(q2,3)；可得结果为：q11=0.462e-2*v2q22=-0.490e-2*v1-.202e-2*v2 5、各关节轨迹规划Matlab程序：% v(t)=diff(f(t),t)=a1+2*a2*t+3*a3*t^2 轨迹上的关节速度% a(t)=diff(f(t),t,2)=2*a2+6*a3*t 轨迹上的关节加速度% 由在起始点和终止点的关节速度要求，规定% v(0)=0 v(tf)=0,分别得出a1=a0=0%运动时间tf设为3s,关节角1终值为bf,初值为0syms a2a3tf t;bf=-28*pi/180;a1=0;a0=0;tf=3;f(tf)=a0+a1*tf+a2*tf^2+a3*tf^3-bf ;v(tf)=a1+2*a2*tf+3*a3*tf^2 ;[a2,a3]=solve(f(tf),v(tf),a2,a3)%绘制关节角随时间的变化的三次曲线t=0:0.01:tf;f3=a0+a1*t+a2*t.^2+a3*t.^3plot(t,f3)gridxlabel('时间t/s')ylabel('关节变量值/°')title('位姿1时关节角1轨迹规划曲线')subplot(1,3,2)v=a1+2*a2*t+3*a3*t.^2plot(t,v)gridxlabel('时间t/s')ylabel('关节角1的关节速度°/s ')title('位姿1时关节角1速度轨迹曲线')subplot(1,3,3)a=2*a2+6*a3*tplot(t,a)gridxlabel('时间t/s')ylabel('关节角1的加速度°/s^2 ')title('位姿1时关节角1加速度轨迹曲线')关节角1三次多项式差值曲线：关节角初值为0°，终值为-28°。
Figure.1关节角2三次多项式差值曲线：关节角初值为-90°，终值为28°。
Figure.2二、位姿2分析（由折叠状态到抓球状态，以折叠状态为初始位姿，图中蓝色线表示）a1=—28*pi/180;a2=（-62）*pi/180; a3=0*pi/180;%各z轴间夹角b1=0;b2=0;b3=-pi/2;%求齐次变换矩阵1T=[ 0.8829 0.4695 0 0 -0.4695 0.8829 0 00 0 1.0000 00 0 0 1.0000] 12T=[0.4695 0.8829 0 245.0000 -0.8829 0.4695 0 00 0 1.0000 00 0 0 1.0000]23T=[ 1.0000 0 0 00 0.0000 1.0000 204.00000 -1.0000 0.0000 0.00000 0 0 1.0000]3T=[ 0.0000 0.0000 1.0000 420.3222 -1.0000 0.0000 0.0000 -115.02050 -1.0000 0.0000 0.00000 0 0 1.0000]2T= [0.0000 1.0000 0 216.3222 -1.0000 0.0000 0 -115.02050 0 1.0000 00 0 0 1.0000]13T=[0.4695 0.0000 0.8829 425.1213 -0.8829 0.0000 0.4695 95.77220 -1.0000 0.0000 0.00000 0 0 1.0000]2、求雅克比矩阵由上述可求出位姿2时的雅克比可求的雅克比矩阵为：J= [ 115.0205 -0.0000420.3222 204.00000 00 0求前述的齐次变换矩阵3，可得，X=420.3222, Y=-115.0205当a1,a2为变量时，求出03T的表达式x=420.3222;y=—115.0205;f1=x-T30(1,4);f2=y-T30(2,4);[a1,a2]=solve(f1,f2,a1,a2)a11=vpa(a1/pi*180,6)a22=vpa(a2/pi*180,6)求出结果为a11 = -2.60843-28.0001a22 = -118.000-61.9997其中（-28.001，-61.9997）为最优解4、操作速度分析设末端执行器的速度为v=[v1;v2;0;0;0;0]，需要根据v的内容来求解关节的速度q=[q1;q2] Matlab程序syms q1 q2 q3 q4 q5 q6 v1 v2 v3 w1 w2 w3v=[v1;v2;0;0;0;0]q=[q1;q2]j=J*qf3=v(1,1)-j (1,1)f4=v(2,1)-j (2,1)[q1,q2]=solve(f3,f4,q1,q2)q11=vpa(q1,3)q22=vpa(q2,3)得到的结果为q1=0 .869e-2*v1q2 = 0.490e-2*v2-.179e-1*v15、各关节轨迹规划关节角1三次多项式差值曲线：关节角初值为-28°，终值为-28°。
Figure.3关节角2三次多项式差值曲线：关节角初值为28°，终值为-62°。
Figure.4。