基于ROS的工业机器人及AGV的视觉协同控制系统*

2023-11-28樊虹岐陈海军吕丞干胡晓兵

组合机床与自动化加工技术 2023年11期

樊虹岐,陈海军,2,3,吕丞干,胡晓兵,3

(1.四川大学机械工程学院,成都 610065;2.四川德恩精工科技股份有限公司,眉山 620460;3.宜宾四川大学产业技术研究院,宜宾 644000)

0 引言

由于全球市场竞争日益激烈,制造行业面临着更高的产品质量和生产效益、更低的生产成本和资源消耗等严苛要求[1]。为了应对上述挑战,制造业借助于信息技术的快速发展,不断革新制造技术,提高生产系统的自治性和协同性,逐步向“智慧工厂”转变[2]。作为智能工厂柔性制造单元的核心组成,机械臂、自动引导车(automated guided vehicle,AGV)等工业机器人构成的自动化上下料系统已成为推动智能工厂实现智能制造的关键技术[3]。但是该系统在开发难度、多机器人协同管理等问题上需要的较大成本投入,这导致多数制造企业的上下料系统仍处于半无人化水平,高度智能化上下料系统难以推广应用,相关技术还需要进一步研究和完善。

当前,自动化上下料技术受到了国内外学者的广泛研究,其实现方式可以分为两类。第一种方式借助激光雷达与反光标志物[4-5]、二维码标志物[6]、全局双目视觉[7]或多传感器融合[8]等方式,实现AGV的高精度定位,将工件准确运输至机械臂的示教点处实现自动化上下料;第二种自动上下料技术则是借助力或视觉等传感器[9-10],实现机械臂的柔性化,动态更改上下料点位补偿AGV的定位误差。上述两种自动上下料技术仅考虑了单一机器人的控制,但机器人之间缺乏配合,智能化程度仍然受限。针对该问题,吕开旺等[11]设计了AGV与机械臂结合的上下料系统,AGV与机械臂之间通过以太网连接,实现了两机器人简单的协作配合。然而,机械臂与AGV仍然由各自独立的控制系统驱动,需要在此基础上建立额外的协同系统,成本高,维护困难,因此需要一个统一的系统控制并协调多机器人实现自动化智能化的上下料。

针对上述问题,本文以被广泛用于机器人开发的ROS系统为平台[12],降低开发难度,提高系统扩展性[13];采用单目全局视觉[14]作为传感器并使用AR Tag作为标志物,研究低成本高稳定性的机械臂和AGV同一视场视觉识别[7];并基于ROS开发了多机器人上下料视觉协作系统,实现了工作区间任意指定位置的上下料接驳,避免了传统固定式上下料托盘与AGV精确定位的难题,为自动化上下料技术提供了一种低成本、易维护、高扩展性的解决方案。

1 平台方案

本文设计的上下料视觉协作平台如图1所示,包括六自由度工业机械臂,差速AGV,标志物,桁架以及2D相机。机械臂由控制柜控制,通过网线与系统连接;AGV搭载ROS系统,通过无线网络与系统连接;二者均可以在工作空间内自由移动,并固定了ArUco Tag标志物用于视觉识别,其中为了确保在遮挡情况下也能正确识别AGV的位姿,在AGV的四角均贴有相同的标志物。相机分辨率为1920×1080,固定在3 m高的桁架上形成全局视觉,确保视场覆盖整个工作空间,能够同时观测到机械臂与AGV。

图1 上下料视觉协作平台

