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

IRB2600的标准DH法

建立IRB2600的标准DH模型：

标准DH法中相邻坐标系之间的齐次变换矩阵：

IRB2600的标准DH参数表：

轴号$i$i	$\alpha_{i-1}$αi−1​	$a_{i-1}$ai−1​	$d_{i}$di​	$\theta_{i}$θi​
1	0	0	$d_{1}(445)$d1​(445)	$\theta_{1}$θ1​
2	$-90^{\circ}$−90∘	$a_{1}(150)$a1​(150)	0	$\theta_{2}+90^{\circ}$θ2​+90∘
3	0	$a_{2}(-700)$a2​(−700)	0	$\theta_{3}$θ3​
4	$90^{\circ}$90∘	$a_{3}(-115)$a3​(−115)	$d_{4}(795)$d4​(795)	$\theta_{4}$θ4​
5	$-90^{\circ}$−90∘	0	0	$\theta_{5}$θ5​
6	$90^{\circ}$90∘	0	$d_{6}(85)$d6​(85)	$\theta_{6}$θ6​

$^{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}} \tag{10}$60​T=⎣⎢⎢⎡​nx​ny​nz​0​ox​oy​oz​0​ax​ay​az​0​px​py​pz​1​⎦⎥⎥⎤​=10​T21​T32​T43​T54​T65​T(10)
其中：
$^{0}_{1}T=\begin{bmatrix} cos(\theta_{1}) & -sin(\theta_{1}) & 0 & 0\\ sin(\theta_{1}) & cos(\theta_{1}) & 0 & 0\\ 0 & 0 & 1 & d_{1}\\ 0 & 0 & 0 & 1 \end{bmatrix} \tag{11}$10​T=⎣⎢⎢⎡​cos(θ1​)sin(θ1​)00​−sin(θ1​)cos(θ1​)00​0010​00d1​1​⎦⎥⎥⎤​(11)

KaTeX parse error: No such environment: align at position 8: \begin{̲a̲l̲i̲g̲n̲}̲ ^{1}_{2}T &=…

$^{2}_{3}T=\begin{bmatrix} cos(\theta_{3}) & -sin(\theta_{3}) & 0 & a_{2}\\ sin(\theta_{3}) & cos(\theta_{3}) & 0 & 0\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1 \end{bmatrix} \tag{13}$32​T=⎣⎢⎢⎡​cos(θ3​)sin(θ3​)00​−sin(θ3​)cos(θ3​)00​0010​a2​001​⎦⎥⎥⎤​(13)

KaTeX parse error: No such environment: align at position 8: \begin{̲a̲l̲i̲g̲n̲}̲ ^{3}_{4}T &=\b…

$^{4}_{5}T=\begin{bmatrix} cos(\theta_{5}) & -sin(\theta_{5}) & 0 & 0\\ 0 & 0 & 1 & 0\\ -sin(\theta_{5}) & -cos(\theta_{5}) & 0 & 0\\ 0 & 0 & 0 & 1 \end{bmatrix} \tag{15}$54​T=⎣⎢⎢⎡​cos(θ5​)0−sin(θ5​)0​−sin(θ5​)0−cos(θ5​)0​0100​0001​⎦⎥⎥⎤​(15)

$^{5}_{6}T=\begin{bmatrix} cos(\theta_{6}) & -sin(\theta_{6}) & 0 & 0\\ 0 & 0 & -1 & -d_{6}\\ sin(\theta_{6}) & cos(\theta_{6}) & 0 & 0\\ 0 & 0 & 0 & 1 \end{bmatrix} \tag{16}$65​T=⎣⎢⎢⎡​cos(θ6​)0sin(θ6​)0​−sin(θ6​)0cos(θ6​)0​0−100​0−d6​01​⎦⎥⎥⎤​(16)

IRB2600的逆运动学解析解推算

推算$\theta_{1}$θ1​:

利用关系式：$(T^1_0)^{-1}T_{end}=T^2_1T^3_2T^4_3T^5_4T^6_5$(T01​)−1Tend​=T12​T23​T34​T45​T56​对$\theta_1$θ1​进行推算，通过对比矩阵元素之间关系，可按照如下过程求解$\theta_1$θ1​
$\displaystyle\frac{T_{left}(2,4)}{T_{left}(2,3)}=\frac{T_{right}(2,4)}{T_{right}(2,3)}\tag{1}$Tleft​(2,3)Tleft​(2,4)​=Tright​(2,3)Tright​(2,4)​(1)
化简得到：
$\displaystyle\theta_{1}=arctan\frac{a_yd_6-p_y}{a_xd_6-p_x}\tag2$θ1​=arctanax​d6​−px​ay​d6​−py​​(2)
对于$\theta_1$θ1​的计算，将会产生两个解。

