机器人模型设计

本学期的《机器人软件工程》课程实验在 Webots 仿真平台上进行，仿真环境如图所示，机器人的任务是根据需要抓取货仓中摆放的货物，填补到四面货架的合适位置上。

仿真环境

针对这一任务，我和 贝塔 设计了一款超市机器人“Super_Transbot”。

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

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

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

机器人模型

机器人模型搭建

为了在 Webots 仿真平台中高效地搭建出机器人，我们充分遵照软件工程复用思想，将例程中“KUKA youBot”的麦克纳姆轮打包成 PROTO，从而实现麦轮节点的复用。四个麦轮直径 0.10m，通过 HingeJoint 安装在车身主体结构上。

车身整体尺寸为 0.59m×0.40m×0.64m（长宽高），搭建车身时采用模块化设计，主要分为车身、机械臂两部分。车身主要由 Transform、Shape 嵌套而成，主要利用到的几何体包括 Cylinder、Box 等；机械臂主要是在 Pioneer3 所配置的机械臂的基础上进行适当改造，包括尺寸、电机扭矩及直线电机伸展长度等，同时在机械臂的抓手增加了一对力传感器，以实现对物品抓取力度的精准控制。

机器人配置的摄像头分辨率为 256*128，视角 1.8，配备 Webots 自带的 Recognition API，一定程度上减少了物品识别的难度，使仿真专注于整体软件功能的开发，体现了离线编程的优势。

为了实现更精确的物理仿真，我们还对机械臂抓手、麦轮的摩擦力参数进行了设置，以保证机器人能够在当前环境保持正常的运动；此外，还设置了合适的车身重量，以保证车辆的运动性能更加优越、抓取稳定性更好。

机器人控制器设计

为了实现流畅的取货补货功能，机器人控制器采用了状态机的思想，初始化完毕后进入 switch 状态机切换，其中 main_state 共九个状态：

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 为错误报警状态。

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

控制器关键函数

  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

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

机器人软件开发与质量管理

最后呼应一下课程名《机器人软件工程》，总结一下我们在开发过程中学习到的一些思路：

  1. 模块化与抽象化技术 在整个超市机器人补货的复杂任务的前提下，将其拆分为“查货架”、“寻货”、“取货”、“回库”及“上货”五个易解决的小任务，实现了任务的模块化；对于每个独立的任务，只考虑该任务本身所需要的入参出参，忽略其细节（比如如何实现、如何计算等），实现了任务的抽象化。

  2. 信息隐藏及局部化技术 对于获取机器人位置、方向角以及商品名的获取等类似只读操作，将其封装成功能函数，只返回需要读取的内容，而不能对其源头进行改动，实现了一定程度上的信息隐藏；而对于主状态的跳转，将其约束在状态机函数中，只能在此处进行更改，而不允许在子函数中发生改变，体现了主状态的局部化。

  3. 复用技术 在超市机器人的模型设计中，大量使用 USE 操作，实现各节点属性的复用；同时，将麦克纳姆轮的模型封装成 PROTO 节点，使其能够为机器人底盘搭建起到重复使用效果。利用这些复用技术一定程度上减少了超市机器人模型搭建的工作量。

  4. 版本控制 为了实现项目良好的管理，我们采用了 GitHub 仓库进行版本控制，每次工作时进行拉取、合并、提交等必须的流程，同时附以简短的提交日志便于对方了解提交内容。除此之外，每完成一个功能之后，利用腾讯会议及 QQ 屏幕共享等软件交流讨论，同时对接下来的工作进行适当的调整和分配，明确各自的任务，并督促对方及时完成。

本次项目已经托管在 GitHub 平台。