CN114888768A - 一种基于多传感器融合的移动双工业机器人协同抓取系统及方法 - Google Patents

Abstract

本发明公开了一种基于多传感器融合的移动双工业机器人协同抓取系统及方法，采用基于多传感器融合的移动双工业机器人协同抓取系统实现，所述系统包括三维深度相机、激光式深度传感器、PC控制端、主、从工业机器人、搭载工业机器人的全方位移动底盘模块，具体包括全方位移动底盘、底盘驱动器、底盘控制器、外部通信接口。本发明将多种传感器得到的信息进行融合，能够有效的适应变化的环境，具有很强的自适应能力、学习能力和自治功能。移动双工业机器人的投入使用降低企业成本、提高效率，如在工业领域中的流转车间、货运码头、大型物流仓库等，往往需要组装、拆卸、分拣、搬运、包装等多重程序，机器作业可以降低人工成本，提高工业生产的自动化和智能化程度。

Description

一种基于多传感器融合的移动双工业机器人协同抓取系统及 方法

技术领域

本发明涉及移动机器人生产领域，具体的说是一种基于多传感器融合的移动双工业机器人协同抓取系统及方法。

背景技术

随着现代工业的发展和科学技术的进步，人们对工业、医疗、服务水平的要求不断提高，对于许多任务而言，单机器人的操作明显不能满足要求，为了适应任务的复杂性、智能性、柔顺性要求，双工业机器人甚至群组机器人系统应运而生。相对于单机器人，双工业机器人可以大大增强对复杂装配任务的适应性，同时可以提高工作空间的利用效率。双工业机器人并不是两个工业机器人的简单组合应用，他们除了要实现协同抓取的控制目标外，还需要满足相互间的协调控制以及对环境的适应性，这使得双工业机器人协调操作的复杂性大大增加，这种高复杂性使双工业机器人操作需要更先进的集成系统、高层规划和推理，以及可调整的控制方法。

同时，随着深度学习、无监督学习、多传感器融合等更深层次的研究，机器人已经拥有了可视化、可语音化的技术水平，且具备了更为关键的思考能力，驱动其得到更为广泛的应用。目前应用在机器人上的感知设备主要有视觉传感器和深度传感器(如激光雷达)，基于视觉传感器的物体的识别与定位与人类依靠眼睛获取外部信息相似，涉及图像处理、机器学习、模式识别、人工智能等多个研究领域。因此研究出一种可以移动的双工业机器人在工作区域下完成环境感知和自主导航，并执行目标物体的抓取放置，具有重要的研究价值和实际意义。

发明内容

本发明针对现有技术中的不足，提供一种基于多传感器融合的移动双工业机器人协同抓取系统及方法，来解决现在企业生产车间在进行货物搬运时受放置位置局限的影响，无法做到作业高灵活性、高柔性。本意在于解决相对传统机器人需要固定安装，工作范围受限，以及需要安全保护栏与人隔离等不便之处，且双工业机器人配合的设置，使其具备了更高的柔性、复杂多变工作环境的适应性以及抓取和移动物体的多选择性。该系统可应用于无人化车间，取代传统的流水线式搬运，有效提高工作灵活性，并且最大程度降低对工作人员造成的安全隐患，提高工厂的自动化水平。

为实现上述目的，本发明采用以下技术方案：

一种基于多传感器融合的移动双工业机器人协同抓取系统及方法，在作业范围下采用基于多传感器融合的移动双工业机器人协同抓取系统实现，所述系统包括三维深度相机、激光式深度传感器、PC控制端、主、从工业机器人、搭载工业机器人的全方位移动底盘模块，具体包括全方位移动底盘、底盘驱动器、底盘控制器、外部通信接口。

所述作业范围由实施下具体情况在x、y方向确定二维平面工作范围；

所述移动底盘由四个直流伺服电机单独控制的麦克纳姆轮作为执行机构，底盘控制器可以通过EtherCAT访问，使移动底盘以时间最短原则移动到目标点；

所述三维深度相机安装在移动底盘上，实现了在抓取场景不固定下的可移动式、眼在手外的手眼标定方式；

所述激光式深度传感器安装在所述移动底盘上，且对主、从工业机器人作业空间不产生干涉；

所述主、从工业机器人固定安装在移动底盘上，比传统单机器人具有更高的灵活性、操作性和负载能力，两机器人安装在同一水平位置上，在x方向相距一定距离。

所述三维深度相机用于完成标定获取相机的内部参数和外部参数，内部参数包括相机的实际焦距、像素大小等，用于解决相机本身安装造成的误差，外部参数包括相机在工作空间下的位置、旋转方向等，用于进一步的将获得的目标物体深度图像提取深度信息完成目标物体的识别与定位；所述激光式深度传感器通过不间断发射红外信号，接受反射回来的信号时间差进行扫描获取二维空间的点阵数据，进一步的配合SLAM技术完成作业环境地图构建，实现起始点到目标点的路径规划，同时实时更新路径上障碍物信息，完成导航避障；所述主、从工业机器人用于完成目标物体的协同抓取作业；所述全方位移动底盘用于搭载三维深度相机、激光式深度传感器、主、从工业机器人，完成上述设备在作业空间下灵活移动；所述PC控制端用于接收三维深度相机获取的深度信息进行坐标变换获取目标物体在工业机器坐标系下的三维坐标，接收激光式深度传感器获取的信息构建环境地图，创建系统中的各个节点，实现整个系统节点间的通信。

所述三维深度相机与PC控制端采用USB串口连接；所述激光式深度传感器与PC控制端之间采用一端为USB，一端为TART的数据线连接，其中USB串口与PC控制端连接,UART串口与激光式深度传感器连接通讯；所述主、从工业机器人与全方位移动底盘各采用24V电源输入和EtherCAT通讯；所述由主、从工业机器人和全方位移动底盘组成的移动双工业机器人与PC控制端采用EtherCAT完成数据通讯。

