智能机器人与旋量代数(12)

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-HModified 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}Xi1XiX_{i}XiZi−1Z_{i-1}Zi1旋转的角度;

did_{i}di: Xi−1X_{i-1}Xi1XiX_{i}Xi 沿 Zi−1Z_{i-1}Zi1 方向的距离;

aia_{i}aiZi−1Z_{i-1}Zi1ZiZ_{i}Zi 沿 Xi−1X_{i-1}Xi1 方向的距离;

αi\alpha_{i}αi: Zi−1Z_{i-1}Zi1ZiZ_{i}ZiXi−1X_{i-1}Xi1 旋转的角度

坐标系 Oi−1O_{i-1}Oi1 与关节 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} ii1T=cosθisinθi00sinθicosαicosθicosαisinαi0sinθisinαicosθisinαicosαi0aicosθiaisinθidi1

Modified DH:

Modified D-H 的关节和坐标系关系中各个参数的含义如下:

αi−1\alpha_{i-1}αi1Zi−1Z_{i-1}Zi1ZiZ_{i}ZiXi−1X_{i-1}Xi1 旋转的角度;

ai−1a_{i-1}ai1Zi−1Z_{i-1}Zi1ZiZ_{i}Zi 沿 Xi−1X_{i-1}Xi1 方向的距离;

θi\theta_{i}θiXi−1X_{i-1}Xi1XiX_{i}XiZiZ_{i}Zi 旋转的角度;

did_{i}diXi−1X_{i-1}Xi1XiX_{i}Xi沿 ZiZ_{i}Zi 方向的距离。

坐标系 Oi−1O_{i-1}Oi1 与关节 i−1i-1i1 对齐,其 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} ii1T=cosθisinθicosαi1sinθisinαi10sinθicosθicosαi1cosθisinαi100sinαi1cosαi10ai1disinαi1dicosαi11

Modified DH 克服了 Classic DH 在用于树型结构机器人时可能出现的问题,比较常用,故之后主要介绍这种方法,并使用该方法进行建模。

机械臂连杆坐标系的建立

建立机械臂连杆坐标系的步骤:

a. 确定各个关节轴和连杆,坐标系的 ZZZ 轴沿关节轴线方向;

b. 找出相邻两关节轴线的交点或公垂线,用于确定坐标系 {i}\{i\}{i} 的原点:以关节轴 iiii+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 轴与公垂线重合,方向为 iiii+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_nXnXn−1X_{n-1}Xn1 方向相同,选取原点使 dn=0d_n = 0dn=0;对于移动关节,取 XnX_nXn 方向使 θn=0\theta_n = 0θn=0,当 dn=0d_n = 0dn=0 时,取 Xn−1X_{n-1}Xn1XnX_nXn 的交点为原点。

D-H 参数表

根据机械臂各个连杆间坐标系的关系,采用 Modified D-H 形式,得到的参数表如下。

iiiαi−1\alpha_{i-1}αi1ai−1a_{i-1}ai1θi−1\theta_{i-1}θi1did_{i}diθ\thetaθ 的范围
10∘0^{\circ}0000θ1\theta_{1}θ1000(−2π3,2π3)(-\frac{2 \pi}{3}, \frac{2 \pi}{3})(32π,32π)
2−90∘-90^{\circ}90a1a_{1}a1θ2\theta_{2}θ2000(−π2,0)(-\frac{\pi}{2}, 0)(2π,0)
30∘0^{\circ}0a2a_{2}a2θ3\theta_{3}θ3000(−2π3,2π3)(-\frac{2 \pi}{3}, \frac{2 \pi}{3})(32π,32π)
40∘0^{\circ}0a3a_{3}a3θ4\theta_{4}θ4000(−7π6,π6)(-\frac{7 \pi}{6}, \frac{\pi}{6})(67π,6π)
5−90∘-90^{\circ}90000θ5\theta_{5}θ5000(−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}T43T54T_{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+c1c2c3c4c5c1c2c5s3s4c1c3c5s2s4c1c4c5s2s3ny=c2c3c4c5s1c1s5c2c5s1s3s4c3c5s1s2s4c4c5s1s2s3nz=c5s2s3s4c2c4c5s3c3c4c5s2c2c3c5s4ox=c5s1c1c2c3c4s5+c1c2s3s4s5+c1c3s2s4s5+c1c4s2s3s5oy=c2s1s3s4s5c2c3c4s1s5c1c5+c3s1s2s4s5+c4s1s2s3s5oz=c2c3s4s5+c2c4s3s5+c3c4s2s5s2s3s4s5ax=c1s2s3s4c1c2c4s3c1c3c4s2c1c2c3s4ay=s1s2s3s4c2c4s1s3c3c4s1s2c2c3s1s4az=c2s3s4+c3s2s4+c4s2s3c2c3c4px=a1c1+a2c1c2+a3c1c2c3a3c1s2s3py=a1s1+a2c2s1+a3c2c3s1a3s1s2s3pz=a2s2a3c2s3a3c3s2

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
评论 23
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

Metaphysicist.

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付:¥1
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者

实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值