基于目標(biāo)工件點(diǎn)云的位姿估計(jì)方法研究

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

（長春理工大學(xué) 光電工程學(xué)院，長春 130022）

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

1 點(diǎn)云特征描述方法

點(diǎn)云特征描述是點(diǎn)云識(shí)別、配準(zhǔn)的前提。特征點(diǎn)是指從點(diǎn)云中提取出的具有代表性和區(qū)分性的點(diǎn)集，用于描述點(diǎn)云的特征屬性。 其關(guān)鍵在于從點(diǎn)云中提取所需目標(biāo)的特征描述子，通過目標(biāo)的特征描述子來識(shí)別物體，識(shí)別準(zhǔn)確度依賴于所提取特征描述子的優(yōu)劣［2］。

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

1.1 表面法線

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

式中，Pm為查詢點(diǎn)所在的位置坐標(biāo)；k為查詢點(diǎn)最近鄰域點(diǎn)的個(gè)數(shù)；為最近鄰域點(diǎn)云的質(zhì)心所在的位置坐標(biāo)；為矩陣的第n個(gè)特征值和特征向量，可以用來表示點(diǎn)云的主要方向。

1.2 點(diǎn)特征直方圖

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

圖1 PFH 特征鄰域圖

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

圖2 局部坐標(biāo)系

則有局部坐標(biāo)系為：

法線的偏差角度如下：

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

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

1.3 快速點(diǎn)特征直方圖

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

圖3 FPFH 特征鄰域圖

FPFH 描述子算法步驟如下：

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

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

與PFH 相比，F(xiàn)PFH 算法在計(jì)算查詢點(diǎn)和鄰域點(diǎn)之間的關(guān)系時(shí)，并沒有完全將二者相連。因此，F(xiàn)PFH 算法的計(jì)算值較少。由于采用了加權(quán)的方法，使得其在計(jì)算時(shí)涵蓋了鄰域點(diǎn)的鄰域值，并且某些點(diǎn)可能會(huì)存在于多個(gè)鄰域中，會(huì)存在重復(fù)計(jì)算的情況。

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

2 點(diǎn)云配準(zhǔn)

2.1 點(diǎn)云配準(zhǔn)原理

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

將目標(biāo)工件點(diǎn)云中的一點(diǎn)P1和模板點(diǎn)云中的一點(diǎn)P1′ 作為一個(gè)匹配點(diǎn)對，兩者的旋轉(zhuǎn)變換矩陣為R，平移變換為t，可得：

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

設(shè)目標(biāo)工件點(diǎn)云和模板點(diǎn)云分別為Pi(i=1,2,…,n)、Qi(i= 1,2,…,n)。為了計(jì)算旋轉(zhuǎn)平移變換的最優(yōu)解，使目標(biāo)工件點(diǎn)云和模板點(diǎn)云間的歐式距離之和為最小，則有如下關(guān)系式：

2.2 選取模板點(diǎn)云

目標(biāo)工件的位姿估計(jì)可以通過計(jì)算目標(biāo)工件點(diǎn)云和模板點(diǎn)云的旋轉(zhuǎn)平移變換來實(shí)現(xiàn)。選擇模板點(diǎn)云可以利用CAD 軟件建立目標(biāo)工件的三維模型，并將其轉(zhuǎn)換為點(diǎn)云數(shù)據(jù)。 也可以選取工件某一個(gè)位姿下的目標(biāo)點(diǎn)云作為模板點(diǎn)云。

在CAD 軟件中建立三維模型時(shí)，利用Solid-Works 軟件構(gòu)造工件實(shí)際尺寸的三維模型，所構(gòu)造模型的坐標(biāo)系原點(diǎn)位置即為相機(jī)視覺系統(tǒng)坐標(biāo)系原點(diǎn)（選取彩色相機(jī)坐標(biāo)系為視覺系統(tǒng)的坐標(biāo)系），并且模型的坐標(biāo)系原點(diǎn)放置在工件的下底面中心位置。

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

選取某一個(gè)位姿下的目標(biāo)點(diǎn)云作為模板點(diǎn)云時(shí)，將該工件點(diǎn)云作為機(jī)械臂的初始抓取位姿。

