基于深度相機(jī)的汽車轉(zhuǎn)向節(jié)位姿估計(jì)研究

劉學(xué)超，張波，鄭魁敬

(1.燕山大學(xué)，先進(jìn)鍛壓成形技術(shù)與科學(xué)教育部重點(diǎn)實(shí)驗(yàn)室，河北秦皇島 066004；2.燕山大學(xué)，河北省并聯(lián)機(jī)器人與機(jī)電系統(tǒng)實(shí)驗(yàn)室，河北秦皇島 066004)

0 前言

目前消費(fèi)者對(duì)產(chǎn)品質(zhì)量要求越來越高，而高級(jí)技工數(shù)量有限，因此希望機(jī)器人能代替一部分勞動(dòng)力。在人工智能推動(dòng)下，機(jī)器人開始智能化，并逐漸進(jìn)入生產(chǎn)一線代替工人工作，但其應(yīng)用范圍及深度還有待挖掘和拓展。張翰博等利用視覺關(guān)系推理實(shí)現(xiàn)了多物體堆疊機(jī)器人抓取。韓冬等人利用解耦控制實(shí)現(xiàn)了腱驅(qū)動(dòng)多指機(jī)械手靈巧抓取。WENG等利用機(jī)器學(xué)習(xí)和彩色攝像頭指導(dǎo)機(jī)器人抓取透明物體。盧道華等利用多個(gè)攝像頭采集圖像，通過PnP算法計(jì)算集裝箱姿態(tài)。MATEO等實(shí)現(xiàn)了剛體六自由度位姿估計(jì)。TEJANI等研究了基于霍夫森林的6D位姿估計(jì)。XIANG等提出了PoseCNN的位姿估計(jì)方法。

針對(duì)汽車轉(zhuǎn)向節(jié)自動(dòng)化加工中的上下料場(chǎng)景，以轉(zhuǎn)向節(jié)內(nèi)孔為基準(zhǔn)點(diǎn)，通過視覺引導(dǎo)機(jī)器人實(shí)現(xiàn)抓取。利用2D圖像獲取轉(zhuǎn)向節(jié)位置并結(jié)合點(diǎn)云確定轉(zhuǎn)向節(jié)姿態(tài)。轉(zhuǎn)向節(jié)位姿檢測(cè)方案分3個(gè)部分：相機(jī)標(biāo)定與點(diǎn)云產(chǎn)生、轉(zhuǎn)向節(jié)抓取點(diǎn)檢測(cè)和姿態(tài)檢測(cè)。

1 RGBD相機(jī)標(biāo)定與點(diǎn)云產(chǎn)生

1.1 RGBD相機(jī)

Intel RealSense D435i相機(jī)如圖1所示，可以輸出彩色圖和深度圖。深度相機(jī)坐標(biāo)系以左側(cè)紅外相機(jī)為基準(zhǔn)，深度信息的獲取由左紅外相機(jī)、右紅外相機(jī)和紅外投射器聯(lián)合工作完成。左右紅外相機(jī)捕獲場(chǎng)景并將成像器數(shù)據(jù)發(fā)送到深度成像處理器，通過左右圖像上匹配的關(guān)聯(lián)點(diǎn)來計(jì)算圖像中每個(gè)像素的深度。

圖1 Intel RealSense D435i相機(jī)

1.2 RGBD相機(jī)標(biāo)定

由于相機(jī)透鏡誤差給圖像帶來了畸變，需對(duì)相機(jī)內(nèi)、外參數(shù)進(jìn)行標(biāo)定。采用一個(gè)長(zhǎng)度為5的向量=[,,,,]描述相機(jī)畸變模型。

所選標(biāo)定板為9×7黑白相間邊長(zhǎng)為25 mm的正方形棋盤格標(biāo)定板，如圖2所示。

圖2 棋盤格

RGB相機(jī)內(nèi)參數(shù)如表1所示，使用RGBD相機(jī)對(duì)不同位置、不同角度下的標(biāo)定板拍攝15張不同姿態(tài)棋盤格圖片進(jìn)行標(biāo)定，RGBD相機(jī)映射誤差為0.14像素。

表1 Intel RealSense D435i彩色相機(jī)內(nèi)參 單位:像素

