机械臂轨迹规划

机器臂运动轨迹规划位姿1分析〔由最初始状态到折叠状态，图中粉色线表示〕关节i关节变量初值关节变量值100°0θ10°-28°22450°0θ2-90°28°30-90°20400°0°一、位姿1分析1、运动学正解，求齐次变换矩阵〔Matlab编程〕symsa1%各关节变量变化量a1=—28*pi/180;a2=28*pi/180;a3=0*pi/180;%各z轴间夹角b1=0;b2=0;b3=-pi/2;%求齐次变换矩阵由公式=[cos(a)-sin(a)0csin(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);0001]=**=*=*由此可求出各其次变换矩阵=[0.88290.469500-0.46950.882900001.0000000]=[0.8829-0.469500.46950.882900001.00000000]=[1.000000000.00001.00000-1.00000.0000000]=[1.00000000.00001.00000-1.00000.0000000]=[1.000000;01.00000;001.00000;000]=[0.8829-0.0000-0.4695;0.46950.00000.8829;0;00000]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)];r2=[T20(1,1)T20(1,2)T20(1,3);T20(2,1)T20(2,2)T20(2,3);T20(3,1)T20(3,2)T20(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=[216.32220000000]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=a22=4、操作速度分析symsq1q2v1v2v=[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*v25、各关节轨迹规划Matlab程序：%利用三次多项式对关节角1进行轨迹规划%设三次多项式为%f(t)=a0+a1*t+a2*t^2+a3*t^3%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)=0v(tf)=0,分别得出a1=a0=0%运动时间tf设为3s,关节角1终值为bf,初值为0symsa2abf=-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°。关节角2三次多项式差值曲线：关节角初值为-90°，终值为28°。Figure.2二、位姿2分析〔由折叠状态到抓球状态，以折叠状态为初始位姿，图中蓝色线表示〕关节i关节变量初值关节变量值100°0θ1-28°-28°22450°0θ228°-62°30-90°20400°0°运动学正解求齐次变换矩阵由上述公式可求出位姿2时的各齐次变换矩阵为%各关节变量变化量a1=—28*pi/180;a2=〔-62〕*pi/180;a3=0*pi/180;%各z轴间夹角b1=0;b2=0;b3=-pi/2;%求齐次变换矩阵=[0.88290.469500-0.46950.882900001.00000000]=[0.46950.88290-0.88290.469500001.00000000]=[1.000000000.00001.00000000]=[0.00000.00001.0000-1.00000.00000.00000000]=[0.00001.00000-1.00000.00000001.0000000]=[0.46950.00000.88290000]2、求雅克比矩阵由上述可求出位姿2时的雅克比可求的雅克比矩阵为：J=[420.32220000001.0000]3、运动学反解求前述的齐次变换矩阵，可得，X=,Y=当a1,a2为变量时，求出的表达式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=a22=其中〔-28.001，-61.9997〕为最优解4、操作速度分析设末端执行器的速度为v=[v1;v2;0;0;0;0]，需要根据v的内容来求解关节的速度q=[q1;q2]Matlab程序symsq1q2q3q4q5q6v1v2v3w1w2w3v=[v1;v2;0;0