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

使用Chrome浏览器效果最佳，继续浏览，你可能不会看到最佳的展示效果，

确定继续浏览么?

复制成功，请在其他浏览器进行阅读

摘要

为了缩短机器人全向移动导航系统的开发周期，提高代码复用率和移植性，选用具有良好开源性并采用分布式架构的机器人操作系统（robot operating system，ROS）作为研发平台；鉴于传统总线协议不统一、实时性差及应用较为复杂，提出采用Linux下的IgH主站实现控制器和驱动器之间的EtherCAT （ether control for automation technology，用于控制自动化技术的以太网）通信。首先，选取合适的任务周期并将IgH主站与ROS平台进行融合；然后，对全向移动平台进行运动学建模，建立了URDF （unified robot description format，统一的机器人描述格式）模型和里程计模型，并结合ROS提供的Navigation功能包集，设计了开放性好、代码复用率高且性能稳定的全向移动导航系统。研究表明，该系统结构简单、同步性好、成本低且能有效实现自主精准导航功能。研究结果为实现全向移动机器人的精准控制提供了新方法。

关键词

机器人操作系统; EtherCAT; 全向移动; 自主导航

机器人操作系统（robot operating system，ROS）^［

1］采用分布式软件架构设计，十分有利于代码的复用和移植。其内部集成了丰富的驱动代码和算法，可方便地应用于不同类型的机器人，大大缩短机器人的研发周期。通信系统是机器人控制器的重要组成部分，其传统总线存在协议不统一、应用较为复杂和实时性较差等问题，大大制约了机器人的控制性能。德国倍福公司开发的EtherCAT （ether control for automation technology，用于控制自动化技术的以太网）^{［参考文献 2

百度学术}2］通信协议作为目前实时性最好、通信速度最快的工业以太网协议已广泛应用于高精度控制系统，成为全球公认的行业标准。近年来，开源操作系统的实时性能得到显著提升，出现了如Xenomai、RT Linux（real-time Linux，实时Linux）、RTAI （real-time application interface，实时应用接口）等实时改造方案^{［参考文献 3

百度学术}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

百度学术}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

索引	对象字典	名称	类型
	0x6040.0	控制字	UINT16
0x1600	0x6060.0	工作模式	SINT8
RxPDO	0x60FF.0	目标速度	DINT32
	0x607A.0	目标位置	DINT32
	0x6041.0	状态字	UINT16
0X1A00	0x6060.0	实际模式	SINT8
TxPDO	0x606C.0	实际速度	DINT32
	0x6064.0	实际位置	DINT32

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 platform

为了取得较好的导航效果，须保证电机间的同步性。在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主站任务周期/μs	从站运行情况
1	10	主站激活后伺服驱动器瞬间报警
2	50	运行十几秒后伺服驱动器报警
3	100	平稳运行，响应较快
4	200	平稳运行，响应时间略长
5	500	响应时间过长，延迟较大

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

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

全向移动导航系统的移动平台采用麦克纳姆轮的四轮全向驱动方式^［

10］。根据右手法则，设：车轮半径为R；轮毂径向与辊子轴线的夹角为α；移动平台前进方向为Y方向，轴距为b；移动平台横移方向为X方向，轴距为a； 1号车轮至4号车轮的角速度分别为w₁、w₂、w₃、w₄；移动平台沿X、Y方向的线速度和绕Z轴转动的角速度分别为v_x、v_y、w₀。移动平台速度与车轮角速度的转换关系为：

\begin{array}{l} [\begin{matrix} w_{1} \\ w_{2} \\ w_{3} \\ w_{4} \end{matrix}] = \\ [\begin{array}{l} \begin{matrix} 1 / R & - 1 / (R t a n α) & - (b t a n α + a) / (2 R t a n α) \end{matrix} \\ \begin{matrix} 1 / R & 1 / (R t a n α) & - (b t a n α + a) / (2 R t a n α) \end{matrix} \\ \begin{matrix} 1 / R & - 1 / (R t a n α) & (b t a n α + a) / (2 R t a n α) \end{matrix} \\ \begin{matrix} 1 / R & 1 / (R t a n α) & (b t a n α + a) / (2 R t a n α) \end{matrix} \end{array}] [\begin{matrix} v_{x} \\ v_{y} \\ w_{0} \end{matrix}] \end{array}

（1）

在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

百度学术}12］，根据全向移动平台可横移的特点，将该算法中的曼哈顿距离公式替换为欧几里得公式，以加快路径的生成。配置完相关功能包且成功定位加载地图后，分别采用改进A*算法和动态窗口法^{［参考文献 13

百度学术}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 under constant speed

图7 变速状态下电机同步性能测试结果

Fig. 7 Test results of motor synchronization performance under variable speed

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

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

4.2　导航性能测试

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

使用rplidar-s1激光雷达、Gmapping功能包^［

