CN109724586B - 一种融合深度图和点云的航天器相对位姿测量方法 - Google Patents

Abstract

本发明提供一种融合深度图和点云的航天器相对位姿测量方法，本发明针对具有轴对称几何特性的目标航天器，利用深度传感器获得的深度图计算相邻帧的视线方向的角偏差，克服了因帧间差距较大而可能造成的滚转角计算错误等问题，并利用此偏差和上一帧的位姿测量结果进行点云配准，计算得到6自由度的相对位姿值，提高了位姿测量的精度。

Description

一种融合深度图和点云的航天器相对位姿测量方法

技术领域

本发明属于空间导航技术领域，具体涉及一种融合深度图和点云的航天器相对位姿测量方法。

背景技术

近年来，随着人类对外太空的不断深入，故障卫星的修理、空间碎片的清理等航天任务的不断开展，航天器相对位姿测量技术的重要性显得越来越明显。

通常采用的测量传感器包括雷达、卫星定位、单目视觉、双目视觉、扫描式激光雷达等。基于视觉的位姿测量技术具有构造简单、性能稳定、可靠性高等优点，一直是近距离阶段实现相对位姿测量的主要手段。双目视觉在相机参数一定的情况下，需要有适当的基线，才能获取高精度的深度信息。由于双目视觉匹配算法复杂，通常采用特征提取的方法，仅获取相应特征点的深度信息，这种方法得到的是稀疏点云，且深度精度与特征匹配的精度关系较大。扫描式激光雷达具有测量范围远、精度高、可靠性强等优点，但其质量大、功耗高、有运动部件，需要逐行扫描或按照特定方式的扫描测量来获取场景的深度数据。非扫描式激光成像雷达在无需扫描的情况下可以获得整个场景的密集点云、深度图和强度图数据，具有对背景杂散光的抑制能力强、探测距离远、且不存在运动模糊、帧率高等优点，可以满足实时测量。

发明内容

本发明的目的在于提出一种融合深度图和点云的航天器相对位姿测量方法。

发明内容：通过二维深度图直线特征的检测辅助三维点云配准，提出了一种实时高精度的相对位姿测量方法。首先，对深度传感器获得的深度图进行直线边框检测；然后对相邻帧最长的边框进行对应直线的边框差角计算；接着利用相邻帧边框差角以及前一帧的位姿配准结果变换点云；最后对变换过后的点云进行ICP配准并解算出六自由度分量。

本发明的具体实现步骤为：

一种融合深度图和点云的航天器相对位姿测量方法，包括以下步骤：

步骤1：利用深度传感器获取第k(k＞1)帧的目标点云C_k及其对应的深度图I_k；

步骤2：对第k帧深度图I_k进行高斯滤波预处理，得到图像G_k；

步骤3：基于步骤2，对第k帧的深度图像G_k进行直线检测；

步骤4：计算第k帧最长边框以及第k-1帧对应边框的偏差角度θ_k；

步骤5：利用滚转偏差角度θ_k对应的位姿矩阵B_k和第k-1帧的位姿矩阵H_k-1同时对第k帧目标点云C_k进行变换，得到待配准的目标点云C'_k：

C'_k＝H_k-1B_kC_k

步骤6，利用ICP算法将待配准的目标点云C'_k与模型点云D进行配准，获取第k帧的位姿矩阵H_k：

步骤7，从位姿变换矩阵H_k中解算出六自由度分量。

更进一步的，所述步骤3具体包括：

步骤3-1：求取图像G_k的梯度grad(x,y)，保留梯度大于ρ(梯度阈值)的像素(x',y')，并对所有的像素(x',y')根据梯度从大到小的顺序列入状态列表list中，并设置Not-Used的状态；

步骤3-2：若列表List中仍有状态为Not-Used的像素点，从列表中取出第一个状态为Not-Used的像素作为起源点P_s(x_s,y_s),根据此像素点进行区域增长，得到区域A；否则结束；区域增长过程如下：

初值为：θ_r＝θ_l；S_x＝cos(θ_r)；S_y＝sin(θ_r)

