CN114750148A

CN114750148A - 一种重力自适应测量的力闭环零力控制方法及系统

Info

Publication number: CN114750148A
Application number: CN202210264601.8A
Authority: CN
Inventors: 管贻生; 王杰; 朱海飞; 陈燊豪; 叶鹏程
Original assignee: Guangdong University of Technology
Current assignee: Guangdong University of Technology
Priority date: 2022-03-17
Filing date: 2022-03-17
Publication date: 2022-07-15

Abstract

本发明涉及工业机器人控制技术领域，公开了一种重力自适应测量的力闭环零力控制方法及系统，包括以下步骤：S1.根据关节驱控测模块的电流、位置、速度、力矩模拟量，构建控测一体的协作机器人的关节层动力学模型；S2.根据所述的关节层动力学模型，构建一般形式的柔性关节动力学模型；S3.根据所述的一般形式的柔性关节动力学模型，构建关节层重力矩补偿矩阵，并构建重力矩自适应测量方法；S4.根据所述的一般形式的柔性关节动力学模型和所述的重力矩自适应测量方法，构建力闭环下的零力控制器，并通过所述的零力控制器，实现重力自适应测量的力闭环零力控制。本发明解决了现有机器人零力控制方法难以直接求解出机器人准确的重力矩及摩擦的问题。

Description

一种重力自适应测量的力闭环零力控制方法及系统

技术领域

本发明涉及工业机器人控制技术领域，更具体的，涉及一种重力自适应测量的力闭环零力控制方法及系统。

背景技术

人机协作在工业生产中发挥着越来越重要的角色。然而，工业机器人缺乏与人之间的协作、交互。工业机器人存在以下三个缺点：一是重量和体积较大，搬运困难；二是示教复杂，需要点动示教；三是因安全需要，机器人在工作时，必须将其置于封闭空间。机器人与人之间无法实现人机共融。

零力控制过程获取外力信息的方式有三种，第一种方式是在关节末端安装六维传感器，示教员通过拖动末端传感器得到力反馈，结合阻抗控制实现直接示教，其缺陷是只能拖动机器人末端关节，机器人不能感知本体信息；第二种方式是无需外力传感器而通过电流信息估计外力矩大小，该方法成本较低，但拖动的响应速度较低、感知能力较弱，常用于小型的机械臂的拖动示教中；第三种方式是在机器人各个关节安装高精度的力矩传感器，结合双编码器检测各个关节的运动量信息，可以通过广义动量法感知机器人本体各个位置的接触信息，并能建立出一个更高精确的动力学模型。

零力控制需解决的最主要的问题是辨识出机器人各个关节的重力矩及摩擦力矩。然而机器人动力学模型的复杂性难以直接求解出机器人准确的重力矩及摩擦力矩，而传统的通过CAD三维模型辨识的方法精度较低，且偏差较大。因此，一种更优化的方法是通过软件或控制算法，先将两者从动力学模型中解耦出来，再分别采集相关的数据进行辨识，从而大大简化模型，得到更精确的重力矩。

针对这一问题，现有专利有一种机器人零力控制方法，包括：S1、基于惯性力、离心力、科里奥利力、粘摩擦力、静摩擦力、重力，建立机器人动力学模型；S2、基于所述机器人动力学模型以及反馈数据，计算机器人各个关节的外力力矩；S3、根据步骤S2中计算的外力力矩计算速度指令；S4、根据速度指令以及位置反馈计算位置指令。

然而现有机器人零力控制方法有难以直接求解出机器人准确的重力矩及摩擦的问题，因此如何发明一种能够准确计算重力矩及摩擦的协作机器人零力控制方法，是本技术领域亟需解决的问题。

发明内容

本发明为了解决以上现有机器人零力控制方法难以直接求解出机器人准确的重力矩及摩擦的问题，提供了一种重力自适应测量的力闭环零力控制方法及系统，其具有计算准确，方便易用的特点。

为实现上述本发明目的，采用的技术方案如下：

一种重力自适应测量的力闭环零力控制系统的控制方法，包括以下步骤：

S1.根据关节驱控测模块的电流、位置、速度、力矩模拟量，构建控测一体的协作机器人的关节层动力学模型；

S2.根据所述的关节层动力学模型，构建一般形式的柔性关节动力学模型；

