工业6轴机器人逆解（PIEPER）

六轴机器人运动学包括运动学正解和运动学逆解，运动学探究的是机器人末端的位姿与六个关节角度的关系。

为了使用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);

第七步，判断8个解需要哪一个