搬运机械手控制系统的设计说明书


湘潭大学 毕业设计说明书
题 目:搬运机械手控制系统的设计

学 专 学 姓

院:机械工程学院 业:机械设计制造及其自动化 号:2003070523 名:陈 辉 煌

指导教师:毛 美 姣

完成日期: 2007 年 6 月 2 日





大 学

毕业论文(设计)任务书
论文(设计)题目:搬运机械手控制系统的设计

学号:2003070523 指导教师: 毛美姣

姓名:陈辉煌

专业: 机械设计制造及其自动化 系主任: 周善炳

一、主要内容及基本要求 在本次搬运机械手控制系统的设计中,要完成的任务有如下几个方面: 1、用机器人运动学求得搬运机械手的正解; 2、由运动学方程解得搬运机械手各关节变量,并求出轨迹方程; 3、用 VB 建立人机交互界面,并根据轨迹方程画出轨迹曲线; 4、实现 VB 界面与 PLC 的通信,以及对搬运机械手的控制; 5、写一份 8000 字以上的毕业设计说明书; 6、一篇不少于 3000 单词的英文原文及中文翻译。

二、重点研究的问题 1、搬运机械手运动学方程正解的求解;
2、建立 VB 界面,实现计算以及画出轨迹曲线; 3、实现搬运机械手的自动控制。

三、进度安排 序号 1 2 3 4 5 6 7 查阅资料 总体反方案设计 搬运机械手的运动学位姿正解 学习 Visual Basic 软件,做交互界面 编写程序,实现机械手运动的控制 写设计说明书 准备答辩 各阶段完成的内容 完成时间 07.3.15~07.3.31 07.4.1~07.4.7 07.4.8~07.4.24 04.4.25~07.4.30 07.5.1~.7.5.20 07.5.21~07.5.31 2007 年 6 月初

四、应收集的资料及主要参考文献 1、刘极峰,易际明.机器人技术基础[M].北京:高等教育出版社,2006 年. 2、张铁.机器人学[M].广州:华南理工大学出版社,2000 年. 3、从爽.实用运动控制技术[M].北京:电子工业出版社,2006 年. 4、钟肇新,范建东.可编程控制器原理及应用[M].广州:华南理工大学出版社,2003 年 5、俞建家.Visual Basic 6.0 程序设计与应用教程[M].福建:厦门大学出版社,2003 年 6、吕伟臣.Visual Basic 6.0 初级编程教程[M].北京:北京大学出版社,2002 年. 7、大熊.机器人控制[M].北京:科学出版社,2002 年. 8、张海根.机电传动控制[M].北京:高等教育出版社,2005 年. 9、丹尼斯.机器人设计与控制[M].北京:科学出版社,2004 年. 10、刘极峰.计算机辅助设计与制造[M].北京:高等教育出版社,2004 年.

湘 潭 大 学 毕业论文(设计)评阅表
学号 2003070523 姓名 陈辉煌 专业 机械设计制造及其自动化

毕业设计题目:搬运机械手控制系统的设计 评价项目 选题 合训练的目的; 2.难度、份量是否适当; 3.是否与生产、科研、社会等实际相结合。 1.是否有查阅文献、综合归纳资料的能力; 2.是否有综合运用知识的能力; 能力 3.是否具备研究方案的设计能力、研究方法和手段的运用能力; 4.是否具备一定的外文与计算机应用能力; 5.工科是否有经济分析能力。 1.立论是否正确, 论述是否充分, 结构是否严谨合理; 实验是否正确, 设计、 论文 质量 计算、分析处理是否科学;技术用语是否准确,符号是否统一,图表图纸 2.文字是否通顺,有无观点提炼,综合概括能力如何; 3.有无理论价值或实际应用价值,有无创新之处。 (设计) 是否完备、整洁、正确,引文是否规范; 评 价 内 容

1.是否符合培养目标,体现学科、专业特点和教学计划的基本要求,达到综

综 合 评 价 评阅人: 年 月 日



潭 大 学

毕业论文(设计)鉴定意见
学号:2003070523 姓名:陈辉煌 65 页 专业:机械设计制造及其自动化 图 表 0 张

毕业论文(设计说明书)

设计题目:

搬运机械手控制系统的设计

内容提要: 本文是关于搬运机械手控制系统的设计,整个设计过程中用到了机器人运动学、 可编程控制器(PLC)和 Visual Basic(VB)程序设计等方面的知识,通过学习和综合 运用,能缩短整个设计工计算量、提高计算的准确率,使系统控制操控直观化、简单 化。 本次课程设计首先通过描述机器人的现状与发展趋势阐述了搬运机械手控制系统 的设计意义。其次,运用机器人运动学求运动学方程正解,运用 Visual Basic 建立人 机交互界面,该界面利用机械手运动学方程计算出关节变量,按三次多项式插值算法 求得搬运机械手的轨迹方程以及根据方程画出相应轨迹曲线。最后编写程序,用 PLC 控制搬运机械手的动作,实现机械手的自动控制。 此次设计,讲述了机械手动作的控制过程,其中 Visual Basic 界面的应用,使整个 过程变得简单而准确,为机械手的研究提供了很大的方便。

指导教师评语

指导教师: 年 月 日

答辩简要情况及评语

答辩小组组长: 年 月 日

答辩委员会意见

答辩委员会主任: 年 月 日





摘要.......................................................... 1 第 1 章 引言 ................................................... 2
1. 1 课题研究趋势与意义 ................................................................................................ 2 1.2 串联关节机器人的发展和现状 ................................................................................. 3 1.3 本文的研究内容和主要工作 ..................................................................................... 3

第 2 章 PLC 控制系统及实现 ...................................... 5
2.1 机械手及控制器主要参数 ......................................................................................... 5 2.2 简述机械手动作的实现 ............................................................................................. 6 2.3 控制软件的设计 ......................................................................................................... 6 2.3.1 PLC 的选择 ....................................................................................................... 6 2.3.2 光电编码器 ...................................................................................................... 6 2.3.3 接口电路 .......................................................................................................... 7 2.3.4 控制原理及程序 .............................................................................................. 7 3.1 引言 ............................................................................................................................. 9 3. 2 机械手运动学数学基础 ............................................................................................ 9 3.2.1 机器人位置与姿态的描述[12] ........................................................................... 9 3.3 空间齐次坐标变换 ................................................................................................... 11 3.3.1 坐标变换 ......................................................................................................... 11 3.3.2 齐次坐标变换 ................................................................................................. 12 3.3 点在空间直角坐标系中绕过原点任意轴的一般旋转变换 .................................. 13 3.4 Denavt-Hartenberg(D-H)表示法 ............................................................................... 15 3.4.1 坐标系的建立 ................................................................................................ 15 3.4.2 几何参数定义 ................................................................................................ 16 3.4.3 建立 i 坐标系和 i ? 1 坐标系的齐次变换矩阵 ............................................... 16

第 4 章 机器人运动学方程的求解 ................................ 18
4.1 机器人正向运动学 ................................................................................................... 18 4.2 机器人逆向运动学 ................................................................................................. 18

第 5 章 机械手轨迹规划 ........................................ 20
5.1 机器人轨迹的概念 .................................................................................................. 20 5.2 轨迹的生成方式 ...................................................................................................... 20 5.2.1 轨迹规划涉及的主要问题 ............................................................................ 20 5.3 插补方式分类 .......................................................................................................... 21 5.4 机器人轨迹插值计算 .............................................................................................. 21 5.4.1 三次多项式插值 ............................................................................................ 22 5.4.2 机械手轨迹规划。 ......................................................................................... 23

第 6 章 系统交流界面设计 ...................................... 24
6.1Visual Basic6.0 简介 .................................................................................................. 24 6.2 VB 设计任务 ............................................................................................................. 25 6.3 窗体设计 .................................................................................................................. 25 6.4 窗体运行 ................................................................................................................... 29

参考文献..................................................... 20 致谢......................................................... 33 附录错误!未找到引用源。程序清单 ............................. 34 附录错误!未找到引用源。程序清单 ............................. 50 附录错误!未找到引用源。外文原文 ............................. 55

搬运机械手控制系统的设计
摘 要:本文运用运动学的研究方法建立了串联机械手运动模型,并用Visual Basic 6.0通过逆运动
学对其运动轨迹的规划做分析。 首先,本文建立了参考坐标系和运动坐标系,给出串联机械手在这两个坐标系中的描述方法, 并且定义了串联机械手的机构参数,以及相关数学知识的介绍。在此基础上,建立起了串联机械手 的正解模型。机械手的动作过程由三菱公司生产的PLC实行控制。其次,通过利用串联机械手的逆解 模型结论,对机械手的轨迹进行分析。此过程中 ,应用了功能强大的Visual Basic 6.0,进行自动 运算,并利用其可视化功能做出简单人机交流界面,计算并显示出机械手的轨迹曲线,使得机械手 的轨迹规划更为直观、易懂。 本文的研究工作对串联关节型机械手的设计具有一定的指导意义。运用本文的研究方法,可以 有效提高机械手的设计质量和缩短设计周期。

关键词:机械手,正解模型,Visual Basic 6.0,PLC,轨迹规划

Design of Handing Manipulator Control System
Abstract:This paper studies the use of kinematic method of Tandem manipulator motion
model, using Visual Basic 6.0 inverse kinematics through its trajectory planning to do the analysis. First, this paper set up a reference coordinate system and coordinates movement, given the description of tandem manipulator in this two coordinates, and the definition of the series manipulator body parameters and the associated mathematical knowledge presentation. On this basis, established a series of positive manipulator model. Mechanical movement of the hands from the process of production of Mitsubishi Corporation PLC control. Secondly, through the use of serial manipulator inverse kinematics model concludes that the manipulator trajectory analysis. During this process, the application of the powerful Visual Basic 6.0, automatic operation, and the use of visualization functions made simple man-machine interface exchange, calculated and shown manipulator curves, make manipulator trajectory planning more intuitive, understandable. The research of this paper is very useful for the chain-structured robot. it willimprove robot design quality and shorten the design time.

Key words:Manipulator, positive solution model, Visual Basic 6.0, PLC, trajectory
planning

1

第 1 章 引言
1.1 工业机器人的发展和现状
机器人是一种被设计用来移动物体、部件、工具或者特定设备的,可以重复编程、 具有多种功能的操作器.它通过一系列可变的程控动作来完成各种各样的任务。通常工 业机器人大致分为以下三类: 1) 串联结构机器人 2) 框架结构机器人 3) 并行机构机器人 从结构上看,并行结构机器人的运动平台通过相互关联的多个运动链与下平台相 连,这使并行结构机器人具有刚性高、承载能力大和精度好等特点。它的主要缺点是控 制复杂,使得它很难应用高阶控制方法,从而妨碍了这种机器人的应用。框架结构机器 人工作空间比较小,操作灵活性受到限制。 串联结构机器人具有较大的工作空间和较高的运动灵巧度, 常用的结构是串联关节 形式,它采用串联链式结构和全关节驱动,容易建立运动学和动力学模型,可以采用一 些先进的、实时的控制算法,其动作灵活,避障性好。由于串联结构机器人采用悬臂梁 式的手臂结构和关联的驱动系统, 使得该类机器人存在着一些固有缺陷如操作精度性欠 佳,难以完全平衡,且运动存在耦合等问题,但总体数量目前应用越来越多。在本文中 研究的机器人是串联机构机器人的一类一一串联关节机器人,如图 1.1 所示。

图 1-1 垂直关节机器人示意图 这类机器人的基本结构是串联的六自由度开链式,关节相互垂直或平行,每个关节 都有单独的驱动机构。前三个关节(包括基座、腰部和臂部)具有三个转动自由度,确定
2

手部在空间的位置, 这三个关节和联接他们的杆件所构成的机构称为机械手的位置机构; 后三个关节(腕部)的主要功能是确定手部在空间的姿势, 这三个关节和联接他们的杆件 所构成的机构称作姿势机构。位置机构可基本确定机械手的工作空间范围,前三个关节 的运动称作机械手的主运动;后三个关节的运动称作姿态运动,姿态机构主要确定机械 手的工作姿态。所有的关节驱动器共同参与控制,来完成机械手的六自由度运动,以实 现终端执行器按一定姿态到达工作空间的每个工作点, 本文主要讨论除手指关节外的其 它五个自由度。 串联关节机器人一般可用一个开环关节链来建模, 此链由数个刚体(杆件)用驱动器 驱动的移动或转动关节串联而成,开链的一端固接在基座上,另一端是自由的,安装着 工具(终端执行器),用以操纵物体,或完成装配作业。这类机械手属于空间机构,运动 通常只用转动和移动两类。用转动相联的关节成为转动关节;以移动相联的关节成为移 动关节。单独驱动的主动关节数目成为机械手的自由度数。本文要研究串联关节机器人 的五个关节都是转动关节。 随着技术的进步,串联结构机器人本体结构近 10 年来发展变化很快,从开始的含 有局部闭链的平行四边形机构,到改变为大储采用新型的非平行四边形的单连杆机构, 工作空间有所增加,本体自重进一步减小,变得更加轻巧,新材料的采用如轻质铝合金 材料的应用,大大提高了机器人的性能,并逐渐成为串联结构机器人普遍采用的结构之 一。