S3.根据所述的一般形式的柔性关节动力学模型，构建关节层重力矩补偿矩阵，并构建重力矩自适应测量方法；

S4.根据所述的一般形式的柔性关节动力学模型和所述的重力矩自适应测量方法，构建力闭环下的零力控制器，并通过所述的零力控制器，实现重力自适应测量的力闭环零力控制。

优选的，步骤S1，具体步骤为：

将所述的关节驱控测模块中的谐波减速器和力矩传感器等效为柔性弹簧，并将柔性弹簧结合关节驱控测模块中的力矩电机，一同等效为柔性关节，并根据所述的柔性关节构建关节层动力学模型：

其中，τ_m为所述的力矩电机的输出力矩，θ为力矩电机的转角，τ_f为力矩电机的摩擦力矩，τ为关节的输出力矩，q为关节的转角，B为力矩电机的惯性特性，K为柔性关节的等效刚度系数，τ_t为外部作用于负载的等效力矩，M(q)表示与柔性关节相连的所有关节的合转动惯量，

为θ的二阶导数、

为q的二阶导数；

式中，力矩电机输出力矩τ_m产生力矩电机的转角θ，通过谐波减速器的力矩放大及摩擦力矩τ_f消耗，将力矩τ_m传递至关节末端，得到关节输出力矩τ，所述的关节输出力矩τ带动负载输出关节转角q。

更进一步的，步骤S2，具体步骤为：

S201.根据所述的关节层动力学模型，结合力矩电机的反电动势，建立观测值与电机性能的参数耦合方程：

其中，τ_m(t)表示力矩电机的实时输出力矩；i_m(t)表示力矩电机的实时输出的电枢电流；e_m(t)表示力矩电机的实时输出的力矩电机的实时反电动势；k_m表示力矩电机的电流-力矩转换系数，h_m表示反电动势与力矩电机转速的比例系数；

S202.根据关节的摩擦特性和自身的转动，构建从静止到动态的柔性关节动力学模型：

S203.根据所述的参数耦合方程和从静止到动态的柔性关节动力学模型，结合电机输出力矩τ_m的重力项、惯性力项及向心力项，建立一般形式的柔性关节动力学模型：

式中，

为q的一阶导数。

更进一步的，步骤S3所述的重力矩自适应测量方法，其具体步骤为：

S301.根据力矩传感器获得的力矩信息，构建重力矩随若干个关节的角度变化模型：

其中，G(q)＝[G₁(q) G₂(q) G₃(q) G₄(q) ... G_n(q)]^T，表示关节重力矩辨识矩阵，G_c＝[G_c1 G_c2 G_c3 G_c4 ... G_cn]^T表示各个关节重力矩影响常数，

表示所述的协作机器人任意设定一组位姿下，下一关节的重力矩与当前关节所成夹角的正弦函数；

S302.将参数矩阵G_c中的行向量和

的列向量相互解耦：

其中，

S303.根据关节重力矩辨识矩阵G(q)，构造所述的若干各关节的重力矩补偿模型：

S304.展开所述的重力矩补偿模型，得到关节层重力矩补偿项：

S305.通过所述的关节层重力矩补偿项，在不同角度下对重力矩G_c进行解耦，并实时解算各个关节的重力矩常数项，并且当末端负载变化，通过辨识角度完成重力矩自测量及补偿参数更新。

更进一步的，步骤S4，根据所述的一般形式的柔性关节动力学模型和所述的重力矩自适应测量方法，构建力闭环下的零力控制器，具体步骤为：

S401.将所述的力矩传感器测得的等效力矩τ_t作为输入，力矩电机产生的力矩τ_m作为输出，得到比例控制器：

τ_m＝-K_t·τ_t；

S402.将得到的比例控制器结合所述的关节层动力学模型，得到外力与需要克服的惯量M(q)和摩擦力矩τ_f方程：

S403.将惯量M(q)和摩擦力矩τ_f方程结合关节层动力学模型，得到第二方程：

S404.所述的第二方程结合所述的重力矩和速度补偿的力闭环，建立力闭环下的零力控制器。

更进一步的，步骤S404，结合重力矩和速度补偿的力闭环，建立力闭环下的零力控制器，具体步骤为：

