中国建设招标网 官方网站下载,橙网站,Linux主机安装wordpress,wordpress 点点主题Chapt 4. 旋量代数在机器人学中的应用
4.1 串联机器人正运动学的指数积(PoE, Product of Exponetial)公式
4.1.1 回顾#xff1a;机器人正运动学的Denavit-Hartenberg (D-H)参数公式
D-H 建模法: D-H 建模方法是由 Denavit 和 Hartenberg (ASME, 1955) 提出的一种建模方法机器人正运动学的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-HModified 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 i1 i1 的交点或公垂线与关节轴 i i i 的交点为原点
c. 确定 X X X 轴:两轴线相交时, X i ⃗ ± Z i 1 ⃗ × Z i ⃗ \vec{X_{i}} \pm \vec{Z_{i1}} \times \vec{Z_{i}} Xi ±Zi1 ×Zi ;两轴线不相交时, X i X_{i} Xi 轴与公垂线重合,方向为 i i i 到 i 1 i1 i1
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 θn0 时, X n X_n Xn 与 X n − 1 X_{n-1} Xn−1 方向相同,选取原点使 d n 0 d_n 0 dn0;对于移动关节,取 X n X_n Xn 方向使 θ n 0 \theta_n 0 θn0,当 d n 0 d_n 0 dn0 时,取 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} 50T10T21T32T43T54T 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} nxs1s5c1c2c3c4c5−c1c2c5s3s4−c1c3c5s2s4−c1c4c5s2s3nyc2c3c4c5s1−c1s5−c2c5s1s3s4−c3c5s1s2s4−c4c5s1s2s3nzc5s2s3s4−c2c4c5s3−c3c4c5s2−c2c3c5s4oxc5s1−c1c2c3c4s5c1c2s3s4s5c1c3s2s4s5c1c4s2s3s5oyc2s1s3s4s5−c2c3c4s1s5−c1c5c3s1s2s4s5c4s1s2s3s5ozc2c3s4s5c2c4s3s5c3c4s2s5−s2s3s4s5axc1s2s3s4−c1c2c4s3−c1c3c4s2−c1c2c3s4ays1s2s3s4−c2c4s1s3−c3c4s1s2−c2c3s1s4azc2s3s4c3s2s4c4s2s3−c2c3c4pxa1c1a2c1c2a3c1c2c3−a3c1s2s3pya1s1a2c2s1a3c2c3s1−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;elsestandard_dh alpha;enddh_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);endif (isa(T,sym))T_out sym(eye(4,4));else T_out eye(4,4);endfor i1:size(T,3)T_out T_out * T(:,:,i);endT T_out;else if (nargin 4)if (nargin 5)standard_dh 1;endif standard_dh % Standard DH convention computationT [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 computationT [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];endendend
