优必选高级算法工程师28页PPT详解实时机器人控制系统【附...

导读：

本文根据优必选北研所高级算法工程师董浩，在「智东西公开课」的超级公开课优必选专场第三讲《实时机器人控制系统在人形机器人中的应用》的讲解整理而来。在本次讲解中，董浩老师从通信、算力和软件三个维度分析了人形机器人对实时机器人控制系统的基本要求，并从硬件平台、实时主站、多任务APP软件框架三个层面对优必选最新实时机器人控制系统进行了深度的拆解分析，从不同维度为我们呈现了一套实时机器人控制系统的全景图。

同时，新一期的机器视觉与机器人公开课优必选专场将于5月18日（下周一）晚7点直播开讲！本次公开课由优必选研究院副院长&视觉组高级总监程骏主讲，主题为《优必选机器视觉技术在服务机器人中的应用》。在本公众号【智东西公开课】后台回复“优必选03”可获得第三讲课件及下周一机器视觉与机器人公开课优必选专场快速报名入口。

在算力层面，优必选实时机器人控制系统可以为开发者提供最多三个主控板卡，每个板卡可以选择Arm、DSP和X86不同的硬件平台来进行配置，以满足不同场景下的算力需求；
在通信层面，采用了EtherCAT实时高速以太网，可以保证控制系统和伺服关节之间的实时通信和数据同步，保证不同算法可以在高动态的工况下发挥出最好的效果；
在算法开发层看，采用组件化、功能可扩展、开发松耦合、调度分布式的设计思想，使得每个开发者都可以独立开发、测试，有效降低了开发难度，实现高效的团队协作。
以下是董浩博士的主讲实录，全文共计8721字，预计读完需要15分钟，本次讲解提纲如下：
1、人形机器人对控制系统实时性和多自由度控制的要求2、实时机器人控制系统架构解析3、Walker机器人在RoboCup比赛中的应用表现和挑战
大家好，我是优必选北研所高级算法工程师董浩，很高兴能和大家一起交流。我今天分享的主题为《实时机器人控制系统在人形机器人中的应用》，我将从以下三个方面来展开讲解：
首先，我会介绍一下人形机器人对控制系统实时性和多自由度控制的要求，主要体现在人形机器人自由度比较多、传感器比较多以及对运动控制算法动态性能要求很高这几个方面；
其次，我会重点讲一下优必选实时机器人控制系统的架构，将分别从硬件、实时系统以及软件三个方面来展开；
最后，我会将Walker机器人参加RoboCup2019比赛中面临的挑战，以及最后取得的成绩，给大家做一个分享，也会从应用的角度来阐述一下这套系统带来的优势。



人形机器人对控制系统实时性和多自由度控制的要求

先从Walker新一代机器人说起。从上面这幅图，大家可以看到Walker机器人一共有36个自由度，即36个一体化的伺服驱动单元，其中包括两个6自由度的腿，两个7自由度的手臂，两个4自由度的灵巧手以及一个2自由度的头。同时还有十个传感器，包括六维力、IMU以及超声传感器等。
对于这样一个复杂的分布式控制系统来说，双足和双臂的算法都要求控制系统和伺服关节之间的通信要足够快，而且周期性的定时跳动还要足够小，这样才能保证算法在高动态的工况下发挥出最好的效果。
此外，实时机器人控制系统还要求所有的伺服关节要同步运动，传感器的数据也要同步采集，从而保证算法的输入和输出都始终处于一个节拍，从而保证算法的性能。此外，所有算法都要满足实时性的要求，所以就要求在一定的时间周期内完成计算，整个系统的算力要能保障复杂的多任务算法APP都能够及时的完成。
从运动控制算法的角度来看，人形机器人的运动控制目标，主要包含末端位置、末端速度、末端姿态和末端力四个方面的控制。



