王 鶴，樊世超，王婉秋，趙振慶

（北京衛(wèi)星環(huán)境工程研究所，北京 100094）

0 引言

地面行走試驗(yàn)是月面巡視器研制過程的重要環(huán)節(jié)，必須在地面對(duì)巡視器移動(dòng)性能、路徑規(guī)劃及自主導(dǎo)航等功能進(jìn)行綜合測(cè)試驗(yàn)證，以考察巡視器行走能力和工作的可靠性[1]。為了巡視器地面行走驗(yàn)證試驗(yàn)的需要，專門研制了月球低重力環(huán)境模擬設(shè)備。該設(shè)備由恒拉力子系統(tǒng)、位姿測(cè)控子系統(tǒng)、支撐子系統(tǒng)等組成[2]，其中：恒拉力子系統(tǒng)以懸吊方式與支撐子系統(tǒng)共同作用，給巡視器提供低重力環(huán)境；位姿測(cè)控子系統(tǒng)包括位姿測(cè)量和二維跟蹤兩個(gè)部分，首先通過位姿測(cè)量獲取巡視器在水平面的二維位置信息，運(yùn)算后可得到吊點(diǎn)在水平面投影的二維位置信息即跟蹤偏差，然后將這一偏差量作為控制器的輸入，控制器根據(jù)偏差輸出控制指令，驅(qū)動(dòng)二維跟蹤平臺(tái)運(yùn)動(dòng)到目標(biāo)位置，再次測(cè)量跟蹤偏 差，從而形成控制閉環(huán)，實(shí)現(xiàn)對(duì)運(yùn)動(dòng)物體（巡視器）的目標(biāo)識(shí)別和位姿測(cè)量。

國內(nèi)外對(duì)運(yùn)動(dòng)物體的目標(biāo)識(shí)別和位姿測(cè)量研究很多，主要是通過計(jì)算機(jī)視覺檢測(cè)技術(shù)完成對(duì)動(dòng)態(tài)或靜態(tài)物體的目標(biāo)識(shí)別。根據(jù)視覺傳感器（一般為CCD 相機(jī)）的數(shù)量可分為單目視覺和多目視覺的測(cè)量方法，其基本工作流程都是將視覺傳感器獲得的測(cè)量圖像經(jīng)圖像采集卡傳送到計(jì)算機(jī)，再使用軟件進(jìn)行圖像處理后獲得感興趣的測(cè)量參數(shù)[3]。

單目視覺測(cè)量方法較為廣泛地應(yīng)用于隨動(dòng)機(jī)器人、航空航天以及軍工等領(lǐng)域的目標(biāo)跟蹤，該方法僅需1 臺(tái)相機(jī)，結(jié)構(gòu)簡(jiǎn)單，相機(jī)內(nèi)參數(shù)標(biāo)定簡(jiǎn)單，同時(shí)避免了多個(gè)視覺傳感器帶來的視場(chǎng)小、立體匹配難的問題。哈爾濱工業(yè)大學(xué)對(duì)基于單目視覺的目標(biāo)位姿測(cè)量算法進(jìn)行了理論和實(shí)際應(yīng)用研究：利用投影儀、CCD 攝像機(jī)、圖像采集卡及PC 機(jī)組成了一個(gè)半物理仿真實(shí)驗(yàn)系統(tǒng)，著重研究了對(duì)目標(biāo)的位置及姿態(tài)測(cè)量；針對(duì)人為生成的目標(biāo)及特征光標(biāo)，利用坐標(biāo)變換和幾何關(guān)系推導(dǎo)了一種用于測(cè)量目標(biāo)三維位置和姿態(tài)的簡(jiǎn)便解析算法；在均勻光照和一般室內(nèi)背景條件下，完成了模擬空間交會(huì)對(duì)接時(shí)的目標(biāo)飛行器位置姿態(tài)測(cè)量試驗(yàn)[4-6]。

在綜合考慮系統(tǒng)需求和可靠性的基礎(chǔ)上，本文提出了一種基于雙相機(jī)的位姿測(cè)控系統(tǒng)，利用LED紅外光源作為特征點(diǎn)即靶標(biāo)，二維跟蹤平臺(tái)上安裝的2 個(gè)CCD 相機(jī)作為視覺圖像捕捉設(shè)備，通過光學(xué)成像原理解算巡視器位姿信息，并完成跟蹤控制。

