RobWork机器人框架（4）：手把手教你推导UR机器人的逆解算法UR3机器人的DH参数表 Transform $\th

UR3机器人的DH参数表

Transform	$\theta$	$d$	$a$	$\alpha$
0~1	$\theta_{1}$	$d_{1}=0.15190$	0	$\pi/2$
1~2	$\theta_{2}$	0	$a_{2}=-0.24365$	0
2~3	$\theta_{3}$	0	$a_{3}=-0.21325$	0
3~4	$\theta_{4}$	$d_{4}=0.11235$	0	$\pi/2$
4~5	$\theta_{5}$	$d_{5}=0.08535$	0	$-\pi/2$
5~6	$\theta_{6}$	$d_{6}=0.08190$	0	0

正向运动学

标准DH建模法下相邻坐标系之间的转换关系为：

\begin{aligned} ^{i-1}_{i}T &=Rot(z, \theta) \times Trans(z, d) \times Trans(x, a) \times Rot(x, \alpha) \\ &= \begin{bmatrix} cos\theta_{i} & -sin\theta_{i}cos\alpha_{i} & sin\theta_{i}sin\alpha_{i} & a_{i}cos\theta_{i}\\ sin\theta_{i} & cos\theta_{i}cos\alpha_{i} & -cos\theta_{i}sin\alpha_{i} & a_{i}sin\theta_{i}\\ 0 & sin\alpha_{i} & cos\alpha_{i} & d_{i}\\ 0 & 0 & 0 & 1 \end{bmatrix} \end{aligned}

^{0}_{6}T=\begin{bmatrix} n_x & o_x & a_x & p_x\\ n_y & o_y & a_y & p_y\\ n_z & o_z & a_z & p_z\\ 0 & 0 & 0 & 1 \end{bmatrix} ={{^{0}_{1}T}{^{1}_{2}T}{^2_{3}T}{^3_{4}T}{^4_{5}T}{^5_{6}T}}

^{0}_{1}T =\begin{bmatrix} cos\theta_{1} & 0 & sin\theta_{1} & 0\\ sin\theta_{1} & 0 & -cos\theta_{1} & 0\\ 0 & 1 & 0 &d_{1} \\ 0 & 0 & 0 & 1 \end{bmatrix} \tag{1}

^{1}_{2}T =\begin{bmatrix} cos\theta_2 & -sin\theta_{2} & 0 & a_{2}cos\theta_{2}\\ sin\theta_{2} & cos\theta_{2} & 0 & a_{2}sin\theta_{2}\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1 \end{bmatrix} \tag{2}

^{2}_{3}T=\begin{bmatrix} cos\theta_{3} & -sin\theta_{3} & 0 & a_{3}cos\theta_{3}\\ sin\theta_{3} & cos\theta_{3} & 0 & a_{3}sin\theta_{3}\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1 \end{bmatrix} \tag{3}

^{3}_{4}T = \begin{bmatrix} cos\theta_{4} & 0 & sin\theta_{4} & 0\\ sin\theta_{4} & 0 & -cos\theta_{4} & 0\\ 0 & 1 & 0 & d_{4}\\ 0 & 0 & 0 & 1 \end{bmatrix} \tag{4}

^{4}_{5}T =\begin{bmatrix} cos\theta_{5}& 0 & -sin\theta_{5} & 0\\ sin\theta_{5}& 0 & cos\theta_{5} & 0\\ 0& -1 & 0 & d_{5}\\ 0& 0 & 0 & 1 \end{bmatrix} \tag{5}

^{5}_{6}T=\begin{bmatrix} cos\theta_{6} & -sin\theta_{6} & 0 & 0\\ sin\theta_{6} & cos\theta_{6} & 0 & 0\\ 0 & 0 & 1 & d_{6}\\ 0 & 0 & 0 & 1 \end{bmatrix} \tag{6}

