3自由度转臂式清洗机器人一级臂定位控制*

潘海鸿1，涂晓晓1，陈琳1，龚建刚1，王盛2，陈剑华2

(1.广西大学 机械工程学院，南宁530004；2.广西叶茂机电自动化有限责任公司，南宁530007)

摘要：针对自主研发的一种新型3自由度转臂式清洗机器人,进行一级臂精确定位的控制问题的研究。提出控制系统采用“PC机+运动控制器+中心回转编码器”的位置闭环控制方案，并引入回转定位插补算法实现对一级臂回转定位的控制。构建基于PMC6496运动控制器的3自由度转臂式清洗机器人实验平台实现一级臂的精确定位控制，实验结果表明转臂式清洗机器人一级臂的回转定位精度可达到±1 mm，验证了所提出的控制方案和回转定位插补算法能有效避免回转定位精度差的问题。

关键词：回转定位；转臂式结构；运动控制器；闭环系统

文章编号：1001-2265(2015)09-0071-03

收稿日期：2014-11-27

基金项目：*国家自然科学

作者简介：潘海鸿(1966—)，男，壮族，广西武鸣人，广西大学教授，博士生导师，博士，研究方向为数控技术、机器人控制技术，(E-mail)hustphh@163.com；通讯作者：陈琳(1973—)，女，山东青岛人，广西大学教授，研究方向为数控技术、数字信号检测与处理、伺服电机控制，(E-mail)gxdxcl@163.com。

中图分类号：TH165;TG659

3-DOF Rotary Arm Cleaning Robot First Rotary Arm Rotation Positioning Control

PAN Hai-hong1， TU Xiao-xiao1， CHEN Lin1， GONG Jian-gang1， WANG Sheng2， CHEN Jian-hua2

(1.College of Mechanical Engineering, Guangxi University, Nanning 530004, China; 2.Guangxi Yemao Electromechanical Automation co., LTD, Nanning 530007, China)

Abstract：To solve the first rotary arm control problem of an independent research and development novel 3-DOF rotary arm cleaning robot, a solution adopted "PC + motion controller + center rotary encoder" closed loop position control system is proposed. A full closed loop rotary positioning interpolation algorithm is adopted to control the first rotary arm angle. 3-DOF rotary arm cleaning robot experimental platform is constructed based on PMC6496 motion controller to realize the first rotary arm position control. Experimental results demonstrate 3-DOF rotary arm cleaning robot first arm rotary positioning accuracy is±1mm. It indicates that the proposed closed loop position control solution and rotary positioning interpolation algorithm can effectively avoid the precision problems of poor positioning accuracy.

Key words： rotary positioning; rotary arm structure; motion controller; closed-loop system

0引言

蒸发罐被广泛应用于食品、化工、医药等行业的蒸发浓缩过程，其内部竖直分布着多达上万条换热管。在蒸发浓缩过程中换热管内壁极易结垢，而结垢会使其传热效率大大降低、能耗增加[1-2]，导致其蒸发能力下降。因此，为恢复蒸发罐正常生产能力，需要对其进行定期清洗除垢[3]。

目前国内外针对蒸发罐除垢清洗设备和装置展开相关研究。马乐凡[4]等开发的清洗装置需人工方式定位清洗换热管；英国斯科特[5]公司在中国市场推出的半自动清洗机需人工方式去同时定位三个换热管，而后进行自动清洗；农谭宏[6]等开发的清洗机，自动定位清洗换热管，其由于采用固定机架，仅适用于直径大小与机架长度相匹配的蒸发罐；邓忠业[7]等采用X、Y轴滑轨分区、换区方式自动定位清洗换热管，其在不同区中执行清洗任务时，需要重复定位原点，影响设备的定位精度。

针对上述现有蒸发罐清洗设备存在的问题：①人工方式定位换热管；②不适用于直径大小不同的蒸发罐；③需换区重新设置参考点，重复定位精度低。为此，提出自主开发3自由度转臂式清洗机器人。因一级臂末端采用履带行走机构，如何解决一级臂回转定位精度问题成为自主开发清洗机器人的关键问题。另外，机器人清洗过程中，需要对换热管逐一定位，为此对一级臂回转定位进行研究。