推算$\theta_2$θ2​和$\theta_3$θ3​:

$\theta_2$θ2​和$\theta_3$θ3​可以由一个等式关系确定：$T_{end}(T^5_4T^6_5)^{-1}=T^1_0T^2_1T^3_2T^4_3$Tend​(T45​T56​)−1=T01​T12​T23​T34​，首先为确定$\theta_2$θ2​关于$\theta_1$θ1​的关系式，通过矩阵中如下元素的等式关系：
$\displaystyle T_{left}(2,4)=T_{right}(2,4) \tag3$Tleft​(2,4)=Tright​(2,4)(3)
$\displaystyle T_{left}(3,4)=T_{right}(3,4) \tag4$Tleft​(3,4)=Tright​(3,4)(4)
化简得：
$\displaystyle a_3sin(\theta_2+\theta_3)-d_4cos(\theta_2+\theta_3)=a_1-a_2sin\theta_2-\frac{p_y-d_6a_y}{sin\theta_1} \tag5$a3​sin(θ2​+θ3​)−d4​cos(θ2​+θ3​)=a1​−a2​sinθ2​−sinθ1​py​−d6​ay​​(5)
$d_4sin(\theta_2+\theta_3)+a_3cos(\theta_2+\theta_3)=d_1-a_2cos(\theta_2)-(p_z-d_6a_z) \tag6$d4​sin(θ2​+θ3​)+a3​cos(θ2​+θ3​)=d1​−a2​cos(θ2​)−(pz​−d6​az​)(6)
为化简需要，令$X=sin(\theta_2+\theta_3)$X=sin(θ2​+θ3​)和$Y=cos(\theta_2+\theta_3)$Y=cos(θ2​+θ3​)，通过式(5)和(6)联立起来可以消除$X$X和$Y$Y，从而得到：
$\displaystyle {a_3}^2+{d_4}^2=(k_1-a_2sin\theta_2)^2+(k_2-a_2cos\theta_2)^2 \tag7$a3​2+d4​2=(k1​−a2​sinθ2​)2+(k2​−a2​cosθ2​)2(7)
其中：
$\displaystyle k_1=a_1-\frac{p_y-d_6a_y}{sin\theta_1} \tag8$k1​=a1​−sinθ1​py​−d6​ay​​(8)
$\displaystyle k_2=d_1-(p_z-d_6a_z) \tag9$k2​=d1​−(pz​−d6​az​)(9)
观察发现，式(7)中只有$\theta_1$θ1​和$\theta_2$θ2​未知量，由此可建立$\theta_2$θ2​关于$\theta_1$θ1​的关系表达式，即$\theta_2$θ2​的解析解：
$\displaystyle \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}}-\phi \tag{10}$θ2​=arcsin2a2​k1​2+k2​2​k1​2+k2​2+a2​2−(a3​2+d4​2)​−ϕ(10)
$\displaystyle sin\phi = \frac{k_2}{\sqrt{{k_1}^2+{k_2}^2}} ，cos\phi = \frac{k_1}{\sqrt{{k_1}^2+{k_2}^2}} \tag{11}$sinϕ=k1​2+k2​2​k2​​，cosϕ=k1​2+k2​2​k1​​(11)
对于$\theta_2$θ2​的计算，亦会产生两个解。

以上求出了$\theta_1$θ1​和$\theta_2$θ2​，将式(5)(6)中$X$X和$Y$Y作为未知数，求出：
$\displaystyle X=sin(\theta_2+\theta_3)=\frac {a_3A+d_4B}{{a_3}^2+{d_4}^2} \tag{12}$X=sin(θ2​+θ3​)=a3​2+d4​2a3​A+d4​B​(12)
$\displaystyle Y=cos(\theta_2+\theta_3)=\frac {a_3B-d_4A}{{a_3}^2+{d_4}^2} \tag{13}$Y=cos(θ2​+θ3​)=a3​2+d4​2a3​B−d4​A​(13)
其中：
$\displaystyle A = k_1-a_2sin\theta_2 ，B=k_2-a_2cos\theta_2 \tag{14}$A=k1​−a2​sinθ2​，B=k2​−a2​cosθ2​(14)
结合式(12)(13)(14)和已算出的$\theta_1 \theta_2$θ1​θ2​可以计算出关节角$\theta_3$θ3​。

