本文描述的是六自由度机械臂逆运动学的几何解法和正向运动学，并附带了C#的实现代码。
文中所描述的算法，未经过严格的测试，如有错误请指正。
本文禁止转载

这次求解的机器人结构如下图所示：

正向运动学求解

有关机械臂正向运动学的通用求解方法请参见 这篇文章

机械臂的初始状态

假设机械臂的初始状态为：所有节点在同一直线上如下图所示

并定义所有关节仅绕Z轴或Y轴旋转，此状态为初始状态，所有旋转矩阵 ${\mathbf{B}}_{i}$均为单位矩阵。

机械臂正向运动学解

定义底座坐标为 ${\mathbf{P}}_{0}$底座到第一个关节的长度为 ${l_0}$ ，${J_1}$至 ${J_2}$ 的连杆长度为 ${l_1}$，${J_2} 至 ${J_3}的连杆长度为 ${l_2}$ 依次类推
$${\mathbf{A}}_{i} = {\begin{bmatrix} {\mathbf{X}}_{i} & {\mathbf{Y}}_{i} & {\mathbf{Z}}_{i} \end{bmatrix}} $$
则根据正向运动学的求解方法可得：
$$ {\mathbf{P}}_{1} = {\mathbf{P}}_{0}+ {{\mathbf{A}}_{0}}{\begin{bmatrix} 0\\0\\{l_0} \end{bmatrix}} $$
$$ {\mathbf{P}}_{2} = {\mathbf{P}}_{1}+ {{\mathbf{A}}_{0}}{{\mathbf{A}}_{1}}{\begin{bmatrix} 0\\0\\{l_1} \end{bmatrix}} $$
$$ {\mathbf{P}}_{3} = {\mathbf{P}}_{2}+ {{\mathbf{A}}_{0}}{{\mathbf{A}}_{1}}{{\mathbf{A}}_{2}}{\begin{bmatrix} 0\\0\\{l_2} \end{bmatrix}} $$
$$ {\mathbf{P}}_{4} = {\mathbf{P}}_{3}+ {{\mathbf{A}}_{0}}{{\mathbf{A}}_{1}}{{\mathbf{A}}_{2}}{{\mathbf{A}}_{3}}{\begin{bmatrix} 0\\0\\{l_3} \end{bmatrix}} $$
$$ {\mathbf{P}}_{5} = {\mathbf{P}}_{4}+ {{\mathbf{A}}_{0}}{{\mathbf{A}}_{1}}{{\mathbf{A}}_{2}}{{\mathbf{A}}_{3}}{{\mathbf{A}}_{4}}{\begin{bmatrix} 0\\0\\{l_4} \end{bmatrix}} $$
$$ {\mathbf{P}}_{6} = {\mathbf{P}}_{5}+ {{\mathbf{A}}_{0}}{{\mathbf{A}}_{1}}{{\mathbf{A}}_{2}}{{\mathbf{A}}_{3}}{{\mathbf{A}}_{4}}{{\mathbf{A}}_{5}}{\begin{bmatrix} 0\\0\\{l_5} \end{bmatrix}} $$
$$ {\mathbf{P}}_{E} = {\mathbf{P}}_{6}+ {{\mathbf{A}}_{0}}{{\mathbf{A}}_{1}}{{\mathbf{A}}_{2}}{{\mathbf{A}}_{3}}{{\mathbf{A}}_{4}}{{\mathbf{A}}_{5}}{{\mathbf{A}}_{6}}{\begin{bmatrix} 0\\0\\{l_6} \end{bmatrix}} $$

逆运动学的几何求解

几何学求解6自由度机械臂的逆运动学需要完整的末端执行器位姿。
在求解之前先要使用欧拉角至旋转矩阵的变换公式将欧拉角转换成旋转矩阵，有关将欧拉角转换为旋转矩阵的方法请参见 这篇文章
首先假设末端执行器的位置为${\mathbf{P}}_E$，相应的旋转矩阵为${\begin{bmatrix} {\mathbf{X_6}} & {\mathbf{Y_6}} & {\mathbf{Z_6}} \end{bmatrix}}$
设第2,3,5号关节运动范围为$\left [0,\frac{\pi}{2} \right ]$，第1,4,6号关节运动范围为$\left [-\pi,\pi \right ]$
求解各关节的旋转角的方法如下

1. 计算${J_5}$的位置

由于已知末端执行器的姿态${\begin{bmatrix} {\mathbf{X_6}} & {\mathbf{Y_6}} & {\mathbf{Z_6}} \end{bmatrix}}$，且$J_6$的旋转角度和末端执行器的位置没有直接关系，则可以构造向量
$${\vec{{J_5}E}} = ({l_5}+{l_6}){\mathbf{Z_6}}$$
该矢量在机械臂简化图中如下图所示。

要计算 $J_5$的位置${\mathbf{P}}_5$仅需在末端位置上减去${\vec{{J_5}E}}$
$${\mathbf{P}_5} = {{\mathbf{P}}_E} – {\vec{{J_5}E}} $$

2. 计算${J_1}$的变换角度${\theta_1}$

由机械臂结构可知将${J_1}$至${J_5}$在同一个平面上，将${J_1}$至${J_5}$投影至X-Y平面，如下图所示，投影线与X轴的夹角即为${\theta_1}$。

在这里假设${\mathbf{P}_5} = {\begin{bmatrix} {x_5} \\ {y_5} \\{z_5} \end{bmatrix}}$
则：
$${\theta_1} = \operatorname{atan2}({y_5},{x_5}) $$

3. 计算${J_3}$的变换角度${\theta_3}$

