基于ROS和IgH主站的多关节机器人控制系统设计

张 洪，黄 昭，张小勇，王通德

(1.江南大学机械工程学院，江苏省食品先进制造装备技术重点实验室，江苏无锡 214122；2.江苏南瑞恒驰电气装备有限公司，江苏无锡 214161)

0 引言

随着智能制造的快速发展和机器人应用范围的扩大，机器人面临的环境越来越复杂，对机器人的性能要求也越来越高。工业机器人通常由机械臂与固定底座构成，在当地的工作空间内执行一系列任务[1]。智能、网络化、高精度、高速正成为工业机器人未来的发展趋势。网络化控制实现了伺服驱动器、控制器和传感器之间的相互通信，以及生产线上各机器人之间的信息交换，简化了系统结构，实现了协同操作。此外，多关节机器人的网络化运动控制器是实现高精度、智能化控制的重要因素。然而，传统机器人系统的开放性和可扩展性较差，大多数不具备网络能力。其控制器通常具有特定的功能，特殊的封闭特性限制了系统的扩展和改进。传统的机器人控制器软件难以从一个系统转移到另一个系统，且由于控制器的封闭结构，难以根据需求进行扩展。

机器人开源操作系统(robot operating system,ROS)对于机器人的各种软硬件进行了封装，对于不同类型的机器人，通过ROS可以使用相同的方式表示，ROS为用户提供模块化的通信机制，用户可以方便地替换系统内的各种模块，从而促使机器人的研发周期显著缩减。实时工业以太网具有高精度、高实时性以及数据传输性能强等优点，在高精度机器人控制系统中得到广泛的关注与应用[2]。

针对现有工业机器人研发周期长、实时性差、扩展性能低等问题，本文通过IgH主站与ROS融合，结合分布式时钟同步技术，设计了一种实时性强、同步性高和性能稳定的多关节机器人控制系统。

1 控制系统硬件组成

为了控制系统能够实现高效性和快速传输性，采用支持实时工业以太网EtherCAT的驱动器进行通讯，多关节机器人为典型的串联机器人。上位机采用带有Linux系统以及IgH主站的PC机，机器人控制系统采用一主多从的线性网络拓扑结构，通过网线将上位机和伺服驱动器连接。

2 控制系统软件架构设计

控制系统软件架构在带有IgH主站和ROS的Linux系统下实现，软件架构如图1所示。其中ROS主要提供Moveit!功能包集和ros_control框架，Moveit!作为上层通过插件进行运动规划，可以和不同的运动规划库链接；ros_control作为下层提供控制器管理器、控制器、硬件资源接口以及硬件抽象层，实现中间控制环节；IgH主站通过EtherCAT总线与伺服驱动器进行通信。利用多线程通信，将IgH主站和ROS平台融合，实现驱动器编码器数据与多关节机器人关节角度的传输与转换。

图1 控制系统软件框架

3 控制系统实现

控制系统实现内容主要包括底层驱动的实现，ROS平台和IgH主站的融合。

3.1 底层驱动控制实现

基于ROS和IgH主站的多关节机器人控制系统底层驱动需要实现以下功能：

(1)通过EtherCAT通信技术实现伺服驱动器与上位机的通信；

(2)通过分布式时钟(distributed clock,DC)控制各从站任务同步执行；

(3)通过硬件交互层为控制器提供硬件抽象层接口，并完成机器人关节角度到电机目标位置的映射。

3.1.1 EtherCAT通信实现

EtherCAT数据通过以太网数据帧进行传输，数据帧由主站发起控制各驱动器数据的接收和发送，由于以太网设备具有独立处理双向传输的功能[3]，在一个运行周期内，各驱动器可以对主站发送的下行报文进行直接处理，并从报文中读取或写入相关用户数据，然后将报文数据传输给下一个驱动器，当所有驱动器完成数据帧的处理后，由最后一个驱动器从后向前依次发回报文至主站[4]。工业机器人通信系统要求能够在规定时间内完成数据的收发，如各轴的状态反馈信息，要求数据精确且通信延迟抖动低。标准的Linux内核只满足软实时的要求，由于机器人控制系统对实时性要求较高，所以采用带有Xenomai实时补丁的内核，并将IgH用户层周期性执行任务移植到Xenomai实时内核中[5]，通过创建实时任务的线程，设定调度优先级，完成Xenomai周期任务，主站和伺服驱动器通信过程如图2所示。

图2 EtherCAT通信过程

主站和上位机实现通信后，过程数据的读写通过EtherCAT应用程序来实现，PDO通过“数据域指针+地址偏移量”的方式直接读写[6]；再将数据域的所有报文插入到主站的报文序列；最后将所有报文发送到传输序列，从而实现主站和伺服驱动器的数据交互。从站的周期性数据根据CiA402去配置，应用层采用CoE协议[7]。本文驱动器的控制模式为周期性同步位置模式(CSP)。所需配置的PDO以及对象字典如表1所示。

表1 PDO配置

3.1.2 分布式时钟实现