末端位置，包括双足和双臂的工作空间的末端工作点。末端位置、姿态和速度的控制主要是为了满足双足行走和双臂操作，对工作空间轨迹的要求。末端力的控制主要是为了实现对环境的适应性，包括和人的物理交互，能够使得Walker机器人更安全、更鲁棒、更协调。
总结来说，人形机器人对控制系统的要求分为以下三个方面：

1、通信，为了解决众多伺服关节的高速同步通信问题，我们采用了EtherCAT实时高速以太网，整个控制系统可以支持不少于50个节点，同时还具备拓扑关系的适应性，这对于Walker机器人是非常重要的。我们知道，Walker机器人的内部空间是非常小的，所以伺服关节之间的走线要求就很高。在这里，EtherCAT的优势就体现在以下两个方面：首先，伺服之间的通信线缆可以采用一进一出的方式，减少了走线的空间；其次，可以采用星型连接，简化四肢末端通信线缆的连接关系，并且这样的网络工作模式也非常利于故障排查，也就是说任何一个伺服关节的节点出现异常，我们都可以快速定位到具体是哪个位置。
2、算力，控制算法的目标是控制末端的位置、姿态、速度和力，这就要求必须要具备多自由度的运动学、动力学算法。对于多体的运动学和动力学算法来说，随着自由度的增加，计算的复杂度会显著增加，而且在某些情况下运动学和动力学还不具备解析解，只能采用数值求解的办法，这样会导致计算的复杂度进一步增加。
此外，Walker机器人未来的目标是要走进千家万户，这就要求必须具备对复杂环境的适应性算法，包括走路时对地面的适应性、手臂工作时对障碍物的适应性、机器人在人机交互、人机协同时的适应性。这些复杂算法都对控制器的算力提出了巨大的挑战。
最后我们都知道处理器芯片的性能也在伴随着摩尔定律高速发展，这也要求我们控制系统的处理器能同步升级，跟上芯片的发展速度，硬件架构要从模块化的角度来匹配升级的要求。
3、软件，刚才我们提到的各种运动控制算法都会以独立的APP方式来运行，这就要求控制系统要提供一个实时的运行框架，能够满足不同运行周期的算法APP实时计算的要求，充分利用硬件的计算资源，同时考虑到复杂算法的不断升级，为了能够实现算力的扩展和均衡，我们采用了多主控板卡的架构，这也要求所有的算法APP能够运行在不同的主控板卡上进行分布式计算。不同算法APP之间还具备高速的实时数据交互能力。为了兼顾功耗、成本以及算力之间的平衡，我们选择了Arm、DSP、X86等多种主控板卡，这就要求算法APP必须具备跨平台能力，才能更好地利用硬件的计算资源。

实时机器人控制系统架构解析

实时机器人控制系统主要包含硬件、实时主站和多任务APP。

其中硬件是提供整个控制系统的通信和计算的载体；实时主站是基于RTOS实现的一套满足实时性要求的EtherCAT主站协议栈程序，可以完成所有的EtherCAT的相关功能，是通信功能的一个核心；多任务的APP是一套多线程的运行框架，能够将不同的算法封装到不同的APP中，以不同的周期在不同的主控板卡上实时运行，并且可以交换数据，当然它也是未来我们进行算法、软件开发的通用平台。



整个控制系统的架构如上图所示。最底层是一套多核异构的处理器硬件平台，再往上是在Linux基础上打了实时补丁的LinuxRT实时操作系统，兼容Arm和X86硬件平台。实时主站也就是EtherCAT相关的通讯的功能，多任务APP的框架主要包括机器人的核心任务，APP就是Robot Core Task和配套的SDK开发包，以及再往上层的感知接口，运动规划、步态生成、手臂控制、头部的动作、优化算法、环境交互、状态估计以及远程控制等一系列的算法APP，它们都会在统一的框架下运行，并且实时的进行数据交互。此外还会有一个APP管理进程，我们叫它App管理任务，它具备实时的消息中心、进程管理和系统log的功能，实现对各个APP的调度。