由于已经求解${J_5}$的位置，并且${J_2}$的位置${\mathbf{P}_2}$是固定的，连接${J_2}$与${J_5}$构成一个三角形，如下图所示

矢量$\vec{{J_2}{J_5}} = {\mathbf{P}_5} – {\mathbf{P}_2}$
根据余弦定理可以建立
$${\cos{\theta_3}} = \frac{{\left \| \vec{{J_2}{J_5}} \right \|}^2 – {l_2^2} -{l_3^2}}{2{l_2}{l_3}} $$
由此可得
$${\theta_3} = \arccos{\left ( \frac{{\left \| \vec{{J_2}{J_5}} \right \|}^2 – {l_2^2} -{l_3^2}}{2{l_2}{l_3}} \right )} $$

4. 计算${J_2}$的变换角度${\theta_2}$

如前图所示，过${J_5}$作垂直于${J_2}{J_3}$延长线的直线，交${J_2}{J_3}$延长线于$S$
过${J_2}$在${\bigtriangleup}{J_2}{J_3}{J_5}$所在的平面上作垂直于Z轴的直线，并作${J_5}T$垂直于该直线于点$T$
连接${J_2}{J_5}$，并命名该线段为$A$
在${\bigtriangleup}{J_2}{J_3}{J_5}$中勾股定理可得
$$A = \sqrt{(({l_3}+{l_4})\sin{\theta_3})^2 + (({l_3}+{l_4})\sin{\theta_3}+{l_2})^2}$$
$$\beta = \arcsin{\left ( \frac{{\mathbf{P}_5}*{\mathbf{Z}_0} – {l_1}}{A} \right )} $$
$$\alpha = \arccos{\left ( \frac{{l_2^2}+{A^2}-{(l_3+l_4)}^2}{2A{l_2}}\right )}$$
由此可得
$${\theta_2} = \frac{ \pi}{2} – \alpha – \beta$$

至此，第1,2,3号关节的旋转量均已计算出，就可以根据正向运动学公式推出${J_3}$的位置${\mathbf{P}}_3$和旋转矩阵 $\begin{bmatrix} {{\mathbf{X}}_3} & {{\mathbf{Y}}_3} &{{\mathbf{Z}}_3} \end{bmatrix}$

5. 计算${J_5}$的变换角度${\theta_5}$

由${\theta_5}$的旋转矩阵正向求解法可知:
$${\mathbf{Z}}_5 = \sin{\theta_5}{\mathbf{X}_4} + \cos{\theta_5}{\mathbf{Z}}_4 $$
又${\mathbf{X}_4} \perp {\mathbf{Z}_4}, \left \| {\mathbf{Z}_4} \right \| =1$在上述方程两边同时乘以$ {\mathbf{Z}_4}$可得：
$$\cos{\theta_5} = {\mathbf{X}_4}\cdot{\mathbf{Z}_4}$$

由于第4,6关节绕Z轴旋转，所以：
$${\mathbf{Z}}_5 = {\mathbf{Z}}_6 , {\mathbf{Z}_4} ={\mathbf{Z}}_3 $$
$$\cos{\theta_5} = {\mathbf{X}_3}\cdot{\mathbf{Z}_6}$$
$${\theta_5} = \arccos{{\mathbf{X}_3}\cdot{\mathbf{Z}_6}}$$

6. 计算${J_4}$的变换角度${\theta_4}$

由${\theta_4}$的旋转矩阵正向求解法可知:
$${\mathbf{X}}_4 = \cos{\theta_4}{\mathbf{X}}_3 + \sin{\theta_4}{\mathbf{Y}_3} $$
将上式代入${\mathbf{Z}}_5$中
$${\mathbf{Z}}_5 = \sin{\theta_5}(\cos{\theta_4}{\mathbf{X}}_3 + \sin{\theta_4}{\mathbf{Y}_3}) + \cos{\theta_5}{\mathbf{Z}}_3$$
又${\mathbf{X}_3} \perp {{\mathbf{Y}}_3}, {\mathbf{X}_3} \perp {\mathbf{Z}_3} \left \| {\mathbf{X}_3} \right \| =1$
在上述方程两边同时乘以$ {\mathbf{X}_3}$可得：
$${{\mathbf{Z}}_5}\cdot {{\mathbf{X}}_3} = \sin{\theta_5} \cos{\theta_4}$$
在上述方程两边同时乘以$ {\mathbf{Y}_3}$可得：
$${{\mathbf{Z}}_5}\cdot {{\mathbf{Y}}_3} = \sin{\theta_5} \sin{\theta_4}$$
将两式相除即可得：
$${\theta_4} = \operatorname{atan2}{\left ( {{{\mathbf{Z}}_6} \cdot {{\mathbf{Y}}_3}},{{{\mathbf{Z}}_6} \cdot {{\mathbf{X}}_3}} \right )} $$

至此，根据第1至5关节的旋转角可以求出第5关节末端的旋转矩阵 $\begin{bmatrix} {{\mathbf{X}}_5} & {{\mathbf{Y}}_5} &{{\mathbf{Z}}_5} \end{bmatrix}$

7. 计算${J_6}$的变换角度${\theta_6}$

由${\theta_6}$的旋转矩阵正向求解法可知:
$${\mathbf{X}}_6 = \cos{\theta_6}{\mathbf{X}}_5 + \sin{\theta_6}{\mathbf{Y}_5} $$
在上述方程两边同时乘以${\mathbf{Y}_5}$，又由于$\left \| {\mathbf{Y}_5} \right \| =1$
$${\theta_6} = \operatorname{atan2}{({{\mathbf{X}}_6}\cdot {{\mathbf{Y}}_5} , {{\mathbf{X}}_6}\cdot {{\mathbf{X}}_5} )} $$