基于優(yōu)化ICP算法的機器人雙目相機點云配準

陳壯，陳閃

(1. 芯恩(青島)集成電路有限公司，山東 青島 266426； 2. 青島濱海學院 機電工程學院，山東 青島 266555)

0 引言

機器人末端執(zhí)行器通常要求較高的抓取精度和識別效率，物體的抓取位姿需要借助外部圖像采集設(shè)備，結(jié)合點云配準求解獲得[1-2]。本研究使用雙目相機作為視覺輸入，重建待抓取物體并生成點云數(shù)據(jù)集，在計算機上呈現(xiàn)物體的三維體貌[3]，再結(jié)合手眼標定得到物體相對機器人末端執(zhí)行器的抓取位姿。點云重建與配準依賴于PCL點云庫和Opencv圖像處理庫[4-5]，點云配準的可靠性直接影響著抓取精度。

點云配準分為局部特征搜索配準法和全局特征搜索配準法兩大類[6]。考慮到Kinova機器人抓取視野范圍較小且周圍噪聲點較多，因而選用具備局部拓撲關(guān)系特性的局部特征搜索配準法。點云配準占用了位姿解算的大部分時間。優(yōu)化點云配準一般從點云存儲結(jié)構(gòu)、迭代次數(shù)、配準精度等方面入手，此外也要考慮在保證全局收斂的前提下，避免陷入局部最優(yōu)解。

DISCHER S等基于KD-Tree數(shù)據(jù)結(jié)構(gòu)改進了點云存儲方式，提高了檢索局部特征點的執(zhí)行效率[7]。LAMINE T M等針對稀疏點云相鄰點間的對應(yīng)關(guān)系，提出基于超密度概念的局部特征搜索方法，保證了不同分辨率傳感器獲取的不同密度點云的配準準確性[8]。DOS SANTOS J GJr等基于粒子群優(yōu)化理論優(yōu)化了RGB-D圖像，生成3D對象跟蹤器，解決了物體部分遮擋問題，縮短了配準時GPU處理點云的運行時間，減少了迭代次數(shù)[9]。

本研究以機器人抓取空間中物體為例，簡述雙目相機成像模型，介紹物體三維重建以及點云配準等技術(shù)。基于KD-Tree加速鄰域算法優(yōu)化ICP，旨在簡化目標物的識別定位流程，提高物體抓取位姿效率及解算精度。

1 雙目立體視覺模型

雙目視覺為物體的三維重建提供原始圖像輸入，通過RGB-D圖像以及相機內(nèi)外參數(shù)矩陣，生成物體點云數(shù)據(jù)集。雙目相機與Kinova機器人采用如圖1所示“手在眼外”的安裝方式，為雙目相機{Cl}{Cr}與機器人基座{B}的相對位置關(guān)系。

圖1 雙目相機安裝位置

雙目相機成像及矯正原理如圖2所示。極點el和er位于成像光心連接線上，ll和lr表示共軛極線，Il和Ir表示相平面，空間任意點P與光心Ol和Or構(gòu)成截交極平面S。

圖2 雙目相機成像原理

調(diào)用Bouguet算法矯正雙目相機，消除畸變，使基線b平行于相平面。保證在兩像平面同一行高度搜尋匹配點完成立體匹配。根據(jù)Semi-Global Block Matching(SGBM)半全局匹配算法生成如圖3所示的8位視差圖。

圖3 RGB圖像和深度圖

視差圖像包含著空間各點的位置信息，根據(jù)雙目視覺平行幾何關(guān)系歸一化焦距，映射像素單位至公制尺寸單位[10]。設(shè)SGBM計算導出的視差絕對值為d，由雙目視覺幾何關(guān)系求得每個像素對應(yīng)的深度值h為

(1)

遍歷深度圖像的每個像素的h，表征著相機距離物體的實際物理距離Z，再結(jié)合各點xy坐標，生成三維點云圖。

2 點云預處理及配準

以雙目視覺生成的RGB-D16位深度圖為數(shù)據(jù)輸入，通過PCL點云庫，重建物體點云，將生成點云的標記為目標點云集。