多核异构处理器硬件平台，我们借鉴了机箱的设计理念，所有的接口、电源、控制板卡都是模块化的设计，采用板卡的方式插入控制器的背板，并且支持现场可更换单元（FRU），这样可以减少线缆连接的不可靠性，易于安装和维护，也更方便控制整个控制器的尺寸和重量，整个控制器的升级和迭代未来也是比较方便的，可扩展性和可定制性也都比较高。

这套硬件平台最多可以支持三个主控板卡，每个板卡都可以选择Arm、DSP、X86不同的硬件平台来进行配置，通过不同的配置板卡组合，从而满足不同的算力、功耗以及成本的约束，与应用场景的适配度也比较高。
接口方面，支持EtherCAT，CAN和485三种总线，基本涵盖了目前不同的伺服关节的接口种类。接口模块还可以针对各种传感器的处理接口单独定制开发，以快速的适应不同应用场景的要求。



硬件平台的整体拓扑关系如上图所示，主要分为接口模块、电源模块和三个主控板卡模块。
接口模块支持EtherCAT、Ethernet、CAN、485、232、USB、WIFI、GPIO等常见的通用接口，同时也会将这些接口通过背板总线和各个主控板卡互联，能够保证每一个主控板卡上的GPIO、以太网和串口都可以导入到外部接口里；
电源模块支持48V输入，输出可以支持24V、12V、5V、3.3V等多路电源，输出的电源接口主要用来给各种外部传感器、输入和输出设备进行供电，以提高整个系统的电源管理能力。
三个主控板卡分别支持Arm、DSP和X86，可以实现算力的扩展和均衡。在实际应用中，可以根据不同的场景需求来选择不同的组合。在使用的数量上，可以只用一个板卡，也可以三个板卡都使用。在背板总线中有三条总线，分别是：
1、高速PCIE总线，负责实现主控板卡之间的高速数据交互，可以实现带宽超过2Ggps的通讯带宽，保证在所有的实时APP在运行过程中充分完成数据交换的功能；
2、I2C总线，用来监控和管理各个模块的运行状态；
3、电源总线，用来完成给各个模块进行供电。
在实时主站上我们实现了一套完整的EtherCAT主站协议栈软件，也是一个比较通用的解决方案。在最底层，我们通过修改以太网卡接口的驱动实现了一个实时的以太网接口驱动，能够在EtherCAT的数据链路层完成数据的实时收发功能。同时我们还实现了一个EtherCAT主站的核心线程，能够完成PDO和SDO的功能，分别对应周期性的指令和非周期性的指令。此外还有一个XML文件解析器，能够将网络拓扑的配置文件解析成核心线程可以识别的网络拓朴信息，从而实现对整个EtherCAT网络里所有从站的管理和控制。
这套实时主站可以支持不少于50个从站节点按照1000Hz的通信频率来同步运行，并且支持分布式时钟DC，同时在1毫秒运行周期下，它最大的跳动是小于20微秒。实时主站的软件是兼容Arm和X86处理器硬件平台的，也就是不管我们选择的主控板卡是Arm版本还是X86版本同样都能提供EtherCAT的主站功能，后续我们也会开发更深入的类似FOE这样的功能，从而通过实时主站软件，对下面所有的从站伺服关节进行固件升级等一系列的配套功能。



