面向板料精準(zhǔn)堆垛的線特征位姿標(biāo)定方法

方歸，劉懷廣,b*

面向板料精準(zhǔn)堆垛的線特征位姿標(biāo)定方法

方歸a，劉懷廣a,b*

（武漢科技大學(xué) a.冶金裝備及其控制教育部重點實驗室 b.機械傳動與制造工程湖北省重點實驗室，武漢 430081）

板料堆垛是現(xiàn)代流程工業(yè)生產(chǎn)中常見的生產(chǎn)環(huán)節(jié)，其堆垛質(zhì)量影響著包裝和運輸安全。針對多邊形板料自動堆垛中機械手抓取和對齊問題，提出一種基于網(wǎng)格篩選的線特征相機位姿標(biāo)定方法，給出空間特征點的選擇標(biāo)準(zhǔn)，解決因板料紋理不足帶來的點特征精度下降的問題。首先通過檢測特征點來表示待匹配線，然后利用基于三角網(wǎng)格的線特征構(gòu)型篩選算法，獲得精確的匹配線；再利用單應(yīng)性矩陣求解獲得轉(zhuǎn)換矩陣，最后通過轉(zhuǎn)換在線板料圖像，獲得實際抓取點和轉(zhuǎn)換角度。通過桌面板料抓取實驗和現(xiàn)場驗證，重投影誤差不超過2像素，1 m板料的堆垛誤差為0.5 mm，證明了此方法在板料自動堆垛過程中針對特征檢測和位姿估計問題的有效性。

位姿估計；特征檢測篩選；網(wǎng)格劃分；機械臂抓取堆垛

近年來，機器人在流程工業(yè)中應(yīng)用越來越廣泛，如鋼板切割、玻璃制造等。這些行業(yè)在生產(chǎn)中，通常需要對產(chǎn)品進(jìn)行抓取，并按要求堆垛，以便進(jìn)行包裝和運輸[1]。將視覺與機械臂結(jié)合是實現(xiàn)高精度抓取堆垛任務(wù)的主要研究方向，而視覺抓取堆垛的核心就是板料的位姿估計和堆垛匹配問題。

對于工業(yè)環(huán)境下的位姿估計問題，主要采用基于模板匹配和特征映射2種方法[2]。在基于模板匹配的方法中，每個目標(biāo)都會選擇某具有代表性的物體圖像作為模板，然后通過掃描特征匹配測試找到姿態(tài)。雖然該方法能夠較好地響應(yīng)弱紋理目標(biāo)，但是其精度受到光照、遮擋和視角變換的影響較大，在實際應(yīng)用中需要結(jié)合深度學(xué)習(xí)和傳感器融合等技術(shù)來提高其準(zhǔn)確性。基于特征映射的方法又分為基于點特征的目標(biāo)位姿估計問題，即PnP問題（Perspective-n- Point）[3]，以及基于線特征的目標(biāo)位姿估計問題，即PnL問題（Perspective-n-Line）[4]。前者根據(jù)已知特征點在目標(biāo)坐標(biāo)系（通常被稱作世界坐標(biāo)系）和圖像坐標(biāo)系下的不同表達(dá)，利用透視投影關(guān)系求得目標(biāo)在這2個坐標(biāo)系之間的變換關(guān)系。對于表面光滑、特征點較少的弱紋理物體，使用特征點的映射很難建立物體的位姿與特征之間的表征關(guān)系，采用基于線特征的目標(biāo)位姿估計方法具有更好的魯棒性，且特征數(shù)目和特征奇異值對它的影響較小。由此，文中利用PnL方法研究像素坐標(biāo)系與機械臂坐標(biāo)系之間的映射關(guān)系。

目前，利用PnL方法解決坐標(biāo)系變換問題的方法有很多種。其中，直接線性變換（Direct Linear Transformation, DLT）方法[5]是一種非常直接的非迭代方法，在有足量空間參考直線情況下，其求解精度較高且簡單有效，但在目標(biāo)特征數(shù)目較少或紋理不足時其求解效果不理想。為了提高PnL算法的適用性，Mirzaei等[6]通過Cayley變換方式旋轉(zhuǎn)矩陣，然后通過矩陣分解和合成的方式，將PnL位姿求解問題轉(zhuǎn)換為最優(yōu)化問題，最后通過矩陣合成技術(shù)求解最優(yōu)化問題。由于Mirzaei的方法采用Cayley參數(shù)表示旋轉(zhuǎn)矩陣，在求解過程中容易出現(xiàn)矩陣奇異值，導(dǎo)致求解的穩(wěn)定性不好。與此同時，劉昶等[7]證明PnL問題解的數(shù)量與相機光心和直線的相對位置有關(guān)，該結(jié)論為應(yīng)用直線特征估計單目視覺位姿提供了理論依據(jù)。

