基于激光雷達的非合作航天器姿態測量

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

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

0 引言

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

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

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

1 姿態測量算法

1.1 算法總體設計

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

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

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

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

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

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

圖2 本文所提方法的流程

1.2 初始姿態獲取算法

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

圖3 衛星結構特征示意

1.2.1 點云分割

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

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

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

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

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

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

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

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

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

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

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

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

高斯曲率和平均曲率為

主曲率為

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

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

1.2.2 點云特征匹配

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

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

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

1.3 姿態跟蹤算法

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

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

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

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

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

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

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

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

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

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

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

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

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

2 實驗結果與分析

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

圖4 衛星模型姿態測量

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

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

圖5 初始姿態獲取實驗結果

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

圖6 姿態測量實驗結果

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

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

圖7 繞Z軸旋轉姿態測量結果

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

3 結論

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