基于單目三維重構的空間非合作目標相對測量

李永飛，王仕成，楊東方，孫大為

火箭軍工程大學 控制工程系，西安 710025

?

基于單目三維重構的空間非合作目標相對測量

李永飛，王仕成，楊東方*，孫大為

火箭軍工程大學 控制工程系，西安 710025

為了解決非合作目標的相對測量問題，提出了一種基于單目圖像序列目標重構結果的非合作目標相對位姿測量方法。該方法將目標的三維重構與相機的位姿信息計算相結合，首先利用觀測前期得到的圖像序列，通過非線性優化算法計算得到目標上部分三維點坐標；然后基于該三維點集合，建立遞推深度模型，對相機的相對位姿信息和新觀測到的目標點同時進行卡爾曼濾波估計。航拍測量試驗表明，隨著圖片數量的增多，精確重構點的比例(重投影誤差小于1個像素的點)不斷提高，80%的圖像中精確重構點比例優于89%；基于公共數據集的試驗表明，該算法對姿態估算精度可達1°以內，位置測量的精度可達到2 cm以內。以上試驗結果表明，該算法具有較高的測量精度。

目標重構；相對位姿測量；單目視覺；非合作目標

空間測量是空間遙操作、空間交互對接等航空航天領域的一項關鍵技術[1]。早期的空間測量傳感器主要包括激光、紅外等距離傳感器，這類傳感器存在價格高，功耗高、體積龐大等缺陷，將其應用于一些成本低、體積小的空間機器人或者小型對地測量平臺等領域中受到很大的限制[2]。近年來，隨著計算機視覺技術的發展，其在成本、可視化等方面存在的優勢受到越來越多學者的關注，僅僅依靠小型視覺傳感器實現空間非合作目標相對測量成為近年來的研究熱點[3]。

根據傳感器的不同，視覺測量可以分為多目視覺測量系統、雙目視覺測量系統和單目視覺測量系統。其中，多目視覺測量系統和雙目視覺測量系統結構復雜，需要標定相機間的相對位姿；由于標定工藝復雜，成本較高，而且標定過程中引入的誤差難以消除[4]。因此本文主要針對單目視覺系統進行研究。

單目位姿估計算法的研究主要包括兩類：線性約束法和非線性優化法。線性約束法給定相鄰時刻針對同一三維場景的兩幅圖像，通過特征點的提取和匹配，利用攝像機對極幾何約束關系，能夠在相差一個非零因子的意義下求解運動參數(R,t)。早期相關論文研究很多，從最早的8點算法，7點算法，到2004年Nister提出的5點算法[5]，人們通過對極約束模型進行分析，不斷降低了位姿計算的復雜度。然而，線性約束法對于圖像中的噪聲數據非常敏感，往往不能得到理想的位姿估計結果。因而，此后對攝像機位姿估計方法的研究主要為非線性遞推算法[6-7]及光束平差法[8-10]。雖然，這類非線性優化方法能夠在給定良好初始值的情況計算得到較精確的結果；但是，這類非線性優化方法存在明顯的不足，針對非凸問題目前仍然沒有有效的優化算法能夠保證其最優解的獲取，這樣得到的位姿估計結果往往容易陷入局部最優[11]；并且往往由于觀測點較多，使得需要優化的變量的維數很高，計算變得復雜，無法滿足相對測量實時性的要求。

為此，本文提出了一種實時高精度的非合作目標測量方法，該方法先采用非線性優化的方法，重構出較精確的非合作目標三維點集，隨后以此為初始三維點集，采用濾波的方法，實現目標相對位姿測量和增量式的三維重構，充分利用對目標觀測的全部信息，提高了相對測量的魯棒性和精度。

1　問題描述

1.1射影相機模型

目前常用的相機模型是針孔相機模型，它可以被定義為一個將世界坐標系的三維點X，映射到圖像坐標系像素點的一個射影變換，如圖1所示，可以用矩陣P來表達該射影變換。對于任一世界坐標系中的點X，其世界坐標系中的坐標和圖像坐標系中的坐標的轉換關系為

