本发明属于工业机器人领域，具体涉及一种机器人姿态保持拖动示教的方法及系统。
背景技术：
工业机器人在投入生产之前通常需要编程者采用各种示教方法“告知”机器人所要进行的动作信息和作业信息，现有的示教方法包括示教盒示教和拖动示教。传统的示教盒示教要求操作者具有一定的机器人技术知识和经验，示教效率较低。与示教盒示教相比，拖动示教可以无需操作者具备机器人编程经验，操作简单且快速，极大地提高了示教的友好度和效率。
现有的拖动示教方法是通过建立机器人逆动力学模型，求解各关节在任意角度时需要补偿的重力矩和摩擦力矩来实现的。然而在机器人的实际使用过程中，往往要求在拖动示教过程中保持末端执行器姿态的固定，例如医药行业常出现的移液操作。现有的拖动示教方法无法满足此类使用要求，造成拖动示教方法实际应用中的不便。
技术实现要素：
为了解决现有技术中存在的拖动示教过程中末端执行器姿态无法固定导致实际应用中不便的问题，本发明提供了一种机器人姿态保持拖动示教方法及系统，采用如下的技术方案：
一种机器人姿态保持拖动示教的方法，包括以下步骤：
步骤1：获得t时刻的机器人所有关节的角度值向量q、位姿向量x以及速度向量
步骤2：设置拖动硬度参数、拖动阻尼参数、所有关节的安全阈值和运动模型参数，建立机器人关节空间控制模型；
步骤3：将步骤1测量的t时刻所有关节的角度值向量q、位姿向量x以及速度向量
其中，γ表示t时刻机器人所有关节的期望驱动力矩向量，j(q)表示t时刻的机器人速度雅各比矩阵，b表示拖动阻尼参数，k表示拖动硬度参数，xd表示t时刻下期望位姿向量，
步骤4：将步骤3得到的机器人在t时刻下所有关节的期望驱动力矩向量中的每个关节的期望驱动力矩值与该关节的安全阈值进行对比，若均未超出安全阈值则将所有关节的期望驱动力矩值发送给关节电机驱动系统，实现姿态保持的拖动示教，若存在超出安全阈值的期望驱动力矩值，则向关节电机驱动系统发送停止运动指令。
一种机器人姿态保持拖动示教系统，该示教系统包括：参数设置模块、状态观测模块、模型构建模块、力矩计算模块和安全保护模块；
所述参数设置模块，用于设置拖动硬度参数、拖动阻尼参数、所有关节的安全阈值和机器人关节空间控制模型需要的运动模型参数；
所述状态观测模块，用于监测所述机器人在某一时刻的所有关节的角度测量值；
所述模型构建模块，用于建立机器人关节空间控制模型；
所述力矩计算模块，用于计算获得某一时刻下机器人所有关节的期望驱动力矩值；
所述安全保护模块，用于根据某一时刻下机器人所有关节的期望驱动力矩值和每个关节的安全阈值，判断所述机器人是否处于安全状态，并根据所述状态输出机器人停止运动指令。
进一步的，所述参数设置模块中的运动模型参数为dh参数。
进一步的，所述状态观测模块通过关节电机尾部的绝对式编码器获得所有关节的角度测量值。
进一步的，所述力矩计算模块是一个结合pd控制律的机器人关节空间控制模型。
本发明还具有以下有益效果：
(1)本发明能在姿态保持固定的状态下拖动机器人末端执行器在任务空间中运动，扩展了拖动示教的应用场景，通过将机器人任务空间控制模型转换到关节空间；将pd控制律应用于机器人关节空间控制模型；调节控制模型中的参数矩阵，实现机器人姿态保持的拖动示教功能。
(2)本发明解决了机器人在拖动示教的过程中，无法保持末端执行器姿态固定的问题，通过设置参数可以方便地修改可拖动的方向，提升了用户的使用体验。
附图说明
图1为根据本发明的一个实施例的机器人姿态保持拖动示教系统的结构示意图；
图2为根据本发明的一个实施例的关节空间控制模型框架图；
图3为根据本发明的一个实施例的机器人末端执行器所受外力随时间的变化关系示意图；
图4为根据本发明的一个实施例的机器人末端执行器的位置变化曲线；
图5为根据本发明的一个实施例的机器人末端执行器的姿态变化曲线。
具体实施方式
一种机器人姿态保持拖动示教的方法，包括以下步骤：
步骤1：获得t时刻的机器人所有关节的角度值向量q、位姿向量x以及速度向量
步骤2：设置拖动硬度参数、拖动阻尼参数、所有关节的安全阈值和运动模型参数，建立机器人关节空间控制模型；
步骤3：将步骤1测量的t时刻所有关节的角度值向量q、位姿向量x以及速度向量
其中，γ表示t时刻机器人所有关节的期望驱动力矩向量，j(q)表示t时刻的机器人速度雅各比矩阵，b表示拖动阻尼参数，k表示拖动硬度参数，xd表示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)＝as2+bs+k(式ⅳ)
其中，a、b、k分别为惯性、阻尼、刚度参数矩阵。
(2)：如图3所示，通过式ⅴ将机器人任务空间控制模型转换到关节空间：
jt(q)f(s)＝sz(s)f(q)(s)(式ⅴ)
其中，f为任务空间中作用在机器人上的力，j(q)为运动学模型中机器人速度雅各比矩阵，q为机器人所有关节的角度值向量，f(q)为机器人正运动学变换矩阵。
(3)：忽略机器人系统的高阶动态项，根据pd控制思想，将式ⅴ中sz(s)f(q)(s)改写成
其中，γ表示t时刻机器人所有关节的期望驱动力矩向量，j(q)表示t时刻的机器人速度雅各比矩阵，b表示拖动阻尼参数，k表示拖动硬度参数，xd表示t时刻下期望位姿向量，
所述的xd和
本发明还涉及一种机器人姿态保持拖动示教系统，如图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时刻下六个关节的期望位姿向量xd和期望速度向量
xd：[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可以看出，机器人末端执行器在受到外力作用的情况下能够进行正常的拖动示教，且在此过程中能够使末端执行器的姿态保持不变，证明了本专利所提出方法的有效性。