CN110666794A - 多自由度协作机器人拖动的末端负载力补偿方法 - Google Patents

Abstract

本发明公开了多自由度协作机器人拖动的末端负载力补偿方法，包括步骤：(1)建立机器人末端法兰受力和力矩的数学模型；(2)建立机器人的关节坐标系、基坐标系、末端法兰坐标系并计算它们之间转换的转换矩阵；(3)测量负载的质心参数；(4)建立一新坐标系并计算工具负载在新坐标系的原点产生的静力和静力矩；(5)计算工具负载在新坐标系的原点产生的动力和动力矩；(6)计算机器人末端坐标系相对于基坐标系的雅克比矩阵；(7)将工具负载造成的力和力矩补偿至机器人的每个关节。通过补偿负载产生的力和力矩，使得带负载的机器人拖动更加轻便，而且本方法不需要添加额外的力或力矩传感器。

Description

多自由度协作机器人拖动的末端负载力补偿方法

技术领域

本发明涉及协作机器人，尤其涉及带工具和负载的协作机器人拖动示教方法。

背景技术

随着工业机器人在打磨、抛光、喷涂、涂胶、焊接等领域的应用，机器人协作控制技术向灵活、柔顺、安全和高效的方向发展。为了实现人机共融协同工作，对机器人示教技术也提出了新的要求。相对于传统的示教器示教技术，直接拖动示教技术具有柔顺、灵活等优点，且对操作者要求低，因此成为了示教技术发展的重点方向。目前的引导示教主要有三种，一是基于机器人末端安装多维力/力矩传感器的引导示教，成本较高，且只能拖动机器人末端；二是基于关节传感器(力矩传感器或双编码器)的引导示教，虽能拖动关节，但因关节传感器的存在，使关节结构复杂，成本提高；三是基于关节力矩补偿零力控制的无传感器引导示教，成本最低，而且该种方式既可以点位拖动，也可以轨迹拖动，适应性强，可以应用到多种场合。

拖动示教功能已经成为协作机器人的标配，然而市面上的大多数协作机器人拖动起来比较费力且只能实现点位拖动。针对无外力矩传感器的协作机器人，虽可以通过动力学力矩补偿实现直接拖动示教，但其由于动力学模型的不全面和不准确，致使零力控制的效果较差，进而影响了拖动的灵活性，只能实现点到点的拖动，很难实现轨迹拖动。因此，有必要研究基于动力学模型精准辨识的、无外部力矩传感传的机器人零力控制技术，以提高协作机器人直接拖动示教的灵活性和柔顺性。当机器人的末端安装工具，工具夹持负载之后，为了确保机器人的关节输出力矩仍然可以平衡机器人本体、工具和负载的重力，需要一种力补偿方法来平衡工具和负载产生的力/力矩。

当协作机器人末端安装了工具负载之后，为估计外部力，需要已知负载的重量和质心，以计算不同姿态下的负载产生的重力和重力矩。然而，此重力和重力矩均为静态力和力矩。为了获得更好的拖动效果，有必要考虑工具负载产生的动力。

发明内容

本发明的目的在于克服已有技术的缺点，提供一种多自由度协作机器人拖动的末端负载力补偿方法，采用本方法使得机器人末端法兰挂载任意工具和负载后仍可以保持平衡，确保操作者更加轻松地拖动机器人。

本发明的技术方案是：

一种机器人末端工具负载力/力矩补偿的方法，包括以下步骤：

步骤S1,建立机器人末端法兰受力和力矩的数学模型，模型如下：

式中，F_end和T_end为机器人末端法兰受到的外力，包括工具负载产生的静力F_s和静力矩T_s，动力F_d和动力矩T_d；

步骤S2，将n自由度机器人等效为n自由度刚性连杆，建立机器人的基坐标系{o₀-x₀y₀z₀}，机器人末端法兰坐标系{o_end-x_endy_endz_end}，计算机器人末端法兰坐标系{o_end-x_endy_endz_end}到机器人的基坐标系{o₀-x₀y₀z₀}的转换矩阵⁰T_end；

步骤S3，建立一个新坐标系{^e′o-^e′x^e′y^e′z}，然后计算机器人的基坐标系{o₀-x₀y₀z₀}到新坐标系{^e′o-^e′x^e′y^e′z}的转换矩阵^e′T₀；所述的新坐标系的方向与基坐标系的方向一致，所述的新坐标系的原点与机器人末端法兰坐标系的原点重合；