1转臂式清洗机器人模型

自主研发的3自由度转臂式清洗机器人机械本体[8]结构如图1所示。因清洗灌表面凹凸不平，清洗机器人的一级臂采用履带行走机构[9]以提高其越障碍能力。同时，为降低电机端的负载力矩，设计履带行走机构置于一级臂的末端。通过伺服电机5驱动履带行走机构6实现一级臂绕中心回转立柱2回转运动，其实际转角由中心回转编码器3检测。二级臂8由二级臂伺服电机7驱动。转盘式送管机构安装在二级臂8末端，在一、二级臂定位后启动水管转盘电机10进行除垢清洗作业。

1.待洗换热管 2.中心回转立柱 3.中心回转编码器 4.一级臂 5.一级臂伺服电机 6.履带行走机构 7.二级臂伺服电机 8.二级臂 9.水管转盘 10.水管转盘电机 11.高压水管 12.高压水喷枪导引头

图1转臂式清洗机器人模型

2闭环控制方案

针对所设计的清洗机器人一级臂的结构特点，提出采用“PC机+运动控制器+中心回转编码器”构成的位置闭环控制系统方案(图2)实现其回转定位控制(驱动履带行走机构到正确位置，修正扰动偏差的影响)，利用控制器提供的运动控制功能快速构建高性能的控制系统。该控制系统由上位机控制模块、通讯模块、运动控制模块、执行模块构成。上位机控制模块由SQL数据库和上位机控制系统组成，数据库负责转角目标及插补参数的存储，上位机控制系统负责发送运动控制参数至运动控制模块，实现一级臂插补参数配置、运行状态监测等控制与人机交互功能。运动控制模块接收上位机控制模块下发的转角目标及插补参数生成指令脉冲。执行模块接收指令脉冲驱动一级臂按照指定参数运动。中心回转编码器3检测一级臂转角位置信息并反馈回运动控制器构成脉冲位置闭环。

图2　一级臂位置闭环控制方案

3回转定位插补算法

3.1回转定位补偿原理

图3　回转定位补偿原理

3.2回转定位插补算法实现

基于回转定位补偿原理的回转定位插补算法详细流程见图4。

图4　回转定位插补算法

步骤1：插补参数初始化操作。分别初始化设置以下各参数：插补加速度vacc，插补减速度vdec，插补速度vspeed，S型插补速度曲线配置vscurveset，回原点速度vzero，回原点模式[10]置为4，中心回转编码器置为4倍频A/B相模式。

步骤2：初始位置初始化操作。执行当前坐标位置清零、中心回转编码器反馈位置清零初始化操作并读取目标位置。

步骤3：输出插补控制量。根据目标位置与中心回转编码器反馈位置的偏差e输出插补控制量，调用[CONTI_ENTER]指令进入连续插补模式[10]执行连续插补运动。

步骤4：程序执行条件判断。

(1)判断程序条件1：中心编码器反馈位置≠目标位置？若二者相等，插补定位运动结束；否则判断程序条件2。

(2)判断程序条件2：中心回转编码器反馈位置≤目标位置？行走过程中履带打滑或扰动偏差，使得中心回转编码器反馈位置小于目标位置将使得程序进入连接点①；否则外界因素的影响导致中心编码器反馈位置大于目标位置时程序进入连接点③。

步骤5：

步骤6：插补定位运动结束。当中心回转编码器反馈位置等于目标位置时，插补定位运动结束。

4实验验证

4.1实验验证平台

为验证所提出的控制系统方案和回转定位插补算法的有效性及定位精度，搭建基于PMC6496运动控制器的实验平台：IPPC-150C工业平板电脑、PMC6496运动控制器、ACM604V60交流伺服电机及驱动器、中心回转编码器ERN-120和机械本体。基于Visual Studio 2012开发环境开发上位机控制系统，依据图4算法流程编制相应的回转定位插补程序，由控制器实现对一级臂的控制与管理。

