CN108489496A - 基于多源信息融合的非合作目标相对导航运动估计方法及系统 - Google Patents

Abstract

一种基于多源信息融合的非合作目标相对导航运动估计方法及系统，首先给出激光与视觉测量系统的联合标定方法，进而基于联合标定后的测量系统获取激光扫描点云在相机坐标系的重投影信心，采用两次插值方法分别获取目标点的深度补偿信息和补偿后的深度信息，实现激光扫描点云与视觉相机图像的信息融合，最后在融合信息基础上获取目标的运动估计。本发明给出了视觉相机与激光扫描的信息融合方法，能够通过激光扫描点云的高精度深度信息弥补视觉图像在深度方向精度差的不足，同时又避免了激光扫描分辨率低的缺点，兼具了激光扫描深度信息精度高与视觉测量系统的图像分辨率高的优点；同时，信息融合的插值计算方法为简单的代数运算，易于工程实现与应用。

Description

基于多源信息融合的非合作目标相对导航运动估计方法及 系统

技术领域

本发明涉及一种基于多源信息融合的非合作目标相对导航运动估计方法及系统，属于导航运动估计技术领域。

背景技术

近年来，伴随着空间技术的快速发展，面向各种任务要求的航天器相继被送上太空，其可靠性和安全性也越来越受到重视。如今，在轨服务已逐渐成为延长航天器使用寿命、完成航天器设备升级以及清理太空垃圾的重要手段，也是今后进行空间探索亟待解决的重要课题，尤其是近些年发展起来的非合作航天器在轨服务技术有着广阔的应用前景。

非合作航天器在轨服务技术首先需要解决的问题就是非合作目标的近距离相对导航问题，在轨服务航天器近距离接近段，需要凭借相对导航系统全面检测和估计相对运动状态。相比于合作目标，非合作目标缺乏先验知识、不能安装人工标记、信息获取途径少，且无法进行航天器间的通信，只能利用摄像机和激光雷达等传感器获取有效信息。目前空间非合作目标相对导航运动估计的方法主要通过视觉图像或激光点云获取目标特征点的三维坐标，进而开展目标的运动估计。但是，视觉摄像与激光扫描又具有不同的特点与优势，视觉相机相比于激光扫描仪可以获得丰富的色彩、纹理、形状等场景信息，便于进行图像处理操作，且可以进行三维重构。激光扫描仪与视觉相机相比具有更高精度的深度信息，视场范围大，数据处理简单等优势，然而其只能获得离散的点云数据。

发明内容

本发明解决的技术问题为：克服现有技术的不足，提供了一种基于多源信息融合的非合作目标相对导航运动估计方法及系统。通过对视觉-激光测量系统进行联合标定后，获取视觉摄像的图像信息与三维激光扫面的点云信息，进行图像信息与点云信息融合，用于最终实现目标运动估计。

本发明解决的技术方案为：一种基于多源信息融合的非合作目标相对导航运动估计方法，步骤如下：

(1)建立双目视觉相机与三维激光扫描仪的联合标定模型；

(2)采用双目视觉相机与三维激光扫描仪的联合标定模型，将激光扫描仪坐标系下的点集转换成相机坐标系下，构造两组点集匹配的误差函数，采用ICP算法优化两组点集匹配的误差函数，实现双目相机与三维激光扫描仪联合标定；

(3)目标双目视觉相机拍摄非合作目标，在步骤(2)双目视觉相机与三维激光扫描仪联合标定得到的最优的外参数下，获取目标双目视觉相机拍摄的图像坐标与激光扫描目标形成的点云在相机坐标系下坐标；采用插值法，完成目标双目视觉相机拍摄图像与激光扫描仪扫描点云的信息融合所需的深度补偿初值；

(4)对步骤(3)的深度补偿初值进一步采用插值法，完成视觉图像的像素点的深度补偿。

(5)通过步骤(4)的结果，估计非合作目标的转动情况。

步骤(1)建立双目视觉-三维激光扫描测量系统的联合标定模型，具体步骤如下：

(1.1)设定坐标系O_c-X_cY_cZ_c为相机坐标系，O_l-X_lY_lZ_l为激光扫描仪坐标系，有空间点P在O_c-X_cY_cZ_c与O_l-X_lY_lZ_l坐标系中坐标分别为(X_c Y_c Z_c)与(X_l Y_l Z_l)；

(1.2)建立双目视觉-三维激光扫描测量系统的联合标定模型，即双目视觉坐标系(即相机坐标系)和激光扫描仪坐标系的转换关系如下：

其中：R为从激光扫描仪坐标系到相机坐标系的坐标变换矩阵；T从从激光扫描仪坐标系到相机坐标系的的平移矢量。

步骤(2)采用双目视觉-三维激光扫描测量系统的联合标定模型，将激光扫描仪坐标系下的点集转换成相机坐标系下，构造两组点集匹配的误差函数，采用ICP算法优化两组点集匹配的误差函数，实现双目相机-三维激光扫描仪联合标定，具体步骤如下：

(2.1)选择不规则几何外形、特征明显的物体作为标定物，选择标定物边界角点作为特征点，使用激光扫描仪扫描标定物获取特征点的点云，组成激光扫描仪坐标系下特征点集M_l：(m_l1 m_l2…m_ln)；

