桂 力,鄭順義,曹姝清,劉宗明,陳 赟

(1.地球空間信息技術協(xié)同創(chuàng)新中心，湖北 武漢 430079; 2.武漢大學 遙感信息工程學院,湖北 武漢 430079; 3.武漢大學 電子信息學院,湖北 武漢 430079; 4.上海航天控制技術研究所,上海 201109)

基于點云的非合作航天器位姿測量方法研究

桂 力1、2、3,鄭順義1、2,曹姝清4,劉宗明4,陳 赟4

(1.地球空間信息技術協(xié)同創(chuàng)新中心，湖北 武漢 430079; 2.武漢大學 遙感信息工程學院,湖北 武漢 430079; 3.武漢大學 電子信息學院,湖北 武漢 430079; 4.上海航天控制技術研究所,上海 201109)

針對已有基于二維影像的非合作航天器定位定姿方法在目標適應性和穩(wěn)定性方面的不足，研究了一種基于三維點云的位姿測量方法。提出一種考慮累積誤差的參數(shù)傳遞法進行序列點云的配準，用參數(shù)傳遞法獲得初始值，在初始值的引導下與基準模型進行快速精確配準，實現(xiàn)了基于實時三維點云的相對運動跟蹤，獲得了較高的解算精度，方法利用目標的幾何形狀信息，有更好的普適性和穩(wěn)健性。分別以仿真模型和現(xiàn)場模型作為測量對象進行位姿跟蹤實驗，用掃描儀獲取動態(tài)衛(wèi)星模型的三維點云并進行位姿解算，測量的運動軌跡與設計值吻合度較好，表明所提方法有較好的穩(wěn)健性和較高的精度。

非合作航天器； 位姿測量； 三維點云； 點云配準； 累積誤差； 三維運動跟蹤； 參數(shù)傳遞； ICP算法

0 引言

隨著航天技術的飛速發(fā)展，對航天器的交會對接、在軌捕獲、自動維修等的應用提出了更高的要求。近年來，國內在這些領域的研究取得了進展，如在2011年實現(xiàn)了神舟八號飛船與天宮一號目標飛行器的空間交會對接，這種對接屬于大型航天器的合作目標對接，可通過安裝主動通信設備及其它特征標志進行識別定位[1]。但針對非合作目標航天器的研究尚不成熟。非合作目標種類繁多，形狀各異，對相關技術研究提出了挑戰(zhàn)。非合作航天器的空間自主交會和空間在軌服務的研究，首先要解決相對運動過程中的位姿測量問題。目前已有學者進行了大量研究。文獻[2]提出了一種利用天文觀測圖像對衛(wèi)星及其子部件進行三維姿態(tài)估計的方法，主要用于解決地面觀測在軌衛(wèi)星的姿態(tài)測量問題；文獻[3]根據(jù)光流法的基本原理，提出了一種基于特征光流信息的空間非合作目標的相對位姿估計方法，通過STK軟件生成的衛(wèi)星視頻驗證了算法的可行性。此外，有多位研究者提出了類似的解決方法[4-5]。此類方法的共同點是基于二維影像數(shù)據(jù)確定三維位姿信息，優(yōu)點是方便靈活、成本低廉，但在光照等干擾因素的影響下常難以保證測量結果的穩(wěn)定性。

隨著三維掃描儀的出現(xiàn)，目標表面的三維點云數(shù)據(jù)的獲取越來越方便，逐漸出現(xiàn)了基于點云數(shù)據(jù)的位姿測量方法。文獻[6]用一種激光相機系統(tǒng)(LCS)估計運動目標的位姿，實驗結果表明在閉環(huán)配置下具穩(wěn)健的位姿跟蹤能力；文獻[7]針對3D測圖提出一種基于平面塊的快速配準方法，在3D成圖過程中恢復了掃描儀的位姿信息。現(xiàn)有的研究多針對合作目標，如已知目標的CAD數(shù)據(jù)，或基于某種特定的約束條件，對目標的幾何形狀有較多的限定。針對非合作目標航天器位姿測量的需求，本文研究了一種與目標形狀無關的定位定姿方法，以提高基于點云的位姿測量方法的適應性。該方法根據(jù)點云配準的基本方法，給出模型位姿參數(shù)的傳遞模型，提出配準精度的優(yōu)化方法以提升位姿測量的精度，并用仿真衛(wèi)星模型數(shù)據(jù)和現(xiàn)場真實衛(wèi)星模型數(shù)據(jù)對本文方法的有效性進行驗證，分析了試驗結果。

