基于區(qū)域點云的工件位姿計算方法

樂 英，趙志成

（華北電力大學(xué)能源動力與機械工程學(xué)院，河北保定071000）

隨著工業(yè)4.0進程的不斷推進，企業(yè)的生產(chǎn)模式正經(jīng)歷著由傳統(tǒng)模式向自動化、智能化模式轉(zhuǎn)變。在這一轉(zhuǎn)化過程中，通過工業(yè)相機獲取所需要的物體信息研究成為了一個重要的研究領(lǐng)域。

在機器人抓取任務(wù)時，不僅能夠抓取到物體，同時也要求機器人以某樣的姿態(tài)去完成抓取任務(wù)，這就要求在獲取空間位置信息的同時也要獲取目標(biāo)物體的姿態(tài)信息。在獲得物體的空間位置信息的基礎(chǔ)上，周凡桂等［1］提出一種基于編碼合作標(biāo)志點的方法估計目標(biāo)位姿。通過將標(biāo)志點布置于模型表面，運用立體匹配技術(shù)和絕對定姿算法求解相對位姿初值，并進行誤差優(yōu)化。雖然這種方法可以獲取精度較高的物體位姿，但是主要依賴于所布置的標(biāo)志點，然而，在實際的生產(chǎn)過程中，文中所提的條件是不易滿足的。蔣萌等［2］根據(jù)標(biāo)定的結(jié)果對選定的特征點進行三維重建，并根據(jù)選定的特征點在圖像和實際場景中對應(yīng)的2D-3D位置信息求解目標(biāo)位姿，但是這種根據(jù)選定的幾個特征點對獲取的位姿矩陣在文中進行反向驗證時，Z軸的誤差最大達到了4.604 mm。Pas等［3］提出了基于幾何條件約束的檢測方法，其主要思想是在物體上定義若干幾何條件為后續(xù)計算目標(biāo)最佳抓取位姿提供約束條件。近些年來，隨著深度學(xué)習(xí)的快速發(fā)展，也有學(xué)者將深度學(xué)習(xí)應(yīng)用到物體位姿估計領(lǐng)域中。文獻［4］提出了一種多模特征深度學(xué)習(xí)構(gòu)建疊層深度網(wǎng)絡(luò)實現(xiàn)抓取位姿最優(yōu)判斷。文獻［5］訓(xùn)練了一種輕量級的GG-CNN，以深度圖作為網(wǎng)絡(luò)模型的輸入，端到端輸出每一個像素上的抓取位姿。但是，基于深度學(xué)習(xí)的位姿檢測算法主要依賴于構(gòu)建的數(shù)據(jù)集，數(shù)據(jù)集的規(guī)模和質(zhì)量直接影響檢測結(jié)果的準(zhǔn)確性。

根據(jù)以上問題，為了獲取物體的穩(wěn)定可靠的位姿信息，本文將結(jié)合機器人的實際工作環(huán)境，基于物體的點云特征信息對目標(biāo)物體的位姿計算進行研究。

1 基于雙目的零件位姿檢測

為了從圖像中獲取目標(biāo)物體的三維信息，根據(jù)相機的成像原理，通過構(gòu)建相似模型檢測左右成像面中同一個物體成像點的視差，采用相似原理計算出該物體點在相機坐標(biāo)系下的深度信息，同時根據(jù)已經(jīng)獲取的深度信息計算該物體在相機坐標(biāo)系下的三維點云。

1.1 相機模型

通過對相機進行標(biāo)定獲取相機的內(nèi)外參數(shù)，建立圖像坐標(biāo)與相機坐標(biāo)系以及世界坐標(biāo)系之間的轉(zhuǎn)換關(guān)系，進而通過轉(zhuǎn)換關(guān)系獲取三維空間中物體在圖像中的二維位置關(guān)系。理想的雙目相機模型如圖1所示。圖中：f為相機的焦距；Ol為左相機的光心；Or為右相機的光心；P（x，y，z）為空間中的一個點，在左相機的成像點為PL，在右相機的成像點為PR；XL為左成像點在相機坐標(biāo)系下距離圖像左邊緣的距離；XR為右成像點在相機坐標(biāo)系下距離左邊緣的距離。兩個光心之間的距離（x11+x22）為兩個相機的基線距離，用B表示。