T101.将第二方程进行扩展，构建考虑重力矩和速度补偿的力闭环，构建零力控制器：

T102.将所述的零力控制器重新代入柔性关节动力学模型，中，得到力闭环下的零力控制器：

更进一步的，所述的柔性关节动力学模型在等效力矩τ_t＝0，并且

时等效为：

其中，

kii表示第i个柔性关节的速度补偿系数，i＝1，2，…，v；

kuu表示第i个柔性关节的力闭环系数，i＝1，2，…，u，K_p及K均为正定对角矩阵。

一种重力自适应测量的力闭环零力控制系统，包括协作机器人、多轴实时控制器；所述的协作机器人包括若干个关节，所述的若干个关节均安装有关节驱控测模块，所述的关节驱控测模块用于控制、驱动、测量所述的关节的运作，所述的多轴实时控制器与所述的关节驱控测模块通信连接，执行所述的重力自适应测量的力闭环零力控制方法。

优选的，所述的关节还包括关节壳和关节座，所述的关节壳和关节座连接，并且所述的关节驱控测模块的两端分别与所述的关节壳和关节座连接。

更一步的，所述的关节驱控测模块包括力矩传感器、谐波减速器、力矩电机、电机端增量编码器、关节端绝对式编码器、电磁抱闸、驱动器；所述的力矩传感器的一端、谐波减速器、力矩电机的一端依次相连，所述的力矩传感器的另一端与所述的关节座固定连接；所述的力矩传感器用于测量关节的扭矩参数，所述的谐波减速器用于调节所述的力矩电机的输出扭矩，所述的力矩电机用于驱动所述的关节的运动；所述的驱动器、关节端绝对式编码器的一端、电磁抱闸、电机端增量编码器分别与所述的力矩电机的另一端连接，所述的关节端绝对式编码器的另一端与所述的关节壳固定连接；所述的驱动器用于控制并驱动所述的力矩电机，所述的关节端绝对式编码器用于测量关节的绝对位置参数，所述的电机端增量编码器用于测量力矩电机的转速和相对位置参数，所述的电磁抱闸用于急停抱闸所述的力矩电机；所述的力矩传感器、电机端增量编码器、关节端绝对式编码器、电磁抱、驱动器闸分别与所述的多轴实时控制器无线连接，并实时提供关节的绝对位置参数、相对位置参数、速度参数、扭矩参数给多轴实时控制器。

本发明的有益效果如下：

本发明通过将驱控测模块获取的电流、位置、速度、力矩模拟量构建控测一体的协作机器人的关节层动力学模型，并结合摩擦力矩及惯性力矩，构建了一般形式的柔性关节动力学模型，又根据零力补偿模型，结合重力矩自适应测量方法，构建力闭环下的零力控制器；由此，本发明实现了重力自适应测量的力闭环零力控制，解决了现有机器人零力控制方法难以直接求解出机器人准确的重力矩及摩擦的问题，且具有计算准确，方便易用的特点。

附图说明

图1是本重力自适应测量的力闭环零力控制方法的流程示意图

图2是柔性关节的等效模型图。

图3是本发明中协作机器人的等效模型。

图4是重力自适应测量的力闭环零力控制方法的实施框图。

图5是本重力自适应测量的力闭环零力控制系统的系统框图。

图6是本重力自适应测量的力闭环零力控制系统的结构示意图。

图7是本重力自适应测量的力闭环零力控制系统中的关节的结构示意图。

具体实施方式

下面结合附图和具体实施方式对本发明做详细描述。

实施例1

如图1所示，一种重力自适应测量的力闭环零力控制系统的控制方法，包括以下步骤：

实施例2

如图2、图3所示，在一个具体实施例中，在一个具体实施例中，步骤S1，具体步骤为：

将所述的关节驱控测模块中的谐波减速器和力矩传感器等效为柔性弹簧，并将柔性弹簧结合关节驱控测模块中的力矩电机，一同等效为柔性关节，电机与输出负载之间通过弹簧进行连接和力矩传递，为提高动力学模型传导准确性，考虑关节输入端及输出端力矩特性，构建关节层动力学模型：

为θ的二阶导数、

为q的二阶导数；

在一个具体实施例中，步骤S2，具体步骤为：