1 點云配準基本原理

兩片點云數(shù)據(jù)間的配準是基于三維點云數(shù)據(jù)進行位姿測量的研究基礎，快速有效完成掃描點的配準對航天器的定位定姿有重要意義，在三維物體識別、基于點云的三維重建等中也有大量的應用。點云數(shù)據(jù)表達了物體的形狀信息，點云配準的基本原理是通過一定的測度和算法使基準點云與待配準點云間達到最佳重合，即實現(xiàn)了三維點間的對應關系。對點云模型的配準問題，已提出了多鐘解決方法，如迭代最近點(ICP)算法、點標記法、遺傳算法、四點一致集法(4pcs)等[8-11]。這些方法各有優(yōu)缺點，在某些特定條件下可較好地解決配準問題，但總體來看，ICP算法由于其普適性強，有很高的幾何精度，得到了更廣泛的應用。

一般來講，配準問題可簡單描述為：對基準模型點Pi(i=1,2,…,n) 和待配準點Qi(i=1,2,…,m)，這兩組點云模型是某個剛性目標上有一定重合度的掃描片段，該掃描片段間存在一種旋轉、平移的轉換關系，通過這種轉換可將待配準點云轉換至模型點的坐標系中，實現(xiàn)重疊區(qū)域的最佳重合。一般用一種目標函數(shù)表示這種最佳重合，有

(1)

式中：qj為待配準點中的某點(一般會選取部分點參與計算)；pj為該點對應的基準模型點中的對應點；F為一種測度函數(shù)，依據(jù)對應點作為參數(shù)，以旋轉矩陣R和平移向量T為未知數(shù)，通過使用某種“距離”準則計算每對對應點間的數(shù)值，當所有數(shù)值的平方和最小時即為待求問題R，T的當前解。但實際上其中的對應點是隨R，T而變的，每解算完一次后重新選取對應點再次計算，直至達到最終的收斂條件。

點云模型配準的基本步驟如圖1所示。整個處理過程基本包括：數(shù)據(jù)預處理、對應點求取、參數(shù)解算、收斂性判斷、待配準模型更新等。其中求取對應點和解算參數(shù)屬于循環(huán)迭代過程，隨每次計算的參數(shù)R，T逐漸趨于合理，待配準模型將越來越接近于基準模型，以此達到解算最終參數(shù)的目的。

圖1 模型配準計算流程Fig.1 Flowchart of model registration calculation

2 基于點云的定位定姿方法

2.1 模型位姿參數(shù)傳遞

以某時刻航天器所在位置為參考基準，以該時刻獲得的航天器三維點云數(shù)據(jù)為基準模型，在運動過程中通過實時運動位姿跟蹤的方法，關聯(lián)相鄰點云幀間的位姿并進行參數(shù)傳遞計算，則可獲得當前三維點云相對基準模型的位置姿態(tài)信息，實現(xiàn)非合作航天器的定位定姿。

用上述兩片點云間的配準方法可確定運動過程中每次掃描的當前點云數(shù)據(jù)與相鄰點云間的位姿關系，其與基準點云間關系的確定方法如下。

設從第一幀點云至第N+1幀點云的序號分別為0，1，2，3，…，N，根據(jù)點云配準的幾何關系，兩片點云(qN,pN)間有滿足變換關系

qN=R×pN+T.

(2)

則對N+1片點云，用X0,X1,…,XN表示點云模型，則XN到XN-1的轉換參數(shù)為RN，TN(N>0)，成立

(3)

由式(3)間的傳遞關系可得點云N傳遞至點云0的計算公式為

(4)

式(4)化簡可得

(N>0,中間式當N>1時存在).

(5)

式(5)即為模型位置姿態(tài)的傳遞公式。

為便于編程計算，還可用另一種方式表達位置姿態(tài)的傳遞，即對P，Q兩片點云，將參數(shù)矩陣R，T寫成齊次型后，則變換公式為

