机器人传送带动态跟踪

《机器人传送带动态跟踪》由会员分享，可在线阅读，更多相关《机器人传送带动态跟踪（73页珍藏版）》请在装配图网上搜索。

1、广东工业大学硕士学位论文（工学硕士）并联Delta机器人的传送带动态抓取系统设计邓明星二一四 年 六月二一四 年 六分类号： 学校代号：11845UDC： 密级： 学 号：2111101011广东工业大学硕士学位论文（工学硕士）并联Delta机器人的传送带动态抓取系统设计邓明星 指导教师姓名、职称： 刘冠峰 教授 学科(专业)或领域名称： 智能工业机器人 学 生 所 属 学 院： 机电工程学院 论 文 答 辩 日 期： 2014年6月1日 A Dissertation Submitted to Guangdong University of Technology for the Degree

2、of Master of Science（Master of Engineering Science）The dynamic conveyor tracking of Delta RobotCandidate: Deng MingxingSupervisor: Prof. Liu GuangfengJune 2014School of Electromechanical EngineeringGuangdong University of TechnologyGuangzhou, Guangdong, P. R. China, 5100摘要摘要Delta机器人是最典型的空间三自由度移动并联机构

3、，具有运动速度快、定位精确、效率高、重量轻等特点。其在3C电子产品、食品包装、生物制药、3D打印行业得到了广泛应用。随着人民生活水平的提高，促使产品质量的提升和需求数量的增加，间接的促使机器人技术的发展。本文主要以并联Delta机器人为研究对象。首先介绍国内外工业机器人的发展状况；接着分析了Delta机器人的运动学正逆解，传送带标定、相机标定；然后提出了PID跟踪算法和改进的跟踪算法；再者给出了Conveyor Administor的软件设计流程图，并解决了其中几个关键问题：目标物体的筛选、排序、位置状态更新、多机器人协同作业。本文的核心内容之一是机器人对运动在传送带上的物体的动态跟踪，通过视

4、觉检测系统定位物体的位姿，再结合传送带编码器反馈的位置数据，计算出物体在机器人坐标系下面的实时位姿。机器人在位置模式下，通过改进后的跟踪算法，能预测传送带上物体的位姿，并实时调整自身移动速度和末端位姿来跟踪传送带上移动的物体，最终实现对目标物体的动态跟踪抓取。本文的核心内容之二是处理传送带上连续不断的物体，在实际应用中，要求物体尽量少被遗漏甚者不能漏，这就要求机器人的移动速度要快，其次是抓取顺序必须规划好，本文的最后给出了解决方法和设计的数据管理软件。通过大量的实验数据和分析验证，该动态跟踪方法基本满足了实际应用要求，为后面的项目应用现提供了技术支持，也为多机器人协调作业提供了一种方法。关键词

5、：并联Delta机器人、传送带跟踪、PID算法、多机器人协同作业、数据流VIIAbastractAbstractThe 3 DOF delta robot is one of the most typical parallel Robot. It has many advantages, such as high movement speed, accurate location, high efficiency, light weight etc. It has been widely used in the electronic products industry, food packag

6、ing industry, biological pharmaceutical industry, 3D printing. With the development of robot technology and the improvement of peoples living standard, It is improvement to improve the quality of product and the number of product.This article mainly took Delta robot as the research object. Firstly,

7、it introduced the development of industrial robots at home and abroad; then analyzed the forward kinematics and inverse kinematics of Delta robot，the calibration of conveyor belt and camera; then introduced a PID tracking algorithm and a improved tracking algorithm; moreover display the flow chart o

8、f software design of Conveyor Administor, and resolved the problem of several joint data filtering, sorting, update of data status, coordination work of multi-robot; One of the core content of this article is the dynamic tracing of robot to the moving objects on the conveyor belt, coupled with the p

9、osition of the conveyor belt encoder feedback data. Visual system locates the objects positions then gives them to the delta robot. Conveyor Administor calculates the position of object in the robot coordinate system with the real-time system. Robot can predict the objects position on the conveyor b

10、elt, and adjust their movement speed and real-time posture to track objects on the conveyor belt in position mode and through the improved tracking algorithm, which finally realizes the dynamic tracing to moving target object. The another core content of this article is the operation for continuous

11、object on a conveyor belt, in actual application, There should no missed object, so, that requires that the robot move faster, objects are followed by fetching order must plan well. In the end, the paper puts forward the solution and design data management software.Through the analysis of the result

12、s and the experimental verification, the basic dynamic tracing method satisfied the requirements of practical application. It provides the technical guidance for the back of the project application, and realize the coordination working of multi-robot.Keywords: Delta robot, Conveyor belt tracking, PI