圖1 面陣相機理想模型Fig.1 Optimal model of phase matrix

世界坐標(biāo)系下的點P在投影到相機的成像面這一過程涉及世界坐標(biāo)系、相機坐標(biāo)系和圖像坐標(biāo)系的轉(zhuǎn)換，具體轉(zhuǎn)換過程如下：

式中：M1為相機內(nèi)參矩陣；M2為相機外參矩陣。因此，可以建立世界坐標(biāo)系與圖像坐標(biāo)系之間的轉(zhuǎn)換關(guān)系。

1.2 特征匹配與深度優(yōu)化

為了獲取工件的三維點云，對比分析這里選用的BM圖像特征立體匹配算法計算視差［6-8］，從而獲取工件的三維坐標(biāo)值。

根據(jù)實際生產(chǎn)環(huán)境中物體所在的背景固定這一特性，采用背景差分法分割圖像上的目標(biāo)物體，獲取物體在圖像上分布的坐標(biāo)信息，以獲取的坐標(biāo)值進行目標(biāo)物體區(qū)域劃分，獲取目標(biāo)物體的特征信息，采用BM算法對兩種方法處理后的數(shù)據(jù)進行特征匹配，結(jié)果如圖2和圖3所示。

圖2 去噪深度圖圖Fig.2 Denoise depth map

圖3 差分深度圖Fig.3 Differ ential depth map

可以看出，本文所用的方法可以在較少的噪聲干擾下獲取豐富的物體深度信息，并且深度圖中孤立的深度孤立點也得到了很大的抑制。根據(jù)得到的物體特征點視差，通過如下公式即可以獲取目標(biāo)物體在相機坐標(biāo)系下的粗糙點云集：

式中：D為同一目標(biāo)點對應(yīng)的視差；Xl和Yl為左相機圖像坐標(biāo)系中的坐標(biāo)值；xc、yc、zc表示在相機坐標(biāo)系下的坐標(biāo)值。

為了消除由于匹配誤差所帶來的深度噪聲，在深度空間對物體的深度信息進行去噪處理。為了不失去目標(biāo)邊緣特征深度，同時可以有效地抑制孤立的深度值點，采用3層金子塔求和算子對深度圖進行卷積采樣，獲取每個采樣點鄰域內(nèi)的點集分布狀態(tài)，去除孤立的深度特征點結(jié)果如圖4所示。

圖4 優(yōu)化后的深度圖Fig.4 Optimized depth map

1.3 位姿特征點計算

為了在獲得的物體點云集中獲取能夠代表物體的特征點，計算獲取的特征點深度梯度，并根據(jù)梯度值進行聚集獲取可靠穩(wěn)定的特征點。設(shè)深度圖像為H（x，y），這里通過如下公式計算深度特征值2個方向的梯度值：

在深度圖中，相鄰兩個點的圖像量度為1，根據(jù)導(dǎo)數(shù)定義分量可寫為

對每一個特征點深度值進行梯度計算，并對梯度值進行分類，結(jié)果如圖5所示。

圖5 深度梯度分布Fig.5 Depth gradient distribution

根據(jù)梯度分布可以得到物體的邊緣和非邊緣信息，根據(jù)連續(xù)的平面深度變化幅度小這一特性進行深度密度檢測，將深度密度小的區(qū)域進行抑制，以獲取穩(wěn)定可靠度高的深度特征點進行位姿計算。

1.4 坐標(biāo)系建立與姿態(tài)估計

根據(jù)剛體旋轉(zhuǎn)平移內(nèi)部特征點相對位置不變的特性，在經(jīng)過1.2節(jié)方法優(yōu)化后的點云集中，獲取可以代表物體平面的特征點。這里根據(jù)物體表面的幾何形狀進行均勻分割，獲取4個區(qū)域R1、R2、R3和R4。在這4個區(qū)域內(nèi)分別對內(nèi)部的點云集進行平滑處理，通過如下公式求取局部均值作為該區(qū)域的初始參考特征點：

式中：X0、Y0、Z0為參考特征點；Xi、Yi、Zi為區(qū)域特征點；s為區(qū)域特征點數(shù)量。