2.3 SAC-IA 采樣一致性粗配準(zhǔn)

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

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

（2）在目標(biāo)點(diǎn)云Q中查找與點(diǎn)云P中采樣點(diǎn)具有相似FPFH 特征的一個(gè)或多個(gè)點(diǎn)，從這些相似點(diǎn)中隨機(jī)選取一個(gè)點(diǎn)作為點(diǎn)云P在目標(biāo)點(diǎn)云Q中的一一對應(yīng)點(diǎn)。

（3）計(jì)算對應(yīng)點(diǎn)之間剛體變換矩陣，然后通過求解對應(yīng)點(diǎn)變換后與采樣點(diǎn)距離的誤差來判斷當(dāng)前配準(zhǔn)結(jié)果的好壞。

（4）重復(fù)以上三個(gè)步驟，達(dá)到迭代次數(shù)后，選擇距離誤差和最小的對應(yīng)點(diǎn)對，作為粗配準(zhǔn)的最優(yōu)結(jié)果。

為了測試采樣一致性粗配準(zhǔn)算法的有效性，使用多個(gè)不同位姿下的工件點(diǎn)云作為目標(biāo)點(diǎn)云，分別進(jìn)行與模板點(diǎn)云的配準(zhǔn)，并通過配準(zhǔn)結(jié)果和均方誤差判斷粗配準(zhǔn)的位置是否可以作為進(jìn)行精配準(zhǔn)的初始位姿。

2.4 優(yōu)化迭代最近點(diǎn)精配準(zhǔn)算法

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

ICP 算法的核心思想是找到兩個(gè)點(diǎn)云中距離最近的對應(yīng)點(diǎn)對，通過反復(fù)迭代，使得點(diǎn)對間的歐氏距離最小，并計(jì)算最佳的旋轉(zhuǎn)矩和平移關(guān)系矩陣。該算法步驟為：

（1）將粗配準(zhǔn)后的兩片點(diǎn)云P′（經(jīng)過坐標(biāo)變換后的源點(diǎn)云）和Q，作為精配準(zhǔn)的初始點(diǎn)集。

（2）對源點(diǎn)云P′ 中的每一個(gè)點(diǎn)pi，在目標(biāo)點(diǎn)云Q中尋找距離最近的對應(yīng)點(diǎn)qi，作為該點(diǎn)在目標(biāo)點(diǎn)云中的對應(yīng)點(diǎn)，組成初始對應(yīng)點(diǎn)對。

（3）初始對應(yīng)點(diǎn)集中的對應(yīng)關(guān)系并不都是正確的，錯(cuò)誤的對應(yīng)關(guān)系會(huì)影響最終的配準(zhǔn)結(jié)果，采用方向向量閾值剔除錯(cuò)誤的對應(yīng)點(diǎn)對。

（4）計(jì)算旋轉(zhuǎn)矩陣R和平移向量t，使函數(shù)的值最小。

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

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

首先，計(jì)算兩個(gè)點(diǎn)云的質(zhì)心：

假設(shè)最優(yōu)旋轉(zhuǎn)矩陣和平移向量為R、t，則Qi與的質(zhì)心相同，對點(diǎn)云P與Q去中心化得：

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

將式（12）展開，則有：

其中，U、V為3×3 的正交矩陣；Λ 為3×3 的對角矩陣。 令X=VUT，當(dāng)det (X) = 1，則R=X=VUT，為最優(yōu)旋轉(zhuǎn)矩陣，此時(shí)。

2.5 Trimmed ICP 精配準(zhǔn)

Trimmed ICP 算法由Chetverikov 等人［7］提出，是一種改進(jìn)的點(diǎn)云精配準(zhǔn)算法，它旨在解決兩片點(diǎn)云之間不能完全重合并且包含大量離群點(diǎn)，以及傳統(tǒng)的ICP 算法對于非重疊部分的點(diǎn)對匹配效果不佳等問題。該算法利用截?cái)嘧钚《耍↙east Trimmed Squares）方法，對匹配點(diǎn)對之間的歐式距離進(jìn)行排序，選取一定百分比的匹配點(diǎn)對參與目標(biāo)方程的優(yōu)化求解，該方法可以有效降低無關(guān)點(diǎn)對配準(zhǔn)結(jié)果的影響，提高配準(zhǔn)算法的魯棒性［10］。

