0 引言

    在航天领域的交会对接以及视觉SLAM（Simultaneous Localization and Mapping）项目中，都需要利用视觉成像技术来获取目标物与相机系统的位姿关系，而解算位姿需要得到目标体的特征点与相机系统的距离信息^[1]。目前，位姿测量时探测系统与目标物体的距离主要采用双目视觉和RGB-D两种方法^[2-3]。双目视觉中是利用两台高分辨率相机形成的视差来估计目标到相机系统的距离，由于需要知道同一个目标点在两个相机成像的对应关系，计算量非常大，两台相机也使得体积变大。RGB-D相机向探测目标发出主动光，通过回波强度来计算距离。TOF(Time of Flight)相机利用飞行时间法原理测算距离，因其具有速度快、体积小、精度高、抗干扰性强的特性^[4]，受到人们关注。近年来，随着光敏感器技术的提高，利用TOF相机探测距离解算位姿优势愈加明显。

    徐文福^[5]等人提出了立体视觉的非合作目标自主识别与位姿测量方法，利用双目视觉模型提取目标特征并进行3D重构，进一步计算位姿，此双目视觉系统体积大，计算量大。张旭东^[6]等人提出了基于PMD相机的跟踪目标方法，利用深度相机直接提取深度信息，然后在图像中提取目标特征，实时计算目标的位姿，但是只进行了近距离实验，应用仍然受到限制。

    因此，本文设计了一套嵌入式TOF原理样机，该相机体积小，在实际应用时显得轻巧，而且采纳激光雷达作为主动光源能进行远距离测距并成像。利用此TOF相机对特定的合作靶标进行识别，计算靶标形心的三维坐标，并解算目标物体的位姿，验证激光雷达TOF相机在交会对接和SLAM中的可行性。

1 系统总体设计

1.1 系统结构与硬件组成

    本文设计的嵌入式TOF相机系统由靶标、TOF相机、电源和计算机组成。在实验室条件下将靶标设计为黑色图形，共计4个，组成“L”形状。常见的TOF相机是采用LED作为驱动光源，而本设计使用的TOF相机利用激光作为驱动光源，可以探测更远的距离。TOF相机可以工作在两种模式下，在灰度模式下时，与普通相机一样，接收被动光源成像即为灰度图；也可以在深度模式下，利用激光发出的主动光源直接获取深度数据，将数据按照图像矩阵排列即形成深度图，图像分辨率是320×240。采用24 V稳压电源给相机供电。而TOF相机模块内部含有嵌入ARM板，此芯片上安装Linux操作系统，接收上位机的指令后将图像数据通过网线传给计算机。系统总体框图如图1所示。

1.2 系统软件设计

    本文的系统软件是基于QT5.9.4开发的一个应用程序，程序主要包括获取TOF相机灰度和深度图像的驱动，图像数据处理，靶标的识别、匹配和位姿解算的算法，数据显示等。此外，为了方便调试算法，直接将灰度图和深度图显示在上位机界面上，可查看每个像素的深度值。

    为了描述靶标P的位置信息，需要确定坐标系，用 (X，Y，Z)表示出来。本文用到两个坐标系来描述靶标的位置信息，一个是物体坐标系O_c-X_cY_cZ_c，即目标物体的本体坐标系，原点设在目标物上；另一个是相机坐标系O_o-X_oY_oZ_o，即观察坐标系，原点设在相机的焦点上。坐标系如图2所示。

    在解算位姿之前，需要先确定相机的内参数与畸变参数，这就要求相机标定。

    本文TOF相机在工作模式下，会交替给上位机传送灰度图和深度图。获取图像数据后，求解目标物体的位姿数据主要包括3个步骤。

    (1)靶标识别：对相机采集到的灰度图像进行畸变校正、降噪处理，利用阈值算法从灰度图中筛选靶标，提取靶标形心坐标，同时也从深度图中提取形心的深度信息。

    (2)靶标匹配：利用相机的内参矩阵、靶标形心的坐标和深度值，计算相机坐标系下靶标的三维坐标值，并求出靶标的相对距离矩阵。计算物体坐标系下相对矩阵。将两组相对距离矩阵作为匹配算法的输出，利用确定性退火算法找到靶标两个坐标系下的靶标序号对。

    (3)位姿计算：把正确匹配的靶标序号对应的三维坐标值作为输入，利用SVD算法，可以求出两个坐标系的旋转矩阵R和位移向量t。

    位姿测量的总体框图如图3所示。

2 位姿测量系统原理与算法

    在位姿测量嵌入式系统中，从TOF相机获取图像矩阵后，最重要的是图像的算法处理，主要包括目标识别、靶标匹配和位姿解算。

2.1 目标识别

    本文利用灰度图对靶标识别与筛选，并用靶标形心坐标去深度图像提取距离值，进一步计算靶标点的三维坐标。计算三维坐标点需要知道相机的参数矩阵和畸变参数，因此要标定相机。

2.1.1 相机标定

    使用针孔模型来描述相机成像，以相机光轴作为Z方向，目标物在像平面形成的像与目标物成相似关系，但由于透镜的存在致使图像会产生畸变，即成像点偏离针孔模型下的投影点。

    相机参数矩阵包括焦距和主点坐标，当知道目标点P在像平面的坐标(u，v)时，就利用式(1)推导出三维点坐标。参数矩阵方程如下所示：

