一种基于示教学习的协作搬运控制系统及方法与流程

本发明涉及智能控制领域，具体涉及一种基于示教学习的协作搬运控制系统及方法。

背景技术：

随着制造加工自动化的程度越来越高，加工和装配的零件越来越多样化，随之而来的是各种零件的搬运问题。目前工厂中搬运方式多使用龙门吊，使用范围有限、定位精度低、迟滞和惯性较大，影响搬运的效率和准确性，而目前逐渐发展的人机协作搬运控制复杂，搬运的柔性和位置精度得不到很好保证。

中国发明专利cn102229147a,机器手控制装置及其控制方法、机器人，其特征在于包括作业姿势信息获取单元,获取作业姿势信息；危险程度算出单元,算出人在搬运物体时的危险程度；刚性控制单元,根据危险程度控制机器手的刚性。该发明专利不会由于搬运物体而对人施加负担，能够实现安全的机器人控制，但是这种方式只能实现简单的搬运操作，无法根据场景变换快速适应。

综上所述，目前工厂中的搬运方式效率低，使用范围有限，而现有的协作搬运控制方法适应场景能力有限。协作搬运因为和人交互，一定的柔顺性可以减小交互内力，但同时柔顺性较大会造成目标位置精度得不到保证，所以提高协作搬运的效率需要很好平衡搬运的柔顺性和目标位置精度。

技术实现要素：

鉴于上述现有技术的不足，本发明的目的在于提供一种基于示教学习的协作搬运控制系统，该系统由示教学习建立的轨迹概率模型修正后作为参考轨迹，叠加变刚度系数的导纳控制，实现协作搬运的柔性搬运，并且保证搬运的位置精度，可以很好满足协作搬运作业需求。

为了解决上述问题，本发明采用的技术方案是：

一种基于示教学习的协作搬运系统，其特征在于：包括

机械臂：具有运动控制功能的多轴机械臂；

六维力/力矩传感器：用来进行交互力的测量，安装在机械臂末端

动作捕捉系统定位点：贴在被搬运物体和人手上，用于获取目标的位姿

pc上位机：与机械臂、六维力/力矩传感器和动作捕捉系统可进行实时通信；运行过程中，机械臂在pc上位机的指令下携带抓取手进行协作搬运操作，所述pc上位机搭建的控制系统基于ros平台。

在上述的一种基于示教学习的协作搬运系统，所述六维力/力矩传感器通过交互力反馈实现协作搬运的柔性交互。

一种基于示教学习的协作搬运方法，该搬运方法采用的设备包括机械臂、六维力/力矩传感器、动捕定位点和pc上位机，其特征在于，包括以下步骤：

步骤1、修正示教学习所得轨迹概率模型，将轨迹概率模型起点和终点的协方差矩阵进行缩小；

步骤2、以修正后的轨迹概率模型为参考轨迹，叠加变刚度系数导纳控制，刚度系数根据轨迹点对应的高斯分布设定；

步骤3、计算示教数据中交互力模长的均值和协方差得到交互力满足的高斯分布，选定交互力阈值在6个标准差处；

步骤4、执行协作搬运任务时，首先使用动作捕捉系统获取被搬运物体和人手的位姿，检测到人手不断靠近目标，则控制机器臂根据示教学习获得的轨迹概率模型运动到目标上方进行抓取，抓取阶段不需要柔性；抓取成功之后人和机械臂协同搬运，以示教学习生成的轨迹概率模型经过修正后作为参考轨迹，在一定干扰力的作用下沿着期望轨迹的方向运动，干扰力超过阈值则切换为自由搬运任务，由人手牵引物体达到目标位置。

在上述的一种基于示教学习的协作搬运方法，步骤1中修正轨迹概率模型使用的缩小函数为：

式中x为轨迹点在整条轨迹中所占的比例。

在上述的一种基于示教学习的协作搬运方法，步骤2中变刚度系数导纳控制以修正后的轨迹概率模型为参考轨迹，具体控制器结构为：

式中δxt分别是机械臂末端实际运动的速度、位置与参考轨迹速度、位置的差值，分别是阻尼和刚度矩阵，是机械臂末端力矩传感器数据经过重力补偿后的力，即干扰力，式中及后面提到的下标x为全局坐标系x；λ为轨迹点对应高斯分布协方差矩阵经对角分解所得对角矩阵，满足式(1-3)；kp为刚度系数，满足式(1-4)，随着距参考点的距离增加而增大；

式中n为机械臂末端点(x,y,z)在距离参考点的n个标准差椭球上，满足

在上述的一种基于示教学习的协作搬运方法，搬运任务切换，搬运任务包括特定搬运任务(已知搬运终点)和自由搬运任务(临时决定搬运终点)，基于交互力阈值实现特定搬运任务到自由搬运任务的切换，首先计算示教过程交互力的高斯分布，阈值初始值选在6个标准差处，随着任务的执行不断更新。

本发明由于采用以上技术方案，具有以下优点：

1.充分利用轨迹的概率分布，结合导纳控制模型，实现机械臂末端柔性交互；

2.保证机械臂末端实际位置在参考轨迹的3个标准差之内，起点和终点的位置精度也得到保证，较好平衡了搬运的柔顺性和目标位置精度。

3.考虑搬运任务的切换，以交互力阈值作为特定搬运任务和自由搬运任务切换的依据，提高协作搬运的灵活性。