^{0}_{6}T=\left[\begin{matrix}r_{11} & r_{12} & r_{13} & p_{x}\\r_{21} & r_{22} & r_{23} & p_{y}\\r_{31} & r_{32} & r_{33} & p_{z}\\0 & 0 & 0 & 1\end{matrix}\right]

下面的三个公式无法完整显示，感觉掘金的公式渲染这一块存在bug，希望能尽快修复这个问题。

\begin{aligned} ^{0}_{6}T &={^{0}_{1}T}{^{1}_{2}T}{^2_{3}T}{^3_{4}T}{^4_{5}T}{^5_{6}T} \\ &=\left[\begin{matrix}\left(\sin{\left(\theta_{1} \right)} \sin{\left(\theta_{5} \right)} + \cos{\left(\theta_{1} \right)} \cos{\left(\theta_{5} \right)} \cos{\left(\theta_{2} + \theta_{3} + \theta_{4} \right)}\right) \cos{\left(\theta_{6} \right)} - \sin{\left(\theta_{6} \right)} \sin{\left(\theta_{2} + \theta_{3} + \theta_{4} \right)} \cos{\left(\theta_{1} \right)} & - \left(\sin{\left(\theta_{1} \right)} \sin{\left(\theta_{5} \right)} + \cos{\left(\theta_{1} \right)} \cos{\left(\theta_{5} \right)} \cos{\left(\theta_{2} + \theta_{3} + \theta_{4} \right)}\right) \sin{\left(\theta_{6} \right)} - \sin{\left(\theta_{2} + \theta_{3} + \theta_{4} \right)} \cos{\left(\theta_{1} \right)} \cos{\left(\theta_{6} \right)} & \sin{\left(\theta_{1} \right)} \cos{\left(\theta_{5} \right)} - \sin{\left(\theta_{5} \right)} \cos{\left(\theta_{1} \right)} \cos{\left(\theta_{2} + \theta_{3} + \theta_{4} \right)} & a_{2} \cos{\left(\theta_{1} \right)} \cos{\left(\theta_{2} \right)} + a_{3} \cos{\left(\theta_{1} \right)} \cos{\left(\theta_{2} + \theta_{3} \right)} + d_{4} \sin{\left(\theta_{1} \right)} + d_{5} \sin{\left(\theta_{2} + \theta_{3} + \theta_{4} \right)} \cos{\left(\theta_{1} \right)} + d_{6} \sin{\left(\theta_{1} \right)} \cos{\left(\theta_{5} \right)} - d_{6} \sin{\left(\theta_{5} \right)} \cos{\left(\theta_{1} \right)} \cos{\left(\theta_{2} + \theta_{3} + \theta_{4} \right)}\\\left(\sin{\left(\theta_{1} \right)} \cos{\left(\theta_{5} \right)} \cos{\left(\theta_{2} + \theta_{3} + \theta_{4} \right)} - \sin{\left(\theta_{5} \right)} \cos{\left(\theta_{1} \right)}\right) \cos{\left(\theta_{6} \right)} - \sin{\left(\theta_{1} \right)} \sin{\left(\theta_{6} \right)} \sin{\left(\theta_{2} + \theta_{3} + \theta_{4} \right)} & \left(- \sin{\left(\theta_{1} \right)} \cos{\left(\theta_{5} \right)} \cos{\left(\theta_{2} + \theta_{3} + \theta_{4} \right)} + \sin{\left(\theta_{5} \right)} \cos{\left(\theta_{1} \right)}\right) \sin{\left(\theta_{6} \right)} - \sin{\left(\theta_{1} \right)} \sin{\left(\theta_{2} + \theta_{3} + \theta_{4} \right)} \cos{\left(\theta_{6} \right)} & - \sin{\left(\theta_{1} \right)} \sin{\left(\theta_{5} \right)} \cos{\left(\theta_{2} + \theta_{3} + \theta_{4} \right)} - \cos{\left(\theta_{1} \right)} \cos{\left(\theta_{5} \right)} & a_{2} \sin{\left(\theta_{1} \right)} \cos{\left(\theta_{2} \right)} + a_{3} \sin{\left(\theta_{1} \right)} \cos{\left(\theta_{2} + \theta_{3} \right)} - d_{4} \cos{\left(\theta_{1} \right)} + d_{5} \sin{\left(\theta_{1} \right)} \sin{\left(\theta_{2} + \theta_{3} + \theta_{4} \right)} - d_{6} \sin{\left(\theta_{1} \right)} \sin{\left(\theta_{5} \right)} \cos{\left(\theta_{2} + \theta_{3} + \theta_{4} \right)} - d_{6} \cos{\left(\theta_{1} \right)} \cos{\left(\theta_{5} \right)}\\\sin{\left(\theta_{6} \right)} \cos{\left(\theta_{2} + \theta_{3} + \theta_{4} \right)} + \sin{\left(\theta_{2} + \theta_{3} + \theta_{4} \right)} \cos{\left(\theta_{5} \right)} \cos{\left(\theta_{6} \right)} & - \sin{\left(\theta_{6} \right)} \sin{\left(\theta_{2} + \theta_{3} + \theta_{4} \right)} \cos{\left(\theta_{5} \right)} + \cos{\left(\theta_{6} \right)} \cos{\left(\theta_{2} + \theta_{3} + \theta_{4} \right)} & - \sin{\left(\theta_{5} \right)} \sin{\left(\theta_{2} + \theta_{3} + \theta_{4} \right)} & a_{2} \sin{\left(\theta_{2} \right)} + a_{3} \sin{\left(\theta_{2} + \theta_{3} \right)} + d_{1} - d_{5} \cos{\left(\theta_{2} + \theta_{3} + \theta_{4} \right)} - d_{6} \sin{\left(\theta_{5} \right)} \sin{\left(\theta_{2} + \theta_{3} + \theta_{4} \right)}\\0 & 0 & 0 & 1\end{matrix}\right] \end{aligned}