一种基于多传感器融合的移动双工业机器人协同抓取方法，包括以下步骤：

步骤1：根据实际作业场景在x、y方向确定二维平面工作范围，分别以工作范围、主工业机器人、从工业机器人、三维深度相机的中心点为原点构建基座标系oxyz、o₁x₁y₁z₁、o₂x₂y₂z₂、o_Kx_Ky_Kz_K；，确定已知的工作区域范围下工业机器人移动底盘起始位置、主从工业机器人起始姿态、待抓取目标物体位置、放置目标物体位置；

步骤2：调试软、硬件设备，完成PC端与激光式深度传感器数据通讯，完成PC端与深度相机数据通讯，实现PC控制端与移动双机器人的通讯，使机器人系统和PC机在同一个局域网下，完成数据显示和实时控制，标定主、从工业机器人与深度相机的相对位置关系；

步骤3：移动底盘与PC控制端完成通讯后，在PC端控制移动底盘至起始位置，设置机器人起始姿态；

步骤4：开启激光式深度传感器和全方位移动底盘，打开PC控制端可视化环境和驱动程序，基于Ubuntu系统ROS环境下，采用SLAM实现移动平台的自动建图，完成工作区域地图；

步骤5：移动底盘规划移动路径到达目标物体抓取位置；

步骤6：；深度相机采集深度图像并传输给PC控制端，完成目标坐标和末端执行器之间坐标转换，末端执行器为二指夹爪，末端坐标取的是二指中间；

步骤7：控制主、从工业机器人上的末端夹持器协同抓取目标物体；

步骤8：移动平台搬运目标物体平稳移动到目标放置点位置；

步骤9：控制主、从工业机器人上的末端夹持器，放置目标物体至放置点；

步骤10：移动底盘返回至起始位置，主、从工业机器人并恢复到起始姿态，或移动机器人回到目标物体抓取位置，再次搬运作业，直至完成全部任务后移动底盘再返回至起始位置，主、从工业机器人恢复到起始姿态。

进一步地，所述步骤1包括：根据实际作业场景在x、y方向确定二维平面工作范围，分别以工作范围、主工业机器人、从工业机器人、三维深度相机的中心点为原点构建基座标系oxyz、o₁x₁y₁z₁、o₂x₂y₂z₂、o_Kx_Ky_Kz_K。构建过程如下：

定义工业机器人移动底盘起始位置为a点，以工作区域地图中心点o为坐标原点，当移动双工业机器人水平放置在a点时，以主、从工业机器人的底盘中心点o₁、o₂连线为x轴，垂直于o₁、o₂连线的方向为y轴，根据右手法则确定z轴方向，建立基坐标系oxyz；

以主机器人底盘中心o₁点为坐标原点，以o₁、o₂连线为x₁轴，与y轴平行方向为y₁轴，与z轴平行方向为z₁轴建立主工业机器人坐标系o₁x₁y₁z₁；

以从机器人底盘中心o₂点为坐标原点，沿x₁轴平移两机器人底盘中心距离建立从工业机器人坐标系o₂x₂y₂z₂；

以三维深度相机的摄像头视角中心k为坐标原点，与x轴平行同向为x_K轴，与y轴平行同向为y_K轴,与z轴平行同向为z_K轴建立坐标系o_Kx_Ky_Kz_K；

起始位置a点在工作区域基坐标系oxyz下的位置坐标为(x_a,y_a,z_a),目标物体抓取位置b点在工作区域基坐标系oxyz下的位置坐标为(x_b,y_b,z_b)，目标物体放置位置c点在工作区域基坐标系OXYZ下的位置坐标为(x_c,y_c,z_c)；

主、从工业机器人关节角用θ_ij表示，起始位置a点下的机器人各关节角度为0，处于零位状态，其中，i＝1,2，1表示为主工业机器人，2表示为从工业机器人，j＝1,2,…,M，M为机器人关节数目且M为正整数；

主、从工业机器人在目标物体抓取位置b点和目标物体放置位置c点时，由机器人末端目标点位置坐标的目标值p_oi在工作区域基坐标系oxyz下沿x轴、y轴、z轴的坐标分量p_oix、p_oiy、p_oiz和姿态坐标的目标值r_oi在工作区域基坐标系oxyz下绕x轴、y轴、z轴的偏航角μ_oi、俯仰角υ_oi、侧滚角ω_oi确定各组关节角的目标值θ_ij，在PC控制端设定主、从机器人各组关节角的目标值θ_ij，控制机器人各关节运动到各组关节角的目标值θ_ij，完成目标物体抓取和放置工作；

进一步地，主、从工业机器人在初始位置、目标抓取位置和目标放置位置下的各关节角度θ_ij可一致可各自调整为最优抓取、放置状态；

进一步地，所述步骤2调试软、硬件设备具体包括以下内容：完成由激光式深度传感器、三维深度相机、全方位移动底盘、主从工业机器人组成的移动双机器人系统与PC控制端的数据通讯，使机器人系统和PC端在同一个局域网下，完成数据显示和实时控制；标定主、从工业机器人与深度相机的相对位置关系；为方便可视化，监测移动双机器人工作状况，在可视化环境中导入移动双机器人的URDF模型；

进一步地，所述步骤4包括：在基于Ubantu系统ROS环境下载并配置RVIZ功能包，获取激光测距仪数据和确定传感器与机器人坐标系之间的坐标转换，方便运用SLAM算法获取里程计信息和观测到的激光信息来构建工作场景；

