張歡 王立武 唐明章

?

一種航天器位姿測量的魯棒正交迭代法

張歡 王立武 唐明章

（北京空間機電研究所，北京 100094）

航天器位姿測量是許多空間任務(wù)和地面大型試驗必不可少的關(guān)鍵環(huán)節(jié)，基于合作標(biāo)志點的單目位姿測量技術(shù)是實現(xiàn)該測量的重要手段之一；由于受到外界復(fù)雜環(huán)境的干擾，標(biāo)志點提取可能會出現(xiàn)野值點或者精度不一致的情況，傳統(tǒng)的正交迭代方法對于該類問題不能夠達到最優(yōu)的求解精度，甚至求解錯誤。文章提出了一種基于M估計的魯棒型正交迭代方法，使用M估計給每個標(biāo)志點分配不同的權(quán)重，優(yōu)化加權(quán)物方殘差目標(biāo)函數(shù)得到位姿參數(shù)。通過仿真驗證和實物試驗，證明了該方法可以較好地消除錯誤數(shù)據(jù)對測量的影響，提高了正交迭代算法的測量精度和魯棒性。

位姿測量 正交迭代法 權(quán)值估計 魯棒 航天器

0 引言

精確測量航天器的位姿是航天領(lǐng)域的一項基礎(chǔ)技術(shù)[1-2]，是進行空間交會對接[3-4]、編隊飛行[5]、太空機器臂[6]操作等空間任務(wù)不可缺少的前提，同時也是探測器緩沖著陸運動拋投實驗[7]等地面驗證試驗的基礎(chǔ)。近年來，由于光學(xué)相機體積小質(zhì)量輕，基于圖像的測量方法近距離測量精度高，因此基于單目相機圖像的合作目標(biāo)位姿測量技術(shù)在航天領(lǐng)域得到了深入的研究和應(yīng)用。例如，美國的軌道快車項目在目標(biāo)星上布置了角反射鏡作為特征點，并在追蹤星上安裝了相機，相機對這些標(biāo)志點進行檢測后，使用合作目標(biāo)位姿測量技術(shù)測得航天器之間的相對位姿[8]。

合作目標(biāo)位姿測量技術(shù)利用人工布設(shè)在航天器上的特征光點或者是航天器本身具有的結(jié)構(gòu)特征點，通過攝影測量可以求解得到航天器的位置和姿態(tài)參數(shù)。在攝影測量學(xué)中，使用個已知標(biāo)志點的三維世界坐標(biāo)以及二維圖像像素坐標(biāo)，求解世界坐標(biāo)系相對于相機坐標(biāo)系的問題，稱為點透視算法（PnP）[9]。近些年，PnP問題研究得到了廣泛的研究：第一個快速而高精度的算法是EPnP算法[10]，該方法通過最小化一個代數(shù)誤差，可以快速解算位姿；直接最小二乘（DLS）算法[11]和OPnP方法[12]是可以快速、精確、能夠處理多解情況的算法，它們使用多項式結(jié)式的方法得到殘差的解析解，但往往提供了過多的可能解析解，且其中的絕大多數(shù)都不符合要求；UPnP方法[13]是一種能夠同時處理中心投影模型和非中心投影模型的、線性復(fù)雜度的、并且是幾何最優(yōu)的算法；正交迭代（Orthogonal Iteration，OI）算法[14]是一種經(jīng)典的廣泛使用的迭代算法，與上述方法不同，該方法專門為了求解位姿估計問題而設(shè)計，具有精度高和計算速度快的優(yōu)點，被廣泛的應(yīng)用于位姿測量領(lǐng)域。

近些年的PnP問題的求解著重在于提高求解速度和提高精度，而在航天領(lǐng)域中的位姿參數(shù)測量中還需要考慮算法的魯棒性。這是由于太空環(huán)境中的強烈太陽光照或者是地面模擬試驗環(huán)境中強烈燈光的影響，會導(dǎo)致圖像出現(xiàn)過曝光現(xiàn)象。過曝光將大大降低標(biāo)志點的提取精度，甚至得到錯誤的標(biāo)志點圖像坐標(biāo)，即出現(xiàn)野值點。以上的介紹的EPnP、DLS和OI等PnP方法本質(zhì)上是求解最小二乘問題，并且它們均假設(shè)每個標(biāo)志點的提取精度一致，這不符合實際中野值點測量誤差遠(yuǎn)大于普通標(biāo)志點測量誤差的事實。因此在存在野值點的情況下，上述PnP方法均不能夠得到較好的位姿參數(shù)測量結(jié)果。

