Webots2020a -- Super_Transbot 超市机器人仿真建模

这是我和 sszxc 为本学期《机器人软件工程》课程所做的的课程设计，整个项目开发历时 1 个月，模型全部在 Webots2020a 中搭建完成。为完成在仿真环境下的超市自主装卸货物任务，我们设计了一套复杂的控制器进行运动控制。演示视频：基于Webots平台的超市补货机器人。

文章目录

1. 任务需求

如图所示，超市中 A,B,C,D 四个货架上各有12个货窗，每个货窗里最多可以摆放1件货物，现每个货架上都有至少3个货窗缺少货物。
要求设计并制作一台自主超市机器人。机器人从场地的红色“起点区”出发，完成货物的补货操作，最后回到起点。

题目比较开放，我们自行拟定的任务需求是：

超市机器人能够自主定位，能按照既定路线进行巡逻并检查货架状态
当发现货架有空位时能根据空位周围的货物自主判别出该空位应该装填的货物类型
能够从场地中心的仓库中识别并抓取待装填货物
能够选取最优路径返回存货点并完成上货任务

2.仿真模型搭建

2.1 测试场景搭建

创建仿真测试场景如下，其中 A、B、C、D 分别为四个货架，4 个橙色点为巡逻定点。

2.2 机器人模型设计

2.2.1 简介

四个麦克纳姆轮组成的全向运动底盘，保证了机器人在狭小空间内的移动性能，内置了GPS以及电子罗盘模块实现自主定位。

前后分别设置一颗摄像头，用于识别货架上、货舱内的货物，并返回相应的类型和位置信息。

模仿叉车外形，前部设置了一个二自由度的机械臂，末端抓手设置了一对力传感器，实现对不同大小的货物的抓取功能，并能将货物提升到任意高度，对应题目中的双层货架要求。

2.2.2 参数介绍

遵照软件工程复用思想，将例程中“KUKA youBot”的麦克纳姆轮打包成PROTO，从而实现麦轮节点的复用。四个麦轮直径0.10m，通过HingeJoint安装在车身主体结构上；
车身整体尺寸为0.59m×0.40m×0.64m（长宽高），搭建车身时采用模块化设计，主要分为车身、机械臂两部分。车身主要由Transform、Shape嵌套而成，主要利用到的几何体包括Cylinder、Box等；机械臂主要是在Pioneer3所配置的机械臂的基础上进行适当改造，包括尺寸、电机扭矩及直线电机伸展长度等，同时在机械臂的抓手增加了一对力传感器，以实现对物品抓取力度的精准控制；
机器人配置的摄像头分辨率为256*128，视角1.8，配备Webots自带的Recognition API，一定程度上减少了物品识别的难度，使仿真专注于整体软件功能的开发，体现了离线编程的优势；
为了实现更精确的物理仿真，我们还对机械臂抓手、麦轮的摩擦力参数进行了设置，以保证机器人能够在当前环境保持正常的运动；此外，还设置了合适的车身重量，以保证车辆的运动性能更加优越、抓取稳定性更好。

2.3 控制器设计

2.3.1 状态机设计

为方便进行机器人各运动状态之间的复杂控制，设计状态机如下：

switch (*main_state)
{
    
    
   case Init_Pose
   case Recognize_Empty
   case Arround_Moving
   case Grab_Item
   case Back_Moving
   case TurnBack_To_LoadItem
   case Item_Loading
   case RunTo_NextShelf
   default
}

Init_Pose 为初始默认进入工作区状态，到达指定位置后跳转到 Recognize_Empty，对货架进行识别。
Recognize_Empty 为识别空货架状态，这个状态主要调用了识别货架的函数，识别完成后将货架上的货物状态更新到数组，若有缺货则进行补货判断，计算出需要的货物名称以及需要放置的位置，然后跳转到 Arround_Moving 寻找货品；若没有缺货则跳转到RunTo_NextShelf 状态，前往下一个货架进行检测。
Arround_Moving 为环游巡检状态，运动过程中一直保持之前暂存的货物名称，以对货仓进行实时检测判断，直到找到了合适可抓取的货物，跳转至 Grab_Item。
Grab_Item 为抓取仓库货物状态，根据暂存的货物名称行进到指定货物的抓取位置，控制机械臂进行抓取，提升后跳转至 Back_Moving 返程。
Back_Moving为回程存货环游状态，机器人会携带着需要的货物返回之前的货架，此时需要进行路径规划选择一条最短路线，到达货架前后跳转至 TurnBack_To_LoadItem。
TurnBack_To_LoadItem 为转身面向货架状态，紧接着跳转至Item_Loading状态。
Item_Loading 为上货状态，根据货物需要放置的位置计算出机器人的目标位置，之后完成上货操作，跳转至 Init_Pose。
RunTo_NextShelf 为前往下一货架状态，根据存储的坐标进行路径规划，当到达下一货架定点时，跳转至 Recognize_Empty。
default为错误报警状态。

利用上述九个状态构成的状态机，能够较地完成对机器人装卸货物的任务要求，各状态内部操作的封装也使得我们的控制器看起来更加简洁干练。

2.3.2 控制器关键函数

1. 基本动作

