基于EtherCAT总线的人形机器人控制系统设计

余秋蕾，张崇峰，陈 萌，韩亮亮(.华中科技大学机械科学与工程学院，武汉430074；.上海宇航系统工程研究所，上海008)



基于EtherCAT总线的人形机器人控制系统设计

余秋蕾1，张崇峰2，陈 萌2，韩亮亮2
(1.华中科技大学机械科学与工程学院，武汉430074；2.上海宇航系统工程研究所，上海201108)

摘要:针对传统人形机器人控制系统存在的软硬件模块化程度低、通讯非实时、控制功能有限的问题，在分析空间站对人形机器人控制系统功能需求的基础上，以四核嵌入式控制器为操作平台，搭建了基于EtherCAT总线的人形机器人分布式实验控制系统，实现了机器人任务规划与决策、数据管理和系统通信等功能，具有灵活高效的网络拓扑结构、模块化的软件架构。实验表明，该系统保证了信号采集的实时性和可靠性，控制的稳定性和高精度。

关键词:人形机器人；EtherCAT总线；分布式控制系统；四核控制器

1　引言

我国载人航天工程任务向更高阶段发展，对空间操控能力的提高也提出进一步需求。利用空间机器人协助航天员完成载荷及设备更换、平台巡检与养护以及出舱协同作业等舱内外任务将成为未来机器人应用的重要方向。人形机器人作为具备人类的外形特征和行动能力的智能机器人[1]，在与航天员进行交互过程中，它类人的外形能够使航天员在交互环境中更舒适。人形机器人系统属于非线性、高耦合系统，在协助航天员完成复杂的舱内外任务时，动力学效应多变，机器人关节脆弱、不稳定，运动过程中存在协调控制等问题[2]，进行机器人控制系统设计时，不仅要满足控制系统的普适性能，提供灵活、方便的操作方式，以减轻航天员的操作负担，对控制系统响应的快速性、实时性和操作的精度、安全可靠性也提出更高要求。

人形机器人的研究起步于1960年代后期[3]，美国是世界上研究机器人航天员最早的国家，目前已经积累了丰富的技术能力和应用经验，2011年其研发的机器人航天员R2已进入太空站，主要进行一系列微重力条件下的功能测试工作[3]。日本研发的首个能够进行人机交流的机器人航天员Kirobo于2013年11月开始在太空进行人机对话试验[3]。俄罗斯研发的人形机器人航天员SAR-401能够执行比R2更复杂的精细操作，它不是完全自动的，而是由工作人员在地面远程遥控执行任务[4-6]。它们均采用分布式控制系统，结合多种交互设备进行控制。

我国在仿人机器人方面也做了大量研究，其中部分是进行双足仿人机器人的研究，但实现的动作和功能十分有限，自主性不足，软硬件模块化程度低，或者系统各层次之间采用非实时通讯总线、串口方式进行通信，系统达不到较高的实时性和可靠性要求[7-8]，且应用于空间操作的人形机器人系统研究较少。

为此，本文以四核嵌入式控制器为运行平台，搭建基于EtherCAT总线的空间人形机器人分布式实验控制系统，以降低底层硬件驱动程序的开发难度，为软件总体框架的设计提供良好的通信、控制思路，控制算法采用模块化设计以简化重用代码的编写，节约系统资源。通过TwinCAT/ C ++编程结合Windows非实时操作系统以实现系统的实时控制，缩短系统的控制周期。

2　控制系统总体设计

2. 1 系统功能设计

考虑空间工作环境的特殊性，控制系统的具体功能分析如下:

1)灵巧、高效的作业任务能力。人形机器人系统应具备开展通用化、高精度和高灵活性任务的能力，其工作空间或可达范围与航天员相当，能够辅助航天员完成或者自主完成舱内周期性的活动。

2)多样化人机交互方式。航天员可通过多种人机交互方式与人形机器人进行自然、直接的交互。

3)实时稳定的通信方案。系统能够提供高效实时的通信方式，且易于进行网络结构拓扑。

4)安全可靠的控制器。为应对恶劣的空间环境，机器人的控制器应该保证具有较高的可靠性和安全性，提供多种控制模式接口。

