CN108921895A - 一种传感器相对位姿估计方法 - Google Patents

Abstract

本发明提供一种传感器相对位姿估计方法，包括以下步骤：预处理步骤，使用2D特征检测子在前一帧和当前帧彩图中分别进行特征点提取和描述，对特征点进行匹配和筛选以建立对应关系，利用特征点在前一帧深度图中的深度信息形成2D‑3D信息关联集，分别将前一帧和当前帧深度图转换为源点云和目标点云，对点云进行降采样；点云对应关系构建步骤，构建源点云和目标点云间的对应关系以形成对应点集合；误差函数构建及求解步骤，使用2D‑3D信息关联集和对应点集合构建误差函数并优化求解，直至满足终止条件，若终止条件不满足，则重新进行点云对应关系构建。基于上述方法，针对空间目标近距离感知中的传感器相对位姿估计问题，计算精度显著提高，鲁棒性显著提高。

Description

一种传感器相对位姿估计方法

技术领域

本发明属于空间目标近距离感知领域，特别涉及一种传感器相对位姿估计 方法。

背景技术

迭代最近点(Iterative Closet Point，ICP)算法最早由Besl等于20世纪90年代初提出，用于估计3D点云之间的变换关系，经过二十余年的发展，目前已广泛 应用于三维重建、目标识别、机器人导航等方面。给定源点云目标点云以及待求解变换的旋转分量R∈SO(3)、平 移分量的初值，ICP算法迭代进行以下两个步骤，直至收敛或达到指定迭代 次数：

对于源点云中的每一点P_i，在目标点云中搜索其最近邻点，即与其距离最 小的点，从而建立对应点集合C：

其中||·||₂不加区分地表示向量或矩阵的弗罗贝尼乌斯(Frobenius)范数，并在不 引起歧义的情况下简写为||·||₂。

根据当前对应关系进行变换估计。

Besl等使用点到点(point-to-point)距离，即变换后源点与其最近邻点间的 欧式距离构建误差函数求得使其最小的R和t作为当前估计：

其中公式(2)具有闭合形式解，也可以使用迭代优化的方法进行求解。闭合 形式解通常使用奇异值分解、单位四元数、正交矩阵、对偶四元数等方法求得。 Eggert等对以上四种方法进行比较，发现在真实世界噪声条件下各个方法取得的 结果区别并不明显。

ICP算法的一个问题在于点云中点的数量众多导致计算量过大，一些方法通 过随机采样、一致采样等预处理过程减少点云中点的数量，加快计算速度。ICP 算法的另一个问题在于对初值的依赖性较强，在两点云间变换不大的情况下，简 单地使用单位矩阵I作为初值就可以得到理想的结果；而在两点云间变换较大的 情况下，仍旧使用单位矩阵作为初值就不再合适，容易陷入局部极小。解决这一 问题的常用方法是采用由粗到精的策略，在预处理阶段通过不同途径提供一个粗 略但较为合理的初值。Zhang和Jost等对点云进行不同压缩程度的降采样，而后 分层进行估计，将压缩程度高的层级所估计的结果作为压缩程度低的层级进行估 计的初值。一些方法首先使用3D尺度不变特征变换(Scale InvariantFeature Transform，SIFT)、法线对齐径向特征(Normal Aligned Radial Feature，NARF)等检测3D特征点，随后使用点特征直方图(Point Feature Histograms，PFH)、 方向直方图特征(Signature of Histograms of Orientations，SHOT)、二进制鲁棒 外观及法向描述子(Binary Robust Appearance and Normals Descriptor，BRAND) 等对检测的特征点进行描述，最后完成特征点匹配并利用匹配点估算一个较为合 理的初值。

在建立点云间对应关系阶段，一些方法使用k维树(k-d tree)、八叉树(Octree)等空间数据结构以加速最近邻点的搜索过程，常用的开源实现包括近似最近邻(Approximate Nearest Neighbors，ANN)库、近似最近邻快速库(Fast Library forApproximate Nearest Neighbors，FLANN)、libnabo等。与传统ICP算法仅依赖 几何信息不同，一些方法还引入额外的信息来搜索最近邻点，这在使用RGBD 数据的方法中尤为常见。如Johnson等和Ji等引入YIQ颜色信息，Men等引入 色调(hue)信息，Korn等引入LAB颜色信息等。