文中針對工業(yè)環(huán)境下帶鋼板料紋理不明顯、特征少且對堆垛精度有較高要求的條件，提出基于線特征的板料位姿評估方法，以減小傳統(tǒng)基于點特征進(jìn)行位姿估計帶來的轉(zhuǎn)換誤差，并通過搭建機械臂抓取實驗平臺，驗證位姿的估計效果。

1 板料抓取任務(wù)模型

如圖1所示，帶鋼經(jīng)過開卷后，沿著輥道進(jìn)入切割工位。切割機由支架支撐，根據(jù)排樣切割出指定形狀。通過相機對切割后的板料進(jìn)行成像處理，并計算抓取位置和相對于第1塊板料的位姿轉(zhuǎn)換信息，然后驅(qū)動機器臂進(jìn)行抓取，并根據(jù)位姿信息轉(zhuǎn)換后堆垛。

圖1 板料堆垛過程模型

1.1 坐標(biāo)轉(zhuǎn)換

在該模型中，主要存在4個坐標(biāo)系：圖像坐標(biāo)系p-、相機坐標(biāo)系c-ccc、世界坐標(biāo)系w-www和機械手末端坐標(biāo)系t-ttt。其中，世界坐標(biāo)系以機器臂的底座為原點。機器臂抓取過程主要包括三次坐標(biāo)轉(zhuǎn)換。

1）相機坐標(biāo)到圖像坐標(biāo)，該過程為小孔成像模型，見式（1）。

式中：為相機焦距；d、d為單個像素在或方向的實際物理尺寸；為2個方向的尺度偏差；0、0為相機坐標(biāo)系原點在成像平面的投影坐標(biāo)。以上參數(shù)組成了相機的內(nèi)參矩陣1，該矩陣通過張氏標(biāo)定[8]來獲得。

2）從世界坐標(biāo)轉(zhuǎn)換到相機坐標(biāo)，該過程為剛體變換過程，見式（2）。

式中：3×3為世界坐標(biāo)系相對于相機坐標(biāo)系的旋轉(zhuǎn)矩陣；3×1為平移矩陣，它們通過組成4×4矩陣的形式表述外參矩陣2。在此模型中，因為相機和機器臂位置都不發(fā)生變化，因此2相對固定，主要通過P3L算法[9]來求解2。

3）從世界坐標(biāo)系轉(zhuǎn)換到機械手末端坐標(biāo)系，該過程稱為剛體變換過程，主要通過機器臂的示教來完成，見式（3）。

整個模型的算法流程如圖2所示。

圖2 算法流程

Fig.2 Algorithm flow

從圖2中可以看出，小孔成像模型1的精度由相機本身決定，機械手末端坐標(biāo)到世界坐標(biāo)的轉(zhuǎn)換矩陣3的精度由機械手示教精度決定，因此從世界坐標(biāo)到相機坐標(biāo)的轉(zhuǎn)換矩陣2很大程度上決定了堆垛精度。

1.2 P3L算法

本文研究位姿的估計方法針對特征數(shù)目較少的弱紋理目標(biāo)，采用一種基于線特征的P3L（Perspective-3-Line）算法[10]，利用世界坐標(biāo)系中的3條參考線和像平面上的投影來求解轉(zhuǎn)換矩陣。建立同一塊板料在機械臂坐標(biāo)系和像素坐標(biāo)系下的不同表達(dá)，在完成線特征匹配后，便可以通過位姿估計構(gòu)建2個坐標(biāo)系之間的聯(lián)系。

