《机器人学导论》 第三章 操作臂运动学

第三章 操作臂运动学

3.1 连杆及连杆连接的描述

3.1.1 连杆参数相关概念

1. 连杆长度$a_{i-1}$：关节轴$i-1$和关节轴$i$之间公垂线的长度，表示连接连杆$i-1$两端关节轴的公垂线长度；

2. 连杆扭转角${\alpha }_{i-1}$：关节轴$i-1$和关节轴$i$之间的夹角

3. 连杆偏距$d_i$：两相邻连杆$i-1$和$i$与关节轴$i$两交点之间的距离，表示公垂线$a_{i-1}$与关节轴$i$的交点到公垂线$a_i$与关节轴$i$的焦点的有向距离

4. 关节角${\theta }_i$：两相邻连杆$i-1$和$i$绕关节轴$i$旋转的夹角

当关节$i$是转动关节时，关节角${\theta }_i$为关节变量，其他三个连杆参数固定不变；当关节$i$是移动关节时，连杆偏距$d_i$为关节变量，其他三个连杆参数固定不变。这种用连杆参数描述机构运动关系的规则为DH方法，这些参数被称为DH参数。

3.1.2 Denavit-Hartenberg方法

3.1.2.1 修正DH参数——定义关节i的轴为$z_i$轴(最后一个坐标系{n}在关节n处）

3.1.2.1.1 连杆坐标系定义

• (a) 运动链中间位置连杆坐标系{$i$}的定义

  固连在连杆i上的固连坐标系称为坐标系{$i$}
  $O_i$原点：关节轴i和i+1的交点或关节轴i和i+1公垂线与关节轴i的交点
  ${\hat{Z}}_i$轴：关节轴$i$
  ${\hat{X}}_i$轴：沿${\hat{Z}}_i$和${\hat{Z}}_{i+1}$的公垂线$a_i$方向由关节$i$指向关节$i+1$，当$a_i=0$时，${\hat{X}}_i$垂直于${\hat{Z}}_i$和${\hat{Z}}_{i+1}$所在的平面
  ${\hat{Y}}_i$轴：根据右手法则确定
  ${\alpha }_i$：根据右手定则，绕${\hat{X}}_i$轴从${\hat{Z}}_{i-1}$到${\hat{Z}}_i$

• （b） 运动链首段连杆{$0$}坐标系的定义（${\hat{Z}}_0$与${\hat{Z}}_1$同向）

  固连于机器人基座（即连杆0）上的坐标系为坐标系{0}，该坐标系可做参考坐标系。
  设定${\hat{Z}}_0$沿关节轴1的方向，当关节变量1（$d_1$或 ${\theta }_1$）为0时，设定参考坐标系{0}与{1}重合。总有$a_0={\alpha }_0=0$，当关节1为移动关节时，${\theta }_1=0$，当关节1为转动关节时，$d_1=0$

（c） 运动链末端连杆{$n$}坐标系的定义（${\hat{X}}_N$和${\hat{X}}_{N-1}$同向）

{n}坐标原点选取在${\hat{X}}_{N-1}$轴与关节轴n的交点位置，使得$d_n=0$，总有$a_n={\alpha }_n=0$
对于转动关节n，始终有$d_n=0$，当${\theta }_n=0$时，设定${\hat{X}}_N$和${\hat{X}}_{N-1}$同向，选取坐标原点位置为${\hat{X}}_{N-1}$轴与关节轴n的交点位置。
对于移动关节n，始终有${\theta }_n=0$，当$d_n=0$时，设定${\hat{X}}_N$和${\hat{X}}_{N-1}$同向，选取坐标原点位置为${\hat{X}}_{N-1}$轴与关节轴n的交点位置。

3.1.2.1.2 连杆参数在连杆坐标系中的表示方法

如果按照上述方法将两岸坐标系固定于连杆上时，连杆参数可以定义为

1. $a_{i-1}$：沿${\hat{X}}_{i-1}$轴，从${\hat{Z}}_{i-1}$移动到${\hat{Z}}_i$的距离，通常取正值；

2. ${\alpha }_{i-1}$：绕${\hat{X}}_{i-1}$轴，从${\hat{Z}}_{i-1}$旋转到${\hat{Z}}_i$的角度，采用右手法则判断正负；

3. $d_i$：沿${\hat{Z}}_i$轴，从${\hat{X}}_{i-1}$移动到${\hat{X}}_i$的距离；

4. ${\theta }_i$：绕${\hat{Z}}_i$轴，从${\hat{X}}_{i-1}$旋转到${\hat{X}}_i$的角度，采用右手法则判断正负；

3.1.2.1.3 建立连杆坐标系的步骤

现根据关节轴确定Z轴，再跟相邻关节轴之间的公垂线确定X轴。具体步骤如下：

1. 找出各关节轴， 并标出（或画出）这些轴线的延长线。 在下面的步骤2至步骤5中，仅考虑两个相邻的轴线（关节轴$i$和$i+1$)；

