CN116423522A - 一种摩擦力自适应补偿的机器人拖动示教零力控制方法 - Google Patents

Abstract

本发明公开了一种摩擦力自适应补偿的机器人拖动示教零力控制方法，包括采用库仑粘性摩擦模型对机器人的关节摩擦力进行建模；采集关节在不同速度等级匀速运动时的力矩反馈值进行关节摩擦力的拟合计算；采用梯形双向交变方法构建启动力矩补偿；设置关节力矩限制和关节速度限制，并结合机器人运动时的摩擦力矩补偿值以及所构建的启动力矩补偿，得到机器人拖动示教零力控制的输出力矩。本发明根据关节运动时速度的变化来进行摩擦力自适应补偿，并针对启动时需要施加较大拖动力矩的问题进行优化，在此基础上，对关节运动时的力矩和速度进行限制，来保证运动过程中的安全性。

Description

一种摩擦力自适应补偿的机器人拖动示教零力控制方法

技术领域

本发明属于机器人控制技术领域，具体涉及一种摩擦力自适应补偿的机器人拖动示教零力控制方法。

背景技术

工业机器人是智能制造过程中的核心自动化设备，包括有六关节机器人、四关节机器人、并联机器人、SCARA(Selective Compliance Assembly Robot Arm)机器人等等。机器人示教是影响机器人工作效率的关键，是机器人领域研究和应用的主要难点问题之一。以SCARA机器人为例，其有三个旋转关节和一个上下移动丝杆组成，具有精度高、速度快、控制简单的特点，被广泛地应用于3C制造、食品、制药等各类行业之中。

然而，SCARA机器人的示教技术是实现精确加工和高效控制的关键难点。拖动示教是指可以直接用手拖动机器人末端来记录示教轨迹，具有使用方便、操作简单等很多优点。实现拖动示教的前提是实现SCARA机器人的零力控制，目前有两种零力控制方案，一是基于位置控制和传感器的零力控制方法，另一种是力矩模式下的零力控制方法。但是位置控制需要配合传感器使用，成本较高，而力矩模式下无需使用传感器，使用更为广泛。

目前，力矩模式下实现零力控制通常只针对运动时的摩擦力和重力进行补偿，然而在关节运功时，关节速度一直在快速变化，当前的力矩模式补偿方法无法对摩擦力进行自适应实时补偿。在实际应用中，除了运动时需要力矩补偿之外，关节启动前的静摩擦力也很大，会出现启动之前需要的拖动力很大的问题，且在进行力矩补偿时，缺少对关节运动的限制，可能出现机器跑飞的现象。因此，需要提出一种摩擦力自适应补偿的机器人拖动示教零力控制方法。

发明内容

本发明的目的在于提供一种摩擦力自适应补偿的机器人拖动示教零力控制方法，根据关节运动时速度的变化来进行摩擦力自适应补偿，并针对启动时需要施加较大拖动力矩的问题进行优化，在此基础上，对关节运动时的力矩和速度进行限制，来保证运动过程中的安全性。

为实现上述目的，本发明所采取的技术方案为：

一种摩擦力自适应补偿的机器人拖动示教零力控制方法，所述摩擦力自适应补偿的机器人拖动示教零力控制方法，包括：

步骤1、采用库仑粘性摩擦模型对机器人的关节摩擦力进行建模；

步骤2、采集关节在不同速度等级匀速运动时的力矩反馈值进行关节摩擦力的拟合计算；

步骤201、采集关节在不同速度等级匀速运动时的力矩反馈值，并采用卡尔曼滤波算法对力矩反馈值进行滤波；

步骤202、取滤波后的力矩反馈值，对每一速度等级的力矩反馈值求平均值，并基于不同速度等级的力矩反馈值的平均值进行最小二乘法线性拟合，得到机器人运动时的摩擦力矩补偿值；

步骤3、采用梯形双向交变方法构建启动力矩补偿；

步骤301、构建梯形双向交变方法中梯形辅助启动力矩在一个周期内包括正向匀加力矩阶段、正向峰值保持阶段、负向匀减力矩阶段和负向峰值保持阶段；

步骤302、计算正向匀加力矩阶段和负向匀减力矩阶段的斜率，以及正向峰值保持阶段和负向峰值保持阶段的辅助启动力矩；