彩色相機(jī)畸變系數(shù)為

[-0.051 99,2.092 44,-0.002 004,-0.000 357,-7.778 8]

深度相機(jī)內(nèi)參數(shù)如表2所示，同時(shí)使用深度相機(jī)對(duì)不同位置、不同角度下的標(biāo)定板拍攝15張不同姿態(tài)棋盤格圖片進(jìn)行標(biāo)定，深度相機(jī)映射誤差為0.12像素。

表2 Intel RealSense D435i深度相機(jī)內(nèi)參 單位:像素

深度相機(jī)畸變系數(shù)為

[-0.028 22,0.309 87,0.002 92,-0.001 14,-0.881 4]

1.3 彩色圖和深度圖對(duì)齊

由于Intel RealSense D435i相機(jī)獲取的彩色圖與深度圖之間存在一定偏移，為確保通過圖像擬合確定的轉(zhuǎn)向節(jié)抓取基準(zhǔn)點(diǎn)能映射到準(zhǔn)確的深度信息，需將深度值和彩色圖像素坐標(biāo)匹配，同時(shí)通過一一對(duì)應(yīng)生成(,,,,,)格式的信息，轉(zhuǎn)化為點(diǎn)云以估計(jì)轉(zhuǎn)向節(jié)姿態(tài)。需要求出深度相機(jī)坐標(biāo)系與彩色相機(jī)坐標(biāo)系之間的變換關(guān)系如圖3所示，空間中一點(diǎn)在世界坐標(biāo)系中為(,,)，在深度相機(jī)坐標(biāo)系中為(,,)，在RGB相機(jī)坐標(biāo)系中為(,,)。

圖3 坐標(biāo)系轉(zhuǎn)換原理

彩色像素坐標(biāo)表示為(,)，深度圖像素坐標(biāo)表示為(,)，則存在以下關(guān)系：

=

(1)

=

(2)

式中：為彩色相機(jī)內(nèi)參；為深度相機(jī)內(nèi)參。

和通過旋轉(zhuǎn)矩陣和平移向量關(guān)聯(lián)：

=+

(3)

點(diǎn)與和之間關(guān)系可以通過RGB相機(jī)和深度相機(jī)的外參矩陣求得：

=+

(4)

=+

(5)

式中:和分別為世界坐標(biāo)系到RGB相機(jī)坐標(biāo)系和深度相機(jī)坐標(biāo)系的旋轉(zhuǎn)矩陣；和分別為世界坐標(biāo)系到RGB相機(jī)坐標(biāo)系和深度相機(jī)坐標(biāo)系的平移向量。

可求出和：

(6)

(7)

RGB相機(jī)和深度相機(jī)之間的旋轉(zhuǎn)矩陣和平移矩陣確定如下：

=[0.014 736 0.000 029 0.000 316]

根據(jù)和進(jìn)行深度圖與彩色圖對(duì)齊，并對(duì)轉(zhuǎn)換的像素坐標(biāo)系的點(diǎn)按式(8)進(jìn)行過濾，剔除不合理的點(diǎn)。

(8)

彩色圖與深度圖對(duì)齊的結(jié)果如圖4(a)所示。根據(jù)對(duì)齊后的信息，每一個(gè)彩色像素點(diǎn)對(duì)應(yīng)的3D坐標(biāo)，通過遍歷所設(shè)置的分辨率為1 280像素×720像素圖像的每個(gè)像素的(,,,,,)格式的信息，并利用PCL點(diǎn)云庫Point(x y z R G B)格式的數(shù)據(jù)結(jié)構(gòu)生成點(diǎn)云，如圖4(b)所示。

圖4 對(duì)齊的結(jié)果與點(diǎn)云

2 轉(zhuǎn)向節(jié)抓取點(diǎn)定位

2.1 圖像預(yù)處理

選取轉(zhuǎn)向節(jié)中心孔上表面圓心為抓取基準(zhǔn)點(diǎn)，確定轉(zhuǎn)向節(jié)基準(zhǔn)點(diǎn)在機(jī)器人坐標(biāo)系和相機(jī)坐標(biāo)系下的坐標(biāo)。在彩色圖中把轉(zhuǎn)向節(jié)單獨(dú)區(qū)分出來，并對(duì)原圖像中的噪聲和圖像二值化后出現(xiàn)的小雜點(diǎn)進(jìn)行圖像預(yù)處理，如圖5所示。

