月面仿人机器人实时控制系统设计

姜朝峰，莫小波，朱秋国，3，吴 俊，3，熊 蓉，3

(1.浙江大学智能系统与控制研究所，杭州310027；2.杭州云深处科技有限公司，杭州310027；3.浙江大学工业控制技术国家重点实验室，杭州310027)

1 引言

月球的表面覆盖着环山、月海、月陆和山脉等[1]，所以月面移动机器人必须有较强的月面地形适应能力和避障越障能力。月面移动机器人主要有轮式机器人[2-4]、腿足式机器人[5-7]、模块化可重构机器人[8]，其中适应地形能力最强的就是腿足式机器人。仿人机器人为腿足式机器人的最典型代表，在复杂环境中的通行和作业具有较大优势，可以在月面复杂地形稳定移动并执行科研任务。仿人机器人可以代替航天员执行有危险性的载人登月任务，如太空行走和出舱维修；可以替代人类在月球基地进行大规模适应性测试，检验人造环境是否适合人类生存与活动。各种为人类准备的设备和工具可以由仿人机器人直接操作使用，不必重复设计。仿人机器人在月球运动的关节受力、姿态数据可以为低重力环境对人类健康可能造成的伤害评估提供支撑，并做出预防措施。这些都是其他智能设备或机器人无法取代的。

目前，月面仿人机器人的研究与开发较少。Baker等[9]研制的R2机器人基于机器人操作系统ROS研发，为航天员提供修理辅助，双足主要起固定作用；Radford等[10]研制的R5仿人机器人也基于ROS，使用了4块高性能主控板保证并发控制计算的实时性，成本和空间要求较高；Kuehn等[11]研制的黑猩猩月球探测机器人的通信和控制频率最高为 100 Hz，通信总带宽为50 kbps，无法满足实时控制的性能要求。

仿人机器人基于腿足交替运动而移动，虽然具有优良的地形适应能力，但其运动过程是一个复杂非稳态过程，微小的控制偏差可能导致整个机器人运动失衡而摔倒。针对外太空复杂崎岖地面，仿人机器人必须具备快速检测自身和环境状态，以及快速做出反应的能力。因此，高速率、高带宽和高稳定的实时性是月面仿人机器人必备的条件。

本文将开展月面仿人机器人控制系统的实时性能分析和实时系统设计，构建QNX实时操作系统和以太网控制自动化技术(Ethernet for Control Automation Technology，EtherCAT)的实时仿人机器人控制系统。通过对线程运算耗时进行试验，建立实时调度策略，优化线程调度，从底层到应用层实现整体实时性。

2 仿人机器人控制系统

WUKONG-II仿人机器人是针对复杂环境下的快速运动而研发的双足机器人，见图1[12]。机器人身高1.5 m，重42 kg，安装有足底力传感器和IMU传感器，腿部总共10个自由度，包括左右髋俯仰、髋滚动和髋偏转3个自由度、膝俯仰关节1自由度、脚踝俯仰关节1自由度。

仿人机器人运动控制系统的响应速度具体表现为仿人机器人运动控制系统的控制周期。控制周期越长，运动控制指令时间间隔越长，系统纯滞后越大，对于外界扰动的反应也就越慢；控制周期越短，运动控制指令时间间隔越短，系统纯滞后越小，机器人能够对外界扰动更快做出响应[13]。

针对仿人机器人WUKONG-II驱动器数量多、对通信速率和准确性要求高的特点，为机器人的运动控制主控机选择运行QNX操作系统，通信接口选择EtherCAT工业以太网协议栈。基于QNX操作系统和EtherCAT实时以太网，主控机网口与支持EtherCAT协议的电机驱动器相连，通过运行过程中的数据交互实现对关节电机的实时控制。WUKONG-II的控制系统硬件结构框图见图2。

机器人上还配置了无线网卡，可以通过WiFi信号与上位机进行无线通讯，获取上位机的控制指令。机器人电源模块为控制器及驱动器供电，并配置有急停开关控制驱动器的急停信号。

为了评估机器人控制系统的性能，接下来将从操作系统、通信总线、控制框架3个方面进行试验测试与分析。

3 QNX操作系统与实时性

实时是指信号的输入、运算和输出都要在极短的时间内完成，并根据过程的变化及时地进行处理[14]。实时性和快速性并不等价，不论处理器的运算处理和网络数据传输速度有多快，只要在规定的响应时间内发生响应动作并进行处理，则称系统具有实时性。

