基于目標工件點云的位姿估計方法研究

閆厚任，孔德杰，郭飛，張科航，譚文

（長春理工大學 光電工程學院，長春 130022）

近年來，機器人在制造領域各個環節的應用越來越廣泛。為了提高機器人在應用中的生產效率，滿足機械臂柔性裝配等方面的需求，通過對任意位姿的目標工件進行三維重建，獲取工件在空間中的位姿信息，再將該信息傳遞至機器人來實現機械臂對工件的準確抓取，從而提升工業生產的效率。 目前，應用最廣泛的點云配準算法是迭代最近點算法ICP，但傳統的ICP算法收斂速度較慢，且在點云數據集初始位置相差較大時，容易陷入局部最優解，為了改善這一問題，本文給出一種改進的點云配準算法［1］。

1 點云特征描述方法

點云特征描述是點云識別、配準的前提。特征點是指從點云中提取出的具有代表性和區分性的點集，用于描述點云的特征屬性。 其關鍵在于從點云中提取所需目標的特征描述子，通過目標的特征描述子來識別物體，識別準確度依賴于所提取特征描述子的優劣［2］。

點云特征描述方法一般分為全局特征和局部特征兩種類型。常見的全局特征有VFH、ESF等，局部特征有PFH、FPFH、SHOT 等。本文選擇局部特征FPFH 作為特征描述子。

1.1 表面法線

在三維物體的描述中，表面法線是非常重要的屬性之一。它不僅可以反映出三維表面的曲率變化，還可以獲得三維表面的形狀特征。 這里采用主成分分析法PCA，通過查詢點的最近鄰域構建協方差矩陣，從而計算出表面法線。 這個過程可以通過求取協方差矩陣的特征值和特征向量來完成。設待求表面法線的點云P中任意一點Pm，創建的協方差矩陣為：

式中，Pm為查詢點所在的位置坐標；k為查詢點最近鄰域點的個數；為最近鄰域點云的質心所在的位置坐標；為矩陣的第n個特征值和特征向量，可以用來表示點云的主要方向。

1.2 點特征直方圖

點特征直方圖PFH［3］是一種常用的點云局部特征描述方法，能夠有效地描述點云中的局部幾何特征。其利用表面法線和查詢點k鄰域中所有點的關系，通過幾何特征處理k鄰域中的點，并且用多維直方圖表示幾何特征。PFH 特征具有很強的抗干擾能力，即使在不同的噪聲下也能表現良好。圖1 為PFH 特征的影響區域圖，圖中pq表示查詢點，虛線部分表示半徑為r 的球體，將查詢點與其所有鄰域點全部相連，計算鄰域中所有的點對關系，即可獲得PFH 直方圖。雖然PFH 特征描述方法在點云處理中具有廣泛的應用，但是其計算復雜度比較高，為O(k2)。

圖1 PFH 特征鄰域圖

設pa與pb為相連的兩個點，兩點的法線分別為na、nb，如圖2 所示。α、?、θ分別表示兩向量的偏差角度。

圖2 局部坐標系

則有局部坐標系為：

法線的偏差角度如下：

其中，兩點之間的歐氏距離為d= (pb-pa)z。通過對每組點對進行四元組(α,?,θ,d) 求解，可以將其特征信息進行壓縮，從12 個值減少為4 個。

在PCL 點云庫中，PFH 特征提取算法會從每組點對的四元組(α,?,θ,d) 中取出前三元(α,?,θ)，并將每個特征值分成五份，形成一個125 維的直方圖，最終得到的PFH 特征向量即為一個125 維的向量。這個向量可以用于描述點云中每個點的幾何特征信息。

1.3 快速點特征直方圖

快速點特征直方圖FPFH［4］描述子是一種改進的點云局部特征描述方法，它是在PFH 的基礎上進行優化得到的，能夠更快速、更準確地描述點云中的局部幾何特征。通過引入點云中每個點與其法向量方向的偏差角度作為權重，來對PFH 進行優化，從而提高其對點云幾何形狀的描述精度。其計算復雜度從FPH 的O(k2)降低到了O(k)。FPFH 特征鄰域圖如圖3 所示。

圖3 FPFH 特征鄰域圖

FPFH 描述子算法步驟如下：

（1）通過計算查詢點pq與k鄰域之間的三元組(α,?,θ) 來計算簡化點特征直方圖SPFH，其中三元組(α,?,θ) 表示鄰域中的每個點的位置關系。

（2）對于每個查詢點，再次查找其鄰域點的k鄰域，計算每個鄰域點的三元組(α,?,θ)，并將其與查詢點的三元組結合起來進行加權平均，權重wk表示第二個鄰域中第k個點在第一個鄰域中的權重。計算公式如下：