其中区域增长的判断条件：

若满足条件，更新公式：

更新

的状态为Used，否则，不更新；

迭代结束条件：不再有满足增长判断条件的点

其中，

为P_s邻域内状态为Not-Used的点，S_x、S_y分别为区域角度的余弦值和正弦值，τ为区域增长的角度阈值，θ_r为区域角度，θ_l为起源点P_s(x_s,y_s)的水平线角，

为

点的水平线角，像素点的水平线方向与像素的梯度方向垂直；

步骤3-3：建立包含A的最小外接矩形框rb；

首先以区域A内点的梯度值为权重计算矩形框rb的重心点(c_x,c_y)，

接着计算矩阵M的最小特征值对应的特征向量，作为矩阵的主方向θ_rb，其中

最后根据区域A中离rb重心点最远的点以及rb的主方向θ_rb可以计算出rb的长和宽；

步骤3-4：根据contrario模型，可以计算矩形框rb的误检度NFA：

其中，L,W为输入深度图的长宽尺寸，n(rb)为矩形框内的点数，t(rb)为满足矩形的主方向与矩形框内点的水平线方向差值小于角度阈值τ的总点数；

t(rb)～b(n(rb),p)，

式中,b代表二项分布；

步骤3-5：如果NFA大于等于误检阈值ε，对矩形框进行一系列改进，诸如减小矩形框的大小等，得到新的矩形框rb’；否则执行步骤3-7；

步骤3-6：计算新矩形框rb’的NFA’；

步骤3-7：如果NFA’的值仍然大于等于ε，转至步骤3-2；否则执行步骤3-8；

步骤3-8：将最终矩形框rb-fianl内的所有像素点的状态改为Used；

步骤3-9：计算矩形框rb-fianl的左右顶点作为检测到的直线的两个端点，即可在图像上做出一条直线段。

更进一步的，所述步骤4具体包括：

步骤4-1：取出每一帧图片中最长的边框线段；

步骤4-2：将最长的边框向量归入二维笛卡尔坐标系的第一象限；

根据边框线段的两个顶点(x₁,y₁)，(x₂,y₂)计算边框的向量，为了将向量能够归到第一象限，需要先比较x₁和x₂的大小，若x₁＞x₂，则向量为(x₁-x₂,y₁-y₂)；否则向量为(x₂-x₁,y₂-y₁)；此时得到的向量定在第一象限或者第四象限，设为中间向量v_l；再继续判断v_l的y值分量，若y_vl＜0，则将其逆时针旋转90°即可得到最终的向量v_f；否则v_f＝v_l

步骤4-3：根据相邻两帧的向量

便可以得出相邻两帧间的旋转角θ_k

再根据坐标的比值可以判断出从一帧到后一帧的旋转方向：

若

则为逆时针旋转；否则为顺时针旋转；

最终可以得出θ_k＝±|θ|，当为逆时针时为+。

更进一步的，所述步骤7具体包括：

其中，H_k是4*4的方阵，R是3*3的方阵，t为3*1的向量；

点云按照姿态角

的顺序进行旋转，其中，

是绕X轴的旋转角，Y(θ)是绕Y轴的旋转角，Z(ψ)是绕Z轴的旋转角；设目标坐标系为静止坐标系，相机相对目标坐标系的旋转矩阵为：

其中，

-90°≤θ≤90°，-180°≤ψ≤180°

t＝[t_x,t_y,t_z]'

其中，t_x、t_y、t_z是X、Y、Z三轴方向上的平移量；解算步骤基于点云的主动旋转。

本发明是航天器相对位姿测量方法的一种改进。其中，相机坐标系的视线方向为X轴，目标点云坐标系的方向与相机坐标系的方向一致。

本发明与现有成果相比，其有益效果为：本发明针对具有轴对称几何特性的目标航天器，利用深度传感器获得的深度图计算相邻帧的视线方向的角偏差，克服了因帧间差距较大而可能造成的滚转角计算错误等问题，并利用此偏差和上一帧的位姿测量结果进行点云配准，计算得到6自由度的相对位姿值，提高了位姿测量的精度。

