并联Delta机器人的传送带动态抓取系统设计.doc

《并联Delta机器人的传送带动态抓取系统设计.doc》由会员分享，可在线阅读，更多相关《并联Delta机器人的传送带动态抓取系统设计.doc（73页珍藏版）》请在三一文库上搜索。

1、 并联 Delta 机器人的传送带动态抓取系统设计 二一四 年 六 分类号： 学校代号： UDC： 密级： 学 号： 并联 Delta 机器人的传送带动态抓取系统设计 指导教师姓名、职称： 教授 学科(专业)或领域名称： 智能工业机器人 学 生 所 属 学 院： 机电工程学院 论 文 答 辩 日 期： A Dissertation Submitted to Guangdong University of Technology for the Degree of Master of Science The dynamic conveyor tracking of Delta Robot 摘要 I

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

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

4、，该动态跟踪方法基本满足了实际应用要求，为 后面的项目应用现提供了技术支持，也为多机器人协调作业提供了一种方法。 关键词：并联 Delta 机器人、传送带跟踪、PID 算法、多机器人协同作业、数据流 II Abstract The 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 wide

5、ly used in the electronic products industry, food packaging 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

6、mainly took Delta robot as the research object. Firstly, 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 impro

7、ved tracking algorithm; moreover display the flow chart of 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 th

8、e moving objects on the conveyor belt, coupled with the position 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.

9、 Robot can predict the objects position on the conveyor belt, 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

10、 content of this article is the operation for continuous 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 da

11、ta management software. Abastract III Through the analysis of the results 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

12、working of multi-robot. Keywords: Delta robot, Conveyor belt tracking, PID algorithm, Multi-robot coordination work, Data stream. IV 目录 摘要 .I ABSTRACT.II 目录 IV CONTENTSVI 第一章 绪论.1 1.1 什么是工业机器人.1 1.2 工业机器人在国内外发展状况.3 1.3 DELTA机器人简介5 1.4 本课题研究内容和研究意义.7 第二章 DELTA 机器人软、硬件系统8 2.1 系统硬件介绍.8 2.2 系统软件介绍.9 第三章

13、 DELTA 机器人的运动学求解和标定12 3.1 空间点、向量、坐标系的表示.12 3.2 DELTA机器人运动学正解（几何法）14 3.3 DELTA机器人运动学逆解18 3.4 传送带标定.21 3.4.1 转换矩阵和比例因子21 3.4.2 传送带的标定方法21 3.5 视觉标定.24 3.5.1 视觉标定原理24 3.5.2 基于机器人的相机标定27 3.5.3 基于传送带的视觉标定28 第四章 动态跟踪算法.30 目录 V 4.1 物体位姿的相互转换.30 4.2 PID 算法的概念31 4.3 PID 形式的跟踪32 4.4 两种改进形式的 PID 跟踪36 第五章 目标物体跟踪

14、序列软件的设计.38 5.1 CONVEYOR ADMINISTOR软件架构.38 5.2 目标物体的筛选和遗漏问题.39 5.3 目标物体的排序.41 5.4 更新目标物体的操作状态和坐标状态.43 5.5 CONVEYOR ADMINISTOR的输出和多机器人协同.46 5.6 CONVEYOR ADMINISTOR的交互.48 5.7 软件流程图.50 结论与展望.52 参考文献.54 学位期间发表的论文.58 学位论文独创性声明.59 致谢.60 VI CONTENTS ABSTRACT(Chinese)I Abstract(English)II Contents(Chinese) .

15、IV CONTENTS(Chinese).VI Chapter1 Introduction 1 1.1 Industrial robot1 1.2 The development of Industrial robot 3 1.3 Delta robot5 1.4 Main works of this paper7 Chapter2 The hardware 日本的 FANUC,Yaskawa,NACHI,OTC,MITSUBISHI;德国的 KUKA Roboter;美国的 Adept Technology,American Robot,Emerson Industrial Automati