(2.2)采用双目视觉-三维激光扫描测量系统的联合标定模型，将特征点集M_l：(m_l1m_l2…m_ln)变换成相机坐标系下的特征点集M_c′：(m_c′₁ m_c′₂…m_c′_n)。

(2.3)使用双目视觉系统的左右相机对标定物拍照，采集左右目两幅图像，基于图像重构特征点集中的所有特征点在相机坐标系下的三维坐标，组成相机坐标系下的特征点集M_c：(m_c1 m_c2…m_cn)，构造两组特征点集匹配的误差函数如下：

其中：A表示最优化目标函数值；

m_ci为特征点i在相机坐标系下的坐标；i＝1,2,…n

m_li为特征点i在激光扫描仪坐标系下的坐标；

m'_ci为m_ci变换到相机坐标系下的坐标；

R为从激光扫描仪坐标系到相机坐标系的坐标变换矩阵；

T从从激光扫描仪坐标系到相机坐标系的的平移矢量。

(2.4)采用ICP算法，实现以上目标函数即两组点集匹配的误差函数的优化，当目标函数值最小时，停止ICP算法迭代，从而得到最优的外参数R与T，其中R为相机坐标系与激光扫描仪坐标系的转动关系，T为相机坐标系与激光扫描仪坐标系的位置关系，实现双目相机-三维激光扫描仪联合标定。

步骤(3)目标双目视觉相机拍摄非合作目标，在最优的外参数R与T下，获取目标双目视觉相机拍摄的图像坐标与激光扫描目标形成的点云；采用插值法，完成目标双目视觉相机拍摄图像与激光扫描仪扫描点云的信息融合所需的深度补偿初值，具体步骤如下：。

(3.1)目标双目视觉相机拍摄非合作目标，在图像平面建立二维笛卡尔坐标系，激光点云重投影在图像系中形成的重投影点l₁必然位于相机成像的某四个像素点c₁、c₂、c₃、c₄中，重投影点l₁坐标为l₁：(x_l1 y_l1 z_l1)；

(3.2)将双目视觉相机拍摄的非合作目标的两幅图像按照双目视觉系统数学模型恢复得到的像素点在相机坐标系的坐标为c₁：(x_c1 y_c1 z_c1)、c₂：(x_c2 y_c2 z_c2)、c₃：(x_c3 y_c3z_c3)、c₄：(x_c4 y_c4 z_c4)；

(3.3)采用线性插值方法，对四个像素点c₁、c₂、c₃、c₄对应深度z方向坐标插值得到l₁点的z坐标，如下表达：

其中：

(3.4)根据视觉成像的四个像素点c₁、c₂、c₃、c₄插值得到的l₁处的估计深度坐标通过与激光扫描点的重投影坐标l₁：(x_l1 y_l1 z_l1)中的z_l1进行比对，得到l₁处的深度补偿量Δz_l1，即目标双目视觉相机拍摄图像与激光扫描仪扫描点云的信息融合所需的深度补偿初值：

步骤(4)对深度补偿初值进一步采用插值法，完成视觉图像的像素点的深度补偿，具体步骤如下：

(4.1)采用步骤(3)的方法，获取所有激光扫描点l_i由视觉图像恢复点坐标插值得到的深度补偿初值，由此组成一组补偿坐标Δl_i：(x_li y_li Δz_li)。

(4.2)对任意像素点外部的四个激光扫描点l₁-l₄内部的视觉像素点c_j通过插值方法获得c_j处补偿量如下：

其中：

(4.3)视觉图像的像素点c_j对应的补偿后的深度信息即为：

其中，z_cj为像素点c_j在图像系下的深度坐标；

(4.4)获得一组融合后的目标特征点集：{{c_j}∪{l_i}}，其中所有像素点c_j经过融合补偿后的对应的坐标为：m为大于等于4的正整数；从而完成视觉图像的像素点的深度补偿；

步骤(5)通过步骤(4)的结果，估计非合作目标的转动情况，具体步骤如下：

(5.1)在初始时刻，设定目标的本体系与相机坐标系平行(即目标的本体系的x、y、z轴分别与相机坐标系x、y、z轴平行)，

(5.2)由初始时刻目标的部分特征点集{{c_j0}∪{l_i0}}，得到目标体上若干矢量的本体系投影r_0k，即三维坐标列向量，k代表特征点集{{c_j0}∪{l_i0}}中某一点k；

(5.3)当目标运动t时刻后，得到相机坐标系下此特征点集{{c_j0}∪{l_i0}}在t时刻的坐标{{c_jt}∪{l_it}}，而这些特征点得到的矢量在目标本体系中的投影仍为r_0k，但在相机坐标系中的投影为r_tk，两投影量的关系式如下：

r_tk＝R·r_0k (9)

其中：

r_0k表示目标体上矢量在目标本体坐标系上的投影，为常量；

r_tk表示t时刻目标体上矢量在相机坐标系下的投影，k为特征点集{{c_j}∪{l_i}}中的第k个点。

(5.4)对于每三个非线性相关矢量组成的矢量组，获得一个姿态矩阵R_s的解为：