图2 WUKONG-II控制硬件结构框图Fig.2 Block diagram of control hardware structure in WUKONG-II

QNX是一个多任务、多用户、分布、可嵌入、符合POSIX标准的嵌入式实时操作系统，由微内核和若干负责系统管理的共操作进程组成，各个管理器与内核之间互相隔离，具有出色的容错性[15]。为验证QNX操作系统是否满足仿人机器人对实时性能的要求，将根据以下指标对QNX系统进行测试评估：

1)上下文切换时间。上下文就是进程执行时的外部变量、环境和数据，包括所有的寄存器变量、进程打开的文件、内存信息等。上下文切换就是保存一个进程任务的上下文状态并运行或恢复另一个进程的上下文，以便该程序执行。

2)中断延迟时间。通常中断的优先级高于普通任务，所以当有中断产生时，正在执行的任务上下文会被保存，转而由中断函数执行，中断延迟时间就是指中断产生到中断函数第一条指令开始运行的时间差。如果中断事件不够及时，可能产生严重后果，所以中断延迟时间是操作系统实时性的一个重要指标。

3)时钟精度。实时时钟为操作系统提供了计时标准，它的作用为提供系统时间、为任务时间片提供基准、更新资源消耗和处理器时间统计值等，时钟的误差会被操作系统累积而放大。

试验测试使用的主控机型号为Kontron KEEX-5106工控机，CPU为Intel Broadwell Core i7-5650U，内存为8G DDR3L。对机器人控制主控机安装不同的操作系统，分别对上下文切换时间、中断延迟时间等指标进行多次测试，并获取平均值。

3.1 上下文切换时间试验

试验目的是测试上下文切换这一过程的最长时间开销。试验方法是创建高、低不同优先级的2个实时进程TaskHigh和TaskLow，并创建一个定时器。先让高优先级进程TaskHigh试图获取低优先级进程TaskLow的消息队列而挂起等待；然后 TaskLow开始运行，并通过消息队列向TaskHigh发送消息，让系统计时器开始计时并得到唤醒高优先级任务的起始时间点t1，然后后延时等待；TaskHigh会收到消息，并由等待状态进入就绪状态，由CPU调度到内核进行运行，在等到消息的代码之后执行定时器的另一次计时输出，记录此时的时间点t2。两次计时时间之差即为上下文切换时间，t(switch)=t2-t1。重复10次，取中位数。测试结果见表1。

表1 操作系统上下文切换时间试验结果Table 1 Test results of context switching time

结果表明，QNX的上下文切换时间开销非常小，可以满足仿人机器人系统的实时切换要求。这一优点得益于其微内核、高优先级任务抢占式的特性。相比之下，Linux Ubuntu 16.04切换时间较长，而Windows 10上下文切换时间更是QNX的十几倍。

3.2 中断响应时间试验

试验目的是测试系统调度延迟。操作系统对一个中断的响应，其实包括了产生中断、调度器进行调度、实现上下文切换3个过程，所以中断响应时间=中断延迟+调度器调度延迟+上下文切换延迟。具体试验方法和上下文切换时间测试类似，即先让一个任务执行，并创建一个定时器，初值设为t1，当定时器减为0时，进入中断服务程序，同时定时器会自动变为初值t1；当切换到中断函数执行时，让中断函数立即调用时间函数记录定时器时间t2，则t(interrupt)=t1-t2。重复10次，取中位数。测试结果见表2。

表2 操作系统中断响应时间试验结果Table 2 Test results of interrupt response time

试验结果表明，相比起 Linux和 Windows，QNX的中断响应时间明显小很多，仅有11.30μs，几乎只是 Windows 10 的 1/3，Linux Ubuntu 16.04的2/3。这足以证明QNX操作系统的小巧和快速。

3.3 时钟精度

实时时钟为周期控制提供了基本依据，准确的时间周期对腿足机器人控制实时性非常重要。Windows、Linux和QNX均提供高精度系统时钟，所以在相同的Core i7 CPU上，时钟精度最高均能到达相同的纳秒级，但系统获取时间的接口会带来定时器误差。试验目的是测试系统时钟误差，试验原理是每隔1 ms，通过IO操作程序向主板的GPIO输出一次方波，用示波器观察波形的实际时间间隔，和1 ms的设定定时周期进行做差比较，重复10次，取中位数。测试结果见表3。

表3 操作系统时钟精度试验结果Table 3 Test results of clock time accuracy

