软实时环境下机器人系统建模与实现

第１２期　２０１１年１２月　组合机床与自动化加工技术　

Ｍｏｄｕｌａｒ　Ｍａｃｈｉｎｅ　Ｔｏｏｌ＆Ａｕｔｏｍａｔｉｃ　Ｍａｎｕｆａｃｔｕｒｉｎｇ　Ｔｅｃｈｎｉｑｕｅ　ＮＯ．１２　Ｄｅｃ．２０１１　

文章编号：１００１—２２６５（２０１１）１２—０００１—０３　软实时环境下机器人系统建模与实现水　

江龙飞，裴海龙　（华南理工大学自动化科学与工程学院，广州　５１０６４０）　

摘要：对软实时控制系统以及机器人正运动学模型的构建进行了研究。该软实时控制系统的硬件电　路结构主要是由ＰＣ１０４和ＦＰＧＡ构成，软件上在ＲＴＬｉｎｕｘ进行开发。利用该系统对工业机器人进行　了实时控制，其实验数据验证了整个控制系统的有效性。　关键词：ＲＴＬｉｎｕｘ；实时操作系统；机器人　中图分类号：ＴＰ２４２　文献标识码：Ａ　

Ｍｏｄｅｌｉｎｇ　ａｎｄ　Ｒｅａｌｉｚａｔｉｏｎ　ｏｆ　Ｒｏｂｏｔ　ｉｎ　Ｔｈｅ　Ｓｏｆｔ　Ｒｅａｌ－ｔｉｍｅ　Ｅｎｖｉｒｏｎｍｅｎｔ　。ＨＡＮＧ　Ｌｏｎｇ—ｆｅｉ．ＰＥＩ　Ｈａｉ—ｌｏｎｇ　（Ｃｏｌｌｅｇｅ　ｏｆ　Ａｕｔｏｍａｔｉｏｎ　Ｓｃｉｅｎｃｅ　ａｎｄ　Ｅｎｇｉｎｅｅｒｉｎｇ，Ｓｏｕｔｈ　Ｃｈｉｎａ　Ｕｎｉｖｅｒｓｉｔｙ　ｏｆ　Ｔｅｃｈｎｏｌｏｇｙ，Ｇｕａｎｇｚｈｏｕ　５１０６４０．Ｃｈｉｎａ）　Ａｂｓｔｒａｃｔ：Ｓｏｆｔ　ｒｅａ１．ｔｉｍｅ　ｃｏｎｔｒｏｌ　ｓｙｓｔｅｍ　ａｎｄ　ｔｈｅ　ｃｏｎｓｔｒｕｃｔｉｏｎ　ｏｆ　ｔｈｅ　ｒｏｂｏｔ　ｋｉｎｅｍａｔｉｃｓ　ｍｏｄｅｌ　ｈａｓ　ｂｅｅｎ　ｓｔｕｄ—　ｉｅｄ　Ｈａｒｄｗａｒｅ　ｓｔｒｕｃｔｕｒｅ　ｏｆ　ｔｈｅ　ＳＯｆｔ　ｒｅａｌ—ｔｉｍｅ　ｃｏｎｔｒｏ１　ｓｙｓｔｅｍ　ｉｓ　ｍａｉｎｌｙ　ｃｏｎｓｔｉｔｕｔｅｄ　ｂｙ　ｔｈｅ　ＰＣ　１　０４　ａｎｄ　ｔｈｅ　ＦＰ—　ＧＡ　ａｎｄ　ｗｅ　ｃａｎ　ｄｅｖｅｌｏｐ　ＳＯｆｔｗａｒｅ　ｉｎ　ＲＴＬｉｎｕｘ．Ｉｎｄｕｓｔｒｉａｌ　ｒｏｂｏｔｓ　ａｒｅ　ｃｏｎｔｒｏｌｌｅｄ　ｂｙ　ｔｈｅ　ｒｅａｌ—ｔｉｍｅ　ｓｙｓｔｅｍ．　Ｔｈｅ　ｅｘｐｅｒｉｍｅｎｔａｌ　ｄａｔａ　ｖａｌｉｄａｔｅ　ｔｈｅ　ｅｆｆｅｃｔｉｖｅｎｅｓｓ　ｏｆ　ｔｈｅ　ｗｈｏｌｅ　ｃｏｎｔｒｏｌ　ｓｙｓｔｅｍ．　Ｋｅｙ　ｗｏｒｄｓ：ＲＴＬｉｎｕｘ；ｒｅａｌ—ｔｉｍｅ　ｏｐｅｒａｔｉｎｇ　ｓｙｓｔｅｍ；ｒｏｂｏｔ　

