Chapt 4. 旋量代数在机器人学中的应用
4.1 串联机器人正运动学的指数积(PoE, Product of Exponetial)公式
4.1.1 回顾：机器人正运动学的Denavit-Hartenberg (D-H)参数公式
D-H 建模法: D-H 建模方法是由 Denavit 和 Hartenberg (ASME, 1955) 提出的一种建模方法，主要用在机器人运动学上。此方法在机器人的每个连杆上建立一个坐标系，通过齐次坐标变换实现两个连杆上的坐标变换，建立多连杆串联系统中首末坐标系的变换关系。
D-H 建模方法的几个要点如下：
a. 建立连杆坐标系；
b.确定四个参数 α \alpha α, a a a, d d d, θ \theta θ；
c. 列D-H参数表；
d.由参数表得到变换矩阵；
D-H 建模方法中，每个连杆使用 4 个参数 α \alpha α, a a a, d d d, θ \theta θ 来描述，2 个描述连杆本身，另外 2 个描述与相邻连杆的位姿（连接或几何关系）。
对于转动关节，其中 θ \theta θ 为关节变量，其他三个参数固定不变，为连杆参数；对于移动关节， d d d 为关节变量，其他三个为关节参数。
根据连杆坐标系和关节对应关系的不同，D-H 建模法可以分为传统 D-H (Classic DH) 和改进 D-H (Modified DH)，二者的主要区别如下表所示。
| 区别 | Classic D-H | Modified D-H |
|---|---|---|
| 连杆固定坐标系的位置 | 后一个关节坐标系 | 前一个关节坐标系 |
| X X X 轴的确定方式 | 当前坐标系 Z Z Z 轴和前一个坐标系 Z Z Z 轴的向量积 | 后一个坐标系 Z Z Z 轴与当前坐标系 Z Z Z 轴的向量积 |
| 坐标系间的参数变换顺序 | θ \theta θ, d d d, a a a, α \alpha α | α \alpha α, a a a, θ \theta θ, d d d |
Classic D-H:
Classic DH 的关节和坐标系关系中各个参数的含义如下：
θ i \theta_{i} θi: X i − 1 X_{i-1} Xi−1 到 X i X_{i} Xi 绕 Z i − 1 Z_{i-1} Zi−1旋转的角度；
d i d_{i} di: X i − 1 X_{i-1} Xi−1 到 X i X_{i} Xi 沿 Z i − 1 Z_{i-1} Zi−1 方向的距离；
a i a_{i} ai： Z i − 1 Z_{i-1} Zi−1 到 Z i Z_{i} Zi 沿 X i − 1 X_{i-1} Xi−1 方向的距离；
α i \alpha_{i} αi: Z i − 1 Z_{i-1} Zi−1 到 Z i Z_{i} Zi 绕 X i − 1 X_{i-1} Xi−1 旋转的角度
坐标系 O i − 1 O_{i-1} Oi−1 与关节 i i i 对齐，其 D-H 参数矩阵为：
i i − 1 T = [ cos θ i − sin θ i cos α i sin θ i sin α i a i cos θ i sin θ i cos θ i cos α i − cos θ i sin α i a i sin θ i 0 sin α i cos α i d i 0 0 0 1 ] _{i}^{i-1}T = \begin{bmatrix} \cos{\theta_{i}} & -\sin{\theta_{i}} \cos{\alpha_{i}} & \sin{\theta_{i}} \sin{\alpha_{i}} & a_{i} \cos{\theta_{i}} \\ \sin{\theta_{i}} & \cos{\theta_{i}} \cos{\alpha_{i}} & -\cos{\theta_{i}} \sin{\alpha_{i}} & a_{i} \sin{\theta_{i}} \\ 0 & \sin{\alpha_{i}} & \cos{\alpha_{i}} & d_{i} \\ 0 & 0 & 0 & 1 \end{bmatrix} ii−1T= cosθisinθi00−sinθicosαicosθicosαisinαi0sinθisinαi−cosθisinαicosαi0aicosθiaisinθidi1
Modified DH:
Modified D-H 的关节和坐标系关系中各个参数的含义如下：
α i − 1 \alpha_{i-1} αi−1： Z i − 1 Z_{i-1} Zi−1 到 Z i Z_{i} Zi 绕 X i − 1 X_{i-1} Xi−1 旋转的角度；
a i − 1 a_{i-1} ai−1： Z i − 1 Z_{i-1} Zi−1 到 Z i Z_{i} Zi 沿 X i − 1 X_{i-1} Xi−1 方向的距离；
θ i \theta_{i} θi： X i − 1 X_{i-1} Xi−1 到 X i X_{i} Xi 绕 Z i Z_{i} Zi 旋转的角度；
d i d_{i} di： X i − 1 X_{i-1} Xi−1到 X i X_{i} Xi沿 Z i Z_{i} Zi 方向的距离。
坐标系
O
i
−
1
O_{i-1}
Oi−1 与关节
i
−
1
i-1
i−1 对齐，其 D-H 参数矩阵为：
i i − 1 T = [ cos θ i − sin θ i 0 a i − 1 sin θ i cos α i − 1 cos θ i cos α i − 1 − sin α i − 1 − d i sin α i − 1 sin θ i sin α i − 1 cos θ i sin α i − 1 cos α i − 1 d i cos α i − 1 0 0 0 1 ] _{i}^{i-1}T = \begin{bmatrix} \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{bmatrix} ii−1T= cosθisinθicosαi−1sinθisinαi−10−sinθicosθicosαi−1cosθisinαi−100−sinαi−1cosαi−10ai−1−disinαi−1dicosαi−11
Modified DH 克服了 Classic DH 在用于树型结构机器人时可能出现的问题,比较常用,故之后主要介绍这种方法,并使用该方法进行建模。
机械臂连杆坐标系的建立
建立机械臂连杆坐标系的步骤:
a. 确定各个关节轴和连杆,坐标系的 Z Z Z 轴沿关节轴线方向；
b. 找出相邻两关节轴线的交点或公垂线,用于确定坐标系 { i } \{i\} {i} 的原点:以关节轴 i i i 和 i + 1 i+1 i+1 的交点或公垂线与关节轴 i i i 的交点为原点；
c. 确定 X X X 轴:两轴线相交时, X i ⃗ = ± Z i + 1 ⃗ × Z i ⃗ \vec{X_{i}} = \pm \vec{Z_{i+1}} \times \vec{Z_{i}} Xi=±Zi+1×Zi;两轴线不相交时, X i X_{i} Xi 轴与公垂线重合,方向为 i i i 到 i + 1 i+1 i+1；
d. 右手定则确定 Y i Y_{i} Yi 轴；
e. 确定基坐标系 { 0 } \{0\} {0}:为了简化问题, Z 0 Z_0 Z0 通常与关节 1 的轴线方向重合,且当关节变量 1 为 0 时,坐标系 { 0 } \{0\} {0} 与 { 1 } \{1\} {1} 重合；
f. 确定末端坐标系 { n } \{n\} {n}:对于转动关节, θ n = 0 \theta_n = 0 θn=0 时, X n X_n Xn 与 X n − 1 X_{n-1} Xn−1 方向相同,选取原点使 d n = 0 d_n = 0 dn=0;对于移动关节,取 X n X_n Xn 方向使 θ n = 0 \theta_n = 0 θn=0,当 d n = 0 d_n = 0 dn=0 时,取 X n − 1 X_{n-1} Xn−1 与 X n X_n Xn 的交点为原点。
D-H 参数表：
根据机械臂各个连杆间坐标系的关系，采用 Modified D-H 形式，得到的参数表如下。
| i i i | α i − 1 \alpha_{i-1} αi−1 | a i − 1 a_{i-1} ai−1 | θ i − 1 \theta_{i-1} θi−1 | d i d_{i} di | θ \theta θ 的范围 |
|---|---|---|---|---|---|
| 1 | 0 ∘ 0^{\circ} 0∘ | 0 0 0 | θ 1 \theta_{1} θ1 | 0 0 0 | ( − 2 π 3 , 2 π 3 ) (-\frac{2 \pi}{3}, \frac{2 \pi}{3}) (−32π,32π) |
| 2 | − 9 0 ∘ -90^{\circ} −90∘ | a 1 a_{1} a1 | θ 2 \theta_{2} θ2 | 0 0 0 | ( − π 2 , 0 ) (-\frac{\pi}{2}, 0) (−2π,0) |
| 3 | 0 ∘ 0^{\circ} 0∘ | a 2 a_{2} a2 | θ 3 \theta_{3} θ3 | 0 0 0 | ( − 2 π 3 , 2 π 3 ) (-\frac{2 \pi}{3}, \frac{2 \pi}{3}) (−32π,32π) |
| 4 | 0 ∘ 0^{\circ} 0∘ | a 3 a_{3} a3 | θ 4 \theta_{4} θ4 | 0 0 0 | ( − 7 π 6 , π 6 ) (-\frac{7 \pi}{6}, \frac{\pi}{6}) (−67π,6π) |
| 5 | − 9 0 ∘ -90^{\circ} −90∘ | 0 0 0 | θ 5 \theta_{5} θ5 | 0 0 0 | ( − 2 π 3 , 2 π 3 ) (-\frac{2 \pi}{3}, \frac{2 \pi}{3}) (−32π,32π) |
齐次变换矩阵
将 DH 参数表代入 Modified DH 的 DH 参数矩阵，可以得到各个坐标系间的齐次变换矩阵 1 0 T _{1}^{0}T 10T, 2 1 T _{2}^{1}T 21T, 3 2 T _{3}^{2}T 32T, 4 3 T _{4}^{3}T 43T 和 5 4 T _{5}^{4}T 54T 则可得基坐标系到末端坐标系的齐次变换矩阵:
5 0 T = 1 0 T 2 1 T 3 2 T 4 3 T 5 4 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 ] _{5}^{0}T = {_{1}^{0}T} {_{2}^{1}T} {_{3}^{2}T} {_{4}^{3}T} {_{5}^{4}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} 50T=10T21T32T43T54T= nxnynz0oxoyoz0;axayaz0px;pypz1
其中，
[
p
x
p
y
p
z
]
T
\begin{bmatrix} p_{x} & p_{y} & p_{z} \end{bmatrix}^T
[pxpypz]T
为机械臂末端在基坐标系中的位置，
[
n
x
n
y
n
z
]
T
\begin{bmatrix} n_{x} & n_{y} & n_{z} \end{bmatrix}^T
[nxnynz]T
为机械臂末端坐标系
X
X
X 轴在基坐标系中的方向矢量，
[
o
x
o
y
o
z
]
T
\begin{bmatrix} o_{x} & o_{y} & o_{z} \end{bmatrix}^T
[oxoyoz]T
为机械臂末端坐标系
Y
Y
Y 轴在基坐标系中的方向矢量，
[
a
x
a
y
a
z
]
T
\begin{bmatrix} a_{x} & a_{y} & a_{z} \end{bmatrix}^T
[axayaz]T
为机械臂末端坐标系
Z
Z
Z 轴在基坐标系中的方向矢量。
代入 D-H 参数，可得
n
x
=
s
1
s
5
+
c
1
c
2
c
3
c
4
c
5
−
c
1
c
2
c
5
s
3
s
4
−
c
1
c
3
c
5
s
2
s
4
−
c
1
c
4
c
5
s
2
s
3
n
y
=
c
2
c
3
c
4
c
5
s
1
−
c
1
s
5
−
c
2
c
5
s
1
s
3
s
4
−
c
3
c
5
s
1
s
2
s
4
−
c
4
c
5
s
1
s
2
s
3
n
z
=
c
5
s
2
s
3
s
4
−
c
2
c
4
c
5
s
3
−
c
3
c
4
c
5
s
2
−
c
2
c
3
c
5
s
4
o
x
=
c
5
s
1
−
c
1
c
2
c
3
c
4
s
5
+
c
1
c
2
s
3
s
4
s
5
+
c
1
c
3
s
2
s
4
s
5
+
c
1
c
4
s
2
s
3
s
5
o
y
=
c
2
s
1
s
3
s
4
s
5
−
c
2
c
3
c
4
s
1
s
5
−
c
1
c
5
+
c
3
s
1
s
2
s
4
s
5
+
c
4
s
1
s
2
s
3
s
5
o
z
=
c
2
c
3
s
4
s
5
+
c
2
c
4
s
3
s
5
+
c
3
c
4
s
2
s
5
−
s
2
s
3
s
4
s
5
a
x
=
c
1
s
2
s
3
s
4
−
c
1
c
2
c
4
s
3
−
c
1
c
3
c
4
s
2
−
c
1
c
2
c
3
s
4
a
y
=
s
1
s
2
s
3
s
4
−
c
2
c
4
s
1
s
3
−
c
3
c
4
s
1
s
2
−
c
2
c
3
s
1
s
4
a
z
=
c
2
s
3
s
4
+
c
3
s
2
s
4
+
c
4
s
2
s
3
−
c
2
c
3
c
4
p
x
=
a
1
c
1
+
a
2
c
1
c
2
+
a
3
c
1
c
2
c
3
−
a
3
c
1
s
2
s
3
p
y
=
a
1
s
1
+
a
2
c
2
s
1
+
a
3
c
2
c
3
s
1
−
a
3
s
1
s
2
s
3
p
z
=
−
a
2
s
2
−
a
3
c
2
s
3
−
a
3
c
3
s
2
n_{x} = s_{1} s_{5} + c_{1} c_{2} c_{3} c_{4} c_{5} - c_{1} c_{2} c_{5} s_{3} s_{4} - c_{1} c_{3} c_{5} s_{2} s_{4} - c_{1} c_{4} c_{5} s_{2} s_{3} \\ n_{y} = c_{2} c_{3} c_{4} c_{5} s_{1} - c_{1} s_{5} - c_{2} c_{5} s_{1} s_{3} s_{4} - c_{3} c_{5} s_{1} s_{2} s_{4} - c_{4} c_{5} s_{1} s_{2} s_{3} \\ n_{z} = c_{5} s_{2} s_{3} s_{4} - c_{2} c_{4} c_{5} s_{3} - c_{3} c_{4} c_{5} s_{2} - c_{2} c_{3} c_{5} s_{4} \\ o_{x} = c_{5} s_{1} - c_{1} c_{2} c_{3} c_{4} s_{5} + c_{1} c_{2} s_{3} s_{4} s_{5} + c_{1} c_{3} s_{2} s_{4} s_{5} + c_{1} c_{4} s_{2} s_{3} s_{5} \\ o_{y} = c_{2} s_{1} s_{3} s_{4} s_{5} - c_{2} c_{3} c_{4} s_{1} s_{5} - c_{1} c_{5} + c_{3} s_{1} s_{2} s_{4} s_{5} + c_{4} s_{1} s_{2} s_{3} s_{5} \\ o_{z} = c_{2} c_{3} s_{4} s_{5} + c_{2} c_{4} s_{3} s_{5} + c_{3} c_{4} s_{2} s_{5} - s_{2} s_{3} s_{4} s_{5} \\ a_{x} = c_{1} s_{2} s_{3} s_{4} - c_{1} c_{2} c_{4} s_{3} - c_{1} c_{3} c_{4} s_{2} - c_{1} c_{2} c_{3} s_{4} \\ a_{y} = s_{1} s_{2} s_{3} s_{4} - c_{2} c_{4} s_{1} s_{3} - c_{3} c_{4} s_{1} s_{2} - c_{2} c_{3} s_{1} s_{4} \\ a_{z} = c_{2} s_{3} s_{4} + c_{3} s_{2} s_{4} + c_{4} s_{2} s_{3} - c_{2} c_{3} c_{4} \\ p_{x} = a_{1} c_{1} + a_{2} c_{1} c_{2} + a_{3} c_{1} c_{2} c_{3} - a_{3} c_{1} s_{2} s_{3} \\ p_{y} = a_{1} s_{1} + a_{2} c_{2} s_{1} + a_{3} c_{2} c_{3} s_{1} - a_{3} s_{1} s_{2} s_{3} \\ p_{z} = -a_{2} s_{2} - a_{3} c_{2} s_{3} - a_{3} c_{3} s_{2}
nx=s1s5+c1c2c3c4c5−c1c2c5s3s4−c1c3c5s2s4−c1c4c5s2s3ny=c2c3c4c5s1−c1s5−c2c5s1s3s4−c3c5s1s2s4−c4c5s1s2s3nz=c5s2s3s4−c2c4c5s3−c3c4c5s2−c2c3c5s4ox=c5s1−c1c2c3c4s5+c1c2s3s4s5+c1c3s2s4s5+c1c4s2s3s5oy=c2s1s3s4s5−c2c3c4s1s5−c1c5+c3s1s2s4s5+c4s1s2s3s5oz=c2c3s4s5+c2c4s3s5+c3c4s2s5−s2s3s4s5ax=c1s2s3s4−c1c2c4s3−c1c3c4s2−c1c2c3s4ay=s1s2s3s4−c2c4s1s3−c3c4s1s2−c2c3s1s4az=c2s3s4+c3s2s4+c4s2s3−c2c3c4px=a1c1+a2c1c2+a3c1c2c3−a3c1s2s3py=a1s1+a2c2s1+a3c2c3s1−a3s1s2s3pz=−a2s2−a3c2s3−a3c3s2
Simple D-H in matlab
function [T] = dh_transform(a, alpha, d, theta, standard_dh)
% dh_transform computes the Denavit-Hartenberg transformation matrix
% Given:
% a (also written as 'r') - distance between origin(i) and origin(i-1)
% about z(i-1)
%
% alpha(?) - angle from z(i-1) to z(i) about x(i)
%
% d - the link offset betwen origin(i) with respect to origin(i-1)
% along z(i-1)
%
% theta (?) - joint angle between from x(i-1) to x(i) about z(i-1)
%
%
%
% standard_dh - uses standard DH convention if 1 or if this
% parameter is not provided. Uses modified DH
% if this value is 0
% OR given:
% a = DH parameter matrix
% i.e. for SCARA manipulator a will look like as follows
% syms q1 q2 d3 q4 a1 a2
% a = [ 0 0 0 q1;
% a1 0 0 q2;
% a2 0 -d3 0 ;
% 0 0 0 q4];
%
%
if (nargin <= 2)
if (nargin == 1)
standard_dh = 1;
else
standard_dh = alpha;
end
dh_parameter_matrix = a;
for row = 1:size(dh_parameter_matrix,1)
dh_row = dh_parameter_matrix(row,:);
a = dh_row(1);
alpha = dh_row(2);
d = dh_row(3);
theta = dh_row(4);
T(:,:,row) = dh_transform(a, alpha, d, theta, standard_dh);
end
if (isa(T,'sym'))
T_out = sym(eye(4,4));
else
T_out = eye(4,4);
end
for i=1:size(T,3)
T_out = T_out * T(:,:,i);
end
T = T_out;
else if (nargin >= 4)
if (nargin < 5)
standard_dh = 1;
end
if standard_dh % Standard DH convention computation
T = [cos(theta) -sin(theta)*cos(alpha) sin(theta)*sin(alpha) a*cos(theta);
sin(theta) cos(theta)*cos(alpha) -cos(theta)*sin(alpha) a*sin(theta);
0 sin(alpha) cos(alpha) d;
0 0 0 1];
else % Modified DH convention computation
T = [cos(theta) -sin(theta) 0 a;
sin(theta)*cos(alpha) cos(theta)*cos(alpha) -sin(alpha) -d*sin(alpha);
sin(theta)*sin(alpha) cos(theta)*sin(alpha) cos(alpha) d*cos(alpha);
0 0 0 1];
end
end
end
