基于YOLOv3和EPnP算法的多藥盒姿態估計

仇 翔，王國順，趙揚揚，滕 游，俞 立

(浙江工業大學 信息工程學院，杭州 310023)

0 引言

機械臂抓取和分揀在當前工業生產和生活中越來越普遍，機械臂代替人工完成復雜和重復的工作不但可以提高生產效率，也可以讓人們的生活更加便利，使人們可以更專注于腦力勞動。現今在醫院和藥房中基本還是通過人工取藥，引入機器人進行藥房藥品的自動抓取和藥品的自動分揀能夠提高醫院和醫藥倉庫的操作效率，具有重要的現實意義和商業價值。在機械臂的藥盒抓取和分揀操作中，實現藥盒的精確定位和姿態估計是機械臂準確抓取藥盒的前提，因此需要對藥盒進行實時定位和6D姿態估計。本文針對此應用場景進行藥盒定位和姿態估計的相關技術研究。

藥房或藥品倉庫是一個光照和背景環境相對復雜的場所，比如交錯的背景和上層貨架對于光線的影響，以及藥盒緊密排列時容易被誤識別為一個整體等，在進行藥盒定位和姿態估計時很容易受到這些不利因素的影響。因此，傳統的圖像邊緣檢測或形狀分割等方法難以勝任此類場景下的任務需求。自2012年Krizhevsky提出深度卷積網絡AlexNet[1]以來，基于深度學習的圖像處理技術在目標檢測[2]、文字識別[3]、語義分割[4]等領域得到了迅猛的發展。深度學習算法可以很大程度地克服圖像處理過程中由光照和復雜背景帶來的不利影響，可以利用高維特征對目標進行檢測，可以大幅提高目標定位效果。自從Redmon等人提出YOLO[5](you only look once)深度學習網絡以來，因其在圖像識別速度上的突出優勢獲得了廣泛關注，并在后續改進版本YOLOv2[6]、YOLOv3[7]中改進了對小目標物體的識別精度，使其在識別速度和識別準確度上都有著很好的效果。最近幾年，已有研究將深度學習算法直接用于姿態估計[8-9]，其中輸入數據僅僅是RGB圖像，這些方法可以實現較好的目標檢測，但這類方法難以實現實時的姿態估計，而且通過深度學習算法直接實現姿態估計需要復雜而龐大的數據集，這些都是難以構建的。

為實現在復雜場景下對多藥盒的精確定位和姿態估計，本文提出一種基于YOLOv3深度學習算法和EPnP[10]算法相結合的多藥盒姿態估計算法。EPnP算法具有很強的抗干擾性能，只需三對共面(非共面需要四對)3D-2D匹配點就可以精確估計目標姿態。本文充分利用YOLOv3算法和EPnP算法兼具準確性和快速性的優點，將多目標藥盒的定位和姿態估計分步進行。首先，利用YOLOv3對于多目標物體的定位框，依次分割出單個藥盒，然后對每個藥盒進行ORB(Oriented FAST and rotated BRIEF)特征提取、特征匹配和EPnP姿態估計。本算法對于多目標、姿態不固定藥盒具有很好的定位和姿態估計效果，滿足機械臂抓取操作過程中對目標定位和姿態估計的實時性要求。最后，本文結合OptiTrack(動作捕捉系統)系統設計了本算法的姿態精度對比實驗，實驗結果表明，本文所提出的多藥盒姿態估計算法能夠滿足實際應用的需求。

圖2 YOLOv3算法網絡結構圖

1 系統結構及原理

針對藥房或藥品倉庫中藥盒分揀任務的要求，本算法主要分為多藥盒定位和姿態估計兩個部分：1)藥盒定位部分采用YOLOv3算法，通過對藥盒訓練集標注的邊框信息進行k-means聚類運算以減少模型訓練量。采集藥盒圖片制作PASCAL VOC數據集并訓練藥盒識別模型；2)姿態估計部分通過藥盒定位框信息分割出目標藥盒并結合藥盒的類別信息進行藥盒模板選取，然后通過模板和目標藥盒之間的ORB特征點進行特征匹配和EPnP藥盒姿態求取。本算法系統流程如圖1所示。硬件環境如表1所示，在此基礎上搭建軟件環境：Ubuntu 16.04、Python、Opencv、CUDA等。

圖1 系統流程圖

表1 工況試驗設置及結果

2 藥盒定位