4.2回转定位精度测试

回转定位测试：根据图4算法流程图，初始化vacc=20000脉冲/秒，vdec=20000脉冲/秒，vspeed=40000脉冲/秒，vscurveset=0.5，vzero=15000脉冲/秒，执行一级臂回原点动作；执行回转定位插补程序定位到设定目标位置，调整百分表到测量正确位置后，重复执行回原点与回转定位插补程序10次，测试并记录在相同的插补参数、不同目标位置下定位测试数据，如图5。分别设定目标位置为±1000脉冲进行定位，测试数据见表1、表2。

目标位置+1000脉冲时最大定位精度误差f为0.68mm，修正后f ′在±0.56mm内；目标位置-1000脉冲时最大定位精度误差f为0.90mm，修正后f ′在±0.69mm，如图6。综上分析可知所提出的方案能够实现一级臂精确回转定位，其精度在±1mm。

表1　目标点位+1000脉冲测试数据(单位：mm)

表2　目标点位-1000脉冲测试数据(单位：mm)

图5　回转定位实验验证

图6　回转定位精度

5结论

以自主开发的3自由度转臂式清洗机器人一级臂为研究对象，根据一级臂特殊的机械结构，提出特有的闭环控制系统方案，并进行回转定位补算法的研究。实验结果表明该算法根据中心回转编码器能实时修正补偿目标值，实现一级臂回转定位精度为±1 mm，证实所提出的闭环控制系统方案和回转定位插补算法能够保证一级臂回转定位精度。

本文中只考虑了在固定的插补参数时一级臂回转定位精度，今后将深入讨论在不同的插补参数下的回转定位精度，进行插补参数优化。另外，将在此基础上对一、二级臂协同定位展开研究，从而实现该转臂式清洗机器人在实际中的应用。

[参考文献]

[1] 冯晓静. 蒸发器结垢的成因和解决措施[J]. 中华纸业, 2010,31(18):88.

[2] Wang Z M, Ji X X, Ma J. Design of an Automatic Device for Cleaning Cellular Evaporator[J]. Applied Mechanics and Materials, 2014,494-495:1012-1015.

[3] 刘殿宇. 蒸发器壳程结垢会导致蒸发器生产能力降低[J]. 发酵科技通讯, 2007,36(1):42.

[4] 马乐凡，林本平，屈琴琴，等. 管式蒸发器高压水清洗水枪夹持装置及其使用方法[P].中国: 201210044787.2.2012-07-18.

[5]Skatoskalo International Limited. AUTO WATER/AIR CLEANING SYSTEMS WITH PNEUMATIC CONTROLS[Z]. Skatoskalo International Limited, 2013.

[6] 农谭宏，陈特青. 蒸发器换热管清洗机[P].中国:201110242209.5. 2011-08-23.

[7] 邓忠业，吴湘柠，张培铭，等. 列管式加热器全自动清洗设备的顶杆式自动换区和定位装置[P].中国:201310161993.6. 2013-09-04.

[8] 潘存云，徐海军，叶丽娜，等. 一种清罐设备[P].中国:201310055352.2.2014-08-27.

[9] 莫海军，朱文坚. 履带式移动机器人越障稳定性分析[J]. 机械科学与技术, 2007,26(1):65-67.

[10] 深圳市雷赛智能控制股份有限公司. PMC6496运动控制器梯形图编程手册[Z]. 深圳: 雷赛智能控制股份有限公司, 2011.

[11] 中国国家标准化管理委员会. GB/T 1958-2004 产品几何量技术规范(GPS)形状和位置公差检测规定[S]. 北京: 中国标准出版社, 2004.

[12] 熊安辉. 圆周均布孔组位置度测量中切向误差的修正[J]. 现代制造工程, 2009(3):134-135.

(编辑李秀敏)