CN105976353A - 基于模型和点云全局匹配的空间非合作目标位姿估计方法 - Google Patents

Abstract

本发明公开了一种基于模型和点云全局匹配的空间非合作目标位姿估计方法，该方法包括：利用深度相机获取目标场景点云，经过滤波后作为待配准数据点云，对目标模型点云进行三维距离变换；对初始待配准数据点云和目标模型点云进行去模糊主方向变换，确定平移域，利用全局ICP算法在平移域和旋转域中进行搜索配准，获取模型坐标系到相机坐标系的初始变换矩阵，即目标的初始位姿；将前一帧的位姿变换矩阵作用于当前帧数据点云，利用ICP算法与模型进行配准，获取当前帧的位姿；从位姿变换矩阵中解算出旋转角和平移量。本发明具有良好的抗噪声性能和实时输出目标位姿的能力，不需要计算数据点云的法线、曲率等几何特征，配准速度快，精度高。

Description

基于模型和点云全局匹配的空间非合作目标位姿估计方法

技术领域

本发明涉及空间非合作目标位姿获取技术，特别是一种基于模型和点云全局匹配的空间非合作目标位姿估计方法。

背景技术

空间目标相对位置和姿态参数的测量是航天器间实现相对导航的重要技术之一，通常采用的测量传感器包括单目视觉、双目视觉、雷达、GPS、惯性导航等，基于视觉的位姿测量技术具有构造简单、性能稳定、抗干扰性好等优点，一直是研究的热点问题。在近距离阶段，基于光学成像和图像处理的方法优于其他方法。

空间非合作目标泛指不能够提供有效合作信息的空间航天器，它们的结构、尺寸以及运动信息完全未知或部分未知，且不能与追踪航天器进行通信。对于大部分空间任务涉及到的目标来说，它们都是非合作的目标，如需要移至遗弃轨道或进行抓捕的废弃卫星或轨道碎片，等待维修或更换故障部件的在轨航天器等。

空间非合作目标视觉位姿测量技术有着广泛的应用前景，如故障卫星的清理和维修、空间碎片清理、及空间攻防中对敌方卫星进行监测和跟踪等，同时可应用于工业领域中，如机器视觉工件自动抓取等，具有重要的经济价值和社会效益。

现有的空间非合作目标位姿测量方法主要采用双目视觉的方法，例如徐文福等人在《非合作航天器的相对位姿测量》中提出了基于立体视觉的位姿测量方法，利用Canny边沿检测器及图像Hough变换，提取待识别对象的直线特征，并计算直线间的交点；对所提取的左、右相机图像的点特征进行3D重构，得到各点在世界坐标系中的坐标，并据此建立目标坐标系，求出其相对于世界坐标系的位置和姿态。由于该方法需要识别非合作目标的几何特征，而视场内目标的成像有可能不存在有效的几何特征，因此不能保证建立出合法有效的目标坐标系，方法可靠性差，并且该方法计算量大，不具备实时性。

发明内容

本发明的目的在于提供一种基于模型和点云全局匹配的空间非合作目标位姿估计方法。

实现本发明目的的技术解决方案为：一种基于模型和点云全局匹配的空间非合作目标位姿估计方法，包括下列步骤：

步骤1，利用深度相机获取目标场景点云，经过滤波后作为待配准数据点云，对目 标模型点云进行三维距离变换；

步骤2，对初始待配准数据点云和目标模型点云进行去模糊主方向变换，确定平移域，利用全局ICP算法在平移域和[-π,π]³旋转域中进行搜索配准，获取模型坐标系到相机坐标系的初始变换矩阵，即目标的初始位姿；

步骤3，将前一帧的位姿变换矩阵作用于当前帧数据点云，利用ICP算法与模型进行配准，获取当前帧的位姿；

步骤4，从位姿变换矩阵中解算出旋转角和平移量。

本发明与现有技术相比，其显著效果为：(1)本发明采用深度相机，而不是传统的可见光相机，更能适应光照条件苛刻的空间环境，同时深度相机可直接获取目标的三维信息，无扫描，帧频高；(2)本发明采用全局ICP算法可以准确可靠的获取目标的初始位姿信息，适应噪声大、深度相机分辨率低的情况；(3)本发明利用基本ICP算法进行连续位姿估计，将前一帧的位姿结果作为下一帧ICP算法获取位姿时的初配准结果，可以快速、连续的获取目标的位姿信息，可靠性高，速度快。