式中，K为相机的参数矩阵，P(X，Y，Z)为相机观察坐标系下实际物体的坐标，f_x、f_y分别为x轴和y轴方向的焦距，c_x、c_y是相平面主点坐标。

    由透镜形状引起的畸变是径向畸变，因相机安装时镜头与成像敏感期不平行会引起切向畸变。这些畸变在数学上可以用多项式函数来表示，如式(2)所示，k₁、k₂、k₃描述径向畸变，p₁、p₂描述切向畸变。当确定畸变参数，就可以对图像进行畸变校正。

式中，x、y为畸变前的坐标点，x_d、y_d是畸变后的坐标点，r表示离图像中心点的距离。

    为了确定这些参数，本文用TOF相机在灰度模式下对黑白棋格标定板拍摄多幅图片，然后利用MATLAB的图像标定库函数得到相机的内参数。

2.1.2 靶标识别

    本文采用TOF相机拍摄的灰度图来识别靶标。为了在实验室条件下更容易识别出靶标，选用具有漫反射特性材质的靶标，表面涂黑，靶标背景板颜色为白色，这样做的好处是提高了深度数据的准确性。

    先通过畸变矫正消除畸变带来的影响。对矫正过图像矩阵用5×5的LOG滤波卷积核做卷积，既对采集的灰度图像进行了降噪，又提高了图像的对比度，利于筛选靶标。经过二值化并统计连通域，对疑似靶标标签化，设计数据结构存储疑似的像平面下坐标点，根据区域靶标的成像大小以及圆心率进一步筛选存储的疑似靶标。对于TOF相机采集到的深度图像运用递归时域中值法得到的深度值更具稳定性，如式(3)所示：

式中，d_k(u，v)表示第k帧深度图在像素坐标(u，v)的深度值。

2.2 靶标匹配

    确定性退火算法是Rose博士于1990年首次提出的，该算法将极小化问题看作是能量极小化状态，当系统达到平衡态时，所设定的自由能函数达到极小^[7]。本文利用确定性退火算法来解决靶标匹配问题。

    在物体坐标系下，靶标坐标为C(X，Y，Z)，则相对距离矩阵为C_relative。在相机坐标系下，疑似靶标的三维坐标O=(X，Y，Z)，相对距离矩阵O_relative。

式中，C_i、C_j表示靶标在物体坐标系下的坐标，O_i、O_j表示靶标在相机坐标系下的坐标。

    于是计算得到两组三维坐标相对距离矩阵，而两组三维坐标构成两个集合。给定初始状态，将三维坐标相对距离矩阵嵌入到退火算法中，在自由能函数中引入相对距离差(C_relative_ij-O_relative_ij)，通过最小化自由能函数得到两个坐标系下靶标匹配序号，存储到匹配成功变量中^[8]。

    靶标匹配的过程中，为了避免匹配时间过长，需要设定两个参数作为终止条件。一个是退火的迭代次数，另一个是退火算法中条件概率矩阵二范数的变化量。当条件概率二范数的变化量小于0.001时，认为匹配过程结束，退出迭代过程。

2.3 位姿解算

    待求解的位姿数据实际上是两个坐标系的平移和旋转，可通过旋转矩阵R（3×3）和平移向量t（1×3）来表示。为了直观地表示姿态，将旋转矩阵转化为欧拉角。从靶标匹配中计算的序号对中，得到两组来自不同坐标系的三维坐标集合。同一点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 系统测试结果

    在实验中，采用固定相机、移动靶标的测量方法。旋转平台有6自由度，位置精度是0.01 mm，角度精度为0.01°。测量时，只保持一个自由度的变化，对平移距离的实验结果见表1，对旋转角度的实验结果见表2，位姿数据是在距离目标5 m左右测量得出的。

    从实验结果可以看出，平移误差精度在0.6%误差水平，角度误差在0.13°内，尤其在远距离（5 m以外）测量时，仍然达到较高的精度。

    在调试过程中，在上位机界面中，可以对TOF相机手动设置参数，同时显示灰度图和深度图，并给出目标位姿信息，如图4所示。

4 结论

    本文基于一套基于TOF相机的嵌入式系统，设计了特定的合作靶标，利用相机拍摄的灰度图识别靶标，提取靶标形心，利用相机拍摄的深度图来获取靶标的三维坐标信息，完成了目标物的识别及其位姿解算，验证了激光雷达作为驱动光源的TOF 相机在交会对接的可行性与可靠性。

    为了提高计算速度与精确度，需要进一步研究深度图识别、卡尔曼滤波跟踪等方法。

参考文献

[1] 初广丽，王延杰，邸男，等.复杂场景中航天器靶标的快速识别[J].光学精密工程，2016，24(4)：865-872.

[2] 徐培智，徐贵力，王彪，等.基于立体视觉的非合作目标位姿测量[J].计算机与现代化，2013(8)：85-91.

[3] 时建奇，许林.基于PMD相机的位姿测量方法研究[J].电脑知识与技术，2016，12(10)：269-271.

[4] 梁斌，何英，邹瑜，等.ToF相机在空间非合作目标近距离测量中的应用[J].宇航学报，2016，37(9)：1080-1088.

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

[6] 张旭东，李文龙，胡良梅，等.基于PMD相机的特征跟踪位姿测量方法[J].电子测量与仪器学报，2013，27(7)：640-646.

[7] 孙冬梅，裘正定.基于确定性退火技术的鲁棒性的点匹配算法[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）