15］和轮式里程计构建栅格地图^{［参考文献 16

百度学术}16］，然后利用move_base功能包和可视化工具rviz进行导航。导航性能试验如图8所示。

图8 全向移动平台导航性能试验

Fig. 8 Navigation performance test of omni-directional mobile platform

图8（a）和图8（b）为全向移动平台的建图位置；图8（c）和图8（d）为建立的栅格地图，其中白色部分为可通行区域，黑色部分为障碍物^［

17］，前者示出了移动平台的初始位置，目标点为G，后者示出了导航目标位置。试验中，移动平台经过路径上的A、B点时姿态变化明显，到达目标位置后的目标姿态如图8（c）箭头方向所示。可见，全向移动平台能够进行全局路径规划和局部路径规划，导航过程中实现了动态避障，最终到达目标位置。

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

表3 基于EtherCAT的全向移动平台的导航误差

Table 3 Navigation errors of omni-directional mobile platform based on EtherCAT

实验次序	导航误差/cm
实验次序	X向	Y向
1	3.5	2.4
2	3.8	$-$ 2.5
3	$-$ 3	3.5
4	$-$ 4.7	4.5
5	4.6	3.8

为方便测量，先分别测出移动平台在X向和Y向的导航误差，再计算出最终的导航误差值。由表3可知，导航误差均值为5.44 cm。考虑到麦克纳姆轮辊子的滑移^［

18］和移动平台设计及安装的误差，可见全向移动平台的导航精度较高，可满足控制要求。

4.3　导航性能对比

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

表4 基于RS-232总线的全向移动平台的导航误差

Table 4 Navigation errors of omni-directional mobile platform based on RS-232 bus

实验次序	导航误差/cm
实验次序	X向	Y向
1	6.8	$-$ 3.6
2	5.9	6.3
3	$-$ 5.3	$-$ 4.5
4	6.5	7.4
5	$-$ 5.5	6.8

由表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的使用有利于代码的复用和移植，使整个控制系统具有开源性和可扩展性，且响应迅速、成本低。试验结果表明该导航系统可有效实现定位建图和自主导航等功能，且具有较高的导航精度。

参考文献

Open Source Robotics Foundation. ROS ［EB/OL］．［2020-07-21］. https：//www.ros.org/. [百度学术]

郇极，刘艳强. 工业以太网现场总线EtherCAT驱动程序设计及应用［M］.北京：机械工业出版社，2019：5-110. [百度学术]

HUAN Ji， LIU Yan-qiang. Design and application of industrial ethernet fieldbus EtherCAT driver ［M］. Beijing： China Machine Press， 2019： 5-110. [百度学术]

张少勋. 基于嵌入式数控平台的EtherCAT主站实现［D］.广州：华南理工大学，2013：5-58. [百度学术]

ZHANG Shao-xun. EtherCAT master station implementation based on embedded CNC platform ［D］. Guangzhou： South China University of Technology， 2013： 5-58. [百度学术]

陈金宝，韩冬，聂宏，等. ROS开源机器人控制基础［M］.上海：上海交通大学出版社，2016：150-180. [百度学术]

CHEN Jin-bao， HAN Dong， NIE Hong， et al. ROS open source robot control basis［M］. Shanghai： Shanghai Jiaotong University Press， 2016： 150-180. [百度学术]

陈永平，何永艳，王凯凯.基于ROS的机械臂模型构建及优化方法研究［J］.微型电脑应用，2020，36（2）：13-16. doi：10.1109/ACCESS.2021.3060738 [百度学术]

CHEN Yong-ping， HE Yong-yan， WANG Kai-kai. Study on ROS based model construction and optimization method of robotic arm ［J］. Application of Microcomputers， 2020， 36（2）： 13-16. [百度学术]

宗光华，邓鲁华，王巍.一种鲁棒的室外移动机器人定位方法［J］.北京航空航天大学学报，2007，33（4）：454-458. doi：10.3969/j.issn.1001-5965.2007.04.018 [百度学术]

ZONG Guang-hua， DENG Lu-hua， WANG Wei. A robust outdoor mobile robot positioning method［J］. Journal of Beijing University of Aeronautics and Astronautics， 2007， 33（4）： 454-458. [百度学术]

张颖，平雪良，王晨学，等.ROS下基于EtherCAT的串联机器人控制系统［J］.传感器与微系统，2018，37（3）：106-109. doi：10.13873/J.1000-9787（2018）03-0106-04 [百度学术]

ZHANG Ying， PING Xue-liang， WANG Chen-xue， et al. Control system of serial robot based on ROS and EtherCAT［J］. Sensors and Microsystems， 2008， 37（3）： 106-109. [百度学术]

李伟光，李晓曦，余漳，等. 基于EtherCAT实时以太网的伺服运动控制系统设计［J］.现代制造工程，2014（3）：100-104. doi：10.3969/j.issn.1671-3133.2014.03.024 [百度学术]

