CN111311679A - 一种基于深度相机的自由漂浮目标位姿估计方法 - Google Patents

Abstract

本发明涉及视觉定位领域，公开了一种基于深度相机的自由漂浮目标位姿估计方法，包括获取自由漂浮目标的RGB图像、深度图像以及点云信息；将目标的三维模型导入OpenGL中，选取合适的视点采集模板；利用采集的模板和RGB图像及深度图像对目标进行检测，计算相似度ε_s获得一组匹配模板；利用匹配模板包含的训练位姿信息(R,t)估计目标的初始位姿，得到初始姿态φ_init和初始位置p_init；对点云进行预处理后，利用ICP算法对初始姿态进行修正，得到最终的位姿结果(φ_final,p_final)，从而完成对自由漂浮目标的位姿估计过程。

Description

一种基于深度相机的自由漂浮目标位姿估计方法

技术领域

本发明涉及一种基于深度相机的自由漂浮目标位姿估计方法，属于视觉定位技术领域。

背景技术

随着空间探索的不断深入，在轨航天器数量越来越多，故障卫星和轨道垃圾也不断累积。利用空间机器人进行故障卫星抓捕、卫星维修、轨道垃圾清理成为近年来的热门研究方向。大多数目标都为非合作目标，处于自由漂浮状态。为了使空间机器人正确地抓捕目标，就需要准确估计目标抓捕点的位姿。目前主流的位姿估计方法有基于RGB图像和基于深度图像两种，前者提取彩色图中的特征点计算位姿，后者提取点云中的特征描述子进行计算和匹配位姿。两种方法的鲁棒性不高，并且对环境有较高的要求。本发明提出的方法结合了RGB图像、深度图像及点云信息，提高了准确率和计算效率的同时降低了对环境的要求。

发明内容

本发明提供了一种基于深度相机的自由漂浮目标位姿估计方法，结合了RGB图像和深度图像及点云信息，提高了准确率和计算效率的同时降低了对环境的要求。

本发明可通过以下技术方案实现：

一种基于深度相机的自由漂浮目标位姿估计方法，其特征在于，包含以下步骤：

步骤1、目标模板采集，将目标的三维模型导入OpenGL，选择视点，采集模板；

步骤2、对目标进行检测，利用采集到的模板和RGB图像、深度图像，利用基于模板匹配的方法对目标进行检测，得到一组匹配模板；

步骤3、目标的初始位姿计算，利用上一步匹配模板包含的训练位姿信息(R,t)计算目标的初始姿态φ_init和初始位置p_init；

步骤4、目标位姿修正，点云预处理之后，基于ICP算法对位姿进行修正，得到最终的位姿结果(φ_final,p_final)。

在上述的一种基于深度相机的自由漂浮目标位姿估计方法，所述模板采集过程中，包含视点采样和减少冗余度特征两个过程；视点采样具体操作过程为：将目标的三维模型导入OpenGL后，采用一个具有162个顶点的上半球，方位角的采样角度步长为15°；包围球的半径以步长10cm进行变化；同时为了采集平面内旋转的模板，相机在每个视点采集时绕自身的Z轴旋转，旋转角度步长设置为10°。

在上述的一种基于深度相机的自由漂浮目标位姿估计方法，对目标的检测中，采用基于模板匹配的方法，以RGB图像中的梯度方向和深度图像中的法向量方向为特征，定义模板为T＝({O_m}_m∈M,P)；

其中O是模板特征，表示梯度方向或者是法向量方向；M是模态，表示RGB图像或者是深度图像；P是一个二元组(r,m)的集合，r是特征在模板图像中的位置；

通过滑动窗口的方式，每张模板在位置c与图像T进行相似度计算：

其中，W表示一个以c+r为中心的窗口区域；t为该窗口区域中心的位置；当模板的相似度ε_s高于阈值τ_s时，该模板被匹配上。

在上述的一种基于深度相机的自由漂浮目标位姿估计方法，在目标的初始位姿计算中，由于上一步的检测结果包含一组模板，每个模板都含有一个训练位姿信息(R,t)，利用这些训练位姿信息去计算目标的初始位姿；先进行姿态的计算，然后进行位置的计算。

在上述的一种基于深度相机的自由漂浮目标位姿估计方法，姿态计算中，基于RGB图像的通道色度剔除检测模板中的异常值，得到初始位姿；

每一个检测到的模板都会包含一个对目标姿态的粗略估计，根据姿态估计值考虑位于对象投影上的像素，并计算其中多少像素具有预期的颜色；如果投影像素和目标的像素之间的各通道灰度值之差小于规定阈值，则判定此像素具有预期的颜色；

