基于深度相機的汽車轉向節位姿估計研究

劉學超，張波，鄭魁敬

(1.燕山大學，先進鍛壓成形技術與科學教育部重點實驗室，河北秦皇島 066004；2.燕山大學，河北省并聯機器人與機電系統實驗室，河北秦皇島 066004)

0 前言

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

針對汽車轉向節自動化加工中的上下料場景，以轉向節內孔為基準點，通過視覺引導機器人實現抓取。利用2D圖像獲取轉向節位置并結合點云確定轉向節姿態。轉向節位姿檢測方案分3個部分：相機標定與點云產生、轉向節抓取點檢測和姿態檢測。

1 RGBD相機標定與點云產生

1.1 RGBD相機

Intel RealSense D435i相機如圖1所示，可以輸出彩色圖和深度圖。深度相機坐標系以左側紅外相機為基準，深度信息的獲取由左紅外相機、右紅外相機和紅外投射器聯合工作完成。左右紅外相機捕獲場景并將成像器數據發送到深度成像處理器，通過左右圖像上匹配的關聯點來計算圖像中每個像素的深度。

圖1 Intel RealSense D435i相機

1.2 RGBD相機標定

由于相機透鏡誤差給圖像帶來了畸變，需對相機內、外參數進行標定。采用一個長度為5的向量=[,,,,]描述相機畸變模型。

所選標定板為9×7黑白相間邊長為25 mm的正方形棋盤格標定板，如圖2所示。

圖2 棋盤格

RGB相機內參數如表1所示，使用RGBD相機對不同位置、不同角度下的標定板拍攝15張不同姿態棋盤格圖片進行標定，RGBD相機映射誤差為0.14像素。

表1 Intel RealSense D435i彩色相機內參 單位:像素

彩色相機畸變系數為

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

深度相機內參數如表2所示，同時使用深度相機對不同位置、不同角度下的標定板拍攝15張不同姿態棋盤格圖片進行標定，深度相機映射誤差為0.12像素。

表2 Intel RealSense D435i深度相機內參 單位:像素

深度相機畸變系數為

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

1.3 彩色圖和深度圖對齊

由于Intel RealSense D435i相機獲取的彩色圖與深度圖之間存在一定偏移，為確保通過圖像擬合確定的轉向節抓取基準點能映射到準確的深度信息，需將深度值和彩色圖像素坐標匹配，同時通過一一對應生成(,,,,,)格式的信息，轉化為點云以估計轉向節姿態。需要求出深度相機坐標系與彩色相機坐標系之間的變換關系如圖3所示，空間中一點在世界坐標系中為(,,)，在深度相機坐標系中為(,,)，在RGB相機坐標系中為(,,)。

圖3 坐標系轉換原理

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

=

(1)

=

(2)

式中：為彩色相機內參；為深度相機內參。

和通過旋轉矩陣和平移向量關聯：

=+

(3)

點與和之間關系可以通過RGB相機和深度相機的外參矩陣求得：

=+

(4)

=+

(5)

式中:和分別為世界坐標系到RGB相機坐標系和深度相機坐標系的旋轉矩陣；和分別為世界坐標系到RGB相機坐標系和深度相機坐標系的平移向量。

可求出和：

(6)

(7)

RGB相機和深度相機之間的旋轉矩陣和平移矩陣確定如下：

=[0.014 736 0.000 029 0.000 316]

根據和進行深度圖與彩色圖對齊，并對轉換的像素坐標系的點按式(8)進行過濾，剔除不合理的點。

(8)

彩色圖與深度圖對齊的結果如圖4(a)所示。根據對齊后的信息，每一個彩色像素點對應的3D坐標，通過遍歷所設置的分辨率為1 280像素×720像素圖像的每個像素的(,,,,,)格式的信息，并利用PCL點云庫Point(x y z R G B)格式的數據結構生成點云，如圖4(b)所示。

圖4 對齊的結果與點云

2 轉向節抓取點定位

2.1 圖像預處理

選取轉向節中心孔上表面圓心為抓取基準點，確定轉向節基準點在機器人坐標系和相機坐標系下的坐標。在彩色圖中把轉向節單獨區分出來，并對原圖像中的噪聲和圖像二值化后出現的小雜點進行圖像預處理，如圖5所示。

圖5 圖像預處理對比圖

2.2 轉向節區域輪廓提取

使用邊緣檢測對轉向節進行像素級提取非常關鍵。在對圖像的邊緣檢測處理后，需要對轉向節內孔邊緣信息進行搜索及提取，以便擬合基準點像素坐標。邊緣檢測結果如圖6所示。

圖6 邊緣檢測結果

邊緣檢測會出現多條輪廓，需要設定條件將轉向節中心孔的輪廓提取出來。輪廓形狀各異，可以很好地進行區分，且輪廓面積周長有顯著差異，可設定輪廓周長為390～420像素，面積為10 200～10 800像素，以提取轉向節中心孔輪廓。

提取轉向節中心孔的一組數據點如圖7所示，這組數據點近似地落在一個圓上，采用最小二乘法來估計這個圓的參數。

圖7 輪廓提取

設要擬合的轉向節中心圓孔曲線方程為

=(-)+(-)

(9)

改寫為

=-2++-2+

(10)

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

++++=0

(11)

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

(12)

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

(13)