(1)

攝像機模型確定了世界坐標系中特征點及其成像坐標之間的關系，這也是進行視覺位置和姿態測量的基本依據。傳統的位姿估計算法需要已知特征點的三維坐標，即式(1)中方程最右邊的特征點向量。然而，在本文所研究解決的問題中，空間目標是非合作目標，因此，其真實世界坐標也是未知的，無法直接計算相對位姿參數。為此，本文提出：能不能利用圖像序列，完成非合作目標的重構后，再進行相應的單目視覺位姿計算，完成單目視覺相對測量呢？這也就是本文所提方案的初衷。

1.2非合作目標相對測量

在合作目標的空間相對測量過程中，利用事先設定的參考標志信息，結合前述攝像機成像幾何模型，計算攝像機和目標之間的相對位姿關系。然而，對于非合作目標而言，由于目標物體上沒有已知的標志物信息，甚至連基本尺寸信息都是缺失的，如圖2所示。

圖2　合作目標和非合作目標測量任務對比示意Fig.2　Comparison between relative measurement for cooperation target and that for non-cooperation target

為了完成這類非合作目標的相對測量，當前的研究往往都是基于目標某些先驗信息已知的假設，例如文獻[12-13]。為了適應于更加一般且真實的應用場景，本文針對被測目標是完全未知的情形進行研究。

2　增量式非合作目標重構測量算法

本文提出的非合作目標測量算法分為兩個階段。第一階段，對觀測的圖像進行捆綁約束的非線性優化，得到目標的三維點坐標集；第二階段，以第一階段得到的三維點集為初始三維點集，采用濾波的方法，增量式優化求解相對位姿關系和新觀測的三維點坐標。

2.1基于圖像序列的目標三維重構

對于單目視覺系統而言，為了根據所拍攝的圖像序列完成對目標的三維重構，需要利用不同時刻所采集的圖像，重構出目標的空間坐標，利用時間域的擴展實現空間域信息的重構。在測量任務第一階段，利用對目標的多位置、多角度觀測得到的圖像可以重構出目標的三維點云坐標。該問題可以描述為：已知相機在m個不同位置k(k=1,2,…,m)對n個點Xi(i=1,2,…,n)進行觀測，觀測結果由(uki,vki)表示，求世界點Xi和各個位置相機的位姿參數(Rk,tk)。

為解決上述問題，本文采用Gerald Schweighofer等于2008年提出的一種具有全局收斂性的相機運動參數的估計方法[14]。該方法基于通用相機模型[15]，對于單目針孔相機系統，通用相機模型可等價表達為

(2)

式中：(c,v)為由通用相機模型表達的測量結果；K為相機的內參數矩陣；R、t分別為相機相對于世界坐標系的旋轉矩陣和平移向量；p為圖像的像素坐標。

基于以上轉換關系，選取目標空間重構誤差作為代價函數，該方法可以表達為求代價函數E(R,t,X)的最小值的問題，即：

(3)

該方法首先求解出由位姿參數R,t表達的世界點坐標最優解X=X(R,t)；其次，求解出由旋轉矩陣R表達的平移向量最優解t=t(R)；最后，將代價函數表達成旋轉矩陣R的函數并給出求最優解的遞推算法R(λ+1)=R(R(λ),t(λ),X(λ))。具體計算過程如下：

(1)世界點的三維坐標最優解

(2)平移向量最優解

At,bt分別為m個具有如下

形式的等式聯立得到的方程組的系數矩陣和常數項矩陣。

(3)旋轉矩陣的求解

在最優解(1)、(2)結果的基礎上，該問題可表達為

(4)

其中，

對該問題進行遞推求解，記第λ次的迭代結果為

(5)

(6)

由以上步驟，可以求出三維世界點云坐標，得到較精確的目標三維點集。

需要指出的是，上述得到的三維點坐標，是相對于固定世界坐標系的相對位置坐標。在本文中，該世界坐標系便是初始時刻的攝像機光心坐標系。