為了克服以上的困難，提高PnP算法魯棒性，本文采用了魯棒估計技術(shù)中的M估計[15]對正交迭代方法進行改進。其關(guān)鍵思想是通過賦予每個標(biāo)志點不同的權(quán)重來改進優(yōu)化經(jīng)典正交迭代方法的目標(biāo)函數(shù)，選擇能使該目標(biāo)函數(shù)達到最小值的位姿參數(shù)作為輸出參數(shù)。權(quán)值的選擇使用了魯棒估計技術(shù)中的M估計，該方法可以自動地估計每個標(biāo)志點的提取精度，精度越高賦予的權(quán)值越大；對于野值點，則賦予零權(quán)值。最終消除野值點與測量誤差較大的點對位姿參數(shù)的影響，使得改進后的正交迭代法在保證計算精度的同時，也具有了較強的魯棒性。

1 經(jīng)典的正交迭代方法

為了使用正交迭代方法測量航天器位姿，需要在目標(biāo)航天器上布置標(biāo)志點，并建立一個與目標(biāo)航天器固連的坐標(biāo)系，稱為世界坐標(biāo)系，此外還需要在追蹤航天器上或者是實驗場中布置一臺相機，用于獲得標(biāo)志的圖像，并建立一個相機坐標(biāo)系[16]。

OI方法的基本思想是將求解旋轉(zhuǎn)矩陣和平移向量的流程分開。在旋轉(zhuǎn)矩陣已知的情況下，首先求解平移向量OI()使得目標(biāo)函數(shù)OI(,)最小，

最后，使用求解絕對定向問題[17]的方法得到旋轉(zhuǎn)矩陣。該流程可以循環(huán)往復(fù)，依次得到一系列的和，當(dāng)位姿參數(shù)變化程度很小的時候停止迭代，輸出和。

2 魯棒正交迭代方法

從式（3）表示的優(yōu)化目標(biāo)函數(shù)中可以看出，該目標(biāo)函數(shù)將所有的觀測點數(shù)據(jù)都視為“平等”，即權(quán)值均相同。此時如果觀測數(shù)據(jù)中存在野值點，該野值點必然會嚴(yán)重影響位姿參數(shù)測量。

為了克服野值點帶來的影響，采用基于M估計的加權(quán)正交迭代算法以提高正交迭代算法的魯棒性。該方法的基本思想是根據(jù)每一個標(biāo)志點的提取精度，自動賦予以不同的權(quán)值。

式中 閾值4.685由M估計方法給定[15]，該值的選取使得當(dāng)殘差滿足標(biāo)準(zhǔn)正態(tài)分布時參數(shù)估計效果最優(yōu)。

雙權(quán)數(shù)函數(shù)的圖像如圖1所示，從圖中可以發(fā)現(xiàn)，歸一化殘差的絕對值越大，權(quán)值越小。當(dāng)歸一化殘差絕對值大于4.685的時候，認(rèn)為該次的觀測數(shù)據(jù)為野值點，賦予的權(quán)值為0，因而野值點數(shù)據(jù)不會影響加權(quán)物方殘差函數(shù)，進而達到了剔除野值點對于位姿參數(shù)測量的影響。

圖1 雙權(quán)數(shù)函數(shù)圖像

權(quán)值確定之后，通過最小化加權(quán)物方殘差函數(shù)可以求解得到位姿參數(shù)。首先將式（3）所示的目標(biāo)函數(shù)進行加權(quán)，得到目標(biāo)函數(shù)WOI(,)，即

式中WOI(,)為加權(quán)正交迭代算法（Weighted Orthogonal Iteration，WOI）的目標(biāo)函數(shù)；為第個標(biāo)志點對應(yīng)的權(quán)重，≥0，權(quán)值的選擇是通過M估計自動計算得出。當(dāng)旋轉(zhuǎn)矩陣給定的時候，WOI(,)可寫成

然后按類似于經(jīng)典的正交迭代的流程，將旋轉(zhuǎn)矩陣和平移向量依次使用迭代的方法進行求解。具體而言，首先分別計算()和的中心和，即

然后計算矩陣

根據(jù)式（7）可以得到平移向量更新值(new)。如此循環(huán)往復(fù)，直到旋轉(zhuǎn)矩陣和平移向量更新值穩(wěn)定為止。

綜合以上所述，魯棒正交迭代算法步驟如下：

1）使用經(jīng)典的正交迭代方法求解得到位姿參數(shù)初始值初始解(k)和(k)，其中表示迭代次數(shù)，初始時=0；

4）根據(jù)式（9）~（11）得到旋轉(zhuǎn)矩陣更新值(k+1)，根據(jù)式（7）算得平移向量更新值(k+1)。