多关节机器人控制系统在实际应用中存在多轴联动的情况，以执行相对复杂的曲线运动，若关节不同步，会产生误差，因此各从站需要使用相同的系统时间，使从站同步执行任务，可以通过分布式时钟产生同步的输出信号，同步主从站之间的时钟，从而实现多轴同步运动。由图3所示，以n个从站节点说明各从站时钟同步过程，包括传输延迟计算、时钟偏移补偿和时钟漂移补偿[8]，其中选择从站1为参考时钟。

下行报文到达每个从站后，从站寄存器会记录到达时刻，假设报文由主站传输至参考时钟从站的时刻记为T1，到达从站n的时刻为T2(n)，则有以下关系：

T2(n)-T1=Tdelay(n)+Toffset(n)

(1)

式中：Tdelay(n)为传输延时；Toffset(n)为初始偏移量。

整理式(1)可得：

Toffset(n)=T2(n)-T1-Tdelay(n)

(2)

上行报文返回主站时，假设到达从站n的时刻为T3(n)，到达参考时钟的时刻为T4，由于线缆传输报文延时均匀，且各从站处理和转发报文时间一致，可得传输延时为

Tdelay={(T4-T1)-[T3(n)-T2(n)]}/2

(3)

根据式(2)和式(3)可得初始偏移Toffset：

Toffset=[T2(n)+T3(n)-(T1+T4)]/2

(4)

主站发送报文读取相关寄存器得到传输延时和时钟偏移的数值后，写入对应从站，由于不同时间读写寄存器，每个从站会与参考从站产生漂移，因此需要对时钟漂移进行补偿，从而实现时钟同步。首先系统的本地时间Tsys_local(n)和参考时间要维持同步关系，可得本地系统时间为

Tsys_local(n)=Tlocal(n)-Toffset(n)

(5)

式中Tlocal(n)为每个从站的本地时间。

其次将参考时钟的系统时间Tsys_ref写入到其他从时钟设备中，然后利用参考时钟系统时间Tsys_ref和传输延时Tdelay(n)，得到时钟漂移补偿Δt：

Δt=Tlocal(n)-Toffset(n)-Tdelay(n)-Tsys_ref

(6)

根据时钟漂移补偿的数值，从站本地时钟需要调整其运行速度，即在每个运行周期通过增加ΔT的值来补偿时间偏差，ΔT的取值方法如下：

(7)

为保证多关节机器人控制系统的同步性，首先控制整个系统和伺服驱动器同时上电，避免通讯部分因非同步上电造成失同步故障。其次，通过初始化时钟完成系统时间的同步，包括传输延时的测量、时钟偏差和时钟漂移的补偿[9]。然后，通过初始化同步信号完成DC从站的同步，设置同步信号的周期时间，同步信号的周期时间和控制系统关节位置插补周期保持一致。最后控制寄存器，设置同步信号的开始时间，并激活同步信号。同步信号激活后，机器人控制系统在中断服务程序中进行插补运算，然后将数据发给从站，从站接收到同步信号后，对数据进行处理，并转发给主站，完成周期性数据交换。

3.1.3 硬件交互层

ros_control定义了机器人硬件接口，控制器通过硬件接口与机器人硬件连接，通过接口层自定义机器人并使用控制器管理器进行控制[10]。

使用硬件交互层与机器人硬件连接首先需要初始化，获取机器人的关节名，并对关节的位置、速度、力矩等信息重新分配空间；然后连接并注册机器人的状态接口和位置接口[11]。完成初始化工作后，对机器人的信息进行读取和写入。通过API读接口，主站可以在从站设备中获取数据帧并处理报文，读取各从站的状态字、当前位置以及当前速度；通过API写接口，主站将控制字、目标位置写入机器人系统，从而控制机器人运动。其中目标位置是由关节角度转化为电机脉冲，再通过中间变量转发给从站实现运动，目标位置与关节角度的转换关系为

(8)

式中：W(n)为各从站的目标位置(n=1,2,3,4,5,6)；θ(n)为每个关节的关节角度(n=1,2,3,4,5,6)；P为电机转一圈指令脉冲数；R为减速比。

上位机读取关节角度的反馈只需反解此式。

完成硬件交互后，在循环中，分别执行读取和发送任务，并周期性地更新控制器的状态，保证实时任务的更迭。

3.2 ROS平台和IgH主站的融合

ROS平台和IgH主站是两个独立的架构，都采用模块化设计[12]。为实现ROS和IgH主站的数据交互，将IgH主站模块封装为ROS下的驱动节点。在相应的节点中，将需要传递的数据保存在自定义消息中，以发布和订阅话题的形式，将所需的机器人关节位置等状态参数传递至主站，在主站的周期性任务中实现过程数据的交换。同时，通过多线程通信，允许多个节点同时存在，不同的功能由不同的线程执行。ROS平台和IgH主站的融合流程如图4所示。

图4 ROS平台和IgH主站融合流程

多线程通信的频率对系统的延迟、实时性以及稳定性具有一定的影响，频率越大，控制系统数据的传输速率越高，但频率过大会造成系统的损坏。为选取合适的多线程通信频率，展开实验，观察不同频率下，系统的运行情况，实验结果如表2所示。

