机器人正逆运动学分析（ABB-IRB2600）

IRB2600的改进D-H法

建立IRB2600的改进D-H模型：

Fig.1 ABB IRB2600 coordinate system

IRB2600的改进D-H参数表：

轴号$i$	${\alpha }_{i-1}$	$a_{i-1}$	$d_i$	${\theta }_i$
1	0	0	$d_1(445)$	${\theta }_1$
2	$-90^{\circ }$	$a_1(150)$	0	${\theta }_2+90^{\circ }$
3	0	$a_2(-700)$	0	${\theta }_3$
4	$90^{\circ }$	$a_3(-115)$	$d_4(795)$	${\theta }_4$
5	$-90^{\circ }$	0	0	${\theta }_5$
6	$90^{\circ }$	0	$d_6(85)$	${\theta }_6$

则机器人的运动学可以公式为：
$${\mathbf{T}}_0^6=\left[\begin{array}{cccc}{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{array}\right]={\mathbf{T}}_1^0\cdot {\mathbf{T}}_2^1\cdot {\mathbf{T}}_3^2\cdot {\mathbf{T}}_4^3\cdot {\mathbf{T}}_5^4\cdot {\mathbf{T}}_6^5\text{\hspace{1em}(1)}$$
其中：
$${\mathbf{T}}_{i-1}^i=\left[\begin{array}{cccc}\cos ({\theta }_i)& -\sin ({\theta }_i)& 0& a_{i-1}\\ \sin ({\theta }_i)\cos ({\alpha }_{i-1})& \cos ({\theta }_i)\cos ({\alpha }_{i-1})& -\sin ({\alpha }_{i-1})& -d_i\sin ({\alpha }_{i-1})\\ \sin ({\theta }_i)\sin ({\alpha }_{i-1})& \cos ({\theta }_i)\sin ({\alpha }_{i-1})& \cos ({\alpha }_{i-1})& d_i\cos ({\alpha }_{i-1})\\ 0& 0& 0& 1\end{array}\right]\text{\hspace{1em}(2)}$$

IRB2600的逆运动学解析解推算

推算${\theta }_1$:

利用关系式：$({\mathbf{T}}_0^1{)}^{-1}\cdot {\mathbf{T}}_{end}={\mathbf{T}}_1^2\cdot {\mathbf{T}}_2^3\cdot {\mathbf{T}}_3^4\cdot {\mathbf{T}}_4^5\cdot {\mathbf{T}}_5^6$对${\theta }_1$进行推算，通过对比矩阵元素之间关系，可按照如下过程求解${\theta }_1$
$$\frac{{\mathbf{T}}_{left}(2,4)}{{\mathbf{T}}_{left}(2,3)}=\frac{{\mathbf{T}}_{right}(2,4)}{{\mathbf{T}}_{right}(2,3)}\text{\hspace{1em}(3)}$$
化简得到：
$${\theta }_1=arctan\frac{a_y{d}_6-p_y}{a_x{d}_6-p_x}\text{\hspace{1em}(4)}$$
对于${\theta }_1$的计算，将会产生两个解。

推算${\theta }_2$和${\theta }_3$:

${\theta }_2$和${\theta }_3$可以由一个等式关系确定：${\mathbf{T}}_{end}\cdot ({\mathbf{T}}_4^5\cdot {\mathbf{T}}_5^6{)}^{-1}={\mathbf{T}}_0^1\cdot {\mathbf{T}}_1^2\cdot {\mathbf{T}}_2^3\cdot {\mathbf{T}}_3^4$，首先为确定${\theta }_2$关于${\theta }_1$的关系式，通过矩阵中如下元素的等式关系：
$${\mathbf{T}}_{left}(2,4)={\mathbf{T}}_{right}(2,4)\text{\hspace{1em}(5)}$$

$${\mathbf{T}}_{left}(3,4)={\mathbf{T}}_{right}(3,4)\text{\hspace{1em}(6)}$$

化简得：
$$a_{3}sin({\theta }_2+{\theta }_3)-d_{4}cos({\theta }_2+{\theta }_3)=a_1-a_{2}sin{\theta }_2-\frac{p_y-d_6{a}_y}{sin{\theta }_1}\text{\hspace{1em}(7)}$$

$$d_{4}sin({\theta }_2+{\theta }_3)+a_{3}cos({\theta }_2+{\theta }_3)=d_1-a_{2}cos({\theta }_2)-(p_z-d_6{a}_z)\text{\hspace{1em}(8)}$$

为化简需要，令$X=sin({\theta }_2+{\theta }_3)$和$Y=cos({\theta }_2+{\theta }_3)$，通过式(7)和(8)联立起来可以消除$X$和$Y$，从而得到：
$${a_3}^2+{d_4}^2=(k_1-a_{2}sin{\theta }_2{)}^2+(k_2-a_{2}cos{\theta }_2{)}^2\text{\hspace{1em}(9)}$$
其中：
$$k_1=a_1-\frac{p_y-d_6{a}_y}{sin{\theta }_1}\text{\hspace{1em}(10)}$$

$$k_2=d_1-(p_z-d_6{a}_z)\text{\hspace{1em}(11)}$$

观察发现，式(9)中只有${\theta }_1$和${\theta }_2$未知量，由此可建立${\theta }_2$关于${\theta }_1$的关系表达式，即${\theta }_2$的解析解：
$${\theta }_2=arcsin\frac{{k_1}^2+{k_2}^2+{a_2}^2-({a_3}^2+{d_4}^2)}{2a_2\sqrt{{k_1}^2+{k_2}^2}}-\varphi \text{\hspace{1em}(12)}$$

$$sin\varphi =\frac{k_2}{\sqrt{{k_1}^2+{k_2}^2}}，cos\varphi =\frac{k_1}{\sqrt{{k_1}^2+{k_2}^2}}\text{\hspace{1em}(13)}$$

对于${\theta }_2$的计算，亦会产生两个解。

以上求出了${\theta }_1$和${\theta }_2$，将式(7)(8)中$X$和$Y$作为未知数，求出：
$$X=sin({\theta }_2+{\theta }_3)=\frac{a_{3}A+d_{4}B}{{a_3}^2+{d_4}^2}\text{\hspace{1em}(14)}$$

$$Y=cos({\theta }_2+{\theta }_3)=\frac{a_{3}B-d_{4}A}{{a_3}^2+{d_4}^2}\text{\hspace{1em}(15)}$$

其中：
$$A=k_1-a_{2}sin{\theta }_2，B=k_2-a_{2}cos{\theta }_2\text{\hspace{1em}(16)}$$
结合式(14)(15)(16)和已算出的${\theta }_1{\theta }_2$可以计算出关节角${\theta }_3$。

推算${\theta }_5$、${\theta }_4$和${\theta }_6$:

经过几次推算尝试，观察矩阵元素的特征（${\mathbf{T}}_{right}$矩阵中元素表达式只含${\theta }_4{\theta }_5{\theta }_6$且简单，${\mathbf{T}}_{left}$矩阵中只含${\theta }_1{\theta }_2{\theta }_3$且已求得），因此确定关系式：$({\mathbf{T}}_0^1\cdot {\mathbf{T}}_1^2\cdot {\mathbf{T}}_2^3{)}^{-1}\cdot {\mathbf{T}}_{end}={\mathbf{T}}_3^4\cdot {\mathbf{T}}_4^5\cdot {\mathbf{T}}_5^6$为推导${\theta }_4{\theta }_5{\theta }_6$的等式关系，根据矩阵中元素等式关系：
$${\mathbf{T}}_{left}(2,3)={\mathbf{T}}_{right}(2,3)\text{\hspace{1em}(17)}$$
可以直接求得${\theta }_5$：
$${\theta }_5=arccos(a_{x}cos({\theta }_2+{\theta }_3)cos{\theta }_1+a_{y}cos({\theta }_2+{\theta }_3)sin{\theta }_1-a_{z}sin({\theta }_2+{\theta }_3))\text{\hspace{1em}(18)}$$
用该解析解计算${\theta }_5$，会得到两个解。

为求得${\theta }_4$，可建立如下关系式：
$${\mathbf{T}}_{left}(1,3)={\mathbf{T}}_{right}(1,3)\text{\hspace{1em}(19)}$$

$${\mathbf{T}}_{left}(3,3)={\mathbf{T}}_{right}(3,3)\text{\hspace{1em}(20)}$$

化简得：
$$sin{\theta }_4=\frac{a_{y}cos{\theta }_1-a_{x}sin{\theta }_1}{sin{\theta }_5}\text{\hspace{1em}(21)}$$

$$cos{\theta }_4=\frac{-a_{z}cos({\theta }_2+{\theta }_3)-a_{x}sin({\theta }_2+{\theta }_3)cos{\theta }_1-a_{y}sin({\theta }_2+{\theta }_3)sin{\theta }_1}{sin{\theta }_5}\text{\hspace{1em}(22)}$$

如此可以确定解${\theta }_4$。

利用矩阵元素对应相等关系：
$${\mathbf{T}}_{left}(2,2)={\mathbf{T}}_{right}(2,2)\text{\hspace{1em}(23)}$$

$${\mathbf{T}}_{left}(2,1)={\mathbf{T}}_{right}(2,1)\text{\hspace{1em}(24)}$$

化简为：
$$sin{\theta }_6=\frac{o_{x}cos({\theta }_2+{\theta }_3)cos{\theta }_1+o_{y}cos({\theta }_2+{\theta }_3)sin{\theta }_1-o_{z}sin({\theta }_2+{\theta }_3)}{sin{\theta }_5}\text{\hspace{1em}(25)}$$

$$cos{\theta }_6=\frac{n_{z}sin({\theta }_2+{\theta }_3)-n_{x}cos({\theta }_2+{\theta }_3)cos{\theta }_1-n_{y}cos({\theta }_2+{\theta }_3)sin{\theta }_1}{sin{\theta }_5}\text{\hspace{1em}(26)}$$

由式(25)(26)可求得${\theta }_6$，其解与${\theta }_5$唯一对应。

————————————————————————————————————————————————

$JiabinPan$ 原创文章，禁止抄袭，转载请注明出处，谢谢
如果可以的话希望大家投稿论文时可以引用一下我的文章，哈哈，谢谢

[1] Pan, J., Fu, Z., Xiong, J. et al. RobMach: G-Code-based off-line programming for robotic machining trajectory generation. Int J Adv Manuf Technol 118, 2497–2511 (2022). https://doi.org/10.1007/s00170-021-08082-3