LI Wei-guang， LI Xiao-xi， YU Zhang， et al. Design of servo motion control system based on the real-time Ethernet EtherCAT［J］. Modern Manufacturing Engineering， 2014（3）： 100-104. [百度学术]

宋孙浩，郑天江，张驰，等.轻量一体化机器人关节的SPI通信及EtherCAT通讯研究［J］. 制造业自动化，2019，41（8）：41-46. [百度学术]

SONG Sun-hao， ZHENG Tian-jiang， ZHANG Chi， et al. Research on SPI and EtherCAT communication of lightweight integrated robot joint［J］. Manufacturing Automation， 2019， 41（8）： 41-46. [百度学术]

张洪，曾裕民，王通德.全向移动操作臂动力学建模与实验分析［J］.机械传动，2019，43（6）：64-69. [百度学术]

ZHANG Hong， ZENG Yu-min， WANG Tong-de. Dynamic modeling and experimental analysis of omnidirectional mobile manipulator［J］. Mechanical Transmission， 2019， 43（6）： 64-69. [百度学术]

胡思旺，李春杰.基于ROS的自主导航系统仿真设计［J］.自动化与仪表，2019，34（7）：80-83. [百度学术]

HU Si-wang， LI Chun-jie. Simulation design of autonomous navigation system based on ROS［J］. Automation & Instrumentation， 2019， 34（7）： 80-83. [百度学术]

李季，孙秀霞.基于改进A-Star算法的无人机航迹规划算法研究［J］.兵工学报，2008，29（7）：788-792. doi：10.3321/j.issn：1000-1093.2008.07.004 [百度学术]

LI Ji， SUN Xiu-xia. A route planning's method for unmanned aerial vehicles based on improved A-Star algorithm［J］. Acta Armamentarii， 2008， 29（7）： 788-792. [百度学术]

程传奇，郝向阳，李建胜，等.融合改进A*算法和动态窗口法的全局动态路径规划［J］.西安交通大学学报，2017，51（11）：137-143. doi：10.7652/xjtuxb201711019 [百度学术]

CHENG Chuan-qi， HAO Xiang-yang， LI Jian-sheng， et al. Global dynamic path planning based on fusion of improved A* algorithm and dynamic window approach［J］. Journal of Xi'an Jiaotong University， 2017， 51（11）： 137-143. [百度学术]

EDWARDS S， LEWIS C. ROS-industrial： applying the robot operating system （ROS） to industrial applications［C］//IEEE Int’l Conference on Robotics and Automation. New York， May 23， 2011. [百度学术]

王丹，黄鲁，李垚.基于点线特征的单目视觉同时定位与地图构建算法［J］. 机器人，2019，41（3）：392-403. doi：10.13973/j.cnki.robot.180368 [百度学术]

WANG Dan， HUANG Lu， LI Yao. A monocular visual SLAM algorithm based on point-line feature［J］. Robotics， 2019， 41（3）： 392-403. [百度学术]

MANIKAS T W， ASHENAYI K， Wainwright R L. Genetic algorithms for autonomous robot navigation［J］. Instrumentation ＆ Measurement Magazine IEEE， 2007， 12（1）： 26-31. doi：10.1109/MIM.2007.4428579 [百度学术]

王永雄，田永永，李璇，等.穿越稠密障碍物的自适应动态窗口法［J］.控制与决策，2019，34（5）：927-936. doi：10.13195/j.kzyjc.2017.1497 [百度学术]

WANG Yong-xiong， TIAN Yong-yong， LI Xuan， et al. Self-adaptive dynamic window approach in dense obstacles［J］. Control and Decision， 2019， 34（5）： 927-936. [百度学术]

王明明，朱莹莹，张磊，等.麦克纳姆轮驱动的移动机器人自适应滑模控制器设计［J］.西北工业大学学报，2018，36（4）：627-635. doi：10.1051/jnwpu/20183640627 [百度学术]

WANG Ming-ming， ZHU Ying-ying， ZHANG Lei， et al. An adaptive robust controller for a mobile robot driven by Mecanum wheels［J］. Journal of Northwestern Polytechnical University， 2018， 36（4）： 627-635. [百度学术]

赵家贵，文波. EIA RS-232-C/RS-485信号LonWorks节点［J］.北京科技大学学报，1999，21（6）：600-602. doi：10.3321/j.issn：1001-053X.1999.06.025 [百度学术]

ZHAO Jia-gui， WEN Bo. A kind of LonWorks node for EIA RS-232-C/RS-485 signal［J］. Journal of University of Science and Technology Beijing， 1999， 21（6）： 600-602. [百度学术]

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

摘要

关键词

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

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

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

3.1 底层控制设计

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

3.3 URDF模型的建立

3.4 路径规划