改進PointNetLK的點云智能配準與位姿圖優化方法

李榮華，董欣基，薛豪鵬，祁宇峰，張建禹

(大連交通大學機械工程學院，大連 116028)

0 引 言

空間非合作目標構型重建是位姿估計、姿態跟蹤[1-2]及在軌捕獲等關鍵環節的基礎，在故障衛星維護及廢棄衛星清理等在軌服務中發揮極其重要的作用[3]。空間非合作目標構型重建主要受目標可測部位點云數據質量和配準精度影響。單目相機與激光雷達都是常用的測量工具，考慮到空間抗干擾及光照影響等因素，單目相機具有缺乏深度信息、特征提取不穩定、適應性較差等缺陷[4-5]，激光雷達是最為理想的空間非合作目標測量載荷[6]。使用激光雷達對目標進行掃描能夠獲取點云數據，因其能夠以相對較低的存儲成本獲取較精確的物體拓撲結構和幾何結構而受到了日益廣泛的重視。在實際掃描過程中，由于被測目標尺寸過大、物體表面被遮蓋或激光雷達的掃描角度過小等因素，單次雷達掃描中往往沒有目標全部的幾何信息，所以需要對兩幅或多幅點云進行配準，以得到完整的幾何信息。

Besl等[7]提出的迭代最近點(Iterative closest point,ICP)算法在點云數據配準領域中做出很大貢獻。劉躍生等[8]為了防止離群值對配準的負面影響，提出了混合稀疏迭代最近點(SM-ICP)方法；王子瑋等[9]為了克服稀疏ICP與稀疏最小切線距離(Tangent distance minimum,TDM)算法的局限性，提出了稀疏混合ICP算法；馬鑫等[10]基于ICP與n點透視(PnP)算法的差異與聯系，提出了RGB-D同步定位與地圖創建系統(Simultaneous localization and mapping,SLAM)中2種PnP算法的切換策略，并在優化問題中根據觀測的不確定性對各誤差項賦予不同權重；劉智遠等[11]基于局部凸連接補片(Locally convex connected patches,LCCP)點云分割算法與點云Harris關鍵點，在已知目標外輪廓點云的基礎上，提出了一種ICP初始迭代位姿獲取方法；李繞波等[12]提出一種利用重心特征變換處理來解決不同場景點云空間特征約束下的點云空間配準方法；Marchel等[13]基于誤差建模的三個原始加權因子，對ICP環境掃描匹配的標準進行修改。當前算法的單向迭代機制易出現因誤差在同一方向上積累導致的配準精度降低，甚至陷入局部最優解，使配準失敗的問題。而對于使用神經網絡，Charles等[14]提出的PointNet算法為第一個采用對稱函數方式、直接輸入點云數據的一個端到端的神經網絡；在PointNet神經網絡的基礎上，Yasuhiro等[15]引入傳統光流法(LK)，提出了PointNetLK點云配準神經網絡；何凱等[16]提出一種基于混合注意力機制和相關性估計網絡的點云配準算法。當前算法解決空間非合作目標可測部位點云帶來的無先驗信息、目標描述不完全的點云配準問題時仍有精度較差、效率不高的問題。

本文提出一種面向空間非合作目標的點云智能配準算法。在使用半物理仿真平臺對空間非合作目標進行掃描后，進行目標提取，剔除場景中散亂目標和噪聲，并對PointNetLK神經網絡配準算法進行改進，將處理后的點云數據作為改進后的智能配準算法的輸入，后端對配準結果進行位姿圖優化，得到空間非合作目標的相對位姿。

1 Straight Through濾波目標提取算法

在進行半物理仿真地面試驗時，由于空間限制，激光雷達對空間非合作目標進行掃描，往往會因為背景信息雜亂，提高數據處理的復雜程度。故在點云配準算法前端對原始目標進行目標提取。

采用Straight Through濾波算法，通過對點云三個維度的范圍限定，對采集到的數據進行預處理。設定采集得到的原始目標點云集為P={P1,P2,P3,…,Pn}，其中任意一點Pi的坐標為Pi=(xi,yi,zi),xi∈X0,yi∈Y0,zi∈Z0。