^{1}_{6}T_{left}=\left[\begin{matrix}r_{11} \cos{\left(\theta_{1} \right)} + r_{21} \sin{\left(\theta_{1} \right)} & r_{12} \cos{\left(\theta_{1} \right)} + r_{22} \sin{\left(\theta_{1} \right)} & r_{13} \cos{\left(\theta_{1} \right)} + r_{23} \sin{\left(\theta_{1} \right)} & p_{x} \cos{\left(\theta_{1} \right)} + p_{y}sin(\theta_{1})\\r_{31} & r_{32} & r_{33} & - d_{1} + p_{z}\\r_{11} \sin{\left(\theta_{1} \right)} - r_{21} \cos{\left(\theta_{1} \right)} & r_{12} \sin{\left(\theta_{1} \right)} - r_{22} \cos{\left(\theta_{1} \right)} & r_{13} \sin{\left(\theta_{1} \right)} - r_{23} \cos{\left(\theta_{1} \right)} & p_{x} \sin{\left(\theta_{1} \right)} - p_{y} \cos{\left(\theta_{1} \right)}\\0 & 0 & 0 & 1\end{matrix}\right]

^{1}_{6}T_{right}=\left[\begin{matrix}- \sin{\left(\theta_{6} \right)} \sin{\left(\theta_{2} + \theta_{3} + \theta_{4} \right)} + \cos{\left(\theta_{5} \right)} \cos{\left(\theta_{6} \right)} \cos{\left(\theta_{2} + \theta_{3} + \theta_{4} \right)} & - \sin{\left(\theta_{6} \right)} \cos{\left(\theta_{5} \right)} \cos{\left(\theta_{2} + \theta_{3} + \theta_{4} \right)} - \sin{\left(\theta_{2} + \theta_{3} + \theta_{4} \right)} \cos{\left(\theta_{6} \right)} & - \sin{\left(\theta_{5} \right)} \cos{\left(\theta_{2} + \theta_{3} + \theta_{4} \right)} & a_{2} \cos{\left(\theta_{2} \right)} + a_{3} \cos{\left(\theta_{2} + \theta_{3} \right)} + d_{5} \sin{\left(\theta_{2} + \theta_{3} + \theta_{4} \right)} - d_{6} \sin{\left(\theta_{5} \right)} \cos{\left(\theta_{2} + \theta_{3} + \theta_{4} \right)}\\\sin{\left(\theta_{6} \right)} \cos{\left(\theta_{2} + \theta_{3} + \theta_{4} \right)} + \sin{\left(\theta_{2} + \theta_{3} + \theta_{4} \right)} \cos{\left(\theta_{5} \right)} \cos{\left(\theta_{6} \right)} & - \sin{\left(\theta_{6} \right)} \sin{\left(\theta_{2} + \theta_{3} + \theta_{4} \right)} \cos{\left(\theta_{5} \right)} + \cos{\left(\theta_{6} \right)} \cos{\left(\theta_{2} + \theta_{3} + \theta_{4} \right)} & - \sin{\left(\theta_{5} \right)} \sin{\left(\theta_{2} + \theta_{3} + \theta_{4} \right)} & a_{2} \sin{\left(\theta_{2} \right)} + a_{3} \sin{\left(\theta_{2} + \theta_{3} \right)} - d_{5} \cos{\left(\theta_{2} + \theta_{3} + \theta_{4} \right)} - d_{6} \sin{\left(\theta_{5} \right)} \sin{\left(\theta_{2} + \theta_{3} + \theta_{4} \right)}\\\sin{\left(\theta_{5} \right)} \cos{\left(\theta_{6} \right)} & - \sin{\left(\theta_{5} \right)} \sin{\left(\theta_{6} \right)} & \cos{\left(\theta_{5} \right)} & d_{4} + d_{6} \cos{\left(\theta_{5} \right)}\\0 & 0 & 0 & 1\end{matrix}\right]

