基于Delta 并联机器人的传送带动态跟踪❋

邓明星，刘冠峰，张国英

（广东工业大学 机电工程学院，广东 广州 510006）

0 引言

工业机器人技术的发展和成熟，正孕育着新一轮的产业革命。Delta机器人是一种高速、轻载的并联机器人，多应用在电子、食品、生物制药等包装行业。目前针对机器人对传送带的动态跟踪研究比较少，本文提出了一种基于Delta并联机器人的新型跟踪算法。

1 Delta机器人硬件系统

如图1所示，本文的机器人是Delta并联机器人，机器人下方有多条传送带，传送带的位置在机器人的工作空间内；除此之外，还有一个视觉检测系统，该系统负责定位传送带上物体的位姿。

图1 Delta并联机器人传送带跟踪系统

如图2所示，把传送带定义为可移动的坐标系C，该坐标系x方向为传送带移动方向，沿x方向，顺流而下为Downstream，逆流而上为Upstream。在图1中，把视觉系统定义成坐标系V，机器人坐标系为R。通过视觉标定和传送带标定，可分别确定V到R和C到R的转换矩阵。故而，可确定C和R的关系。

视觉系统定位的物体位置矢量为VP，在R中的位置为［1］：

最终得到物体的实际位置为：

图2 传送带的定义和区域

2 PID算法引进

当系统要求较高时，常常采用有源校正环节，而PID调节器已经形成了典型结果，其参数整定简单，在许多工业过程控制中获得了较为满意的效果。所谓的PID控制规律，就是一种对偏差ε（t）进行比例、积分和微分变化的控制规律［2］：

其中：m（t）为t时刻系统的调整量；Kp为系统的比例系数；Ti和Td分别为系统的积分时间常数和微分时间常数。

值得注意的是：①比例系数Kp直接决定控制作用的强弱，加大Kp可以减小系统的稳态误差；②在比例环节的基础上，由积分时间常数Ti控制的积分环节可以消除系统的稳态误差，但控制过强会使系统的超调量增大，导致系统的稳定性变差；③微分控制能够预测偏差，微分时间常数Td越大，响应速度越快，噪声信号也越大。

3 基于PID的跟踪算法

在本文中，假如物体在传送带的上游，当其进入视觉系统的视场之后，定位得到物体的瞬时位置为VP（x0，y0，z0），通过式（1）～式（4）转换，可得到拍照时刻物体在机器人坐标系下的位置RP（x1，y1，z1）［3－4］，任意时刻机器人末端的位置为RPr0（xr0，yr0，zr0）；而RP＋ve（t）－ve（0）即为机器人任意时刻末端的目标位置，因此：

其中：ve（t），ve（0）分别为t时刻和拍照时传送带编码器的读数［5］。

设εx（t）、εy（t）、εz（t）分别为ε（t）在机器人坐标系中x，y，z轴的分量：

其中：θ，γ分别为传送带与x轴和xoy平面的夹角。下一个周期时，机器人末端将要到的位置为Pn：

需采用离散的方法对时间积分，每隔一个采样周期Δt，更新一下Pn，如采样周期足够短，式（8）即被看作连续的，因此机器人在t时刻的调整量m（t）可以写成：

4 测试基本的PID算法

用式（8）来做跟踪测试，结果如图3所示。在跟踪过程中，机器人与目标位置的偏差逐渐缩小，最后收敛。刚开始时机器人会出现剧烈振荡，原因是当前位置与目标位置的偏差较大，为立即减小偏差，机器人增大了调整幅度，导致其出现抖动现象，并且会持续一段时间［6］。

图3 Td调整前的跟踪偏差图

为了抑制系统的振荡现象，可考虑逐渐减小Td的值［7］。调整后的结果如图4所示。

经Td调整后，系统的稳定性和动态响应得到了明显的改善。但传送带移动速度快慢不均［8］，导致机器人自调整不够及时和平滑，从而出现明显的滞后和抖动现象。

5 改进的PID跟踪算法

由于基本的PID跟踪算法在速度突变时机器人的反应有一定的滞后，因此，提出了改进的跟踪算法，使机器人能够时刻做好应对“速度突变”的准备［9］。

图4 Td调整后的跟踪偏差图

其中：Ta为加速度调整系数；Td1为速度平方的调整系数。为确保跟踪更准确，可使用连续两个周期的平均值来求得ε（t）和预测速度。引进以下公式：

测试式（10）、式（11），得到的结果如图5、图6所示。

图5 基于速度二次曲线的跟踪偏差图

图5中，系统表现良好，振动较少，误差大致被控制在±4mm内，稳定性较高。

图6中，机器人的跟踪精度得到大幅度提升，误差控制在±1mm内，可以看出，由于加速度的引进，有效地抑制了系统的滞后。

6 结束语

由实验结果可知，改进的PID算法基本能实现机器人对传送带的跟踪。机器人系统的稳定性、快速响应性表现良好，将会对Delta机器人跟踪算法的发展产生一定的影响。

图6 基于加速度的跟踪偏差图

［1］赵杰，朱延河，蔡鹤皋，等.Delta型并联机器人运动学正解集合方法［J］.哈尔滨工业大学学报，2003，35（1）：25-27.

［2］杨叔子，杨克冲.机械工程控制［M］.武汉：华中科技大学出版社，2005.

［3］蔡自兴.机器人学［M］.北京：清华大学出版社，2009.

［4］于东兴.高速机器人饮料包装生产线技术及其关键装备研究［D］.天津：天津大学，2011：35-37.

［5］Wilhelm W E.Conveyor tracking［M］//International Encyclopedia of Robotics：Applications and Automation.New York：Wiley，1988：283-298.

［6］Shin K G，Mckay N D.A dynamic programming approach to trajectory planning of robotic manipulators［J］.IEEE Trans Automatic Contr，1986，31（9）：491-500.

［7］Taylor R H.Planning and execution of straight line manipulator trajectories［J］.IBM J Res Develop，1979，23（4）：424-436.

［8］Park T H，Lee B H.An approach to robot motion analysis and planning for conveyor tracking［J］.IEEE Transaction on Systems，Man and Cybernetics，1992，22（2）：378-384.

［9］Ik Sang Shin，Sang-Hyun Nam，Hyun Geun Yu.Conveyor visual tracking using robot vision［C］//Florida Conference on Recent Advances in Robotics.Florida：［s.n.］，2006：1-5.