2.2增量式重構測量算法

前述過程完成了目標三維點云模型的重構，雖然上述方法經證明能夠取得較為準確的重構精度，然而，需要指出的是，上述方法中，只能對圖像集合進行捆綁優化處理，不能實現圖像序列的序貫處理，這對于常用的目標相對測量任務而言是不能滿足需求的。為此，本節以該初始化模型為基礎，提出了一種增量式的重構測量算法。該算法首先根據初始化重構得到的三維點集，利用攝像機成像幾何關系，得到相機相對目標的位置和姿態參數信息；然后，利用該位姿參數信息，對每一幀新采集的圖像進行特征點重構，隨后，再利用該重構優化后的點云結果，反過來輔助下一個時刻攝像機位姿參數的計算。從上述過程可以看出，該方法實際上是一個圖像序列序貫處理的過程，隨著圖像序列的增加，重構精度經過不斷優化越來越高，而攝像機相對目標的位姿參數的精度也隨之逐步提高，因此，本文將該方法稱為“增量式重構測量算法”。

如前所述，在得到了目標三維點云初始化重構結果后，可以實現相機相對目標位姿的實時測量。對于每一幀新采集的圖像，由于其在觀測初期階段受重構精度的影響，其空間三維坐標會存在誤差，該誤差反過來又會引起攝像機相對位姿參數的計算出現誤差。為此，通常將場景中的特征點三維坐標和攝像機位姿參數同時構建狀態空間方程，利用圖像序貫觀測量進行濾波計算，同時對特征點坐標和位姿參數進行遞推計算。本文沿用作者于2014年提出的遞推深度參數模型[16]，并在此基礎上，根據本文所涉及的目標三維點更新迭代過程應用需求，將該模型進行適應性改進如下：

(1)增量式重構測量算法中的狀態更新方程

(7)

(8)

需要指出的是，在持續的觀測過程中，無法保證能對特定點實現連續不間斷的觀測，因此針對狀態量Zi(k)的選取，不能采用一成不變的目標特征點，而需要進行實時更新。本測量方案提出一種堆棧式迭代更新的處理方式，該方法利用特征點的反復觀測，不斷提高其深度特征的估計精度，此時可以認為，觀測壽命越長的特征點，其估計精度也越高。因此，在狀態方程中，將涉及的N個特征點按照觀測壽命長短進行排序，當壽命最長的特征點的估計精度滿足一定要求后，便可以用新觀測到的新特征點去替換這些目標特征點，增加到系統狀態方程之中，組成新的N個特征狀態。被替換的目標特征點由于其觀測壽命較長，其精度也已經滿足要求，可以將其作為固化的三維世界坐標。而新加入的特征點，隨著觀測圖像的增加，其觀測壽命也不斷提升，其深度估計精度也隨之不斷提高，當其達到一定的觀測壽命后，也被新加入的特征點替換，成為固化的三維重構結果。

如圖3所示，設定狀態量中目標特征點數為N，即僅僅對N個特征點進行狀態估計濾波處理；選取在前一階段圖像序列捆綁優化得到的N個三維特征點，作為狀態量中的Zi(k)(i=1,2，…,N)；當壽命最長的m個觀測點(即圖3中最上面的m個特征點)的精度滿足要求，將其作為固化的三維點加入三維模型，不再作為狀態向量的元素，同時將新觀測到的m個特征點，添加進入狀態向量。m的確定原則為，使得每個新添入的三維點的觀測次數不小于10次，因此有N/m≥10。

圖3　狀態向量更新示意Fig.3　Update of state vector

(2)增量式重構測量算法中的觀測方程

(9)

基于上述模型的相對位姿測量和新三維點的添加算法可以總結為以下步驟：

1)根據已恢復的三維點坐標，分別計算出相機的旋轉矩陣RCkW和平移向量rWCW(k)，并根據上一時刻計算得到的旋轉矩陣RCk-1W和平移向量rWCW(k-1)，并由此計算觀測量y；

