YOLOv3 + Matlab Robotics ToolBox 实战 -- 软包装药品的机器人分拣系统设计与仿真

本项目为大三上《机器人技术基础》课程团队研讨课题之一，当时做这个研讨课题还花了挺多的时间，又觉得还比较有意思，因此放在博客中记录一下。不过当时班上很多大佬的成果更牛逼，我们就属于弟弟水平hhhhh
下面是详细介绍：

一、课题背景

随着人^{[minsheng在简书里竟然是敏感词所以诞生了如此长的前缀字符，54个不区分全角半角字符正好抵消不信你数。]}民生活水平的提高，健康问题正受到前所未有的重视，药房药品分拣的工作量和工作强度非常大，自动化分拣的需求非常迫切。西药通常是以盒装的形式存在，通常比较容易实现自动识别和抓取。而中药以及部分粉状药品通常以软袋包装的形式存在，在生产运输过程中，会发生形变而改变了改观和规格，目前主流的西药分拣机都无法实现对软袋药品的分拣。
针对这一需求，可设计一套机器人分拣系统，实现对下图所示软包装药品进行目标检测与自动定位拾取。

二、课题任务

1、在给定一组数据集的情况下，设计一套机器人分拣系统的方案，系统应具备包装袋目标检测和机器人自动定位拾取功能，暂不考虑软包装的分类问题；
2、设计机器人从起始位置出发到周转箱中取出软包装物品后放置到目标位置的合理的、无碰撞的轨迹；
3、使用Matlab Robotics ToolBox实现机器人分拣系统的验证原型，并进行分拣全过程的动画仿真。

三、开发平台及辅助工具

开发平台：Ubuntu
辅助工具：YOLOv3、LabelImg、Matlab Robotics ToolBox

这里对简单说明一下：
（1）Ubuntu：使用Ubuntu的原因是YOLOv3在这上面配置起来比在Windows上方便的多。
（2）YOLOv3：一种快速的目标检测方法，YOLO（You Only Look Once），顾名思义就是像人眼一样能够实现快速目标检测的同时还能保证较高的准确率。选择YOLOv3的原因是相较于v2和v1，其准确率和速度都有较大提升，毕竟用啥都得用好的不是。
（3）Matlab Robotics ToolBox：该工具搭载在Matlab上，提供了用于设计、仿真和测试操纵器、移动机器人及人形机器人的工具和算法。包括但不止于机器人运动学建模、轨迹生成、正向和逆向运动学以及动力学算法，是一款面向机器人开发的很好的基础工具。

四、方案设计及实现

4.1、目标检测

4.1.1、数据集标注
为了方便进行深度学习训练，需要对大量图片进行标注来创建数据集， LabelImg 是一个可视化的图像标定工具，使用该工具前需配置环境python + lxml。具体安装及使用方法可以参考官网教程：https://pypi.org/project/labelImg。
数据集的标注过程确实比较繁琐，需要对每幅图片一个个手动进行框选，工作量比较大，下图是标注过程截图：

另外，由于老师提供的数据集只有35张图片，虽然考虑到每一张图中都含有数个不同样本，但是总量还是比较少的，因此还需要进行数据集的扩充（主要是旋转、缩放等），图像增强开源包链接： https://github.com/Paperspace/DataAugmentationForObjectDetection，扩充结果如下：

4.1.1、训练数据集
使用YOLOv3进行训练，网上教程挺多的，我这就不再赘述，可以参考：【学习笔记—Yolov3】Yolov3训练VOC数据集&训练自己的数据集，也可以查阅官方教程： YOLO: Real-Time Object Detection。训练结果如下图：

将检测结果的矩形框中心位置保存成txt文件，为4.2.6节药品建模做准备。

4.2、机器人分拣系统建模

4.2.1、机械臂模型
以PUMA560 作为仿真模型：

利用Matlab Robotics ToolBox进行建模，其使用教程可以参考官方教程： Robotics ToolBox for Matlab。
通过下述语句即可调用PUMA560模型并进行简单的正逆运动学分析：