13、D algorithm, Multi-robot coordination work, Data stream.目录目录摘要IABSTRACTII目录IVCONTENTSVI第一章 绪论11.1 什么是工业机器人11.2 工业机器人在国内外发展状况31.3 Delta机器人简介51.4本课题研究内容和研究意义7第二章DELTA机器人软、硬件系统82.1 系统硬件介绍82.2 系统软件介绍9第三章DELTA机器人的运动学求解和标定123.1 空间点、向量、坐标系的表示123.2 Delta机器人运动学正解（几何法）143.3 Delta机器人运动学逆解183.4传送带标定213.4.1 转换矩阵

14、和比例因子213.4.2 传送带的标定方法213.5 视觉标定243.5.1 视觉标定原理243.5.2 基于机器人的相机标定273.5.3 基于传送带的视觉标定28第四章 动态跟踪算法304.1 物体位姿的相互转换304.2 PID算法的概念314.3 PID形式的跟踪324.4 两种改进形式的PID跟踪36第五章 目标物体跟踪序列软件的设计385.1 Conveyor Administor软件架构385.2 目标物体的筛选和遗漏问题395.3 目标物体的排序415.4 更新目标物体的操作状态和坐标状态435.5 Conveyor Administor的输出和多机器人协同465.6 Conv

15、eyor Administor的交互485.7 软件流程图50结论与展望52参考文献54学位期间发表的论文58学位论文独创性声明59致谢60ContentsCONTENTSABSTRACT(CHINESE)IABSTRACT(ENGLISH)IICONTENTS(CHINESE)IVCONTENTS(CHINESE)VICHAPTER1 INTRODUCTION11.1 Industrial robot11.2 The development of Industrial robot31.3 Delta robot51.4 Main works of this paper7CHAPTER2 TH

16、E HARDWARE & SOFTWARE OF DELTA ROBOT82.1 Hardware system82.2 Software system9CHAPTER3 THE KINEMATICS & CALIBRATION OF DELTA ROBOT123.1 The representation of point，vector，coordinate123.2 The forward kinematics of delta robot143.3 The inverse kinematics of delta robot183.4 Calibrate conveyor213.4.1 Tr

17、ansition matrix & scale factor213.4.2 Calibrate conveyor213.5 Calibrate vision243.5.1 The theory of visual calibration243.5.2 Calibrate vision based on robot273.5.3 Calibrate vision based on conveyor28CHAPTER4 THE ALGORITHM OF TRACKING304.1 Transfer position of different parts304.2 The concept of PI

18、D314.3 The form of PID tracking324.4 Two kinds of improved PID tracking36CHAPTER5 CONVEYOR ADMINISTOR385.1 The software architecture of Conveyor Administor385.2 Design filter for target parts395.3 Sort the target parts415.4 Update the operational state & position state435.5 The export & multi robot4

19、65.6 The User interface of Conveyor Administor485.7 Software flow pattern50CONCLUSION & PROSPECT52REFERANCES54PUBLISHED PAPERS DURING STUDYING DEGREE58ORIGINALITY DECLARATION59ACKNOWLEDGEMENTS60第一章 绪论第一章 绪论1.1 什么是工业机器人工业机器人是一种面向工业领域的具有多个自由度或多个关节的特殊机器人1；一般以机械手的形式呈现，包括串联的工业机器人和并联的工业机器人，它一般由本体、控制系统、动力系

20、统三个基本部分。工业机器人是一种自动完成工作的智能机器装置，是依靠控制系统来控制、动力系统来执行任务的一种机器。它可以与使用者交互，通过示教的方式来学习新的功能，丰富自身的知识数据库、工业机器人可以依靠视觉图像处理技术、运动控制技术、人工智能学、听觉模糊识别技术、计算机技术、网络通信技术等来增加自身的功能。工业机器人可以替代人类来完成一些危险、枯燥、重复、单调、环境恶劣的工作。它们通常应用于焊接、搬运、锻造、热处理、化学物品制造、高精度定位以及原子工业部门中。工业机器人具有一定的通用性和普遍性，因种类繁多、形态各异，所以它们多应用在各种生产线上，是柔性制造单元系统中不可或缺的组成部分。图1-1

21、 机器人工作流水线Figure1-1 The working line of robot工业机器人具有智能化、可编程化、通用性等特点。它涵盖的科学技术相当广泛，总结下来，工业机器人就是机械技术和电子技术的结合，即机电一体化发展的高级产物。当前的工业机器人已经相当智能了，不仅具备了获得外部环境数据的各种传感器，还具有记忆存储、示教学习、语音识别、图像处理、逻辑推理判断功能。所以，在很大程度上，机器人技术的发展势必带动其他领域学科的发展。机器人技术的发展和应用程度也是一个国家科学技术和工业技术发展水平的衡量标准。 图1-2 常见的工业机器人Figure1-2 The common industri

