郭瑞科，王 立，吳 云，朱飛虎

(北京控制工程研究，北京100190)

一種空間非合作目標(biāo)的稀疏點云配準(zhǔn)算法*

郭瑞科，王 立，吳 云，朱飛虎

(北京控制工程研究，北京100190)

點云數(shù)據(jù)配準(zhǔn)是三維重構(gòu)的關(guān)鍵技術(shù)之一，為了提高空間非合作目標(biāo)的稀疏掃描點云數(shù)據(jù)配準(zhǔn)的速度和精度，提出一種改進(jìn)的基于四點算法的全局配準(zhǔn)算法進(jìn)行初始配準(zhǔn)，再使用迭代最近點算法精確配準(zhǔn).針對直接掃描所得到點云數(shù)據(jù)量大的問題，本文提出一種基于KD-Tree點云均勻采樣簡化算法，并且對傳統(tǒng)基于四點算法中的閾值參數(shù)進(jìn)行了統(tǒng)一，確定了各誤差閾值參數(shù)和點云密度之間的關(guān)系.仿真結(jié)果表明，該方法能夠快速、有效地實現(xiàn)衛(wèi)星稀疏點云的配準(zhǔn)，改進(jìn)的四點算法配準(zhǔn)耗時僅為幾何哈希算法的42.49%.

稀疏點云;點云配準(zhǔn);四點算法;ICP算法

0 引言

空間非合作目標(biāo)的在軌服務(wù)技術(shù)可實現(xiàn)對目標(biāo)航天器的在軌維護(hù)，空間垃圾碎片清理、抓捕或破壞敵方軍事衛(wèi)星等，具有很高的民用和軍事價值.在這些在軌服務(wù)任務(wù)中，服務(wù)航天器在跟蹤、接近和抓捕目標(biāo)時，需要實現(xiàn)航天器和目標(biāo)間的相對位置和姿態(tài)測量.相比較于傳統(tǒng)的合作目標(biāo)，非合作目標(biāo)上沒有光標(biāo)線或者角反射器等配合標(biāo)志，無法提供顯著、可靠的參考信息，這就給非合作航天器的準(zhǔn)確位姿測量帶來了極大的挑戰(zhàn).特別是對于完全未知的空間目標(biāo)，需要首先在軌建立三維模型，然后利用所重構(gòu)的三維模型實現(xiàn)六自由度位姿估計和特征識別.因而如何對未知目標(biāo)進(jìn)行三維重構(gòu)，成為了非合作目標(biāo)在軌服務(wù)的一項關(guān)鍵技術(shù).

激光掃描敏感器通過獲取目標(biāo)表面的點云數(shù)據(jù)，得到目標(biāo)三維模型，具有精度高、不受光照影響的優(yōu)點.由于敏感器視場角的限制，要得到目標(biāo)完整的三維模型，需要從多個視角對目標(biāo)進(jìn)行掃描，將不同視角得到的點云數(shù)據(jù)配準(zhǔn)到同一坐標(biāo)系下.點云配準(zhǔn)一般包括全局配準(zhǔn)和精確配準(zhǔn)兩個步驟，全局配準(zhǔn)縮小兩片點云之間的旋轉(zhuǎn)和平移錯位，為精確配準(zhǔn)提供比較好的初值.

常用的精確配準(zhǔn)算法是Besl和McKay于1991年提出的點到點的ICP算法，可以在比較好初值的情況下實現(xiàn)點云的精確配準(zhǔn)［1］.

針對任意初始位置點云數(shù)據(jù)的全局配準(zhǔn)問題，學(xué)者們做了大量研究.Wolfson等［2］提出了幾何哈希算法，用于點云數(shù)據(jù)的全局配準(zhǔn).作為改進(jìn)，吳君愷等［3］提出了一種多邊形哈希表的配準(zhǔn)算法，將點對之間的距離存儲在哈希表中，在較低時間復(fù)雜度下實現(xiàn)點云數(shù)據(jù)的自動全局粗配準(zhǔn)，但是所需要的內(nèi)存空間比較大，不適合空間應(yīng)用.Rusu等［4］提出的FPFH(fast point feature histograms)基于局部幾何特征的全局配準(zhǔn)方法，計算速度快，但是依賴點云的鄰域特征，不適合稀疏點云.謝冬香等［5］采用主元分析法，可較快配準(zhǔn)兩幅點云，但要求兩片點云重疊區(qū)域足夠大，且二者的主軸形狀相差不大.

