CN109746913B - 一种机器人姿势保持拖动示教的方法及系统 - Google Patents

Abstract

本发明属于工业机器人领域，具体涉及一种机器人姿态保持拖动示教的方法及系统，方法包括以下步骤：步骤1：获得t时刻的机器人所有关节的角度值向量、位姿向量以及速度向量；步骤2：建立机器人关节空间控制模型；步骤3：计算机器人在t时刻下机器人所有关节的期望驱动力矩值：步骤4：将t时刻下所有关节的期望驱动力矩向量中的每个关节的期望驱动力矩值与该关节的安全阈值进行对比，实现姿态保持的拖动示教。本发明能在姿态保持固定的状态下拖动机器人末端执行器在任务空间中运动，扩展了拖动示教的应用场景。

Description

一种机器人姿势保持拖动示教的方法及系统

技术领域

本发明属于工业机器人领域，具体涉及一种机器人姿态保持拖动示教的方 法及系统。

背景技术

工业机器人在投入生产之前通常需要编程者采用各种示教方法“告知” 机器人所要进行的动作信息和作业信息，现有的示教方法包括示教盒示教和拖 动示教。传统的示教盒示教要求操作者具有一定的机器人技术知识和经验，示 教效率较低。与示教盒示教相比，拖动示教可以无需操作者具备机器人编程经 验，操作简单且快速，极大地提高了示教的友好度和效率。

现有的拖动示教方法是通过建立机器人逆动力学模型，求解各关节在任意 角度时需要补偿的重力矩和摩擦力矩来实现的。然而在机器人的实际使用过程 中，往往要求在拖动示教过程中保持末端执行器姿态的固定，例如医药行业常 出现的移液操作。现有的拖动示教方法无法满足此类使用要求，造成拖动示教 方法实际应用中的不便。

发明内容

为了解决现有技术中存在的拖动示教过程中末端执行器姿态无法固定导 致实际应用中不便的问题，本发明提供了一种机器人姿态保持拖动示教方法及 系统，采用如下的技术方案：

一种机器人姿态保持拖动示教的方法，包括以下步骤：

步骤1：获得t时刻的机器人所有关节的角度值向量q、位姿向量X以及速 度向量

t≥0；

步骤2：设置拖动硬度参数、拖动阻尼参数、所有关节的安全阈值和运动 模型参数，建立机器人关节空间控制模型；

步骤3：将步骤1测量的t时刻所有关节的角度值向量q、位姿向量X以及 速度向量

代入步骤2建立的机器人关节空间控制模型，采用式Ⅰ计算机器人 在t时刻下机器人所有关节的期望驱动力矩值：

其中，Γ表示t时刻机器人所有关节的期望驱动力矩向量，J(q)表示t时刻的 机器人速度雅各比矩阵，B表示拖动阻尼参数，K表示拖动硬度参数，X^d表示 t时刻下期望位姿向量，

表示t时刻下期望速度向量，G表示t时刻下所有关 节的重力补偿向量；

步骤4：将步骤3得到的机器人在t时刻下所有关节的期望驱动力矩向量 中的每个关节的期望驱动力矩值与该关节的安全阈值进行对比，若均未超出安 全阈值则将所有关节的期望驱动力矩值发送给关节电机驱动系统，实现姿态保 持的拖动示教，若存在超出安全阈值的期望驱动力矩值，则向关节电机驱动系 统发送停止运动指令。

一种机器人姿态保持拖动示教系统，该示教系统包括：参数设置模块、状 态观测模块、模型构建模块、力矩计算模块和安全保护模块；

所述参数设置模块，用于设置拖动硬度参数、拖动阻尼参数、所有关节的 安全阈值和机器人关节空间控制模型需要的运动模型参数；

所述状态观测模块，用于监测所述机器人在某一时刻的所有关节的角度测 量值；

所述模型构建模块，用于建立机器人关节空间控制模型；

所述力矩计算模块，用于计算获得某一时刻下机器人所有关节的期望驱动 力矩值；

所述安全保护模块，用于根据某一时刻下机器人所有关节的期望驱动力矩 值和每个关节的安全阈值，判断所述机器人是否处于安全状态，并根据所述状 态输出机器人停止运动指令。

进一步的，所述参数设置模块中的运动模型参数为DH参数。

进一步的，所述状态观测模块通过关节电机尾部的绝对式编码器获得所有 关节的角度测量值。