附图说明

图1是本发明基于深度图和点云的航天器相对位姿测量方法的流程图。

图2是本发明实施例中仿真点云序列位姿估计的旋转量误差对比曲线图。

图3是本发明实施例中仿真点云序列位姿估计的平移量误差对比曲线图。

具体实施方式

如图1所示，本发明基于深度图和点云的航天器相对位姿测量方法，包括以下步骤：

步骤1：利用深度传感器获取第k(k＞1)帧的目标点云C_k及其对应的深度图I_k

步骤2：对第k帧深度图I_k进行高斯滤波预处理，得到图像G_k。

步骤3：基于步骤2，对第k帧的深度图像G_k进行直线检测。步骤3具体包括：

步骤3-1：求取图像G_k的梯度grad(x,y)，保留梯度大于ρ(梯度阈值)的像素(x',y')，并对所有的像素(x',y')根据梯度从大到小的顺序列入状态列表list中，并设置Not-Used的状态；

步骤3-2：若列表List中仍有状态为Not-Used的像素点，从列表中取出第一个状态为Not-Used的像素作为起源点P_s(x_s,y_s),根据此像素点进行区域增长，得到区域A；否则结束；区域增长过程如下：

初值为：θ_r＝θ_l；S_x＝cos(θ_r)；S_y＝sin(θ_r)

其中区域增长的判断条件：

若满足条件，更新公式：

更新

的状态为Used

否则，不跟新；

迭代结束条件：不再有满足增长判断条件的点

其中，

为P_s邻域内状态为Not-Used的点，S_x、S_y分别为区域角度的余弦值和正弦值，τ为区域增长的角度阈值，θ_r为区域角度，θ_l为起源点P_s(x_s,y_s)的水平线角，

为

点的水平线角，像素点的水平线方向与像素的梯度方向垂直。

步骤3-3：建立包含A的最小外接矩形框rb；

具体过程为：首先以区域A内点的梯度值为权重计算矩形框rb的重心点(c_x,c_y)，

接着计算矩阵M的最小特征值对应的特征向量，作为矩阵的主方向θ_rb，其中

最后根据区域A中离rb重心点最远的点以及rb的主方向θ_rb可以计算出rb的长和宽。

步骤3-4：根据contrario模型，可以计算矩形框rb的误检度NFA：

其中，L,W为输入深度图的长宽尺寸，n(rb)为矩形框内的点数，t(rb)为满足矩形的主方向与矩形框内点的水平线方向差值小于角度阈值τ的总点数

t(rb)～b(n(rb),p)，

式中,b代表二项分布。

步骤3-5：如果NFA大于等于误检阈值ε，对矩形框进行一系列改进，诸如减小矩形框的大小等，得到新的矩形框rb’；否则执行步骤3-7；

步骤3-6：计算新矩形框rb’的NFA’；

步骤3-7：如果NFA’的值仍然大于等于ε，转至步骤3-2；否则执行步骤3-8；

步骤3-8：将最终矩形框rb-fianl内的所有像素点的状态改为Used；

步骤3-9：计算矩形框rb-fianl的左右顶点作为检测到的直线的两个端点，即可在图像上做出一条直线段。

步骤4，计算第k帧最长边框以及第k-1帧对应边框的偏差角度θ_k。步骤4具体包括：

步骤4-1：取出每一帧图片中最长的边框线段；

步骤4-2：将最长的边框向量归入二维笛卡尔坐标系的第一象限。

具体步骤为：根据边框线段的两个顶点(x₁,y₁)，(x₂,y₂)计算边框的向量，为了将向量能够归到第一象限，需要先比较x₁和x₂的大小，若x₁＞x₂，则向量为(x₁-x₂,y₁-y₂)；否则向量为(x₂-x₁,y₂-y₁)。此时得到的向量定在第一象限或者第四象限，设为中间向量v_l。再继续判断v_l的y值分量，若y_vl＜0，则将其逆时针旋转90°即可得到最终的向量v_f；否则v_f＝v_l

