0 引言

    在航天領(lǐng)域的交會(huì)對(duì)接以及視覺(jué)SLAM（Simultaneous Localization and Mapping）項(xiàng)目中，都需要利用視覺(jué)成像技術(shù)來(lái)獲取目標(biāo)物與相機(jī)系統(tǒng)的位姿關(guān)系，而解算位姿需要得到目標(biāo)體的特征點(diǎn)與相機(jī)系統(tǒng)的距離信息^[1]。目前，位姿測(cè)量時(shí)探測(cè)系統(tǒng)與目標(biāo)物體的距離主要采用雙目視覺(jué)和RGB-D兩種方法^[2-3]。雙目視覺(jué)中是利用兩臺(tái)高分辨率相機(jī)形成的視差來(lái)估計(jì)目標(biāo)到相機(jī)系統(tǒng)的距離，由于需要知道同一個(gè)目標(biāo)點(diǎn)在兩個(gè)相機(jī)成像的對(duì)應(yīng)關(guān)系，計(jì)算量非常大，兩臺(tái)相機(jī)也使得體積變大。RGB-D相機(jī)向探測(cè)目標(biāo)發(fā)出主動(dòng)光，通過(guò)回波強(qiáng)度來(lái)計(jì)算距離。TOF(Time of Flight)相機(jī)利用飛行時(shí)間法原理測(cè)算距離，因其具有速度快、體積小、精度高、抗干擾性強(qiáng)的特性^[4]，受到人們關(guān)注。近年來(lái)，隨著光敏感器技術(shù)的提高，利用TOF相機(jī)探測(cè)距離解算位姿優(yōu)勢(shì)愈加明顯。

    徐文福^[5]等人提出了立體視覺(jué)的非合作目標(biāo)自主識(shí)別與位姿測(cè)量方法，利用雙目視覺(jué)模型提取目標(biāo)特征并進(jìn)行3D重構(gòu)，進(jìn)一步計(jì)算位姿，此雙目視覺(jué)系統(tǒng)體積大，計(jì)算量大。張旭東^[6]等人提出了基于PMD相機(jī)的跟蹤目標(biāo)方法，利用深度相機(jī)直接提取深度信息，然后在圖像中提取目標(biāo)特征，實(shí)時(shí)計(jì)算目標(biāo)的位姿，但是只進(jìn)行了近距離實(shí)驗(yàn)，應(yīng)用仍然受到限制。

    因此，本文設(shè)計(jì)了一套嵌入式TOF原理樣機(jī)，該相機(jī)體積小，在實(shí)際應(yīng)用時(shí)顯得輕巧，而且采納激光雷達(dá)作為主動(dòng)光源能進(jìn)行遠(yuǎn)距離測(cè)距并成像。利用此TOF相機(jī)對(duì)特定的合作靶標(biāo)進(jìn)行識(shí)別，計(jì)算靶標(biāo)形心的三維坐標(biāo)，并解算目標(biāo)物體的位姿，驗(yàn)證激光雷達(dá)TOF相機(jī)在交會(huì)對(duì)接和SLAM中的可行性。

1 系統(tǒng)總體設(shè)計(jì)

1.1 系統(tǒng)結(jié)構(gòu)與硬件組成

    本文設(shè)計(jì)的嵌入式TOF相機(jī)系統(tǒng)由靶標(biāo)、TOF相機(jī)、電源和計(jì)算機(jī)組成。在實(shí)驗(yàn)室條件下將靶標(biāo)設(shè)計(jì)為黑色圖形，共計(jì)4個(gè)，組成“L”形狀。常見(jiàn)的TOF相機(jī)是采用LED作為驅(qū)動(dòng)光源，而本設(shè)計(jì)使用的TOF相機(jī)利用激光作為驅(qū)動(dòng)光源，可以探測(cè)更遠(yuǎn)的距離。TOF相機(jī)可以工作在兩種模式下，在灰度模式下時(shí)，與普通相機(jī)一樣，接收被動(dòng)光源成像即為灰度圖；也可以在深度模式下，利用激光發(fā)出的主動(dòng)光源直接獲取深度數(shù)據(jù)，將數(shù)據(jù)按照?qǐng)D像矩陣排列即形成深度圖，圖像分辨率是320×240。采用24 V穩(wěn)壓電源給相機(jī)供電。而TOF相機(jī)模塊內(nèi)部含有嵌入ARM板，此芯片上安裝Linux操作系統(tǒng)，接收上位機(jī)的指令后將圖像數(shù)據(jù)通過(guò)網(wǎng)線傳給計(jì)算機(jī)。系統(tǒng)總體框圖如圖1所示。