mdl_puma560
%载入工具箱提供的example，建立好的puma560机械臂模型
qn = [0 pi/4 pi 0 pi/4  0] ; %期望的关节角
T = transl(0.5, 0.5, 0.7) * rpy2tr(0, 3*pi/4, 0)
T = p560.fkine(qn)
%上面两句都是求出正运动学的位姿，也可以直接自己给出一个期望的位姿T
qi = p560.ikine(T)
qi = p560.ikine6s(T)
%这两句都是求解逆运动学，但是第二句适用于6自由度机械臂
%由于存在多解情况，我们可以人为的限定期望的位型解
qi = p560.ikine6s(T, 'ru')
%'l','r'   左手/右手
%'u','d'   肘部在上/肘部在下
%'f','n'    手腕翻转/手腕不翻转
p560.plot(qi)

Robotics ToolBox中的PUMA560模型

4.2.2、工作空间问题
在实际问题中，机械臂的工作空间问题很重要，它决定了机械臂能否正确且完整的完成搬运任务。在对药箱建模时，应当考虑到其位置是否位于机械臂的工作空间中。对于机械臂的工作空间求取，这里我采用的是在各关节角范围之内各随机1000个关节角并进行正运动学分析得到末端执行器的空间位置：

mdl_puma560;
close all;
clc;

deg = pi/180;
thetamin = [-160 -45 -225 -110 -100 -266]*deg;
thetalength = [320 270 270 280 200 532]*deg;
figure
theta = zeros(1,6);%关节角
p560.plot(theta);
hold on;

countsum = 1000;%总的随机数量
Tjtraj2 = zeros(countsum,3);%
for i = 1:countsum
    theta = thetamin + thetalength.*rand(1,6);%关节角随机
    Txy=p560.fkine(theta);%正运动学求解位姿
    Tjtraj2(i,:)=transl(Txy);%提取x、y、z坐标
    if Tjtraj2(i,1) < xmin
        xmin = Tjtraj2(i,1);
    elseif Tjtraj2(i,1) > xmax
        xmax = Tjtraj2(i,1);
    end
    if Tjtraj2(i,2) < ymin
        ymin = Tjtraj2(i,2);
    elseif Tjtraj2(i,1) > ymax
        ymax = Tjtraj2(i,2);
     end
     if Tjtraj2(i,3) < zmin
        zmin = Tjtraj2(i,3);
    elseif Tjtraj2(i,3) > zmax
        zmax = Tjtraj2(i,3);
     end
     plot3(Tjtraj2(:,1),Tjtraj2(:,2),Tjtraj2(:,3),'color',[249 206 226]/255);%绘制关节末端点
     hold on; 
end
%输出极值
xmin
xmax
ymin
ymax
zmin
zmax

通过这样的方法能够得到机械臂的空间位置点云图和x、y、z坐标的最大值和最小值：

4.2.3轨迹规划
将机械臂一次完整的搬运过程分为七段：

第一段：从初始工作点O前往待取货位置B正上方一点A，简记为OA或进入工作点过程；
第二段：从A点到待取货位置B，简记为AB或下降取货过程；
第三段：从B点取货并上升到A点，简记为BA或上升存货过程；
第四段：从A点到待存货位置D正上方一点位置C，简记为AC或搬运过程；
第五段：从C到待存货位置D,，简记为CD或下降存货过程；
第六段：从D存货结束并上升到C点，简记为DC或上升二次取货过程；
第七段：从D点返回初始位置O，简记为DO或返回二次取货过程。

其中，搬运过程AC和返回二次取货过程DO两端采用七次多项式进行中间轨迹规划，其余五段均采用笛卡尔坐标规划。以深蓝色箭头末端表示末端执行器位置，机械臂一次完整的搬运过程如下图：