由表2可知，多线程通信频率过高，会引起控制系统的卡死，频率过低，会增大系统的延迟，降低实时性，控制效果不佳。因此，将多线程通信频率控制在1 000～1 250 Hz。

4 实验

本系统运行在ROS平台和IgH主站中，运行环境为Ubuntu14.04+Xenomai2.6.5 +IGH EtherCAT Master，上位机采用一台华硕笔记本，采用一主多从的线性拓扑结构将驱动器和多关节机器人连接，多关节机器人实验平台如图5所示。

图5 控制系统实验平台

4.1 系统实时性测试

在机器人的控制系统中，提高系统的实时性是关键问题，本文通过多线程通信，将系统任务划分为实时域和非实时域。对本文设计的控制系统进行实时性能评估，将系统程序分别运行在Linux内核和Xenomai实时内核下，测量并记录IgH主站运行周期时间，结果如图6所示。由图6(a)可知，在Linux内核下，周期在0.2～6.5 ms波动；由图6(b)可知，在Xenomai实时内核下，周期波动基本稳定在0.04 ms，表明在Xenomai内核下，IgH主站的周期抖动性更小，EtherCAT的通信更加稳定，实时性更强，保证了数据传输的精确性。

通过实时性测试工具Latency，设置实时任务周期为1 ms，分别对系统在用户、内核以及定时器中断模式下的任务调度延迟进行测试，测试结果如表3所示。由表3可知，用户模式下，实时任务调度最大延迟为11.038 μs，占整个测试周期的0.11%；内核模式下，实时任务调度最大延迟为8.672 μs，占整个测试周期的0.08%；定时器中断模式下，实时任务调度最大延迟为5.691 μs，占整个测试周期的0.05%。由测试结果可知，系统任务调度延迟从用户、内核到定时器中断模式依次降低，系统实时性依次增强，说明该系统具有良好的实时性。

(a)Linux内核运行周期

(b)Xcnomai内核运行周期图6 主站周期分布

表3 系统任务调度延迟测试 μs

4.2 关节同步性测试

多关节机器人在执行较复杂的任务时，关节间的同步性影响着机器人轨迹规划的精度：同步性越好，精度越高。为验证关节间的同步性，对各关节设置相同的目标位置，观察机器人从初始位置到目标位置的轨迹变化，根据各关节轨迹变化的一致度，判断关节的同步性能。在实际应用中，机器人不会一直处于无负载状态，因此，进行了无负载状态和有负载状态下关节同步性能的测试。利用基于Qt的应用程序PlotJuggler，记录关节的轨迹变化，直观反映关节间的同步误差。无负载和有负载状态下关节同步性的测试结果如图7和图8所示。

(a)关节原始测量角度

(b)编码器输出数值图7 无负载关节同步性能测试

(a)关节原始测量角度

(b)编码器输出数值图8 有负载关节同步性能测试

由图7(a)可知，无负载状态下，在关节刚接收到控制指令运动后，两关节的输出角度误差在0.001 25 rad左右，短暂时间后，各关节轨迹曲线重合。由图7(b)可知，将关节角度处理成电机的编码器数值输出，电机的编码器数值曲线重合。

由图8(a)可知，有负载状态下，各关节轨迹未重合，关节间的输出角度误差稳定在0.001 rad。将关节角度处理成编码器数值后，由图8(b)可知，由于误差极小，编码器数值曲线重合。由测试结果分析可知，无负载和有负载状态下，关节间均呈现出较高的同步性，满足工业机器人的控制要求。

4.3 轨迹规划误差测试

完成控制系统的实时性和同步性测试后，需要对多关节机器人的轨迹规划误差进行测试，设置机器人各关节角度的期望值，采用改进的快速扩张随机树算法进行运动规划，测试结果如图9所示。

图9 关节轨迹误差

由图9可知，各关节的期望值为[0.2,0.1,-0.1,-0.15],运行结果为[0.199 993,0.099 977 2,-0.100 054,-0.150 023]，误差较小。为验证控制系统的稳定性，设置60个不同的位姿，得出每个关节轨迹误差的平均值，测试结果如表4所示。

表4 关节轨迹误差测试

由表4可知，关节1到关节4的平均轨迹误差值分别为1.2×10-4rad、1.7×10-4rad、3.0×10-5rad和2.0×10-5rad，轨迹规划精度较高，满足控制要求。

5 结论

本文针对现有工业机器人控制系统存在的实时性差、同步性低以及轨迹规划精度低等问题，设计了基于ROS和IgH主站的多关节机器人控制系统，采用多线程通信，将IgH主站、Moveit!以及ros_control融合，选取合适的任务控制周期，在IgH主站下采用分布式时钟，使该机器人得到有效控制。通过对系统的实时性、同步性以及轨迹规划误差进行测试：该机器人控制系统运行周期波动稳定在0.04 ms；关节间的输出角度误差稳定在0.001 rad；关节1到关节4的平均轨迹误差值为1.2×10-4rad、1.7×10-4rad、3.0×10-5rad和2.0×10-5rad。表明该多关节机器人控制系统具有良好的实时性、同步性以及可靠性。