2. 找出关节轴$i$和$i+1$之间的公垂线$a_i$或关节轴$i$和$i+1$的交点， 以关节轴$i$和$i+1$的交点或公垂线$a_i$与关节轴$i$的交点作为连杆坐标系{$i$}的原点；

3. 规定${\hat{Z}}_i$轴沿关节轴i的指向；

4. 规定${\hat{X}}_i$轴沿公垂线$a_i$的指向， 如果关节轴i和i+1相交， 则规定${\hat{X}}_i$轴垂直于关节轴i和i+1所在的平面；

5. 按照右手定则确定${\hat{Y}}_i$轴；

6. 当第一个关节变量为 0 时， 规定坐标系｛0｝和｛1｝重合。 对于坐标系｛N｝， 其原点和的${\hat{X}}_N$方向可以任意选取。 但是在选取时， 通常尽量使连杆参数为0。

3.1.2.1.4 连杆变换的推导

通过定义三个中间坐标系{P},{Q},{R}建立坐标系{i}相对于坐标系{i-1}的变换：
{i}沿${\hat{Z}}_i$平移$d_i$得到{P}，{P}绕${\hat{Z}}_P$旋转${\theta }_i$得到{Q}，{Q}沿${\hat{X}}_Q$平移$a_{i-1}$得到{R}，{R}绕${\hat{X}}_R$旋转${\alpha }_{i-1}$得到{i-1}

KaTeX parse error: \tag works only in display equations
其中，
KaTeX parse error: \tag works only in display equations

3.1.2.2 标准DH参数——定义关节i的轴为$z_{i-1}$轴（最后一个坐标系{n}在连杆n的末端）

3.1.2.2.1 连杆坐标系定义

• （a）运动链中间位置连杆坐标系{$i$}的定义

  ${\hat{Z}}_i$轴：关节轴$i+1$
  ${\hat{X}}_i$轴：沿轴$z_{i-1}$和轴$z_i$的公垂线$a_i$方向由关节$i$指向关节$i+1$，当$a_i=0$时，即轴$z_{i-1}$和轴$z_i$相交时，${\hat{X}}_i$垂直于${\hat{Z}}_{i-1}$和${\hat{Z}}_i$所在的平面
  ${\hat{Y}}_i$轴：根据右手法则确定
  $O_i$点：在关节$i+1$的轴$Z_i$轴与公垂线$a_i$的交点

• （b）运动链首段连杆{$0$}坐标系的定义

  设定${\hat{Z}}_0$沿关节轴1的方向，$O_0$和${\hat{X}}_0$可以任意选择

• （c）运动链末端连杆末端执行器手部{$n$}坐标系的定义

  对坐标系{n}而言，由于没有关节n+1，但x_n轴必须与轴$z_{n-1}$垂直，但$z_n$不是唯一定义的，当关节n是转动的，$z_n$依照$z_{n-1}$的方向设置

3.1.2.2.2 连杆参数在连杆坐标系中的表示方法

如果按照上述方法将连杆坐标系固定于连杆上时，下标为i的连杆参数可以由坐标系$i$和坐标系$i-1$的位置和方向定义为

1. $a_i$：沿${\hat{X}}_i$轴，从${\hat{Z}}_{i-1}$移动到${\hat{Z}}_i$的距离，Link_i的长度，通常取正值；

2. ${\alpha }_i$：绕${\hat{X}}_i$轴，从${\hat{Z}}_{i-1}$旋转到${\hat{Z}}_i$的角度，采用右手法则判断正负；

3. $d_i$：沿${\hat{Z}}_{i-1}$轴，从${\hat{X}}_{i-1}$移动到${\hat{X}}_i$的距离，即$x_{i-1}$和$x_i$与$z_{i-1}$轴交点的距离；

4. ${\theta }_i$：绕${\hat{Z}}_{i-1}$轴，从${\hat{X}}_{i-1}$旋转到${\hat{X}}_i$的角度，采用右手法则判断正负；

4 个参数中有2 个（$a_i$和${\alpha }_i$〉始终为常数，只取决于由连杆i建立的相继关节之间的几何连接关系。其他两个参数中只有一个是变量，取决于连接连杆i-1 和连杆i 的关节的类型。详述如下：