1.2 课题的提出及意义
进入 20 世纪 90 年代以来,由于具有一般功能的传统工业机器人的应用趋向饱和, 而许多高级生产和特种应用则需要具有各种智能的机器人参与, 因而促使智能机器人获 得较为迅速的发展。无论从国际或国内的角度来看,复苏和继续发展机器人产业的一条 重要途径就是开发各种智能机器人,以求提高机器人的性能,扩大其功能和应用领域。 回顾近 10 多年来国内外机器人技术的发展历程,可以归纳出下列趋势: 1)传感型智能机器人发展较快 2)开发新型智能技术 3)采用模块化设计技术 4)机器人工程系统呈上升趋势 5)微型机器人的研究有所突破 6)应用领域向非制造业和服务业扩展 从整个工业领域来看,对工业机器人需求越来越大,性能指标越来越高。运动学系 统是工业机器人的底层核心部分,对其关键技术,如运动学建模、运动学方程的求解、

3

运动空间插值算法等的研究,将从很大程度上决定着一个机器人系统的基本性能。随着 科技的发展,机器人已成为工业的现代化程度的标志,机器人学是一门高度交叉的前沿 学科,与机械学、生物学、人类学、计算机科学与工程、控制论与控制工程学、人工智 能、社会学等。机器人学包含机器人运动学、机器人动力学、机器人控制、机器人智能 化等领域有着不同层次意义的联系,本文以对机器手的控制及运动学系统为主线,对如 何实现机器手动作作了较深入的研究与讨论。 总的说来,对于机器人控制及运动系统的研究在理论和应用上都具有重要的意义。 本文基于上述的串联关节机器人为研究对象,对 GR-1 型教学机械手的运动及控制进行 研究。

1.3 本文的研究内容和主要工作
本文以 GR-1 型教学机械手为研究对象,对其进行运动学求解;并利用其结果对机 械手的轨迹进行规划;最后利用其轨迹规划的结果,对机械手的运动进行自动控制。该 内容由两人共同完成,本人具体完成以下工作: 1、对机械手进行位姿描述,应用 D-H 参数法建立串联关节机器人杆件坐标系,求 解位姿正解。 2、对机械手的轨迹进行规划,采用三次多项式插值运算的方法,利用由机器人运 动学解得的关节变量求得运动轨迹方程。 3、用 Visual Basic 6.0 软件做出人机交互界面,在界面上显示计算结果和轨迹曲 线图。 4、编写机械手控制程序,通过 Visual Basic 6.0 界面将结果输出并将控制数据输 入 PLC,实现对机械手的自动控制。

4

第 2 章 PLC 控制系统及实现
2.1 机械手及控制器主要参数
1 机械手

a、自由度:5(不包括手指开闭控制) b、各关节活动范围: 腰: ≥ ? 180
肩: 肘: 手俯仰: ≥ 200 ≥270。 ≥ 270

手指旋转:≥ 360

c、最大活动区域:
垂直方向:850mm 水平方向:600mm

d、基本尺寸:
上、下臂长:228mm 手后部:73mm

e、各轴分辨率如表 2-1:
表 2-1 各轴分辨率参数 轴 躯干 上臂 下臂 手俯仰 手指旋转 2 电机 工作电压:U = ? 12 V 空载转速:n = 3000 r/min 空载电流:I = 0.15 A 电机代号 M1 M2 M3 M4 M5 分辨率(度/脉冲) 0.14 0.11 0.11 0.11 0.24

5

2.2 简述机械手动作的实现
GR—1 型教学机械手主要由机械和控制两大部分组成,本论文主要对其控制部分进
行研究。 该机械手采用的是串联式关节机械手,全部采用开链式,结构简单,整个机械手分 5 个自由度:腰、肩关节、肘关节、腕关节和手指,全部有直流电机驱动。控制功能的 实现主要是通过对控制器的编程,由光电编码器检测各电机的旋转角位移,再结合 PLC 来控制各个电机的角位移, 从而实现对机械手各个关节较精确的运动以实现机械手预期 的动作。 光电编码器起控制直流电机角位移精度作用,固定于电机转轴上与电机同步运转。 系统工作时,各光电编码器输出一系列与直流电机运转角度相对应的脉冲信号,经转换 电路将序列脉冲电压信号转换成 PLC 可以接收的开关信号, 在 PLC 内部通过计数器计数 后得到一个电机实际的角位移信号,运行 PLC 控制程序,获得确定输出,再通过外电路 由 PLC 的输出信号来控制电机的启动停止,以实现机械手的预期动作。下面将对 PLC 如 何控制直流电机来实现机械手动作实现的方案来做说明。

2.3 控制软件的设计
实现 PLC 对机械手的控制, 要将各杆件的运动控制转换成对机械手各电机角位移的 控制。要使机械手手抓到达预期的位置就要计算出每个关节运动的角度,即每个电机所 应转的角度,这可通过对各空间坐标系的相对运动在运动学基础上计算得到,最后,将 其转换成 PLC 内部控制指令即可实现软件控制。 2.3.1 PLC 的选择

PLC 类型的选择主要考虑两个方面:输入输出点的数目和存储容量。容量有计算公
式: 指令条数=(输入点数+输出点数) ? (10--12) 再留有 15%左右的备用量,根据外部电路的特点得到输入、输出点的数目,最后选 定用三菱公司生产的 FX2N—64MR 型 PLC。具体计算和分析过程参照同组同学毕业设计 说明书。 2.3.2 光电编码器 机械手各个电机角位移的准确控制是通过光电编码器反馈的信息来实现的, 每个直 流电机输出轴上都装有光电编码器,通过它实现光电脉冲转换及对电机转角的检测。光 电编码器主要是由控制电路板、光电耦合器及遮光盘组成。随码盘旋转,输出一系列计 数脉冲。增量式编码器需要预先指定一个基数:零位。输出脉冲相对于基数进行加减, 从而测量出码盘位移,即电机的角位移。码盘固定在电机转轴上,随电机的旋转,编码
6

器产生代表角位移的脉冲。 光电编码器输出包括 A、B 两个频道的信号,码盘为 6 孔均匀分布的金属盘。当总 线通电时,光电耦合器的发光二极管发出不可见红外线光,通过旋转的码盘通光孔而被 光敏器接收,光敏器件两端由截止变为导通,两端电压发生变化,从 A、B 输出信号, 码盘每旋转一周发送 12 次信号 2.3.3 接口电路 1、输入接口 输入接口有位置传感器信号的输入,限位信号输入以及手动开关的接入。

FX2N—64MR 提供 21 个高速计数器,即 C235 至 C255,但它们共享同一个 PLC 上的
6 个高速计数器输入端(X0~X5)。即如果输入已经被某个计数器占用,它就不能再用 于另一个高速计数器(或其他用途)。也就是说,由于只有 6 个高速计数器的输入,因 此,最多同时用 6 个高速计数器。在本文中将用到的是没有启动/复位的单相高速计数 器 C235~C239。计数器端口分配如表 2-2。 表 2-2 高计数器表 输入 C235 C236 C237 C238 C239 X0 U/D X1 U/D X2 U/D X3 U/D X4 U/D 注:U—增计数输入;D—减计数输入。 2、输出接口 输出接口主要接电机,由于五个电机都要实现正反转,即能实现正反向供电,每个 电机分配 4 个输出端口。 接线电路及其设计参考同组同学设计说明书。 2.3.4 控制原理及程序
1、机械手动作控制原理

程序启动后,各个电机按照程序要求逐个开始运动使得机械手开始动作。首先,腰 部电机 M1 按程序指定要求正向转动,相应计数器开始对光电编码器返回的信号进行计 数,计数器计满数据后,PLC 输出控制指令,使 M1 停止转动,同时依次逐个启动控制 电机 M2、M3、M4、M5 开始转动,各个电机控制方式的实现同电机 M1。当电机 M5 动作 完毕后, 整个机械手完成了其全部动作, 由规定的起点到达终点。 流程图如图 2-1 所示。

7

PLC



计 数 器
C238

手 腕 转 动

电机 M1 启动



计 数 器
C235

腰 部 转 动



计 数 器
C239

手 指 转 动

M1 停,M2 启动

M5 停止,整个动 作完成



计 数 器
C236

上 臂 转 动

M2 停,M3 启动

图 2-1 机械手动作流程图

2 编写控制程序 搬运机械手动作由电机驱动各个关节实现,转动角度由计数器计数控制,计数器值 满后断电使电机停止运转。各计数器的值由表 2-1 和轨迹规划的结果表 5-4 确定如表 2-3。
表 2-3 各计数器数值

计数器 数值 梯形图见附录。

C235 4

C236 147

C237 118

C238 31

C239 16

8

第 3 章 机械手运动学系统
3.1 引言
机器人运动学主要有以下两个基本问题: (1)对一给定机器人,已知杆件几何参数和关节变量,求末端执行器相对给定坐 标系的位置和姿态。给定坐标系为固定在大地上的笛卡儿坐标系,作为机器人的总坐标 系,也称为世界坐标系(World Coordinate)。 (2)已知机器人的杆件参数,给定末端执行器相对于总体坐标系的位置和姿态, 确定关节变量的大小。 第一个问题常称为运动学正问题(DKP-Direct Kinematic Problems),第二个问 题通常称为运动学逆问题(IKP-Inverse Kinematic Problems)。机器人手臂的关节变 量是独立变量,而末端执行器的作业通常在总体坐标系中说明。根据末端执行器在总体 坐标系中的位姿来确定相应各关节变量要进行运动学逆题的求解。 机器人运动学逆问题 是编制机器人运动控制系统软件所必备的知识。

3.2 机械手运动学数学基础
机械手是机器人系统的机械运动部分. 为了描述机械手的操作, 必须建立机械手各 连杆间以及机械手与周围环境间的运动关系, 研究机械手的运动, 不仅涉及机械手本身, 而且涉及各物体间以及物体与机械手的关系, 齐次坐标及其变换就是用来表达这些关系 的.齐次坐标交换不仅能够表示运动学问题,而且能够表达机械手控制算法、计算机视 觉和计算机图形学等问题。 3.2.1 机器人位置与姿态的描述 在描述物体,如零件、工具或机械手间的关系时,要用到位置矢量、平面和坐标系 等概念,对于工业机器人运动学系统的描述就是建立在这些概念的基础上的。 1.位置描述 描述物体(如零件、工具或机械手)间关系时,一旦建立了一个坐标系,我们就能够 用某个 3 ? 1 位置矢量来确定该空间内任一点的位置.对于直角坐标系{A},空间任意一 A 点的 P 位置可用 3 ? 1 的列矢量 P

9

zA

A

P

O

yA

xA

图 3-1 位姿表示
A

? px ? ? P?? ? py ? ? ? pz ? ?

(3.1)

A 表示。其中, px 、 py 、 pz 是点 P 在坐标系{A}中的三个坐标分量 P 的上标 A 代表 A 参考坐标系{A}。我们称 P 为位置矢量,如图 3-1 所示。

2.位姿描述 为了研究机器人的运动与操作,往往不仅要表示空间某个点的位置,而且需要表示 物体的方位。物体的方位可由某个固接于此物体的坐标系描述。为了规定空间某刚体 B 的方位,设置一个直角坐标系{B}与此刚体固接。用坐标系{B}的三个单位主矢量

xB , yB , zB 相对于参考坐标系{A}方向余弦组成的 3×3 矩阵:
A B

R?? ? xB
A

A

yB

A

? r11 ? zB ? ? ? ? r21 ? ? r31

r12 r22 r32

r13 ? r23 ? ? r33 ? ?

(3.3)

A 来表示刚体 B 相对于坐标系{A}的方位。 B R 称为旋转矩阵。式中,上标 A 表参考坐 A 标系{A},下标 B 代表被描述的坐标系{B}. B R 共有 9 个元素,但只有 3 个是独立的.由 A 于B R 的三个列矢量 A xB , A yB 和 A

z B ,都是单位矢量,且双双相互垂直,因而它的 9 个
(3.3) (3.4)

元素满足 6 个约束条件(正交条件)。 A xB . A xB ? A yB . A yB ? A zB . A zB ? 1
A

xB . A yB ? A yB . A zB ? A zB . A xB ? 0

对应于轴 x,y 或 z 作转角为θ 的旋转变换,其旋转矩阵分别为:
?1 0 Rot ( x, ? ) ? ? ?0 c? ? ? 0 s? 0 ? ? s? ? ? c? ? ?

(3.5)

10

? c? Rot ( y, ? ) ? ? ? 0 ? ? ? s? ? c? Rot ( z , ? ) ? ? ? s? ? ?0

0 s? ? 1 0? ? 0 c? ? ? ? s? c? 0 0? 0? ? 1? ?

(3.6)

(3.7)

zA
{A}

OB

{B}

zB

OA

yA

xA

图 3-2 方位描述

式中, s 表示 sin , c 表示 cos 。图 3-2 表示一个物体的方位,此物体与坐标系{B} 固接,并相对于参考坐标系{A}运动。

3.3 空间齐次坐标变换
3.3.1 坐标变换 空间中任意点 P 在不同坐标系中的描述是不同的。 为了阐明从一个坐标系的描到另 一个坐标系的描述关系,需要讨论这种变换的数学问题。坐标变换一般有两种形式:平 移坐标变换和旋转坐标变换。设坐标系{B}与{A}具有相同的方位,但{B}坐标系的原点 A 与{A}的原点不重合。 用位置矢量 PB ,描述它相对于{A}的位置,如图 3-3 所示。称 A P
B

为{B}相对于{A}的平移矢量。如果点 P 在坐标系{B}中的位置为 P ,那么它相对于坐标 A 系{A}的位置矢量 P 可由矢量相加得出,即
A

B

P = B P + A PB

(3.8)

式(3.8)被称为坐标平移方程。

11

zR zA {A}
A

{B}
B

p oR

p yR