所述的转换矩阵^e′T₀的计算方法如下：

其中，P_x、P_y和P_z为通过正运动学计算得到的机器人末端法兰坐标系在基坐标系中的位置坐标；

步骤S4，在机器人末端法兰坐标系中测量安装在机器人末端法兰上的工具负载的质心的坐标，记为^endP(x_end,y_end,z_end)，则工具负载的质心在新坐标系{^e′o-^e′x^e′y^e′z}中的坐标^e′P(^e′x,^e′y,^e′z)为：

步骤S5，计算工具负载在新坐标系{^e′o-^e′x^e′y^e′z}中对新坐标系{^e′o-^e′x^e′y^e′z}的原点产生的静力和静力矩：

其中，^e′F_s为工具负载在新坐标系{^e′o-^e′x^e′y^e′z}中对新坐标系{^e′o-^e′x^e′y^e′z}的原点产生的静力，^e′T_s为工具负载在新坐标系{^e′o-^e′x^e′y^e′z}中对新坐标系{^e′o-^e′x^e′y^e′z}的原点产生的静力矩；

为重力加速度在新坐标系{^e′o-^e′x^e′y^e′z}中三个坐标轴方向上的分量；

步骤S6，工具负载在新坐标系{^e′o-^e′x^e′y^e′z}中对新坐标系{^e′o-^e′x^e′y^e′z}的原点产生的动力^e′F_d和动力矩^e′T_d：

其中，^e′F_d为工具负载在新坐标系{^e′o-^e′x^e′y^e′z}中的动力，^e′T_d为工具负载在新坐标系{^e′o-^e′x^e′y^e′z}中的动力矩，m_tool为负载的质量，s_end为负载相对于末端法兰坐标系的一阶质量矩，I_end为负载相对于末端坐标系的惯量；

和ω_e′分别为工具负载在新坐标系{^e′o-^e′x^e′y^e′z}中的角加速度和角速度，为工具负载在新坐标系{^e′o-^e′x^e′y^e′z}中的线性加速度；

步骤S7，将步骤S5求得的静力和静力矩和步骤S6求得的动力和动力矩代入步骤S1的公式中得到在新坐标系下{^e′o-^e′x^e′y^e′z}工具负载对对新坐标系{^e′o-^e′x^e′y^e′z}的原点形成的力和力矩，公式如下：

步骤S8，求解机器人末端坐标系相对于基坐标系的雅克比矩阵；

步骤S9，将工具负载造成的力和力矩补偿至机器人的每个关节，得到机器人各个关节的补偿力矩为：

n为机器人的自由度数。

本发明的优势在于：采用本方法协作机器人带负载拖动更加轻便，在没有施加任何外力时，带负载的协作机器人在力矩模式下仍然可以保持平衡。操作者拖动带有负载的协作机器人更加轻便，快速和慢速拖动的轻便度都较无动力补偿得到了大幅提升。该方法适用于多自由度机器人，无需添加额外力/力矩传感器，不受机器人安装方式的限制。

附图说明

图1为n自由度机器人的各坐标系转换关系示意图；

图2为机器人末端法兰处工具负载产生的静力/静力矩示意图；

图3为n自由度协作机器人拖动的末端负载力补偿流程图。

具体实施方式

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

本发明提出一种机器人末端工具负载力/力矩补偿的方法，包括以下步骤：

步骤S1,对应于流程图3，建立机器人末端法兰受力和力矩的数学模型，其模型如下：

式中，F_end和T_end为机器人末端法兰受到的外力，包括工具负载产生的静力F_s和静力矩T_s，动力F_d和动力矩T_d。

步骤S2，如图1，将n自由度机器人等效为n自由度刚性连杆，建立机器人的基坐标系{o₀-x₀y₀z₀}，机器人末端法兰坐标系{o_end-x_endy_endz_end}，计算机器人末端法兰坐标系{o_end-x_endy_endz_end}到机器人的基坐标系{o₀-x₀y₀z₀}的转换矩阵⁰T_end。

所述的转换矩阵⁰T_end的计算方法如下：

⁰T_end＝⁰T₁ ¹T₂…^i-1T_i…^n-2T_n-1 ^n-1T_end

