基于速度环补偿的零力控制方法

高朋帅， 蔡锦达， 方佳伟

(上海理工大学 机械工程学院， 上海 200093)

目前常用的机器人示教方式是通过示教器进行示教，但是示教器示教对示教人员的要求比较高，尤其对复杂工艺路线进行示教时，效率低下。拖动示教是通过人手直接拖动机器人完成目标路线，机器人对轨迹进行记忆而后复现的示教方式，又称直接示教。拖动示教的方式操作简单，而且容易实现复杂的工艺路线，对示教人员的要求较低。在拖动示教的过程中示教人员需要克服机器人的重力、摩擦力和惯性力等，良好的零力控制是拖动示教必不可少的核心技术之一。

拖动示教可以划分为功率级脱离示教和伺服级接通示教[1]。由于工业机器人一般质量较大，采用功率级脱离示教方式的话，示教人员操作难度大，因而工业机器人的拖动示教普遍采用伺服级接通示教[2-3]。

1 面向拖动示教的零力控制方法

使机器人可以在外力的作用下运动，就好像机器人处在一个不受摩擦力和重力影响的环境下的控制就是机器人的零力控制[4]。

机器人零力控制的基础是关节型机器人的动力学方程[5]：

(1)

伺服级接通示教方式下，目前比较常用的主要有以下2种实现方式：基于位置控制的零力控制和基于转矩控制的零力控制。

1.1 基于位置控制的零力控制

如图1控制框图所示是基于位置控制的零力控制基本原理[6-7]，该方法是将外力的大小及方向通过力传感器和机器人控制器转换为各关节电机对应的位置指令。这种方法灵敏度高、系统稳定性好，但是成本高，抗干扰能力差，系统比较复杂。

图1 基于位置控制的零力控制原理图Figure 1 Principle diagram of force-free control based on position control

1.2 基于转矩控制的零力控制

如图2所示是基于转矩控制的零力控制技术控制框图[2]11，该方法是计算出各关节所在部分的重力和摩擦力，通过控制各关节电机输出对应大小的力矩进行补偿。该方法只对机械臂的重力及摩擦力做补偿，不需要力传感器，从而降低了制造成本和控制系统的复杂性。因受机器人惯性大小影响明显，仅适用于结构较小的机器人；同时由于弱化对位置环和速度环的控制，安全性系统设计难度较大。

图2 基于转矩控制的零力控制Figure 2 Principle diagram of force-free control based on torque control

2 基于速度环补偿的零力控制模型

2.1 基于速度环控制模型

课题组提出基于速度环补偿的零力控制模型，通过伺服电机的励磁电力检测获得外部力(矩)，而不使用力(矩)传感器。通过计算得到各关节对应的重力(矩)及摩擦力(矩)，利用伺服控制器对各关节速度进行补偿实现零力控制。该控制方法不仅继承了力矩控制方法计算量小、系统简洁的优点，还通过对机器人速度环的控制降低了系统的安全性成本。

基于速度环补偿的零力控制模型方框图如图3所示，Kp，Kv，Kτ分别为伺服电机的位置环、速度环以及力矩常数；τf，τg，τF和τm分别为摩擦力矩、重力矩、外力矩和电机输出力矩；cF为外力矩系数。

图3 基于速度环补偿的零力控制Figure 3 Force-free control based on speed loop compensation

根据式(1)，在拖动示教过程中考虑外部力矩的作用，关节型机器人的动力学方程为：

(2)

2.2 基于速度环补偿的零力控制的数学模型

根据图3中的伺服电机方框图得伺服电机的输出力矩可表示为：

(3)

通过式(2)与式(3)可建立各关节的动力学方程与伺服电机控制器的关系如下：

(4)

通过式(4)可以确定控制器输入转角的公式：

(5)

又由(2)可得下式：

(6)

在拖动示教的过程中，拖动的速度一般比较缓慢，各关节的转速一般小于额定转速的1/5，故可将式(5)中的高阶项视为零，在物理意义上表示不考虑其惯性力矩和科氏力矩，所以在拖动示教过程中，式(5)可以转化为：

τF=τf+τg-τm。

(7)

将式(6)、式(7)代入式(5)可得：

(8)

以上确定了各关节输出转速的表达式。通过上述分析，基于速度环补偿的零力控制的控制框图如图4所示。

图4 控制框图Figure 4 Control block diagram

2.3 动力学模型简化

以如图5所示的连杆机构作为机器人简化模型。图中：I0，I1，I2分别代表对应连杆的转动惯量；l1，l2分别代表连杆的长度；m1，m2分别代表对应连杆的质量；g为重力加速度。

图5 机器人简化动力学模型Figure 5 Simplified dynamic model of robot

采用拉格朗日能量法对本课题组设计的机器人动力学方程进行推导[8]，拉格朗日函数定义为：

L=K-P。

(9)

式中：L是拉格朗日函数，K是动能，P是势能。

所设计的机器人关节均为转动关节，所以有下式：

(10)

式中：Ti表示关节运动的所有力矩之和，θi为旋转变量。

假设机器人简化模型的质心在连杆的中央可将该模型的动能K写出：

(11)

系统的势能P可表示为：

(12)

舍去各非线性耦合项，可解得惯性力矩M(q)为：

(13)

(14)

