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

(大連交通大學(xué)機(jī)械工程學(xué)院，大連 116028)

0 引 言

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

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

本文提出一種面向空間非合作目標(biāo)的點(diǎn)云智能配準(zhǔn)算法。在使用半物理仿真平臺(tái)對空間非合作目標(biāo)進(jìn)行掃描后，進(jìn)行目標(biāo)提取，剔除場景中散亂目標(biāo)和噪聲，并對PointNetLK神經(jīng)網(wǎng)絡(luò)配準(zhǔn)算法進(jìn)行改進(jìn)，將處理后的點(diǎn)云數(shù)據(jù)作為改進(jìn)后的智能配準(zhǔn)算法的輸入，后端對配準(zhǔn)結(jié)果進(jìn)行位姿圖優(yōu)化，得到空間非合作目標(biāo)的相對位姿。

1 Straight Through濾波目標(biāo)提取算法

在進(jìn)行半物理仿真地面試驗(yàn)時(shí)，由于空間限制，激光雷達(dá)對空間非合作目標(biāo)進(jìn)行掃描，往往會(huì)因?yàn)楸尘靶畔㈦s亂，提高數(shù)據(jù)處理的復(fù)雜程度。故在點(diǎn)云配準(zhǔn)算法前端對原始目標(biāo)進(jìn)行目標(biāo)提取。

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

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

(1)

2 改進(jìn)PointNetLK的點(diǎn)云智能配準(zhǔn)算法

一般深度學(xué)習(xí)神經(jīng)網(wǎng)絡(luò)模型對于圖像等規(guī)范化的輸入，易獲得較好的輸出，而點(diǎn)云相對于圖像有個(gè)十分突出的特點(diǎn)，即無序性，這也對網(wǎng)絡(luò)設(shè)計(jì)提出要求。為了克服無序的特點(diǎn)，可引入通過對稱函數(shù)的數(shù)學(xué)計(jì)算把每個(gè)點(diǎn)的數(shù)據(jù)進(jìn)行整合的方法。

2.1 改進(jìn)架構(gòu)的PointNet算法

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

圖1 PointNet架構(gòu)Fig.1 PointNet architecture

2.2 改進(jìn)損失函數(shù)的PointNetLK算法

G-1=exp(-ξ∧)

(2)

式中：ξ是李群G對應(yīng)的李代數(shù)；n∧表示將向量轉(zhuǎn)化為反對稱矩陣。

三維點(diǎn)云配準(zhǔn)問題則為找到G-1，使得：

(3)

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

(4)

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

(5)

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

(6)

式中：J+是J的偽逆。

(7)

最后的估算值Gest是迭代循環(huán)中計(jì)算的所有增量估算值的組合：

Gest=ΔGn·…·G1·ΔG0

(8)

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

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

由于空間非合作目標(biāo)沒有先驗(yàn)信息，無法提供真實(shí)變換矩陣Ggt，故損失函數(shù)應(yīng)重新設(shè)定。損失函數(shù)的目標(biāo)是使配準(zhǔn)結(jié)果點(diǎn)云和目標(biāo)點(diǎn)云中對應(yīng)點(diǎn)之間的距離最小化。所以使用式(9)表示損失函數(shù)。

(9)

配準(zhǔn)結(jié)果如圖3所示，在空間非合作目標(biāo)可測部位點(diǎn)云配準(zhǔn)中，改進(jìn)的PointNetLK點(diǎn)云智能配準(zhǔn)算法會(huì)得到較高精度的配準(zhǔn)結(jié)果。其中，紅色的、點(diǎn)最小的為源點(diǎn)云；藍(lán)色的、點(diǎn)較小的為目標(biāo)點(diǎn)云；綠色的、點(diǎn)最大的為配準(zhǔn)結(jié)果。

圖3 改進(jìn)的PointNetLK算法配準(zhǔn)效果Fig.3 The registration effects of an improved PointNetLK algorithm

2.3 非合作目標(biāo)配準(zhǔn)方式

由于ModelNet40中各類別的數(shù)據(jù)之間大小不同、位姿隨機(jī)變化，所以在初始PointNetLK算法中，無論是訓(xùn)練或是測試過程中，都以數(shù)據(jù)集的每個(gè)點(diǎn)云為目標(biāo)點(diǎn)云，隨機(jī)生成或手動(dòng)添加變換矩陣Ggt，將目標(biāo)點(diǎn)云集整體施加Ggt得到源點(diǎn)云集，最后將目標(biāo)點(diǎn)云與源點(diǎn)云配準(zhǔn)。

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