oA xA

A

yA

pB

xR

图 3-3 平移变换 设坐标系{B}与{A}有共同的坐标原点,但两者的方位不同,如图 3-4 所示.用旋转
A 矩阵 B R 描述{B}对于{A}的方位。同一点 P 在两个坐标系{A}和{B}中的描述 B P 和 A P 具

有如下变换关系:
A

A P=B R BP

(3.9)

称(3.9)式为坐标旋转方程。我们可以类似地用穿 R 描述坐标系{A}相对于{B}的方
A B 位。 B R和A R 都是正交矩阵,两者互逆。

对于最一般的情形:坐标系{B}的原点与{A}的原点既不重合,{B}的方位与{A}的方
A 位也不相同。用位置矢量 A PB 描述{B}的坐标原点相对于{A}的位置:用旋转矩阵 B R 描述

{B}相对于{A}的方位,如图 3-4 所示。

yC zA {A}

yA xA 图 3-4 平移变换与旋转变换

A B 对于任一点 P 在两坐标系{A}和{B}中的描述 P 和 P 具有以下变换关系:

A

A B P? B R P ? APB

(3.10)

3.3.2 齐次坐标变换 己知一直角坐标系中的某点坐标, 那么该点在另一直角坐标系中的坐标可通过齐次
12

坐标变换求得。
B 变换式(3.10)对于点 P 而言是非齐次的,但是可以将其表示为等价的齐次变换形


A ? AP? ? B R ? ? ? ? ?1? ? 0 A

PB ? ? B P ? ??? ? 1 ? ?1?

(3.11)

A B 其中,4x1 的列矢量表示三维空间的点,称为点的齐次坐标,仍然记为 P 或 P 。

可把式(3.11)写成矩阵形式
A B P=A P BT

(3.12)

A B 式中,齐次坐标 P 或 P 是 4x1 的列矢量。齐次变换矩阵 A BT 是 4x4 的方阵,具有

如下形式
A B

? AR T ? ?B ? 0

A

PB ? ? 1 ?

(3.13)

A B

T 综合地表示了平移变换和旋转变换。

3.3 点在空间直角坐标系中绕过原点任意轴的一般旋转变换
1.RPY 角(绕固定轴 X-Y-Z 旋转)

RPY 角是描述船舶在海中航行时的姿态的一种方法。将船的行驶方向取为 Z 轴,则 绕 Z 轴的旋转( ? 角)称为滚动(Roll);把绕 Y 轴的旋转( ? 角)称为俯仰(Pitch);
而把垂直方向取为 X 轴,将绕 X 轴的旋转( ? 角)称为偏转(Yaw),操作臂手抓姿态 的规定方法类似,故习惯上称为 RPY 角方法。 2.描述活动坐标系方位的法则如下: 活动系的初始方位与参考系重合,首先将活动系绕参考系的 X 轴旋转 ? 角,再绕参 考系的 Y 轴转 ? 角,最后绕参考系的 Z 轴转 ? 角, 因三次旋转都是相对于参考系的,所以得相应的旋转矩阵
RPY (? , ? , ? ) ? Rot ( z, ? ) Rot ( y, ? ) Rot ( x, ? )

(3.14)

?c? ? s? RPY (? , ? , ? ) ? ? ?0 ? ?0

? s? c? 0 0

0 0 1 0

0 ? ? c? ? 0? ?? 0 0? ? ?s? ?? 1? ? 0

0 s? 1 0 0 c? 0 0

0 ? ?1 0 ? 0? ? ?0 c? 0 ? ?0 s? ?? 1 ? ?0 0

0 ? s? c? 0

0? 0? ? 0? ? 1?

其中 c? ? cos ? , s? ? sin ? 。将矩阵相乘得

13

?c? c ? ? s? c ? RPY (? , ? , ? ) ?? ? ? ?s? ? ? 0
旋转的 RPY 角法。令: ? nx ?n RPY (? , ? , ? ) ? ? y ? nz ? ?0

c? s ? s? ? s? c? s? s ? s? ? c? c? c ? s? 0

c ? s ? c ? ? s? s? s? s ? c? ? c? s? c ? c? 0

0? 0? ? 0? ? 1?

(3.15)

它表示绕固定坐标系的三个轴依次旋转得到的旋转矩阵,因此称为绕固定轴 X-Y-Z

ox oy oz 0

ax ay az 0

0? 0? ? 0? ? 1?

(3.16)

(1)由给定的旋转矩阵求出等价的绕固定轴 X-Y-Z 的旋转角 ? 、?、? 。 式(3.16)中有 3 个未知数,共 9 个方程,其中 6 个方程不独立,因此可以利用其 中 3 个方程解出未知数。 由式(3.15)、(3.16)可以看出:
2 2 cos ? ? nx ? ny

(3.17)

如果 cos ? ? 0 ,则得到各个角的反正切表达式:

? ? ? A tan 2(?n , n 2 ? n 2 ) z x y ? ? (3.18) ? ? A tan 2(ox , nx ) ? ? ? ? A tan 2(a y , az ) ? ? 式中, A tan 2( y, x) 是双变量反正切函数,用其计算 arctan(y/x)的优点在于利用了
X 和 Y 的符号能够确定所得角度所在的象限这一现象。 式(3.17)中的根式一般有两个解,我们总是取 ?90 ? ? ? 90 中的一个解。 (2)从给定的绕固定轴 X-Y-Z 的旋转角 ? 、?、? ,阵求出等价的旋转矩阵。 将给定的角 ? 、?、? 代入式(3.15)与式(3.16)中元素对应相等即可以求得所求 矩阵。

14

3.4 Denavt-Hartenberg(D-H)表示法

图 3-4 关节参数规定 机械手由一串用转动或平移关节连接的刚体(杆件)组成。每一对关节-杆构成一 个自由度。杆件编号由手臂的固定机座开始,固定机座可看成杆件 0,第一个运动体是 杆件 1,依次类推,最后一个杆件与工具相连;关节 1 处于连接杆件 1 和基座之间,每 个杆件至多与另外两个杆件相联,而不构成闭环。 个关节轴线的最短距离;杆件 i 的扭转角 ?i ,是两个关节轴线的夹角。 任何杆件 i 都可以用两个尺度表征,如图 2.5 所示,杆件 i 的长度 ai ,是杆件上两

通常,在每个关节轴线上连接有两根杆件,每个杆件各有一根和轴线垂直的法线。 两个杆件的相对位置由两杆间的距离 d i(关节轴上两轴间法线的距离)和夹角 ? i (关节 轴上两个法线的夹角)确定。 为描述相邻杆件间平移和转动的关系。Denavit 和 Hatenberg(1955)提出了一种 为关节链中的每一杆件建立附体坐标系的矩阵方法。D-H 方法是为每个关节处的杆件坐 标系建立 4 ? 4 齐次变换矩阵,表示它与前一杆件坐标系的关系。这样逐次变换,有“手 部坐标”表示的末端执行器可被变换并用机座坐标表示。 3.4.1 坐标系的建立

N 关节机器人需建立 n+1 个坐标系,其中参考(机座)坐标系为 O0 x0 y0 z0 ,机械手 末端的坐标系为 On x n yn zn ,第 i 关节上的坐标系为 Oi ?1 xi ?1 yi ?1 zi ?1 。确定和建立每个坐标
系应根据下面 3 条规则: (1) zi ?1 轴沿着第 i 关节轴的运动轴; (2) xi 轴垂直于 zi ?1 和 z i 轴并指向离开 zi ?1 轴的方向;
15

按照这些规则,第 0 号坐标系在机座上的位置和方向可任选,只要 z0 轴沿着第 1 关节运动轴。第 n 坐标系可防在手的任何部位,只要 x n 轴与 zn ?1 轴垂直。 3.4.2 几何参数定义 根据上述对杆件参数及坐标系的定义, 描述串联机器人相邻坐标系之间的关系可归 结如下 4 个参数:

(3) yi 按右手坐标系的要求建立。

?i
第 i 关节

zi

zi ?1 ai zi ? 2 Oi
第 i ? 1 关节

xi

O'i ?1

?i
di

O' i ? 2

Oi ?1

xi ?1

图3-5 杆件的参数和坐标系

? i :绕轴(右手规则)由 zi ?1 轴向 xi ?1 轴的关节角;
d i :从第 i ? 1 坐标系的原点到 zi ?1 轴和 xi 轴的交点沿 zi ?1 轴的距离; ai :从 zi ?1 和 xi 的交点到第 i 坐标系原点沿 xi 轴的偏置距离(或者说,是 zi ?1 和 zi 两
轴间的最小距离); ?i :绕 xi 轴(右手规则)由 zi ?1 轴转向 zi 轴的偏角。

对于转动关节 d i 、 ?i 和 ai 是关节参数, ? i 是关节变量。移动关节的关节参数是 ai 、 ?i 和 ? i , d i 是关节变量。

3.4.3 建立 i 坐标系和 i ? 1 坐标系的齐次变换矩阵 将第 i 个坐标系的点 ri 在 i ? 1 坐标系表示,需建立 i 坐标系和 i ? 1 坐标系的齐次变换 矩阵,因而需经过以下变换: 1、将坐标系 Oi ?1 xi ?1 yi ?1 zi ?1 绕 zi ?1 轴转 ? i 角,使 xi ?1 轴与 xi 轴平行并指向同一方向; 2、将坐标系 Oi ?1 xi ?1 yi ?1 zi ?1 沿 zi ?1 轴平移距离 d i ,使 xi ?1 轴与 Oi xi yi zi 的 xi 轴重合; 3、将坐标系 Oi ?1 xi ?1 yi ?1 zi ?1 沿 xi ?1 轴平移距离 ai ,使两坐标系的原点重合;
16

4、将坐标系 Oi ?1 xi ?1 yi ?1 zi ?1 绕 xi ?1 轴转 ?i 角,使两坐标系完全重合。 i ?1 从而, i 坐标系和 i ? 1 坐标系的齐次变换矩阵 Ai 可以根据矩阵的合成规则得到,
i ?1

Ai 称为相邻坐标系 i 和 i ? 1 的变换矩阵。即(由算子左、右乘规则得)
i ?1

Ai = Rz ,? Tz ,d Tx,? Rx,?
? s?i c?i 0 0 0 0 1 0 0 ? ?1 ? 0? ? ?0 0 ? ?0 ?? 1 ? ?0 s? i s?i ? s? i c?i c? i 0 0 1 0 0 0 0 ? ?1 ? 0 0? ? ?0 1 di ? ?0 ?? 0 1 ? ?0 ai c?i ? ai s?i ? ? di ? ? 1 ? 0 1 0 0 0 ai ? ?1 0 ? 0 0? ? ?0 c? i 1 0 ? ?0 s? i ?? 0 1 ? ?0 0 0 ? s? i c? i 0 0? 0? ? 0? ? 1?

?c?i ? s? ?? i ?0 ? ?0 ?c?i ? s? ?? i ?0 ? ?0

?c? i s?i c? i c?i s? i 0

(3.19)

式中, s 表示 sin , c 表示 cos 。 对于在第 i 个坐标系的点 ri 在 i ? 1 坐标系表示为:
0 确定第 i 坐标系相对于机座坐标系的位置的齐次变换矩阵 Ti 是各齐次变换矩阵 i ?1

ri ?1 ? i ?1 Ai ri

Ai 的连乘积,可表示成
0

Ti ? 0 A1 1 A2 ,…,i-1 Ai =? j ?1 Aj
j ?1

i

是固连杆在杆件 i 上的第 i 个坐标系的姿态矩阵, Pi 是由机座坐 0 标系原点指向第 i 个坐标系原点的位置矢量。特别当 i=6 时,求得 T 矩阵,T ? A6 ,它 确定了机械手的末端相对于机座坐标系的位置和状态。可以把 T 矩阵写成 ? nx ox ax px ? ? ? 0 0 ? n6 o6 a6 P6 ? ? R6 P6 ? ? n o a P ? ? n y o y a y p y ? T ?? ??? ??? ?? 1 ? ? 0 0 0 1 ? ? nz oz az pz ? ?0 0 0 1? ? 0 ? ? ?0 0 0 1 ? 式中, n 为手的法向矢量, o 为手的滑动矢量, a 为手的接近矢量, P 为手的位置 矢量。

式中, ?

ni

oi

ai ?

?n T ?? i ?0

oi 0

ai 0

Pi ? ? 0 Ri ?? 1? ? ? 0

0

Pi ? ? 1 ?

17

第 4 章 机器人运动学方程的求解
4.1 机器人正向运动学

4.2 机器人逆向运动学
参考同组同学论文逆向运动学部分可得如下结果:

18

?1 ? arctan ?

? py ? ? , ?1? ? ?1 ?180 p ? x?
? s1nx ? c1n y ? s o ?c o ? 1 x 1 y ? ? ? ?5 ? 180 , ?5 ? ? ?

?5 ? arctan ?

? 234 ? arctan

oz ? ? ?234 ?180 , ?234 c1ox ? s1oy

c3 ?

( px c1 ? py s1 ? c234a4 )2 ? ( pz ? s234a4 )2 ? a22 ? a32 2a2 a3

s3 ? ? 1 ? c32

?3 ? arctan
s2 ? c2 ?

s3 c3

(c3a3 ? a2 )( pz ? s234a4 ) ? s3a3 ( pxc1 ? p y s1 ? c234a4 ) (c3a3 ? a2 )2 ? s32 a32 (c3a3 ? a2 )( pxc1 ? p y s1 ? c234a4 ) ? s3a3 ( pz ? s234a4 ) (c3a3 ? a2 )2 ? s32 a32

