0 引言

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

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

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

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

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

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

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

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

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

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

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

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

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

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

    位姿測量的總體框圖如圖3所示。

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

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

2.1 目標識別

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

2.1.1 相機標定

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

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

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

    由透鏡形狀引起的畸變是徑向畸變，因相機安裝時鏡頭與成像敏感期不平行會引起切向畸變。這些畸變在數(shù)學上可以用多項式函數(shù)來表示，如式(2)所示，k₁、k₂、k₃描述徑向畸變，p₁、p₂描述切向畸變。當確定畸變參數(shù)，就可以對圖像進行畸變校正。

式中，x、y為畸變前的坐標點，x_d、y_d是畸變后的坐標點，r表示離圖像中心點的距離。

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

2.1.2 靶標識別

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

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

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

2.2 靶標匹配

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

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

式中，C_i、C_j表示靶標在物體坐標系下的坐標，O_i、O_j表示靶標在相機坐標系下的坐標。

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

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

2.3 位姿解算

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

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

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

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

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

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

4 結(jié)論

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

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

參考文獻

[1] 初廣麗，王延杰，邸男，等.復雜場景中航天器靶標的快速識別[J].光學精密工程，2016，24(4)：865-872.

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

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

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

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

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

[7] 孫冬梅，裘正定.基于確定性退火技術(shù)的魯棒性的點匹配算法[J].計算機學報，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.

作者信息:

趙樹磊，劉敬猛，張  慧，呂志宇

（北京航空航天大學 自動化與電氣工程學院，北京100083）