摩擦力项f(q)为：

(15)

其中摩擦力各项见下文。

本研究涉及的机器人使用黏性-库伦摩擦力模型[9-11]。当电机速度为零时各关节所受到的是静摩擦力，外部所施加的力必须先克服静摩擦力才能够使关节运动，此时需要克服的是黏性摩擦力。黏性-库伦摩擦力公式如下：

(16)

式中：fv表示黏性摩擦力，B表示黏性摩擦因数，fc表示库伦摩擦力。其函数如图6所示。

图6 黏性-库伦摩擦力函数模型Figure 6 Viscosity-Coulomb friction function model

图中fs表示关节所受静摩擦力值，fc表示动摩擦力中的库伦力值，B表示动摩擦力中黏性摩擦因数值。关节运动存在正向与反向，所以图中fs和fc有正负之分。

根据各关节的受力情况，本研究中的工业机器人的摩擦力项为：

(17)

3 实验验证

通常情况下，人施加的拖动力都需要通过力矩传感器进行测量和补偿的。但是当伺服电机工作在位置模式时，电机输出转矩和电枢电流之间是存在着线性关系，因此可以利用电枢电流表示电磁转矩进而实现力矩控制[12-13]。

Tm=Km·I。

(18)

式中：Tm表示电机输出转矩，Km表示转矩常数，I表示电枢电流。

3.1 单关节零力拖动实验方法

在拖动示教中外部人手的拖动力作为关节的负载，负载可根据需求与电机旋转方向同向或反向。与电机旋转方向相反的负载比较常见，与电机旋转方向相同的负载实现较为困难。本实验选用2个伺服电机使其分别工作在转矩模式和位置模式下，实现人手拖动力模拟。

转矩模式下的伺服电机用于模拟外部力矩，通过调节外部直流稳压电源实现对转矩模式下的伺服电机转矩值的调节。实际输出转矩值可以通过伺服电机控制器的数码管监测。

位置模式下的伺服电机用于模拟关节电机，在受到外部转矩的作用下，将相电流模拟量接入下位机的A/D转换接口，下位机根据实时采集到的电流值完成反馈控制，图7为单关节零力控制实验台的电机安装示意图。

图7 单关节零力控制实验的电机安装示意图Figure 7 Schematic diagram of motor installation for single joint force-free control experiment

3.2 试验设备及数据处理

本实险所使用的驱动器型号为RYH401F5-VV2，电机的具体参数如表1所示。

表1 单关节零力控制实验的电机参数设定表

通过图7 所示的单关节零力控制试验台完成的零力控制实验结果如表2、表3所示。

表2 加载转矩与关节外力电流

表2中加载转矩是指通过调节直流稳压电源的值计算得出转矩模式下伺服电机的理论输出转矩值；测量转矩是指转矩模式下伺服电机通过伺服电机控制器的数码管观测到的转矩值；关节外力电流是指关节电机受到外力矩时的励磁电流测量值；拟合力矩是指根据测量力矩拟合出的曲线所计算出的力矩值；拟合误差是指拟合力矩值相对测量力矩的误差百分比。

将表2中的加载转矩与关节外力电流进行线性拟合，得到转矩与关节外力电流的关系如图8所示。

图8 加载转矩与关节励磁电流关系Figure 8 Relationship between loading torque and joint excitation current

由图8可知关节外力电流与测量转矩具有良好的线性关系，虽然在转矩较小时由于系统误差、测量误差等因素导致拟合误差较大，但拟合差值的绝对数值很小；并且当测量转矩超过0.6 N·m后拟合误差不超过3%，在小型机器人系统中该误差很容易被操作者克服，所以该误差完全可以被接受，采用励磁电流检测进而表征伺服电机外部力矩值的方案是可行的。

当加载转矩相同时，不同的转速对应不同的励磁电流，测试结果如表3所示。

表3 关节电机外力电流随转速的变化

对表3中数据进行数据拟合，其结果如图9所示。

根据图9可知,图中3条曲线分别是在不同外力矩作用下关节外力电流与转速所拟合的，在转速变化过程中关节外力电流存在明显的极小值点，根据关节外力电流与外力矩的线性关系，认定该极小值点即为零力状态。图中还可以发现不同外力矩作用下，其零力状态所对应关节外力电流的拟合函数近似为常值函数。故在速度控制过程中，当电流达到极小值点时就认为达到零力状态。

当人手施加力矩到关节电机希望电机加速时会受到较大的阻力，相当于电机负载增大电机电流也会增大，通过速度环调整电机转速使电机电流达到零力状态，此时电机的负载达到极小，也就是人手感到的阻力达到极小，实现零力控制。

图9 关节外力电流与转速的关系Figure 9 Relationship between joint external force current and speed

4 结语

课题组通过理论推导出基于速度环补偿的控制原理；设计了模拟人手拖动力和测量零力电流的实验方法；并通过实验验证了拖动力与励磁电流的线性关系，找到了关节电机所对应的零力电流；该零力电流是闭环控制的重要调节依据。课题组所提出的基于速度环补偿的零力控制方法忽略了科氏力矩和惯性力矩，因此只适用于质量较小的机械臂的零力控制。另外本研究只验证了基于速度环补偿的零力控制方法的可行性，之后将对其控制效果进行研究。