為了獲取穩(wěn)定且具有代表性的特征點，以初始特征點作為參考點，建立如下D-D評價函數(shù)：

式中：α1和α2為比例系數(shù)；m為待評價特征點的深度值在其±amm范圍內(nèi)的點云數(shù)量，a的值取決于區(qū)域范圍內(nèi)點云的深度變化幅值；M為區(qū)域內(nèi)總點云數(shù)量。

該函數(shù)共有2個部分：第1部分為區(qū)域內(nèi)的點云與特征點的絕對距離；第2部分為點云深度密度，加入點云密度是為了防止距離參數(shù)被獨立的奇異特征點影響。將最大F值對應(yīng)的特征點作為該區(qū)域的表示點P1、P2、P3和P4。

為了描述物體在相機坐標(biāo)下的位姿，建立物體的坐標(biāo)系OT（x，y，z）。以其中一個點為坐標(biāo)原點，同時在P1P2P3組成的平面H上，將P1P2向量單位化作為X軸，根據(jù)如下公式求出平面的法向量：

以P1點作為原點建立Z軸，根據(jù)右手定則，在平面H上以P1點為原點，建立Y軸，結(jié)果如圖6所示。

圖6 物體坐標(biāo)系Fig.6 Physical coordinate system

在建立的物體坐標(biāo)系下描述所挑選出來的4個特征點，通過如下公式求取：

P4在工件坐標(biāo)系下的描述如圖7所示。圖中，a、b和c分別為與3個軸的夾角。根據(jù)式（9）可以獲取各個夾角的余弦值，歸一化P1P4的模長，可以獲得P4在工具坐標(biāo)系下的描述，同時根據(jù)P4點的計算，可以計算任意的特征點在工件坐標(biāo)系下的描述。

圖7 特征點描述Fig.7 Description of feature point

此時，已知4個目標(biāo)點在相機坐標(biāo)系下的坐標(biāo)值Cm和4個點在物體坐標(biāo)系下的坐標(biāo)值Tm，通過如下公式可以建立工件坐標(biāo)系與相機坐標(biāo)系的變換關(guān)系R：

根據(jù)如下公式建立平移向量T：

根據(jù)所獲得的R，以及X-Y-Z固定角表示法，通過如下公式獲得回轉(zhuǎn)角γ、俯仰角β和偏轉(zhuǎn)角α：

2 試驗驗證與分析

2.1 實驗環(huán)境與數(shù)據(jù)處理

圖像采集設(shè)備采用的是德國Basler acA1600-20 gm面陣雙目工業(yè)相機，采用Matlab、Pycharm編譯環(huán)境下的Python 3.6和Opencv 3.4.2，實現(xiàn)對目標(biāo)物體位姿的檢測和計算。相機與工作臺的垂直距離為591 mm，采用厚度為15.8 mm的螺母零件作為試驗對象并進行數(shù)據(jù)采集。

將處理好的左右相機圖像進行立體校正，在左右圖像上根據(jù)極線約束，對目標(biāo)特征點進行搜索匹配獲取目標(biāo)的深度圖像，并對深度圖像進行優(yōu)化、除噪聲和去粗差，獲得穩(wěn)定可靠的目標(biāo)三維點云并對物體空間分布進行限定，如圖8所示。

圖8 物體空間分布Fig.8 Spatial distribution of objects

在優(yōu)化后的物體特征點云中，隨機采樣5個特征點對其深度特征值進行統(tǒng)計并與真實值進行對比，結(jié)果如表1所示。

分析表1可知，獲取的點云深度特征值與真實的特征值的平均誤差為1.628 mm，可以滿足任務(wù)要求。

表1 特征點云深度統(tǒng)計Tab.1 Feature point cloud depth statistics

2.2 位姿估計與誤差評估

對圖像上的物體進行分割，并通過深度邊緣檢測獲取位姿估計所需要的特征點，通過式（7）對每一個區(qū)域內(nèi)的特征點進行計算評價，結(jié)果如圖9所示。

圖9 區(qū)域特征點評價值Fig.9 Value of regional feature reviews

將最優(yōu)得分特征點作為建立工件坐標(biāo)系的特征點，建立的2個物體工件坐標(biāo)系如圖10和圖11所示。

