Denso机器人

Denso机器人 Denso垂直关节机器人的运动分析 一、 实验目的: 1. 理解串联机器人运动学的D-H坐标系的建立方法; 2. 掌握串联机器人的运动学方程的建立; 3. 会运用运动方程求解运动学的正解和反解。 二、 实验原理: 1. 机器人结构分析 (1)Denso机器人齐次坐标系的建立(给出坐标系示意图) (2)Denso机器人的杆件参数(参考指标参考实验一结构示意图) 变量 d/mm a/mm 关节 1 125 0 -90 2 0 210 0 3 0 75 -90 4 210 0 90 5 0 0 90 6 0 0 0 (3)机器人位置变换矩阵 00n-212n-1T=TTTTTn123……n-1n 0012345T=TTTTTT61 2 3 4 5 6 0T= 1 1T= 2 2T= 3 3T= 4 4T= 5 5T= 6 0T= 6 其中: 式中 2.运动学正解 把相应的角度值代入以上各式便得到了运动学正解的变换矩阵。 3.运动逆解 现在已知机器人的末端位姿矩阵，求出各关节旋转的角度。 注意到 将上面两式相除得: ， 0012345由式T=TTTTTT，得: 61 2 3 4 5 6 0-1 012345TT=TTTTT16 2 3 4 5 6 取第一行第四列和第二行第四列得: 令，，移项得: 对上面两式左右两边同时平方然后相加得: 化简得: 同理，求第三个角: 1-10-1 0令Q=TTT则: 2 16 2345Q=TTTT 3 4 5 6 取第一行第四列得: 可以求得 2-1 1-10-1 0令M=TTTT则: 32 16 345M=TTT 4 5 6 取第一行第三列，第三行第二列，第三行第三列得: 于是就求出了。 正解实验代码: PI=3.1415926; a=importdata('1.txt'); syms a1; syms a2; syms a3; syms a4; syms a5; syms a6; a1=a(1)/180*PI; a2=a(2)/180*PI; a3=a(3)/180*PI; a4=a(4)/180*PI; a5=a(5)/180*PI; a6=a(6)/180*PI; A1=[cos(a1) 0 -sin(a1) 0;sin(a1) 0 cos(a1) 0;0 -1 0 125;0 0 0 1]; A2=[cos(a2) -sin(a2) 0 210*cos(a2);sin(a2) cos(a2) 0 210*sin(a2);0 0 1 0;0 0 0 1]; A3=[cos(a3) 0 -sin(a3) 75*cos(a3);sin(a3) 0 cos(a3) 75*sin(a3);0 -1 0 0;0 0 0 1]; A4=[cos(a4) 0 sin(a4) 0;sin(a4) 0 -cos(a4) 0;0 1 0 210;0 0 0 1]; A5=[cos(a5) 0 sin(a5) 0;sin(a5) 0 -cos(a5) 0;0 1 0 0;0 0 0 1]; A6=[cos(a6) -sin(a6) 0 0;sin(a6) cos(a6) 0 0;0 0 1 0;0 0 0 1]; B=A1*A2*A3*A4*A5*A6 当输入目标角度:10 30 30 40 50 60 运行结果: 逆解实验代码: %%机器人技术，denso机器人运动学逆解 %%创建时间2011年10月25日 %%说明:Matrix.txt存放4X4的变化矩阵 PI=3.1415926; P=importdata('Matrix.txt'); syms a1 a2 a3 a4 a5 a6; A1=[cos(a1) 0 -sin(a1) 0;sin(a1) 0 cos(a1) 0;0 -1 0 125;0 0 0 1]; A2=[cos(a2) -sin(a2) 0 210*cos(a2);sin(a2) cos(a2) 0 210*sin(a2);0 0 1 0;0 0 0 1]; A3=[cos(a3) 0 -sin(a3) 75*cos(a3);sin(a3) 0 cos(a3) 75*sin(a3);0 -1 0 0;0 0 0 1]; A4=[cos(a4) 0 sin(a4) 0;sin(a4) 0 -cos(a4) 0;0 1 0 210;0 0 0 1]; A5=[cos(a5) 0 sin(a5) 0;sin(a5) 0 -cos(a5) 0;0 1 0 0;0 0 0 1]; A6=[cos(a6) -sin(a6) 0 0;sin(a6) cos(a6) 0 0;0 0 1 0;0 0 0 1]; a1=atan(P(2,4)/P(1,4)); %求第一个角度的值 f1= P(1,4)*cos(a1)+P(2,4)*sin(a1); f2=125-P(3,4); l1=420*f2; l2=420*f1; l3=f1*f1+f2*f2-75*75; r=(l1^2+l2^2)^(1/2); j1=atan(l2/l1); %a*cos+b*sin=0型式，过度角角度 j2=atan((l3/r)/((1-(l3/r)^2)^(1/2))); %过度角角度 a2=j2-j1; %第二个角度的值 A1=[cos(a1) 0 -sin(a1) 0;sin(a1) 0 cos(a1) 0;0 -1 0 125;0 0 0 1]; A2=[cos(a2) -sin(a2) 0 210*cos(a2);sin(a2) cos(a2) 0 210*sin(a2);0 0 1 0;0 0 0 1]; K=inv(A2)*inv(A1)*P; %左乘A1，A2的逆 l4=K(2,4); r3=(210^2+75^2)^(1/2); j3=atan(75/210); j4_1=atan(((1-(l4/r3)^2)^(1/2))/(l4/r3)); %a3第一个过度角 j4_2=atan(-((1-(l4/r3)^2)^(1/2))/(l4/r3)); %a3第二个过度角 j4=j4_1; a3_1=j3+j4; %求第三个角度的第一个值对应输入 j4=j4_2; a3_2=j3+j4; %求第三个角度的第二个值 a3=a3_1; A3=[cos(a3) 0 -sin(a3) 75*cos(a3);sin(a3) 0 cos(a3) 75*sin(a3);0 -1 0 0;0 0 0 1]; T=inv(A3)*inv(A2)*inv(A1)*P; costemp=-T(3,3); %已知cos值求角度的临时存放变量 a5_1=atan(((1-costemp^2)^(1/2))/costemp); %a5的第一个解，即对应输入,对应于a3_1 a5_2=atan((-(1-costemp^2)^(1/2))/costemp); %a5的第二个解 a3=a3_2; A3=[cos(a3) 0 -sin(a3) 75*cos(a3);sin(a3) 0 cos(a3) 75*sin(a3);0 -1 0 0;0 0 0 1]; T=inv(A3)*inv(A2)*inv(A1)*P; costemp=-T(3,3); %已知cos值求角度的临时存放变量 a5_3=atan(((1-costemp^2)^(1/2))/costemp); %a5的第三个解,对应于a3_2 a5_4=atan((-(1-costemp^2)^(1/2))/costemp); %a5的第四个解 a3=a3_1; %前面四个解对应于a3_1,从新求解A3 A3=[cos(a3) 0 -sin(a3) 75*cos(a3);sin(a3) 0 cos(a3) 75*sin(a3);0 -1 0 0;0 0 0 1]; T=inv(A3)*inv(A2)*inv(A1)*P; a5=a5_1; sintemp=T(2,3)/sin(a5); a4_1=atan(sintemp/((1-sintemp^2)^(1/2))); %a4的第一个解对应a5_1 a4_2=atan(sintemp/(-(1-sintemp^2)^(1/2))); %a4的第二个解对应a5_1 a5=a5_2; sintemp=T(2,3)/sin(a5); a4_3=atan(sintemp/((1-sintemp^2)^(1/2))); %a4的第三个解对应a5_2 a4_4=atan(sintemp/(-(1-sintemp^2)^(1/2))); %a4的第四个解对应a5_2 a3=a3_2; %后面四个对应a3_2,从新求解A3 A3=[cos(a3) 0 -sin(a3) 75*cos(a3);sin(a3) 0 cos(a3) 75*sin(a3);0 -1 0 0;0 0 0 1]; T=inv(A3)*inv(A2)*inv(A1)*P; a5=a5_3; sintemp=T(2,3)/sin(a5); a4_5=atan(sintemp/((1-sintemp^2)^(1/2))); %a4的第五个解对应a5_1 a4_6=atan(sintemp/(-(1-sintemp^2)^(1/2))); %a4的第六个解对应a5_1 a5=a5_4; sintemp=T(2,3)/sin(a5); a4_7=atan(sintemp/((1-sintemp^2)^(1/2))); %a4的第七个解对应a5_2 a4_8=atan(sintemp/(-(1-sintemp^2)^(1/2))); %a4的第八个解对应a5_2 a3=a3_1; %前面四个解对应于a3_1,从新求解A3 A3=[cos(a3) 0 -sin(a3) 75*cos(a3);sin(a3) 0 cos(a3) 75*sin(a3);0 -1 0 0;0 0 0 1]; T=inv(A3)*inv(A2)*inv(A1)*P; a5=a5_1; sintemp=-T(3,2)/sin(a5); a6_1=atan(sintemp/((1-sintemp^2)^(1/2))); %a6的第一个解对应a5_1 a6_2=atan(sintemp/(-(1-sintemp^2)^(1/2))); %a6的第二个解对应a5_1 a5=a5_2; sintemp=-T(3,2)/sin(a5); a6_3=atan(sintemp/((1-sintemp^2)^(1/2))); %a6的第三个解对应a5_2 a6_4=atan(sintemp/(-(1-sintemp^2)^(1/2))); %a6的第四个解对应a5_2 a3=a3_2; %后面四个解对应于a3_2,从新求解A3 A3=[cos(a3) 0 -sin(a3) 75*cos(a3);sin(a3) 0 cos(a3) 75*sin(a3);0 -1 0 0;0 0 0 1]; T=inv(A3)*inv(A2)*inv(A1)*P; a5=a5_3; sintemp=-T(3,2)/sin(a5); a6_5=atan(sintemp/((1-sintemp^2)^(1/2))); %a6的第五个解对应a5_1 a6_6=atan(sintemp/(-(1-sintemp^2)^(1/2))); %a6的第六个解对应a5_1 a5=a5_4; sintemp=-T(3,2)/sin(a5); a6_7=atan(sintemp/((1-sintemp^2)^(1/2))); %a6的第七个解对应a5_2 a6_8=atan(sintemp/(-(1-sintemp^2)^(1/2))); %a6的第八个解对应a5_2 B=[a1 a2 a3_1 a4_1 a5_1 a6_1;a1 a2 a3_1 a4_2 a5_1 a6_2;a1 a2 a3_1 a4_3 a5_2 a6_3;a1 a2 a3_1 a4_4 a5_2 a6_4;a1 a2 a3_2 a4_5 a5_3 a6_5;a1 a2 a3_2 a4_6 a5_3 a6_6;a1 a2 a3_2 a4_7 a5_4 a6_7;a1 a2 a3_2 a4_8 a5_4 a6_8;]; B=180/PI*B 当输入为: 输出为: 第一行正好对应之前的输入。