原始生成的點云數(shù)據(jù)集往往信息量較大，需使用VoxelGrid filter算法創(chuàng)建局部體素柵格進行下采樣濾波，疏散點云以提高處理速度。因環(huán)境光照或遮擋產(chǎn)生噪聲和離群點，也需要對其進行降噪。為了方便提取點云輪廓，也需要消除背景。如圖4所示，提取左上角的點云作為實驗對象，預處理后的點云才能滿足后續(xù)配準的需要。

圖4 預處理后的點云

使用Solid Wroks建立物體的1∶1模型并劃分曲面網(wǎng)格，作為源點云集。點云的配準分為兩步，首先進行SAC-IA粗配準，盡可能趨近兩點云集[11]。接著進行ICP精配準，以粗配準后的點云作為迭代初始位置，將精配準生成的目標點云與源點云矩陣之間的方均根差設(shè)為迭代中止條件。ICP精配準的實質(zhì)是按照已有的局部特征點，以最小二乘法進行矩陣剛體運算，迭代數(shù)次后得到最優(yōu)旋轉(zhuǎn)矩陣和位移矩陣，實現(xiàn)目標點云與源點云數(shù)據(jù)集的相交區(qū)域特征描述符最大限度重合。配準前后只變更點云的相對位置，不改變點云數(shù)據(jù)。

設(shè)源點云集S的點云數(shù)為ns。目標點云集Q的點云數(shù)為nq。配準過程中S點云集中的任意點si在Q點云集中搜尋對應(yīng)點qi。設(shè)R表示點云空間旋轉(zhuǎn)矩陣，T表示空間位移矩陣，則相鄰兩次目標點云變換后的表示形式如下：

qi+1=R×qi+T

(2)

建立歸一化非線性最小二乘法的目標函數(shù)的最小值為MinF(R,T)：

(3)

式中qi表示S點云集中的任意點si對應(yīng)于Q點云集定點直線距離最小值點。

設(shè)兩個點云集的質(zhì)心為μs和μq，構(gòu)造互協(xié)方差方程∑S-Q，求得四元對稱矩陣G(∑S-Q)，此矩陣對應(yīng)R，代入T=μs-Rμq求得最優(yōu)解。

目標點云集Q通過求解得到的R和T矩陣變換后得到新的目標點云集Q′，新點記為q′i，迭代k次后，計算新目標點云集Q′與源點云集S的歐式距離：

(4)

相鄰兩次迭代前后的匹配點對應(yīng)距離呈收斂趨勢，完成第一次目標點云集Q以最優(yōu)解矩陣向源點云集S靠近的過程。多次重復矩陣轉(zhuǎn)換，根據(jù)實際情況設(shè)定迭代終止閾值δ=0.05，當dk+1-dk<δ成立時，停止迭代，得到的矩陣經(jīng)過坐標轉(zhuǎn)換成為抓取位姿矩陣。

ICP算法在局部尋優(yōu)時出錯率較高，容易陷入局部最優(yōu)解，此外原始的數(shù)據(jù)結(jié)構(gòu)不利于處理數(shù)據(jù)量大的點云集。為了改善傳統(tǒng)ICP點云的局限性，需優(yōu)化其數(shù)據(jù)結(jié)構(gòu)和迭代效率，使其更能滿足各類場景的點云配準。常用的優(yōu)化方式為粒子群或KD-Tree，在保證配準精度的前提下，極大地提高了包圍盒內(nèi)臨近點的搜索速度。

3 基于KD-Tree優(yōu)化點云配準

