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

IRB2600的改进D-H法

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

Fig.1 ABB IRB2600 coordinate system

IRB2600的改进D-H参数表：

则机器人的运动学可以公式为：
T 0 6 = [ 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 ] = T 1 0 ⋅ T 2 1 ⋅ T 3 2 ⋅ T 4 3 ⋅ T 5 4 ⋅ T 6 5 (1) \mathbf{T}^{6}_{0}=

$$\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}$$ ={{\mathbf{T}^{0}_{1}}\cdot{\mathbf{T}^{1}_{2}}\cdot{\mathbf{T}^2_{3}}\cdot{\mathbf{T}^3_{4}}\cdot{\mathbf{T}^4_{5}}\cdot{\mathbf{T}^5_{6}}} \tag{1} T06​= ​nx​ny​nz​0​ox​oy​oz​0​ax​ay​az​0​px​py​pz​1​ ​=T10​⋅T21​⋅T32​⋅T43​⋅T54​⋅T65​(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