改進ICP算法在屏風骨架點云配準中的應用研究

邰鳳陽 王化明

（南京航空航天大學機電學院 江蘇省南京市 210016）

1 引言

屏風的上料過程大多為人工操作，不但費時費力而且精度也無法保障。抓取隨機擺放的物體需要計算物體的空間位姿，而傳統機器只能按事先編好的程序動作，無法完成這種帶隨機性的任務。由于點云數據包含深度信息，在場景感知、工業測量等多方面具有重要的應用，因此可以指導機械臂對屏風進行自動化上料，在節省人力成本的同時提高生產效率。

目前，基于點云的物體位姿方法主要分為基于對應點的方法、基于模板的方法和基于投票的方法三類[1]。點云配準屬于基于模板的位姿估計方法，其中最經典算法是Besl提出的ICP算法。該算法簡單、易于實現，但要求原始點云和目標點云有較好的初始位姿，而且對噪聲和缺失較為敏感。因此，眾多學者提出一系列改進算法。在國外，Chetverikov等[2]提出TrICP算法，對低重疊率點云仍可以較好的配準。Censi等[3]提出PLICP算法，該算法的距離度量標準是點到線的距離，從而加速迭代計算過程。Serafin等[4]提出了一種考慮法線和曲率等局部表面特征的點云配準方法。在國內，朱琛琛等[5]通過提取點云的ISS特征點作為對應點以加快配準的速度，并通過距離閾值過濾錯誤點對來提高魯棒性。李為名等[6]提出一種基于PCA的ICP改進算法，利用主成分分析計算兩組點云的主軸方向進行粗配準，再進行精配準。郎萍[7]提出一種基于旋轉圖像特征描述子改進的ICP算法。

針對屏風較扁平的形狀特征、紋理較弱及在生產過程中易發生堆疊的特點，本文采用基于點云配準的位姿估計方法。算法分為粗配準和精配準兩部分，通過屏風點云邊界提取和頂點提取的結果來進行粗配準，用粗配準得到的位姿變換矩陣作為精配準的初始位姿矩陣，使用歐氏距離約束以去除匹配較差的點對；根據屏風上料抓取任務的特點，在精配準中引入權重系數以提升算法的魯棒性，并對旋轉矩陣進行軸向校正以解決上料定位時可能發生的軸向反向問題。

2 屏風骨架點云邊界和頂點提取

圖1顯示了屏風的實物圖和點云圖，其外形近似為矩形，骨架部分為金屬，其他部分表面貼有蒙布。通過三維掃描直接獲得的屏風點云數據量巨大，因此在處理時需要對其精簡以提高處理效率。屏風點云的幾何特征主要以平面和直線為主，可以通過提取其頂點位置來確定粗略位姿。

圖1：屏風的實物圖和點云圖

2.1 屏風骨架點云邊界提取

為了提取屏風骨架點云的頂點，需要先提取其邊界。常用的點云的邊界提取方法有經緯線掃描法[8]、網格劃分法[9]、梯度估計法[10]等。本文采用經緯線掃描法提取屏風骨架點云邊界，該方法簡單、計算量小，其步驟如下：

Step1求出點云x坐標值的最大值xmax和最小值xmin；

Step3掃描點云，統計出x坐標值在[xmin+(i-1)Δx,xmin+iΔx)(i=1,2,L,res)范圍內y坐標取最小值和最大值時的點，即為沿x方向掃描提取得到的點云邊界點集；

Step4 仿上述步驟，沿y方向對點云再次掃描，兩次提取的點集組成點云的邊界。

2.2 屏風骨架點云頂點提取

得到點云邊界后，使用RANSAC算法對屏風骨架點云邊界做直線擬合。通過直線擬合可以得到屏風骨架邊框四條邊所在的直線方程，進而可以計算四個頂點的坐標。計算兩條空間直線的交點，就是求它們最小公垂線兩個垂足的中點。

通過窮舉法求出兩兩組合的空間直線的交點，如果求出的兩個垂足點距離小于給定閾值Δd，則認為該直線組合的交點為頂點。統計提取出的頂點個數，若等4于則認為頂點提取成功，否則頂點提取失敗。若提取失敗，則需增大距離閾值并重復上述步驟。

使用經緯線掃描法對經過濾波、分割等預處理得到的屏風骨架點云提取邊界，并在邊界提取的基礎上提取頂點，結果如圖2所示。

圖2：屏風骨架點云邊界提取和頂點提取結果

3 屏風骨架點云配準方法

傳統的ICP算法對點云初始位姿要求較高，在迭代中容易陷入局部極小值，而且收斂速度不夠快。為了解決這些問題，可以采用由粗到精的配準方法[11]。本文中的屏風骨架點云配準方法由粗配準和精配準組成，以實現快速、穩定的點云配準。