• 如果关节i 是转动型的， 则变最为${\theta }_i$。
• 如果关节i 是移动型的，则变量为$d_i$。

3.1.2.2.3 建立连杆坐标系的步骤

3.1.2.2.4 连杆变换的推导

$\begin{array}{rl}{}_i^{i-1}T(q_i)& =R_Z({\theta }_i)D_Z(d_i)R_Z({\alpha }_i)D_X(a_i)\\ & =[\begin{array}{cccc}{c}_{{\theta }_i}& -s_{{\theta }_i}& 0& 0\\ s_{{\theta }_i}& c_{{\theta }_i}& 0& 0\\ 0& 0& 1& d_i\\ 0& 0& 0& 1\end{array}]\left[\begin{array}{cccc}1& 0& 0& a_i\\ 0& c_{{\alpha }_i}& -s_{{\alpha }_i}& 0\\ 0& s_{{\alpha }_i}& c_{{\alpha }_i}& 0\\ 0& 0& 0& 1\end{array}\right]\\ & =[\begin{array}{cccc}{c}_{{\theta }_i}& -s_{{\theta }_i}{c}_{{\alpha }_i}& s_{{\theta }_i}{s}_{{\alpha }_i}& a_i{c}_{{\theta }_i}\\ s_{{\theta }_i}& c_{{\theta }_i}{c}_{{\alpha }_i}& -c_{{\theta }_i}{s}_{{\alpha }_i}& a_i{s}_{{\theta }_i}\\ 0& s_{{\alpha }_i}& c_{{\alpha }_i}& d_i\\ 0& 0& 0& 1\end{array}]\end{array}$

从坐标系 i 到坐标系i - 1 的变换矩阵是一个只与关节变量$q_i$有关的函数，如果是
转动关节则变量为${\theta }_i$， 如果是移动关节则变量为$d_i$。

3.1.3 MDH和SDH两者对比

3.1.3.1 平面3R机器人

• 标准DH参数

• 修正DH参数
  待补充

3.2 驱动器空间、关节空间、笛卡尔空间

3.2.1 驱动器空间

我们已知假设每个运动关节都直接由某种驱动器驱动。但并非如此。有时两个驱动器以差动方式驱动一个关节，有时用直线驱动器通过四连杆机构驱动旋转关节。这就需要考虑驱动器位置。由于测量操作臂位置的传感器常常安装在驱动器上，因此进行某些计算时必须把关节矢量表示成一组驱动器函数，即驱动器矢量。

3.2.2 关节空间

对于一个具有n个自由度的操作臂来说，他的所有连杆位置可由一组n个关节变量加以确定，这个$n\times 1$的变量称为关节矢量。所有关节矢量组成的空间称为关节空间。

3.2.3 笛卡尔空间

当位置是在空间相互正交的轴上测量，且姿态是按照第2章的任何一种规定测量时（角坐标系表示法共24中，其中12中为固定角坐标系法，12种为欧拉角坐标系法。附录B有24种角坐标系的旋转矩阵定义），我们称这个空间为笛卡尔空间，有时称为任务空间或操作空间。

3.3 两种典型机器人运动学问题

平行轴可以先利用和差化积求解$T$，如下面例题中的${}_1^{3}T$，$T$相似的也可以先乘起来，如下面例题中的${}_3^{6}T$

3.3.1 Unimation PUMA 560机器人

(1) Frames

(2) 建立坐标轴

(3) 建立6自由度的DH表格

(4) 列出6个旋转矩阵

(5) 合并旋转矩阵

3.4 坐标系的标准命名

3.4.1 基坐标系{$B$}

**基坐标系｛B｝位于操作臂的基座上。**它仅是赋予坐标系｛0｝的另一个名称。 因为它固连连在机器人的静止部位， 所以有时称为连杆0

3.4.2 固定坐标系{$S$}

又名任务坐标系、 世界坐标系或通用坐标系

固定坐标系｛S｝位置与任务相关。 固坐标系｛S｝是一个通用坐标系， 机器人所有的运动都是相对于它来执行的。固定坐标系通常根据基坐标系确定， 即${}_S^{B}T$.

3.4.3 腕部坐标系{$W$}

腕部坐标系{W}附于操作臂的末端连杆。 这个固连在机器人的末端连杆上的坐标系也可以称为坐标系｛N｝。大多数情况， 腕部坐标系{W}的原点位于操作臂手腕上， 它随操作臂的末端连杆移动，根据基坐标系确定． 即 { W } = W B T = N 0 T \{W\}=^B_WT=^0_NT {W}=WB​T=N0​T。