?2 ? arctan

(c3a3 ? a2 )( pz ? s234 a4 ) ? s3a3 ( px c1 ? py s1 ? c234a4 ) s2 ? arctan c2 (c3a3 ? a2 )( px c1 ? py s1 ? c234a4 ) ? s3a3 ( pz ? s234a4 )

因 ?234、?2、?3 均已知,所以可进而求得 ?4 。

?4 ? ?234 ? ?2 ? ?3
以上结果,给出了机械手置于任何期望位姿时所需的关节值,为下一章轨迹规划所 用。

19

第 5 章 机械手轨迹规划
5.1 机器人轨迹的概念
机器人轨迹泛指工业机器人在运动过程中的运动轨迹,即运动点的位移、速度和加 速度。 机器人在作业空间要完成给定的任务, 其手部运动必须按一定的轨迹(trajectory) 进行。 轨迹的生成一般是先给定轨迹上的若干个点, 将其经运动学反解映射到关节空间, 对关节空间中的相应点建立运动方程,然后按这些运动方程对关节进行插值,从而实现 作业空间的运动要求,这一过程通常称为轨迹规划。工业机器人轨迹规划属于机器人低 层规划,基本上不涉及人工智能的问题,本章仅讨论在关节空间或笛卡尔空间中工业机 器人运动的轨迹规划和轨迹生成方法。 机器人运动轨迹的描述一般是对其手部位姿的描述, 此位姿值可与关节变量相互转 换。控制轨迹也就是按时间控制手部或工具中心走过的空间路径。

5.2 轨迹的生成方式
运动轨迹的描述或生成有以下几种方式: (1) 示教-再现运动。这种运动由人手把手示教机器人,定时记录各关节变量,得 到沿路径运动时各关节的位移时间函数 q(t);再现时,按内存中记录的各点的值产生 序列动作。 (2) 关节空间运动。这种运动直接在关节空间里进行。由于动力学参数及其极限值 直接在关节空间里描述,所以用这种方式求最短时间运动很方便。 (3) 空间直线运动。 这是一种直角空间里的运动, 它便于描述空间操作, 计算量小, 适宜简单的作业。 (4) 空间曲线运动。 这是一种在描述空间中用明确的函数表达的运动, 如圆周运动、 螺旋运动等。 5.2.1 轨迹规划涉及的主要问题 为了描述一个完整的作业,往往需要将上述运动进行组合。通常这种规划涉及到以 下几方面的问题: (1) 对工作对象及作业进行描述,用示教方法给出轨迹上的若干个结点(knot)。 (2) 用一条轨迹通过或逼近结点,此轨迹可按一定的原则优化,如加速度平滑得到 直角空间的位移时间函数 X(t)或关节空间的位移时间函数 q(t); 在结 点之间如何进行
20

插补,即根据轨迹表达式在每一个采样周期实时计算轨迹上点的位姿和各关节变量值。 (3) 以上生成的轨迹是机器人位置控制的给定值, 可以据此并根据机器人的动态参 数设计一定的控制规律。 (4) 规划机器人的运动轨迹时,尚需明确其路径上是否存在障碍约束的组合。一般 将机器人的规划与控制方式分为四种情况,如表 5-1 所示。 表 5-1 机器人的规划与控制方式 障 碍 约 束 有 无 有 离线无碰撞路径规则+在线路 离线路径规划+在线路径跟踪 径跟踪 路径约束 位置控制+ 在线障碍探测 位置控制 无 和避障 本章主要讨论连续路径的无障碍轨迹规划方法。

5.3 插补方式分类
点位控制(PTP 控制)通常没有路径约束,多以关节坐标运动表示。点位控制只要求 满足起终点位姿,在轨迹中间只有关节的几何限制、最大速度和加速度约束;为了保证 运动的连续性,要求速度连续,各轴协调。连续轨迹控制(CP 控制)有路径约束,因此 要对路径进行设计。路径控制与插补方式分类如表 5-2 所示。 表 5-2 路径控制与插补方式分类
路径控制 不插补 关节插补(平滑) (1) 各轴协调运动定时插补。 (2) 各关节最大加速度限制 (1) 在空间插补点间进行关节 定时插补。 (2) 用关节的低阶多项式拟合 空间直线使各轴协调运动。 (3) 各关节最大加速度限制 空间插补 (1) 各轴独立快 点 位 控 制 速到达。 (2) 各关节最大 PTP 加速度限制

连续路径 控制 CP

(1) 直线、圆弧、曲线等距插 补。 (2) 起停线速度、线加速度给 定,各关节速度、加速度限制

5.4 机器人轨迹插值计算
给出各个路径结点后,轨迹规划的任务包含解变换方程,进行运动学反解和插值计 算。在关节空间进行规划时,需进行的大量工作是对关节变量的插值计算。插补分为:
21

直线插补、圆弧插补、定时插补、定距插补和关节空间插补等。 在关节空间中进行轨迹规划,需要给定机器人在起始点和终止点手臂的位形。对关 节进行插值时应满足一系列的约束条件,例如抓取物体时手部的运动方向(初始点)、提 升物体离开的方向(提升点)、放下物体(下放点)和停止点等结点上的位姿、速度和加速 度的要求;与此相应的各个关节位移、速度、加速度在整个时间间隔内的连续性要求以 及其极值必须在各个关节变量的容许范围之内等。满足所要求的约束条件之后,可以选 取不同类型的关节插值函数,生成不同的轨迹。常用的关节空间插补有:三次多项式插 值、高阶多项式插值、用抛物线过渡的线性插值等方法,本章重点对关节空间插补中的 三次多项式插值进行讨论。 5.4.1 三次多项式插值 在机器人运动过程中,若末端执行器的起始和终止位姿已知,由逆向运动学即可求 出对应于两位姿的各个关节角度。 末端执行器实现两位姿的运动轨迹描述可在关节空间 中用通过起始点和终止点关节角的一个平滑轨迹函数 ?(t)来表示。 为实现系统的平稳运动,每个关节的轨迹函数 ?(t)至少需要满足四个约束条件, 即两端点位置约束和两端点速度约束。 端点位置约束是指起始位姿和终止位姿分别所对应的关节角度。?(t)在时刻 t 0 =0 t ? 时的值是起始关节角度 ?0 ,在终端时刻 f 时的值是终止关节角度 f ,即 ? (0) ? ? 0 ? (5.1) ? (t f ) ? 0 ? ? 为满足关节运动速度的连续性要求,起始点和终止点的关节速度可简单地设定为零, 即

? (0) ? 0 ? ?
?

? (t f ) ? 0 ? ?
上面给出的四个约束条件可以惟一地确定一个三次多项式 ? ?t ? ? a0 ? a1t ? a2t 2 ? a3t 3 运动过程中的关节速度和加速度则为 ? ? (t ) ? a1 ? 2a2t ? 3a3t 2 ? ? .. ? ? (t ) ? 2a2 ? 6a3t ? ? 条件,有方程组 (5.5) 求解该方程组,可得 (5.6)

?

?

(5.2)

(5.3)

(5.4)

为求得三次多项式的系数 a0,a1,a2 和 a3,将式(5.1)和式(5.2)代以给定的约束

22

对于起始速度及终止速度为零的关节运动, 满足连续平稳运动要求的三次多项式插 值函数为 5.4.2 机械手轨迹规划。 本文所研究的是五自由度机械手的正运动问题。在起点和终点,末端关节的位姿是 已知的,故可通过前文所讨论的求得两位置时各关节角度。因此,可以用平滑的插值函 数来描述机械手的起始位置和目标位置之间的运动轨迹。另外,为了使实际生成的轨迹 平滑,在机械手的整个工作期间,除了保证其运动和速度的连续之外,还应该保证加速 度不发生突变,使运动平稳。根据已知条件和前文的讨论,选用三次多项式轨迹规划方 法进行规划。 起点和终点位姿如表 5-3 所示 表 5-3 机械手起点、终点给定值 ? ? pz px py 控制点 ? 起点 151 231 250 32 45 60 终点 200 300 178 20 50 80 由机械手运动学方程的逆解可以求得控制点各个关节变量值,如表 5-4 所示: 表 5-4 机械手起点、终点关节变量值 ?1 ?2 ?4 ?3 ?5 控制点 起点 终点 各关节轨迹的生成 由于在三次多项式的插值计算中,为实现系统的平稳运动,每个关节的轨迹函数 ? (t ) 至少需要满足四个约束条件,即两端点的位置约束和两端点的速度约束。在机器人 正运动过程中,末端执行器的起始和终止位姿已经给出,由逆向运动学即可求得对应于 两位姿的各个关节角度,如表(5-4)所示;再假设在初始位置和目标位置的速度为零, 即在这两个位置的位姿函数的导数为零,即有四个约束条件,满足了解得各个关节的三 次多项式轨迹的要求。最后,根据位移表达式(5.7)代入已知数据求得位移,再分别 求一阶导数,二阶导数即式(5.8)和(5.9)便可求得速度和加速度表达式。下一章将 用 Visual Basic 6.0 对机械手各关节的运动轨迹作介绍。 56.31 56.828 80.922 -66.006 -45.244 73.282 -52.738 -83.861 -24.683 -9.074

23

第 6 章 系统交互界面设计
在 Windows 平台上使用 Visual Basic6.0 (以下简称 VB) 对机械手的轨迹进行规划, 由于 VB 拥有着可视化平台、易学易用的集成开发环境、强大数据功能等特点,使得机 械手的轨迹规划直观、明了的显示出来,使得机械手的整个运动控制过程变得直观操作 更加简单,降低了对使用及控制能力的要求。

6.1Visual Basic6.0 简介
VB 中文版是 Microsoft 公司提供的一种可视化开发工具—Visual Studio98 的一个
组成部分。“Visual”意即可视的,指的是开发像 Windows 操作系统的图形用户界面 (Graphic User Interface, GUI)的方法,它不需要编写大量代码去描述界面元素的外 观和位置,只要把预先建立好的对象拖放到屏幕上相应位置即可。Basic 指的是 BASIC (Beginners All-Purpose Symbolic Instruction Code,初学者通用符号指令代码)语 言—— 一种在计算机技术发展历史上应用的最广泛的语言,也是 Microsoft 公司开发 的第一个程序设计语言。Visual Basic 在原有的 BASIC 语言的基础上进一步发展,至 今己包含了数百条语句、函数和关键词。无论对专业人员还是初学者 VB 都提供了整套 开发工具,可以说是开发 Windows 应用程序最迅速、最简捷的方法。现在,人们可以使 用 VB 提供的工具为自己或工作组创建实用程序及大型企业应用系统,甚至可以创建通 过 Internet 遍及全球的分布式应用程序。

VB6.0 是一种以对象为基础,以事件驱动为编程机制的可视化程序设计语言,功能
强大、简单易学。与普通的数据库系统相比,用 VB 作为数据库开发平台有以下优点: (1)简单。VB 提供了数据控件,利用该控件,用户只要编写少量的代码甚至不编写 任何代码就可以访问数据库,对数据库进行浏览。另外其可视化的用户界面设计功能, 把程序员从繁琐复杂的界面设计中解脱了出来。 (2)灵活。VB 不像一般的数据库(如 Access)那样局限于特定的应用程序结构,也不 需要用某些指令对当前打开的数据库进行操作,因而比较灵活。 (3)可扩充。VB 是一种可以扩充的语言,其中包括在数据库应用方面的扩充。在 Visual Basic 中,可以使用 ActiveX 控件(以前版本中称为 VBX 或 UUH 控件),这些控 件可以由 Microsoft 公司提供,也可以由第三方开发者提供。有了这些控件,可以很容 易地在 Visual Basic 中增加新功能,扩充 VB 数据存取控制的指令系统。在其它数据库 开发平台中,目前还不具备 ActiveX 控件。

24

6.2 VB 设计任务
本系统中 VB 主要是实现人与控制器的交流。首先进入的是一个分区的人机交互界 面,有机械手起点和终点位姿参数输入、各关节变量值显示、各关节轨迹图形的显示和 命令控制区。其中命令控制区有 5 个控件:计算、轨迹规划、输出、写入 PLC 和清除。 当输入位姿参数后,按计算控件得出关节变量,通过轨迹规划在图形显示区显示出各关 节的运动轨迹,最后输出并写入 PLC 以实现对机械手的控制。其运算流程可简要表示如 图 6-1。

开始

输入位姿参数

清除 N

计算关节变量

是否符 合要求

Y

输出

输出变量值

显示位移图形

写入 PLC

轨迹规划

结束

图 6-1 轨迹计算流程 图

6.3 窗体设计
窗体分为 5 个区:输入起点位姿区、输入终点位姿区、输出关节变量区、命令区和 关节运动轨迹区。下面对窗体各个区的设计过程和功能加以说明。 1 位姿输入区 位姿输入区有起点输入和终点输入两个区, 通过这两个区域可以输入机械手在起点 和终点两个时刻的位姿参数,并将其读入,准备计算。界面控件如图 6-1 所示。
25

图 6-1 位姿参数输入界面 2 输出关节变量角 该区域是变量角值的输出区域,VB 将由上述输入区域中的位姿参数通过计算得出 各个关节变量角 ? 的值显示在相应的文本框中。 3 命令区域和关节轨迹区 该区域是整个窗体的控制区域,它包括计算、轨迹规划、输出、写入 PLC、和清除 5 个命令控件。 计算控件是执行将从输入区读取的位姿参数通过给定的算法得出关节变量值, 并输 出在输出关节变量角区的相应文本框中; 轨迹规划控件是利用计算得来的变量角值得出 各关节轨迹表达式,并根据该表达式画出轨迹曲线,在关节运动轨迹区显示各关节轨迹 图,该控件在计算前,或在数据超出范围时呈现灰色,不能对其进行操作;写入 PLC 控 件是实现 PC 机与 PLC 的通信;清除控件是对输入、输出区的数据进行清除。