2)更新狀態方程；

3)離散擴展卡爾曼濾波(EKF)；

4)相機位姿信息更新及新三維點世界坐標計算：

Pos(k)=Pos(k-1)+RCkWkVC(k)Δt

Pos(k)表示k時刻相機在世界坐標系中的位置。

3　試驗驗證

3.1空間航拍重構測量試驗

本試驗利用四旋翼無人飛行器搭載的攝像機對固定未知目標進行拍攝，并對所得到的圖像進行重構和位姿測量試驗。拍攝平臺和試驗場景如圖4所示。

圖4　試驗場景示意Fig.4　Experiment scene

(1)試驗描述

試驗中，操作四旋翼無人飛行器，模擬飛行器從不同角度對固定目標進行拍攝的過程，以30幀/s的幀頻進行拍攝，得到一組圖片序列(視頻)；基于該圖片序列，進行了如下試驗。

首先利用采集到的視頻的前10s，每10幀選取一幀，選取出30幀圖像，采用第2.1節的方法，重構出一系列目標的三維點坐標，這里得到了357個三維點，如圖5所示。

圖5　初始重構結果Fig.5　Initial reconstruction result

圖5為初始重構的結果，由于僅僅通過單目視覺傳感器，無法得到非合作目標的絕對尺度信息，因此圖中的坐標無量綱。

隨后，在上述三維點的基礎上，采用第2.2節的方法，實現對相對位姿信息和新的特征點的同時優化估計，圖6顯示了幾個時刻相機的位姿。試驗數據的處理硬件平臺為一臺配置i7八核處理器的PC機，整個過程使用了2 160幀(72s視頻)圖像，總處理時間為223.56s，平均處理速度約為10幀/s。

圖6　相對測量結果Fig.6　Results of relative measurement

圖6顯示了測量過程中不同時刻相機的位姿，并標示出了相機光心的運動軌跡和相機的位姿，黑色點云標示了重構得到的一系列三維點。

(2)試驗結果分析

由于無法得到相機在空間的位姿信息的真值，無法直接驗證結果的正確性。為此，對重構測量的結果進行了重投影誤差的分析。

1)單幅圖像重投影誤差分析。

在初始重構測量階段使用的圖片中，選取了第1幀、第15幀、第30幀3幅圖像，進行重投影誤差的統計分析。在此，選取精確重構點比例(在本文中，將重投影誤差小于一個像素的點稱為精確重構點)這一指標作為測量精度的衡量指標。對3幅圖像的精確重構點比例隨時間的變化進行了統計，結果如圖7所示。

圖7　單幅圖像精確重構點比例變化Fig.7　Changing trend of the percentage of preciously reconstructed points

圖7中，直線段是采用第2.1節的非線性約束的方法得到的試驗結果，由于該過程不是一個增量式逐漸遞推的過程，因此在此作為整體處理。其后是通過濾波處理得到的結果，由于在濾波初期，新觀測的點的深度具有較大的不確定度，因此，精確重構點的比例有所下降；隨著濾波器的收斂，精確重構點的比例逐漸提高；當一幅圖中，所有特征點都成為固定的三維點，其精確重構點比例不再變化。可以看出，隨著時間的推移，同一世界點被觀測次數的增多，同一圖像的精確重構點的比例逐漸增加。由于對于單幅圖像，其對應的相機的位姿參數是確定的，重投影誤差的大小僅取決于三維點坐標，精確重構點比例的增加表明重構得到的三維點坐標精度的提高。

2)重構測量最終結果重投影誤差分析。

對測量完成后，得到的三維點坐標和相機的位姿進行全局的重投影誤差分析，統計了測量過程中所使用的每幀圖像的精確重構點的比例，結果如圖8所示。

在全部使用的2 190幅圖像中，精確重構點比例大于89%的圖像有1 754幅，占所有圖片的80%，表明該方法能夠得到較為精確的重構測量結果。

