基于PMD相機(jī)的位姿測量方法研究

時建奇+許林

摘要：針對航天器對接操作過程中相對位姿測量的問題，該文利用航天器對接口為圓環(huán)的結(jié)構(gòu)特征，設(shè)計了一種基于PMD相機(jī)的位姿測量方法，該方法利用PMD相機(jī)獲取的三維點云數(shù)據(jù)，通過點云配準(zhǔn)的方法，實現(xiàn)了目標(biāo)相對位姿的準(zhǔn)確測量。

關(guān)鍵詞： PMD；位姿測量；三維點云；配準(zhǔn)

中圖分類號：TP274 文獻(xiàn)標(biāo)識碼：A 文章編號：1009-3044（2016）10-0269-03

Abstract：To deal with the problem of measured relative pose in spacecraft docking operations ， This paper use the circular structure of the spacecraft ， designed a pose measurement method based on the PMD camera ， The method uses the camera to obtain three-dimensional point cloud data ，by this method of point cloud registration to achieve an accurate measurement of the relative pose of the target.

Key words：PMD； pose measurement； 3D point cloud； registration

1 概述

在航天領(lǐng)域，要完成航天器目標(biāo)的對接任務(wù)，需要獲得目標(biāo)的相對位姿參數(shù)，對于非合作目標(biāo)，航天器本身沒有預(yù)先安裝合作標(biāo)識器，需要利用航天器本身的結(jié)構(gòu)特征來實現(xiàn)目標(biāo)的識別和定位[1] 。航天器有對接環(huán)和發(fā)動器噴口等圓形結(jié)構(gòu)，本文利用目標(biāo)的圓特征，進(jìn)行位姿測量。相較于其他特征，利用圓特征進(jìn)行位姿測量的最大優(yōu)勢是能夠抗遮擋，通過部分圓特征就能解算出位姿。然而，對于非合作目標(biāo)，在沒有任何約束條件下，基于單個圓的單目視覺位姿測量的解具有二義性[2]，所以在實際應(yīng)用中需要解決目標(biāo)二義性的問題。

本文提出了一種基于PMD相機(jī)的非合作目標(biāo)[3]的空間位姿測量方法，通過空間點云配準(zhǔn)的方法能夠直接獲取位姿的唯一解，解決了二義性的問題，該方法首先對PMD相機(jī)采集的深度圖像進(jìn)行濾波去噪、去除不可靠點和異常值。然后把目標(biāo)的深度數(shù)據(jù)轉(zhuǎn)化為空間三維點云數(shù)據(jù)，通過模版匹配的方法對兩幅目標(biāo)點云進(jìn)行粗配準(zhǔn)，再利用改進(jìn)型的ICP算法[4]對點云進(jìn)行精配準(zhǔn)，獲取兩幅點云的平移矩陣和旋轉(zhuǎn)矩陣，最終得到目標(biāo)相對位姿測量結(jié)果。最后，通過實驗驗證了算法的有效性。

2 位姿測量過程

2.1 相機(jī)簡介

本文采用3D相機(jī)是德國PMDTec公司的PMD[vision]?Camcube 2.0相機(jī)。它是一種基于飛行時間（Time-of-Flight）原理的3D相機(jī)，它是一種新型、小型化的主動式立體成像設(shè)備，無需掃描便可實時的獲取目標(biāo)的深度信息、灰度信息和幅度信息[5]。PMD相機(jī)測距原理是通過紅外發(fā)射器向目標(biāo)發(fā)射光脈沖，到達(dá)目標(biāo)物體后發(fā)生反射，反射的紅外光被相機(jī)上的探測器接收，計算發(fā)射和接收光信號的相位差來計算相機(jī)到目標(biāo)的距離，從而獲取整個場景的深度信息。

2.2 三維點云的獲取

基于視覺測量系統(tǒng)的相機(jī)成像模型基本都是采用針孔成像模型，該模型為一個理想的投影成像模型。如圖1所示，[OW-XWYWZW]為世界坐標(biāo)系，[OC-XCYCZC]為相機(jī)坐標(biāo)系，原點[OC]為相機(jī)的鏡頭中心，[OI-XIYI]為像平面坐標(biāo)系，[o-uv]為圖 像 像 素坐 標(biāo) 系，這兩個坐標(biāo)系為二維坐標(biāo)系，描述成像點坐標(biāo)即空間點在相機(jī)像平面的投影坐標(biāo)。P為空間中的任意一點，P′為P點在像 平 面上的成像點， [OCOI]的距離值為焦 距[f]，[ZC]軸為主光軸的方向，[XC]軸和[YC]軸分別與圖像像素坐標(biāo)系的[u]和[v]平行，并與其他兩軸構(gòu)成右手系。

三維點云是利用PMD相機(jī)獲取的每個像素點的深度信息轉(zhuǎn)化為目標(biāo)在相機(jī)坐標(biāo)系中的三維坐標(biāo)點集，結(jié)合公式（1）～（3）即可求出每個點的三維坐標(biāo)。

2.3 位姿測量過程

本文位姿測量過程如圖2所示：

1）深度圖像獲?。豪肞MD相機(jī)采集場景的幅度圖像和深度圖像。