^i-1T_i为两等效连杆之间的通用变换矩阵(具体计算方法参见：宋朋宇.一种非正交6自由度协作机器人运动学及动力学分析[D].天津大学,2017.)，n为机器人的自由度数。

本步骤中基坐标系和末端法兰坐标系的建立可以根据使用者的需求建立，尽管不同使用者建立的坐标系可能不同，转换矩阵也不同，但不影响最终末端负载力补偿的效果。

步骤S3，如图1，建立一个新坐标系{^e′o-^e′x^e′y^e′z}，然后计算机器人的基坐标系{o₀-x₀y₀z₀}到新坐标系{^e′o-^e′x^e′y^e′z}的转换矩阵^e′T₀；所述的新坐标系的方向与基坐标系的方向一致，所述的新坐标系的原点与机器人末端法兰坐标系的原点重合。

所述的转换矩阵^e′T₀的计算方法如下：

其中，P_x、P_y和P_z为通过正运动学计算得到的机器人末端法兰坐标系在基坐标系中的位置坐标。

步骤S4，在机器人末端法兰坐标系中测量(采用现有的测量方法即可，如：垂吊法等)安装在机器人末端法兰上的工具负载的质心的坐标，记为^endP(x_end,y_end,z_end)，则工具负载的质心在新坐标系{^e′o-^e′x^e′y^e′z}中的坐标^e′P(^e′x,^e′y,^e′z)为：

步骤S5，如图2，计算工具负载在新坐标系{^e′o-^e′x^e′y^e′z}中对新坐标系{^e′o-^e′x^e′y^e′z}的原点产生的静力和静力矩：

其中，^e′F_s为工具负载在新坐标系{^e′o-^e′x^e′y^e′z}中对新坐标系{^e′o-^e′x^e′y^e′z}的原点产生的静力，^e′T_s为工具负载在新坐标系{^e′o-^e′x^e′y^e′z}中对新坐标系{^e′o-^e′x^e′y^e′z}的原点产生的静力矩。

为重力加速度在新坐标系{^e′o-^e′x^e′y^e′z}中三个坐标轴方向上的分量，该项与机器人的安装方式有关。

步骤S6，对应于流程图3，工具负载在新坐标系{^e′o-^e′x^e′y^e′z}中对新坐标系{^e′o-^e′x^e′y^e′z}的原点产生的动力^e′F_d和动力矩^e′T_d：

其中，^e′F_d为工具负载在新坐标系{^e′o-^e′x^e′y^e′z}中的动力，^e′T_d为工具负载在新坐标系{^e′o-^e′x^e′y^e′z}中的动力矩，m_tool为负载的质量，s_end为负载相对于末端法兰坐标系的一阶质量矩，I_end为负载相对于末端坐标系的惯量；

和ω_e′分别为工具负载在新坐标系{^e′o-^e′x^e′y^e′z}中的角加速度和角速度，

为工具负载在新坐标系{^e′o-^e′x^e′y^e′z}中的线性加速度；

可以通过雅克比矩阵计算得到也可以通过直接测量得出；s_end和I_end通过机器人的参数辨识获得，具体计算方法见：张秋隆.基于参数辨识的协作机器人力矩补偿与检测控制研究[D].天津大学,2019.；ω_e′、

和

均可通过读取与机器人关节电机相连的驱动器参数和机器人雅克比矩阵计算求得。

步骤S7，将步骤S5求得的静力和静力矩和步骤S6求得的动力和动力矩代入步骤S1的公式中得到在新坐标系下{^e′o-^e′x^e′y^e′z}工具负载对对新坐标系{^e′o-^e′x^e′y^e′z}的原点形成的力和力矩，公式如下：

步骤S8，求解机器人末端坐标系相对于基坐标系的雅克比矩阵，具体过程如下：对应于流程图3，雅克比矩阵J(q)将各个关节速度与末端法兰的笛卡尔速度联系起来，同时J^T(q)可以将关节力矩与末端法兰的笛卡尔力和力矩联系起来，有机器人末端坐标系相对于基坐标系的雅克比矩阵(具体计算方法见：范凯杰.模块化六自由度机械臂运动学及轨迹规划研究[D].安徽理工大学,2018.)：

其中，Z_i为第{i}关节坐标系(按照DH法建立的关节坐标系)中Z轴的单位向量，ⁱP_n ⁰为根据末端法兰坐标系原点相对于关节坐标系{i}的位置在基坐标系{o₀-x₀y₀z₀}中的表示。