16、on,S-T Robotics;意大利 COMAU;英国的 AutoTech Robotics;加拿大的 Jcd International Robotics;以色列的 Robogroup Tek。 与国外相比，我国工业机器人基础研究启动比较晚，从上世纪七十年代初期才开 始发展。截止 2010 年底，国内正在工作和使用的工业机器人数量超过 50000 台，普 及程度仍不及欧美等发达国家，这个数量大致是德国的 1/4、日本的 1/10，由此可知， 中国工业机器人行业发展空间巨大。到 2014 年底，中国购买机器人的数量将位居全 球首位。预计到 2015 年底，我国机器人需要数量大约为 35000

17、 台，占全球总量的 16.9%，而中国对机器人的需求量在全球仍将保存首位。分析表示，未来五年，中国 工业机器人市场需求增长率可达 35%，呈井喷式增长。到 2016 年或成为全球最大的机 器人市场。 目前“十二五”规划中，中国开始实行经济转型。中国的制造业面临着向智能化、 高效率、低成本、更绿色方向转变。加速工业机器人基础研究即是中国的一个历史机 遇又是一种挑战，因此，大力发展工业机器人产业的工作要进一步落实。因为：第一， 工业机器人技术是十二五期间经济转型的主要手段和途径，政府要对国产工业机器人 有更多的政策与经济支持，高校和科研机构要学习国外先进技术，拓展更多的机器人 应用领域；第二，在国

18、家的发展规划中，应继续对机器人基础研究和应用给予大力支 持，力争形成产品和自动化制造装备同步协调的新局面；第三，部分自主研发工业机 器人已具备质量高、功能全、高度智能化、低成本等特点，国内企业购买工业机器人 时不要盲目崇洋媚外，应全面评估，支持国产。 中国国内也投入了相当多的经费对工业机器人技术做研究。国内的工业机器人公 第一章 绪论 5 司也雨后春笋般发展。沈阳新松、众为兴、埃斯顿、启帆、李群科技、广州数控、乐 佰特，虽然起步晚，但是劲头足。 1.3 Delta 机器人简介 Delta 机器人是一种并联的、高速的、轻载的、智能的机器人。由于机械结构的 牵扯和限制，它一般只有 3 到 4 个自

19、由度，故只能在工作空间内实现三个方向的平动 和绕 Z 轴的旋转运动。它是由法国人 Clavel 博士于 1985 年发明的，随后又提出三种 机构变异形式，以适应不同空间需求。Delta 机器人具有重量轻、体积小、运动速度 快、定位精确、效率高等特点。 Delta 机器人在近几年得到了飞速发展，无论国内还是国外，各大机器人制造厂 商分别制造出各自的机器人。表 1-1 是各机器人制造商的 Delta robot，图 1-4 是形 态各异的 Delta robot. 表 1-1 各机器人制造商的 Delta robot Table 1-1 The Delta robots of each manuf

20、acturer CompanyDelta Robot ABBFlexPicker AdeptQuattro YaskawaHigh speed robot FANUCM-3iA BOSCHPaloma QKMApollo 6 图 1-4 形态各异的 Delta robot Figure 1-4 Different kinds of Delta robots 在中国，随着市场经济的发展和生活水平的提高，国民对食品、药物、电子产品 的需求越来越大，对这些行业来说，既是一种机遇又是一种挑战，而其中的产品包装 工序尤为值得一提。随着产业结构升级，劳动资本提高，越来越多的生产厂家不愿依 靠雇佣更多的员工

21、而投入大量的财力去提高生产率，而是希望寻求更高效、更安全的 方法来提高产品的质量和生产效率。Delta robot 完全可以满足上述需要：高精度、 高速、高质量、高效率。因此对 Delta 机器人的研究变得尤为重要。 Delta 机器人的技术研究包括：前向运动学，后向运动学（forward kinematics、Inverse kinematics） 、动力学（Dynamics） 、机器视觉技术 （machine vision） 、精确标定技术、传动带跟踪、机械设计、机械优化、计算机科 学、网络通信。本论文中主要研究传送带跟踪、运动学求解、机器人标定。 1.4 本课题研究内容和研究意义 由于D