6.4 窗体运行
1 按计算控件,对输入机械手起点和终点的位姿参数进行判断,当数据符合要求就会得 出计算出并显示关节变量角数值,同时,轨迹控制控件回到可控状态,如图 6-2 所示。

26

图 6-2 运行计算控件 2 运行轨迹规划控件,输出运动轨迹曲线,如图 6-3 所示。该轨迹规划控件在执行 计算之前显灰色,不可用,只有通过计算,并得出输入数据符合要求后该控件才能被操 作。 3.运行输出控件将计算结果及图形输出。 4.写入 PLC 控件的运行是将前一步输出的控制数据写入 PLC 计数器中, 以实现对机 械手运动的控制。 5.清除控件执行后是将整个界面上的数据清空,为下次输入数据做准备。

27

图 6-3 轨迹曲线

28

参考文献
1. 刘极峰,益际明.机器人技术基础[M].北京:高等教育出版社,2006. 2. 张铁,谢存喜.机器人学[M].广州:华南理工大学出版社,2003 年. 3. 从爽,李泽湘.实用运动控制技术[M].北京:电子工业出版社,2006. 4. 钟肇新,范建东.可编程控制器原理及应用[M].广州:华南理工大学出版社,2003 年. 5. 俞建家.Visual Basic 6.0[M].福建:厦门大学出版社,2005 年. 6. 吕伟臣.Visual Basic 6.0 初级编程教程[M].北京:北京大学出版社,2002 年. 7. 龚沛曾.Visual Basic 程序设计教程[M].北京:高等教育出版社,2004 年. 8. 邓星钟.机电传动控制[M].武汉:华中科技大学出版社,2000 年. 9. 张海根.机电传动控制[M].北京:高等教育出版社,2005 年. 10. 大熊.机器人控制[M].北京:科学出版社,2002 年. 11. 丹尼斯.机器人设计与控制[M].北京:科学出版社,2004 年. 12. 刘极峰.计算机辅助设计与制造[M].北京:高等教育出版社,2004 年.

29

致谢
在此次设计完成之际,我首先要感谢毛美姣老师多我的谆谆教诲,感谢她每天陪 同我们在实验室,时刻为我们解答疑团,让我再次感觉到大学老师并不是许多人想象中 的那样可望不可及;相反,比起中学老师显得更加和蔼、平易近人,少了几分严肃。她 对工作呕心沥血,孜孜不倦的追求精神和严谨的治学态度永远值得我们学习。 在本次设计过程中,得到了谭志飞老师的悉心指导,他一丝不苟的治学态度使我 受益匪浅。 本次设计是在毛老师和谭老师的耐心指导下才得以顺利完成,在此衷心感谢两位 老师,感谢他们给我的教导和帮助,感谢他们给我们提供了这么好的环境。

30

附录错误!未找到引用源。 程序清单
1、机械手控制程序梯形图

M8002 RST RST RST RST RST X20 Y40 Y40 Y40 M1 Y1 Y4 C235 Y0 C235 C236 C235 C235 C235 Y40

Y5

M11

M8235

Y0

Y5

C235 K1

Y3

C235 Y40

M2

C236

Y6

Y2

Y7

31

M12

M8236

Y2

Y7

C236 K2

C236

M3

Y40

Y11

Y14 C237

Y10

Y15

M13

M8237 C237 K3

Y10

Y15

C237 Y40

Y13

Y16 C238

Y12

Y17

M14

M8238

Y12

Y17

C238 K4

C239 Y40

Y21

Y30

Y20

Y31

M15

M8239

Y20

Y31

C239 K5
32

X20 Y40 Y40 Y40 M1 Y0 Y5 C235

Y40

Y1

Y4

M11

M8235

Y1

Y4

C235 K1

Y2

C235 Y40

M2

C236

Y7

Y3

Y6

M12

M8236

Y3

Y6

C236 K2

C236

M3

Y40

Y10

Y15 C237

Y11

Y14

M13

M8237

33

Y11 Y114

C237 K3

C237 Y40

Y12

Y17 C238

Y13

Y16

M14

M8238

Y13

Y16

C238 K4

C239 Y40

Y20

Y31

Y21

Y30

M15

M8239

Y21

Y30

C239 K5

X21 Y41 Y41 X6

Y41

Y1

Y4

34

X7

Y3

Y6

X10

Y11

Y14

X11

Y13

Y16

X12

Y20

Y31

END

35

轨迹规划界面 VB 程序如下: Dim a2, a3, a4, PI, a, b, c, d, h, m, n, max, min As Double

Private Sub Command1_Click() Dim Px0, Py0, Pz0, α0, β0, γ0 As Single Dim θ1, θ2, θ3, θ4, θ5, θ234 As Single, c3 As Double Dim Px1, Py1, Pz1, α1, β1, γ1 As Single Dim θf1, θf2, θf3, θf4, θf5, θf234 As Single, cf3 As Double a2 = 228 a3 = 228 a4 = 73 PI = 3.141593

Px0 = Val(Text1.Text) Py0 = Val(Text2.Text) Pz0 = Val(Text3.Text) α 0 = Val(Text4.Text) β 0 = Val(Text5.Text) γ 0 = Val(Text6.Text) Px1 = Val(Text7.Text) Py1 = Val(Text8.Text) Pz1 = Val(Text9.Text) α 1 = Val(Text10.Text) β 1 = Val(Text11.Text) γ 1 = Val(Text12.Text) Command3.Enabled = True If Sqr((Px0 - Px1) * (Px0 - Px1) + (Py0 - Py1) * (Py0 - Py1)) > 850 Then Command3 = False ElseIf Abs(Pz0 - Pz1) > 600 Then Command3.Enabled = False End If θ 1 = (180 / PI) * (Atn(Py0 / Px0)) Text13.Text = Format(θ1, "####.###") θ 5 = (180 / PI) * (Atn((Sin(θ1) * Cos(α0) * Cos(β0) - Cos(θ1) * Sin(α0) * Cos(β0)) / (Sin(θ1) * (Cos(α0) 36

* Sin(β0) * Sin(γ0) - Sin(α0) * Cos(γ0)) - Cos(θ1) * (Sin(α0) * Sin(β0) * Sin(γ0) + Cos(α0) * Cos(γ0))))) Text17.Text = Format(θ5, "####.###") θ 234 = (180 / PI) * (Atn((Cos(β0) * Sin(γ0)) / (Cos(θ1) * (Cos(α0) * Sin(β0) * Sin(γ0) - Sin(α0) * Cos(γ0)) + Sin(θ1) * (Sin(α0) * Sin(β0) * Sin(γ0) + Cos(α0) * Cos(γ0))))) c3 = ((Px0 * Cos(θ1) + Py0 * Sin(θ1) - Cos(θ234) * a4) * (Px0 * Cos(θ1) + Py0 * Sin(θ1) - Cos(θ234) * a4) + (Pz0 - Sin(θ234) * a4) * (Pz0 - Sin(θ234) * a4) - a2 * a2 - a3 * a3) / (2 * a2 * a3) θ 3 = (180 / PI) * (Atn(Sqr(Abs(1 - c3 * c3)) / c3)) Text15.Text = Format(θ3, "####.###") θ 2 = (180 / PI) * (Atn(((Cos(θ3) * a3 + a2) * (Pz0 - Sin(θ234) * a4) - Sin(θ3) * a3 * (Px0 * Cos(θ1) + Py0 * Sin(θ1) + Cos(θ234) * a4)) / ((Cos(θ3) * a3 + a2) * (Px0 * Cos(θ1) + Py0 * Sin(θ1) + Cos(θ234) * a4) + Sin(θ3) * a3 * (Pz0 - Sin(θ234) * a4)))) Text14.Text = Format(θ2, "####.###") θ 4 = θ234 - θ2 - θ3 Text16.Text = Format(θ4, "####.###") θ f1 = (180 / PI) * (Atn(Py1 / Px1)) Text18.Text = Format(θf1, "####.###") θ f5 = (180 / PI) * (Atn((Sin(θf1) * Cos(α0) * Cos(β0) - Cos(θf1) * Sin(α0) * Cos(β0)) / (Sin(θf1) * (Cos(α0) * Sin(β0) * Sin(γ0) - Sin(α0) * Cos(γ0)) - Cos(θf1) * (Sin(α0) * Sin(β0) * Sin(γ0) + Cos(α0) * Cos(γ0))))) Text22.Text = Format(θf5, "####.###") θ f234 = (180 / PI) * (Atn((Cos(β0) * Sin(γ0)) / (Cos(θf1) * (Cos(α0) * Sin(β0) * Sin(γ0) - Sin(α0) * Cos(γ0)) + Sin(θf1) * (Sin(α0) * Sin(β0) * Sin(γ0) + Cos(α0) * Cos(γ0))))) cf3 = ((Px1 * Cos(θf1) + Py1 * Sin(θf1) - Cos(θf234) * a4) * (Px1 * Cos(θf1) + Py1 * Sin(θf1) Cos(θf234) * a4) + (Pz1 - Sin(θf234) * a4) * (Pz1 - Sin(θf234) * a4) - a2 * a2 - a3 * a3) / (2 * a2 * a3) θ f3 = (180 / PI) * (Atn(Sqr(Abs(1 - cf3 * cf3)) / cf3)) Text20.Text = Format(θf3, "####.###") θ f2 = (180 / PI) * (Atn(((Cos(θf3) * a3 + a2) * (Pz1 - Sin(θf234) * a4) - Sin(θf3) * a3 * (Px1 * Cos(θf1) + Py1 * Sin(θf1) + Cos(θf234) * a4)) / ((Cos(θf3) * a3 + a2) * (Px1 * Cos(θf1) + Py1 * Sin(θf1) + Cos(θf234) * a4) + Sin(θf3) * a3 * (Pz1 - Sin(θf234) * a4)))) Text19.Text = Format(θf2, "####.###") θ f4 = θf234 - θf2 - θf3 Text21.Text = Format(θf4, "####.###") End Sub Private Sub Command3_Click() 37

Dim m1, m2, m3, m4, m5, n1, n2, n3, n4, n5 As Double Frame6.Visible = True '关节 1 轨迹 '取值 a = Text13.Text b = Text18.Text c = 10 d = a + (3 / (c * c)) * (b - a) * (10 * 10) - (2 / (c * c * c)) * (b - a) * (10 * 10 * 10) If Val(a) > Val(d) Then m1 = a n1 = d Else m1 = d n1 = a End If Picture1.Cls '清空画图区域 Picture1.Scale (0, m1 + (m1 - n1) / 5)-(12, n1) '设置画图区域 根据需要进行设置大小 Picture1.ForeColor = &H0 Picture1.DrawWidth = 1 '画 X 轴 'Picture1.Line (0, n1)-(5, n1) '标记 t 轴 Picture1.CurrentX = 11.5 Picture1.CurrentY = n1 + (m1 - n1) / 10 Picture1.Print "t" '画 Y 轴 Picture1.Line (0, n1 - 5)-(0, m1 + (m1 - n1) / 5) '标记 Y 轴 Picture1.CurrentX = 0.2 Picture1.CurrentY = m1 + (m1 - n1) / 5 Picture1.Print "Y" '画坐标 Dim i As Single For i = 1 To 10 38

Picture1.CurrentX = i: Picture1.CurrentY = n1 + (m1 - n1) / 10: Picture1.Print i Picture1.Line (i, n1)-(i, n1 + (m1 - n1) / 15) Next i Dim j As Integer For j = 1 To 5 Picture1.Line (0, n1 + (m1 - n1) / 5 * j)-(0.1, n1 + (m1 - n1) / 5 * j) Picture1.CurrentX = 0.1: Picture1.CurrentY = n1 + (m1 - n1) / 5 * j: Picture1.Print Fix(n1 + ((m1 - n1) / 5) * j) Next j Picture1.DrawWidth = 2 Picture1.Line (0, 0)-(0, 0) '归零收笔 '开始画图 For X = 0 To 10 Step 0.001 '0.001 改小可以获得更高的精度 Y = a + (3 / (c * c)) * (b - a) * (X * X) - (2 / (c * c * c)) * (b - a) * (X * X * X) '输入函数 可以写入自己的函数 Picture1.CurrentY = (m1 + n1) / 2 Picture1.CurrentX = 0.5 Picture1.PSet (X, Y), vbRed '标记 X,Y 轴 Next Picture1.CurrentX = 4 Picture1.CurrentY = m1 Picture1.Print "Y = θ 1(t)" '将函数显示在图形上 '关节 2 轨迹 '取值 a = Text14.Text b = Text19.Text c = 10 d = a + (3 / (c * c)) * (b - a) * (10 * 10) - (2 / (c * c * c)) * (b - a) * (10 * 10 * 10) If Val(a) > Val(d) Then m2 = a n2 = d Else m2 = d 39