(6)

用R表示中間的變換矩陣，則第N幀點云數(shù)據(jù)至基準幀數(shù)據(jù)間的變換關系可表示為

X0=R1×R2…RN×XN.

(7)

式中R的表達涵蓋了位置、姿態(tài)參數(shù)，形式更簡潔，也便于編程使用。

上述公式即確定了某時刻獲得的三維點云數(shù)據(jù)與基準點云數(shù)據(jù)的變換關系。由于掃描得到的序列點云間具很大的時空相關性，兩片相鄰點云間差異性很小，直接得到的三維點云數(shù)據(jù)已有較好的配準初始值。故使用每次獲得的點云與其上一幀點云進行配準，能快速獲得較好的配準結果。然后再由參數(shù)傳遞公式得到當前幀與基準幀間的變換關系，最終可獲得基準模型坐標系中的解算結果。

2.2 配準精度優(yōu)化

通過點云配準參數(shù)的傳遞計算，在得到相鄰兩片點云間的位置姿態(tài)條件下可完成任意點云至參考系的計算。但實際試驗中常發(fā)現(xiàn)當點云數(shù)量累積較多時，通過參數(shù)傳遞計算得到的結果會存在一定的系統(tǒng)誤差，該誤差隨跟蹤次數(shù)增加而增大。誤差主要源于儀器自身誤差、噪聲點云誤差、目標的幾何結構導致的配準誤差等，在兩兩配準時點云間的誤差很小，對結果的影響不大，但當累積計算時，所有配準的誤差都會參與計算，產(chǎn)生了誤差傳播，故掃描的次數(shù)越多累積誤差就越大。對這種誤差的累積傳播問題已有文獻進行了分析，給出了嚴格的誤差傳播模型，從該模型可知初始的配準誤差會隨配準過程進行傳遞[12-13]。

由上述分析知：該誤差主要由參數(shù)的傳播過程產(chǎn)生，如采取適當方法對傳播過程進行優(yōu)化，就能最大程度消除該誤差的影響。綜合考慮配準速度和精度，本文采用一種參數(shù)傳遞和精確配準結合的方法，流程如圖2所示。

圖2 點云精確配準流程Fig.2 Flowchart of precise point cloud registration

配準精度優(yōu)化計算主要步驟為：

a)對采集的最新幀點云數(shù)據(jù)N，與第N-1幀的點云數(shù)據(jù)進行配準。由于相鄰兩幀間差異性很小，該配準步驟能快速完成。

b)用基于模型位置的參數(shù)傳遞法求得當前點云幀數(shù)據(jù)相對基準模型的位姿參數(shù)，以該參數(shù)作為配準的初始值，將當前點云幀變換至基準模型所在坐標系中。通過此變換能保證待配準點云與基準模型間差異最小化，利于配準的收斂。

c)將變換后的當前點云幀與基準模型進行快速配準，獲得精確的配準結果。將配準參數(shù)與初始參數(shù)進行合并，即得到第N幀點云到基準模型間的位姿信息，完成了坐標系的統(tǒng)一。

d)采集下一幀點云數(shù)據(jù)，記N=N+1，繼續(xù)重復上述步驟。

通過上述步驟，完成了序列點云數(shù)據(jù)的精確配準。上述算法中完成了兩次點云配準過程：與前一幀相鄰點云配準及與基準模型的配準。由于序列點云數(shù)據(jù)的空間連續(xù)性，相鄰點云幀間有較高的一致性，能很快完成配準；在較準確的初始值引導下，待配準點云亦能快速完成與基準模型的配準。該算法巧妙利用了序列點云間的時空連續(xù)性，用參數(shù)傳遞方法獲得與基準模型配準的初始值，保證了配準的收斂性和精確性。實際應用中，若相對運動緩慢，在小時空范圍內輸入的序列點云量必然增多，點云間一致性更大，可間隔一段時間獲取一幀進行處理，或直接以第N-1幀的配準參數(shù)作為初始值引導第N幀點云與基準模型進行配準，也能獲得較好的效果。