22、elta机器人具有速度快、负载轻、定位准特点，因此，它常常被运用在 包装流水线上，待包装物品在一条传送带上，包装盒在另外一条传送带上。在实 际应用中，传送带是移动的，物体的位置信息可以通过传感器、视觉系统获取， 但是这些位置信息只是某一时刻的。因此，机器人不得不实时计算物体的位置， 并且不断调整自身末端的位置和移动速度，让它不断的接近目标物体，最终动态 抓取。抓取完目标物体之后，机器人仍需要定位包装盒，并且实时跟踪它，然后 第一章 绪论 7 把目标物体放进包装盒里面。 除了动态跟踪物体以外，实际项目中还要求传送带上的物体不能被遗漏。如 何解决这个问题呢？可以降低传送带的移动速度，增加机器人的移

23、动速度，但是 这样会影响到机器人的生成效率。机器人的抓取顺序也非常重要，如果机器人不 先抓快走出工作空间的物体，而去抓刚进工作空间的物体。那么肯定会造成物体 被遗漏。 如果传送带上的待捡物体确实太多，一个机器人根本解决不了遗漏问题，那 么应该考虑多个机器人协同作业。上一个机器人遗漏的物体传递给下一个机器人， 下一个机器人继续作业，依次传递下去。这样既可以提高生成效率、解决遗漏问 题，又可以节约成本。如果采用视觉系统，只需要一个视觉系统即可，不再需要 为每一个机器人都配一个视觉系统。也许实际应用中是上一个机器人分拣，下一 个机器人贴标签，再下一个机器人抓取到包装盒。总之，如果实现了多机器人的 协

24、调作业，这些问题便迎刃而解了。而且还可以通过它建立一个庞大的Workspace. 本文的研究内容大致有以下几个方面：Delta机器人的前向运动学、逆向运动 学、传动带标定、相机的标定、目标物体动态跟踪、目标物体重复筛选、目标物 体排序、多机器人标定、多机器人协同。 本文的研究意义不但要解决上述的实际应用问题，而且还提出了一种新的跟 踪算法，编写了一个数据管理软件。 8 第二章 Delta 机器人软、硬件系统 2.1 系统硬件介绍 本论文的硬件平台是基于 QKM 公司的 Apollo 机器人，如图 2-1。在这个硬件 平台上，研究 Delta 机器人的相关技术。 图 2-1 Apollo 机器人

25、 Figure 2-1 Apollo delta robot 如图 2-1，机器人硬件系统由机械本体、伺服电机、传送带、工业摄像机、工控 电脑、PA 运动控制卡和示教盒组成3。工业相机通过 USB 数据线与工控电脑连接，摄 像机把图像传递给工控电脑，电脑对图像进行特征值提取，分析出物体的位姿，再将 物体的位姿信息通过以太网传递给 PA 控制卡。 用户可以在示教盒上编写简单的控制程序，也可以通过示教盒移动机器人，类似 于一个遥控器，可以很方便地控制机器人。示教盒与控制卡是通过 485 串口来通讯。 除此之外，图 2-2 中有两条传送带，一条传送带上是待分拣的物体，另外一条上 是托盘。一般情况下，

26、视觉系统被安装在机器人的工作空间之外，这样的好处保证了 机器人在工作空间内正常作业时，视觉系统不会受干扰。 第二章 Delta 机器人软、硬系统 9 图 2-2 Delta robot 控制系统 Figure 2-2 The Delta robots control system 在图 2-2 中，控制系统的核心为控制卡，本系统中的运动控制卡为 Presice Automation Controller，简称 PA 卡。PA 运动控制卡是由美国 Precise Automation 公司所研发生产的，是一款新型的驱控一体、支持高级编程、嵌入多种运动学模型 的支持串口、以太网通信的控制卡。该控制