1.2 系統(tǒng)軟件設(shè)計(jì)

    本文的系統(tǒng)軟件是基于QT5.9.4開(kāi)發(fā)的一個(gè)應(yīng)用程序，程序主要包括獲取TOF相機(jī)灰度和深度圖像的驅(qū)動(dòng)，圖像數(shù)據(jù)處理，靶標(biāo)的識(shí)別、匹配和位姿解算的算法，數(shù)據(jù)顯示等。此外，為了方便調(diào)試算法，直接將灰度圖和深度圖顯示在上位機(jī)界面上，可查看每個(gè)像素的深度值。

    為了描述靶標(biāo)P的位置信息，需要確定坐標(biāo)系，用 (X，Y，Z)表示出來(lái)。本文用到兩個(gè)坐標(biāo)系來(lái)描述靶標(biāo)的位置信息，一個(gè)是物體坐標(biāo)系O_c-X_cY_cZ_c，即目標(biāo)物體的本體坐標(biāo)系，原點(diǎn)設(shè)在目標(biāo)物上；另一個(gè)是相機(jī)坐標(biāo)系O_o-X_oY_oZ_o，即觀察坐標(biāo)系，原點(diǎn)設(shè)在相機(jī)的焦點(diǎn)上。坐標(biāo)系如圖2所示。

    在解算位姿之前，需要先確定相機(jī)的內(nèi)參數(shù)與畸變參數(shù)，這就要求相機(jī)標(biāo)定。

    本文TOF相機(jī)在工作模式下，會(huì)交替給上位機(jī)傳送灰度圖和深度圖。獲取圖像數(shù)據(jù)后，求解目標(biāo)物體的位姿數(shù)據(jù)主要包括3個(gè)步驟。

    (1)靶標(biāo)識(shí)別：對(duì)相機(jī)采集到的灰度圖像進(jìn)行畸變校正、降噪處理，利用閾值算法從灰度圖中篩選靶標(biāo)，提取靶標(biāo)形心坐標(biāo)，同時(shí)也從深度圖中提取形心的深度信息。

    (2)靶標(biāo)匹配：利用相機(jī)的內(nèi)參矩陣、靶標(biāo)形心的坐標(biāo)和深度值，計(jì)算相機(jī)坐標(biāo)系下靶標(biāo)的三維坐標(biāo)值，并求出靶標(biāo)的相對(duì)距離矩陣。計(jì)算物體坐標(biāo)系下相對(duì)矩陣。將兩組相對(duì)距離矩陣作為匹配算法的輸出，利用確定性退火算法找到靶標(biāo)兩個(gè)坐標(biāo)系下的靶標(biāo)序號(hào)對(duì)。

    (3)位姿計(jì)算：把正確匹配的靶標(biāo)序號(hào)對(duì)應(yīng)的三維坐標(biāo)值作為輸入，利用SVD算法，可以求出兩個(gè)坐標(biāo)系的旋轉(zhuǎn)矩陣R和位移向量t。

    位姿測(cè)量的總體框圖如圖3所示。