3.4.4 工具坐标系{$T$}

工具坐标系｛T｝附于机器人所夹持工具的末端。 手部没有夹持工具时，工具坐标系{T｝的原点位于机器人的指尖之间。 工具坐标系通常相对下腕部坐标系来确定。 下图中，工具坐标系的原点定义在机器人抓持轴销的未端。

3.4.5 目标坐标系{$G$}

目标坐标系｛G｝是对机器人移动工具到达的位置描述。特指在机器人运动结束时，工具坐标系应当与目标坐标系重舍。 目标坐标系{G}通常根据固定坐标系来确定。下图中，目标坐标系为位于将要插入轴销的轴孔。

3.5 matlab习题

%D:\Users\jinqiyuan\Documents\MATLAB\Introduction to Robotics\charpter_3\Exercise1.m
%根据DH参数定义变量
syms alpha0 a0 d1 theta1 ...
alpha1 a1 d2 theta2 ...
alpha2 a2 d3 theta3
alpha0=0;a0=0;d1=0;
alpha1=0;a1=4;d2=0;
alpha2=0;a2=3;d3=0;

syms alphai_1 ai_1 di thetai
i_1iT=[cos(thetai) -sin(thetai) 0 ai_1
sin(thetai)*cos(alphai_1) cos(thetai)*cos(alphai_1) -sin(alphai_1) -sin(alphai_1)*di
sin(thetai)*sin(alphai_1) cos(thetai)*sin(alphai_1) cos(alphai_1) cos(alphai_1)*di
0 0 0 1];%利用书中公式
T01=subs(i_1iT,[alphai_1 ai_1 di thetai],[alpha0 a0 d1 theta1])
T12=subs(i_1iT,[alphai_1 ai_1 di thetai],[alpha1 a1 d2 theta2])
T23=subs(i_1iT,[alphai_1 ai_1 di thetai],[alpha2 a2 d3 theta3]) % sub()是symbolic中的替换函数
T3H=transl(2,0,0) % 来自工具箱

T03=T01*T12*T23
T0H=T01*T12*T23*T3H
% 关节角度参数为[0 0 0]
T03i=subs(T03,[theta1 theta2 theta3],[0 0 0])
T0Hi=subs(T0H,[theta1 theta2 theta3],[0 0 0])
% 角度为[10 20 30]
T03ii=subs(T03,[theta1 theta2 theta3],deg2rad([10 20 30]));
double(T03ii)
T0Hii=subs(T0H,[theta1 theta2 theta3],deg2rad([10 20 30]));
double(T0Hii) % 这里用double()输出具体数值，不然就会只代入不计算
% 角度为[90 90 90]
T03iii=subs(T03,[theta1 theta2 theta3],deg2rad([90 90 90]))
T0Hiii=subs(T0H,[theta1 theta2 theta3],deg2rad([90 90 90]))

%% 工具箱检查
%用link()定义三根连杆
syms theta1 theta2 theta3
L(1)=Link('alpha',0,'a',0,'d',0,'modified');
L(2)=Link('alpha',0,'a',4,'d',0,'modified');
L(3)=Link('alpha',0,'a',3,'d',0,'modified'); % modified表示需要改进的DH参数
bot=SerialLink(L,'name','robot','tool',[2,0,0]) % Serialink表示把三根杆连起来，'tool'表示工具位置

T01=L(1).A(theta1)
T12=L(2).A(theta2)
T23=L(3).A(theta3) % link类的方法A(q)可以根据q求出杆的变换矩阵T，返回SE3类，即工具箱中定义的4*4的齐次变换矩阵
T3H=SE3(2,0,0) % 为了统一，用SE3()创建T3H

T03=bot.A(1:3,[theta1,theta2,theta3]) %SerialLink类的方法A(jlist,q)输入关节的编号jlist和对应的角度q，输出变换矩阵。可用来求c)中的T03。
T0H=bot.fkine([theta1,theta2,theta3]) %SerialLink类的方法fkine(q)根据输入角度q，输出末端执行器的正向运动学矩阵。可用来求c)中的T0H。
T03i=bot.A(1:3,[0,0,0])
T0Hi=bot.fkine([0,0,0])
T03ii=bot.A(1:3,deg2rad([10,20,30]))
T0Hii=bot.fkine(deg2rad([10,20,30]))
T03iii=bot.A(1:3,deg2rad([90,90,90]))
T0Hiii=bot.fkine(deg2rad([90,90,90]))