[发明专利]一种机器人同步定位和地图构建方法和系统

权利要求书

1.一种同步定位和地图构建SLAM方法，应用于机器人在未知环境中的定位和地图构建，其特征在于，包括：

A、从图像中提取线段特征、交叉点和消隐点，根据交叉点在图像之间建立线段的匹配，根据线段匹配的结果进行相机位姿估计，选取新关键帧；其中，消隐点是指一组平行线在图像平面上的交汇点；

B、在地图中插入关键帧，计算新加入的线段特征的三维坐标，并进行局部地图的集束调整，剔除野点观测；

C、基于交叉点进行闭环检测和全局的地图优化；

其中，所述B包括：

在局部地图的集束调整中，建立消除线段的重投影误差和消隐点的重投影误差的误差函数，其中：

线段的重投影误差定义为平面上检测的线段与投影线段的两个端点的距离之和；

消隐点的重投影误差的定义为投影到单位球面上的消隐点坐标和实际检测的消隐点坐标之间的距离；

如果线段的重投影误差超过设定阈值，则剔除该线段，如果消隐点的重投影误差超过阈值，则剔除该消隐点。

2.根据权利要求1所述的方法，其特征在于，所述A中从图像中提取线段特征、交叉点和消隐点包括：

A1、采集当前图像，从当前图像中提取线段特征；

A2、提取线段的交叉点；

A3、提取交叉点的描述子；

A4、根据描述子建立交叉点的初始匹配，利用对极几何和非刚性匹配消除错误的匹配；

A5、在线段特征的基础上提取消隐点。

3.根据权利要求2所述的方法，其特征在于，所述A2包括：

如果两条线段满足以下三个条件，则判定所述两条线段构成一个候选的交叉点：

条件1：两条线段的长度均大于一设定阈值；

条件2：交叉点到两条线段的距离均小于一设定阈值；

条件3：如果两条线段延长之后交叉，交叉点须位于图像平面内。

4.根据权利要求2所述的方法，其特征在于，所述A3包括：

定义交叉点的方向和尺度，其中，将交叉点的方向定义为构成该交叉点的两条线段的中分线，并采用至少两个尺度的策略；

将交叉点视为特征点，在交叉点周围提取描述子。

5.根据权利要求2所述的方法，其特征在于，所述A4中根据描述子建立交叉点的初始匹配包括：

对于双目图像和非双目图像采用不同的策略进行匹配，其中：

对于双目摄像头获取的双目图像，根据所述双目图像中的一幅图像中给定的交叉点，在另一幅图像的极线附近寻找匹配的交叉点；如果在极线附近存在匹配的交叉点，则计算两个交叉点之间的描述子距离；如果描述子距离小于设定的阈值，则判定所述两个交叉点构成一对匹配；

对于非双目图像，根据所述非双目图像中的一幅图像中给定的交叉点，在另一幅图像中寻找描述子距离最小、以及距离次小的交叉点，并计算最小距离和次小距离的比值，如果所述比值小于设定的阈值，则判定所述一幅图像中给定的交叉点与所述另一幅图像中距离最小的交叉点构成一对匹配。

6.根据权利要求1至5任一项所述的方法，其特征在于，所述误差函数为：

其中，ρ(·)是鲁棒的柯西函数；

e_l是一个四维向量，表示线段的重投影误差；

e_v是一个三维向量，表示消隐点的重投影误差；

和分别是4×4和3×3的对角阵，分别表示线段和消隐点的信息矩阵；

i是图像的索引；

k是线段的索引；

j是属于同一组平行线的所有直线的索引。

7.根据权利要求1所述的方法，其特征在于，所述C包括：

C1、根据当前图像的交叉点描述子，构建所述描述子的词袋特征，并利用当前图像的词袋特征，从关键帧中挑选相似度大于设定阈值的帧作为候选关键帧，然后将当前图像与候选关键帧之间进行几何一致性验证；如果一候选关键帧通过了几何一致性验证，则认为检测到闭环；

C2、当检测到闭环后，对关键帧建立消除线段的重投影误差和消隐点的重投影误差的误差函数，进行全局地图的优化。

8.一种同步定位和地图构建SLAM系统，应用于机器人在未知环境中的定位和地图构建，其特征在于，包括：

运动估计模块，用于从图像中提取线段特征、交叉点和消隐点，根据交叉点在图像之间建立线段的匹配，根据线段匹配的结果进行相机位姿估计，选取新关键帧；其中，消隐点是指一组平行线在图像平面上的交汇点；

局部地图更新模块，用于在地图中插入关键帧，计算新加入的线段特征的三维坐标，并进行局部地图的集束调整，剔除野点观测；所述局部地图更新模块具体用于：在局部地图的集束调整中，建立消除线段的重投影误差和消隐点的重投影误差的误差函数，其中：

线段的重投影误差定义为平面上检测的线段与投影线段的两个端点的距离之和；

消隐点的重投影误差的定义为投影到单位球面上的消隐点坐标和实际检测的消隐点坐标之间的距离；

如果线段的重投影误差超过设定阈值，则剔除该线段，如果消隐点的重投影误差超过阈值，则剔除该消隐点；

闭环优化模块，用于基于交叉点进行闭环检测和全局的地图优化。