求解第1个关节

联立公式 $^{1}_{6}T_{left}$ 和 $^{1}_{6}T_{right}$ 中的（3, 3）和（3, 4）元素分别相等，有

\left\{ \begin{matrix} \begin{aligned} r_{13} \sin{\left(\theta_{1} \right)} - r_{23} \cos{\left(\theta_{1} \right)} &= \cos{\left(\theta_{5} \right)} \\ - d_{4} - d_{6} \cos{\left(\theta_{5} \right)} + p_{x} \sin{\left(\theta_{1} \right)} - p_{y} \cos{\left(\theta_{1} \right)}&=0 \end{aligned} \end{matrix} \right .

消除 $\theta_{5}$ ，有

(d_{6}r_{23}-p_{y})cos(\theta_{1})+(p_{x}-d_{6}r_{13})sin(\theta_{1})=d_{4}

令 $A_{1}=d_{6}r_{23}-p_{y}$ ， $B_{1}=p_{x}-d_{6}r_{13}$ ， $C_{1}=d_{4}$

即公式（9）为 $A_{1}cos(\theta_{1})+B_{1}sin(\theta_{1})=C_{1}$

由半角正切公式有

\left\{ \begin{matrix} A_{1} &=\sqrt{A_{1}^{2}+B_{1}^{2}}sin(\alpha) \Rightarrow sin(\alpha)=\frac{A_{1}}{\sqrt{A_{1}^{2}+B_{1}^{2}}}\\ B_{1} &=\sqrt{A_{1}^{2}+B_{1}^{2}}cos(\alpha) \Rightarrow cos(\alpha)=\frac{B_{1}}{\sqrt{A_{1}^{2}+B_{1}^{2}}} \end{matrix} \right .

即

\sqrt{A_{1}^{2}+B_{1}^{2}}sin(\alpha)cos(\theta_{1})+\sqrt{A_{1}^{2}+B_{1}^{2}}cos(\alpha)sin(\theta_{1})=C_{1}

从而有

\sqrt{A_{1}^{2}+B_{1}^{2}}sin(\alpha+\theta_{1})=C_{1} \Rightarrow sin(\alpha+\theta_{1})=\frac{C_{1}}{\sqrt{A_{1}^{2}+B_{1}^{2}}}

cos(\alpha+\theta_{1})=\pm\sqrt{1-sin(\alpha+\theta_{1})^{2}}=\pm\sqrt{\frac{A_{1}^{2}+B_{1}^{2}-C_{1}^{2}}{A_{1}^{2}+B_{1}^{2}}}

综上所述，有

\begin{aligned} \theta_{1}&=Atan2(sin(\alpha+\theta_{1}), cos(\alpha+\theta_{1}))-Atan2(sin(\alpha), cos(\alpha)) \\ &=Atan2(C_{1}, \pm\sqrt{A_{1}^{2}+B_{1}^{2}-C_{1}^{2}})-Atan2(A_{1}, B_{1}) \tag{8} \end{aligned}

求解第5个关节

联立公式 $^{1}_{6}T_{left}$ 和 $^{1}_{6}T_{right}$ 中的（3, 3）元素相等，有

\cos{\left(\theta_{5} \right)}=r_{13} \sin{\left(\theta_{1} \right)} - r_{23} \cos{\left(\theta_{1} \right)}

令 $A_{5}=r_{13}sin(\theta_{1})-r_{23}cos(\theta_{1})$ ，有

cos(\theta_{5})=A_{5} \Rightarrow sin(\theta_{5})=\pm\sqrt{1-A_{5}^{2}}

由半角正切公式，有

\theta_{5}=Atan2(sin(\theta_{5}), cos(\theta_{5}))=Atan2(\pm\sqrt{1-A_{5}^{2}}, A_{5}) \tag{9}

求解第6个关节

联立公式 $^{1}_{6}T_{left}$ 和 $^{1}_{6}T_{right}$ 中的（3, 1）和（3, 2）元素分别相等，有

\left\{ \begin{matrix} \begin{aligned} r_{11} \sin{\left(\theta_{1} \right)} - r_{21} \cos{\left(\theta_{1} \right)} &=\sin{\left(\theta_{5} \right)} \cos{\left(\theta_{6} \right)} \\ r_{12} \sin{\left(\theta_{1} \right)} - r_{22} \cos{\left(\theta_{1} \right)}&=- \sin{\left(\theta_{5} \right)} \sin{\left(\theta_{6} \right)} \end{aligned} \end{matrix} \right.

当 $\theta_{5} \ne 0$ 时，有

\left \{ \begin{matrix} \begin{aligned} A_{6}=sin(\theta_{6}) &= \frac{-r_{12}sin(\theta_{1})+r_{22}cos(\theta_{1})}{sin(\theta_{5})} \\ B_{6}=cos(\theta_{6}) &=\frac{r_{11}sin(\theta_{1})-r_{21}cos(\theta_{1})}{sin(\theta_{5})} \end{aligned} \end{matrix} \right.

由半角正切公式，有

\theta_{6}=Atan2(sin(\theta_{6}), cos(\theta_{6}))=Atan2(A_{6}, B_{6}) \tag{10}

当 $sin(\theta_{5})=0$ 时，机器人处于奇异位形，此时关节6轴与2, 3, 4关节轴平行，机器人存在无数组解。

求解第2个关节

联立公式 $^{1}_{6}T_{left}$ 和 $^{1}_{6}T_{right}$ 中的（1, 3）元素和（2, 3）元素分别相等，有

\left\{ \begin{matrix} \begin{aligned} r_{13} \cos{\left(\theta_{1} \right)} + r_{23} \sin{\left(\theta_{1} \right)} &=- \sin{\left(\theta_{5} \right)} \cos{\left(\theta_{2} + \theta_{3} + \theta_{4} \right)} \\ r_{33} &=- \sin{\left(\theta_{5} \right)} \sin{\left(\theta_{2} + \theta_{3} + \theta_{4} \right)} \end{aligned} \end{matrix} \right.

当 $\theta_{5} \ne 0$ 时，有

\left\{ \begin{matrix} \begin{aligned} A_{234} &= sin(\theta_{2}+\theta_{3}+\theta_{4}) = -\frac{r_{33}}{sin(\theta_{5})} \\ B_{234} &= cos(\theta_{2}+\theta_{3}+\theta_{4}) =-\frac{r_{13}cos(\theta_{1})+r_{23}sin(\theta_{1})}{sin(\theta_{5})} \end{aligned} \end{matrix} \right.

由半角正切公式，有

\theta_{2}+\theta_{3}+\theta_{4}=Atan2(A_{234}, B_{234}) \tag{11}

联立公式 $^{1}_{6}T_{left}$ 和 $^{1}_{6}T_{right}$ 中的（1, 4）元素和（2, 4）元素分别相等，有

\left\{ \begin{matrix} \begin{aligned} p_{x} \cos{\left(\theta_{1} \right)} + p_{y}sin(\theta_{1}) &=a_{2} \cos{\left(\theta_{2} \right)} + a_{3} \cos{\left(\theta_{2} + \theta_{3} \right)} + d_{5} \sin{\left(\theta_{2} + \theta_{3} + \theta_{4} \right)} - d_{6} \sin{\left(\theta_{5} \right)} \cos{\left(\theta_{2} + \theta_{3} + \theta_{4} \right)} \\ - d_{1} + p_{z} &=a_{2} \sin{\left(\theta_{2} \right)} + a_{3} \sin{\left(\theta_{2} + \theta_{3} \right)} - d_{5} \cos{\left(\theta_{2} + \theta_{3} + \theta_{4} \right)} - d_{6} \sin{\left(\theta_{5} \right)} \sin{\left(\theta_{2} + \theta_{3} + \theta_{4} \right)} \end{aligned} \end{matrix} \right.

接着，分别定义 $M$ 和 $N$ ，即

\left\{ \begin{matrix} \begin{aligned} M &= a_{2}cos(\theta_{2})+a_{3}cos(\theta_{2}+\theta_{3}) \\ &= p_{x}cos(\theta_{1})+p_{y}sin(\theta_{1})-d_{5}sin(\theta_{2}+\theta_{3}+\theta_{4})+d_{6}sin(\theta_{5})cos(\theta_{2}+\theta_{3}+\theta_{4})\\ N &= a_{2}sin(\theta_{2})+a_{3}sin(\theta_{2}+\theta_{3}) \\ &= -d_{1}+p_{z}+d_{5}cos(\theta_{2}+\theta_{3}+\theta_{4})+d_{6}sin(\theta_{5})sin(\theta_{2}+\theta_{3}+\theta_{4}) \end{aligned} \end{matrix} \right.

结合 $A_{234}$ 和 $B_{234}$ ，可以计算出 $M$ 和 $N$ 的数值

\left\{ \begin{matrix} \begin{aligned} M^{2} &= a_{2}^{2}cos(\theta_{2})^{2}+a_{3}^{2}cos(\theta_{2}+\theta_{3})^{2}+2a_{2}a_{3}cos(\theta_{2})cos(\theta_{2}+\theta_{3}) \\ N^{2} &= a_{2}^{2}sin(\theta_{2})^{2}+a_{3}^{2}sin(\theta_{2}+\theta_{3})^{2}+2a_{2}a_{3}sin(\theta_{2})sin(\theta_{2}+\theta_{3}) \end{aligned} \end{matrix} \right.

\left\{ \begin{matrix} \begin{aligned} L &= M cos(\theta_{2}) + N sin(\theta_{2}) \\ &= a_{2}cos(\theta_{2})^{2}+a_{3}cos(\theta_{2})cos(\theta_{2}+\theta_{3})+a_{2}sin(\theta_{2})^{2}+a_{3}sin(\theta_{2})sin(\theta_{2}+\theta_{3}) \end{aligned} \end{matrix} \right.

由于

\begin{aligned} M^{2}+N^{2}-a_{2}^{2}-a_{3}^{2}&=2a_{2}(L-a_{2}) \\ &=2a_{2}(Mcos(\theta_{2})+Nsin(\theta_{2})-a_{2}) \end{aligned}

代入 $L$ ，化简可得

L = Mcos(\theta_{2})+Nsin(\theta_{2})=\frac{M^{2}+N^{2}-a_{2}^{2}-a_{3}^{2}}{2a_{2}}+a_{2} = \frac{M^{2}+N^{2}+a_{2}^{2}-a_{3}^{2}}{2a_{2}}

令 $A_{2}=M$ ， $B_{2}=N$ ， $C_{2}=L=\frac{M^{2}+N^{2}+a_{2}^{2}-a_{3}^{2}}{2a_{2}}$

有 $A_{2}cos(\theta_{2})+B_{2}sin(\theta_{2})=C_{2}$

A_{2}=\sqrt{A_{2}+B_{2}}sin(\alpha) \Rightarrow sin(\alpha)=\frac{A_{2}}{\sqrt{A_{2}+B_{2}}}\\ B_{2}=\sqrt{A_{2}+B_{2}}cos(\alpha) \Rightarrow cos(\alpha)=\frac{B_{2}}{\sqrt{A_{2}+B_{2}}}

因此，有

\alpha=Atan2(sin(\alpha), cos(\alpha))=Atan2(A_{2}, B_{2})

\sqrt{A_{2}+B_{2}}sin(\alpha)cos(\theta_{2})+\sqrt{A_{2}+B_{2}}cos(\alpha)sin(\theta_{2})=C_{2}

即

\sqrt{A_{2}+B_{2}}sin(\alpha+\theta_{2})=C_{2}

sin(\alpha+\theta_{2})=\frac{C_{2}}{\sqrt{A_{2}+B_{2}}}

cos(\alpha+\theta_{2})=\pm\frac{\sqrt{A_{2}+B_{2}-C_{2}}}{\sqrt{A_{2}+B_{2}}}

\begin{aligned} \theta_{2}&=Atan2(sin(\alpha+\theta_{2}), cos(\alpha+\theta_{2}))-Atan2(sin(\alpha), cos(\alpha))\\ &=Atan2(C_{2}, \pm\sqrt{A_{2}+B_{2}-C_{2}})-Atan2(A_{2}, B_{2}) \end{aligned}

求解第3个关节

由于

\left\{ \begin{matrix} \begin{aligned} M &= a_{2}cos(\theta_{2})+a_{3}cos(\theta_{2}+\theta_{3}) \\ N &= a_{2}sin(\theta_{2})+a_{3}sin(\theta_{2}+\theta_{3}) \end{aligned} \end{matrix} \right.

有

\left\{ \begin{matrix} \begin{aligned} A_{23} &=sin(\theta_{2}+\theta_{3}) = \frac{N-a_{2}sin(\theta_{2})}{a_{3}} \\ B_{23} &=cos(\theta_{2}+\theta_{3}) = \frac{M-a_{2}cos(\theta_{2}) }{a_{3}} \end{aligned} \end{matrix} \right.

由半角正切公式，有

\theta_{2}+\theta_{3}=Atan2(sin(\theta_{2}+\theta_{3}), cos(\theta_{2}+\theta_{3}))=Atan2(A_{23}, B_{23})

故

\theta_{3} = Atan2(A_{23}, B_{23}) - \theta_{2}

求解第4个关节

\theta_{4}=Atan2(A_{234}, B_{234}) -Atan2(A_{23}, B_{23})

至此，UR机器人的所有6个关节的求解公式推导完毕。下一篇文章，我将重点介绍如何基于本文推导的公式，使用C++编程实现UR机器人的封闭逆解算法。