22、al robot如图1-2，市场上常见的工业机器人：Delta robot,5、6轴串联机械手, Dual-arm robot, SCARA robot, youbot。 1.2 工业机器人在国内外发展状况工业机器人技术的发展和成熟，即将掀起新一轮的产业革命。大多数发达国家：美国、德国、意大利、瑞士、日本、法国等早已普及工业机器人。电子产品、汽车制造、机械工程、食品包装、生物制药等行业的自动化生产线已经大量投入工业机器人。这些机器人的普及提升了产品质量、提高了劳动生成率、节约了用人成本、解放了生产力、减少了工伤事故。自动化设备的发展方向正逐渐转向工业机器人成套设备的发展，正是此发展转变，促使政

23、府、企业、高校投入更多的精力和经费来研究工业机器人技术，也推动了生产力的发展。根据国际机器人联合会(IFR)和联合国欧洲经济委员会(UNECE)对各国使用工业机器人情况的统计，工业机器人的发展一片光明。从上世纪中叶，工业机器人产值一直保持着稳定增长的良好形势。到九十年代，工业机器人发展形势瞬间变得猛烈，平均年增长率在10上下。而到了2004年，平均增长率更是达到了前所未有的21。其中，在亚洲机地区，机器人的增长率尤为明显，高达44%。2004年全球地区至少使用了10万台新的工业机器人。其中：欧洲地区3.1万台，比上一年多增加15；北美地区大约1.6万台，该数目比上一年增加27；亚洲地区大约5.

24、1万台，日本使用最多，但中国市场也不容小觑，增长异常迅速，比上一年多增长242。根据美国电气和电子工程师协会对机器人使用情况的统计，至08年底，全球地区已经布局了大约1000000台工业机器人，而日本是世界上使用工业机器人最多、普及最广的国家。图1-3 每10000工人占有机器人的个数Figure 1-3 The number of robot per 10000 workers图1-3是依照每10000个工人使用工业机器人数量的比例绘制的，从图中可以清晰的知道，日本工人的工业机器人分配密度达到了世界平均水平的9倍，几乎是第二位新加坡的2倍。排在前5的国家分别是：日本每一万工人拥有295台工业

25、机器人、新加坡169台、韩国164台、德国163台、瑞典126台。虽然在这前五位国家中，有三位属于亚洲地区，但欧洲的工业机器人普及程度是全世界最广的。欧洲国家工业机器人密度为每万名工人50台、北美洲为31台、亚洲27台。国外的工业机器人制造厂商：瑞典的ABBRobotics；瑞士的staubli; 日本的FANUC,Yaskawa,NACHI,OTC,MITSUBISHI;德国的KUKARoboter;美国的AdeptTechnology,AmericanRobot,EmersonIndustrialAutomation,S-TRobotics;意大利COMAU;英国的AutoTechRobo

26、tics;加拿大的JcdInternationalRobotics;以色列的RobogroupTek。与国外相比，我国工业机器人基础研究启动比较晚，从上世纪七十年代初期才开始发展。截止2010年底，国内正在工作和使用的工业机器人数量超过50000台，普及程度仍不及欧美等发达国家，这个数量大致是德国的1/4、日本的1/10，由此可知，中国工业机器人行业发展空间巨大。到2014年底，中国购买机器人的数量将位居全球首位。预计到2015年底，我国机器人需要数量大约为35000台，占全球总量的16.9%，而中国对机器人的需求量在全球仍将保存首位。分析表示，未来五年，中国工业机器人市场需求增长率可达35%

27、，呈井喷式增长。到2016年或成为全球最大的机器人市场。目前“十二五”规划中，中国开始实行经济转型。中国的制造业面临着向智能化、高效率、低成本、更绿色方向转变。加速工业机器人基础研究即是中国的一个历史机遇又是一种挑战，因此，大力发展工业机器人产业的工作要进一步落实。因为：第一，工业机器人技术是十二五期间经济转型的主要手段和途径，政府要对国产工业机器人有更多的政策与经济支持，高校和科研机构要学习国外先进技术，拓展更多的机器人应用领域；第二，在国家的发展规划中，应继续对机器人基础研究和应用给予大力支持，力争形成产品和自动化制造装备同步协调的新局面；第三，部分自主研发工业机器人已具备质量高、功能全、

28、高度智能化、低成本等特点，国内企业购买工业机器人时不要盲目崇洋媚外，应全面评估，支持国产。中国国内也投入了相当多的经费对工业机器人技术做研究。国内的工业机器人公司也雨后春笋般发展。沈阳新松、众为兴、埃斯顿、启帆、李群科技、广州数控、乐佰特，虽然起步晚，但是劲头足。1.3 Delta机器人简介Delta机器人是一种并联的、高速的、轻载的、智能的机器人。由于机械结构的牵扯和限制，它一般只有3到4个自由度，故只能在工作空间内实现三个方向的平动和绕Z轴的旋转运动。它是由法国人Clavel博士于1985年发明的，随后又提出三种机构变异形式，以适应不同空间需求。Delta机器人具有重量轻、体积小、运动速度

