本項目為大三上《機器人技術基礎》課程團隊研討課題之一，當時做這個研討課題還花了挺多的時間，又覺得還比較有意思，因此放在博客中記錄一下。不過當時班上很多大佬的成果更牛逼，我們就屬於弟弟水平hhhhh
下面是詳細介紹：
一、課題背景
隨着人民生活水平的提高，健康問題正受到前所未有的重視，葯房葯品分揀的工作量和工作強度非常大，自動化分揀的需求非常迫切。西葯通常是以盒裝的形式存在，通常比較容易實現自動識別和抓取。而中葯以及部分粉狀葯品通常以軟袋包裝的形式存在，在生產運輸過程中，會發生形變而改變了改觀和規格，目前主流的西葯分揀機都無法實現對軟袋葯品的分揀。
針對這一需求，可設計一套機器人分揀系統，實現對下圖所示軟包裝葯品進行目標檢測與自動定位拾取。
二、課題任務
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)
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.