YOLOv3 + Matlab Robotics ToolBox 實戰(zhàn) -- 軟包裝藥品的機器人分揀系統(tǒng)設計與仿真

????本項目為大三上《機器人技術基礎》課程團隊研討課題之一，當時做這個研討課題還花了挺多的時間，又覺得還比較有意思，因此放在博客中記錄一下。不過當時班上很多大佬的成果更牛逼，我們就屬于弟弟水平hhhhh
????下面是詳細介紹：

一、課題背景

????隨著人[minsheng在簡書里竟然是敏感詞所以誕生了如此長的前綴字符，54個不區(qū)分全角半角字符正好抵消不信你數。]民生活水平的提高，健康問題正受到前所未有的重視，藥房藥品分揀的工作量和工作強度非常大，自動化分揀的需求非常迫切。西藥通常是以盒裝的形式存在，通常比較容易實現自動識別和抓取。而中藥以及部分粉狀藥品通常以軟袋包裝的形式存在，在生產運輸過程中，會發(fā)生形變而改變了改觀和規(guī)格，目前主流的西藥分揀機都無法實現對軟袋藥品的分揀。
????針對這一需求，可設計一套機器人分揀系統(tǒng)，實現對下圖所示軟包裝藥品進行目標檢測與自動定位拾取。

二、課題任務

1、在給定一組數據集的情況下，設計一套機器人分揀系統(tǒng)的方案，系統(tǒng)應具備包裝袋目標檢測和機器人自動定位拾取功能，暫不考慮軟包裝的分類問題；
2、設計機器人從起始位置出發(fā)到周轉箱中取出軟包裝物品后放置到目標位置的合理的、無碰撞的軌跡；
3、使用Matlab Robotics ToolBox實現機器人分揀系統(tǒng)的驗證原型，并進行分揀全過程的動畫仿真。

三、開發(fā)平臺及輔助工具

開發(fā)平臺：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上，提供了用于設計、仿真和測試操縱器、移動機器人及人形機器人的工具和算法。包括但不止于機器人運動學建模、軌跡生成、正向和逆向運動學以及動力學算法，是一款面向機器人開發(fā)的很好的基礎工具。

四、方案設計及實現
4.1、目標檢測

4.1.1、數據集標注
????為了方便進行深度學習訓練，需要對大量圖片進行標注來創(chuàng)建數據集， LabelImg 是一個可視化的圖像標定工具，使用該工具前需配置環(huán)境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節(jié)藥品建模做準備。

4.2、機器人分揀系統(tǒng)建模

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] ; %期望的關節(jié)角
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、工作空間問題
????在實際問題中，機械臂的工作空間問題很重要，它決定了機械臂能否正確且完整的完成搬運任務。在對藥箱建模時，應當考慮到其位置是否位于機械臂的工作空間中。對于機械臂的工作空間求取，這里我采用的是在各關節(jié)角范圍之內各隨機1000個關節(jié)角并進行正運動學分析得到末端執(zhí)行器的空間位置：

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);%關節(jié)角
p560.plot(theta);
hold on;

countsum = 1000;%總的隨機數量
Tjtraj2 = zeros(countsum,3);%
for i = 1:countsum
    theta = thetamin + thetalength.*rand(1,6);%關節(jié)角隨機
    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);%繪制關節(jié)末端點
     hold on; 
end
%輸出極值
xmin
xmax
ymin
ymax
zmin
zmax

????通過這樣的方法能夠得到機械臂的空間位置點云圖和x、y、z坐標的最大值和最小值：

4.2.3軌跡規(guī)劃
????將機械臂一次完整的搬運過程分為七段：

第一段：從初始工作點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兩端采用七次多項式進行中間軌跡規(guī)劃，其余五段均采用笛卡爾坐標規(guī)劃。以深藍色箭頭末端表示末端執(zhí)行器位置，機械臂一次完整的搬運過程如下圖：

（1）七次多項式軌跡規(guī)劃
????該部分作為單獨的一篇博客進行闡述，博客鏈接：
????值得注意的是，由于高次多項式易產生冗余的軌跡，因此對兩個中間點位置進行調整，可使得軌跡變得簡單，如下圖：

