基于點云的空間非合作目標初始相對位姿獲取

劉智遠，郭延寧，梁維奎，徐 杭

(1. 哈爾濱工業(yè)大學控制科學與工程系，哈爾濱 150001；2. 上海宇航系統(tǒng)工程研究所，上海 201109)

0 引 言

空間非合作目標范指不能提供有效合作信息的空間目標[1]，由于其不具備已提前安裝的定位識別標志，在近距離解算相對位姿時只能依賴服務航天器的觀測信息[2]。對空間非合作目標進行相對位姿解算，是服務航天器相對導航、進而實施在軌服務或空間碎片清除等空間操作任務的基本前提。

根據所采用的傳感器不同，已有的非合作目標相對位姿解算研究主要分為視覺傳感器與激光雷達兩類。Hu等[3]使用單目視覺傳感器完成對非合作目標星艦對接環(huán)的識別與相對位姿建模；Christan等[4]使用視覺傳感器與結構光深度傳感器，進行傳感器融合以解算非合作目標相對位姿并完成自主導航。視覺傳感器成本低，可適應復雜的工作環(huán)境，但對空間觀測光照條件要求較高；激光雷達則避免了由于復雜光照條件帶來的影響，成為空間近距離觀測與相對位姿解算的主流方法：Woods和Christan[5]使用掃描式激光雷達獲取目標點云數據，并使用點云全局特征擬合以獲取ICP迭代初值，提高了點云配準與位姿解算的魯棒性；Liu[6]使用閃光式激光雷達以獲取目標點云數據來解算相對位姿，并設計數字仿真實驗與物理實驗完成對非合作目標模型的相對位姿建模與接近。

由于激光雷達的觀測不受空間光照條件影響，在弱光、逆光、陰影等觀測條件下均具有較高的魯棒性[7]，使用其進行通過點云構建解算相對位姿具有顯著的應用前景。然而，由于需使用點云全局特征進行點云主方向擬合以獲取初始位姿，對非合作目標點云模型與觀測點云完整度要求較高。鑒于此，如何使用目標外輪廓點云，在初始位姿未知的情況下完成迭代最近點位姿解算，成為一項關鍵工作。

在使用點云進行迭代最近點解算目標相對位姿時，需要使用ICP算法[8]擬合當前幀點云與目標點云模型。然而，由于ICP算法對初值要求較高[9]，當前幀點云與目標模型點云之間實際為錯誤點對時，直接使用ICP算法將使得逐步迭代到模型的錯誤位置，進而產生誤匹配，得到錯誤的位姿解算，如圖1所示。

圖1 ICP初始位姿對匹配結果的影響示意圖Fig.1 Iteration result infected by initiol ICP relative pose

考慮到非合作目標可能具有的高度對稱性，如典型的通信衛(wèi)星都具有兩個對稱的太陽帆板等，簡單地僅使用ICP進行當前幀點云配準極有可能將其匹配到模型的另一對稱部分或對稱角度，因此容易造成誤匹配。

目前已有的點云ICP初值選取算法包括兩種：使用點云局部特征及點云關鍵點匹配處理算法，以及使用點云全局特征的點云擬合算法[10]。其中，點云FPFH特征是使用較為廣泛的點云局部特征，但其計算結果受點云質量及計算參數等影響較大[11]，其應用場合多為靜態(tài)物體三位點云拼接與SFM[12]；VFH與更為穩(wěn)定的OUR-CVFH是兩種常用的點云全局特征[13]，目前已有多名學者使用OUR-CVFH特征對待匹配點云進行預處理，并得到良好的匹配結果與初始位姿。然而，針對外表高度對稱的非合作目標點云模型，不同角度的觀測點云計算得到的OUR-CVFH特征將具備相當高的相似度，亦會產生一定概率的誤匹配。

然而，非合作目標可能攜帶不同的表面載荷，如雷達、星敏感器、軌控發(fā)動機、姿控噴嘴等。這些載荷增加了目標表面的不對稱度，并由于其構型位于目標本體之外，其點云模型也將成為目標本體點云之外的點云凸包。考慮到非合作目標攜帶不同表面載荷帶來的不對稱性與唯一性，通過對載荷點云進行提取，結合所占面積較大的目標太陽能帆板共同構成核心點云，并使用此點云求解位姿變換矩陣，進而以此矩陣作為ICP位姿解算的預處理結果進行輸入，將會給ICP提供良好的迭代初值。