27、卡不需要额外的伺服电机驱动器 （driver） ，它可以直接和电机相连。 图 2-3 PA 控制卡 Figure 2-3 PA controller 2.2 系统软件介绍 如图 2-4，PV 是图像处理软件，它会提取出目标物体，分析出目标物体的位置信 10 息。而得到的坐标是像素坐标，通过转换矩阵（内参和外参） ，可以将像素坐标转换 成机器人坐标。最终通过以太网把物体的位姿数据传递给 PA 控制卡，PA 控制卡控制 机器人电机，电机的转动反映在机械本体的末端，最终实现对目标物体的抓取。 图 2-4 Precise Vision：机器视觉软件 Figure 2-4 Precise Vision：

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

29、器人数据分析软件 Figure 2-6 Datalogger：Data analysis software 12 第三章 Delta 机器人的运动学求解和标定 3.1 空间点、向量、坐标系的表示 如图 3-1 所示，空间点 P 可以用它的相对于参考坐标系的三个坐标来表示： (3.1.1) xyz Pa ib jc k 其中， 是参考坐标系中表示该点的坐标。, xyz a b c 图 3-1 空间点的表示 Figure 3-1 The representation of a space point 向量可以由一个起始点和终止点来表示4-5。如果一个向量起始于点 A，终止于点 B，那么它可以表示为

30、。特殊情况下，如果一()()()AB xxyyzz PBA iBAjBA k 个向量起始于原点，如图 3-1 所示，则有： (3.1.2) xyz Pa ib jc k 其中是该向量在参考坐标系中的三个分量。实际上，点 P 就是用原点连, xyz a b c 接到该点的向量来表示的，具体地说，也就是用该向量的三个坐标来表示。 图 3-2 空间向量的表示 Figure 3-2 The representation of a space vector 第三章 Delta 机器人的运动学求解和标定 13 向量的三个分量也可以写成矩阵的形式，如式（3.1.2）所示。 (3.1.3) x y z a P

31、b c 这种表示法也可以稍做变化：加入一个比例因子 w,如果 x, y, z 各除以 w,则得 到。于是，这时向量可以写为：, xyz a b c (3.1.4) x y P z w 变量 w 可以为任意数，而且随着它的变化，向量的大小也会发生变化，这与在计 算机图形学中缩放一张图片十分类似。随着 w 值的改变，向量的大小也相应地变化。 如果 w 大于 1，向量的所有分量都变大；如果 w 小于 1，向量的所有分量都变小。这 种方法也用于计算机图形学中改变图形与画片的大小。 如果 w 是 1，各分量的大小保持不变。但是，如果 w=0，则为无穷大。在, xyz a b c 这种情况下，x，y 和

32、z（以及）表示一个长度为无穷大的向量，它的方向即, xyz a b c 为该向量所表示的方向。这就意味着方向向量可以由比例因子 w=0 的向量来表示，这 里向量的长度并不重要，而其方向由该向量的三个分量来表示。 一个中心位于参考坐标系原点的坐标系由三个向量表示，通常这三个向量相互垂 直，称为单位向量，分别表示法线（normal） 、指向（orientation）和接近, ,n o a （approach）向量，如图 3-3 所示。正如公式 3.1.2，每一个单位向量都由它们所在 参考坐标系着的三个分量表示。这样，坐标系 F 可以由三个向量以矩阵的形式表示为： 图 3-3 移动坐标系原点在参考坐

33、标系下面的表示 Figure 3-3 The representation of a moving coordinate system origin in the reference frame 14 (3.1.5) xxx yyy zzz noa Fnoa noa 如果一个坐标系不在固定参考坐标系的原点，那么该坐标系的原点相对于参考坐 标系的位置也必须表示出来。因此，在该坐标系原点与参考坐标系原点之间做一个向 量来表示该坐标系的位置，如图 3-4 所示。这个向量由相对于参考坐标系的三个向量 来表示。这样，这个坐标系就可以由三个表示方向的单位向量以及第四个位置向量来 表示6。 图 3-4 一个

