ROS下基于EtherCAT的串联机器人控制系统*

张 颖， 平雪良， 王晨学， 仇恒坦

(江南大学 机械工程学院 江苏省食品先进制造装备技术重点实验室，江苏 无锡 214122)

0 引 言

开源机器人操作系统 ( robot operating system，ROS)[1]通过提供统一的软件平台，使用统一的方式来封装机器人，使得代码重用和模块化设计变得更加简便，可方便应用在不同的机器人上，显著提高了机器人控制系统应用开发速度。基于以太网以其数据传送能力强，技术成熟度高，开发成本低等优点，受到工业控制现场总线领域的广泛关注。在机器人领域中，EtherCAT[1]网络已成为全球公认的标准。

针对目前传统机器人控制系统适用面窄，代码复用率低、扩展性差以及移植困难的问题，本文基于ROS并利用EtherCAT通信技术，设计了一种低成本、可移植、可扩展、适用面广的开源的工业串联机械人控制系统。

1 控制系统硬件组成

控制系统硬件既要保证快速性、高集成性，又要保证兼容性、开放性。采用支持EtherCAT通信的伺服驱动器，工业机器人为典型的六轴串联机器人，机械臂末端带有法兰，方便安装不同类型的末端执行器。硬件结构摒弃了传统的运动控制器，直接将上位机与执行机构中的伺服驱动器以EtherCAT总线拓扑结构连接。

2 控制系统软件设计

2.1 控制软件总体设计

控制软件框架基于ROS平台提供的ROS_control[2]实现。上层使用moveit![3]实现运动规划。底层使用ROS提供的ros_control框架实现，并使用EtherCAT技术实现通信。控制系统整体框架如图1所示。

用户可通过moveit!提供的Rviz人机接口或者基于moveit!开发的扩展功能发出运动指令，controller_manager根据用户的需求启动相应的controller，controller实现相应的运动控制算法并将计算结果通过由hardware_interface提供的接口传递到底层，实现对机器人的运动控制。

图1 控制系统框图

2.2 底层控制实现

根据控制系统的需求，底层控制模块需要实现如下功能：1)使用EtherCAT通信技术实现控制系统与机器人伺服驱动通信；2)实现机器人关节角度值到目标位置的映射；3)为controller提供硬件层接口。加载运动控制模块后运行的节点如图2所示。

图2 底层模块

pos_based_pos_traj_controller为底层运动控制模块，对上层运动规划结果进行插值计算；follow_joint_trajectory/actions:提供moveit!输出接口，传递使用moveit!接口运动规划的结果；position_command：负责接收位置控制的命令；/joint_states:读取机器人当前位置并发布在此主题上；/tf:实时输出各连杆坐标系的位姿信息；robot_HI为实现底层控功能的节点。

2.2.1 EtherCAT通信技术实现

主站控制所有从站数据的接收和发送，在一个通信周期中，主站发送以太网数据帧下行报文，从站在数据帧经过节点时对相关数据进行读取或插入操作，全部从站处理完数据帧后，由系统末端从站将数据帧向前顺序返回，并由首端从站作为响应报文将其发送给主站。

目前IgH EtherCAT Master[4]对Linux环境下EtherCAT主站的实现功能比较完善，并提供相应网卡的实时驱动。本文通信主站基于IgH实现ROS系统下主从站通信。主从站通信过程如图3所示。

图3 EtherCAT主站实现过程

EtherCAT在应用层支持CANopen(CANopen over EtherCAT)[5]协议。通过COE实现周期性任务[6]。COE完全遵从CANopen的应用层行规，伺服和运动控制的应用层行规为CiA402。本文伺服交流电机的控制模式为周期性同步位置模式。本文实现周期性任务所需配置的PDO以及对应的对象字典如表1中所示。

表1 配置PDO

2.2.2 机器人关节与执行机构映射

控制系统控制机器人运动最终以关节角度的形式进行输出，而机器人关节角度需转换为目标位置值发给相应的从站实现运动。同理，控制系统需获取实物机器人当前实际位置，驱动器中各个电机当前位置需要转换成机器人的关节角度值，反馈给控制系统。目标位置值与机器人关节角度之间的关系为