步骤303、构建一个周期内对启动力矩进行补偿的梯形辅助启动力矩；

步骤4、设置关节力矩限制和关节速度限制，并结合机器人运动时的摩擦力矩补偿值以及所构建的启动力矩补偿，得到机器人拖动示教零力控制的输出力矩。

以下还提供了若干可选方式，但并不作为对上述总体方案的额外限定，仅仅是进一步的增补或优选，在没有技术或逻辑矛盾的前提下，各可选方式可单独针对上述总体方案进行组合，还可以是多个可选方式之间进行组合。

作为优选，所述采用库仑粘性摩擦模型对机器人的关节摩擦力进行建模，包括：

式中，τ_f为待求解的关节摩擦力，f_cou为库仑摩擦力系数，f_vis为粘性摩擦力系数，

为当前关节的运动速度，符号函数/>

为关节运动速度的方向，其定义如下：

式中，σ为判定速度是否为0的界定阈值。

作为优选，所述采用卡尔曼滤波算法对力矩反馈值进行滤波，包括：

建立力矩反馈值的离散状态方程和观测方程如下：

x_k＝x_k-1+u_k-1+w_k-1

z_k＝x_k+v_k

式中，x_k是在第k时刻控制***的状态，x_k-1是在第k-1时刻控制***的状态，u_k-1是在第k-1时刻对控制***的控制量，w_k-1是在测量过程中第k-1时刻的过程噪声，z_k是运动控制过程中在第k时刻进行测量的实际测量值，v_k是在测量过程中第k-1时刻的测量噪声；

建立卡尔曼滤波器的更新方程如下：

建立卡尔曼滤波器状态的更新方程为：

式中，

表示在第k时刻的先验状态估计值，/>

和/>

分别表示在第k-1时刻和k时刻的后验状态估计值，P_k-1和P_k分别表示在第k-1和k时刻的后验估计协方差，/>

表示在第k时刻的先验估计协方差，K_k代表在卡尔曼滤波器的更新方程中的卡尔曼增益，Q为过程噪声的方差，R为测量噪声的方差。

作为优选，所述基于不同速度等级的平均值进行最小二乘法线性拟合，得到机器人运动时的摩擦力矩补偿值，包括：

计算不同速度等级的力矩反馈值的平均值与关节电机的最大力矩的比值百分比作为摩擦力矩百分比，基于最小二乘法线性拟合得到速度等级与摩擦力矩百分比的拟合关系；

基于速度等级与摩擦力矩百分比的拟合关系，将摩擦力矩百分比作为关节电机的转矩百分比，取速度等级值相同、方向相反的反向速度等级，取转矩百分比值相同、方向相反的反向转矩百分比，得到双侧速度等级与双侧转矩百分比的拟合关系，并将双侧速度等级与双侧转矩百分比的拟合关系中速度等级对应的转矩百分比下的转矩值作为机器人运动时的摩擦力矩补偿值。

作为优选，所述计算正向匀加力矩阶段和负向匀减力矩阶段的斜率，以及正向峰值保持阶段和负向峰值保持阶段的辅助启动力矩，包括：

f_peak＝η·f_{sta_max}

-f_peak＝η·(-f_{sta_max})

式中，f_peak为正向峰值保持阶段的辅助启动力矩，η为常系数，f_{sta_max}为正向关节最大静摩擦力矩，-f_peak为负向峰值保持阶段的辅助启动力矩，-f_{sta_max}为负向关节最大静摩擦力矩，k为正向匀加力矩阶段的斜率，-k为负向匀加力矩阶段的斜率，T_a为正向匀加力矩阶段或负向匀减力矩阶段的时间。

作为优选，所述输出一个周期内对启动力矩进行补偿的梯形辅助启动力矩，包括：

式中，τ_start为梯形辅助启动力矩，k为正向匀加力矩阶段的斜率，t为一个周期内的当前时间，T_a为正向匀加力矩阶段或负向匀减力矩阶段的时间，f_peak为正向峰值保持阶段的辅助启动力矩，T为一个周期的总时间，T_u为正向峰值保持阶段或负向峰值保持阶段的时间，-k为负向匀加力矩阶段的斜率，-f_peak为负向峰值保持阶段的辅助启动力矩。

作为优选，所述设置关节力矩限制和关节速度限制，并结合机器人运动时的摩擦力矩补偿值以及所构建的启动力矩补偿，得到机器人拖动示教零力控制的输出力矩，包括：