进一步地，所述步骤5包括：移动底盘规划出到达目标抓取点时间最短路径后，若路径无障碍物，移动底盘直接导航到达位置；当移动过程中激光雷达在路径上检测到障碍物体时，全方位移动底盘依次向左前方45°方向前行，直至成功越过障碍物，继续规划路径到达目标位置；

进一步地，所述步骤6包括：深度相机获取包含物体的深度图像，利用图像识别和检测算法得到物体的像素坐标，将物体的位姿从像素坐标变换到相机坐标系，再变换到主、从工业机器人坐标系下。相机拍摄图像还需识别异形物体形状，判断物体可抓取性，预测目标物体姿态，分析抓取区域，为其完成自主抓取提供决策；

进一步地，所述步骤10包括：只需要完成一次作业时，完成上述任务后，移动底盘返回至起始位置，主、从工业机器人并恢复到起始姿态，至此完成一次搬运作业；当需要移动机器人多次往复作业时，移动底盘直接回到目标物体抓取位置，重新规划主从工业机器人抓取目标后，移动至放置点位置放置物体，直至完成全部搬运作业，移动底盘再返回至起始位置，主、从工业机器人并恢复到起始姿态。

一种基于多传感器融合的移动双工业机器人协同抓取方法具体实现过程，包括以下步骤：

步骤1：根据实际作业场景在x、y方向确定二维平面工作范围、分别以工作范围、主工业机器人、从工业机器人、三维深度相机的中心点为原点构建基座标系oxyz、o₁x₁y₁z₁、o₂x₂y₂z₂、o_Kx_Ky_Kz_K；，确定已知的工作区域范围下工业机器人移动底盘起始位置、主从工业机器人起始姿态、待抓取目标物体位置、放置目标物体位置；

步骤2：调试软、硬件设备，具体包括以下内容：

(1)完成PC端与激光式深度传感器数据通讯；

(2)完成PC端与深度相机数据通讯；

(3)实现PC控制端与移动双机器人的通讯，使机器人系统和PC机在同一个局域网下，完成数据显示和实时控制；

(4)标定主、从工业机器人与深度相机的相对位置关系

在标定系统中，主要包括以下坐标系：主工业机器人基座标系R1₀、从工业机器人基座标系R2₀、主工业机器人末端标定的参考点坐标系R_S1、从工业机器人末端标定的参考点坐标系R_S2和深度相机坐标系R_K。标定时，分别将参考点S1、S2固定在主、从工业机器人的末端执行器上。以主工业机器人为例，通过机器人正运动学求出基座标系R1₀到第j关节坐标系R1_j转换矩阵

转换矩阵

的转换过程为

式中

为主工业机器人基坐标系R1₀到第一关节坐标系R1₁的转换矩阵，

为第一关节坐标系R1₁到第二关节坐标系R1₂的转换矩阵，

为第j-2关节坐标系R1_j-2到第j-1关节坐标系R1_j-1的转换矩阵，

为第j-1关节坐标系R1_j-1到第j关节坐标系R1_j的转换矩阵。参考点坐标系R_S1到第j关节坐标系的位姿转换矩阵为

参考点坐标系到深度相机坐标系R_K的位姿转换矩阵为

因此，只要不改变主工业机器人末端的标定参考点R_S1相对于工业机器人末端夹爪的位置，通过计算两次标定过程完成标定，每次标定只需改变工业机器人各关节角度，设两次标定过程变换矩阵分别为

和

下列等价公式可求解出相机相对于主工业机器人基坐标系的变换矩阵

上式中，

为第一次标定主工业机器人与三维深度相机相对位置时，深度相机坐标系相对主工业机器人末端标定的参考点坐标R_S1的转换矩阵，

为第二次标定主工业机器人与三维深度相机相对位置时，深度相机坐标系相对主工业机器人末端标定的参考点坐标R_S1的转换矩阵。

同理，可求出相机相对于从工业机器人基坐标系的变换矩阵

(5)RVIZ中导入移动双机器人的URDF模型

为方便可视化，监测移动双机器人工作状况，在RVIZ中导入移动双机器人的URDF模型。加载完模型后，启动joint_state_publisher和wheel_state_publisher控制节点，发布工业机器人关节角度和移动底盘移动方位，在终端运行TF坐标变换节点生成移动双机器人的TF树：

$:rosrun tf view_frames；

当移动底盘和主从工业机器人的URDF语法结构正确时，可在当前目录下生成TF树的PDF文件，如果关节连杆和全方位移动轮定义有误，再使用gedit命令对模型修改，直至正确，生成PDF文件和修改模型指令如下：

$:envince frames.pdf；

$:sudo gedit hqbot.urdf；

步骤3：移动底盘至起始位置，设置机器人起始姿态；

步骤4：SLAM构建场景地图

在完成以上步骤后，控制机器人移动至起始位置，在连通机器人系统与PC机后，只开启激光雷达驱动和全方位移动底盘驱动，在ROS软件中打开amcl、move_base、RVIZ可视化环境，用于驱动机器人移动和跟踪机器人当前位姿，以及监视机器人在作业环境地图构建完成程度；

步骤5：地图构建完成后，在控制终端输入机器人当前位置，告诉机器人在地图中的位置，按住鼠标不动选定机器人，移动鼠标显示方向，确定好方向后，点击目标抓取点位置，机器人自主导航至目标点。当移动过程中激光雷达在移动路径上检测到障碍物体时，全方位移动底盘依次向左前方45°方向前行，直至成功越过障碍物，继续规划路径到达目标位置；

步骤6：相机识别目标物体并发送位置信息