n为机器人的自由度数。

步骤S9，对应于流程图3，将工具负载造成的力和力矩补偿至机器人的每个关节，得到机器人各个关节的补偿力矩为：

n为机器人的自由度数。

根据本发明实施例的多自由度协作机器人拖动的末端负载力补偿方法，具有以下特点：

1.该方法适用于多种机器人，不限于机器人的自由度数，机器人的结构。

2.无需添加额外传感器。

3.机器人的安装方式多种多样。

尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在不脱离本发明的原理和宗旨的情况下在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。本发明的范围由所附权利要求极其等同限定。

Claims (1)

1.一种机器人末端工具负载力/力矩补偿的方法，其特征在于包括以下步骤：

步骤S1,建立机器人末端法兰受力和力矩的数学模型，模型如下：

式中，F_end和T_end为机器人末端法兰受到的外力，包括工具负载产生的静力F_s和静力矩T_s，动力F_d和动力矩T_d；

步骤S2，将n自由度机器人等效为n自由度刚性连杆，建立机器人的基坐标系{o₀-x₀y₀z₀}，机器人末端法兰坐标系{o_end-x_endy_endz_end}，计算机器人末端法兰坐标系{o_end-x_endy_endz_end}到机器人的基坐标系{o₀-x₀y₀z₀}的转换矩阵⁰T_end；

步骤S3，建立一个新坐标系{^e′o-^e′x^e′y^e′z}，然后计算机器人的基坐标系{o₀-x₀y₀z₀}到新坐标系{^e′o-^e′x^e′y^e′z}的转换矩阵^e′T₀；所述的新坐标系的方向与基坐标系的方向一致，所述的新坐标系的原点与机器人末端法兰坐标系的原点重合；

所述的转换矩阵^e′T₀的计算方法如下：

其中，P_x、P_y和P_z为通过正运动学计算得到的机器人末端法兰坐标系在基坐标系中的位置坐标；

步骤S4，在机器人末端法兰坐标系中测量安装在机器人末端法兰上的工具负载的质心的坐标，记为^endP(x_end,y_end,z_end)，则工具负载的质心在新坐标系{^e′o-^e′x^e′y^e′z}中的坐标^e′P(^e′x,^e′y,^e′z)为：

步骤S5，计算工具负载在新坐标系{^e′o-^e′x^e′y^e′z}中对新坐标系{^e′o-^e′x^e′y^e′z}的原点产生的静力和静力矩：

其中，^e′F_s为工具负载在新坐标系{^e′o-^e′x^e′y^e′z}中对新坐标系{^e′o-^e′x^e′y^e′z}的原点产生的静力，^e′T_s为工具负载在新坐标系{^e′o-^e′x^e′y^e′z}中对新坐标系{^e′o-^e′x^e′y^e′z}的原点产生的静力矩；

为重力加速度在新坐标系{^e′o-^e′x^e′y^e′z}中三个坐标轴方向上的分量；

步骤S6，工具负载在新坐标系{^e′o-^e′x^e′y^e′z}中对新坐标系{^e′o-^e′x^e′y^e′z}的原点产生的动力^e′F_d和动力矩^e′T_d：

其中，^e′F_d为工具负载在新坐标系{^e′o-^e′x^e′y^e′z}中的动力，^e′T_d为工具负载在新坐标系{^e′o-^e′x^e′y^e′z}中的动力矩，m_tool为负载的质量，s_end为负载相对于末端法兰坐标系的一阶质量矩，I_end为负载相对于末端坐标系的惯量；

和ω_e′分别为工具负载在新坐标系{^e′o-^e′x^e′y^e′z}中的角加速度和角速度，

为工具负载在新坐标系{^e′o-^e′x^e′y^e′z}中的线性加速度；

步骤S7，将步骤S5求得的静力和静力矩和步骤S6求得的动力和动力矩代入步骤S1的公式中得到在新坐标系下{^e′o-^e′x^e′y^e′z}工具负载对对新坐标系{^e′o-^e′x^e′y^e′z}的原点形成的力和力矩，公式如下：

步骤S8，求解机器人末端坐标系相对于基坐标系的雅克比矩阵；

步骤S9，将工具负载造成的力和力矩补偿至机器人的每个关节，得到机器人各个关节的补偿力矩为：

n为机器人的自由度数。

Images