基于EtherCAT的ROS全向移动导航系统

张 洪，邱晓天

（江南大学机械工程学院，江苏无锡214122）

机器人操作系统（robot operating system，ROS）［1］采用分布式软件架构设计，十分有利于代码的复用和移植。其内部集成了丰富的驱动代码和算法，可方便地应用于不同类型的机器人，大大缩短机器人的研发周期。通信系统是机器人控制器的重要组成部分，其传统总线存在协议不统一、应用较为复杂和实时性较差等问题，大大制约了机器人的控制性能。德国倍福公司开发的EtherCAT（ether control for automation technology，用于控制自动化技术的以太网）［2］通信协议作为目前实时性最好、通信速度最快的工业以太网协议已广泛应用于高精度控制系统，成为全球公认的行业标准。近年来，开源操作系统的实时性能得到显著提升 ，出现了如 Xenomai、RT Linux（real‐time Linux，实时Linux）、RTAI（real‐time application inter‐face，实时应用接口）等实时改造方案［3］，因此机器人控制向着高精度、高实时性的方向发展。

针对目前移动机器人代码复用率低以及应用场景受限等问题，本文基于ROS平台和EtherCAT通信技术，设计了一种开源性好、实时性好、可移植性强、成本低的全向移动导航系统。

1 全向移动导航系统的硬件组成

全向移动导航系统采用支持EtherCAT通信的步科伺服驱动器。全向移动平台配置了4个伺服驱动器，舍弃了传统的运动控制器，通过网线以EtherCAT总线线性串行的方式直接将上位机与伺服驱动器相连。通过采集激光雷达和里程计的数据实现系统自主导航，既保证了其高集成性又保证了开放性。

2 全向移动导航系统的软件框架设计

笔者提出的全向移动导航系统的软件部分主要由ROS平台提供的导航功能框架和编写相关功能包实现［4］。全向移动导航系统的软件框架如图1所示。整个导航系统运行在Xenomai实时内核中，上层路径规划由ROS功能包集Navigation实现，底层采用EtherCAT技术进行通信，配置好相关文件后可驱动移动平台。用户可通过可视化工具rviz与机器人进行交互。controller_manager根据用户发出的指令启动相应的controller，经过相关的路径规划算法运算后，将结果发送至底层，最终实现对机器人的控制。

图1 全向移动导航系统的软件框架Fig.1 Software framework of omni‐directional mobile navigation system

3 全向移动导航系统的实现

全向移动导航系统的实现过程主要包括：底层控制设计；全向移动平台运动学模型的建立；URDF（unified robot description format，统一的机器人描述格式）模型［5］的建立；里程计odometry［6］的建立；根据所用的传感器，在导航功能框架下设计和配置全向移动平台的运动控制器。

3.1 底层控制设计

根据实际的控制要求，全向移动平台的底层控制须实现如下2个功能：

1）采用EtherCAT通信技术实现伺服驱动器与上位机之间的通信；

2）根据同步性控制要求选择EtheCAT数据帧周期并实现IgH主站与ROS平台的融合。

3.1.1 EtherCAT通信的实现

在1个通信周期内，主站发送EtherCAT数据帧给各个从站；数据帧抵达从站后，每个EtherCAT从站控制器（EtherCAT slave control，ESC）依据数据帧内容判断是否进行读写操作并将判断后的数据写入数据帧；全部从站处理完数据帧后由最后一个从站将处理后的数据帧依次按照顺序返回主站。主站在整个EtherCAT网络中起控制作用。本文采用Linux下功能较为完善的 IgH EtherCAT Master［7］作为主站。考虑到EtherCAT通信以及机器人控制系统的实时性要求，采用带有Xenomai实时补丁的内核，将IgH用户层中的周期性任务移植到Xenomai的实时微内核中。EtherCAT主从站通信过程如图2所示。

图2 EtherCAT主从站通信过程Fig.2 Communication process of master station and slave stations of EtherCAT

激活主站后，分别使用ec_read（）和ec_write（）函数进行过程数据的读写，再通过ecrt_master_send（）函数发送，实现主从站间的数据交换。伺服电机的周期性任务通过EtherCAT应用层COE（CAN open over EtherCAT）协议实现，其完全遵循CAN open的应用层行规。其中伺服和运动控制的专用应用层行规为CiA402［8］。全向移动导航系统的电机控制模式为周期性同步速度模式。完成周期性任务所需的PDO（process data object，过程数据对象）配置如表1中所示。