通過結合半物理仿真實驗平臺、非合作目標主體尺寸在不同姿態下的x,y,z三個方向的最大值和最小值，確定三個方向的閾值范圍xmax,xmin,ymax,ymin,zmax,zmin，對維度范圍內的點云進行快速提取。提取后的第i幀點云集為Pt={P1,P2,P3,…,Pm},m

(1)

2 改進PointNetLK的點云智能配準算法

一般深度學習神經網絡模型對于圖像等規范化的輸入，易獲得較好的輸出，而點云相對于圖像有個十分突出的特點，即無序性，這也對網絡設計提出要求。為了克服無序的特點，可引入通過對稱函數的數學計算把每個點的數據進行整合的方法。

2.1 改進架構的PointNet算法

PointNet算法的網絡結構如圖1所示，輸入由n個具有三維坐標信息的點所組成的點云數據。PointNet以點的數量n為二維卷積的高度，三維坐標為寬度。為面對空間非合作目標時仍有一定的高效率與高精度，同時保留各點的信息，利用權重共享的MLP(64，64)進行特征提取之后，再利用平均池化層壓縮數據大小，在獲得部分點的32維特征之后，再繼續使用MLP，得到2048維特征，最后利用平均池化層獲得點云的全局特征向量，將其通過全連接層附加在32維特征后面，經過分類器得出點的預測得分。

圖1 PointNet架構Fig.1 PointNet architecture

2.2 改進損失函數的PointNetLK算法

G-1=exp(-ξ∧)

(2)

式中：ξ是李群G對應的李代數；n∧表示將向量轉化為反對稱矩陣。

三維點云配準問題則為找到G-1，使得：

(3)

將式(2)進行一階泰勒展開得：

(4)

通過添加左擾動ΔT=exp(δξ∧)求得：

(5)

此時將式(4)變形得到：

(6)

式中：J+是J的偽逆。

(7)

最后的估算值Gest是迭代循環中計算的所有增量估算值的組合：

Gest=ΔGn·…·G1·ΔG0

(8)

迭代的停止判據為ΔG小于設定的最小閾值ΔGth。以上模型流程如圖2所示。

圖2 PointNetLK流程圖Fig.2 Flow chart of PointNetLK

由于空間非合作目標沒有先驗信息，無法提供真實變換矩陣Ggt，故損失函數應重新設定。損失函數的目標是使配準結果點云和目標點云中對應點之間的距離最小化。所以使用式(9)表示損失函數。

(9)

配準結果如圖3所示，在空間非合作目標可測部位點云配準中，改進的PointNetLK點云智能配準算法會得到較高精度的配準結果。其中，紅色的、點最小的為源點云；藍色的、點較小的為目標點云；綠色的、點最大的為配準結果。

圖3 改進的PointNetLK算法配準效果Fig.3 The registration effects of an improved PointNetLK algorithm

2.3 非合作目標配準方式

由于ModelNet40中各類別的數據之間大小不同、位姿隨機變化，所以在初始PointNetLK算法中，無論是訓練或是測試過程中，都以數據集的每個點云為目標點云，隨機生成或手動添加變換矩陣Ggt，將目標點云集整體施加Ggt得到源點云集，最后將目標點云與源點云配準。

而在空間非合作目標點云配準中，采集到的點云模型大小統一、運動狀態規律，并且無法提供有效的先驗信息即變換矩陣Ggt，所以配準方式需要改變。將采集到的點云數據按照采集順序編號i(i=0,1,2,…,n)，從i=0開始索引得到目標點云集，再從i=1開始索引得到源點云集，最后將目標點云與源點云配準n次，得到n個各幀之間的相對位姿。

3 位姿圖優化

在點云數據配準完成后，往往配準存在誤差，因此在配準算法后端鏈接位姿圖優化，達到提高配準精度，進而提升后續重建質量的目的。

3.1 位姿圖優化問題構建

圖由點與邊組成，以點代表優化變量，邊代表誤差，其優化變量即是各幀間的相對位姿。

首先定義相對位姿李群表達形式如下：

(10)

式中：Ti,Ti+1為第i幀和第i+1幀的位姿；Ti(i+1)為第i幀與第i+1幀之間的相對位姿。

其李代數形式為：

exp(ξi+1)∧)∨

(11)

所以誤差表達式為：