当机器人进入拖动示教且速度为0时，机器人拖动示教零力控制的输出力矩为所述梯形辅助启动力矩；

当机器人进入拖动示教且速度不为0时，首先进行关节力矩限制判断，若满足关节力矩限制，则根据关节力矩限制得到机器人拖动示教零力控制的输出力矩，否则再进行关节速度限制判断，若满足关节速度限制判断，则根据关节速度限制得到机器人拖动示教零力控制的输出力矩，否则读取机器人关节步骤202得到的摩擦力矩补偿值作为机器人拖动示教零力控制的输出力矩。

作为优选，所述关节力矩限制和关节速度限制设置如下：

设置关节力矩限制如下：

式中，τ_{out_tor}表示关节力矩限制下得到的机器人拖动示教零力控制的输出力矩，τ_current为当前关节的实时力矩，τ_max为当前关节的正向最大限制力矩，-τ_max为当前关节的负向最大限制力矩；

设置关节速度限制如下：

式中，τ_{out_vel}表示关节速度限制下得到的机器人拖动示教零力控制的输出力矩，v_current为当前关节的实时速度，k为临近最大限制速度系数，且0≤k≤1，-v_max为当前关节的负向最大限制速度，v_max为当前关节的正向最大限制速度。

当SCARA机器人在进行拖动时，关节速度在不断变化，此时关节摩擦力也在不断变化，如何对关节摩擦力快速辨识和自适应补偿成为了一个很重要的问题。因此，本发明提出了一种摩擦力自适应补偿的机器人拖动示教零力控制方法，应用于SCARA机器人的拖动示教场景，针对SCARA机器人在运动时的关节摩擦力进行自适应补偿，识别方法简单，计算量小，且改善了以往的力矩模式下零力控制方法启动时静摩擦力大的问题，对其进行了优化，在***没有摩擦模型或者末端夹具更换导致负载量变化时可以快速识别计算并补偿。在使用低算力芯片例如嵌入式单片机设备STM32等作为SCARA机器人控制器的低算力场景下也有较好的应用价值。

附图说明

图1为本发明的一种摩擦力自适应补偿的机器人拖动示教零力控制方法的流程图；

图2为本发明的速度等级与摩擦力矩百分比的拟合关系示意图；

图3为本发明的双侧速度等级与双侧转矩百分比的拟合关系示意图；

图4为本发明的梯形辅助启动力矩的示意图；

图5为本发明关节力矩限制和关节速度限制判断流程图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

除非另有定义，本文所使用的所有的技术和科学术语与属于本发明的技术领域的技术人员通常理解的含义相同。本文中在本发明的说明书中所使用的术语只是为了描述具体的实施例的目的，不是在于限制本发明。

在进行SCARA机器人的拖动示教时，受到各种因素影响，人手无法精确地对关节的运动速度进行控制，这就导致了关节的运动速度在不停地变化，而关节摩擦力也随着速度的变化在不断地变化，因此要根据关节的不同运动速度来对关节摩擦力进行自适应补偿。

SCARA机器人的动力学模型如下：

式中，τ为关节力矩，M(θ)为机械臂质量矩阵，

为惯性力的作用，/>

为哥氏力和离心力的作用，G(θ)为机械臂重力，τ_f为关节摩擦力。

由于SCARA机器人的第三、四关节是在第二关节的末端基础上进行上下位移和姿态旋转，因此在实际应用中SCARA机器人在笛卡尔空间下的轨迹运动主要由第一、二关节决定。基于以上原因，在SCARA机器人的零力控制拖动示教过程中，选择SCARA机器人的前两个关节来进行力矩补偿以实现零力控制，即针对所选中的两个关节中的每个关节单独通过本实施例提出的摩擦力自适应补偿的机器人拖动示教零力控制方法进行控制。

SCARA机器人的前两个关节为转动关节，并且其关节轴线与铅垂线平行，在拖动示教过程中前两轴不受重力影响，且拖动的速度较低，受到的惯性力、哥氏力的影响几乎可以忽略不计，因此对于SCARA机器人的零力控制来说辨识不同运动状态下的关节摩擦力并对其进行自适应补偿是最为重要的部分。

如图1所示，本实施例提供一种摩擦力自适应补偿的机器人拖动示教零力控制方法，包括以下步骤：