1 非接觸式視覺測(cè)量原理

立體靶標(biāo)上分布著5 個(gè)空間位置坐標(biāo)已知的紅外發(fā)光二極管，即5 個(gè)特征點(diǎn)。測(cè)量過程中，系統(tǒng)通過相機(jī)獲取場(chǎng)景的灰度圖像；在從圖像中提取靶標(biāo)位姿信息的過程中，對(duì)灰度圖像進(jìn)行二值化處理。所謂二值化處理是將圖像分割成背景和目標(biāo)兩部分，其灰度值分別用二進(jìn)制數(shù)0 與1 來表示。對(duì)圖像進(jìn)行二值化處理可以有效減少數(shù)據(jù)量、部分消除背景噪聲，提取目標(biāo)幾何特征。根據(jù)這5 個(gè)特征點(diǎn)的成像分析以及各特征點(diǎn)之間的幾何約束，利用姿態(tài)估計(jì)算法確定這些特征點(diǎn)相對(duì)于相機(jī)坐標(biāo)系的坐標(biāo)。

假設(shè)世界坐標(biāo)系是絕對(duì)坐標(biāo)系，定義為相機(jī)坐標(biāo)系，且空間特征點(diǎn)Pi投影在u-v像平面上的像點(diǎn)ai用相機(jī)坐標(biāo)系下的坐標(biāo)表示。光學(xué)靶標(biāo)測(cè)量原理如圖1所示，可以看出，qi為從相機(jī)坐標(biāo)系原點(diǎn)出發(fā)指向像點(diǎn)ai的單位向量，空間特征點(diǎn)Pi處于相同的方向。因此，假如已知比例因子xi，則可以通過像點(diǎn)ai求得Pi的坐標(biāo)位置，即：

用像點(diǎn)ai代替Pi且計(jì)算空間特征點(diǎn)之間的距離，并利用點(diǎn)積和qi·qi=1 得到二次方程形式

式(2)可改寫為

對(duì)應(yīng)著空間的n個(gè)特征點(diǎn)，可以得到關(guān)于n個(gè) 未知數(shù)的n(n-1)/2 維二次方程組，其中為由靶標(biāo)上特征點(diǎn)分布所決定的已知量，且qi·qj可以由像點(diǎn)坐標(biāo)ai和aj以及相機(jī)焦距確定。因此可以通過非線性優(yōu)化算法對(duì)該方程組進(jìn)行求解得到未知的比例系數(shù)xi，并進(jìn)而通過式(1)確定點(diǎn)Pi的空間坐標(biāo)[7]。

圖1 光學(xué)靶標(biāo)測(cè)量原理Fig.1 Measurement principle of the optical drone

得到特征點(diǎn)在空間坐標(biāo)系的位置和在相機(jī)像平面中的像點(diǎn)坐標(biāo)ai后，可以根據(jù)式(3)得到一個(gè)超定方程。如何根據(jù)這個(gè)超定方程解出所需的位置參數(shù)，這是典型的投影n點(diǎn)姿態(tài)估計(jì)問題（PnP問題）。對(duì)于空間3 個(gè)特征點(diǎn)的情況，理論上式(3)對(duì)于[x1,x2,x3]存在8 種可能的解；考慮到特征點(diǎn)實(shí)際上不可能處于相機(jī)的另一側(cè)，因此對(duì)于實(shí)際的姿態(tài)估計(jì)至多存在4 個(gè)解。

假設(shè)靶標(biāo)上有5個(gè)特征點(diǎn)，對(duì)系統(tǒng)進(jìn)行求解，利用式(3)，可以得到10個(gè)方程：

通過抵消可以得到的6個(gè)四次多項(xiàng)式，將其排列在矩陣方程中有

對(duì)該超定方程進(jìn)行奇異值分解可得到向量t5，并求得x值。由于xi不可能為負(fù)，根據(jù)x=x12可以得到x1=，并可將此結(jié)果代入方程組(5)求得對(duì)應(yīng)的x2～x5。

最后可以利用式(1)求得光學(xué)靶標(biāo)上特征點(diǎn)在相機(jī)坐標(biāo)系的坐標(biāo)，當(dāng)設(shè)置巡視器的質(zhì)心為靶標(biāo)空間坐標(biāo)系的原點(diǎn)時(shí)，就可以得到巡視器質(zhì)心在相機(jī)坐標(biāo)系的坐標(biāo)，再根據(jù)相機(jī)和吊點(diǎn)的位置就可得到巡視器質(zhì)心相對(duì)于吊點(diǎn)的偏移。