在誤差傳播過程中，掃描儀自身的位置精度和指向精度也非常重要。兩種精度在實際使用時又可分為絕對精度和相對精度，決定這兩種精度指標的誤差因素有系統(tǒng)誤差、檢校誤差和偶然誤差等。一般可用通過差分運算抵消相同影響因素，所得相對精度將遠高于絕對精度，故在本文研究的求相對位姿關系問題中，相對位姿的誤差對最終的位姿測量精度影響較小。由模型位姿參數(shù)解算的參數(shù)傳遞法到配準精度的優(yōu)化方法，考慮了由相對目標的位置精度和對目標探測的指向精度及其它因素引起的配準誤差累積問題，用優(yōu)化配準方法可消除這種誤差累積的影響。

3 實驗與分析

3.1 實驗環(huán)境

為驗證本文所提方法的有效性，用微軟Kinect掃描儀進行數(shù)據(jù)掃描和算法測試，Kinect掃描儀采用“體編碼”技術，根據(jù)散斑投影在不同深度會形成不同的圖案原理，利用拍攝得到的散斑圖案與事先標定的圖案進行相關處理，可快速獲得照片的深度信息，從而得到被掃描目標表面的完整點云。

用該設備進行三維點云掃描。先將其放置在某較平穩(wěn)的平臺上作為主動方，控制平臺運動以模擬主動方與被掃描對象間的相對運動過程。在本文實驗中，將先針對仿真衛(wèi)星模型進行定性實驗分析，再用現(xiàn)場衛(wèi)星模型在有運動軌道約束條件下進行定性與定量的分析。

3.2 仿真衛(wèi)星模型實驗結果與分析

仿真中用簡易材料設計了一個衛(wèi)星模型，如圖3(a)所示。該模型由主面板、圓形天線、載荷等組成，具備一般衛(wèi)星模型的特征。Kinect掃描儀置于一個易推拉的物體上，由人工推拉該物體帶動掃描儀運動，以模仿主動方與衛(wèi)星目標間的相對運動。實驗中，設計了主動方由遠至近、由近至遠的多個來回的運動路線，并在每次運動過程中人工控制其行走路線盡可能保持直線，以驗證實驗效果。

圖3 仿真衛(wèi)星模型實驗結果Fig.3 Experimental results of simulation satellite model

通過將Kinect掃描儀與PC機相互連接，由自主開發(fā)的位姿測量軟件實時獲取掃描儀的點云數(shù)據(jù)，用本文算法解算運動過程中的相對位姿信息，結果如圖3(b)所示。圖3(b)中：為顯示解算出的所有位姿參數(shù)在空間中的可視化效果，用紅綠色三維線框表達了當前位置與姿態(tài)信息。由圖3(b)可知：所有的位置朝向較一致，表明運動跟蹤過程中在方向上無大的變化，這與實際的運動情況一致。將所有的位置點連接起來，形成圖中的黃色軌跡線，由軌跡線的走勢可發(fā)現(xiàn)：整個運動過程呈現(xiàn)為數(shù)條往返的直線狀，符合實際實驗中設計的來回走動路線。待配準點完成配準后與基準模型的疊加效果如圖3(c)所示。圖3(c)中：藍色點為基準模型；綠色點為待配準點。由點云疊加結果可知：兩者能較準確地重合，表明配準精度較高。

3.3 現(xiàn)場衛(wèi)星模型實驗結果與分析

仿真實驗測試從定性的角度說明了本文方法定位定姿有效性。為進一步分析位姿測量的精度，本文用某實驗室現(xiàn)場衛(wèi)星模型進行了另一組實驗。該組實驗使用一種精確制作的衛(wèi)星模型，在該實驗室現(xiàn)場設計了一種高精度直線導軌，導軌末端正對該衛(wèi)星模型，導軌上安置有可控制移動的運動平臺，將掃描儀置在于運動平臺上，控制其由近至遠進行運動以模擬空間目標的相對運動過程。在本實驗中，由于導軌設計為直線狀，整個運動過程將嚴格在直線性約束下進行，為實驗結果的精確分析提供了依據(jù)。

現(xiàn)場衛(wèi)星模型在一個序列運動過程中的實驗結果如圖4所示。從圖4(b)可知：衛(wèi)星相對運動參數(shù)形成的軌跡在空間呈現(xiàn)為一條很嚴格的直線，與導軌控制運動的實驗設計相符，表明位置信息得到較好恢復；所有位置的朝向一致，表明姿態(tài)信息恢復良好。變換后的待配準點云與基準點云疊加效果較佳(圖4(c))，也反映了測量精度較高。