圖8　精確重構點比例分布Fig.8　A diagram of the percentage of precious reconstructed points

3.2公共數據集試驗

第3.1節以空間小型測量平臺為例，對所提方法的有效性進行了試驗驗證。然而，由于試驗條件的限制，上述試驗不能直接對單目視覺的測量精度進行評價。為此，本節借助于慕尼黑工大提供的單目SLAM公共數據集[17]，對算法的精度指標進行驗證。重構測量結果如圖9所示。

圖9　公共數據集重構測量結果Fig.9　Results of reconstruction based on RGB-D SLAM dataset

圖10對試驗結果進行了描述。試驗數據集采集過程中，攝像機的旋轉和平移參數參考真值由慣性測量系統得到，由于試驗時間較短，因此慣性測量結果可以用來作為參考。從試驗結果可以看出，用本文所提出的方法，能夠由單目攝像機采集的圖像序列得到關于攝像機運動的姿態和位置參數。從圖10中可以看出，姿態估計精度可以達到1°以內，而位置估計精度可以達到2cm以內。

圖10　位姿測量結果Fig.10　Results of relative measure

5　結束語

針對非合作目標的相對位姿測量問題，提出了一種基于單目三維重構的實時測量方法。試驗表明，該算法能夠實現以10幀/s對非合作目標進行實時測量，姿態測量精度優于1°，能夠滿足針對非合作目標的高精度實時測量任務要求。由于僅僅通過單目視覺測量算法，無法得到真實世界的絕對尺度，因此，通過引入其他傳感器信息，實現對非合作目標的具有絕對尺度的相對位姿測量，是下一步的研究方向。

References)

[1]崔乃剛，王平，郭繼峰，等．空間在軌服務技術發展綜述[J]．宇航學報，2007，28(4)：805-811．

CUI N G，WANG P，GUO J F，et al. A review of on-orbit servicing[J]. Journal of Astronautics，2007，28(4)： 805-811(in Chinese)．

[2]劉勇，徐鵬，徐世杰．航天器自主交會對接的視覺相對導航方法[J]．中國空間科學技術，2013，33(6)：33-40.

LIU Y， XU P， XU S J. Vision-based relative navigation for rendezvous and docking of spacecraft[J]. Chinese Space Science and Technology， 2013，12(6)：33-40(in Chinese).

[3]ZHOU J Y，ZHOU J P，JIANG Z C，et al．Investigation into pilot handling qualities in teleoperation rendezvous and docking with time delay[J]．Chinese Journal of Aeronautics，2012，25(4)：622-630．

[4]余慧杰，韓平疇. 雙目視覺系統的測量誤差分析[J].光學技術，2007，33(增刊)：157-159.

YU H J，HAN P C. Measuring errors analysis of binocular vision system[J]. Optical Technique， 2007，33(Suppl.)：157-159(in Chinese).