本文基于對ICP算法的分析，充分考慮非合作目標可能具有的高度對稱性結構，設計了一種基于LCCP點云分割與點云Harris3D角點的點云匹配預處理：通過對合理降采樣后的點云進行點云分割，提取太陽帆板部分并割離目標本體；進而對本體點云模型進行Harris3D角點提取，獲取點云中可能代表表面不同載荷的凸包點云；以Harris3D角點為中心，衍生周邊點云區(qū)域作為潛在關鍵載荷區(qū)域；結合太陽帆板點云并進行多角度預變換處理，分別求解各種預變換位姿下的匹配度，最終選取最佳匹配度作為匹配結果；在匹配上的Harris角點間使用SVD求解最佳位姿變換；最終，使用該位姿變換矩陣作為ICP初始迭代位姿，完成對ICP算法的預處理與優(yōu)化。

1 基于點云分割與關鍵點的初始位姿獲取方法

基于點云的非合作目標相對位姿解算流程圖如圖2所示。

圖2 典型位姿解算流程圖Fig.2 Typical relative pose estimation process

其中，ICP迭代過程為相對位姿求取的關鍵，且對初始迭代位姿要求較高。本文旨在通過點云處理獲取較好的ICP初始迭代位姿，以保證迭代結果的可靠性。

本節(jié)將具體闡釋本文提出的ICP初始迭代位姿獲取方法流程。該方法具體流程如表1所示。

表1 初始位姿獲取方法Table 1 Initial relative pose acquire process

現以某通信衛(wèi)星模型為例進行算法流程演示。

1.1 LCCP點云分割

LCCP(Locally convex connected patches)算法，不依賴點云模型的顏色，僅使用點云的三維位置分布進行點云分割[14]。通過計算點云晶核聚類后不同超體之間的凸凹關系，進行相鄰超體的重聚類并完成最終點云的劃分。超體之間的凸凹關系將使用相鄰超體中心連線向量與兩超體法向量夾角進行計算,如圖3所示。

圖3 相鄰超體凸凹判據Fig.3 Criterion between adjacent superbodies

且若滿足

α1≥α2

(1)

則判定相鄰超體為凸連接，反之為凹連接。

進而，若相鄰超體判定為凸連接，使用SC判據計算相鄰超體中心連線與超體臨接線夾角θ進行超體連通性判斷,如圖4所示。

圖4 相鄰超體中心連線與超體臨接線夾角θFig.4 The angle θ between the adjacent super-body center line and the super-body wiring

夾角θ越接近90°，則SC判據判定相鄰超體為連通的可能性更大。最終，LCCP使所有連通的相鄰超體跨越凸邊界增長，并完成點云的分割。

使用降采樣后的點云模型，設定粒子距離、晶核距離容差閾值、空間與法線權值等參數，對點云模型進行LCCP分割。分割結果如如圖5所示。

圖5 LCCP點云分割結果Fig.5 Point cloud segmentation result

獲取點云維度最大的方向，并提取該方向上第一和最后一個點的點云標簽，其對應的點云則分別為目標的太陽帆板。

1.2 Harris關鍵點提取及周邊區(qū)域衍生

由于本文算法旨在充分利用表面載荷點云信息，避免誤匹配并提高匹配精度，為避免求取點云法向量以降低計算量，不使用SIFT關鍵點；同時，考慮當前幀點云為非完整點云包絡，點云的邊緣不應作為關鍵點被提取出，因此按照算法原理不選擇NARF關鍵點而選擇Harris角點[15]作為關鍵點。

基于已經提取的Harris3D角點，可以通過提取其周圍某一范圍區(qū)域的點云作為關鍵點云凸包，而這些凸包則極有可能包含目標表面攜帶的關鍵載荷。以目標點云本體模型為例，Harris3D角點及其周邊衍生區(qū)域的提取效果圖如圖6所示。

圖6 Harris3D關鍵點及周邊衍生區(qū)域Fig.6 Harris keypoints and peripheral derivative point cloud

1.3 關鍵點云多角度預變換處理

考慮到非合作目標高對稱度外觀，以及相對規(guī)整的幾何輪廓，其不同Harris3D角點可能被從不同的類似位置提取出來，例如正多面體目標本體的多個相似角點等。因此，不論從被提取出的關鍵點本身的描述子及基于描述子的關鍵點匹配角度，抑或從單一關鍵點周邊衍生區(qū)域分別進行模板匹配的角度，都將無法解決上述相似角點所帶來的誤匹配問題。因此，關鍵點或關鍵點周邊衍生區(qū)域不能被拆開使用，只用對其進行統(tǒng)一的變換與匹配，才可以求解得到正確且有意義的變換矩陣，進而作為ICP精確配準的初始迭代位姿。將目標太陽帆板與本體角點及衍生區(qū)域組合后構成的關鍵點云如圖7所示。

圖7 目標帆板與本體關鍵點衍生區(qū)域Fig.7 Solar panels and peripheral derivative point cloud