基于该平台搭建的控制系统结构如图2所示,主要由图像处理单元、协同节点、机械臂控制节点和AGV控制节点四部分组成。该系统基于ROS开发,系统中每个功能由一个或多个节点程序实现,添加与维护功能节点只需要添加或修改一个或几个节点,因此系统维护简便,扩展性能好。图像处理采用优化后的ArUco Tag识别算法,将识别到的机器人位姿发送给协同、机械臂控制与AGV控制节点。协同节点根据机器人当前位姿和协同算法计算出机械臂与AGV的目标位姿,并分别发送给机械臂与AGV控制节点。AGV控制节点收到AGV的当前位姿与目标位姿后,计算出左右轮的运行速度并通过主从机通信发送给AGV,以控制AGV运动。机械臂控制节点收到机械臂的当前与目标位姿后将运动指令发送至move_group节点进行运动学解算,解算结果通过TCP/IP通信发送至机械臂控制柜,控制机械臂运动。

图2 上下料视觉协作系统结构

2 平台设计与实现

2.1 基于ArUco Tag的视觉识别

2.1.1 视觉识别

自动化上下料需要机械臂与AGV多机器人之间协同工作,为了能够正确引导工作空间内的机器人执行各自工作,系统的视觉识别节点需要同时识别多个不同的目标,并且获得多个目标各自的位姿信息。因此,本文采用ArUco Tag作为辅助视觉识别的标志物。ArUco Tag[15]是一种用于机器人定位的方形标志物,可以计算出标志物的6自由度信息,而且每一个标志物都对应唯一的ID号,因此可以使用ID区分不同机器人。适用于本文系统中工作空间内多机器人识别与定位。由于噪声等因素的影响,单个标志物的方形轮廓可能被重复检测两次,得到两个ID相同的识别结果。为了避免上述现象,在识别到两个相同的ID结果时,会去除掉其中一个。但由于本文中AGV存在多个ID相同的标志物,因此该方法并不适用于本文中的AGV识别,其识别效果如图3a所示,并不能准确识别出所有标志物。因此本文对重复检测的判断逻辑进行了优化,改进后的ArUco Tag识别算法流程如图4所示。

(a) 去重算法改进之前,只能识别到左侧两个标志物,右侧的标志物被视为重复检测而被去除 (b) 去重算法改进之后,可以同时识别4个ID相同的标志物

图4 改进后的ArUco Tag 识别流程

该算法核心在于添加了两结果是否相交的检测,只有当两结果中心点距离大于两结果矩形轮廓的任意边长时,表明这两个相邻结果并不相交,是识别两个ID相同的标志物得到的,两个结果均保留。基于上述改进的ArUco Tag识别算法,在不重复检测到单一标志物的同时,能够正确识别出工作空间中多个相同ID的标志物。效果如图3b所示。

2.1.2 位姿计算

通过上述识别算法可以得到标志物4个角点在图像中的像素坐标,像素坐标与世界坐标系的转换关系如图5所示。

图5 图像坐标转换关系

图5中对应转换关系式为:

(1)

式(1)中内参矩阵为:

(2)

式中:fx和fy分别为x轴、y轴方向上的尺度因子,(cx,cy)、(u0,v0)均为光轴对应的像素坐标,dx、dy为像元尺寸,f为相机焦距。内参矩阵可以通过内参标定方式获得,本文采用张正友标定法[16],标定流程简单,精度高,标定过程如图6所示。标定完成后,获得内参矩阵为:

图6 相机内参标定

(3)

(4)

式中:R3×3为旋转矩阵,T3×1为平移矩阵,二者表示标志物在相机坐标系下的六自由度位姿信息。外参矩阵基于Levenberg-Marquardt迭代估计标记姿势[17]得出,使得外参矩阵的重投影误差最小。

2.2 机械臂识别与控制

机械臂控制节点如图7所示中虚线框内所示。每个控制周期中,首先由驱动节点接收来自机械臂识别节点识别得到的机械臂当前六自由度信息,以及协同节点发送的目标位置信息,取差值作为控制信号发送给MoveIt,并由MoveIt通过逆运动学求解出目标关节角度。通信节点接收到目标关节角度后,将其转化为动态指令,通过TCP/IP通信发送给机械臂,以实现机械臂的运动控制。同时通信节点通过动态指令查询机械臂当前六关节角度,并将关节状态同步给MoveIt。直到机械臂运动到目标位置允许误差范围内,或者目标点发生更改时结束伺服,开始新的控制周期。