在误差函数构建方面，研究人员还探索点到点(point-to-point)距离之外的 度量方法。如与Besl等同时期的Chen等就提出使用点到平面(point-to-plane) 距离，即变换后源点与目标点局部切平面之间的欧式距离，这利用了目标点的法 向量信息。Segal等则提出使用平面到平面(plane-to-plane)距离，同时利用源 点和目标点的法向量信息。然而目前方法存在估计精度不高、鲁棒性弱等问题， 需要新的方法解决上述问题。

发明内容

本发明的目的是通过以下技术方案实现的。

本发明提供一种传感器相对位姿估计方法，包括以下步骤：

预处理步骤，使用2D特征检测子在前一帧彩图和当前帧彩图中进行特征 点提取和描述，对特征点进行匹配和筛选以建立特征点间的对应关系，形成 2D-3D信息关联集，分别将前一帧深度图和当前帧深度图转换为源点云和目标 点云，对源点云和目标点云进行降采样；

点云对应关系构建步骤，构建源点云和目标点云间的对应关系以形成对应 点集合；

误差函数构建及求解步骤，使用2D-3D信息关联集和对应点集合构建误差 函数并优化求解，直至满足终止条件，若终止条件不满足，则重新进行点云对 应关系构建。

优选地，预处理包括以下步骤：

使用SIFT在彩图中进行特征点提取和描述，SIFT包括特征检测子和描述 子两部分，SIFT的检测结果包含特征点位置、主方向、检测时所处的尺度三方 面信息；

按顺序依次对特征点进行双向最近邻匹配、对称性筛选、像素距离筛选、 特征距离筛选，获得匹配点对；

任取匹配点对，建立图像像素坐标，若在前一帧深度图中，图像像素坐标 处存在有效深度值，则2D-3D信息关联成功并形成关联集；

分别将前一帧深度图和当前帧深度图转换为源点云和目标点云，对源点云 和目标点云进行降采样。

优选地，双向最近邻匹配使用特征点间的特征距离作为度量标准进行最近 邻匹配，分为正向最近邻匹配和反向最近邻匹配；

正向最近邻匹配中，对于中的在中找到与其特征距离最 小的特征点次小的特征点作为候选，若小于给定阈值， 则与间匹配成功，反之则匹配不成立；

反向最近邻匹配与正向最近邻匹配类似但方向相反，通过反向匹配与 建立匹配关系；

其中，表示前一帧彩图特征点集合，表示中的每个特征点， 表示当前帧彩图特征点集合，表示中的每个特征点，表 示特征点和之间的特征距离，i、j均为正整数。

优选地，对称性筛选包括以下步骤：

通过正向匹配与建立匹配关系，且通过反向匹配也与建 立匹配关系，则对称性筛选通过，获得筛选后的匹配点集合；

否则对称性筛选失败，取消相应的匹配关系。

优选地，像素距离筛选包括以下步骤：

任取筛选后的匹配点集合中的匹配点对，二者间像素距离大于给定阈值， 则像素距离筛选失败，需要将匹配关系剔除。

优选地，特征距离筛选包括以下步骤：

在所有匹配点中按比例保留特征距离小于一定比例阈值α的的匹配点构成 匹配点集合。

优选地，误差函数构建包括以下步骤：

利用预处理过程中获取的2D-3D信息关联集以及点云对应关系构建过 程中获取的对应点集合C构建误差函数

其中，误差函数由2D项和3D项组成，表示变量p的齐次形式 即K为标定已知的相机内参数矩阵，w为2D项权重，为源点云， 为目标点云，p_proj为2D特征点p_prev所对应的3D空间点P经变换后在当前 帧图像中的投影点，为平移分量，d为特征点对应的深度值。

优选地，误差函数求解包括以下步骤：

使用李代数对位姿进行参数化表示，表示为以下无约束最小二乘的形式：

其中，是对旋转矩阵R参数化表示形式，二者一一对 应；为[ω]_×，[·]_×表示向量生成的反对称矩阵； 表示变量p 的齐次形式即 就表示的齐次形式。