R_s＝[r_t1 r_t2 r_t3]·[r₀₁ r₀₂ r₀₃]^-1 (10)

(5.5)对于若干组这样的矢量组，解得若干个姿态矩阵R_s，然后进一步采用优化方法优化，如遗传算法优化得到最优的姿态矩阵R的所有元素值的所有元素值；

(5.6)然后将步骤(5.5)得到的R代入式(9)，应用式(9)对非合作目标上所有观测点估计其运动量，即能估计出非合作目标的转动情况。

双目视觉系统采用左右目光学相机，两相机主光轴平行，空间间隔一定距离安装，此距离范围需要根据具体相机指标计算得到，单个相机成像图像像元数长宽相等，帧率大于60FPS，成像传感器尺寸小于1”，镜头采用定焦镜头，焦段范围选取：6mm、8mm、12.5mm、16mm、25mm。

采用激光扫描仪与三轴转台组成激光扫描系统，三轴转台可以实现水平方向180°和垂直方向180°扫描，从而保证扫描点云为正方形网格，保证水平和垂直扫描分辨率相等且小于1°，激光扫描仪扫描频率小于25Hz。

一种基于多源信息融合的非合作目标相对导航运动估计系统，包括：建立模块、标定模块、第一补偿模块、第二补偿模块、转动情况估计模块；

建立模块，建立双目视觉相机与三维激光扫描仪的联合标定模型；

标定模块，采用双目视觉相机与三维激光扫描仪的联合标定模型，将激光扫描仪坐标系下的点集转换成相机坐标系下，构造两组点集匹配的误差函数，采用ICP算法优化两组点集匹配的误差函数，实现双目相机与三维激光扫描仪联合标定；

第一补偿模块，目标双目视觉相机拍摄非合作目标，在标定模块的双目视觉相机与三维激光扫描仪联合标定得到的最优的外参数下，获取目标双目视觉相机拍摄的图像坐标与激光扫描目标形成的点云在相机坐标系下坐标；采用插值法，完成目标双目视觉相机拍摄图像与激光扫描仪扫描点云的信息融合所需的深度补偿初值；

第二补偿模块，对第一补偿模块的深度补偿初值进一步采用插值法，完成视觉图像的像素点的深度补偿；

转动情况估计模块，通过第二补偿模块的结果，估计非合作目标的转动情况。

本发明与现有技术相比的优点在于：

(1)本发明激光扫描仪高精度的深度信息与视觉相机丰富的场景信息具有很强的互补性，将这两种传感器获得的数据进行融合将对非合作目标的相对导航具有很大的工程价值。

(2)本发明结合我国航天工程中非合作目标的近距相对导航这一迫切工程需求，给出了一种基于多源信息融合的非合作目标相对导航运动估计方法的方案流程，进一步给出了利用插值方法的视觉与激光的信息融合方法，最后根据融合的三维信息估计目标运动，重构目标的三维实体空间运动。

(3)本发明给出了一种基于多源信息融合的非合作目标相对导航运动估计方法，可用于非合作目标相对导航运动估计。本发明采用视觉与激光两套测量系统获取信息进行融合，弥补每个单独系统测量的不足，可以获取更精确与丰富的目标信息，进一步可以提高运动估计的准确度。

(4)该方法基于视觉摄像与三维激光扫描的信息融合，传统的激光扫描点云较稀疏、分辨率较低，但是每个点的位置精度高，而视觉测量的分辨率很高、像素点密集，但是每个点的深度方向精度差，本方法能够综合视觉成像与激光扫描的优点，提高目标特征点位置测量精度，弥补激光扫描点云稀疏的缺点。

(5)该方法提出了一种基于两次插值的特征点深度误差估计与补偿，首先基于插值方法获取补偿初值，然后再基于插值方法获取所有点的最终深度补偿，方法依据简单的代数运算，理论方法易于工程实现，能够完成视觉与激光测量的信息快速融合。

附图说明

图1为本方法的流程图；

图2本发明的相机图像像素点与激光点云重投影示意图。

具体实施方式

下面结合附图和具体实施例对本发明做进一步详细描述。

本发明的基本思路为：一种基于多源信息融合的非合作目标相对导航运动估计方法及系统，首先给出视觉-激光测量系统的联合标定方法，获得两个传感器坐标系之间的相对位姿关系；进一步获取视觉摄像的图像信息与三维激光扫面的点云信息，通过插值方法进行图像信息与点云信息融合；最后通过特征点获取运动学参数进一步完成目标的三维运动估计。

双目视觉相机安装于安装台上，保证双目相机的空间位置姿态固定；激光扫描仪具有垂直两方向扫描能力，两个方向扫描角度中分线定为标称指向；根据相机的内参数确定满足精度的观测距离，即非合作目标的距离要求，同时保证双目视觉相机与激光扫描仪都对准目标，目标位于每个相机的视野中，同时激光扫描仪标称指向对准目标。

本发明给出了一种基于多源信息融合的非合作目标相对导航运动估计方法，可用于非合作目标相对导航运动估计。本发明采用视觉与激光两套测量系统获取信息进行融合，弥补每个单独系统测量的不足，可以获取更精确与丰富的目标信息，进一步可以提高运动估计的准确度。

