CN114131617B - 一种工业机器人的智能柔顺控制方法和装置 - Google Patents

Abstract

本发明公开了一种工业机器人的智能柔顺控制方法和装置，属于工业机器人控制技术领域，该方法包括：在工业机器人执行工作任务时，采集末端执行器的实际位置信息和实际接触力信息；将实际位置信息和期望位置信息通过机器人运动学模型输入机器人位置伺服环，以获取位置驱动量；将期望位置信息和实际接触力信息输入机器人动力学模型，获得第一力驱动量；将期望接触力信息、实际接触力信息和实际位置信息输入阻抗控制模型，获取第二力驱动量；进而利用位置驱动量和两个力驱动量对末端执行器的运动位置和接触力分别进行反馈控制。本发明能够提高机器人的末端执行器的控制柔顺度，使得工业机器人执行任务时状态更加平稳。

Description

一种工业机器人的智能柔顺控制方法和装置

技术领域

本发明属于工业机器人控制技术领域，更具体地，涉及一种工业机器人的智能柔顺控制方法和装置。

背景技术

目前广泛成熟应用的机器人是采用位置控制的机器人，机器人工作时在任务空间中追踪离线编程或者示教编程生成的机器人任务轨迹，如将工业机器人应用于相对简单的应用场合，如点焊、喷漆、取放操作。但当末端执行器与机器人任务对象发生力交互产生能量交换时，纯粹的位置控制已经不适用了。

举例来说，使用海绵擦窗的机器人，利用海绵的柔性可以通过控制末端执行器与玻璃之间的位置来调整施加在窗户上的力。如果海绵十分柔软，或者已知玻璃的精确位置，则机器人可以工作得很好。但是，如果末端执行器、工具或环境的刚性很高，则机器人压贴在平面上的操作执行起来就非常困难。假设不使用海绵擦洗，而是机器人使用刚性刮削工具从玻璃表面刮油漆。如果距离玻璃表面的位置存在任何不确定性，或者机器人存在任何位置误差，则完成该项工作是不可能的。要么玻璃被碰碎，要么机器人在玻璃上方摆动而不与玻璃接触。在清洗和刮擦这两个任务中，可能不指定玻璃表面的位置，而是指定沿着垂直于玻璃表面的力更为合理。力控制已经在一些场合得到应用，例如，一些机器人已经能够实现简单的力控制，如磨削和去毛刺工业机器人。可以预见，工业机器人终究会应用于需要执行力控制的任务中。

然而，现有高刚度工业机器人无法适用于人机协作、智能柔顺制造以及医疗复健等领域。

发明内容

针对现有技术的以上缺陷或改进需求，本发明提供了一种工业机器人的智能柔顺控制方法和装置，其目的在于感知工业机器人的末端执行器在工作时受到的实际接触力、力矩和位置，将其作为机器人智能柔顺控制系统输入信号，基于智能柔顺控制算法得到位置驱动量和力驱动量，从而控制工业机器人做出顺应环境的动态行为，由此解决现有工业机器人应用场景受限的技术问题。

为实现上述目的，按照本发明的一个方面，提供了一种工业机器人的智能柔顺控制方法，包括：

S1：在所述工业机器人执行工作任务时，利用六维力觉传感器采集末端执行器的实际位置信息和实际接触力信息；

S2：将所述实际位置信息和期望位置信息通过机器人运动学模型输入机器人位置伺服环，以获取位置驱动量；

S3：将所述期望位置信息和所述实际接触力信息输入机器人动力学模型，以使其输出第一力驱动量；

S4：将期望接触力信息、所述实际接触力信息和所述实际位置信息输入阻抗控制模型，以获取第二力驱动量；

S5：利用所述位置驱动量、所述第一力驱动量和所述第二力驱动量控制所述工业机器人继续执行工作任务。

在其中一个实施例中，所述S1包括：

S11：在所述工业机器人执行工作任务时，定位所述末端执行器的实际位置信息；利用与所述末端执行器连接的六维力觉传感器采集位置对应的原始电信号；

S12：利用滑动均值滤波算法对所述原始电信号进行滤波处理；

S13：根据所述实际位置信息和所述末端执行器的质量对滤波后的原始电信号进行重力补偿得到所述实际接触力信息，实际接触力信息包括：实际接触力和实际接触力矩。

在其中一个实施例中，所述S13包括：

定义所述滤波后的原始电信号为：F₀＝[f_x f_y f_z M_x M_y M_z]^T，所述实际接触力信息为F_e＝[f_x′ f_y′ f_z′ M_x′ M_y′ M_z′]^T；其中，