2）濾波去噪：幅度圖像的每個像素值表示相機(jī)傳感器接收到反射信號的強(qiáng)度大小，幅度值較低說明該像素點接收回反射信號強(qiáng)度較低，所以該點測量的深度信息不可靠，通過設(shè)置幅值門限來濾除不可靠的像素點。PMD相機(jī)的分辨率不高，圖像中含有噪聲，本文采用自適應(yīng)雙邊濾波[6]算法，該方法不僅可以有效地濾除噪聲，還能較好的保存圖像的細(xì)節(jié)部分。

3）點云預(yù)處理：由深度圖像轉(zhuǎn)化為三維點云，在點云的邊緣處存在跳變點，也稱作孤立點，需要去除這些不必要的孤立點，本文采用Knn最鄰近結(jié)點算法[7]對點云中每個點搜索其K個鄰近點，計算該點與這K個鄰近點的距離，如果存在某一距離值大于一定的閾值，則去除該點。

4）點云配準(zhǔn)：點云配準(zhǔn)是通過對兩片目標(biāo)點云進(jìn)行匹配來確定點云的變換關(guān)系，得到兩片點云之間的旋轉(zhuǎn)矩陣和平移矩陣。由于本文采用圓錐目標(biāo)作為測量對象，很難提取到目標(biāo)表面的特征點，本文采用模型匹配[8]的方法，在第一幅點云中建立點對特征向量，由特征點對構(gòu)建目標(biāo)模型，然后根據(jù)點對特征向量在另一幅點云中搜索相同特征向量的點對，利用兩幅點云中的點對進(jìn)行配準(zhǔn)，完成粗配準(zhǔn)，得到兩幅點云之間的初始旋轉(zhuǎn)和平移矩陣。

然后采用改進(jìn)型的ICP算法對其進(jìn)行精確配準(zhǔn)，提高了配準(zhǔn)的速度和精度。最后，根據(jù)配準(zhǔn)得到的旋轉(zhuǎn)和平移矩陣計算出目標(biāo)的相對位姿。

3 實驗結(jié)果與分析

為驗證測量方法的可行性和準(zhǔn)確性，自行搭建位姿測量系統(tǒng)，如下圖所示，圖3為實驗所用的PMD相機(jī)。圖4為測量控制平臺，目標(biāo)模型安裝在平臺上面，通過軟件控制電機(jī)使目標(biāo)進(jìn)行6個自由度的變換。圖5為控制平臺的軟件控制界面，通過設(shè)置參數(shù)可以實現(xiàn)目標(biāo)精確的平移和角度旋轉(zhuǎn)。

首先使目標(biāo)正對于PMD相機(jī)，相對于相機(jī)的旋轉(zhuǎn)角度設(shè)置為0°，在距離相機(jī)0.5米處采集數(shù)據(jù)，圖6（a）為目標(biāo)深度圖像，圖6（b）為濾波后的深度圖像。然后把目標(biāo)繞y軸（偏航角）旋轉(zhuǎn)10°采集數(shù)據(jù)，如圖6（c）所示。獲取兩幅深度圖像的三維點云，經(jīng)過預(yù)處理后進(jìn)行點云配準(zhǔn)，圖6（e）是兩片點云配準(zhǔn)的結(jié)果，白色點是參考模型點，綠色點是數(shù)據(jù)模型點。固定距離，旋轉(zhuǎn)不同角度驗證算法，測量結(jié)果如表1所示，與實際值相比，測量最大誤差在1.2°以內(nèi)，表明了本文算法能夠有效的測量非合作目標(biāo)的位姿，能夠滿足實際位姿測量需要。

4 結(jié)論與展望

本文利用PMD相機(jī)，采用空間三維點云配準(zhǔn)的方法對目標(biāo)進(jìn)行相對位姿測量，搭建實驗系統(tǒng)驗證了該方法的有效性，實驗結(jié)果表明該方法能夠有效的測量目標(biāo)的位姿，能夠滿足實際測量的需要。

參考文獻(xiàn)：

[1] Khongsab P. Signal processing and performance evaluation of a PMD camera for space docking[J]. Kiruna： Masters thesis， Lulea University of Technology， 2009.

[2] Shiu Y C， Ahmad S. 3D location of circular and spherical features by monocular model-based vision[C]// Proceedings of the IEEE International Conference on Systems， Man and Cybernetics， Massachusetts， USA，1989： 576-581.

[3] INABAN ， ODAM ， ASANOM. Rescuing a stranded satellite in space-experimental robotic capture of non2cooperative satellites[J].Transact ions of the Japan Society for Aeronautical and Space Sciences ， 2006 ，48 （162） ：2132220.

[4] Bergstr?m P， Edlund O. Robust registration of point sets using iteratively reweighted least squares[J]. Computational Optimization and Applications， 2014， 58（3）： 543-561.

[5] Andreas Kolb， Erhardt Barth， Reinhard Koch， et al. Time-of-Flight Cameras in Computer Graphics[C]. In Computer Graphics Forum， 2010，29.

[6] Gao Jun， Jia Bao Zhu， Zhang Xu Dong，et al. PMD camera calibration based on adaptive bilateral filter[C]// Wuhan， China ：2011 International Symposium on Photonics and Optoelectronics （SOPO），， 2011： 1-4.

[7] 張旭東，吳國松，胡良梅，等.基于TOF三維相機(jī)相鄰散亂點云配準(zhǔn)技術(shù)研究[J].機(jī)械工程學(xué)報， 2013，49（12）：8-16.

[8] Drost B， Ulrich M， Navab N， et al. Model globally， match locally： Efficient and robust 3D object recognition[C]. 2013 IEEE Conference on Computer Vision and Pattern Recognition，2013.