０　引言　控制任务。　机器人控制系统是一种典型的多轴实时运动控　制系统。传统的机器人控制系统基本上是一种设计　者基于自己的独立结构和生产目的而开发，它采用　了专用计算机、专用机器人语言、专用微处理器的封　闭式体系结构。这种结构的控制器存在制造和使用　高、开发周期长、升级换代困难、无法添加系统的新　功能等一系列缺点。由此，本文设计了一种基于开　源软实时操作系统架构机器人控制系统。该机器人　控制系统采用开发式硬件和软件结构，可以根据需　要方便地扩展功能，具有良好的开发性和扩展性。　１　机器人控制系统的组成　该机器人控制系统采用基于ＰＣ的技术路线。　其硬件结构上主要以ＰＣ１０４架构为基础通过ＰＣＩ／　ＩＳＡ总线连接以ＦＰＧＡ为核心的底层硬件电路。软　件上主要在开源操作系统ＲＴＬｉｎｕｘ上进行二次开发，　利用其实时性和多任务调度功能实现复杂的机器人　２　ＲＴＬｉｎｕｘ　ＲＴＬｉｎｕｘ是在Ｌｉｎｕｘ内核下层添加了一个简单的实　时内核，将Ｌｉｎｕｘ本身作为这个实时内核的优先级最　低的任务，也即所有的实时任务的优先级都高于Ｌｉｎｕｘ　系统本身的以及Ｌｉｎｕｘ系统下的一般任务（图１）。　

直　接　操　作　硬　件　

图１　ＲＴＬｉｎｕｘ结构图　正常的Ｌｉｎｕｘ进程仍可以在Ｌｉｎｕｘ内核上运行，　这样既可以使用标准分时操作系统即Ｌｉｎｕｘ的各种　服务，又能提供低延时的实时环境。　这体现了ＲＴＬｉｎｕｘ设计过程中的原则：在实时模　

收稿日期：２０１１—０５—２６　基金项目：国家自然科学基金（６０７３６０２４）；教育部重大培育项目（７０８０６９）　作者简介：江龙飞（１９８６～），男，安徽桐城人，华南理工大学自动化科学与工程学院硕士研究生，研究方向为数控技术，（Ｅ—ｍａｉｌ）ｊｌｆ００８＠　１２６．ｃｏｒｎ。　・２・　组合机床与自动化加工技术　第１２期　块中运行的程序的工作量要尽量少，如果能在Ｌｉｎｕｘ　中完成而不影响实时性能的话，就尽量在Ｌｉｎｕｘ中完　成。由此ＲＴＬｉｎｕｘ内核才可以尽量做得简单。在　ＲＴＬｉｎｕｘ内核中，不应该等待资源，也不需要使用共　享旋转锁。实时任务和Ｌｉｎｕｘ进程之间的通信也是　非阻塞的，从来不用等待进队列和出队列的数据。　ＲＴＬｉｎｕｘ将系统和设备的初始化交给了Ｌｉｎｕｘ完成，　对动态资源的申请和分配也交给了Ｌｉｎｕｘ。ＲＴＬｉｎｕｘ　使用静态分配的内存来完成硬件实时任务，因为在　没有内存资源的时候，被阻塞的线程是不可能具有　实时能力的。　３应用系统结构　机器人控制系统的软件按照功能来划分的话，　则如图２所示，该系统软件部分是在ＲＴＬｉｎｕｘ平台　下，用Ｃ，Ｃ＋＋语言编写的，可以分为４部分：人机　接口（示教盒），任务调度与分解，运动规划与控制，　传感器信息处理。这四个部分各自以进程的形式独　立运行，通过共享内存方式进行交互通信。　图２机器人控制系统结构　（１）人机接口　用户界面进程实现人机界面模块和数据输入输　出模块的功能，把其他进程在共享内存发布的状态　数据转换为可为操作人员所了解的数字和图形，如　示教，设定参数、勾画运动插补运动轨迹、各传感器　的状态等；而把操作人员的操作和输入的数据转换　为指令和数据发送到指定的模块执行。　（２）任务调度与分解　任务控制器位于系统三层结构的中间，负责从用　户界面进程获得指令输入，结合从运动规划与控制进　程和传感器管理进程获得当前系统状态做出系统动作　决策，把当前允许执行的指令通过共享内存消息机制　发送到下一层的进程执行。它实现了程序处理模块、　实时诊断模块和参数管理模块三部分功能。　（３）运动规划与控制　运动规划与控制进程是系统核心部分，根据任务　调度与分解模块发出的运动指令，实现机器人正逆学　分解及运动规划／插补计算、位置控制等功能。运动　控制器是一个实时进程，在ＲＴＬｉｎｕｘ平台下，实时进程　是内核的一部分，不能使用常规的进程间通信方法进　行运动控制器和任务控制器之间的通信。因此，运动　规划与控制进程和任务调度与分解进程之间的通信只　能使用ＦＩＦＯ　ｂｕｆｆｅｒ或者共享内存的方式。　（４）传感器信息处理　传感器信息处理进程主要是将外界信息接收后　进行预处理，然后传递给任务调度与分解进程，后者　根据这些信息做相关的决策，然后调度系统进入不　同的状态。　