圖5 圖像預(yù)處理對(duì)比圖

2.2 轉(zhuǎn)向節(jié)區(qū)域輪廓提取

使用邊緣檢測(cè)對(duì)轉(zhuǎn)向節(jié)進(jìn)行像素級(jí)提取非常關(guān)鍵。在對(duì)圖像的邊緣檢測(cè)處理后，需要對(duì)轉(zhuǎn)向節(jié)內(nèi)孔邊緣信息進(jìn)行搜索及提取，以便擬合基準(zhǔn)點(diǎn)像素坐標(biāo)。邊緣檢測(cè)結(jié)果如圖6所示。

圖6 邊緣檢測(cè)結(jié)果

邊緣檢測(cè)會(huì)出現(xiàn)多條輪廓，需要設(shè)定條件將轉(zhuǎn)向節(jié)中心孔的輪廓提取出來。輪廓形狀各異，可以很好地進(jìn)行區(qū)分，且輪廓面積周長(zhǎng)有顯著差異，可設(shè)定輪廓周長(zhǎng)為390～420像素，面積為10 200～10 800像素，以提取轉(zhuǎn)向節(jié)中心孔輪廓。

提取轉(zhuǎn)向節(jié)中心孔的一組數(shù)據(jù)點(diǎn)如圖7所示，這組數(shù)據(jù)點(diǎn)近似地落在一個(gè)圓上，采用最小二乘法來估計(jì)這個(gè)圓的參數(shù)。

圖7 輪廓提取

設(shè)要擬合的轉(zhuǎn)向節(jié)中心圓孔曲線方程為

=(-)+(-)

(9)

改寫為

=-2++-2+

(10)

令=-2、=-2、=+-，可得圓曲線方程的另一個(gè)形式：

++++=0

(11)

提取的中心孔輪廓點(diǎn)集(，)(∈(1,2,3，…,))到圓心的距離為

(12)

點(diǎn)(,)到圓心距離的平方與半徑平方的差為

(13)

要求距離的平方和最?。?/p>

(14)

計(jì)算使(,,)具有最小值的、、,以確定擬合圓的參數(shù),對(duì)(,,)求導(dǎo)可得：

(15)

則：

(16)

式中：

將輪廓操作提取的轉(zhuǎn)向節(jié)中心孔的輪廓點(diǎn)集代入式(16)，求得、、、、的值，并求出、、的值，得到通過最小二乘法擬合的轉(zhuǎn)向節(jié)中心圓孔的參數(shù)、和。其中，和為轉(zhuǎn)向節(jié)抓取基準(zhǔn)點(diǎn)的像素坐標(biāo)，分別為565、320像素，執(zhí)行結(jié)果如圖8所示。

圖8 擬合圓結(jié)果

2.3 相機(jī)坐標(biāo)系下抓取點(diǎn)坐標(biāo)確定

為獲取轉(zhuǎn)向節(jié)中心孔上表面圓心深度，可以計(jì)算轉(zhuǎn)向節(jié)中心孔上表面圓的一組對(duì)應(yīng)點(diǎn)深度的平均值作為深度坐標(biāo)。根據(jù)確定出來的圓心，設(shè)置過圓心與水平線成0°、±45°、90°的8組方向。根據(jù)擬合求得的半徑，設(shè)置搜索區(qū)域[-3,+7]進(jìn)行坐標(biāo)搜索，搜索區(qū)域如圖9紅線部分所示。

圖9 設(shè)置搜索半徑

