協(xié)作機(jī)械臂末端執(zhí)行器的快速定位算法

喻 靖，江文松，羅 哉，楊 力，周根明

(1.中國計(jì)量大學(xué) 計(jì)量測試工程學(xué)院，浙江 杭州 310018；2.浙江科力車輛控制系統(tǒng)有限公司，浙江 江山 324100)

1 引 言

近年來，協(xié)作機(jī)械臂在測量領(lǐng)域發(fā)展迅速，被廣泛應(yīng)用于室內(nèi)地圖構(gòu)建[1,2]、工業(yè)材料尺寸測量等方面[3]。協(xié)作機(jī)械臂末端執(zhí)行器工作時，當(dāng)前的位姿決定其運(yùn)動方式，進(jìn)行快速準(zhǔn)確的位姿估計(jì)是問題的關(guān)鍵[4]。

協(xié)作機(jī)械臂末端定位方法，包括非合作目標(biāo)與合作目標(biāo)兩類[5,6]。非合作目標(biāo)有幀差法、視覺里程計(jì)方法和視覺慣性融合里程計(jì)方法。幀差法計(jì)算量小，速度快，但是定位誤差大[7]。基于視覺里程計(jì)方法對運(yùn)動物體進(jìn)行定位，隨著運(yùn)動時間的累加，誤差會越來越大[8]。視覺慣性融合里程計(jì)方法引入慣性測量單元進(jìn)行信息融合，解決了誤差累加問題，但難以達(dá)到實(shí)時性要求[9,10]。合作目標(biāo)有自定義圓形標(biāo)簽與方形標(biāo)簽。雷金周[11]采用提取靶標(biāo)參考板圓孔特征中心坐標(biāo)求解位姿信息，通常應(yīng)用于手眼標(biāo)定場合。方形標(biāo)簽AprilTag識別算法可以精確計(jì)算目標(biāo)相對相機(jī)的位姿信息，但算法對高分辨率圖片處理速度有限[12]。

綜上所述，當(dāng)前機(jī)械臂末端定位主要存在處理速度慢，難以達(dá)到實(shí)時性等問題。劉虹等[13]設(shè)計(jì)了一種基于交比不變性的編碼靶標(biāo)，以降低處理時間，前提是處在黑暗環(huán)境的條件下才能快速定位。賈配洋等[14]提出基于AprilTag識別的改進(jìn)算法，對移動目標(biāo)進(jìn)行局部搜索，并結(jié)合Kalman濾波器實(shí)時估計(jì)目標(biāo)下一時刻位置，提升了算法處理速度，僅限于目標(biāo)加速度較小的情況。張堅(jiān)等[15]通過添加圓環(huán)標(biāo)記物，確定包圍AprilTag標(biāo)簽的最小矩形區(qū)域，提高了實(shí)時性。Duan H Y等[16]通過AprilTag標(biāo)簽與視圖分辨率的面積比實(shí)現(xiàn)自動數(shù)字變焦，簡化識別目標(biāo)提高了檢測效率，但對于相機(jī)設(shè)備要求更高。Ren R N等[17]提出了動態(tài)下采樣算法，最后基于AprilTag識別原理設(shè)計(jì)二次檢測算法，提高了識別速度。

針對上述快速定位方法應(yīng)用情況?；诿芪鞲髮W(xué)April實(shí)驗(yàn)室設(shè)計(jì)和改進(jìn)AprilTag檢測算法，被應(yīng)用于高精度機(jī)器人輔助定位方面，具備快速和低誤報(bào)率的優(yōu)點(diǎn)[18]。AprilTag是一種簡化二維碼圖，易獲取，該標(biāo)簽在普通工業(yè)相機(jī)下易于識別與探測，可以計(jì)算標(biāo)簽相對于相機(jī)的位姿信息。

本文提出基于圖像處理方法的AprilTag標(biāo)簽快速定位算法。標(biāo)簽張貼于機(jī)械臂末端，提出下采樣算法，確定目標(biāo)局部搜索范圍。在局部定位圖中，通過幾何約束保留標(biāo)簽邊緣像素點(diǎn)，大幅提升算法處理速度，成功解決了機(jī)械臂末端實(shí)時性定位位姿問題。

2 算法原理

2.1 傳統(tǒng)AprilTag標(biāo)簽識別算法