具有预期颜色的像素百分比小于百分之七十，认定检测结果无效，剔除此模板；在操作过程中，通过预先对投影进行腐蚀以去除投影边界的像素；

通过剔除异常值后，余下的模板被认为可以为检测结果位姿提供足够的可信度，对这些模板的姿态求平均值，得到检测结果的初始姿态φ_init。

在上述的一种基于深度相机的自由漂浮目标位姿估计方法，位置计算中，首先基于初始姿态φ_init和训练距离d渲染物体的三维模型，得到物体在此视角下的模型点云和掩模图像，记此时物体的位置为p_render＝[0,0,d]；然后将掩模图像投影到场景深度图像中，分割出与掩模对应的感兴趣区域；再利用相机内参把该感兴趣区域转换为三维点云，称之为场景点云；

计算从模型点云到场景点云的平移向量t；计算模型点云和场景点云的几何中心

和

然后分别在模型点云和场景点云中搜索离

和

最近点的

和

并认为

和

是一组场景-模型对应点，最后将两点相减即可以得到平移向量t；

最后物体的初始位置为：p_init＝p_render+t。

在上述的一种基于深度相机的自由漂浮目标位姿估计方法，利用ICP方法对点云进行配准实现位姿修正得到最终的位姿(φ_final,p_final)，基于给定三维参考点集S＝{s₁,s₂,...,s_n}，作为点云配准算法的目标；给定三维点集M＝{m₁,m₂,...,m_n}，M在迭代过程中不断地进行刚体转换，逐渐逼近S；目标函数为：

其中R代表旋转变换矩阵，t代表平移变换矩阵；目标函数代表找到一个刚体转换，使得转换后M和S的位置误差平方和最小；然后，利用经过ICP位姿修正后的旋转矩阵R和平移向量t更新检测结果的初始位姿(φ_init,p_init)，得到最终的位姿(φ_final,p_final)；具体包含两个点云预处理和点云配准两个过程；点云预处理包含去噪、点云平滑和点云降采样三个步骤：

所述去噪过程是利用高斯滤波去除离散的孤立点；点云平滑是利用移动最小二乘法(MLS)进行点云平滑；降采样是利用体素栅格来对点云进行降采样；

所述点云配准是利用ICP算法对预处理后的点云进行配准，实现位姿修正；在ICP算法中，给定三维参考点集S＝{s₁,s₂,...,s_n}，作为点云配准算法的目标；给定三维点集M＝{m₁,m₂,...,m_n}，M在迭代过程中不断地进行刚体转换，逐渐逼近S；目标函数为：

其中R代表旋转变换矩阵，t代表平移变换矩阵；目标函数代表找到一个刚体转换，使得转换后M和S的位置误差平方和最小；然后，利用经过ICP位姿修正后的旋转矩阵R和平移向量t更新检测结果的初始位姿(φ_init,p_init)，得到最终的位姿(φ_final,p_final)。

在上述的一种基于深度相机的自由漂浮目标位姿估计方法，点云配准具体包括：

(1)初始化点集M；

(2)对于点集M中的每一个点m_i，在S中搜索与其距离最近的点s_j，构建一组对应点(m_j,s_j)；

(3)使用一个距离阈值d_t进行误匹配判断，即如果m_i和s_j的距离大于d_t，则将该组对应点删除；

(4)通过最小化式的目标函数求得对应的旋转矩阵R和平移向量t；求得的R和t用于更新S，直到误差E(R,t)小于设定的阈值或者迭代次数大于设定的次数，停止迭代，否则返回(2)重新寻找最近点。

本发明有益的技术效果在于：

首先利用目标的三维模型进行模板采集，采集后减小了采集模板的特征冗余度，在保证识别精度的前提下提高了运算速度，具有高效的时效性。接下来结合RGB图像和深度图像，基于模板匹配方法，得到一组模板；在利用梯度特征时，忽略其大小而只使用其大小，使得对于太空场景背景有很强的适用性。然后根据RGB图像各通道强度特征对模板进行筛选，进而初步计算目标的位姿。最后结合ICP方法对位姿进行修正，得到最终的位姿识别结果，提高了识别的准确率。本发明简单可靠，操作方便，易于实现，便于推广应用。

附图说明

图1为本发明的流程示意图。

图2为自由漂浮目标抓取示意图。

具体实施方式

下面结合附图对本发明作进一步描述。以下实施例仅用于更加清楚地说明本发明的技术方案，而不能以此来限制本发明的保护范围。