根據(jù)對(duì)齊后的深度圖，獲取搜索區(qū)域深度，按照由內(nèi)向外的順序組成列表。依照順序計(jì)算相鄰兩值的差值，并定位最大差值的位置。利用該位置之后的值計(jì)算的平均值代替該搜索方向確定的深度。以同樣方法獲取8組搜索區(qū)域的深度。由于轉(zhuǎn)向節(jié)內(nèi)孔部分有小凹槽，還需去除對(duì)應(yīng)方向上深度值相差超過5 mm的搜索方向，將剩余搜索方向的平均值作為抓取基準(zhǔn)點(diǎn)的深度。根據(jù)內(nèi)參矩陣、轉(zhuǎn)向節(jié)抓取基準(zhǔn)點(diǎn)像素坐標(biāo)及深度值得到基準(zhǔn)點(diǎn)在彩色相機(jī)坐標(biāo)系下的三維坐標(biāo)，結(jié)果如圖10所示。

圖10 轉(zhuǎn)向節(jié)抓取基準(zhǔn)點(diǎn)定位坐標(biāo)

3 轉(zhuǎn)向節(jié)姿態(tài)估計(jì)

3.1 點(diǎn)云預(yù)處理

由于轉(zhuǎn)向節(jié)會(huì)傾斜，不能保持同一姿態(tài)，需要確定轉(zhuǎn)向節(jié)姿態(tài)從而實(shí)現(xiàn)機(jī)器人精準(zhǔn)抓取。轉(zhuǎn)向節(jié)材質(zhì)為鋁，表面存在一定的反光、光線干擾等導(dǎo)致獲取點(diǎn)云時(shí)出現(xiàn)噪聲和離群點(diǎn)。如果不去除離群點(diǎn)，會(huì)影響轉(zhuǎn)向節(jié)特征描述子計(jì)算，導(dǎo)致點(diǎn)云配準(zhǔn)出現(xiàn)誤差，影響轉(zhuǎn)向節(jié)姿態(tài)估計(jì)精度。在對(duì)點(diǎn)云處理之前，需對(duì)轉(zhuǎn)向節(jié)部分?jǐn)?shù)據(jù)去除離群點(diǎn)。如果直接把整個(gè)原始點(diǎn)云進(jìn)行配準(zhǔn)計(jì)算會(huì)帶來很大干擾，需從原始點(diǎn)云中將轉(zhuǎn)向節(jié)部分單獨(dú)分離出來。算法流程如圖11所示，處理結(jié)果如圖12所示。

圖11 點(diǎn)云預(yù)處理算法流程

圖12 點(diǎn)云預(yù)處理結(jié)果圖

實(shí)際應(yīng)用中，轉(zhuǎn)向節(jié)孔口朝上特征明顯，無須利用完整的轉(zhuǎn)向節(jié)點(diǎn)云，通過局部點(diǎn)云模型法可以構(gòu)建出滿足轉(zhuǎn)向節(jié)姿態(tài)估計(jì)的模板點(diǎn)云。點(diǎn)云預(yù)處理獲取轉(zhuǎn)向節(jié)部分點(diǎn)云后采用ICP(Iterative Closest Point)算法進(jìn)行拼接，得到相對(duì)完整的轉(zhuǎn)向節(jié)點(diǎn)云模板。點(diǎn)云生成流程如圖13所示，轉(zhuǎn)向節(jié)模板點(diǎn)云如圖14所示。

圖13 模板點(diǎn)云生成流程

圖14 模板點(diǎn)云

3.2 基于特征描述子的轉(zhuǎn)向節(jié)姿態(tài)初步估計(jì)

對(duì)轉(zhuǎn)向節(jié)進(jìn)行姿態(tài)估計(jì)是通過模板轉(zhuǎn)向節(jié)點(diǎn)云與通過點(diǎn)云預(yù)處理得到的待估計(jì)轉(zhuǎn)向節(jié)點(diǎn)云的配準(zhǔn)實(shí)現(xiàn)的。區(qū)分轉(zhuǎn)向節(jié)曲面空間的分布情況和尋找相對(duì)應(yīng)的點(diǎn)，引入局部特征描述子，通過計(jì)算鄰域內(nèi)相應(yīng)特征，使特征描述子能夠描述轉(zhuǎn)向節(jié)表面，通過表面的幾何特征尋找對(duì)應(yīng)點(diǎn)，找到對(duì)應(yīng)點(diǎn)之后再基于ICP算法計(jì)算轉(zhuǎn)換矩陣。采樣一致性初始配準(zhǔn)算法(SAC-IA)可以通過幾種不同的特征描述子來尋找對(duì)應(yīng)點(diǎn)進(jìn)行初始配準(zhǔn)，因此用來作為轉(zhuǎn)向節(jié)姿態(tài)初始估計(jì)，其算法流程如圖15所示。