为所述末端执行器的位姿矩阵，r＝[x y z]^T-[p_x p_y p_z]^T，M＝r×mg；[x y z]为所述末端执行器的质心在笛卡尔空间坐标；m为所述末端执行器的质量。

在其中一个实施例中，所述S2之前，所述方法还包括：引入互锁对角矩阵S、S′将所述工作任务对应的空间分为位置控制子空间和力控制子空间；

所述S2包括：将所述实际位置信息和所述期望位置信息通过机器人运动学模型输入机器人位置伺服环，并将输出经过矩阵S所述位置控制子空间对应的位置驱动量；

所述S4包括：将所述期望接触力信息、所述实际接触力信息和所述实际位置信息输入阻抗控制模型，以使其输出所述力控制子空间对应的第二力驱动量。

在其中一个实施例中，所述S4中阻抗控制模型为：

其中，Δf＝F_d-F_e为所述阻抗控制模型的输出，F_d为期望接触力信息，F_e为所述实际接触力信息；

利用公式和/>能够求解位移差值Δs；Δt为采样周期，Δs₁，Δs₂和Δs₃分别为获取Δs前第1，2，3个采样周期所获得的位移差值。

在其中一个实施例中，所述S4之前，所述方法还包括：

将原始期望接触力信息F_D输入线性微分跟踪器对进行平滑处理得到追踪信号和所述期望接触力信息F_d；

k时刻的追踪信号为：k+1时刻的期望接触力信息为：/>

其中， d＝δh；d₀＝hd；y＝x₁+hx₂；/>

在其中一个实施例中，所述S3中机器人动力学模型为：

其中，q为角位移，/>为关节角速度，/>为角加速度，J为雅克比矩阵，D为惯性矩阵，/>为雅克比矩阵的求导；/>h为科氏力项和重力项的叠加，/>τ_f为关节的驱动力矩；F_e为实际接触力信息。

按照本发明的另一方面，提供了一种工业机器人的智能柔顺控制装置，包括：

采集模块，用于在所述工业机器人执行工作任务时，利用六维力觉传感器采集末端执行器的实际位置信息和实际接触力信息；

位置驱动模型，用于将所述实际位置信息和期望位置信息通过机器人运动学模型输入机器人位置伺服环，以获取位置驱动量；

第一力驱动模块，用于将所述期望位置信息和所述实际接触力信息输入机器人动力学模型，以使其输出第一力驱动量；

第二力驱动模块，用于将期望接触力信息、所述实际接触力信息和所述实际位置信息输入阻抗控制模型，以获取第二力驱动量；

控制模块，用于利用所述位置驱动量、所述第一力驱动量和所述第二力驱动量控制所述工业机器人继续执行工作任务。

总体而言，通过本发明所构思的以上技术方案与现有技术相比，能够取得下列有益效果：

1、本发明将实际位置信息和期望位置信息通过机器人运动学模型输入机器人位置伺服环，以获取位置驱动量；将期望位置信息和实际接触力信息输入机器人动力学模型，以使其输出第一力驱动量；将期望接触力信息、实际接触力信息和实际位置信息输入阻抗控制模型，以获取第二力驱动量；利用位置驱动量和两个力驱动量对末端执行器的运动位置和接触力分别进行控制，从而提高机器人的末端执行器的控制柔顺度；

2、从控制策略方面来说，基于机器人动力学模型，建立基于力矩的阻抗控制算法提高柔顺控制中的力控制精度；

3、采用离线编程规划机器人运动参数，将期望位置信息和实际接触力信息输入机器人动力学模型，大大减少了因为高度非线性复杂动力学模型计算所需要的时间；

4、从获取机器人与环境之间的接触力这方面来说，引入六维力觉传感器代替环境刚度采集模型，可以得到更为准确的机器人与环境之间的接触力大小；

5、从信号处理方面来说，采用滑动均值滤波算法对六维力矩传感器采集到的信号进行滤波处理，提高力控制精度。此外采用线性微分跟踪器算法对控制系统的阶跃输入信号期望力安排过渡处理，使得系统启动时机械装置更加平稳。

附图说明

图1是本发明一实施例中工业机器人的智能柔顺控制方法的流程图；

图2是本发明一实施例中工业机器人的智能柔顺控制方法的逻辑框图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。