對于兩幅三維點云的配準(zhǔn)，至少需要從各自點云中選取對應(yīng)不共線的三點，才能計算出二者的剛體變換矩陣.若待配準(zhǔn)兩片點云P、Q分別有m、n個點，在P、Q中選取對應(yīng)的三點計算旋轉(zhuǎn)平移矩陣，時間復(fù)雜度可達(dá) O(m3n3).Irani等［6］采用基于RANSAC(random sample consensus)的方法配準(zhǔn)，可以降低復(fù)雜度到O(n3logn)，然而對于空間應(yīng)用，時間復(fù)雜度還是太高.考慮在P點云中選取更多的共面點(5個或者以上)，則在Q點云中能成功選擇到正確的對應(yīng)點集的概率急劇降低，難以實現(xiàn)配準(zhǔn)［7］.

四點算法是Aiger等人于2008年提出的一種基于RANSAC法則的全局配準(zhǔn)算法，通過在點集中選取共面四點，可實現(xiàn)三維點云數(shù)據(jù)的配準(zhǔn)，時間復(fù)雜度只有O(n2)，并且受噪聲影響?。?］.但是算法中涉及到的可調(diào)閾值參數(shù)較多，當(dāng)配準(zhǔn)對象變化或者多幅點云連續(xù)配準(zhǔn)時，影響配準(zhǔn)的效率，仍有改進(jìn)的空間.

本文針對空間目標(biāo)需要內(nèi)存占用小，點云稀疏的特點，采用基于改進(jìn)四點算法的全局配準(zhǔn)算法，具有對噪聲魯棒性好，速度快的優(yōu)點，可以得到滿足精度的初始配準(zhǔn)結(jié)果.將全局配準(zhǔn)的結(jié)果作為精確配準(zhǔn)算法ICP的初值，可以保證收斂精度的同時，提高配準(zhǔn)的效率.仿真實驗驗證，該方法可以很好的實現(xiàn)衛(wèi)星稀疏點云數(shù)據(jù)的配準(zhǔn).

1 點云數(shù)據(jù)的簡化

激光掃描敏感器的測量精度高，尤其是在近距離掃描時，采用高效的李薩茹掃描方式和圖1所示，獲取的單幅掃描數(shù)據(jù)點數(shù)多達(dá)數(shù)萬到數(shù)十萬.這樣大的數(shù)據(jù)量對于星載計算機(jī)有限的存儲空間和計算能力帶來了比較大的挑戰(zhàn).圖1可知邊緣點云密度高于中間部分，而且點云都集中在掃描線上，因而并不是所有的點都可用于配準(zhǔn).其中冗余的點云數(shù)據(jù)如果都參與配準(zhǔn)，不僅會降低計算速度，而且點越多，越難從中找到符合特征的點對，影響配準(zhǔn)的精度.

圖1 李薩茹掃描方式Fig.1 Lissajous scan pattern

為了解決上述問題，必須對測量的點云數(shù)據(jù)進(jìn)行精簡，以提高計算速度，減少存儲空間，提高配準(zhǔn)精度等.考慮到我們所處理的對象為衛(wèi)星點云，在空間模型重建中對時效性要求較高，本文提出一種基于KD-Tree點云均勻采樣算法對點云數(shù)據(jù)進(jìn)行精簡.

KD-Tree是三維點云數(shù)據(jù)中常用的一種數(shù)據(jù)格式，可以快速實現(xiàn)數(shù)據(jù)點的k鄰域搜索［9］，也就是在點集中找到k個與該點歐氏距離最近的點.

在基于KD-Tree點云均勻采樣算法中，首先利用KD-Tree對待簡化點云數(shù)據(jù)建立空間拓?fù)浣Y(jié)構(gòu)，搜索點集中每個點的k鄰域，然后計算該點與k個最近鄰點的重心，用該重心代替這點，最后對所得到的點集均勻采樣，可以得到簡化的稀疏點云.

圖2是從一個視角掃描得到的單幅原始衛(wèi)星模型點云，包含10171個數(shù)據(jù)點，采用上述簡化算法，k 取4，均勻采樣率設(shè)置為7%.得到簡化后的點云如圖3所示，數(shù)據(jù)點數(shù)為710，可以看到數(shù)據(jù)得到很好的精簡，衛(wèi)星模型的特征也可以比較好的保留.

圖2 原始點云Fig.2 Original point cloud

圖3 簡化后的點云Fig.3 Simplified point cloud

2 基于改進(jìn)四點算法的全局配準(zhǔn)算法

本文針對原始四點算法中閾值參數(shù)比較多的情況，通過點云密度對閾值參數(shù)進(jìn)行了統(tǒng)一，簡化了多視角點云配準(zhǔn)時的閾值參數(shù)設(shè)置.