步骤1、采用库仑粘性摩擦模型对机器人的关节摩擦力进行建模。

常见的摩擦力模型有很多种，例如Stribeck摩擦模型和LuGre摩擦模型为相对复杂的摩擦力模型，虽然精确度更高，但是在进行参数辨识时需要经过大量的运算，增加了***的负荷。而在人手牵引SCARA机器人末端的过程中，库仑摩擦力(摩擦力跟作用在摩擦面上的正压力成正比，跟外表的接触面积无关)和粘性摩擦力为主要摩擦力，其他摩擦力可以忽略不计，因此本实施例采用库仑粘性摩擦力模型作为SCARA机器人的摩擦力模型，库仑粘性摩擦力模型表示如下：

式中，τ_f为待求解的关节摩擦力；f_cou为库仑摩擦力系数，库仑摩擦力的大小由库仑摩擦力系数决定，但是其方向和关节运动速度方向相关；f_vis为粘性摩擦力系数，由公式可以得出粘性摩擦力和关节的运动速度大小为正比关系；

为当前关节的运动速度；符号函数/>

为关节运动速度的方向，其定义如下：

式中，考虑到误差影响，设定σ为判定速度是否为0的界定阈值，当

时，认为关节并没有运动，σ可以根据具体机械参数和使用工况结合计算精度取值，例如σ＝0.001～0.01。

库仑粘性摩擦力模型适合于SCARA机器人且在计算上相对简单，适合应用于低算力SCARA机器人控制器场景中，而且其包含了误差的主要部分，与真实摩擦力误差很小，在实际拖动示教应用场景中误差摩擦力可以忽略不计，具有很大的适用性和实用性。

步骤2、采集关节在不同速度等级匀速运动时的力矩反馈值进行关节摩擦力的拟合计算。

步骤201、采集关节在不同速度等级匀速运动时的力矩反馈值，并采用卡尔曼滤波算法对力矩反馈值进行滤波。

在选定摩擦力模型后便开始进行参数辨识过程，由物理知识可得，当关节保持匀速运动时，关节力矩和摩擦力矩值量相等，因此摩擦力参数辨识中设定为单关节匀速转动，每个关节的运动范围为[-π/4,π/4]，并控制其他变量不变，按照不同速度等级来进行匀速运动。

力矩反馈值可以通过电机驱动器获取，也可以通过电流值来进行估算，本实施例中不进行限制。由于库仑粘性摩擦力模型具有对称性，因此在数据采集时可以将双向的数据进行符号统一化合并运算，减少了一半的采集工作时间，即可以仅采集正向速度下的力矩反馈值，负向速度下的力矩反馈值由正向速度下的力矩反馈值对称得到。

由于实际获取到的力矩反馈值会有波动，且在存在干扰的情况下，可能存在突变的无效值，如果直接用于计算会降低计算精度，因此采用卡尔曼滤波算法来对力矩反馈值数据进行滤波，力矩反馈值的离散状态方程和观测方程如下：

x_k＝x_k-1+u_k-1+w_k-1

z_k＝x_k+v_k

式中，x_k是在第k时刻控制***的状态，x_k-1是在第k-1时刻控制***的状态，u_k-1是在第k-1时刻对控制***的控制量，w_k-1是在测量过程中第k-1时刻的过程噪声，z_k是运动控制过程中在第k时刻进行测量的实际测量值，v_k则表示测量过程中第k-1时刻的测量噪声。

建立卡尔曼滤波器的更新方程如下：

建立卡尔曼滤波器状态的更新方程为：

式中，

表示在第k时刻的先验状态估计值，/>

和/>

表示在第k-1时刻和k时刻的后验状态估计值，P_k-1和P_k表示在第k-1和k时刻的后验估计协方差，/>

表示k时刻的先验估计协方差，K_k代表在卡尔曼滤波器的更新方程中的卡尔曼增益，Q为过程噪声w的方差，R为测量噪声v的方差。

采用卡尔曼滤波算法时每次迭代只用到前几个数据，运算量很小，适用于低算力的应用场景，式中Q和R的值可以根据不同的机械结构组成来设定。本实施例的卡尔曼滤波算法为常规算法。设采样速度等级个数为M个，每个速度等级采样的数据点个数为N个，则采样后的数据为t_i(j)(i＝1,2,...,M；j＝1,2,...,N)，i表示速度等级即当前速度和最大速度的百分比，则可以得出M＜100，每次只取一个速度等级的力矩反馈值进行滤波处理，经过滤波后的力矩反馈值为t_k_i(j)。