n2 = a End If Picture2.Cls '清空画图区域 Picture2.Scale (0, m2 + (m2 - n2) / 5)-(12, n2) '设置画图区域 根据需要进行设置大小 Picture2.ForeColor = &H0 Picture2.DrawWidth = 1 '画 X 轴 Picture2.Line (0, n2)-(12, n2) '标记 t 轴 Picture2.CurrentX = 11.5 Picture2.CurrentY = n2 + (m2 - n2) / 10 Picture2.Print "t" '画 Y 轴 Picture2.Line (0, n2 - 5)-(0, m2 + (m2 - n2) / 5) '标记 Y 轴 Picture2.CurrentX = 0.2 Picture2.CurrentY = m2 + (m2 - n2) / 5 Picture2.Print "Y" '画坐标 For i = 1 To 10 Picture2.CurrentX = i: Picture2.CurrentY = n2 + (m2 - n2) / 10: Picture2.Print i Picture2.Line (i, n2)-(i, n2 + (m2 - n2) / 15) Next i For j = 1 To 5 Picture2.Line (0, n2 + (m2 - n2) / 5 * j)-(0.1, n2 + (m2 - n2) / 5 * j) Picture2.CurrentX = 0.1: Picture2.CurrentY = n2 + (m2 - n2) / 5 * j: Picture2.Print Fix(n2 + ((m2 - n2) / 5) * j) Next j Picture2.DrawWidth = 2 Picture2.Line (0, 0)-(0, 0) '归零收笔 '开始画图 For X = 0 To 10 Step 0.001 '0.005 改小可以获得更高的精度 Y = a + (3 / (c * c)) * (b - a) * (X * X) - (2 / (c * c * c)) * (b - a) * (X * X * X) '输入函数 可以写入自己的函数 40

Picture2.CurrentY = (m2 + n2) / 2 Picture2.CurrentX = 0.5 Picture2.PSet (X, Y), vbRed '标记 X,Y 轴 Next Picture2.CurrentX = 4 Picture2.CurrentY = m2 Picture2.Print "Y = θ 2(t)" '关节 3 轨迹 '取值 a = Text15.Text b = Text20.Text c = 10 d = a + (3 / (c * c)) * (b - a) * (10 * 10) - (2 / (c * c * c)) * (b - a) * (10 * 10 * 10) If Val(a) > Val(d) Then m3 = a n3 = d Else m3 = d n3 = a End If Picture3.Cls '清空画图区域 Picture3.Scale (0, m3 + (m3 - n3) / 5)-(12, n3) '设置画图区域 根据需要进行设置大小 Picture3.ForeColor = &H0 Picture3.DrawWidth = 1 '画 X 轴 Picture3.Line (0, n3)-(12, n3) '标记 t 轴 Picture3.CurrentX = 11.5 Picture3.CurrentY = n3 + (m3 - n3) / 10 Picture3.Print "t" '画 Y 轴 Picture3.Line (0, n3 - 5)-(0, m3 + (m3 - n3) / 5) 41 '将函数显示在图形上

'标记 Y 轴 Picture3.CurrentX = 0.2 Picture3.CurrentY = m3 + (m3 - n3) / 5 Picture3.Print "Y" '画坐标 For i = 1 To 10 Picture3.CurrentX = i: Picture3.CurrentY = n3 + (m3 - n3) / 10: Picture3.Print i Picture3.Line (i, n3)-(i, n3 + (m3 - n3) / 15) Next i For j = 1 To 5 Picture3.Line (0, n3 + (m3 - n3) / 5 * j)-(0.1, n3 + (m3 - n3) / 5 * j) Picture3.CurrentX = 0.1: Picture3.CurrentY = n3 + (m3 - n3) / 5 * j: Picture3.Print Fix(n3 + ((m3 - n3) / 5) * j) Next j Picture3.DrawWidth = 2

Picture3.Line (0, 0)-(0, 0) '归零收笔 '开始画图 For X = 0 To 10 Step 0.001 '0.001 改小可以获得更高的精度 Y = a + (3 / (c * c)) * (b - a) * (X * X) - (2 / (c * c * c)) * (b - a) * (X * X * X) '输入函数 可以写入自己的函数 Picture3.CurrentY = (m3 + n3) / 2 Picture3.CurrentX = 0.5 Picture3.PSet (X, Y), vbRed '标记 X,Y 轴 Next Picture3.CurrentX = 4 Picture3.CurrentY = m3 Picture3.Print "Y = θ 3(t)" '将函数显示在图形

'关节 4 轨迹 '取值 a = Text16.Text b = Text21.Text 42

c = 10 d = a + (3 / (c * c)) * (b - a) * (10 * 10) - (2 / (c * c * c)) * (b - a) * (10 * 10 * 10) If Val(a) > Val(d) Then m4 = a n4 = d Else m4 = d n4 = a End If Picture4.Cls '清空画图区域 Picture4.Scale (0, m4 + (m4 - n4) / 5)-(12, n4) '设置画图区域 根据需要进行设置大小 Picture4.ForeColor = &H0 Picture4.DrawWidth = 1 '画 X 轴 Picture4.Line (0, n4)-(12, n4) '标记 t 轴 Picture4.CurrentX = 11.5 Picture4.CurrentY = n4 + (m4 - n4) / 10 Picture4.Print "t" '画 Y 轴 Picture4.Line (0, n4 - 5)-(0, m4 + (m4 - n4) / 5) '标记 Y 轴 Picture4.CurrentX = 0.2 Picture4.CurrentY = m4 + (m4 - n4) / 5 Picture4.Print "Y" '画坐标 For i = 1 To 10 Picture4.CurrentX = i: Picture4.CurrentY = n4 + (m4 - n4) / 10: Picture4.Print i Picture4.Line (i, n4)-(i, n4 + (m4 - n4) / 15) Next i For j = 1 To 5 Picture4.Line (0, n4 + (m4 - n4) / 5 * j)-(0.1, n4 + (m4 - n4) / 5 * j) Picture4.CurrentX = 0.1: Picture4.CurrentY = n4 + (m4 - n4) / 5 * j: Picture4.Print Fix(n4 + ((m4 - n4) / 5) * j) 43

Next j Picture4.DrawWidth = 2 Picture4.Line (0, 0)-(0, 0) '归零收笔 '开始画图 For X = 0 To 10 Step 0.001 '0.005 改小可以获得更高的精度 Y = a + (3 / (c * c)) * (b - a) * (X * X) - (2 / (c * c * c)) * (b - a) * (X * X * X) '输入函数 可以写入自己的函数 Picture4.CurrentY = (m4 + n4) / 2 Picture4.CurrentX = 0.5 Picture4.PSet (X, Y), vbRed '标记 X,Y 轴 Next Picture4.CurrentX = 4 Picture4.CurrentY = m4 Picture4.Print "Y = θ 4(t)" '关节 5 轨迹 '取值 a = Text17.Text b = Text22.Text c = 10 d = a + (3 / (c * c)) * (b - a) * (10 * 10) - (2 / (c * c * c)) * (b - a) * (10 * 10 * 10) If Val(a) > Val(d) Then m5 = a n5 = d Else m5 = d n5 = a End If '将函数显示在图形上

Picture5.Cls '清空画图区域 Picture5.Scale (0, m5 + (m5 - n5) / 5)-(12, n5) '设置画图区域 根据需要进行设置大小 Picture5.ForeColor = &H0 Picture5.DrawWidth = 1 '画 X 轴 44

Picture5.Line (0, n5)-(12, n5) '标记 t 轴 Picture5.CurrentX = 11.5 Picture5.CurrentY = n5 + (m5 - n5) / 10 Picture5.Print "t" '画 Y 轴 Picture5.Line (0, n5 - 5)-(0, m5 + (m5 - n5) / 5) '标记 Y 轴 Picture5.CurrentX = 0.2 Picture5.CurrentY = m5 + (m5 - n5) / 5 Picture5.Print "Y" '画坐标 For i = 1 To 10 Picture5.CurrentX = i: Picture5.CurrentY = n5 + (m5 - n5) / 10: Picture5.Print i Picture5.Line (i, n5)-(i, n5 + (m5 - n5) / 15) Next i

For j = 1 To 5 Picture5.Line (0, n5 + (m5 - n5) / 5 * j)-(0.1, n5 + (m5 - n5) / 5 * j) Picture5.CurrentX = 0.1: Picture5.CurrentY = n5 + (m5 - n5) / 5 * j: Picture5.Print Fix(n5 + ((m5 - n5) / 5) * j) Next j Picture5.DrawWidth = 2 Picture5.Line (0, 0)-(0, 0) '归零收笔 '开始画图 For X = 0 To 10 Step 0.001 '0.005 改小可以获得更高的精度 Y = a + (3 / (c * c)) * (b - a) * (X * X) - (2 / (c * c * c)) * (b - a) * (X * X * X) '输入函数 可以写入自己的函数 Picture5.CurrentY = (m5 + n5) / 2 Picture5.CurrentX = 0.5 Picture5.PSet (X, Y), vbRed '标记 X,Y 轴 Next Picture5.CurrentX = 4 45

Picture5.CurrentY = m5 Picture5.Print "Y = θ 5(t)" '将函数显示在图形上

End Sub

Private Sub Command4_Click() Text1.Text = "" Text2.Text = "" Text3.Text = "" Text4.Text = "" Text5.Text = "" Text6.Text = "" Text7.Text = "" Text8.Text = "" Text9.Text = "" Text10.Text = "" Text11.Text = "" Text12.Text = "" Text13.Text = "" Text14.Text = "" Text15.Text = "" Text16.Text = "" Text17.Text = "" Text18.Text = "" Text19.Text = "" Text20.Text = "" Text21.Text = "" Text22.Text = ""

End Sub

46

附录错误!未找到引用源。 外文翻译
自动表面粗糙度同三维机器视觉和合作的机器人控制
Chris Marshall, Robert Bicker and Paul Taylor

摘要:本文介绍了创新和务实的自动检测皮革表面粗糙度,利用结构光三维机器视觉对物体 轮廓的知觉和NURBS插补准确和平稳轨迹生成。作为高压喷砂用于粗加工,要考虑空间限制,在爆 破庭额外的自由度介绍利用一个转盘,它支持工件,协同控制是实施之间的一个六自由度机器人和 转台尽量机器人动作在满足要求的变速控制,精确的轨迹跟踪和定位控制。实验结果一致的业绩已 经显示出了该方法的有效性。 关键词∶三维视觉;曲线曲面的非均匀有理B样条内插法;合作管理;机器人;表面粗糙度

1 、引言
表面处理,特别适用于鞋类制造业,质量关键的大问题是为行业的生产鞋用水泥粘结和直接注 塑鞋底。一个主要因素在于完整的胶合面,这就意味着要充分清除材料的表面,以促进市场的基本 结构,或者在发生天然皮革的表皮。目前,表面处理等两种形式进行,采用了气缸钢丝刷或磨料表 面旋转车轮或带;这是做手工应用的历时鞋面的旋转钢丝刷,或自动机适用该鞋面以钢丝刷控制四 个或五个轴,显然,手工操作,需要高超的技巧,是难以维持在一个良好的水准,由于始终不断注 意经营者。控制系统的自动化进程通常开环定位装置和过程控制是有限事先调整刷特点,如丝的刚 度和敏锐性, 转速和控制力量与毛笔适用。 在过去十年中, 研究工作已取得根本性改善的轨迹控制, 运用传感器和分析方法能够检测过程实时[1],[2],[3],智能控制[4],[5]的接触力,转速和进 给速度的钢丝刷优化材料去除和表面纹理。大多数的这些努力都局限于实验室示威由于有实际困难 的调谐有很大差异,在刷特色。因此,新工艺是可取推进素质表面处理技术。 磨料爆破和喷丸有效而广泛采用的表面处理工艺去除缝针,提高强度和其它相关的表面制备应 用。一系列的皮革表面筛选试验采用喷砂机根据不同的条件下在我们的实验室最近表现一致的材料 去除性能。相对于这些方法用接触力控制,这是比较容易控制参数的喷砂工艺,如空气压力,速度 砂粒流迁入表面距离喷嘴表面,而更重要表现较为一致,较不敏感的工艺参数变化。 一个主要问题自动鞋上摔打,是如何衡量可靠的三维剖面上,并确定适当的粗径上,这使得机 器人操纵粗工具加以引导沿着指定的速度。常规传感器所不具备足够的计算粗径的上表面因复杂几 何概况,尤其是当今许多女人的时尚鞋。机器视觉是最适当的感官方法,在三维测量,而且越来越 便宜,为双方研究和过程控制。以前的相关研究著作描述[6],[7]在某种程度上成功地证明效力结 构光机视觉在自动唯一和粘接上的应用。然而,与其建立了视觉系统的模型,固定价值尺度因素来

47