2.1 四點算法

四點算法在每次的迭代中，在點集P(data點集)中隨機(jī)尋找不在同一條直線上的共面四點，根據(jù)共面四點的兩條對角線交點將線段所分比例r1、r2在剛體變換中的不變性，在點集Q(model點集)中選取對應(yīng)的共面四點所圖5所示，這兩對共面四點組成一致共面四點集，利用該一致共面四點集可以計算出旋轉(zhuǎn)矩陣R和平移矩陣T.四點算法不需要計算復(fù)雜的幾何特征，速度快，對噪聲具有比較高的魯棒性.

圖4 中所選的共面四點［8］Fig.4 Four coplanar points extracted from

圖5 Q中對應(yīng)的共面一致四點Fig.5 Four affine invariant congruent points extracted from Q

四點算法中，Q點集中候選點對可以用式(1)中共面四點長度來篩選，即滿足與線段ab和cd近似相等的點對都是對應(yīng)候選點對，δ1為該長度的誤差閾值;根據(jù)不變量r1、r2由式(2)可以確定可能的交點e';近似相等交點所對應(yīng)的點對即組成Q點集中的共面一致四點.利用點集(a，b，c，d)和(a'，b'，c'，d')可以計算對應(yīng)的歐式變換矩陣，用所得到的歐式變換矩陣作用于點集P，驗證在一定閾值下，P點集中成功配準(zhǔn)到Q點集的點數(shù)目，即一致性度量.

式中，q1、q2是從點集找到的長度符合的候選共面四點集，q1是候選的a'b'點，q2是候選的c'd'點.

2.2 四點算法的閾值參數(shù)設(shè)置方法

文中所配準(zhǔn)的點云數(shù)據(jù)是從完全非合作目標(biāo)掃描所得到的，全局配準(zhǔn)中沒有點云數(shù)據(jù)的先驗知識，同時四點算法中涉及到的誤差閾值參數(shù)眾多，這對算法中的閾值參數(shù)設(shè)置帶來了挑戰(zhàn).在軌模型重建中需要多次點云配準(zhǔn)，每次配準(zhǔn)的兩幅點云特征都有所差別，如果分別設(shè)定這些閾值參數(shù)，可能會費(fèi)時費(fèi)力，甚至?xí)?dǎo)致不能成功配準(zhǔn).

點云數(shù)據(jù)密度是稀疏點云的關(guān)鍵幾何特征，本文中待配準(zhǔn)點云的密度τ，定義為點云中所有點k鄰域距離的平均值.一般三維散亂點云的k鄰域點數(shù)可設(shè)置為6，即前后左右上下六個方向，這樣可以比較好的保持點云的三維特征，準(zhǔn)確確定其它局部幾何特征.本文中待配準(zhǔn)的點云是經(jīng)過簡化的衛(wèi)星點云，衛(wèi)星帆板和本體部分以平面特征為主，因而k鄰域點數(shù)設(shè)置為4，能比較好的保留衛(wèi)星點云特征，準(zhǔn)確估計點云數(shù)據(jù)密度.本文通過確定點云密度與各誤差閾值參數(shù)的關(guān)系，對誤差閾值參數(shù)進(jìn)行了統(tǒng)一.

上述四點算法中涉及到的參數(shù)有:在Q點集中篩選候選對應(yīng)點對時的距離誤差閾值δ1;選取近似相等交點時的位置誤差閾值，即滿足 e1－＜δ2可認(rèn)為二者相等;一致性度量時，若歐氏變換后P點集與Q點集中點對的距離小于誤差閾值δ3時，標(biāo)記為配準(zhǔn)成功的點對.

本文通過仿真實驗確定了各誤差閾值δi與點云密度τ之間的關(guān)系，這樣當(dāng)多幅點云數(shù)據(jù)連續(xù)配準(zhǔn)時，無需重新調(diào)節(jié)閾值參數(shù)，有效提高了配準(zhǔn)算法的穩(wěn)定性和自動化程度.

3 基于ICP算法的精確配準(zhǔn)算法

ICP算法是在兩個點集中搜索最近的點對，以這些最近點對作為控制點來估算旋轉(zhuǎn)變換矩陣和平移變換矩陣，并將這個坐標(biāo)變換作用到data點集上，迭代地進(jìn)行這一操作過程，直到某個表示正確匹配的收斂準(zhǔn)則得到滿足.