在完成步骤5后，移动底盘和主、从工业机器人在目标位置处于待抓取状态，抓取前要通过深度相机对目标物体进行识别定位。使用深度相机拍摄目标物体所处区域，获得一幅RGB图和一幅Depth图，使用模板匹配识别目标物体在RGB图中的像素区域[(u₁ v₁)，(u₂v₂)]，获取的像素区域中心为目标物体形心在Depth图中的像素坐标(u₀ v₀),读取其深度值，进行PCL三维模型重建并将模型保存至模型库中，计算(u₀ v₀)相对应的三维坐标(x_b,y_b,z_b)。找到目标物体后，可以获取目标物体的位姿，根据目标物体形状大小，确定主、从工业机器人抓取点，得到位姿

通过以下变换公式，结合步骤2得到的标定后的

和

分别计算，得到实际抓取情况下的基座标系R1₀到第j关节坐标系R1_j转换矩阵

上式中，

为主工业机器人下的抓取点坐标相对于第j关节坐标系的转换矩阵，

为从工业机器人下的抓取点坐标相对于第j关节坐标系的转换矩阵。

步骤7：主、从机器人协同抓取目标

找到目标物体，确定抓取点位置后，设定主从工业机器人抓取物体和放置物体时各关节速度与加速度均为0，确定主从机器人路径规划时间t,确定根据步骤2和步骤6获取的目标抓取点的工业机器人位姿，通过逆运动学求解出最优解，完成主从工业机器人的抓取规划，规划要求为，主工业机器人关节从左侧移动到设定的目标抓取点，从工业机器人关节从右侧移动到设定的目标抓取点，避免在规划过程中两个手臂发生碰撞。基于ROS中的MoveIt！，通过robot_state_publisher计算逆运动学，发布机器人状态；启动move_group规划节点，joint_states订阅并发布主从工业机器人反馈数据；启动arm规划组控制器和夹爪控制器完成目标抓取。

当主从工业机器人成功抓取到目标物体后，主、从工业机器人协作搬运目标物体时，机器人与物体之间形成闭链后需要考虑主从机器人之间的位姿约束问题，主从机器人先后规划，同时运动，机器人之间的位姿约束关系可以下式表达,式中T_B是目标物体的中心坐标

在搬运过程中，保持主、从末端执行器、主、从工业机器人与目标物体之间相对位置不变，主机器人与目标物体中心变换矩阵为

目标物体中心与从机器人变换矩阵为

确定以下位姿转化齐次矩阵公式为常数矩阵：

步骤8：在RVIZ构建的地图中点击确定好的目标放置点位置，移动平台搬运目标物体平稳移动到目标放置点位置；

步骤9：主、从机器人协同放置目标

设定主从工业机器人放置物体时各关节速度与加速度均为0，确定目标放置点的主从工业机器人位姿，通过逆运动学求解出最优解。MoveIt！通过robot_state_publisher计算逆运动学，发布此时的机器人状态；启动move_group规划节点，joint_states订阅并发布主从工业机器人反馈数据；启动arm规划组控制器和夹爪控制器完成目标物体的放置；

步骤10：只需要完成一次作业时，完成上述任务后，移动底盘返回至起始位置，主、从工业机器人并恢复到起始姿态，至此完成一次搬运流程；当需要移动机器人多次往复作业时，完成上述任务后，为节约时间，移动底盘直接回到目标物体抓取位置，重新规划主从工业机器人抓取目标后，移动至放置点位置放置物体，直至完成全部搬运作业，移动底盘再返回至起始位置，主、从工业机器人并恢复到起始姿态。

在现有作业场景之下，本发明的有益效果是：

本发明提出的一种基于多传感器融合的移动双工业机器人协同抓取系统及方法，将主从工业机器人固定在全方位移动底盘上，跟随移动底盘的移动实现不同位置下的作业，移动双工业机器人具有移动平台的可移动性能及工业机器人的可操作性。当搬运大型异状物体时，单工业机器人无法完成作业要求，此时在单工业机器人的基础上发展双工业机器人的使用，认知度高，协同搬运效果好；

本发明提出的一种基于多传感器融合的移动双工业机器人协同抓取系统及方法，将三维深度相机与移动底盘组装融合，改变了传统的相机固定在工业机器人上或相机固定在目标物体区域，优化了前者相机固定在工业机器人的不稳定性和后者相机固定在目标物体区域的不灵活性、视觉范围窄等缺陷，不需要针对不同的作业位置不断调整相机视角，简化标定过程，使标定结果更加准确；

本发明提出的一种基于多传感器融合的移动双工业机器人协同抓取系统及方法，当主从工业机器人使用同一个通信接口时，二者交互数据，形成物理耦合关系，从机器人可对主机器人的反应做出相应的决策；进一步的，双工业机器人也可各自独立工作完成对多目标的操作与控制。

本发明的有益效果是：本发明提出的一种基于多传感器融合的移动双工业机器人协同抓取系统及方法，将多种传感器得到的信息进行融合，能够有效的适应变化的环境，具有很强的自适应能力、学习能力和自治功能。移动双工业机器人的投入使用降低企业成本、提高效率，如在工业领域中的流转车间、货运码头、大型物流仓库等，往往需要组装、拆卸、分拣、搬运、包装等多重程序，机器作业可以降低人工成本，提高工业生产的自动化和智能化程度。

附图说明

图1为本发明实施例下基于多传感器融合的移动双工业机器人系统组成示意图；

图2为本发明实施例下基于多传感器融合的移动双工业机器人平台结构示意图；

图3为本发明实施例下的坐标位置和移动底盘运动路径示意图；

图4为本发明实施例下基于多传感器融合的移动双工业机器人工作流程示意图；

图5为本发明实施例下调试软、硬件设备流程示意图；

图6为本发明实施例下移动双工业机器人的URDF模型树状图；

图7为本发明实施例下SLAM构建场景地图流程图；

图8为本发明实施例下的目标物体识别定位流程图。

具体实施方式

现在结合附图对本发明作进一步详细的说明。