一种基于多源信息融合的非合作目标相对导航运动估计方法，步骤如下：

(1)建立双目视觉-三维激光扫描测量系统的联合标定模型；在由双目相机和三维激光扫描仪组成的传感集成系统中，双目相机负责获取场景的二维图像信息，三维激光扫描仪负责获取三维场景的深度信息。由于二者在视场上重合，于是可以获得同一场景的图像和深度信息，从而为高精度三维重构奠定基础。

设定坐标系O_c-X_cY_cZ_c为相机坐标系，O_l-X_lY_lZ_l为激光扫描仪坐标系，有空间点P在O_c-X_cY_cZ_c与O_l-X_lY_lZ_l坐标系中坐标分别为(X_c Y_c Z_c)与(X_l Y_l Z_l)，那么两坐标系的转换关系如下，此关系即可表示激光扫描仪与双目视觉系统的位置与姿态关系：

其中：

R为从激光扫描仪坐标系到相机坐标系的坐标变换矩阵；

T从从激光扫描仪坐标系到相机坐标系的的平移矢量。

(2)采用ICP算法实现双目相机-三维激光扫描仪联合标定。

首先选择不规则几何外形特征明显的物体作为标定物，这样有利于获得匹配特征点。在确定标定物方位后选择激光扫描区域边界角点作为特征点。

然后使用激光扫描仪扫描标定物，并确定激光扫描仪坐标系下特征点集M_l：(m_l1m_l2…m_ln)。同时使用预先单独完成标定的双目视觉系统采集标定物图像，对以上预先确定的特征点重构相机坐标系下的三维坐标，组成点集M_c′：(m_c′₁ m_c′₂…m_c′_n)。

之后，构造两组点集匹配的误差函数，通过使误差函数值最小寻找到最优的旋转矩阵R与平移矢量T。

误差函数如下：

其中：

m_ci为特征点i在相机坐标系下的坐标；

m_li为特征点i在激光扫描仪坐标系下的坐标；

m'_ci为m_ci变换到相机坐标系下的坐标；

R为从激光扫描仪坐标系到相机坐标系的坐标变换矩阵；

T从从激光扫描仪坐标系到相机坐标系的的平移矢量。

最后，采用ICP算法实现以上目标函数的优化，当目标函数值最小时，停止算法迭代，从而得到最优的R与T。

(3)采用插值法完成目标双目视觉相机拍摄图像与激光扫描仪扫描点云的信息融合。按照双目视觉成像的原理可以得到目标成像后所有图像像素点的三维空间坐标，这些点坐标以其中一相机坐标系为参考描述；同时，双目视觉-三维激光扫描测量系统经过联合标定后，可以较精确的将激光扫描点云通过转换关系式可以重投影到此相机坐标系中，相机图像像素点与激光点云重投影后示意如图1所示。由于相机成像与激光扫描的特点，所以激光扫描点云的分辨率比相机图像低，点云更加稀疏。

激光点云在图像系中重投影点l₁必然位于相机成像的某四个图像点c₁、c₂、c₃、c₄中，如图2所示。重投影点坐标为l₁：(x_l1 y_l1 z_l1)，由视觉图像恢复的图像点坐标为c₁：(x_c1y_c1 z_c1)、c₂：(x_c2 y_c2 z_c2)、c₃：(x_c3 y_c3 z_c3)、c₄：(x_c4 y_c4 z_c4)。

由于视觉成像在xy方向精度较好，而在深度z方向精度较差，那么采用线性插值方法，对图像所有像素点对应深度z方向坐标作为重构目标，通过x和y方向坐标建立插值权重。通过插值得到l₁点的z坐标如下表达：

其中：

综上，由视觉成像的四个像素点插值得到了l₁处的估计深度坐标通过与激光扫描点的重投影坐标进行比对，得到l₁处的深度补偿量Δz_l1：

首先基于插值方法获取补偿初值，然后再基于插值方法获取所有点的最终深度补偿，方法依据简单的代数运算，理论方法易于工程实现，能够完成视觉与激光测量的信息快速融合。

(4)进一步采用插值法完成视觉图像的像素点的深度补偿。按照步骤(3)，可以对所有激光扫描点得到此位置由视觉像素点插值的深度补偿量，由此得到一组补偿坐标Δl_i：(x_li y_li Δz_li)，其中x_li、y_li表示位置信息，用于获得插值权重，Δz_li为此位置补偿量，那么对任意四个激光扫描点l₁～l₄，如图2，对其内部的视觉图像点通过补偿坐标Δl₁～Δl₄插值进行补偿，依然采用式(3)、～(5)的插值方法，可以获得c_j处补偿量如下：

其中：

那么，视觉图像的像素点c_j对应的补偿后的深度信息即为：

至此，通过本方法获得一组融合后的目标特征点集：{{c_j}∪{l_i}}，其中所有像素点c_j经过融合补偿后的对应的坐标为：

(5)通过若干特征点组估计目标转动情况。对于任一姿态旋转矩阵，可以表示一空间矢量在不同坐标系的变换关系，表达式如下：

r_t＝R·r₀ (10)

R即表示了矢量由0坐标系变换到t坐标系的变换关系。