由于待配準(zhǔn)的兩幅點云只有部分區(qū)域是重合的，在搜索最近點對的時候，不可避免的會存在錯誤的匹配點對.為了消除這些錯誤匹配點對對精確配準(zhǔn)結(jié)果的影響，需要剔除這些錯誤匹配點對.點云數(shù)據(jù)密度是稀疏點云的關(guān)鍵幾何特征，初始配準(zhǔn)時已經(jīng)估計過點云的密度.對于部分重疊的兩片點云，只有使用重疊區(qū)域的最近點對，才能準(zhǔn)確計算出兩者之間的變換矩陣，而非重疊區(qū)域的最近點對距離偏大.本文采取距離閾值的方法來剔除錯誤匹配點對，即認(rèn)為搜索的最近點距離大于點云密度一定倍數(shù)的點對為錯誤匹配點對.

多幅點云配準(zhǔn)的基礎(chǔ)是兩幅點云的配準(zhǔn)，配準(zhǔn)后的兩幅點云需要去除掉重疊區(qū)域冗余的點云，才能得到兩個視角拼接到一起的單幅完整點云，方便用于后續(xù)視角的配準(zhǔn)拼接.由ICP配準(zhǔn)算法的原理可知，配準(zhǔn)中最后一次迭代時所搜索到的最近點對，就是兩幅點云的重疊區(qū)域，對重疊區(qū)域的每對點云取坐標(biāo)的平均值，將平均值作為該區(qū)域的新數(shù)據(jù)，即可實現(xiàn)重疊區(qū)域冗余點云的去除.

4 實驗仿真

本文仿真實驗使用簡化后某衛(wèi)星仿真點云數(shù)據(jù)，model點云為0°視角衛(wèi)星點云數(shù)據(jù)，data點云是30°視角點云數(shù)據(jù)，數(shù)據(jù)點數(shù)為719，兩幅點云有部分重疊.配準(zhǔn)誤差定義為均方根誤差(root mean square error，RMSE)

式中，N為歐氏變換后兩塊待配準(zhǔn)點云重疊部分點數(shù)目，pi、qj分別為歐氏變換后點云中重疊區(qū)域的點.

首先使用基于改進(jìn)四點算法的全局配準(zhǔn)算法進(jìn)行了配準(zhǔn).圖6是待配準(zhǔn)的兩片點云，圓點為model點云，五角星為data點云.圖7是全局配準(zhǔn)的結(jié)果，可以看到對于兩幅初始位置偏差比較遠(yuǎn)的稀疏點云，全局配準(zhǔn)算法能夠比較好的實現(xiàn)配準(zhǔn)，配準(zhǔn)誤差為0.1339.從圖9所示的全局配準(zhǔn)點云在OYZ平面上的投影，可以看到沿X軸方向仍有滾轉(zhuǎn)的誤差，需要精確配準(zhǔn)算法的進(jìn)一步優(yōu)化.

精確配準(zhǔn)使用全局配準(zhǔn)點云的結(jié)果作為初始值，配準(zhǔn)過程中通過距離閾值去除掉錯誤匹配.圖8是精確配準(zhǔn)后的點云，可以看到兩個視角的點云已經(jīng)很好的配準(zhǔn)到一起，配準(zhǔn)誤差降低到0.0908.圖10是精確配準(zhǔn)的點云在OYZ平面的投影，相比于圖9，滾轉(zhuǎn)誤差已經(jīng)消除.從圖11的誤差曲線可以看到迭代15次時，已經(jīng)收斂.

圖6 待配準(zhǔn)點云Fig.6 Model and Data point cloud

圖9 全局配準(zhǔn)點云在OYZ平面投影Fig.9 Projection of coarse registration in OYZ

圖10 精確配準(zhǔn)點云在OYZ平面投影Fig.10 Projection of fine registration in OYZ

圖11 均方差變化曲線Fig.11 Variation of RMSE

圖12所示是去除掉重合部分冗余點云，所得到的拼接在一起的兩個視角的點云.圓點是兩個視角的重疊部分，五角星是僅在data點集中的點云，十字是僅在model中的點云.

圖12 拼接在一起的兩幅點云Fig.12 Spliced point cloud

傳統(tǒng)的幾何哈希算法需要先離線建立哈希表，再搜索配準(zhǔn)，從表1可以看到，文中的全局配準(zhǔn)算法用時小于幾何哈希算法配準(zhǔn)，配準(zhǔn)速度得到提高，二者精度相差不大.

表1 文中配準(zhǔn)算法與幾何哈希配準(zhǔn)算法對比Tab.1 The comparison with Geometric Hashing

5 結(jié)論