3 位姿圖優(yōu)化

在點(diǎn)云數(shù)據(jù)配準(zhǔn)完成后，往往配準(zhǔn)存在誤差，因此在配準(zhǔn)算法后端鏈接位姿圖優(yōu)化，達(dá)到提高配準(zhǔn)精度，進(jìn)而提升后續(xù)重建質(zhì)量的目的。

3.1 位姿圖優(yōu)化問題構(gòu)建

圖由點(diǎn)與邊組成，以點(diǎn)代表優(yōu)化變量，邊代表誤差，其優(yōu)化變量即是各幀間的相對位姿。

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

(10)

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

其李代數(shù)形式為：

exp(ξi+1)∧)∨

(11)

所以誤差表達(dá)式為：

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

(12)

為得到ei(i+1)關(guān)于ξi,ξi+1的導(dǎo)數(shù)，對其添加左擾動(dòng)，得到：

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

(13)

根據(jù)變換群伴隨性質(zhì)：

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

(14)

得到：

(15)

式中：

(16)

得到了誤差關(guān)于優(yōu)化變量的導(dǎo)數(shù)。

將所有誤差項(xiàng)相加得到總體目標(biāo)函數(shù)為：

(17)

3.2 基于最小二乘法的位姿圖優(yōu)化

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

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

(18)

式中：Jk為誤差函數(shù)ek在xk附近的導(dǎo)數(shù)，即雅可比矩陣。

因此，對于第k個(gè)誤差函數(shù)得目標(biāo)函數(shù)為：

(19)

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

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

(20)

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

HΔx=-b

(21)

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

4 空間非合作目標(biāo)仿真智能配準(zhǔn)實(shí)驗(yàn)

使用半物理仿真平臺(tái)，通過線陣激光雷達(dá)掃描空間非合作目標(biāo)獲取多幀目標(biāo)的可測部位點(diǎn)云，驗(yàn)證本文算法相對于傳統(tǒng)ICP算法在效率和精度方面的提高效果。

應(yīng)用Pycharm Community Edition 2021，Visual Studio 2019，點(diǎn)云庫(Point cloud library,PCL)與GTSAM庫，實(shí)驗(yàn)系統(tǒng)基本參數(shù)為：Windows 10系統(tǒng)，Intel CORE i5-8300處理器，NVIDIA GTX 1050 Ti顯卡，16 G內(nèi)存。

根據(jù)任務(wù)要求和技術(shù)指標(biāo)，任務(wù)的技術(shù)指標(biāo)：配準(zhǔn)平移距離誤差為±5 mm，旋轉(zhuǎn)角度誤差為±5°，半物理仿真實(shí)驗(yàn)縮比為1∶10，運(yùn)動(dòng)狀態(tài)為自旋運(yùn)動(dòng)，雷達(dá)掃描幀頻率為1 Hz，目標(biāo)運(yùn)動(dòng)時(shí)長約為100 s，即得到101幀點(diǎn)云數(shù)據(jù)。在線陣激光雷達(dá)可完整捕捉非合作目標(biāo)的條件下，非合作目標(biāo)外形可完整獲得。

4.1 空間非合作目標(biāo)提取

采用激光雷達(dá)對模擬的空間非合作目標(biāo)進(jìn)行掃描，由于環(huán)境的復(fù)雜性，獲得的點(diǎn)云數(shù)據(jù)包含環(huán)境中的其他目標(biāo)，如圖4所示。采用Straight Through濾波算法對點(diǎn)云數(shù)據(jù)進(jìn)行濾波，經(jīng)過計(jì)算與實(shí)驗(yàn)，得到包含模擬空間非合作目標(biāo)的最小長方體，提取出空間非合作目標(biāo)點(diǎn)云，如圖5所示。

圖4 半物理仿真試驗(yàn)系統(tǒng)掃描結(jié)果Fig.4 Scanning results of semi-physical simulation test system

圖5 空間非合作目標(biāo)提取結(jié)果Fig.5 Results of spatial non-cooperative object extraction

4.2 智能配準(zhǔn)實(shí)驗(yàn)

