具身智能——轮式人形机器人软件系统设计

本文将基于分层模块化的方式设计机器人的软件系统。首先介绍软件系统的总体框架，然后基于ROS2设计搭建机器人整体的软件系统，包括使用ros2_control组件搭建底层执行器与传感器驱动系统，基于运动规划框架MoveIt2搭建机器人手臂、腿部等部分的运动控制系统及基于导航框架Nav2搭建底盘导航系统等。此外，还对机器人软件系统中较为重要的遥控子系统及自动停靠充电子系统进行了设计说明。

1 软件系统总体框架

如图1所示为轮式人形机器人整体的软件框架，总体分为底层、中间层和顶层三个方面。机器人底层软件系统主要负责传感器数据采集与滤波、关节电机PD柔顺控制、伺服总线驱动及安全防护程序等；中间层主要包括机器人整体及各部分的状态估计、机器人手臂等部分的运动学解算及轨迹规划、机器人底盘及上身的全身运动控制等；机器人的顶层可以分为感知模块、感知计算单元及运动捕捉遥控模块等，其中感知模块包括视觉识别、语音交互等，感知计算单元包括自主导航、三维重建及组合导航等，运动捕捉遥控包括手臂手部捕捉、头部追踪及关节遥控等模块。

机器人整体的软件系统基于机器人操作系统ROS2搭建，其中执行器、传感器等底层硬件驱动系统基于ros2_contro搭建，在此基础上使用ROS2生态下的开源机械臂操作平台MoveIt2进行手臂、腿部等部分的运动规划，使用开源的导航规划框架Nav2进行底盘的运动规划及导航。下面将详细介绍轮式人形机器人软件系统的搭建流程。

2 基于ROS2的机器人软件系统设计

ROS2是用于开发机器人应用的开源软件平台。其软件生态丰富，如功能强大的中间件、感知规划算法及功能完善的开发工具等。在构建机器人软件时，ROS2使用分布式系统的方式将机器人的驱动程序、感知系统、控制系统及执行器等分成独立的组件并通过显式通信共享数据；在通信管理方面，ROS2建立了接口抽象层组件以将特定的硬件或软件抽象出可用的接口；基于异步通信的方式，ROS2中的每个组件都能够以不同的频率提供数据或接收命令。

ROS2提供的节点之间的通信接口如图2所示，包含用于异步信息传递的话题、请求-响应模式的服务及具有请求、响应、定期反馈及取消的行动三种方式。与ROS1相比，ROS2中提供了用于管理节点生命周期的方法，通过状态机将节点的生命周期定义为未配置、未激活、激活和最终确定等几个状态，是协调分布式异步通信系统各部分运行的重要工具。ROS1的网络建立在TCP/IP的基础上，通常情况下TCP/IP较难在无线通信中传输数据，而ROS2采用数据分发服务（Data Distribution Service, DDS）的方式通过UDP优化数据传输的带宽和延迟。此外，ROS2还提供了身份验证、访问控制和加密等工具以确保机器人的安全性。ROS2系统包含丰富的软件生态，可以快速地搭建机器人的软件系统。

2.1 基于ros2_control的硬件驱动系统

ros2_control作为ROS2的核心组件，主要包括资源管理器（Resource Manager, RM）和控制器管理器（Controller Manager, CM）两部分，其逻辑框架如图3所示。

资源管理器将机器人的执行器、传感器等硬件及相关驱动程序抽象为标准化的接口，实际使用时通过插件库的方式加载硬件组件并且管理硬件的整个生命周期、硬件当前的状态及硬件的命令接口。由于相关的硬件组件可以复用，因此可以降低开发成本。资源管理器提供了非常灵活的状态接口与指令接口，例如状态接口通过实时读取编码器数据获取机器人关节电机当前的位置、速度及扭矩等状态数据。控制器管理器将资源管理器的硬件抽象与ros2_control框架下的控制器连接起来，控制器管理器不仅管理控制器及其所需的接口，还可以访问相关的硬件组件。ros2_control中的控制器基于相关的控制理论，通过比较测量值与参考值的误差计算控制输入，ros2_control框架下提供了丰富的控制器，例如差速驱动控制器、关节轨迹控制器等。

ros2_control框架下的硬件组件主要包括三种类型：系统级组件，如工业机器人这样的多自由度复杂的机器人硬件；传感器组件，如机器人编码器、力矩传感器等只具备读取功能的硬件；执行器组件，如电机、阀门等具备读写或只写功能的硬件。由于选用的机器人关节电机均支持CANOPEN通信方式，使用基于ros2_control的ros2_canopen功能包对关节电机进行驱动系统搭建。