S201.根据所述的关节层动力学模型，结合力矩电机的反电动势，电机所产生力矩随电枢电流线性变化而产生，表现为电动机的反电动势与电机的角速度成正比，建立观测值与电机性能的参数耦合方程：

S202.根据关节的摩擦特性和自身的转动，考虑协作机器人从静止状态直接切换至零力控制模式，需施加较大的外力来克服静摩檫力以及自身的转动惯量，实现对各个关节的初始拖动，由此考虑摩擦特性地构建从静止到动态的柔性关节动力学模型：

本实施例中，由于电机输出力矩τ_m经过谐波减速器放大后直接输出末端力矩，此力矩值已经实现摩擦力矩的解耦，只包含

式中，

为q的一阶导数。

在一个具体实施例中，步骤S3所述的重力矩自适应测量方法，其具体步骤为：

S302.将参数矩阵G_c中的行向量和

的列向量相互解耦：

其中，

本实施例中，本发明搭建的协作机器人的各关节重力矩为：

G₁＝0

G₂＝(M₂x₂+M₃d₂+M₅d₂+M_Ld₂)sin(θ₂)+G₃

G₃＝(M₃x₃+M₅d₃+M_Ld₃)sin(θ₂+θ₃)+G₅

G₄＝0

G₅＝(M₅x+M_Ld₅)sin(θ₂+θ₃+θ₅)

将上式常数项表述为矩阵形式，即：

φ(q)＝[φ₁(q) φ₂(q) φ₃(q) φ₄(q) φ₅(q)]^T＝[0 sin(α₂) sin(α₂+α₃) 0sin(α₂+α₃+α₅)]

则

式中，M₂表示关节2和关节3之间连杆的质量，M₃表示关节3和关节5之间连杆的质量，M₅表示关节5从关节转轴到末端之间连杆的质量，M_L表示末端负载的质量，x_i表示关节i与关节i+1之间的连杆质心到关节转轴的距离，d_i表示关节i与关节i+1之间的连杆杆长，α_i表示关节i转动角度。

在一个具体实施例中，本协作机器人构型下负载切换时重力矩自测量位姿矩阵。

在一个具体实施例中，当完成负载切换时，可构建实时参数矩阵：

将机器人切换至位置控制模式，下发重力矩测量位姿给各关节：

解算重力矩常数项：

G₂₀＝τ₂₀

G₃₀＝τ₂₁＝τ₃₀

G₅₀＝τ₂₂＝τ₃₁＝τ₅₁；

本实施例中，由于拖动示教的过程，受到外界力矩反馈的作用，力矩电机输出的角速度为

可根据公式

通过反馈力矩电机工作电动势的信息，获取力矩电机的输出的角速度。

如图4所示，在一个具体实施例中，步骤S4，根据所述的一般形式的柔性关节动力学模型和所述的重力矩自适应测量方法，构建力闭环下的零力控制器，具体步骤为：

τ_m＝-K_t·τ_t；

本实施例中，可看出，若K_t＞0，在使用相当外力下拖动机械臂，机器人本体惯量和摩擦力矩影响将缩小为原来的

倍，且K_t值越大，拖动者可获取更轻的效果。

本实施例中假设在某时刻，拖动外力撤离或机器人即将静止，此时有：

此时柔性关节动力学模型可表征为：

若在该电机角度下：

-G(q)-(1+K_t)^-1τ_f＞0

即

机器人各关节将因重力矩影响而摆动，即无法在外力撤去后保持在当前位置，故比例控制器无法保证稳定性，不满足实际控制需求。

在一个具体实施例中，步骤S404，结合重力矩和速度补偿的力闭环，建立力闭环下的零力控制器，具体步骤为：

T102.将所述的零力控制器重新代入柔性关节动力学模型中，得到力闭环下的零力控制器：

在一个具体实施例中，所述的柔性关节动力学模型在等效力矩τ_t＝0，并且

时等效为：

其中，

kii表示第i个柔性关节的速度补偿系数，i＝1，2，…，v；

实施例3

如图5所示，一种重力自适应测量的力闭环零力控制系统，包括协作机器人、多轴实时控制器；所述的协作机器人包括若干个关节，所述的若干个关节均安装有关节驱控测模块，所述的关节驱控测模块用于控制、驱动、测量所述的关节的运作，所述的多轴实时控制器与所述的关节驱控测模块通信连接，执行所述的重力自适应测量的力闭环零力控制方法。

