馮 田，馮志輝，南亞明，雷 銘

（1.中國(guó)科學(xué)院空間光電精密測(cè)量技術(shù)重點(diǎn)實(shí)驗(yàn)室，四川 成都 610209；2.中國(guó)科學(xué)院光電技術(shù)研究所，四川 成都 610209；3.中國(guó)科學(xué)院大學(xué)，北京 100049）

0 引言

空間非合作目標(biāo)是指未安裝光學(xué)輔助測(cè)量裝置的目標(biāo)航天器［1］。對(duì)非合作目標(biāo)的姿態(tài)測(cè)量是航天器相對(duì)導(dǎo)航、交會(huì)對(duì)接進(jìn)而實(shí)施在軌服務(wù)或者太空垃圾清理等任務(wù)的前提［2］，對(duì)推動(dòng)航天技術(shù)進(jìn)步具有重要意義。

非合作目標(biāo)姿態(tài)測(cè)量主要有兩類(lèi)技術(shù)路線：1）基于視覺(jué)的測(cè)量［3］，利用單目或雙目相機(jī)對(duì)目標(biāo)成像，結(jié)合圖像處理算法解算相對(duì)姿態(tài)；2）基于激光雷達(dá)（LiDAR）的測(cè)量［4］，利用LiDAR等主動(dòng)光學(xué)敏感器獲取目標(biāo)三維點(diǎn)云，再由點(diǎn)云處理算法解算姿態(tài)。太空環(huán)境光照條件較為惡劣，相機(jī)系統(tǒng)難以連續(xù)獲得高質(zhì)量圖像，LiDAR等主動(dòng)光學(xué)成像系統(tǒng)通過(guò)計(jì)算激光回波信號(hào)實(shí)現(xiàn)測(cè)量，受到目標(biāo)運(yùn)動(dòng)狀態(tài)、光照影響較小，易于全天候長(zhǎng)時(shí)間穩(wěn)定觀測(cè)與跟蹤。近年來(lái)，許多學(xué)者在點(diǎn)云配準(zhǔn)［5］框架下提出了點(diǎn)云投影［6］、點(diǎn)法向量［7］、卡爾曼濾波［8］、模型匹配［9，10］等姿態(tài)測(cè)量方法，但現(xiàn)有方法存在一些急需解決的問(wèn)題。首先是在交會(huì)對(duì)接過(guò)程中，LiDAR只能從單一視角獲取點(diǎn)云數(shù)據(jù)，目標(biāo)自遮擋會(huì)導(dǎo)致點(diǎn)云結(jié)構(gòu)不全，現(xiàn)階段的姿態(tài)測(cè)量算法對(duì)點(diǎn)云質(zhì)量要求較高，點(diǎn)云結(jié)構(gòu)不完整會(huì)對(duì)點(diǎn)云配準(zhǔn)精度造成較大影響［11］。其次是現(xiàn)有配準(zhǔn)算法計(jì)算效率較低［12］，計(jì)算速度不能滿足在軌實(shí)時(shí)測(cè)量的要求。

針對(duì)目標(biāo)自遮擋導(dǎo)致的姿態(tài)解算困難以及點(diǎn)云配準(zhǔn)算法效率較低等問(wèn)題，本文開(kāi)展對(duì)姿態(tài)估計(jì)算法的研究，提出了一種基于LiDAR三維點(diǎn)云的非合作目標(biāo)姿態(tài)測(cè)量方法，并對(duì)所提方法進(jìn)行了實(shí)驗(yàn)驗(yàn)證。

1 姿態(tài)測(cè)量算法

1.1 算法總體設(shè)計(jì)

為便于解算目標(biāo)航天器姿態(tài)，建立目標(biāo)航天器坐標(biāo)系（Ot-XtYtZt），LiDAR 坐標(biāo)系（Os-XsYsZs）以及世界坐標(biāo)系（O-XYZ），目標(biāo)航天器基準(zhǔn)點(diǎn)云主軸方向?yàn)椋∣0-X0Y0Z0），第i時(shí)刻目標(biāo)點(diǎn)云主軸方向?yàn)椋∣i-XiYiZi），目標(biāo)航天器分別繞X0，Y0，Z0軸旋轉(zhuǎn)時(shí)如圖1所示。

圖1 交會(huì)對(duì)接空間坐標(biāo)系示意