附图说明

图1是本发明协作搬运系统示意图。

图2是本发明协作搬运各部分通信图。

图3是本发明写作搬运系统控制方案图。

图4是本发明协作搬运系统流程图。

其中，1-机械臂，2-pc上位机，3-六维力/力矩传感器，4-动捕定位点。

具体实施方式

下面结合附图来对本发明提供的协作搬运系统进行详细说明，所述是对本发明的解释而不是限定。

如图2所示，控制系统框图包含机械臂1，pc上位机2，六维力/力矩传感器3，动捕定位点4，所述机械臂1为具有控制功能的多轴机械臂，所述六维力/力矩传感器3用来进行交互力测量，安装在机械臂1末端，pc上位机2与机械臂1、六维力/力矩传感器3以及动作捕捉系统通信相连，运行过程中，机械臂1在pc上位机2的指令下携带抓取手进行协作搬运操作，所述pc上位机2搭建的控制系统基于ros平台。所述控制系统基于ros平台，包括控制算法节点，六维力/力矩传感器3的反馈节点，利用动作捕捉系统获取动捕定位点4位置的节点，和控制机械臂1运动节点。六维力/力矩传感器3、机械臂1、动作捕捉系统和pc上位机连接至同一局域网。机械臂1与pc之间采用modbus通信协议，采用ros中统一的通信接口实现机械臂1状态读取和运动控制。六维力/力矩传感器3和动作捕捉系统vicon采用ethernet协议将数据发送至pc端，在控制算法节点中接收各传感器状态，并计算下周期控制指令。

所述机械臂1为ur5e机械臂，有基于ros的驱动程序和功能包，安装完成即可实现对机械臂的运动控制。所述六维力/力矩传感器3有基于ros的驱动程序，实时反馈协作搬运的交互力信息。本实施例中，选择了ur5e作为机器人平台，六维力/力矩传感器3选用了atinetf/t六维力/力矩传感器。动作捕捉系统选用了viconmotionsystems光学运动捕捉系统，有基于ros的驱动程序，可以在ros中获取定位点的三维位置信息。

所述控制算法采用变刚度系数的导纳控制，基于simulink搭建控制系统模型，并将其转化为c++代码。该算法由示教学习生成的轨迹概率模型修正后作为参考轨迹，叠加变刚度系数的导纳控制，以六维力/力矩传感器3和机械臂1末端三维位置为输入量，基于六维力/力矩传感器3反馈，实现机械臂1的位置控制，完成协作搬运任务。

所述示教学习所得轨迹概率模型包括轨迹上n个点的均值和对应协方差矩阵σ，针对获得的轨迹概率模型，采用缩小函数修正轨迹概率模型，缩小起点和终点附近协方差矩阵。

采用的缩小函数为：

式中x为轨迹点在整条轨迹中所占的比例。

所述变刚度系数导纳控制以修正后的轨迹概率模型为参考轨迹，具体控制器结构为：

式中δxt分别是机械臂末端实际运动的速度、位置与参考轨迹速度、位置的差值，分别是阻尼和刚度矩阵，是机械臂末端力矩传感器数据经过重力补偿后的力，即干扰力，式中及后面提到的下标x为全局坐标系x。λ为轨迹点对应高斯分布协方差矩阵对角分解所得对角矩阵，满足式(1-3)。kp为刚度系数，满足式(1-4)，随距参考点的距离增加而增大。

式中n为机械臂末端点(x,y,z)在距离参考点的n个标准差椭球上，满足

可得协作搬运控制系统传递函数为

如图3所示是一个协作搬运控制系统方案。本例中控制周期δt＝0.01s，六维力/力矩传感器3获得的交互力进行滤波后，针对被搬运物体进行重力补偿，经过控制系统传递函数，得到机械臂末端偏离参考轨迹的距离，再叠加参考位置得到机械臂末端位置，计算出位置后通过逆运动学计算六轴机械臂1关节角度，从而控制机器人运动，实现人机协作搬运。

如图4所示是一个协作搬运系统整体流程图，整个过程分一下六步：

①动作捕捉系统获取定位点位置，从而获得被搬运物体和人手的位姿；

②检测到人手不断靠近被搬运物体；

③机械臂基于示教学习获得的轨迹概率模型运动到物体上方实现抓取，抓取阶段不需要末端柔性；

④根据被搬运物体重力进行重力补偿；

⑤基于图3所示协作搬运控制方案，采用变刚度系数的导纳控制，在一定交互力的作用下机械臂沿着修正后的轨迹概率模型方向运动；

⑥协作搬运保持特定搬运任务状态直到终点或者交互力超过阈值切换为自由搬运任务，由人牵引机械臂运动到目标位置。

所述搬运任务包括特定搬运任务和自由搬运任务。所述特定搬运任务已知搬运终点位置，所述自由搬运任务在搬运过程中临时指定终点。所述搬运任务切换，基于交互力阈值实现特定搬运任务到自由搬运任务的切换，首先计算示教过程交互力的高斯分布，阈值选在6个标准差处，随着任务的执行不断更新。

本文中所描述的具体实施例仅仅是对本发明精神作举例说明。本发明所属技术领域的技术人员可以对所描述的具体实施例做各种各样的修改或补充或采用类似的方式替代，但并不会偏离本发明的精神或者超越所附权利要求书所定义的范围。