需要说明的是，工业机器人的智能柔顺控制方法的应用场景中具体包括工业机器人、平板电脑(手机或示教器)、传感器转接法兰和六维力觉传感器。工业机器人为执行装置，作为下位机负责执行用户设置的机器人运动程序；平板电脑(或手机、示教器等)为控制装置，作为上位机方便用户在终端上对机器人工作进行机器人运动轨迹编程，使得机器人可以完成指定的工作任务需求；传感器转接法兰用于连接机器人的末端执行器与六维力觉传感器；六维力觉传感器安装在机器人末端，用于感知机器人的末端执行器在工作时，受到的实际接触力及力矩，将接触力及接触力矩信息作为机器人智能柔顺控制系统输入信号，基于智能柔顺控制算法控制下位机工业机器人做出顺应环境的动态行为。

其中，六维力觉传感器通过转接法兰安装于机器人本体的末端，末端执行器安装于六维力觉传感器的另外一端用来监测工作过程中末端执行器与外界之间的实际接触力，用户通过终端控制工业机器人对工作对象进行任务操作。

如图1和图2所示，本发明提供了一种工业机器人的智能柔顺控制方法，包括：

S1：在工业机器人执行工作任务时，采集末端执行器的实际位置信息和实际接触力信息；

S2：将实际位置信息和期望位置信息通过机器人运动学模型输入机器人位置伺服环，以获取位置驱动量；

S3：将期望位置信息和实际接触力信息输入机器人动力学模型，以使其输出第一力驱动量；

S4：将期望接触力信息、实际接触力信息和实际位置信息输入阻抗控制模型，以获取第二力驱动量；

S5：利用位置驱动量、第一力驱动量和第二力驱动量控制机器人继续执行工作任务。

在其中一个实施例中，S1包括：

S11：在工业机器人执行工作任务时，定位末端执行器的实际位置信息；利用与末端执行器连接的六维力觉传感器采集位置对应的原始电信号；S12：利用滑动均值滤波算法对原始电信号进行滤波处理；S13：根据实际位置信息和末端执行器的质量对滤波后的原始电信号进行重力补偿得到实际接触力信息，实际接触力信息包括：实际接触力和实际接触力矩。

具体的，首先将六维力觉传感器安装在机器人末端与末端执行器之间，六维力觉传感器用于监测机器人的末端执行器在笛卡尔空间下的接触力信息。六维力觉传感器采集到的原始电压信号通常会受到环境噪声和系统噪声的电磁与信号干扰，且大多数工业干扰信号都是高频噪声。故采用滑动均值滤波算法对采集的信号进行滤波处理，滑动均值滤波算法实现过程如下：

(1)、根据加工工况确定接触力矩的采样宽度为N数据队列；

(2)、将新采集的力矩信号添加到队列的尾部，将队列的头部移除队列；

(3)、将队列中的N个数据进行算术平均运算；

(4)、将N算术平均值返回，即为当前数据滤波后的结果。

在其中一个实施例中，S13包括：

定义滤波后的原始电信号为：F₀＝[f_x f_y f_z M_x M_y M_z]^T，实际接触力信息为F_e＝[f_x′ f_y′ f_z′ M_x′ M_y′ M_z′]^T；其中，

为末端执行器的位姿矩阵，r＝[x y z]^T-[p_x p_y p_z]^T，M＝r×mg；[x y′ z]为末端执行器的质心在笛卡尔空间坐标；m为末端执行器的质量。

具体的，考虑机器人的末端执行器自重对原始电信号的影响，根据机器人位置姿态信息以及末端执行器质量对滤波后的原始电信号进行重力补偿，还原机器人末端所受到的实际接触力及接触力矩。重力补偿算法实现过程如下：

定义：末端执行器的质量m、末端执行器的质心在笛卡尔空间中的空间位置P＝[xy z]、机器人末端姿态4×4矩阵力矩传感器的采集信号六维列向量F₀＝[f_x f_y f_z M_x M_y M_z]^T，补偿后的结果F_e＝[f_x′ f_y′ f_z′ M_x′ M_y′ M_z′]^T；

r＝[x y z]^T-[p_x p_y p_z]^T；

M＝r×mg；

在其中一个实施例中，S2之前，方法还包括：引入互锁对角矩阵S、S′将工作任务对应的空间分为位置控制子空间和力控制子空间；

