智能机器人与旋量代数(12)_denavit-hartenberg-优快云博客

本文链接：https://blog.youkuaiyun.com/Metaphysicist/article/details/135816892

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$ , $d$ , $θ\theta$ ；

c. 列D-H参数表；

d.由参数表得到变换矩阵；

D-H 建模方法中，每个连杆使用 4 个参数 $α\alpha$ , $a$ , $d$ , $θ\theta$ 来描述，2 个描述连杆本身，另外 2 个描述与相邻连杆的位姿（连接或几何关系）。

对于转动关节，其中 $θ\theta$ 为关节变量，其他三个参数固定不变，为连杆参数；对于移动关节， $d$ 为关节变量，其他三个为关节参数。

根据连杆坐标系和关节对应关系的不同，D-H 建模法可以分为传统 D-H (Classic DH) 和改进 D-H (Modified DH)，二者的主要区别如下表所示。

区别	Classic D-H	Modified D-H
连杆固定坐标系的位置	后一个关节坐标系	前一个关节坐标系
$X$ 轴的确定方式	当前坐标系 $Z$ 轴和前一个坐标系 $Z$ 轴的向量积	后一个坐标系 $Z$ 轴与当前坐标系 $Z$ 轴的向量积
坐标系间的参数变换顺序	$θ\theta$ , $d$ , $a$ , $α\alpha$	$α\alpha$ , $a$ , $θ\theta$ , $d$

Classic D-H:

Classic DH 的关节和坐标系关系中各个参数的含义如下：

$θi\theta_{i}$ : $X_{i-1}$ 到 $X_{i}$ 绕 $Z_{i-1}$ 旋转的角度；

$d_{i}$ : $X_{i-1}$ 到 $X_{i}$ 沿 $Z_{i-1}$ 方向的距离；

$a_{i}$ ： $Z_{i-1}$ 到 $Z_{i}$ 沿 $X_{i-1}$ 方向的距离；

$αi\alpha_{i}$ : $Z_{i-1}$ 到 $Z_{i}$ 绕 $X_{i-1}$ 旋转的角度

坐标系 $O_{i-1}$ 与关节 $i$ 对齐，其 D-H 参数矩阵为：

$_{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}$

Modified DH:

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

$αi−1\alpha_{i-1}$ ： $Z_{i-1}$ 到 $Z_{i}$ 绕 $X_{i-1}$ 旋转的角度；

$a_{i-1}$ ： $Z_{i-1}$ 到 $Z_{i}$ 沿 $X_{i-1}$ 方向的距离；

$θi\theta_{i}$ ： $X_{i-1}$ 到 $X_{i}$ 绕 $Z_{i}$ 旋转的角度；

$d_{i}$ ： $X_{i-1}$ 到 $X_{i}$ 沿 $Z_{i}$ 方向的距离。

坐标系 $O_{i-1}$ 与关节 $i - 1$ 对齐，其 D-H 参数矩阵为：
在这里插入图片描述

$_{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}$

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

机械臂连杆坐标系的建立

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

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

b. 找出相邻两关节轴线的交点或公垂线,用于确定坐标系 ${i\}$ 的原点:以关节轴 $i$ 和 $i + 1$ 的交点或公垂线与关节轴 $i$ 的交点为原点；

c. 确定 $X$ 轴:两轴线相交时, $Xi⃗=±Zi+1⃗×Zi⃗\vec{X_{i}} = \pm \vec{Z_{i+1}} \times \vec{Z_{i}}$ ;两轴线不相交时, $X_{i}$ 轴与公垂线重合,方向为 $i$ 到 $i + 1$ ；

d. 右手定则确定 $Y_{i}$ 轴；

e. 确定基坐标系 ${0\}$ :为了简化问题, $Z_0$ 通常与关节 1 的轴线方向重合,且当关节变量 1 为 0 时,坐标系 ${0\}$ 与 ${1\}$ 重合；

f. 确定末端坐标系 ${n\}$ :对于转动关节, $θn=0\theta_n = 0$ 时, $X_n$ 与 $X_{n-1}$ 方向相同,选取原点使 $d_n = 0$ ;对于移动关节,取 $X_n$ 方向使 $θn=0\theta_n = 0$ ,当 $d_n = 0$ 时,取 $X_{n-1}$ 与 $X_n$ 的交点为原点。

D-H 参数表：

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

$i$	$αi−1\alpha_{i-1}$	$a_{i-1}$	$θi−1\theta_{i-1}$	$d_{i}$	$θ\theta$ 的范围
1	$0∘0^{\circ}$	$0$	$θ1\theta_{1}$	$0$	$(−2π3,2π3)(-\frac{2 \pi}{3}, \frac{2 \pi}{3})$
2	$−90∘-90^{\circ}$	$a_{1}$	$θ2\theta_{2}$	$0$	$(−π2,0)(-\frac{\pi}{2}, 0)$
3	$0∘0^{\circ}$	$a_{2}$	$θ3\theta_{3}$	$0$	$(−2π3,2π3)(-\frac{2 \pi}{3}, \frac{2 \pi}{3})$
4	$0∘0^{\circ}$	$a_{3}$	$θ4\theta_{4}$	$0$	$(−7π6,π6)(-\frac{7 \pi}{6}, \frac{\pi}{6})$
5	$−90∘-90^{\circ}$	$0$	$θ5\theta_{5}$	$0$	$(−2π3,2π3)(-\frac{2 \pi}{3}, \frac{2 \pi}{3})$

在这里插入图片描述

齐次变换矩阵

将 DH 参数表代入 Modified DH 的 DH 参数矩阵，可以得到各个坐标系间的齐次变换矩阵 $_{1}^{0}T$ , $_{2}^{1}T$ , $_{3}^{2}T$ , $_{4}^{3}T$ 和 $_{5}^{4}T$ 则可得基坐标系到末端坐标系的齐次变换矩阵:

$_{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}$

其中，
$\begin{bmatrix} p_{x} & p_{y} & p_{z} \end{bmatrix}^T$
为机械臂末端在基坐标系中的位置，
$[nxnynz]T\begin{bmatrix} n_{x} & n_{y} & n_{z} \end{bmatrix}^T$
为机械臂末端坐标系 $X$ 轴在基坐标系中的方向矢量，
$[oxoyoz]T\begin{bmatrix} o_{x} & o_{y} & o_{z} \end{bmatrix}^T$
为机械臂末端坐标系 $Y$ 轴在基坐标系中的方向矢量，
$[axayaz]T\begin{bmatrix} a_{x} & a_{y} & a_{z} \end{bmatrix}^T$
为机械臂末端坐标系 $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}$

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