2 基于雙相機(jī)的位姿測(cè)控系統(tǒng)

2.1 硬件結(jié)構(gòu)優(yōu)化設(shè)計(jì)

位姿測(cè)控系統(tǒng)（見圖2）由兩個(gè)經(jīng)過精確標(biāo)定的高分辨率CCD 相機(jī)、立體靶標(biāo)、交換機(jī)、控制一體機(jī)、驅(qū)動(dòng)電機(jī)和跟蹤平臺(tái)等組成。由于月球車停止位置和行進(jìn)路徑不同，可能造成相機(jī)的測(cè)量光路被恒拉力子系統(tǒng)的吊索遮擋，因此，將2 個(gè)相機(jī)分別安裝在恒張力索兩側(cè)，這樣既可以覆蓋整個(gè)視場(chǎng)且不會(huì)同時(shí)被恒張力索遮擋[8]，又解決了月球車單向運(yùn)動(dòng)時(shí)的信息丟失問題。系統(tǒng)啟動(dòng)后，2 個(gè)相機(jī)同時(shí)工作，測(cè)量結(jié)果通過網(wǎng)絡(luò)交換機(jī)傳輸至控制一體機(jī)；用戶根據(jù)工況選擇相機(jī)，對(duì)所選擇相機(jī)的測(cè)量結(jié)果進(jìn)行處理并將處理后的偏差量輸入至電機(jī)控制卡，由電機(jī)驅(qū)動(dòng)器驅(qū)動(dòng)電機(jī)旋轉(zhuǎn)，使平臺(tái)運(yùn)動(dòng)。每次切換，保證2 個(gè)相機(jī)測(cè)量的目標(biāo)位置信息在允許的偏差范圍內(nèi)?？刂埔惑w機(jī)定時(shí)將獲取的位置信息和跟蹤偏差傳回地面綜合控制系統(tǒng)顯示并存儲(chǔ)。

圖2 位姿測(cè)控系統(tǒng)示意圖Fig.2 The schematic diagram of the position-pose measurement and control system

單個(gè)相機(jī)測(cè)量如圖3所示。為提高系統(tǒng)的可靠性，采用封裝的CCD 模塊和成熟的信號(hào)處理電路，通過視頻采集卡及DSP 進(jìn)行數(shù)據(jù)采集和處理；采用帶通數(shù)字濾波的方式對(duì)采集到的信號(hào)的特定頻段進(jìn)行提取，進(jìn)一步降低外界雜光及多個(gè)LED之間的干擾。采用大功率的紅外發(fā)光二極管作為特征點(diǎn)，同時(shí)在鏡頭前面加上一個(gè)紅外濾光片，這樣可以有效過濾非紅外波段，提高視覺測(cè)量信噪比。

圖3 單個(gè)相機(jī)的測(cè)量示意圖Fig.3 Measurement of single camera

2.2 軟件設(shè)計(jì)

系統(tǒng)軟件包括網(wǎng)絡(luò)通信模塊、控制模塊、磁尺信息采集模塊、相機(jī)測(cè)量模塊、電機(jī)控制模塊、雙相機(jī)切換模塊、相機(jī)圖像監(jiān)測(cè)模塊和LED 光源監(jiān)測(cè)模塊，主要實(shí)現(xiàn)以下任務(wù)：

1）根據(jù)切換指令選擇相機(jī)；

2）根據(jù)控制指令按照控制策略將指令分配給二維平臺(tái)；

3）接收位姿測(cè)定子系統(tǒng)的偏差信息；

4）采集碼盤、磁尺等敏感器的信息；

5）將二維跟蹤系統(tǒng)的狀態(tài)信息及敏感器信息以固定周期發(fā)送給綜合控制系統(tǒng)。

軟件的工作過程可劃分為初始化、工作和關(guān)閉三部分（見圖4）。

圖4 位姿測(cè)控系統(tǒng)軟件工作流程Fig.4 Flowchart of the software of the position-pose measurement and control system

3 試驗(yàn)驗(yàn)證