步骤202、取滤波后的力矩反馈值，对每一速度等级的力矩反馈值求平均值，并基于不同速度等级的平均值进行最小二乘法线性拟合，得到机器人运动时的摩擦力矩补偿值。

在得到滤波后的力矩反馈值后，由于在拟合计算时每个速度等级只需要一个数据即可，因此对将滤波后的力矩反馈值求平均值，可以得出每个速度等级的力矩反馈值的平均值t_ka_i：

式中，t_ka_i为速度等级i的力矩反馈值的平均值，t_k_i(j)为滤波后的速度等级i的第j个力矩反馈值。

基于不同速度等级的平均值进行最小二乘法线性拟合得到速度等级(即速度百分比)与力矩反馈值的拟合关系，为了和速度等级相对应，将力矩反馈值换算成为摩擦力矩百分比，即当前摩擦力矩(即力矩反馈值的平均值)与电机最大力矩的比值百分比，得到的拟合关系如图2所示，图2中圆圈为离散值，直线为拟合后的关系线。

基于速度百分比与摩擦力矩百分比的拟合关系，取速度百分比值相同、方向相反的反向速度百分比，取摩擦力矩百分比相同、方向相反的反向摩擦力矩百分比，得到双侧速度等级与双侧摩擦力矩百分比的拟合关系，由于电机需要提供转矩来消除摩擦力矩的影响，因此可以计算得出电机应输出的转矩百分比，即输出转矩(与摩擦力矩相同)和最大转矩的比值百分比，即将摩擦力矩百分比作为关节电机的转矩百分比，结果如图3所示，并将双侧速度等级与双侧转矩百分比的拟合关系中速度等级对应的转矩百分比下的转矩值作为机器人运动时的摩擦力矩补偿值。

步骤3、针对启动时静摩擦力大的问题，采用梯形双向交变方法构建启动力矩补偿。

在得到运动后的摩擦力矩补偿值后，便可以对拖动示教时的运动力矩进行补偿，但是启动时的静摩擦力矩需要进行处理。静止状态下的关节静摩擦力矩与拖动力矩有关，且量值上相等，方向相反。假设人手施加的拖动力矩为f_hand，SCARA机器人的关节摩擦力矩为f_sta，关节最大静摩擦力矩为f_{sta_max}，那么它们之间的关系为：若关节处于静止状态，则f_hand＜f_{sta_max}且f_hand＝-f_sta；若关节即将开始运动，则f_hand＝f_{sta_max}。如果在启动之前不施加任何力矩，那么当人手拖动时需要施加大于等于最大静摩擦力矩的拖动力矩才可以使得SCARA机器人的关节开始运动，此时需要施加的正反方向的启动拖动力矩为f_{sta_max}和-f_{sta_max}。

如果电机施加单方向的辅助启动力矩，且其大小为f_{sta_max}，那么此时需要施加的相同方向的启动拖动力矩为0，不同方向的启动拖动力矩则在量值上变为了原来的两倍，且根据辅助启动力矩的方向不同，拖动力矩的方向也不同，正向为2f_{sta_max}，反向为-2f_{sta_max}。

针对这种情况，本实施例提出一种梯形双向交变的辅助启动力矩，即随着时间的变化，通过电机在关节上施加以不同方向的辅助启动力矩，且辅助启动力矩的变化方式为周期性的梯形变化，简称为梯形辅助启动力矩。

步骤301、如图4所示，设梯形辅助启动力矩的变化周期为T，即正向和负向交变一次的时间。在一个变化周期里存在四个阶段，分别是正向匀加力矩阶段、正向峰值保持阶段，负向匀减力矩阶段和负向峰值保持阶段，其中峰值保持阶段时长为T_u，(即正向峰值保持阶段的时长为T_u，负向峰值保持阶段的时长也为T_u)正向匀加和负向匀减阶段时长为T_a(即正向匀加力矩阶段的时长为T_a，负向匀加力矩阶段的时长也为T_a)它们之间的关系如下：

T＝2(T_u+2T_a)