优选地，优化求解最小二乘需要计算误差函数的雅克比(Jacobian)矩阵， 2D项的雅克比矩阵为：

其中，R为旋转矩阵。

优选地，优化求解最小二乘需要计算误差函数的雅克比矩阵，3D项的雅克 比矩阵为：

[I-[R·P+t]_×]

其中，I为3x3的单位矩阵。

本发明的优点在于：

本发明提供了一种传感器相对位姿估计方法，改进迭代最近点算法，该算 法包括：预处理、点云对应关系构建、误差函数构建及求解，使用2D-3D信息 关联集和对应点集合构建误差函数并优化求解。基于上述算法，针对空间目标 近距离感知中的传感器相对位姿估计问题，计算精度显著提高，鲁棒性显著提 高。

附图说明

通过阅读下文具体实施方式的详细描述，各种其他的优点和益处对于本领 域普通技术人员将变得清楚明了。附图仅用于示出具体实施方式的目的，而并 不认为是对本发明的限制。而且在整个附图中，用相同的参考符号表示相同的 部件。在附图中：

附图1示出了根据本发明实施方式的融合2D-3D信息的迭代最近点算法框 架图；

附图2示出了根据本发明实施方式的在彩图中进行特征提取和描述：(a)彩 图；(b)使用SIFT进行特征提取结果；

附图3示出了根据本发明实施方式的特征匹配示意图：(a)正向最近邻匹配； (b)反向最近邻匹配；(c)对称性筛选；

附图4示出了根据本发明实施方式的特征匹配和筛选结果：(a)正向最近邻 匹配结果；(b)对称性筛选结果；(c)像素距离筛选结果；(d)特征距离筛选结果；

附图5示出了根据本发明实施方式的2D-3D信息关联示意图；

附图6示出了根据本发明实施方式的体素栅格对点云进行降采样；

附图7示出了根据本发明实施方式的误差函数构建示意图。

具体实施方式

下面将参照附图更详细地描述本公开的示例性实施方式。虽然附图中显示 了本公开的示例性实施方式，然而应当理解，可以以各种形式实现本公开而不 应被这里阐述的实施方式所限制。相反，提供这些实施方式是为了能够更透彻 地理解本公开，并且能够将本公开的范围完整的传达给本领域的技术人员。

根据本发明的实施方式，提出一种传感器相对位姿估计方法，利用RBD相 机获取的2D图像和3D点云，提出一种融合2D-3D信息的改进型ICP，简称为 2d-3d ICP，用于传感器相对位姿/运动估计。如图1所示，算法框架主要分为预 处理、点云对应关系构建、误差函数构建及求解三个部分：首先使用2D特征 检测子在前一帧和当前帧彩图中提取特征点，并使用特征描述子进行描述。在 此基础之上，进行特征匹配和筛选以建立特征点间的对应关系，形成包含2D 信息的匹配点对集随后将深度图中的3D信息与进行关联，形成2D-3D 信息关联集与此同时，根据相机系与图像像素坐标系之间的关系，分别将 前一帧和当前帧深度图转换为源点云和目标点云在对点云进行降采样以 后，构建两点云间的对应关系，形成对应点集合C。最后使用2D-3D信息关联 集和对应点集合C构建误差函数并优化求解相对位姿，直至满足终止条 件。其中，点云对应关系构建部分使用FLANN完成。对于源点云中每一点P_i， 使用随机k-d tree构建其最近邻索引，随后从其中搜索最近邻点。与遍历目标点 云每一点寻找最近邻点的蛮力方法相比，使用此种方法可以有效提升搜索效率。

本发明的工作主要体现在预处理和误差函数构建及求解部分，以下分别进行 详述。

1.预处理

1.1特征提取和描述

本发明使用SIFT在彩图中进行特征点提取和描述。SIFT最早由David G. Lowe提出，包括特征检测子和描述子两部分。检测子的核心思想一是构建图像 金字塔，在不同尺度空间中检测特征点，从而对图像的尺度变换具有不变性；二 是根据特征点邻域的局部结构计算主方向，从而对图像的旋转变换具有不变性。