由圖1可以知道，通過(guò)求解目標(biāo)點(diǎn)云與基準(zhǔn)點(diǎn)云的空間變換關(guān)系即可解出目標(biāo)姿態(tài)信息。因此，本文通過(guò)對(duì)目標(biāo)模型進(jìn)行地面數(shù)據(jù)采集將對(duì)接姿態(tài)目標(biāo)航天器的點(diǎn)云數(shù)據(jù)存儲(chǔ)于追逐航天器計(jì)算機(jī)，將姿態(tài)測(cè)量問(wèn)題轉(zhuǎn)化為目標(biāo)點(diǎn)云與基準(zhǔn)點(diǎn)云的配準(zhǔn)問(wèn)題。目標(biāo)點(diǎn)云空間變換可以分解為旋轉(zhuǎn)變換和平移變換，目標(biāo)航天器三維點(diǎn)云的變換矩陣由式（1）給出

式中 R為旋轉(zhuǎn)變換矩陣，T為平移變換矩陣，V為視變化因子，S為比例縮放因子。其中，旋轉(zhuǎn)矩陣R可以分解為滾轉(zhuǎn)、俯仰、偏航軸旋轉(zhuǎn)矩陣

對(duì)于按照Z(yǔ) 軸、Y 軸、X 軸旋轉(zhuǎn)順序進(jìn)行的變換，總的旋轉(zhuǎn)變換關(guān)系R為

基于上述分析，本文將姿態(tài)測(cè)量方法分為初始姿態(tài)獲取和快速姿態(tài)跟蹤兩部分，初始姿態(tài)獲取階段利用特征點(diǎn)云的分割與匹配解決目標(biāo)自遮擋導(dǎo)致的點(diǎn)云配準(zhǔn)困難問(wèn)題，在姿態(tài)跟蹤階段提出一種快速點(diǎn)云配準(zhǔn)算法實(shí)現(xiàn)姿態(tài)實(shí)時(shí)跟蹤測(cè)量，該方法流程如圖2所示。

圖2 本文所提方法的流程

1.2 初始姿態(tài)獲取算法

目標(biāo)航天器通常為失效或故障的衛(wèi)星［13］，通過(guò)觀察空間結(jié)構(gòu)可以發(fā)現(xiàn)，衛(wèi)星主要有：衛(wèi)星主體、通信天線、太陽(yáng)能帆板三大部分，且具有不同的結(jié)構(gòu)特征，如圖3 所示。依據(jù)結(jié)構(gòu)特征可以實(shí)現(xiàn)對(duì)關(guān)鍵特征的分割和識(shí)別，再利用關(guān)鍵特征與模板匹配獲得初始變換矩陣。

圖3 衛(wèi)星結(jié)構(gòu)特征示意

1.2.1 點(diǎn)云分割

首先提取特征比較明顯的太陽(yáng)能帆板，通過(guò)提取點(diǎn)云平面可以分割出帆板的點(diǎn)云數(shù)據(jù)。以隨機(jī)采樣不斷提出局外點(diǎn)的方式分割點(diǎn)云，分割步驟如下：

1）設(shè)定算法隨機(jī)點(diǎn)選取次數(shù)t，然后在目標(biāo)三維點(diǎn)云數(shù)據(jù)中隨機(jī)選取初始點(diǎn)分別為p1（x1，y1，z1），p2（x2，y2，z2），p3（x3，y3，z3），再以初始點(diǎn)確定一個(gè)平面并計(jì)算平面法向量

2）計(jì)算任意一點(diǎn)pi（xi，yi，zi）到該平面距離

3）設(shè)定閾值τ，對(duì)于di＜τ的點(diǎn)云數(shù)據(jù)判定為平面中的點(diǎn)，將符合條件的點(diǎn)云保存下來(lái)，并記錄點(diǎn)的數(shù)量。

4）重復(fù)步驟（1）～步驟（3），將點(diǎn)數(shù)量最多的點(diǎn)云保存下來(lái)作為帆板特征。

結(jié)合通信天線曲面特征明顯的特點(diǎn)，采用曲率聚類(lèi)分割的方法，具體實(shí)現(xiàn)步驟如下：

1）計(jì)算點(diǎn)主曲率，再基于主曲率對(duì)點(diǎn)云進(jìn)行排序選取種子點(diǎn)，通過(guò)最小二乘法擬合構(gòu)建二次曲面求解點(diǎn)曲率，以點(diǎn)pi為鄰域中心，半徑為r的球體空間內(nèi)的鄰域點(diǎn)構(gòu)建二次曲面M，曲面方程為