29、快、定位精确、效率高等特点。Delta机器人在近几年得到了飞速发展，无论国内还是国外，各大机器人制造厂商分别制造出各自的机器人。表1-1是各机器人制造商的Delta robot，图1-4是形态各异的Delta robot.表1-1 各机器人制造商的Delta robotTable 1-1 The Delta robots of each manufacturerCompanyDelta RobotABBFlexPickerAdeptQuattroYaskawaHigh speed robotFANUCM-3iABOSCHPalomaQKMApollo 图1-4 形态各异的Delta robot

30、Figure 1-4 Different kinds of Delta robots在中国，随着市场经济的发展和生活水平的提高，国民对食品、药物、电子产品的需求越来越大，对这些行业来说，既是一种机遇又是一种挑战，而其中的产品包装工序尤为值得一提。随着产业结构升级，劳动资本提高，越来越多的生产厂家不愿依靠雇佣更多的员工而投入大量的财力去提高生产率，而是希望寻求更高效、更安全的方法来提高产品的质量和生产效率。Delta robot完全可以满足上述需要：高精度、高速、高质量、高效率。因此对Delta机器人的研究变得尤为重要。Delta机器人的技术研究包括：前向运动学，后向运动学（forward ki

31、nematics、Inverse kinematics）、动力学（Dynamics）、机器视觉技术（machinevision）、精确标定技术、传动带跟踪、机械设计、机械优化、计算机科学、网络通信。本论文中主要研究传送带跟踪、运动学求解、机器人标定。1.4本课题研究内容和研究意义由于Delta机器人具有速度快、负载轻、定位准特点，因此，它常常被运用在包装流水线上，待包装物品在一条传送带上，包装盒在另外一条传送带上。在实际应用中，传送带是移动的，物体的位置信息可以通过传感器、视觉系统获取，但是这些位置信息只是某一时刻的。因此，机器人不得不实时计算物体的位置，并且不断调整自身末端的位置和移动速度，

32、让它不断的接近目标物体，最终动态抓取。抓取完目标物体之后，机器人仍需要定位包装盒，并且实时跟踪它，然后把目标物体放进包装盒里面。除了动态跟踪物体以外，实际项目中还要求传送带上的物体不能被遗漏。如何解决这个问题呢？可以降低传送带的移动速度，增加机器人的移动速度，但是这样会影响到机器人的生成效率。机器人的抓取顺序也非常重要，如果机器人不先抓快走出工作空间的物体，而去抓刚进工作空间的物体。那么肯定会造成物体被遗漏。如果传送带上的待捡物体确实太多，一个机器人根本解决不了遗漏问题，那么应该考虑多个机器人协同作业。上一个机器人遗漏的物体传递给下一个机器人，下一个机器人继续作业，依次传递下去。这样既可以提高

33、生成效率、解决遗漏问题，又可以节约成本。如果采用视觉系统，只需要一个视觉系统即可，不再需要为每一个机器人都配一个视觉系统。也许实际应用中是上一个机器人分拣，下一个机器人贴标签，再下一个机器人抓取到包装盒。总之，如果实现了多机器人的协调作业，这些问题便迎刃而解了。而且还可以通过它建立一个庞大的Workspace.本文的研究内容大致有以下几个方面：Delta机器人的前向运动学、逆向运动学、传动带标定、相机的标定、目标物体动态跟踪、目标物体重复筛选、目标物体排序、多机器人标定、多机器人协同。本文的研究意义不但要解决上述的实际应用问题，而且还提出了一种新的跟踪算法，编写了一个数据管理软件。61第二章

34、Delta机器人软、硬系统第二章Delta机器人软、硬件系统2.1 系统硬件介绍本论文的硬件平台是基于QKM公司的Apollo机器人，如图2-1。在这个硬件平台上，研究Delta机器人的相关技术。图2-1 Apollo 机器人 Figure 2-1 Apollo delta robot如图2-1，机器人硬件系统由机械本体、伺服电机、传送带、工业摄像机、工控电脑、PA运动控制卡和示教盒组成3。工业相机通过USB数据线与工控电脑连接，摄像机把图像传递给工控电脑，电脑对图像进行特征值提取，分析出物体的位姿，再将物体的位姿信息通过以太网传递给PA控制卡。用户可以在示教盒上编写简单的控制程序，也可以通过

35、示教盒移动机器人，类似于一个遥控器，可以很方便地控制机器人。示教盒与控制卡是通过485串口来通讯。除此之外，图2-2中有两条传送带，一条传送带上是待分拣的物体，另外一条上是托盘。一般情况下，视觉系统被安装在机器人的工作空间之外，这样的好处保证了机器人在工作空间内正常作业时，视觉系统不会受干扰。图2-2 Delta robot控制系统Figure 2-2 The Delta robots control system在图2-2中，控制系统的核心为控制卡，本系统中的运动控制卡为Presice Automation Controller，简称PA卡。PA运动控制卡是由美国Precise Automa

