改進(jìn)ICP算法在屏風(fēng)骨架點(diǎn)云配準(zhǔn)中的應(yīng)用研究

邰鳳陽(yáng) 王化明

（南京航空航天大學(xué)機(jī)電學(xué)院 江蘇省南京市 210016）

1 引言

屏風(fēng)的上料過(guò)程大多為人工操作，不但費(fèi)時(shí)費(fèi)力而且精度也無(wú)法保障。抓取隨機(jī)擺放的物體需要計(jì)算物體的空間位姿，而傳統(tǒng)機(jī)器只能按事先編好的程序動(dòng)作，無(wú)法完成這種帶隨機(jī)性的任務(wù)。由于點(diǎn)云數(shù)據(jù)包含深度信息，在場(chǎng)景感知、工業(yè)測(cè)量等多方面具有重要的應(yīng)用，因此可以指導(dǎo)機(jī)械臂對(duì)屏風(fēng)進(jìn)行自動(dòng)化上料，在節(jié)省人力成本的同時(shí)提高生產(chǎn)效率。

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

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

2 屏風(fēng)骨架點(diǎn)云邊界和頂點(diǎn)提取

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

圖1：屏風(fēng)的實(shí)物圖和點(diǎn)云圖

2.1 屏風(fēng)骨架點(diǎn)云邊界提取

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

Step1求出點(diǎn)云x坐標(biāo)值的最大值xmax和最小值xmin；

Step3掃描點(diǎn)云，統(tǒng)計(jì)出x坐標(biāo)值在[xmin+(i-1)Δx,xmin+iΔx)(i=1,2,L,res)范圍內(nèi)y坐標(biāo)取最小值和最大值時(shí)的點(diǎn)，即為沿x方向掃描提取得到的點(diǎn)云邊界點(diǎn)集；

Step4 仿上述步驟，沿y方向?qū)c(diǎn)云再次掃描，兩次提取的點(diǎn)集組成點(diǎn)云的邊界。

2.2 屏風(fēng)骨架點(diǎn)云頂點(diǎn)提取

得到點(diǎn)云邊界后，使用RANSAC算法對(duì)屏風(fēng)骨架點(diǎn)云邊界做直線擬合。通過(guò)直線擬合可以得到屏風(fēng)骨架邊框四條邊所在的直線方程，進(jìn)而可以計(jì)算四個(gè)頂點(diǎn)的坐標(biāo)。計(jì)算兩條空間直線的交點(diǎn)，就是求它們最小公垂線兩個(gè)垂足的中點(diǎn)。

通過(guò)窮舉法求出兩兩組合的空間直線的交點(diǎn)，如果求出的兩個(gè)垂足點(diǎn)距離小于給定閾值Δd，則認(rèn)為該直線組合的交點(diǎn)為頂點(diǎn)。統(tǒng)計(jì)提取出的頂點(diǎn)個(gè)數(shù)，若等4于則認(rèn)為頂點(diǎn)提取成功，否則頂點(diǎn)提取失敗。若提取失敗，則需增大距離閾值并重復(fù)上述步驟。

使用經(jīng)緯線掃描法對(duì)經(jīng)過(guò)濾波、分割等預(yù)處理得到的屏風(fēng)骨架點(diǎn)云提取邊界，并在邊界提取的基礎(chǔ)上提取頂點(diǎn)，結(jié)果如圖2所示。

圖2：屏風(fēng)骨架點(diǎn)云邊界提取和頂點(diǎn)提取結(jié)果

3 屏風(fēng)骨架點(diǎn)云配準(zhǔn)方法

傳統(tǒng)的ICP算法對(duì)點(diǎn)云初始位姿要求較高，在迭代中容易陷入局部極小值，而且收斂速度不夠快。為了解決這些問(wèn)題，可以采用由粗到精的配準(zhǔn)方法[11]。本文中的屏風(fēng)骨架點(diǎn)云配準(zhǔn)方法由粗配準(zhǔn)和精配準(zhǔn)組成，以實(shí)現(xiàn)快速、穩(wěn)定的點(diǎn)云配準(zhǔn)。

