基于優化ICP算法的機器人雙目相機點云配準

陳壯，陳閃

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

0 引言

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

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

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

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

1 雙目立體視覺模型

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

圖1 雙目相機安裝位置

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

圖2 雙目相機成像原理

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

圖3 RGB圖像和深度圖

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

(1)

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

2 點云預處理及配準

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

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

圖4 預處理后的點云

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

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

qi+1=R×qi+T

(2)

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

(3)

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

設兩個點云集的質心為μs和μq，構造互協方差方程∑S-Q，求得四元對稱矩陣G(∑S-Q)，此矩陣對應R，代入T=μs-Rμq求得最優解。

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

(4)

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

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

3 基于KD-Tree優化點云配準

KD-Tree屬于二進制K維空間的數據結構。廣泛應用于數據集的最近鄰搜索。本研究基于KD-Tree鄰域特征優化ICP算法，加速點云全局匹配，優化點云數據結構，以更快的速度完成精配準，使得優化后的ICP建立了點云臨近點的對應關系，更能合理規劃點云的數據結構。將其與點云重建領域相結合，對點云數據的三維空間信息進行分割，根據KD-Tree的分叉樹特性，確定劃分左子樹和右子樹[12]。KD-Tree分為查詢和回溯兩個過程，通過查詢來查找數據點，通過回溯來保證所查找近鄰點的正確性?；厮莸哪康氖菍γ恳粋€子樹數據集進行查詢，確保最近鄰點的有效性[13]。具體近鄰搜索方法流程為：

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

(5)

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

3)比較該子節點和鄰域內目標點之間的歐式距離dk，刪除子集合外的點，若未達到最小距離，則依據新點集的質心設定新的鄰域。

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

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

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

4 點云配準實驗

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

圖5 三維模型和點云模型

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

圖6 第1組點云配準結果

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

表1 代碼執行效率表

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

5 結語

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