exp((-ξi)∧)exp(ξi+1)∧)∨

(12)

為得到ei(i+1)關于ξi,ξi+1的導數，對其添加左擾動，得到：

exp(δξi+1)∧Ti+1)∨

(13)

根據變換群伴隨性質：

exp(ξ∧)T=Texp((Ad(T-1)ξ)∧)

(14)

得到：

(15)

式中：

(16)

得到了誤差關于優化變量的導數。

將所有誤差項相加得到總體目標函數為：

(17)

3.2 基于最小二乘法的位姿圖優化

可以采用高斯-牛頓法來解決后續的圖優化問題。假設初始量為xk,Δx為在初值附近的微小增量，k為迭代次數，則誤差為ek(xk+Δx)，對其進行一階泰勒展開得：

ek(xk+Δx)≈ek(xk)+JkΔx

(18)

式中：Jk為誤差函數ek在xk附近的導數，即雅可比矩陣。

因此，對于第k個誤差函數得目標函數為：

(19)

式中：ek,bk,Δx都為列向量；Ck表示該邊在變化前的取值；Hk為高斯-牛頓法中對于海塞矩陣的近似。

目標是找到使得增量最小的Δx，所以直接令Fk(xk+Δx)對Δx的偏導數為0，得：

(20)

考慮所有邊，得到增量方程：

HΔx=-b

(21)

綜上，關于位姿圖優化問題，首先給定一個初值x0，然后計算雅可比矩陣J(xk)與誤差e(xk)，并計算增量方程，如果所得到的結果Δxk小于預定閾值，則終止迭代，否則令xk+1=xk+Δxk，重新計算。最終得到使得誤差最小的相對位姿。

4 空間非合作目標仿真智能配準實驗

使用半物理仿真平臺，通過線陣激光雷達掃描空間非合作目標獲取多幀目標的可測部位點云，驗證本文算法相對于傳統ICP算法在效率和精度方面的提高效果。

應用Pycharm Community Edition 2021，Visual Studio 2019，點云庫(Point cloud library,PCL)與GTSAM庫，實驗系統基本參數為：Windows 10系統，Intel CORE i5-8300處理器，NVIDIA GTX 1050 Ti顯卡，16 G內存。

根據任務要求和技術指標，任務的技術指標：配準平移距離誤差為±5 mm，旋轉角度誤差為±5°，半物理仿真實驗縮比為1∶10，運動狀態為自旋運動，雷達掃描幀頻率為1 Hz，目標運動時長約為100 s，即得到101幀點云數據。在線陣激光雷達可完整捕捉非合作目標的條件下，非合作目標外形可完整獲得。

4.1 空間非合作目標提取

采用激光雷達對模擬的空間非合作目標進行掃描，由于環境的復雜性，獲得的點云數據包含環境中的其他目標，如圖4所示。采用Straight Through濾波算法對點云數據進行濾波，經過計算與實驗，得到包含模擬空間非合作目標的最小長方體，提取出空間非合作目標點云，如圖5所示。

圖4 半物理仿真試驗系統掃描結果Fig.4 Scanning results of semi-physical simulation test system

圖5 空間非合作目標提取結果Fig.5 Results of spatial non-cooperative object extraction

4.2 智能配準實驗

在進行配準前，首先要對智能配準網絡進行訓練，將激光雷達對各種物體在不同工況下運動掃描所獲取的點云數據制作成訓練集；將半物理仿真實驗平臺得到的點云數據制作成測試集，其中三軸平移距離真值為0 mm，圍繞X,Y,Z軸線的旋轉角度真值分別為15°,20°和25°。

圖6 本文算法與ICP算法平移量對比Fig.6 Comparison of translation between the proposed algorithm and ICP algorithm

表1 配準平移誤差統計Table 1 Registration translation error statistics

圖7 本文算法與ICP算法旋轉量對比Fig.7 Comparison of rotation between the proposed algorithm and ICP algorithm

為進一步證明本文算法的高精度性，與文獻[9,12,16]所提出的算法進行比較。比較效果見表3。

綜上所述，本文算法的三軸平移距離優于ICP算法，但略低于其他算法；旋轉角度明顯優于ICP與其他算法；在三軸平移距離誤差為±5 mm以及三軸旋轉角度誤差為±5°的容忍范圍內，本文算法在三軸平移距離誤差、旋轉角度誤差上優于ICP算法。