式中a为发送到从站的目标位置值；J为电机与执行机构之间的减速比；q为机器人关节需要转过的角度；2n为电机旋转一周实际位置读数的增量;a0为机器人关节于运动学零点时的实际位置值。同理，从驱动器中读取的实际位置值可由此式反解出机器人当前各关节角度值。

2.2.3 系统控制接口

控制模块在硬件层接口通过hardware_interface与真实机器人进行交互，运动控制模块由控制模块管理器调度，载入和切换由不同算法实现的各个运动控制算法模块。控制模块在管理器中的运行根据时间和运行状态的不同可分为4个过程：init()，starting()，update()，stopping()。

本文底层运动控制模块根据上层运动规划的结果是否对加速度限制进行3次或者5次样条插值[7]，也可根据用户需求实现B样条曲线等插值算法[8]。最终将运动指令解释为系列机器人关节角度值与时间的函数。

2.3 Moveit!实现

上层的运动规划通过使用moveit!接口实现，moveit!是ROS系统下为机械臂运动控制设计的功能包与工具的集合，通过使用moveit!可以快速实现机械臂的运动学正逆解、运动路径规划、碰撞检测等功能，moveit!同时提供了RVIZ显示界面，可以方便的查看机器人运动规划的路径。

2.3.1 机器人运动学模型建立

机器人数学模型的建立是对机器人进行设计与控制的前提。ROS使用统一机器人描述格式 ( unified robot description format，URDF)[9]。URDF 基于XML语言，文件中将机器人抽象为通过各个关节连接而成的连杆结构。URDF可以通过ROS可视化工具RVIZ显示。

在SolidWorks 中构建的机器人三维模型导出为STL 格式，将机器人连杆参数写入URDF 文件中。基于URDF中的机器人模型属性及当前关节数值，ROS 将自动生成各关节相连的两个连杆坐标系间的转换关系，坐标系间的转换关系可通过tf节点进行广播。机器人连杆坐标系在RVIZ下的显示效果如图4所示。

图4 机器人运动学模型

表2中为URDF中表示机器人坐标系之间的转换关系的方式，由位置和姿态来表示，姿态使用绕自身旋转的raw，pitch，yaw表示，最后3个参数表示旋转轴和转方向。

2.3.2 moveit!配置

表2 连杆坐标系转换参数

moveit!借助moveit_setup_assistant基于机器人URDF文件通过进行初步配置，此外需对controller进行配置，使用底层驱动提供的controller，moveit!借助moveit_simple_controller_manager插件实现与controller的连接。运动规划结果通过action传递到底层，在相应controller进行插值计算后将角度值转换成目标位置值传递到驱动器，从而实现控制执行元件运动到用户定义的位置处。其中机器人正逆解通过OROCOS提供的KDL插件进行求解。

2.3.3 运动规划

运动规划算法通常有两个评价指标；完备性和最优性。完备性指在有限时间内能解决所有有解问题，最优性旨在找到最优路径。开源运动规划库(open motion planning library,OMPL)运动算法库即可基于随机采样的运动规划算法库，旨在最有时间内找到有效路径。moveit!集成了OMPL，其包含诸多先进的算法库，其中快速扩张随机树(rapidly exploring random tree,RRT)[10]算法是一个概率完备的路径规划方法。在机器人自由工作空间有足够多的采样点且存在有效路径，RRT算法能够搜索到满足要求的路径。此外，基于随机采样的运动规划算法效率高，能满足实时规划的要求。本文的运动规划主要使用了RRT算法以及基于RRT扩展的RRTconnect[11]算法。