兩幅點云的配準(zhǔn)是多幅點云配準(zhǔn)的基礎(chǔ)和關(guān)鍵，空間應(yīng)用中，點云的配準(zhǔn)算法要兼顧精確性、快速性和內(nèi)存占用大小等因素.本文針對衛(wèi)星掃描點云數(shù)據(jù)，提出基于KD-Tree點云均勻采樣簡化算法，在保持衛(wèi)星特征的情況下比較好的實現(xiàn)點云簡化.對于衛(wèi)星稀疏點云數(shù)據(jù)，使用基于改進(jìn)四點算法進(jìn)行初始配準(zhǔn)，將初始配準(zhǔn)結(jié)果作為精確ICP配準(zhǔn)的初值，實現(xiàn)了點云數(shù)據(jù)的精確配準(zhǔn).對于四點算法中誤差閾值參數(shù)多的情況，給出了誤差閾值參數(shù)與點云數(shù)據(jù)密度之間的關(guān)系，有效提高算法的穩(wěn)定性.作為多幅點云的配準(zhǔn)基礎(chǔ)，研究了兩幅點云配準(zhǔn)之后重疊區(qū)域的去除，以方便未來實現(xiàn)多幅點云的配準(zhǔn).

［1］ FISHLER M A，BOLLES R C.Random sample consensus:a paradigm for model fitting with applications to image analysis and automated cartography［J］.Communications of the ACM.1981，24(6):381-395.

［2］ WOLFSON H，RIGOUTSOS I.Geometric Hashing:an Overview［J］.IEEE Computer Science and Engineering，1997:10-21.

［3］ 吳君愷，楊光.基于點云配準(zhǔn)的空間目標(biāo)定位技術(shù)的補(bǔ)償問題研究［C］//全國射線數(shù)字成像與CT新技術(shù)研討會，廈門，2014:20-28.WU J K，YANG G.Compensation in space targeting technology based on point cloud registration［C］//The X-ray Digital Imaging and CT New Technology Conference，Xiamen，2014:20-28.

［4］ RUSU R B.Fast Point Feature Histograms(FPFH)for 3D Registration［C］//In Robotics and Automation，2009.ICRA 09.IEEE International Conference.New York:IEEE，2009:3212-3217.

［5］ 謝冬香，劉先勇.一種快速的三維點云自動配準(zhǔn)方法［J］.微型機(jī)與應(yīng)用，2013，32(6):47-49.XIE D X，LIU X Y.A robust and automatic registration of 3D point sets［J］.Microcomputer＆its Applications，2013，32(6):47-49.

［6］ IRANI S，RAGHAVAN P.Combinatorial and experimental results for randomized point matching algorithms ［J］.Computational Geometry，1999，12(1-2):17-31.

［7］ YAO L，RUGGERI M R，TADDEI P，et al.Robust surface registration using N-points approximate congruent sets［J］.EURASIP Journal on Advances in Signal Processing，2011:72.

［8］ AIGER D，NILOY M J.4-Points Congruent Sets for Robust Pairwise Surface Registration［J］ACM Transactions on Graphics(TOG)，2008，27(3):85-94.

［9］ BENTLEY J L.Multidimensional binary search trees used for associative searching［J］.Communications of the ACM，1975，18(9):509-517.

A Sparse Point Cloud Registration Algorithm of Non-Cooperative Spacecraft

GUO Ruike，WANG Li，WU Yun，ZHU Feihu
(Beijing Institute of Control Engineering，Beijing 100190)

The point cloud data registration is one of the key technologies of three-dimensional reconstruction.To solve the registration issue of sparse point cloud scanned from the non-cooperative spacecraft，we propose a improved 4-points congruent sets(4PCS)algorithm to obtain the preliminary registration result，and optimize the final alignment with the improved iterated closest points(ICP)algorithm.Then，a novel point cloud simplification algorithm using uniform sampling is proposed based on KD-Tree.The uniform relation of the threshold parameters is established via the density of the point cloud.The results show that the proposed algorithm can effectively achieve good alignments of the sparse point cloud of the satellite，and the consuming time is decreased to 42.49%compared with the Geometric Hashing algorithm.

non-cooperative target;sparse point cloud;point cloud registration;4PCS algorithm;ICP algorithm

TP391

A 文章編號:1674-1579(2016)05-0031-06

10.3969/j.issn.1674-1579.2016.05.006

郭瑞科(1989—)，男，碩士研究生，研究方向為模式識別與智能系統(tǒng);王 立(1977—)，男，研究員，研究方向為視覺導(dǎo)航;吳 云(1985—)，男，工程師，研究方向為激光雷達(dá);朱飛虎(1985—)，男，高級工程師，研究方向為激光雷達(dá).

*國家重點基礎(chǔ)研究發(fā)展計劃(973)資助項目(2013CB733100).

2016-04-06