圖15 SAC-IA算法流程

提取模板轉(zhuǎn)向節(jié)點(diǎn)云特征點(diǎn)和FPFH(Fast Point Feature Histograms)特征描述子建立特征集合=[,,,]，其中，(,,)表示SIFT3D特征點(diǎn)坐標(biāo)、表示FPFH局部特征。以同樣方式建立待檢測(cè)轉(zhuǎn)向節(jié)點(diǎn)云的特征集合=[,,,]，用SAC-IA算法對(duì)模板轉(zhuǎn)向節(jié)和待檢測(cè)轉(zhuǎn)向節(jié)進(jìn)行配準(zhǔn)，獲取初始位姿信息。

配準(zhǔn)過程中，通過Huber函數(shù)計(jì)算轉(zhuǎn)向節(jié)匹配點(diǎn)變換后的距離誤差和，判斷待檢測(cè)轉(zhuǎn)向節(jié)點(diǎn)云與模板轉(zhuǎn)向節(jié)點(diǎn)云間配準(zhǔn)變換的精度。Huber函數(shù)對(duì)噪聲有好的魯棒性，表示為

(17)

式中:為Huber的參數(shù)，需要預(yù)先設(shè)定；為第組匹配區(qū)域的距離誤差。

利用式(17)對(duì)誤差進(jìn)行評(píng)價(jià)，在所有的匹配中尋找一組最優(yōu)匹配，使兩轉(zhuǎn)向節(jié)間距離誤差和最小。獲得的轉(zhuǎn)換矩陣作為粗配準(zhǔn)的結(jié)果，不同姿態(tài)下FPFH配準(zhǔn)結(jié)果如圖16所示。

圖16 FPFH配準(zhǔn)結(jié)果

3.3 基于NDT的轉(zhuǎn)向節(jié)姿態(tài)精準(zhǔn)估計(jì)

利用粗配準(zhǔn)算法估計(jì)的轉(zhuǎn)向節(jié)姿態(tài)結(jié)果達(dá)不到精度要求，還需要細(xì)配準(zhǔn)來提高轉(zhuǎn)向節(jié)姿態(tài)估計(jì)精度。利用NDT(Normal Distributions Transform)算法對(duì)待檢測(cè)轉(zhuǎn)向節(jié)與模板轉(zhuǎn)向節(jié)進(jìn)行細(xì)配準(zhǔn)，流程如圖17所示。

圖17 NDT算法流程

統(tǒng)計(jì)模板轉(zhuǎn)向節(jié)與待檢測(cè)轉(zhuǎn)向節(jié)的點(diǎn)云分布信息，利用轉(zhuǎn)換矩陣將待檢測(cè)轉(zhuǎn)向節(jié)的點(diǎn)云轉(zhuǎn)換到模板轉(zhuǎn)向節(jié)坐標(biāo)系中，如果其點(diǎn)云數(shù)據(jù)點(diǎn)在模板轉(zhuǎn)向節(jié)點(diǎn)云中的概率密度很大，則該矩陣參數(shù)較好。細(xì)配準(zhǔn)結(jié)果如圖18所示。

圖18 多姿態(tài)NDT配準(zhǔn)結(jié)果

4 軟件開發(fā)與測(cè)試

為驗(yàn)證轉(zhuǎn)向節(jié)基準(zhǔn)點(diǎn)定位和轉(zhuǎn)向節(jié)姿態(tài)估計(jì)算法，在VS平臺(tái)上利用C++多線程技術(shù)實(shí)現(xiàn)多窗口同步顯示，數(shù)據(jù)流程如圖19所示。

圖19 姿態(tài)估計(jì)數(shù)據(jù)流程

圖20所示為直觀顯示轉(zhuǎn)向節(jié)位姿信息的系統(tǒng)界面。該系統(tǒng)采用MFC設(shè)計(jì)，調(diào)用OpenCV圖像處理庫和PCL點(diǎn)云庫等對(duì)圖像及點(diǎn)云進(jìn)行處理。