多任务APP软件框架主要由四大块构成，分别是Robot Core Task（机器人核心任务进程）、SDK开发工具包、不同算法APP和APP管理进程。
核心任务APP主要负责实现机器人上所有关节的控制和传感器的处理任务，包括伺服关节的状态机切换、运动指令的下发以及位置、速度等状态信息的处理。同时还有一个安全保护算法，能够对所有下发给伺服关节的指令进行保护性的判断，避免因为算法的异常导致动作的异常，比如超出位置限定、超出速度限定、超出力的限定等一系列的约束条件，还会有机器人关节间的碰撞检测，能够保证所有发给机器人的指令都是可以安全执行的，不会造成任何异常的后果。
状态监控是对当前机器人所有硬件模块的重大信息进行实时监测，遇到任何异常情况就会进行告警和记录，包括有些关节过载了、有些传感器超出量程了、外部有了异常的碰撞和接触等，对机器人进行全方位的安全监控和处理。
运动模式管理算法主要是为了满足包括行走、操作以及人机交互等动作的需求，可以对不同的任务来调度和处理不同的算法APP发送的指令进行融合，并在处理以后发送给关节。
多任务APP可以理解为将机器人的硬件执行端、传感端与算法进行隔离，使得所有的算法研究人员可以不用关注底层的硬件如何交互，直接开发算法来实现相应的功能即可。而且底层所有的硬件可以随时升级、迭代，不会影响上层算法程序的实施过程。
SDK开发包提供了一套和机器人的伺服关节以及传感器进行数据交互的接口，以及开发各种通用的机器人算法包的接口，使得上层APP的开发人员可以通过很友好的编程方式来实现相应功能。
上层实现的各种算法APP用来实现运动各个方面的任务，主要包括：
感知接口，负责与感知部分进行数据交互，接收感知决策和指令，同时回传机器人的状态；
运动规划则是根据感知决策发过来的任务指令，类似于走到什么位置、操作什么物体等，来规划出双足行走和双臂运动以及头部动作的指令输入，以提供给后续的算法来进行处理；
步态生成是根据运动规划发送的指令生成双足的运动轨迹，从而控制双足完成行走。手臂的控制同样也是根据运动规划发送的操作和抓取指令来生成双臂的运动轨迹，实现操作抓取。同样，运动规划也会告诉头部往什么位置去看，跟踪什么位置的物体等，这样就会实时的生成一个头部的运动轨迹，来完成相关任务。上面这一行主要是一些运动规划方面的任务，而下面这一行就是一些相关的控制算法，包括一些优化算法，会根据完成的任务目标，搜索寻优来得到一条最佳的运动轨迹。
环境交互主要是根据与环境接触的力和位置的情况来实现一些柔顺的控制效果，最终达到机器人与人和环境是一种安全的交互方式，这里会用到类似于阻抗控制、柔顺控制等相关的控制算法；
状态估计会将所有传感器的信息进行融合，综合的估计出机器人的一些运动状态，作为其他控制算法的输入量；
远程控制就是实现遥操作的功能，可以通过远端的计算机来控制机器人上的关节，实现在关节层次或者在工作空间层次的一种运动。
右边这个图是所有算法APP运行周期的分布。
首先在最底层，1000Hz是优先级最高、实时性要求最强的一层，包括了前面提到的核心任务、状态估计以及环境交互等算法。这些算法都是要按照最快的周期来处理各种传感器数据，并且给伺服关节发送指令，同时也是满足机器人与环境交互的时候，是以最快的反应来做出运动上的一些调整；
在200Hz的层次上，主要是完成感知方面的一些数据交互以及步态、手臂和头部的一些动作规划；
在1Hz层次上，主要是完成运动规划的算法，包括一些路径的规划、手臂操作的路径规划等，一般称之为中期规划；
而在0.1Hz层面，会跑一些优化算法，都是比较消耗计算资源的，同时也是为了在长期的运动中能够给出一个优化过的动作路径，作为后续一些运动的参考轨迹。
APP Manager管理进程，主要是进行所有APP之间消息的注册和收发处理。功能方面可以实现所有APP的调度，包括启动、暂停、终止等。
系统log功能，会把所有APP在运行过程中产生的各种运行状态的信息，包括异常的信息、出错的信息等保护信息全部log下来，以供开发人员在事后进行相关的分析。
SDK算法包目前主要实现了以下九个方面的功能：
基础数学工具包，能够实现一些矩阵运算、线性变换、矩阵分解等基础功能，而且所有操作符的方式，我们都借鉴了MATLAB的操作方式，所以这些工具对用户是非常友好的；
运动学，包含正运动学和逆运动学，分别实现了位置的正逆运动学和速度的正逆运动学，加速度的正逆运动学以及质心的正逆运动学，这些都是机器人运动控制必不可少的基本功能；
动力学层次分别实现了固定基和浮动基的串联正动力学，并联正动力学以及串联逆动力学和并联逆动力学，也为机器人控制打造了坚实的基础；
系统辨识，实现了单关节的辨识和整个Walker机器人系统质量属性的辨识，对于动力学里一些参数的调整有很好的帮助，可以让我们得到的结果更符合实际机器人的效果；
轨迹生成，目前实现了三角函数、样条曲线、摆线等不同的曲线函数，可以用来做机器人很多轨迹生成的参考；
阻抗控制实现了惯性的、容性的、阻性的等不同的阻抗操作符功能，这样上层APP开发工程师在做阻抗控制的时候，可以很容易地实现阻抗控制的效果，而不用自己再去写这部分代码；
状态估计，实现了一些常见的各种滤波器算法，包括卡尔曼滤波，还有一些基本的带通、高通和低通滤波器，可以为后面做机器人的状态估计提供一些基本的工具。



