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

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

图1 典型腕关节结构

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

R_{3}^{6} = [\begin{matrix} n_{x} & s_{x} & a_{x} \\ n_{y} & s_{y} & a_{y} \\ n_{z} & s_{z} & a_{z} \end{matrix}]

$q_{3}$ , $q_{4}$ , $q_{6}$ 的值为：
当 $q_{5} \in (0 ， π)$

\begin{aligned} (205) & q_{4} = atan 2 (a_{y}, a_{x}) \\ (206) & q_{5} = a \tan 2 (\sqrt{{a_{x}}^{2} + {a_{y}}^{2}}, a_{z}) \\ (207) & q_{6} = a \tan 2 (s_{z}, - n_{z}) \end{aligned}

当

q_{5} \in (- π ， 0)

\begin{aligned} (208) & q_{4} = atan 2 (- a_{y}, - a_{x}) \\ (209) & q_{5} = a \tan 2 (- \sqrt{{a_{x}}^{2} + {a_{y}}^{2}}, a_{z}) \\ (210) & q_{6} = a \tan 2 (- s_{z}, n_{z}) \end{aligned}

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