推算$\theta_{5}$θ5​、$\theta_4$θ4​和$\theta_6$θ6​:

经过几次推算尝试，观察矩阵元素的特征（$T_{right}$Tright​矩阵中元素表达式只含$\theta_4\theta_5\theta_6$θ4​θ5​θ6​且简单，$T_{left}$Tleft​矩阵中只含$\theta_1\theta_2\theta_3$θ1​θ2​θ3​且已求得），因此确定关系式：$(T^1_0T^2_1T^3_2)^{-1}T_{end}=T^4_3T^5_4T^6_5$(T01​T12​T23​)−1Tend​=T34​T45​T56​为推导$\theta_4\theta_5\theta_6$θ4​θ5​θ6​的等式关系，根据矩阵中元素等式关系：
$\displaystyle T_{left}(2,3)=T_{right}(2,3) \tag{15}$Tleft​(2,3)=Tright​(2,3)(15)
可以直接求得$\theta_5$θ5​：
$\displaystyle \theta_5=arccos(a_xcos(\theta_2+\theta_3)cos\theta_1+a_ycos(\theta_2+\theta_3)sin\theta_1-a_zsin(\theta_2+\theta_3)) \tag{16}$θ5​=arccos(ax​cos(θ2​+θ3​)cosθ1​+ay​cos(θ2​+θ3​)sinθ1​−az​sin(θ2​+θ3​))(16)
用该解析解计算$\theta_5$θ5​，会得到两个解。

为求得$\theta_4$θ4​，可建立如下关系式：
$T_{left}(1,3)=T_{right}(1,3) \tag{17}$Tleft​(1,3)=Tright​(1,3)(17)
$T_{left}(3,3)=T_{right}(3,3) \tag{18}$Tleft​(3,3)=Tright​(3,3)(18)
化简得：
$\displaystyle sin\theta_4=\frac{o_ycos\theta_1-o_xsin\theta_1}{sin\theta_5} \tag{19}$sinθ4​=sinθ5​oy​cosθ1​−ox​sinθ1​​(19)
$\displaystyle cos\theta_4=\frac{-o_zcos(\theta_2+\theta_3)-o_xsin(\theta_2+\theta_3)cos\theta_1-o_ysin(\theta_2+\theta_3)sin\theta_1}{sin\theta_5} \tag{20}$cosθ4​=sinθ5​−oz​cos(θ2​+θ3​)−ox​sin(θ2​+θ3​)cosθ1​−oy​sin(θ2​+θ3​)sinθ1​​(20)
如此可以确定解$\theta_4$θ4​。

利用矩阵元素对应相等关系：
$T_{left}(2,2)=T_{right}(2,2) \tag{21}$Tleft​(2,2)=Tright​(2,2)(21)
$T_{left}(2,1)=T_{right}(2,1) \tag{22}$Tleft​(2,1)=Tright​(2,1)(22)
化简为：
$\displaystyle sin\theta_6=\frac{o_xcos(\theta_2+\theta_3)cos\theta_1+o_ycos(\theta_2+\theta_3)sin\theta_1-o_zsin(\theta_2+\theta_3)}{sin\theta_5} \tag{23}$sinθ6​=sinθ5​ox​cos(θ2​+θ3​)cosθ1​+oy​cos(θ2​+θ3​)sinθ1​−oz​sin(θ2​+θ3​)​(23)
$\displaystyle cos\theta_6=\frac{n_zsin(\theta_2+\theta_3)-n_xcos(\theta_2+\theta_3)cos\theta_1-n_ycos(\theta_2+\theta_3)sin\theta_1}{sin\theta_5} \tag{24}$cosθ6​=sinθ5​nz​sin(θ2​+θ3​)−nx​cos(θ2​+θ3​)cosθ1​−ny​cos(θ2​+θ3​)sinθ1​​(24)
由式(23)(24)可求得$\theta_6$θ6​，其解与$\theta_5$θ5​唯一对应。

——————————————————————————-——————————————————————

$JiabinPan$JiabinPan 原创文章，禁止抄袭，转载请注明出处，谢谢