如圖3所示，世界坐標(biāo)通過旋轉(zhuǎn)（Rotation）、平移（Translation）變換為像素平面坐標(biāo)。AB=(AB,A,B)，表示一條世界坐標(biāo)系下的線特征。AB表示這個線特征的標(biāo)準(zhǔn)化向量。A、B表示該線特征的板料角點。AB=(ab,a,b)，表達(dá)該線在像素坐標(biāo)系下的特征。根據(jù)直線AB在2個坐標(biāo)系的約束條件，將2的旋轉(zhuǎn)矩陣參數(shù)化，見式（4）。

圖3 線特征的匹配關(guān)系

Fig.3 Matching relation of line features

式（3）中平移向量可以通過平面特征點ab垂直于求解得到，見式（5）。

式中：為像素平面的法向量。

2 基于網(wǎng)格篩選的線特征獲取

空間中的線由2個空間點來確定，因此首先需要獲得待抓取平面的關(guān)鍵點。根據(jù)多邊形板料的邊緣線性關(guān)系，把板料邊緣角點作為關(guān)鍵點來求解線特征，從而將線特征獲取問題轉(zhuǎn)換為關(guān)鍵點提取問題，以提高求解精度。

2.1 關(guān)鍵點提取

式中：為像素位置；P為ORB算法處理該區(qū)域中心像素點的閾值。

完整的關(guān)鍵點提取流程如圖4所示。這種自適應(yīng)的閾值設(shè)置方法解決了整幅圖像因閾值取值經(jīng)驗化所致的關(guān)鍵點漏檢問題，最終關(guān)鍵點提取結(jié)果如圖5所示。

2.2 基于網(wǎng)格篩選的線特征評價

在完成關(guān)鍵點的提取后，可任取2個關(guān)鍵點組成線特征，結(jié)合示教得到它在機械臂坐標(biāo)系下的物理坐標(biāo)，即可求解抓取平面與機械臂之間的轉(zhuǎn)換關(guān)系。由于P3L算法求解與選擇空間線的構(gòu)型緊密相關(guān)[13]，

圖4 關(guān)鍵點提取流程

圖5 ORB關(guān)鍵點檢測結(jié)果

P3L求解的精度會隨著選擇線特征的交點數(shù)量及它們在空間內(nèi)的位置關(guān)系而變化，因此選擇合適的關(guān)鍵點組成線特征十分重要。這里將關(guān)鍵點集劃分為三角網(wǎng)格，并對不同網(wǎng)格進(jìn)行評價，選出最合適的空間線構(gòu)型。

針對提取的關(guān)鍵點，可以利用逐點插入法形成三角網(wǎng)格。如果不加以限制，則會使網(wǎng)格呈幾何級數(shù)增長。為了保證劃分的三角網(wǎng)格最大化和具有唯一性，這里引入空圓特性，即三角形的外接圓內(nèi)部不再包含任意一個其他三角形頂點，則該三角網(wǎng)格具有空圓特性（如圖6中的△、△）?？請A特性保證了三角網(wǎng)格的唯一性，并保證網(wǎng)格內(nèi)每個角盡可能大，使得該三角網(wǎng)格對應(yīng)的線特征構(gòu)型交點數(shù)量最多，且空間位置最理想。

圖6 平面點集的2種劃分方式

針對提取的關(guān)鍵集形成后的三角形，利用空圓特性檢測，剔除公共邊，形成最理想的三角網(wǎng)格劃分方案，如圖7所示。以4個平面點集{1,2,3,4}為例，具體步驟如下。

1）設(shè)計任意的大三角△，包含所有待處理的平面點集{1,2,3,4}，將所有點當(dāng)作待插入點逐個插入。

3) 更新:經(jīng)過分裂步驟產(chǎn)生子集的某些整體特征可能與原始數(shù)據(jù)并不一致，為了保持?jǐn)?shù)控機床熱誤差數(shù)據(jù)的這些整體特征，需要一個更新的過程。將更新過程用算子U來代替，其過程為：

2）當(dāng)插入第1個點1時，直接與△各頂點相連。

3）在插入2點時，檢測已形成的三角形的外接圓是否包含2點，并將包含2的外接圓對應(yīng)的三角形的公共邊刪除，然后以2點為中心與包絡(luò)它的多邊形1各頂點相連。

4）在插入3、4時，各重復(fù)1次步驟3），當(dāng)所有點集都被檢測時，刪除包含大三角形頂點的全部三角邊。

圖7 逐點插入法生成網(wǎng)格

