基于TwinCAT的视觉Delta3机器人应用与实现.pdf

1、1 基于 TwinCAT 的 视觉 Delta3 机器人应用 与 实现 王进 摘 要： 本研究基于 Beckhoff 的自动化平台 ,实现 Delta 并联机构 应用 程序开发。首先对 Delta 并联机构进行 简单 分析和 介绍 。 其次对 机器人怎么利用图像处理 技术 实现分拣和拾取做了比较和分析。 然后借助 Beckhoff 工业自动化控制平台自带的 编程 工具 ,将已确定的轨迹规划的在程序内部的进行实现。最后将开发的 Delta 并联机构轨迹规划的程序 和 视觉应用相结合 在 包装生产线上 进行实机的应用测试 ,测试的结果表明 Delt 机器人与 视觉 相结合 在 Beckhoff 平

2、台上实现方法的正确性及其性能满足 Delt并联机构高速平稳的指标。 0. 引言 在食品、包装等行业 ,通常需要机械手对物品进行高速、精确和平稳的完成诸如包装、分拣、封装等抓放操作。由于 Delta 并联机构具有刚度大、承载能力强、精度高且自重负荷比小等特点 ,在高速轻型机械手领域有广阔的应用前景。针对 Delta 并联机器人的应用领域对高速、精确和平稳 的 特点 ，在 高速轻型包装机械手领域有广阔的应用。 针对 Delta 并联机器人的这一特点，结合 视觉 图像处理技术的快速发展， 应用 基于 PC 控制技术的 TwinCAT 软件，归纳出 Delt 机器人的具体应用方法。 1. Delta3

3、 介绍 Delta3 机器人是一种典型的并联机器人，一般具有 3 自由度（ XYZ），如果配上旋转轴则可以达到 4 自由度。该机器人主要应用于食品包装及电子等行业，实现物品的定位抓取及按指定要求进行排列分 组及装箱（盒）等，其特点是快速抓取拾放，但基本只适合小负载物品。下图为实物图片： 2 其数学模型如下图，主要参数为： 上臂长度（ Inner arm length） 下臂长度（ Outer arm length） 底座半径（ Displacement） 抓手固定半径（ TCP Displacement） 2. 硬件配置 基于 beckhoff 的控制系统，针对于 Treiboots1100

4、机械手的主要配置如下： a).倍 福 CX10200113 高性能 控制器 b).倍 福 AX5206*2 伺服驱动器 c). 倍 福 AM80321E210000*3 电机（关节 电机） d).倍 福 AM80321D210000*1 （旋转 电机） 另外连接三根上臂电机的减速机的速比为 31:1，旋转轴的速比为 8:1。 其中 4 个伺服电机对应于三根上臂及旋转轴（ A,B,C,Add）。此配置目前只是应用于该款机械臂，对 于其他型号的 Delta3 机械臂，由于其重量长度等原因，电机的型号需另外计算。 3. 主要应用点介绍 3.1 寻零 对于机械臂实际上是不需要寻零的，因为其使用的电机都

5、为多圈绝对值编码器，上电直接读取当前编码器值就可以。但对于电机初次使用时需要进行零点标定工作，即将电机的零位匹配上机械臂的机械零位。参考其数学模型如下： 3 电机控制的机械上臂是根据角度进行控制的，其机械零点为上臂持平。因此在进行首次零位标定时，需要在外部的特殊机械装置的辅助下使电机达到某一固定位置，该位置可以是机械上的零角度位，也可以是其他任意某一角度；当机械臂到达该位置时将此位置的电机位置值设为该机械的角度值，并 保存，这样就完成机械零位的标定。 3.2 视觉的应用 对于 Delta3 机械臂的应用，往往需配合视觉系统，通过视觉检测位于输送带上的产品的坐标，通过总线传给控制系统，然后控制机