其中，利用机器人离线编程软件生成目标机器人运动控制程序；也可以用人机交互手动牵引机器人进行示教，也可用示教器(平板或者手机)移动机器人到目标点，机器人运动程序的编写视现场加工过程情况而定。力位混合控制可以根据机器人实际任务需求来确定。引入互锁对角矩阵S、S′将工作任务空间分为两个子空间，在不同的空间实现采用不同的控制策略(位置控制或者力控制)。通过雅可比矩阵、机器人动力学将机器人任务空间任意方向的力和位置控制分配到各个关节控制器上，机器人以独立的形式同时控制力和位置。以型面柔顺加工为例，整个任务空间分为两个子空间，接触曲面线的法线方向为力控制子空间只做力控制，在接触曲面线切线方向为位置控制子空间，只作位置控制。

S2包括：将实际位置信息和期望位置信息通过机器人运动学模型输入机器人位置伺服环，并将输出经过矩阵S位置控制子空间对应的位置驱动量；

其中，目前典型工业机器人控制器的结构，通常使用两级结构，顶层CPU作为控制系统的主机。主计算机向每个低级控制器发送指令，每一个低级控制器对应一个关节。每一个低级控制器控制一个关节伺服，上面通常运行简单的PD、PI或者PID控制规律，每个关节上都安装了光学编码器作为位置反馈，速度信号由关节控制器做微分后得到。因此，为实现机器人对笛卡尔空间下的任务轨迹的追踪功能。采用标准DH参数法建立机器人运动学模型，实现机器人笛卡尔空间下的运动轨迹信息向机器人关节空间运动参数信息转换。采用PD线性反馈的方法对各关节进行独立控制，建立机器人位置追踪系统，实现笛卡尔空间下的机器人位置控制。

机器人位置控制PD控制算法：

传递函数：

式中：K_p为比例系数，T_d为微分时间常数，K_d为微分系数。

S4包括：将期望接触力信息、实际接触力信息和实际位置信息输入阻抗控制模型，以使其输出力控制子空间对应的第二力驱动量。

在其中一个实施例中，S4中阻抗控制模型为：

其中，Δf＝F_d-F_e为阻抗控制模型的输出，F_d为期望接触力信息，F_e为实际接触力信息；

利用公式和/>能够求解位移差值Δs；Δt为采样周期，Δs₁，Δs₂和Δs₃分别为获取Δs前第1，2，3个采样周期所获得的位移差值。

具体的，根据工作任务设置好的柔顺控制系统模型参数，以及离线编程生成的机器人运动路径，启动机器人执行指定任务。在机器人运动过程中，六维力觉传感器实时感知机器人与外界环境的接触力大小，控制器对传感器采集力信号进行滤波、重力补偿处理，得到机器人与环境之间的实际接触力大小。利用阻抗控制算法能够实现机器人柔顺控制，通过控制机器人关节的驱动力矩可以调节末端外力及位置误差的大小，实现机器人顺应外力的柔顺运动。

在其中一个实施例中，S4之前，方法还包括：

将原始期望接触力信息F_D输入线性微分跟踪器对进行平滑处理得到追踪信号和期望接触力信息F_d；

k时刻的追踪信号为：k+1时刻的期望接触力信息为：/>

其中，

d＝δh；d₀＝hd；y＝x₁+hx₂；

需要说明的是，原始期望接触力信息F_D是一个阶跃信号，为保证机器人柔顺控制系统稳定工作，可以采用线性微分跟踪器进行平滑处理。输入为原始期望接触力信息F_D，输出为和F_d，将F_d作为平滑处理的结果，即期望接触力信息。

在其中一个实施例中，S3中机器人动力学模型为：

其中，q为角位移，/>为关节角速度，/>为角加速度，J为雅克比矩阵，D为惯性矩阵，/>为雅克比矩阵的求导；/>h为科氏力项和重力项的叠加，/>τ_f为关节的驱动力矩；F_e为实际接触力信息。

具体的，利用雅克比矩阵建立器机器人末端笛卡尔空间下受力与机器人关节空间下的负载关节力矩之间的映射关系，进而建立机器人动力学模型，基于机器人动力学模型结合机器人运动状态能够获取期望接触力信息。其中，建立机器人动力学模型的过程如下：

首先建立机器人与环境接触的带有摩擦力动力学方程：

将科氏力项与重力项g(q)合并为/>变为如下形式：

利用雅可比矩阵J(q)建立关节角速度角加速度/>与笛卡尔空间下的速度/>加速度/>其中，/>

转变形式：

代入之后变为：

通过雅可比矩阵建立机器人末端受力F′与机器人关节受力之间的关系：

τ＝J^TF′；

新的动力学方程变成以下形式：

其中：最后/>

最终得到的机器人动力学模型表示为：

按照本发明的另一方面，提供了一种工业机器人的智能柔顺控制装置，包括：