附图1为本发明实施例下基于多传感器融合的移动双工业机器人系统组成示意图，主要包括工业机器人系统、全方位移动平台系统、三维深度相机、激光式深度传感器以及PC控制端；其中工业机器人系统具体包括主、从工业机器人机械本体、机器人控制器；全方位移动平台系统包括移动底盘机械本体、底盘驱动器、底盘控制器、外部通信接口；PC控制端主要用于接收三维深度相机获取的深度图像信息进行图像数据处理，接收激光式深度传感器的红外信息进行数据交互，并通过RVIZ可视化平台完成对获取信息的直观显示，此外，基于PC控制端可对主、从工业机器人、三维深度相机、激光式深度传感器、全方位移动底盘进行操作控制。

附图2为本发明实施例下基于多传感器融合的移动双工业机器人平台结构示意图，该简化示意图进一步描述主、从工业机器人、三维深度相机、激光式深度传感器安装在移动底盘上的相对位置关系和建立的主、从机器人工作坐标系、相机坐标系位置及方向。

附图3为本发明实施例下的坐标位置和移动底盘运动路径示意图，其中外框矩形为确定的工作区域范围，座标系oxyz、o₁x₁y₁z₁、o₂x₂y₂z₂、o_Kx_Ky_Kz_K分别为构建的工作区域、主工业机器人、从工业机器人、三维深度相机的基坐标系位置，a点为初始状态下移动机器人位置点，b点为移动机器人抓取目标物体的抓取位置，c点为移动机器人放置目标物体的放置位置，带有箭头指向的虚线为移动底盘的运动路径。

附图4为本发明实施例下基于多传感器融合的移动双工业机器人工作流程示意图，通过该流程图说明基于多传感器融合下的移动双工业机器人协同抓取过程和步骤，包括前期规划作业场景、确定目标物体位置和放置位置，中期构建场景地图和目标识别抓取与放置，后期主、从机器人复位完成搬运作业。

更进一步的说明如下：

步骤1：根据实际作业场景在x、y方向确定二维平面工作范围，分别以确定的工作区域范围、主工业机器人、从工业机器人、三维深度相机的中心点为原点构建基座标系oxyz、o₁x₁y₁z₁、o₂x₂y₂z₂、o_Kx_Ky_Kz_K；，确定已知的工作区域范围下工业机器人移动底盘起始位置、主从工业机器人起始姿态、待抓取目标物体位置、放置目标物体位置；

步骤2：如附图5所示，调试软、硬件设备，具体包括以下内容：

(1)完成PC端与激光式深度传感器数据通讯；

(2)完成PC端与深度相机数据通讯；

(3)实现PC控制端与移动双机器人的通讯，使机器人系统和PC机在同一个局域网下，完成数据显示和实时控制；

以youbot机器人为例，具体实现过程如下：通过网线接口连通机器人和装有Ubuntu系统ROS环境的PC机后，在PC端将export ROS_HOSTNAME＝youbot_shiyan.local写在～/.bashrc保存；将export ROS_HOSTNAME＝youbot.local和export ROS_MASTER_URI＝http://youbot_shiyan.local:11311写在～/.bashrc保存；在PC端输入source/.bashrc命令，更新basgrc,当机器人启动ROS内核时，PC端可以接受相应的节点，以此来控制机器人运动。其中，youbot_shiyan为机器人系统名，youbot为PC端系统名。

(4)标定主、从工业机器人与深度相机的相对位置关系

在标定系统中，主要包括以下坐标系：主工业机器人基座标系R1₀、从工业机器人基座标系R2₀、主工业机器人末端标定的参考点坐标系R_S1、从工业机器人末端标定的参考点坐标系R_S2和深度相机坐标系R_K。标定时，分别将参考点S1、S2固定在主、从工业机器人的末端执行器上。以主工业机器人为例，通过机器人正运动学求出基座标系R1₀到第五关节坐标系R1₅转换矩阵

转换矩阵

的转换过程为

式中

为基坐标系R1₀到第一关节坐标系R1₁的转换矩阵，

为第一关节坐标系R1₁到第二关节坐标系R1₂的转换矩阵，

为第二关节坐标系R1₂到第三关节坐标系R1₃的转换矩阵，

为第三关节坐标系R1₃到第四关节坐标系R1₄的转换矩阵，

为第四关节坐标系R1₄到第五关节坐标系R1₅的转换矩阵。参考点坐标系R_S1到第五关节坐标系的位姿转换矩阵为

参考点坐标系到深度相机坐标系R_K的位姿转换矩阵为

因此，只要不改变主工业机器人末端的标定参考点R_S1相对于工业机器人末端夹爪的位置，通过计算两次标定过程完成标定，每次标定只需改变工业机器人各关节角度，设两次标定过程变换矩阵分别为

和

下列等价公式可求解出相机相对于主工业机器人基坐标系的变换矩阵

上式中，

为第一次标定主工业机器人与三维深度相机相对位置时，深度相机坐标系相对主工业机器人末端标定的参考点坐标R_S1的转换矩阵，

为第二次标定主工业机器人与三维深度相机相对位置时，深度相机坐标系相对主工业机器人末端标定的参考点坐标R_S1的转换矩阵。

同理，求出相机相对于从工业机器人基坐标系的变换矩阵

(5)RVIZ中导入移动双机器人的URDF模型

为方便可视化，监测移动双机器人工作状况，在RVIZ中导入移动双机器人的URDF模型。如附图6所示，为本发明实施列下移动双工业机器人的URDF模型树状图。加载完模型后，启动joint_state_publisher和wheel_state_publisher控制节点，发布工业机器人关节角度和移动底盘移动方位，在终端运行TF坐标变换节点生成移动双机器人的TF树：

$:rosrun tf view_frames；