圖4 現(xiàn)場衛(wèi)星模型實驗結果Fig.4 Experimental results of field satellite model

進一步可更精確地分析位姿參數(shù)的精度。因實驗過程中在直線軌道上行走，方向都是一致的，則可認為運動位置軌跡滿足直線模型，運動姿態(tài)全局一致。將運動過程中解算的位置參數(shù)用數(shù)值可視化方式顯示，結果如圖5所示。由圖5可更精確地觀察到整個運動過程中所有空間點呈現(xiàn)直線狀，這與圖4(b)的結果相互印證。使用直線模型，用最小二乘法對空間三維位置進行三維直線擬合，所得空間直線如圖6所示。由圖6可認為該直線為軌跡的真值位置。

圖5 位姿測量軌跡Fig.5 Trajectory of POS measurement

圖6 位姿數(shù)據(jù)擬合空間直線結果Fig.6 3-D line fitting result of POS data

為分析位置的精度，對某個空間位置點，計算該點至真值直線最近的點作為其參考真值，然后將該空間位置點的X，Y，Z三軸位置分量分別與參考真值進行差值計算，可得當前空間位置點誤差的三個分量。部分測量序列的X，Y，Z分量誤差分布如圖7所示。由圖7可知：位置量整體分布較一致，無大的跳躍現(xiàn)象，表明其穩(wěn)定性較佳；最大誤差不超過4 mm，表明位置精度較高。

圖7 位置分量誤差分布Fig.7 Error distribution of position components

對角度精度，為測量角度的真值，實驗設計中保持掃描儀與被掃描目標在三個角度分量上均無旋轉變動，即全局角度信息保持一致，因此可取所有解算出的角度值的平均值作為真值。將三個角度分量分別與該真值進行差值計算，可得各角度分量的誤差量，其中三個角度分量的誤差分布如圖8所示。由圖8可知：角度誤差量均較小，最大偏差不大于0.15°，能滿足實際應用需求。

圖8 角度分量誤差分布Fig.8 Error distribution of angle componets

4 結束語

本文針對非合作航天器相對運動過程中位姿測量問題，提出了一種基于點云配準和參數(shù)精確傳遞的位姿測量方法。用掃描儀仿真獲取非合作航天器點云數(shù)據(jù)進行測量，仿真實驗結果和現(xiàn)場實驗結果均表明本文方法有較好的穩(wěn)定性和較高的精度，能滿足實際應用需求。因本文方法無需事先獲取被測量目標的幾何信息，故對各類非合作目標航天器都具有適應性。已有的基于影像的定位定姿方法常要求目標具有某些已知的形狀或紋理信息，普適性不強，且易受光照等的影響，其結果往往會產(chǎn)生較大的跳躍，穩(wěn)定性不佳。相比之下，基于點云的方法能較好地利用目標的幾何信息，具更好的適應性和穩(wěn)定性。另一方面，相對已有的依賴于特征提取方法，本文方法能用于更多種類的航天器的位姿測量，不要求目標本身具備任何特殊的標識，可自適應地利用目標自身的幾何特征進行測量，無需對目標進行特殊處理，故也可用于未知目標的定位定姿，應用范圍更廣。由于受限于硬件，目前點云數(shù)據(jù)的獲取還無法如影像那樣對遠距離目標進行成像，本文方法的使用距離將可能受到硬件的限制，后續(xù)研究中可考慮結合兩者的優(yōu)點組合使用，以進一步提高實際應用中可測量的距離。

[1] 李隆球, 張廣玉, 柏合民, 等. 非合作目標衛(wèi)星三臂型對接機構及其力學分析[J]. 上海航天, 2015, 32(1): 5-11.

[2] 魏小峰, 耿則勛, 婁博, 等. 空間目標三維姿態(tài)估計方法[J]. 武漢大學學報(信息科學版), 2015, 40(1): 96-101+111.

[3] 程文華, 姚紅, 張雅聲. 基于特征光流信息的空間非合作目標相對位姿估計[J]. 上海航天, 2015, 32(5): 9-14.