要求距離的平方和最小：

(14)

計算使(,,)具有最小值的、、,以確定擬合圓的參數,對(,,)求導可得：

(15)

則：

(16)

式中：

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

圖8 擬合圓結果

2.3 相機坐標系下抓取點坐標確定

為獲取轉向節中心孔上表面圓心深度，可以計算轉向節中心孔上表面圓的一組對應點深度的平均值作為深度坐標。根據確定出來的圓心，設置過圓心與水平線成0°、±45°、90°的8組方向。根據擬合求得的半徑，設置搜索區域[-3,+7]進行坐標搜索，搜索區域如圖9紅線部分所示。

圖9 設置搜索半徑

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

圖10 轉向節抓取基準點定位坐標

3 轉向節姿態估計

3.1 點云預處理

由于轉向節會傾斜，不能保持同一姿態，需要確定轉向節姿態從而實現機器人精準抓取。轉向節材質為鋁，表面存在一定的反光、光線干擾等導致獲取點云時出現噪聲和離群點。如果不去除離群點，會影響轉向節特征描述子計算，導致點云配準出現誤差，影響轉向節姿態估計精度。在對點云處理之前，需對轉向節部分數據去除離群點。如果直接把整個原始點云進行配準計算會帶來很大干擾，需從原始點云中將轉向節部分單獨分離出來。算法流程如圖11所示，處理結果如圖12所示。

圖11 點云預處理算法流程

圖12 點云預處理結果圖

實際應用中，轉向節孔口朝上特征明顯，無須利用完整的轉向節點云，通過局部點云模型法可以構建出滿足轉向節姿態估計的模板點云。點云預處理獲取轉向節部分點云后采用ICP(Iterative Closest Point)算法進行拼接，得到相對完整的轉向節點云模板。點云生成流程如圖13所示，轉向節模板點云如圖14所示。

圖13 模板點云生成流程

圖14 模板點云

3.2 基于特征描述子的轉向節姿態初步估計

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

圖15 SAC-IA算法流程

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

配準過程中，通過Huber函數計算轉向節匹配點變換后的距離誤差和，判斷待檢測轉向節點云與模板轉向節點云間配準變換的精度。Huber函數對噪聲有好的魯棒性，表示為

(17)

式中:為Huber的參數，需要預先設定；為第組匹配區域的距離誤差。

利用式(17)對誤差進行評價，在所有的匹配中尋找一組最優匹配，使兩轉向節間距離誤差和最小。獲得的轉換矩陣作為粗配準的結果，不同姿態下FPFH配準結果如圖16所示。

圖16 FPFH配準結果

3.3 基于NDT的轉向節姿態精準估計

利用粗配準算法估計的轉向節姿態結果達不到精度要求，還需要細配準來提高轉向節姿態估計精度。利用NDT(Normal Distributions Transform)算法對待檢測轉向節與模板轉向節進行細配準，流程如圖17所示。

圖17 NDT算法流程

統計模板轉向節與待檢測轉向節的點云分布信息，利用轉換矩陣將待檢測轉向節的點云轉換到模板轉向節坐標系中，如果其點云數據點在模板轉向節點云中的概率密度很大，則該矩陣參數較好。細配準結果如圖18所示。

圖18 多姿態NDT配準結果

4 軟件開發與測試

為驗證轉向節基準點定位和轉向節姿態估計算法，在VS平臺上利用C++多線程技術實現多窗口同步顯示，數據流程如圖19所示。

圖19 姿態估計數據流程

圖20所示為直觀顯示轉向節位姿信息的系統界面。該系統采用MFC設計，調用OpenCV圖像處理庫和PCL點云庫等對圖像及點云進行處理。

圖20 系統界面

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

圖21 粗配準結果

圖22 細配準結果

不同姿態下，姿態1、姿態2粗配準分別對應的旋轉矩陣如、所示:

不同姿態下,姿態1、姿態2細配準分別對應的旋轉矩陣如下、所示：

為配準實現數值化，計算30次轉向節間點云匹配中getFitnessScore()函數的平均值，該值是所有對應點間的距離平方和，值越小配準效果越好。設置了對照組測試，選取模板轉向節的點云進行配準來作為對照組，粗、細配準結果分別如圖23、圖24所示。最終getFitnessScore得分如表3所示。

圖23 對照組粗配準結果

圖24 對照組細配準結果

表3 對照點云與粗細配準得分 單位：mm2

單純地基于特征描述子的姿態估計難以獲得良好結果，從對照組的結果看，同一轉向節獲得的分數更低，配準效果更好，不同轉向節間有微小差別，對配準有一定影響。NDT算法在粗配準基礎上進行細配準，取得了更好效果，getFitnessScore(最近對應點距離平方和)得分為27 mm，表明轉向節姿態估計算法有效。

5 結論

對轉向節進行基準點定位，通過輪廓檢測過濾及輪廓擬合確定抓取點的像素坐標，設置搜索區域中的突變值取平均值獲取深度，利用相機參數獲取相機坐標系下的三維坐標。通過去除背景將轉向節從原始點云中分離出來，利用濾波算法去除噪聲及離群點。提取轉向節特征點及特征描述子，用SAC-IA算法粗配準，用NDT算法細配準，getFitnessScore得分為27 mm，實現了轉向節姿態的精確估計。