AprilTag標(biāo)簽的具體識別步驟如下：

(1)灰度圖像的二值化處理[19]。原始測量圖像的分辨率為2 048×2 048像素，令目標(biāo)圖像周圍區(qū)域的像素值為P。為了防止在極值差異較大的圖塊邊界之間出現(xiàn)偽影，將圖像劃分為4×4像素陣列，計(jì)算各圖塊的灰度極值，令

Pmax-Pmin=δ

(1)

式中：Pmax為4×4像素陣列的灰度極大值；Pmin為 4×4 像素陣列的灰度極小值。

當(dāng)δ≥5時，則判定像素對比度明顯，利用像素閾值p′=(Pmax-Pmin)/2來分配該區(qū)域的黑白效果；當(dāng)δ<5時，則判定該區(qū)域像素對比度不足，并刪除該區(qū)域以節(jié)省計(jì)算時間。圖1(a)為灰度圖，二值化后為圖1(b)自適應(yīng)閾值圖像。

圖1 AprilTag檢測器的步驟Fig.1 Steps of the AprilTag detector

(2)自適應(yīng)閾值圖的圖像邊界分割。使用聯(lián)合查找算法[20]對亮像素和暗像素的連通部分分割。遍歷閾值圖上的黑白點(diǎn)，按圖2方式查找，相鄰像素值相等則分配唯一父節(jié)點(diǎn)，得出每個分量唯一的ID。

圖2 連通查找方式Fig.2 Connected search

使用Hash表完成聚類。遍歷閾值圖上黑白點(diǎn)，針對相鄰黑白區(qū)域，將該區(qū)域上的點(diǎn)標(biāo)識為不同聚類。按圖2方法查找相鄰點(diǎn)x0與x1，記錄對應(yīng)像素值和為255的父節(jié)點(diǎn)ID，P[x0]與P[x1]。利用ID值為每個聚類建立索引，按如下約束

(2)

式中：“”為取余符號；常數(shù)s=32；T=8 388 607；m=2 654 435 761 是2～232的近似黃金分割數(shù)，該數(shù)的取值方式讓不同聚類建立的索引值不易重復(fù)。

圖1(c)是聚類后線段分割圖，各聚類用不同顏色表示。

(3)同一聚類點(diǎn)預(yù)處理。將點(diǎn)按其質(zhì)心以角度大小排序。每個聚類包含N個點(diǎn)的有序坐標(biāo)，令質(zhì)心坐標(biāo)為(XC,YC)，定義

(3)

式中：Xmax、Xmin、Ymax、Ymin分別為N個像素點(diǎn)橫、縱坐標(biāo)的最大、最小值；常數(shù)用于調(diào)整像素中心偏差。

根據(jù)公式(4)，依次計(jì)算各像素點(diǎn)(Xi,Yi)與質(zhì)心連接的角度θ。

(4)

式中i=1,2,…,N。

聚類點(diǎn)采用相鄰像素值加權(quán)處理,其中一像素點(diǎn)坐標(biāo)為(Xi,Yi)，令該點(diǎn)權(quán)重值Wi為

(5)

式中：P1、P2為相鄰上、下點(diǎn)的像素值；P3、P4為相鄰左、右點(diǎn)的像素值。

(4)主成分分析(PCA)[21]計(jì)算線擬合。聚類點(diǎn)中依次取n(n·N)個數(shù)據(jù)，如(X1,Y1),(X2,Y2),…,(Xn,Yn)構(gòu)成一個數(shù)據(jù)集，設(shè)擬合后的直線方程為

ax+by+c=0

(6)

式中：a、b、c為待求解未知常數(shù)。

(7)

則該數(shù)據(jù)集的協(xié)方差矩陣為

(8)

通過公式(9)可以求解公式(8)協(xié)方差矩陣的最大特征值對應(yīng)的特征向量，也就是直線方程的系數(shù)a、b，β為擬合直線的法線相對橫坐標(biāo)軸正方向角度。

(9)

擬合直線方程為

(10)

擬合直線誤差的計(jì)算，將該誤差函數(shù)中的峰值標(biāo)識為候選角。令

(11)

式中e為擬合直線誤差大小。

遍歷4個候選角所有排列，四邊形各側(cè)進(jìn)行線擬合，計(jì)算每側(cè)擬合直線誤差。設(shè)

