基于激光雷達的非合作航天器姿態(tài)測量

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

（1.中國科學院空間光電精密測量技術重點實驗室，四川 成都 610209；2.中國科學院光電技術研究所，四川 成都 610209；3.中國科學院大學，北京 100049）

0 引言

空間非合作目標是指未安裝光學輔助測量裝置的目標航天器［1］。對非合作目標的姿態(tài)測量是航天器相對導航、交會對接進而實施在軌服務或者太空垃圾清理等任務的前提［2］，對推動航天技術進步具有重要意義。

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

針對目標自遮擋導致的姿態(tài)解算困難以及點云配準算法效率較低等問題，本文開展對姿態(tài)估計算法的研究，提出了一種基于LiDAR三維點云的非合作目標姿態(tài)測量方法，并對所提方法進行了實驗驗證。

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

1.1 算法總體設計

為便于解算目標航天器姿態(tài)，建立目標航天器坐標系（Ot-XtYtZt），LiDAR 坐標系（Os-XsYsZs）以及世界坐標系（O-XYZ），目標航天器基準點云主軸方向為（O0-X0Y0Z0），第i時刻目標點云主軸方向為（Oi-XiYiZi），目標航天器分別繞X0，Y0，Z0軸旋轉時如圖1所示。

圖1 交會對接空間坐標系示意

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

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

對于按照Z 軸、Y 軸、X 軸旋轉順序進行的變換，總的旋轉變換關系R為

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

圖2 本文所提方法的流程

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

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

圖3 衛(wèi)星結構特征示意

1.2.1 點云分割

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

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

2）計算任意一點pi（xi，yi，zi）到該平面距離

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

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

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

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

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

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

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

曲面M在點pi處的法向量表示為

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

高斯曲率和平均曲率為

主曲率為

2）設定種子集合S，聚類集合O，對點曲率進行分析，若曲率大于閾值，則放入S中作為種子點；然后將該點的鄰域點放入集合O。

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

1.2.2 點云特征匹配

經(jīng)過太陽能帆板和通信天線的分割、識別后，將目標點云剩余部分識別為衛(wèi)星主體特征。以特征置信概率來表示特征結構完整度，計算特征置信概率并進行排序來選擇保存結構最完整的特征點云，置信概率計算方式為

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

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

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

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

點云粗配準通過對點云主成分分析提取點云特征向量，然后依據(jù)特征向量建立粗配準變化關系，實現(xiàn)步驟如下：

1）計算樣本重心。以第i時刻點云Qi為例，重心坐標計算方式為

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

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

同理，第i ＋1點云Qi＋1重心及特征向量

4）建立特征向量變換關系

5）利用旋轉矩陣R0和平移矩陣T0對點云進行轉換，完成粗配準。

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

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

2）對應點搜索。首先從根節(jié)點開始按照目標點與各個節(jié)點的比較結果向下訪問KD-tree，直至到達葉子節(jié)點。然后回溯搜索路徑，判斷其他子節(jié)點空間是否有更鄰近的節(jié)點，如果存在則進入其他子節(jié)點空間進行搜索，直至搜索路徑為空。

3）求解變換矩陣。依據(jù)步驟（2）所得的對應點關系，求解當次迭代對應的旋轉矩陣Ri和平移矩陣Ti，更新誤差函數(shù)值，得到配準誤差ei和本次迭代誤差變化值εi。

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

2 實驗結果與分析

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

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

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

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

圖5 初始姿態(tài)獲取實驗結果

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

圖6 姿態(tài)測量實驗結果

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

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

圖7 繞Z軸旋轉姿態(tài)測量結果

從圖7中可以看出，姿態(tài)測量誤差的分布依然表現(xiàn)出受初始姿態(tài)估計精度影響，存在累計誤差的特點，LiDAR從側面獲取目標點云時結構完整度降低姿態(tài)誤差變大。但對繞Z軸旋轉過程中的姿態(tài)測量表現(xiàn)出足夠的魯棒性。

3 結論

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