本文采用YOLOv3算法對藥盒進行定位，YOLOv3算法由YOLO算法發展改進而來，YOLO算法將目標的侯選區選取和識別兩個階段合二為一，只看一眼圖片就能知道哪些對象以及它們的位置，在目標識別定位的速度上有很大的優勢，可以達到45FPS到155FPS。YOLOv3算法針對YOLO算法的不足之處進行了改進， YOLOv3算法通過采用多尺度特征進行對象檢測，在保持YOLO算法檢測速度的前提下，大幅提高了對小對象目標的檢測精度。YOLOv3算法采用9個先驗框，在3種尺度特征圖上分別設置了3個不同的先驗框用于預測，對于416*416的輸入圖片，3種尺度分別為13*13特征圖、26*26特征圖和52*52特征圖，它們分別用于預測大的物體、中等大小物體和小的物體。YOLOv3算法網絡結構如圖2所示。

YOLOv3算法采用了新的網絡結構Darknet-53進行圖像特征提取，借鑒了殘差網絡Residual network的做法，大量使用3*3與1*1卷積層依次連接的形式，并且添加了Shortcut連接，以防止出現過擬合現象。除此之外，YOLOv3的對象分類中采用Logistic函數取代了YOLO算法中采用的Softmax函數，使其能夠支持對多標簽對象的檢測。

藥盒具有豐富的特征信息，包括顏色、紋理、大小以及卷積神經網絡提取的高維特征等，通過這些特征對藥盒識別模型進行訓練。本文針對5種不同種類藥盒，制作PASCAL VOC數據集時一共采集了1500張不同類別、數目、背景和姿態的藥盒圖片，按照3∶1的比例劃分為訓練集和測試集，通過訓練獲取藥盒識別模型。模型訓練參數如表2所示。

表2 模型訓練參數表

YOLOv3損失函數如式(1)所示。

(1)

YOLOv3損失函數第一部分為預測邊界框的中心坐標損失，第二部分為預測邊界框的寬高損失，第三、四部分為預測的置信度損失，第五部分為預測的類別損失。通過反向傳播進行模型訓練以調整權重參數，當總體IOU(預測框和真實框的交并比)值穩定為1，總體損失穩定到0.06以下時結束模型訓練。模型訓練Loss(損失)函數曲線和IOU函數曲線及模型識別定位結果分別如圖3～5所示。

圖3 Loss曲線

圖4 IOU曲線

圖5 藥盒識別效果圖

由Loss曲線可以看出，隨著訓練次數的增加，YOLOv3訓練損失逐漸降低，迭代次數達到5000次之后總體損失趨于穩定并保持在0.06以下。

IOU曲線顯示，隨著迭代次數的增加總體IOU值逐漸上升，迭代次數在1 000～4 000期間IOU值達到1左右但不穩定，迭代次數大于4 000次之后，IOU值穩定為1，表明此時模型訓練精度達到了預定要求。

通過YOLOv3算法可以確定每個藥盒的類別以及定位框的坐標，根據這些定位框信息對藥盒進行ROI(感興趣區域)區域分割，可以將多藥盒的處理轉化為對單個藥盒的處理，另外根據檢測的類別信息選取模板庫中對應的藥盒模板并在后續操作中進行特征提取和特征匹配。藥盒定位是實現姿態估計的前提，本文所采用的姿態估計方法將在下一章具體闡述。

3 姿態估計

根據藥盒定位框進行ROI區域分割后可以對ROI區域和模板圖片進行ORB特征點提取，特征點對可以通過文獻[10]方法很容易求得它們之間的單應矩陣。單應矩陣描述了兩幅圖像共面特征點之間的轉換關系，雖然單應矩陣本身包含有相機的姿態信息，但通過單應矩陣直接分解出的旋轉矩陣和平移矩陣會有多個解[11]，需要通過其他約束條件去除不合理的解，并且獲取的最優解也會存在一定的誤差。基于單應矩陣的位姿求取方法主要包括單應矩陣的辨識，單應矩陣的分解，以及唯一性的確定3個步驟，相對復雜。為了簡化藥盒姿態求取以及避免單應性矩陣分解產生的多解問題，結合EPnP算法和藥盒的固有尺寸信息進行藥盒姿態求取。首先對前述步驟獲取的單應矩陣進行矩陣透視變換獲取模板在目標圖像上的映射，從而獲得包圍目標藥盒的外包框和4個角點的像素坐標。將獲取的目標藥盒4個角點像素坐標作為EPnP算法求取藥盒姿態所需的2D點，根據藥盒的精確尺寸在相機坐標系Z=0平面上構建藥盒平面并作為EPnP求解所需的3D點。上述做法可以極大地減少EPnP算法中的特征點計算數量，加快了算法運行速度。此外，利用單應矩陣通過透視矩陣變換可以在藥盒不全或者部分遮擋的情況下有效地獲取藥盒角點坐標，此方法在復雜場景下具有更強的魯棒性。EPnP求解所需對應點對如圖6所示。

圖6 EPnP求解所需對應點對

3.1 特征提取和特征匹配

ORB特征點由關鍵點和描述子兩部分組成，添加了尺度和旋轉的描述，改進了FAST檢測子不具有方向性的問題，并且通過結合二進制描述子BRIEF(binary robust independent elementary feature)，極大地加快了對于圖像特征的提取速度。除此之外，ORB特征還可以指定最終要提取的特征點數量。

