基于YOLOv3和EPnP算法的多藥盒姿態(tài)估計

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

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

0 引言

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

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

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

圖2 YOLOv3算法網(wǎng)絡(luò)結(jié)構(gòu)圖

1 系統(tǒng)結(jié)構(gòu)及原理

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

圖1 系統(tǒng)流程圖

表1 工況試驗設(shè)置及結(jié)果

2 藥盒定位

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

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

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

表2 模型訓(xùn)練參數(shù)表

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

(1)

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

圖3 Loss曲線

圖4 IOU曲線

圖5 藥盒識別效果圖

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

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

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

3 姿態(tài)估計

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

圖6 EPnP求解所需對應(yīng)點對

3.1 特征提取和特征匹配

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

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

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

步驟2：設(shè)定FAST關(guān)鍵點檢測閾值T。

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

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

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

圖7 FAST特征點

ORB特征點通過構(gòu)建圖像金字塔描述尺度不變性，通過灰度質(zhì)心法來實現(xiàn)旋轉(zhuǎn)描述。灰度質(zhì)心法定義過程如下：

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

(2)

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

(3)

θ=artan(m01,m10)

(4)

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

圖8 ORB特征點匹配

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

圖9 單應(yīng)矩陣反饋的4個點

3.2 EPnP算法原理

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

(5)

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

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

(6)

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

(7)

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

(8)

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

4 實驗結(jié)果與分析

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

圖10 OptiTrack系統(tǒng) 圖11 測量板

圖12 姿態(tài)估計效果

圖13 偏航角誤差

圖14 俯仰角誤差

圖15 滾轉(zhuǎn)角誤差

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

5 結(jié)束語

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