36、tion公司所研发生产的，是一款新型的驱控一体、支持高级编程、嵌入多种运动学模型的支持串口、以太网通信的控制卡。该控制卡不需要额外的伺服电机驱动器（driver），它可以直接和电机相连。 图2-3 PA控制卡Figure 2-3 PA controller2.2 系统软件介绍如图2-4，PV是图像处理软件，它会提取出目标物体，分析出目标物体的位置信息。而得到的坐标是像素坐标，通过转换矩阵（内参和外参），可以将像素坐标转换成机器人坐标。最终通过以太网把物体的位姿数据传递给PA控制卡，PA控制卡控制机器人电机，电机的转动反映在机械本体的末端，最终实现对目标物体的抓取。图2-4 Precise Vi

37、sion：机器视觉软件Figure 2-4 Precise Vision：machinevisionsoftware图2-4是机器人控制软件GDE，用户可以在该软件上编写复杂的控制程序，机器人语言是高级语言C#，用户可以很方便的进行二次开发。除此之外，GDE还提供了大量关于机器人运动，以太网通讯，文件处理的API函数。图2-5 GDE：程序开发软件Figure 2-5 GDE：Develop software图2-6是机器人数据分析软件Datalogger。在对电机进行PID参数设置时：该软件可以很好的辅助用户分析电流环、速度环、位置环数据。图2-6 Datalogger：机器人数据分析软件F

38、igure 2-6 Datalogger：Data analysis software第三章 Delta机器人的运动学求解和标定第三章Delta机器人的运动学求解和标定3.1 空间点、向量、坐标系的表示如图3-1所示，空间点P可以用它的相对于参考坐标系的三个坐标来表示： (3.1.1)其中， 是参考坐标系中表示该点的坐标。图3-1 空间点的表示Figure 3-1 The representation of a space point向量可以由一个起始点和终止点来表示4-5。如果一个向量起始于点A，终止于点B，那么它可以表示为。特殊情况下，如果一个向量起始于原点，如图3-1所示，则有：(3.1

39、.2)其中是该向量在参考坐标系中的三个分量。实际上，点P就是用原点连接到该点的向量来表示的，具体地说，也就是用该向量的三个坐标来表示。图3-2 空间向量的表示Figure 3-2 The representation of a space vector向量的三个分量也可以写成矩阵的形式，如式（3.1.2）所示。 (3.1.3)这种表示法也可以稍做变化：加入一个比例因子w,如果x, y, z各除以w,则得到。于是，这时向量可以写为：(3.1.4) 变量w可以为任意数，而且随着它的变化，向量的大小也会发生变化，这与在计算机图形学中缩放一张图片十分类似。随着w值的改变，向量的大小也相应地变化。如果w

40、大于1，向量的所有分量都变大；如果w小于1，向量的所有分量都变小。这种方法也用于计算机图形学中改变图形与画片的大小。如果w是1，各分量的大小保持不变。但是，如果w=0，则为无穷大。在这种情况下，x，y和z（以及）表示一个长度为无穷大的向量，它的方向即为该向量所表示的方向。这就意味着方向向量可以由比例因子w=0的向量来表示，这里向量的长度并不重要，而其方向由该向量的三个分量来表示。一个中心位于参考坐标系原点的坐标系由三个向量表示，通常这三个向量相互垂直，称为单位向量，分别表示法线（normal）、指向（orientation）和接近（approach）向量，如图3-3所示。正如公式3.1.2，每

41、一个单位向量都由它们所在参考坐标系着的三个分量表示。这样，坐标系F可以由三个向量以矩阵的形式表示为：图3-3 移动坐标系原点在参考坐标系下面的表示Figure 3-3 The representation of a moving coordinate system origin in the reference frame(3.1.5)如果一个坐标系不在固定参考坐标系的原点，那么该坐标系的原点相对于参考坐标系的位置也必须表示出来。因此，在该坐标系原点与参考坐标系原点之间做一个向量来表示该坐标系的位置，如图3-4所示。这个向量由相对于参考坐标系的三个向量来表示。这样，这个坐标系就可以由三个表示方

42、向的单位向量以及第四个位置向量来表示6。图3-4 一个坐标系在另一个坐标系中的表示Figure 3-4 The representation of a moving coordinate in the reference frame(3.1.6)如式（3.1.5）所示，前三个向量是w=0的方向向量，表示该坐标系的三个单位向量的方向，而第四个w=1的向量表示该坐标系原点相对于参考坐标系的位置。与单位向量不同，向量P的长度十分重要，因而使用比例因子为16。3.2 Delta机器人运动学正解（几何法）到目前为止，并联Delta机器人运动学正解的封闭解问题仍没有得到全面解决。 常用通过代数方程组的数值