6、械臂去抓取。此应用一般采用平面视觉系统，主要有两种采集方式 ：线扫描和区域扫描。 1） 线扫描 视觉系统检测其正下方的一条线，随着输送带的行进，对输送带逐行进行扫描；这样的视觉系统需要输送带的位置信号来完成检测，一般在输送带上安装位置编码器，并将编码器信号反馈给视觉系统。 采用这样的视觉系统基本可以不用考虑区域扫描方式所产生的下述问题，但这种方案相对成本较高 2） 区域扫描 这是常用的视觉采集方式，主要通过外部信号触发，对镜头正下方的区域进行抓拍，进行产品处理分析；对于区域扫描的方式，有几个问题需要注意：漏检测及重复检测，下面通过图片说明 4 图一）如图所示，蓝色边框表示输送带，运行方向为从右

7、往左，圆圈表示输送带上的产品，红色方框表示视觉的拍摄区域，通过字母表示其随着输送带的运行其在输送带上的拍摄区间； 在此图中是皮带每前进一个拍摄区域的长度就进行一次抓拍，这种方式虽然可以将输送带上的所有区域都拍摄在内，但可以发现产品 3 和产品 5 正好处于两次拍摄的中间，结果是该产品将无法被检 测出来，因为视觉必须检测到完整的产品才会认为是有效的。所以这样的抓拍方式是不对的，会产生产品的漏检测。 图二） 在此方式中，抓拍的间距设为拍摄区域的 1/2。可以看到，产品 3 在抓拍 a 和抓拍 c 中是不认的， 但在抓拍 b 中将会被确认，同样的是产品 4 和产品 5，该方式可以避免产品被漏拍；但会

8、产生另外一 个问题，即产品被重复拍摄。比如产品 2，它即在抓拍 a 中被确认，同时又在抓拍 b 中被确认，同样的问题会产生在产品 6 和产品 7 上，对于这样的问题，需要在程序里通过比较输送带的位置进行产品剔除。 基本上对于区域扫描的方式重要保证拍摄的重叠区域不小于产品的长度就能保证不会产生漏拍得 现象，但无论如何重复拍摄是始终会存在的，需要在程序里进行处理。 5 4. 程序介绍 4.1 System Manager 的配置 在 System Manager 中需要进行机器人的配置及伺服轴的配置，其中 Axis_A， Axis_B， Axis_C 和 Axis_Add 为实际轴，对应于实际中的

9、三个上臂轴及中间的旋转轴。轴 Axis_X， Axis_Y， Axis_Z，Axis_Ma， Axis_XBT， Axis_BeltBt， Axis_Belt_Infeed 为虚拟轴，其中 X,Y,Z 为实际控制抓 手的空间位置轴， Axis_Ma 为空间曲线的凸轮主轴， Axis_XBT 和 Axis_BeltBT 用于进行皮带跟随控制， Axis_Belt 为模拟实际的输送 皮带，在实际应用中为实际轴。 4.1.1 机器人及 Nci 的添加 右击 NCTask 添加 NCChannel（ for Interpolation），命名为 Nci。 6 右击 NCTask 添加机器人 chann

10、el，命名为 Kin。 4.1.2 导入机器人库 右击 Kin 下的 Group2，点击 Append Object。 7 进入 TwinCAT 目录下 Io 文件夹，找到 TcNcKin.tmc 并打开。 在弹出对话框中选中如下选项，点击 Ok。 8 4.1.3 机器人参数配置 主要设定机械臂的数据，来建立数学模型： 4.1.4 添加轴： Axis_A， Axis_B， Axis_C， Axis_Add， Axis_X， Axis_Y， Axis_Z， Axis_Ma， Axis_XBT， Axis_BeltBT， Axis_Belt_Infeed。 将轴 X,Y,Z,Ma,XBT,Belt

11、BT 的参数 “ Allow motion command to slave axis” 设为 TRUE，此设置是为了 PLC 程序中一些功能的实现。 在此该 System Manager 程序主要用于模拟，实际中需将每根轴连接至实际电机，并对电机进行相应的参数设置。 9 4.2 PLC 程序 在此将主要介绍 PLC 程序，分两部分： 1.先介绍一个样例程序，主要用于模拟机器人的运动控制； 2.再对其中的主要控制功能（空间运动及皮带跟随等）做详细解释。 4.2.1 样例程序 1. 新建 PLC 程序 程序命名为 Robotic，添加库文件： TcMc2.lib TcMc_Camming.lib