[4] 張劍清, 張春森, 賀少軍. 雙目序列影像三維運動參數(shù)確定[J]. 武漢大學學報(信息科學版), 2006, 31(1): 43-46.

[5] 周軍, 白博, 于曉洲. 一種非合作目標相對位置和姿態(tài)確定方法[J]. 宇航學報, 2011, 32(3): 516-521.

[6] AGHILI F, KURYLLO M, OKOUNEVA G, et al. Robust pose estimation of moving objects using laser camera data for autonomous rendezvous and docking[C]// In Proceedings of the International Society of Photogrammetry and Remote Sensing Archives. Paris: [s. n.], 2009: XXXⅧ-3/W8, 253-258.

[7] PATHAK K, BIRK A, VAKEVIIUS N, et al. Fast registration based on noisy planes with unknown correspondences for 3-D mapping[J]. IEEE Transactions on Robotics, 2010, 26(3): 424-441.

[8] BESL P J, MCKAY N D. A method for registration of 3-D shapes[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence, 1992, 14(2): 239-256.

[9] 梁云波, 鄧文怡, 婁小平, 等. 基于標志點的多視三維數(shù)據(jù)自動拼接方法[J]. 北京信息科技大學學報(自然科學版), 2010, 25(1): 30-33.

[10] SILVA L, BELLON O R P, BOYER K L. Precision range image registration using a robust surface interpenetration measure and enhanced genetic algorithms[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence, 2005, 27(5): 762-776.

[11] MELLADO N, AIGER D, MITRA N J. Super 4PCS fast global pointcloud registration via smart indexing[C]// Computer Graphics Forum. [S. l.], [s. n.], 2014, 33(5): 205-215.

[12] 程效軍, 施貴剛, 王峰, 等. 點云配準誤差傳播規(guī)律的研究[J]. 同濟大學學報(自然科學版), 2009, 37(12): 1668-1672.

[13] 徐源強, 高井祥, 張麗, 等. 地面三維激光掃描的點云配準誤差研究[J]. 大地測量與地球動力學, 2011, 31(2): 129-132.

Research of Pose and Altitude Measurement for Non-Cooperative Spacecraft Based on 3D Point Clouds

GUI Li1, 2, 3, ZHENG Shun-yi1, 2, CAO Shu-qing4, LIU Zong-ming4， CHEN Yun4

(1. Collaborative Innovation Center of Geospatial Technology, Wuhan 430079, Hubei, China; 2. School of Remote Sensing and Information Engineering, Wuhan University, Wuhan 430079, Hubei, China; 3. School of Electronic Information, Wuhan University, Wuhan 430079, Hubei, China; 4. Shanghai Institute of Spaceflight Control Technology, Shanghai 201109, China)

To solve the problem that existing methods of estimating of the pose for non-cooperative spacecraft based on 2-D images have the disadvantage of poor adaptability and instability for variant spacecraft, a pose and altitude (POS) measurement method is researched, which can make full use of shape information for objects and lead to better versatility and robustness. A method of parameter transfer with considering of the error accumulation problem was proposed for sequential point clouds registration in this paper. An initial value with parameter transfer algorithm was got firstly. And then a registration with base model was made quickly by the initial value. This method leads to a result of tracking with relative movement based on real-time scanning 3-D point clouds and a high accuracy is achieved in pose and altitude measurement. Using simulation satellite model and field satellite model apart for pose tracking experiments, a series of satellite model point clouds with a 3-D point scanner and solving the results when scanning were carried on. The results showed that the measured trajectory was consistent with the designed trajectory, which meant that the method proposed in this paper was robust and high accuracy.

Non-cooperative spacecraft; Pose and altitude measurement; 3D point clouds; Point cloud registration; Accumulative error; 3D motion tracking; Parameter transfer; Iterative closest point algorithm

1006-1630(2016)06-0122-07

2016-06-13；

2016-11-29

中央高?；究蒲袠I(yè)務費專項資金資助(2042016kf0012)；上海航天科技創(chuàng)新基金資助(SAST201446)

桂 力(1986—)，男，助理研究員，主要從事計算機視覺研究。

V448.21

A

10.19328/j.cnki.1006-1630.2016.06.018