E=e1+e2+e3+e4

(12)

式中：E為四邊形擬合總誤差；e1、e2、e3、e4為四邊形四邊擬合直線誤差。

當(dāng)E為最小擬合誤差，取對應(yīng)4個候選角劃分的四組數(shù)據(jù)點(diǎn)，進(jìn)行四邊形擬合。根據(jù)兩條直線相交求解4個角點(diǎn)像素坐標(biāo)，圖1(d)中紅色框表示擬合候選的四邊形。丟棄不良的四邊形擬合和不可解碼的標(biāo)簽，并輸出有效的標(biāo)簽，如圖1(e)所示。

2.2 識別速度的優(yōu)化

直線擬合算法處理速度慢。針對分辨率為 2 048×2 048 像素的圖片，只保留標(biāo)簽邊緣像素信息，達(dá)到了加快傳統(tǒng)算法檢測速度的目的。

圖片下采樣處理降低分辨率。原始圖片像素點(diǎn)(u,v)的像素值為F(u,v)，用最近鄰插值縮小圖片，確定縮放比例因子K

(13)

式中：sw×sh為原圖分辨率；dw×dh為目標(biāo)圖片分辨率。下采樣后圖片像素點(diǎn)(u,v)的像素值為f(u,v)，計(jì)算公式為

f(u,v)=F[u·(sw/dw), v·(sh/dh)]

(14)

下采樣圖片標(biāo)簽局部定位。本文K取值64，利用OpenCV函數(shù)findCountours，檢測下采樣圖片中標(biāo)簽的大致輪廓，得到4個角點(diǎn)的像素級坐標(biāo)。與K相乘，計(jì)算出原圖中標(biāo)簽的4個角點(diǎn)像素坐標(biāo)，最終確定標(biāo)簽所在區(qū)域。圖3為相機(jī)定位標(biāo)簽局部圖，圖3(a)小傾角下的圖像，記為0°，圖3(b)為標(biāo)簽繞相機(jī)Z軸旋轉(zhuǎn)約45°后的圖像。

圖3 相機(jī)定位標(biāo)簽局部圖Fig.3 Partial view of camera positioning label

(1)改進(jìn)1：局部定位圖中，標(biāo)簽非邊緣內(nèi)部像素點(diǎn)的去除。根據(jù)幾何關(guān)系確定標(biāo)簽內(nèi)部像素點(diǎn)位置，設(shè)內(nèi)部點(diǎn)I，則滿足如下關(guān)系

SABCD=SABI+SBCI+SCDI+SADI

(15)

式中：A、B、C、D為下采樣局部定位確定的4個角點(diǎn)；SABI、SBCI、SCDI、SADI分別為點(diǎn)I與四邊組成的三角形像素面積；SABCD為四邊形像素面積。

內(nèi)部點(diǎn)I的位置需要進(jìn)一步約束。設(shè)定閾值h，點(diǎn)I與標(biāo)簽四邊的垂直距離應(yīng)大于該閾值，具體約束條件為

(16)

式中LAB、LBC、LCD、LAD分別為標(biāo)簽四邊的像素長度。

標(biāo)簽非邊緣內(nèi)部像素區(qū)域?yàn)辄c(diǎn)I的有效范圍。將該區(qū)域設(shè)置像素值為255，二值化時該區(qū)域?yàn)榈蛯Ρ榷?，刪除該區(qū)域。改進(jìn)1中將標(biāo)簽非邊緣內(nèi)部像素點(diǎn)去除，如圖4。

圖4 標(biāo)簽內(nèi)部像素點(diǎn)去除Fig.4 Removal of pixels inside the label

(2)改進(jìn)2：局部定位圖中，標(biāo)簽非邊緣外部像素點(diǎn)的去除。確定標(biāo)簽外部像素點(diǎn)位置，設(shè)外部一點(diǎn)O，則滿足如下關(guān)系

SABO+SBCO+SCDO+SADO>SABCD

(17)

式中SABO、SBCO、SCDO、SADO分別為點(diǎn)O與四邊組成的三角形像素面積。

標(biāo)簽邊緣像素信息的保留，需對外部點(diǎn)O進(jìn)一步約束。令

ΔS=SABO+SBCO+SCDO+SADO-SABCD