根据上述功能要求进行设计，人形机器人控制系统结构如图1所示。系统采用基于Ether-CAT总线的分布式控制方式，以四核嵌入式PC为控制器操作平台，实时性高，易于拓展应用，各模块之间通过接口进行连接实现主从关系或平行关系。冗余自由度的双臂保证机器人有足够的操作空间，左臂末端安装的可变工具集完成规则对象的操作，右臂末端的五指灵巧手完成螺钉旋拧、电连接器拆装等任务，腰颈关节进一步扩大机器人的工作范围。

图1　控制系统框图Fig. 1 Block diagram of the control system

从系统框图可以看出，中央控制器是人形机器人控制系统的核心模块，本文将主要针对中央控制器模块的软件设计进行分析与验证。

2. 2 系统组成

基于EtherCAT总线的人形机器人系统是由遥操作子系统、中央控制子系统和操作环境等组成的天地半自主服务体系。其中，中央控制子系统又包括中央控制器和驱动执行机构。系统结构如图2所示。

遥操作子系统的基本构成为上位机和多种人机交互设备，其中，上位机包括生成指令的遥操作计算机与视频图像处理的视觉计算机；中央控制子系统控制器采用倍福公司提供的四核控制器，该控制器基于Intel CoreTMi7处理器，主频2. 1 GHZ。作为中央控制子系统的核心组成，它主要完成指令解析与高速率的数据处理，并将处理后的指令数据定时发送至下位机；驱动执行机构是由两个7自由度手臂、2自由度腰部、3自由度颈部以及装有双目视觉相机和深度相机的机器人构成。

图2　控制系统组成Fig. 2 Composition of the control system

3　软件设计

中央控制器作为控制系统的核心，对系统的实施起决定性作用，也是系统优化的主要功能模块，需要考虑与上位机、下位机的通信，机器人各模块的任务规划及分配、协调控制以及模块间数据交换的安全策略等问题。

在前人研究的基础上，本文采用一种新的机器人控制器平台-倍福四核嵌入式计算机，结合一种新的软件编写平台-TwinCAT3实现多个模块的协调任务操作，结合高速实时现场总线降低底层驱动软件开发难度，提高网络拓扑灵活性。

中央控制器软件总体框架如图3所示。由控制器中单独一个核作为指令接收与解析单位，便于集中控制；为保证双臂能够进行协同作业，分别在其各自核中进行任务规划与设计。头腰协同要求较低、控制算法简单，共用一个核进行控制。在TwinCAT3平台下，各核分别执行三个任务，每个任务固定一个时间片，即控制周期运行，核之间通过共享内存进行数据交换。

图3　软件总体设计框架Fig. 3 General design frame of the software

从指令发送-机器人任务执行-关节数据反馈全过程进行分析，对应软件设计框图中的顺序，中央控制器软件模块主要实现六个功能:

1)通过与上位机进行通讯接收控制指令，并进行数据帧的解析；

2)根据解析的数据进行任务分解和模式控制，即确定采用哪种模式进行控制以及采用哪种路径规划算法；

3)为实现双臂、双足、头颈部、腰部单个模块的控制和多个模块协调控制任务，系统需要进行单腿/单臂路径规划算法、双腿/双臂运动协调算法、力柔顺控制算法、头腰协调控制算法等的算法设计；

4)通过现场总线将步骤三中解算出的控制指令分别发送到对应的关节控制器中；

5)获取双臂、双足、头颈部、腰部各关节的传感器信息，包括位置、电流、模拟量等信息，并进行信息的管理，返回给中央控制器；

6)中央控制器通过与上位机通讯，将组帧后的反馈信息发送至遥操作端。

从以上分析可以看出，上位机与中央控制器、中央控制器与下位机的通讯是指令数据接收、解析、发送以及反馈的前提条件，任务经过分解及模式识别后，各模块的控制算法是下发控制指令的核心。因此，通讯模块及控制算法的设计即为软件设计的重点。

3. 1 通信拓扑结构

人形机器人控制系统的通信拓扑结构如图4所示。

图4　控制系统通信拓扑图Fig. 4 Communication topology of the control system