进一步的，所述力矩计算模块是一个结合PD控制律的机器人关节空间控 制模型。

本发明还具有以下有益效果：

(1)本发明能在姿态保持固定的状态下拖动机器人末端执行器在任务空 间中运动，扩展了拖动示教的应用场景，通过将机器人任务空间控制模型转换 到关节空间；将PD控制律应用于机器人关节空间控制模型；调节控制模型中 的参数矩阵，实现机器人姿态保持的拖动示教功能。

(2)本发明解决了机器人在拖动示教的过程中，无法保持末端执行器姿 态固定的问题，通过设置参数可以方便地修改可拖动的方向，提升了用户的使 用体验。

附图说明

图1为根据本发明的一个实施例的机器人姿态保持拖动示教系统的结构 示意图；

图2为根据本发明的一个实施例的关节空间控制模型框架图；

图3为根据本发明的一个实施例的机器人末端执行器所受外力随时间的 变化关系示意图；

图4为根据本发明的一个实施例的机器人末端执行器的位置变化曲线；

图5为根据本发明的一个实施例的机器人末端执行器的姿态变化曲线。

具体实施方式

一种机器人姿态保持拖动示教的方法，包括以下步骤：

步骤1：获得t时刻的机器人所有关节的角度值向量q、位姿向量X以及速 度向量

t≥0；所述位姿表示机器人末端执行器的位置和姿态；

步骤2：设置拖动硬度参数、拖动阻尼参数、所有关节的安全阈值和运动 模型参数，建立机器人关节空间控制模型；

步骤3：将步骤1测量的t时刻所有关节的角度值向量q、位姿向量X以及 速度向量

代入步骤2建立的机器人关节空间控制模型，采用式Ⅰ计算机器人 在t时刻下机器人所有关节的期望驱动力矩值：

其中，Γ表示t时刻机器人所有关节的期望驱动力矩向量，J(q)表示t时刻的 机器人速度雅各比矩阵，B表示拖动阻尼参数，K表示拖动硬度参数，X^d表示 t时刻下期望位姿向量，

表示t时刻下期望速度向量，G表示t时刻下所有关 节的重力补偿向量；

步骤4：将步骤3得到的机器人在t时刻下所有关节的期望驱动力矩向量 中的每个关节的期望驱动力矩值与该关节的安全阈值进行对比，若均未超出安 全阈值则将所有关节的期望驱动力矩值发送给关节电机驱动系统，实现姿态保 持的拖动示教，若存在超出安全阈值的期望驱动力矩值，则向关节电机驱动系 统发送停止运动指令。

本发明能在姿态保持固定的状态下拖动机器人末端执行器在任务空间中 运动，扩展了拖动示教的应用场景，通过将机器人任务空间控制模型转换到关 节空间；将PD控制律应用于机器人关节空间控制模型；调节控制模型中的参 数矩阵，实现机器人姿态保持的拖动示教功能。

具体的，在计算式Ⅰ中关节重力补偿向量时，通过局部坐标系的空间变换 矩阵和机器人的运动模型参数计算获得所述机器人运动学模型，通过牛顿-欧 拉方法搭建逆动力学模型的框架，对利用机器人运行特性的激励轨迹以及运动 数据进行滤波，根据滤波后的结果辨识得到动力学参数，完成完整逆动力学模 型的搭建，通过逆动力学模型得到关节重力补偿向量。

优选的，所述机器人刚体逆动力学模型为：

其中，τ为关节驱动力矩、M为机器人惯性矩阵、C为科里奥力矩阵、G 为关节重力补偿向量，即机器人所受重力在各关节的映射、τf为摩擦力矩、τext 为作用在机器人上的外力在各关节的映射、q为所有关节的角度值向量。

具体的，步骤3的推导过程为

(1)：首先通过式Ⅲ在频域建立机器人任务空间控制模型，用于描述任务 空间中作用在机器人上的力与末端执行器位姿的映射关系：

F(s)＝sZ(s)X(s) (式Ⅲ)

其中，F为任务空间中作用在机器人上的力，Z为机器人系统的等效阻抗， X为末端执行器在任务空间中位姿，s是控制工程中传递函数的组成部分；

优选的，将机器人刚体等效为二阶质量-弹簧-阻尼器系统得到阻抗Z的传 递函数，所述阻抗Z的传递函数可以用式Ⅳ表示为：