其中，a，b，c為二次曲面系數(shù)，根據(jù)最小二乘原理，二次曲面系數(shù)使得式（7）取最小值

式中 xi，yi，zi為鄰域點(diǎn)坐標(biāo)；通過(guò)對(duì)式（7）求偏導(dǎo)并使偏導(dǎo)數(shù)為零，可以求解最佳擬合曲面的參數(shù)a，b，c的值

曲面M在點(diǎn)pi處的一階、二階偏導(dǎo)數(shù)為

曲面M在點(diǎn)pi處的法向量表示為

根據(jù)曲面第一、二基本公式計(jì)算曲面M 的第一、二基本量

高斯曲率和平均曲率為

主曲率為

2）設(shè)定種子集合S，聚類(lèi)集合O，對(duì)點(diǎn)曲率進(jìn)行分析，若曲率大于閾值，則放入S中作為種子點(diǎn)；然后將該點(diǎn)的鄰域點(diǎn)放入集合O。

3）若S為非空集合，重復(fù)步驟（2），直到S為空集，保存點(diǎn)數(shù)量最多的聚類(lèi)集合。

1.2.2 點(diǎn)云特征匹配

經(jīng)過(guò)太陽(yáng)能帆板和通信天線的分割、識(shí)別后，將目標(biāo)點(diǎn)云剩余部分識(shí)別為衛(wèi)星主體特征。以特征置信概率來(lái)表示特征結(jié)構(gòu)完整度，計(jì)算特征置信概率并進(jìn)行排序來(lái)選擇保存結(jié)構(gòu)最完整的特征點(diǎn)云，置信概率計(jì)算方式為

式中 psolar_panel，pantenna，pmain分別為太陽(yáng)帆板特征、通信天線特征、衛(wèi)星主體特征的置信概率；N為對(duì)應(yīng)特征點(diǎn)云的點(diǎn)數(shù)量。

模擬地面預(yù)存儲(chǔ)目標(biāo)衛(wèi)星的特征模板，將模型區(qū)間設(shè)定為π／2，即分別以X，Y，Z 軸旋轉(zhuǎn)π／2，π，3π／2，2π 采集特征點(diǎn)云，共計(jì)64 組特征模板；以最近點(diǎn)距離平方和作為評(píng)價(jià)參數(shù)，以距離最小的模型姿態(tài)計(jì)算初始變換矩陣R0，T0。然后對(duì)目標(biāo)點(diǎn)云和基準(zhǔn)點(diǎn)云P0應(yīng)用迭代最近點(diǎn)（iterative closest point，ICP）算法精配準(zhǔn)，計(jì)算初始姿態(tài)獲取變換矩陣Rorigin，Torigin；最后結(jié)合式（3）解算初始姿態(tài)信息。

1.3 姿態(tài)跟蹤算法

為實(shí)現(xiàn)對(duì)目標(biāo)航天器的實(shí)時(shí)姿態(tài)跟蹤，本文提出一種改進(jìn)的點(diǎn)云配準(zhǔn)算法實(shí)現(xiàn)快速點(diǎn)云配準(zhǔn)。對(duì)兩幀點(diǎn)云進(jìn)行主成分分析提取特征向量，通過(guò)計(jì)算特征向量變換關(guān)系獲取粗配準(zhǔn)矩陣，然后利用KD-tree最近鄰搜索提高對(duì)應(yīng)點(diǎn)搜索效率對(duì)ICP算法進(jìn)行改進(jìn)，完成精配準(zhǔn)。

點(diǎn)云粗配準(zhǔn)通過(guò)對(duì)點(diǎn)云主成分分析提取點(diǎn)云特征向量，然后依據(jù)特征向量建立粗配準(zhǔn)變化關(guān)系，實(shí)現(xiàn)步驟如下：

1）計(jì)算樣本重心。以第i時(shí)刻點(diǎn)云Qi為例，重心坐標(biāo)計(jì)算方式為

2）計(jì)算協(xié)方差矩陣，如式（16）所示

3）特征值分解。對(duì)協(xié)方差矩陣奇異值分解取前3 個(gè)方差最大特征值對(duì)應(yīng)的特征向量

同理，第i ＋1點(diǎn)云Qi＋1重心及特征向量

4）建立特征向量變換關(guān)系

5）利用旋轉(zhuǎn)矩陣R0和平移矩陣T0對(duì)點(diǎn)云進(jìn)行轉(zhuǎn)換，完成粗配準(zhǔn)。