高维空间内的路径搜索相比于低维空间更为复杂，RRT算法基本思想是在搜索空间中从机器人初始点开始树状搜索，在可行空间中随机采样并拓展枝叶，搜索树覆盖目标点区域后即可停止生长。将有k个节点的搜索随机树称为Tk，xinit为初始状态，xgoal，为目标状态，xrand为在构型空间中随机选取的状态点，通过不断产生随机点并扩展RRT搜索树枝叶的方法直到搜索到目标位置。遍历随机树Tk找到Tk上离xrand距离最近的叶子节点xnear，dist(xnear，xrand)代表构型空间中两个节点之间的尺度函数，表示两个节点之间的距离。如果 dist(xnear,xrand)

为加快收敛速度，RRTconnect算法基于RRT进行了改进，规划时首先初始化两棵树，两棵树在构型空间向着对方交替扩展，反复迭代，直到两棵树相遇，则成功找到一条从始态到终态的路径。

3 实 验

启动底层驱动节点和配置后的moveit!文件，通过ROS提供的ros_graph查看当前运行的节点的信息，节点图直观显示出当前运行的节点，以及节点订阅和发布的主题，如图5所示。

图5 控制系统节点

实验主要通过moveit!提供的move_group的接口实现，定义两个点，机器人首先沿直线从目标起始点运动到目标终点，再从目标终点在关节空间下运动到目标起始点。机器人运行实物以及仿真环境中轨迹如图6所示。

图6 机器人运行状态

通过RVIZ提供的rqt_plot查看机器人运行状态，如图7(a)为某一时刻监控机器人各关节的位置与时间曲线，图7(b)为机器人各关节速度与实践的曲线。rqt_plot能够显示机器人运行状态的大致曲线，当数据比较多的时，需通过rosbag将数据记录，可在MATLAB中导入进行数据分析处理。通过监控界面可看出机器人在运动过程中，位置和速度连续平滑，机器人能够完成上层规划的运动，该控制系统能够完成运动控制需求。

图7 rqt_plot监控机器人状态

4 结 论

实验结果表明:基于ROS搭建的控制系统能够满足工业机器人控制需求。基于ROS 的工业六自由度串联机器人控制系统摒弃了传统的运动控制器，使用先进的现场总线EtherCAT技术，实现了ROS系统下对六自由度串联工业机器人有效控制。该控制系统具有开源、低成本、高性能、可扩展等特点，满足工业机器人基本控制需求。由于ros_control框架实现不具备硬实时，后续研究可以基于硬实时系统进行框架搭建，由底层运送控制硬实时，实现对机器人更高的控制要求。

[1] 余秋蕾,张崇峰,陈 萌,等. 基于EtherCAT总线的人形机器人控制系统设计[J].载人航天,2016(1):16-22.

[2] Edwards S,Lewis C.ROS-industrial:Applying the robot operating system(ROS)to industrial applications[C]∥IEEE Int’l Confe-rence on Robotics and Automation,ECHORD Workshop,2012.

[3] Chitta S,ucan I A S,Cousins S.MoveIt![ROS Topics][J].IEEE Robotics and Automation Magazine,2012,19(1):18-19.

[4] 张少勋.基于嵌入式数控平台的EtherCAT主站实现[D].广州：华南理工大学,2013.

[5] 夏 勇,叶晓东,赵江海.基于CANopen协议的数字伺服控制[J].电气自动化,2014(1):57-59，81.

[6] 刘 鑫,闵华松,陈友东,等.基于EtherCAT的工业机器人控制器设计[J].计算机工程,2012(11):290-293.

[7] 刘汝佳,王 芳,张 强,等.基于ROS的机械臂轨迹规划研究[J].导航定位与授时,2016(6):82-88.

[8] 王 宪,王 伟,张方生,等.基于嵌入式系统的机器人关节空间轨迹规划[J].传感器与微系统,2011，30 (10):19-21，25.

[9] 曹正万,平雪良,陈盛龙,等.基于ROS的机器人模型构建方法研究[J].组合机床与自动化加工技术,2015(8):51-54.

[10] 钱 伟.基于ROS的移动操作机械臂底层规划及运动仿真[D].哈尔滨：哈尔滨工业大学,2015.

[11] 刘园园.基于RRTConnect算法的双履带起重机路径规划研究[D].大连：大连理工大学,2012.