步骤302、在梯形双向交变的辅助启动力矩设计中，峰值保持阶段的力矩可以用一个常系数η∈(0,1)来代表其与最大静摩擦力矩的关系。一般来说，η的值要设置的与1接近，否则还是会出现需要加大的启动拖动力矩的情况。设正向匀加力矩阶段的斜率为k，则负向匀减力矩阶段的斜率为-k。则可以计算得出斜率k和峰值保持阶段的力矩f_peak如下：

f_peak＝η·f_{sta_max}

则得到正向峰值保持阶段的辅助启动力矩为f_peak，负向峰值保持阶段的辅助启动力矩为-f_peak。

步骤303、构建一个周期内对启动力矩进行补偿的梯形辅助启动力矩。

设关节电机输出的梯形辅助启动力矩为τ_start，那么在SCARA机器人进入拖动示教之后且处于静止期间需要提供的梯形辅助启动力矩表达式为(以第一个周期为例)：

上述梯形辅助启动力矩为τ_start的表达公式中，t的时间为0～T，时间段0～T可以理解为第一个周期的时间，当需要推广至各个周期时，将0理解为需要计算梯形辅助启动力矩的这个周期的起始时刻，T理解为需要计算梯形辅助启动力矩的这个周期的结束时刻即可。

提供的梯形辅助启动力矩为τ_start可以在极短的周期时长内进行辅助启动力矩方向的切换。在实际工作时，即使人手在施加拖动力时的方向和辅助力方向相反，那么也可以在很短的时间内切换到另一个方向使得辅助力矩和人手拖动力矩方向相同，而这段时间对于人手的感知来说是几乎可以忽略不计的。在选取常系数η时，可以选择0.9以上的接近于1的数值使得辅助启动力矩尽可能的抵消静摩擦力，从而可以使人手以几乎为零的拖动力矩使SCARA机器人的关节运动起来。

步骤4、对计算得出的关节摩擦力补偿进行限幅设计以保证关节运动的安全性，因此设置关节力矩限制和关节速度限制，并结合机器人运动时的摩擦力矩补偿值以及所构建的启动力矩补偿，得到机器人拖动示教零力控制的输出力矩。

由于在进行零力控制时处于转矩控制模式，在得到关节不同运动速度下的摩擦力补偿计算值之后，需要对其进行限制来保证运动过程的安全，防止出现跑飞和失控的现象。设当前关节的正向最大限制速度为v_max，正向最大限制转矩为τ_max。

需要说明的是，本实施例中的转矩与力矩应理解为同等互换概念，针对关节通常称为力矩，而在电机领域一般把力矩称为转矩，因此本实施例中出现力矩和转矩两种描述。

如图5所示，首先对转矩进行限制，当关节未启动时，速度为0，此时采用上一步骤设计的梯形辅助启动力矩τ_start即可；当关节速度不为0时，若关节转矩超过最大转矩，应将转矩限制在最大转矩，若当前转矩小于最小转矩，则应将转矩限制为最小转矩，如下式所示。

式中，τ_{out_tor}表示计算出的转矩限制下当前关节电机需要输出的转矩，τ_current为当前关节的实时转矩，-τ_max为当前关节的负向最大限制转矩。

其次对关节速度进行限制。当关节速度非常接近最大限制速度时，应停止提供力矩，防止关节超出限制速度，如果此时关节速度仍在增加直至超越最大限制速度，则应提供反向的力矩来进行减速，为了快速减速，可以将减速力矩大小设置为最大限制转矩，直至关节速度低于最大限制速度，如下式所示。

式中，τ_{out_vel}表示计算出的速度限制下当前关节电机需要输出的转矩；v_current为当前关节的实时速度；k为临近最大限制速度系数，其值大小限制为0≤k≤1，在实际使用中可以根据需求设置，-v_max为当前关节的负向最大限制速度。

因此，结合关节力矩限制和关节速度限制，可以得到机器人拖动示教零力控制的输出力矩如下：

当机器人进入拖动示教且速度为0时，机器人拖动示教零力控制的输出力矩为所述梯形辅助启动力矩。

当机器人进入拖动示教且速度不为0时，首先进行关节力矩限制判断，若满足关节力矩限制，则根据关节力矩限制得到机器人拖动示教零力控制的输出力矩，否则再进行关节速度限制判断，若满足关节速度限制判断，则根据关节速度限制得到机器人拖动示教零力控制的输出力矩，否则读取机器人关节步骤202得到的摩擦力矩补偿值作为机器人拖动示教零力控制的输出力矩。