本文主要采用ADS协议和EtherCAT总线进行通信。上位机和中央控制器之间采用基于TCP/ IP的ADS协议进行非实时通讯，该协议对Windows Socket类进行了再封装，支持定时、通知、异步等多种传输模式；下位机与中央控制器端、中央控制器与视觉控制器之间采用EtherCAT总线进行实时通讯。EtherCAT具有高速和高效率的传输特性，支持多种设备连接拓扑结构，正因如此，本文的控制系统采用分布式设计的方法，形成星形拓扑结构。该总线从站节点使用专门的控制芯片，主站使用标准的以太网控制器。Ether-CAT刷新周期短，可以达到小于100 μs的数据刷新周期，并且各从站设备可以达到小于1 μs的时钟同步精度[9-11]。

在TwinCAT3软件平台下，支持EtherCAT总线的设备可直接通过控制器进行扫描，完成设备的添加、删除操作，进行网络拓扑结构的更改；通过修改EtherCAT总线协议的对象字典进行与底层设备的网络连接和对底层设备驱动控制。

3. 2 控制算法设计

为了保证精度和稳定性，在进行任务操作过程中，本文结合路径规划、柔顺控制、视觉伺服等多种算法实现机器人多臂化操作，在TwinCAT平台下，对控制算法进行封装操作，生成可模块化调用的静态库。控制算法的框图如图5所示。

对于两自由度腰部、三自由度颈部，若想到达笛卡尔空间内某一位置，仅存在唯一一组解，对于七自由度冗余左右臂，则存在无数组解使得机械臂到达笛卡尔空间内指定位置。在执行任务中，主要执行机构为左右臂，辅助机构为腰部和颈部，手臂七自由度反解是本文控制算法设计的基础。

机器人在执行任务的过程中，会与操作环境发生接触，左右手臂之间可能会产生接触，较小的位置偏差可能产生较大的接触力，若不加以控制，会对机器人和末端接触物体造成损坏，甚至可能会导致系统崩溃[12]，力控制在人形机器人控制系统中也是一项非常重要的任务。

因此，本文主要进行七自由度反解和力控制算法的研究。

图5　控制算法框图Fig. 5 Block diagram of the control algorithm

3. 2. 1 七自由度反解

目前，进行七自由度逆运动学求解的方法主要有解析法和迭代法两种，当各关节无限位时，两种方法求解效果差别不大，但解析法能够更快的排除无效解，缩短计算时间，减小空间复杂度[13]。

本文采用基于臂形角的参数化方法进行各关节角的求解，依次求出肘部、腕部和肩部关节的角度。软件控制流程如图6所示。

臂形角可以确定机械臂的基本构型，实际求解过程中，臂形角是一定范围内的变量，既保证了左右臂在关节限位条件下有更多可行较优解，又避免了由于限位导致的逆解突变，保证了逆解的连续性。

3. 2. 2 力控制算法

关节力矩控制模式复杂，须各关节扭矩传感器采集数据作为检测力/力矩是否超限的判据，保证机械臂处于安全工作状态，主要采用末端的六维力传感器进行阻抗控制。阻抗控制的特点是不直接控制机器人与环境的作用力，而是通过控制力和位置之间的动态关系来实现柔顺功能[14]。首先进行重力补偿，补偿后若末端力/力矩超过设定的阈值时，则进行力/力矩控制，然后将力/力矩转换为末端位姿修正值，进行位置闭环控制。阻抗控制的动态关系方程[14]表示为式(1):

式中: Md为期望的惯性质量，Bd为期望的阻尼系数，Kd为期望的刚度系数，T为末端各方向力/力矩，xr为实际的末端各方向位置/姿态，xd为期望的末端各方向位置/姿态。

基于阻抗控制的人形机器人控制器主要有两种工作模式:手把手示教工作模式与过程中柔顺控制模式。两种工作模式的软件控制流程图如图7(a)、(b)所示。

图6　七自由度逆解流程图Fig. 6 Flow chart of the seven DOF inverse

图7　力控制流程图Fig. 7 Flow chart of the force control

手把手示教模式即指机器人在一个给定位置进入力控制模式，操作人员直接引领机器人手臂末端，中央控制器采集末端六维力传感器信息，将操作者所施加力/力矩转化为相应的位置/姿态大小发送给底层驱动器，从而驱动各关节电机运动，整个手臂沿操作人员操作方向运动。该种模式能够实现手动对准任务，不需要借助三维模型的测量即可得出理想操作位置的笛卡尔空间表示，经过一定训练后，并结合滤波算法，能够完成路径复现功能。

