CN105345823A - 一种基于空间力信息的工业机器人自由驱动示教方法 - Google Patents

Abstract

本发明公开了一种基于空间力信息的工业机器人自由驱动示教方法，包括：通过安装在机器人末端的六维力传感器采集外部受力信息；对采集的外部受力信息进行滤波和重力补偿处理，得到补偿处理后的外部受力信息；根据补偿处理后的外部受力信息和设定的阈值判断机器人末端是否受力；根据判断的结果采用人手拖动物体模拟模型对机器人平移的速度进行更新，然后控制机器人以更新后的速度进行运动。本发明通过安装在机器人末端的六维力传感器采集外部受力信息，结构简单，成本更低；通过模拟人手拖动物体的过程来建立相应的数学模型以及更新机器人平移的速度，更符合人体工程学设计，柔顺性更好。本发明可广泛应用于自动化控制领域。

Description

一种基于空间力信息的工业机器人自由驱动示教方法

技术领域

本发明涉及自动控制领域，尤其是一种基于空间力信息的工业机器人自由驱动示教方法。

背景技术

工业机器人是工作机器，其可以装备用于对对象进行自动处理和/或加工的工具，并可以对多个运动轴，例如就方向、位置和工作流程进行编程控制。

传统机器人的示教主要依赖于示教盒的操作实现，但该示教方式操作繁琐、示教效率低、且对操作者的技术水平要求较高。为了克服传统示教方式的缺陷，有人提出了一种基于力传感器的示教方法，该方法通过在机器人的每个关节处安装扭矩传感器，使机器人的各个关节根据扭矩传感器检测的扭矩大小进行运动，来达到自由驱动示教的目的。然而，这种基于力传感器的机器人示教驱动方法存在着以下缺陷或不足：

(1)需在机器人的每个关节各安装一个扭矩传感器，这样会导致总成本增加。

(2)示教的柔顺性效果不佳。该方法的柔顺效果是根据每个关节所在的轴的扭矩产生的，而每个关节所在的轴是独立运动的，因此如果该方法在机器人末端施加力时，会因各个关节所在的轴的不同步而产生不协调的效果(即机器人末端不能沿着力的方向运功或卡顿的效果)，柔顺性效果不佳。

综上所述，业内亟需一种成本低和柔顺性好的工业机器人自由驱动示教方法。

发明内容

为了解决上述技术问题，本发明的目的是：提供一种成本低和柔顺性好的，基于空间力信息的工业机器人自由驱动示教方法。

本发明解决其技术问题所采用的技术方案是：

一种基于空间力信息的工业机器人自由驱动示教方法，包括：

S1、通过安装在机器人末端的六维力传感器采集外部受力信息；

S2、对采集的外部受力信息进行滤波和重力补偿处理，得到补偿处理后的外部受力信息；

S3、根据补偿处理后的外部受力信息和设定的阈值判断机器人末端是否受力；

S4、根据判断的结果采用人手拖动物体模拟模型对机器人平移的速度进行更新，然后控制机器人以更新后的速度进行运动。

进一步，所述六维力传感器采集的外部受力信息包括沿X、Y和Z轴方向的3个力Fx、Fy和Fz以及绕着X、Y和Z轴的3个扭矩Mx、My和Mz。

进一步，所述步骤S2，其包括：

S21、对采集的外部受力信息进行卡尔曼滤波处理，得到滤波处理后的外部受力信息；

S22、对滤波处理后的外部受力信息进行重力补偿处理，以消除夹具和六维力传感器对外部受力的影响。

进一步，所述步骤S3，其具体为：

将补偿处理后的外部受力信息与设定的阈值进行比较，若补偿处理后的外部受力大于设定的阈值，则表明此时机器人末端受到人施加的力；反之，则表明此时机器人末端并没有受到人施加的力，机器人保持停止状态。

进一步，所述步骤S4，其包括：

S41、根据判断的结果采用人手拖动物体模拟模型对机器人平移的速度进行更新；

S42、采用模糊PID控制算法控制机器人以更新后的速度进行运动。