在劃分過程中，每生成1個網(wǎng)格便提取其3個頂點1(1,1)、2(2,2)、3(3,3)，并計算該網(wǎng)格的面積，見式（7）。

對所有關(guān)鍵點進(jìn)行網(wǎng)格劃分完畢后，提取面積最大的三角網(wǎng)格作為目標(biāo)提取線特征Smax，如圖8中的紅色三角網(wǎng)格所示。由于該算法劃分出的三角網(wǎng)格都具有使最小角最大化的特性，因此該目標(biāo)網(wǎng)格最接近正三角形，是構(gòu)型最飽滿的三角形，它對應(yīng)的3條線特征最滿足P3L算法精度的特征。

在提取最大三角形后，利用三角形的3條邊形成標(biāo)定平面，建立像素坐標(biāo)和物理坐標(biāo)的轉(zhuǎn)換模型，見式（3），并結(jié)合式（5）完成相機的姿態(tài)標(biāo)定。

3 結(jié)果與分析

3.1 位姿精度比較

為了研究直線構(gòu)型對位姿估計精度的影響，這里設(shè)計了1組對照實驗。以PNP算法求解得到的旋轉(zhuǎn)矩陣為對照，對各種構(gòu)型情況下的P3L算法求解結(jié)果進(jìn)行比較，如圖9所示。為了保證結(jié)果的魯棒性，先對某圖像進(jìn)行足量的特征提取，然后使用PNP算法[14]求解這些特征點的位姿，得到旋轉(zhuǎn)矩陣，把該結(jié)果當(dāng)成近似正確的位姿解，記作true。針對每種構(gòu)型，使用P3L算法計算得到相應(yīng)的旋轉(zhuǎn)矩陣，記作est。通過比較旋轉(zhuǎn)矩陣間歐氏距離的模（即矩陣差值的范數(shù)）來度量不同構(gòu)型下的位姿精度，模越小，表明越接近PNP算法的近似解，對應(yīng)的P3L算法位姿精度越高，見式（8）。

由表1的結(jié)果可知，P3L算法求解位姿的精度隨著線特征在空間中交點數(shù)目的減少而降低。當(dāng)線特征構(gòu)型盡可能接近正三角形時，位姿精度最高，符合通過網(wǎng)格篩選算法提取線特征的要求。

在此利用前文方法采集的數(shù)據(jù)（圖10），藍(lán)色三角形為經(jīng)過構(gòu)型篩選的3條線特征，把它們作為輸入進(jìn)行基于線特征的位姿估計，黃色角點是隨機挑選的特征點且基本滿足奇異值和數(shù)量要求，把它們作為輸入進(jìn)行基于點特征的位姿估計。使用P3L算法和傳統(tǒng)的PNP算法分別對它們在位姿估計精度誤差方面進(jìn)行分析和比較。

表1 位姿結(jié)果比較

Tab.1 Comparison of pose results

圖10 點線特征處理結(jié)果

這里采用的評價標(biāo)準(zhǔn)為重投影誤差[15]，將兩者估計得到的位姿結(jié)果帶回原始的投影公式，重新計算板料角點從世界坐標(biāo)系投影到像素坐標(biāo)系的新的像素坐標(biāo)，即最開始采用ORB算法提取到的關(guān)鍵點坐標(biāo)，通過比較重投影誤差來評價2種方法的效率和精度，見表2。

將已知世界坐標(biāo)系點重投影為像素坐標(biāo)系點的表達(dá)式，見式（9）。

式中：為重投影前像素坐標(biāo)系的二維坐標(biāo)。

由表2和圖11可知，采用P3L算法，對角點的重投影誤差為1.17像素；采用PNP算法，對角點的重投影誤差為3.66像素。由此可見，P3L算法在弱紋理目標(biāo)特征數(shù)目較少的情況下相較于PNP算法，其精度更高、實用性更強。

表2 2種算法重投影結(jié)果及誤差

Tab.2 Projection results and errors of two algorithms

圖11 重投影效果比較

3.2 機械臂抓取堆垛驗證