????本項目具體使用截取片段如下：

    %七次多項式四個過程點的姿態(tài)
    q1= p560.ikine6s(T1);
    q2= p560.ikine6s(T2);
    q3= p560.ikine6s(T3);
    q4= p560.ikine6s(T4);
    %%前進%%
    qmove=[q1; q2; q3; q4];
    %初始姿態(tài)
    qinit = p560.ikine6s(Tinit);
    qinitup = p560.ikine6s(Tinitup);
    %七次多項式規(guī)劃轉移
    [Qmove,Qvmove,Qamove] = traj_7(qmove,2,3,2);

（2）笛卡爾坐標軌跡規(guī)劃
????利用函數 function traj = ctraj(T0, T1, t) 可進行初始位姿和目標位姿之間的位姿進行插補得到中間位姿，其空間軌跡為出初始位置到目標位置的一條線段。再利用 ikine6s進行逆運動學求解可以得到每個中間位姿的關節(jié)角結果。截取程序片段如下：

    %笛卡爾坐標軌跡規(guī)劃
    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節(jié)所述工作空間繪制程序稍作調整，并考慮藥品箱的位置可以得到：

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深度攝像頭
????考慮到實際藥品存放時存在高度差，這將影響到機械臂的決策問題，而通過普通二維圖像難以獲取深度信息，因此本方案中提出在通過獲取到目標檢測結果區(qū)域得到的x、y坐標位置之后，采用深度攝像頭進一步獲取到該區(qū)域藥品的高度，通過在初始位置進行檢測之后比較多個目標中平均高度最高的藥品作為待拾取藥品。
4.2.6、藥品建模
????在建模時，由于我們并不能容易地從二維圖像中得到藥品高度，因此此處僅利用隨機的給出藥品的高度，利用目標檢測得到的txt文件保存的矩形框位置進行藥品入隊操作，利用簡單的先入后出原則+碰撞檢測可以實現藥品的堆疊，如下圖所示：

4.2.7、末端執(zhí)行器
????由于需要抓取軟包裝藥品，同時藥品體積較小，因此末端執(zhí)行器采用手爪是不合適的，因此我們提出使用吸盤進行藥品抓取。但是在實際應用中很容易出現吸附不牢而導致軟包裝物品掉落的情況，因此我們查閱相關資料[1]，選取下圖所示高度仿生特性吸盤：

????文中寫道：“本發(fā)明公開了一種具有高度仿生特性吸盤， 包括仿壁虎腳掌殼體、沿該仿壁虎腳掌殼體邊緣 并對應仿壁虎腳掌殼體凸起部設置的仿生腳趾 結構、設于所述仿壁虎腳掌殼體下端的褶皺結構 及設于該褶皺結構下端的膜；所述褶皺結構由若 干褶皺發(fā)生單元組合而成，所述仿壁虎腳掌殼 體、褶皺結構及膜的中心均開設有通孔，仿壁虎 腳掌殼體的通孔處設有真空裝置，褶皺結構及膜 通過各自的通孔在真空裝置的作用下實現無間 隙貼合。本發(fā)明的吸盤通過將褶皺結構和膜結 合，進而能夠在真空裝置的作用下進行無間隙貼 合，貼合后形成大量納米級的微小真空單元體， 達到“小尺寸效應”所要求的臨界尺寸，進而增加 了與外界接觸面間接觸的比表面積，增強了吸附 效果?！笨傮w滿足我們的要求。

五、成果展示

????演示視頻截圖如下圖所示，完整視頻放在B站，傳送門：https://www.bilibili.com/video/av88800561

參考文獻：
[1] 江蘇科技大學.一種具有高度仿生特性吸盤 :中國, 110203295 A [P]. 2019.05.14.

最后編輯于 ：
?著作權歸作者所有,轉載或內容合作請聯系作者
【社區(qū)內容提示】社區(qū)部分內容疑似由AI輔助生成，瀏覽時請結合常識與多方信息審慎甄別。
平臺聲明：文章內容（如有圖片或視頻亦包括在內）由作者上傳并發(fā)布，文章內容僅代表作者本人觀點，簡書系信息發(fā)布平臺，僅提供信息存儲服務。

友情鏈接更多精彩內容