如图2所示为自由漂浮目标抓捕示意图，目标的抓捕点选择具有较高识别度的推进器，将深度相机安装在机械臂基座上对目标进行RGB图像、深度图像和点云信息采集。

如图1所示，具体的步骤如下：

步骤一、目标模板采集：由于一张模板只能表示在单一视角下对物体的观测，因此要实现任意位姿下的三维物体检测，必须从多个视角和在不同距离下采集模板图像。模板采集包含以下两个步骤：

步骤Ⅰ、视点采样：将目标的三维模型导入OpenGL后，采用一个具有162个顶点的上半球，方位角的采样角度步长为15°。包围球的半径以步长10cm进行变化。同时为了采集平面内旋转的模板，相机在每个视点采集时绕自身的Z轴旋转，旋转角度步长设置为10°。

步骤Ⅱ、减少冗余度特征：为了加快运算速度，我们只使用采集到的模板特征的一部分。对于颜色梯度特征，只保留目标轮廓上的主要颜色渐变特征，因为我们针对的目标表面不一定会有纹理，并且获得的目标的三维模型不会详尽到附带纹理。对于表面法线特征，在目标轮廓内部选择表面法线特征，这是因为投影对象边界上的表面法线通常无法可靠估计或根本无法估计。

步骤二、目标的检测：采用基于模板匹配的方法，以RGB图像中的梯度方向和深度图像中的法向量方向为特征，定义模板为T＝({O_m}_m∈M,P)。

其中O是模板特征，表示梯度方向或者是法向量方向；M是模态，表示RGB图像或者是深度图像；P是一个二元组(r,m)的集合，而r是特征在模板图像中的位置。

目标图像的采集通过图2所示的双目深度相机获取，深度相机固定在机械臂的基座上。

通过滑动窗口的方式，每张模板在位置c与输入图像I进行相似度计算：

其中，W表示一个以c+r为中心的窗口区域；t为该窗口区域中心的位置。当模板的相似度ε_s高于阈值τ_s时，该模板被匹配上。

由上式可以看出，在计算梯度特征的相似度时，计算余弦值之差的绝对值让黑色背景对此方法的检测准确率没有影响，对太空场景具有很强的适用性。

(1)梯度方向的计算：分别在图像的RGB三个通道进行计算，只保留模长值最大的梯度。对于RGB图像I，计算像素点x处梯度方向I_g(x)：

(2)法向量的计算：在深度图像中用一阶泰勒展开的深度函数D(x)，得到：

D(x+dx)-D(x)＝dx^T▽D+h.o.t.

在一个邻域范围内，每个偏置dx都满足上式，最优的深度梯度

由最小二乘法求出。基于

可求出一个经过X,X₁,X₂三点的平面：

X＝v(x)D(x)

其中，v(x)是基于相机内参计算得到的过x的视线向量。然后，对向量X₁-X和X₂-X的叉乘结果做归一化的处理，得到像素点x的法向量。

步骤三、目标的初始位姿计算：由于上一步的检测结果包含一组模板，每个模板都含有一个训练位姿信息(R,t)，因而利用这些训练位姿去计算目标的初始位姿。包含姿态的计算和位置的计算两个步骤。

步骤Ⅰ、姿态计算：基于RGB图像的每个通道灰度值剔除检测模板中的异常值，得到初始位姿。

每一个检测到的模板都会包含一个对目标姿态的粗略估计，根据姿态估计值考虑位于对象投影上的像素，并计算其中多少像素具有预期的颜色。如果投影像素和目标的像素之间的各通道灰度值之差小于规定阈值，则判定此像素具有预期的颜色。

具有预期颜色的像素百分比小于百分之七十，认定检测结果无效，剔除此模板。在操作过程中，通过预先对投影进行腐蚀以去除投影边界的像素。

处理抓捕目标的黑色和白色组件时将它们映射到类似颜色的通道灰度值：黑色映射到蓝色，白色映射到黄色。计算灰度值之差前，检查相应的饱和度和值分量。如果值分量低于阈值t_v，则将色相设置为蓝色；如果值分量大于t_v且饱和度分量小于阈值t_s，则将色相设置为黄色。

通过剔除异常值后，余下的模板被认为可以为检测结果位姿提供足够的可信度，对这些模板的姿态求平均值，得到检测结果的初始姿态φ_init。

步骤Ⅱ、位置计算：首先基于初始姿态φ_init和训练距离d渲染物体的三维模型，得到物体在此视角下的模型点云和掩模图像，记此时物体的位置为p_render＝[0,0,d]。然后将掩模图像投影到场景深度图像中，分割出与掩模对应的感兴趣区域。再利用相机内参把该感兴趣区域转换为三维点云，称之为场景点云。