與PFH 相比，FPFH 算法在計算查詢點和鄰域點之間的關系時，并沒有完全將二者相連。因此，FPFH 算法的計算值較少。由于采用了加權的方法，使得其在計算時涵蓋了鄰域點的鄰域值，并且某些點可能會存在于多個鄰域中，會存在重復計算的情況。

在PCL 點云庫中，FPFH 特征提取算法將每個特征值分成11 份，形成一個33 維的直方圖。因此FPFH 特征向量是一個33 維的向量，用于描述點云中每個點的幾何特征信息。

2 點云配準

2.1 點云配準原理

目標工件點云和模板點云存在相似的部分，并且兩個點云位于不同的坐標系中。本文選取的目標工件均為剛體，利用點云配準使目標工件點云和模板點云對齊，并獲取位姿變換矩陣。

將目標工件點云中的一點P1和模板點云中的一點P1′ 作為一個匹配點對，兩者的旋轉變換矩陣為R，平移變換為t，可得：

設α、β、γ分別為沿著x軸、y軸、z軸的旋轉角，tx、ty、tz分別為沿著x軸、y軸、z軸的平移距離，共有六個未知數α、β、γ、tx、ty、tz。 因此，需要不少于三個匹配點才能求取位姿變換矩陣，且三個點不共線。由于噪聲的存在，通過一個匹配點對無法得到精確的變換矩陣，因此需要使用多個匹配點對進行求解。對于已知匹配點對的情況下，可以通過最小二乘法來求解變換矩陣，最小化點云之間的距離誤差。

設目標工件點云和模板點云分別為Pi(i=1,2,…,n)、Qi(i= 1,2,…,n)。為了計算旋轉平移變換的最優解，使目標工件點云和模板點云間的歐式距離之和為最小，則有如下關系式：

2.2 選取模板點云

目標工件的位姿估計可以通過計算目標工件點云和模板點云的旋轉平移變換來實現。選擇模板點云可以利用CAD 軟件建立目標工件的三維模型，并將其轉換為點云數據。 也可以選取工件某一個位姿下的目標點云作為模板點云。

在CAD 軟件中建立三維模型時，利用Solid-Works 軟件構造工件實際尺寸的三維模型，所構造模型的坐標系原點位置即為相機視覺系統坐標系原點（選取彩色相機坐標系為視覺系統的坐標系），并且模型的坐標系原點放置在工件的下底面中心位置。

在PCL 點云庫中，需要將CAD 模型轉換為點云支持的文件格式，例如obj 或ply 格式。為此，首先需要使用SolidWorks 軟件將CAD 模型轉換為ply 格式的文件。然后，將該文件導入到點云轉換程序中，并設置點云生成的合適密度。 需要注意的是，點數過多可能會降低點云配準的計算速度，點數過少則可能會導致配準結果錯誤。 因此，需要根據具體情況選擇適當的點密度，可以通過體素濾波降采樣降低點的采樣密度，進而提高配準的效率和結果。

選取某一個位姿下的目標點云作為模板點云時，將該工件點云作為機械臂的初始抓取位姿。

2.3 SAC-IA 采樣一致性粗配準

為了使點云配準的方向趨于正確，使旋轉平移后的點云之間有較好的初始位置，避免陷入局部最優解的情況［5］，使用SAC-IA 采樣一致性算法獲取初始的目標點云位姿。 如果直接用ICP 迭代最近點算法，會導致計算量過大，而且可能會得不到正確的轉換參數。SAC-IA 算法步驟如下［6］：

（1）從待配準點云P中選取n個采樣點，為了盡量保證所采樣的點具有不同的FPFH 特征，采樣點兩兩之間的距離應該滿足大于預先給定最小距離閾值d。

（2）在目標點云Q中查找與點云P中采樣點具有相似FPFH 特征的一個或多個點，從這些相似點中隨機選取一個點作為點云P在目標點云Q中的一一對應點。

（3）計算對應點之間剛體變換矩陣，然后通過求解對應點變換后與采樣點距離的誤差來判斷當前配準結果的好壞。

（4）重復以上三個步驟，達到迭代次數后，選擇距離誤差和最小的對應點對，作為粗配準的最優結果。

為了測試采樣一致性粗配準算法的有效性，使用多個不同位姿下的工件點云作為目標點云，分別進行與模板點云的配準，并通過配準結果和均方誤差判斷粗配準的位置是否可以作為進行精配準的初始位姿。

2.4 優化迭代最近點精配準算法