43、解法来解决这个问题, 该方法缺点在于推导过程极其复杂, 最终有多组解，不得不寻找更多的约束来对这些解做取舍。因而，本论文中采用更直观的解法，运用矢量代数和空间几何学来建立三自由度Delta机器人的简化运动学模型, 求解并联机器人运动学正解。相比于基于代数方程组的求解方法，几何法推导过程更简单、直观，并且回避了并联机器人运动学正解多解取舍的问题, 可直接获得工作空间内满足运动连续性的合理解7。图3-5是Delta机器人的结构图。一个静平台、三个主动臂、三个平行四边形的从动臂、一个运动平台构成了该机器人的框架。可以看出三组平行四边形结构的应用消除了运动平台绕的转动自由度，但向各个方向的平动自由度没

44、有被消除。即运动平台只能向X、Y、Z轴平动，不能向X、Y、Z轴做旋转运动。图3-5 Delta robot 结构图Figure 3-5 The structure of Delta robot 将上述结构简化一下，如图3-6。图3-6 Delta robot 结构简化图Figure 3-6 The structure of Delta robot 首先，将模型改造，分别构造三个平行四边形的中线：、，这三条中线的运动分别与平行四边形两边的运动一样。动平台的中心为，沿着平移，交于，向量、分别与、相等。可以很容易求得B、C、D坐标。如下公式：，=1，2，3(3.2.1)其中，、分别是三个输入杆与X坐

45、标轴的夹角；R为静平台所在圆的半径。，=1，2，3(3.2.2)，=1，2，3 (3.2.3)，=1，2，3 (3.2.4)其中L为输入杆的长度，、分别为输入杆的输入角8-9。图3-7 Delta robot 结构简化图Figure 3-7 The structure of Delta robot (3.2.5)，=1，2，3 (3.2.6)(3.2.7)图3-8 三棱锥简图Figure 3-8 The structure of triangularpyramid由此，可以求得B、C、D在基坐标系下面的坐标。要求得静平台中心A的坐标，可以通过三菱锥A-BCD求得，因B、C、D坐标已求得，而AB

46、、AC、AD的边长也是已知的，且AB=AC=AD。设F为三角形的外心，E为BC的中点，那么；。根据立体几何知识可知，；同理可以得到、。所以，F既是的外心，又是A在的投影。(3.2.8) (3.2.9)(3.2.10)通过上述这些公式可以求得运动平台中心A在机器人坐标系下面的坐标，这便是Delta机器人的正解。3.3 Delta机器人运动学逆解将Delta的几何结构再次简化，如图3-9图3-9 Delta robot 结构简化图Figure 3-9 The structure of Delta robot其中O为静平台的中心，也是机器人坐标系的原点；为机器人的主动杆；为平行四边形从动杆中线；为静

47、平台；为动平台。固定平台3个铰链点的坐标为10：，=1，2，3(3.3.1)其中R为静平台的平面的圆半径；为三个输入杆与与X轴的夹角。输入杆末端点的坐标可以通过向量表示：(3.3.2)向量可以求得：，=1，2，3.(3.3.3)，=1，2，3(3.3.4)其中L为输入杆的长度，分别为三个输入杆的输入角。假设动平台中心的坐标为，那么动平台铰链点的坐标为10：，=1，2，3.(3.3.5)其中r为动平台的平面的圆半径；为三个从动杆与与x轴的夹角。通过上述公式，可以得到：(3.3.6)其中，为从动杆的长度。联立=1，2，3三个方程：(3.3.7)由公式3.3.7得：(3.3.8)其中：（=1，2，3

48、）表达式为：(3.3.9)最终可以得到：(3.3.10)而公式3.3.9便是Delta机器人的逆运动学的解，知道动平台的位置，反求机器人的输入角。3.4传送带标定3.4.1 转换矩阵和比例因子传动带标定（conveyor calibration）就是确定传动带坐标系相对于机器人坐标系的位姿。设机器人坐标系为R，传送带坐标系为C，那么传动带坐标系相对于机器人坐标系的关系可通过转换矩阵来表示。由平移向量和旋转矩阵组成11。如果知道这个转换矩阵和物体在传送带坐标系下面的位置，那么就能确定该物体在机器人坐标系下面的位置。(3.4.1)传动带的标定实质就是确定这个转换矩阵和传送带编码器的比例因子（enc

49、oder factor）。所谓的传送带编码器比例因子就是传送带移动一段距离，这段距离分别在机器人坐标系和编码器上对应了一个变化值，而这两个变化值的比例关系即为比例因子。3.4.2 传送带的标定方法1） 在传送带上确定一点P，移动Delta机器人，将其末端触碰到该点P，记录传送带的编码器值为，机器人此时末端的位姿为（）。2） 移动传送带，在保证机器人可以再次触碰到P的条件下，让这段移动距离越大越好。再次移动机器人，使末端触碰到P点。记录编码器的值为，机器人末端的位姿为（）。3） 让机器人沿着传送带垂直的方向移动一段距离，记录机器人末端的位姿（）12如图3-10所示图3-10 传送带标定简图Fig