如果在初始时刻，设定目标的本体系即与相机坐标系是平行的，那么由此时刻目标的部分特征点集{{c_j0}∪{l_i0}}可以得到目标体上若干矢量投影r_0k，即在目标本体坐标系下描述目标上某特征点k的坐标。

当目标运动t时刻后，得到相机坐标系下此特征点集t时刻的坐标{{c_jt}∪{l_it}}，那么由这些特征点得到的矢量在目标本体系中的投影仍为r_0k，而在相机坐标系的投影为r_tk。

其中：

r_0k表示初始时刻目标体上矢量在相机坐标系下的投影，也表示t时刻在目标本体坐标系上的投影；

r_tk表示t时刻目标体上矢量在相机坐标系下的投影。

那么，对于每三个非线性相关矢量可以获得一个姿态矩阵R_s的解为：

R_s＝[r_t1 r_t2 r_t3]·[r₀₁ r₀₂ r₀₃]^-1 (12)

对于若干组这样的矢量组，可以解得若干个姿态矩阵R_s，此处可进一步采用遗传算法优化得到最优的姿态矩阵然后应用式(11)可以对初始时刻观测到的目标上任一观测点估计其t时刻后的运动量。

本发明的一种基于多源信息融合的非合作目标相对导航运动估计系统，包括：建立模块、标定模块、第一补偿模块、第二补偿模块、转动情况估计模块；

建立模块，建立双目视觉相机与三维激光扫描仪的联合标定模型；

标定模块，采用双目视觉相机与三维激光扫描仪的联合标定模型，将激光扫描仪坐标系下的点集转换成相机坐标系下，构造两组点集匹配的误差函数，采用ICP算法优化两组点集匹配的误差函数，实现双目相机与三维激光扫描仪联合标定；

第一补偿模块，目标双目视觉相机拍摄非合作目标，在标定模块的双目视觉相机与三维激光扫描仪联合标定得到的最优的外参数下，获取目标双目视觉相机拍摄的图像坐标与激光扫描目标形成的点云在相机坐标系下坐标；采用插值法，完成目标双目视觉相机拍摄图像与激光扫描仪扫描点云的信息融合所需的深度补偿初值；

第二补偿模块，对第一补偿模块的深度补偿初值进一步采用插值法，完成视觉图像的像素点的深度补偿；

转动情况估计模块，通过第二补偿模块的结果，估计非合作目标的转动情况。

建立模块，建立双目视觉-三维激光扫描测量系统的联合标定模型，具体步骤如下：

(1.1)设定坐标系O_c-X_cY_cZ_c为相机坐标系，O_l-X_lY_lZ_l为激光扫描仪坐标系，有空间点P在O_c-X_cY_cZ_c与O_l-X_lY_lZ_l坐标系中坐标分别为(X_c Y_c Z_c)与(X_l Y_l Z_l)；

(1.2)建立双目视觉-三维激光扫描测量系统的联合标定模型，即双目视觉坐标系(即相机坐标系)和激光扫描仪坐标系的转换关系如下：

其中：R为从激光扫描仪坐标系到相机坐标系的坐标变换矩阵；T从从激光扫描仪坐标系到相机坐标系的的平移矢量。

标定模块，采用双目视觉-三维激光扫描测量系统的联合标定模型，将激光扫描仪坐标系下的点集转换成相机坐标系下，构造两组点集匹配的误差函数，采用ICP算法优化两组点集匹配的误差函数，实现双目相机-三维激光扫描仪联合标定，具体步骤如下：

(2.1)选择不规则几何外形、特征明显的物体作为标定物，选择标定物边界角点作为特征点，使用激光扫描仪扫描标定物获取特征点的点云，组成激光扫描仪坐标系下特征点集M_l：(m_l1 m_l2 … m_ln)；

(2.2)采用双目视觉-三维激光扫描测量系统的联合标定模型，将特征点集M_l：(m_l1m_l2 … m_ln)变换成相机坐标系下的特征点集M′_c：(m′_c1 m′_c2…m′_cn)。

(2.3)使用双目视觉系统的左右相机对标定物拍照，采集左右目两幅图像，基于图像重构特征点集中的所有特征点在相机坐标系下的三维坐标，组成相机坐标系下的特征点集M_c：(m_c1 m_c2 … m_cn)，构造两组特征点集匹配的误差函数如下：

其中：A表示最优化目标函数值；

m_ci为特征点i在相机坐标系下的坐标；i＝1,2,…n

m_li为特征点i在激光扫描仪坐标系下的坐标；

m'_ci为m_ci变换到相机坐标系下的坐标；

R为从激光扫描仪坐标系到相机坐标系的坐标变换矩阵；

T从从激光扫描仪坐标系到相机坐标系的的平移矢量。

(2.4)采用ICP算法，实现以上目标函数即两组点集匹配的误差函数的优化，当目标函数值最小时，停止ICP算法迭代，从而得到最优的外参数R与T，其中R为相机坐标系与激光扫描仪坐标系的转动关系，T为相机坐标系与激光扫描仪坐标系的位置关系，实现双目相机-三维激光扫描仪联合标定。