12、 TcNcKinematicTransformation.lib TcRobot2012.lib 1） 建立轴变量（全局变量），与之前的 tmc 程序中的轴相对应。 2） 编译程序，并在 System Manager 中导入程序，完成变量链接。 2. 程序架构内容 程序主要内容的编写过程不再此详细说明了，可以参考直接参考程序内容，以下主要介绍各程序的架构。 1） Main 主程序 10 如图，主程序是基于 SFC 语言编写的，在 PLC Task 中只有主程序是被调用的，其余所有的子程序都在主程序中被调用。 2） Power 在主程序的起始步中调用子程序 SR_00_Power，主要进行所有轴

13、的使能，当所有轴使能完成后主程序当前步跳到下一步。 3） KinGroup 主程序在使能完成后跳转到 KinGroup，在该步调用子程序 SR_01_KinGroup，该程序完成机械臂数学 模 型的建立，将实际轴（ A,B,C）经过 Transformation 转换为空间轴（ X,Y,Z），当模型建立后 进行跳转。 以上所介绍的 Power 和 KinGroup 在程序中只执行一次，命令完成后即跳出该步；但下面介 绍 的所有步是程序主要执行的内容，且都处于同一层面。 4） Mode 对于设备的控制主要分两个模式：手动（ Manu）和自动（ Auto），该步主要用于对模式的切换，通过调用子程序

14、 SR_02_Manu 进行控制。模式切换时，只有在退出当前模式下才能激活其他模式，通过模式的 Start 和 Stop 执行激活及退出。 11 5） Manu 通过调用子程序 SR_03_Manu 进行对空间轴的手动控制。 6） Auto 在 Auto 中调用子程序 SR_04_Auto，关于自动运行的程序在此被执行，由于是模拟程序，只有一个关于机械臂运行空间点到点的自动程序，关于自动运行的内容在后面解释。 7） Belt 在此步中调用子程序 SR_06_Belt，主要对输送带进行控制。实际应用中输送带可以是由伺服控制，也可以由一般变频控制，但需要在外部加以编码器并反馈到控制器。 8） St

15、atus 在此通过调用 SR_05_Status 子程序进行对每根轴进行监控 3. 在线调试 在 Visualization 中建立了 TC_VISU 画面，可以对 Delta 机器人进行简单的控制，比如手动控制自 动控制，及显示机械抓手的当前空间位置。 12 4.2.2 机器人的运动控制 此部分内容主要介绍机器人空间运动是如何实现的，主要是三部分 1. 函数 FC_PickandPlace_Splinecalc 的介绍， 2.子程序 Robot_Spline_V3 的介绍， 3.子程序 SR_RobotMotion 的介绍 1. 函数 FC_PickandPlace_Splinecalc 1

16、） 该函数可以根据设定的两个空间坐标点（ X,Y,Z）并给点空间抛物线的顶点高度， 自动生成一条连接两点的空间抛物线，以一组三维数组的形式给出反馈。其本质是空间的样条曲线 2） 输入变量 i_stStartPos：空间抛物线的起点坐标变量（ KoordinateType 类型变量） i_stEndPos：空间抛物线的终点坐标变量（ KoordinateType 类型变量） i_lrAbsHeight：空间抛物线最高点的 Z 值 3） 输入输出变量 13 iq_stExtendedDat：扩展参数（ ROB_ST_Splinecalc 类型变量） iq_alrSplineTable：生成的空间路