(18)

圖5中外部點(diǎn)O與標(biāo)簽的兩種位置關(guān)系影響ΔS的值。由幾何關(guān)系可知，圖5(a)情形1中面積差ΔS為△ADO面積的兩倍，圖5(b)情形2中ΔS為△ADO與△CDO面積和的兩倍。

圖5 點(diǎn)O與四邊形ABCD的位置關(guān)系Fig.5 Positional relationship between point O and quadrilateral ABCD

通過限制ΔS的大小，合理保留標(biāo)簽邊緣像素信息。令

ΔS>4·0.5·max(LAB,LBC,LCD,LAD)·h

(19)

式中h為設(shè)定閾值。

保證標(biāo)簽的正常檢測，h大小適當(dāng)可調(diào)。確定了點(diǎn)O的有效范圍后，后續(xù)圖像處理方法同第2.2節(jié)中的(1)所述方法。

改進(jìn)2中將標(biāo)簽非邊緣外部像素點(diǎn)去除,見圖6。對于不同角度標(biāo)簽提速效果差別大。為了進(jìn)一步減少搜索空間，結(jié)合兩種方法，將標(biāo)簽非邊緣內(nèi)、外像素點(diǎn)同時去除，基于幾何學(xué)提取感興趣區(qū)域(region of interest,ROI)，圖7為提取的ROI標(biāo)簽內(nèi)外像素點(diǎn)去除效果圖。

圖6 標(biāo)簽外部像素點(diǎn)去除Fig.6 Removal of pixels outside the label

圖7 標(biāo)簽內(nèi)外像素點(diǎn)去除Fig.7 Removal of pixels inside and outside the label

3 實(shí)驗(yàn)與結(jié)果分析

3.1 角點(diǎn)檢測實(shí)驗(yàn)

本實(shí)驗(yàn)以機(jī)械臂為平臺，在機(jī)械臂末端激光掃描儀上張貼標(biāo)簽，標(biāo)簽規(guī)格大小為7 cm×7 cm，每小格1 cm×1 cm的誤差在1 μm之內(nèi)，滿足實(shí)驗(yàn)所需要求。使用相機(jī)型號為TS4MCL-180M/C，分辨率為2 048×2 048像素，幀率大小為149幀/s。

在標(biāo)簽距離相機(jī)55～65 cm、75～85 cm、95～105 cm、115～125 cm處，標(biāo)簽任意擺放，角度范圍為0°～25°，所指角度為相機(jī)光軸與垂直標(biāo)簽方向上的夾角。從不同的角度位置各拍攝了12組，1組包含10幀標(biāo)簽位置相同的圖片，圖8為部分實(shí)驗(yàn)圖片。

圖8 相機(jī)拍攝各角度實(shí)驗(yàn)圖Fig.8 Experimental image of camera shooting at various angles

閾值h的確定將影響本文算法角點(diǎn)檢測相對傳統(tǒng)算法的誤差。設(shè)

R=r1+r2+r3+r4

(20)

式中：R為一幀圖片的相對像素誤差；r1、r2、r3、r4分別為兩種算法檢測后每個角點(diǎn)的像素距離。

最大像素誤差值為

Rmax=max(R1,R2,…,Rk)

(21)

式中R1,R2,…,Rk為同一距離上k幀圖片相對像素誤差。

圖9為角點(diǎn)檢測相對誤差與閾值h之間的關(guān)系。

圖9 h與Rmax的關(guān)系Fig.9 The relationship between h and Rmax

從圖9中看出，h值越大，誤差最大值越小，h≥14時，誤差最大值趨近平穩(wěn)。

取h=14，實(shí)驗(yàn)距離相機(jī)55～125 cm處拍攝大量的圖片，計(jì)算各幀圖片角點(diǎn)檢測相對誤差的值，實(shí)驗(yàn)結(jié)果如圖10所示。結(jié)果顯示相對傳統(tǒng)算法角點(diǎn)檢測誤差保持在0.1 pixel以內(nèi)，而本文算法角點(diǎn)檢測相對誤差平均值低于0.1 pixel。當(dāng)閾值h=14時，本文算法保證了角點(diǎn)檢測精度。

圖10 角點(diǎn)檢測相對誤差Fig.10 Relative error of corner detection