第一补偿模块，控制目标双目视觉相机拍摄非合作目标，在最优的外参数R与T下，获取目标双目视觉相机拍摄的图像坐标与激光扫描目标形成的点云；采用插值法，完成目标双目视觉相机拍摄图像与激光扫描仪扫描点云的信息融合所需的深度补偿初值，具体步骤如下：。

(3.1)目标双目视觉相机拍摄非合作目标，在图像平面建立二维笛卡尔坐标系，激光点云重投影在图像系中形成的重投影点l₁必然位于相机成像的某四个像素点c₁、c₂、c₃、c₄中，重投影点l₁坐标为l₁：(x_l1 y_l1 z_l1)；

(3.2)将双目视觉相机拍摄的非合作目标的两幅图像按照双目视觉系统数学模型恢复得到的像素点在相机坐标系的坐标为c₁：(x_c1 y_c1 z_c1)、c₂：(x_c2 y_c2 z_c2)、c₃：(x_c3 y_c3z_c3)、c₄：(x_c4 y_c4 z_c4)；

(3.3)采用线性插值方法，对四个像素点c₁、c₂、c₃、c₄对应深度z方向坐标插值得到l₁点的z坐标，如下表达：

其中：

(3.4)根据视觉成像的四个像素点c₁、c₂、c₃、c₄插值得到的l₁处的估计深度坐标通过与激光扫描点的重投影坐标l₁：(x_l1 y_l1 z_l1)中的z_l1进行比对，得到l₁处的深度补偿量Δz_l1，即目标双目视觉相机拍摄图像与激光扫描仪扫描点云的信息融合所需的深度补偿初值：

第二补偿模块，对深度补偿初值进一步采用插值法，完成视觉图像的像素点的深度补偿，具体步骤如下：

(4.1)采用第一补偿模块的方法，获取所有激光扫描点l_i由视觉图像恢复点坐标插值得到的深度补偿初值，由此组成一组补偿坐标Δl_i：(x_li y_li Δz_li)。

(4.2)对任意像素点外部的四个激光扫描点l₁-l₄内部的视觉像素点c_j通过插值方法获得c_j处补偿量如下：

其中：

(4.3)视觉图像的像素点c_j对应的补偿后的深度信息即为：

其中，z_cj为像素点c_j在图像系下的深度坐标；

(4.4)获得一组融合后的目标特征点集：{{c_j}∪{l_i}}，其中所有像素点c_j经过融合补偿后的对应的坐标为：m为大于等于4的正整数；从而完成视觉图像的像素点的深度补偿。

转动情况估计模块，通过第二补偿模块的结果，估计非合作目标的转动情况，具体步骤如下：

(5.1)在初始时刻，设定目标的本体系与相机坐标系平行(即目标的本体系的x、y、z轴分别与相机坐标系x、y、z轴平行)，

(5.2)由初始时刻目标的部分特征点集{{c_j0}∪{l_i0}}，得到目标体上若干矢量的本体系投影r_0k，即三维坐标列向量，k代表特征点集{{c_j0}∪{l_i0}}中某一点k；

(5.3)当目标运动t时刻后，得到相机坐标系下此特征点集{{c_j0}∪{l_i0}}在t时刻的坐标{{c_jt}∪{l_it}}，而这些特征点得到的矢量在目标本体系中的投影仍为r_0k，但在相机坐标系中的投影为r_tk，两投影量的关系式如下：

r_tk＝R·r_0k (9)

其中：

r_0k表示目标体上矢量在目标本体坐标系上的投影，为常量；

r_tk表示t时刻目标体上矢量在相机坐标系下的投影，k为特征点集{{c_j}∪{l_i}}中的第k个点。

(5.4)对于每三个非线性相关矢量组成的矢量组，获得一个姿态矩阵R_s的解为：

R_s＝[r_t1 r_t2 r_t3]·[r₀₁ r₀₂ r₀₃]^-1 (10)

(5.5)对于若干组这样的矢量组，解得若干个姿态矩阵R_s，然后进一步采用优化方法优化，如遗传算法优化得到最优的姿态矩阵R的所有元素值的所有元素值；

(5.6)然后将步骤(5.5)得到的R代入式(9)，应用式(9)对非合作目标上所有观测点估计其运动量，即能估计出非合作目标的转动情况。

双目视觉系统采用左右目光学相机，两相机主光轴平行，空间间隔一定距离安装，此距离范围需要根据具体相机指标计算得到，单个相机成像图像像元数长宽相等，帧率大于60FPS，成像传感器尺寸小于1”，镜头采用定焦镜头，焦段范围优选选取：6mm、8mm、12.5mm、16mm、25mm。

本发明激光扫描仪高精度的深度信息与视觉相机丰富的场景信息具有很强的互补性，将这两种传感器获得的数据进行融合将对非合作目标的相对导航具有很大的工程价值，

结合我国航天工程中非合作目标的近距相对导航这一迫切工程需求，给出了一种基于多源信息融合的非合作目标相对导航运动估计方法的方案流程，进一步给出了利用插值方法的视觉与激光的信息融合方法，最后根据融合的三维信息估计目标运动，重构目标的三维实体空间运动。

本发明的系统和方法基于视觉摄像与三维激光扫描的信息融合，能够综合视觉成像与激光扫描的优点，提高目标特征点位置测量精度，弥补激光扫描点云稀疏的缺点。

