（电力电子与电力传动专业论文）基于ipcdsp的机器人运动控制系统的设计.pdf

东南大学硕上学位论文 a b s t r a c t w mt h ed e v e i o p m e n ta n da p p c a “o n0 fc o m p u t e ra n dd s pt e c h n o i o g mr o b o t s e n ，oc o n t r o l t e c h n o i o g yh a sg o tg 怕a td e v e i o p m e n t f 汀s t i yt h ep a p e ri n 臼o d u si n d u s t r i a ir o b o t st e c h n o l o g ya n dt h e d e v e i o p m e n ta n da p p c a 廿o no fr o b o tc o n t r o e lg i v e ss t u d yo b i e c ta n dp a 阳m e t e r s t h e nt h ep a p e r d i s c u s s e sm e t h o do fr o b o t ，sk i n e m a t i c sa n di n v e r s ek i n e m a t i c sa n dr o b o r sc o n t r o lar i t h m e t i cm o s t i va b o u t p i d n t r o i b a s e d0 nt h ec o m p a 陀o fc o n t r o i t h e o 眦w es e i e c tt h en 0 一i e v e im b o t sc o n t r o is y s t e mb a s e d o ni p ca n dd s pa so u rs y s t e m d s pc o n t m ic a r da n ds e n ，od n v e rm a k eu po fi u n i o rs e n ，oc o n t r o l s y s t e m i n d u s t n a ip cc o m p o s e ss u p e r f o rc o n t r o is y s t e m t w o 1 e v e is y s t e mc o m m u n i t e sw i t he a c ho t h e r b vp c ib u s s e n ，oc o n t r 0 ic a r du s e st m s 3 2 0 v c 3 3a sc o n t r o i 悖。u s e sc p l dt od e s g n 陀v e 倦b l e c o u n t e la n du s ) a c 7 7 2 5a sd ao u t p u tc h i p t h e y m p o s ec i o s 昏i o o pc o n t m is y s t e mo fs e r v om o t o 【 i n d u s tr i 副p cf u 洲l sr o b o t 毫s e t t i n g so fd a 陷m e t e c o n t r o l so fc o m m a n d o p e 怕们n a i i n t e n 刍，c o m 洲t e d c o n 的ia n t h m e 廿ca n ds oo n s u b o r d i n a t em o d ec h i pp l x 9 0 5 2i su s e df o rp c i b u sc o n n e c t l n gc h i pw d m d v e ri sp m g 怕m m e df ；o rp l x 9 0 5 2 p m t o c 0 i l sd e s i g n e df o rc o m m u n i c a t i o nb e b 旧e ni p ca n dd s pc o n t r o l c a r d w bm a k et h es e r v oj n t e r f a c eb o a r df o rs i g n a ic o n v e 懵i o nb e ，e e nd s pc o n t r o ic a r da n ds e r v o d r i v e 晤w es u c s sm a k e i n au po fr o b o r sc o n 仃o is y s t e mf o rp 陀p a 怕o fa d v a n c e ds l u d v k e y w o r d ：r o b o t k i n e m a t i ，w d md r i v e 懵，t m s 3 2 0 v c 3 3 p c i 9 0 5 2 ，c p l d d p r a m i l 东南大学学位论文独创性声明 本人声明所呈交的学位论文是我个人在导师指导下进行的研究工作及取得的研究成果。 尽我所知，除了文中特别加以标注和致谢的地方外，论文中不包含其他人己经发表或撰写过 的研究成果，也不包含为获得东南大学或其它教育机构的学位或证书而使用过的材料。与我 一同工作的同志对本研究所做的任何贡献均已在论文中作了明确的说明并表示了谢意。 旷 研究生签名：奠隆恤日 期： 东南大学学位论文使用授权声明 东南大学、中国科学技术信息研究所、国家图书馆有权保留本人所送交学位论文的复印 件和电子文档，可以采用影印、缩印或其他复制手段保存论文。本人电子文档的内容和纸质 论文的内容相一致。除在保密期内的保密论文外，允许论文被查阅和借阅，可以公布( 包括 刊登) 论文的全部或部分内容。论文的公布( 包括刊登) 授权东南大学研究生院办理。 研究生签名： 窿到者、 导师签名日期： 第一章绪论 1 1 工业机器人技术综述 第一章绪论 工业机器人是“一种装备有记忆装置和末端执行装置的、能够完成各种移动来代替人类劳动的通用机 器”。工业机器人一般由操作机( 机械本体) 、控制器、伺服驱动器和检测传感装置构成，基本结构如图1 1 所示。工业机器人特别适合于多品种、变批量的柔性生产，对提高产品质量和生产效率、改善劳动条件和 快速更新产品起着十分重要的作用。 图1 1 工业机器人的基本结构 到目前为止，工业机器人的发展大约经历了三代： ( 1 ) 第一代示教，再现机器人。它的特点是不配备任何传感器，一般采用简单的开关控制、示教，再现控制 和可编程序控制。机器人的作用路径或运动参数都需要示教或编程给定，在工作过程中它们无法通过环境 的改变来改善自身的性能品质。它已经被广泛应用于现代生产中。 ( 2 ) 第二代感觉型机器人。这一代机器人是随着计算机、自动控制理论的发展以及工业生产的需要发展 起来的。这种工业机器人配备了简单的内部和外部传感器，能感知自身运行的速度、位置、姿态等物理量， 并以这些信息的反馈构成闭环控制，因而具有部分适应外部环境的能力。 ( 3 ) 第三代智能型机器人。目前这种机器人正处于研究和发展之中，它具有多种外部传感器组成的感觉 系统，可通过对外部环境信息的获取处理，确切地描述外部环境，自主地完成某一项任务。它拥有自己的 知识库，多信息处理系统可在结构和半结构化的环境中工作，能够根据环境的变化做出相应的决策。 在国外，工业机器人使用相当广泛，应用日趋成熟，已经成为一种标准设备在工业上被广泛应用。据 u n e c e ，i f r 预测，至2 0 0 7 年，全球新安装机器人的数量将从2 0 0 3 年的8 1 8 0 0 套增至2 0 0 7 年的1 0 6 0 0 0 套，年平均增长7 ，其中，日本2 0 0 3 - 2 0 0 7 年工业机器人的销售将从2 0 0 3 年的3 1 6 0 0 增长至2 0 0 7 年 的4 1 0 0 0 套；欧洲2 0 0 3 2 0 0 7 年工业机器人将从2 0 0 3 年的2 7 1 0 0 套增长至2 0 0 7 年的3 4 0 0 0 套；北美 2 0 0 3 - 2 0 0 7 年工业机器人市场每年平均增长5 8 ，至2 0 0 7 年将增长到1 6 0 0 0 套。 在我国，机器人产业起步较晚，发展大致分为3 个阶段：7 0 年代萌芽期、8 0 年代开发期和9 0 年代以 来的适用化时期。在国家的支持下，通过“七五”、“八五”等科技攻关，我国已基本掌握了机器人的设 计制造技术、控制系统硬件和软件设计技术、运动学和轨迹规划技术，生产了部分机器人关键元器件，开 发出喷漆、弧焊、点焊、装配、搬运等机器人。随着我国经济的快速发展，我国机器人市场正处于由传统 装备向先进制造装备转型的时期，市场潜力巨大，发展前景广阔。但是，我国还不能自主设计和生产先进 的大型机器人自动化成套装备，更形成不了整体配套能力，目前的状况是几乎全部依赖于进口，被国外公 司所垄断，由此严重制约了我国机器人制造业的健康发展。因此，我们要大力发展机器人及其自动化成套 装备产业，填补国内的空白，保证我国国民经济的可持续发展。 1 2 机器人控制器的应用和发展 在各种作业中，机器人的运动主要表现为沿着预定的路径移动，并保持预定的姿态，在运动中或在给 定的某些位置上完成作业规定的操作，这些都是在机器人控制器的控制下完成。机器人控制器相当于机器 l 东南大学硕士学位论文 人的“大脑”，是机器人的核心和灵魂，设计的成功与否最终影响到机器人的优劣。机器人控制器由一组 硬件和软件组成，是机器人系统不可分割的一部分，与机器人系统的工作原理、机器人的结构和功能、机 器人的精度都有密切的关系。 早期由于计算机技术比较落后，很多先进的控制方法无法在机器人上完全实现。九十年代以来，随着 计算机技术的成熟并且引入机器人领域，机器人迅速地发展起来。机器人控制器已经和计算机技术紧密结 合在一起。以计算机为核心的机器人控制器的发展经历了下面几个阶段： ( 1 ) 基于通用微型处理器。这种控制器由8 0 8 8 、8 0 3 1 等为核心部件，加上存储器、编码器信号处理电 路及d ，a 转换电路等，其位置环控制算法由事先编好的程序固化在存储器中。这种方案采用元器件较多， 可靠性低，体积比较大，而且控制参数不易更改，软硬件设计工作量较大。 ( 2 ) 基于专用微控制器。采用专门生产用于伺服控制的芯片，如l m 6 2 8 、h c p l l l 0 0 、p c l 8 3 3 等，完 成速度曲线规划、p i d 伺服控制算法、编码器信号的处理等多种功能。电机位置、速度、加速度、p i d 参 数等需要经常更改的参数存储在芯片的r a m 区，但由于运算速度的限制，复杂的控制算法和功能很难在 其中实现。近年来，专用控制芯片的运算速度越来越快，功能越来越强大，这种方案目前使用还较广泛。 ( 3 ) 基于通用数字信号处理器( 0 s p ) 。近年来，d s p 在伺服控制系统中的运用越来越广泛，为设计高 精度机器人开辟了道路。d s p 的高速运算能力可以实现各种复杂的控制算法， 而它本身独特的硬件结构 可实现快速的位置捕捉等功能。在伺服控制领域中，比较常用的d s p 有t i 公司c 2 4 ) 0 ( 和c 2 8 x 系列； m o t 0 陷i a 公司的d s p 5 6 k 系列；a d 公司的a d s p 2 1 联系列和a _ r t 公司d s p l 6 x 系列等。典型的d s p 多轴运动控制卡是d e i t a t a u 公司开发的开放结构运动控制器p m a c ，它以m o t o 怕l a 的d s p 5 6 k 系列d s p 为核心，支持多种总线插槽方式，一块卡可以同时控制8 3 2 轴，可以并联运行。 随着d s p 价格的下降，采用d s p 的机器人控制器应用日趋广泛，d s p 的高速运算能力和计算机的 强大资源相结合，可以开发出高性能的机器人，本课题就是采用l p c ( 工控机) + d s p 的两级控制方案设 计机器人控制器。 1 3 本课题的研究对象和内容 本课题的研究对象是一个6 关节机器人手臂，由底座主体、大臂、小臂和手腕等几个部分组成，其结 构如图1 - 2 所示，连杆参数如表1 1 所示。本课题是在上一届师兄研究工作的基础上进行进一步工作，上 一届师兄主要完成了机器人的运动学分析、轨迹规划分析、电脑机器人三维图形的绘制和机器人离线编程。 本课题延续他的工作，完成的主要内容有如下几个方面： 1 ) 讨论了机器人相关控制算法； 2 ) 研制了d s p 运动控制卡和伺服接口板，搭建了机器人伺服控制系统的硬件平台； 3 ) 开发了p c iw d m 驱动程序，设计工控机和d s p 运动控制卡的通讯协议。 4 ) 编写了d s p 引导程序和c p l d 计数器程序，为搭建了机器人控制系统的软件平台打下了基础。 5 ) 编写了工控机测试程序和d s p 控制测试程序，完成了d s p 运动控制卡和伺服接口板的调试。 6 ) 完成了机器人单关节和双关节的软硬件调试。 本课题设计中使用的集成开发环境和软件资源有：p r o t e i9 9s e ，t ic c 4 0 ，m a x p i u s i i ，v i s u a ic + + 6 0 ， 、i n d d w s 2 0 0 0d dk ，d r v e r s t u d i 0 2 7 。工控机操作系统采用m i c r o s 矾公司的w i n d o w s2 0 0 0 。 工控机c p u 配置为p e n t u i m 1 0 g h z ，内存大小为2 5 6 m 。d s p 仿真器使用闻亭公司的t d s 5 1 0 j t a g 仿真器。机器人各关节上面安装有交流伺服电机，电机额定转速为3 0 0 0 转份，带有1 6 位绝对式光 电码盘和制动器，和伺服电机配套的是日本安川公司的s g d m a 伺服驱动器。 本课题采用l p c ( 工控机) + d s p 的机器人两级控制方案，设计一块以d s p 为核心的运动控制卡， 作为机器人控制器的下级，内部设计合适的控制算法，实现对机器人的实时控制：工控机作为机器人控制 器的上级，主要负责向d s p 运动控制卡传递命令和参数、人机接口、路径规划等工作，也可以实现多台 机器人的网络化。 2 第一章绪论 图卜2 机器人结构图 表1 1 ：连杆参数 杆号关节变量o i ( 度)a i ( m m )d i ( m m )转角范围 o i ( 度) ( 度) 1 日1 - 9 01 5 0 ( a ，)01 7 0 2 o2 ( 一9 0 ) 1 8 0 2 6 0 ( a 2 ) 01 3 5+ 6 0 3 03 9 06 0( a 3 )0- 7 0 + 1 9 0 4 04 9 00- 2 6 0 ( 山)1 8 0 5 059 0o0 1 3 5 6 06 1 8 0o 9 0 ( d b ) 3 5 0 注：o i ：绕z 从x 到x l 转角，以逆时针为正；o i ：绕x i 从z i 1 到z l 转角，以逆时针为正； a i ：沿) ( 1 从4 1 平移到五的距离，与x i 正方向一致时为正；d j ：沿z 从x 平移到) ( i 的距离，与 z 正方向一致时为正。 1 4 章节安排 全文共分为六章，内容如下： 第一章绪论。介绍了工业机器人技术和机器人控制器的应用和发展，给出了本课题的研究对象和 内容。 第二章机器人理论基础。简单介绍了机器人的运动学正解和逆解的求解方法，修改了师兄求解过 程中的不足，探讨了机器人控制算法，详细介绍了p | d 控制算法。 第三章机器人硬件部分设计。详细介绍了机器人d s p 运动控制卡和伺服接口板硬件电路的选型 和设计。 第四章机器人软件部分设计。编写了p c iw d m 驱动程序和d s p 下位机程序，设计了l p c 和d s p 通讯协议，编写了上位机控制程序。 第五章机器人调试。完成了机器人单关节和双关节的调试，对控制系统进行性能和控制余量的探 讨。 第六章结论与展望。对本课题的设计做出总结，对下一步的工作做了展望。 东南大学硕士学位论文 2 1 机器人运动学 第二章机器人控制理论基础 机器人运动学描述了机器人关节与组成机器人的各刚体之间的运动关系，是机器人末端工具的直角坐 标空间与机器人各关节空间之间进行相互转换的桥梁，是机器人位置控制的理论基础。机器人运动学主要 描述了以下两个基本问题： 1 ) 运动学正问题。对一个给定的机器人，已知机器人的几何尺寸和关节空间的值，求末端执行机构 相对于给定直角坐标系的位置和姿态。 2 ) 运动学逆问题。己知机器人几何尺寸和末端执行机构相对于给定直角坐标系的位置和姿态，确定 关节空间变量的值。 本课题机器人如图1 2 所示，具有六个关节自由度，且六个关节均为旋转关节。为了研究机器人各连 杆之间的位移关系，可在每个连杆上固定一个坐标系，然后描述这些坐标系之问的关系。根据d e n a v n 和 h a 旭n b e 叼提出的通用方法，用4 4 的奇次变换矩阵描述相邻两连杆的空间关系，建立机器人的运动方 程。根据图1 - 2 建立的坐标系如图2 1 所示，下面讨论对这个坐标系下机器人运动学正问题求解方法( 下 面简称正解) 和逆问题的求解方法( 下面简称逆解) 。 图2 一l 机器人坐标系 4 第二章机器人控制理论基础 2 1 1 机器人正解和求解 参考文献川中，已经对图2 1 坐标系下的机器人正解做了计算，我仔细研究后，发现该参考文献在求 解过程中，从关节3 坐标系到关节2 坐标系的转换矩阵彳：不正确，下边在参考文献【1 】正解的求解过程中 加入一个修正矩阵彳1 2 ，修正原求解的不足并重新求解6 关节机器人的正解。6 个关节坐标系的变换矩 阵和修正矩阵如下： 要享 三芎 臻 。 | | ； ：刘 琴 ；1 lc 2s 2 o 口2 c 2 肚悻一c 2 o 够： lool0 【o oo1 i c qs 4 o 昏ko c 4 o io 1o 以 lo ool 一：= c 6& s 6 一c 6 0o 00列 式中：s = s i n e ，c = c o s 只，f = o 6 ，4 2 是纠的修正矩阵。 为了得到机器人末端坐标系相对于机器人基础坐标系的位置与姿态矩阵t ，从最后坐标系开始计算a 矩阵的积，一直返回计算到基础坐标系，下面直接给出计算结果： 露= 4 7 4 ：趔彳；彳4 彳；一1 2z2 置i l 吒q 只l lo o01l ( 2 - 1 ) 式中： n ，= c l 【s 2 3 ( c 4 c 5 c 6 - s 4 s 6 ) 一c 2 3 s 5 c 6 卜s i ( s 4 c 5 c 6 + c 4 s 6 ) n ，= s 】 s 2 3 ( c 4 c 5 c 6 - s 4 s 6 ) - c 2 3 s 5 c 6 + c l ( s 4 c 5 c 6 + c 4 s 6 ) 聍：= c 2 3 ( c 4 c 5 c 6 一s 4 s 6 ) + s 2 3 s 5 c 6 d ，= c l s 2 3 ( c 4 c 5 s 6 + s 4 c 6 ) c 2 3 s 5 s 6 】- s l ( s 4 c ，s 6 - c 4 c 6 ) d 。= s l 【s 2 3 ( c 4 c 5 s 6 + s 4 c 6 ) 一c 2 3 s 5 s 6 】+ c l ( s 4 c5 s 6 一c 4 c 6 ) d ：= c 2 3 ( c 4 c 5 s 6 + s 4 c 6 ) + s 2 3 s 5 s 6 5 ( 2 1 ) 1l，j 1j q 墨0 0 q 墨o o g b o oo一o o r。l r。l r。，l r。，。l = = ： = 钟 管 鬈 如 东南大学硕士学位论文 d ，= c l ( s 2 3 c 4 s 5 + c 2 3 c 5 ) - s l s 4 s 5 d ，= s l ( s 2 3 c 4 s 5 + c 2 3 c 5 ) + c l s 4 s 5 口：= c 2 3 c 4 c j s 2 3 c j 只2d 6 ( 一c l s 2 3 c 4 s 5 + s i s 4 s 5 一c l c 2 3 c 5 ) 一d 4 c l c 2 3 + a 3 c l s 2 3 + a 2 c l s 2 + a l c l 只= d 6 ( 一s i s 2 3 c 4 s ，+ c l s 4 s 5 一s l c 2 3 c j ) - d 4 s i c 2 3 + a 3 s l s 2 3 + a 2 s i s 2 + a i s l = - d 6 ( - c 2 3 c 4 c 5 + s 2 ，c5 ) + d 4 s 2 3 + a 3 c 2 3 + a 2 c 2 其中：c 2 3 = c o s ( 岛一岛) ，s 2 3 = s i n ( 口2 一口3 ) 2 1 2 机器人逆解和解析法求解 机器人的逆解是根据已知的工具位姿求解相应的关节运动量。对六自由度机器人手臂，只有两类结构 的逆解存在封闭解，即( a ) 后三关节轴交于一点：( b ) 有三个关节轴相互平行，而其他类型的操作手臂 只有数值方法求逆解，本课题机器人的腕部三轴交于一点，可直接求出封闭解。参考文献f 1 】中给出了机器 人的逆解的解析解求法，由于加入了矩阵a 1 2 ，对参考文献【1 】中逆解也重新进行了求解。 已知机器人的位置与姿态表达式为 h ，d j n yd y h rd f oo 口，只 a vp y 吒只 o1 = 一? 4 ：4 ：4 ；4 爿；爿： ( 2 2 ) 显然，可得到n 个简单方程式，正是这些方程式产生了所要求的解。对于解析法，不是对1 2 个方 程式联立求解，而是用一个有规律的方法得到，在每一个方程式中用一系列变换矩阵的逆( a i 1 ) 左乘。然 后考察方程式右端的元素，找出那些为零或常数的元素，并令这些元素与左端元素相等以产生一个有 效方程式，然后求解这个三角函数方程式。 爿f l r = 一1 2 爿2 3 4 5 以 c l ，+ 而九yc 】d j + s 1 d ，c 】j + j l d ，c l p f + 置p y 一 一以 一毛玎，+ q ，l ， 0 一d 一 一岛d f + c l d y 0 一日 一s l n l + c l a v 0 一p2 一s l p x + c t pv 1 ( 2 3 ) 也妇岛一岛南k 一是幽q 妇c 5 一c 2 h + 毛 c 6邑妇曲+ 岛南嚷( 也而墨一岛南) 一乞矗+ 吩+ 屯呸 心妇乌一是热h + c 2 鼎( - c 2 妇岛一是热茂一岛幽c 6 一呸妇毛+ 巳蠡壤心羹悬一是蠡) 一是五一吗e 3 一是呸i s # # 6 七c 卒、s 等b c 母 s 争，一s 卒垂b ooo1l 一 4 2 _ 1 4 f l r = 3 4 爿5 6 是如嘿+ s q ) + 哦 岛鸽咚十s r ) 机砚 耳嚏q r o 蔓如q + 焉q ) + c 照 蝇q 椰r ) + 啦 耳q c i q o 蔓如咚+ 焉q ) + 一色( q q + 墨q ) + 镀 珥g 弓晖 0 6 嘶怕e _ q ) 啵心 心( q 霉峋- q ) 坞只 哦- q 只 1 = ( 2 - 4 ) 第二章机器人控制理论基础 c 6 【与c 4 岛s 3 s 5 j 。c 3 s 4 s 6 c 6 ( 蚋c 5 + c 3 s 5 ) 一s 3 s 4 s 6 。s 4 c 5 c 6 。c 4 s 6 0 ( q c 4 c s s 3 s 5 ) + c 3 s 4 c 6 s 6 ( c 4 c 5 + c 3 s 5 ) + s 3 s 4 c 6 8 4 岛s 6 + c 4 c 6 0 爿，爿f 1 1 2 1 爿f l r = 一4 5 彳6 是妊嘿悄吩) 吗挹邑她柏b ) 喝把 冯嚏+ q 吩 幽嗨怕呦坞捉 o q 托r 弓锻蝎q ) 迕见 o q c 4 8 5 + c 5 s 3 c 4 s 5 c 3 c 5 一& 岛 o 毛妃q 粥) _ h 吃廷 晖手弼 咤妊q 怕b ) 郴j 建 o fc 4 岛c 6 一s 4 s 6 c 4 c 5 j 6 + j 4 c 6c 4 j 5一c 4 s 5 d 6 l i 如屯羔乏c 4 如c 5 羹乏q 气二耋乏：2i l s 5 c 6 s 5 j 6 一c 5 c 5 d 6 + d 4l f0o01i 口- = a r c 加髻糌口j 口6 + ，j 口4 ：a r c t a i l 妄! ! ：二鱼! 竺： s2 3 ( ci a ，+ sl 口y ) + c 拈乜z 当s i n 口，= 0 时，口4 有无数个解，取前一插补点相同的关节角。 ( 2 5 ) 具体推导过程从略。 ( 2 击) ( 2 - 7 ) 3 ) 皖 由式( 2 5 ) 第三行的第一列和第二列得： 盱a r c t a n 专舞筹兰岩 任8 ，” - c 2 3 ( cl nx + sl n y ) + s2 3 n z 、1 当s i i l 护fo 时，最有无数个解，取前一插补点相同的关节角。 4 ) 只 由式( 2 5 ) 第三列前三行得： 曰，。r c t 姐圭塑坐! 生旦型堕型鱼竺塑望( 2 9 ) c2 3 ( c1 a x + s l a y ) s2 3 a z 、 7 叫叫 誊李 慧哔 嚣 东南大学硕士学位论文 5 ) 岛 由式( 2 - 5 ) 第四列的第一行和第三行可得： c 2 3 ( c l p 。+ s 1 p y 一口1 ) 一s 2 3 p ：+ c 5 d 6 + d 4 f 3 = a r c t a n 二二_ 二一 s 2 3 ( c 1 p x + s l 岛一a 1 ) + c 2 3 p ：+ c 4 s 5 d 6 一日3 6 ) 气 由式( 2 - 3 ) 右端第四列的一行和第二行和式( 2 - 5 ) 第三列第一行和第三行可得 口a = a r c t 眦( 鲁) 一a r c t 眦习万南) 其中： k = 2 ( k ，a ，k d ) k 2 = 2 ( k3 d 4 + k 4 a 3 ) k3 = c 1 px + s i p y a l + d 6 c l a x + d 6 s 1 a y k4 = pz d 6 a ： k = a 2 2 一( k32 + k 42 + d 42 + a 32 ) 7 ) 岛 e2 = 02 3 + 03 ( 2 1 0 ) ( 2 1 1 ) ( 2 1 2 ) ( 2 1 3 ) 上面求出的逆解，b ，、俄都有两个解，对于一个真实的机器人，只有一组解与实际情况相对应。必 须作出判断，选择合适的解： ( 1 ) 根据关节运动空间限制来选择合适的解：例如求得关节角1 的两个解分别为5 0 0 和2 3 0 0 ，由于e 1 的运 动范围是1 7 0 0 ，所以5 0 0 为有效解，应选择该解。 ( 2 ) 选择一个最接近的解，为使机器人运动连续又平稳，当它具有多解时，应选择最接近上一时刻的解： 例如求得关节角6 得两个解分别为3 0 0 和2 1 0 。，该关节的运动范围为3 5 0 9 设其前一采样时刻为 1 8 0 ”，则选择2 1 0 ”。 ( 3 ) 当出现无穷解时选择前一个时刻的解作为当前值。 2 2 伺服电机控制算法 机器人手臂关节的转动由伺服电机控制，伺服电机分为交流和直流两大类，本系统中使用的是交流伺 服电机，配套有伺服驱动器，整个伺服环节的控制框图如图2 _ 2 所示。在伺服控制系统中，系统设定值和 从传感器反馈回来的值相减，形成误差信号，控制器根据这个误差信号，通过一定的控制算法产生伺服电 机的控制信号。伺服控制系统主要有如下两种控制： 1 )根据输入速度和加速度给定值。控制伺服电机的速度和加速度的变化。 2 ) 根据输入的位置信号给定值，将伺服电机转动到希望的角度位置。 伺服电机位置控制算法有很多种，如p j d 控制、自适应控制、模糊控制、神经网络控制等都是很好的 控制算法。因为p i d 控制结构简单、算法易懂、使用方便、适应性广、鲁棒性强，在机器人伺服电机控制 系统设计的初期，我们采用这种控制算法实现对机器人关节伺服电机的控制，在实现控制的基础上，可以 考虑采用更加先进的控制算法。 第二二章机器人控制理论基础 图2 - 2 伺服控制系统 最简单的控制是将误差信号乘以一个比例系数形成比例控制。但是比例控制的动态性能不好，当给定 值变化太大或电机惯性较大时，电机到达目标位置时容易过冲，引起振荡，当误差信号变化太小或电机摩 擦太大时，电机到目标位置前就就停下来了，造成稳态误差。为了克服上面两种不足，在比例控制的基础 上加入微分项和积分项，就构成了比例微分积分( p l d ) 控制。 p i d 控制算法中，输入的偏差信号p ( r ) 与输出控制信号c ( r ) 之间的关系为 c ( ，) ：p 口( f ) + ，卜( r ) 出+ d 皇掣 ( 2 1 4 ) 口f 其中，p 、l 和d 分别为比例项、积分项和微分项系数。只需要调节这几个系数，就可以灵活地调节控制 信号，p 、l 和d 的作用如下： 1 ) 比例环节p 可以成比例地反映控制系统的偏差信号e ( f ) ，偏差一旦产生，控制器立即产生控制作用，以 减少偏差。 2 ) 积分环节主要用于消除静差，提高系统的无静差度。积分作用的强弱取决于积分系数l ，l 越大，积分作 用越强，反之则越弱。积分太大会引起震荡。 3 ) 微分环节能反映偏差信号的变化趋势，并能在偏差信号值变得太大之前，在系统中引入一个有效的早期 修正信号，从而加快系统的动作速度，减少调节时间，改善系统的动态性能。 公式( 2 1 4 ) 适合在模拟控制系统中使用，现代控制系统很多都是数字控制系统，需要将模拟p | d 控 制离散化，改为数字p l d 控制。数字p l d 控制分为位置式p i d 和增量式p i d 控制两种形式。将模拟p | o 控制离散化，就形成了位置式p i d 控制： 土 甜( 灯) 2 髟p ( 耵) + 蜀p ( ，) + 【p ( 七7 1 ) 一p ( 耵一丁) 】 ( 2 1 5 ) 卢o 式中，r 一采样周期，时间越短，越接近模拟p i d 控制； 七一采用序号，依次为0 、1 、2 等； “似n 一第k 采样时刻的计算机输出值； p ( r ) 一第k 采样时刻输入的偏差值； p ( 七丁一r ) 一第k 一1 采样时刻输入的偏差值 9 东南大学硕士学位论文 如一微分弑驴竿。 将位置式p l d 控制连续两个时刻的输出相减： ( 七丁) = 甜( 七丁) 一甜( 盂? 一丁) ( 2 1 6 ) 可以推导出增量式p i d 控制： rn “( i 丁) = 足p e ( 七r ) 一e ( 七丁一r ) + g ( 七丁) + 詈 p ( 矗7 ) 一2 e ( 七r r ) + p ( 七r 一2 r ) 】) ( 2 1 7 ) 分析式( 2 1 5 ) 和式( 2 1 7 ) 可知，位置式p i d 算法每次输出与整个过去状态有关，计算中要用到过去偏 差的累加值，容易产生较大的累积误差，丽增量式算法只计算增量，当存在计算误差或精度不足时，对控 制量计算的影响较小，不足之处是积分截断效应大，有静态误差，溢出影响大。本机器人伺服控制系统宜 采用增量式p j d 进行调节。 数字p i d 控制是在计算机或控制器中编程实现，因此灵活性很大，可以采用一些措施改进原p i d 的不足， 因此产生了许多p l d 的改进算法，如遇限削弱积分p | d 算法、不完全微分p i d 算法、微分先行p l d 算法、积 分分离式p i d 控制、带死区的p i d 算法、模糊p i d 算法等，可以参考文献k ，4 】。在程序设计中，可以根据各 种算法的特点结合使用，如在p i d 加入死区和上下限，可以达到较好的效果。本系统设计种选择了带积分 分离和死区的增量式p i d 位置控制算法，在5 - 1 小节将做详细的介绍。 2 3 本章小节 针对本课题的6 关节机器人控制对象，分析了该机器人的正解和逆解的求解方法，并给出了求解的结 果。讨论了p i d 闭环控制算法，这个算法在第五章中被应用于机器人的位置闭环调试，取得了较好的控制 效果。 1 0 第三章机器人运动控制系统硬件设计 第三章机器人运动控制系统硬件设计 3 1 机器人运动控制系统简介 机器人控制器的设计一般采用两种方式：分散式和集中式。分散式是为了降低风险和设计难度。设计 控制1 、2 或者3 个关节的控制器，多块相同的控制卡构成对多个关节的控制，由计算机或另外设计一块 控制卡进行集中控制：集中式是将对多个关节的控制集成在一个控制器上，集成度和控制性能较前一种高， 但是具有较高的开发风险和难度。本设计采用了后一种控制方式，将对6 关节机器人的控制集中在一个控 制器上完成。 为了实现对6 关节机器人手臂的控制，本课题研制了基于p c i 总线的d s p 运动控制卡和连接d s p 运 动控制卡与伺服驱动器的伺服接口板，具体实物图见附录b 。d s p 运动控制卡使用四层板布线，中间两层 定义为电源层和地层，以提高控制卡的抗干扰能力，伺服接口板采用2 层板布线，板卡设计是在专用p c b 设计软件p r o l e i9 9 s e 环境下完成的。 图3 1 是整个机器人的控制系统硬件总体结构图，由工控机、d s p 运动控制卡、伺服接口板、伺服驱 动器和伺服电机等5 大部分组成，图中虚线框中是d s p 伺服控制卡的部分，是硬件设计的主要部分。 运动控制卡的处理器选用t j 公司的高性能浮点型d s p t m s 3 2 d v c 3 3 ( 后边简称v c 3 3 ) ，控制卡上6 路d a 转换器，d a 转换器的输出信号作为伺服驱动器的速度给定。每个伺服电机上装有一个1 6 位精度的 绝对式光电码盘( 经过伺服驱动器转换后可以作为相对式码盘使用) ，当光电码盘随伺服电机转动时，产 生正交脉冲输出，脉冲经过鉴相和倍频后。送给1 6 位可逆计数器进行计数( 鉴相、倍频和可逆计数器电 路均在c p l d 中实现) ，v c 3 3 读取可逆计数器中的值，形成当前位置值，和给定位置值做比较，信号之 差作为下一次控制的输入，形成了伺服电机的位置闭环系统。v c 3 3 内部没有数字量输入输出单元，在伺 服接口板上设计数字量输入输出口扩展，并且数字量输入和输出单元与外界都采用了光耦进行隔离。d s p 运动控制卡通过p c i 总线和工控机连接，选用了p l x 公司的从模式芯片p c i 9 0 5 2 实现p c i 总线和板卡局 部总线的桥按。为了实现v c 3 3 和工控机之间的高速通讯，两者中间加入了双端口r a mi o t 7 1 3 2 。 丸b 相 图3 1 机器人控制系统硬件结构图 将控制系统分为d s p 控制模块、c p l d 计数模块、p c i 总线接口模块、伺服接口板模块和控制系统电 源模块五大模块，下面对这五大模块的硬件设计做详细的介绍。 东南大学硕士学位论文 3 2d s p 控制模块 3 2 1 芯片选型 用于伺服控制的数字信号处理芯片主要有t i 公司的c 2 0 0 0 系列、a d i 公司的a d s p 2 1 ) ( x 系列、m o t o r o i a 公司的的d s p 5 6 k 系列等，其中，最常用的是t i 公司开发的c 2 0 0 0 系列d s p ，这个系列的d s p 为开发伺服 控制系统做了特别的优化处理，提供了丰富的片内资源，以其中代表性的1 6 位定点型d s pt m s 3 2 0 l f 2 4 0 7 为例，它芯片内部集成了f l a s hr o m ( 闪存) 、1 6 路1 0 位模数转换通道，多个通用定时器和一个监视 ( w a t c h d o g ) 定时器，1 6 通道p w m ( 脉宽调制) 通道，两路码盘信号输入接口，4 1 个通用i ，o 引脚等。 这样的设计可以极大的减少伺服控制系统的外围芯片，提高系统的可靠性和集成度。 本课题中设计的d s p 运动控制卡，需要6 路d a 转换通道，用于读取6 个码盘的1 2 个路的码盘信号( 每 个码盘接收a 和b 两路) 和多达6 6 个数字量输入输出通道，如果选用c 2 0 0 0 系列d s p ，可以减少部分外设 的开销，但是大部分仍然需要外部扩展，并且片上许多外设资源未用到造成浪费，因此本系统中选用c 2 0 0 0 系列定点d s p 并不是很经济的做法。 最终，本d s p 运动控制卡选用了t i 公司的c 3 0 0 0 型浮点d s p t m s 3 2 d v c 3 3 作为核心处理器，v c 3 3 价 位和l f 2 4 0 7 相近，但是计算能力远远超过l f 2 4 0 7 ，是一款3 2 位型浮点处理器。v c 3 3 有2 种速度级别：1 2 0 m f l o p s 和1 5 0 mf l o p s ，其中v c 3 3 1 5 0 的定点运算能力为7 5 m l p s ，浮点运算速度为1 5 0 mf l o p s ，它 一个周期内可以进行一次加法和一次乘法运算。v c 3 3 内部结构如图3 2 所示。 图3 2t m s 3 2 0 v c 3 3 芯片内部结构图 v c 3 3 的片上资源比较丰富，主要如下： ( 1 ) 3 4 k 3 2 位双存取s r a m 。s r a m 分成4 块，其中两块为1 6 k 双字，两块为1 k 双字，可以把s r a m 映射成程序或数据存储空间，当系统的程序容量不超过3 4 k 时，可充分利用内部s r a m ，从而降低 系统的设计成本和难度，而且加快了研制周期。 ( 2 ) 4 k 3 2 位的片内屏蔽式r o m ( 0 0 0 h f f f h ) 。片内r o m 包含了b o o h o a d e 圬导程序，当芯片的模 式选择脚m c b l ，m p 为高电平时，v c 3 3 工作于微计算机模式，r o m 内的b o o t i o a d e 旧i 导程序在d s p 加电后，将外部载体他口r o m 、串口) 中的用户代码加载到片外程序存储器或片内s r a m 中。 ( 3 ) 内部有6 4 3 2 ( 位) 高速缓存，可以大大提高访问外部存储器的速度。 1 2 整三童垫墨重垫鳖型墨笪堡! 塑过 ( 3 ) 符合j e e e 标准1 1 4 9 1 的j 1 a g 仿真口。 ( 4 ) 内部锁相环电路( p l l ) 。由于使用了内部锁相环电路，v c 3 3 在保持工作频率相同的情况下，能够 使用低速的晶振或晶体，从而降低了系统成本，锁相倍数最高为5 倍频。 ( 5 ) 片上包括一个串口、两个定时器及一个d m a 控制器。 ( 6 ) v c 3 3 使用两种电源。内核电源1 8 v 和l ，o 端口电源3 3 v ，使v c 3 3 的功耗太大降低，v c 3 3 全速运 行时功耗仅为2 0 0 m w 。 v c 3 3 的硬件方面有其自身独特的设计，为系统的构造提供很大的便利，特点如下： ( 1 ) 分开的程序总线、数据总线和d m a 总线，读写数据和d m a 操作可以并行。 ( 2 ) 具有四个内部译码页选r a g e 0 一r a g e 3 ，可以大大简化与i ，o 及存储器的接口。 ( 3 ) 具有3 2 条数据线和2 4 条地址线，寻址能力达1 6 m 。 ( 4 ) 具有2 个地址发生器、2 个辅助寄存器算术单元( a r a u ) 、8 个辅助寄存器和8 个扩展精度寄存器。 ( 5 ) 提供了2 个通用外部引脚x f 0 、x f l ，可由软件设置为数字量输入输出口，也可用作v c 3 3 在多 处理器通信中的互锁操作。 v c 3 3 的软件资源也比较丰富： ( 1 ) 具有丰富的指令系统，v c 3 3 支持数据传送类、二操作数算术艘辑类、三操作数算术逻辑类、程 序控制类、互锁操作类及并行操作类指令。 ( 2 ) 灵活的程序控制，它提供了重复、跳转、调用、陷阱及返回等类型的程序控制。 ( 3 ) 高效的流水线操作。 ( 4 ) v c 3 3 支持多种寻址方式，除去常用的寄存器寻址、直接寻址、间接寻址、立即数寻址和p c 寻址 方式以外，还有循环寻址、比特寻址，2 倒序寻址等，循环寻址和比特寻址多用于求取卷积和操作， 2 倒序寻址主要用于计算f f t 。 3 2 2 模块总体框图 d s p 运动控制卡中v c 3 3 模块的结构如图3 3 所示，图中给出了v c 3 3 的重要引脚和与v c 3 3 连接的 周边器件：电压源信号模块、外部时钟源、j a t g 仿真口、复位电路、d a 转换器和f l a s hr o m 。下边 对这些周边设备作较为细致的介绍，电压源信号模块在3 7 小节统一介绍。 3 2 3 时钟基准 d s p t m 8 3 2 0 v c 3 3 图3 3d s p 控制模块结构图 v c 3 3 提供了两种外部时钟基准：无源晶振和有源晶振。无源晶振方式不需要提供外部电源，但在d s p 中需要有振荡电路，为无源晶振起振；有源晶振需要提供外部电源，晶振输出直接提供给d s p 作为时钟 基准，d s p 内部不需要提供起振电路。v c 3 3 同时支持这两种方式，连接方法分别如下： ”无源品振方式如图3 - 4 ( 1 ) 所示，在x i n