如圖12所示，通過任意放置的板料來模擬實際生產(chǎn)線上的場景。提取板料在像素坐標(biāo)系中各個特征角點后，使用Dobot Magician機械臂自帶的示教功能獲取各角點在世界坐標(biāo)系中的坐標(biāo)。為了讓機械臂實現(xiàn)精確的堆垛功能，還需要獲得板料在世界坐標(biāo)系內(nèi)擺放位置的基本信息。對采集的圖樣進(jìn)行二值化處理后，提取板料的輪廓信息，進(jìn)行簡單的輪廓篩選后，便可將板料進(jìn)行分割和定位。使用最小矩形包絡(luò)算法將所有板料的抓取基準(zhǔn)進(jìn)行統(tǒng)一?？紤]到實際場景中板料面積較大，抓取后上下晃動會影響堆疊精度的問題，以機械臂的抓取點為板料的質(zhì)心，使用圖像矩的方法對圖像像素進(jìn)行加權(quán)平均，從而找到板料的質(zhì)心。

將得到的堆垛信息經(jīng)旋轉(zhuǎn)平移變換，變換為世界（機械臂）坐標(biāo)系下的坐標(biāo)，如表3所示。

圖12 板料堆垛信息獲取

表3 堆垛信息變換

Tab.3 Stacking information transformation

在完成基本數(shù)據(jù)處理工作后，通過調(diào)用機械臂的動態(tài)鏈接庫，將數(shù)據(jù)全部輸入機械臂的控制程序中，執(zhí)行板料的抓取堆垛工作。桌面機器臂算法驗證過程如圖13所示。

圖13 桌面機械臂抓取實驗

3.3 生產(chǎn)現(xiàn)場實驗驗證

根據(jù)文中算法的流程，類比實驗中的亞克力板料及桌面型機械臂，開發(fā)了一套板料堆垛的機器人控制系統(tǒng)，并應(yīng)用于浙江某企業(yè)，如圖14所示。結(jié)果表明，在板寬為1 m的情況下，堆垛精度在0.5 mm以下，滿足生產(chǎn)要求，驗證了文中算法的有效性。

4 結(jié)論

在生產(chǎn)線上，板料紋理特征少且取樣背景單一的特點決定了精準(zhǔn)求解此類目標(biāo)位姿精度的難度。結(jié)合實際生產(chǎn)需求，提出了只需要較少特征數(shù)目就能準(zhǔn)確估計位姿的方法，在此基礎(chǔ)上對平面點集數(shù)據(jù)進(jìn)行了網(wǎng)格劃分處理，保證了位姿求解的穩(wěn)定性。經(jīng)實驗驗證，該方法相較于傳統(tǒng)方法，在解決板料堆垛問題方面具有更好的效果，且堆垛精度滿足生產(chǎn)需求。在后續(xù)工作中，針對重疊遮擋對象的分割，進(jìn)一步研究解決三維空間復(fù)雜目標(biāo)的堆垛問題。

[1] 袁浩. 機器人在物流倉儲中的應(yīng)用現(xiàn)狀與發(fā)展趨勢分析[J]. 物流技術(shù), 2022, 41(12): 11-14.

YUAN H. Current Status and Development Trend of Robot Application in Logistics and Warehousing[J]. Logistics Technology, 2022, 41(12): 11-14.

[2] 周芮, 劉延芳, 齊乃明, 等. 面向空間應(yīng)用的視覺位姿估計技術(shù)綜述[J]. 光學(xué)精密工程, 2022, 30(20): 2538-2553.

ZHOU R, LIU Y F, QI N M, et al. Overview of Visual Pose Estimation Methods for Space Missions[J]. Optics and Precision Engineering, 2022, 30(20): 2538-2553.

[3] PAN S Y, WANG X M. A Survey on Perspective-n-Point Problem[C]// 2021 40th Chinese Control Conference (CCC). IEEE, 2021: 2396-2401.

[4] YU Q D, XU G L, WANG Z S, et al. An Efficient and Globally Optimal Solution to Perspective-n-Line Problem[J]. Chinese Journal of Aeronautics, 2022, 35(3): 400-407.

[5] ABDEL-AZIZ Y, KARARA H, HAUCK M. Direct Linear Transformation from Comparator Coordinates into Object Space Coordinates in Close-Range Photogrammetry[J]. Photogrammetric Engineering & Remote Sensing, 2015, 81(2): 103-107.