本实施例中，所述的多轴实时控制器通过24V电源驱动。

本实施例中，所述的协作机器人通过48V电源驱动。

本实施例中，所述的多轴实施控制器通过EtherCAT总线与关节驱控测模块相连，读取各关节的电流、位置、速度、力矩模拟量等参数信息，并按照规定的控制周期向各驱控测一体关节控制单元发送控制信息，进而完成整个协作机器人的实时控制；所述的多轴实时控制器还设有实时控制程序，且所述的多轴实时控制器通过TCP/IP外连PC机；所述的PC机通过编译实时控制程序，实时连接控制监测所述的协作机器人。

本实施例中，所述的协作机器人为五自由度的驱控测一体的协作机器人，所述的协作机器人包括基座1、第一关节2、第二关节3、第三关节4、第四关节5、第五关节6，所述的基座1、第一关节2、第二关节3、第三关节4、第四关节5、第五关节6依次相连，其中第一关节2、第四关节5输出端轴线与安装轴线重合，结构相同，并且第一关节2尺寸大于第四关节5；第二关节3、第三关节4、第五关节6输出轴线与中心轴线垂直，结构相同，第二关节3、第三关节4尺寸相同，且第二关节3和第三关节4的尺寸大于第五关节6。所述协作机器人的所有组件的中心轴线重合。

如图6所示，在一个具体实施例中，所述的关节还包括关节壳7和关节座8，所述的关节壳7和关节座8固定连接，并且所述的关节驱控测模块的两端分别与所述的关节壳7和关节座8连接。

如图6、图7示，在一个具体实施例中，所述的关节驱控测模块包括力矩传感器9、谐波减速器10、力矩电机11、电机端增量编码器12、关节端绝对式编码器13、电磁抱闸14、驱动器15；所述的力矩传感器9的一端、谐波减速器10、力矩电机11的一端依次相连，所述的力矩传感器9的另一端与所述的关节座8固定连接；所述的力矩传感器9用于测量关节的扭矩参数，所述的谐波减速器10用于调节所述的力矩电机11的输出扭矩，所述的力矩电机11用于驱动所述的关节的运动，并且相连的谐波减速器10和关节力矩传感器9可等效为柔性弹簧结构；所述的驱动器15、关节端绝对式编码器13的一端、电磁抱闸14、电机端增量编码器12分别与所述的力矩电机11的另一端连接，所述的关节端绝对式编码器13的另一端与所述的关节壳7固定连接；所述的驱动器15用于控制并驱动所述的力矩电机11，所述的关节端绝对式编码器13用于测量关节的绝对位置参数，所述的电机端增量编码器12用于测量力矩电机11的转速和相对位置参数，所述的电磁抱闸14用于急停抱闸所述的力矩电机11；所述的力矩传感器9、电机端增量编码器12、关节端绝对式编码器13、电磁抱、驱动器15闸分别与所述的多轴实时控制器无线连接，并实时提供关节的绝对位置参数、相对位置参数、速度参数、扭矩参数给多轴实时控制器。

本实施例中，所述的关节驱控测模块为中空走线，导线从关节壳分别通过通所述的驱动器15、电磁抱闸14、关节端绝对式编码器13、电机端增量编码器12、力矩电机11、谐波减速器10、力矩传感器9走线至关节座，由此实现关节内部走线，将所有关节串联起来。若干个关节驱控测模块之间通过EtherCAT无线连接。

在一个具体实施例中，本重力自适应测量的力闭环零力的控制方法通过各个关节集成的驱控测模块将获取电流、位置、速度、力矩模拟量，构建控测一体的协作机器人的关节层动力学模型结合摩擦力矩及惯性力矩，构建了一般形式的柔性关节动力学模型，又根据零力补偿模型，结合重力矩自适应测量方法，构建力闭环下的零力控制器，实现重力自适应测量的力闭环零力控制，实现了关节摩檫力矩解耦，减少了摩擦力矩辨识带来的不稳定性，实现了力闭环下的控制响应速度及鲁棒性均好于单纯的力矩控制及位置控制，解决了现有机器人零力控制方法难以直接求解出机器人准确的重力矩及摩擦的问题，且具有计算准确，方便易用的特点。

