前往我的博客阅读体验更佳：本文链接
UR3工业机器人是一个6自由度的连杆机器人，其结构图如下图所示：
1 正运动学分析
1.1 建立机器人连杆坐标系
UR3机器人属于连杆机器人，对于一个连杆机器人来说，可以用四个参数：连杆长度 a i a_i ai，连杆扭角 α i \alpha_i αi，连杆距离 d i d_i di，关节转角 θ i \theta_i θi来描述。这四个参数即为连杆机器人的DH参数，在不同类型的连杆坐标系下，DH参数的含义有所不同。
连杆坐标系的建立有标准型DH法（SDH）建模和改进型DH法（MDH）建模两种方法，下面分别使用两种方法建立UR3机器人的连杆坐标系。
1.1.1 标准型DH法（SDH）建模
标准型DH法建模规则如下：
-
找出各关节轴，并画出这些轴线的延长线；
-
找 Z Z Z轴：与轴线重合， Z i − 1 Z_{i-1} Zi−1轴为关节 i i i的运动轴；
-
找 X X X轴：关节轴线相离时，在公垂线分析上定义 X X X轴，如果 a i a_i ai表示 Z i − 1 Z_{i-1} Zi−1与 Z Z Z之间的公垂线，则 X i X_i Xi的方向沿 a i a_i ai；关节轴线平行时，选取前一关节的公垂线共线的一条公垂线作为 X X X轴；关节轴线相交时， X X X的方向与两周的叉积方向相同。
根据此建模规则，有SDH建模示意图如图1.1所示：
在该坐标系下，各DH参数的含义为：
- a i a_i ai：关节轴线 i − 1 i-1 i−1和关节轴线 i i i的公垂线长度；
- α i \alpha_i αi：关节轴线 i − 1 i-1 i−1和关节轴线 i i i的夹角，指向为从轴线 i − 1 i-1 i−1到轴线 i i i；
- d i d_i di：关节 i i i上的两条公垂线 a i a_i ai与 a i − 1 a_{i-1} ai−1之间的距离，沿关节轴 i i i测量；
- θ i \theta_i θi：连杆 i i i相对于连杆 i − 1 i-1 i−1绕轴线 i i i的旋转角度，绕关节轴线 i − 1 i-1 i−1测量。
根据UR3机器人参数得到其DH参数如表1.1所示：
| 连杆序号 i i i | 连杆长度 a i / m m a_i/mm ai/mm | 连杆扭角 α i / r a d \alpha_i/rad αi/rad | 连杆距离 d i / m m d_i/mm di/mm | 关节转角 θ i / r a d \theta_i/rad θi/rad |
|---|---|---|---|---|
| 1 | 0 0 0 | π / 2 \pi/2 π/2 | 0 0 0 | θ 1 \theta_1 θ1 |
| 2 | 243.65 243.65 243.65 | 0 0 0 | 119.85 119.85 119.85 | θ 2 \theta_2 θ2 |
| 3 | 213 213 213 | 0 0 0 | − 92.85 -92.85 −92.85 | θ 3 \theta_3 θ3 |
| 4 | 0 0 0 | π / 2 \pi/2 π/2 | 83.4 83.4 83.4 | θ 4 \theta_4 θ4 |
| 5 | 0 0 0 | π / 2 \pi/2 π/2 | 83.4 83.4 83.4 | θ 5 \theta_5 θ5 |
| 6 | 0 0 0 | 0 0 0 | 82.4 82.4 82.4 | θ 6 \theta_6 θ6 |
1.1.2 改进型DH法（MDH）建模
改进型DH法建模规则如下：
-
找原点：找出关节轴 i i i和 i + 1 i+1 i+1之间的公垂线或关节轴 i i i和 i + 1 i+1 i+1的交点，以关节轴 i i i和 i + 1 i+1 i+1的交点或公垂线与关节轴 i i i的交点作为连杆坐标系 i i i的原点；
-
找 Z Z Z轴： Z i Z_i Zi轴沿关节轴 i i i的指向；
-
找 X X X轴： X i X_i Xi轴沿公垂线的指向，若关节轴 i i i和 i + 1 i+1 i+1相交，则规定 X i X_i Xi垂直于关节轴 i i i和 i + 1 i+1 i+1所在平面；
-
确定 Y Y Y轴：按照右手定则确定 Y i Y_i Yi轴。
根据此建模规则，有MDH建模示意图如图1.3所示：
使用MDH法建立UR3机器人的连杆坐标系如图1.4所示：
其中各参数的含义如下：
-
a i − 1 a_{i-1} ai−1：沿 X i − 1 X_{i-1} Xi−1轴，从 Z i − 1 Z_{i-1} Zi−1移动到 Z i Z_i Zi的距离；
-
α i − 1 \alpha_{i-1} αi−1：沿 X i − 1 X_{i-1} Xi−1轴，从 Z i − 1 Z_{i-1} Zi−1旋转到 Z i Z_i Zi的角度；
-
d i d_i di：沿 Z i Z_i Zi轴，从 X i − 1 X_{i-1} Xi−1 移动到$ X_i$的距离；
-
θ i \theta_i θi：沿 Z i Z_i Zi轴，从 X i − 1 X_{i-1} Xi−1旋转到$ X_i$的角度；
根据UR3机器人参数得到其DH参数如表1.2所示：
| 连杆序号 i i i | 连杆长度 a i − 1 / m m a_{i-1}/mm ai−1/mm | 连杆扭角 α i − 1 / r a d \alpha_{i-1}/rad αi−1/rad | 连杆距离 d i / m m d_i/mm di/mm | 关节转角 θ i / r a d \theta_i/rad θi/rad |
|---|---|---|---|---|
| 1 | 0 0 0 | 0 0 0 | 151.9 151.9 151.9 | θ 1 \theta_1 θ1 |
| 2 | 0 0 0 | π / 2 \pi/2 π/2 | 119.85 119.85 119.85 | θ 2 \theta_2 θ2 |
| 3 | − 243.65 -243.65 −243.65 | 0 0 0 | 0 0 0 | θ 3 \theta_3 θ3 |
| 4 | − 213 -213 −213 | 0 0 0 | − 9.45 -9.45 −9.45 | θ 4 \theta_4 θ4 |
| 5 | 0 0 0 | π / 2 \pi/2 π/2 | 83.4 83.4 83.4 | θ 5 \theta_5 θ5 |
| 6 | 0 0 0 | − π / 2 -\pi/2 −π/2 | 82.4 82.4 82.4 | θ 6 \theta_6 θ6 |
1.2 UR3机器人正运动学
根据所建立的坐标系，分别在SDH坐标系以及MDH坐标系下进行正运动学的建模。
1.2.1 SDH坐标系下的正运动学建模
根据SDH坐标系下的连杆关系，有：
KaTeX parse error: No such environment: equation at position 8: \begin{̲e̲q̲u̲a̲t̲i̲o̲n̲}̲ T_{i}^{i-1}=Ro…
将式(1)右边的四个变换展开可得：
KaTeX parse error: No such environment: align at position 8: \begin{̲a̲l̲i̲g̲n̲}̲ T_i^{i-1}=\lef…
式中
i
=
1
,
2
,
3
,
.
.
.
,
6
i=1,2,3,...,6
i=1,2,3,...,6。
为方便表示，定义如下简写：
- c i = c o s θ i c_i=cos\theta_i ci=cosθi
- s i = s i n θ i s_i=sin\theta_i si=sinθi
- c i j = c o s ( θ i + θ j ) c_{ij}=cos(\theta_i+\theta_j) cij=cos(θi+θj)
- s i j = s i n ( θ i + θ j ) s_{ij}=sin(\theta_i+\theta_j) sij=sin(θi+θj)
- c i j k = c o s ( θ i + θ j + θ k ) c_{ijk}=cos(\theta_i+\theta_j+\theta_k) cijk=cos(θi+θj+θk)
- s i j k = s i n ( θ i + θ j + θ k ) s_{ijk}=sin(\theta_i+\theta_j+\theta_k) sijk=sin(θi+θj+θk)
根据表1.1中的DH参数以及式(2)可得各连杆得变换矩阵
T
i
i
−
1
T_i^{i-1}
Tii−1，将各连杆的变换矩阵相乘可得机械臂的变矩阵
T
6
0
T^0_6
T60。
{
T
1
0
=
[
c
1
0
s
1
0
s
1
0
−
c
1
0
0
1
0
d
1
0
0
0
1
]
T
2
1
=
[
c
2
−
s
2
0
a
2
c
2
s
2
c
2
0
a
2
s
2
0
0
1
d
2
0
0
0
1
]
T
3
2
=
[
c
3
−
s
3
0
a
3
c
3
s
3
c
3
0
a
3
s
3
0
0
1
d
3
0
0
0
1
]
T
4
3
=
[
c
4
0
s
4
0
s
4
0
−
c
4
0
0
1
0
d
4
0
0
0
1
]
T
5
4
=
[
c
5
0
s
5
0
s
5
0
−
c
5
0
0
1
0
d
5
0
0
0
1
]
T
6
5
=
[
c
6
−
s
6
0
0
s
6
c
6
0
0
0
0
1
d
6
0
0
0
1
]
\begin{cases} T_1^0= \left[\begin{array}{cccc} c_1 & 0 & s_1 & 0\\ s_1 & 0 & -c_1 & 0\\ 0 & 1 & 0 & d_1\\ 0 & 0 & 0 & 1 \end{array}\right]\\ T_2^1= \left[\begin{array}{cccc} c_2 & -s_2 & 0 & a_2c_2\\ s_2 & c_2 & 0 & a_2s_2\\ 0 & 0 & 1 & d_2\\ 0 & 0 & 0 & 1 \end{array}\right]\\ T_3^2= \left[\begin{array}{cccc} c_3 & -s_3 & 0 & a_3c_3\\ s_3 & c_3 & 0 & a_3s_3\\ 0 & 0 & 1 & d_3\\ 0 & 0 & 0 & 1 \end{array}\right]\\ T_4^3= \left[\begin{array}{cccc} c_4 & 0 & s_4 & 0\\ s_4 & 0 & -c_4 & 0\\ 0 & 1 & 0 & d_4\\ 0 & 0 & 0 & 1 \end{array}\right]\\ T_5^4= \left[\begin{array}{cccc} c_5 & 0 & s_5 & 0\\ s_5 & 0 & -c_5 & 0\\ 0 & 1 & 0 & d_5\\ 0 & 0 & 0 & 1 \end{array}\right]\\ T_6^5= \left[\begin{array}{cccc} c_6 & -s_6 & 0 & 0\\ s_6 & c_6 & 0 & 0\\ 0 & 0 & 1 & d_6\\ 0 & 0 & 0 & 1 \end{array}\right]\\ \end{cases}
⎩⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎧T10=⎣⎢⎢⎡c1s1000010s1−c10000d11⎦⎥⎥⎤T21=⎣⎢⎢⎡c2s200−s2c2000010a2c2a2s2d21⎦⎥⎥⎤T32=⎣⎢⎢⎡c3s300−s3c3000010a3c3a3s3d31⎦⎥⎥⎤T43=⎣⎢⎢⎡c4s4000010s4−c40000d41⎦⎥⎥⎤T54=⎣⎢⎢⎡c5s5000010s5−c50000d51⎦⎥⎥⎤T65=⎣⎢⎢⎡c6s600−s6c600001000d61⎦⎥⎥⎤
KaTeX parse error: No such environment: equation at position 8: \begin{̲e̲q̲u̲a̲t̲i̲o̲n̲}̲ T_6^0=T_1^0T_2…
式(3)中各简式为：
{
n
x
=
c
6
(
s
1
s
5
+
c
1
c
234
c
5
)
+
c
1
s
234
s
6
n
y
=
s
1
s
234
s
6
−
c
6
(
c
1
s
5
−
s
1
c
234
c
5
)
n
z
=
s
234
c
5
c
6
−
c
234
s
6
o
x
=
c
1
s
234
c
6
−
s
6
(
s
1
s
5
+
c
1
c
234
c
5
)
o
y
=
s
6
(
c
1
s
5
−
s
1
c
234
c
5
)
+
s
1
s
234
c
6
o
z
=
−
c
234
c
6
−
s
234
c
5
s
6
a
x
=
c
1
c
234
s
5
−
s
1
c
5
a
y
=
s
1
c
234
s
5
+
c
1
c
5
a
z
=
s
345
s
5
p
x
=
(
c
1
c
234
s
5
−
s
1
c
5
)
d
6
+
c
1
s
234
d
5
+
s
1
d
4
+
c
1
c
23
a
3
+
s
1
d
3
+
c
1
c
2
a
2
+
s
1
d
2
p
y
=
(
s
1
c
234
s
5
+
c
1
c
5
)
d
6
+
s
1
s
234
d
5
−
c
1
d
4
+
s
1
c
23
a
3
−
s
1
d
3
+
s
1
c
2
a
2
−
c
1
d
2
p
z
=
s
234
s
5
d
6
−
c
234
d
5
+
s
23
a
3
+
s
2
a
2
\begin{cases} n_x = c_6(s_1s_5+c_1c_{234}c_5)+c_1s_{234}s_6\\ n_y = s_1s_{234}s_6-c_6(c_1s_5-s_1c_{234}c_5)\\ n_z = s_{234}c_5c_6-c_{234}s_6\\ o_x = c_1s_{234}c_6-s_6(s_1s_5+c_1c_{234}c_5)\\ o_y = s_6(c_1s_5-s_1c_{234}c_5)+s_1s_{234}c_6\\ o_z = -c_{234}c_6-s_{234}c_5s_6\\ a_x = c_1c_{234}s_5-s_1c_5\\ a_y = s_1c_{234}s_5+c_1c_5\\ a_z = s_{345}s_5\\ p_x = (c_1c_{234}s_5-s_1c_5)d_6+c_1s_{234}d_5+s_1d_4+c_1c_{23}a_3+s_1d_3+c_1c_2a_2+s_1d_2\\ p_y = (s_1c_{234}s_5+c_1c_5)d_6+s_1s_{234}d_5-c_1d_4+s_1c_{23}a_3-s_1d_3+s_1c_2a_2-c_1d_2\\ p_z = s_{234}s_5d_6-c_{234}d_5+s_{23}a_3+s_2a_2 \end{cases}
⎩⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎧nx=c6(s1s5+c1c234c5)+c1s234s6ny=s1s234s6−c6(c1s5−s1c234c5)nz=s234c5c6−c234s6ox=c1s234c6−s6(s1s5+c1c234c5)oy=s6(c1s5−s1c234c5)+s1s234c6oz=−c234c6−s234c5s6ax=c1c234s5−s1c5ay=s1c234s5+c1c5az=s345s5px=(c1c234s5−s1c5)d6+c1s234d5+s1d4+c1c23a3+s1d3+c1c2a2+s1d2py=(s1c234s5+c1c5)d6+s1s234d5−c1d4+s1c23a3−s1d3+s1c2a2−c1d2pz=s234s5d6−c234d5+s23a3+s2a2
为了验证模型的正确性，可将其中一个状态代入模型进行检验。对于图1.2中UR3机器人的形态，其对应的关节变量为
Θ
=
[
0
π
2
0
π
2
π
0
]
T
\Theta=[0\ \frac{\pi}{2}\ 0\ \frac{\pi}{2}\ \pi\ 0]^T
Θ=[0 2π 0 2π π 0]T，将其带入式(3)中，得到如下的转换矩阵：
T
6
0
=
[
1
0
0
0
0
0
−
1
−
(
d
2
+
d
3
+
d
4
+
d
6
)
0
1
0
d
4
+
a
2
+
a
3
0
0
0
1
]
=
[
1
0
0
0
0
0
−
1
−
192.8
0
1
0
540.05
0
0
0
1
]
T_6^0=\left[ \begin{array}{cccc} 1 & 0 & 0 & 0\\ 0 & 0 & -1 & -(d_2+d_3+d_4+d_6)\\ 0 & 1 & 0 & d_4+a_2+a_3\\ 0 & 0 & 0 & 1 \end{array} \right]=\left[ \begin{array}{cccc} 1 & 0 & 0 & 0\\ 0 & 0 & -1 & -192.8\\ 0 & 1 & 0 & 540.05\\ 0 & 0 & 0 & 1 \end{array} \right]
T60=⎣⎢⎢⎡100000100−1000−(d2+d3+d4+d6)d4+a2+a31⎦⎥⎥⎤=⎣⎢⎢⎡100000100−1000−192.8540.051⎦⎥⎥⎤
其中
[
1
0
0
0
0
−
1
0
1
0
]
\left[ \begin{array}{ccc} 1 & 0 & 0 \\ 0 & 0 & -1 \\ 0 & 1 & 0 \\ \end{array} \right]
⎣⎡1000010−10⎦⎤
正好对应{0}系到{6}系的旋转矩阵，并且
[
0
−
192.8
540.05
]
T
\left[0\ -192.8\ 540.05 \right]^T
[0 −192.8 540.05]T正好对应{0}系到{6}系平移向量，因此可以初步验证所建立的正运动学模型是正确的。
1.2.2 MDH坐标系下的正运动学建模
根据MDH坐标系下的连杆关系，有：
KaTeX parse error: No such environment: equation at position 8: \begin{̲e̲q̲u̲a̲t̲i̲o̲n̲}̲ T_i^{i-1}=Tran…
将式(4)右边的四个变换展开可得：
KaTeX parse error: No such environment: align at position 8: \begin{̲a̲l̲i̲g̲n̲}̲ T_i^{i-1}=\lef…
式中
i
=
1
,
2
,
3
,
.
.
.
,
6
i=1,2,3,...,6
i=1,2,3,...,6
根据表1.2中的DH参数以及式(5)可得各连杆得变换矩阵
T
i
i
−
1
T_i^{i-1}
Tii−1，将各连杆的变换矩阵相乘可得机械臂的变矩阵
T
6
0
T^0_6
T60。
{
T
1
0
=
[
c
1
−
s
1
0
0
s
1
c
1
0
0
0
0
1
d
1
0
0
0
1
]
T
2
1
=
[
c
2
−
s
2
0
0
0
0
−
1
−
d
2
s
2
c
2
0
0
0
0
0
1
]
T
3
2
=
[
c
3
−
s
3
0
a
2
s
3
c
3
0
0
0
0
1
0
0
0
0
1
]
T
4
3
=
[
c
4
−
s
4
0
a
3
s
4
c
4
0
0
0
0
1
d
4
0
0
0
1
]
T
5
4
=
[
c
5
−
s
5
0
0
0
0
−
1
−
d
5
s
5
c
5
0
0
0
0
0
1
]
T
6
5
=
[
c
6
−
s
6
0
0
0
0
1
d
6
−
s
6
−
c
6
0
0
0
0
0
1
]
\begin{cases} T_1^0= \left[\begin{array}{cccc} c_1 & -s_1 & 0 & 0\\ s_1 & c_1 & 0 & 0\\ 0 & 0 & 1 & d_1\\ 0 & 0 & 0 & 1 \end{array}\right]\\ T_2^1= \left[\begin{array}{cccc} c_2 & -s_2 & 0 & 0\\ 0 & 0 & -1 & -d_2\\ s_2 & c_2 & 0 & 0\\ 0 & 0 & 0 & 1 \end{array}\right]\\ T_3^2= \left[\begin{array}{cccc} c_3 & -s_3 & 0 & a_2\\ s_3 & c_3 & 0 & 0\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1 \end{array}\right]\\ T_4^3= \left[\begin{array}{cccc} c_4 & -s_4 & 0 & a_3\\ s_4 & c_4 & 0 & 0\\ 0 & 0 & 1 & d_4\\ 0 & 0 & 0 & 1 \end{array}\right]\\ T_5^4= \left[\begin{array}{cccc} c_5 & -s_5 & 0 & 0\\ 0 & 0 & -1 & -d_5\\ s_5 & c_5 & 0 & 0\\ 0 & 0 & 0 & 1 \end{array}\right]\\ T_6^5= \left[\begin{array}{cccc} c_6 & -s_6 & 0 & 0\\ 0 & 0 & 1 & d_6\\ -s_6 & -c_6 & 0 & 0\\ 0 & 0 & 0 & 1 \end{array}\right] \end{cases}
⎩⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎧T10=⎣⎢⎢⎡c1s100−s1c100001000d11⎦⎥⎥⎤T21=⎣⎢⎢⎡c20s20−s20c200−1000−d201⎦⎥⎥⎤T32=⎣⎢⎢⎡c3s300−s3c3000010a2001⎦⎥⎥⎤T43=⎣⎢⎢⎡c4s400−s4c4000010a30d41⎦⎥⎥⎤T54=⎣⎢⎢⎡c50s50−s50c500−1000−d501⎦⎥⎥⎤T65=⎣⎢⎢⎡c60−s60−s60−c6001000d601⎦⎥⎥⎤
KaTeX parse error: No such environment: equation at position 8: \begin{̲e̲q̲u̲a̲t̲i̲o̲n̲}̲ T_6^0=T_1^0T_2…
式(6)中各简式如下：
{
n
x
=
c
6
(
s
1
s
5
+
c
1
c
234
c
5
)
−
c
1
s
234
s
6
n
y
=
−
c
6
(
c
1
s
5
−
s
1
c
234
c
5
)
−
s
1
s
234
s
6
n
z
=
c
234
s
6
+
s
234
c
5
c
6
o
x
=
−
s
6
(
s
1
s
5
+
c
1
c
234
c
5
)
−
c
1
s
234
c
6
o
y
=
s
6
(
c
1
s
5
−
s
1
c
234
c
5
)
−
s
1
s
234
c
6
o
z
=
c
234
c
6
−
s
234
c
5
s
6
a
x
=
s
1
c
5
−
c
1
c
234
s
5
a
y
=
−
c
1
c
5
−
s
1
c
234
s
5
a
z
=
−
s
234
s
5
p
x
=
(
s
1
c
5
−
c
1
c
234
s
5
)
d
6
+
c
1
s
234
d
5
+
c
1
c
23
a
3
+
s
1
d
4
+
c
1
c
2
a
2
+
s
1
d
2
p
y
=
−
(
c
1
c
5
+
s
1
c
234
s
5
)
d
6
+
s
1
s
234
d
5
+
s
1
c
23
a
3
−
c
1
d
4
+
s
1
c
2
a
2
−
c
1
d
2
p
z
=
−
s
234
s
5
d
6
−
c
234
d
5
+
s
23
a
3
+
s
2
a
2
+
d
1
\begin{cases} n_x = c_6(s_1s_5+c_1c_{234}c_5)-c_1s_{234}s_6\\ n_y = -c_6(c_1s_5-s_1c_{234}c_5)-s_1s_{234}s_6\\ n_z = c_{234}s_6+s_{234}c_5c_6\\ o_x = -s_6(s_1s_5+c_1c_{234}c_5)-c_1s_{234}c_6\\ o_y = s_6(c_1s_5-s_1c_{234}c_5)-s_1s_{234}c_6\\ o_z = c_{234}c_6-s_{234}c_5s_6\\ a_x = s_1c_5-c_1c_{234}s_5\\ a_y = -c_1c_5-s_1c_{234}s_5\\ a_z = -s_{234}s_5\\ p_x = (s_1c_5-c_1c_{234}s_5)d_6+c_1s_{234}d_5+c_1c_{23}a_3+s_1d_4+c_1c_2a_2+s_1d_2\\ p_y = -(c_1c_5+s_1c_{234}s_5)d_6+s_1s_{234}d_5+s_1c_{23}a_3-c_1d_4+s_1c_2a_2-c_1d_2\\ p_z = -s_{234}s_5d_6-c_{234}d_5+s_{23}a_3+s_2a_2+d_1 \end{cases}
⎩⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎧nx=c6(s1s5+c1c234c5)−c1s234s6ny=−c6(c1s5−s1c234c5)−s1s234s6nz=c234s6+s234c5c6ox=−s6(s1s5+c1c234c5)−c1s234c6oy=s6(c1s5−s1c234c5)−s1s234c6oz=c234c6−s234c5s6ax=s1c5−c1c234s5ay=−c1c5−s1c234s5az=−s234s5px=(s1c5−c1c234s5)d6+c1s234d5+c1c23a3+s1d4+c1c2a2+s1d2py=−(c1c5+s1c234s5)d6+s1s234d5+s1c23a3−c1d4+s1c2a2−c1d2pz=−s234s5d6−c234d5+s23a3+s2a2+d1
为了验证模型的正确性，可将其中一个状态代入模型进行检验。对于图1.4中UR3机器人的形态，其对应的关节变量为
Θ
=
[
0
−
π
2
0
−
π
2
0
0
]
T
\Theta=[0\ -\frac{\pi}{2}\ 0\ -\frac{\pi}{2}\ 0\ 0]^T
Θ=[0 −2π 0 −2π 0 0]T，将其带入式(6)中，得到如下的转换矩阵：
T
6
0
=
[
−
1
0
0
0
0
0
−
1
−
(
d
6
+
d
4
+
d
2
)
0
−
1
0
d
1
+
d
5
−
a
2
−
a
3
0
0
0
1
]
=
[
−
1
0
0
0
0
0
−
1
−
192.8
0
−
1
0
691.95
0
0
0
1
]
T_6^0=\left[ \begin{array}{cccc} -1 & 0 & 0 & 0\\ 0 & 0 & -1 & -(d_6+d_4+d_2)\\ 0 & -1 & 0 & d_1+d_5-a_2-a_3\\ 0 & 0 & 0 & 1 \end{array} \right]=\left[ \begin{array}{cccc} -1 & 0 & 0 & 0\\ 0 & 0 & -1 & -192.8\\ 0 & -1 & 0 & 691.95\\ 0 & 0 & 0 & 1 \end{array} \right]
T60=⎣⎢⎢⎡−100000−100−1000−(d6+d4+d2)d1+d5−a2−a31⎦⎥⎥⎤=⎣⎢⎢⎡−100000−100−1000−192.8691.951⎦⎥⎥⎤
其中
[
−
1
0
0
0
0
−
1
0
−
1
0
]
\left[ \begin{array}{ccc} -1 & 0 & 0 \\ 0 & 0 & -1 \\ 0 & -1 & 0 \\ \end{array} \right]
⎣⎡−10000−10−10⎦⎤
正好对应{0}系到{6}系的旋转矩阵，并且
[
0
−
192.8
691.95
]
T
\left[0\ -192.8\ 691.95 \right]^T
[0 −192.8 691.95]T正好对应{0}系到{6}系平移向量，因此可以初步验证所建立的正运动学模型是正确的。后文中将通过仿真来进一步验证模型的正确性。
由于在不同的坐标系下DH参数的含义有所不同，推导出来的正运动学模型也会有所差别。在后文的分析中，若无特别说明，将使用MDH坐标系下推导出来的正运动学模型。
扫一扫
7561
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