void lift(double position);
给定高度，使机械臂上升直线电机运动到指定位置。
void moveFingers(double position);
给定宽度，使机械臂手中左右两个直线电机运动到指定位置。
bool Moveto_CertainPoint(double fin_posture[], double reach_precision);
给定位姿 fin_posture，包括 x、z 坐标与姿态角 angle；给定位控精度 reach_precision；内嵌函数 caculate_tmp_target()，给定目标位姿 double fin_posture[]，根据当前位姿与目标位置，进行位姿的线性插值，差值结果保存在 tmp_posture[] 中作为下一临时目标位姿。
为了解决罗盘值在2π与0之间跳转时出现的机器人原地旋转一圈的冗余运动，因此在姿态角的线性差值前引入逻辑判断：if (fabs(fin_posture[2] – compass_angle) > PI) 以达到向旋转角度更小的方向作旋转运动。

2.空货架识别

bool Find_Empty(WbDeviceTag camera)
选定车体后侧摄像头，调用 API 识别货物，根据货架大小拟合公式，计算货架上货物的位置更新存入数组。具体来说，每格货架的宽度为 0.25，给机器人设置的定点位于货架水平方向中间，因此可以由检测到的货物与摄像头的相对位置，来判断货物位于哪一列货架。为高矮两种货架高度设置不同的阈值后，也可以根据纵向的相对位置来判断货物位于哪一层。

3.寻找货物

bool Find_Goods(WbDeviceTag camera,char *good_name,int *item_grasped_id);
选定车体前侧摄像头，给定物品名 good_name，货物 ID 号 item_grasped_id，通过调用 API 识别并判断视野中货物信息（包括 ID、尺寸等），返回是否找到待存放货物的 bool 类型变量。考虑到机械臂的抓取能力，寻找的过程中滤除了距离机器人过远的货物，只考虑当前面对的货仓区域。

4.对准并抓取

bool Aim_and_Grasp(int *grasp_state, WbDeviceTag camera, int objectID);
选定车体前侧摄像头，传入给定的货物 ID 号来锁定摄像头目标。将抓取货物分为 “调整位置、抓、举” 三个步骤。首先针对不同货物的大小计算一个最合适的抓取位置，然后根据摄像头返回的物体大小粗略地计算手抓的位置，此时激活力传感器，不断收紧抓手直到力传感器达到阈值，表示已经抓紧物体，最后抬升物体达到指定高度，完成抓取功能。

5.返回货架

TurnBack_To_LoadItem
这部分算法直接包含在状态机内，根据货物的目标位置，计算机器人水平方向相应的偏移距离，给定一个固定的前后偏移，将上货路径分为两步，由此确定机器人的行驶路径。

3. 总结 - - 超市机器人软件开发及其质量管理

在本次超市机器人的软件开发课程设计中，首要任务是完成实现机器人软件工程的功能性，其次再考虑其他方面的要求。因此，在最开始的开发过程中，在明确我们的需求后，我们将任务重心放在底层基础功能的开发上，包括底盘及移动轮的设计、抓手的开合及升降运动设计以及物品识别等多个方面。在完成相当一部分的开发之后，转而进行机器人核心状态机的设计，并留出各部分的接口，这一任务的完成不仅一定程度上减轻了后期的开发难度，并且还加快了后期的开发速度。
在完整的超市机器人软件开发的过程中，为了实现其功能性、可靠性、可维护性、可理解性、可测试性及可修改性等多个方面的要求，我们贯彻并采用了如下的措施：

模块化与抽象化技术
在整个超市机器人补货的复杂任务的前提下，将其拆分为“查货架”、“寻货”、“取货”、“回库”及“上货”五个易解决的小任务，实现了任务的模块化；对于每个独立的任务，只考虑该任务本身所需要的入参出参，忽略其细节（比如如何实现、如何计算等），实现了任务的抽象化。
合适的程序设计语言及风格
实验中选择了 C 语言进行开发。相较于 python，由于 C 语言特殊的变量声明及函数定义要求，使得它能够使得程序更加地干净整洁，更加利于软件的维护。同时，在程序编写过程中，采用合适的变量名的命名、注释的使用、规律的数据说明格式、清晰简洁的语句结构和输入/输出格式、有序的层次结构，规避了合作开发过程中的理解困难问题，使得整个项目的开发更加快速。
信息隐藏及局部化技术
对于获取机器人位置、方向角以及商品名的获取等类似只读操作，将其封装成功能函数，只返回需要读取的内容，而不能对其源头进行改动，实现了一定程度上的信息隐藏；而对于主状态的跳转，将其约束在状态机函数中，只能在此处进行更改，而不允许在子函数中发生改变，体现了主状态的局部化。
意外情况的防范
当发生主状态机跳转错误时，打印警告信息 “Error form State Machine！”，起到一定的警示作用。
复用技术
在超市机器人的模型设计中，大量使用 USE 操作，实现各节点属性的复用；同时，将麦克纳姆轮的模型封装成 PROTO 节点，使其能够为机器人底盘搭建起到重复使用效果。利用这些复用技术一定程度上减少了超市机器人模型搭建的工作量。
项目管理
为了实现项目良好的管理，我们采用了 Github 仓库进行版本控制，每次工作时进行拉取、合并、提交等必须的流程，同时附以简短的提交日志便于对方了解提交内容。除此之外，每完成一个功能之后，利用腾讯会议及 QQ 屏幕共享等软件进行汇报并讨论改进的方案，同时对接下来的工作进行适当的调整和分配，明确各自的任务，并督促对方及时完成。

4.项目开源

本项目为 -贝塔- 与 sszxc 的合作项目，托管在 Github。
项目资源：

-贝塔- 的仓库地址：Super_Transbot
sszxc 的仓库地址：Super_Transbot
演示视频：基于Webots平台的超市补货机器人