17、径数据表，三维数组 ROB_ST_Splinecalc： diNumberOfSplinePoints：定义空间曲线特征点的数量 diNumberOfVerticalStartPoints：定义起点处垂直高度的特征点数量 diNumberOfVerticalEndPoints：定义终点处垂直高度的特征点数量 xUserApex：使能参数 lrApexScalingFactor 在曲线生时的作用 lrApexScalingFactor：定义抛物线最高点的在 XY 平面上相距中心点的偏移（ 11） lrApexSmoothZone：在中心点周围，处于该区域内的特征点抹去，起到平滑作用 lrXYDi

18、stanceStartToObstacle：起点处空间障碍物在 XY 平面离起点的直线距离 lrZDistanceStartToObstacle：起点空间障碍物的高度 lrWidthObstacleStart：起点空间障碍物的宽度 lrXYDistanceEndToObstacle：终点处空间障碍物在 XY 平面离终点的直线距离 lrZDistanceEndToObstacle：终点空间障碍物的高度 lrWidthObstacleEnd：终点空间障碍物的宽度 diStartSmoothPoints：起点平滑特征点数 diEndSmoothPoints：终点平滑特征点数 lrMinVertica

19、lDistanceStart：起点最小垂直距离 lrMinVerticalDistanceEnd：终点最小垂直距离 lrVertStartPointsDist：起点垂直特征点数 lrVertEndPointsDist：终点垂直特征点数 stFeedback：反馈值 对于该函数的具体使用，会在下面的实际应用中再解释 2. Robot_Spline_V3 该程序将函数 FC_PickandPlace_Splinecalc 生成的空间样条曲线分解到空间 XYZ 轴，通过凸轮耦合到虚拟主轴 Axis_Ma，启动 Axis_Ma 运行 PTP，则可以实现机械臂在空间走出一条抛物线。关于每根轴之间的关系，

20、如图： 14 轴 Ma 为空间运行的主轴，从轴分别为 XBT， Y， Z，其凸轮曲线为前述所分解的样条曲线；旋转轴 Add 的凸轮曲线点为 MotionFunctionPoint 类型，根据其运行前的当前角度和运行的目标角度来生成。在此需说明的是关于 XBT 及 BeltBT 的作用，其中 XBT 是通过样条曲线耦合到 Ma，然后叠加皮带的运行后同时作用到 X，就可以实现皮带跟随功能，当不使用皮带跟随功 能时通过程序变量可以将 BeltBT 屏蔽掉，这样就等同于 Ma 直接作用于 X。 1） 输入变量 i_xEnable：程序使能 i_lrRobSpeed： Axis_Ma 运行速度（即机器人

21、运行速度） i_xBeltTrackEnable：皮带跟踪功能使能，当该变量为 true 的情况下执行程序，则将进行皮带跟踪；否则不作皮带跟随动作 i_xBeltSyncStop：当在作皮带跟随动作时，该变量用于进行取消皮带跟随动作 i_uiBeltID：作皮带跟随时，指定跟随皮带的编号 i_stAddData：旋转轴数据设定（ ST_Add 类型变量） ST_Add： xAddEnable：使能旋转轴，使其跟随主轴 Ma 进行旋转 15 lrCurrentPos：指定旋转轴起点时的角度 lrTargetPos：指定旋转轴终点的角度 lrMasterOffset：设定旋转起始点，相对于主轴的偏

22、移（ 01，即指定在主轴运行路径的何处开始启动旋转） lrMasterFactor：设定旋转所需的主轴运行位移，即在此设定范围内旋转轴旋转到位 注意：（ lrMasterOffset+ lrMasterFactor）需小于 1 2） 输出变量 q_xActive：程序执行标志 q_xDone：程序执行结束标志（包括跟随动作完毕） q_xInPosition： Ma 运行到位标志（即机器人运行完成，但仍在执行皮带跟随；当不执行皮带跟随时，该变量将与 Done 信号几乎同时为 true） q_xRobMoving： Ma 运行过程标志位 q_xError：程序报错 q_eRobStatus：机器人