2 位姿測(cè)量系統(tǒng)原理與算法

    在位姿測(cè)量嵌入式系統(tǒng)中，從TOF相機(jī)獲取圖像矩陣后，最重要的是圖像的算法處理，主要包括目標(biāo)識(shí)別、靶標(biāo)匹配和位姿解算。

2.1 目標(biāo)識(shí)別

    本文利用灰度圖對(duì)靶標(biāo)識(shí)別與篩選，并用靶標(biāo)形心坐標(biāo)去深度圖像提取距離值，進(jìn)一步計(jì)算靶標(biāo)點(diǎn)的三維坐標(biāo)。計(jì)算三維坐標(biāo)點(diǎn)需要知道相機(jī)的參數(shù)矩陣和畸變參數(shù)，因此要標(biāo)定相機(jī)。

2.1.1 相機(jī)標(biāo)定

    使用針孔模型來(lái)描述相機(jī)成像，以相機(jī)光軸作為Z方向，目標(biāo)物在像平面形成的像與目標(biāo)物成相似關(guān)系，但由于透鏡的存在致使圖像會(huì)產(chǎn)生畸變，即成像點(diǎn)偏離針孔模型下的投影點(diǎn)。

    相機(jī)參數(shù)矩陣包括焦距和主點(diǎn)坐標(biāo)，當(dāng)知道目標(biāo)點(diǎn)P在像平面的坐標(biāo)(u，v)時(shí)，就利用式(1)推導(dǎo)出三維點(diǎn)坐標(biāo)。參數(shù)矩陣方程如下所示：

式中，K為相機(jī)的參數(shù)矩陣，P(X，Y，Z)為相機(jī)觀察坐標(biāo)系下實(shí)際物體的坐標(biāo)，f_x、f_y分別為x軸和y軸方向的焦距，c_x、c_y是相平面主點(diǎn)坐標(biāo)。

    由透鏡形狀引起的畸變是徑向畸變，因相機(jī)安裝時(shí)鏡頭與成像敏感期不平行會(huì)引起切向畸變。這些畸變?cè)跀?shù)學(xué)上可以用多項(xiàng)式函數(shù)來(lái)表示，如式(2)所示，k₁、k₂、k₃描述徑向畸變，p₁、p₂描述切向畸變。當(dāng)確定畸變參數(shù)，就可以對(duì)圖像進(jìn)行畸變校正。

式中，x、y為畸變前的坐標(biāo)點(diǎn)，x_d、y_d是畸變后的坐標(biāo)點(diǎn)，r表示離圖像中心點(diǎn)的距離。

    為了確定這些參數(shù)，本文用TOF相機(jī)在灰度模式下對(duì)黑白棋格標(biāo)定板拍攝多幅圖片，然后利用MATLAB的圖像標(biāo)定庫(kù)函數(shù)得到相機(jī)的內(nèi)參數(shù)。

2.1.2 靶標(biāo)識(shí)別

    本文采用TOF相機(jī)拍攝的灰度圖來(lái)識(shí)別靶標(biāo)。為了在實(shí)驗(yàn)室條件下更容易識(shí)別出靶標(biāo)，選用具有漫反射特性材質(zhì)的靶標(biāo)，表面涂黑，靶標(biāo)背景板顏色為白色，這樣做的好處是提高了深度數(shù)據(jù)的準(zhǔn)確性。

    先通過(guò)畸變矯正消除畸變帶來(lái)的影響。對(duì)矯正過(guò)圖像矩陣用5×5的LOG濾波卷積核做卷積，既對(duì)采集的灰度圖像進(jìn)行了降噪，又提高了圖像的對(duì)比度，利于篩選靶標(biāo)。經(jīng)過(guò)二值化并統(tǒng)計(jì)連通域，對(duì)疑似靶標(biāo)標(biāo)簽化，設(shè)計(jì)數(shù)據(jù)結(jié)構(gòu)存儲(chǔ)疑似的像平面下坐標(biāo)點(diǎn)，根據(jù)區(qū)域靶標(biāo)的成像大小以及圓心率進(jìn)一步篩選存儲(chǔ)的疑似靶標(biāo)。對(duì)于TOF相機(jī)采集到的深度圖像運(yùn)用遞歸時(shí)域中值法得到的深度值更具穩(wěn)定性，如式(3)所示：