5）如果旋轉(zhuǎn)矩陣和平移向量的變化量均小于給定閾值，則停止迭代，否則返回步驟2）。

3 仿真和試驗驗證

3.1 仿真試驗

為了驗證魯棒正交迭代方法的有效性，利用Matlab進行了仿真驗證，在計算精度和魯棒性方面將其與傳統(tǒng)的正交迭代方法進行對比。

為了與實驗室實際使用的相機的內(nèi)參數(shù)矩陣保持一致，仿真中采用的相機內(nèi)參數(shù)矩陣為

由于實驗室采用的中心投影相機，因此矩陣中方向和方向的等效焦距數(shù)值相同，光心圖像坐標(biāo)兩個分量也相同。

每一次仿真中，隨機生成旋轉(zhuǎn)矩陣和平移向量。由于只有三個自由度，表示剛體繞三個軸的轉(zhuǎn)角，可使用三維的羅德里格斯向量來等價表示。平移向量的、方向的分量t、t在[–100 mm，100 mm]內(nèi)獨立均勻分布，最后一個分量t選擇為1 500 mm加一個[–250 mm，250 mm]間的均勻分布。仿真使用的標(biāo)志點個數(shù)為12個，均勻分布在世界坐標(biāo)系[–100 mm，100 mm]×[–100 mm，100 mm]× [–100 mm，100 mm]的立方體內(nèi)。設(shè)置圖像點的點位提取誤差均值為0，方差為0.4個像素。為了測試魯棒性，隨機選擇2個標(biāo)志點的圖像坐標(biāo)，加入[0,50]像素的均勻分布噪聲。根據(jù)仿真得到的羅德里格斯向量cal、平移向量cal，定義姿態(tài)的計算誤差為D=cal–，平移向量的計算誤差為D=cal–。仿真次數(shù)為15次。

為了方便顯示，繪圖時對姿態(tài)誤差和位置誤差進行了對數(shù)運算，圖2分別給出了仿真的平移向量以及旋轉(zhuǎn)矩陣的計算誤差，表1為15次仿真試驗的平移向量測量誤差結(jié)果。從圖2（a）以及表1可以看出，相比于經(jīng)典的OI方法，本文提出的魯棒正交迭代方法可以大幅度的提高平移向量的測量精度，有效克服了野值點帶來的影響；由圖2（b）以及表2可以看出，魯棒正交迭代結(jié)果優(yōu)于經(jīng)典的正交迭代結(jié)果，可以有效地減少野值點的影響，提高測量精度，具有較好的魯棒性。

在計算時間方面，由于需要額外進行權(quán)值的計算，魯棒正交迭代法的運行時間要多于經(jīng)典的正交迭代方法，魯棒正交迭代方法需要的時間為0.04s左右，能夠滿足正常的航天器位姿測量的時間需求。

圖2 經(jīng)典正交迭代方法與本文方法的位置和姿態(tài)參數(shù)測量結(jié)果比較

表1 仿真試驗中平移向量測量誤差數(shù)據(jù)

Tab.1 Translation vector measurement error data in simulation experiment

表2 仿真試驗中姿態(tài)測量誤差數(shù)據(jù)

Tab.2 Attitude measurement error data in simulation experiment

3.2 實驗驗證

為了進一步驗證魯棒正交迭代方法的有效性，進行一組實物實驗。實驗裝置由高精度控制平臺和一臺事先標(biāo)定好內(nèi)參數(shù)的相機組成，其中相機內(nèi)參數(shù)使用張正友標(biāo)定方法得到[18]。控制平臺上固定了一塊布置有若干標(biāo)志點的鋁板，世界坐標(biāo)系固連在鋁板上。標(biāo)志點在世界坐標(biāo)系中的坐標(biāo)可以通過激光跟蹤儀測量得到。由于世界坐標(biāo)系到相機坐標(biāo)系之間的旋轉(zhuǎn)矩陣和平移向量難以直接測量，因此本文借助高精度控制平臺移動世界坐標(biāo)系，通過相機對標(biāo)志點進行拍照，然后通過圖像計算出前后兩個世界坐標(biāo)系相對于相機坐標(biāo)系的位姿，進而計算出移動前后兩個世界坐標(biāo)系之間的位姿，將該位姿與高精度控制平臺的結(jié)果進行對比，以此驗證本文算法的魯棒性。