上述对上下文切换时间、中断响应时间和时钟精度的测试结果表明，作为最广泛使用的桌面用户操作系统和服务计算系统，Windows和Linux为了更广泛的用户交互友好和兼容性、提高CPU有效利用率，其内核任务调度算法更倾向于公平的循环调度和非实时抢占，而牺牲了实时与快速响应性能。QNX操作系统则具有良好的实时性能，在不牺牲CPU密集型任务的情况下，保证了IO密集型任务的反应能力，能良好地保证仿人机器人控制系统的底层快速响应，可以作为月面仿人机器人控制的实时性操作系统。

4 EtherCAT以太网总线与实时性

EtherCAT是德国Beckhoff公司最早提出的开放式实时以太网，其高速性能确定了实时以太网性能的新标准[16]。EtherCAT的主站为标准的以太网卡，从站为专用芯片，采用主从介质访问模式，通信过程中由主站控制所有从站的收发，它可在30μs内通过双绞线或光纤电缆处理1000个数字量 I/O，全网可连接设备多达 65 535台。EtherCAT技术使得总线系统不再是控制理念的瓶颈，通讯技术开始可以和高性能的工业计算机相匹配[17]。

由于其他以太网总线在每个连接点接收以太网数据包，然后解码并复制到过程数据存储区，而EtherCAT报文在设备的持续传送过程中，就由每个I/O端子中的现场总线存储管理单元FMMU读出报文数据。同样，输入数据可以在报文通过时插入报文中，报文仅有几纳秒延迟。通过专门的芯片硬件完成收发。因此，EtherCAT突破了其它以太网解决方案的系统收发数据高延迟问题，实现了其它的以太网解决方案不能达到的实时能力。因此，可以让仿人机器人各电机驱动器、传感器和控制器之间的控制信号传输速度产生巨大的提升。

连接好11个电机驱动器，通过EtherCAT以太网周期地获取API读取电机控制通信周期，每隔5 ms读取一次并记录数据。测试EtherCAT总线通信周期时间和每个周期的抖动。每个周期读取和写入的数据内容如表4所示。

表4 输入/输出数据参数和长度Table 4 Input/output data parameters and length

每个电机需要0xE0 bit的数据传输量，11个电机在250μs的周期内共传输0xE0×11=154 bit，则通信带宽为154 bit÷0.000 25 s=616 000 bit/s≈0.62 Mbps。对于EtherCAT标称的百兆甚至千兆带宽，即使再加上帧头、帧尾、标志符、控制帧等其他帧封装数据，也完全满足通信需求。

设定EtherCAT通信周期为250μs，每隔5 ms(即20个通信周期)利用EtherCAT协议栈提供的API接口读取一次数据，获取平均通信周期、最大通信周期、最小通信周期和平均抖动并写入记录，数据见图3。

图3 Ether CAT通信周期图Fig.3 Diagram of communication cycle of Ether CAT

根据试验结果可知，平均通信周期一直非常稳定地保持在249μs，这是机器人系统通信实时性和稳定性的保证。但同时每20个周期内，最小和最大通信周期都和平均值发生了一定的偏离。这在通信网络中无法避免，-25～+35μs的抖动在控制系统可接受范围之内。

5 控制系统程序框架

基于QNX实时操作系统和EtherCAT工业以太网，为仿人机器人WUKONG-II搭建了控制系统程序框架。为了提高数据分发速度，减少进程间通信带来的数据传输延迟，该程序框架在一个进程中实现，并在进程中创建了其他6个线程，并行实现电机控制、外设监听、IMU和足底力传感器数据读取、过程数据记录、手臂舵机控制和外部通信等仿人机器人需要的功能。

5.1 线程功能模块

线程1是主进程本身所在的线程。负责创建并启动其他功能线程，并在程序结束时注销其他线程，把记录数据存入硬盘，释放内存，结束进程。

线程2是Motor(电机)模块。通过EtherCAT的数据镜像，将电机驱动器这一周期的目标控制参数写入，并将当前周期的实际状态参数读取，通过姿态控制算法计算得到下一周期的目标数据，等待下一周期再次写入数据。

线程3是Sensor(传感器)模块。通过2个串口分别读取力传感器和IMU的上传数据。力传感器和IMU都采用主动上传协议模式，即脚底力传感器模块和IMU模块每采集一个完整的新数据包，就主动上传到主控板。

