[发明专利]一种传感器相对位姿估计方法

说明书

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

技术领域

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

背景技术

迭代最近点(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) 等对检测的特征点进行描述，最后完成特征点匹配并利用匹配点估算一个较为合 理的初值。