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

2 篇文章 0 订阅

## IRB2600的标准DH法

IRB2600的标准DH参数表：

100 d 1 ( 445 ) d_{1}(445) θ 1 \theta_{1}
2 − 9 0 ∘ -90^{\circ} a 1 ( 150 ) a_{1}(150) 0 θ 2 + 9 0 ∘ \theta_{2}+90^{\circ}
30 a 2 ( − 700 ) a_{2}(-700) 0 θ 3 \theta_{3}
4 9 0 ∘ 90^{\circ} a 3 ( − 115 ) a_{3}(-115) d 4 ( 795 ) d_{4}(795) θ 4 \theta_{4}
5 − 9 0 ∘ -90^{\circ} 00 θ 5 \theta_{5}
6 9 0 ∘ 90^{\circ} 0 d 6 ( 85 ) d_{6}(85) θ 6 \theta_{6}

6 0 T = [ 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 ] = 1 0 T 2 1 T 3 2 T 4 3 T 5 4 T 6 5 T (10) ^{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}

1 0 T = [ c o s ( θ 1 ) − s i n ( θ 1 ) 0 0 s i n ( θ 1 ) c o s ( θ 1 ) 0 0 0 0 1 d 1 0 0 0 1 ] (11) ^{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}

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

3 2 T = [ c o s ( θ 3 ) − s i n ( θ 3 ) 0 a 2 s i n ( θ 3 ) c o s ( θ 3 ) 0 0 0 0 1 0 0 0 0 1 ] (13) ^{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}

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

5 4 T = [ c o s ( θ 5 ) − s i n ( θ 5 ) 0 0 0 0 1 0 − s i n ( θ 5 ) − c o s ( θ 5 ) 0 0 0 0 0 1 ] (15) ^{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}

6 5 T = [ c o s ( θ 6 ) − s i n ( θ 6 ) 0 0 0 0 − 1 − d 6 s i n ( θ 6 ) c o s ( θ 6 ) 0 0 0 0 0 1 ] (16) ^{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}

## IRB2600的逆运动学解析解推算

### 推算 θ 1 \theta_{1} :

T l e f t ( 2 , 4 ) T l e f t ( 2 , 3 ) = T r i g h t ( 2 , 4 ) T r i g h t ( 2 , 3 ) (1) \displaystyle\frac{T_{left}(2,4)}{T_{left}(2,3)}=\frac{T_{right}(2,4)}{T_{right}(2,3)}\tag{1}

θ 1 = a r c t a n a y d 6 − p y a x d 6 − p x (2) \displaystyle\theta_{1}=arctan\frac{a_yd_6-p_y}{a_xd_6-p_x}\tag2

### 推算 θ 2 \theta_2 和 θ 3 \theta_3 :

θ 2 \theta_2 θ 3 \theta_3 可以由一个等式关系确定： T e n d ( T 4 5 T 5 6 ) − 1 = T 0 1 T 1 2 T 2 3 T 3 4 T_{end}(T^5_4T^6_5)^{-1}=T^1_0T^2_1T^3_2T^4_3 ，首先为确定 θ 2 \theta_2 关于 θ 1 \theta_1 的关系式，通过矩阵中如下元素的等式关系：
T l e f t ( 2 , 4 ) = T r i g h t ( 2 , 4 ) (3) \displaystyle T_{left}(2,4)=T_{right}(2,4) \tag3
T l e f t ( 3 , 4 ) = T r i g h t ( 3 , 4 ) (4) \displaystyle T_{left}(3,4)=T_{right}(3,4) \tag4

a 3 s i n ( θ 2 + θ 3 ) − d 4 c o s ( θ 2 + θ 3 ) = a 1 − a 2 s i n θ 2 − p y − d 6 a y s i n θ 1 (5) \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
d 4 s i n ( θ 2 + θ 3 ) + a 3 c o s ( θ 2 + θ 3 ) = d 1 − a 2 c o s ( θ 2 ) − ( p z − d 6 a z ) (6) d_4sin(\theta_2+\theta_3)+a_3cos(\theta_2+\theta_3)=d_1-a_2cos(\theta_2)-(p_z-d_6a_z) \tag6