ros2_canopen基于Lely Core CANOPEN协议栈和ROS2组件概念，将CANOPEN主站与驱动程序都封装为组件，使驱动电机不需要进行复杂繁琐的硬件编码，只需要进行简单的配置即可，极大地提高了软件开发效率。在主站中，可以定义节点ID、配置同步周期或心跳等选项。在驱动程序中，可以定义设备启动后自动配置的rpdo和tpdo及调用自动执行的SDO配置。ros2_canopen中自动生成总线配置的流程如图4所示，用户只需要创建配置相关的总线描述文件、运行设备容器即可完成CANOPEN设备的驱动。

基于ros2_control中的SystemInterface系统接口，ros2_canopen从机器人的描述文件URDF加载机器人的相关参数，由预先定义的YAML/BIN/DCF配置文件配置主站及设备驱动，之后加载至主站节点及从站节点并运行驱动程序，其完整流程如图5所示。

根据控制需求对机器人各关节电机进行配置，如对于底盘和头部这样只需要进行位置控制及速度控制的部位，为相关的电机配置位置接口和速度接口；对于手臂及腿部等既需要进行位置控制与速度控制还需要进行力矩控制的部位，为相关的电机配置位置接口、速度接口和力矩接口。

如图6所示为基于ros2_control的轮式人形机器人的底层软件系统框架。在基于ros2_control的执行器驱动接口的基础上，可以集成ROS2生态下丰富的控制器，如为机器人底盘配置差速控制驱动器；为机器人的手臂配置关节状态广播器、关节速度控制器、关节轨迹控制器、PID控制器、导纳控制器等；为机器人的IMU配置IMU传感器广播器等。

在控制器的基础上，可以实时接收来自软件上层的指令同时将传感器等底层硬件信息实时反馈给上层。如机械臂各关节接收来自MoveIt2的关节轨迹指令、反馈各关节的关节位置速度状态及力矩信息等；机器人底盘接受来自Nav2的底盘运动指令；接收来自用户的其他上层指令等。此外，通过ros2_control框架下的gz_ros2_control插件可以集成开源仿真器Gazebo，通过插件进行机器人控制器的硬件在环仿真测试；ROS2社区也提供了面向Isaac Sim、Mujoco等开源仿真器的接口，方便进行机器人软件系统的仿真与开发。

2.2 基于MoveIt2的运动控制系统

MoveIt2是ROS2机器人操作系统社区中面向机器人机械臂操作的软件开发平台，其集成了关于机械臂的运动规划、操作、感知、控制及导航等功能，如运动规划功能可以使机械臂在动态杂乱环境中生成灵活高自由度轨迹；使用抓取生成功能机械臂可以分析环境并与之交互；通过MoveIt2内置的逆运动学求解器可以为全驱动或欠驱动机械臂求解逆运动学；通过通用接口可以将规划好的时间参数化关节轨迹传递至低级的ros2_control硬件控制器并实时执行；使用Octomap工具可以连接机械臂的外部传感器或传感信息，如深度传感器或点云等；在机械臂运行时通过几何基元、网格或点云等数据进行碰撞检测以实时避开障碍物。此外，MoveIt2中还内置了能够进行任务规划的任务构造器、适用于机器人遥操作的遥控规划器、能够生成更平滑轨迹的三次样条算法及时间最优轨迹参数化等。MoveIt2还可以进行混合规划，将能够绕过复杂障碍物、避免陷入局部最小值但实时性较差的全局规划器与快速反应式的局部规划器结合。

如图7所示为MoveIt2软件包中的核心组件节点move_group，其使用ROS2中的话题和动作与机器人进行通信，如获取机器人当前的关节状态、监控机器人坐标系及其变换信息、维护规划场景等。该节点通过机器人的URDF等配置文件获取机器人的运动学参数，可以使用多种接口向MoveIt2请求相关的服务，如正逆运动学、规划路径、拾放物体等。

在MoveIt2中，通过指定规划组的方式对机器人各部分进行逆运动学控制，将轮式人形机器人划分为头部、躯干、左手臂、右手臂、腿部及底盘五个规划组，分别进行运动规划配置，如手臂、底盘等部分使用OMPL运动规划求解器，移动底盘使用差速求解器等。为集成机器人的视觉感知系统，可以通过MoveIt2内置的moveit_calibration功能包进行轮式人形机器人的手眼标定，完成校准相机参数、计算相机到末端执行器的坐标变换等。轮式人形机器人的深度相机位于机器人头部，可以将其看作眼在手外，需要得到深度相机至左右两只手臂末端执行器的坐标变换矩阵。