ICP（Iterative Closest Point，迭代最近點）［7］算法采用不斷迭代的方式，用最小二乘法得到兩個點云之間位置關系的最優解［8］。其對點云初始位置的要求較為嚴格，為了提高點云配準的成功率和精度，從而獲得更準確的位姿信息，需要先進行粗配準，再使用ICP 算法進行優化。

ICP 算法的核心思想是找到兩個點云中距離最近的對應點對，通過反復迭代，使得點對間的歐氏距離最小，并計算最佳的旋轉矩和平移關系矩陣。該算法步驟為：

（1）將粗配準后的兩片點云P′（經過坐標變換后的源點云）和Q，作為精配準的初始點集。

（2）對源點云P′ 中的每一個點pi，在目標點云Q中尋找距離最近的對應點qi，作為該點在目標點云中的對應點，組成初始對應點對。

（3）初始對應點集中的對應關系并不都是正確的，錯誤的對應關系會影響最終的配準結果，采用方向向量閾值剔除錯誤的對應點對。

（4）計算旋轉矩陣R和平移向量t，使函數的值最小。

（5）設定某一閾值ε和最大迭代次數Nmax，將上一步得到的剛體變換作用與源點云P′，得到新位置點云P″，計算P″ 和Q的距離誤差，如果兩次迭代的誤差小于閾值ε或者當前迭代次數大于Nmax，則迭代結束，否則將初始配準的點集更新為P″ 和Q，繼續重復上述步驟，直到滿足收斂條件。得到最終的旋轉矩陣R與平移矩陣t。

本文使用SVD（奇異值分解）法［9］求解對應點集間的均方誤差，求解步驟如下。

首先，計算兩個點云的質心：

假設最優旋轉矩陣和平移向量為R、t，則Qi與的質心相同，對點云P與Q去中心化得：

將式（9）與式（10）代入式（6）可知：

將式（12）展開，則有：

其中，U、V為3×3 的正交矩陣；Λ 為3×3 的對角矩陣。 令X=VUT，當det (X) = 1，則R=X=VUT，為最優旋轉矩陣，此時。

2.5 Trimmed ICP 精配準

Trimmed ICP 算法由Chetverikov 等人［7］提出，是一種改進的點云精配準算法，它旨在解決兩片點云之間不能完全重合并且包含大量離群點，以及傳統的ICP 算法對于非重疊部分的點對匹配效果不佳等問題。該算法利用截斷最小二乘（Least Trimmed Squares）方法，對匹配點對之間的歐式距離進行排序，選取一定百分比的匹配點對參與目標方程的優化求解，該方法可以有效降低無關點對配準結果的影響，提高配準算法的魯棒性［10］。

其算法原理如下：

對于模板點云P中點的個數不等于目標點云Q中點個數的兩個點集，模板點云中的一些點在目標點集中可能沒有對應關系。假設可以匹配的點的最小保證率為σ，那么可以配對的點數量為NP0=σN，當σ= 1 時就是ICP。

（1）對于P中的每個點，找出Q中最近的點，并計算各個點對之間的距離。

（3）滿足約束條件（與ICP 相同）則退出程序，否則令sum′=sum 并繼續下一步。

（4）利用（2）中選定的前Np0個對應點對，采用最小二乘方法計算最佳變換(R,t)。

（5）利用(R,t) 變換待配準點云P并循環。

重復上述步驟，直到滿足如下三個條件之一時，停止迭代并得出配準結果。

（1）達到最大迭代次數。

（3）兩次迭代值幾乎不變。

3 實驗結果及分析

為驗證本文算法，采用深度相機Kinect v2 分別采集長方體工件和內六角螺絲工件的點云數據進行對比實驗。實驗平臺為Windows10 系統，CPU 主頻2.3 GHz，內存16 G，利用Visual Studio C++編程進行點云配準實驗。

3.1 長方體目標工件

利用SolidWorks 建立工件實際尺寸的三維模型，并導出為點云文件。 由于目標工件是對稱的長方體，且目標點云為工件點云的一部分，如果將目標點云與工件完整點云進行配準，可能會得到不同的位姿信息。 因此，將模型轉換的點云沿z= 0 的平面分割，利用平面上部的點云進行配準。模板點云如圖4 所示。

圖4 分割后的點云模板

為了驗證采樣一致性粗配準算法的可行性，將長方體工件擺放了6 個不同的位姿，分別進行目標點云與模板點云的配準，工件位置擺放方式如圖5 所示。

圖5 長方體工件擺放方式圖

首先計算兩點云的法向量與FPFH 特征值，設置FPFH 搜索半徑為0.1 m。在提取匹配點對后，進行SAC-IA 粗配準。其中第一個位姿粗配準后的結果如圖6 所示。