当移动底盘和主从工业机器人的URDF语法结构正确时，可在当前目录下生成TF树的PDF文件，如果关节连杆和全方位移动轮定义有误，再使用gedit命令对模型修改，直至正确，生成PDF文件和修改模型指令如下：

$:envince frames.pdf；

$:sudo gedit hqbot.urdf；

步骤3：移动底盘至起始位置，设置机器人起始姿态；

步骤4：SLAM构建场景地图

如附图7所示作业环境地图构建流程，在完成以上步骤后，控制机器人移动至起始位置，在连通机器人系统与PC机后，只开启激光雷达驱动和全方位移动底盘驱动，在ROS软件中打开amcl、move_base、RVIZ可视化环境，用于驱动机器人移动和跟踪机器人当前位姿，以及监视机器人在作业环境地图构建完成程度。当机器人在工作环境中起始位置时，在机器人电脑终端输入机器人启动节点：

$:roslaunch youbot_oodl youbot_oodl_driver_hokuyo.launch；

把全方位移动底盘驱动和激光雷达驱动写在同一个launch文件里面，在机器人电脑终端输入amcl、move_base、RVIZ启动节点：

$:roslaunch youbot_navigation_global amcl.launch；

$:rosrun rviz rviz；

$:roslaunch youbot_navigation_global move_base_global.launch；

$:rosrun rqt_graph rqt_graph；

得到各节点状态图后，基于move_base移动指令，驱动移动底盘在工作环境中移动，激光雷达进行扫描，在RVIZ可视化环境中监测全局地图和实时地图构建情况，直至实际工作环境与建立的可视地图完美匹配。

步骤5：地图构建完成后，在控制终端输入机器人当前位置，告诉机器人在地图中的位置，按住鼠标不动选定机器人，移动鼠标显示方向，确定好方向后，点击目标抓取点位置，机器人自主导航至目标点。当移动过程中激光雷达在移动路径上检测到障碍物体时，全方位移动底盘依次向左前方45°方向前行，直至成功越过障碍物，继续规划路径到达目标位置。

步骤6：相机识别目标物体并发送位置信息

如附图8所示识别过程，在完成步骤5后，移动底盘和主、从工业机器人在目标位置处于待抓取状态，抓取前要通过深度相机对目标物体进行识别定位。使用深度相机拍摄目标物体所处区域，获得一幅RGB图和一幅Depth，使用模板匹配识别目标物体在RGB图中的像素区域[(u₁ v₁)，(u₂ v₂)]，获取的像素区域中心为目标物体形心在Depth图中的像素坐标(u₀ v₀),读取其深度值，进行PCL三维模型重建并将模型保存至模型库中，计算(u₀ v₀)相对应的三维坐标(x_b,y_b,z_b)。找到目标物体后，可以获取目标物体的位姿，根据目标物体形状大小，确定主、从工业机器人抓取点，得到位姿

通过以下变换公式，结合步骤2得到的标定后的

和

分别计算，得到实际抓取情况下的基座标系R1₀到第五关节坐标系R1₅转换矩阵

上式中，

为主工业机器人下的抓取点坐标相对于第五关节坐标系的转换矩阵，

为从工业机器人下的抓取点坐标相对于第五关节坐标系的转换矩阵。

步骤7：主、从机器人协同抓取目标

找到目标物体，确定抓取点位置后，设定主从工业机器人抓取物体和放置物体时各关节速度与加速度均为0，确定主从臂路径规划时间t,确定根据步骤2和步骤6获取的目标抓取点的工业机器人位姿，通过逆运动学求解出最优解，完成主从工业机器人的抓取规划，规划要求为，主工业机器人关节从左侧移动到设定的目标抓取点，从工业机器人关节从右侧移动到设定的目标抓取点，避免在规划过程中两个手臂发生碰撞。基于ROS中的MoveIt！，通过robot_state_publisher计算逆运动学，发布机器人状态；启动move_group规划节点，joint_states订阅并发布主从工业机器人反馈数据；启动arm规划组控制器和夹爪控制器完成目标抓取。

案例确定主工业机器人起始位置a点下的机器人各关节角度为θ₁₅＝(0.05,0.05,-0.05,0,0)，从工业机器人起始位置a点下的机器人各关节角度为θ₂₅＝(3.1915,0.05,-0.05,0,0)，根据

经过运动学逆解计算出的主机器人在抓取位置b点对应的各关节角度为(2.5261，2.3039,1.6208,2.8144,3.6149)，从机器人在抓取位置b点对应的各关节角度为(5.6676,2.3039，-1.6208,2.8144,3.6149)，主、从机器人抓取点对应的各关节角度为根据MOVEIT！规划好的各路径点，主从机器人根据轨迹慢慢移动到各路径点，开启末端执行器，完成抓取。

当主从工业机器人成功抓取到目标物体后，协作搬运目标物体时，主从机器人与物体之间形成闭链后需要考虑机器人之间的位姿约束问题，主、从机器人先后规划，同时运动，机器人之间的位姿约束关系可以下式表达,式中T_B是目标物体的中心坐标

在搬运过程中，保持主、从末端执行器、主、从工业机器人与目标物体之间相对位置不变，主机器人与目标物体中心变换矩阵为

目标物体中心与从机器人变换矩阵为

确定以下位姿转化齐次矩阵公式为常数矩阵：

步骤8：在RVIZ构建的地图中点击确定好的目标放置点位置，移动平台携带目标物体平稳移动到标放置点位置。

步骤9：主、从机器人协同放置目标

设定主从工业机器人放置物体时各关节速度与加速度均为0，确定目标放置点的主从工业机器人位姿，通过逆运动学求解出最优解。MoveIt！通过robot_state_publisher计算逆运动学，发布此时的机器人状态；启动move_group规划节点，joint_states订阅并发布主从工业机器人反馈数据；启动arm规划组控制器和夹爪控制器完成目标物体的放置。