线程4是Listener(监听)模块。循环查询键盘或手柄的按键指令中断，当收到外部设备的远程按键指令就修改机器人状态，使机器人从一个状态切换到另一个状态，实现不同的运动模式。

线程5是Recorder(记录)模块。每个控制周期都将电机实时位置、速度、力矩、控制参数、状态值等重要数据放到内存空间，等待主线程结束后把内存中的记录写入硬盘，为测试后的分析保留数据。

线程6是Arm(手臂控制)模块。根据双足姿态调整手臂运动状态，并将生成的手臂运动姿态位置值通过Ethernet网口发送到舵机，实现手和脚的协同动作。

线程7是 Head(感知与通信)模块。通过Ethernet网口和 UDP/IP协议，获取“WUKONGII”感知系统的反馈信息，针对不同的感知、路径规划结果，做出不同的运动策略。

5.2 传感器数据采集模块

为了实现仿人机器人的可靠稳定行走，需要IMU传感器和足底力传感器提供姿态和状态信息。WUKONG-II在腹部位置安装了IMU模块，设定数据采集周期为5 ms，在脚底安装了4个足底力传感器模块，设定数据采集周期为5 ms。在各自的数据采集线程中一直轮询式读取数据，每完成一次数据包读取，记录时间间隔，数据记录见图4。

图4 数据采集周期-运行时间图Fig.4 Diagram of data collection cycle vs.runtime

由图4可知，数据采集模块实际周期时间的方差较大，伴随着随机误差。IMU模块和脚底力传感器模块的实际读取周期都在5 ms左右，符合设定值。其中，IMU的读取周期方差小，脚底力传感器的读取周期方差更大。

5.3 控制算法模块

仿人机器人的双足步行模式使得机器人易受到各类模型误差与环境扰动的影响，导致机器人失衡而摔倒，较短的控制周期使仿人机器人能够根据传感器信息针对各类扰动及时进行反馈补偿与矫正，保持机器人稳定性[13]。

系统控制周期是指整个程序完成算法计算、电机控制、数据采集、数据记录等所有任务所需的时间，控制算法周期是指其中控制算法的运行时间。控制算法周期的波动很大且没有规律，这是由于机器人在不同的状态下，选择的算法模式不同，计算量不同，因此算法运行时间一直在改变。而且机器人控制算法要时刻根据传感数据反馈进入不同的模式，因此难以预测下一时刻的系统状态。为了给算法的运行留出足够的CPU计算时间，系统周期必须大于其中最大的控制算法周期。测试时间内，算法运行时间集中在250μs以内，所以给系统周期设定为1000μs可以满足算法的时间要求。分别使用系统定时器函数记录控制系统和控制算法计算的始、终时间，并做差，得到算法周期和系统周期，见图5。

图5 系统周期和算法周期图Fig.5 Diagram of system cycle and algorithm cycle

根据3.2可知，当计时器计时满1000μs产生系统中断后，中断响应仍然需要一定时间。因此，实际周期会略大于设定周期值。由图5可知，设定系统周期为1000μs，大多数实际周期误差都在3μs以内。但会出现1250μs的异常值，这是由于获取系统控制周期时间的函数精度为250μs、细粒度不够导致。

最终，在WUKONG-II上开展了双足跑步试验，并实现了双足机器人室外操场和平整路面的快速小跑，最快速度达到3.24 km/h，可以适用于月面不平整路面的较快速移动。试验证明，本文所开发的控制系统在仿人机器人WUKONG-II上具有良好的实时性，确保了机器人控制与运行的稳定性，也为以后的航天员-仿人机器人人机协同、航天员远程运动操控等更多智能任务的开发提供实时控制基础。

6 结论

1)在相同的硬件条件下，QNX6.6操作系统的上下文切换时间、中断响应时间明显短于Windows 10和Linux Ubuntu 16.04，时钟精度高于这两者，具有良好的硬实时性能；

2)在0.62 Mbps的有效数据带宽下，Ether-CAT的通信周期可以稳定在250μs附近，并且抖动较小，具有高带宽、低时延、小抖动的特点；

3)在IMU数据采集周期和足底力传感器数据采集周期为5 ms的基础上，设计的多线程并发程序框架可以保证控制周期稳定在1000μs、时间抖动在可接受范围内，并在仿人机器人WUKONG-II上实现了3.24 km/h的平地快速移动，证明了该实现电机控制、多传感器数据采集、手臂运动、数据记录、外部通信功能的程序框架的实时有效性。