圖6 長方體工件SAC-IA 粗配準結果

圖6 中模板點云為綠色，目標工件點云為紅色。目標工件在6 個不同位姿下的SAC-IA 粗配準結果如表1 所示，均方誤差MSE 表示迭代結束后所有對應點距離平方和的平均值。

表1 長方體工件SAC-IA配準結果

從表1 中可以計算出粗配準的平均MSE 為0.608 5 mm2，可以作為進行精配準的初始位姿。

使用SAC-IA 粗配準結果作為配準初始位置。分別使用ICP 算法與Trimmed ICP 算法進行精配準，長方體工件其中一個位姿的配準結果如圖7、圖8 所示，分別為ICP 算法和Trimmed ICP算法配準后的結果。 其中紅色表示目標點云，綠色表示模板點云。

圖7 長方體工件ICP 算法配準后的結果

圖8 長方體工件Trimmed ICP 算法配準后的結果

從圖7、圖8 可以看出，ICP 算法配準之后，兩片點云已經大致重合，Trimmed ICP 算法配準之后，兩片點云的各個部位均得到了較好的融合，綠色的模板點云基本配準到紅色的目標點云的對應位置。 為了驗證算法的有效性和優劣，采用均方誤差（MSE）作為點云配準精度的評價指標。表2 統計了6 個不同位姿下精配準ICP 算法和Trimmed ICP 算法的用時與誤差。

表2 長方體工件精配準結果對比

從表2 中可以看出，相對于ICP 算法，Trimmed ICP 算法無論是配準速率還是配準精度都有了明顯的改善。在6 組點云數據實驗中，該算法配準時間平均降低了91%，MSE 降低了17.0%，配準效率得到了提升。

3.2 內六角螺絲目標工件

將內六角螺絲擺放12 個不同的位姿，選取第一個位姿下的工件點云作為模板點云，分別進行其余11 個位姿下目標點云與模板點云的配準，工件位置擺放方式如圖9 所示。

圖9 內六角螺絲工件擺放方式圖

進行采樣一致性粗配準，計算FPFH 特征值，設置FPFH 搜索半徑為0.1 m，在提取匹配點對后進行粗配準。內六角螺絲工件在11 個不同位姿下的SAC-IA 粗配準結果如表3 所示。

表3 內六角螺絲工件SAC-IA配準結果

從表3 中可以計算出粗配準的平均MSE 為0.562 602 73 mm2，可以作為進行精配準的初始位姿。

使用SAC-IA 粗配準結果作為初始配準位置，分別進行ICP 精配準和Trimmed ICP 精配準，內六角螺絲工件其中一個位姿下的配準結果如圖10、圖11 所示。 分別為ICP 算法配準后的結果、Trimmed ICP 算法配準后的結果。 其中紅色表示待配準螺絲工件點云，綠色表示模板點云。

圖10 內六角螺絲工件ICP 算法配準后的結果

圖11 內六角螺絲工件Trimmed ICP 算法配準后的結果

從圖10、圖11 中可以看出，ICP 算法配準之后，兩片點云仍然大致重合，Trimmed ICP 算法配準之后，兩片點云的各個部位均得到了較好的融合，綠色的模板點云基本配準到紅色的目標點云的對應位置。 同時，仍然采用均方誤差（MSE）作為點云配準精度的評價指標。 表4 統計了11 個不同位姿下精配準ICP 算法和Trimmed ICP 算法的用時與誤差數據。

表4 內六角螺絲工件精配準結果對比

從表4 中可以看出，相對于ICP 算法，Trimmed ICP 算法無論是配準速率還是配準精度都有了明顯的改善。在11 組點云數據實驗中，該算法配準時間平均降低了78.2%，MSE 降低了18.0%。由上述分析可知，SAC-IA 粗配準和Trimmed ICP精配準能夠在配準結果有效的基礎上，在誤差允許的范圍內，提高配準精度，并大幅提升配準的收斂速度。

4 結論

本文結合三維點云，完成了對長方體工件和內六角螺絲工件在不同擺放方式下的位姿估計方法研究。通過計算FPFH 特征值提取目標點云和模板點云的特征點對，完成了點云配準實驗。實驗中，SAC-IA 粗配準之后，在優化后的傳統ICP 精配準基礎上，進行了Trimmed ICP 精配準，并統計了不同工件不同擺放方式下兩種配準方法的用時和誤差。上述實驗表明，結合SAC-IA粗配準與Trimmed ICP 精配準的點云配準方法具有相對較高的配準精度和更快的收斂速度，配準效率有了顯著提升。