确定相应的放置点，经过运动学逆解计算出主机器人在放置位置c点对应的各关节角度为(3.0148,0.5184，-3.7056，0.7531,2.8984)，从机器人在放置位置c点对应的各关节角度为(6.1562,0.5184，-3.7056,0.7531,6.0399)，根据MoveIT！规划好的各路径点，主从机器人根据轨迹慢慢移动到各路径点，开启末端执行器，完成物体放置。

步骤10：只需要完成一次作业时，完成上述任务后，移动底盘返回至起始位置，主、从工业机器人恢复到起始姿态，至此完成一次搬运；当需要移动机器人多次往复作业时，完成上述任务后，为节约时间，移动底盘直接回到目标物体抓取位置，重新规划主从工业机器人抓取目标后，移动至放置点位置放置物体，直至完成全部搬运作业，移动底盘再返回至起始位置，主、从工业机器人恢复到起始姿态。

需要注意的是，发明中所引用的如“上”、“下”、“左”、“右”、“前”、“后”等的用语，亦仅为便于叙述的明了，而非用以限定本发明可实施的范围，其相对关系的改变或调整，在无实质变更技术内容下，当亦视为本发明可实施的范畴。

以上仅是本发明的优选实施方式，本发明的保护范围并不仅局限于上述实施例，凡属于本发明思路下的技术方案均属于本发明的保护范围。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理前提下的若干改进和润饰，应视为本发明的保护范围。

Claims (9)

1.一种基于多传感器融合的移动双工业机器人协同抓取系统，其特征在于，采用基于多传感器融合的移动双工业机器人协同抓取系统实现，所述系统包括三维深度相机、激光式深度传感器、PC控制端、主、从工业机器人、搭载工业机器人的全方位移动底盘模块，具体包括全方位移动底盘、底盘驱动器、底盘控制器、外部通信接口；

所述移动底盘由四个直流伺服电机单独控制的麦克纳姆轮作为执行机构，底盘控制器可以通过EtherCAT访问，使移动底盘以时间最短原则移动到目标点；

所述三维深度相机安装在移动底盘上，实现了在抓取场景不固定下的可移动式、眼在手外的手眼标定方式；

所述激光式深度传感器安装在所述移动底盘上，且对主、从工业机器人作业空间不产生干涉；

所述主、从工业机器人固定安装在移动底盘上，比传统单机器人具有更高的灵活性、操作性和负载能力，两机器人安装在同一水平位置上，在x方向相距一定距离；

所述三维深度相机用于完成标定获取相机的内部参数和外部参数，内部参数包括相机的实际焦距、像素大小等，用于解决相机本身安装造成的误差，外部参数包括相机在工作空间下的位置、旋转方向等，用于进一步的将获得的目标物体深度图像提取深度信息完成目标物体的识别与定位；所述激光式深度传感器通过不间断发射红外信号，接受反射回来的信号时间差进行扫描获取二维空间的点阵数据，进一步的配合SLAM技术完成作业环境地图构建，实现起始点到目标点的路径规划，同时实时更新路径上障碍物信息，完成导航避障；所述主、从工业机器人用于完成目标物体的协同抓取作业；所述全方位移动底盘用于搭载三维深度相机、激光式深度传感器、主、从工业机器人，完成上述设备在作业环境下灵活移动；所述PC控制端用于接收三维深度相机获取的深度信息进行坐标变换获取目标物体在工业机器人坐标系下的三维坐标，接收激光式深度传感器获取的信息构建作业环境地图，创建系统中的各个节点，实现整个系统节点间的通信；

所述三维深度相机与PC控制端采用USB串口连接；所述激光式深度传感器与PC控制端之间采用一端为USB，一端为TART的数据线连接，其中USB串口与PC控制端连接,UART串口与激光式深度传感器连接通讯；所述主、从工业机器人与全方位移动底盘各采用24V电源输入和EtherCAT通讯；所述由主、从工业机器人和全方位移动底盘组成的移动双工业机器人与PC控制端采用EtherCAT完成数据通讯。

2.一种基于多传感器融合的移动双工业机器人协同抓取方法，其特征在于，包括以下步骤：

步骤1：根据实际作业场景在x、y方向确定二维平面工作范围，分别以工作范围、主工业机器人、从工业机器人、三维深度相机的中心点为原点构建基座标系oxyz、主工业机器人工作坐标系o₁x₁y₁z₁、从工业机器人工作坐标系o₂x₂y₂z₂、相机坐标系o_kx_ky_kz_k，确定已知的作业区域范围下工业机器人移动底盘起始位置、主从工业机器人起始姿态、待抓取目标物体位置、放置目标物体位置；

步骤2：调试软、硬件设备，完成PC端与激光式深度传感器数据通讯，完成PC端与深度相机数据通讯，实现PC控制端与移动双工业机器人的通讯，使机器人系统和PC机在同一个局域网下，完成数据显示和实时控制，标定主、从工业机器人与深度相机的相对位置关系；

步骤3：移动底盘与PC控制端完成通讯后，PC端控制移动底盘至起始位置，设置机器人起始姿态；

步骤4：开启激光式深度传感器和全方位移动底盘，打开PC控制端可视化环境和驱动程序，在Ubuntu系统ROS环境下，采用SLAM实现移动平台的自动建图，完成作业环境地图；

步骤5：移动底盘规划移动路径到达目标物体抓取位置；

步骤6：；深度相机采集深度图像并传输给PC控制端，完成目标坐标和末端执行器之间坐标转换；

步骤7：控制主、从工业机器人上的末端夹持器协同抓取目标物体；

步骤8：移动平台搬运目标物体平稳移动到目标放置点位置；