图7 机械臂控制流程图

2.2.1 机械臂识别与手眼标定

本文采用全局视觉的方式识别机械臂上特定ID的ArUco Tag标志物,实现对机械臂的识别,并在此基础上指导机械臂运动到指定位置。为了确保位置控制的精确性,首先需要进行手眼标定,获得相机坐标系到世界坐标系之间的坐标转换关系,如图8所示。

图8 机器人坐标系

图8中坐标系存在如下转换关系:

(5)

在标定过程中,机械臂不断改变位姿,同时相机记录不同的机器人位姿下,ArUco Tag的六自由度位姿信息,有:

(6)

由式(5)和式(6)可得:

(7)

整理可得:

(8)

通过上式即可求出相机坐标系到世界坐标系之间的坐标转换关系,为了确保转换关系唯一,机械臂至少需要改变3次位姿,且位姿改变次数越多,得到结果越精确稳定。本文经过图9所示手眼标定得到的转换关系为:

图9 手眼标定过程图

(9)

2.2.2 通信节点

MoveIt是ROS系统中包含的机器人相关工具集,可以有效地实现机械臂的路径规划与逆运动学求解。由于机械臂本身为独立的系统,无法直接与ROS系统连接以获取MoveIt的计算结果,因此为了实现ROS系统对机械臂的有效控制,本文搭建了机械臂与ROS系统之间的通信节点。

通信节点作为连接控制系统与机械臂桥梁,负责将得到的机械臂目标关节角发送至机械臂,同时将机械臂当前关节角反馈回控制系统,从而在不更改机械臂原有功能的前提下,实现控制系统对机械臂的有效控制。

(1)通信方式。通信节点与机械臂之间采用TCP/IP协议,使用socket套接字连接,实现了机械臂与系统之间可靠高效的信息交互。通信中使用的动态指令采用JSON格式,结构如下:

{"token":"googoltech123","post":"Get1":"1"}

动态指令主要分为两个部分,前半部分为“token”口令作为校验位,校验通过了才会执行“post”指令;后半部分为“post”指令,用于查询机器人状态或控制机器人执行相关功能。

(2)发送控制指令。通信节点采用TCP/IP通信,将设定好的动态运动指令发送至机械臂,具体流程如图10所示。

图10 动态指令发送流程

2.3 AGV识别与控制

AGV作为重要的运输设备,在上下料工作中不可获缺。为了发挥全局视觉的优势,确保AGV在工作空间中正确运行,系统在AGV进入工作空间后对其进行识别,并控制其运行。整个AGV控制系统的流程图如图11所示。

图11 AGV控制系统流程图

2.3.1 AGV识别

AGV作为物料搬运的重要组成部分,在自动化上下料中不可或缺。为了提高工作效率,同一工作空间内可能会存在多台AGV,此外,由于全局相机视角固定,在工作过程中可能会出现机械臂遮挡部分AGV的情况。上述情况给AGV的识别与控制带来了困难。为此,本文将相同ID的4张标志物按照同样的位姿固定在AGV四角,不同的AGV之间使用不同ID的标志物区分,提高了识别的准确性及稳定性,同时可以得到AGV托盘的准确范围。

通过识别标志物,可以得到每个标志物的物体坐标系,该坐标系与标志物相对固定(如图4中坐标系),因此可以根据坐标系判断AGV的朝向。AGV的识别算法流程图如图12所示。

图12 AGV识别算法流程图

为了区分不同AGV之间的识别信息与位姿信息,两种信息的格式如图13所示。

图13 AGV识别信息与AGV位姿信息格式

2.3.2 AGV控制

(1)AGV运动模型。差速AGV的运动模型如图14所示。

根据上图运动模型可以得到AGV的运动方程[18]:

(10)