圖20 系統(tǒng)界面

根據(jù)深度誤差與深度的關(guān)系，設(shè)定相機(jī)距離采集平面為800 mm，采集轉(zhuǎn)向節(jié)不同姿態(tài)下的點(diǎn)云圖像，估計(jì)不同姿態(tài)下的姿態(tài)。圖21所示為采用SAC-IA算法得到的結(jié)果，可以看出粗配準(zhǔn)的結(jié)果還有一定的誤差，不能用于最終的姿態(tài)估計(jì)結(jié)果。圖22所示為NDT算法的結(jié)果，NDT算法的初始轉(zhuǎn)換矩陣設(shè)置為SAC-IA算法得到的轉(zhuǎn)換矩陣，可以看出：配準(zhǔn)后獲得了更精確的姿態(tài)估計(jì)結(jié)果；轉(zhuǎn)向節(jié)在不同位置及姿態(tài)下，都能準(zhǔn)確識(shí)別姿態(tài)信息。

圖21 粗配準(zhǔn)結(jié)果

圖22 細(xì)配準(zhǔn)結(jié)果

不同姿態(tài)下，姿態(tài)1、姿態(tài)2粗配準(zhǔn)分別對(duì)應(yīng)的旋轉(zhuǎn)矩陣如、所示:

不同姿態(tài)下,姿態(tài)1、姿態(tài)2細(xì)配準(zhǔn)分別對(duì)應(yīng)的旋轉(zhuǎn)矩陣如下、所示：

為配準(zhǔn)實(shí)現(xiàn)數(shù)值化，計(jì)算30次轉(zhuǎn)向節(jié)間點(diǎn)云匹配中g(shù)etFitnessScore()函數(shù)的平均值，該值是所有對(duì)應(yīng)點(diǎn)間的距離平方和，值越小配準(zhǔn)效果越好。設(shè)置了對(duì)照組測(cè)試，選取模板轉(zhuǎn)向節(jié)的點(diǎn)云進(jìn)行配準(zhǔn)來作為對(duì)照組，粗、細(xì)配準(zhǔn)結(jié)果分別如圖23、圖24所示。最終getFitnessScore得分如表3所示。

圖23 對(duì)照組粗配準(zhǔn)結(jié)果

圖24 對(duì)照組細(xì)配準(zhǔn)結(jié)果

表3 對(duì)照點(diǎn)云與粗細(xì)配準(zhǔn)得分 單位：mm2

單純地基于特征描述子的姿態(tài)估計(jì)難以獲得良好結(jié)果，從對(duì)照組的結(jié)果看，同一轉(zhuǎn)向節(jié)獲得的分?jǐn)?shù)更低，配準(zhǔn)效果更好，不同轉(zhuǎn)向節(jié)間有微小差別，對(duì)配準(zhǔn)有一定影響。NDT算法在粗配準(zhǔn)基礎(chǔ)上進(jìn)行細(xì)配準(zhǔn)，取得了更好效果，getFitnessScore(最近對(duì)應(yīng)點(diǎn)距離平方和)得分為27 mm，表明轉(zhuǎn)向節(jié)姿態(tài)估計(jì)算法有效。

5 結(jié)論

對(duì)轉(zhuǎn)向節(jié)進(jìn)行基準(zhǔn)點(diǎn)定位，通過輪廓檢測(cè)過濾及輪廓擬合確定抓取點(diǎn)的像素坐標(biāo)，設(shè)置搜索區(qū)域中的突變值取平均值獲取深度，利用相機(jī)參數(shù)獲取相機(jī)坐標(biāo)系下的三維坐標(biāo)。通過去除背景將轉(zhuǎn)向節(jié)從原始點(diǎn)云中分離出來，利用濾波算法去除噪聲及離群點(diǎn)。提取轉(zhuǎn)向節(jié)特征點(diǎn)及特征描述子，用SAC-IA算法粗配準(zhǔn)，用NDT算法細(xì)配準(zhǔn)，getFitnessScore得分為27 mm，實(shí)現(xiàn)了轉(zhuǎn)向節(jié)姿態(tài)的精確估計(jì)。