【机器人学】机械臂球形手腕的逆解

        如图1所示的球形手腕(三个关节的轴线相交于一点)是常用的机械臂结构，我们希望在已知坐标系3至坐标系6的旋转矩阵的条件下求解3个关节值 q3 q 3 , q4 q 4 , q6 q 6 。旋转矩阵可通过欧拉角集合、四元数或直接用3×3的矩阵给出。

图1 典型腕关节结构

        假设现在已知的旋转矩阵为：

R63=⎡⎣⎢nxnynzsxsyszaxayaz⎤⎦⎥ R 3 6 = [ n x s x a x n y s y a y n z s z a z ]

         q3 q 3 , q4 q 4 , q6 q 6 的值为 ：
        当 q5∈(0，π) q 5 ∈ ( 0 ， π )

q4=atan2(ay,ax)q5=atan2(ax2+ay2−−−−−−−−√,az)q6=atan2(sz,−nz)(205)(206)(207) (205) q 4 = atan ⁡ 2 ( a y , a x ) (206) q 5 = a tan ⁡ 2 ( a x 2 + a y 2 , a z ) (207) q 6 = a tan ⁡ 2 ( s z , − n z )

        当 q5∈(−π，0) q 5 ∈ ( − π ， 0 )

q4=atan2(−ay,−ax)q5=atan2(−ax2+ay2−−−−−−−−√,az)q6=atan2(−sz,nz)(208)(209)(210) (208) q 4 = atan ⁡ 2 ( − a y , − a x ) (209) q 5 = a tan ⁡ 2 ( − a x 2 + a y 2 , a z ) (210) q 6 = a tan ⁡ 2 ( − s z , n z )

参考文献：
布鲁诺・西西里安诺.《机器人学：建模，规划和控制》 西安交通大学出版社 2015