进一步，所述步骤S41，其具体为：

若补偿处理后的外部受力大于设定的阈值，则采用人手拖动物体模拟模型对机器人平移的速度进行更新；反之，则结束自由驱动示教流程，所述人手拖动物体模拟模型为：

$\left\{\begin{array}{c}(F-{\mathrm{\ζ}}_f.v_n^{\′}).\mathrm{\Δ}t=m.\mathrm{\Δ}v\\ v_{n+1}=v_n^{\′}+\mathrm{\Δ}v\end{array}\right.,$

其中，F是人对机器人末端施加的力，m是机器人末端的负载的等效质量，ζ_f为力阻尼系数，v'_n是n时刻机器人平移的实际速度，v_n+1是机器人更新后的速度，Δt是更新的时间间隔。

进一步，所述步骤S42，其具体为：

PID模糊控制器根据机器人更新后的速度v_n+1与实际速度v′_n的误差e以及给定的误差变化率ec，计算出PID控制器相应的比例参数Kp、微分参数Ki和积分参数Kd，然后根据计算的比例参数Kp、微分参数Ki和积分参数Kd控制机器人以更新后的速度v_n+1进行运动。

进一步，在所述步骤S4之后还设有步骤S5，所述步骤S5其包括：

S51、根据判断的结果采用姿态角的转速数学模型对机器人姿态角的转速进行更新；

S52、采用模糊PID控制算法控制机器人以更新后的姿态角的转速进行转动。

进一步，所述步骤S51，其具体为：

若补偿处理后的外部受力大于设定的阈值，则采用姿态角的转速数学模型对机器人姿态角的转速进行更新；反之，则结束自由驱动示教流程，所述姿态角的转速数学模型为：

$\left\{\begin{array}{c}(M-{\mathrm{\ζ}}_m.w_n^{\′}).\mathrm{\Δ}t=I.\mathrm{\Δ}w\\ w_{n+1}=w_n^{\′}+\mathrm{\Δ}w\end{array}\right.,$

其中，M为扭矩矩阵，ζ_m为扭矩矩阵阻尼系数，w'_n是n时刻姿态角的实际转速，I是机器人末端所在轴的等效转动惯量，w_n+1为更新后的姿态角的转速，Δt是更新的时间间隔。

进一步，所述步骤S52，其具体为：

PID模糊控制器根据机器人更新后的姿态角的转速w_n+1与实际速度w′_n的误差e以及给定的误差变化率ec，计算出相应的PID控制器的比例参数Kp、微分参数Ki和积分参数Kd，然后根据计算的比例参数Kp、微分参数Ki和积分参数Kd控制机器人以新后的姿态角的转速w_n+1进行运动。

本发明的有益效果是：通过安装在机器人末端的六维力传感器采集外部受力信息，不在需要在机器人的每个关节安装扭矩传感器，结构简单，成本更低；采用人手拖动物体模拟模型对机器人平移的速度进行更新，然后控制机器人以更新后的速度进行运动，通过模拟人手拖动物体的过程来建立相应的数学模型以及更新机器人平移的速度，不再会因各个关节所在的轴的不同步而产生不协调的效果，更符合人体工程学设计，柔顺性更好。进一步，采用姿态角的转速数学模型对机器人姿态角的转速进行更新，然后控制机器人以更新后的姿态角的转速进行转动，综合考虑了姿态角转速和平移速度的影响，更加全面和准确。

附图说明

下面结合附图和实施例对本发明作进一步说明。

图1为本发明一种基于空间力信息的工业机器人自由驱动示教方法的整体流程图；

图2为本发明实施例一六维力传感器的安装结构示意图；

图3为本发明实施例一人手拖动物体模拟模型的示意图；

图4为本发明实施例一姿态角的转速数学模型的示意图；

图5为本发明实施例二机器人平移的速度更新过程的步骤流程图；

图6为本发明实施例二模糊PID控制算法的框架图。

具体实施方式

参照图1，一种基于空间力信息的工业机器人自由驱动示教方法，包括：

S1、通过安装在机器人末端的六维力传感器采集外部受力信息；

S2、对采集的外部受力信息进行滤波和重力补偿处理，得到补偿处理后的外部受力信息；

S3、根据补偿处理后的外部受力信息和设定的阈值判断机器人末端是否受力；

S4、根据判断的结果采用人手拖动物体模拟模型对机器人平移的速度进行更新，然后控制机器人以更新后的速度进行运动。

进一步作为优选的实施方式，所述六维力传感器采集的外部受力信息包括沿X、Y和Z轴方向的3个力Fx、Fy和Fz以及绕着X、Y和Z轴的3个扭矩Mx、My和Mz。

进一步作为优选的实施方式，所述步骤S2，其包括：

S21、对采集的外部受力信息进行卡尔曼滤波处理，得到滤波处理后的外部受力信息；

S22、对滤波处理后的外部受力信息进行重力补偿处理，以消除夹具和六维力传感器对外部受力的影响。

进一步作为优选的实施方式，所述步骤S3，其具体为：

将补偿处理后的外部受力信息与设定的阈值进行比较，若补偿处理后的外部受力大于设定的阈值，则表明此时机器人末端受到人施加的力；反之，则表明此时机器人末端并没有受到人施加的力，机器人保持停止状态。

进一步作为优选的实施方式，所述步骤S4，其包括：

S41、根据判断的结果采用人手拖动物体模拟模型对机器人平移的速度进行更新；

S42、采用模糊PID控制算法控制机器人以更新后的速度进行运动。

进一步作为优选的实施方式，所述步骤S41，其具体为：

若补偿处理后的外部受力大于设定的阈值，则采用人手拖动物体模拟模型对机器人平移的速度进行更新；反之，则结束自由驱动示教流程，所述人手拖动物体模拟模型为：

$\left\{\begin{array}{c}(F-{\mathrm{\ζ}}_f.v_n^{\′}).\mathrm{\Δ}t=m.\mathrm{\Δ}v\\ v_{n+1}=v_n^{\′}+\mathrm{\Δ}v\end{array}\right.,$

其中，F是人对机器人末端施加的力，m是机器人末端的负载的等效质量，ζ_f为力阻尼系数，v'_n是n时刻机器人平移的实际速度，v_n+1是机器人更新后的速度，Δt是更新的时间间隔。

进一步作为优选的实施方式，所述步骤S42，其具体为：

PID模糊控制器根据机器人更新后的速度v_n+1与实际速度v′_n的误差e以及给定的误差变化率ec，计算出PID控制器相应的比例参数Kp、微分参数Ki和积分参数Kd，然后根据计算的比例参数Kp、微分参数Ki和积分参数Kd控制机器人以更新后的速度v_n+1进行运动。

进一步作为优选的实施方式，在所述步骤S4之后还设有步骤S5，所述步骤S5其包括：

S51、根据判断的结果采用姿态角的转速数学模型对机器人姿态角的转速进行更新；

S52、采用模糊PID控制算法控制机器人以更新后的姿态角的转速进行转动。

进一步作为优选的实施方式，所述步骤S51，其具体为：

若补偿处理后的外部受力大于设定的阈值，则采用姿态角的转速数学模型对机器人姿态角的转速进行更新；反之，则结束自由驱动示教流程，所述姿态角的转速数学模型为：

$\left\{\begin{array}{c}(M-{\mathrm{\ζ}}_m.w_n^{\′}).\mathrm{\Δ}t=I.\mathrm{\Δ}w\\ w_{n+1}=w_n^{\′}+\mathrm{\Δ}w\end{array}\right.,$

其中，M为扭矩矩阵，ζ_m为扭矩矩阵阻尼系数，w'_n是n时刻姿态角的实际转速，I是机器人末端所在轴的等效转动惯量，w_n+1为更新后的姿态角的转速，Δt是更新的时间间隔。

进一步作为优选的实施方式，所述步骤S52，其具体为：

PID模糊控制器根据机器人更新后的姿态角的转速w_n+1与实际速度w′_n的误差e以及给定的误差变化率ec，计算出相应的PID控制器的比例参数Kp、微分参数Ki和积分参数Kd，然后根据计算的比例参数Kp、微分参数Ki和积分参数Kd控制机器人以新后的姿态角的转速w_n+1进行运动。

下面结合说明书附图和具体实施例对本发明作进一步详细说明。

实施例一

本实施例对本发明六维力传感器的安装、人手拖动物体模拟模型、姿态角的转速数学模型以及示教柔顺性的调整规律进行详细说明。

(一)六维力传感器的安装

为了降低成本和提高示教的柔顺性，本发明在机器人末端安装六维力传感器，以采集机器人的受力信息(包括X、Y、Z三个轴方向的力Fx、Fy和Fz以及绕着三个轴的扭矩Mx、My和Mz，如图2所示)。

(二)人手拖动物体模拟模型

本发明所提出的算法的基本思路是使机器人末端运动情况模拟人在平面推动一个物体或拖动一个物体的运动情况，如图3所示。其中，m是机器人末端(即力传感器的末端)的负载(不包括机器人末端，只是其负载)的质量，整个过程相当于人拿着一个等效质量为m的物体在运动。等效质量m不同时，人拖动物体的效果也不同，其给人的感觉是：等效质量越大，拖动起来越沉；反之，则拖动起来越轻。

本发明在X、Y和Z轴这三个方向都建立了一个数学模型，由牛顿第二定律，可得：

F-ζ_f.v＝ma

设控制器控制周期为Δt，则有：

(F-ζ_f.v).Δt＝ma.Δt＝m.Δv

因此，结合以上两式可得到人手拖动物体模拟模型的平移速度更新公式为：

$\left\{\begin{array}{c}(F-{\mathrm{\ζ}}_f.v_n^{\′}).\mathrm{\Δ}t=m.\mathrm{\Δ}v\\ v_{n+1}=v_n^{\′}+\mathrm{\Δ}v\end{array}\right.$

(三)姿态角的转速数学模型

假设某个空间固定点的坐标为(x，y，z，a，b，c)，通过这六个参数，可以确定机器人末端的位置，但是其未必能确定机器人的姿态，因此还须制定相应的姿态值，而a，b，c则是绕固定坐标系X、Y、Z三个轴的转角。也就是说，机器人的运动不仅是位置的改变，而且需要姿态的改变才能适合各种作业情况。故本发明的示教驱动方法，不仅涉及机器人的平移还涉及姿态的改变。因此，本发明不仅引入平移的速度更新模型，还引入了姿态角的转速更新数学模型。

如图4所示，本发明对于姿态建立姿态角的转速更新数学模型，类比平移的速度更新公式的推导过程及相应的物理知识，最终可得到姿态角的转速更新公式为：

$\left\{\begin{array}{c}(M-{\mathrm{\ζ}}_m.w_n^{\′}).\mathrm{\Δ}t=I.\mathrm{\Δ}w\\ w_{n+1}=w_n^{\′}+\mathrm{\Δ}w\end{array}\right.$

其中，M这一扭矩矩阵代表了相对于机器人末端坐标系的3个扭矩，即M代表了图2所示的3个扭矩Mx、My和Mz。I是机器人末端等效质量为m的物体相对于机器人末端坐标系的X、Y和Z轴这三个轴的转动惯量，如图2所示。若设定的物体是正方体小方块，则其三个方向的转动惯量都为L是正方体小方块的边长。

(四)示教柔顺性的调整规律

由(二)和(三)可知，等效质量m、力阻尼系数ζ_f、扭矩阻尼系数ζ_m是调整示教柔顺性的三个主要参数，根据不同工作条件可以通过调整这三个参数来得到合适的示教柔顺性。结合(二)和(三)的公式及相应的物理学知识可知，示教柔顺性的调整规律为：

A、增大等效质量m，柔顺性降低(此时，由牛顿第二定可知使用同等的力，质量越大，所能获得的加速度越小)；反之，降低m，柔顺性提高(此时，由牛顿第二定可知使用同等的力，质量越低，所能获得的加速度越大)。

B、阻尼系数ζ_f或ζ_m越大，柔顺性越低，但是速度达到匀速的时间越短(由其速度更新公式得知)。

实施例二

本实施例以机器人平移的速度更新过程为例，对本发明的具体实现过程进行说明。如图5所示，本发明机器人平移的速度更新过程包括以下步骤：

A、采集力信息：通过六维力传感器采集外部受力信息，分别是3个方向的拉力或压力Fx、Fy和Fz以及3个扭矩Mx、My和Mz的数据。

B、力信息的卡尔曼滤波处理：将采集到的力信息滤波得到准确和稳定的力信息。；

C、对夹具和传感器进行重力补偿：由于采集到的力信息包括了夹具和六维传感器自身的重力，要得到真实的外部受力信息就需要进行重力补偿处理，以消除夹具和六维力传感器自身重力所带来的影响。

D、判断是否超出外部受力的阈值：当受力超过外部受力的阈值，说明机器人末端受到人的拖动或拉力；反之则认为机器人末端并没有受力，机器人保持停止状态。

E、利用平移公式更新机器人平移的速度：机器人平移的速度的更新方法是模拟人手推动一个物体的运动情况，其平移公式为：

$\left\{\begin{array}{c}(F-{\mathrm{\ζ}}_f.v_n^{\′}).\mathrm{\Δ}t=m.\mathrm{\Δ}v\\ v_{n+1}=v_n^{\′}+\mathrm{\Δ}v\end{array}\right..$

F、PID控制器使用模糊PID控制算法使机器人以更新后的平移速度运动：为了使机器人能以更新后的平移速度运动，使用模糊PID控制算法进行控制。

如图6所示，模糊PID控制算法的具体控制过程为：PID模糊控制器根据机器人更新后的速度v_n+1与实际速度v′_n的误差e以及给定的误差变化率ec，计算出PID控制器相应的比例参数Kp、微分参数Ki和积分参数Kd，然后根据计算的比例参数Kp、微分参数Ki和积分参数Kd控制机器人以更新后的速度v_n+1进行运动。由于Kp、Ki和Kd这三个参数是通过模糊控制器不断优化得出的，因此其可以提高PID控制器的响应速度和精确度。

与传统的机器人示教方式相比，本发明所提出的工业机器人自由驱动示教方法，通过安装在机器人末端的六维力传感器采集外部受力信息，成本更低，更加方便简单；通过模拟人手拖动物体的过程来建立相应的数学模型以及更新机器人平移的速度，更符合人体工程学设计，柔顺性更好。

以上是对本发明的较佳实施进行了具体说明，但本发明创造并不限于所述实施例，熟悉本领域的技术人员在不违背本发明精神的前提下还可做作出种种的等同变形或替换，这些等同的变形或替换均包含在本申请权利要求所限定的范围内。

Claims (10)

1.一种基于空间力信息的工业机器人自由驱动示教方法，其特征在于：包括：

S1、通过安装在机器人末端的六维力传感器采集外部受力信息；

S2、对采集的外部受力信息进行滤波和重力补偿处理，得到补偿处理后的外部受力信息；

S3、根据补偿处理后的外部受力信息和设定的阈值判断机器人末端是否受力；

S4、根据判断的结果采用人手拖动物体模拟模型对机器人平移的速度进行更新，然后控制机器人以更新后的速度进行运动。

2.根据权利要求1所述的一种基于空间力信息的工业机器人自由驱动示教方法，其特征在于：所述六维力传感器采集的外部受力信息包括沿X、Y和Z轴方向的3个力Fx、Fy和Fz以及绕着X、Y和Z轴的3个扭矩Mx、My和Mz。

3.根据权利要求1所述的一种基于空间力信息的工业机器人自由驱动示教方法，其特征在于：所述步骤S2，其包括：

S21、对采集的外部受力信息进行卡尔曼滤波处理，得到滤波处理后的外部受力信息；

S22、对滤波处理后的外部受力信息进行重力补偿处理，以消除夹具和六维力传感器对外部受力的影响。

4.根据权利要求2所述的一种基于空间力信息的工业机器人自由驱动示教方法，其特征在于：所述步骤S3，其具体为：

将补偿处理后的外部受力信息与设定的阈值进行比较，若补偿处理后的外部受力大于设定的阈值，则表明此时机器人末端受到人施加的力；反之，则表明此时机器人末端并没有受到人施加的力，机器人保持停止状态。

5.根据权利要求4所述的一种基于空间力信息的工业机器人自由驱动示教方法，其特征在于：所述步骤S4，其包括：

S41、根据判断的结果采用人手拖动物体模拟模型对机器人平移的速度进行更新；

S42、采用模糊PID控制算法控制机器人以更新后的速度进行运动。

6.根据权利要求5所述的一种基于空间力信息的工业机器人自由驱动示教方法，其特征在于：所述步骤S41，其具体为：

若补偿处理后的外部受力大于设定的阈值，则采用人手拖动物体模拟模型对机器人平移的速度进行更新；反之，则结束自由驱动示教流程，所述人手拖动物体模拟模型为：

$\left\{\begin{array}{c}(F-{\mathrm{\ζ}}_f.v_n^{\′}).\mathrm{\Δ}t=m.\mathrm{\Δ}v\\ v_{n+1}=v_n^{\′}+\mathrm{\Δ}v\end{array}\right.,$

其中，F是人对机器人末端施加的力，m是机器人末端的负载的等效质量，ζ_f为力阻尼系数，v'_n是n时刻机器人平移的实际速度，v_n+1是机器人更新后的速度，Δt是更新的时间间隔。

7.根据权利要求6所述的一种基于空间力信息的工业机器人自由驱动示教方法，其特征在于：所述步骤S42，其具体为：

PID模糊控制器根据机器人更新后的速度v_n+1与实际速度v′n的误差e以及给定的误差变化率ec，计算出PID控制器相应的比例参数Kp、微分参数Ki和积分参数Kd，然后根据计算的比例参数Kp、微分参数Ki和积分参数Kd控制机器人以更新后的速度v_n+1进行运动。

8.根据权利要求4所述的一种基于空间力信息的工业机器人自由驱动示教方法，其特征在于：在所述步骤S4之后还设有步骤S5，所述步骤S5其包括：

S51、根据判断的结果采用姿态角的转速数学模型对机器人姿态角的转速进行更新；

S52、采用模糊PID控制算法控制机器人以更新后的姿态角的转速进行转动。

9.根据权利要求8所述的一种基于空间力信息的工业机器人自由驱动示教方法，其特征在于：所述步骤S51，其具体为：

若补偿处理后的外部受力大于设定的阈值，则采用姿态角的转速数学模型对机器人姿态角的转速进行更新；反之，则结束自由驱动示教流程，所述姿态角的转速数学模型为：

$\left\{\begin{array}{c}(M-{\mathrm{\ζ}}_m.w_n^{\′}).\mathrm{\Δ}t=I.\mathrm{\Δ}w\\ w_{n+1}=w_n^{\′}+\mathrm{\Δ}w\end{array}\right.,$

其中，M为扭矩矩阵，ζ_m为扭矩矩阵阻尼系数，w'_n是n时刻姿态角的实际转速，I是机器人末端所在轴的等效转动惯量，w_n+1为更新后的姿态角的转速，Δt是更新的时间间隔。

10.根据权利要求9所述的一种基于空间力信息的工业机器人自由驱动示教方法，其特征在于：所述步骤S52，其具体为：

PID模糊控制器根据机器人更新后的姿态角的转速w_n+1与实际速度w′_n的误差e以及给定的误差变化率ec，计算出相应的PID控制器的比例参数Kp、微分参数Ki和积分参数Kd，然后根据计算的比例参数Kp、微分参数Ki和积分参数Kd控制机器人以新后的姿态角的转速w_n+1进行转动。