采集模块，用于在工业机器人执行工作任务时，采集末端执行器的实际位置信息和实际接触力信息；

位置驱动模型，用于将实际位置信息和期望位置信息通过机器人运动学模型输入机器人位置伺服环，以获取位置驱动量；

第一力驱动模块，用于将期望位置信息和实际接触力信息输入机器人动力学模型，以使其输出第一力驱动量；

第二力驱动模块，用于将期望接触力信息、实际接触力信息和实际位置信息输入阻抗控制模型，以获取第二力驱动量；

控制模块，用于利用位置驱动量、第一力驱动量和第二力驱动量控制机器人继续执行工作任务。

本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (2)

1.一种工业机器人的智能柔顺控制方法，其特征在于，包括：

S1：在所述工业机器人执行工作任务时，利用六维力觉传感器采集末端执行器的实际位置信息和实际接触力信息；

S2：将所述实际位置信息和期望位置信息通过机器人运动学模型输入机器人位置伺服环，以获取位置驱动量；

S3：将所述期望位置信息和所述实际接触力信息输入机器人动力学模型，以使其输出第一力驱动量；

S4：将期望接触力信息、所述实际接触力信息和所述实际位置信息输入阻抗控制模型，以获取第二力驱动量；

S5：利用所述位置驱动量、所述第一力驱动量和所述第二力驱动量控制所述工业机器人继续执行工作任务；

所述S1包括：

S11：在所述工业机器人执行工作任务时，定位所述末端执行器的实际位置信息；利用与所述末端执行器连接的六维力觉传感器采集位置对应的原始电信号；

S12：利用滑动均值滤波算法对所述原始电信号进行滤波处理；

S13：根据所述实际位置信息和所述末端执行器的质量对滤波后的原始电信号进行重力补偿得到所述实际接触力信息，实际接触力信息包括：实际接触力和实际接触力矩；

所述S13包括：

定义所述滤波后的原始电信号为：，所述实际接触力信息为/>；其中，

，/>；

为所述末端执行器的位姿矩阵，/>，/>；为所述末端执行器的质心在笛卡尔空间坐标；m为所述末端执行器的质量；

所述S2之前，所述方法还包括：引入互锁对角矩阵S、S'将所述工作任务对应的空间分为位置控制子空间和力控制子空间；

所述S2包括：将所述实际位置信息和所述期望位置信息通过机器人运动学模型输入机器人位置伺服环，并将输出经过矩阵S，以获取所述位置控制子空间对应的位置驱动量；

所述S4包括：将所述期望接触力信息、所述实际接触力信息和所述实际位置信息输入阻抗控制模型，以使其输出所述力控制子空间对应的第二力驱动量；

所述S4中阻抗控制模型为：，

其中，为所述阻抗控制模型的输出，/>为期望接触力信息，/>为所述实际接触力信息；

利用公式和/>能够求解位移差值/>；/>为采样周期，/>，/>和/>分别为获取/>前第1，2，3个采样周期所获得的位移差值；

所述S4之前，所述方法还包括：

将原始期望接触力信息输入线性微分跟踪器进行平滑处理得到追踪信号/>和所述期望接触力信息/>；

k时刻的追踪信号为：；k+1时刻的期望接触力信息为：；

其中，，/>，/>；/>；/>；；

所述S3中机器人动力学模型为：；

其中，，/>，/>为角位移，/>为关节角速度，/>为角加速度，J为雅克比矩阵，/>为惯性矩阵，/>为雅克比矩阵的求导；/>，/>，h为科氏力项和重力项的叠加，/>，/>为关节的驱动力矩；F_e为实际接触力信息。

2.一种工业机器人的智能柔顺控制装置，其特征在于，用于执行权利要求1所述的工业机器人的智能柔顺控制方法，包括：

采集模块，用于在所述工业机器人执行工作任务时，利用六维力觉传感器采集末端执行器的实际位置信息和实际接触力信息；

位置驱动模型，用于将所述实际位置信息和期望位置信息通过机器人运动学模型输入机器人位置伺服环，以获取位置驱动量；

第一力驱动模块，用于将所述期望位置信息和所述实际接触力信息输入机器人动力学模型，以使其输出第一力驱动量；

第二力驱动模块，用于将期望接触力信息、所述实际接触力信息和所述实际位置信息输入阻抗控制模型，以获取第二力驱动量；

控制模块，用于利用所述位置驱动量、所述第一力驱动量和所述第二力驱动量控制所述工业机器人继续执行工作任务。