本发明结合SCARA机器人的机械结构设计了一种摩擦力自适应补偿的机器人拖动示教零力控制方法，根据拖动时关节的速度来进行自适应摩擦力补偿，并采用梯形双向交变力矩补偿的方式来解决启动时静摩擦力大的问题，在此基础上，对关节的力矩和速度进行限制，以保证运行过程中的安全。

本发明首先力矩采取库仑粘性摩擦力模型进行摩擦力辨识，这个模型获取到的摩擦力和真实摩擦力之间误差很小且计算简单。在进行参数辨识时只需通过简单的往返匀速运动便可以采集到所需的数据，并经过一维卡尔曼滤波算法和最小二乘法进行线性拟合便可以快速得出摩擦力计算模型，简单便捷。如果在更换了末端夹具之后，末端负载变化带来了摩擦力变化，也可以进行快速识别和补偿。整个识别和计算过程运算量小，响应快，在低算力使用场景也有很好的适用性。

以上所述实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。

以上所述实施例仅表达了本发明的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对发明范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。因此，本发明的保护范围应以所附权利要求为准。

Claims (8)

1.一种摩擦力自适应补偿的机器人拖动示教零力控制方法，其特征在于，所述摩擦力自适应补偿的机器人拖动示教零力控制方法，包括：

步骤1、采用库仑粘性摩擦模型对机器人的关节摩擦力进行建模；

步骤2、采集关节在不同速度等级匀速运动时的力矩反馈值进行关节摩擦力的拟合计算；

步骤201、采集关节在不同速度等级匀速运动时的力矩反馈值，并采用卡尔曼滤波算法对力矩反馈值进行滤波；

步骤202、取滤波后的力矩反馈值，对每一速度等级的力矩反馈值求平均值，并基于不同速度等级的力矩反馈值的平均值进行最小二乘法线性拟合，得到机器人运动时的摩擦力矩补偿值；

步骤3、采用梯形双向交变方法构建启动力矩补偿；

步骤301、构建梯形双向交变方法中梯形辅助启动力矩在一个周期内包括正向匀加力矩阶段、正向峰值保持阶段、负向匀减力矩阶段和负向峰值保持阶段；

步骤302、计算正向匀加力矩阶段和负向匀减力矩阶段的斜率，以及正向峰值保持阶段和负向峰值保持阶段的辅助启动力矩；

步骤303、构建一个周期内对启动力矩进行补偿的梯形辅助启动力矩；

步骤4、设置关节力矩限制和关节速度限制，并结合机器人运动时的摩擦力矩补偿值以及所构建的启动力矩补偿，得到机器人拖动示教零力控制的输出力矩。

2.如权利要求1所述的摩擦力自适应补偿的机器人拖动示教零力控制方法，其特征在于，所述采用库仑粘性摩擦模型对机器人的关节摩擦力进行建模，包括：

式中，τ_f为待求解的关节摩擦力，f_cou为库仑摩擦力系数，f_vis为粘性摩擦力系数，

为当前关节的运动速度，符号函数/>

为关节运动速度的方向，其定义如下：

式中，σ为判定速度是否为0的界定阈值。

3.如权利要求1所述的摩擦力自适应补偿的机器人拖动示教零力控制方法，其特征在于，所述采用卡尔曼滤波算法对力矩反馈值进行滤波，包括：

建立力矩反馈值的离散状态方程和观测方程如下：

x_k＝x_k-1+u_k-1+w_k-1

z_k＝x_k+v_k

式中，x_k是在第k时刻控制***的状态，x_k-1是在第k-1时刻控制***的状态，u_k-1是在第k-1时刻对控制***的控制量，w_k-1是在测量过程中第k-1时刻的过程噪声，z_k是运动控制过程中在第k时刻进行测量的实际测量值，v_k是在测量过程中第k-1时刻的测量噪声；

建立卡尔曼滤波器的更新方程如下：

建立卡尔曼滤波器状态的更新方程为：

式中，

表示在第k时刻的先验状态估计值，/>

和/>

分别表示在第k-1时刻和k时刻的后验状态估计值，P_k-1和P_k分别表示在第k-1和k时刻的后验估计协方差，/>