sZ(s)＝As²+Bs+K (式Ⅳ)

其中，A、B、K分别为惯性、阻尼、刚度参数矩阵。

(2)：如图3所示，通过式Ⅴ将机器人任务空间控制模型转换到关节空间：

J^T(q)F(s)＝sZ(s)f(q)(s) (式Ⅴ)

其中，F为任务空间中作用在机器人上的力，J(q)为运动学模型中机器人 速度雅各比矩阵，q为机器人所有关节的角度值向量，f(q)为机器人正运动学 变换矩阵。

(3)：忽略机器人系统的高阶动态项，根据PD控制思想，将式Ⅴ中 sZ(s)f(q)(s)改写成

的形式，为了提高系统的响 应性能，根据步骤1得到的逆动力学模型增加重力补偿向量，采用式Ⅰ计算机 器人在t时刻下机器人关节端的期望驱动力矩值：

其中，Γ表示t时刻机器人所有关节的期望驱动力矩向量，J(q)表示t时刻的 机器人速度雅各比矩阵，B表示拖动阻尼参数，K表示拖动硬度参数，X^d表示 t时刻下期望位姿向量，

表示t时刻下期望速度向量，G表示t时刻下所有关 节的重力补偿向量；

所述的X^d和

这两组参数的实质是有关柔顺中心的一组系数，用于确定 拖动示教过程中末端执行器的固定姿态，通过设置参数可以方便地修改可拖动 的方向，提升了用户的使用体验。

本发明还涉及一种机器人姿态保持拖动示教系统，如图1所示，该示教系 统包括：参数设置模块、状态观测模块、模型构建模块、力矩计算模块、安全 保护模块；

所述参数设置模块，用于设置拖动硬度参数、拖动阻尼参数、所有关节的 安全阈值和机器人关节空间控制模型需要的运动模型参数；所述拖动硬度参数 用于描述所述机器人末端执行器的可拖动方向，所述拖动阻尼参数用于描述所 述机器人末端执行器的拖动阻力大小，所述安全阈值用于对比各个关节角度测 量值，所述运动模型参数用于所述机器人运动学模型和逆动力学模型的计算；

所述状态观测模块，用于监测所述机器人在某一时刻的所有关节的角度测 量值；

所述模型构建模块，用于建立机器人关节空间控制模型；

所述力矩计算模块，用于计算获得某一时刻下机器人所有关节的期望驱动 力矩值；

所述安全保护模块，用于根据某一时刻下机器人所有关节的期望驱动力矩 值和每个关节的安全阈值，判断所述机器人是否处于安全状态，并根据所述状 态输出机器人停止运动指令。

具体的，在构建机器人关节空间控制模型时，需要构建运动学模型和逆动 力学模型，根据所述运动模型参数，建立运动学模型和逆动力学模型并进行系 统动力学参数辨识；所述运动学模型用于关节角度与任务空间位置和速度的映 射变换，所述逆动力学模型用于所述机器人在任意时刻关节重力补偿向量的计 算；

所述力矩计算模块，用于根据所述所有关节角度值、所述机器人关节重力 补偿向量、所述拖动硬度参数和拖动阻尼参数，计算获得所述机器人所有关节 的期望驱动力矩值；

所述安全保护模块，用于根据所述关节端的期望驱动力矩值和关节端安全 阈值，判断所述机器人是否处于安全状态，并根据所述状态输出机器人停止运 动指令。

具体的，所述机器人为四轴工业机器人、六轴工业机器人或协作机器人中 的一种。

具体的，所述参数设置模块中的运动模型参数为DH参数。

具体的，所述状态观测模块通过关节电机尾部的绝对式编码器获得关节端 角度测量值。

具体的，所述力矩计算模块是一个结合PD控制律的机器人关节空间控制 模型。

以下给出本发明的具体实施方式，需要说明的是本发明并不局限于以下具 体实施例，凡在本申请技术方案基础上做的等同变换均落入本发明的保护范围。

实施例

根据本发明所述的一种机器人姿态保持拖动示教方法，本实施例以六自由 度机械臂为研究对象展开验证。

步骤1：测量六个关节的角度值向量q，六个关节的位姿向量X以及速度向 量

步骤2：设置拖动硬度参数、拖动阻尼参数、所有关节的安全阈值和运动 模型参数，建立机器人关节空间控制模型；