Claims (10)

1.一种基于多源信息融合的非合作目标相对导航运动估计方法，其特征在于步骤如下：

(1)建立双目视觉相机与三维激光扫描仪的联合标定模型；

(2)采用双目视觉相机与三维激光扫描仪的联合标定模型，将激光扫描仪坐标系下的点集转换成相机坐标系下，构造两组点集匹配的误差函数，采用ICP算法优化两组点集匹配的误差函数，实现双目相机与三维激光扫描仪联合标定；

(3)目标双目视觉相机拍摄非合作目标，在步骤(2)双目视觉相机与三维激光扫描仪联合标定得到的最优的外参数下，获取目标双目视觉相机拍摄的图像坐标与激光扫描目标形成的点云在相机坐标系下坐标；采用插值法，完成目标双目视觉相机拍摄图像与激光扫描仪扫描点云的信息融合所需的深度补偿初值；

(4)对步骤(3)的深度补偿初值进一步采用插值法，完成视觉图像的像素点的深度补偿；

(5)通过步骤(4)的结果，估计非合作目标的转动情况。

2.根据权利要求1所述的一种基于多源信息融合的非合作目标相对导航运动估计方法，其特征在于步骤(1)建立双目视觉-三维激光扫描测量系统的联合标定模型，具体步骤如下：

(1.1)设定坐标系O_c-X_cY_cZ_c为相机坐标系，O_l-X_lY_lZ_l为激光扫描仪坐标系，有空间点P在O_c-X_cY_cZ_c与O_l-X_lY_lZ_l坐标系中坐标分别为(X_c Y_c Z_c)与(X_l Y_l Z_l)；

(1.2)建立双目视觉-三维激光扫描测量系统的联合标定模型，即双目视觉坐标系和激光扫描仪坐标系的转换关系如下：

其中：R为从激光扫描仪坐标系到相机坐标系的坐标变换矩阵；T从从激光扫描仪坐标系到相机坐标系的的平移矢量。

3.根据权利要求1所述的一种基于多源信息融合的非合作目标相对导航运动估计方法，其特征在于步骤(2)采用双目视觉-三维激光扫描测量系统的联合标定模型，将激光扫描仪坐标系下的点集转换成相机坐标系下，构造两组点集匹配的误差函数，采用ICP算法优化两组点集匹配的误差函数，实现双目相机-三维激光扫描仪联合标定，具体步骤如下：

(2.1)选择不规则几何外形、特征明显的物体作为标定物，选择标定物边界角点作为特征点，使用激光扫描仪扫描标定物获取特征点的点云，组成激光扫描仪坐标系下特征点集M_l：(m_l1 m_l2 … m_ln)；

(2.2)采用双目视觉-三维激光扫描测量系统的联合标定模型，将特征点集M_l：(m_l1 m_l2… m_ln)变换成相机坐标系下的特征点集M′_c：(m′_c1 m′_c2 … m′_cn)；

(2.3)使用双目视觉相机对标定物拍照采集左右目两幅图像，基于图像重构特征点集中的所有特征点在相机坐标系下的三维坐标，组成相机坐标系下的特征点集M_c：(m_c1 m_c2 …m_cn)，构造两组特征点集匹配的误差函数如下：

其中：A表示最优化目标函数值；

m_ci为特征点i在相机坐标系下的坐标，i＝1,2,…n；

m_li为特征点i在激光扫描仪坐标系下的坐标；

m'_ci为m_ci变换到相机坐标系下的坐标；

R为从激光扫描仪坐标系到相机坐标系的坐标变换矩阵；

T从从激光扫描仪坐标系到相机坐标系的的平移矢量；

(2.4)采用ICP算法，实现以上目标函数即两组点集匹配的误差函数的优化，当目标函数值最小时，停止ICP算法迭代，从而得到最优的外参数R与T，实现双目相机-三维激光扫描仪联合标定。

4.根据权利要求1所述的一种基于多源信息融合的非合作目标相对导航运动估计方法，其特征在于：步骤(3)目标双目视觉相机拍摄非合作目标，在最优的外参数R与T下，获取目标双目视觉相机拍摄的图像坐标与激光扫描目标形成的点云；采用插值法，完成目标双目视觉相机拍摄图像与激光扫描仪扫描点云的信息融合所需的深度补偿初值，具体步骤如下：

(3.1)目标双目视觉相机拍摄非合作目标，激光点云重投影在图像系中形成的重投影点l₁必然位于相机成像的某四个像素点c₁、c₂、c₃、c₄中，重投影点l₁在相机坐标系中的坐标为l₁：(x_l1 y_l1 z_l1)；

(3.2)将双目视觉相机拍摄的非合作目标的两幅图像恢复得到的像素点在相机坐标系的坐标为c₁：(x_c1 y_c1 z_c1)、c₂：(x_c2 y_c2 z_c2)、c₃：(x_c3 y_c3 z_c3)、c₄：(x_c4 y_c4 z_c4)；

(3.3)采用线性插值方法，对四个像素点c₁、c₂、c₃、c₄对应深度z方向坐标插值得到l₁点的z坐标，如下表达：

其中：