附图说明

图1是本发明基于模型和点云全局匹配的空间非合作目标位姿估计方法的流程图。

图2是本发明基于模型和点云全局匹配的初始位姿估计的流程图。

图3是本发明基于模型和点云全局匹配的连续位姿估计的流程图。

图4是本发明实施例中仿真数据点云和模型点云及其配准示意图，灰度较亮的为模型，灰度较暗的为数据点云。

图5是本发明实施例中实拍数据点云和模型点云及其配准示意图，灰度较亮的为模型，灰度较暗的为数据点云。

图6是本发明实施例中仿真点云位姿测量误差曲线图，图6(a)为平移量误差曲线图，图6(b)为旋转量误差曲线图。

图7是本发明实施例中实拍点云静态位姿测量误差曲线图，图7(a)为平移量误差曲线图，图7(b)为旋转量误差曲线图。

图8是本发明实施例中实拍点云动态位姿测量误差曲线图，图8(a)为平移量误差曲线图，图8(b)为旋转量误差曲线图。

具体实施方式

如图1、图2所示，本发明基于模型和点云全局匹配的空间非合作目标位姿估计方法，包括以下步骤：

步骤1，基于深度相机的数据点云获取、滤波及模型预处理：利用深度相机获取目标场景点云，经过滤波后作为待配准数据点云，对目标模型点云进行三维距离变换；其中滤波采用幅值滤波和统计滤波相结合的方式：

首先，从目标场景点云中去除幅值小于设定阈值的点；

其次，采用统计滤波的方法进一步去除噪声，具体包括：

对数据点云中的每个点x_i，计算它到它的k个近邻点的平均距离计算全局平均距离以及标准差φ：

其中n为数据点云的点数，1≤i≤n；

平均距离在标准范围d之外的点，判定为噪声点，从数据点云中去除；其中标准范围d为：

式中λ为标准差系数，0≤λ≤1。

步骤2，基于全局ICP配准的初始位姿估计：对初始待配准数据点云和目标模型点云进行去模糊主方向变换，确定平移域，利用全局ICP算法在平移域和[-π,π]³旋转域中进行搜索配准，获取模型坐标系到相机坐标系的初始变换矩阵，即目标的初始位姿；如图2所示，步骤2具体包括以下步骤：

步骤2-1、对初始待配准数据点云和目标模型点云进行去模糊主方向变换，确定平移域，包括以下步骤：

步骤2-1-1、首先对目标模型点云和初始待配准数据点云利用点云的一阶矩和二阶矩进行主方向变换：

点云的一阶矩和二阶矩cov分别为：

其中x_i是点云中的一点；

以均值为原点，点云协方差矩阵cov的三个按特征值升序排列的特征向量(ev₁,ev₂,ev₃)依次对应XYZ轴，重新建立点云的坐标系；

对点云进行以下旋转平移变换：

x_i'＝R^-1x_i-R^-1T

其中R＝(ev₁,ev₂,ev₃)^T，x_i'为x_i变换后的对应点，x_i'组成的点云为主方向点云；

步骤2-1-2、对主方向点云进行去模糊处理，使数据主方向点云的朝向与模型主方向点云的朝向一致；具体为：

首先，将点云协方差矩阵的三个特征向量按特征值大小升序排列，依次为x⁺，y⁺和z⁺轴，其对应的相反方向为x^-，y^-和z^-轴，去模糊x轴定义为：

对z轴进行同样的过程得到去模糊的z轴，y轴通过z×x获得；

其次，计算去模糊主方向点云：

记初始数据点云D₀和目标模型点云M₀的去模糊主方向变换分别为P_D和P_M：

其中R_D、R_M分别为点云D₀、M₀协方差矩阵去模糊特征向量构成的旋转矩阵，T_D、 T_M分别为点云D₀、M₀的均值构成的平移向量；经过P_D、P_M变换生成的去模糊主方向数据点云和去模糊主方向模型点云分别记为D₁、M₁；

步骤2-1-3、对D₁、M₁分别求解平行于坐标轴的包围盒；记去模糊主方向数据点云包围盒为D，去模糊主方向模型点云包围盒记为M，记D的中心点为O_D，XYZ三轴方向的边长分别为l_x、l_y和l_z；记M的中心点为O_M，XYZ三轴方向的边长分别为l_x'、l_y'和l_z'；将去模糊主方向数据点云的原点移到其包围盒的中心，得到调整的去模糊主方向数据点云p_t：