23、运行状态 q_sResult：未使用 3） 输入输出变量 iq_stExtData：扩展变量（主要设定皮带跟随参数，ROB_ST_ExtData 类型变量） iq_stSplineExtData：扩展变量，取自函数 FC_PickandPlace_Splinecalc iq_alrSplineTable：空间曲线 ROB_ST_ExtData： astBeltData :ARRAY 12 OF Rob_BeltData（皮带跟随数组） stZUpSetData : ROB_ST_ZUpData（皮带跟随提升动作参数） Rob_BeltData: stAxisBelt：轴变量（ Axis_REF

24、） lrBeltTrackMaxDis：皮带跟随的最大值，当跟随距离超过该值后自动脱离跟随 lrBeltVektor：皮带跟随基于 X 轴的矢量方向（如果与 X 轴正方向一致则该值为 1；如果与 X 轴反向则为 1） 16 stZUpSetData： xZUpEnable：垂直运行使能 lrZUpDisAbs：运行高度 3. SR_RobotMotion 在该段程序中将对机器人的运动进行逻辑编程，其逻辑顺序如下： 1） 初始化设定 17 在初始化设定中有两个 Action 程序： ExtData_Init 和 Pos_Init。前者主要设定机器人所要跟随的皮带轴的变量及跟随参数的设定；后者用于

25、设定空间坐标点的数值（由于是模拟程序， 所以是定值，实际中放置及拾取的位置时根据实际情况变化着的） 2） 运行启动 当启动后，首先将 Z 轴提升至运行最高点（并非最高限位，而是项目运行需要所设定的最高点）；然后将 X,Y 运行至零点位置，作为等待位置。变量 stSplineExtDat.lrApexScalingFactor:=1，由于起始是从最高点开始的，所以所要生成的抛物线中的最高点是位于起点位置，因此该值需要设为 1；如果是终点在最高点，则该值要设为 1 ；正常两点之间运行时，最高点处于中间位置，所以需要设为 0 。变量 stSplineExtData.diNumberOfSplineP

26、oint 是用来定义曲线中特征点的数量，在此定义为 81，该值不能超过 98。 3） 启动循环运行 变量 g_xRobEnable 为机器人循环运行标志，该变量为 true 时机器人就在两个设定点之间作来回运行，相当于做拾取及放置的动作；当该变量在运行时变为 false 时，机器人将在完成当前放置的动作后回归零位（ stPos） 4） 运行至 Pos1，同时进行皮带跟随 将当前坐标值赋给 stCurrentPoint，目标坐标值赋给 stTargetPoint，然后执行函数 FC_PickandPlace_Splinecalc，当函数返回 0 时表示执行完成；同时由于需要进行做旋转动作，因此对

27、旋转轴运行进行使能，并设定参数 18 判断 Robot_Spline_V3 是否为运行，如果为 false ，则启动机器人运行；当变量 xRobBeltTrackEnable 为 true 时表示此次运行将对皮带进行跟随；当变量 q_xInPosition 为true 时表示机器人运行到位了，但仍进行皮带跟随中，此时启动抓取延时，当延时到达时，将变量 xBeltStop 置为 true，停止皮带跟随；当检测到 q_xDone 后表示运动执行完毕。 5） 运行至 stPos3，不作皮带跟随 生成从当前位置到 stPos3 的曲线 启动运行，当 q_xDone 为 true 后开始做放置延时，延时

28、到达后结束 6） 判断当前运行条件 判断变量 g_xRobEnable 是否为 true，如果为 true 则返回 iState=100，将机器人运行至 stPos1，并跟随皮带，实现循环运行；如果为 false 则需将机器人运行至原点进行等待 7） 运行至 stPosZero 生成曲线 启动运行，到达位置后程序跳转至 iState=30 或 iState=0 19 5结论 实践 表明基于 TwinCAT 软件把 Delt 机器人技术与图像处理 技术相 结合， 非常适合 应用于包装或其他生产线实现装箱 、分拣、分拣 等具体应用， 发挥出 Delt 机器人精度高 、速度快、承载 能力强 、刚度 大的优势 ， 具有广阔的市场应用前 景 。