2.3基于Nav2的导航系统

Nav2导航规划框架基于ROS2系统，其采用动作服务器的形式进行导航中任务的请求，该动作服务器支持如导航至目标点等长时间任务且支持进度反馈和异步操作，使用状态机管理节点的生命周期。此外，Nav2使用行为树构建机器人的导航任务流程。规划器与控制器是Nav2导航任务的核心。规划器可以规划出一条路径，如计算由当前位姿至目标位姿的有效路径。控制器也称为局部规划器，在全局规划器规划出可行路径时，控制器使机器人跟踪此路径，控制器还具备恢复行为功能。Nav2中的平滑器将机器人的路径细化，便于机器人跟踪。此外，Nav2还具备路径点跟踪功能，使轮式人形机器人能够到达多个目的地。如图8所示为轮式人形机器人导航具体流程示意图。

2.4 机器人遥控系统

遥控是控制机器人运动的重要方法，本节对使用手柄控制机器人的移动底盘移动、末端执行器及各关节移动的设计方法加以说明。为更方便地控制机器人整体运动，使用MoveIt中的moveit_servo功能包，通过遥控手柄遥控的方式控制末端执行器及各关节速度。以控制右手臂末端执行器及其关节运动为例，遥控手柄中各按钮及遥控杆的映射设置如图9所示。

在实际遥控时，主要有两种遥控方式：指定关节运动与指定末端执行器运动。这两种方式分别通过JointJog与TwistStamped这两个消息类型定义。发布JointJog消息时，moveit_servo功能包将遥控手柄的输入转换为关节的速度增量，关节速度控制器通过ros2_control驱动机器人关节进行运动；当发布TwistStamped消息时，由于TwistStamped消息定义了末端执行器的速度增量，需要通过MoveIt中的逆运动学求解器求解各关节的运动，再由速度控制器结合ros2_control驱动机器人各关节运动使机器人末端执行器执行指定的运动。

由于单个遥控手柄按键有限，在控制机器人时，可以将按键组合作为指令的发布方式，例如将手柄按键A与摇杆组合控制移动底盘运动；采用不同的按键组合方式，可以控制轮式人形机器人的各个关节及两只手臂、底盘的运动。

2.5 机器人自主停靠与充电系统

在家庭或工厂环境中，轮式人形机器人在执行完指定任务后需要停靠在指定的位置等待任务命令发布，并且在电量低需要充电的情况下还需要在指定的位置进行充电，因此机器人的自主停靠与充电系统尤为重要。机器人自主停靠软件系统使用ROS2中的opennav_docking软件包，该软件包是ROS社区开发的一套完整、成熟且通用的自主停靠与充电方案，其功能完善，适用于不同类型的移动底盘、充电方法等。内置的任务服务器可以由行为树调用，使机器人在完成任务时或电量不足时自动完成停靠或充电。

机器人自主停靠及充电流程如图10所示。首先机器人根据接收的停靠或充电请求获取充电站插件及充电站的位姿；如果机器人不在预先指定的中间位姿的容差范围内，则导航至中间位姿；使用停靠插件检测停靠区域并且返回停靠位姿；之后加入视觉控制循环回路，在控制回路中，机器人会尝试到达停靠位置，同时视觉系统会对机器人的位姿进行修正；如果检测到机器人与充电站接触或机器人开始充电，则退出视觉控制循环；最后等待充电开始并返回成功。若机器人在停靠过程中的某个步骤失败，可以重试预设的N次，即机器人运行至中间位姿并再次尝试。如果仍然不成功，则返回表明失败类型的代码。若需要解除机器人停泊或充电状态，机器人先导航至中间姿态，再执行相应的任务。机器人自主停靠充电站如图11所示。

为准确快速检测停靠点，使用Isaac ROS中的isaac_ros_apriltag功能包，该功能包是专门针对Jetson系列主控制器开发的GPU实时加速功能包。如图12所示为机器人停靠充电站二维码示意图，在isaac_ros_apriltag功能包的输出数组中包含了二维码标签的ID，二维码图像的四个角点坐及中心点坐标及二维码标签的位姿。

3 本文小结

本文基于分层模块化的方式搭建了机器人软件系统总体框架，介绍了在ROS2系统下搭建轮式人形机器人软件系统的过程，基于ros2_control组件搭建了机器人底层硬件驱动系统，在此基础上集成了运动规划框架MoveIt2及导航框架Nav2，还介绍了遥控与自主停靠充电子系统，由此为机器人的全身运动控制奠定基础。