為充分利用目標表面不同關鍵載荷所帶來的不對稱性與唯一性，將當前幀關鍵載荷潛在點云凸包進行多次整體旋轉變換，以避免由于目標的高對稱度而在預處理時就出現誤匹配的結果。以上述點云模型為例，以俯仰-偏航-滾轉的旋轉順序，繞每個軸旋轉的角度分別設定為0π,1/2π,2/2π,3/2π的角度并進行排列組合，得到64種預處理旋轉變換矩陣R，及相應的預變換點云：

R=Rx·Rz·Ry

(2)

(3)

式中:Rx,Rz,Ry分別為滾轉、偏航、俯仰軸變換矩陣；xi,yi,zi分別為原始點云坐標，x′i,y′i,z′i分別為變換后點云坐標。

1.4 關鍵點云最佳匹配提取

通過對關鍵點云進行不同的預旋轉變換處理，分別對其與目標模型關鍵點云凸包進行最佳匹配，以充分消除由于目標對稱性與初始位姿所帶來的誤匹配結果。以上述點云模型為例，匹配方式使用ICP，匹配相似度變換率閾值設為5%，迭代次數閾值設為10次。預變換結果如圖8所示，分別按照俯仰、偏航與滾轉變換次數進行命名。

圖8 不同角度預變換后的點云凸包匹配結果Fig.8 Alignment results through different pretreatment

對于不同角度預變換處理后的點云匹配結果，匹配契合度的量化結果為ICP最近鄰點距。此上展示的不同預處理變換角度及最近鄰點距值如表2所示。

表2 預變換角度及最近鄰點距Table 2 Angles of pretreatment and closest points’ distances

ICP最近鄰點距越小代表匹配契合度越高，選取最高契合度對應的匹配結果作為點云最佳匹配結果。

1.5 SVD求解關鍵點云配對最佳變換矩陣

選取最佳匹配的點云對，分別按空間距離求取匹配后的Harris角點配對。點云模型初始位姿下的Harris角點配對如圖9所示。

圖9 Harris 3D關鍵點配對Fig.9 Harris 3D keypoints pairs

獲取全部配對Harris3D關鍵點的三維坐標后，構建最小二乘問題：

(4)

2 仿真校驗

2.1 仿真環(huán)境搭建及坐標系定義

為驗證本文基于點云分割與Harris關鍵點的初始位姿獲取算法的可行性，設計了數字仿真實驗完成對算法的校驗，并對比：1)不加預處理的ICP點云配準算法；2)使用點云FPFH特征并進行點云關鍵點匹配的預處理算法；3)使用點云OUR-CVFH特征的預處理算法，并評估各種算法對位姿解算的可行性與準確率。算法測試與校驗的相關代碼使用C++編寫，編譯并運行于Ubuntu系統(tǒng)中。

使用Solidworks繪制非合作目標模型，模型縮放比例為1:10。仿真環(huán)境選用Gazebo進行構建，點云傳感器選用SR4000激光雷達。

本實驗已知目標點云模型，且仿真環(huán)境中的目標姿態(tài)已知，因此算法解算出的位姿誤差可計算得到。

實驗中使用到的坐標系，包括慣性系O-xyz，目標本體坐標系Ot-xtytzt與服務航天器坐標系Os-xsyszs。坐標系規(guī)劃與設定如下圖所示：

圖10 慣性系、目標本體坐標系與服務航天器坐標系Fig.10 Inertial coordinate system, target body coordinate system and service spacecraft coordinate system

滾轉、偏航、俯仰變換矩陣Rx；Rz；Ry分別為：

(5)

(6)

(7)

式中：φ，φ，θ分別為滾轉、偏航、俯仰角度。

傳感器坐標系與目標本體坐標系之間點云坐標變化如下：

(8)

式中:Δxt,Δyt,Δzt分別為目標中心在世界坐標系中的偏移量，Δxs,Δys,Δzs分別為觀測器位置在世界坐標系中偏移量。

2.2 數字仿真實驗

仿真實驗將使目標繞zt軸旋轉，測試算法的可行性并與傳統(tǒng)ICP及其他預處理算法進行對比驗證，同時對實驗中出現的各種現象做出分析與臨時調整，在實驗中對本文算法進行了完善。

目標中心置于坐標原點(0, 0, 2)處(單位：m)，觀測點位置為(2, 0, 2)；目標初始姿態(tài)(φ,φ,θ) = (0°, 0°, -180°)，其θ將以20°的間隔從-180°變化至180°。

考慮到某些特定角度范圍內，由于目標可能側向于觀測點，其太陽帆板可能會無法完整觀測，甚至被本體遮擋。因此，在點云分割后的太陽帆板提取步驟中，加入左右太陽帆板點云個數差的限制：