其算法原理如下：

對于模板點(diǎn)云P中點(diǎn)的個(gè)數(shù)不等于目標(biāo)點(diǎn)云Q中點(diǎn)個(gè)數(shù)的兩個(gè)點(diǎn)集，模板點(diǎn)云中的一些點(diǎn)在目標(biāo)點(diǎn)集中可能沒有對應(yīng)關(guān)系。假設(shè)可以匹配的點(diǎn)的最小保證率為σ，那么可以配對的點(diǎn)數(shù)量為NP0=σN，當(dāng)σ= 1 時(shí)就是ICP。

（1）對于P中的每個(gè)點(diǎn)，找出Q中最近的點(diǎn)，并計(jì)算各個(gè)點(diǎn)對之間的距離。

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

（4）利用（2）中選定的前Np0個(gè)對應(yīng)點(diǎn)對，采用最小二乘方法計(jì)算最佳變換(R,t)。

（5）利用(R,t) 變換待配準(zhǔn)點(diǎn)云P并循環(huán)。

重復(fù)上述步驟，直到滿足如下三個(gè)條件之一時(shí)，停止迭代并得出配準(zhǔn)結(jié)果。

（1）達(dá)到最大迭代次數(shù)。

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

3 實(shí)驗(yàn)結(jié)果及分析

為驗(yàn)證本文算法，采用深度相機(jī)Kinect v2 分別采集長方體工件和內(nèi)六角螺絲工件的點(diǎn)云數(shù)據(jù)進(jìn)行對比實(shí)驗(yàn)。實(shí)驗(yàn)平臺(tái)為Windows10 系統(tǒng)，CPU 主頻2.3 GHz，內(nèi)存16 G，利用Visual Studio C++編程進(jìn)行點(diǎn)云配準(zhǔn)實(shí)驗(yàn)。

3.1 長方體目標(biāo)工件

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

圖4 分割后的點(diǎn)云模板

為了驗(yàn)證采樣一致性粗配準(zhǔn)算法的可行性，將長方體工件擺放了6 個(gè)不同的位姿，分別進(jìn)行目標(biāo)點(diǎn)云與模板點(diǎn)云的配準(zhǔn)，工件位置擺放方式如圖5 所示。

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

首先計(jì)算兩點(diǎn)云的法向量與FPFH 特征值，設(shè)置FPFH 搜索半徑為0.1 m。在提取匹配點(diǎn)對后，進(jìn)行SAC-IA 粗配準(zhǔn)。其中第一個(gè)位姿粗配準(zhǔn)后的結(jié)果如圖6 所示。

圖6 長方體工件SAC-IA 粗配準(zhǔn)結(jié)果

圖6 中模板點(diǎn)云為綠色，目標(biāo)工件點(diǎn)云為紅色。目標(biāo)工件在6 個(gè)不同位姿下的SAC-IA 粗配準(zhǔn)結(jié)果如表1 所示，均方誤差MSE 表示迭代結(jié)束后所有對應(yīng)點(diǎn)距離平方和的平均值。

表1 長方體工件SAC-IA配準(zhǔn)結(jié)果

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

使用SAC-IA 粗配準(zhǔn)結(jié)果作為配準(zhǔn)初始位置。分別使用ICP 算法與Trimmed ICP 算法進(jìn)行精配準(zhǔn)，長方體工件其中一個(gè)位姿的配準(zhǔn)結(jié)果如圖7、圖8 所示，分別為ICP 算法和Trimmed ICP算法配準(zhǔn)后的結(jié)果。 其中紅色表示目標(biāo)點(diǎn)云，綠色表示模板點(diǎn)云。

圖7 長方體工件ICP 算法配準(zhǔn)后的結(jié)果

圖8 長方體工件Trimmed ICP 算法配準(zhǔn)后的結(jié)果