过程中柔顺控制模式即指在机器人任务操作过程中，控制器实时地进行六维力信息的检测，同时进行位置修正，自动调整操作过程中力/力矩大小，防止其超过阈值对机器人甚至系统造成损伤，当检测到的力/力矩数据超过一定值时，机器人自动执行紧急停止操作。在这种控制模式下，机器人能够自动适应环境，避开任务过程中出现的障碍物，若此时障碍物对其产生的作用力过大，机器人启动自动报警功能，提醒操作人员关闭电源，并完成自身紧急停止任务。

4　实验平台搭建与结果分析

4. 1 实验平台搭建

基于本文前三节的设计思想，进行实验平台的搭建以及软件实现，如图8所示。图8(a)所示为地面验证实验平台，由人机交互设备、上位机、控制计算机、下位机、电源模块及模拟操作面板等组成。图8(b)为人机交互界面，左中右依次为指令发送、数据反馈、虚拟仿真界面。图8(c)为中央控制器设置各核任务执行周期及优先级的界面。

4. 2 结果分析

底层设备扫描周期为T =1 ms，在图8(c)中的TwinCAT3平台下设置控制周期C为T的整数倍，当C =2 ms时，机器人运动稳定，当C＜2 ms时，数据反馈链路阻塞，控制出现中断，设置控制系统的周期为2 ms。在控制稳定的前提下，验证运动学及动力学算法的有效性，以插孔操作为例，结合路径规划和阻抗控制算法完成圆孔的柔顺插拔，其中，操作圆孔固定在图8(a)所示机器人正前方黑色操作面板上。圆孔的直径为4 cm，为保证效果明显，在实验过程中给定末端y方向1 cm的位置偏差，即目标点与圆孔中心在水平方向有1 cm的偏差，图9所示为末端六维力传感器在柔顺插拔过程中采集的各方向的力、力矩大小和末端y方向的位置，图9(d)为规划的末端x、z方向位置与运动过程中采集的末端x、z实际位置之间的差值，即末端跟踪误差。由图9(a)～(c)可知，当末端力/力矩超过阈值时，阻抗控制可以有效修正偏差，使末端所受力/力矩保持在有效范围内，并且使末端到达规划目标位置，由图9(d)可知，末端位置跟踪误差控制在1. 5 mm以内，一定周期后误差逐渐减少至接近为零，控制器实现了对机械臂的实时控制。

图8　实验平台及软件界面Fig. 8 Experimental platform and software interface

5　结论

在分析空间站对人形机器人控制系统功能需求的基础上，本文完成了基于EtherCAT总线的人形机器人控制系统设计，进行了实验分析与验证，得出以下结论:

1)EtherCAT总线通信的方式将系统总的控制周期缩短至2 ms，保证信号采集的实时性和控制的稳定性。

2)在柔顺控制模式下，基于位置的阻抗控制算法能够将机械臂与操作对象的接触力修正到一定范围内，保证控制的高精度，且避免了运动过程中较大接触力对机械臂产生的损坏。同时，通过调整阻抗控制参数，实现了机器人手把手示教功能，节省了模型测量时间。

图9　实验采集数据信息Fig. 9 The experimental data information

参考文献(References)

[ 1 ] 虞汉中，冯雪梅.人形机器人技术的发展与现状[J].机械工程师，2010(7): 3-6. Yu H Z，Feng M X. Development and actuality of humanoid robot technology[J]. Mechanical Engineer，2010(7): 3-6. (in Chinese)

[ 2 ] 陈光伟.仿人机器人控制系统设计与稳定性研究[D].成都:西华大学，2010. Chen G Y. Study on Control System Design and Stability of Humanoid Robot[D]. Chengdu: Northwest university，2010. (in Chinese)

[ 3 ] 刘宏，蒋再男，刘业超.空间机械臂技术发展综述[J].载人航天，2015，21(5): 435-443. Liu H，Jiang Z N，Liu Y C. Review of space manipulator technology[J]. Manned Spaceflight，2015，21(5): 435-443 (in Chinese).

[ 4 ] Tsay T I J，Lai C H. Design and control of a humanoid robot [C] / / Intelligent Robots and Systems，2006 IEEE/ RSJ International Conference on. IEEE，2006: 2002-2007.