计算鞋廓坐标从图像数据。本文将建立一个数学模型之间的图像数据和世界的坐标它可以大大提高 精确度剖面测量。 测量结果,从机器视觉一般是离散的位置坐标,从一个连续剖面上可重构,经分段直线段或通 过复杂曲线插补。扫描数据是有限的分辨率,由于速度要求的装配线,并花费大量时间形象加工一 幢医科相机使用的建议视觉系统扫描决议排列为5毫米,在脚背面积和2.5毫米的脚趾和足跟区。大 多数工业机器人控制器的位置直流装置内置插值,使他们能够处理断面位置。然而,这些控制器通 常采用串行通信传输数据和指令上位PC机和机器人控制器相对缓慢的更新率,它必然导致低精度和 呆滞的动态响应。一个美洲豹760个工业机器人被重新利用多轴运动控制器与以太网通信和位置更 新率 多达200余等高线模式,这可大大提高精度。鉴于先前的嵌入式插不再提供,非均匀有理B样条 插补应用于重建顺利连续轨迹由离散路径坐标。NURBS插补提供了一个统一的代表性分析和自由型 曲线,并明显优于线性插值常规数控机床和机器人通常使用。速度和加速度的连续性整个曲线是至 关重要的特点,以避免急冲提案的机器人末端效应。 表面喷砂进行内密封庭该机器人已在有限空间内操控粗喷嘴。为减少运动造成的机器人手臂, 额外自由度引入到工件使鞋上支撑平台可以转动实时控制下。因此,协同控制算法所需要的 7 个自 由度的冗余系统。保持一贯的表面处理性能,在某些砂粒空气流量,恒明或线速度的粗喷嘴相对的 上表面是至关重要的,这一目标已经实现,开发新颖的 NURBS 轨迹生成算法和插值方法。 2、三维机器视觉轮廓测量发达结构光扫描系统显示 如图1;它包括一个模拟式摄像机,激光线发生器和步进电机驱动直线下滑提供扫描议案照相机 和激光打印机。鞋将扫描是紧紧空气钳位在一个转盘是位于45度角的水平面。这种配置可以减轻媒 体爆破积累了放在桌上的喷砂工艺。照相机和激光线发生器装在步进电机驱动直线下滑,与激光线 垂直的转盘和一个夹角,照相机的光轴与激光线。客观的视觉系统,是提供一个三维轮廓的钳位鞋 基于坐标系统定义的转盘,边鞋面,然后是发现和粗径是指形匹配的上唯一的。经过粗加工的道路, 得到了 鞋钳位的转盘是运到爆破厅通过直线滑机器人操纵喷砂处理。

图1 结构光视觉系统。 如图 2,存在两个坐标系,图像坐标系团与物体坐标系统哎哟,它们之间的关系需要加以确定 的物理位置和方向的对象点提取图像数据。图像坐标系,是指与镜头的光轴作为 ocz 轴方向增加图 48

像连续坐标 ocy 轴线,轴线的 ocx 决定用右手规则。物体坐标系是指与该中心的转盘出身,向下方 向沿转盘和平行的直线滑携带激光照相机作为 owz 轴截至方向垂直于转盘作为 owx 轴线;owy 可以 定义方便用右手规则。结构光三维扫描钻机是精心,刻意设计,激光平面平行于 xowy 平面物体坐 标系,使每一个点上的激光线相交的对象有一个固定的 Z 坐标对象坐标系因此,其他两个部件坐标 X 和 Y 可以单独确定从二维图像数据。

图 2 摄像机与世界坐标系统。 数学关系的摄像机坐标系与物体坐标系中是不可或缺的连接二维 图像数据与三维物体的坐 标。 有很多方法[8],[9], [10],[11]和[12]开发构建数学模型的视觉系统, 其中蔡明亮的方法[8], 是最常采用的。一般来说,模拟视觉系统分为两阶段相关参数的外在和内在参数。外在参数描述空 间关系中的两个坐标系,如平移和旋转变换的坐标系中的物体坐标系统可以映射到 摄像机坐标系; 这些转变可以在数学上表示为:

(2)
在这里,R 是旋转矩阵定义的欧拉角,由 TAIPEI,烃厝翻译沿新 owx,owy 和 owz 轴旋转。分子 RI 在矩阵 R 可以表示为功能旋转角度α ,β ,γ 如下:

(3)
成反比,欧拉角可确定由 R:

(4)
第二阶段的模拟进程,是与照相机的内在参数:它是基于针孔相机的透视投影模型径向变形 49

及其它异常现象的考虑。针孔相机的模式来改造点(圆度量,zc)在摄像机坐标系奥委会相关位置图 像缓冲像素。这一阶段共分三个转变,第一次描述了变换从相机坐标(XC细胞度量,ZC)的失真,以 二维传感器平面坐标(徐钰)由方程:

(5)
其中 f 是有效焦距的镜头。第二个层面的转变反映了径向几何失真,这是造成的镜头,其实这点在 不同径向距离镜头轴线发生明显放大。坐标,在一个失真图像传感器飞机(徐钰),可从观测(扭曲) 图像坐标(XD 型,YD 型)

(6)
在这里,是径向距离观测点的摄像机光轴,K1 的是系数的径向畸变。 最后的变换叙述关系的观察 阵地的图像传感器平面坐标,在缓冲图像帧,它被描述的:

(7)
而泰航与 CY 是像素坐标相交光轴与传感器平面,DX 的镝的有效培训中心之间的距离照相机的传感 器组成,在第十和昌方向分别 S、X 的,是一个尺度因子,以弥补任何不确定的数目比例分子传感器 关于 CCD 和象素数目在照相机的帧缓冲 X 方向。所以关系的对象坐标和图像数据相结合,建立了有 效性。(1),(5),(6),(7)在一起。照相机的模型所述标定需要确定的外部和内部参数,然后它可 以把图像数据用来作为坐标在转台坐标系。用标定目标,并根据 Tsai 的方法,建议的摄像系统标定 以及由此产生的参数有:

3、合作机器人控制 图 3 显示了成立机器人鞋上喷砂, 其中喷砂嘴,是他操纵的机器人末端微跟随造成粗径与某 些方向。一个金属盘,并采取适当的薰陶作为掩模的路径跟踪,以防止流砂从 破坏以外地区的皮革 表面。双方轨迹跟踪和定位控制的机器人末端效应可引起较大规模的联合运动。作为喷砂会发生议 事堂,机器人动作应限制在一个受约束的小空间。 为了减少机器人动作,额外的自由度,旋转 引 入到鞋面支持转台,这不可避免地导致了冗余系统七个自由度。

50

图3 机器人鞋上喷砂成立的。 一个简单的协同控制策略适用于转盘和机器人系统,旨在减轻关节运动。算法是描图。B 和 C 是坐 标的粗径产生 NURBS 插补,角度α ,β ,可据此计算,同样的计算,可以做到对所有插值点的路径。 简单地转动转盘角度,以α ,β 分别可以带多点,B 和 C 上的 X 轴为中心的 A1,B1 和 C1 组,同样, 所有坐标的轨迹,这可以大大降低运动的机器人。理论上,有没有运动,在 y 方向做一系列动作, 在 X 方向不会改变,小的方向变化的最终效应在 Z 轴方向高度变化。 4、结论 本文描述了一种新的自动皮革技术,它利用空气助推喷砂表面处理表面粗度,三维机器视觉物体轮 廓测量,NURBS 曲线插补轨迹重建和工业机器人的自动操纵和控制。一个数学模型的形象,制度的 建立及相关参数进行了校准,这大大提高了测量精度。平稳轨迹的作品再现,由离散图像数据的 NURBS 曲线插补基于这个原因,泰勒的二阶逼近扩张被用来执行速度控制的机器人末端效应。顺利 机器人准确地跟踪目标。协同控制实施机器人与工件支持平台,以最小的机器人动作。实验结果一 致的表面处理表现的效率的方法。 可以进一步改进, 采用高帧频摄像机结合标定的视觉系统登记的 机器人末端微向工件平台和工艺优化。

51

附录错误!未找到引用源。 外文原文

Automatic surface roughing with 3D machine vision and cooperation robot control
Chris Marshall, Robert Bicker and Paul Taylor

Abstract
This paper presents an innovative and practical strategy for automated leather surface roughing, using structured light 3D machine vision for object profile perception, and NURBS interpolation for accurate and smooth trajectory generation. As high pressure grit blasting is used for roughing, considering the spacial constraints in the blasting chamber, an additional degree of freedom is introduced using a rotary table, which supports the workpiece. Cooperative control is implemented between a 6-DOF robot and the rotary table to minimize robot movements, while satisfying the requirements of variable velocity control, accurate trajectory tracking and orientation control. Experimental results of consistent roughing performance have shown the efficiency of the proposed method. Keywords: 3D vision; NURBS interpolation; Cooperative control; Robot; Surface roughing

1. Introduction
In surface treatment with particular application in the footwear manufacturing industry, the quality of sole bond is a major problem for the sectors that manufacture shoes with cement bonded and direct injection moulded soles. A main factor in the integrity of the sole bond is the preparation of the mating surfaces, which means to remove the surface of the material adequately so as to facilitate a bond to the underlying structure of the upper material, or in the case of natural leather to the epidermis. Currently, surface treatment is carried out either by means of a cylinder wire brush or an abrasive surface on a rotating wheel or band; this is done either by manual application of the lasted shoe upper to the rotating wire brush, or an automatic machine that applies the shoe upper to the wire brush by control of four or five axes. Obviously, manual operation requires great skills and is difficult to maintain at a good standard consistently due to constant attention by the operator. The control systems for the automatic process are generally open loop positional devices, and the process control is limited to prior adjustment of brush characteristics such as wire stiffness and sharpness, speed of rotation and control of force with which the brush is applied. During the last decade, research efforts have been made on making fundamental improvements to the path control, applying sensor and analysis methodology capable of inspecting the process in real time [1], [2] and [3], intelligent control [4] and [5] of the contact force, rotation speed and feed rate of the wire brush to optimize material removal and surface texture. Most of these efforts were limited to laboratory demonstrations due to practical difficulties of tuning the large variations in brush characteristics. Consequently, new process technology is desirable to advance the quality of surface treatment. Abrasive blasting and shot peening are effective and widely used surface treatment processes for the removal of sharp edges, improvement of strength and other related surface preparation applications. A series of leather surface screening experiments using a grit blasting machine under distinct conditions 52

undertaken in our laboratory recently have shown consistent material removal performance. Comparing with those methods using contact force control, it is much easier to control parameters of the grit blasting process, such as air pressure, velocity of grit flow moving across the surface and distance from the nozzle to the surface and, more importantly, the performance is more consistent and less sensitive to process parameters variations. A major problem for automatic shoe-upper roughing is how to reliably measure the 3D profile of the upper, and define a proper roughing path on it, this enables the robot manipulated roughing tool to be guided along the path with specified velocity. Conventional sensors are not sophisticated enough to calculate the roughing path on the upper surface due to the complex geometrical profile, particularly many of today’s women’s fashion shoes. Machine vision is the most appropriate sensory methodology in 3D measurement, and is becoming more affordable both for research and in-process control. Previous related research works described in [6] and [7] have to some extent successfully demonstrated the effectiveness of structured light machine vision in the automatic sole and upper bonding application. However, instead of establishing the vision system’s model, fixed value scale factors were used to calculate the shoe profile coordinates from image data. This paper will establish a mathematical model between the image data and the world coordinates, which can greatly improve precision of profile measurement. Measurement results obtained from machine vision are generally discrete position coordinates, from which a continuous profile can be reconstructed, either via piecewise straight line segments or through complex curve interpolation. Scan data are of limited resolution due to velocity requirements of the assembly line and the time spent on image processing. For a 25 fps camera used in the proposed vision system, the scan resolutions are arranged as 5 mm at the instep area and 2.5 mm at the toe and heel areas. Most industrial robot controllers are positional servo devices with embedded interpolation, which makes them capable of dealing with positional discontinuities. However, these controllers usually use serial communication to transfer data and commands between the host PC and the robot controller with a relatively slow update rate, which inevitably leads to low accuracy and sluggish dynamic response. A PUMA 760 industrial robot was reengineered using a Galil multi-axes motion controller with Ethernet communication and position update rate up to 2 ms in contour mode, this can significantly improve precision. As the previous embedded interpolation is no longer available, Non-Uniform Rational B-Spline interpolation is applied to reconstruct a smooth continuous trajectory from discrete path coordinates. NURBS interpolation provides uniform representation of both analytical and free-form parametriccurves, and has significant advantages over linear interpolation that conventional CNC machines and robots usually use. Velocity and acceleration continuities throughout the whole parametric curve are vital characteristics to avoid jerky motions of the robot end-effector. Surface grit blasting is carried out inside a sealed chamber, in which the robot has limited space to manipulate the roughing nozzle. In order to minimize motion of the robot arm, an additional degree of freedom is introduced to the workpiece, so that the shoe-upper support platform can rotate under real time control. Therefore, cooperative control algorithms are necessary for the 7 degrees of freedom redundant system. To maintain consistent surface treatment performance under certain air grit flow rates, constant or prescribed line velocity of the roughing nozzle relative to the upper surface is vital, this has been achieved by development of novel NURBS trajectory generation algorithm and interpolation methods. The rest of the paper is organized as follows: description of the 3D vision system for shoe upper profile measurement is given in Section 2; NURBS interpolation for smooth trajectory reconstruction from discrete scan data is placed in Section 3; variable feed rate NURBS interpolator for the robot can be found in Section 4; experimental grit blasting results is given in Section 5. 53

2. 3D machine vision for profile measurement
The developed structured light scanning system is shown in Fig. 1; it consists of an analogue camera, laser line generator and stepper motor driven linear slide providing scanning motions for the camera and laser. The shoe to be scanned is tightly air clamped on a rotary table which is located at an angle of 45 degrees to the horizontal plane. This configuration can alleviate blasting media accumulating on the table during the grit blasting process. The camera and laser line generator are mounted on the stepper motor driven linear slide, with the laser line perpendicular to the rotary table and an angle between the camera’s optical axis and the laser line. The objective of the vision system is to provide a 3D profile of the clamped shoe based on a coordinate system defined on the rotary table, edge of shoe upper is then detected and roughing path is defined by conformal matching of the upper and sole. After the roughing path is obtained, the shoe clamped on the rotary table is delivered into the blasting chamber through a linear slide for robotic manipulated grit blasting treatment.