3.2 重復(fù)性檢測實(shí)驗(yàn)

通過c++的庫函數(shù)getTickCount()計(jì)算AprilTag標(biāo)簽檢測角點(diǎn)總耗時。檢測標(biāo)簽相同位置圖片，計(jì)算各種算法檢測耗時，實(shí)驗(yàn)結(jié)果如圖11所示，縱坐標(biāo)軸為以10為底的對數(shù)坐標(biāo)軸，圖12、圖13、圖14采用相同規(guī)則設(shè)定縱坐標(biāo)軸。

圖11 標(biāo)簽相同位置重復(fù)性檢測實(shí)驗(yàn)Fig.11 Repeatability test experiment at same distance

由圖11數(shù)據(jù)可知，所有算法檢測耗時的偏差率都在6.8%以內(nèi)，本文算法耗時偏差在2 ms以內(nèi)，算法檢測耗時具有穩(wěn)定性。

3.3 不同環(huán)境下標(biāo)簽的檢測實(shí)驗(yàn)

選用距離相機(jī)55～65 cm、75～85 cm、95～105 cm、115～125 cm處12個拍攝不同位置標(biāo)簽的圖片。取相同位置圖片檢測耗時平均值，圖12中數(shù)據(jù)為各檢測耗時平均值。結(jié)果顯示文獻(xiàn)[18]中檢測耗時在2 000 ms以上，實(shí)時性差。文獻(xiàn)[14,15]方法在標(biāo)簽處于不同角度下，其檢測耗時波動大，最長耗時達(dá)300 ms/幀。文獻(xiàn)[17]中檢測耗時穩(wěn)定，實(shí)時性較好。針對改進(jìn)1與改進(jìn)2進(jìn)行耗時檢測，隨著標(biāo)簽角度的不同，降低耗時的效果不同，但是都優(yōu)于文獻(xiàn)[14,15]中的檢測方法。本文算法結(jié)合的改進(jìn)1與改進(jìn)2，檢測耗時都優(yōu)于對比文獻(xiàn)中的算法檢測，檢測效果可觀，標(biāo)簽角度影響較小，其耗時在25 ms/幀以下。

圖12 各距離下不同位置檢測實(shí)驗(yàn)Fig.12 Different position detection experiments at various distances

用相同距離下不同位置標(biāo)簽的檢測耗時取平均值，分析標(biāo)簽距離的不同，其檢測時間的關(guān)系。由圖13可以看出，文獻(xiàn)[18]中算法耗時維持穩(wěn)定，耗時較長。文獻(xiàn)[17]中算法檢測速度快，但是隨著距離的增加檢測耗時增加。文獻(xiàn)[14,15]中算法隨距離增加，耗時降低。本文算法隨距離增加，耗時降低。

圖13 平均檢測耗時與距離的關(guān)系Fig.13 Relationship between average detection time and distance

圖14 動態(tài)驗(yàn)證實(shí)驗(yàn)Fig.14 Dynamic verification experiment

3.4 動態(tài)驗(yàn)證實(shí)驗(yàn)

末端執(zhí)行器在距離相機(jī)55～125 cm處做無規(guī)則運(yùn)動，保存時間序列圖片。由圖14中本文算法耗時曲線可知，每幀耗時均小于20 ms，因此可以實(shí)現(xiàn)40幀/s的檢測速度。

4 結(jié) 論

傳統(tǒng)AprilTag標(biāo)簽檢測算法速度低下，本文在保證標(biāo)簽角點(diǎn)檢測精度的同時提出了一系列改進(jìn)算法檢測速度的方法。通過局部定位，去除標(biāo)簽內(nèi)外非邊緣像素，進(jìn)行精確快速的檢測AprilTag標(biāo)簽角點(diǎn)，然后采用EPnP+LM算法對3D-2D點(diǎn)進(jìn)行位姿解算優(yōu)化[22,23]，通過最小化重投影誤差得出解決方案。實(shí)驗(yàn)證明機(jī)械臂末端到相機(jī)的工作距離，即55～125 cm范圍內(nèi)，本文算法1幀耗時在25 ms之內(nèi)，檢測速率大于40幀/s，且隨工作距離的增加，其檢測速率越快，可實(shí)現(xiàn)對機(jī)械臂末端的實(shí)時定位。