精配準(zhǔn)階段應(yīng)用改進(jìn)的ICP 算法來(lái)提高點(diǎn)云配準(zhǔn)效率。根據(jù)ICP算法的原理可以分析到，該算法是對(duì)整個(gè)點(diǎn)云數(shù)據(jù)逐一尋找對(duì)應(yīng)關(guān)系，計(jì)算效率較低［14］。為提高精配準(zhǔn)計(jì)算速度，本文采用基于k 維二叉樹(shù)的最近鄰搜索改進(jìn)ICP算法的對(duì)應(yīng)點(diǎn)搜索策略，具體步驟分為二叉樹(shù)搜索和回溯查找，改進(jìn)ICP算法配準(zhǔn)步驟如下：

1）配準(zhǔn)初始化。輸入經(jīng)過(guò)粗配準(zhǔn)的源點(diǎn)云Qi＋1和目標(biāo)點(diǎn)云，建立點(diǎn)云KD-tree，設(shè)置最大迭代次數(shù)imax、誤差閾值efinal和單次迭代誤差變化閾值εfinal。

2）對(duì)應(yīng)點(diǎn)搜索。首先從根節(jié)點(diǎn)開(kāi)始按照目標(biāo)點(diǎn)與各個(gè)節(jié)點(diǎn)的比較結(jié)果向下訪問(wèn)KD-tree，直至到達(dá)葉子節(jié)點(diǎn)。然后回溯搜索路徑，判斷其他子節(jié)點(diǎn)空間是否有更鄰近的節(jié)點(diǎn)，如果存在則進(jìn)入其他子節(jié)點(diǎn)空間進(jìn)行搜索，直至搜索路徑為空。

3）求解變換矩陣。依據(jù)步驟（2）所得的對(duì)應(yīng)點(diǎn)關(guān)系，求解當(dāng)次迭代對(duì)應(yīng)的旋轉(zhuǎn)矩陣Ri和平移矩陣Ti，更新誤差函數(shù)值，得到配準(zhǔn)誤差ei和本次迭代誤差變化值εi。

4）更新點(diǎn)集并判斷迭代條件。重復(fù)步驟（2）～步驟（3），直至滿足任一迭代結(jié)束條件：配準(zhǔn)誤差小于閾值、單次迭代誤差變化值小于閾值、或者迭代次數(shù)大于最大迭代次數(shù)，得到最終的變換矩陣［R，T］，進(jìn)而解算目標(biāo)姿態(tài)角。

2 實(shí)驗(yàn)結(jié)果與分析

實(shí)驗(yàn)通過(guò)3D打印衛(wèi)星模型，利用三維轉(zhuǎn)臺(tái)模擬衛(wèi)星轉(zhuǎn)動(dòng)，使用16線LiDAR采集目標(biāo)的三維點(diǎn)云信息，使用仿真軟件做算法運(yùn)行與姿態(tài)解算，運(yùn)行于Intel?CoreTMi5—8250U＠1.6 GHz的CPU、8 GB內(nèi)存、64 位Windows10 操作系統(tǒng)的計(jì)算機(jī)上，實(shí)驗(yàn)平臺(tái)如圖4所示。

圖4 衛(wèi)星模型姿態(tài)測(cè)量

衛(wèi)星模型實(shí)測(cè)實(shí)驗(yàn)首先模擬地面對(duì)在軌目標(biāo)的基準(zhǔn)點(diǎn)云預(yù)建立，即地面采集目標(biāo)模型的基準(zhǔn)點(diǎn)云并建立特征點(diǎn)云模板庫(kù)。通過(guò)LiDAR在目標(biāo)±X，±Y，±Z方向的6個(gè)視角采集衛(wèi)星模型點(diǎn)云數(shù)據(jù)，然后進(jìn)行分割、融合等作為基準(zhǔn)點(diǎn)云。隨后在不同的姿態(tài)角采集點(diǎn)云建立特征模板庫(kù)。然后模擬在軌服務(wù)任務(wù)中交會(huì)對(duì)接最終逼近段對(duì)目標(biāo)航天器的姿態(tài)測(cè)量，由LiDAR 采集目標(biāo)點(diǎn)云數(shù)據(jù)，并依據(jù)本文所提算法進(jìn)行姿態(tài)解算。

