基于ROS和EtherCAT的机械臂控制系统_金翰扬.pdf

电气与自动化金翰扬，等基于 OS 和 EtherCAT 的机械臂控制系统第一作者简介:金翰扬(1997)男，江苏南京人，硕士研究生，研究方向为智能机器人。DOI:1019344/j cnki issn16715276202301044基于 OS 和 EtherCAT 的机械臂控制系统金翰扬，陈富林(南京航空航天大学 机电学院，江苏 南京 210016)摘要:为方便机械臂的使用和后续开发，提出一种基于 OS 和 EtherCAT 的机械臂控制系统。在上位机 OS 中构建避障检测、路径规划和通信节点等模块，将机械臂的关节信息用自定义的通信协议串口发送给 STM32;在 STM32 中移植开源的 EtherCAT 协议 SOEM，将机械臂的信息通过工业以太网发送给伺服控制器，完成机械臂的控制，并通过实验验证了该控制系统的可行性。关键词:机械臂;OS;EtherCAT;控制系统中图分类号:TP241文献标志码:B文章编号:1671-5276(2023)01-0177-04Manipulator Control System Based on OS and EtherCATJIN Hanyang，CHEN Fulin(College of Mechanical and Electrical Engineering，Nanjing University of Aeronautics and Astronautics，Nanjing 210016，China)Abstract:To facilitate the use and subsequent development of the manipulator，a manipulator control system based on OS andEtherCAT is proposed In the upper computer OS，modules such as obstacle avoidance detection，path planning andcommunication nodes are constructed，and the joint information of the manipulator is sent to STM32 via a user definedcommunication protocol serial port The open source EtherCAT protocol SOEM is transplanted into STM32，sending the informationof the manipulator to the servo controller through industrial Ethernet to complete the control of the manipulator And experiment isconducted to verify the feasibility of the proposed manipulator control systemKeywords:mechanical arm;OS;EtherCAT;control system0引言随着科学技术的发展，机械臂不断地融入到人们的生产和生活之中，而机械臂的开发与研究也与时俱进。OS(robotic operation system)是 Willow Garage 公司在 2010 年开发的一款开源协议的机器人操作系统。OS具有点对点的设计、多语言支持、架构精简，集成度高、组件化工具包丰富和免费且开源这五大特点，使得 OS 具有分布式结构，每个功能模块可以单独设计、编译，在运行时以松散耦合的方式结合，并且支持多种语言，使得其成为众多机器人开发者的主要工具1。EtherCAT 是由德国倍福公司提出的以以太网为基础的现场总线技术，具有低延时、高响应等特点，支持多种设备连接的拓扑结构2。EtherCAT 也符合 OSI 七层模型框架，但只需要物理层、数据链路层和应用层。EtherCAT 为了支持更多种类的设备以及更广泛的应用层，建立了以下应用协议:CoE(基于 EtherCAT 的 CAN 应用协议)、SoE(符合 IEC 61800 7 204 标准的伺服驱动行规)、EoE(EtherCAT 实现以太网)、FoE(EtherCAT 实现文件读取)，而从站设备无需支持所有的通信协议，只需选择合适的即可3。因此，本文将结合 OS 和 EtherCAT 开发一种机械臂控制系统4，该系统包括机械臂末端的路径规划、位置姿态的控制、人机交互界面、机械臂检测等功能。1控制系统整体框架机械臂控制系统如图 1 所示，系统主要分为两部分5，一部分是在上位机 OS 中进行人机交互、路径规划等功能，另一部分是偏向于控制伺服电机等底层的硬件。硬件部分本系统选择是 STM32F407ZGT6 芯片，STM32 和伺服电机驱动器利用 Ethercat 工业以太网总线进行通信，通过 Ethercat 总线可以根据生产环境的具体需求，添加或删减机械臂关节电机的数量或者其他 Ethercat 设备，提高了整个系统的开放性和拓展性。2上位机的搭建OS 具有分布式结构，每个节点都有各自的任务，通过对各节点间的信息交流，从而达到对机械臂统一控制作用。所以上位机主要是对避障检测、路径规划和通信节点的搭建。771电气与自动化金翰扬，等基于 OS 和 EtherCAT 的机械臂控制系统图 1机械臂控制系统总框架图21机械臂的避障处理由于机械臂和障碍物的形状各异，对其进行建模，将大大增加计算量，所以对机械臂的关节采用包围盒技术，用最小的圆柱体将机械臂包裹住6。如图 2 所示是在机械臂 DH 参数模型中的包围盒模型。由于第 1 个连杆固定在地面上，第 2 个连杆绕着第 1个连杆进行旋转，这两个连杆位置基本不会变化，不用对其进行碰撞检测。第 4 个关节绕其连杆轴线进行旋转，其相对位置不发生改变，第 4 个连杆和第 5 个连杆可以将其看为一个整体。同理，第 6 个连杆和第 7 个连杆也可以看为整体。所以可以将机械臂简化为 3 个圆柱体 C1、C2、C3相连。图 2机械臂包围盒模型如图 3 所示，用球体将障碍物进行包裹，可以将障碍物的信息简化为球心和半径。但当障碍物为细长状时，仅用一个包围球会将障碍物的体积扩大很多倍，浪费很多空间，不利于路径规划。所以如图 4 所示，采用多个相同包围球进行包围。图 3普通包围球图 4细长状障碍物包围球综上，根据各关节的齐次位置矩阵可以计算出各圆柱体的中心线的方程，通过计算中心线到球心的最短距离是否大于半径和来判断是否碰撞7。建立 OS 节点，将此碰撞检测导入 OS 中。22路径规划算法快速扩展随机树算法(rapidlyexploring random trees，T)是由 LaValle 教授于 1998 年提出的一种随机采样路径规划算法。T 算法采用的是增量式的空间搜索方式，在满足机械臂自身结构限制和环境约束的条件下，解决在静态障碍环境里的路径规划问题8。因此在机器人的路径规划方向得到了广泛的应用。如图 5 所示，在空间 V 中，确定随机树 Tree起始点Xinit、目标点 Xgoal。在 T 算法的计算过程中，在空间 V下随机采样每个关节角度=(123456)，通过机械臂运动学正解得到采样点 Xrand，遍历随机树 Tree中所有子节点，计算与采样点 Xrand之间的距离，选取距离最小的子节点 Xnear，即Xnear=min XrandX，XTree(1)图 5标准单支 T 算法的扩展图连接 Xnear和 Xrand两个子节点，并在线段上截取步长为L 的点，并对该点进行碰撞和约束检测，若条件满足，将新的节点 Xnew加入到扩展随机树 Tree中，若不符合，则重复采样的过程。新节点 Xnew的扩展公式为Xnew=Xnear+LXrandXnearXrandXnear(2)在重复执行采样 Xrand和扩展随机树 Tree的过程中，会产生 Xnew和 Xgoal之间的距离小于所设定的阈值，连接两点，从而完成路径规划。建立 OS 节点，将算法导入OS 中。871电气与自动化金翰扬，等基于 OS 和 EtherCAT 的机械臂控制系统23建立 OS 与机械臂控制器通信OS 提供了一种用于 OS 设备和非 OS 设备通信方式 rosserial，rosserial 为非 OS 设备的应用程序提供了OS 节点，使其能够通过串口与 OS 进行数据交互9。由于 OS 和机械臂控制器采用的是串口通信，串口通信是按字节进行传输的，为了确保数据传输的准确性，将路径点信息进行打包，建立下面的通信协议:union Datauint8_t buff 32;struct Joint_positionuint32_t Header;uint32_t receivenumpoints;float receivearm1;float receivearm2;float receivearm3;float receivearm4;float receivearm5;float receivearm6;joint_position;data;当 STM32 接收到 Header 标志时，表示开始接收新一轮的信息，在这数据中包含此次规划路径的路径点数和 6个关节的角度信息。3机械臂控制器的搭建31SOEMEtherCAT 主站协议SOEM(simple open ether CATMaster)是一个开源的EtherCAT 主站协议库。如图 6 所示，SOEM 是基于 C 语言编写，由若干的模块组成，并采用了分层设计。底层提供了一个由操作系统抽象层(OSAL)和硬件抽象层(OSHW)组成的抽象层。抽象层可以将操作系统和硬件分开，使得SOEM 理论上可以移植到任意的操作系统和硬件平台上。所以移植的主要目标就是重写 OSAL 和 OSHW，在移植的操作系统和硬件平台上进行具体的实现。由于 SOEM 可以不需要操作系统，所以本文采用直接移植在 STM32F4硬件平台上舍弃了操作系统层10。图 6SOEM 框架图32定时器的移植系统抽象层 OSAL 主要包含了时间和定时器还有线程相关的函数接口。由于 EtherCAT 的特性，主站必须有一个系统时钟，EtherCAT 可以根据主站的系统时钟对从站的本地时钟进行插补，从而使得从站的本地时钟近乎统一，达到时钟同步的效果。与此同时 SOEM 采用的是轮询的方式对数据帧进行接收和发送，从站会依次接收到主站发送的数据并且发送返回帧。为防止等待返回帧时间过长，采用了超时机制，当系统时钟超过了设定的时间，就不再接收返回帧从而继续进行下一步的通信。STM32F4 包含有多个微秒级定时器，将 TIM5 作为系统时钟定时器，设置 1ms 产生定时器中断。SOEM 的系统时钟采用一个关于 usec、sec 的结构体，当系统时钟定时器产生一次中断时，usec+=1 000，如果超过 106，则 sec+=1，usec=106，从而完成系统时钟的设定。33网络驱动移植硬件抽象层(OSHW)是 SOEM 移植到 STM32 上重点部分，为 STM32 提供了网络服务，是移植工作的核心部分。OSHW 主要是由 oshw 和 nicdrv 两个模块组成。OSHW 中 oshw 模块的主要功能是实现大小端的转换。STM32 存储方式是小端模式，而网络字节序是大端模式。所以必须在 oshw 中对数据进行大小端的转换，OSHW 中的 nicdrv 模块主要是实现 EtherCAT 帧的接收和发送。如图 7 所示，在 nicdrv 中设立了 16 个数据缓冲区，当有数据帧需要发送时，在数据缓冲区中先申请一个空间。在申请的数据缓冲区中和要发送的数据帧组成报文通过 MAC 层发送出去;在报文返回时，通过解读报文中的信息，从而确定发送的数据帧所在的数据缓冲区，确保发送和接收的数据帧属于同