基于雙相機(jī)的位姿測(cè)控系統(tǒng)優(yōu)化完成后，利用經(jīng)緯儀測(cè)試雙相機(jī)安裝后的數(shù)據(jù)一致性，如表1所示，可以看出，巡視器行進(jìn)至不同位置，2 個(gè)相機(jī)切換時(shí)x向、y向（見圖5）的跟蹤偏差在8 mm 以內(nèi)，滿足巡視器二維隨動(dòng)跟蹤的誤差要求。

圖5 巡視器移動(dòng)示意圖Fig.5 Rover movement

表1 雙相機(jī)數(shù)據(jù)一致性記錄Table 1 Data consistency of the two cameras

在巡視器發(fā)射前的行走試驗(yàn)中，應(yīng)用該系統(tǒng)對(duì) 巡視器在模擬地形行走中進(jìn)行了位姿測(cè)定和跟蹤，應(yīng)用效果良好。x、y向跟蹤誤差如圖6所示。

圖6 吊點(diǎn)x 向和y 向跟蹤誤差曲線Fig.6 Tracking error curves of the hoisting point in x and y directions

4 結(jié)束語

本文對(duì)月球低重力模擬試驗(yàn)的位姿測(cè)量方法進(jìn)行了研究，提出了基于雙相機(jī)的非接觸測(cè)量方法，對(duì)硬件結(jié)構(gòu)和軟件進(jìn)行了優(yōu)化設(shè)計(jì)和分析。利用該位姿測(cè)控系統(tǒng)進(jìn)行了巡視器行走的驗(yàn)證試驗(yàn)測(cè)試，測(cè)試結(jié)果表明該系統(tǒng)很好地解決了由于遮擋引發(fā)的位姿跟蹤失效問題，滿足巡視器行走試驗(yàn)要求，可以在后續(xù)型號(hào)試驗(yàn)中投入使用。

（References）

[1]樊世超, 賈陽, 向樹紅, 等.月面地形地貌環(huán)境模擬初步研究[J].航天器環(huán)境工程, 2007, 24(1)∶15-20 Fan Shichao, Jia Yang, Xiang Shuhong, et al.A preliminary study on simulation of lunar surface terrain[J].Spacecraft Environment Engineering, 2007, 24(1)∶15-20

[2]齊乃明, 張文輝, 高九州, 等.三維空間微重力地面模擬試驗(yàn)系統(tǒng)設(shè)計(jì)[J].機(jī)械工程學(xué)報(bào), 2011, 47(9)∶16-20 Qi Naiming, Zhang Wenhui, Gao Jiuzhou, et al.Design of ground simulation test system for three-dimensional spatial microgravity environment[J].Journal of Mechanical Engineering, 2011, 47(9)∶16-20

[3]陳杉.物體位姿單目視覺測(cè)量系統(tǒng)的研究[D].天津∶天津大學(xué), 2007∶6-12

[4]馬國松.基于單目視覺的位姿測(cè)量方法及仿真系統(tǒng)研究[D].哈爾濱 ∶哈爾濱工業(yè)大學(xué), 2005∶8-37

[5]侯建, 齊乃明.月球車立體視覺系統(tǒng)的一種實(shí)現(xiàn)方 案[C]∥中國宇航學(xué)會(huì)深空探測(cè)技術(shù)專業(yè)委員會(huì)第一屆學(xué)術(shù)會(huì)議.哈爾濱, 2005∶29-32

[6]侯建, 齊乃明.月球車視覺系統(tǒng)立體匹配算法研究[J].南京理工大學(xué)學(xué)報(bào) ∶自然科學(xué)版, 2008, 32(2)∶176-180 Hou Jian, Qi Naiming.Stereo matching algorithm for lunar rover[J].Journal of Nanjing University of Science and Technology, 2008, 32(2)∶176-180

[7]徐杰.機(jī)器視覺中攝像機(jī)標(biāo)定Tsai兩步法的分析與改進(jìn)[J].計(jì)算機(jī)工程與科學(xué), 2010, 32(4)∶46-47 Xu Jie.Analyzing and improving the Tsai camera calibration method in machine vision[J].Computer Engineering＆Science, 2010, 32(4)∶46-47

[8]李春艷, 王立, 盧欣, 等.一種雙目立體視覺相機(jī)的標(biāo)定方法[J].空間控制技術(shù)與應(yīng)用, 2010, 36(3)∶51-53 Li Chunyan, Wang Li, Lu Xin, et al.A binocular stereo camera calibration method[J].Aerospace Control and Application, 2010, 36(3)∶51-53