SIFT的检测结果包含特征点位置、主方向、检测时所处的尺度三方面信息。 如图2中所示，每个圆代表一个检测出的特征点，其中圆心表示特征点位置，圆 的大小与检测时所处的尺度成正比，圆内的直线表示特征点主方向。SIFT检测 子在图像亮度、视角变化情况下均能保持稳定性，在可重复性、鲁棒性和特征点 定位精度上亦有上佳表现。SIFT描述子则将特征点邻域分成16个子区域，分别 对每个子区域中的像素在8个方向上的梯度分布情况进行统计，据此将特征点表 示成128维的向量。

1.2特征匹配和筛选

在两帧图像中完成特征检测和描述以后，进行特征匹配和筛选以建立特征点间的对应关系，按顺序依次分为双向最近邻匹配、对称性筛选、像素距离筛选、 特征距离筛选四个部分。假设前一帧和当前帧彩图中检测出的特征点集合分别 记为和其中N_prev和N_curr分别表示各 自点集中特征点的数量。特征点和之间的特征距离记为表示二 者在128维特征向量空间中的欧式距离；像素距离记为表示二者在图像 像素坐标系中的欧式距离。

(1)双向最近邻匹配

使用特征点间的特征距离作为度量标准进行最近邻匹配，分为正向和反向两 个方向进行，所建立的匹配点集合分别记为和在正向最近邻 匹配中，对于中每个特征点遍历中所有特征点，找到与其特征 距离最小的和次小的作为候选。若小于给定阈值，则和 间匹配成立，反之则匹配不成立。如图3(a)中所示，假设与特征距离最 小的两个特征点为和但是不满足要求，匹配不成立；与 特征距离最小的两个特征点为和且满足要求，匹配 成立，使用红色箭头表示建立的正向匹配关系。如图3(b)中所示，反向最近邻 匹配与正向最近邻匹配类似但方向相反，即对于中每个特征点需要 遍历中每个特征点。图中使用绿色箭头表示建立的反向匹配关系。

(2)对称性筛选

在双向最近邻匹配完成后，施加对称性筛选以减少误匹配数量，筛选后的匹 配点集合记为若通过正向匹配与建立匹配关系，且通过 反向匹配也与建立匹配关系，则对称性筛选通过；否则对称性筛选失败，取 消相应的匹配关系。如图3(c)中所示，通过正向匹配与建立匹配关系， 也通过反向匹配与建立匹配关系，对称性筛选通过；通过正向匹配 与建立匹配关系，但是通过反向匹配却与建立匹配关系，对称性筛 选失败。

(3)像素距离筛选

对称性筛选完成以后，施加像素距离筛选以进一步减少误匹配数量，筛选后 的匹配点集合记为考虑相机在两不同位姿处分别进行成像的过程中发 生的位姿变换有限，两帧图像中对应同一目标的特征点其图像像素坐标的变化也 应该是有限的。任取匹配点对若二者间像素距离大于给 定阈值，则像素距离筛选失败，需要将匹配关系剔除。

(4)特征距离筛选

像素距离筛选完成以后，还需要进行特征距离筛选，在所有匹配点中按比例 保留特征距离较小的那些匹配点，筛选后的匹配点集合记为特征匹配和筛 选结果如图4所示，其中两帧图像间存在匹配关系的特征点对使用直线进行连接 以方便显示。由(a)中看出仅使用正向最近邻匹配存在诸多误匹配点对。这些误 匹配点对并非对应同一目标，但由于在特征空间中的相似性，错误地建立了匹配 关系。由(b)中看出通过对双向匹配的结果进行对称性筛选，误匹配的情况得到 有效缓解。由(c)中看出通过像素距离筛选，有效地剔除了在图像像素坐标系中距 离过大的误匹配点对。由(d)中看出通过特征距离筛选，匹配结果得到了进一步 提升。

1.3 2D-3D信息关联

特征匹配和筛选完成以后，2D特征点间的对应关系已经建立，还需要通过 深度图关联3D信息。2D-3D信息关联过程如图5所示，其中深度图以伪彩色进 行显示，颜色越红表示深度值越大，颜色越蓝则表示深度值越小，深度值无效区 域使用白色显示。任取匹配点对假设特征点和在 对应彩图中的图像像素坐标分别为(u_prev,v_prev)和(u_curr,v_curr)。若在前一帧深度图 中，图像像素坐标([u_prev],[v_prev])处存在有效深度值d，其中[·]表示对坐标值取整， 则2D-3D信息关联成功。最终形成的关联集可以表示为其中表示匹配点对个数。

