看过其他机器人逆解解析法的求解,过程蛮复杂的,而且有的文献说ABB-IRB5400型号的机器人是斜交末端型结构,这样的特殊机构导致其逆运动学无封闭解,所以无法使用代数法求解。我要用的是ABB-IRB4400,不知道它是不是和ABB-IRB5400一样,有解析解吗,如果没有的话,我就不做无用功用解析法去求解了。
syms theta1 theta2 theta3 theta4 theta5 theta6 a1 a2 a3 d1 d4 d6 nx ox ax px ny oy ay py nz oz az pz
T01=[cos(theta1) 0 -sin(theta1) a1*cos(theta1);
sin(theta1) 0 cos(theta1) a1*sin(theta1);
0 -1 0 d1;
0 0 0 1];
T12=[cos(theta2) -sin(theta2) 0 a2*cos(theta2);
sin(theta2) cos(theta2) 0 a2*sin(theta2);
0 0 1 0;
0 0 0 1];
T23=[cos(theta3) 0 -sin(theta3) a3*cos(theta3);
sin(theta3) 0 cos(theta3) a3*sin(theta3);
0 -1 0 0;
0 0 0 1];
T34=[cos(theta4) 0 sin(theta4) 0;
sin(theta4) 0 -cos(theta4) 0;
0 1 0 d4;
0 0 0 1];
T45=[cos(theta5) 0 -sin(theta5) 0;
sin(theta5) 0 cos(theta5) 0;
0 -1 0 0;
0 0 0 1];
T56=[cos(theta6) -sin(theta6) 0 0;
sin(theta6) cos(theta6) 0 0;
0 0 1 d6;
0 0 0 1];
T06=[nx ox ax px;
ny oy ay py;
nz oz az pz;
0 0 0 1];
T1=inv(T01)*T06
T2=T12*T23*T34*T45*T56