Fig. 1. Structured light vision system. As illustrated in Fig. 2, there exist two coordinate systems, the image coordinate system OC and object coordinate system OW, between them a relationship needs to be established to determine the physical position and orientation of object points from extracted image data. The image coordinate system is defined with the camera’s optical axis as OCZ axis, the direction of increasing image row coordinates as OCY axis, and OCX axis is decided using the right-hand rule. The object coordinate system is defined with the centre of the rotary table as origin, downwards direction along the rotary table and parallel to the linear slide carrying the laser and camera as OWZ axis, up direction vertical to the rotary table as OWX axis; OWY can be defined conveniently using the right-hand rule. The structured light 3D scanning rig is carefully and deliberately designed that the laser light plane is parallel to the XOWY plane in the object coordinate system, so that each point on the laser line intersecting with the object has a constant z coordinate in the object coordinate system, therefore the other two coordinate components x and y can be solely determined from the 2D image data.

54

Fig. 2. Camera and world coordinate systems. The mathematical relationship between the camera coordinate system and the object coordinate system is vital and essential which connects 2D image data with 3D object coordinates. There are a number of methods [8], [9], [10], [11] and [12] developed to construct a mathematical model for the vision system, among which, Tsai’s methodology [8] is most commonly applied. In general, modelling of a vision system consists of two stages related the extrinsic parameters and intrinsic parameters. The extrinsic parameters describing the spatial relationship of the two coordinate systems, such as the translational and rotational transformations by which coordinates in the object coordinate system can be mapped to the camera coordinate system;these transformations can be expressed mathematically as:

(2)
here, R is the rotation matrix defined by the Euler Angles , TX,TY and TZ are translations along the new OwX, OwY and OwZ axis after rotation respectively. Elements ri in matrix R can be expressed as function of rotation angles α, β, and γ as follows:

(3)
Inversely, Euler angles can be determined from R by:

(4)
The second stage of the modelling process is related to the camera’s intrinsic parameters: it is based on the pinhole camera’s perspective projection model with radial distortion and other aberrations taken into account. Pinhole camera’s model is used to transform point (XC,YC,ZC) in the camera coordinates system OC to associated position (Xf,Yf) in the image buffer in pixels. This stage consists of three transformations, the first one describes the transform from camera coordinates (XC,YC,ZC) to undistorted 2D sensor-plane coordinates (XU,YU) by equation 55

(5)
where f is the effective focal length of lens. The second transformation reflects the geometric radial distortion of the lens, which is caused by the fact that points at different radial distance from the lens axis undergo distinct magnifications. The coordinates in an undistorted image sensor plane (XU,YU) can be obtained from the observed (distorted) image coordinates (XD,YD) by

(6)
here, is the radial distance from the observed point to the camera optical axis, k1 is the

coefficient of radial lens distortion. The final transformation describes the relation between observed positions on the image sensor-plane to coordinates (Xf,Yf) in the image frame buffer, which is described by:

(7)
where CX and CY are the pixel coordinates of the intersection of the optical axis and the sensor-plane, dX and dY are the effective centre to centre distances between the camera’s sensor elements in the XC and YC directions respectively, and SX is a scaling factor to compensate for any uncertainty in the ratio between the number of sensor elements on theCCD and the number of pixels in the camera’s frame buffer in the X direction. Therefore the relationship between the object coordinates and image data is established by combining Eqs. (1), (5), (6) and (7) together. The camera’s model described above need calibration to identify the exterior and interior parameters before it can be used to convert image data (Xf,Yf) into coordinates in the rotary table coordinate system. Using a calibration target and according to Tsai’s method, the proposed camera system is calibrated and resulting parameters are:

To calculate 3D object’s coordinate from 2D image data (Xf,Yf) using the vision system’s model
constructed above, the constraint imposed on the ZW coordinate when the laser line intersects with the object is used together with model Eq. (1) to determine a unique world coordinate (xw,yw,zw). For simplicity, in this design, the laser beam is adjusted to be critically vertical to the rotary table or parallel to YWOWZW plane, so that all ZW coordinates of intersection points on the laser line are constant. In this case, two high precision linear slides and one rotary table manufactured by Parker were used, to adjust and verify the perpendicularity of the laser light sheet with the rotary table, during the whole motion range of the laser and camera linear slide, make sure the image of the laser line intersecting the rotary table is horizontal. Therefore, world coordinates can be calculated from image data by equation as follows: 56

(8)
Discrete shoe profile coordinates are obtained by image processing to find the edge points and conformal matching of the shoe upper and sole for further fine trim, two angles that define the shoe upper surface are also calculated from neighbouring edge points and individual scan line. Detailed description of the strategy can be found in [13] published by the authors. Fig. 3 shows an example 3D shoe profile consists of discrete points extracted from the laser scanning process.

Fig. 3. Measurement of shoe upper contour trajectory.

3. NURBS interpolation for trajectory production
3.1. NURBS curve interpolation NURBS is an acronym for Non-Uniform Rational B-Splines, they provide a single precise mathematical method for describing common analytical shapes including lines, planes, conic curves, free-form curves, quadratic and sculptured surfaces. The one dimension B-Spline curve is defined over a free parameter 0≤u≤1 by the following equation:

(9)
The point on the curve at the parameter value u is denoted by P(u), which is a weighted average of all the control points denoted by Pi, the blending function or basis function Ni,k(u) decides the extent to which a particular control point controls the curve at a particular parameter value u. The basis function Ni,k(u) depends on the parameter value u and the order of the curve k, and is recursively defined as follows:

(11)
the constants [u0,…,un+k], called knot values, are specific instances of the parametric value u, and are strictly in nondecreasing order. The order of a NURBS curve defines the number of nearby control points that influence any given control point, generally, high order interpolation leads to smoother curve recreation. If the knot values are decided so that u1?u0=u2?u1= =un+k?un+k?1, then all the basis functions are of identical shape and every control point has identical effect on the resulting parametric curve; this is called Uniform B-spline, otherwise, Non-Uniform. Introducing an additional term, weight of the control point, will allow B-spline to represent conics and 57

any other free-form curves and surfaces, and have more control over the shapes. The resulting curves, with a weight for each control point, are called rational curves. Therefore, a Non-Uniform Rational B-spline curve is defined by the formula:

(13)
where Ri,k(u) is the single rational B-spline, which denotes the extent to which the control point has control over the curve. NURBS curve interpolation, combined with basic approximation principles for parametric curves such as first and second order Taylor series expansion, has successfully been utilised in real time CNC variable feed rate command generation [14], [15], [16], [17] and [18], robot control [19], [20], [21] and [22] and computer graphics. In this work it is used for the reconstruction of a continuous profile model from discrete scan data; Fig. 4 shows the result of a 12th order NURBS interpolation of shoe-upper profile. As described in the above sections, NURBS is expressed in relatively complex equations; the higher the order, the more complex, and therefore the more computationally intensive. However, simple conventional piecewise linear interpolation or polynomial spline interpolation either leads to velocity and acceleration discontinuities at the junction of line segments or is unable to represent free form curves and therefore cause significant deviation from original data points. Fig. 5 shows velocity profile calculated from third order NURBS interpolation, and there exist high velocity fluctuations. In this work, the interpolation calculation is implemented offline after discrete data points are obtained from image processing; thus intensive computation is not a critical issue.

Fig. 4. 3D trajectory from NURBS curve interpolation.

Fig. 5. Velocity plot from 3 order NURBS interpolation. 3.2. Command generator for robot controller By using NURBS interpolation, smooth trajectory has been achieved from discrete image data, which is represented in parametric form in three-dimension space in terms of a free parameter u:

58

(14)
The redesigned robot controller consists of several-position servo devices, which control each robot joint according to position increments received from the command generator. Velocity control of the robot end-effector is critical not only because it is essential formaterial removal control during the grit blasting process, but also because significant acceleration changes will cause mechanical vibrations. For simplicity, the robot dynamics was not considered in this work, based on the assumption that coordinated control of robot end-effector and rotary table can reduce the end effector’s orientation change to a minimum value, and velocity commands generated by NURBS interpolation and Taylor approximation are relatively smooth, so that they don’t exceed the robot dynamic constraints. In practice, with a 4 ms robot sampling time, the proposed control method can achieve absolute velocity of 100 mm/s without causing a vibration problem. To further improve processing speed and efficiency, strategies of time optimal control proposed in [19] and [21] can be applied. The key to velocity control of the robot end effector is to provide appropriate position increments in the cartesian coordinate during each sampling interval such that the ratio of the position increment and the sampling time equal to the prescribed velocity, based on NURBS interpolation of the desired trajectory and prescribed velocity profile of robot end effector. Although the NURBS curve is parametric and has continuous first and second order derivatives over parameter u, there is not a linear relationship between this parameter and the distance along the curve. Segmentation of the curve should be based on the required lengths of curve segments rather than equal parameter increments, accordingly, curve approximation is needed to determine successive values of u such that the lengths of curve segments are constant or proportional to the prescribed velocity profile. Line velocity along the curve in a three-dimension space can be expressed as follows:

(15) and (16)
Therefore, solution to parameter u for a specified line velocity along the curve can be found via the equation below:

(17)
However, a solution to the above equation is difficult in the general case; a computational efficient solution of Eq. (17) based on Taylor’s expansion approximation is used. The Taylor’s expansion up to second order is given by:

(18)
In the case that the sample time T is little, and the trajectory does not have a small radius of curvature, the first order approximation is usually adequate. However, considering the high curvature of the shoe upper particularly at the toe and heel areas, two terms of Taylor’s expansion were used to provide approximation of higher precision. Second order derivative of parameter u over time:

(19)
here 59

(20) if substituting Eq. (16) into (19): (21) If substituting Eqs. (17) and (21) into (18):

(22)
here, K=0,1,…n, indicating the kth segmented curve. Using the velocity profile derived in the first section, the next command parameter can be calculated using the above equation. 3.3. Velocity control for NURBS generated trajectory A trapezoidal velocity profile was used as the desired motion profile to achieve smooth motion of the robot end effector, enabling more accurate path tracking. The PUMA 760 robot controller was upgraded using a GALIL multi-axis motion controller with Ethernet communication for data communication between a host computer and the robot controller. The sampling rate for robot command updating depends mainly on the motion controller’s servo update rate which is an order of 2 ms. Here the robot command update rate is set to 4 ms, which is a significant improvement to the 28 ms updating rate of the original PUMA 760 controller. Second order Taylor’s expansion approximation of the interpolated trajectory is implemented to generate the parameter sequence {u(0),u(1),…u(n)}, which maintains a desired velocity profile as appropriate for surface grit blasting treatment. As shown in Fig. 6, a high accuracy calculation of the velocity tracking is achieved, the velocity fluctuation is contained to be within 2% of the desired speed, which is adequate for this application and gives a smooth movement of the robot end-effector.

4. Cooperative robot control
Fig. 7 shows the set-up for robotic shoe-upper grit blasting, in which the blasting nozzle is manipulated by the robot end-effector to follow the created roughing path with certain orientations. A metal disc is mounted and properly orientated as a mask during the path tracking to prevent grit flow from damaging the outside areas of the leather surface. Both trajectory tracking and orientation control of the robot end-effector can cause relatively large-scale joint movements. As grit blasting will be happening inside the chamber, robot movements should be limited within a constrained small space. In order to reduce robot movements, an additional degree of freedom, rotation, was introduced to the shoe upper supportive rotary table, which inevitably leads to a redundant system of seven degrees of freedom.

Fig. 7. Robotic shoe-upper grit blasting set-up. A simple cooperative control strategy was applied to the rotary table and robot system, aiming at 60

minimizing joint movements. The algorithm is depicted in Fig. 8, points A, B and C are coordinates on the roughing path generated from NURBS interpolation, angles α,β and can be calculated accordingly, the same calculation can be done to all interpolated points on the path. Simply rotating the rotary table with angles α,β and respectively can bring points A, B and C onto the X axis as points A1, B1 and C1, the same is true with all coordinates on the trajectory, this can greatly reduce movements of the robot. Theoretically, there are no movements at the Y direction, the range of movements at the X direction will be - and little orientation variation of the end-effector due to height variation in the Z direction.

6. Conclusion
This paper describes a novel methodology for automatic leather surface roughing, which uses air-propelled grit blasting for surface treatments, 3D machine vision for object profile measurement, NURBS curve interpolation for trajectory reconstruction and an industrial robot for automatic manipulation and control. A mathematical model for the image system was established and related parameters were calibrated, which significantly improved the precision of measurement. A smooth trajectory of the work piece was reconstructed from discrete image data using NURBS curve interpolation, based on that, Taylor’s second order expansion approximation was used to implement velocity control of the robot end-effector. Smooth robot motion and accurate trajectory tracking were achieved. Cooperative control was implemented between robot and work piece supportive platform to minimize robot movements. Experimental results of consistent surface treatment performance show the efficiency of the proposed method. Further improvements can be achieved by applying a higher frame rate camera, combining calibration of the vision system with registration of the robot end-effector to the work piece platform, and process optimization.

61



相关文档

更多相关文档

搬运机械手及其控制系统设计
搬运机械手的结构和控制系统设计(例)
搬运机械手控制系统设计
搬运机械手控制系统设计 论文
搬运机械手PLC控制系统设计
搬运机械手控制系统设计10
搬运机械手及其控制系统设计(要0
搬运机械手的设计说明书
某搬运工件机械手控制系统设计
玻璃板搬运机械手系统设计说明书
搬运机械手PLC控制系统设计毕业设计[1](1)
搬运机械手PLC控制系统设计毕业设计[1]
机械手PLC控制系统的组态王仿真模拟
仓储搬运机械手主体设计说明书
毕业设计搬运机械手PLC控制系统设计
电脑版