软实时环境下机器人运动学逆解研究.pdf

软实时环境下机器人运动学逆解研究李启源，等 软实时环境下机器人运动学逆解研究 R e s e a r c ho nt h eI n v e r s eK in e m a t ic so fR o b o tu n d e rS o f tR e a l t im eE n v ir o n m e n t 李名源裴洛龙 ( 华南理工大学自动化科学与工程学院，广东广州5 1 0 6 4 0 ) 摘要：为有效抑制串联机构的末端振动，使机器人运动系统的运行更加平滑，设计了以P C I 0 4 为硬件基础、在R T L in u x 上进行二次 软件开发的机器人控制系统。它通过P C I I S A 总线连接上位机和底层硬件接口电路，以控制伺服电机。分别研究了机器人系统的模 型，以及在这一软实时系统上采用反变换法+ P ie p e r 方法求解机器人的逆运动学问题。通过比较工业机器人在伺服控制、板卡控制 和软件控制乏种模式下的运动性能，验证了软件方式的优越性，以及其在运动控制领域中的应用价值。 关键词：机器人软实时系统硬件接口机器人建模逆运动学 中图分类号：T P 2 4 2文献标志码：A A b s t r a c t ：I no r d e rt oe f f e c t iv e lyin h ib itt h ev ib r a t io na tt h ee n do fs e r ia lm e c h a n is ma n dt om a k et h er o b o tm o t io ns y s t e mr o nm o r es m o o t h ly ， t h er o b o tc o n t r o ls y s t e mw it hP C I 0 4 船t h eh R r d w a r eb a s isa n ds e c o n d a r ys o f t w a r ed e v e lo p m e n tu n d e rR T L in u xisd e s ig n e d T h eh o s tc o m p u t e r isc o n n e c t e dw it hb o t t o mla y e rh a r d w a r ein t e r f a c et h i_ ；0 u # P C I I S Ab u sf o rc o n t r o llin gs e r v om o t o r T h em o d e lo fr o b o ts y s t e ma n dt h es o lu t io n f o rin v e r s ek in e m a t ic so fr o b o tb ya d o p t in gin v e r s et r a n s f o r m a t io n + P ie p e rm e t h o do nt h is s o f tr e a l- t im es y s t e ma r er e s e a r c h e dr e s p e c t iv e iy T h r o u g ht h ein t e r - c o m p a r is o na m o n gs e r v oc o n t r o l，b o a r dc o n t r o la n ds o f t w a r ec o n t r o lf o rin d u s t r ia lr o b o t ，t h er e s u ltv e r if ie st h es u p e r io r it yo f 8 0 f lw 毗c o n t r o la n dit sa p p lic a b lev a lu e sinm o t io nc o n t r o lf ie ld K e y w o r d s ：R o b o tS o f tr e a lt im es y s t e m H a r d w a r ein t e r f a c eR o b o tm o d e lin gI n v e r s ek in e m a t ic s O 引言 典型机器人控制系统一般采用分层递阶结构 ， 即将一个复杂的任务层分解落实，最终形成一系列底 层硬件系统可高速串行执行的简单“原子级动作”。 这样的控制系统适用于大型、复杂、多任务环境下的机 器人系统，然而当考虑到空间安装尺寸、运算速度和价 格等因素时，上述传统控制系统架构就有了其“力不 从心”的地方。在这种情形下，开源软实时操作系统 架构下的机器人控制系统应运而生。 l机器人控制系统的组成 机器人控制系统采用基于P C 的技术路线。硬件 结构以P C I 0 4 架构为基础，通过P C L I S A 总线连接以 F P G A 为核心的底层硬件电路；软件结构在R T L in u x 上 进行二次开发，利用其实时性和多任务调度功能实现 复杂的机器人任务1 。 国家自然科学基金资助项目( 编号：6 0 5 7 4 0 0 4 ) ； 固家自然科学基全资助重点项目( 编号：6 0 7 3 6 a 泓) ； 教育部科技创新工程重大项目培育基金资助项目( 编号：7 0 6 9 ) 。 修改稿收到日期：2 0 1 l 0 6 一lO 。 第一作者李启源( 1 9 8 6 一) ，男。现为华南理工大学控制理论与控制 工程专业在读硕士研究生；主要从事机器智能及应用的研究。 自动化仪表第3 3 卷第4 期2 0 1 2 年4 月 2 R T L in u x 及应用系统 R T L in u x 是在L in u x 内核下层添加了一个简单的 实时内核，将L in u x 本身作为这个实时内核的优先 级最低的任务，即所有的实时任务的优先级都高于 L in u x 系统本身的以及L in u x 系统下的一般任务 。 R T U n u 【结构如图1 所示。 I lL h x 系统用户程序 图1R T L in u x 结构图 F ig 1 S t r u c t u r eo fR T L in u x 正常的L in u x 进程仍可以在L in u x 内核上运行，这 样既可以使用标准分时操作系统即L in u x 的各种服 务，又能提供低延时的实时环境“ 】。 机器人控制系统的软件部分采用lm J I K I X 平台，以C 、 C + + 语言编写，可以分为四部分：人机接口( 示教盒) ； 任务调度与分解进程，负责从用户界面进程获得指令输 1 3 直接操作硬件 软实时环境下机器人运动学逆解研究李启源，等 人，结合从运动规划与控制进程和传感器管理进程获得的 当前系统状态，作出系统动作决策，并将当前允许执行的指 令通过共享内存消息机制发送到下一层的进程执行；运 动规划与控制，根据任务调度与分解模块发出的运动指令， 实现机器人正逆学分解及运动规戈眇插补计算、位置控制 等；传感器信息处理，接收外界信息后进行预处理，然后 传递给任务调度与分解进程。这四部分各自以进程的形式 独立运行，通过共享内存方式进行交互通信，其中运动规划 与控制进程是实时进程，其他为常规进程。 按照功能划分，机器人控制系统结构如图2 所示。 一 墼堕R ，垡啦! 鲞堡 L 一 一： 示教盒 r L 申幸 实时R T L t n u z 进程 垂蓁l l嚣交 - 一L 厂 任务调度与分解进程 善0t I 0 处理解释传感嚣信息处理 L 一运动规划与控制 0 L ln u x 接口驱动 善 常规R T L ln u I 内核实时R T L in u x 内核 00 硬件设备中断 图2 机器人控制系统结构图 F ig 2 S t r u c t u r eo fr o b o tc o n t r o l s y s t e m 3 硬件接口设计 本文设计的机器人控制系统由上位机、运动控制 接口电路和被控对象构成。上位机使用桑达的 P C I 0 4 ，接口电路的核心部件是F P G A ，被控对象为安 川公司的交流伺服电机。机器人控制系统硬件电路结 构如图3 所示。 i 彳 一西；菘i葑菘：i墓占若i ； 坚! 竺 I ilF P G A 驱动程序 R T L in u x 矧位置速度命令的发送 驱动程序I ：I 传感器信号的采集 差分信号 及光电隔离 伺服电机 及外部环境 图3 硬件电路结构图 F ig 3 S c h e m a t ico ft h eh a r d w a r ec ir c u it 伺服电机自带光电编码器，能自动检测并反馈电 机的位置值，反馈信号为相位差9 0 。的A B 两相信号。 A B 两相信号进入F P G A 后，通过F P G A 内部设计的 计数器模块对A B 信号进行计数，获得各个电机的位 置信息并存储在F P G A 的内部寄存器中。P C I 0 4 通过 P C I I S A 总线，从F P G A 中读取该寄存器的值，从而得 到电机的位置值。将该电机的实际位置值与系统传输 的理想位置值比较，得到一个偏差值，再根据一定的算 法对偏差进行插补、P I D 运算等处理，得到一个新的控 制信号。通过P C I I S A 总线，将控制信号发送给伺服 控制板，最后通过F P G A 中的p u ls e 模块发送一定脉宽 的脉冲，并通过伺服控制器驱动电机达到想要的速度 和位置J 。从上述原理来看，本设计构成对电机的双 闭环控制，外部为位置环、内部为速度环，确保了电机 运动的速度与精度。 4 机器人建模 现有机器人本体属于关节式机器人，其坐标系如 图4 所示7 。 手抓坐标系 图4 机器人坐标系统 F ig 4 R o b o tc o o r d in a t es y s t e m 图4 中，机器人的六个关节都是转动关节。前三个 关节确定手腕参考点的位置，后三个关节确定手腕的方 位。与大多数的工业机器人一样，最后三个关节轴交于 一点。将该点选作手腕的参考点，同时选作为连杆坐标 系 4 、 5 、 6 的原点。关节1 的轴线为铅直方向，关节 1 和关节2 的轴线垂直交错，距离为口；关节2 和关节3 的轴线水平且平行，距离为G 2 ；关节3 和关节4 的轴线垂 直交错，距离为0 3 ；关节3 和关节4 沿；轴方向的距离为 五。由图4 可知，口l= 1 5 0n R n 、D 2 = 5 7 0I 砌、吧= 1 5 0 舢l、 也= 6 5 0ln lil。相应机器人的连杆参数如表1 所示。 表l机器人的连杆参数表 T a b 1L in kp a r a m e t e r so fr o b o t 连杆 连杆长度扭转角连杆偏移量关节角 关节范围 标号i口l- I m m 口1 1 ( 。)d 。n u n0 , ( 。)( 。) 1 4 P R O C E S SA U T O M A T O NI N S T R U M E N T A T I O NV o L3 3N o 4A p r il2 0 1 2 晏一么蕊嗒 施，业：；心一上砭 怂么 软实时环境下机器人运动学逆解研究李启源，等 5 求解过程 基于现有机器人D H 模型，使用反变换法+ P ie p e r 方法讨论机器人的逆运动学问题，具体为利用 P ie p e r 方法求解0 2 、0 3 ，其他利用反变换法求解。 在此已知的是世界坐标值x y 、z 、a 、6 、c ，在求解 过程中，首先要将世界坐标值转换为奇次变换矩阵。 奇次变换矩阵包含平移和旋转两部分。平移部分即 奇次变换矩阵的第四列的前三行的三个元素，它们 是x , y 、Z 的值；而旋转部分是前三行和前三列部分的 九个元素，它们是r , p 、y ( 即口、6 、c ) 三个角度的正余 弦组合。即： c d c o 孵 c o 眈射n 3 s in y 一8 in a c 0 8 y c 0 8 a s i够c o s y + 8 m a 8 in y ：R m ( 7 ，卢，a ) 2I s in a c o s 母 s in , v s in 卢s in y + c o s a c o s Ts in a s iI 班c o s T c o s a s in y ( 1 ) L s i咿c 。印s in 7c 。印c 。s 7 J 式中：7 = 口锣= 6 、a = c 。这样，由已知的世界坐标系中 系统的构建，即可形成真正的机器人控制系统。为验 的口、b 、c 值就可以求得旋转矩阵：足，。( y ，卢，a ) 。 证机器人控制系统软件算法的正确性和可靠性，需要 0 。求解过程即利用反变换法求解0 。，由于上述模对工业机器人在伺服控制、运动控制板卡控制以及软 型中d 2 = 0 ，故： 件控制三种控制模式下的运动性能进行对比测试。对 0 。= A t a n 2 ( p ，P ，) 一A t a n 2 ( d ：，机器人各轴在同一控制模式下不同速度运行进行对比 “丁巧R ) = A t a n 2 ( p ，以) ( 2 ) 测试，可以揭示不同速度时的运动性能。 利用P ie p e r 方法求解0 2 和0 3 ，分别有： 本方案采用激光测振仪对工业机器人各轴进行运 舅：口t 锄2 ( r 2 一k ， 动测试，给出各轴运动速度曲线和相关参数，从以上三 ( 2 口1 p ) 2 一( r 2 一k 32 ) 一a t a n 2 ( k 1 ，k 2 ) ( 3 ) 0 3 = a t a n 2 ( ，p 2 一菇2 ，石) 一a t a n 2 ( 以，a 3 )( 4 ) 至此，可以求出0 。、0 2 、0 3 的表达式，代入相应的参 数即可求出其值。下面根据手腕方位来反解出以、侠 和以。在此选用反变换法，由于现有的机器人D H 模 型与P U M A 5 6 0 的差别仅在：j i：1 2 T 的第四列不同，即奇 次变换矩阵；r 中关于平移的部分不同，关于旋转的部 分是完全一样的。 由于上文已经求出了0 。、0 2 和以，则经推导可知： 口lc lc + a y $ lc 一口；s 2 3 = 一c 4 ( 5 ) 一a z $ l+ 口y c l= s 4 s 5 ( 6 ) 只要s ，O ，便可求出以，即： 以= A t a n 2 ( 一口，5 l+ 口，C l， 一a x C lc 2 3 一a y $ lc 2 3 + a z $ ”) ( 7 ) 当s ，= 0 时，操作臂处于奇异形位。此时，关节4 与关节6 重合，只能解出以与优的和或者差。奇异形 位可以由式( 7 ) 中A t a n 2 的两个变量是否都接近0 来 判断。若都接近0 。则为奇异形位，否则不是奇异形 位。在奇异形位时，可以任意选取以的值，再计算相 应的以的值。根据求得的六，便可以进一步求解以， 方法和求解以是一样的。0 ，的封闭解为： 0 5 = A t a n 2 ( s 5 ，c 5 ) ( 8 ) 有了上述解，可求出以的封闭解为： 吼= A t a n 2 ( ，c 6 )( 9 ) 6 试验验证 通过上述软件实时系统和机器人坐标系统运动学 自动化仪表第3 3 卷第4 期2 0 1 2 年4 月 个角度对机器人的运动性能进行对比分析。 测试仪器为P o ly t e c 激光测振仪和T e k t r o n ix 四通 道数字示波器。测试方案具体介绍如下。 测试点布置：取点原则是尽量反映各轴的运动 状态，以便于测量。机器人姿态为原点。 测试方法：在三种控制模式下对各轴的运动性 能对比测试。 测试步骤：第一步是在机器人本体各测试点粘 贴反光纸，调试仪器；第二步是在伺服控制模式下，将 第一轴的运行速度分别设置为5o s 、1 0o s 、2 0o s ，从 初始位置单独进行小范围运动( 一lO 。一+ 1 0 。) ，测量 测试点的运动速度曲线和相应参数；第三步是在板卡 控制模式下，将第一轴的运行速度分别设置为5o s 、 1 0o s 、2 0o s 。从初始位置单独进行小范围运动 ( 一1 0 0 一+ 1 0 0 ) ，测量测试点的运动速度曲线及相应 参数；第四步是在软件控制模式下，将第一轴的运行速 度分别设置为5 。s 、1 0o s 、2 0o s ，从初始位置单独进 行小范围运动( 一1 0 0 + 1 0 0 ) ，测量测试点的运动速 度陷线和相应参数；第五步是使用第二步至第四步的 方法，对2 6 轴进行测试。 测试结果如图5 所示，限于篇幅，只表现第六轴。 与图5 ( a ) 和图5 ( c ) 相比，图5 ( b ) 是经过软件运动规 划伺服控制处理后系统第六轴以2 0 。s 的速度运行得 到的曲线，运行平滑稳定，毛刺较少。该结果充分说明 了以R T L in u x 为核心的软实时控制系统在稳定性以及 串联机构末端振动的抑制方面具有较好的效果。 ( 下转第2 0 页) 1 5 合成氨清洁生产监控系统设计与实现薛美盛，等 4 结束语 本文针对目前国内包括合成氨企业在内的化工企 业清洁生产评估信息瓶颈的问题，研究构建了一套基 于I n T o u c h 的合成氨企业清洁生产监控系统，实现了 合成氨企业资源能耗、生产设备技术及工况、原料产 品、污染物排放和环境管理五个子系统的信息整合，解 决了企业清洁生产的信息瓶颈。在此基础上，研究实 现了基于M a t la b 与I n T o u c h 混合编程的合成氨企业清 洁生产在线评估考核示范系统。系统采用模糊分层评 估算法评估考核了合成氨企业整厂母系统和不同层次 子系统清洁生产现状，给出了在线清洁生产水平的模 糊隶属度判断，评估结果验证了系统的可行性和有 效性。 参考文献 1 F in n v e d e nG ，M 0 b e 呼A E m , in a m le m a ls y s t e m sa n a ly s ist 0 0 】8 一锄 o v e r v ie w J J o u r n a l o fC le a n e rP r o d u c t io n , 2 0 0 5 ，1 3 ( 1 2 ) ：1 1 6 6 1 1 7 3 2 a 撕c P ，h lk lm n 凡R e v ie w0 fs m t a im b iiit y t e x lma n d t h e ir d d ir f it io n s J J o u m a l0 fC le m e rP r o d u e a o n , 2 0 0 7 ，1 5 ( 1 8 ) ：1 8 7 5 1 8 8 5 3 国家环境保护总局标准司氮肥制造业清洁生产审核指南 M 北京：化学工业出版社，2 0 0 4 【4 郑重环境污染在线监控系统架构研究 D 上海：复旦大 学，2 0 0 7 5 金闯基于I n t e m e t G P R S W e b G I S 的三峡库区水环境安全监测 系统f D 重庆：重庆大学2 0 0 7 6 曲乐安基于G I S 的洪泽湖水环境管理评价信息系统研究 D 西安：长安大学，2 0 0 9 7 冯字辽宁省鸭绿江流域水污染总量监控技术与方法研究 D 大连：大连理丁大学，2 0 0 7 8 李菲基于P L C 的油田污水处理监控系统设计实现 D 武汉： 华中科技大学，2 0 0 8 9 葛爱欣。贾敏智，吴秀红基于G P R S 的污水在线检测系统 J 机械工程与自动化，2 0 0 9 ，1 5 7 ( 6 ) ：8 5 8 7 1 0 顾薇琼石化企业外排污水水质自动监控系统应用分析 J 石 油化工环境保护，2 0 0 6 ，2 9 ( 4 ) ：4 4 4 7 1 1 A v o u r isNM ，P a g eB E n v ir o m m m a lin f m n m t le s ：m e t h o d o lo g ya n d a p p lic a t io n so fin f o r m a t io n 印in g M D o r d r e c h t ：K h w e rA c a d e m ic P u b lis h e r ，1 9 9 5 1 2 张清宇，田伟利环境管理信息系统 M 北京：化学工业出版 社，2 0 0 3 1 3 吴玉萍合成氨工艺 M 北京：化学T 业出版社，2 0 0 6 1 4 张兴民浅谈氮肥企业污水零排放治理技术 J 化肥设计， 2 0 0 7 ，4 5 ( 4 ) ：3 8 - 4 1 1 5 化工清洁生产中心H J T 1 8 8 - 2 0 0 6 清洁生产标准氮肥制造业 s 北京：国家环境保护部。2 0 0 6 1 6 】国家环保部标准司G B1 3 4 5 8 - 2 0 0 1 合成氨工业水污染物排放 标准 s 北京：国家环境保护部，2 0 0 1 1 7 T s e n gML ，L inYH ，C h iuSF F u z z yA H P - b a s e ds t u d yo fc le a n e r p r o d u c t io nim p le m e n t a t io n inT a iw a nP W Bm a n u f a c t u r e r J J o u r n a lo f C le a n e rP r o d u c t io n ，2 0 0 9 。1 7 ( 1 4 ) ：1 2 4 9 1 2 5 6 1 8