表示在第k时刻的先验估计协方差，K_k代表在卡尔曼滤波器的更新方程中的卡尔曼增益，Q为过程噪声的方差，R为测量噪声的方差。

4.如权利要求1所述的摩擦力自适应补偿的机器人拖动示教零力控制方法，其特征在于，所述基于不同速度等级的平均值进行最小二乘法线性拟合，得到机器人运动时的摩擦力矩补偿值，包括：

计算不同速度等级的力矩反馈值的平均值与关节电机的最大力矩的比值百分比作为摩擦力矩百分比，基于最小二乘法线性拟合得到速度等级与摩擦力矩百分比的拟合关系；

基于速度等级与摩擦力矩百分比的拟合关系，将摩擦力矩百分比作为关节电机的转矩百分比，取速度等级值相同、方向相反的反向速度等级，取转矩百分比值相同、方向相反的反向转矩百分比，得到双侧速度等级与双侧转矩百分比的拟合关系，并将双侧速度等级与双侧转矩百分比的拟合关系中速度等级对应的转矩百分比下的转矩值作为机器人运动时的摩擦力矩补偿值。

5.如权利要求1所述的摩擦力自适应补偿的机器人拖动示教零力控制方法，其特征在于，所述计算正向匀加力矩阶段和负向匀减力矩阶段的斜率，以及正向峰值保持阶段和负向峰值保持阶段的辅助启动力矩，包括：

f_peak＝η·f_{sta_max}

-f_peak＝η·(-f_{sta_max})

式中，f_peak为正向峰值保持阶段的辅助启动力矩，η为常系数，f_{sta_max}为正向关节最大静摩擦力矩，-f_peak为负向峰值保持阶段的辅助启动力矩，-f_{sta_max}为负向关节最大静摩擦力矩，k为正向匀加力矩阶段的斜率，-k为负向匀加力矩阶段的斜率，T_a为正向匀加力矩阶段或负向匀减力矩阶段的时间。

6.如权利要求1所述的摩擦力自适应补偿的机器人拖动示教零力控制方法，其特征在于，所述输出一个周期内对启动力矩进行补偿的梯形辅助启动力矩，包括：

式中，τ_start为梯形辅助启动力矩，k为正向匀加力矩阶段的斜率，t为一个周期内的当前时间，T_a为正向匀加力矩阶段或负向匀减力矩阶段的时间，f_peak为正向峰值保持阶段的辅助启动力矩，T为一个周期的总时间，T_u为正向峰值保持阶段或负向峰值保持阶段的时间，-k为负向匀加力矩阶段的斜率，-f_peak为负向峰值保持阶段的辅助启动力矩。

7.如权利要求1所述的摩擦力自适应补偿的机器人拖动示教零力控制方法，其特征在于，所述设置关节力矩限制和关节速度限制，并结合机器人运动时的摩擦力矩补偿值以及所构建的启动力矩补偿，得到机器人拖动示教零力控制的输出力矩，包括：

当机器人进入拖动示教且速度为0时，机器人拖动示教零力控制的输出力矩为所述梯形辅助启动力矩；

当机器人进入拖动示教且速度不为0时，首先进行关节力矩限制判断，若满足关节力矩限制，则根据关节力矩限制得到机器人拖动示教零力控制的输出力矩，否则再进行关节速度限制判断，若满足关节速度限制判断，则根据关节速度限制得到机器人拖动示教零力控制的输出力矩，否则读取步骤202得到的摩擦力矩补偿值作为机器人拖动示教零力控制的输出力矩。

8.如权利要求1或7所述的摩擦力自适应补偿的机器人拖动示教零力控制方法，其特征在于，所述关节力矩限制和关节速度限制设置如下：

设置关节力矩限制如下：

式中，τ_{out_tor}表示关节力矩限制下得到的机器人拖动示教零力控制的输出力矩，τ_current为当前关节的实时力矩，τ_max为当前关节的正向最大限制力矩，-τ_max为当前关节的负向最大限制力矩；

设置关节速度限制如下：

式中，τ_{out_vel}表示关节速度限制下得到的机器人拖动示教零力控制的输出力矩，v_current为当前关节的实时速度，k为临近最大限制速度系数，且0≤k≤1，-v_max为当前关节的负向最大限制速度，v_max为当前关节的正向最大限制速度。

Images