1.4点云生成和降采样

将前一帧和当前帧深度图像分别转换为源点云和目标点云随后根据 不同需求，使用体素(Voxel)栅格对点云进行降采样，以减少点云中包含的点 的数量，从而提升算法计算速度。降采样过程如图6所示，使用体素栅格对3D 空间进行划分，点云中的每个点均看做质点，每个栅格中的点使用其质心进行近 似。

2误差函数构建及求解

2.1误差函数构建

利用预处理过程中获取的2D-3D信息关联集以及点云对应关系构建过程中 获取的对应点集合C构建误差函数预处理只进行一次，而对应关系构建和 误差函数构建求解迭代进行：给定一个待求解位姿的初值，然后进行点云对应 关系构建；根据获得的对应点集合构建误差函数并优化求解，从而对待求解位 姿进行更新；之后进行判断，若满足终止条件则终止，不满足则根据更新后的 位姿重新进行点云对应关系构建。

如公式所示，误差函数由2D项和3D项组成，其中表示变量的 齐次形式即K为标定已知的相机内参数矩阵，w为2D项权重。如 图7所示，3D项与传统ICP算法中的误差函数一致，用于衡量3D空间中源点经变换后与其最近邻点间的欧式距离；2D项则用于衡量2D图像中p_proj与 间的欧式距离，其中p_proj为2D特征点所对应的3D空间点经变换 后在当前帧图像中的投影点。

其中，误差函数由2D项和3D项组成，表示变量p的齐次形式 即K为标定已知的相机内参数矩阵，w为2D项权重，为源点云， 为目标点云，p_proj为2D特征点p_prev所对应的3D空间点P经变换后在当前 帧图像中的投影点，为平移分量，d为特征点对应的深度值。

2.2误差函数求解

为获取最优的R和t，需要求解如下所示带约束最小二乘问题

一方面，若使用拉格朗日乘子来施加约束会增大优化问题的复杂性，导致更 多的计算量；另一方面，若使用3×3的正交矩阵对R进行表示需要9个变量， 是一种过参数表示形式，会对优化过程造成负面影响。而根据群理论，每个李群 (Lie Group)如特殊正交群SO(3)和特殊欧式群也是光滑流形，可以使 用其对应的李代数(Lie Algebra)进行局部近似。李代数构成李群在幺元处的切 空间，可以在其上计算导数，也方便在优化过程中进行插值。此外李群和李代数 之间可以通过指数和对数操作进行映射。

作为一种最小参数化形式，李代数仅需要3个参数即可以对旋转R∈SO(3) 进行表示。若则

其中[·]_×表示向量生成的反对称矩阵。同理使用李代数仅需要6个参数即可 以对变换矩阵进行表示。若则

因此本发明使用李代数对位姿进行参数化表示，则可以重新整理为以下无约 束最小二乘的形式，易于求解。

其中，是对旋转矩阵R参数化表示形式，二者一一对 应；为[ω]_×，[·]_×表示向量生成的反对称矩阵； 表示变量p 的齐次形式即 就表示的齐次形式。

在优化求解最小二乘的过程中，关键是计算误差函数的雅克比(Jacobian) 矩阵。根据李群李代数的求导特性，2D项的雅克比矩阵为

其中，R为旋转矩阵。

而3D项的雅克比矩阵为

[I-[R·P+t]_×] (9)

其中，I为3x3的单位矩阵。

本发明提供了一种融合2D-3D信息的传感器相对位姿估计方法，运用迭代 最近点算法，该算法包括：预处理、点云对应关系构建、误差函数构建及求解， 使用2D-3D信息关联集和对应点集合构建误差函数并优化求解。基于上述算法， 针对空间目标近距离感知中的传感器相对位姿估计问题，计算精度显著提高， 鲁棒性显著提高。

以上，仅为本发明示例性的具体实施方式，但本发明的保护范围并不局限 于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想 到的变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范 围应以权利要求的保护范围为准。

Claims (10)

1.一种传感器相对位姿估计方法，其特征在于，包括以下步骤：