简单来说，多任务APP软件框架具有四个重要的特点：
算法的组件化，即所有实现不同功能的算法都是以APP的形式开发和运行，可以多人协作，也可以独立的进行测试和调试，从而有效地提高开发的效率和质量；
功能可扩展，未来随着机器人完成任务复杂度的不断增加，可以不断的扩展新功能，开发新的算法APP，系统也可以不断的去适应更加复杂的场景；
调度分布式，主要是指所有的算法APP都可以分布式的运行在不同的主控板卡上，根据对计算资源的需求进行分配，实现算力均衡，也能够保证系统整体的实时性；
开发松耦合，即算法是以APP形式进行开发，可以分布式的运行在不同的主控板卡上，便于开发人员独立来完成工作，更加有利于团队的协作开发。

Walker机器人在RoboCup比赛中的应用表现和挑战

今年是优必选和清华大学火神队联合组队参加RoboCup比赛的第三年，随着Walker机器人不断的升级，我们的成绩也在不断的进步。
RoboCup比赛的目标是希望能够在2050年前后组建一支机器人足球队，去战胜当年人类的世界杯足球冠军。这个比赛也相当于通过一套比较容易评价的标准平台来促进人工智能和机器人技术的发展。RoboCup比赛目前已经成为国际上级别最高、规模最大、影响力最广泛的机器人赛事，它不仅是一项综合性的国际活动，也是学术成分很高的赛事。
也许大家会觉得机器人足球队战胜人类足球冠军还是一件非常遥不可及的事情，但是作为亲历者而言，我们非常有信心来实现这个目标。甚至在今年的RoboCup学术大会上，这项赛事的创始人之一（Manuela Veloso）提出了一个更加激进的新口号，就是争取到2050年的时候，机器人可以成为世界杯足球冠军的一员，也就是说机器人可以和人类联合组队去夺取冠军，这对我们来说会是一个更大的挑战。
就我们参加的人形机器人比赛而言，这个赛事分为Kid-Size、Teen-Size和Adult-Size三个组别，分别对应儿童组、青年组和成年组，对应的机器人高度分别就是40～90cm，80～140cm，130～180cm，而对应的机器人数量分别为4V4、3V3、2V2，我们参加的是成人组比赛，也是未来机器人战胜人类足球队的终极载体。
我们在今年的比赛中拿到了技术挑战赛的亚军、Drop-in比赛的亚军以及2V2足球比赛的季军。技术挑战赛定义了四个技术挑战项目，通过各个项目最终的排名来计算总的排名，主要是为了鼓励发展机器人的一些单项能力。Drop-in比赛即每个队会出一个机器人，组成两个机器人的联队，然后和另一个联队去进行比赛，最终根据每个队的机器人的进球情况来进行排名。2V2的比赛就是根据足球比赛规则来进行2V2的对抗。
根据现在RoboCup比赛发展的趋势来看，整个比赛主要有以下四个方面的挑战：
1、更大的场地，今年比赛的场地从去年的6m*9m增加到了9m*14m，面积扩大1倍还多，这样的变化会给感知、定位导航、运动等算法带来更大的挑战，包括要识别更远距离的球、标志点，在更少的信息输入下完成自身的定位和导航，更大的场地就需要更高的运动能力、行走速度以及踢球力量都要加强，否则就会输掉比赛，甚至无法完成比赛；
2、更快的比赛节奏，规则中对于比赛的各种停顿时间也进行了缩短，意味着机器人有效运动时间在增多，对机器人的续航能力也提出了挑战。机器人需要处理的比赛信息也在增多，同时所有参赛队的机器人运动能力的提升带来了球的运行速度也会越来越快。这样导致感知算法对球的识别难度也越来越大；
3、更激烈的对抗，各个参赛队的机器人运动能力都在提升，导致比赛中会出现更多的机器人之间的物理接触和对抗，这对机器人的平衡控制提出了很高的要求。因为规则中明确指出，如果机器人发生碰撞后倒地的话，保护员如果对它进行保护，机器人就要被罚出场30秒，而如果机器人在比赛中有很好的碰撞后恢复能力，就会有很大的优势。
4、更多机器人，成人组的比赛从过去的1V1到现在的2V2，明年可能会升级到3V3，机器人数量的增加会要求更多的协作算法、更稳定可靠的无线通信、更精确的定位信息以及更精确的运动能力，比如机器人之间的传球能力会成为比赛的一个巨大的挑战。
针对这些挑战，技术挑战赛的四个项目也就随之确定，所以我这里就重点分享一下我们在技术挑战赛上取得的一些结果。
首先是High Kick ，这个项目要求机器人要能够将球踢高，以球最终飞过的最高距离作为排序的依据，受益于Walker机器人精准踢球的运动规划，以及伺服关节的高功率密度和控制器对它同步性控制的精准度，最终实现了0.4米的高度，拿到了这个项目的第一名；
第二个是High Jump ，也就是跳跃，纯粹在比较机器人的弹跳能力，更准确来说是要比拼伺服关节的爆发力，以及落地之后的缓冲控制算法。要知道对于跳跃比赛来说，如果落地控制没有做好的话，会在跳跃落地以后产生损坏的情况，所以对于控制器层面来说，要在最快的时间内在落地的时候把缓冲算法的功能都实现，对机器人产生一个很好的保护。最终Walker在这个项目实现了0.275秒的滞空时间，拿到了High Jump项目的第一名；
第三个是Push Recovery ，这个项目考验的是机器人身体的平衡抗干扰能力，比赛要求在机器人在原地踏步过程中用一定质量的重物去撞击机器人，按照最终承受的重量、有效的撞击距离来排名。在这个项目有一点比较遗憾的地方是，虽然Walker机器人承受的重量和等效的距离都是最大的，但是因为比赛最终的结果还要除以机器人的重量，因为Walker机器人的重量有一点偏高，所以最终拿到了Push Recovery项目的第三名；
最后一个是Goal-Kick from Moving Ball，即传球挑战，也是所有项目里难度最大的一项，它的要求是由人或者一个机器人将球传给另一个机器人进行射门操作，类似于我们人类开角球的过程，最终根据完成的成功率来排名。这个项目考验了非常综合的能力，包括机器人对运动中的球的识别、预测以及踢球的角度和力量的精确控制，成功完成该项目的难度非常大。我们最终拿到这个项目的第二名，后面会有精彩视频，可以来看一下最终的效果。
点击底部「阅读原文」可观看完整回看。

课件获取