（控制理论与控制工程专业论文）远程医疗嵌入式辅助机械臂的定位控制研究.pdf

本人完全了解北京工商大学有关保留和使用学位论文的规定，即：研究生在校 攻读学位期间论文工作的知识产权单位属北京工商大学。学校有权保留并向国家有 关部门或机构送交论文的复印件和电子版，允许学位论文被查阅和借阅；学校可以 公布学位论文的全部或部分内容，可以采用影印、缩印或其它复制手段保存、汇编 学位论文。( 保密的学位论文在解密后遵守此规定) 学位论文电子版同意提交后，可于口当年口一年口二年后在学校图书馆 网站上发布，供校内师生浏览。 作者签名：必导师签名： 摘要 随着科学技术的发展和社会的进步，机器人的应用越来越普及，已逐渐渗透到工 业生产和制造及医疗领域等。在远程医疗手术系统中，控制指令通过网络传送至机械 臂，控制机械臂的工作状态、各种姿态及位移等，从而配合医生实施医疗手术。因此， 机械臂控制系统的设计问题不仅具有普遍的工程意义，还具有深入的理论意义。 本课题针对8 6 3 计划“基于远程医疗的嵌入式控制终端研究”项目中的四自由度 机械臂，提出了基于嵌入式技术的上下位机的解决方案。嵌入式控制器是机械臂系统 的核心部分，因此，首先为嵌入式控制器搭建了一个基本的嵌入式硬件平台，并选择 了合适的l i n u x 操作系统；其次，研究了t c p i p 协议、以太网及通信s o c k e t 套接 字，实现了嵌入式控制器对远程控制命令的接收、存储与处理，并在研究了平面关节 型机器人的基础上，采用d s p 实现在各关节电机的位置控制，设计了分布式控制系 统的通讯协议，实现了对各个电机的监控。采用d e n a v i t h a r t e n b e r g 参数法建立了机 械臂的运动学方程，推导出逆运动学的解析表达式。最后，利用v c + + 环境开发了远 端p c 机的控制界面，应用w i n s o c k 编写网络通信程序，构架了客户机与嵌入式控制 器的远程通信桥梁。 关键词：嵌入式系统；模糊p i d ；l i n u x 操作系统；套接字；机械臂 m o t o ri s a c c o m p l i s h e db y a l li n d e p e n d e n tj o i n tc o n t r o l l e rb a s do nd s eas e to f c o m m u n i c a t i o np r o t o c o li sd e s i g n e df o rs y s t e m t h ek i n e t i ce q u a t i o no ft h em a n i p u l a t o ri s d e d u c e du s i n gt h ed e n a v i t h a r t e n b e r gm e t h o da n dt h ei n v e r s ek i n e t i cs o l u t i o n sa r ea l s o o b t a i n e d f i n a l l y , w eh a v ep r o g r a m m e df o rp l a t f o r mi n t e r f a c eo fr e m o t ep cb yv c 抖 s 0 1 c a r e , a n da p p l i c a t i o nw a sp r o g r a m m e db yt h en e t w o r kw i n s o c k ，s ot h i sw i l lc o n n e c t t h ec l i e n ta n de m b e d d e dc o n t r o l l e r k e y w o r d s ：e m b e d d e ds y s t e m ，f u z z yp i d ，l i n u xo p e r a t i n gs y s t e m ，s o c k e t , m e c h a n i c a la r l n i i 目录 第l 章前言1 1 1 课题背景与发展现况1 1 2 嵌入式技术的发展现状与应用2 1 2 1 嵌入式的现状与发展趋势2 1 2 2 嵌入式l i n u x 国内外的研究背景3 1 2 3 基于嵌入式技术的远程机械臂系统的优点3 l - 3 课题研究的主要内容与创新点4 1 3 1 主要内容4 1 3 2 创新点5 1 4 课题研究的目的与意义5 第2 章控制对象建模与运动学分析7 2 1d e n a v i t h a r t e n b e r g 齐次变换矩阵描述多体系统7 2 2r b t - 4 s 0 1 s 机械臂运动建模与程序实现1 0 2 2 1 正运动学分析1 0 2 2 2 逆运动学分析1 3 第3 章控制系统硬件平台设计l6 3 1 控制系统总体结构1 6 3 2 硬件系统总体设计16 3 3 嵌入式服务器硬件设计l8 3 3 1a r m 系统简介1 8 3 3 2 嵌入式服务器硬件组成与设计一1 9 3 3 3 触摸屏部分硬件设计2 2 3 3 4 以太网接口硬件设计2 5 3 3 5g p r s 扩展模块设计2 7 3 4 控制卡与机械臂硬件设计3l 3 4 1 机械臂的组成与技术指标3 l 3 4 2 底层驱动控制系统硬件设计3 2 第4 章系统软件平台设计4 0 4 1 服务器端软件设计4 0 4 1 1 嵌入式系统各模块驱动程序设计一4 0 4 1 2 通信协议与通信程序设计5 1 4 1 3 嵌入式系统软件环境与控制程序开发5 6 4 2 底层运动控制器软件设计6 7 t t i 致谢。9 5 i v 岳明：远程医疗嵌入式辅助机械臂的定位控制研究 1 1 课题背景与发展现状 第一章前言 帚一早刖西 由于以计算机和通信为代表的信息技术得到广泛应用并随着自动化的高速发展， 2 0 世纪中期，科学界已开始了对现代机器人的研究。 自1 9 4 6 年第一台数字电子计算机问世以来，计算机取得了惊人的进步，向高速 度、大容量、低价格的方向发展。大批量生产的迫切需求推动了自动化技术的进展， 其结果之一便是1 9 5 2 年数控机床的诞生。另一方面，原子能实验室的恶劣环境要求 某些操作机械代替人处理放射性物质。在这一需求背景下，美国原子能委员会的阿尔 贡研究所于1 9 4 7 年开发了遥控机械手。而在1 9 5 4 年美国戴沃尔最早提出了工业机器 人的概念，并申请了专利【l j 。 此后，机器人也被用于了医学领域。1 9 9 1 年，骨科机器人r o b o t 被用于辅助完 成全髋关节置换手术。2 0 0 1 年，美国纽约的医生使用宙斯( z e u s ) 系统成功地为身处 法国的患者施行了机器人辅助内窥镜下胆囊摘术。另一方面，由于新兴技7 i i 嵌入 式系统的发展，使得它在远程控制领域中发挥着越来越重要的作用【2 】。目前计算机的 发展已显示出微型化和专业化的趋势。其中，以嵌入式系统为代表的“后p c ”时代日 益临近。 嵌入式系统已经有了近3 0 年的发展历史，它是硬件和软件交替发展的双螺旋式 发展。嵌入式系统是根据实际应用的要求，将操作系统和功能软件集成于计算机硬件 系统中，从而实现了软件与硬件一体化的计算机系统，具有软件代码小，高度自动化， 响应速度快等特点，特别适合于对实时性和多任务操作要求较高的应用场合，嵌入式 系统的基础是以应用为中心的芯片设计和面向应用的软件开发。a r m 体系作为专为 嵌入式系统设计的处理器内核，由于其高性能、低功耗、易扩展的特点，已成为嵌入 式系统领域应用最广泛的处理器之一。而与此同时，l i n u x 操作系统由于其性能优越、 支持硬件平台广泛、源代码公开、具有极强的网络功能等优点，而成为设计嵌入式系 统的一种很好的选择l 引。 近几年，国内外对远程医疗控制系统的研究正在迅速发展，尤其是基于网络技术 的远程控制系统。但是，目前在远程医疗控制领域，国内外主要还是应用单片机或 p l c 技术实现，其缺点是处理速度慢、不能处理多任务管理及控制要求，传输路径单 一，很难满足医疗控制中实际应用的要求【4 】。 由此可看出，嵌入式作为一种实时多任务操作系统，十分适用于医疗控制要求， 原因在于可将嵌入式操作系统移植到控制器中，从而满足了多线程、多任务的工作要 求，并且具有较高可靠性和稳定性。所以本课题将采用嵌入式作为控制器平台，展开 进一步研究。 岳明：远程医疗嵌入式辅助机械臂的定位控制研究 本课题来源于国家8 6 3 计划课题所属子课题“基于远程医疗的嵌入式控制终端研 究遥操作机器人手术系统。本课题为该项目的一个子课题( 编号：2 0 0 4 a a 4 2 1 0 0 2 ) ， 负责完成远程机械臂辅助手术的控制以及嵌入式系统的软硬件平台的移植。 1 2 嵌入式技术的发展现状与应用 1 2 1 嵌入式的现状与发展趋势 嵌入式计算机系统是近几年在国内外发展最为迅猛的领域之一，它几乎包括了所 有电器设备：移动计算机设备、手机、多媒体、蜂窝式电话等等。嵌入式系统日益呈 现出复杂性和多样性的特点，特别是基于a r m 处理器的嵌入式计算机系统具有功 能强大、低功耗的特点，目前在嵌入式3 2 位微处理器市场占了8 5 左右的份额【6 】。 a r m 已成为移动通信、手持设备和多媒体数字消费等嵌入式解决方案的实际标准。 基于a r m 芯片的嵌入式计算机在制造工业、过程控制、通讯、仪器仪表、汽车、船 舶、航空航天、军事装备、消费类产品等应用领域更是随处可见。 嵌入式操作系统是伴随着嵌入式计算机的发展而发展起来的，目前比较著名的嵌 入式操作系统有l i n u x 、p s o s 、n e c u l e u s 和w i n d o w sc e ，v x w o r k s 【7 】。这些嵌入 式操作系统为嵌入式计算机的发展提供了更为广阔的应用空间，特别是源码开放的嵌 入式l i n u x 的推广使得嵌入式计算机呈现出广阔的发展前景。如今，业界已经达成 广泛的共识：即嵌入式l i n u x 是大势所趋。目前l i n u x 已经被移植到多种硬件平台， 这对受开销、时间限制影响的研究与开发项目是很有吸引力的。伴随着嵌入式l i n u x 操作系统的发展，嵌入式图形界面系统也得到飞速的发展，比较著名的有： m i c r o w i n d o w s ，m i n i g u i ，q t e m b e d d e d 。嵌入式图形界面系统为嵌入式计算机系统 提供了更为友好的图形界面人机接口【8 】。 随着嵌入式操作系统的应用领域越来越广泛，嵌入式开发应运而生。在嵌入式系 统开发中，嵌入式软件是实现各种系统功能的关键。通常，对嵌入式软件的基本要求 是体积小、执行速度快，具有较好的可裁减性和可移植性。随着嵌入式系统的发展和 应用的多样性，对嵌入式软件的要求也发生了相应的变化，主要有以下几点： 1 、操作系统的支持 嵌入式系统覆盖面很广，主要是由具体应用决定的，简单的嵌入式系统没有操作 系统，而只是一个控制循环。但是，当系统变得越来越复杂时，就需要一个嵌入式操 作系统来支持，否则，应用软件就会变得过于复杂，使开发难度过大，安全性和可靠 性都难于保证。 2 、多任务且具有实时性 在多任务嵌入式系统中，合理的任务调度必不可少，单纯通过提高处理器速度无 法达到目的，这样就要求嵌入式系统的软件必须具有多任务调度能力。 2 岳明：远程医疗嵌入式辅助机械臂的定位控制研究 3 、强大的网络功能 在网络日益重要的今天，越来越多的嵌入式系统有了网络要求。嵌入式系统与各 种网络相连，这样就给系统提出了很多要求，系统需要支持协议和相关实用程序，并 且需要处理安全认证和访问控制问题。 4 、窗口交互功能 当前，很多应用领域都要求嵌入式系统能够提供类似于窗口交互系统。由于嵌入 式系统越来越追求数字化、网络化和智能化，要达到上述要求，整个系统必须是开放 的、提供标准的、方便的与众多第三方软硬件进行沟通的。 1 2 2 嵌入式l i n u x 国内外的研究背景 嵌入式l i n u x 由于代码开放性以及强大的网络功能，在嵌入式网络设备中的应用， 比起p s o s ，v x w o r k s 有许多优势。l i n u x 现在甚至支持蓝牙技术。开发嵌入式l i n u x 产 品，从产品上市时间到产品系列化，除人员以外所需的投入均较少。相对于w i n d o w s c e ，q n r x 以及国内的h o p e n 等嵌入式操作系统，嵌入式l i n u x 以其特有的开放性、与 生俱来的网络特性成为嵌入式操作系统的主流之一【9 】。 嵌入式l i n u x 是近年来国际上的一个开发热点，韩国三星电子公司已经率先推出 了采用嵌入式l i n u x 的掌上计算机的概念样机。如今，国内外纷纷开展嵌入式l i n u x 的 研究，其中，由美国新墨西哥理工学院开发的基于l i n u x 的嵌入式操作系统r t l i n u x ， 已成功地应用于航天飞机的空间数据采集、科学仪器测控、电影特技图像处理等领域。 由嵌入式l i n u x 行业的厂商l i n e o 推出的e m b e d i x ，提供了超过2 5 种的l i n u x 系统服务。 由美国网虎公司推出的x l i n u x ，是世界上比较小的嵌入式l i n u x 系统 1 0 】。我国博利思 软件公司也在致力于嵌入式l i n u x 操作系统的研究，它的产品可适应个人p d a ，w a p 手机、机顶盒等广泛的智能信息产品。中科红旗l i n u x 把工控和信息家电作为主要的 发展领域 1 1 】。可见，无论国内外，嵌入式l i n u x 的研究都是比较热的，而且取得的 效益也是可观的。 1 2 3 基于嵌入式技术的远程机械臂系统的优点 嵌入式操作系统的选择是前期设计过程的一项重要工作，这将影响到工程后期的 发布以及软件的维护。不管选用什么样的系统，都应该考虑操作系统对硬件的支持， 如果选择的系统不支持将来要使用的硬件平台，那这个系统是不合适的；其次要考虑 的是开发调试用的工具，特别是对于开销敏感和技术水平不强的企业来说，开发工具 往往在开发过程中起决定性作用，第三要考虑的问题是该系统能否满足应用需求。 由于数字化医疗仪器的核心为数据处理系统，目前医疗仪器已经从传统的p c 和工业控制计算机转向嵌入式计算机系统。并且嵌入式计算机系统由于其独特的优 3 岳明：远程医疗嵌入式辅助机械臂的定位控制研究 势，特别是可靠性和稳定性，因此使得它非常适合于应用到数字化医疗仪器。 基于l i n u x 下的嵌入式远程遥控机械臂系统的优势主要表现在设计和市场两方 面的优势： 首先，国内目前绝大多数的嵌入式软件开发技术都是在l i n u x 基础上建立起来 的，而与l i n u x 并称嵌入式市场“三巨头”的p a l mo s 和w i n d o w sc e 在国内开发 型企业中缺少根基【1 2 】。鉴于这种开发环境并考虑到系统扩展性，同时l i n u x 系统本 身在嵌入式系统开发中展现了非常具有吸引力的性能。本论文选用l i n u x 为遥控机 械臂系统的操作系统是系统中软件设计方面的优势，这一点将在随后的章节进行详细 的介绍和说明。 更为重要的是，在医疗方面的操控中，控制器一方面要控制机器人的运行，另一 方面还要兼顾手术床的控制，此外随着各种功能的增多，多任务并行处理要求成为必 然趋势，又由于医疗控制系统的特殊性，其对可靠性和实时性要求较高，不允许出现 延时或任务线程干扰的情况出现。基于嵌入式l i n u x 的控制器通常是以多线程、多任 务的模式运行，通过操作系统完成对多线程调度的管理，提高了处理器执行用户线程 的整体效率，简化了用户在多线程条件下的编程复杂度，增强了在多线程运行环境下 对各线程的保护。正是由于嵌入式控制器的这一特点非常适用于协同机器人医疗的工 作要求，有利于整个医疗手术系统的后续开发。 本系统中采用$ 3 c 2 4 1 0 x 微处理器作为控制器主机，是由韩国三星电子公司开发 的a r m 9 系列处理器。$ 3 c 2 4 1 0 x 微处理器使用a r m 9 2 0 t 核、采用0 1 8 u m 工艺c m o s 标准宏单元和存储编译器开发而成。由于采用了由a r m 公司设计的1 6 、3 2 位 a r m 9 2 0 tr i s c 处理器，$ 3 c 2 4 1 0 x 从而实现了m m u 和独立的1 6 k b 指令和1 6 k b 数据哈佛结构的缓存，且每个缓存均为8 个字长度的流水线。因此$ 3 c 2 4 1 0 x 功能强 大，可开发出具有十分竞争力的产品。 1 3 课题研究的主要内容与创新点 1 3 1 主要内容 远程遥控机械臂主要是为医学上手术服务的，本课题采用的架构是：客户机 嵌入式服务器一机器臂，本系统是一种上下位机结构，由机械臂控制系统完成伺服 运算、轨迹插补、i o 控制等功能；上位机转而进行其他的处理，如图像传输与网络通信、 机器人运动正反解运算等，大幅提高控制效率。主要控制流程是：客户机通过网络传 送命令，嵌入式服务器通过网口接收并通过串口传送给机械臂，由底层控制器完成对 机械臂的控制。 本课题将针对四自由度机械臂，建立精确的运动学模型，分析其工作空间，在此基 础上完成点到点运动的位置控制，完成整个分布式控制系统的电气控制线路的设计， 4 量塑：望堡垦堑堂塞塑壁垫塑壁箜室篁笙型堕窒 制定可靠的通信协议，在硬件平台上实现运动控制算法。最后在服务器与客户机端编 写通信程序与控制界面，实现远程控制。 1 3 2 创新点 1 、采用a r m l i n u x 平台作为机械臂控制系统的上位机，取代传统的p c 机，对位置 控制进行了软硬件的移植，发挥嵌入式系统体积小、运算快、稳定性高的特点，从技 术上推动了被动式机器人辅助手术的应用发展。 2 、从算法方面，创造性的将模糊算法应用于机械臂位置控制，将模糊控制和p i d 控 制结合起来，解决了传统控制算法对非线性、强耦合、不确定性对象无能为力的局面。 3 、将网络控制技术应用于机器人辅助手术中，使医学专家不用亲临现场就能完成被 动机器人的辅助手术，充分发挥了医学界的资源，并实现了各地专家的互动交流。 1 4 课题研究的目的与意义 任何新事物的出现或新技术的发明都是有其深刻目的。远程医疗机械臂也不例 外。 本课题研究的就是基于嵌入式控制器的远程医疗机械臂系统。当今信息化呈高速 发展的趋势，使得人们迫切需要更先进的医疗设备来保障人们的生命安全。其中手术 是最具危险性的，这就促成了远程医疗机械臂手术的发展。其现实原因如下： ( 1 ) 我国幅员辽阔医疗资源短缺，各地区医疗水平差距很大。客观上需要发展远 程医疗技术以解决这些困难。 ( 2 ) 现代网络技术、机器人技术以及计算机辅助控制技术的发展使得远程医疗手 术成为可能和必然。 ( 3 ) 远程医疗技术的实现，可使医学专家不用亲临现场就能完成病人的医治工作， 充分发挥了医学界的资源，并实现各地专家的互动交流。 ( 4 ) 从病人角度看，依靠高水平的医学专家提高了手术的质量、减少了手术创伤， 缩短病人的恢复周期，降低病人和医院的开支等，会带来一系列的技术变革。目前已 形成了机器人与计算机辅助医疗技术的研究热点。 ( 5 ) 从医生角度看，对于某些需要耗费医生巨大体力或重复性的工作，完全可以 通过机械臂的操控来辅助医生完成，以使医生节省体力，集中精力解决关键问题。另 外，由于某些放射性手术的存在，长期从事此工作的医生，其健康必然受到损害，那 么对于这种手术如果应用远程控制机械臂来辅助医生完成，那么就会避免上述问题的 发生，从而保障医生的健康。 ( 6 ) 在军事医学方面，机器人辅助手术技术在野战外科领域有重要意义，它可以 为远距离军事人员提供高质量医疗服务，将阵亡率降至最低。通过这项技术，诸如神 5 岳明：远程医疗嵌入式辅助机械臂的定位控制研究 经外科、骨科的专科医生可以在最短的时间里参与战伤外科救治，专科医生无须部署 到现场，即可以开展多地点、多病例的复杂手术，使得救治可在伤后的“黄金救治时 间”内完成【b 】。 而本课题的意义在于研究一种带有操作系统的医疗手术嵌入式控制器，由于此 操作系统具有多线程、多任务管理功能，因此可用于解决手术中复杂的协同实时工作 问题。同时由于嵌入式系统的应用，以后还可以进行g p r s 远程控制，这将进一步推 进了嵌入式技术在远程医疗手术上的应用。因此，本课题的研究具有较高的理论价值 和实用价值。 典型体上给定点和矢量的齐次坐标表达 对于空间点p ，如果在笛卡儿三坐标向量上再加上1 ，构成一个四维列向量： 尸= ：。 = c p 。，p ：，p ，t ，r ，c i = ，2 ，3 ， c 2 t ， 则称之为点p 的齐次坐标，p 。、p 2 和p ，确定了点p 的坐标。 4 对于空间矢量v ，则在其三个笛卡儿坐标轴上的投影加上0 构成四维列向量： y = ( q ，呸，0 ) r _ 式中q 、0 2 、矢量y 的分量。 2 、理想情况下相邻体间变换矩阵 ( 2 - 2 ) 根据齐次坐标表达，对多体系统的两个典型相邻体，把3 x 3 变换矩阵s j k 与矢量吼 组合起来，可以形成4x4 阶变换矩阵a j k ，其形式如下： 彳j k 】_ f 【脒】。4 1 0 7 ( 2 - 3 ) 坐标 可 ( 2 4 ) 确定 ( 2 - 5 ) ，为 计算机建模提供了方便，推广了多体系统运动学在实际中的运用。 3 、典型体上给定点的位置表达 理想状况下，若既，p 和成分别是典型体色上给定点p 在体坐标系，刀_ ，及参考 系r 中的矢径，饥) 、仞j ) 和 见) 分别为慨，既：，n ，) r 、仞护所：，乃，) r 和 扫小p 0 2 ，p 0 3 ) r ，则p j ) 可表示为： m = 吲俐 仁6 ， 典型体上给定点p 在惯性坐标系中的位置矢p o 可表示为： m 剐伽栉 仁7 ， 其中： 8 为： a s v l , ( z ) = = l0 o1 oo o o o o o o 1z o1 ( 2 - 1 1 ) 当任意体相对其相邻低序体分别绕x 轴、y 轴和z 轴转动时， a s v ，可分别表示 - s y l ( 口) = 阻s 矿】，( ) = 9 100 o 0c o s 口 0s i n 口 o0 c o s p o s i n 0 一s i n 口o c o s 口0 o1 0 s i n f l 0 l0o 0 c o s 0 0 o1 ( 2 - 1 2 ) ( 2 1 3 ) 岳明：远程医疗嵌入式辅助机械臂的定位控制研究 - s y 】，( 7 ) = c o s ,s i n ? 00 一s i n yc o s y 0 0 oo 10 ooo1 根据不同情况，即可将理想情况下，昂在坐标系r 中的具体位置求出。 ( 2 - 1 4 ) 考虑误差后，引入位置误差矢量及位移误差矢i t s 拓，相邻体间变换矩阵变为： 【a s v 】= a s v p a s v 弦 a s v ， a s v ( 2 15 ) 式中 a s v p 理想情况下，低序体分支中任意体的体运动参考坐标系相对于 其低序体参考坐标系的位置变换矩阵； 【a s v ，理想情况下，低序体分支中任意体参考坐标系相对于其体运动 参考坐标系的位移变换矩阵； a s v 雕任意体位置误差变换矩阵； a s v 。为任意体位移误差变换矩 阵。 在有误差的情况下， a o k 表达式变为： 0 a o k = h ( 【彳s y 】p a s v 胆 a s v ， a s v 。) ( 2 一l6 ) f = 那么，典型体上给定点p 在惯性坐标系r 中的实际位置可表示为： ? = 垂c c 彳s y ， a s v ，a a s v , t 彳s y ，。) 0 c 2 一，7 ， 从上式可以看出，只要确定了相邻体间各种变换矩阵，即可得到典型体上给定点 在惯性系中的坐标位置。 2 2r b t - 4 s 0 1 s 机械臂运动建模与程序实现 2 2 1 正运动学分析 机器人运动学的完整参数，即d h ( d e n a v i t h a r t e n b e r g ) 参数，包括：绕z 轴的 旋转角0 ，z 轴上两条相邻公垂线问的距离d ，每一条公垂线的长度a ，以及两个相邻 z 轴之间的夹角a 。它们通常由d h 法则来确定。d h 法则是指：用齐次坐标来描 述机器人各连杆相对于参考坐标系的空间几何关系，用4 x 4 的齐次变换矩阵来描述相 岳明：远程医疗嵌入式辅助机械臂的定位控制研究 邻两杆的空间几何关系，进而推导出机械臂末端坐标系相对于参考坐标系的空间位姿 关系。 如图2 2 所示，首先确定机械臂的参考坐标系以及各关节坐标系，确定 d h ( d e n a v i t h a r t e a l b e r s ) 运动学参数，关节变量及其变化范围。 图2 - 2 机械臂运动分析 本课题中的r b t - 4 s 0 1 s 关节机器人只涉及到物体的运动规律，不考虑产生运动 的力和力矩。机器人正运动学所研究的内容是：给定机械臂各关节的角度或位移，求 解计算机械臂末端执行器相对于参考坐标系的位置和姿态问题。各连杆变换矩阵相 乘，根据手术精度的实际需求，可忽略各项误差，从而得到机械臂末端执行器的位姿 方程为： o 五= a l 彳2 鸣彳4 = n jo 。 n y0 y ，l zd 2 0o n xp x a ) ，p ， n zpz o1 ( 2 - 1 8 ) 式( 2 1 8 ) 表示了机械臂变换矩阵o l ，它描述了末端连杆坐标系相对基坐标系的位姿， 是机械臂运动分析和综合的基础。根据上述运动方程，末端执行器的位姿给定，即已 知p 和n 、o 、a 时，找出得到该位姿的各个关节变量o i 的表达式。在机器人存在多 解的情况下，选择其中符合实际要求的最满意一组解。为了控制机械臂末端的运动， 还需要讨论机械臂运动的轨迹规划和轨迹生成方法。轨迹规划就是根据作业任务的要 求，计算出预期的轨迹。 2 18 0 幸p i ) 拳s i n ( a 4 18 0 宰p i ) ； o x = 一c o s ( a l 18 0 宰p i ) 掌c o s ( a 2 18 0 木a t ) 木s i n ( a 4 18 0 事p i ) + s i n ( a i 18 0 宰p i ) 宰s i n ( a 2 18 0 木a t ) 事 s i n ( a 4 18 0 奉p i ) 一c o s ( a l 18 0 奉p i ) 掌s i n ( a 2 18 0 木p i ) 木c o s ( a 4 18 0 宰p i ) - s i n ( a l 18 0 宰p i ) 奉c o s ( a 2 18 0 木p i ) 木c o s ( a 4 18 0 幸p d ； o y = 一s i n ( a l 18 0 幸p i ) 幸c o s ( a 2 18 0 宰p 1 ) 奉s i n ( a 4 18 0 p i ) - c o s ( a l 18 0 木p i ) 宰s i n ( a 2 18 0 * p i ) * s i n ( a 4 1 8 0 宰p i ) s i n ( a l 1 8 0 宰p i ) 宰s i n ( a 2 1 8 0 木p 1 ) 禾c o s ( a 4 1 8 0 宰p i ) + c o s ( a l 1 8 0 奉a i ) 誊c o s ( a 2 18 0 幸p i ) c o s ( a 4 18 0 宰p d ； p x = c o s ( a l 18 0 枣p i ) 宰c o s ( a 2 18 0 奎p 1 ) 宰( 1 2 + 1 4 ) 一s i n ( a l 18 0 宰p i ) 宰s i n ( a 2 18 0 木p i ) 宰( 1 2 + 1 4 ) + c o s ( a l 18 0 宰p i ) 拳1 1 ；+ 1 4 奉c o s ( a 4 l8 0 宰p i ) p y = s i n ( a 1 18 0 p i ) 宰c o s ( a 2 18 0 幸p i ) 幸( 1 2 + 1 4 ) + c o s ( a 1 18 0 木p i ) 木s i n ( a 2 18 0 幸p i ) 木( 1 2 + 1 4 ) + s i n ( a l 18 0 掌p i ) 宰1 1 ；+ 1 4 幸s i n ( a 4 18 0 宰p i ) p z = d i s + d 3 ； 1 2 运动学的方程。五= a l a z a 3 a 4 的两边分别乘a l 的逆矩阵，如下： 4 1 0 五= a z a 3 a 4 电平 ，7 、 s 蹦1 0 0 转换 c ：冈 图3 1 4g p r s 模块与$ 3 c 2 4 1 0 x 的接口硬件结构 图3 1 5g p r s 模块实物图 3 0 岳明：远程医疗嵌入式辅助机械臂的定位控制研究 3 4 控制卡与机械臂硬件设计 3 4 1 机械臂的组成与技术指标 机器人是一种具有高度灵活性的自动化机器，是一种复杂的机电一体化设备。机 器人按技术层次分为：固定程序控制机器人，示教再现机器人，智能机器人等。本课 题所使用的机器人为四自由度机器人。整个系统包括四自由度机器人1 台，控制柜l 台，控制卡1 块，实验附件l 套( 包括轴、轴座) 。 机器人采用平面关节式开链结构，即机器人各连杆由旋转关节或移动关节串联连 接，如图3 1 6 所示。各关节轴线相互平行或垂直。连杆的一端装在固定的支座上( 底 座) ，另一端处于自由状态，可安装各种工具以实现机器人作业。关节的作用是使相 互联接的两个连杆产生相对运动。关节的传动采用模块化结构，由同步齿型带和谐波 减速器等多种传动结构配合实现。 机器人各关节均采用伺服电机驱动，并通过运动控制卡实现对机器人的控制，使 机器人能够在工作空间内任意位置精确定位。 i 关节 图3 1 6 机械臂示意图 机器人技术参数如表3 2 所示1 2 4 。 表3 - 2 机器人技术参数 机构形态平面关节型 自由度 4 负载能力 2k g 动作范围关节i 转动1 l o o + l l o o 关节i i 转动 9 0 0 + 9 0 。 3 1 岳明：远程医疗嵌入式辅助机械臂的定位控制研究 关节升降o 1 5 0 m m 关节转动0 0 3 6 0 0 关节i 转动1 8 0 0 s 关节i i 转动3 6 0 0 s 最大速度 关节升降1 5 0 m m s 关节转动 3 6 0 0 s 最大展开半径4 0 0 m m 高度6 8 0 m m 本体重量4 8k g 温度o + 4 5 湿度 2 0 , - - , 8 0 不结露 振动0 5g 以下 安装环境 避免易燃、腐蚀性气体、液体 其它 勿溅水、油、粉尘等 勿接近电器噪声源 操作方式编程 电源容量 单相2 2 0 v5 0 h z8 a 3 4 2 底层驱动控制系统硬件设计 运动控制卡由高性能专用微处理器、大规模可编程器件及接v i 器件等组成，用于 实现步进伺服电机的位置、速度、加速度的控制及多个步进伺服电机的多轴协调控 制。其主要功能为：s 形、梯形自动加减速速度曲线规划；输出控制脉冲到电机驱动 器以使电机运动；能利用限位开关传感器实现高速高精度原点返回操作。 电机驱动器用来把运动控制卡提供的低功率的脉冲信号放大为能驱动电机的大 功率电信号，以驱动电机带动负载旋转。 电源部分用来提供电机供电电源及各驱动器的控制用电源，包括相关保护、滤波 器件等。 控制电路提供电气系统所需的电源开、关顺序操作及保护、报警、状态指示等控 制操作。 l 控制卡芯片选择与电路设计 本课题选用了t l 公司的2 0 0 0 系列d s p t m s 3 2 0 f 2 4 0 作为控制单元。其时钟频 3 2 另外，同时将a 和b 通道的信号接到c a p 3 i o p c 6 以及c a p 4 i o p c 7 引脚上， 利用d s p 内部的捕捉单元和一个定时器，通过测量增量式编码器a 或b 通道的脉冲 信号的频率就可以获取电机的转速大小。增量式编码器的z 通道的信号接到外部中断 输入引脚x i n t i 上，零位霍尔接近开关的输出信号接到x i n t z 上，与z 通道的信号 配合使用，确定电机的实际位置。 岳明：远程医疗嵌入式辅助机械臂的定位控制研究 粤已鬟苎翻啊r - 。t _ _ 一 鼍譬 h 裂鼢釉 m 瓷魂拦 蝴 - 甲 芋 三商 1 “ 茸 融一，r 11 一一碍；穗嘲r l 节 罂登 。 1 | 商r m 特 剿鋈_ 摹芎 壕 臻 疆 ! 莽囊 o 。- - _ 日 o o o _ _ r = = 然 a ， 荨嬲 z 罡 一# 答黑“ 蓥 葺篓 惩“确塞二努；嬲瓣二。 寰。萎雾雾 蘩 殛 雾“掣| | ，蕊溪i 鐾” 鞴 蟹 蓁。薹爹薹i蓁萎孽冒碧。 盘 |产 篓。| | 怒：一怒妄墓。” f 蓍菱泷溺溺墓蓠； i l2 g g i i - n m 盔 i 嘻委c ，l 苎e 亡0 苎! 事理 猢： i 魄； j i g ，滋 】箧一 商r l j 一 臻 rr 图3 - 1 7t m s 3 2 0 f 2 4 0d s p 最小系统 2 驱动器选择与接口电路设计 由于机械臂自身重量很大，导致电机运动时的扭矩很大，故要求直流电机驱动器 既具有较小的体积，又具有较大的电压和电流输出。经过调研，本系统选用美国a p e x 公司出品的h 桥组件s a 6 0 。 s a 6 0 是一个脉冲调制型的运算放大器，它能给负载提供1 0 a 的连续电流，电源 电压范围宽达8 0 v ，全桥运放可在较宽的电源电压范围内工作。所有的开关驱动电路 和p w m 电路都集成在运放内部，用户只需提供一个模拟输入信号，内部的p w m 频 率可通过外接电容来调整，用户也可用外接个可兼容的t t l 型的p w m 信号来同 步四象限模式的幅值和方向。s a 6 0 的外部管脚连接图如图3 1 8 所示。 b 个管 的旁 路电容，以确保在开关期间抑制干扰。电容必须尽可能的靠近管脚。 3 ) a o u t ( 9 脚) ：h 桥的半个输出端，输入电压的增大将引起a o u t 端的占空因数增大。 4 ) b o u t ( 11 脚) ：h 桥的另半个输出端，输入电压的降低将引起b o u t 端的占空因数增大。 5 ) i s e n s e a ( 8 脚) ：连接到半桥a 的底部，可通过一个电阻接到v s 的回路地作为限流 传感用，也可直接接地，这个管脚的最大电压为士2 v 。 6 ) a n a l o g g n d ( 1 脚) ：输入逻辑信号和v c c 的回路地。 7 ) i s e n s e b ( 1 2 脚) ：连接到半桥b 的底部，可通过一个电阻接到v s 的回路地作为限流 传感作用，也可以直接接地，这个管脚的最大电压为士2 v 。 8 ) a n a l o g i n p u t ( 4 脚) ：控制p w m 脉宽和模拟输入，高于v c c 2 的电压将在a o u t 端产生占空因数大于5 0 的脉冲。低于v c d 2 的电压将会在b o u t 端产生占空因数大 于5 0 的脉冲。 9 ) d i s a b l e ( 3 脚) ：控制驱动电路，该脚为高电平时，输入将失效，低电平时，所有功 能正常，如果这个管脚开路，则在内部被上拉到v c c 为高电平。 1 0 ) p w m i n p u t ( 2 脚) ：控制p w m 脉宽的t t l 电平输入，占空因数大于5 0 的输入将 在a o u t 端产生占空因数大于5 0 的脉冲，占空因数小于5 0 的输入将在b o u t 端产生 占空因数大于5 0 的脉冲。对于模拟输入，内部时钟的积分电容必须连接在这个管脚 和模拟地之间，通过选择积分电容，开关频率可在2 2 1 2 5 k h z 间变化，电容c f 计算 3 5 掉，同时加入信号反相器7 4 h c l 4 滤出叠加在p w m 信号上的支流分量干扰。 图3 - 2 0 光耦接口电路图 3 增量式编码器信号处理电路 增量式编码器信号处理电路如图3 2 l 所示。j 8 是m r 编码器的信号输入接口， 采用a m 2 6 c 3 2 把m r 编码器输出的3 个通道的r s - 4 2 2 差分信号转换成t t l 电平， 得到a ，b ，z 3 路信号。 q e p 2 掰r 图3 2 2电路编码器电路单元译码原理图 t m s 3 2 0 f 2 4 0 的允许的最高计数频率为c p u 时钟频率的1 4 ，i p ：1 6 m h z 4 = 4 m h z ， 由于计数频率是a 、b 脉冲信号频率的4 倍，所以a ，b 脉冲信号的频率不应超过 1 m h z 。 测量电机的转速有两种方法： ( 1 ) m 法，即把前述正交编码器电路单元和计数器测得的电机位置信号作差分，这种方法在电机转 速较大时测量比较准确： ( 2 ) t 法，即把a 和b 通道的信号接到c a p 3 i o p c 6 以及c a p 4 i o p c 7 引脚上，利用 d s p 内部的捕捉单元和定时器，测量a 或b 信号脉冲的宽度，计算出脉冲的频率， 3 7 岳明：远程医疗嵌入式辅助机械臂的定位控制研究 进而求出电机的转速。这种方法在电机转速较小时测量值比较准确。系统实际运行时， 可以综合利用这两种方法测量的转速信号作为速度反馈值。 4 电源系统 控制器统一采用1 2 v 外部直流电源供电，电源系统原理图见图3 2 3 所示。1 2 v 外部电源经过三端稳压模块m i c 2 9 3 0 0 5 o b u 转换成s v 直流电v c c ，给d s p 以及 其它数字电路芯片供电，v c c 经过电感滤波后供给d a 等需要模拟电源的芯片，1 2 v 外部直流电源经过大电容滤波后还将给运算放大器l m 3 5 8 供电。 m i c 2 9 3 0 0 - 5 o b u 性能稳定，最大能够提供3 a 的电流。但是，长时间工作的话， 该芯片温度将会很高，所以应该在芯片表面适当贴上散热片，防止芯片过热。由于关 节控制器将安装在机械臂狭窄的管道内，因此希望电气线路在可靠的前提下尽量简 洁，利于维修。因此电源输入端采用了一进一出两个接插件，把所有关节控制器串到 电源母线上。p o w e ri n l 用于从电源或者上一个关节控制器的电源引入电流， p o w e ro u t l 用于给下一个关节控制器提供电流。这样减少了电源线。越靠近1 2 v 外部电源的导线中电流就越大，因此采用的导线也就应该越粗。 j _ ， 懈卜音捌了k 一l “ i j i ii 亲生量= a 懈 印上( ：l l ，t 、 r 辩l 1 骥a l m lo l f | 1 飘零：科j，船媾：辩if 1 碡箩谚 ：1lljlll 图3 - 2 3电源系统原理图 3 8 ? 燃屏亍二习否 消息? l ； 、，71 日 备在l i n u x 系统中 启定时器。触摸屏 删h 结束 除定时器广_ 1 瑚累 图4 - l 触摸屏驱动流程 与按键的驱动程序相比，触摸屏驱动程序所需传递的数据结构要稍微复杂一些。 需要传递3 个数据：x ( 横) 坐标，y ( 纵) 坐标和笔动作( 按下抬起拖拽) 。因为这 里有拖拽的动作，所以当触摸屏被按下时，使用定时器跟踪查询用户的移动动作。触 摸屏驱动程序将不断地发出拖拽事件，直到用户抬起触摸屏。 为明晰程序结构与组成，以下将展示部分触摸屏驱动程序的实现代码： # u n d e fd e b u g 衔f d e fd e b u g # d e f i n ed p r i n t k ( x ) p r i n t k ( ”s 3 c 2 4 10 一t s ：”绷x ) 严由于驱动主