(9)

式中：nLeft，nRight分別為左、右帆板點云個數，nPoints為點云模型整體點云個數；以及太陽帆板整體點云個數閾值限制：

(10)

若不滿足上述任一條件，將默認太陽帆板受到嚴重遮擋導致點云分割的誤識別，或目標相對觀測點的角度使得獲取到太陽帆板的點云數量過少。此時將跳過點云分割過程，直接進行全體點云的Harris關鍵點提取。

基于點云模型的Harris關鍵點，計算關鍵點的FPFH描述子[16]并進行關鍵點匹配，匹配結果如圖11所示。

圖11 某幀點云FPFH描述子配對結果Fig.11 FPFH description matching results

由于點云局部特征對點云質量及計算參數魯棒性低，同一物體不同觀測的點云計算得到的FPFH描述子將具有一定差別。本文使用的點云模型并不能通過FPFH描述子進行有效的關鍵點匹配，亦無法進一步求解變換位姿。

圖12 三種算法整體角度誤差Fig.12 Overall angular error of the three algorithms

文中預處理算法、使用OUR-CVFH特征的預處理算法及傳統(tǒng)ICP算法解算出的整體角度誤差α如圖12所示。

其中，整體角度誤差α將按照羅德里格斯公式(Rodriguez formula)計算：

(11)

式中：R0為目標模型旋轉矩陣，Rresult為預處理過程由SVD解算得到的旋轉矩陣或傳統(tǒng)ICP算法迭代求解的變換矩陣。

通過數據可以分析出，本文算法對目標點云具備大角度偏航角位姿的情況可以魯棒地求解最佳匹配；同時可以在一定程度上有效避免由于太陽帆板無法完整觀測所帶來的誤匹配；給出的SVD解算值也相對準確，可作為進一步ICP配準的有效初值。傳統(tǒng)ICP在目標具備大角度偏航角的條件下易根據迭代最近鄰點的算法特性，與目標大面積太陽帆板的特征，而產生180°的誤匹配；在太陽帆板受到遮擋或觀測點云數量較少時，由于目標本體的高度對稱性，將產生極高的誤匹配率；基于OUR-CVFH的預處理算法[5]在太陽帆板可良好觀測的條件下可以有效地消除初始位姿帶來的局部最優(yōu)，但點云的高度對稱性所帶來的點云OUR-CVFH特征的相似性將有可能產生誤匹配，產生誤匹配時的特征如圖13所示。

圖13 不同角度觀測的OUR-CVFH特征Fig.13 OUR-CVFH features get from different perspectives

由于非合作目標的高度對稱性，不同觀測角度下點云的OUR-CVFH特征具備相當高的相似性，因此有一定概率發(fā)生誤匹配。

三種算法的平移量誤差如圖14-16所示，點云平移誤差將按照點云重心坐標計算：

圖14 本文算法x，y，z軸平移誤差Fig.14 Translation error component of our algorithm

圖15 傳統(tǒng)ICP算法x，y，z軸平移誤差Fig.15 Traditional ICP translation error component

圖16 OUR-CVFH算法x，y，z軸平移誤差Fig.16 OUR-CVFH translation error component

在不同偏航角度下，三種算法均可有效縮減點云間的距離，且位置誤差均在容許范圍內。衛(wèi)星模型最大維度尺寸約為1.5 m，觀測距離2 m，三種算法匹配結果距離誤差均在0.1 m級。

當偏航角度在[-60°, 60°]范圍內，三種算法均得到了正確的迭代匹配結果。在正確匹配的情況下，本文算法在三軸上角度誤差如圖17所示。

圖17 本文算法角度誤差分量Fig.17 Angular error component of our algorithm

分析數據可得出，本文算法由于需使用SVD求解變換矩陣數值解而非迭代求解，其結果精度突變性較大，但角度誤差均在10°以內，可作為進一步ICP配準的有效初值。

3 結 論

本文設計了一種基于LCCP點云分割與點云Harris3D關鍵點的改進ICP預處理過程，該預處理過程可以有效地減小不同初始位姿情況下，由非合作目標高度對稱性所引起誤匹配的概率。所提出的算法針對非合作目標的高度對稱性外觀，提取目標表面潛在攜帶載荷區(qū)域，利用攜帶載荷所帶來的不對稱性與唯一性，同時考慮觀測角度帶來的太陽帆板受遮擋或觀測不佳的情況，結合目標攜帶的太陽帆板進行輔助定位，求解當前幀點云與目標模型點云之間的最佳位姿，并以此位姿輸入ICP求解器作為初始迭代位姿，減小由于非合作目標高度對稱性帶來的誤匹配率。