3.1 基于距離約束的粗配準(zhǔn)

在上一節(jié)中已經(jīng)得到了屏風(fēng)點(diǎn)云四個(gè)頂點(diǎn)的坐標(biāo)，可以用這四點(diǎn)做點(diǎn)云粗配準(zhǔn)，給精配準(zhǔn)提供較好的初始位姿。設(shè)原始點(diǎn)云的四個(gè)頂點(diǎn)分別為p1，p2，p3，p4，目標(biāo)點(diǎn)云的四個(gè)頂點(diǎn)分別為q1，q2，q3，q4，給定距離約束閾值d，則可以通過(guò)如下距離約束確定原始點(diǎn)云頂點(diǎn)和目標(biāo)點(diǎn)云頂點(diǎn)的對(duì)應(yīng)關(guān)系：

設(shè)原始點(diǎn)云頂點(diǎn)的點(diǎn)集為P，目標(biāo)點(diǎn)云頂點(diǎn)的點(diǎn)集為Q，而且這些點(diǎn)集中點(diǎn)的對(duì)應(yīng)關(guān)系已經(jīng)確定，因此可以通過(guò)SVD分解來(lái)確定粗配準(zhǔn)的位姿變換矩陣。

經(jīng)過(guò)粗配準(zhǔn)，原始點(diǎn)云和目標(biāo)點(diǎn)云已經(jīng)大致對(duì)齊。在粗配準(zhǔn)后，利用對(duì)應(yīng)點(diǎn)對(duì)間的歐式距離約束，可以去除部分匹配效果不佳的點(diǎn)對(duì)。設(shè)p1和p2分別是粗配準(zhǔn)得到點(diǎn)云和目標(biāo)點(diǎn)云上的對(duì)應(yīng)點(diǎn)對(duì)，c1和c2分別是粗配準(zhǔn)得到點(diǎn)云和目標(biāo)點(diǎn)云的幾何中心，如果滿足關(guān)系：

則認(rèn)為p1和p2的匹配效果較好；反之認(rèn)為匹配

效果不好，并去除該對(duì)應(yīng)點(diǎn)對(duì)。

3.2 引入權(quán)重系數(shù)和軸向校正的精配準(zhǔn)

精配準(zhǔn)的目的是對(duì)粗配準(zhǔn)得到的位姿矩陣進(jìn)行細(xì)微調(diào)整，使最終結(jié)果更加準(zhǔn)確。為了提升算法的魯棒性，本文引入了權(quán)重系數(shù)。

傳統(tǒng)的ICP算法在目標(biāo)殘差函數(shù)中，所有匹配點(diǎn)對(duì)具有相同的影響程度。然而在實(shí)際配準(zhǔn)問(wèn)題中，待配準(zhǔn)點(diǎn)云的不同區(qū)域可能具有不同重要程度。在屏風(fēng)上料過(guò)程中，抓取任務(wù)往往針對(duì)的是其中心的“十”字型肋條；另外在屏風(fēng)的運(yùn)輸過(guò)程中，邊框容易受到擠壓而發(fā)生變形，因此靠近屏風(fēng)中心處肋條的權(quán)重應(yīng)高于其他區(qū)域的權(quán)重。

設(shè)權(quán)重系數(shù)α>1，屏風(fēng)點(diǎn)云中點(diǎn)到點(diǎn)云中心的距離為d，給定閾值dmin，權(quán)重σ可用公式表示如下：

傳統(tǒng)ICP算法的核心是求解優(yōu)化問(wèn)題：

其中H為位姿變換矩陣，Np為原始點(diǎn)云中點(diǎn)的個(gè)數(shù)，pi和qi分別為原始點(diǎn)云和目標(biāo)點(diǎn)云中的點(diǎn)。

引入權(quán)重信息后，優(yōu)化問(wèn)題轉(zhuǎn)變?yōu)椋?/p>