50、ure 3-10 Conveyor belt calibration diagram通过以上步骤之后，可以得到编码器比因子：(3.4.2)(3.4.3)(3.4.4)还可以求得变化矩阵。在这里，规定传送带移动的方向为传送带坐标系的x轴方向，通过空间立体的几何建立直线方程组，再用Matlab来计算，可以得到传送带坐标系原点O在机器人坐标系下面的坐标。(3.4.5)机器人坐标系到传送带坐标系的旋转矩阵为：(3.4.6)(3.4.7)(3.4.8)(3.4.9)那么机器人坐标系到传送带坐标系的转换矩阵为：(3.4.10)有了转换矩阵，那么传送带运动时，传送带坐标系下面的点在机器人坐标系下面的位置为：

51、(3.4.11)其中为传送带移动的距离。3.5 视觉标定3.5.1 视觉标定原理摄像机是一种重建工具，它在计算机视觉中对物理三维世界进行重建，即以二维的图像来描述三维世界，通过该重建过程，可以计算处理得到的二维图像。对摄像机的标定，是以二维图像描述外部三维世界的第一步13，因为摄像机的标定就是建立三维空间坐标系和二维图像坐标系的相对关系。标定过程中，涉及到几个坐标系：世界坐标系（机器人坐标系）、相机坐标系、像素坐标系。世界坐标系是由用户定义的三维坐标系，用来描述三维空间物体，相机坐标位置，在这里世界坐标系是机器人坐标系；相机坐标系是以相机光心为坐标系原点，z轴与光轴平行，且满足右手定则；像素坐

52、标系是物体成像平面，以像素为单位的直角坐标系。如图3-11，相机坐标系是以U为坐标系原点，z轴满足右手法则，即xUy；像素平面坐标系以为坐标系原点，即。以相机原点在像平面坐标系的投影为原点， 为坐标轴建立一个物理坐标系，。图3-11 世界坐标系、相机坐标系、像素坐标系的关系（1）Figure 3-11 The relationship between camera coordinate, pixel coordinate system and world coordinate system图3-11中，P在世界坐标系的位置矢量为，是P在像素平面坐标系的投影点，它在相机坐标系下的位置矢量为。 是

53、相机坐标系z轴与像平面夹角，一般情况下z轴垂直于像平面，即=，因此，相机坐标系平面与像素平面坐标系平行，两个平面的距离为，该距离称之为相机的焦距。相机坐标系是以毫米（mm）为单位来表示，而像平面坐标系是以像素（pixel）为单位来表示的，根据小孔成像原理，可知两个坐标系单位之间呈线性关系。一个像平面的点如何转换到相机坐标系中表示，那么这里需要内参转换矩阵。在图3-11中，相机光轴中心z轴方向上与像平面坐标系的交点称为投影中心，在像素坐标系下的坐标为，单位为像素，每个像素在和的物理尺寸为和，单位是像素/毫米，则像素坐标系的像素坐标与坐标系坐标的线性关系如下：(3.5.1)通过公式3.5.1表示它

54、，其中为像素坐标系下面的坐标，为坐标系下的坐标。(3.5.2)由小孔模型下投影变换原理，可得到公式3.5.3。(3.5.3)用矩阵表示为：(3.5.4)结合公式3.5.1和公式3.5.4，得：(3.5.5)其中，是相机的六个常见内参数，其组成的矩阵即为相机内参数矩阵。而从世界坐标系到相机坐标系的变换，可以通过转换矩阵来表示，此矩阵即为相机外参矩阵。它是由旋转矩阵R和平移向量所组成的，如图3-12所示。图3-12 世界坐标系、相机坐标系、像素坐标系的关系Figure 3-12 The relationship between camera coordinate, pixel coordinate

55、 system and world coordinate system其中，平移矩阵T是三维列向量构成，旋转矩阵R是坐标轴依次绕轴旋转角度所形成的三个矩阵的总乘积。(3.5.6)因此，旋转矩阵R最终的计算公式如式3.5.7：(3.5.7)因此，从相机坐标系到世界坐标系的变换关系如公3.5.8，其中，表示，为旋转矩阵，为平移向量。便是标定所确定的变换矩阵，它也被称为外s参数矩阵。 (3.5.8)相机的标定其实就是为了确定内参数矩阵和外参数矩阵。如果要把精度提上去，还需要把相机的畸变问题考虑进来，在本论文中暂时没有深入地探讨畸变问题。最终，机器人坐标到像素坐标的转换矩阵可表示为内参矩阵和外参矩阵的

