基于PMD相机的位姿测量方法研究

时建奇+许林

摘要：针对航天器对接操作过程中相对位姿测量的问题，该文利用航天器对接口为圆环的结构特征，设计了一种基于PMD相机的位姿测量方法，该方法利用PMD相机获取的三维点云数据，通过点云配准的方法，实现了目标相对位姿的准确测量。

关键词： PMD；位姿测量；三维点云；配准

中图分类号：TP274 文献标识码：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 概述

在航天领域，要完成航天器目标的对接任务，需要获得目标的相对位姿参数，对于非合作目标，航天器本身没有预先安装合作标识器，需要利用航天器本身的结构特征来实现目标的识别和定位[1] 。航天器有对接环和发动器喷口等圆形结构，本文利用目标的圆特征，进行位姿测量。相较于其他特征，利用圆特征进行位姿测量的最大优势是能够抗遮挡，通过部分圆特征就能解算出位姿。然而，对于非合作目标，在没有任何约束条件下，基于单个圆的单目视觉位姿测量的解具有二义性[2]，所以在实际应用中需要解决目标二义性的问题。

本文提出了一种基于PMD相机的非合作目标[3]的空间位姿测量方法，通过空间点云配准的方法能够直接获取位姿的唯一解，解决了二义性的问题，该方法首先对PMD相机采集的深度图像进行滤波去噪、去除不可靠点和异常值。然后把目标的深度数据转化为空间三维点云数据，通过模版匹配的方法对两幅目标点云进行粗配准，再利用改进型的ICP算法[4]对点云进行精配准，获取两幅点云的平移矩阵和旋转矩阵，最终得到目标相对位姿测量结果。最后，通过实验验证了算法的有效性。

2 位姿测量过程

2.1 相机简介

本文采用3D相机是德国PMDTec公司的PMD[vision]?Camcube 2.0相机。它是一种基于飞行时间（Time-of-Flight）原理的3D相机，它是一种新型、小型化的主动式立体成像设备，无需扫描便可实时的获取目标的深度信息、灰度信息和幅度信息[5]。PMD相机测距原理是通过红外发射器向目标发射光脉冲，到达目标物体后发生反射，反射的红外光被相机上的探测器接收，计算发射和接收光信号的相位差来计算相机到目标的距离，从而获取整个场景的深度信息。

2.2 三维点云的获取

基于视觉测量系统的相机成像模型基本都是采用针孔成像模型，该模型为一个理想的投影成像模型。如图1所示，[OW-XWYWZW]为世界坐标系，[OC-XCYCZC]为相机坐标系，原点[OC]为相机的镜头中心，[OI-XIYI]为像平面坐标系，[o-uv]为图 像 像 素坐 标 系，这两个坐标系为二维坐标系，描述成像点坐标即空间点在相机像平面的投影坐标。P为空间中的任意一点，P′为P点在像 平 面上的成像点， [OCOI]的距离值为焦 距[f]，[ZC]轴为主光轴的方向，[XC]轴和[YC]轴分别与图像像素坐标系的[u]和[v]平行，并与其他两轴构成右手系。

三维点云是利用PMD相机获取的每个像素点的深度信息转化为目标在相机坐标系中的三维坐标点集，结合公式（1）～（3）即可求出每个点的三维坐标。

2.3 位姿测量过程

本文位姿测量过程如图2所示：

1）深度图像获取：利用PMD相机采集场景的幅度图像和深度图像。

2）滤波去噪：幅度图像的每个像素值表示相机传感器接收到反射信号的强度大小，幅度值较低说明该像素点接收回反射信号强度较低，所以该点测量的深度信息不可靠，通过设置幅值门限来滤除不可靠的像素点。PMD相机的分辨率不高，图像中含有噪声，本文采用自适应双边滤波[6]算法，该方法不仅可以有效地滤除噪声，还能较好的保存图像的细节部分。

3）点云预处理：由深度图像转化为三维点云，在点云的边缘处存在跳变点，也称作孤立点，需要去除这些不必要的孤立点，本文采用Knn最邻近结点算法[7]对点云中每个点搜索其K个邻近点，计算该点与这K个邻近点的距离，如果存在某一距离值大于一定的阈值，则去除该点。

4）点云配准：点云配准是通过对两片目标点云进行匹配来确定点云的变换关系，得到两片点云之间的旋转矩阵和平移矩阵。由于本文采用圆锥目标作为测量对象，很难提取到目标表面的特征点，本文采用模型匹配[8]的方法，在第一幅点云中建立点对特征向量，由特征点对构建目标模型，然后根据点对特征向量在另一幅点云中搜索相同特征向量的点对，利用两幅点云中的点对进行配准，完成粗配准，得到两幅点云之间的初始旋转和平移矩阵。

然后采用改进型的ICP算法对其进行精确配准，提高了配准的速度和精度。最后，根据配准得到的旋转和平移矩阵计算出目标的相对位姿。

3 实验结果与分析

为验证测量方法的可行性和准确性，自行搭建位姿测量系统，如下图所示，图3为实验所用的PMD相机。图4为测量控制平台，目标模型安装在平台上面，通过软件控制电机使目标进行6个自由度的变换。图5为控制平台的软件控制界面，通过设置参数可以实现目标精确的平移和角度旋转。

首先使目标正对于PMD相机，相对于相机的旋转角度设置为0°，在距离相机0.5米处采集数据，图6（a）为目标深度图像，图6（b）为滤波后的深度图像。然后把目标绕y轴（偏航角）旋转10°采集数据，如图6（c）所示。获取两幅深度图像的三维点云，经过预处理后进行点云配准，图6（e）是两片点云配准的结果，白色点是参考模型点，绿色点是数据模型点。固定距离，旋转不同角度验证算法，测量结果如表1所示，与实际值相比，测量最大误差在1.2°以内，表明了本文算法能够有效的测量非合作目标的位姿，能够满足实际位姿测量需要。

4 结论与展望

本文利用PMD相机，采用空间三维点云配准的方法对目标进行相对位姿测量，搭建实验系统验证了该方法的有效性，实验结果表明该方法能够有效的测量目标的位姿，能够满足实际测量的需要。

参考文献：

[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].机械工程学报， 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.