FAST關鍵點通過比較一個像素與其周圍像素值的差值(亮度)來確定，因為只需要比較像素值的大小，所以檢測速度極快，可滿足姿態檢測的實時性要求。FAST關鍵點檢測方法如圖7所示。其實現步驟如下：

步驟1：定位圖像中的一個像素p及其亮度Ip。

步驟2：設定FAST關鍵點檢測閾值T。

步驟3：獲取以像素p為中心，半徑為3的圓上的16個像素亮度值，如圖6所示。

步驟4：若圓上連續N個像素亮度值大于Ip+T或者小于Ip-T則定義為FAST角點(N通常取12，即為FAST-12)。

步驟5：對每個像素進行以上操作。

圖7 FAST特征點

ORB特征點通過構建圖像金字塔描述尺度不變性，通過灰度質心法來實現旋轉描述。灰度質心法定義過程如下：

1)在一個圖像塊B中定義圖像塊的矩為：

(2)

2)通過矩可以找到圖像塊的質心：

(3)

θ=artan(m01,m10)

(4)

BRIEF描述子通過0和1來編碼特征點附近像素的量級關系，BRIEF采用隨機選點的比較方法，速度較快，通過FAST和BRIEF的組合可以非常高效地描述ORB特征點。ORB特征點匹配如圖8所示。

圖8 ORB特征點匹配

特征點描述了模板圖像與目標圖像之間的轉換關系，采用文獻[10]中所提出的方法可以獲取單應性矩陣，根據單應性矩陣的透視矩陣變換可以獲取藥盒平面外包框和4個角點的像素坐標，并且可以有效地克服部分遮擋情況。單應性矩陣透視矩陣變換后的藥盒外包框和4個角點如圖9所示。

圖9 單應矩陣反饋的4個點

3.2 EPnP算法原理

EPnP算法姿態估計的核心思想是利用4個虛擬控制點線性加權地表示真實物體上的各個點在相機坐標系下的坐標。

(5)

藥盒參考坐標系下3D點可用虛擬控制點加權表示，aij為均一重心坐標(homogeneous barycentric coordinates)。

同理虛擬控制點的坐標和3D參考點坐標之間的關系在相機坐標系下可表示為：

(6)

假設3D參考點在像素坐標系的投影為(ui,vi)，則：

(7)

其中：zi為參考點投影深度，A為相機內參矩陣，fu、fv、uc、vc為相機內部參數。由式(7)可得每個參考點和像素坐標點的對應關系為：

(8)

通過4個參考點和像素點對可得到包含8個方程的線性方程組，記為Mx=0，M為8×12的矩陣，x為12×1的向量，包含了4個虛擬控制點在相機坐標系下的非齊次坐標。由于4個參考點在相機坐標系下坐標已知，從而將求解三維到二維的PNP問題轉換成經典的三維到三維的剛體運動問題，從而可求出目標藥盒的旋轉矩陣R和平移矩陣t。

4 實驗結果與分析

為了驗證本算法在藥盒姿態求取中的姿態估計精度，通過結合OptiTrack系統進行了姿態精度對比實驗。OptiTrack系統通過檢測感光Marker點來進行姿態測量，具有亞毫米級的重建精度。本實驗以OptiTrack系統的姿態測量值作為標準值，依次對偏航角、俯仰角、滾轉角進行誤差檢測。通過對比OptiTrack系統的姿態測量值與本算法的測量值并進行分析。圖10為OptiTrack系統，圖11為測量板，圖12為姿態估計效果圖，圖13～圖15分別為偏航角誤差曲線、俯仰角誤差曲線和滾轉角誤差曲線圖。

圖10 OptiTrack系統 圖11 測量板

圖12 姿態估計效果

圖13 偏航角誤差

圖14 俯仰角誤差

圖15 滾轉角誤差

由實驗數據可以看到采用本文提出的算法進行實時姿態估計時，偏航角的誤差在0～90°內的平均誤差在0.5°以內。俯仰角的平均誤差在0～25°內小于0.5°，在25～45°內的平均誤差小于0.8°，滾轉角誤差在0～20°以內平均誤差小于0.5°，在25～45°平均誤差小于0.6°。實驗結果表明本算法具有足夠的姿態估計精度并且可以滿足機械臂藥盒分揀任務需求。

5 結束語

在這項工作中，本文成功基于YOLOv3算法和EPnP算法相結合的多藥盒定位和姿態估計算法實現了藥盒姿態實時估計。實驗表明本算法可以實現快速精準的藥盒定位，藥盒姿態估計精度較高，可以滿足藥房或藥品倉庫中對于藥盒抓取操作的實時性和準確性的要求。