显然，本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定。凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明权利要求的保护范围之内。

Claims

1.一种重力自适应测量的力闭环零力控制系统的控制方法，其特征在于：包括以下步骤：

2.根据权利要求1所述的重力自适应测量的力闭环零力控制方法，其特征在于：步骤S1，具体步骤为：

为θ的二阶导数、

为q的二阶导数；

3.根据权利要求2所述的重力自适应测量的力闭环零力控制方法，其特征在于：步骤S2，具体步骤为：

式中，

为q的一阶导数。

4.根据权利要求3所述的重力自适应测量的力闭环零力控制方法，其特征在于：步骤S3所述的重力矩自适应测量方法，其具体步骤为：

其中，G(q)＝[G₁(q) G₂(q) G₃(q) G₄(q) … G_n(q)]^T表示关节重力矩辨识矩阵，G_c＝[G_c1 G_c2 G_c3 G_c4 … G_cn]^T表示各个关节重力矩影响常数，

S302.将参数矩阵G_c中的行向量和

的列向量相互解耦：

其中，

S303.根据关节重力矩辨识矩阵G(q)，构造若干各关节的重力矩补偿模型：

5.根据权利要求4所述的重力自适应测量的力闭环零力控制方法，其特征在于：步骤S4，根据所述的一般形式的柔性关节动力学模型和所述的重力矩自适应测量方法，构建力闭环下的零力控制器，具体步骤为：

τ_m＝-K_t·τ_t；

6.根据权利要求5所述的重力自适应测量的力闭环零力控制方法，其特征在于：步骤S404，结合重力矩和速度补偿的力闭环，建立力闭环下的零力控制器，具体步骤为：

7.根据权利要求6所述的重力自适应测量的力闭环零力控制方法，其特征在于：所述的柔性关节动力学模型在等效力矩τ_t＝0，并且

时等效为：

其中，

k_ii表示第i个柔性关节的速度补偿系数，i＝1,2,…,v；

k_uu表示第i个柔性关节的力闭环系数，i＝1,2,…,u，K_p及K均为正定对角矩阵。

8.一种重力自适应测量的力闭环零力控制系统，其特征在于：包括协作机器人、多轴实时控制器；所述的协作机器人包括若干个关节，所述的若干个关节均安装有关节驱控测模块，所述的关节驱控测模块用于控制、驱动、测量所述的关节的运作，所述的多轴实时控制器与所述的关节驱控测模块通信连接，执行如权利要求1～7任一项所述的重力自适应测量的力闭环零力控制方法。

9.根据权利要求8所述的重力自适应测量的力闭环零力控制系统，其特征在于：所述的关节还包括关节壳和关节座，所述的关节壳和关节座连接，并且所述的关节驱控测模块的两端分别与所述的关节壳和关节座连接。

10.根据权利要求9所述的重力自适应测量的力闭环零力控制系统，其特征在于：所述的关节驱控测模块包括力矩传感器、谐波减速器、力矩电机、电机端增量编码器、关节端绝对式编码器、电磁抱闸、驱动器；所述的力矩传感器的一端、谐波减速器、力矩电机的一端依次相连，所述的力矩传感器的另一端与所述的关节座固定连接；所述的力矩传感器用于测量关节的扭矩参数，所述的谐波减速器用于调节所述的力矩电机的输出扭矩，所述的力矩电机用于驱动所述的关节的运动；所述的驱动器、关节端绝对式编码器的一端、电磁抱闸、电机端增量编码器分别与所述的力矩电机的另一端连接，所述的关节端绝对式编码器的另一端与所述的关节壳固定连接；所述的驱动器用于控制并驱动所述的力矩电机，所述的关节端绝对式编码器用于测量关节的绝对位置参数，所述的电机端增量编码器用于测量力矩电机的转速和相对位置参数，所述的电磁抱闸用于急停抱闸所述的力矩电机；所述的力矩传感器、电机端增量编码器、关节端绝对式编码器、电磁抱、驱动器闸分别与所述的多轴实时控制器无线连接，并实时提供关节的绝对位置参数、相对位置参数、速度参数、扭矩参数给多轴实时控制器。