步骤9：控制主、从工业机器人上的末端夹持器，放置目标物体至放置点；

步骤10：移动底盘返回至起始位置，同时主、从工业机器人恢复到起始姿态，或移动机器人回到目标物体抓取位置，再次搬运作业，直至完成全部任务后移动底盘再返回至起始位置，主、从工业机器人恢复到起始姿态。

3.根据权利要求书2所述的一种基于多传感器融合的移动双工业机器人协同抓取方法，其特征在于，所述步骤1包括：根据实际作业场景在x、y方向确定二维平面工作范围，分别以工作范围、主工业机器人、从工业机器人、三维深度相机的中心点为原点构建基座标系oxyz、主工业机器人工作坐标系o₁x₁y₁z₁、从工业机器人工作坐标系o₂x₂y₂z₂、相机坐标系o_kx_ky_kz_k，构建过程如下：

定义工业机器人移动底盘起始位置为a点，以工作区域地图中心点o为坐标原点，当移动双工业机器人水平放置在a点时，以主、从工业机器人的底盘中心点o₁、o₂连线为x轴，垂直于o₁、o₂连线的方向为y轴，根据右手法则确定z轴方向，建立基坐标系oxyz；

以主机器人底盘中心o₁点为坐标原点，以o₁、o₂连线为x₁轴，与y轴平行方向为y₁轴，与z轴平行方向为z₁轴建立主工业机器人工作坐标系o₁x₁y₁z₁；

以从机器人底盘中心o₂点为坐标原点，沿x₁轴平移两机器人底盘中心距离建立从工业机器人工作坐标系o₂x₂y₂z₂；

以三维深度相机的摄像头视角中心k为坐标原点，与x轴平行同向为x_K轴，与y轴平行同向为y_K轴,与z轴平行同向为z_K轴建立相机坐标系o_kx_ky_kz_k；

起始位置a点在工作区域基坐标系oxyz下的位置坐标为(x_a,y_a,z_a),目标物体抓取位置b点在工作区域基坐标系oxyz下的位置坐标为(x_b,y_b,z_b)，目标物体放置位置c点在工作区域基坐标系oxyz下的位置坐标为(x_c,y_c,z_c)；

主、从工业机器人关节角用θ_ij表示，起始位置a点下的机器人各关节角度为0，处于零位状态，其中，i＝1,2，1表示为主工业机器人，2表示为从工业机器人，j＝1,2,…,M，M为机器人关节数目且M为正整数；

主、从工业机器人在目标物体抓取位置b点和目标物体放置位置c点时，由机器人末端目标点位置坐标的目标值p_oi在工作区域基坐标系oxyz下沿x轴、y轴、z轴的坐标分量p_oix、p_oiy、p_oiz和姿态坐标的目标值r_oi在工作区域基坐标系oxyz下绕x轴、y轴、z轴的偏航角μ_oi、俯仰角υ_oi、侧滚角ω_oi确定各组关节角的目标值θ_ij，在PC控制端设定主、从机器人各组关节角的目标值θ_ij，控制机器人各关节运动到各组关节角的目标值θ_ij，完成目标物体抓取和放置工作。

4.根据权利要求书1所述的一种基于多传感器融合的移动双工业机器人协同抓取方法，其特征在于，主、从工业机器人在初始位置、目标抓取位置和目标放置位置下的各关节角度θ_ij可一致也可各自调整为最优抓取、放置状态。

5.根据权利要求书2所述的一种基于多传感器融合的移动双工业机器人协同抓取方法，其特征在于，所述步骤2调试软、硬件设备具体包括以下内容：完成由激光式深度传感器、三维深度相机、全方位移动底盘、主从工业机器人组成的移动双机器人系统与PC控制端的数据通讯，使机器人系统和PC端在同一个局域网下，完成数据显示和实时控制；标定主、从工业机器人与深度相机的相对位置关系；在可视化环境中导入移动双机器人的URDF模型。

6.根据权利要求书2所述的一种基于多传感器融合的移动双工业机器人协同抓取系统及方法，其特征在于，所述步骤4包括：获取激光式深度传感器数据和确定传感器与机器人坐标系之间的坐标转换，运用SLAM算法获取里程计信息和观测到的激光信息来构建工作区域地图。

7.根据权利要求书2所述的一种基于多传感器融合的移动双工业机器人协同抓取系统及方法，其特征在于，所述步骤5包括：根据时间最短原则确定移动底盘到达目标抓取位置的最优路径，若路径无障碍物，移动底盘直接导航到达位置；当移动过程中激光雷达在路径上检测到障碍物体时，全方位移动底盘依次向左前方45°方向前行，直至成功越过障碍物，继续规划最优路径到达目标位置。

8.根据权利要求书2所述的一种基于多传感器融合的移动双工业机器人协同抓取系统及方法，其特征在于，所述步骤6包括：深度相机获取物体的深度图像，利用图像识别和检测算法得到物体的像素坐标，将物体的位姿从像素坐标变换到相机坐标系，再变换到主、从工业机器人工作坐标系下，相机拍摄图像还需识别异形物体形状，判断物体可抓取性，预测目标物体姿态，分析抓取区域，为其完成自主抓取提供决策。

9.根据权利要求书2所述的一种基于多传感器融合的移动双工业机器人协同抓取系统及方法，其特征在于，所述步骤10包括：只需要完成一次作业时，完成上述任务后，移动底盘返回至起始位置，同时，主、从工业机器人恢复到起始姿态，至此完成一次搬运作业；当需要移动机器人多次往复作业时，移动底盘直接回到目标物体抓取位置，重新规划主从工业机器人抓取目标后，移动至放置点位置放置物体，直至完成全部搬运作业，移动底盘再返回至起始位置，主、从工业机器人恢复到起始姿态。

Images