機器人正逆運動學分析（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​/span>+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​