表1 PDO配置Table 1 The configuration of PDO

3.1.2 IgH主站与ROS平台的融合

IgH主站和ROS平台均采用独立的模块化设计。因此，只需将IgH主站模块改造成ROS下的驱动节点，通过ROS硬件资源接口层以及相关的硬件接口将所需的机器人速度等状态参数下发至主站模块，在主站的周期性任务中实现过程数据的交换，而无需修改IgH主站源码。IgH主站与ROS平台的融合过程如图3所示。

图3 IgH主站与ROS平台的融合过程Fig.3 Fusion process of IgH master station and ROS plat‐form

为了取得较好的导航效果，须保证电机间的同步性。在ROS环境下编写程序时应尽可能选取较短的任务周期［9］，以减小系统的延迟和反应时间，提升实时性。但限于步科伺服驱动器的性能，不能无限制地缩短任务周期，否则会造成系统的抖动过大，数据帧丢失，伺服驱动器报警出错。

为选取合适的任务周期，进行几组IgH主站任务周期与从站运行情况的对照试验，试验结果如表2所示。

表2 IgH主站任务周期与从站运行情况对照试验的结果Table 2 The results of the control test of task cycle of IgH master station and the operation of slave station

经过测试发现，IgH主站任务周期过短容易引起系统抖动，引发伺服驱动器报警，任务周期过长则会增大系统延迟和响应时间，影响控制效果。因此，将IgH主站任务周期控制在100～200 μs。

3.2 全向移动平台运动学模型和里程计模型的建立

全向移动导航系统的移动平台采用麦克纳姆轮的四轮全向驱动方式［10］。根据右手法则，设：车轮半径为R；轮毂径向与辊子轴线的夹角为α；移动平台前进方向为Y方向，轴距为b；移动平台横移方向为X方向，轴距为a；1号车轮至4号车轮的角速度分别为w1、w2、w3、w4；移动平台沿X、Y方向的线速度和绕Z轴转动的角速度分别为vx、vy、w0。移动平台速度与车轮角速度的转换关系为：

在EtherCAT周期性任务中读取地址位0x606C.0的真实值，根据式（1）算得移动平台的真实速度值，并对其瞬时速度进行积分，得出平台的航迹即里程计odometry所需的数值。

3.3 URDF模型的建立

为抽象地描述机器人的外观，便于人机交互，ROS应用URDF来建立机器人模型。该格式是基于XML（extensible markup language，可扩展标记语言），将机器人各部件抽象成连杆，通过关节及其运动学关系将连杆相连接的。ROS提供的可视化工具rviz可显示URDF模型，方便调试。ROS中的TF（transform，变换）树能够自动求解各个坐标系之间的转换关系并广播出来。机器人连杆坐标系的布局如图4所示。

图4 机器人连杆坐标系的布局Fig.4 Coordinate system layout of robot connecting rods

3.4 路径规划

在进行路径规划前须对导航功能包进行配置［11］，主要包括全局路径规划器和本地实时规划器的设计和配置。针对在传统A*算法计算下路径冗余的问题［12］，根据全向移动平台可横移的特点，将该算法中的曼哈顿距离公式替换为欧几里得公式，以加快路径的生成。配置完相关功能包且成功定位加载地图后，分别采用改进A*算法和动态窗口法［13］进行全局和局部路径的规划。

4 全向移动导航系统的性能测试

笔者搭建了一个完整的全向移动平台用来测试全向移动导航系统的性能。其中，上位机采用一台联想Y480笔记本电脑，通过EtherCAT与4台步科伺服电机通信，搭配上海思岚科技有限公司生产的rplidar‐s1激光雷达进行建图定位和导航。全向移动平台如图5所示。

图5 全向移动平台Fig.5 Omni‐directional mobile platform

4.1 电机同步性能的测试