[6] MIRZAEI F M, ROUMELIOTIS S I. Globally Optimal pose Estimation from Line Correspondences. In: IEEE International Conference on Robotics and Automation, Shanghai, China: IEEE Press, 2011: 5581-5588.

[7] 劉昶, 朱楓, 歐錦軍. 基于三條相互垂直直線的單目位姿估計[J]. 模式識別與人工智能, 2012, 25(5): 737-744.

LIU C, ZHU F, OU J J. Monocular Pose Determination from Three Perpendicular Lines[J]. Pattern Recognition and Artificial Intelligence, 2012, 25(5): 737-744.

[8] ZHANG Z. A Flexible New Technique for Camera Calibration[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence, 2000, 22(11): 1330-1334.

[9] XU C, ZHANG L L, CHENG L, et al. Pose Estimation from Line Correspondences: A Complete Analysis and a Series of Solutions[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence, 2017, 39(6): 1209-1222.

[10] 王平, 何衛(wèi)隆, 張愛華, 等. EPnL: 一種高效且精確的PnL問題求解算法[J]. 自動化學(xué)報, 2022, 48(10): 2600-2610.

WANG P, HE W L, ZHANG A H, et al. EPnL: An Efficient and Accurate Algorithm to the PnL Problem[J]. Acta Automatica Sinica, 2022, 48(10): 2600-2610.

[11] RUBLEE E, RABAUD V, KONOLIGE K, et al. ORB: An Efficient Alternative to SIFT or SURF[C]// 2011 International Conference on Computer Vision. IEEE, 2011: 2564-2571.

[12] OTSU N. A Threshold Selection Method from Gray-Level Histograms[J]. IEEE Transactions on Systems, Man, and Cybernetics, 1979, 9(1): 62-66.

[13] 趙汝進(jìn), 張啟衡, 左顥睿, 等. 一種基于直線特征的單目視覺位姿測量方法[J]. 光電子激光, 2010, 21(6): 894-897.

ZHAO R J, ZHANG Q H, ZUO H R, et al. A Mono-Vision Method of Measuring Pose Based on Line Features[J]. Journal of Optoelectronics Laser, 2010, 21(6): 894-897.

[14] LEPETIT V, MORENO-NOGUER F, FUA P. EPnP: An Accurate O(n) Solution to the PnP Problem[J]. International Journal of Computer Vision, 2009, 81(2): 155-166.

[15] 陳天擇, 葛寶臻, 羅其俊. 重投影優(yōu)化的自由雙目相機位姿估計方法[J]. 中國光學(xué), 2021, 14(6): 1400-1409. CHEN T Z, GE B Z, LUO Q J. Pose Estimation for Free Binocular Cameras Based on Reprojection Error Optimization[J]. Chinese Optics, 2021, 14(6): 1400-1409.

Line Feature Pose Calibration Method for Accurate Sheet Stacking

FANG Guia,LIU Huaiguanga,b*

(a. Key Laboratory of Metallurgical Equipment and Its Control, Ministry of Education, b. Hubei Provincial Key Laboratory of Mechanical Transmission and Manufacturing Engineering, Wuhan University of Science and Technology,Wuhan 430081, China)

Sheet stacking is a common production link in the industrial production of modern process, and its stacking quality affects packaging and transportation safety. Aiming at the problem of robot arm grasping and alignment in automatic stacking of polygonal sheets, the work aims to propose a method for calibrating the pose of line feature camera based on mesh screening and put forward the selection criteria of spatial feature points to overcome the problem of point feature accuracy decline caused by insufficient sheet texture. Firstly, the line to be matched was represented by detecting feature points, and then the accurate matching line was obtained by the line feature configuration screening algorithm based on triangular mesh. Next, the transformation matrix was obtained by solving homography matrix, and finally the actual grasping point and transformation angle were obtained by transforming the online sheet image. Through the desktop sheet grabbing experiment and field verification, the error of re-projection was less than 2 pixels, and the error of 1 meter sheet stacking was 0.5 mm. It is proved that this method is effective for feature detection and pose estimation in the process of automatic stacking of sheets.

pose estimation; feature detection and screening; grid division; mechanical arm grasping and stacking

TB485.3

A

1001-3563(2024)09-0185-08

10.19554/j.cnki.1001-3563.2024.09.024

2023-10-23

國家自然科學(xué)基金（52272377）