34、坐标系在另一个坐标系中的表示 Figure 3-4 The representation of a moving coordinate in the reference frame (3.1.6) 0001 xxxx yyyy zzzz noap noap F noap 如式（3.1.5）所示，前三个向量是 w=0 的方向向量，表示该坐标系的三个单位向 量的方向，而第四个 w=1 的向量表示该坐标系原点相对于参考坐标系的位置。, ,n o a 与单位向量不同，向量 P 的长度十分重要，因而使用比例因子为 16。 3.2 Delta 机器人运动学正解（几何法） 到目前为止，并联 Delta 机器

35、人运动学正解的封闭解问题仍没有得到全面解决。 常用通过代数方程组的数值解法来解决这个问题, 该方法缺点在于推导过程极其复杂, 最终有多组解，不得不寻找更多的约束来对这些解做取舍。因而，本论文中采用更直 观的解法，运用矢量代数和空间几何学来建立三自由度 Delta 机器人的简化运动学模 型, 求解并联机器人运动学正解。相比于基于代数方程组的求解方法，几何法推导过 第三章 Delta 机器人的运动学求解和标定 15 程更简单、直观，并且回避了并联机器人运动学正解多解取舍的问题, 可直接获得工 作空间内满足运动连续性的合理解7。 图 3-5 是 Delta 机器人的结构图。一个静平台、三个主动臂、三

36、个平行四边形的 从动臂、一个运动平台构成了该机器人的框架。可以看出三组平行四边形结构的应用 消除了运动平台绕的转动自由度，但向各个方向的平动自由度没有被消除。即运动平 台只能向 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 首先，将模型改造，分别构造三个平行四边形的中线：、，这三条 11 AB 22

37、A B 33 A B 中线的运动分别与平行四边形两边的运动一样。动平台的中心为，沿着平移 A 1 A A ，交于，向量、分别与、相等。可以很容易求 ii ABA 1 A A 2 A A 3 A A 1 B B 2 B C 3 B D 16 得 B、C、D 坐标。如下公式： ， =1，2，3(3.2.1) cos( ) sin( ) 0 i ii Ra OCRa i 其中，、分别是三个输入杆与 X 坐标轴的夹角；R 为静平台所在圆的半径。 1 a 2 a 3 a ， =1，2，3(3.2.2) iiii OBOCC B i ， =1，2，3 (3.2.3) cos( )cos( ) cos( )

38、sin( ) sin( ) ii iiii i La C BLa L i ， =1，2，3 (3.2.4) cos( )cos( )cos( ) sin( )cos( )sin( ) sin( ) iii iiii i RaLa OBRaLa L i 其中 L 为输入杆的长度，、分别为输入杆的输入角8-9。 123 , 图 3-7 Delta robot 结构简化图 Figure 3-7 The structure of Delta robot (3.2.5) 1111 2222 3333 OBOBB BOBA A OCOBB COBA A ODOBB DOBA A 第三章 Delta 机器人

39、的运动学求解和标定 17 ， =1，2，3 (3.2.6) rcos( ) rsin( ) 0 i ii a AAa i (3.2.7) 1111 1111 1 2222 2222 2 3 cos()cos( )cos()rcos() sin()cos( )sin()rsin() sin( ) cos()cos()cos()rcos() sin()cos()sin()rsin() sin() cos()c RaLaa OBRaLaa L RaLaa OCRaLaa L RaL OD 333 3333 3 os()cos()rcos() sin()cos()sin()rsin() sin() a

