概述
最近一段时间，为了支持自己的科研工作，我和小伙伴合作写了一套在宇树科技的A1机器人上运行的四足机器人控制器和本体传感器状态估计算法。现在我们准备把这些算法开源，特此介绍一下。除我以外，前Nvidia A1/Robotics Research Scientisit 达兴烨博士编写了控制器的主要框架，CMU的机械工程硕士生张子昕实现了部分模型预测控制、地形匹配和一些调试工作。
开源代码链接：
当代机器人研究者，特别是刚入学的博士生，往往陷入一个“搞理论还是搞硬件”的困境。足式机器人研究特别如此，如果一个研究者加入一个新成立的实验室准备做足式机器人，但是却发现没有成熟的硬件可以用来做实验，那么可预见的未来一两年时间都会花在基本的搭建硬件和系统开发调试工作上。但这会给研究者很大的压力，因为这些基本工作不能很快带来可发表的科研成果。在2015年-2019年之间，波士顿动力公司不断发布惊艳的足式机器人演示视频，吸引了很多研究者进入这个领域，但是市场上并没有成熟的硬件产品能够让研究者快速低成本地搭建一个足式机器人的研究平台。世界上只有小部分进入麻省理工学院仿生机器人实验室（MIT Biomimetic Lab）或者苏黎世联邦理工学院机器人系统实验室（ETH Robotic System Lab）的幸运儿才能接触到优秀的研究平台Cheetah和Anymal机器人。这些实验室都深耕四足机器人至少五年以上，一届一届研究生做了大量的系统搭建和调试工作之后才做出这两款机器人。2019年后随着一批四足机器人公司的发展，很多较为成熟的研究平台面市，但是我们依然缺乏具有以下特点的易用方案：
- 机器人硬件本体小巧、轻便、低成本；
- 支持对所有关节实时高频力控，通信稳定，软件接口易用；
- 开源控制器，实现最先进的模型预测控制器，便于研究人员将自己的研究成果与之对比；
- 提供使用紧凑传感器和低计算量即可稳定运行的长距离定位测速算法
目前市面上已有的商用机器人产品多半具有以上前两点特征，因为廉价的半直驱电机已经被证明是一种成熟优秀的实现方案。后两点则在商用机器人产品上难以获得，因为商业公司往往不愿意开源自己的控制器和定位算法。所以研究人员只好选择具有前两个特点的成熟商业硬件方案，自己开发控制器和定位算法。这两年由于四足机器人比较新颖，很多基于learning的工作可以不用和基于模型的方法做对比就能够发表，但是可预见的一段时间后人们会开始要求研究人员把learning的工作和传统的基于模型的工作做对比，此时很多人的研究就会碰到一些阻碍。我们工作的重点就是提供开源的控制器，并且方便研究人员安装和使用。
控制器的一些性能演示视频如下
雪地行走
防撞击
我们的控制器和本体传感器状态估计算法并没有什么理论创新之处，只是对MIT Biomimetic Robotics Lab在Cheetah 3机器人上实现的控制器的重新实现，我们的工作更多地关注机器人软件开发的便利性和兼容性，使得其他从事四足机器人的研究人员可以只敲几行代码就把整套开发环境设置起来控制A1机器人硬件，软件有如下特点
- 控制器运行在Docker容器当中，不管执行控制器的计算机是板载嵌入式设备还是通过网线远程连接的台式电脑，Docker都提供了统一的控制器运行环境，可以方便在不同计算设备间移植。
- 我们实现了两种控制器方便用户参考学习，这两种控制器可以说是目前最好的基于模型的控制方法，稳定性非常高。
- 控制器程序直接通过局域网和机器人通信，保证稳定性。高层控制指令和控制器的状态数据通过ROS消息发送，方便连接其他ROS的导航、调试、可视化功能。
代码的核心理论来自以下两篇论文，用户可以在阅读论文的同时在程序源码中寻找相应的部分
[1] Bledt, Gerardo, et al. "MIT Cheetah 3: Design and control of a robust, dynamic quadruped robot." 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2018.
[2] Di Carlo, Jared, et al. "Dynamic locomotion in the mit cheetah 3 through convex model-predictive control." 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2018.
快速入门 - 控制Gazebo中的仿真机器人
首先我们推荐用户运行我们的Gazebo仿真来熟悉控制器。运行前最理想的系统环境是：一台Ubunbu 18.04或20.04的电脑，CPU主频在3GHz左右，使用Nvidia系列的GPU（让Gazebo运行得更好）；系统上安装docker；系统连接一个XBox的游戏手柄（我们需要用它控制机器人）
然后按照README安装软件，就可以跑起下面这个演示：
全部安装过程大概只需要不到二十行指令，用时约四十分钟（大部分时间在编译docker镜像）。
用户可能注意到仿真运行的速率低于实际时间，因为我们调低了速率保证仿真在性能较差的电脑上也能稳定运行。用户可参考Gazebo的官方教程进行仿真速率调整，相关信息在README中有介绍。
软件架构介绍
仿真例程运行时，a1_cpp_ctrl_docker中的控制器程序gazebo_a1_ctrl与a1_unitree_gazebo_docker中的仿真环境通过ROS的消息进行沟通，收取仿真的机器人传感器信息、发送力矩控制指令。此外，ROS的joy_node读取XBox joystick的信息，发给控制器，控制器把joystick的摇杆和按钮指令解析成机器人的行为控制指令。
代码结构介绍
整个控制器由C++实现，作为一个ROS package进行编译。github.com/ShuoYangRob…下的主要文件如下
| 文件/文件夹 | 功能 |
|---|---|
| 文件夹legKinematics | 包含A1Kinematics类，提供腿前向运动学计算 |
| 文件夹test | 一些测试脚本 |
| 文件夹utils | 工具函数 |
| A1BasicEKF(.h/.cpp) | 核心类，状态估计卡尔曼滤波器 |
| A1CtrlStates.h | 一个包含控制器所需的变量的结构体 |
| A1Params.h | 一些常量定义 |
| A1RobotControl(.h/.cpp) | 核心类，定义控制器的核心逻辑架构，定义论文[1]中的QP力矩控制算法 |
| ConvexMpc(.h/.cpp) | 核心类，定义论文[2]中的模型预测控制算法 |
| ***A1ROS(.h/.cpp) | 接口类，***代表不同的名字（Hardware，Gazebo，Isaac），分别对应硬件机器人、Gazebo中的仿真机器人、Isaac Sim中的仿真机器人。 |
| Main***.cpp | 程序的主函数，***代表不同的名字（Hardware，Gazebo，Isaac），分别对应硬件机器人、Gazebo中的仿真机器人、Isaac Sim中的仿真机器人。 |
需要注意的是，有的用户可能发现自己没法用自己的XBox 手柄控制Gazebo里的机器人，或者移动摇杆机器人的运动方式和README中说明的不一样，这是因为有很多厂家按照XBox的标准布局制作手柄，但是使用不同的硬件方案读取手柄，于是按键和摇杆的顺序都有所不同。用户需要先通过rostopic echo joy确认好自己手柄的按键和摇杆与sensor_msgs/Joy中数组元素的对应关系，然后在代码中修改joy_callback函数中的数组元素下标。
实际机器人控制 - 硬件准备
首先需要购买一台宇树科技的A1机器人，选择具有开放的编程接口的版本。
我们需要使用合适的算力来控制机器人。A1的硬件上有内置树莓派或者内置Nvidia TX2两种选择，但是这两者任何一个的CPU都不太强，运行高频的控制器和视觉里程计很吃力。我们推荐A1就选硬件最便宜的版本，然后额外再购买一台Intel NUC的 i7或者i9型号迷你电脑产品。
大部分使用A1机器人的研究者通常采用网线直连的方式控制机器人。将台式或者笔记本电脑的IP设置到192.168.123.X网段，直接用网线把机器人和电脑连接起来，在电脑上就可以接收到机器人的数据，然后在电脑上运行控制器即可控制机器人。这种方法比较简单，坏处是机器人就不得不被限制在室内空间和电脑附近。
稍微麻烦一点但是易于扩展的办法是在机器人上安装一台Intel NUC迷你电脑，把迷你电脑和机器人用网线直接连接，在迷你电脑中运行控制器。此外为了方便调试还可以在迷你电脑上外接USB Wi-Fi热点或者路由器，这样就可以把自己的台式机和迷你电脑通过Wi-Fi连接，然后通过SSH和VS Code Remote开发模式远程调试迷你电脑里的控制器。Intel NUC的系统安装推荐Ubuntu 18.04 和ROS melodic，其他系统可能需要进行更多的定制化修改。
另外A1机器人内部的电脑运行了一些读取Realsense相机的程序，虽然方便但是带来了额外的传输开销。我们推荐把A1头部的Realsense相机的连接线拔下来，直接把Realsense相机连接到Intel NUC上做图像读取，或者额外安装一个Realsense相机直连Intel NUC。
为了方便调试和野外实验，我们还给Intel NUC单独设置了一套供电系统和电池，这样可以在不开启机器人的情况下调试程序。NUC使用的是19V的输入，因此需要合适的直流变压模块。构建系统的所有硬件列表如下：
| 模块 | 价格（元） | 备注 |
|---|---|---|
| A1机器人 | 以官网询价为准 | |
| Intel NUC | 8代i7约3000元，以官网询价为准 | 需额外购买内存和SSD硬盘 |
| 24V锂电池 | 200 | 常规航模电池即可 |
| 24V-19V变压模块 | 200 | 连续输出功率必须大于95W |
| Intel Realsense D435 | 2000 | 连接NUC的USB口 |
| 迷你路由器GL-AR750S | 400 | 通过NUC的USB口供电，通过USB-LAN转接线连接NUC |
| 5.5mmx2.5mm 同轴电源公头 | 30 | 给NUC供电，需线规美标20AWG以上 |
| Xbox摇杆手柄 | 100 | 一定选用无线版本，将USB接收机插在NUC上 |
| 线材 | 网线，电源线，USB线等 |
实际机器人控制 - 控制器安装
我们把通过网线和机器人直接连接的电脑叫做“控制电脑”。控制电脑上除基本的Ubuntu 18.04操作系统和ROS melodic以外，只需再额外安装Docker。接下来我们按照README来在控制电脑上安装控制器。
为了方便调试，我们推荐在控制电脑中首先在一个终端里单独启动roscore，然后在另一个终端中启用ROS的joystick读取程序，这样ROS系统可以读取Xbox 摇杆手柄。最后在第三个终端里打开Docker容器，首先通过rostopic echo joy来确定摇杆手柄响应正常。然后运行docker中的控制器。
机器人开始运行程序的标志是机器人关节全部上电，机身稍稍离地。此时可以推动左摇杆让机器人站起来，其他操作与Gazebo中的版本相同。
在实际机器人运行时，系统的软件架构如下
和之前Gazebo仿真的版本对比，区别只是控制器使用了真机器人的数据作为接口。A1机器人的接口设计非常高效，使得我们不用给控制电脑做任何操作系统层级的修改就能够实现500Hz的高频力矩控制。
注意！宇树科技官方的unitree_legged_sdk最新版本（v3.5.1）不支持A1机器人，如果不打算使用我们的Docker容器、而是直接在电脑上安装控制器的话，需要在git clone github.com/unitreerobo… 之后，手动切换到v3.3或者v3.2分支。
控制其他仿真器中的机器人
目前的控制器结构将控制算法和硬件/仿真器接口解耦开来，用户如果想要使用别的仿真器控制机A1机器人，可以对比几个文件中的构造格式连接自己的仿真器。Nvidia在去年推出了Omniverse旗下的机器人仿真工具Isaac Sim，过去一年里我在Isaac Sim开发团队实习，也打通了使用这套控制器来控制Isaac Sim中的仿真机器人的接口，等到下一次Isaac Sim更新时就可以给用户带来Isaac Sim的控制演示。
视觉里程计安装
我们推荐使用Vins-Fusion作为视觉里程计方案。足式机器人在运动中有大量的振动和快速姿态变化，因此参数和一般适用于无人机的Vins-Fusion的参数不同。在我们的测试中，Vins-fusion展现出了较好的长距离定位表现。下面的视频演示了A1机器人在CMU的操场行走的过程中，通过视觉里程计进行定位的过程。在走过452米的路程后里程计有30米左右的误差，基本满足局部路径规划的定位需求，不过精度还有很多提升的空间。
运行视觉里程计需要在控制电脑上安装VINS-Fusion和realsense的驱动以及realsense-ros，安装后可以下载这个Github库github.com/ShuoYangRob… 用其中的一系列脚本来帮助快速运行VINS-Fusion
其中脚本github.com/ShuoYangRob… 用于读取realsense并对相机进行配置，用户需要将其中第一行的变量
<arg name="serial_no_camera_forward" default="938422070270"/>
的default值修改成自己的realsense相机的序列号。由于realsense相机固件、驱动、ROS套件的版本问题，用户可能需要在这一步进行一些测试，确保能够在运行脚本后，在ROS环境中可以看到两个名为infra1和infra2的相机图像消息。
则能够运行系统上安装的VINS-Fusion，然后使用我们的控制器发布的A1机器人的IMU数据以及realsense的相机图像进行定位。实测表明，目前性能最好的Intel NUC电脑（i9）可以稳定地同时运行我们的控制和VINS-Fusion。在性能稍差的控制电脑上VINS-Fusion需要进行调整以保证实时性。另外千万不要用Jetson系列的产品，因为CPU性能比较羸弱，不能支持高效的控制和定位。
另外我自己正在做一个融合了视觉、IMU和机器人腿部传感器（关节编码器和脚上的接触传感器）多种传感器的里程计方案（VILO，Visual Inertial Leg Odometry）。这一部分工作还没有发表，暂时还不能开源，有兴趣的朋友可以单独联系我。
其他工具
为了方便机器人的导航功能开发，另外实现了一个比较粗糙的机器人可视化工具：
GitHub - ShuoYangRobotics/A1_visualizer
这个工具其实有点自己重复造轮子（使用tranform graph来publish marker array而不是使用Rviz的源生robot description来显示机器人），而且作为一个简单的可视化工具有过多的依赖工具包，不是很优雅，期待有缘人能帮助优化一下。
结语
由于作者水平和时间有限，代码库可能会有种种不同的问题，欢迎用户在这个文章下留言或者直接在Github上提issue讨论使用中出现的bug。