表2 配準旋轉誤差統計Table 2 Registration rotation error statistics

表3 算法精度對比Table 3 Comparison of algorithm accuracy

本文算法與其他算法配準耗時結果見表4。

表4 算法耗時對比Table 4 Comparison of algorithm time consumption

由表4可知，本文配準算法耗時略優于文獻[8]所提算法，明顯優于文獻[7,10,12]所提算法。

4.3 位姿圖優化

在配準之后對配準得到的相對位姿進行位姿圖優化，以第0幀為基準，空間非合作目標的位姿為點，位姿之間的關系即變換矩陣為邊。經過位姿圖優化后的三軸平移與旋轉誤差見表5。

表5 配準誤差統計Table 5 Registration translation error statistics

根據平移距離和旋轉角度對整體運動的影響作用，較好旋轉角度會導致平移距離誤差降低，因此對六個參數誤差都賦予了權重，其中三軸平移距離權重均為1/9，三軸旋轉角度權重均為2/9，綜合描述配準效果。本文智能配準算法綜合誤差為1.7291，ICP算法為6.3598。與ICP算法相比提高約72.81%，整體配準效果有較大提升。在效率方面本文智能算法優化部分耗時為4 s，整體耗時為420 s，ICP算法整體耗時為3316 s，效率提升約87.33%。

4.4 魯棒性測試

為驗證本文智能配準算法的魯棒性，現增加對不同工況與不同噪聲下目標的配準實驗。

改變模擬目標工況，設定多組實驗，每組實驗對象工況三軸平移距離為0 mm，三軸旋轉角度見表6。

表6 各組工況Table 6 Working conditions of each group

重復4.1～4.3節的實驗，實驗結果分析見表7～8，配準結果可視化如圖8所示。其中，紅色的、點最小的為源點云；藍色的、點較小的為目標點云；綠色的、點最大的為配準結果。

表7 各不同工況組實驗結果誤差Table 7 Error of experimental results of different working conditions

表8 各不同工況組實驗結果均方根誤差Table of experimental results of different working conditions

由此可得，本文算法在針對不同工況的空間非合作目標點云配準時，仍可獲得較好穩定性與較高的配準精度。

再次對目標施以不同程度的高斯噪聲，噪聲程度如表9所示。

再次重復4.1～4.3節的實驗，實驗結果分析見表10～11，配準結果可視化如圖9所示。其中，紅色的、點最小的為源點云；藍色的、點較小的為目標點云；綠色的、點最大的為配準結果。

表9 各組噪聲程度Table 9 Noise level of each group

實驗結果表明，本文算法在面對不同工作狀態下與不同程度噪聲下的空間非合作目標可測部位點云配準時，盡管綜合誤差等有所波動，但是有較好穩定性，且仍然在任務的技術指標允許范圍內，因此本文算法具有較好魯棒性。

圖8 各不同工況配準結果Fig.8 Registration results of each working condition

表10 各不同程度噪聲組實驗結果誤差Table 10 Error of experimental results of different noise groups

表11 各不同程度噪聲組實驗結果均方根誤差Table of experimental results of different noise groups

圖9 各不同程度噪聲組別配準結果Fig.9 Registration results of different noise groups

5 結 論

本研究以解決空間非合作目標位姿測量問題為主要目的，提出了一種面向空間非合作目標的點云智能配準算法，針對空間非合作目標沒有先驗信息的特點對PointNetLK算法進行改進，并在配準后對配準結果進行位姿圖優化。在平移距離累積誤差為±5 mm、配準旋轉角度累積誤差為±5°的條件下，驗證本文算法的配準精度與效率，使用本文智能配準算法與ICP算法進行對比實驗，配準綜合誤差較ICP算法從6.3598降低到1.7291，約提升72.81%，可有效提高配準精度；單次耗時較ICP算法從33.16 s降低到4.2 s，約提升87.33%，可有效提升配準效率。與其他算法相比，精度與效率也略有提升。并且在不同工況與點云數據存在不同程度的噪聲時，仍有較好的魯棒性。為解決空間非合作目標相對位姿測量難題以及配準算法的實際應用提供了技術支持。