圖10 目標(biāo)1位姿Fig.10 Target 1 position

圖11 目標(biāo)2位姿Fig.11 Target 2 position

根據(jù)相機坐標(biāo)系下的特征信息與對應(yīng)的工件坐標(biāo)信息的轉(zhuǎn)換關(guān)系，獲取工件坐標(biāo)系與相機坐標(biāo)系的轉(zhuǎn)換關(guān)系R矩陣。同時根據(jù)式（18）～式（20）計算回轉(zhuǎn)角γ、俯仰角β和偏轉(zhuǎn)角α，計算結(jié)果如表2所示。

表2 3個坐標(biāo)軸的夾角Tab.2 Angle of the three axes

分析獲取的夾角數(shù)值，建立的工件1、2坐標(biāo)系與實際坐標(biāo)系的Z軸分別相差1.212°和1.503°，能夠滿足任務(wù)的要求。

為了驗證所獲取的相機坐標(biāo)系與工件坐標(biāo)系的位姿矩陣的準(zhǔn)確性，這里針對相機坐標(biāo)系與工件坐標(biāo)系的轉(zhuǎn)換誤差進行分析。在點云集中隨機采樣20個點。通過相機坐標(biāo)系和工件坐標(biāo)系的幾何關(guān)系求取特征點在工具坐標(biāo)系下的三維坐標(biāo)，同時根據(jù)所獲得的位姿矩陣進行相同點的相機坐標(biāo)系下與工件坐標(biāo)系的轉(zhuǎn)換，前后對比通過獲得的轉(zhuǎn)換矩陣獲得的X、Y和Z位置信息誤差進行驗證，如圖12所示。

圖12 基于點云的X-Y-Z軸誤差統(tǒng)計Fig.12 X-Y-Z axis error statistics based on point cloud

選擇目標(biāo)物體的4個定點計算其在相機坐標(biāo)系下的坐標(biāo)值，并根據(jù)4個定點建立工件坐標(biāo)系獲取相機與工件坐標(biāo)系的轉(zhuǎn)換矩陣M。對上述過程中的20個采樣點進行坐標(biāo)系轉(zhuǎn)換獲取X、Y和Z軸的誤差，如圖13所示。

圖13 基于選定點的X-Y-Z軸誤差統(tǒng)計Fig.13 X-Y-Z axis error statistics based on selected points

統(tǒng)計20個采樣點在2種方法下，3個軸上的誤差如圖14所示。

圖14 X-Y-Z軸相對誤差統(tǒng)計Fig.14 X-Y-Z axis relative error statistics

對比分析可知，在Y軸上本文方法比定點算法的平均誤差低了0.75 mm，Z軸誤差低了0.96 mm，X軸誤差高了0.21 mm。分析采樣點的誤差結(jié)果可以看出，3個坐標(biāo)軸的誤差變化幅值差異較大，這是由于相機的徑向畸變和切向畸變的變化速度不同引起的，且相機畸變校正不易完全還原，因此，深度方向的測量誤差會引起整體誤差增大。

3 結(jié)論

本文通過對點云區(qū)域估計獲取了具有代表性的特征點，在雙目立體匹配獲取的誤差基礎(chǔ)上，根據(jù)特征點云能夠獲得穩(wěn)定可靠且誤差相對較小的位姿矩陣。通過實驗分析獲得以下結(jié)論：①采用背景差分法得到了圖像中物體的位置并進行分割，獲得噪聲抑制的圖像數(shù)據(jù)集，并采用圖像立體匹配獲得目標(biāo)物體的三維點云，通過對點云進行優(yōu)化和去粗差，可獲得穩(wěn)定可靠的物體點云信息。②在深度維度上對目標(biāo)物體每一個特征點求取梯度，通過對梯度的分類可以獲取到具有代表性的物體特征點。③根據(jù)區(qū)域點云評價函數(shù)可以得到建立工件坐標(biāo)系需要的特征點，通過建立的工件坐標(biāo)系，可以得到工件相對于相機的位姿矩陣。在3個坐標(biāo)軸的平均誤差和為2.43 mm，3個軸轉(zhuǎn)角的平均誤差為1.08°，驗證了所提方法對結(jié)構(gòu)規(guī)整的工件進行位姿計算時的可行性。