[5]NISTER D. An efficient solution to the five-point relative pose problem[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence，2004，26(6)：756-777.

[6]KLEIN G，MURRAY D. Improving the agility of keyframe-based SLAM[C]∥Proceeding of the 10th European Conference on Computer Vision，Marseille，France，October 12-18，2008.

[7]MOURAGNON E，LHUILLIER M，DHOME M. Generic and real-time structure from motion using local bundle adjustment[J]. Image and Vision Computing，2009，27(8)： 1178-1193.

[8]ENGELS C，STEWENIUS H. Bundle adjustment rules[C]∥Symposium of ISPRS Commission III Photogrammetric Computer Vision PCV， Bonn， Germany， September 20-22， 2006.

[9]TARDIF J，PAVLIDIS Y. Monocular visual odometry in urban environments using an omnidirectional camera[C]∥International Conference on Intelligent Robots and Systems， Nice， France， September 22-26，2008.

[10]楊東方，孫富春. 單目視覺系統自運動的滾動時域估計方法[J]. 模式識別與人工智能，2013，26(1)：76-82.

YANG D F，SUN F C. Moving horizon estimation of ego-motion in monocular visual systems[J]. Pattern Recognition and Artificial Intelligence，2013，26(1)： 76-82(in Chinese).

[11]FRAUNDORFER F，SCARAMUZZA D. A constricted bundle adjustment parameterization for relative scale estimation in visual odometry[C]∥International Conference on Robotics and Automation Anchorage Convention District，Anchorage，Alaska，USA，May 3-8，2010.

[12]王曉亮，邵曉巍，龔德仁. 空間非合作目標交會對接影像測量自適應處理[J]. 中國空間科學技術，2011，31(3)： 76-83.

WANG X L， SHAO X W， GONG D R. An adaptive approach to vision measurement for rendezvous and docking with non-cooperative target in space[J]. Chinese Space Science and Technology，2011，31(3)：76-83(in Chinese).

[13]徐文福，梁斌.基于立體視覺的航天器相對位姿測量方法與仿真研究[J]. 宇航學報，2009，30(4)：1421-1428.

XU W F，LIANG B. Stereo vision-based relative pose measurement method for spacecraft[J]. Journal of Astronautics，2009，30(4)：1421-1428(in Chinese).

[14]GERALD SCHWEIGHOFER，AXEL PINZ. Fast and globally convergent structure and motion estimation for general camera models[C]∥Proceedings of British Machine Vision Conference，Edinburgh，UK，September 4-7，2006.

[15]GROSSBERG M D，NAYAR S K. A general imaging model and a method for finding its parameters[C]∥ICCV，Vancouver，Canada，July 9-12，2001.

[16]YANG D F，LIU Z G. Recursive depth parametrization of monocular visual navigation：observability analysis and performance evaluation[J]. Information Sciences，2014，287：38-49.

[17]STURM J，ENGELHARD N，ENDRES F，et al. A benchmark for the evaluation of RGB-D SLAM systems[C]∥International Conference on Intelligent Robot Systems (IROS)，Vilamoura，Portugal，October 7-12，2012.

(編輯：車曉玲)

Aerial relative measurement based on monocular reconstruction for non-cooperation target

LI Yongfei，WANG Shicheng，YANG Dongfang*， SUN Dawei

Department of Control Engineering，Rocket Force University of Engineering，Xi′an 710025，China

A monocular vision-based relative position and attitude measurement algorithm was presented， which combined the reconstruction of targets with the measurement of relative positions and attitude for non-cooperation targets. Firstly， a series of points on the targets were reconstructed using the images obtained in the earlier measurement with a fast and globally convergent structure and motion estimation.Secondly， the relative position and attitude were calculated by using a Kalman filter based on recursive depth parametrization， in which new points observed from new images were recovered and added to the 3D point set to improve the accuracy and robustness of the measurement. Experiment result shows that the attitude measurement can achieve an accuracy higher than 1°,and the position measurement can achieve an accuracy higher than 2 cm， which indicates the high precision of the proposed algorithm.

target reconstruction；measurement for relative position and attitude；monocular vision；non-cooperation target

10.16708/j.cnki.1000-758X.2016.0057

2015-12-25；

2016-04-28；錄用日期：2016-08-22；

時間：2016-09-2113:41:25

http:∥www.cnki.net/kcms/detail/11.1859.V.20160921.1341.006.html

國家自然科學基金(61403398)；總裝預研基金(9140A01010413JB47005)

李永飛(1993-)，男，碩士研究生，lyfei314@163.com,研究方向為圖像制導

楊東方(1985-)，男，博士，講師，yangdf301@163.com，研究方向為圖像制導

V448.2

A

http:∥zgkj.cast.cn

引用格式：李永飛，王仕成，楊東方，等. 基于單目三維重構的空間非合作目標相對測量[J].中國空間科學技術， 2016，36(5)：

48-56.LIYF，WANGSC，YANGDF，etal.Aerialrelativemeasurementbasedonmonocularreconstructionfornon-cooperationtarget[J].ChineseSpaceScienceandTechnology， 2016，36(5)：48-56(inChinese).