KD-Tree屬于二進制K維空間的數(shù)據(jù)結(jié)構(gòu)。廣泛應(yīng)用于數(shù)據(jù)集的最近鄰搜索。本研究基于KD-Tree鄰域特征優(yōu)化ICP算法，加速點云全局匹配，優(yōu)化點云數(shù)據(jù)結(jié)構(gòu)，以更快的速度完成精配準，使得優(yōu)化后的ICP建立了點云臨近點的對應(yīng)關(guān)系，更能合理規(guī)劃點云的數(shù)據(jù)結(jié)構(gòu)。將其與點云重建領(lǐng)域相結(jié)合，對點云數(shù)據(jù)的三維空間信息進行分割，根據(jù)KD-Tree的分叉樹特性，確定劃分左子樹和右子樹[12]。KD-Tree分為查詢和回溯兩個過程，通過查詢來查找數(shù)據(jù)點，通過回溯來保證所查找近鄰點的正確性。回溯的目的是對每一個子樹數(shù)據(jù)集進行查詢，確保最近鄰點的有效性[13]。具體近鄰搜索方法流程為：

1)已知源點云集S任意點si的鄰域集合為Si，目標點云集Q任意點qi的鄰域集合為Qi，默認點云為理想包容塊，結(jié)合鄰域集合點云集各點相對于質(zhì)心μs和μq位置關(guān)系，求解點云集為最大方差維度k。

(5)

2)從中選取方差最大的第k維，將其作為分割軸，過中值點P作為劃分軸對點數(shù)據(jù)集合進行劃分，創(chuàng)建兩個子數(shù)據(jù)集。同時判定優(yōu)先級，判別左子樹和右子樹的搜索路徑。

3)比較該子節(jié)點和鄰域內(nèi)目標點之間的歐式距離dk，刪除子集合外的點，若未達到最小距離，則依據(jù)新點集的質(zhì)心設(shè)定新的鄰域。

4)以目標點為中心向周圍閾值半徑內(nèi)建立包容塊，繼續(xù)搜索鄰域目標點。

5)兩個子樹區(qū)域分別由異于之前軸建立的分割面進行等分；再將新生成的子樹區(qū)域順序等分，當后續(xù)包容塊為空集時，停止區(qū)域分割。

6)當子節(jié)點與目標點的歐氏距離滿足收斂閾值時，或者搜索回溯到KD-Tree根結(jié)點時，則KD-Tree回溯完成，保證搜索所得結(jié)點為最近臨近結(jié)點。

4 點云配準實驗

雙目相機拍攝機器人抓取物體的場景，經(jīng)過立體匹配及矩陣轉(zhuǎn)換后獲取RGB-D圖像信息，由PCL點云庫生成抓取場景點云集。使用Solid Works建立如圖5所示的易拉罐三維模型并劃分網(wǎng)格。

圖5 三維模型和點云模型

提取圖4左上角易拉罐目標點云與源點云進行三組配準實驗，對比驗證“ICP精配準”和“優(yōu)化后ICP精配準”，比較代碼執(zhí)行效率及精度驗證算法有效性和可行性。如圖6所示，源點云與目標點云每次迭代后趨于重合，進行29次后停止迭代。粗配準后的點云，收斂至設(shè)定閾值δ=0.05，完成點云配準，再結(jié)合手眼標定得到待抓取物體相對機器人末端執(zhí)行器的抓取位姿矩陣：

圖6 第1組點云配準結(jié)果

如表1所示，對比了兩個算法收斂后的迭代次數(shù)和執(zhí)行時間。

表1 代碼執(zhí)行效率表

由式(4)計算得出的歐氏距離差值作為迭代精度判據(jù)。顯然在保證了迭代精度的前提下，經(jīng)過KD-Tree優(yōu)化后ICP配準的迭代效率更高，以更少的迭代次數(shù)收斂至最優(yōu)解。

5 結(jié)語

本研究簡述了雙目視覺三維重建的基本理論，介紹了ICP配準的思路和算法。結(jié)合機器人實際抓取場景對比驗證了傳統(tǒng)ICP精配準算法和基于KD-Tree數(shù)據(jù)結(jié)構(gòu)來優(yōu)化的點云配準算法。從點云配準結(jié)果明顯看出，優(yōu)化的ICP配準算法，縮短了特征點匹配時間，減少了迭代次數(shù)，避免局部錯誤收斂，提升了代碼執(zhí)行效率，同時也控制了點云配準的誤差。