通过计算从模型点云到场景点云的平移向量t，补偿场景点云中心作为结果带来的偏差。计算模型点云和场景点云的几何中心

和

然后分别在模型点云和场景点云中搜索离

和

最近点的

和

并认为

和

是一组场景-模型对应点，最后将两点相减即可以得到平移向量t。

最后物体的初始位置为：p_init＝p_render+t。

步骤四、在位姿修正阶段，利用ICP算法对位姿进行修正，包含两个步骤：

步骤Ⅰ、进行点云预处理，包括点云去噪，点云平滑和点云降采样三个过程。

(1)去噪：采用高斯滤波去除点云中离散的孤立点。

(2)平滑：利用移动最小二乘法(MLS)进行点云平滑。

(3)降采样：使用体素栅格对点云进行降采样。

步骤Ⅱ、利用ICP算法对预处理后的点云进行配准，实现位姿修正。在ICP算法中，给定三维参考点集S＝{s₁,s₂,...,s_n}，作为点云配准算法的目标；给定三维点集M＝{m₁,m₂,...,m_n}，点集M在迭代过程中不断地进行刚体转换，逐渐逼近点集S。目标函数为：

其中R代表旋转变换矩阵，t代表平移变换矩阵。目标函数代表找到一个刚体转换，使得转换后点集M和点集S的位置误差平方和最小，包含以下过程：

(1)初始化：点集M必须处于一个与点集S相近的位置。

(2)寻找最近点：刚体转换矩阵的求解建立在点集S和点集M的对应关系上。ICP认为对应关系由最短距离来决定，即对于点集M中的每一个点m_i，在S中搜索与其距离最近的点s_j，构建一组对应点(m_j,s_j)。

(3)移除误匹配组：一般情况下，S点集的部分并不位于M中，由此上一步得到的部分对应点组(m_i,s_j)是错误的匹配。使用一个距离阈值d_t进行误匹配判断，如果m_i和s_j的距离大于d_t，则该组对应点应该排除。

(4)求解最优的刚体转换：通过目标函数求得对应的旋转矩阵R和平移向量t。求得的R和t用于更新点集S，直到误差E(R,t)小于设定的阈值或迭代次数大于设定的次数时，停止迭代，否则返回(2)重新寻找最近点。

最后，利用旋转矩阵R和平移向量t更新检测结果的初始位姿(φ_init,p_init)，得到最终的位姿(φ_final,p_final)。

本文中所描述的具体实施例仅仅是对本发明精神作举例说明。本发明所属技术领域的技术人员可以对所描述的具体实施例做各种各样的修改或补充或采用类似的方式替代，但并不会偏离本发明的精神或者超越所附权利要求书所定义的范围。

Claims (8)

1.一种基于深度相机的自由漂浮目标位姿估计方法，其特征在于，包含以下步骤：

步骤1、目标模板采集，将目标的三维模型导入OpenGL，选择视点，采集模板；

步骤2、对目标进行检测，利用采集到的模板和RGB图像、深度图像，利用基于模板匹配的方法对目标进行检测，得到一组匹配模板；

步骤3、目标的初始位姿计算，利用上一步匹配模板包含的训练位姿信息(R,t)计算目标的初始姿态φ_init和初始位置p_init；

步骤4、目标位姿修正，点云预处理之后，基于ICP算法对位姿进行修正，得到最终的位姿结果(φ_final,p_final)。

2.根据权利要求1所述的一种基于深度相机的自由漂浮目标位姿估计方法，其特征在于，所述模板采集过程中，包含视点采样和减少冗余度特征两个过程；视点采样具体操作过程为：将目标的三维模型导入OpenGL后，采用一个具有162个顶点的上半球，方位角的采样角度步长为15°；包围球的半径以步长10cm进行变化；同时为了采集平面内旋转的模板，相机在每个视点采集时绕自身的Z轴旋转，旋转角度步长设置为10°。

3.根据权利要求1所述的一种基于深度相机的自由漂浮目标位姿估计方法，其特征在于，对目标的检测中，采用基于模板匹配的方法，以RGB图像中的梯度方向和深度图像中的法向量方向为特征，定义模板为T＝({O_m}_m∈M,P)；

其中O是模板特征，表示梯度方向或者是法向量方向；M是模态，表示RGB图像或者是深度图像；P是一个二元组(r,m)的集合，r是特征在模板图像中的位置；

通过滑动窗口的方式，每张模板在位置c与图像T进行相似度计算：

其中，W表示一个以c+r为中心的窗口区域；t为该窗口区域中心的位置；当模板的相似度ε_s高于阈值τ_s时，该模板被匹配上。