3.1 基于距離約束的粗配準

在上一節中已經得到了屏風點云四個頂點的坐標，可以用這四點做點云粗配準，給精配準提供較好的初始位姿。設原始點云的四個頂點分別為p1，p2，p3，p4，目標點云的四個頂點分別為q1，q2，q3，q4，給定距離約束閾值d，則可以通過如下距離約束確定原始點云頂點和目標點云頂點的對應關系：

設原始點云頂點的點集為P，目標點云頂點的點集為Q，而且這些點集中點的對應關系已經確定，因此可以通過SVD分解來確定粗配準的位姿變換矩陣。

經過粗配準，原始點云和目標點云已經大致對齊。在粗配準后，利用對應點對間的歐式距離約束，可以去除部分匹配效果不佳的點對。設p1和p2分別是粗配準得到點云和目標點云上的對應點對，c1和c2分別是粗配準得到點云和目標點云的幾何中心，如果滿足關系：

則認為p1和p2的匹配效果較好；反之認為匹配

效果不好，并去除該對應點對。

3.2 引入權重系數和軸向校正的精配準

精配準的目的是對粗配準得到的位姿矩陣進行細微調整，使最終結果更加準確。為了提升算法的魯棒性，本文引入了權重系數。

傳統的ICP算法在目標殘差函數中，所有匹配點對具有相同的影響程度。然而在實際配準問題中，待配準點云的不同區域可能具有不同重要程度。在屏風上料過程中，抓取任務往往針對的是其中心的“十”字型肋條；另外在屏風的運輸過程中，邊框容易受到擠壓而發生變形，因此靠近屏風中心處肋條的權重應高于其他區域的權重。

設權重系數α>1，屏風點云中點到點云中心的距離為d，給定閾值dmin，權重σ可用公式表示如下：

傳統ICP算法的核心是求解優化問題：

其中H為位姿變換矩陣，Np為原始點云中點的個數，pi和qi分別為原始點云和目標點云中的點。

引入權重信息后，優化問題轉變為：

由于屏風點云的形狀近似對稱，在配準中會出現和模型點云軸向方向相反的問題，這可能導致最后計算出的機械臂抓取位姿不合理，因此要對旋轉矩陣進行約束校正以確保原始點云和目標點云軸向同向。設有矩陣：

旋轉矩陣R對角線上的三個元素分別為R11、R22、R33，在每輪迭代計算中依次判斷R11、R22、R33的符號并進行如下操作：若R11<0，則R=RxR；若R22<0，則R=RyR；若R33<0，則R=RzR。

4 試驗和結果

本文采用自行研制的線結構光掃描系統獲得屏風點云。掃描得到的屏風點云經過降采樣、分割等處理得到屏風骨架點云，其點數約為3萬，點距約為0.4mm，用于和通過CAD生成已知尺寸的三維模型點云進行配準。本文程序的測試運行環境為Intel Core i7-9750H處理器，內存為16GB。

如圖3所示，經過粗配準，原始點云和目標點云已經基本對齊。在尋找對應點時，采用KD-tree儲存點云以加速搜索速度；取δ=0.1，并使用歐氏距離約束去除匹配較差的對應點對。通過實驗，如果直接對原始點云和目標點云進行精配準，則算法迭代次數會達到設定的最大次數（kmax=50），耗時530ms。若采用先粗配準再精配準的方法，則粗配準部分耗時1ms，精配準部分耗時287ms，總耗時少于直接配準。

圖3：屏風點云配準結果

如圖4所示，在原始點云和目標點云相對初始位姿相差較大的情況下，本文算法仍然能較好的工作，其他配準算法如ICP算法往往產生錯誤的配準結果。如表1所示，通過本文算法和其他點云配準算法的性能對比，可看出本文算法具有速度快、精度高的特點。

圖4：初始位姿相差較大的配準

表1：不同配準算法性能對比

5 結語

本文提出一種基于改進ICP算法的屏風骨架點云配準方法，為解決實際應用中屏風自動化上料抓取的位姿估計問題打下了基礎。本文針對屏風的形狀特點使用點云邊界提取和頂點提取的結果來進行粗配準；根據上料抓取任務的特點，在精配準中引入權重系數以增強算法的魯棒性，并對旋轉矩陣進行軸向校正以解決上料定位時可能發生的軸向反向問題。通過試驗證明，該算法針對屏風點云配準有較高的配準精度以及較快的收斂速度，并且在原始點云和目標點云初始位姿相差較大的情況下具有較強的魯棒性。