步骤4-3：根据相邻两帧的向量

便可以得出相邻两帧间的旋转角θ_k

再根据坐标的比值可以判断出从一帧到后一帧的旋转方向：

若

则为逆时针旋转；否则为顺时针旋转。

最终可以得出θ_k＝±|θ|，当为逆时针时为+。

步骤5，利用滚转偏差角度θ_k对应的位姿矩阵B_k和第k-1帧的位姿矩阵H_k-1同时对第k帧目标点云C_k进行变换，得到待配准的目标点云C'_k：

C'_k＝H_k-1B_kC_k

步骤6，利用ICP算法将待配准的目标点云C'_k与模型点云D进行配准，获取第k帧的位姿矩阵H_k：

步骤7，从位姿变换矩阵H_k中解算出六自由度分量。

其中，H_k是4*4的方阵，R是3*3的方阵，t为3*1的向量。

点云按照姿态角

的顺序进行旋转，其中，

是绕X轴的旋转角，Y(θ)是绕Y轴的旋转角，Z(ψ)是绕Z轴的旋转角。设目标坐标系为静止坐标系，相机相对目标坐标系的旋转矩阵为：

其中，

-90°≤θ≤90°，-180°≤ψ≤180°

t＝[t_x,t_y,t_z]'

其中，t_x、t_y、t_z是X、Y、Z三轴方向上的平移量；解算步骤是基于点云的主动旋转而进行的。

实施例

为了对本发明算法的有效性进行说明，充分展现出该方法具有更加准确的位姿获取性能，完成实验如下：

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

仿真实验采用虚拟深度传感器对模型点云拍摄产生的数据点云。虚拟深度传感器的参数设置为：分辨率500×500，视场角20°×20°。在60m处开始拍摄模型点云。数据点云的位姿设置为：平移量为沿着X轴从60m开始以1m间隔减少到10m，旋转量为绕着X轴从50°开始以1°间隔减少到0°，其余自由度分量均为0。

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

(2)实验结果分析

图2为仿真点云配准过程的示意图，灰度较亮的为目标模型点云，灰度较暗的为目标仿真相机点云，图2右侧为配准结果，相机点云和目标点云重合，表明位姿估计结果良好。

将估计序列帧采用的基本ICP算法记为改进前，将本专利使用的配准算法记为改进后。图3为序列位姿估计的旋转量误差对比。从图3(a)可以看出，改进后的X轴方向旋转量的误差在0.5°以内，明显比改进前的误差要小。从图3(b)、图3(c)可以看出，改进后的Y轴和Z轴方向的旋转量误差依然和改进前一样，在0.1°以内。综合可以看出，本专利的改进确实大大提高了序列帧点云配准的精度，尤其是对滚转角误差的改进，显得尤为突出。

Claims (3)

1.一种融合深度图和点云的航天器相对位姿测量方法，其特征在于，包括以下步骤：

步骤1：利用深度传感器获取第k帧的目标点云C_k及其对应的深度图I_k，其中k＞1；

步骤2：对第k帧深度图I_k进行高斯滤波预处理，得到图像G_k；

步骤3：基于步骤2，对第k帧的深度图像G_k进行直线检测，具体包括：

步骤3-1：求取图像G_k的梯度grad(x，y)，保留梯度大于梯度阈值ρ的像素(x′，y′)，并对所有的像素(x′，y′)根据梯度从大到小的顺序列入状态列表list中，并设置Not-Used的状态；

步骤3-2：若列表List中仍有状态为Not-Used的像素点，从列表中取出第一个状态为Not-Used的像素作为起源点P_s(x_s，y_s)，根据此像素点进行区域增长，得到区域A；否则结束；区域增长过程如下：

初值为：θ_r＝θ_l；S_x＝cos(θ_r)；S_y＝sin(θ_r)

其中区域增长的判断条件：

若满足条件，更新公式：

更新

的状态为Used，否则，不更新；

迭代结束条件：不再有满足增长判断条件的点

其中，