40、a RaLaa L 图 3-8 三棱锥简图 Figure 3-8 The structure of triangular pyramid 由此，可以求得 B、C、D 在基坐标系下面的坐标。要求得静平台中心 A 的坐标， 可以通过三菱锥 A-BCD 求得，因 B、C、D 坐标已求得，而 AB、AC、AD 的边长也是已 知的，且 AB=AC=AD。设 F 为三角形的外心，E 为 BC 的中点，那么；BCD AEBC 。根据立体几何知识可知，；同理可以得到、 EFBC AFBCAFCD 。所以，F 既是的外心，又是 A 在的投影。AFDBBCDBCD 18 (3.2.8) OAOFFA (3.2.9

41、) 22 2 4 4* 2 EF EF OFOEEF OCOB OE EFEF n BCCDBC n BCCDBC EFBFBE abcabc BF S p papbpc abc p (3.2.10) 22 * FA FA FAFA n FAABBF BCCD n BCCD 通过上述这些公式可以求得运动平台中心 A 在机器人坐标系下面的坐标，这便是 Delta 机器人的正解。 3.3 Delta 机器人运动学逆解 将 Delta 的几何结构再次简化，如图 3-9 第三章 Delta 机器人的运动学求解和标定 19 图 3-9 Delta robot 结构简化图 Figure 3-9 The s

42、tructure of Delta robot 其中 O 为静平台的中心，也是机器人坐标系的原点；为机器人的主动杆； ii PA 为平行四边形从动杆中线；为静平台；为动平台。固定平台 3 个 ii AB 123 PP PO 123 B B B O 铰链点的坐标为10： i P ， =1，2，3(3.3.1) cos sin() 0 i ii R OPR i 其中 R 为静平台的平面的圆半径；为三个输入杆与与 X 轴的夹角。输入杆末端 i 点的坐标可以通过向量表示： i A i OA (3.3.2) iiii OAOPPA 向量可以求得： ii PA ， =1，2，3.(3.3.3) i i i

43、 os( ) cos cos( )*sin() ( ) i iii Lc PAL Lsin i 20 ， =1，2，3(3.3.4) i i i cos()os( ) cos sin()cos( )*sin() ( ) ii iii RLc OARL Lsin i 其中 L 为输入杆的长度，分别为三个输入杆的输入角。 i 假设动平台中心的坐标为，那么动平台铰链点的坐标为10：O( , , ) xyz OOO i B ， =1，2，3.(3.3.5) cos sin() xi iyi z Or OBOr O i 其中 r 为动平台的平面的圆半径；为三个从动杆与与 x 轴的夹角。通过上述公 i 式

44、，可以得到： (3.3.6) i i i cos()os( ) coscos() sin()cos( )*sin()sin() ( ) iixi iiiiiiyi z RLcOr lABOBOARLOr LsinO 其中， 为从动杆的长度。联立 =1，2，3 三个方程：l i (3.3.7) 2 1111 2 2 2 11111 2 222 2 22222 cos()os( ) coscos() sin()cos( )*sin()sin()( ) cos()os() coscos() sin()cos()*sin()sin()() x yz ix yz RLcOr RLOrLsinOl RLc

45、Or RLOrLsinO 2 2 2 3333 2 2 2 33333 cos()os() coscos() sin()cos()*sin()sin()() x yz l RLcOr RLOrLsinOl 由公式 3.3.7 得： (3.3.8) cossin0 iiiii IJK 其中：（ =1，2，3）表达式为：, iii I J Ki 第三章 Delta 机器人的运动学求解和标定 21 (3.3.9) 22222 2coscoscos(sinsin)sin 2 (coscos)(sinsin) ixiiiyiii xiiyiiz ILOraRaaOraRaa JiLO z KiOraRaOraRaOLl 最终可以得到： (3.3.10) 222 2tan iiii ii ii JJIK a KI 而公式 3.3.9 便是 Delta 机器人的逆运动学的解，知道动平台的位置，反求机器 人的输入角。 3.4 传送带标定 3.4.1 转换矩阵和