(3.4)根据视觉成像的四个像素点c₁、c₂、c₃、c₄插值得到的l₁处的估计深度坐标通过与激光扫描点的重投影坐标l₁：(x_l1 y_l1 z_l1)中的z_l1进行比对，得到l₁处的深度补偿量Δz_l1，即目标双目视觉相机拍摄图像与激光扫描仪扫描点云的信息融合所需的深度补偿初值：

5.根据权利要求1所述的一种基于多源信息融合的非合作目标相对导航运动估计方法，其特征在于：步骤(4)对深度补偿初值进一步采用插值法，完成视觉图像的像素点的深度补偿，具体步骤如下：

(4.1)采用步骤(3)的方法，获取所有激光扫描点l_i由视觉像素点插值得到的深度补偿初值，由此组成一组补偿坐标Δl_i：(x_li y_li Δz_li)；

(4.2)对任意像素点外部的四个激光扫描点l₁ˉl₄内部的视觉像素点c_j通过插值方法获得c_j处补偿量如下：

其中：

(4.3)视觉图像的像素点c_j对应的补偿后的深度信息即为：

其中，z_cj为像素点c_j在图像系下的深度坐标；

(4.4)获得一组融合后的目标特征点集：{{c_j}∪{l_i}}，其中所有像素点c_j经过融合补偿后的对应的坐标为：j＝1,2…m，m为大于等于4的正整数；从而完成视觉图像的像素点的深度补偿。

6.根据权利要求1所述的一种基于多源信息融合的非合作目标相对导航运动估计方法，其特征在于：步骤(5)通过步骤(4)的结果，估计非合作目标的转动情况，具体步骤如下：

(5.1)在初始时刻，设定目标的本体系与相机坐标系平行(即目标的本体系的x、y、z轴分别与相机坐标系x、y、z轴平行)；

(5.2)由初始时刻目标的部分特征点集{{c_j0}∪{l_i0}}，得到目标体上若干矢量的本体系投影r_0k；

(5.3)当目标运动t时刻后，得到相机坐标系下此特征点集{{c_j0}∪{l_i0}}在t时刻的坐标{{c_jt}∪{l_it}}，由这些特征点得到的矢量在目标本体系中的投影仍为r_0k，而在相机坐标系中的投影为r_tk，公式如下：

其中：

r_0k表示目标体上矢量在目标本体坐标系上的投影，为常量；

r_tk表示t时刻目标体上矢量在相机坐标系下的投影，k为特征点集{{c_j}∪{l_i}}中的第k个点；

(5.4)对于每三个非线性相关矢量组成的矢量组，获得一个姿态矩阵R_s的解为：

R_s＝[r_t1 r_t2 r_t3]·[r₀₁ r₀₂ r₀₃]^-1 (10)

(5.5)对于若干组这样的矢量组，解得若干个姿态矩阵R_s，然后进一步采用优化方法优化得到最优的姿态矩阵

(5.6)然后将步骤(5.5)得到的R代入式(9)，应用式(9)对非合作目标上所有观测点估计其运动量，即能估计出非合作目标的转动情况。

7.根据权利要求1所述的一种基于多源信息融合的非合作目标相对导航运动估计方法，其特征在于：双目视觉系统采用左右目光学相机，两相机主光轴平行，空间间隔一定距离安装，此距离范围需要根据具体相机指标计算得到。

8.根据权利要求1所述的一种基于多源信息融合的非合作目标相对导航运动估计方法，其特征在于：双目视觉系统采用左右目光学相机，单个相机成像图像像元数长宽相等，帧率大于60FPS，成像传感器尺寸小于1”，镜头采用定焦镜头，焦段范围选取：6mm、8mm、12.5mm、16mm、25mm。

9.根据权利要求1所述的一种基于多源信息融合的非合作目标相对导航运动估计方法，其特征在于：采用激光扫描仪与三轴转台组成激光扫描系统，三轴转台可以实现水平方向180°和垂直方向180°扫描，从而保证扫描点云为正方形网格，保证水平和垂直扫描分辨率相等且小于1°，激光扫描仪扫描频率小于25Hz。

10.一种基于多源信息融合的非合作目标相对导航运动估计系统，其特征在于包括：建立模块、标定模块、第一补偿模块、第二补偿模块、转动情况估计模块；

建立模块，建立双目视觉相机与三维激光扫描仪的联合标定模型；

标定模块，采用双目视觉相机与三维激光扫描仪的联合标定模型，将激光扫描仪坐标系下的点集转换成相机坐标系下，构造两组点集匹配的误差函数，采用ICP算法优化两组点集匹配的误差函数，实现双目相机与三维激光扫描仪联合标定；

第一补偿模块，控制目标双目视觉相机拍摄非合作目标，在标定模块的双目视觉相机与三维激光扫描仪联合标定得到的最优的外参数下，获取目标双目视觉相机拍摄的图像坐标与激光扫描目标形成的点云在相机坐标系下坐标；采用插值法，完成目标双目视觉相机拍摄图像与激光扫描仪扫描点云的信息融合所需的深度补偿初值；

第二补偿模块，对第一补偿模块的深度补偿初值进一步采用插值法，完成视觉图像的像素点的深度补偿；

转动情况估计模块，通过第二补偿模块的结果，估计非合作目标的转动情况。