式中，d_k(u，v)表示第k幀深度圖在像素坐標(biāo)(u，v)的深度值。

2.2 靶標(biāo)匹配

    確定性退火算法是Rose博士于1990年首次提出的，該算法將極小化問(wèn)題看作是能量極小化狀態(tài)，當(dāng)系統(tǒng)達(dá)到平衡態(tài)時(shí)，所設(shè)定的自由能函數(shù)達(dá)到極小^[7]。本文利用確定性退火算法來(lái)解決靶標(biāo)匹配問(wèn)題。

    在物體坐標(biāo)系下，靶標(biāo)坐標(biāo)為C(X，Y，Z)，則相對(duì)距離矩陣為C_relative。在相機(jī)坐標(biāo)系下，疑似靶標(biāo)的三維坐標(biāo)O=(X，Y，Z)，相對(duì)距離矩陣O_relative。

式中，C_i、C_j表示靶標(biāo)在物體坐標(biāo)系下的坐標(biāo)，O_i、O_j表示靶標(biāo)在相機(jī)坐標(biāo)系下的坐標(biāo)。

    于是計(jì)算得到兩組三維坐標(biāo)相對(duì)距離矩陣，而兩組三維坐標(biāo)構(gòu)成兩個(gè)集合。給定初始狀態(tài)，將三維坐標(biāo)相對(duì)距離矩陣嵌入到退火算法中，在自由能函數(shù)中引入相對(duì)距離差(C_relative_ij-O_relative_ij)，通過(guò)最小化自由能函數(shù)得到兩個(gè)坐標(biāo)系下靶標(biāo)匹配序號(hào)，存儲(chǔ)到匹配成功變量中^[8]。

    靶標(biāo)匹配的過(guò)程中，為了避免匹配時(shí)間過(guò)長(zhǎng)，需要設(shè)定兩個(gè)參數(shù)作為終止條件。一個(gè)是退火的迭代次數(shù)，另一個(gè)是退火算法中條件概率矩陣二范數(shù)的變化量。當(dāng)條件概率二范數(shù)的變化量小于0.001時(shí)，認(rèn)為匹配過(guò)程結(jié)束，退出迭代過(guò)程。

2.3 位姿解算

    待求解的位姿數(shù)據(jù)實(shí)際上是兩個(gè)坐標(biāo)系的平移和旋轉(zhuǎn)，可通過(guò)旋轉(zhuǎn)矩陣R（3×3）和平移向量t（1×3）來(lái)表示。為了直觀地表示姿態(tài)，將旋轉(zhuǎn)矩陣轉(zhuǎn)化為歐拉角。從靶標(biāo)匹配中計(jì)算的序號(hào)對(duì)中，得到兩組來(lái)自不同坐標(biāo)系的三維坐標(biāo)集合。同一點(diǎn)P坐標(biāo)系O_c-X_cY_cZ_c下的三維坐標(biāo)定義為p，坐標(biāo)系O_o-X_oY_oZ_o下的三維坐標(biāo)定義為q，那么：

式中，x_i表示靶標(biāo)在物體坐標(biāo)系下的中心向量，y_i表示靶標(biāo)在相機(jī)坐標(biāo)系下的中心向量。中心向量矩陣X=(x₁，x₂，…，x_n)，中心向量矩陣 Y=(y₁，y₂，…，y_n)，W是由w_i組成的對(duì)角矩陣，有：

