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α, aaa, ddd, θ\thetaθ;
c. 列D-H参数表;
d.由参数表得到变换矩阵;
D-H 建模方法中,每个连杆使用 4 个参数 α\alphaα, aaa, ddd, θ\thetaθ 来描述,2 个描述连杆本身,另外 2 个描述与相邻连杆的位姿(连接或几何关系)。
对于转动关节,其中 θ\thetaθ 为关节变量,其他三个参数固定不变,为连杆参数;对于移动关节,ddd 为关节变量,其他三个为关节参数。
根据连杆坐标系和关节对应关系的不同,D-H 建模法可以分为传统 D-H (Classic DH) 和改进 D-H (Modified DH),二者的主要区别如下表所示。
区别 | Classic D-H | Modified D-H |
---|---|---|
连杆固定坐标系的位置 | 后一个关节坐标系 | 前一个关节坐标系 |
XXX 轴的确定方式 | 当前坐标系 ZZZ 轴和前一个坐标系 ZZZ 轴的向量积 | 后一个坐标系 ZZZ 轴与当前坐标系 ZZZ 轴的向量积 |
坐标系间的参数变换顺序 | θ\thetaθ, ddd, aaa, α\alphaα | α\alphaα, aaa, θ\thetaθ, ddd |
Classic D-H:
Classic DH 的关节和坐标系关系中各个参数的含义如下:
θi\theta_{i}θi: Xi−1X_{i-1}Xi−1 到 XiX_{i}Xi 绕Zi−1Z_{i-1}Zi−1旋转的角度;
did_{i}di: Xi−1X_{i-1}Xi−1 到 XiX_{i}Xi 沿 Zi−1Z_{i-1}Zi−1 方向的距离;
aia_{i}ai:Zi−1Z_{i-1}Zi−1 到 ZiZ_{i}Zi 沿 Xi−1X_{i-1}Xi−1 方向的距离;
αi\alpha_{i}αi: Zi−1Z_{i-1}Zi−1 到 ZiZ_{i}Zi 绕 Xi−1X_{i-1}Xi−1 旋转的角度
坐标系 Oi−1O_{i-1}Oi−1 与关节 iii 对齐,其 D-H 参数矩阵为:
ii−1T=[cosθi−sinθicosαisinθisinαiaicosθisinθicosθicosαi−cosθisinαiaisinθi0sinαicosαidi0001] _{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: Zi−1Z_{i-1}Zi−1 到 ZiZ_{i}Zi 绕 Xi−1X_{i-1}Xi−1 旋转的角度;
ai−1a_{i-1}ai−1:Zi−1Z_{i-1}Zi−1 到 ZiZ_{i}Zi 沿 Xi−1X_{i-1}Xi−1 方向的距离;
θi\theta_{i}θi:Xi−1X_{i-1}Xi−1 到 XiX_{i}Xi 绕 ZiZ_{i}Zi 旋转的角度;
did_{i}di: Xi−1X_{i-1}Xi−1到 XiX_{i}Xi沿 ZiZ_{i}Zi 方向的距离。
坐标系 Oi−1O_{i-1}Oi−1 与关节 i−1i-1i−1 对齐,其 D-H 参数矩阵为:
ii−1T=[cosθi−sinθi0ai−1sinθicosαi−1cosθicosαi−1−sinαi−1−disinαi−1sinθisinαi−1cosθisinαi−1cosαi−1dicosαi−10001] _{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. 确定各个关节轴和连杆,坐标系的 ZZZ 轴沿关节轴线方向;
b. 找出相邻两关节轴线的交点或公垂线,用于确定坐标系 {i}\{i\}{i} 的原点:以关节轴 iii 和 i+1i+1i+1 的交点或公垂线与关节轴 iii 的交点为原点;
c. 确定 XXX 轴:两轴线相交时,Xi⃗=±Zi+1⃗×Zi⃗\vec{X_{i}} = \pm \vec{Z_{i+1}} \times \vec{Z_{i}}Xi=±Zi+1×Zi;两轴线不相交时,XiX_{i}Xi 轴与公垂线重合,方向为 iii 到 i+1i+1i+1;
d. 右手定则确定 YiY_{i}Yi 轴;
e. 确定基坐标系 {0}\{0\}{0}:为了简化问题,Z0Z_0Z0 通常与关节 1 的轴线方向重合,且当关节变量 1 为 0 时,坐标系 {0}\{0\}{0} 与 {1}\{1\}{1} 重合;
f. 确定末端坐标系 {n}\{n\}{n}:对于转动关节,θn=0\theta_n = 0θn=0 时,XnX_nXn 与 Xn−1X_{n-1}Xn−1 方向相同,选取原点使 dn=0d_n = 0dn=0;对于移动关节,取 XnX_nXn 方向使 θn=0\theta_n = 0θn=0,当 dn=0d_n = 0dn=0 时,取 Xn−1X_{n-1}Xn−1 与 XnX_nXn 的交点为原点。
D-H 参数表:
根据机械臂各个连杆间坐标系的关系,采用 Modified D-H 形式,得到的参数表如下。
iii | αi−1\alpha_{i-1}αi−1 | ai−1a_{i-1}ai−1 | θi−1\theta_{i-1}θi−1 | did_{i}di | θ\thetaθ 的范围 |
---|---|---|---|---|---|
1 | 0∘0^{\circ}0∘ | 000 | θ1\theta_{1}θ1 | 000 | (−2π3,2π3)(-\frac{2 \pi}{3}, \frac{2 \pi}{3})(−32π,32π) |
2 | −90∘-90^{\circ}−90∘ | a1a_{1}a1 | θ2\theta_{2}θ2 | 000 | (−π2,0)(-\frac{\pi}{2}, 0)(−2π,0) |
3 | 0∘0^{\circ}0∘ | a2a_{2}a2 | θ3\theta_{3}θ3 | 000 | (−2π3,2π3)(-\frac{2 \pi}{3}, \frac{2 \pi}{3})(−32π,32π) |
4 | 0∘0^{\circ}0∘ | a3a_{3}a3 | θ4\theta_{4}θ4 | 000 | (−7π6,π6)(-\frac{7 \pi}{6}, \frac{\pi}{6})(−67π,6π) |
5 | −90∘-90^{\circ}−90∘ | 000 | θ5\theta_{5}θ5 | 000 | (−2π3,2π3)(-\frac{2 \pi}{3}, \frac{2 \pi}{3})(−32π,32π) |
齐次变换矩阵
将 DH 参数表代入 Modified DH 的 DH 参数矩阵,可以得到各个坐标系间的齐次变换矩阵10T_{1}^{0}T10T, 21T_{2}^{1}T21T, 32T_{3}^{2}T32T,43T_{4}^{3}T43T 和 54T_{5}^{4}T54T 则可得基坐标系到末端坐标系的齐次变换矩阵:
50T=10T21T32T43T54T=[nxox;axpxnyoyay;pynzozazpz0001] _{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
其中,
[pxpypz]T
\begin{bmatrix} p_{x} & p_{y} & p_{z} \end{bmatrix}^T
[pxpypz]T
为机械臂末端在基坐标系中的位置,
[nxnynz]T\begin{bmatrix} n_{x} & n_{y} & n_{z} \end{bmatrix}^T[nxnynz]T
为机械臂末端坐标系 XXX 轴在基坐标系中的方向矢量,
[oxoyoz]T\begin{bmatrix} o_{x} & o_{y} & o_{z} \end{bmatrix}^T[oxoyoz]T
为机械臂末端坐标系 YYY 轴在基坐标系中的方向矢量,
[axayaz]T\begin{bmatrix} a_{x} & a_{y} & a_{z} \end{bmatrix}^T[axayaz]T
为机械臂末端坐标系 ZZZ 轴在基坐标系中的方向矢量。
代入 D-H 参数,可得
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
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