（1）七次多项式轨迹规划
该部分作为单独的一篇博客进行阐述，博客链接：
值得注意的是，由于高次多项式易产生冗余的轨迹，因此对两个中间点位置进行调整，可使得轨迹变得简单，如下图：

本项目具体使用截取片段如下：

    %七次多项式四个过程点的姿态
    q1= p560.ikine6s(T1);
    q2= p560.ikine6s(T2);
    q3= p560.ikine6s(T3);
    q4= p560.ikine6s(T4);
    %%前进%%
    qmove=[q1; q2; q3; q4];
    %初始姿态
    qinit = p560.ikine6s(Tinit);
    qinitup = p560.ikine6s(Tinitup);
    %七次多项式规划转移
    [Qmove,Qvmove,Qamove] = traj_7(qmove,2,3,2);

（2）笛卡尔坐标轨迹规划
利用函数 function traj = ctraj(T0, T1, t) 可进行初始位姿和目标位姿之间的位姿进行插补得到中间位姿，其空间轨迹为出初始位置到目标位置的一条线段。再利用 ikine6s进行逆运动学求解可以得到每个中间位姿的关节角结果。截取程序片段如下：

    %笛卡尔坐标轨迹规划
    cutnum = 30;
    Twork0 = ctraj(Tinit,T1,cutnum);%进入工作点 
    Twork1 = ctraj(T1,Tfetch,cutnum);%下降取货 
    Tup = ctraj(Tfetch,T1,cutnum);%提过程
    Tdown = ctraj(T4,Tput,cutnum);%放过程
    Qwork0 = p560.ikine6s(Twork0);
    Qwork1 = p560.ikine6s(Twork1);
    Qup = p560.ikine6s(Tup);%提过程
    Qdown = p560.ikine6s(Tdown);%放过程

4.2.4、药箱建模
将药箱抽象为立方体，且药箱无盖，通过下述程序可以完成一个立方体的绘制：

%P1,P2 为长方体对角线两端点
function out = draw_box(P1, P2,color,varargin)%P1、P2为对角度顶点

    opt.color = color;
    opt.alpha = 1;
    opt.mesh = 'none';
    opt.n = 40;

    [opt,args] = tb_optparse(opt, varargin);
    % backward compatibility with RVC
    if ~isempty(args)
        opt.color = args{1};
    end
    if length(args) > 1
        opt.alpha = args{2};
    end
    
    daspect([1 1 1])
    %hold on;
     %h = surf([P1(1),P2(1);P1(1),P2(1)],[P1(2),P1(2);P2(2),P2(2)],[P2(3),P2(3);P2(3),P2(3)], 'FaceColor', opt.color, 'EdgeColor', opt.mesh, 'FaceAlpha', opt.alpha); %顶   
    h = surf([P1(1),P2(1);P1(1),P2(1)],[P1(2),P1(2);P2(2),P2(2)],[P1(3),P1(3);P1(3),P1(3)], 'FaceColor', opt.color, 'EdgeColor', opt.mesh, 'FaceAlpha', opt.alpha);%底
    h = surf([P1(1),P1(1);P1(1),P1(1)],[P1(2),P1(2);P2(2),P2(2)],[P1(3),P2(3);P1(3),P2(3)], 'FaceColor', opt.color, 'EdgeColor', opt.mesh, 'FaceAlpha', opt.alpha);%左
    h = surf([P2(1),P2(1);P2(1),P2(1)],[P1(2),P1(2);P2(2),P2(2)],[P1(3),P2(3);P1(3),P2(3)], 'FaceColor', opt.color, 'EdgeColor', opt.mesh, 'FaceAlpha', opt.alpha);%右
    h = surf([P1(1),P2(1);P1(1),P2(1)],[P1(2),P1(2);P1(2),P1(2)],[P1(3),P1(3);P2(3),P2(3)], 'FaceColor', opt.color, 'EdgeColor', opt.mesh, 'FaceAlpha', opt.alpha);%前
    h = surf([P1(1),P2(1);P1(1),P2(1)],[P2(2),P2(2);P2(2),P2(2)],[P1(3),P1(3);P2(3),P2(3)], 'FaceColor', opt.color, 'EdgeColor', opt.mesh, 'FaceAlpha', opt.alpha);%后