從圖7、圖8 可以看出，ICP 算法配準(zhǔn)之后，兩片點(diǎn)云已經(jīng)大致重合，Trimmed ICP 算法配準(zhǔn)之后，兩片點(diǎn)云的各個(gè)部位均得到了較好的融合，綠色的模板點(diǎn)云基本配準(zhǔn)到紅色的目標(biāo)點(diǎn)云的對應(yīng)位置。 為了驗(yàn)證算法的有效性和優(yōu)劣，采用均方誤差（MSE）作為點(diǎn)云配準(zhǔn)精度的評價(jià)指標(biāo)。表2 統(tǒng)計(jì)了6 個(gè)不同位姿下精配準(zhǔn)ICP 算法和Trimmed ICP 算法的用時(shí)與誤差。

表2 長方體工件精配準(zhǔn)結(jié)果對比

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

3.2 內(nèi)六角螺絲目標(biāo)工件

將內(nèi)六角螺絲擺放12 個(gè)不同的位姿，選取第一個(gè)位姿下的工件點(diǎn)云作為模板點(diǎn)云，分別進(jìn)行其余11 個(gè)位姿下目標(biāo)點(diǎn)云與模板點(diǎn)云的配準(zhǔn)，工件位置擺放方式如圖9 所示。

圖9 內(nèi)六角螺絲工件擺放方式圖

進(jìn)行采樣一致性粗配準(zhǔn)，計(jì)算FPFH 特征值，設(shè)置FPFH 搜索半徑為0.1 m，在提取匹配點(diǎn)對后進(jìn)行粗配準(zhǔn)。內(nèi)六角螺絲工件在11 個(gè)不同位姿下的SAC-IA 粗配準(zhǔn)結(jié)果如表3 所示。

表3 內(nèi)六角螺絲工件SAC-IA配準(zhǔn)結(jié)果

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

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

圖10 內(nèi)六角螺絲工件ICP 算法配準(zhǔn)后的結(jié)果

圖11 內(nèi)六角螺絲工件Trimmed ICP 算法配準(zhǔn)后的結(jié)果

從圖10、圖11 中可以看出，ICP 算法配準(zhǔn)之后，兩片點(diǎn)云仍然大致重合，Trimmed ICP 算法配準(zhǔn)之后，兩片點(diǎn)云的各個(gè)部位均得到了較好的融合，綠色的模板點(diǎn)云基本配準(zhǔn)到紅色的目標(biāo)點(diǎn)云的對應(yīng)位置。 同時(shí)，仍然采用均方誤差（MSE）作為點(diǎn)云配準(zhǔn)精度的評價(jià)指標(biāo)。 表4 統(tǒng)計(jì)了11 個(gè)不同位姿下精配準(zhǔn)ICP 算法和Trimmed ICP 算法的用時(shí)與誤差數(shù)據(jù)。

表4 內(nèi)六角螺絲工件精配準(zhǔn)結(jié)果對比

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

4 結(jié)論

本文結(jié)合三維點(diǎn)云，完成了對長方體工件和內(nèi)六角螺絲工件在不同擺放方式下的位姿估計(jì)方法研究。通過計(jì)算FPFH 特征值提取目標(biāo)點(diǎn)云和模板點(diǎn)云的特征點(diǎn)對，完成了點(diǎn)云配準(zhǔn)實(shí)驗(yàn)。實(shí)驗(yàn)中，SAC-IA 粗配準(zhǔn)之后，在優(yōu)化后的傳統(tǒng)ICP 精配準(zhǔn)基礎(chǔ)上，進(jìn)行了Trimmed ICP 精配準(zhǔn)，并統(tǒng)計(jì)了不同工件不同擺放方式下兩種配準(zhǔn)方法的用時(shí)和誤差。上述實(shí)驗(yàn)表明，結(jié)合SAC-IA粗配準(zhǔn)與Trimmed ICP 精配準(zhǔn)的點(diǎn)云配準(zhǔn)方法具有相對較高的配準(zhǔn)精度和更快的收斂速度，配準(zhǔn)效率有了顯著提升。