[ 5 ] Taira T，Kamata N，Yamasaki N. Design and implementation of reconfigurable modular humanoid robot architecture[C]/ / Intelligent Robots and Systems，2005. (IROS 2005). 2005 IEEE/ RSJ International Conference on. IEEE，2005: 3566-3571.

[ 6 ] Qi J，Wang L，Jia H，et al. Design and performance evaluation of networked data acquisition systems based on EtherCAT [C] / / Information Management and Engineering (ICIME)，2010 The 2nd IEEE International Conference on. IEEE，2010: 467-469.

[ 7 ] 郑嫦娥，钱桦.仿人机器人国内外研究动态[J].机床与液压，2006(3): 1-4. Zheng C E，Qian H. The domestic and international research situation of humanoid robot[J]. Machine tool&Hydraulics，2006(3):1-4. (in Chinese)

[ 8 ] 刘莉，汪劲松，陈恳，等. THBIP-1拟人机器人研究进展[J].机器人，2002，24(3): 262-267. Liu L，Wang J S，Chen K，et al. The research on the biped humanoid robot THBIP-1 [J]. Robot，2002，24 (3):262-267. (in Chinese)

[ 9 ] 郇极，刘艳强.工业以太网现场总线EtherCAT驱动程序设计及应用[M].北京:北京航空航天大学出版社，2012: 3-4. Xun J. Liu Y Q. EtherCAT-Industrial Ethernet[M]. Beijing: Beijing University of Aeronautics and Astronautics Press，2012:2-4. (in Chinese)

[10] 蔡林海，蒋晓春，赵国亮，等.基于EtherCAT总线技术的链式STATCOM控制系统[J].电力建设，2012，33(8): 27-30. Cai L H，Jiang X C，Zhao G L，et al. Chain STATCOM control system based on EtherCAT bus technology[J]. Electric Power Construction，2012，33(8):27-30. (in Chinese)

[11] 张桢，赵君，刘卫华，等. EtherCAT总线分布式多电机控制研究[J].电子科技，2014，27(6):106-112. Zhang Z，Zhao J，Liu W H，et al. Research on distributed control system of multi-motor based on EtherCAT bus[J]. E-lectronic Technology，2014，27(6):106-112. (in Chinese)

[12] 徐文福，周瑞兴，孟得山.空间机器人在轨更换ORU的力/位混合控制方法[J].宇航学报，2013，34 (10): 1353-1361. Xu W F，Zhou R X，Meng D S. A hybrid Force/ Position Control Method of Space Robot Performing On-Orbit ORU Replacement[ J]. Acta Astronautica，2013，34 (10): 1353-1361. (in Chinese)

[13] Zhang Z，Xu D，Yu J. Research and latest development of ping-pong robot player[C] / / Intelligent Control and Automation，2008. WCICA 2008. 7th World Congress on. IEEE，2008: 4881-4886.

[14] JUNG S，HSIA T C，BONITZ R G. Force tracking impedance control of robot manipulators under unknown environment[J]. IEEE Transaction Control Systems Technology，2004，12 (3): 474-483.

Control System Design for Humanoid Robot Based on EtherCAT Bus Technology

YU Qiulei1，ZHANG Chongfeng2，CHEN Meng2，HAN Liangliang2
(1. School of MechanicalScience& Engineering of HUST，Wuhan 430074，China；2. Shanghai Aerospace System Engineering Research Institute，Shanghai 201108，China)

Abstract:The control system of the traditional humanoid robot has many problems such as low degree of modularity in software and hardware，non-real-time communication and limited control functions. On the basis of analyzing the function requirements for humanoid robot system in the space station，a distributed control system based on EtherCAT bus for humanoid robot was put forward. It adopted the quad-core embedded controller as its operating platform and achieved the following functions: robot mission planning and decision-making，data management and system communication. The experiment results showed that the system could not only guarantee the real-time and reliability of the signal acquisition process，but also ensure the stability and high precision of the control system.

Key words:humanoid robot；EtherCAT bus；distributed control system；quad-core controller

作者简介:余秋蕾(1991 - )，女，硕士，助理研究员，研究方向为机器人控制。E-mail:yuqiulei@ hust. edu. cn

基金项目:载人航天预先研究项目(050101)

收稿日期:2015-09-26；修回日期:2015-12-16

中图分类号:TP39

文献标识码:A

文章编号:1674-5825(2016)01-0016-07