式中:vr、vL分别为AGV的右轮和左轮转速,(xt,yt,θt)为目标位姿,(xc,yc,θc)为当前位姿,ΔT为从当前位姿移动到目标位姿需要的时间。上式中左右轮速度会随当前位姿与目标位姿间的距离变化,不利于AGV的稳定运行,因此可以将式(10)中的AGV前进速度设为固定值u,则左右轮速度为:

(11)

(2)AGV运动路径。相比于AGV系统内的传感器及路径规划算法,系统依靠全局视觉可以准确快速识别出AGV在当前工作空间中的位置,并规划出在当前工作空间中更加合理的运动路径。因此,当AGV进入工作空间后,其控制权由系统接管,并根据系统规划的路径运动。为了能够让AGV平稳地从当前位姿移动到目标位姿,考虑到工作空间中不存在障碍物,本系统采用光滑的贝塞尔曲线算法对AGV的运动路径进行规划,如图15所示。

图15 AGV的运动轨迹中间点

图16 本文中协同节点的工作流程

AGV的当前位姿与目标位姿确定了两个位置点P0=(x0,y0)、P2=(x2,y2)以及朝向θ0、θ2从两点沿朝向做延长线交于一点,得到P1(x1,y1),有:

(12)

以P0、P1、P2三点作为控制点做二次方贝塞尔曲线,可以得到一条平滑的运动轨迹,使AGV从当前位姿移动到目标位姿。该曲线根据下式得出:

B(t)=(1-t)2P0+2t(1-t)P1+t2P2,t∈[0,1]

(13)

式中:t=kh,k∈{0,1,…,n},h为步长。曲线上任一点切线的朝向角度为:

θb=artan(B′(t))=arctan((2t-2)P0+
(2-4t)P1+2tP2)t∈[0,1]

(14)

通过上式计算得到中间点{Pb1,Pb2,…,Pbn}及其对应的切线方向{θb1,θb2,…,θbn},将中间点的位姿作为目标点带入式(14)即可得出左右轮的速度。

(3)AGV控制规则。多个ROS系统之间可以通过主从机的方式通信,AGV作为从机接入多机器人控制系统,可以直接获取主机及控制系统发布的信息(图13)。AGV在运行过程中接收识别节点发送的识别信息,并与自身ID核对,若与自身ID相同,则进一步核对识别标志位,若标志位为真,则判断自身被系统成功识别,并开始接收系统发布的控制信息,按照控制信息中的左右轮速度运行。若发布ID与自身ID不同或标志为假,则按照预定规则运行。

2.4 工作区域识别与协同控制

协同节点的主要作用根据当前的工作内容设定协同方式,并按照协同方式将目标位姿发送给机械臂与AGV的控制节点,控制机器人按照预定方式协同工作。本文中,在AGV进入动作空间后,系统根据AGV的ID判断当前任务,并根据任务计算机械臂与AGV的目标位姿,然后控制机械臂与AGV同时向目标区域移动,以实现机械臂与AGV的初步定位,便于后续分拣工作的开展。

2.4.1 工作区域识别与工作任务判断

通过矩形识别得到对应的工作区域的4个角点,取角点坐标的均值作为中心点(xa,ya),并取相邻两点中距离最大的边作为长边,计算长边与水平线夹角θa,将中心点坐标与夹角发布至协同节点。

在实际生产过程中,不同的AGV进入工作空间执行的任务不同,其目标位置与协同方式也不同。在AGV进入工作空间后,系统通过AGV识别节点获取其ID,根据ID查询其预先设定好的工作任务,根据工作任务获取机械臂与AGV的工作区域、机械臂与AGV的启动时间与运行速度等协同配合信息,并开始计算AGV与机械臂的目标位姿。

2.4.2 机械臂与AGV目标位姿算法

(1)AGV目标位姿计算。在实际控制中,若将工作区域信息直接作为AGV目标位姿,则AGV无法正确运行至工作区域内,这是由于相机的小孔成像原理导致现实中同一点在不同高度下会投影在图像的不同位置,如图17所示。