p_t＝x_i-O_D

调整的去模糊主方向数据点云作为最终用来配准的数据点云，记为D₂，同理调整的去模糊主方向模型点云，记为M₂；

以x轴方向为例，D₂到M₂的平移域为：

x_trans＝{x:-(|l'_x-l_x|/2+x_offest)≤x≤(|l'_x-l_x|/2+x_offest)}

y轴和z轴的平移域同理可得：

y_trans＝{y:-l'_y-l_y|/2+y_offest)≤y≤(|l'_y-l_y|/2+y_offest)}

z_trans＝{z:-(|l'_z-l_z|/2+z_offxst)≤z≤(|l'_z-l_z|/2+z_offest)}

基于长方体平移域的平移不确定半径γ_t为：

式中：l、w和h分别为长方体平移域的长、宽和高，t为平移域中任意一点，x₀为平移域中心；，x_offest、y_offest和z_offest为位移补偿，取x_offest为l_x的5％，y_offest为l_y的5％，z_offest为l_z的5％；

步骤2-2、利用全局ICP算法对点云D₂和M₂进行全局搜索配准，获取目标的初始位姿；

首先，计算搜索域C_r×C_t的配准误差上界和下界E，分别为：

式中：C_r为角轴形式表示的立方体旋转域,C_t为立方体平移域，为旋转矩阵，r₀为C_r中的一个向量，t₀为C_t中的平移量，是每一个点的L²误差，n为数据集点数，和γ_t分别为旋转不确定半径和平移不确定半径：

其中σ_r和σ_t分别为平移子立方体的半边长和旋转子立方体的半边长，x_i为数据点集里一点；

记点云D₂和M₂的配准结果为P_DM，则最终配准结果P_f为：

其中E为单位矩阵；

则模型坐标系到相机坐标系的初始位姿为P_f ^-1。

步骤3，基于ICP算法的连续位姿估计：将前一帧的位姿变换矩阵作用于当前帧数据点云，利用ICP算法与模型进行配准，获取当前帧的位姿，形成连续位姿估计；如图3所示，步骤3具体为：

将前一帧的位姿变换矩阵作为初配准结果，作用于当前帧数据点云：

D′＝P_k-1D_k

式中：D_k为当前数据点云，P_k-1为前一帧位姿结果，D′为初配准之后的数据点云；

然后利用基本ICP算法与模型点云进行配准，基本ICP算法配准误差 E_ICP(a,D,M)计算公式为：

其中，D为数据点云，M为模型点云，a＝(R,t)，n为数据点云点数，m_j为d_i对应M中的欧氏距离最近点，d_i∈D，R、t分别为旋转矩阵和平移向量；

对数据点云中的每一点，在目标模型点云中查找欧氏距离最近点，形成对应点集，根据对应点集计算刚体变换矩阵，然后将刚体变换矩阵作用于当前数据点云，重复上述过程，直到算法收敛，获得当前位姿矩阵P_temp，从而获得连续的位姿P_k：

P_k＝P_temp

步骤4，从位姿变换矩阵中解算出旋转角和平移量，具体为：

X，Y，Z三轴分别主动旋转的坐标变换矩阵为：

3*3旋转矩阵R记如下形式：

姿态角按的顺序进行转换，目标坐标系作为静止坐标系，相机相对目标坐标系的旋转矩阵为：

其中，-180≤θ≤180，-180≤φ≤180

记平移量为：

T＝[x,y,z]'

则位姿变换矩阵pose_mat为：

每个旋转矩阵可求出两组解，分别为和

旋转角为和中模长较小的一组。

下面结合附图和具体实施例对本发明作进一步说明。

实施例

为了对本发明进行说明，充分展现出该方法具有连续、准确获取目标位姿的性能和对相机低分辨率及噪声的适应性，完成位姿测量实验如下：

(1)实验初始条件及参数设置

仿真实验采用虚拟相机对模型点云拍摄生成数据点云；虚拟相机的参数设置与实际SR4000相机一致，即分辨率为144*176，焦距10mm，像素尺寸0.04mm。在10m处开始拍摄模型点云。相机点云的位姿设置为：延Z轴平移10m，旋转量为绕Z轴从-180°开始以10°间隔增加到180°，其余量为0。

