六轴机器人运动学包括运动学正解和运动学逆解,运动学探究的是机器人末端的位姿与六个关节角度的关系。
为了使用C++编写机器人正逆解,我需要知道机器人逆解的各关节角度公式,但查遍全网,大多数的博客都讲的模林两可,许多都直接调用了matlab的工具箱。最后我还是在机器人学导论里找到了我想要的六个公式,注意,第三版的书中有些公式错误需要改正。
机器人学导论书籍的下载地址为:
链接:https://pan.baidu.com/s/1Eea4Im-QXYjRf7_Pbu4nsQ
提取码:pt33
以puma560为例
第一步,计算 θ1
–代码表示:–
tmp=px*px+py*py-d3*d3;
θ1 = atan2(py, px)-atan2(d3,+sqrt(tmp) );
θ11 = atan2(py, px)-atan2(d3,-sqrt(tmp) );
第二步,计算 θ3
–代码表示:–
K = ( px*px + py*py + pz*pz - a2*a2 - a3*a3 - d3*d3 - d4*d4) / (2*a2);
tmp = a3*a3 + d4*d4 - K*K;
θ3 = atan2(a3,d4) - atan2(K, +sqrt(tmp)) ;
θ31 = atan2(a3,d4) - atan2(K, -sqrt(tmp));
第三步,计算 θ2
–代码表示:–
c1=cos(θ1);
s1=sin(θ1);
c3=cos(θ3);
s3=sin(θ3);
θ23 = atan2( (-a3-a2*c3 )*pz - (c1*px+s1*py)*(d4-a2*s3), ...
(a2*s3-d4)*pz + (a3+a2*c3)*(c1*px + s1*py));
θ2 = θ23 - θ3;
第四步,计算 θ4
–代码表示:–
c23 = cos(θ2+θ3);
s23 = sin(θ2+θ3);
θ4 = atan2( (-r13*s1 + r23*c1), (-r13*c1*c23-r23*s1*c23 + r33*s23) );
第五步,计算 θ5
–代码表示:–
c4 = cos(θ4);
s4 = sin(θ4);
s5 = -( r13*(c1*c23*c4 + s1*s4) + r23*(s1*c23*c4 - c1*s4)-r33*(s23*c4) );
c5 = r13*(-c1*s23) + r23*(-s1*s23) + r33*(-c23);
θ5 = atan2(s5, c5);
第六步,计算 θ6
–代码表示:–
s6 = -r11*(c1*c23*s4 - s1*c4) - r21*(s1*c23*s4 + c1*c4) + r31*(s23*s4);
c6 = r11*((c1*c23*c4 + s1*s4)*c5 - c1*s23*s5 )...
+ r21*((s1*c23*c4 - c1*s4)*c5 - s1*s23*s5)...
- r31*(s23*c4*c5 + c23*s5);
θ6 = atan2(s6, c6);