在第一組實(shí)驗(yàn)中，利用六自由度轉(zhuǎn)臺(tái)作為控制模塊，控制精度在微弧度量級(jí)。具體實(shí)驗(yàn)方案如下：使用三維轉(zhuǎn)臺(tái)控制模型分別繞X，Y，Z軸旋轉(zhuǎn)，轉(zhuǎn)動(dòng)范圍為（－10°，10°），每5°采集點(diǎn)云數(shù)據(jù)，共計(jì)125個(gè)初始姿態(tài)測(cè)量數(shù)據(jù)。然后應(yīng)用本文所提初始姿態(tài)獲取算法進(jìn)行姿態(tài)解算，驗(yàn)證初始姿態(tài)獲取算法的精度，每次測(cè)量平均姿態(tài)誤差計(jì)算結(jié)果如圖5所示。

圖5 初始姿態(tài)獲取實(shí)驗(yàn)結(jié)果

由圖5可以知道，本文所提初始姿態(tài)估計(jì)算法在不同姿態(tài)具有穩(wěn)定姿態(tài)解算能力，平均初始姿態(tài)估計(jì)誤差小于1.5°。然后將初始姿態(tài)角度分別設(shè)定為（0°，0°，0°），（－10°，－10°，－10°），（－5°，－5°，－5°），（5°，5°，5°），（10°，10°，10°）進(jìn)行初始姿態(tài)測(cè)量，然后在三維轉(zhuǎn)臺(tái)量程范圍（－10°，10°）隨機(jī)設(shè)置三維姿態(tài)進(jìn)行姿態(tài)跟蹤實(shí)驗(yàn)，每組實(shí)驗(yàn)跟蹤姿態(tài)變換20 次，共125 個(gè)姿態(tài)測(cè)量數(shù)據(jù)，測(cè)量結(jié)果如圖6所示。

圖6 姿態(tài)測(cè)量實(shí)驗(yàn)結(jié)果

從圖6中可以看出，本文所提姿態(tài)估計(jì)算法具有較高魯棒性，姿態(tài)測(cè)量誤差小于2°，姿態(tài)跟蹤精度建立在初始姿態(tài)估計(jì)的基礎(chǔ)上，并且隨著跟蹤時(shí)間的增多，會(huì)積累一定累計(jì)誤差。

在第二組實(shí)驗(yàn)中，機(jī)械轉(zhuǎn)臺(tái)控制衛(wèi)星模型繞Z軸360°旋轉(zhuǎn)測(cè)量。試驗(yàn)方案如下：機(jī)械轉(zhuǎn)臺(tái)控制衛(wèi)星模型轉(zhuǎn)動(dòng)，角度范圍（－360°，360°），分布將初始姿態(tài)設(shè)置為（0°，0°，0°），（0°，0°，60°），（0°，0°，120°），（0°，0°，180°），（0°，0°，240°），（0°，0°，360°），在姿態(tài)區(qū)間間隔5°采集目標(biāo)點(diǎn)云進(jìn)行姿態(tài)測(cè)量與跟蹤，共計(jì)72個(gè)測(cè)量數(shù)據(jù)，測(cè)量結(jié)果如圖7所示。

圖7 繞Z軸旋轉(zhuǎn)姿態(tài)測(cè)量結(jié)果

從圖7中可以看出，姿態(tài)測(cè)量誤差的分布依然表現(xiàn)出受初始姿態(tài)估計(jì)精度影響，存在累計(jì)誤差的特點(diǎn)，LiDAR從側(cè)面獲取目標(biāo)點(diǎn)云時(shí)結(jié)構(gòu)完整度降低姿態(tài)誤差變大。但對(duì)繞Z軸旋轉(zhuǎn)過(guò)程中的姿態(tài)測(cè)量表現(xiàn)出足夠的魯棒性。

3 結(jié)論

本文面向空間交會(huì)對(duì)接任務(wù)中LiDAR 對(duì)模型已知非合作目標(biāo)姿態(tài)測(cè)量需求，針對(duì)目標(biāo)自遮擋導(dǎo)致點(diǎn)云配準(zhǔn)性能差等問(wèn)題提出了一種基于LiDAR 的非合作目標(biāo)姿態(tài)測(cè)量方法。通過(guò)點(diǎn)云特征分割與匹配魯棒地獲取目標(biāo)初始姿態(tài)，再結(jié)合快速點(diǎn)云配準(zhǔn)算法實(shí)現(xiàn)姿態(tài)跟蹤。搭建衛(wèi)星模型姿態(tài)測(cè)量平臺(tái)對(duì)所提方法進(jìn)行實(shí)驗(yàn)驗(yàn)證，實(shí)驗(yàn)結(jié)果表明：本文方法能夠穩(wěn)定完成姿態(tài)測(cè)量。