3 系統(tǒng)測(cè)試結(jié)果

    在實(shí)驗(yàn)中，采用固定相機(jī)、移動(dòng)靶標(biāo)的測(cè)量方法。旋轉(zhuǎn)平臺(tái)有6自由度，位置精度是0.01 mm，角度精度為0.01°。測(cè)量時(shí)，只保持一個(gè)自由度的變化，對(duì)平移距離的實(shí)驗(yàn)結(jié)果見(jiàn)表1，對(duì)旋轉(zhuǎn)角度的實(shí)驗(yàn)結(jié)果見(jiàn)表2，位姿數(shù)據(jù)是在距離目標(biāo)5 m左右測(cè)量得出的。

    從實(shí)驗(yàn)結(jié)果可以看出，平移誤差精度在0.6%誤差水平，角度誤差在0.13°內(nèi)，尤其在遠(yuǎn)距離（5 m以外）測(cè)量時(shí)，仍然達(dá)到較高的精度。

    在調(diào)試過(guò)程中，在上位機(jī)界面中，可以對(duì)TOF相機(jī)手動(dòng)設(shè)置參數(shù)，同時(shí)顯示灰度圖和深度圖，并給出目標(biāo)位姿信息，如圖4所示。

4 結(jié)論

    本文基于一套基于TOF相機(jī)的嵌入式系統(tǒng)，設(shè)計(jì)了特定的合作靶標(biāo)，利用相機(jī)拍攝的灰度圖識(shí)別靶標(biāo)，提取靶標(biāo)形心，利用相機(jī)拍攝的深度圖來(lái)獲取靶標(biāo)的三維坐標(biāo)信息，完成了目標(biāo)物的識(shí)別及其位姿解算，驗(yàn)證了激光雷達(dá)作為驅(qū)動(dòng)光源的TOF 相機(jī)在交會(huì)對(duì)接的可行性與可靠性。

    為了提高計(jì)算速度與精確度，需要進(jìn)一步研究深度圖識(shí)別、卡爾曼濾波跟蹤等方法。

參考文獻(xiàn)

[1] 初廣麗，王延杰，邸男，等.復(fù)雜場(chǎng)景中航天器靶標(biāo)的快速識(shí)別[J].光學(xué)精密工程，2016，24(4)：865-872.

[2] 徐培智，徐貴力，王彪，等.基于立體視覺(jué)的非合作目標(biāo)位姿測(cè)量[J].計(jì)算機(jī)與現(xiàn)代化，2013(8)：85-91.

[3] 時(shí)建奇，許林.基于PMD相機(jī)的位姿測(cè)量方法研究[J].電腦知識(shí)與技術(shù)，2016，12(10)：269-271.

[4] 梁斌，何英，鄒瑜，等.ToF相機(jī)在空間非合作目標(biāo)近距離測(cè)量中的應(yīng)用[J].宇航學(xué)報(bào)，2016，37(9)：1080-1088.

[5] 徐文福，劉宇，梁斌，等.非合作航天器的相對(duì)位姿測(cè)量[J].光學(xué)精密工程，2009，17(7)：1570-1581.

[6] 張旭東，李文龍，胡良梅，等.基于PMD相機(jī)的特征跟蹤位姿測(cè)量方法[J].電子測(cè)量與儀器學(xué)報(bào)，2013，27(7)：640-646.

[7] 孫冬梅，裘正定.基于確定性退火技術(shù)的魯棒性的點(diǎn)匹配算法[J].計(jì)算機(jī)學(xué)報(bào)，2002，25(6)：606-611.

[8] CHRISTIAN J A，ROBINSON S B，D′SOUZA C N，et al.Cooperative relative navigation of spacecraft using flash light detection and ranging sensors[J].Journal of Guidance Control & Dynamics，2014，37(2)：452-465.

[9] SORKINE O.Least-squares rigid motion using SVD[Z].Technical notes，2009.

作者信息:

趙樹(shù)磊，劉敬猛，張  慧，呂志宇

（北京航空航天大學(xué) 自動(dòng)化與電氣工程學(xué)院，北京100083）