机器人基础之DH建模

齐次变换矩阵

绕X轴旋转$\theta$角度的齐次变换矩阵为： $$\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& \cos \theta & -\sin \theta & 0\\ 0& \sin \theta & \cos \theta & 0\\ 0& 0& 0& 1\end{array}\right]\text{\hspace{1em}(n)}$$
绕Y轴旋转$\theta$角度的齐次变换矩阵为： $$\left[\begin{array}{cccc}\cos \theta & 0& \sin \theta & 0\\ 0& 1& 0& 0\\ -\sin \theta & 0& \cos \theta & 0\\ 0& 0& 0& 1\end{array}\right]\text{\hspace{1em}(n)}$$
绕Z轴旋转$\theta$角度的齐次变换矩阵为： $$\left[\begin{array}{cccc}\cos \theta & -\sin \theta & 0& 0\\ \sin \theta & \cos \theta & 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\text{\hspace{1em}(n)}$$
绕X轴平移x长度的齐次变换矩阵为： $$\left[\begin{array}{cccc}1& 0& 0& x\\ 1& 1& 0& 0\\ 1& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\text{\hspace{1em}(n)}$$
绕Y轴平移y长度的齐次变换矩阵为： $$\left[\begin{array}{cccc}1& 0& 0& 0\\ 1& 1& 0& y\\ 1& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\text{\hspace{1em}(n)}$$
绕Z轴平移z长度的齐次变换矩阵为： $$\left[\begin{array}{cccc}1& 0& 0& 0\\ 1& 1& 0& 0\\ 1& 0& 1& z\\ 0& 0& 0& 1\end{array}\right]\text{\hspace{1em}(n)}$$

用MATLAB实现齐次变换矩阵

function rot_X_matrix = rotX(theta)
% 计算绕X轴旋转的矩阵
rot_X_matrix = [
    1, 0, 0, 0;
    0, cos(theta), -sin(theta), 0;
    0, sin(theta), cos(theta), 0;
    0, 0, 0, 1];

function rot_Y_matrix = rotY(theta)
% 计算绕Y轴旋转的矩阵
rot_Y_matrix = [
   cos(theta), 0, sin(theta), 0;
   0, 1, 0, 0;
   -sin(theta), 0, cos(theta), 0;
   0, 0, 0, 1];

function rot_Z_matrix = rotZ(theta)
% 计算绕Z轴旋转的矩阵
rot_Z_matrix = [
   cos(theta), -sin(theta), 0, 0;
   sin(theta), cos(theta), 0, 0;
   0, 0, 1, 0;
   0, 0, 0, 1];

function trans_X_matrix = transX(scaler)
% 计算沿X轴平移的向量
trans_X_matrix = [
    1, 0, 0, scaler;
    0, 1, 0, 0;
    0, 0, 1, 0;
    0, 0, 0, 1];

function trans_Y_matrix = transY(scaler)
% 计算沿Y轴平移的向量
trans_Y_matrix = [
   1, 0, 0, 0;
   0, 1, 0, scaler;
   0, 0, 1, 0;
   0, 0, 0, 1];

function trans_Z_matrix = transZ(scaler)
% 计算沿Z轴平移的向量
trans_Z_matrix = [
   1, 0, 0, 0;
   0, 1, 0, 0;
   0, 0, 1, scaler;
   0, 0, 0, 1];

DH建模

DH建模方法一种基于齐次变换矩阵的低副机构建模方法（低副指压强低的运动副，常见的有移动副和转动副，齿轮副属于高副）。
DH建模方法简单，具有通用性，广泛用于机器人的运动学建模。
在应用DH方法前，首先需要在各运动刚体上固连坐标轴。
在选取Z轴方向的时候，若运动是旋转运动，Z轴方向是旋转轴的方向；若运动直线运动，Z轴方向是直线运动的方向。
X轴沿两相邻运动轴的公垂线。
DH建模方法可以概括为：先使Z轴重合，再使X轴重合。
具体有以下四步：

1. 转动X轴，使得两坐标系的Z轴同向
2. 移动X轴，使得两坐标系的Z轴重合
3. 转动Z轴，使得两坐标系的X轴同向
4. 移动Z轴，使得两坐标系的X轴重合

由于每次变换均是相对于动坐标系，因此变换矩阵依次右乘。

MATLAB实现DH变换矩阵

function Matrix_DH = DH(DH_parameter)
% 输入参数为DH参数表，参数表有四列，参数表的行数是运动轴的个数。比如六自由度机械臂的参数表为6行4列
% 第一列是Z轴偏置trans_X；
% 第二列是Z轴偏转角theta_X；
% 第三列是X轴偏置trans_Z；
% 第四列是X轴偏转角theta_Z；

[dim_row, dim_col] = size(DH_parameter);
Matrix_DH = eye(4);

for row_index = 1:dim_row
    % 先绕X轴转theta_X(Z轴偏转角)， 再沿X轴平移trans_X(Z轴偏置)，将两个坐标系的Z轴重合
    theta_X = DH_parameter(row_index, 2);
    trans_X = DH_parameter(row_index, 1);
    T_X = rotX(theta_X) * transX(trans_X);
    
    % 先绕Z轴转theta_Z(X轴偏转角)， 再沿Z轴平移trans_Z(X轴偏置)，将两个坐标系的X轴重合
    theta_Z = DH_parameter(row_index, 4);
    trans_Z = DH_parameter(row_index, 3);
    T_Z = rotZ(theta_Z) * transZ(trans_Z);
   
    Matrix_DH = Matrix_DH * T_X * T_Z;
end

算例

某RRRP机器人的DH参数表如下。

i	a	$\alpha$	d	$\theta$
1	0	0	0	${\theta }_1$
2	10	0	0	${\theta }_2$
3	20	-90	0	${\theta }_3$
4	0	0	30	0

当${\theta }_1$、${\theta }_2$、${\theta }_3$均为0时，在MATLAB编写以下代码。

theta1 = 0;
theta2 = 0;
theta3 = 0;

para = [
    0, 0, 0, theta1;
    10, 0, 0, theta2;
    20, -90/180*pi, 0, theta3;
    0, 0, 30, 0];

T = DH(para)

最终计算得到矩阵T如下。

T =

    1.0000         0         0   30.0000
         0    0.0000    1.0000   30.0000
         0   -1.0000    0.0000    0.0000
         0         0         0    1.0000