%     if ~ishold||hold_on != 1
%         hold off
%     end
    if nargout > 0
        out = h;
    end
end

对4.2.1节所述工作空间绘制程序稍作调整，并考虑药品箱的位置可以得到：

mdl_puma560;
close all;
clc;

deg = pi/180;
thetamin = [-160 -45 -225 -110 -100 -266]*deg;
thetalength = [320 270 270 280 200 532]*deg;
figure
theta = zeros(1,6);
p560.plot(theta);
hold on;
  
xmin = 100;
ymin = 100;
zmin = 100;
xmax = -100;
ymax = -100;
zmax = -100;

countsum = 1000;
Tjtraj2 = zeros(countsum,3);
for i = 1:countsum
    theta = thetamin + thetalength.*rand(1,6);
    Txy=p560.fkine(theta);
    Tjtraj2(i,:)=transl(Txy);
    plot3(Tjtraj2(:,1),Tjtraj2(:,2),Tjtraj2(:,3),'color',[249 206 226]/255);%运物品轨迹图像
    hold on; 
end
draw_box([0.3;-0.265;-0.5],[0.7;0.265;-0.35],'b','mesh','k','alpha',0.3);%画待取货箱
draw_box([-0.365;0.3;-0.5],[0.1653;0.7;-0.35],'b','mesh','k','alpha',0.3);%画存货箱

运行上述程序可以得到药品箱位于工作空间中的情况：

4.2.5、RGB-D深度摄像头
考虑到实际药品存放时存在高度差，这将影响到机械臂的决策问题，而通过普通二维图像难以获取深度信息，因此本方案中提出在通过获取到目标检测结果区域得到的x、y坐标位置之后，采用深度摄像头进一步获取到该区域药品的高度，通过在初始位置进行检测之后比较多个目标中平均高度最高的药品作为待拾取药品。
4.2.6、药品建模
在建模时，由于我们并不能容易地从二维图像中得到药品高度，因此此处仅利用随机的给出药品的高度，利用目标检测得到的txt文件保存的矩形框位置进行药品入队操作，利用简单的先入后出原则+碰撞检测可以实现药品的堆叠，如下图所示：

4.2.7、末端执行器
由于需要抓取软包装药品，同时药品体积较小，因此末端执行器采用手爪是不合适的，因此我们提出使用吸盘进行药品抓取。但是在实际应用中很容易出现吸附不牢而导致软包装物品掉落的情况，因此我们查阅相关资料^[1]，选取下图所示高度仿生特性吸盘：

文中写道：“本发明公开了一种具有高度仿生特性吸盘，包括仿壁虎脚掌壳体、沿该仿壁虎脚掌壳体边缘并对应仿壁虎脚掌壳体凸起部设置的仿生脚趾结构、设于所述仿壁虎脚掌壳体下端的褶皱结构及设于该褶皱结构下端的膜；所述褶皱结构由若干褶皱发生单元组合而成，所述仿壁虎脚掌壳体、褶皱结构及膜的中心均开设有通孔，仿壁虎脚掌壳体的通孔处设有真空装置，褶皱结构及膜通过各自的通孔在真空装置的作用下实现无间隙贴合。本发明的吸盘通过将褶皱结构和膜结合，进而能够在真空装置的作用下进行无间隙贴合，贴合后形成大量纳米级的微小真空单元体，达到“小尺寸效应”所要求的临界尺寸，进而增加了与外界接触面间接触的比表面积，增强了吸附效果。”总体满足我们的要求。

五、成果展示

演示视频截图如下图所示，完整视频放在B站，传送门：https://www.bilibili.com/video/av88800561

参考文献：
[1] 江苏科技大学.一种具有高度仿生特性吸盘 :中国, 110203295 A [P]. 2019.05.14.