56、乘积，如公式3.5.9。为外参矩阵，为内参数矩阵。(3.5.9)3.5.2 基于机器人的相机标定本文中，相机的标定主要有两种方式：基于机器人的标定和基于传送带的标定。基于机器人的标定主要是指依靠机器人坐标系来标定相机，确定二者之间的转化矩阵，一般情况下，相机被安装在工作空间之内；而基于传送带的标定是指依靠传送带坐标系，确定传动带坐标系与视觉坐标系的关系，通过传动带坐标系这个媒介来确定视觉坐标系与机器人坐标系之间的转换矩阵，这种情况下，相机一般安装在机器人工作空间外。基于机器人的标定步骤：确定相机的内参：图3-13是标准的标定块，图中有16个大小和间距都相同的正方形色块，色块的尺寸和色块的间隔均

57、为已知的，该步骤之后，可根据公式3.5.13.5.5来确定相机的内参数矩阵14。图3-13 内参标定目标块Figure 3.5.3 The calibration target block for internal parameters确定相机的外参：用相机分别定位图3-14中四个目标点的位置，此位置为像素坐标。根据相机内参矩阵，可求得目标物体在相机坐标系中的位置。示教移动机器人，让机器人末端去触碰这四个目标点，分别记录这四个点的世界坐标。有了这四组数据后，即可根据公式3.5.63.5.8来确定外参矩阵。图3-14 外参标定目标块Figure 3-14 The calibration targ

58、et block for external parameters上述两个步骤之后，即完成了对相机的标定。3.5.3 基于传送带的视觉标定基于传送带来标定相机其实和基于机器人来标定原理几乎一样。只是这里需要引进传送带坐标系来做桥梁，通过这个中介，最终确定相机的外参，即机器人坐标系与相机坐标系之间的关系。1) 与“基于机器人的相机标定”步骤一相同，确定相机内参。2) 由于相机在机器人工作空间之外，所以在标定相机外参数的时候，需要借助传送带编码器。相机定位之后，移动传送带，让标定目标移动到机器人的工作空间之内。让机器人分别去触碰这四个点。可以用公式3.5.10描述上述标定过程。(3.5.10)第四章

59、 动态跟踪算法第四章 动态跟踪算法4.1 物体位姿的相互转换如图4-1所示：本文的机器人是并联Delta机器人；机器人下方有多条传送带，传送带的位置在机器人的工作空间内；除此之外，还有一个视觉检测系统，该系统负责定位传送带上物体的位姿。图4-1 Delta并联机器人传送带跟踪系统Figure 4-1 The tracking system of Delta robot如下图4-2所示，把传送带看成一个可以移动的坐标系：conveyor transformation。该坐标系x方向为传送带传送的方向。沿着传送带方向，顺流而下，称之为Downstream，同理，逆流而上，称之为Upstream。在

60、图4-1中，把视觉检测系统也定义成一个坐标系：vision transformation。通过第三章的视觉标定（vision calibration），可以得到视觉坐标系与机器人坐标系的关系；同理，通过传送带标定（conveyor calibration），可以得到传送带坐标系与机器人坐标系的相对关系。根据这两个关系，可以确定传送带坐标系和视觉坐标系的相对关。因此，可以通过数学表达式来表示以上关系。图4-2 传送带的定义和区域Figure 4-2 The conveyor belt and area设机器人坐标系为R，视觉坐标系为V, 传送带坐标系为C，视觉系统定位的物体位置为（物体在视觉坐标

61、系下的位置），那么通过视觉坐标系来表示物体在机器人坐标系下的位置：(4.1.1)其中是机器人坐标系到视觉坐标系的转换矩阵。通过传送带坐标系来表示物体在机器人坐标系下的位姿：(4.1.2)其中是机器人坐标系到传送带坐标系的变化矩阵。由上述二式可以求得物体在传送带坐标系的位置：。因为传送带坐标系随着传送带的移动而变化，而这个变化只是坐标系的平移，故：(4.1.3)最终，(4.1.4)此时物体的实际位置即为。4.2 PID算法的概念当控制系统精度要求较高时，常采用反馈调节，有源校正。有源校正通常是由电阻、运算放大器和电容组成的反馈网络联结而成，被广泛地应用在工业控制系统中，时常被称为调节器。其中，按

62、偏差的比例（Proportional）、积分（Integral）和微分（Derivative）进行控制的PID调节器（PID校正器）是应用最广泛的一种调节器。PID调节器已经形成了典型结果，其参数整定方便，结果改变灵活（P、PI、PD、PID等），在许多工业过程控制中获得了良好的效果，对于那些数学模型不易精确求得、参数变化较大的被控对象，采用PID调节器也往往能得到满意的控制效果。所谓的PID控制规律15-16，即为一种对偏差进行比例、积分和微分变化的控制规律：(4.2.1)式4.2.1中，为比例控制项，为比例系数； 为积分控制项，为积分时间常数；为微分控制项，为微分时间常数。比例控制项与微分、积分控制项的不同组合可以分别构成PD