在進(jìn)行配準(zhǔn)前，首先要對智能配準(zhǔn)網(wǎng)絡(luò)進(jìn)行訓(xùn)練，將激光雷達(dá)對各種物體在不同工況下運(yùn)動(dòng)掃描所獲取的點(diǎn)云數(shù)據(jù)制作成訓(xùn)練集；將半物理仿真實(shí)驗(yàn)平臺(tái)得到的點(diǎn)云數(shù)據(jù)制作成測試集，其中三軸平移距離真值為0 mm，圍繞X,Y,Z軸線的旋轉(zhuǎn)角度真值分別為15°,20°和25°。

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

表1 配準(zhǔn)平移誤差統(tǒng)計(jì)Table 1 Registration translation error statistics

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

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

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

表2 配準(zhǔn)旋轉(zhuǎn)誤差統(tǒng)計(jì)Table 2 Registration rotation error statistics

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

本文算法與其他算法配準(zhǔn)耗時(shí)結(jié)果見表4。

表4 算法耗時(shí)對比Table 4 Comparison of algorithm time consumption

由表4可知，本文配準(zhǔn)算法耗時(shí)略優(yōu)于文獻(xiàn)[8]所提算法，明顯優(yōu)于文獻(xiàn)[7,10,12]所提算法。

4.3 位姿圖優(yōu)化

在配準(zhǔn)之后對配準(zhǔn)得到的相對位姿進(jìn)行位姿圖優(yōu)化，以第0幀為基準(zhǔn)，空間非合作目標(biāo)的位姿為點(diǎn)，位姿之間的關(guān)系即變換矩陣為邊。經(jīng)過位姿圖優(yōu)化后的三軸平移與旋轉(zhuǎn)誤差見表5。

表5 配準(zhǔn)誤差統(tǒng)計(jì)Table 5 Registration translation error statistics

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

4.4 魯棒性測試

為驗(yàn)證本文智能配準(zhǔn)算法的魯棒性，現(xiàn)增加對不同工況與不同噪聲下目標(biāo)的配準(zhǔn)實(shí)驗(yàn)。

改變模擬目標(biāo)工況，設(shè)定多組實(shí)驗(yàn)，每組實(shí)驗(yàn)對象工況三軸平移距離為0 mm，三軸旋轉(zhuǎn)角度見表6。

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

重復(fù)4.1～4.3節(jié)的實(shí)驗(yàn)，實(shí)驗(yàn)結(jié)果分析見表7～8，配準(zhǔn)結(jié)果可視化如圖8所示。其中，紅色的、點(diǎn)最小的為源點(diǎn)云；藍(lán)色的、點(diǎn)較小的為目標(biāo)點(diǎn)云；綠色的、點(diǎn)最大的為配準(zhǔn)結(jié)果。

表7 各不同工況組實(shí)驗(yàn)結(jié)果誤差Table 7 Error of experimental results of different working conditions

表8 各不同工況組實(shí)驗(yàn)結(jié)果均方根誤差Table of experimental results of different working conditions

由此可得，本文算法在針對不同工況的空間非合作目標(biāo)點(diǎn)云配準(zhǔn)時(shí)，仍可獲得較好穩(wěn)定性與較高的配準(zhǔn)精度。

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

再次重復(fù)4.1～4.3節(jié)的實(shí)驗(yàn)，實(shí)驗(yàn)結(jié)果分析見表10～11，配準(zhǔn)結(jié)果可視化如圖9所示。其中，紅色的、點(diǎn)最小的為源點(diǎn)云；藍(lán)色的、點(diǎn)較小的為目標(biāo)點(diǎn)云；綠色的、點(diǎn)最大的為配準(zhǔn)結(jié)果。

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

實(shí)驗(yàn)結(jié)果表明，本文算法在面對不同工作狀態(tài)下與不同程度噪聲下的空間非合作目標(biāo)可測部位點(diǎn)云配準(zhǔn)時(shí)，盡管綜合誤差等有所波動(dòng)，但是有較好穩(wěn)定性，且仍然在任務(wù)的技術(shù)指標(biāo)允許范圍內(nèi)，因此本文算法具有較好魯棒性。

圖8 各不同工況配準(zhǔn)結(jié)果Fig.8 Registration results of each working condition

表10 各不同程度噪聲組實(shí)驗(yàn)結(jié)果誤差Table 10 Error of experimental results of different noise groups

表11 各不同程度噪聲組實(shí)驗(yàn)結(jié)果均方根誤差Table of experimental results of different noise groups

圖9 各不同程度噪聲組別配準(zhǔn)結(jié)果Fig.9 Registration results of different noise groups

5 結(jié) 論

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