a 3 2 + d 4 2 = ( k 1 − a 2 s i n θ 2 ) 2 + ( k 2 − a 2 c o s θ 2 ) 2 (7) \displaystyle {a_3}^2+{d_4}^2=(k_1-a_2sin\theta_2)^2+(k_2-a_2cos\theta_2)^2 \tag7

k 1 = a 1 − p y − d 6 a y s i n θ 1 (8) \displaystyle k_1=a_1-\frac{p_y-d_6a_y}{sin\theta_1} \tag8
k 2 = d 1 − ( p z − d 6 a z ) (9) \displaystyle k_2=d_1-(p_z-d_6a_z) \tag9

θ 2 = a r c s i n k 1 2 + k 2 2 + a 2 2 − ( a 3 2 + d 4 2 ) 2 a 2 k 1 2 + k 2 2 − ϕ (10) \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}
s i n ϕ = k 2 k 1 2 + k 2 2 ， c o s ϕ = k 1 k 1 2 + k 2 2 (11) \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}

X = s i n ( θ 2 + θ 3 ) = a 3 A + d 4 B a 3 2 + d 4 2 (12) \displaystyle X=sin(\theta_2+\theta_3)=\frac {a_3A+d_4B}{{a_3}^2+{d_4}^2} \tag{12}
Y = c o s ( θ 2 + θ 3 ) = a 3 B − d 4 A a 3 2 + d 4 2 (13) \displaystyle Y=cos(\theta_2+\theta_3)=\frac {a_3B-d_4A}{{a_3}^2+{d_4}^2} \tag{13}

A = k 1 − a 2 s i n θ 2 ， B = k 2 − a 2 c o s θ 2 (14) \displaystyle A = k_1-a_2sin\theta_2 ，B=k_2-a_2cos\theta_2 \tag{14}

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

T l e f t ( 2 , 3 ) = T r i g h t ( 2 , 3 ) (15) \displaystyle T_{left}(2,3)=T_{right}(2,3) \tag{15}

θ 5 = a r c c o s ( a x c o s ( θ 2 + θ 3 ) c o s θ 1 + a y c o s ( θ 2 + θ 3 ) s i n θ 1 − a z s i n ( θ 2 + θ 3 ) ) (16) \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}

T l e f t ( 1 , 3 ) = T r i g h t ( 1 , 3 ) (17) T_{left}(1,3)=T_{right}(1,3) \tag{17}
T l e f t ( 3 , 3 ) = T r i g h t ( 3 , 3 ) (18) T_{left}(3,3)=T_{right}(3,3) \tag{18}

s i n θ 4 = o y c o s θ 1 − o x s i n θ 1 s i n θ 5 (19) \displaystyle sin\theta_4=\frac{o_ycos\theta_1-o_xsin\theta_1}{sin\theta_5} \tag{19}
c o s θ 4 = − o z c o s ( θ 2 + θ 3 ) − o x s i n ( θ 2 + θ 3 ) c o s θ 1 − o y s i n ( θ 2 + θ 3 ) s i n θ 1 s i n θ 5 (20) \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}

T l e f t ( 2 , 2 ) = T r i g h t ( 2 , 2 ) (21) T_{left}(2,2)=T_{right}(2,2) \tag{21}
T l e f t ( 2 , 1 ) = T r i g h t ( 2 , 1 ) (22) T_{left}(2,1)=T_{right}(2,1) \tag{22}

s i n θ 6 = o x c o s ( θ 2 + θ 3 ) c o s θ 1 + o y c o s ( θ 2 + θ 3 ) s i n θ 1 − o z s i n ( θ 2 + θ 3 ) s i n θ 5 (23) \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}
c o s θ 6 = n z s i n ( θ 2 + θ 3 ) − n x c o s ( θ 2 + θ 3 ) c o s θ 1 − n y c o s ( θ 2 + θ 3 ) s i n θ 1 s i n θ 5 (24) \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}

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

J i a b i n P a n JiabinPan 原创文章，禁止抄袭，转载请注明出处，谢谢

• 8
点赞
• 6
评论
• 28
收藏
• 一键三连
• 扫一扫，分享海报

04-07
09-26

05-19 1万+
03-19
07-06