其中运动模型参数采用如下表格所述的DH参数：

拖动硬度参数设置为：[0,0,0,100,100,100]；

拖动阻尼参数设置为：[0,0,0,50,50,50]；

安全阈值设置为：[0.5,1.5,0.8,0.15,0.12,0.1]，单位为：Nm；

在上述机器人的控制系统中建立如下所示的六个关节的期望驱动力矩计 算方程：

选定如下参数表示t时刻下六个关节的期望位姿向量X^d和期望速度向量

X^d：[0.46,0.117,0.1015,-π/2,0,0]^T；

其中重力补偿向量G的形式：G＝[G1,G2,G3,G4,G5,G6]^T,且G由搭建的逆 动力学模型获得，逆动力学模型搭建时，通过自动参数识别获得动力学参数从 而完善逆动力学模型。

实验过程中，在上述机器人的末端执行器上作用一个外力，如图3所示， 为所述外力在X、Y、Z方向上的投影。

如图4所示，为末端执行器在受到上述外力的作用下，末端执行器的空间 位置随时间的变化曲线。由图3可知，末端执行器在受到外力的作用下改变了 空间位置。

如图5所示，为末端执行器在受到上述外力的作用下，末端执行器的空间 姿态随时间的变化曲线。由图4可知，末端执行器在外力刚开始作用时其姿态 发生了较大的波动，约1s后其姿态基本恢复到初始时的状态。

综合图4、图5可以看出，机器人末端执行器在受到外力作用的情况下能 够进行正常的拖动示教，且在此过程中能够使末端执行器的姿态保持不变，证 明了本专利所提出方法的有效性。

Claims (5)

1.一种机器人姿态保持拖动示教的方法，其特征在于，包括以下步骤：

步骤1：获得t时刻的机器人所有关节的角度值向量q、位姿向量X以及速度向量

t≥0；

步骤2：设置拖动硬度参数、拖动阻尼参数、所有关节的安全阈值和运动模型参数，建立机器人关节空间控制模型；

步骤3：将步骤1测量的t时刻所有关节的角度值向量q、位姿向量X以及速度向量

代入步骤2建立的机器人关节空间控制模型，采用式Ⅰ计算机器人在t时刻下机器人所有关节的期望驱动力矩值：

其中，Γ表示t时刻机器人所有关节的期望驱动力矩向量，J(q)表示t时刻的机器人速度雅各比矩阵，B表示拖动阻尼参数，K表示拖动硬度参数，X^d表示t时刻下期望位姿向量，

表示t时刻下期望速度向量，G表示t时刻下所有关节的重力补偿向量；

步骤4：将步骤3得到的机器人在t时刻下所有关节的期望驱动力矩向量中的每个关节的期望驱动力矩值与该关节的安全阈值进行对比，若均未超出安全阈值则将所有关节的期望驱动力矩值发送给关节电机驱动系统，实现姿态保持的拖动示教，若存在超出安全阈值的期望驱动力矩值，则向关节电机驱动系统发送停止运动指令。

2.一种机器人姿态保持拖动示教系统，其特征在于，该示教系统用于实施权利要求1中机器人姿态保持拖动示教的方法，该示教系统包括：参数设置模块、状态观测模块、模型构建模块、力矩计算模块和安全保护模块；

所述参数设置模块，用于设置拖动硬度参数、拖动阻尼参数、所有关节的安全阈值和机器人关节空间控制模型需要的运动模型参数；

所述状态观测模块，用于监测所述机器人在某一时刻的所有关节的角度测量值；

所述模型构建模块，用于建立机器人关节空间控制模型；

所述力矩计算模块，用于计算获得某一时刻下机器人所有关节的期望驱动力矩值；

所述安全保护模块，用于根据某一时刻下机器人所有关节的期望驱动力矩值和每个关节的安全阈值，判断所述机器人是否处于安全状态，并根据所述状态输出机器人停止运动指令。

3.如权利要求2所述的机器人姿态保持拖动示教系统，其特征在于，所述参数设置模块中的运动模型参数为DH参数。

4.如权利要求2所述的机器人姿态保持拖动示教系统，其特征在于，所述状态观测模块通过关节电机尾部的绝对式编码器获得所有关节的角度测量值。

5.如权利要求2所述的机器人姿态保持拖动示教系统，其特征在于，所述力矩计算模块是一个结合PD控制律的机器人关节空间控制模型。

Images