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

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

基于點(diǎn)云的非合作航天器位姿測(cè)量方法研究

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

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

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

非合作航天器； 位姿測(cè)量； 三維點(diǎn)云； 點(diǎn)云配準(zhǔn)； 累積誤差； 三維運(yùn)動(dòng)跟蹤； 參數(shù)傳遞； ICP算法

0 引言

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

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

1 點(diǎn)云配準(zhǔn)基本原理

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

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

(1)

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

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

圖1 模型配準(zhǔn)計(jì)算流程Fig.1 Flowchart of model registration calculation

2 基于點(diǎn)云的定位定姿方法

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

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

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

設(shè)從第一幀點(diǎn)云至第N+1幀點(diǎn)云的序號(hào)分別為0，1，2，3，…，N，根據(jù)點(diǎn)云配準(zhǔn)的幾何關(guān)系，兩片點(diǎn)云(qN,pN)間有滿(mǎn)足變換關(guān)系

qN=R×pN+T.

(2)

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

(3)

由式(3)間的傳遞關(guān)系可得點(diǎn)云N傳遞至點(diǎn)云0的計(jì)算公式為

(4)

式(4)化簡(jiǎn)可得

(N>0,中間式當(dāng)N>1時(shí)存在).

(5)

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

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

(6)

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

X0=R1×R2…RN×XN.

(7)

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

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

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

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

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

圖2 點(diǎn)云精確配準(zhǔn)流程Fig.2 Flowchart of precise point cloud registration

配準(zhǔn)精度優(yōu)化計(jì)算主要步驟為：

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

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

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

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

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

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

3 實(shí)驗(yàn)與分析

3.1 實(shí)驗(yàn)環(huán)境

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

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

3.2 仿真衛(wèi)星模型實(shí)驗(yàn)結(jié)果與分析

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

圖3 仿真衛(wèi)星模型實(shí)驗(yàn)結(jié)果Fig.3 Experimental results of simulation satellite model

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

3.3 現(xiàn)場(chǎng)衛(wèi)星模型實(shí)驗(yàn)結(jié)果與分析

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

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

圖4 現(xiàn)場(chǎng)衛(wèi)星模型實(shí)驗(yàn)結(jié)果Fig.4 Experimental results of field satellite model

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

圖5 位姿測(cè)量軌跡Fig.5 Trajectory of POS measurement

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

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

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

對(duì)角度精度，為測(cè)量角度的真值，實(shí)驗(yàn)設(shè)計(jì)中保持掃描儀與被掃描目標(biāo)在三個(gè)角度分量上均無(wú)旋轉(zhuǎn)變動(dòng)，即全局角度信息保持一致，因此可取所有解算出的角度值的平均值作為真值。將三個(gè)角度分量分別與該真值進(jìn)行差值計(jì)算，可得各角度分量的誤差量，其中三個(gè)角度分量的誤差分布如圖8所示。由圖8可知：角度誤差量均較小，最大偏差不大于0.15°，能滿(mǎn)足實(shí)際應(yīng)用需求。

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

4 結(jié)束語(yǔ)

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

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

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

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

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

[5] 周軍, 白博, 于曉洲. 一種非合作目標(biāo)相對(duì)位置和姿態(tài)確定方法[J]. 宇航學(xué)報(bào), 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] 梁云波, 鄧文怡, 婁小平, 等. 基于標(biāo)志點(diǎn)的多視三維數(shù)據(jù)自動(dòng)拼接方法[J]. 北京信息科技大學(xué)學(xué)報(bào)(自然科學(xué)版), 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] 程效軍, 施貴剛, 王峰, 等. 點(diǎn)云配準(zhǔn)誤差傳播規(guī)律的研究[J]. 同濟(jì)大學(xué)學(xué)報(bào)(自然科學(xué)版), 2009, 37(12): 1668-1672.

[13] 徐源強(qiáng), 高井祥, 張麗, 等. 地面三維激光掃描的點(diǎn)云配準(zhǔn)誤差研究[J]. 大地測(cè)量與地球動(dòng)力學(xué), 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è)務(wù)費(fèi)專(zhuān)項(xiàng)資金資助(2042016kf0012)；上海航天科技創(chuàng)新基金資助(SAST201446)

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

V448.21

A

10.19328/j.cnki.1006-1630.2016.06.018