在實驗過程中，通過操作高精度控制平臺，對鋁板進行若干次平移，每次平移后，通過相機圖像提取得到標(biāo)志點的像素坐標(biāo)。為了測試本文算法的魯棒性，每次隨機選擇4個標(biāo)志點的圖像坐標(biāo)，加入[0,50]的均勻分布噪聲。將含有野值點的圖像坐標(biāo)代入本文魯棒正交迭代方法，得到世界坐標(biāo)系到相機坐標(biāo)系之間的位姿參數(shù)，進而可以換算出兩次平移之間的距離。將通過圖像測量得到的平移量和控制平臺輸入的平移量進行對比即可驗證本算法的魯棒性。

實驗數(shù)據(jù)如圖3所示，其中圖3（a）展示了實驗中平移向量的測量誤差結(jié)果；圖3（b）展示了姿態(tài)參數(shù)的測量誤差結(jié)果?？梢钥闯?，使用魯棒正交迭代方法，能夠有效克服野值點帶來的影響，正確地進行位姿參數(shù)的求解。因此，本文提出的魯棒正交迭代方法具有較好的魯棒性。

圖3 實驗誤差

4 結(jié)論

采用單目視覺系統(tǒng)對航天器位置姿態(tài)測量具有非接觸、精度高、可靠性好、實施操作簡單等特點，尤其在近距離范圍內(nèi)優(yōu)勢更為明顯。為了提高經(jīng)典的正交迭代算法的魯棒性，本文基于攝影測量理論和魯棒估計理論，提出了一種基于M估計的改進的魯棒正交迭代算法。仿真與實驗表明，相比于經(jīng)典的正交迭代方法，該方法可以有效地克服野值點帶來的影響，提高位姿解算的魯棒性和計算精度。該方法可以應(yīng)用于太空環(huán)境下的航天器位姿參數(shù)測量，也適用于地面的復(fù)雜光環(huán)境下的位姿參數(shù)測量，能夠輔助完成空間交會對接、探測器緩沖著陸運動拋投實驗等重大任務(wù)。

[1] 張慶君, 胡修林, 葉斌, 等. 基于雙目視覺的航天器間相對位置和姿態(tài)的測量方法[J]. 宇航學(xué)報, 2008, 29(1): 156-161. ZHANG Qingjun, HU Xiulin, YE Bin, et al. Binocular Vision-based Relative Position and Attitude Determination Between Spacecrafts[J]. Journal of Astronautics, 2008, 29(1): 156-161. (in Chinese)

[2] 曹喜濱, 張世杰. 航天器交會對接位姿視覺測量迭代算法[J]. 哈爾濱工業(yè)大學(xué)學(xué)報, 2005, 37(8): 1123-1126. CAO Xibin, ZHANG Shijie. An Iterative Method for Vision-based Relative Pose Parameters of RVD Spacecrafts[J]. Journal of Harbin Institute of Technology, 2005, 37(8): 1123-1126. (in Chinese)

[3] 杜小平, 趙繼廣, 崔占忠, 等. 基于計算機視覺的航天器間相對狀態(tài)測量系統(tǒng)[J]. 光學(xué)技術(shù), 2003, 29(6): 664-666. DU Xiaoping, ZHAO Jiguang, CUI Zhanzhong, et al. Optical Method for Position-attitude Determination between Spacecrafts Based on Computer Vision[J]. Optical Technique, 2003, 29(6): 664-666. (in Chinese)

[4] 王保豐. 航天器交會對接和月球車導(dǎo)航中視覺測量關(guān)鍵技術(shù)研究與應(yīng)用[D]. 鄭州: 解放軍信息工程大學(xué), 2007. WANG Baofeng. Study on the Key Technologies of Computer Vision and Applications In RVD and Lunar Rover navigation[D]. Zhengzhou: Information Engineering University, 2007. (in Chinese)

[5] 徐文福, 梁斌, 李成, 等. 基于立體視覺的航天器相對位姿測量方法與仿真研究[J]. 宇航學(xué)報, 2009, 30(4): 1421-1428. XU Wenfu, LIANG Bin, LI Cheng,et al. The Approach and Simulation Study of the Relative Pose Measurement Between Space-crafts Based on Stereo Vision[J]. Journal of Astronautics, 2009, 30(4): 1421-1428. (in Chinese)

[6] 譚啟蒙, 胡成威, 高升. 空間機械臂視覺相機內(nèi)參標(biāo)定技術(shù)研究[J]. 航天返回與遙感, 2013, 34(6):74-80.TAN Qimeng, HU Chengwei, GAO Sheng. Research on Calibration of Intrinsic Parameters for Space Manipulator Camera Based on 2D Planar Pattern[J]. Spacecraft Recovery & Remote Sensing, 2013, 34(6): 74-80. (in Chinese)

