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

权利要求书

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

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

所述预处理包括以下步骤：

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

其中，R为旋转矩阵。

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

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

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