误差计算为：模拟产生相机点云的参数作为位姿真实值，仿真实验计算出的位姿结果为测量值，计算误差为测量值减去真实值。

实际实验分为静态实验和动态实验，静态实验是指目标模拟器不动，测量静止位姿并与真实位姿比较，得出静态误差。动态实验是指目标模拟器连续运动，对目标进行连续位姿估计并与真实位姿比较，得出动态位姿误差。实际实验采用SR4000相机对模拟器实拍产生数据点云，静态实验时目标模拟器静止在6m处，动态实验时模拟器从6m位置处开始，以0.5m间隔接近模拟器。

(2)实验结果分析

图4为仿真点云示意图，灰度较亮的为目标模型点云，灰度较暗的为目标仿真数据点云，图4右侧为配准结果，数据点云和目标点云配准重合，表明位姿估计结果良好；图5暗灰色的为深度相机实拍的目标数据点云，灰度较亮的为目标模型点云，同样，配准结果良好，获得了精确的位姿估计结果。图6(a)为仿真实验得出的平移量误差结果，图6(b)为仿真实验得出的旋转角误差结果，从图中可以看出，平移量误差为毫米级，旋转量误差小于0.1度，仿真实验精度很高。图7(a)为实际静态实验得出的旋转角结果曲线图，图7(b)为实际静态实验得出的平移量结果曲线图，从图中可以看出，旋转角结果在真值附近有1°左右的误差，而平移量很稳定，几乎没有误差。图8为动态测量误差图，其中图8(a)为旋转角误差，图8(b)为平移量误差，从图中可以看出，旋转角误差和平移量误差均很小，小于0.1度和0.1cm，动态测量精度高，满足实际应用要求。虽然相机分辨率较低，实拍点云比较稀疏，并且也存在大量噪声，但本发明方法仍然能准确、鲁棒的估计出目标的位姿。

Claims (5)

1.一种基于模型和点云全局匹配的空间非合作目标位姿估计方法，其特征在于，包括下列步骤：

步骤1，利用深度相机获取目标场景点云，经过滤波后作为待配准数据点云，对目标模型点云进行三维距离变换；

步骤2，对初始待配准数据点云和目标模型点云进行去模糊主方向变换，确定平移域，利用全局ICP算法在平移域和[-π,π]³旋转域中进行搜索配准，获取模型坐标系到相机坐标系的初始变换矩阵，即目标的初始位姿；

步骤3，将前一帧的位姿变换矩阵作用于当前帧数据点云，利用ICP算法与模型进行配准，获取当前帧的位姿；

步骤4，从位姿变换矩阵中解算出旋转角和平移量。

2.根据权利要求1所述的基于模型和点云全局匹配的空间非合作目标位姿估计方法，其特征在于，步骤1中滤波处理包括幅值滤波和统计滤波，具体为：

首先，从目标场景点云中去除幅值小于设定阈值的点；

其次，采用统计滤波的方法进一步去除噪声，具体包括：

对数据点云中的每个点x_i，计算它到它的k个近邻点的平均距离计算全局平均距离以及标准差φ：

其中n为数据点云的点数，1≤i≤n；

平均距离在标准范围d之外的点，判定为噪声点，从数据点云中去除；其中标准范围d为：

式中λ为标准差系数，0≤λ≤1。

3.根据权利要求1所述的基于模型和点云全局匹配的空间非合作目标位姿估计方法，其特征在于，步骤2的具体过程为：

步骤2-1、对初始待配准数据点云和目标模型点云进行去模糊主方向变换，确定 平移域，包括以下步骤：

步骤2-1-1、首先对目标模型点云和初始待配准数据点云利用点云的一阶矩和二阶矩进行主方向变换：

点云的一阶矩和二阶矩cov分别为：

其中x_i是点云中的一点。

以均值为原点，点云协方差矩阵cov的三个按特征值升序排列的特征向量(ev₁,ev₂,ev₃)依次对应XYZ轴，重新建立点云的坐标系；

对点云进行以下旋转平移变换：

x_i'＝R^-1x_i-R^-1T

其中R＝(ev₁,ev₂,ev₃)^T，x_i'为x_i变换后的对应点，x_i'组成的点云为主方向点云；

步骤2-1-2、对主方向点云进行去模糊处理，使数据主方向点云的朝向与模型主方向点云的朝向一致；具体为：