4.根据权利要求1所述的一种基于深度相机的自由漂浮目标位姿估计方法，其特征在于，在目标的初始位姿计算中，由于上一步的检测结果包含一组模板，每个模板都含有一个训练位姿信息(R,t)，利用这些训练位姿信息去计算目标的初始位姿；先进行姿态的计算，然后进行位置的计算。

5.根据权利要求1所述的一种基于深度相机的自由漂浮目标位姿估计方法，其特征在于，姿态计算中，基于RGB图像的通道色度剔除检测模板中的异常值，得到初始位姿；

每一个检测到的模板都会包含一个对目标姿态的粗略估计，根据姿态估计值考虑位于对象投影上的像素，并计算其中多少像素具有预期的颜色；如果投影像素和目标的像素之间的各通道灰度值之差小于规定阈值，则判定此像素具有预期的颜色；

具有预期颜色的像素百分比小于百分之七十，认定检测结果无效，剔除此模板；在操作过程中，通过预先对投影进行腐蚀以去除投影边界的像素；

通过剔除异常值后，余下的模板被认为可以为检测结果位姿提供足够的可信度，对这些模板的姿态求平均值，得到检测结果的初始姿态φ_init。

6.根据权利要求1所述的一种基于深度相机的自由漂浮目标位姿估计方法，其特征在于，位置计算中，首先基于初始姿态φ_init和训练距离d渲染物体的三维模型，得到物体在此视角下的模型点云和掩模图像，记此时物体的位置为p_render＝[0,0,d]；然后将掩模图像投影到场景深度图像中，分割出与掩模对应的感兴趣区域；再利用相机内参把该感兴趣区域转换为三维点云，称之为场景点云；

计算从模型点云到场景点云的平移向量t；计算模型点云和场景点云的几何中心

和

然后分别在模型点云和场景点云中搜索离

和

最近点的

和

并认为

和

是一组场景-模型对应点，最后将两点相减即可以得到平移向量t；

最后物体的初始位置为：p_init＝p_render+t。

7.根据权利要求1所述的一种基于深度相机的自由漂浮目标位姿估计方法，其特征在于，利用ICP方法对点云进行配准实现位姿修正得到最终的位姿(φ_final,p_final)，基于给定三维参考点集S＝{s₁,s₂,...,s_n}，作为点云配准算法的目标；给定三维点集M＝{m₁,m₂,...,m_n}，M在迭代过程中不断地进行刚体转换，逐渐逼近S；目标函数为：

其中R代表旋转变换矩阵，t代表平移变换矩阵；目标函数代表找到一个刚体转换，使得转换后M和S的位置误差平方和最小；然后，利用经过ICP位姿修正后的旋转矩阵R和平移向量t更新检测结果的初始位姿(φ_init,p_init)，得到最终的位姿(φ_final,p_final)；具体包含两个点云预处理和点云配准两个过程；点云预处理包含去噪、点云平滑和点云降采样三个步骤：

所述去噪过程是利用高斯滤波去除离散的孤立点；点云平滑是利用移动最小二乘法(MLS)进行点云平滑；降采样是利用体素栅格来对点云进行降采样；

所述点云配准是利用ICP算法对预处理后的点云进行配准，实现位姿修正；在ICP算法中，给定三维参考点集S＝{s₁,s₂,...,s_n}，作为点云配准算法的目标；给定三维点集M＝{m₁,m₂,...,m_n}，M在迭代过程中不断地进行刚体转换，逐渐逼近S；目标函数为：

其中R代表旋转变换矩阵，t代表平移变换矩阵；目标函数代表找到一个刚体转换，使得转换后M和S的位置误差平方和最小；然后，利用经过ICP位姿修正后的旋转矩阵R和平移向量t更新检测结果的初始位姿(φ_init,p_init)，得到最终的位姿(φ_final,p_final)。

8.根据权利要求7所述的一种基于深度相机的自由漂浮目标位姿估计方法，其特征在于，点云配准具体包括：

(1)初始化点集M；

(2)对于点集M中的每一个点m_i，在S中搜索与其距离最近的点s_j，构建一组对应点(m_j,s_j)；

(3)使用一个距离阈值d_t进行误匹配判断，即如果m_i和s_j的距离大于d_t，则将该组对应点删除；

(4)通过最小化式的目标函数求得对应的旋转矩阵R和平移向量t；求得的R和t用于更新S，直到误差E(R,t)小于设定的阈值或者迭代次数大于设定的次数，停止迭代，否则返回(2)重新寻找最近点。

Images