电机间的同步性影响着全向移动平台的建图效果和导航精度：同步性越好，建图效果越好，导航精度越高。试验时，同时对4个电机设置相同的速度，在一定的时间内读取每个电机的真实角度值，以此判断电机的同步性能。导航时，移动平台不会一直维持在匀速状态，因此进行了匀速状态和变速状态下电机同步性能的测试。根据室内机器人的运动要求，设置移动平台的速度极值为0.3 m/s，通过遥控手柄调控加速度。利用ROS中的ros_plot工具［14］，将电机输出结果处理成直线图，以直观地反映电机间的同步误差。匀速和变速状态下电机同步性能的测试结果如图6和图7所示。

图6 匀速状态下电机同步性能测试结果Fig.6 Test results of motor synchronization performance un‐der constant speed

图7 变速状态下电机同步性能测试结果Fig.7 Test results of motor synchronization performance un‐der variable speed

伺服驱动器间采取线性串行方式相连，Ether‐CAT数据帧由1号电机依次传递至4号电机。由图6可知，匀速状态下两电机之间输出角度的误差稳定在0.05°左右。

由图7（a）可知，在变速状态下，4个电机的编码器数值曲线几乎重合。编码器数值经过处理后转换成角度值输出，由图7（b）可知，在电机加上负载的情况下，两电机之间输出角度的最大误差为0.13°，呈现较高的同步性，可满足机器人的控制要求。

4.2 导航性能测试

本文构建的全向移动导航系统用于全向移动平台，实现其自主导航。为了测试其建图定位和路径规划的效果，搭建试验场地，进行全向移动平台导航性能的测试。

使用rplidar‐s1激光雷达、Gmapping功能包［15］和轮式里程计构建栅格地图［16］，然后利用move_base功能包和可视化工具rviz进行导航。导航性能试验如图8所示。

图8 全向移动平台导航性能试验Fig.8 Navigation performance test of omni‐directional mo‐bile platform

图8（a）和图8（b）为全向移动平台的建图位置；图8（c）和图8（d）为建立的栅格地图，其中白色部分为可通行区域，黑色部分为障碍物［17］，前者示出了移动平台的初始位置，目标点为G，后者示出了导航目标位置。试验中，移动平台经过路径上的A、B点时姿态变化明显，到达目标位置后的目标姿态如图8（c）箭头方向所示。可见，全向移动平台能够进行全局路径规划和局部路径规划，导航过程中实现了动态避障，最终到达目标位置。

为检验EtherCAT通信良好的实时性和电机间的同步性，设置同样的导航目标位置，进行多次试验，记录每次移动平台实际到达位置与目标位置间的误差，如表3所示。

为方便测量，先分别测出移动平台在X向和Y向的导航误差，再计算出最终的导航误差值。由表3可知，导航误差均值为5.44 cm。考虑到麦克纳姆轮辊子的滑移［18］和移动平台设计及安装的误差，可见全向移动平台的导航精度较高，可满足控制要求。

表3 基于EtherCAT的全向移动平台的导航误差Table 3 Navigation errorsofomni‐directionalmobile platform based on EtherCAT

4.3 导航性能对比

为检验笔者搭建的全向移动导航系统的性能优势，进行基于EtherCAT和基于RS‐232总线的全向移动平台的自主导航对比试验。使用相同的移动平台和4.2节所述测试方法，基于RS‐232总线的全向移动平台的导航误差如表4所示。

表4 基于RS-232总线的全向移动平台的导航误差Table 4 Navigation errorsofomni‐directionalmobile platform based on RS‐232 bus

由表4可知，导航误差均值为8.51 cm。可见，基于基于EtherCAT的全向移动导航系统具有更高的导航精度。

在系统硬件方面，由于RS‐232接口在总线上只允许连接1个从站［19］，需要额外的串口扩展卡才能满足使用要求，而EtherCAT主站理论上可连接65 536个从站且不需要额外的扩展硬件。此外RS‐232接口的有效传输距离为15 m，远短于EtherCAT总线的有效传输距离。

5 结 论

针对传统机器人控制系统存在的实时性差、定位精度低的问题，笔者搭建了基于EtherCAT的ROS全向移动导航系统。其采用性能优越的EtherCAT通信技术实现PC（personal computer，个人计算机）与伺服驱动器之间的通信，舍弃了传统的运动控制器，精简了控制系统，提高了电机间的同步性。ROS的使用有利于代码的复用和移植，使整个控制系统具有开源性和可扩展性，且响应迅速、成本低。试验结果表明该导航系统可有效实现定位建图和自主导航等功能，且具有较高的导航精度。