４硬件接口设计　该机器人控制系统由上位机、运动控制接口电　路和被控对象三部分组成。上位机使用桑达的　ＰＣ１０４，接口电路的核心部件是ＦＰＧＡ，被控对象为安　川公司的交流伺服电机如图３所示。　

以ＦＰＧＡ为核心接口电路ｌ　伺服　程序位置速１　ｌ孽　１　电机　度，命令的Ｉ＜　＝　）ｌ些　及外　誊霉篱橐辇ｌｆ詈隔Ｉ部环　信号的采集ｌ　ｌ禺　ｌ　境　

—－—————————————＿Ｊ　—————　

图３硬件电路结构图　在交流伺服电机有一个自带光电编码器，该编　码器能自动检测电机的位置，并反馈相位差９０度的　Ａ／Ｂ两相信号。ＦＰＧＡ检测到Ａ／Ｂ两相信号，通过　其内部计数器模块对Ａ／Ｂ两相信号进行计数，获得　电机的位置信息并存储在ＦＰＧＡ的内部寄存器中。　然后ＰＣ１０４通过ＰＣＩ／ＩＳＡ总线从ＦＰＧＡ中读取该寄　存器的值，从而得到电机的位置值，之后将该电机的　实际位置值和系统传输的理想位置值进行比较，从　而得到一个偏差值，然后对该偏差值进行插补、ＰＩＤ　运算等处理，就得到一个新的控制信号。再通过　ＰＣＩ／ＩＳＡ总线，发送该控制信号给伺服控制板，最后　通过ＦＰＧＡ中的ｐｕｌｓｅ模块下发一定脉宽的脉冲，通　过伺服控制器驱动电机达到想要的速度和位置。从　上述原理来看，本设计中构成对电机的双闭环控制，　外部为位置环，内部为速度环，以确保电机运动的速　度与精度。　５机器人建模　现有机器人本体坐标系如图４所示，属于关节　式机器人，６个关节都是转动关节。前３个确定手腕　参考点的位置，后三个关节确定手腕的方位。和大　多数工业机器人一轴线交于一点。该点选作为手腕　的参考点，也选作为连杆坐标系｛４｝，｛５｝，｛６｝的原　点。关节１的轴线为铅直方向，关节２和３地轴线水　平，且平行，距离为ｎ　；关节１和２的轴线垂直交错，　２０１１年ｌ２月　江龙飞，等：软实时环境下机器人系统建模与实现　・３・　距离为ｏ　；关节３和４的轴线垂直交错，距离为。　。　根据Ｄ．Ｈ模型的建模方法可以得到该机器人本体的　连杆坐标系。相应的连杆参数见表１。　图４机器人本体　表１机器人的连杆参数表　Ⅱｉ　ｌ　Ｉ　ｄ　０　关节范围　连杆参数　（ｍｍ）　（。）　（ｍｍ）　（。）　（。）　（ｍｍ）　ｌ　Ｏ　０　０　０ｌ（０）　２　ａＩ　一９Ｏ　Ｏ　０２（一９０）　［一１８０，１　８０。Ｉ　ａ．＝１５Ｏ　３　ａ２　Ｏ　Ｏ　Ｏ３（０）　［一１０５，１７５１　ａ　：５７０　［一２３５，８５］　４　ａ３　—９Ｏ　ｄ４　０　（０）　ａ，＝ｌ５０　［一１８Ｏ，１８０＝１　５　０　９０　０　０５（０）　［一４０，２２０］　ｄ　＝６５０　ｄ　＝ｌＯ５　６　０　—９０　Ｏ　０６（０）　［一３６０，３６０：　７　Ｏ　Ｏ　ｄ７　Ｏ　根据Ｄ—Ｈ模型的建模方法可以得到该机器人本　体的连杆坐标系。　在建立连杆坐标系的基础上，接下来进行机器　人的正运动学分析及模型推导（限于篇幅逆运动学　推导在此略过）：　已知机器人各个关节的旋转角度０，来求解机器　人的世界坐标，即相对于基座坐标系｛０｝的坐标值。　首先将机器人的Ｄ—Ｈ模型的参数表ｌ带入连杆　变换：　的通式１中，即可得到　；ｒ，’　；ｒ厂：　个连杆变换矩阵：　杆变换：　的通式：　ｃｏｌ　ｓＯｉ　ｃ　一１　ｓＯ　ｓ　一１　Ｏ　—ｓＯｉ　ｃＯ　｛ｃ　一１　ｃｏｌ　ｓ　一１　０　ａｉ一１　一ｄｉ　ｓｎ　一１　ｄ　％ｃ　一１　１　对比ＰＵＭＡ５６０的连杆变换矩阵，我们发现只有　连杆变换矩阵　不同，其他的５个都是完全相同的。　在进一步分析　的不同之处，发现旋转部分是完全　相同的，只有平移部分有差异，ＰＵＭＡ５６０的坐标系　｛１｝除了旋转，再在Ｙ，的方面平移ｄ　得到坐标系　｛２｝；我们的模型是在坐标系｛１｝除了旋转，再在　的方面平移ｏ，得到坐标系｛２｛。明确了这些区别和　联系，我们可以适当的时候利用ＰＵＡＭ５６０的一些东　西来分析我们现有的机器人本体模型。　最后，我们可以求出６个连杆变换之积：　：Ｔ＝　（０１）　（　）；　（０３）　（　）；　（０５）：　（　）＝　ｎ　ＯＸ　ｎｙ　ｏｙ　ｎ　ＯＺ　Ｏ　０　ｎ　ｐｘ　ａｙ　ＰＹ　０Ｚ　口　Ｏ　１　ｎ　＝ｃｌ［ｃ２３（ｃ４ｃ５ｃ６一ｓ４ｓ６）一ｓ２３ｓ５ｃ６］＋ｓ１（ｓ４ｃ５ｃ６＋ｃ４ｓ６）　ｎｙ＝ｓｌ［ｃ２３（ｃ４ｃ５ｃ６一ｓ４ｓ６）一ｓ２３ｓ５ｃ６］一ｃｌ（ｓ４ｃ５ｃ６＋ｃ４ｓ６）　，　＝一ｓ２３（ｃ４ｃ５ｃ６一ｓ４ｓ６）一ｃ２３ｓ５ｃ６　０　＝ｃｌ［ｃ２３（一ｃ４ｃ５ｓ６一ｓ４ｃ６）＋ｓ２３ｓ５ｓ６］＋ｓ１（一ｓ４ｃ５ｃ６＋ｃ４ｃ６）　ｏｙ＝ｓｌ［ｃ２３（一ｃ４ｃ５ｓ６一ｓ４ｃ６）＋ｓ２３ｓ５ｓ６］一ｃ１（一ｓ４ｃ５ｃ６＋ｃ４ｃ６）　ＯＺ＝一ｓ２３（一ｃ４ｃ５ｓ６一ｓ４ｃ６）一ｃ２３ｓ５ｓ６　。　＝一ｅｌ（ｃ２３ｃ４ｓ５＋ｓ２３ｃ５）一ｓ１ｓ４ｓ５　ａｙ：一ｓｌ（ｃ２３ｃ４ｓ５＋ｓ２３ｃ５）＋ｃ１　ｓ４ｓ５　＝ｓ２３ｃ４ｓ５一ｃ２３ｃ５　ｐｘ＝ｃｌ（ａｌ＋ａ２ｃ２＋ａ３ｃ２３一ｄ４ｓ２３）　ＰＹ＝ｓｌ（ａｌ＋ａ２ｃ２＋ａ３ｃ２３一ｄ４ｓ２３）　ｐｚ＝—－ａ３ｓ２３一ａ２ｓ２—－ｄ４ｃ２３　（２）　式（２）表示我们机器人手臂变换矩阵：　，它描述