【Webots项目实战｜超市机器人（一）】需求建模｜模型设计｜基于Webots的Youbot模型搭建

视频演示：https://video.zhihu.com/video/1377498292964802560

内容列表

4 机器人控制器设计与实现

超市机器人作为一个复杂的机器人系统，需要运行一个高效的机器人操作系统才能更好地完成任务， 且为了提高代码的复用性以及合作开发的效率，我们选择在 ROS 下进行开发。 Webots 提供了 Webots ROS 包，其中封装了众多话题及服务，可以基于此快速进行二次开发。

4.1 开发环境

PC 机操作系统：Ubuntu 20.04

仿真环境：ROS、Webots、Webots ROS 包

4.2 控制器整体设计

控制器结构：

1) 整个控制器采用串级控制

2) myYoubot 和 superMarketRobot 对应于两个 ROS 节点

3) 第一个主管运动控制，另外一个主管智能流程

4) 通过 ROS 中话题的订阅和发布进行通讯。

4.3 状态机设计

首先来说说为什么选用 smach 有限状态机。

状态机什么时候用？

1) 在很多应用场景中，我们需要设计一些复杂的机器人任务，任务中包含多个状态模块，而这些状态 模块之间在某些情况下会发生跳转，这就是 SMACH 可以发挥作用的地方；

2) 快速原型设计：基于 Python 语法的 SMACH 可以实现状态机原型的快速开发测试；

3) 复杂状态机模型：SMACH 支持设计、维护、调试大型复杂的状态机；

4) 可视化：SMACH 提供可视化工具 smachviewer ，可以看到完整状态机的状态跳转、数据流等信息。

状态机什么时候不用？

1) 非结构化任务：非结构化任务调度中可能存在未知的状态跳转；

2) 低层次系统：SMACH 适用于任务机调度，不适合相对简单、不包含任务级调度的系统；

3) 拆分模块：SMACH 的使用并不是为了让我们将模块拆分。

状态机的数据传递

1) 在很多场景下，状态和状态之间有一定耦合，后一个状态的工作需要使用到前一个状态中的数据，这 个时候就需要在状态跳转的同时，将需要的数据传递给下一个状态。SMACH 支持状态之间的数据 传递。

2) 在状态机中添加状态时，多了一个 remapping 参数，类似 ROS 中的 remapping 重映射机制，这里 可以将参数重映射，每个状态在设计的时候不需要考虑输入输出的变量具体是什么，只需要留下接 口，使用重映射的机制就可以很方便的组合这些状态了。这个和 ROS 的框架原理很类似，SMACH 确实和 ROS 很配呀！

采用 Python 编写 smach 有限状态机，实现给四个货架上货，状态图如图 15 所示。

状态机运行顺序为：

1) 寻找空货架；

2) 前往备货区抓取相应的物品并上货；

3) 重新识别当前货架，若仍有空位置，回到步骤 1，若货架已满，继续下面的步骤；

4) 若四个货架全部上货完成，则结束任务，否则，前往下一个货架，从步骤 1 开始执行。

主函数中，参数映射关系如图 16 所示。

图 16: smach 状态机参数映射关系

4.4 Youbot 控制器设计

自定义 arm.msg 和 move.msg 消息类型。

# arm.msg 
string type 
bool grabFlag 
bool[5] operateFlag 
float64 [5] angle
# move.msg
string moveType 
float64 speed 
float64 x 
float64 z 
float64 alphe

将机械臂的运动封装为一系列连续的运动，如预抓取、抓取等动作，通过话题来发布指令控制机械臂和 底盘的运动，底盘和机械臂所有动作的封装如图 17 所示。

图 17: 底盘和机械臂动作封装

预抓取和抓取物品的动作如图 18 所示。

抬起和放上层的上货动作如图 19 所示。

4.5 机器人定位与动态避障

采用 ROS 中的 gammping 包完成 SLAM 建图，如图 20 所示。

图 20: SLAM 建图

利用 youbot 中的激光传感器结合 SLAM 完成机器人的定位以及动态避障。

4.6 物体识别

物体识别采用 Webots 中 Camera 相机的 Recognition 功能，具体流程如下：

1) 请求 /<device_name>/has_recognition 服务开启识别功能；

2) 订阅 /<device_name>/recognition_objects 话题获取物体在图像中的位置信息以及物品类别；

3) 用事先计算好的货架位置与物品位置进行匹配，找到空货架的位置（行、列）。

对货架上的物品识别的效果如图 21 所示。

Webots 给 C、C++、Python、Java、Matlab 提供的 Recognition 物品识别接口中均可以返回图片 中物品的数量，唯独 ROS 无法返回物品数量，这个问题是很小的细节，但对于空货架的识别却很关键， 在前期调研的过程中，没有注意到这个问题。尝试过在 ROS 下链接到 Webots C/C++ 进行多文件的混 合编译，但对于 Cmake 文件的编写尚不熟悉，故放弃，后又尝试调用 Webots 的 Python 库函数，但也 未能实现。

在实验似乎停滞不前时，我又发现识别物体发布的消息，订阅的消息会漏掉确定的物体信息，怀疑 是 webots_ros 包本身存在的 bug，仔细观察发现，中间漏掉的物品数加 1 即为摄像头视野中所有物品 的数量，借助这个问题解决了 recognition 无法返回物体数量的问题。

对于返回物品信息的消息，由于它是不断重复发送的，无法得知是否重复，只能通过集合这个数据 结构取并集，获取不重复的消息。如果 Webots 封装了服务类型，会更能适应我们实验的需求。