预处理步骤，使用2D特征检测子在前一帧彩图和当前帧彩图中进行特征点提取和描述，对所述特征点进行匹配和筛选以建立所述特征点间的对应关系，形成2D-3D信息关联集，分别将前一帧深度图和当前帧深度图转换为源点云和目标点云，对所述源点云和目标点云进行降采样；

点云对应关系构建步骤，构建所述源点云和目标点云间的对应关系以形成对应点集合；

误差函数构建及求解步骤，使用所述2D-3D信息关联集和所述对应点集合构建误差函数并优化求解，直至满足终止条件，若所述终止条件不满足，则重新进行所述点云对应关系构建。

2.如权利要求1所述的方法，其特征在于，所述预处理包括以下步骤：

使用SIFT在彩图中进行特征点提取和描述，所述SIFT包括特征检测子和描述子两部分，所述SIFT的检测结果包含特征点位置、主方向、检测时所处的尺度三方面信息；

按顺序依次对所述特征点进行双向最近邻匹配、对称性筛选、像素距离筛选、特征距离筛选，获得匹配点对；

任取所述匹配点对，建立图像像素坐标，若在前一帧深度图中，图像像素坐标处存在有效深度值，则2D-3D信息关联成功并形成关联集；

分别将前一帧深度图和当前帧深度图转换为源点云和目标点云，对所述源点云和目标点云进行降采样。

3.如权利要求2所述的方法，其特征在于，所述双向最近邻匹配使用所述特征点间的特征距离作为度量标准进行最近邻匹配，分为正向最近邻匹配和反向最近邻匹配；

所述正向最近邻匹配中，对于中的在中找到与其特征距离最小的特征点次小的特征点作为候选，若小于给定阈值，则所述与所述间匹配成功，反之则匹配不成立；

所述反向最近邻匹配与所述正向最近邻匹配类似但方向相反，通过反向匹配与建立匹配关系；

其中，表示前一帧彩图特征点集合，表示中的每个特征点，表示当前帧彩图特征点集合，表示中的每个特征点，表示特征点和之间的特征距离，i、j均为正整数。

4.如权利要求3所述的方法，其特征在于，所述对称性筛选包括以下步骤：

通过正向匹配与建立匹配关系，且通过反向匹配也与建立匹配关系，则对称性筛选通过，获得筛选后的匹配点集合；

否则对称性筛选失败，取消相应的匹配关系。

5.如权利要求4所述的方法，其特征在于，所述像素距离筛选包括以下步骤：

任取所述筛选后的匹配点集合中的匹配点对，若二者间像素距离大于给定阈值，则像素距离筛选失败，将匹配关系剔除。

6.如权利要求2所述的方法，其特征在于，所述特征距离筛选包括以下步骤：

在所有匹配点中按比例保留特征距离小于一定比例阈值α的匹配点构成匹配点集合。

7.如权利要求3所述的方法，其特征在于，所述误差函数构建包括以下步骤：

利用所述预处理过程中获取的2D-3D信息关联集以及点云对应关系构建过程中获取的对应点集合构建误差函数

其中，误差函数由2D项和3D项组成，表示变量p的齐次形式即K为标定已知的相机内参数矩阵，w为2D项权重，为源点云，为目标点云，p_proj为2D特征点p_prev所对应的3D空间点P经变换后在当前帧图像中的投影点，为平移分量，d为特征点对应的深度值，R为旋转矩阵。

8.如权利要求7所述的方法，其特征在于，所述误差函数求解包括以下步骤：

使用李代数对位姿进行参数化表示，表示为以下无约束最小二乘的形式：

其中，是对旋转矩阵R参数化表示形式，二者一一对应；为 表示向量生成的反对称矩阵； 表示变量p的齐次形式即 就表示的齐次形式。

9.如权利要求8所述的方法，其特征在于，优化求解所述最小二乘时，计算误差函数的雅克比矩阵，2D项的雅克比矩阵为：

其中，R为旋转矩阵。

10.如权利要求8所述的方法，其特征在于，优化求解所述最小二乘时，计算误差函数的雅克比矩阵，3D项的雅克比矩阵为：

[I -[R·P+t]_×]

其中，I为3x3的单位矩阵。