为P_s邻域内状态为Not-Used的点，S_x、S_y分别为区域角度的余弦值和正弦值，τ为区域增长的角度阈值，θ_r为区域角度，θ_l为起源点P_s(x_s，y_s)的水平线角，

为

点的水平线角，像素点的水平线方向与像素的梯度方向垂直；

步骤3-3：建立包含A的最小外接矩形框rb；

首先以区域A内点的梯度值为权重计算矩形框rb的重心点(c_x，c_y)，

接着计算矩阵M的最小特征值对应的特征向量，作为矩阵的主方向θ_rb，其中

最后根据区域A中离rb重心点最远的点以及rb的主方向θ_rb计算出rb的长和宽；

步骤3-4：根据contrario模型，计算矩形框rb的误检度NFA：

其中，L，W为输入深度图的长宽尺寸，n(rb)为矩形框内的点数，t(rb)为满足矩形的主方向与矩形框内点的水平线方向差值小于角度阈值τ的总点数；

t(rb)～b(n(rb)，p)，

式中，b代表二项分布；

步骤3-5：如果NFA大于等于误检阈值ε，对矩形框进行一系列改进，减小矩形框的大小，得到新的矩形框rb’；否则执行步骤3-7；

步骤3-6：计算新矩形框rb’的NFA’；

步骤3-7：如果NFA’的值仍然大于等于ε，转至步骤3-2；否则执行步骤3-8；

步骤3-8：将最终矩形框rb-fianl内的所有像素点的状态改为Used；

步骤3-9：计算矩形框rb-fianl的左右顶点作为检测到的直线的两个端点，即在图像上做出一条直线段；

步骤4：计算第k帧最长边框以及第k-1帧对应边框的偏差角度θ_k；

步骤5：利用滚转偏差角度θ_k对应的位姿矩阵B_k和第k-1帧的位姿矩阵H_k-1同时对第k帧目标点云C_k进行变换，得到待配准的目标点云C′_k：

C′_k＝H_k-1B_kC_k

步骤6：利用ICP算法将待配准的目标点云C′_k与模型点云D进行配准，获取第k帧的位姿矩阵H_k：

步骤7：从位姿矩阵H_k中解算出六自由度分量。

2.根据权利要求1所述的一种融合深度图和点云的航天器相对位姿测量方法，其特征在于，所述步骤4具体包括：

步骤4-1：取出每一帧图片中最长的边框线段；

步骤4-2：将最长的边框向量归入二维笛卡尔坐标系的第一象限；

根据边框线段的两个顶点(x₁，y₁)，(x₂，y₂)计算边框的向量，为了将向量能够归到第一象限，需要先比较x₁和x₂的大小，若x₁＞x₂，则向量为(x₁-x₂，y₁-y₂)；否则向量为(x₂-x₁，y₂-y₁)；此时得到的向量定在第一象限或者第四象限，设为中间向量v_l；再继续判断v_l的y值分量，若

则将其逆时针旋转90°即得到最终的向量v_f；否则v_f＝v_l

步骤4-3：根据相邻两帧的向量

便得出相邻两帧间的旋转角θ_k

再根据坐标的比值判断出从一帧到后一帧的旋转方向：

若

则为逆时针旋转；否则为顺时针旋转；

最终得出θ_k＝±|θ|，当为逆时针时为+。

3.根据权利要求1所述的一种融合深度图和点云的航天器相对位姿测量方法，其特征在于，所述步骤7具体包括：

其中，H_k是4*4的方阵，R是3*3的方阵，t为3*1的向量；

点云按照姿态角

的顺序进行旋转，其中，

是绕X轴的旋转角，Y(θ)是绕Y轴的旋转角，Z(ψ)是绕Z轴的旋转角；设目标坐标系为静止坐标系，相机相对目标坐标系的旋转矩阵为：

其中，

-90°≤θ≤90°，-180°≤ψ≤180°

t＝[t_x，t_y，t_z]′

其中，t_x、t_y、t_z是X、Y、Z三轴方向上的平移量；解算步骤基于点云的主动旋转。

Images