图17 相机成像原理与目标点偏移

工作区域中心点A投影在图像中a处,若以该点作为AGV的目标位姿则AGV运动之B′点,与预期不符。AGV的真正目标位姿位于B点,其投影在图像b处。因此为了使AGV能够正确的运行到工作区域中,需要将b点位置计算出来,得到AGV的目标位姿(xb,yb,θa),计算公式如下:

(15)

式中,(cx,cy)、(u0,v0)均为光轴对应的像素坐标,H为相机镜头至地面的高度,h为AGV的高度。

(2)机械臂目标位姿计算。通过工作区域的识别与前文中目标点的计算得到了目标位姿的像素坐标(xb,yb,θa),由于机械臂控制需要目标位姿的六自由度信息,因此需要对上述像素坐标进行转换,进而得到机械臂的目标位姿(Xc,Yc,Zc,Qc):

(16)

式中:(Xc,Yc,Zc)为相机坐标系下目标点的XYZ坐标,Zc为预先设定值,fx、fy为尺度因子,(xb,yb)为目标点像素坐标,可以根据协同方式更改,此处协同方式为机械臂末端与AGV中心位于图像中相同位置。

(17)

式中:Qc为相机坐标系下目标点四元数,其等式右侧乘法顺序根据实际绕轴旋转的顺序而定,在本文中旋转是沿着XYZ轴依次旋转,因此Qx,Qy,Qz的以此右乘;α、β、γ分别为XYZ轴旋转角度。

3 实验结果及分析

为了验证系统是否能够实现多机器人之间的有效协同,本文设计了AGV与机械臂的模拟上下料实验。工作空间中划分出了3个区域,分别对应AGV的启动区域、上料区域和下料区域。启动区域表示AGV进入工作空间,由系统接管控制,上料区域为AGV1(ID:103)的目标点,下料区域为AGV2(ID:104)的目标点。

在实验过程中,系统将自动识别工作区域的中心位置以及AGV的标志ID,将机械臂与AGV引导至ID对应的工作区域。为了便于结果的直观展示,多机器人的引导配合以图像中的配合为基准,即机械臂末端与AGV中心位于图像中相同位置。

3.1 实验流程

AGV ID为103,位于初始区域,机械臂处于初始姿态。系统控制AGV与机械臂移动至与AGV ID对应的工作区域,记录从开始到二者均移动到位所花费的时间。如图18所示。

(a) 两机器人同时启动 (b) 两机器人运动过程中,系统在遮挡情况下仍能正常识别AGV

更换AGV的标志物ID为104,并更改AGV判断程序,模拟另外一台AGV进入工作空间。将AGV和机械臂移动至初始区域,并由系统控制二者同时移动至对应的工作区域,记录多机器人协作系统完成初始定位花费的时间。如图19所示。

(a) 两机器人同时启动 (b) 两机器人运动过程中,机械臂遮挡了AGV的部分标志物,系统仍能成功识别出AGV的ID、中心点和朝向

3.2 实验结果

实验结果如表1所示。多次上下料时间数据表明,控制系统工作稳定可靠,可以有效地识别工作区域。机械臂与AGV标志物,并且能根据标志物ID正确地引导机械臂与AGV移动到目标区域,实现任意指定位置上下料的协同。在运行过程中,即使两机器人之间存在遮挡关系,系统仍然能够根据剩余标志物正确的计算出当前AGV的位姿并引导其继续运动,并实现与机械臂的配合。这说明系统在控制多机器人协同上下料上有较好的抗干扰性。因此基于该系统,工件托盘可以通过AGV在任意指定位置灵活放置,提高了整个工作空间的柔性,避免了传统上下料中,AGV需要精确定位的问题。证明了该工业机器人与AGV的视觉协同系统能够在上下料的场景中正确的实现多机器人的稳定识别、控制以及协同。