[7] 晁志超, 姜廣文, 黃偉, 等. 無人月球探測器緩沖著陸拋投試驗視覺測量系統(tǒng)[J]. 光學(xué)精密工程, 2010, 18(9): 2044-2052.CHAO Zhichao, JIANG Guangwen, HUANG Wei, et al. Vision Measuring System for Soft-landing Drop Test of Unmanned Lunar Explorer[J]. Optics and Precision Engineering, 2010, 18(9): 2044-2052. (in Chinese)

[8] 徐文福, 劉宇, 梁斌, 等. 非合作航天器的相對位姿測量[J]. 光學(xué)精密工程, 2009, 17(7): 1570-1581. XU Wenfu, LIU Yu, LIANG Bin, et al. Measurement of Relative Poses between Two Non-cooperative Spacecrafts[J]. Optics and Precision Engineering, 2009, 17(7): 1570-1581. (in Chinese)

[9] 尚洋. 基于視覺的空間目標(biāo)位置姿態(tài)測量方法研究[D]. 長沙: 國防科學(xué)技術(shù)大學(xué), 2006. SHANG Yang. Researches on Vision-based Pose Measurements for Space Targets[D]. Changsha: National University of Defense Technology, 2006. (in Chinese)

[10] LEPETIT V, MORENO-NOGUER F, FUA P. EPnP: An Accurate O(n) Solution to the PnP Problem[J]. International Journal of Computer Vision, 2009, 81(2): 155-166.

[11] HESCH J A, ROUMELIOTIS S I. A Direct Least-Squares (DLS) Method for PnP[C]//IEEE International Conference on Computer Vision. IEEE, 2012: 383-390.

[12] ZHENG Y, KUANG Y, SUGIMOTO S, et al. Revisiting the PnP Problem: A Fast, General and Optimal Solution[C]//IEEE International Conference on Computer Vision, 2013: 2344-2351.

[13] KNEIP L, LI H, SEO Y. UPnP: An Optimal O(n) Solution to the Absolute Pose Problem with Universal Applicability[C]//Computer Vision-ECCV, 2014: 127-142.

[14] LU C P, HAGERG D, MJOLSNESS E. Fast and Globally Convergent Pose Estimation from Video Images[J]. IEEE Transactions on Pattern Analysis & Machine Intelligence, 2000, 22(6): 610-622.

[15] SUSANTIY, PRATIWI H, SRI S H, et al. M Estimation, S Estimation, and MM Estimation in Robust Regression[J]. International Journal of Pure & Applied Mathematics, 2014, 91(3): 349-360.

[16] 于起峰, 尚洋. 攝像測量學(xué)原理與應(yīng)用研究[M]. 北京: 科學(xué)出版社, 2009.YU Qifeng, SHANG Yang. Videometrics: Principles and Researches[M]. Beijing: Science Press, 2009. (in Chinese)

[17] HORN BERTHOLD K P, HUGH M. Hilden and Shahriar Negahdaripour, Closed-form Solution of Absolute Orientation Using Orthonormal Matrices[J]. Journal of the Optical Society of America, 1988, 5(7): 1127-1135.

[18] ZHANG Z. A Flexible New Technique for Camera Calibration[J]. IEEE Transations on Pattern Analysis and Machine Intelligence, 2000, 22(11): 1330-1334.

A Robust Orthogonal Iteration Method for Measuring the Pose of Spacecraft

ZHANG Huan WANG Liwu TANG Mingzhang

（Beijing Institute of Space Mechanics & Electricity, Beijing 100094, China）

Pose estimation of spacecraft is a key link indispensable for many space missions and large-scale ground tests. Monoclinic pose estimation based on cooperation points is one of the important means to realize this measurement. Due to the interference from complex environment, the sign point extraction may have outliers or inconsistencies in accuracy. The traditional orthogonal iteration method can’t achieve optimal solution accuracy and even obtain false solution for such problems. In this paper, a robust orthogonal iterative method based on M estimation is proposed, in which M markers are used to assign different weights to each marker point, and the residual objective function of weighted object is optimized to get the pose parameters. The results of simulation and experiment show that this method can eliminate the misdata influence on the measurement and improve the measurement accuracy and robustness of orthogonal iterative algorithm.

pose estimation; orthogonal iteration; weighted estimation; robustness; spacecrafts

O436；V19

A

1009-8518(2018)05-0066-08

10.3969/j.issn.1009-8518.2018.05.009

張歡，男，1987年生，2012年獲南京理工大學(xué)軍事化學(xué)與煙火技術(shù)專業(yè)碩士學(xué)位，工程師。目前主要從事航天器系統(tǒng)試驗光學(xué)測量方面的工作。E-mail：491933842@qq.com。

2018-01-29

（編輯：夏淑密）