一、机械臂运动学建模原理

机械臂可以看作为一个开式运动链，它由一系列连杆通过转动关节串联而成。为了研究这些连杆之间的关系，可以在每个连杆上固接一个坐标系，然后描述这些坐标系之间的关系。Denavit和Hartenberg提出了一种通用的方法，即D-H方法，用一个4X4的齐次变换矩阵描述相邻两连杆的关系，从而推导出末端坐标系相对于基坐标系的齐次变换矩阵，建立机械臂的运动学方程。

对于一个拥有N个关节的机械臂来说，它有N+1个杆件，编号为0到N，其中，杆件0是操作臂的基底，杆件N携带着机械臂的末端执行器，通常指手爪。关节j连接杆件j-1和杆件j，关节j的转动带动杆件j运动。一个杆件可以用两个参数定义，杆件的长度a j 和杆件的扭角a j ，关节也可以用两个参数表示，关节转角 j  和偏置 。具体如图1所示，在关节j和关节j+1处分别固定坐标系{j-1}和{j}，D-H参数a j 、 j 、 j 、d j的定义如表1所示。

图1 连杆坐标系设定

表 1 D-H参数定义

图 2 手爪坐标系

此外，针对关节1，它连接了杆件0和杆件1，杆件0是机械臂的基底，通常我们定义d1 1  0，针对最后一个关节N，它连接了杆件N-1和杆件N，杆件N是机械臂的手爪，参数dN 和 aN表示手爪的长度和手爪x轴的偏置，此外，Z轴的方向就是手爪的指向，如图2中，a的方向是z轴的方向。则基于上面的定义，坐标系{j-1}到坐标系{j}之间的转换关系为：

     （1）

连杆变化j1Aj 依赖于四个参数 j , aj , d j , j  ，其中，针对转动关节j，j1Aj是 j  的函数。将各个连杆变化j1Aj（j=1,2,3…N）相乘，得到：

（2）

 0AN 称为手臂变换矩阵。它是N个关节变量1， 2... N的函数，表示末端连杆坐标系{N}^[19]相对于基坐标系{0}的描述，根据各个关节位置传感器的输出，得到各关节变量1， 2... N的值，即可以得出0AN。

    （3）

式（3）称为运动方程。它表示末端连杆的位姿（n,o,a,p）与关节变量1， 2... N之间的关系。

二、遥操作机器人主端设备正运动学建模

l 主端设备连杆参数和连杆坐标系

本文采用Geomagic Touch设备作为主端机械臂，对其各个关节建立相应的坐标系，如图3和图4所示。

图3 Touch设备结构及关节情况

图 4 Touch各关节坐标系简图

根据其结构和尺寸建立了相应的DH参数表，如表2所示。

表 2 Touch设备D-H参数表

l 主端设备运动学方程

根据式（1）和式（2）以及主端Touch设备的D-H参数表，可以得到主端设备的运动学方程：

利用MATLAB软件中的Robotics Toolbox，可以加快机器人运动学的求解。该工具箱允许我们基于机器人D-H参数表用m语言建立相应的机器人模型，并在此基础上，为我们提供了许多方法，来便捷地操作机器人。基于上节建立的D-H参数表，可以在MATLAB中建立相应的Touch设备模型，如下面代码所示：

mdl_Touch.m:

L(1)=Revolute('d',0,'a',0,'alpha',-pi/2);

L(2)=Revolute('d',0,'a',0.13335,'alpha',0);

L(3)=Revolute('d',0,'a',0,'alpha',pi/2,'offset',-pi/2);

L(4)=Revolute('d',-0.13335,'a',0,'alpha',-pi/2);

L(5)=Revolute('d',0,'a',0,'alpha',pi/2,'offset',pi/2);

L(6)=Revolute('d',0,'a',0,'alpha',pi,'offset',-pi/2);

Touch = SerialLink(L,'name','Touch');

Touch.base=transl(0,0.02339,-0.16841)*eul2tform([pi/2,-pi/2,0]);

   建立了相应的模型后，就可以使用Robotics Toolbox工具箱所提供的方法，如：T=Touch.fkine([0 0 0 0 0 0])

利用fkine的方法来快速求解关节角度均为0时末端连杆的位姿矩阵在基坐标下的表示。利用plot的方法来画出关节角度均为0时机器人的状态，如图5所示，这与实际Touch设备在关节角度均为0时的机器人状态相同。

图 5 MATLAB中Touch模型在关节角度均为0时的姿态

二、遥操作机器人从端设备正运动学建模

工业机械臂

l 从端设备连杆参数和连杆坐标系

从端机械臂采用某商用机械臂，对其各个关节建立相应的坐标系，如图6所示。

图 6 从端机械臂结构及各关节坐标系

根据其结构和尺寸建立相应的DH参数表，如表7所示。

表 7 从端机械臂D-H参数表

l 从端设备运动学方程

根据式（1）和式（2）以及从端机械臂的D-H参数表，可以得到从端设备的运动学方程：

仍然使用MATLAB机器人RVC工具箱，即正运动学函数fkine()来实现六轴机械臂正运动学求解，基于上节建立的D-H参数表，可以在MATLAB中建立相应的从端设备模型，则MATLAB代码如下：

dof6_forw_model.m

%% 输入一组关节角，计算末端执行器相对于固定坐标系的齐次变换矩阵

function T = dof6_forw_model(q)

      if nargin == 0

          q = [0,-pi/2,0,0,0,0];

      end

      a_0 = 0;

      a_1 = -425;

      a_2 = -392.25;

      a_3 = 0;

      a_4 = 0;

      a_5 = 0;

      alpha_0 = pi/2;

      alpha_1 = 0;

      alpha_2 = 0;

      alpha_3 = pi/2;

      alpha_4 = -pi/2;

      alpha_5 = 0;

      d_1 = 89.16;

      d_2 = 0;

      d_3 = 0;

      d_4 = 109.15;

      d_5 = 94.65;

      d_6 = 82.3;

      RL1 = Link([0,d_1,a_0,alpha_0],'modefied');

      RL2 = Link([0,d_2,a_1,alpha_1],'modefied');

      RL3 = Link([0,d_3,a_2,alpha_2],'modefied');

      RL4 = Link([0,d_4,a_3,alpha_3],'modefied');

      RL5 = Link([0,d_5,a_4,alpha_4],'modefied');

      RL6 = Link([0,d_6,a_5,alpha_5],'modefied');

      robot_m = SerialLink([RL1 RL2 RL3 RL4 RL5 RL6],'name','robot');

      T = robot_m.fkine(q);

      robot_m.plot(q);  

end

测试代码：

%% 正运动学

% 输入六个关节角

q = [0,0,0,0,0,0];

% 计算末端执行器相对于固定坐标系的齐次变换矩阵

T = dof6_forw_model(q);

   函数运行结束后，可以得到T矩阵以及如图7的机械臂连杆图：

图 7 MATLAB中UR5工业机械臂模型初始位置姿态

我们可以直观地看到，这里的连杆图与六轴机械臂实物在结构特征上(包括杆长、位置和姿态等)都是一致的。

扫一扫，加入松灵官方ROS交流群！