首先，将点云协方差矩阵的三个特征向量按特征值大小升序排列，依次为x⁺，y⁺和z⁺轴，其对应的相反方向为x^-，y^-和z^-轴，去模糊x轴定义为：

对z轴进行同样的过程得到去模糊的z轴，y轴通过z×x获得；

其次，计算去模糊主方向点云：

记初始数据点云D₀和目标模型点云M₀的去模糊主方向变换分别为P_D和P_M：

其中R_D、R_M分别为点云D₀、M₀协方差矩阵去模糊特征向量构成的旋转矩阵，T_D、T_M分别为点云D₀、M₀的均值构成的平移向量；经过P_D、P_M变换生成的去模糊主方向数据点云和去模糊主方向模型点云分别记为D₁、M₁；

步骤2-1-3、对D₁、M₁分别求解平行于坐标轴的包围盒；记去模糊主方向数据点云包围盒为D，去模糊主方向模型点云包围盒记为M，记D的中心点为O_D，XYZ三轴方向的边长分别为l_x、l_y和l_z；记M的中心点为O_M，XYZ三轴方向的边长分别为l_x'、l_y'和l_z'；将去模糊主方向数据点云的原点移到其包围盒的中心，得到调整的去模糊主方向数据点云p_t：

p_t＝x_i-O_D

调整的去模糊主方向数据点云作为最终用来配准的数据点云，记为D₂，同理调整的去模糊主方向模型点云，记为M₂；

以x轴方向为例，D₂到M₂的平移域为：

x_trans＝{x:-(|l'_x-l_x|/2+x_offest)≤x≤(|l'_x-l_x|/2+x_offest)}

其中x_offest是位移补偿，y轴和z轴的平移域同理可得；

基于长方体平移域的平移不确定半径γ_t为：

式中：l、w和h分别为长方体平移域的长、宽和高，t为平移域中任意一点，x₀为平移域中心；

步骤2-2、利用全局ICP算法对点云D₂和M₂进行全局搜索配准，获取目标的初始位 姿；

首先，计算搜索域C_r×C_t的配准误差上界和下界E，分别为：

式中，C_r为角轴形式表示的立方体旋转域,C_t为立方体平移域，为旋转矩阵，r₀为C_r中的一个向量，t₀为C_t中的平移量，是每一个点的L²误差，n为数据集点数，和γ_t分别为旋转不确定半径和平移不确定半径：

其中σ_r和σ_t分别为平移子立方体的半边长和旋转子立方体的半边长，x_i为数据点集里一点；

记点云D₂和M₂的配准结果为P_DM，最终配准结果P_f为：

其中E为单位矩阵；

则模型坐标系到相机坐标系的初始位姿为P_f ^-1。

4.根据权利要求1所述的空间非合作目标位姿估计方法，其特征在于，步骤3的具体过程为：

将前一帧的位姿变换矩阵作为初配准结果，作用于当前帧数据点云：

D′＝P_k-1D_k

式中：D_k为当前数据点云，P_k-1为前一帧位姿结果，D′为初配准之后的数据点云；

然后利用基本ICP算法与模型点云进行配准；其中基本ICP算法配准误差

E_ICP(a,D,M)计算公式为：

D为数据点云，M为模型点云，a＝(R,t)，n为数据点云点数，m_j为d_i对应M中的欧氏距离最近点，d_i∈D，R、t分别为旋转矩阵和平移向量；

对数据点云中的每一点，在目标模型点云中查找欧氏距离最近点，形成对应点集，根据对应点集计算刚体变换矩阵，然后将刚体变换矩阵作用于当前数据点云，重复上述过程，直到算法收敛，获得配准结果P_temp，从而获得连续的位姿P_k：

P_k＝P_temp。

5.根据权利要求1所述的基于模型和点云全局匹配的空间非合作目标位姿估计方法，其特征在于，步骤4的具体过程为：

按坐标轴主动旋转的坐标变换矩阵为：

姿态角按的顺序进行解算，目标坐标系作为静止坐标系，相机相对目标坐标系的旋转矩阵为：

其中，-180≤θ≤180，-180≤φ≤180

记平移量为：

T＝[x,y,z]'

则位姿变换矩阵pose_mat为：

每个旋转矩阵求出两组解，分别为和

旋转角为和中模长小的那一组。