由于屏風(fēng)點(diǎn)云的形狀近似對(duì)稱，在配準(zhǔn)中會(huì)出現(xiàn)和模型點(diǎn)云軸向方向相反的問(wèn)題，這可能導(dǎo)致最后計(jì)算出的機(jī)械臂抓取位姿不合理，因此要對(duì)旋轉(zhuǎn)矩陣進(jìn)行約束校正以確保原始點(diǎn)云和目標(biāo)點(diǎn)云軸向同向。設(shè)有矩陣：

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

4 試驗(yàn)和結(jié)果

本文采用自行研制的線結(jié)構(gòu)光掃描系統(tǒng)獲得屏風(fēng)點(diǎn)云。掃描得到的屏風(fēng)點(diǎn)云經(jīng)過(guò)降采樣、分割等處理得到屏風(fēng)骨架點(diǎn)云，其點(diǎn)數(shù)約為3萬(wàn)，點(diǎn)距約為0.4mm，用于和通過(guò)CAD生成已知尺寸的三維模型點(diǎn)云進(jìn)行配準(zhǔn)。本文程序的測(cè)試運(yùn)行環(huán)境為Intel Core i7-9750H處理器，內(nèi)存為16GB。

如圖3所示，經(jīng)過(guò)粗配準(zhǔn)，原始點(diǎn)云和目標(biāo)點(diǎn)云已經(jīng)基本對(duì)齊。在尋找對(duì)應(yīng)點(diǎn)時(shí)，采用KD-tree儲(chǔ)存點(diǎn)云以加速搜索速度；取δ=0.1，并使用歐氏距離約束去除匹配較差的對(duì)應(yīng)點(diǎn)對(duì)。通過(guò)實(shí)驗(yàn)，如果直接對(duì)原始點(diǎn)云和目標(biāo)點(diǎn)云進(jìn)行精配準(zhǔn)，則算法迭代次數(shù)會(huì)達(dá)到設(shè)定的最大次數(shù)（kmax=50），耗時(shí)530ms。若采用先粗配準(zhǔn)再精配準(zhǔn)的方法，則粗配準(zhǔn)部分耗時(shí)1ms，精配準(zhǔn)部分耗時(shí)287ms，總耗時(shí)少于直接配準(zhǔn)。

圖3：屏風(fēng)點(diǎn)云配準(zhǔn)結(jié)果

如圖4所示，在原始點(diǎn)云和目標(biāo)點(diǎn)云相對(duì)初始位姿相差較大的情況下，本文算法仍然能較好的工作，其他配準(zhǔn)算法如ICP算法往往產(chǎn)生錯(cuò)誤的配準(zhǔn)結(jié)果。如表1所示，通過(guò)本文算法和其他點(diǎn)云配準(zhǔn)算法的性能對(duì)比，可看出本文算法具有速度快、精度高的特點(diǎn)。

圖4：初始位姿相差較大的配準(zhǔn)

表1：不同配準(zhǔn)算法性能對(duì)比

5 結(jié)語(yǔ)

本文提出一種基于改進(jìn)ICP算法的屏風(fēng)骨架點(diǎn)云配準(zhǔn)方法，為解決實(shí)際應(yīng)用中屏風(fēng)自動(dòng)化上料抓取的位姿估計(jì)問(wèn)題打下了基礎(chǔ)。本文針對(duì)屏風(fēng)的形狀特點(diǎn)使用點(diǎn)云邊界提取和頂點(diǎn)提取的結(jié)果來(lái)進(jìn)行粗配準(zhǔn)；根據(jù)上料抓取任務(wù)的特點(diǎn)，在精配準(zhǔn)中引入權(quán)重系數(shù)以增強(qiáng)算法的魯棒性，并對(duì)旋轉(zhuǎn)矩陣進(jìn)行軸向校正以解決上料定位時(shí)可能發(fā)生的軸向反向問(wèn)題。通過(guò)試驗(yàn)證明，該算法針對(duì)屏風(fēng)點(diǎn)云配準(zhǔn)有較高的配準(zhǔn)精度以及較快的收斂速度，并且在原始點(diǎn)云和目標(biāo)點(diǎn)云初始位姿相差較大的情況下具有較強(qiáng)的魯棒性。