CN108287550A - 基于数据关联及错误检测的slam系统和构造数据关联的方法 - Google Patents

Abstract

本发明提供了一种基于数据关联及错误检测的SLAM系统和构造数据关联的方法，其中，系统包括前端模块，关键帧数据库；前端模块构造数据关联的过程包括：S100，接收普通帧及该普通帧的预测位姿T_predict；S200，从关键帧数据库中查找出与该普通帧相似的n个候选帧，将其分类为全局候选帧集合和局部候选帧集合；S300，计算每一候选帧到该普通帧的位姿变换关系T_pc，T_pc作为关联结果；S400，按候选帧的分类对关联结果作错误检测以滤除错误关联结果；S500，判断经过所述S400之后是否有正确的关联结果，若判断结果为是，则返回执行S100，若判断结果否，则执行S600；S600，根据临时区中的历史普通帧及其预测位姿、该普通帧及其预测位姿创建新的关键帧。

Description

基于数据关联及错误检测的SLAM系统和构造数据关联的方法

技术领域

本发明属于移动机器人领域，尤其涉及一种基于数据关联及错误检测的SLAM系统和构造数据关联的方法。

背景技术

移动式机器人的应用越来越普遍。例如，移动式机器人可以被用于移动式巡逻站。这样的移动式巡逻站配备有适当的传感器，使得它们能够在环境中自主地行驶，同时监测环境的异常状况，例如监测是否有非法入侵或者是否有行人跌倒等等一系列作业项目。

对于这样的移动式机器人，通常都有自主移动的需求，机器人的自主移动，一般都依赖于同时定位与地图构建(SLAM)技术。目前主流的SLAM技术主要使用了激光雷达或摄像机作为关联数据源。激光雷达方案的缺点是传感器价格比较贵，而且全局闭环能力相对较差，同时在机器人对环境的语义理解方面没有太多帮助，不太适合普遍地应用于消费级机器人。相应地，基于视觉的SLAM方案在移动机器人领域得到了广泛应用，而目前主流的视觉SLAM方案对算力都有较高的要求，都依赖于高帧率和高实时性运行，并且对使用场景也有一定要求，在低纹理或重复纹理区域都不能有鲁棒的结果，而且容易跟丢，视觉追踪失败后，需要有一套复杂的重定位机制，还需要机器人主动配合移动到之前走过的区域，这对于大部分没有人为干预的机器人系统来说，都是很难实现的，也是比较笨拙的行为，因此目前主流的视觉SLAM方案都难以落地于普通的嵌入式硬件平台上。

发明内容

为了解决背景技术中提到的问题，本发明提供一种基于数据关联及错误检测的SLAM系统，包括：前端模块，关键帧数据库；所述前端模块用于构造数据关联，关键帧数据库为关键帧的集合。前端模块构造数据关联的过程包括：

S100，接收数据，其中，数据至少包括普通帧及该普通帧的预测位姿T_predict；

S200，从关键帧数据库中查找出与该普通帧相似的n个候选帧，将n个候选帧分类为全局候选帧集合和局部候选帧集合；

S300，计算每一候选帧到该普通帧的位姿变换关系T_pc，如果T_pc计算成功，则确定该普通帧关联到了该候选帧，T_pc作为关联结果；

S400，按候选帧的分类对关联结果作错误检测以滤除错误关联结果；

S500，判断经过所述S400之后是否有正确的关联结果，若判断结果为是，则返回执行S100，若判断结果否，则执行S600；

S600，根据临时区中的历史普通帧及其预测位姿、该普通帧及其预测位姿创建新关键帧，若创建出了新关键帧，则将新关键帧存储于关键帧数据库，若未创建出新关键帧，则将该普通帧存储于临时区。

进一步实施例中，所述系统还包括后端模块，

所述S500中，判断结果为是时，还将该普通帧、该普通帧的预测位姿、正确的关联结果发送至后端模块；

所述S600中，创建出新关键帧时，还将关键帧发送至后端模块；

所述后端模块用于接收前端模块发送的数据，并将其存储于后端数据库，用于优化后端数据库中的关键帧。

进一步实施例中，所述S200中，查找候选帧之前还包括：根据后端数据库更新关键帧数据库。

进一步实施例中，所述S100中接收到数据之后还包括：

判断接收到的数据是否满足预定需求，若不满足，则丢弃接收到的数据，重新接收新的数据，否则，执行S200。

进一步实施例中，所述S100判断接收到的数据是否满足预定需求的过程包括：

S110，检测接收到的数据是否合法；

S120，提取该普通帧的特征点，检测特征点的个数是否满足需求；

S130，利用该普通帧估计光照，检测光照是否满足需求；

若S110至S130均满足，则接收到的数据满足预定需求，否则，接收到的数据不满足预定需求。

进一步实施例中，所述S200中从关键帧数据库中找出与该普通帧相似的n个候选帧的过程包括：

比较该普通帧与关键帧数据库中关键帧的相似度；

按相似度从大到小的顺序对关键帧数据库中关键帧进行排序；

提取前n个关键帧为候选帧。

进一步实施例中，所述S200中将n个候选帧分类为全局候选帧集合和局部候选帧集合的过程包括：

计算每一候选帧的位姿与该普通帧的预测位姿T_predict的相对位姿；

将大于第一阈值的相对位姿对应的候选帧划分至全局候选帧集合中，其余候选帧划分至局部候选帧集合中。

进一步实施例中，所述S200中将n个候选帧分类为全局候选帧集合和局部候选帧集合的过程包括：

计算候选帧的生成时间与该普通帧的生成时间的时间差，或候选帧上一次位姿被调整的时间与该普通帧的生成时间的时间差；

将大于第二阈值的时间差对应的候选帧划分至全局候选帧集合中，其余候选帧划分至局部候选帧集合中。

进一步实施例中，所述S400中对关联结果作错误检测的同时还统计关键帧的错误闭环次数、不稳定闭环次数及成功闭环次数。

进一步实施例中，所述S400中对每一候选帧i对应的关联结果T_pci，执行如下操作来进行错误检测：

S430，根据候选帧i的位姿T_ki和关联结果T_pci计算该普通帧的绝对位姿T_c；

S440，如果候选帧i属于局部候选帧集合，则根据该普通帧的绝对位姿T_c计算移动机器人的行驶速度，判断该行驶速度是否小于或等于最大行驶速度，若判断结果为否，则确定关联结果T_pci错误，反之，确定关联结果T_pci正确；

S450，如果候选帧i属于全局候选帧集合，则进行如下判断：

S451，确定该普通帧的预测位姿T_predict的置信范围，判断该普通帧的绝对位姿T_c是否在该普通帧的预测位姿T_predict的置信范围内，若判断结果为否，则确定关联结果T_pci错误，同时候选帧i的错误闭环次数加1，若判断结果为是，执行S452；

S452，将候选帧i暂存在闭环候选区，判断接下来连续n个普通帧是否都关联到候选帧i，且n个普通帧中的每一帧的绝对位姿均符合其预测位姿T_predict的置信范围，若判断结果为是，则确定关联结果T_pci正确，候选帧i的成功闭环次数加1，若判断结果为否，则确定关联结果T_pci错误，从闭环候选区剔除候选帧i，同时候选帧i的不稳定闭环次数加1。

进一步实施例中，所述S430之前还包括：

S410，根据候选帧i、该普通帧、关联结果T_pci及相机内参矩阵统计重投影误差的均方根，若均方根大于预定阈值，则确定关联结果T_pci错误；和/或

S420，判断该普通帧的绝对位姿T_c指示的状态与其他传感器指示的状态是否冲突，如果冲突，则确定关联结果T_pci错误。

进一步实施例中，所述S600中根据临时区中的历史普通帧及其预测位姿、该普通帧及其预测位姿创建新关键帧的过程包括：

S610，根据临时区中历史普通帧的预测位姿及该普通帧的预测位姿从临时区中查找出满足预定位姿条件的m个历史普通帧；

S620，对查找出的每一历史普通帧执行如下操作：

S621，搜索该历史普通帧与该普通帧之间的匹配特征点，判断匹配特征点个数是否大于第三阈值，若判断结果为是，执行S622，否则对下一历史普通帧执行操作；

S622，利用相机信息、该历史普通帧和该普通帧各自的预测位姿计算所述匹配特征点的3d坐标，判断3d坐标个数是否大于第四阈值，若判断结果为是，执行S623，否则对下一历史普通帧执行操作；

S623，利用Bundle Adjustment方法对匹配特征点的3d坐标和该普通帧的预测位姿进行优化以创建新关键帧。

进一步实施例中，当创建出新关键帧、有正确关联结果、系统出现异常任一情况发生时，清空所述临时区；和/或

设定临时区的存储容量，存储容量满时删除临时区内最早存入的普通帧；和/或

设定有效时间间隔，删除超过有效时间间隔的普通帧。

本发明还提供一种数据关联及其错误处理的方法，适用于SLAM系统的前端模块，包括：

S100，接收数据，其中，数据至少包括一普通帧及该普通帧的预测位姿T_predict；

S200，从关键帧数据库中查找出与该普通帧相似的n个候选帧，将n个候选帧分类为全局候选帧集合和局部候选帧集合；

S300，计算每一候选帧到该普通帧的位姿变换关系T_pc，如果T_pc计算成功，则确定该普通帧关联到了该候选帧，T_pc作为关联结果；

S400，按候选帧的分类对关联结果作错误检测以滤除错误关联结果；

S500，判断经过所述S400之后是否有正确的关联结果，若判断结果为是，则返回执行S100，若判断结果否，则执行S600；

S600，根据临时区中的历史普通帧及其预测位姿、该普通帧及其预测位姿创建新关键帧，若创建出了新关键帧，则将新关键帧存储于关键帧数据库，若未创建出新关键帧，则将该普通帧存储于临时区。

进一步实施例中，所述S500中，判断结果为是时，还将该普通帧、该普通帧的预测位姿T_predict、正确的关联结果发送至后端模块；

所述S600中，创建出新关键帧时，还将关键帧发送至后端模块；

其中，所述后端模块用于接收前端模块发送的数据，并将其存储于后端数据库，用于优化后端数据库中的关键帧。

进一步实施例中，所述S200中，查找候选帧之前还包括：根据后端数据库更新关键帧数据库。

进一步实施例中，所述S100中接收到数据之后还包括：

判断接收到的数据是否满足预定需求，若不满足，则丢弃接收到的数据，重新接收新的数据，否则，执行S200。

进一步实施例中，所述S100判断接收到的数据是否满足预定需求的过程包括：

S110，检测接收到的数据是否合法；

S120，提取该普通帧的特征点，检测特征点的个数是否满足需求；

S130，利用该普通帧估计光照，检测光照是否满足需求；

若S110至S130均满足，则接收到的数据满足预定需求，否则，接收到的数据不满足预定需求。

进一步实施例中，所述S200中从关键帧数据库中查找出与该普通帧相似的n个候选帧的过程包括：

比较该普通帧与关键帧数据库中关键帧的相似度；

按相似度从大到小的顺序对关键帧数据库中关键帧进行排序；

提取前n个关键帧为候选帧。

进一步实施例中，所述S200中将n个候选帧分类为全局候选帧集合和局部候选帧集合的过程包括：

计算每一候选帧的位姿与该普通帧的预测位姿T_predict的相对位姿；

将大于第一阈值的相对位姿对应的候选帧划分至全局候选帧集合中，其余候选帧划分至局部候选帧集合中。

进一步实施例中，所述S200中将n个候选帧分类为全局候选帧集合和局部候选帧集合的过程包括：

计算候选帧的生成时间与该普通帧的生成时间的时间差，或候选帧上一次位姿被调整的时间与该普通帧的生成时间的时间差；

将大于第二阈值的时间差对应的候选帧划分至全局候选帧集合中，其余候选帧划分至局部候选帧集合中。

进一步实施例中，所述S400中对关联结果作错误检测的同时还统计关键帧的错误闭环次数、不稳定闭环次数及成功闭环次数。

进一步实施例中，所述S400中对每一候选帧i对应的关联结果T_pci，执行如下操作来进行错误检测：

S430，根据候选帧i的位姿T_ki和关联结果T_pci计算该普通帧的绝对位姿T_c；

S440，如果候选帧i属于局部候选帧集合，则根据该普通帧的绝对位姿T_c计算移动机器人行驶速度，判断该移动机器人行驶速度是否小于或等于最大行驶速度，若判断结果为否，则确定关联结果T_pci错误，反之，确定关联结果T_pci正确；

S450，如果候选帧i属于全局候选帧集合，则进行如下判断：

S451，确定该普通帧的预测位姿T_predict的置信范围，判断该普通帧的绝对位姿T_c是否在该普通帧的预测位姿T_predict的置信范围内，若判断结果为否，则确定关联结果T_pci错误，同时候选帧i的错误闭环次数加1，若判断结果为是，执行S452；

S452，将候选帧i暂存在闭环候选区，判断接下来连续n个普通帧是否都关联到候选帧i，且n个普通帧中的每一帧的绝对位姿均符合其预测位姿T_predict的置信范围，若判断结果为是，则确定关联结果T_pci正确，候选帧i的成功闭环次数加1，若判断结果为否，则确定关联结果T_pci错误，从闭环候选区剔除候选帧i，同时候选帧i的不稳定闭环次数加1。

进一步实施例中，所述S430之前还包括：

S410，根据候选帧i、该普通帧、关联结果T_pci及相机内参矩阵统计重投影误差的均方根，若均方根大于预定阈值，则确定关联结果T_pci错误；和/或

S420，判断该普通帧的绝对位姿T_c指示的状态与其他传感器指示的状态是否冲突，如果冲突，则确定关联结果T_pci错误。

进一步实施例中，所述S600中根据临时区中的历史普通帧及其预测位姿、该普通帧及其预测位姿创建新关键帧的过程包括：

S610，根据临时区中历史普通帧的预测位姿及该普通帧的预测位姿从临时区中查找出满足预定位姿条件的m个历史普通帧；

S620，对查找出的每一历史普通帧执行如下操作：

S621，搜索该历史普通帧与该普通帧之间的匹配特征点，判断匹配特征点个数是否大于第三阈值，若判断结果为是，执行S622，否则对下一历史普通帧执行操作；

S622，利用相机信息、该历史普通帧和该普通帧各自的预测位姿计算所述匹配特征点的3d坐标，判断3d坐标个数是否大于第四阈值，若判断结果为是，执行S623，否则对下一历史普通帧执行操作；

S623，利用Bundle Adjustment方法对匹配特征点的3d坐标和该普通帧的预测位姿进行优化以创建新关键帧。

进一步实施例中，当创建出新关键帧、有正确关联结果、系统出现异常任一情况发生时，清空所述临时区；和/或

设定临时区的存储容量，存储容量满时删除临时区内最早存入的普通帧；和/或

设定有效时间间隔，删除超过有效时间间隔的普通帧。

本发明提供的基于数据关联及错误检测的SLAM系统和构造数据关联的方法，不依赖于环境纹理，对帧率也没有太高要求，没有复杂的初始化和重定位机制，为机器人提供全局不发散的定位，并且可以实时运行在低端嵌入式平台上。

附图说明

为了更清楚地说明本发明实施例的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本发明一实施例的基于数据关联及错误检测的SLAM系统的结构图；

图2为本发明一实施例的前端模块构造数据关联的流程图；

图3为本发明另一实施例的前端模块构造数据关联的流程图；

图4为本发明再一实施例的前端模块构造数据关联的流程图；

图5为本发明另一实施例的基于数据关联及错误检测的SLAM系统的结构图；

图6为本发明实施例的判断接收到的数据是否满足预定需求的过程的流程图。

具体实施方式

为了使本发明的技术特点及效果更加明显，下面结合附图对本发明的技术方案做进一步说明，本发明也可有其他不同的具体实例来加以说明或实施，任何本领域技术人员在权利要求范围内做的等同变换均属于本发明的保护范畴。

在本说明书的描述中，参考术语“一实施例”、“一具体实施例”、“一些实施例”、“比如”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。各实施例中涉及的步骤顺序用于示意性说明本发明的实施，其中的步骤顺序不作限定，可根据需要作适当调整。

如图1，图2所示，图1为本发明一实施例的基于数据关联及错误检测的SLAM系统的结构图，图2为本发明一实施例的前端模块构造数据关联的流程图。本实施例以错误检测为核心，对帧率也没有太高要求，没有复杂的初始化和重定位机制，能够不依赖于环境纹理，为机器人提供全局不发散的定位，并且可以实时运行在低端嵌入式平台上。

具体的，基于数据关联及错误检测的SLAM系统包括前端模块，关键帧数据库，所述前端模块用于构造数据关联，关键帧数据库为关键帧的集合。前端模块构造数据关联的过程包括：

S100，接收数据，其中，接收到的数据至少包括一普通帧F(即图像数据)以及该普通帧的预测位姿T_predict，预测位姿T_predict包括姿态及位置信息。详细的说，前端模块接收到的数据可以由机器人移动平台统一分发，或由传感器直接发送，普通帧F由移动机器人的摄像机获取，预测位姿T_predict由光电编码器、惯性测量单元(IMU)、以及上一普通帧数据计算的绝对位姿计算得到。

S200，从关键帧数据库中查找出与该普通帧F相似的n个候选帧，将n个候选帧分类为全局候选帧集合和局部候选帧集合。本步骤能够便于后续步骤有针对性的进行关联结果错误检测。

S300，计算每一候选帧到普通帧F的位姿变换关系T_pc，如果T_pc计算成功，则确定该普通帧关联到了该候选帧，T_pc作为关联结果。

S400，按候选帧的分类对关联结果作错误检测以滤除错误关联结果。本步骤能够在重复纹理区域具有鲁棒效果。

S500，判断经过所述S400之后是否有正确的关联结果，若判断结果为是，则返回执行S100，若判断结果否，则执行S600。

S600，根据临时区中的历史普通帧及其预测位姿、该普通帧F及其预测位姿创建新关键帧，若创建出了新关键帧，则将新关键帧存储于关键帧数据库，若未创建出新关键帧，则将该普通帧存储于临时区，其中，临时区存放有最近N个没有关联到任何候选帧且没有创建出新的关键帧的历史普通帧。

本发明一实施例中，如图4、图5所示，所述系统还包括后端模块。所述S500中，判断结果为是时，还将该普通帧、该普通帧的预测位姿、正确的关联结果发送至后端模块。具体实施时，还可计算该普通帧预测位姿的置信度及信息矩阵(信息矩阵用来描述数据关联结果的好坏，例如包括关联结果的置信度、重投影误差，匹配的特征点数量等)，将该普通帧预测位姿的置信度及信息矩阵发送至后端模块。所述S600中，创建出新关键帧时，还将新关键帧发送至后端模块。所述后端模块用于接收前端模块发送的数据，并将其存储于后端数据库，用于优化后端数据库中的关键帧，以及控制后端数据库的复杂度。详细的说，后端数据库由后端模块维护，是历史普通帧及关键帧的集合，后端模块会对关键帧进行优化，如删除冗余历史普通帧、冗余关键帧，调整历史普通帧和关键帧位姿等，具体参见后续实施例，此处不再赘述。

具体实施时，后端模块可与前端模块同步运行，也可在另外一个线程异步运行，后端模块由前端模块的发送的数据驱动执行。值得注意的是，创建新关键帧，局部或全局候选帧正确关联，这两种结果总是不会同时出现。

本发明一实施例中，如图4所示，所述S200中，查找候选帧之前还包括：根据后端数据库更新关键帧数据库。本步骤能够根据后端数据库中关键帧的变化对前端关键帧数据库进行调整，如删除关键帧、调整关键帧位姿等，保证前端关键帧数据库与后端数据库的信息一致，提高定位准确度。

本发明一实施例中，如图3所示，所述S100中接收到数据之后还包括：

判断接收到的数据是否满足预定需求，若不满足，则丢弃接收到的数据，重新接收新的数据，否则，执行S200。

本发明一实施例中，如图6所示，上述步骤S100判断接收到的数据是否满足预定需求的过程包括：

S110，检测接收到的数据是否合法；比如接收到的数据是否包括必要的数据，即至少包括普通帧及普通帧的预测位姿。具体实施时，接收到的数据还可包括其它传感器采集的数据，比如移动机器人倾角测量值、移动机器人是否打滑、移动机器人是否被抱起等。

S120，提取该普通帧F的特征点，检测特征点的个数是否满足需求。比如判断特征点个数是否大于80，具体需求可根据实际情况确定，本发明对此不作限定。

S130，利用该普通帧F估计光照，检测光照是否满足需求。比如通过光照检测摄像机是否有遮挡或者环境光照是否过暗等异常状态。

若S110至S130均满足，则接收到的数据满足预定需求，否则，接收到的数据不满足预定需求。

本发明一实施例中，上述S200中从关键帧数据库中查找出与该普通帧F相似的n个候选帧的过程包括：比较普通帧与关键帧数据库中关键帧的相似度；按相似度从大到小的顺序对关键帧数据库中关键帧进行排序；提取前n个关键帧为候选帧。具体实施时，可通过快速查询关键帧数据库中闭环检测子数据库来找到与该普通帧F相似度最高的n个关键帧，比较相似度的方法可以使用Bag of Word，也可以使用k-d树，具体采用哪种方法取决于闭环检测子数据库的存储方式，该闭环检测子数据库为用于加速闭环检测的逆向索引数据库，由关键帧数据库利用关键帧的特征点信息生成。

其它实施例中，还可按一定规则将关键帧数据库中所有关键帧分段，比如，从创建时间先后的维度将所有关键帧分为k段，每一段分别与该普通帧F进行比较相似度，取相似度最高的前n/k个关键帧加入候选帧集合。

本发明一实施例中，从空间层面上，上述S200中将n个候选帧分类为全局候选帧集合和局部候选帧集合的过程包括：

计算每一候选帧的位姿与普通帧F的预测位姿T_predict的相对位姿；

将大于第一阈值的相对位姿对应的候选帧划分至全局候选帧集合中，其余候选帧划分至局部候选帧集合中。实施时，可根据实际情况设定，本发明对其具体取值不做限定。

本发明一实施例中，从时间层面上，所述S200中将n个候选帧分类为全局候选帧集合和局部候选帧集合的过程包括：

计算候选帧的生成时间与普通帧F的生成时间的时间差，或该候选帧上一次位姿被调整的时间与普通帧F的生成时间的时间差；

将大于第二阈值的时间差对应的候选帧划分至全局候选帧集合中，其余候选帧划分至局部候选帧集合中。实施时，可根据实际情况设定，本发明对其具体取值不做限定。

其它实施例中，当系统出现一些异常情况时，比如出现了移动机器人打滑、移动机器人被抱起等，所有候选帧全部划分到全局候选帧集合。

本发明一实施例中，所述S300中计算每一候选帧j到该普通帧F的位姿变换关系T_pc的过程包括：首先，利用特征匹配的方法找到候选帧j中的2d特征点在该普通帧F对应的特征点；然后，根据上一步匹配出的特征点，使用epnp的方法或者优化的方法计算候选帧j到该普通帧F的位姿变换关系T_pc。相应地，如果这个T_pc计算成功，可以称之为该普通帧F观测到了候选帧j，T_pc作为关联结果。

本发明一实施例中，所述S400中对关联结果作错误检测的同时还统计关键帧的错误闭环次数、不稳定闭环次数及成功闭环次数。关键帧的错误闭环次数、不稳定闭环次数及成功闭环次数为后端数据库复杂度控制提供参考建议。比如，后端数据库可能需要删除一些关键帧，可根据错误闭环次数、不稳定闭环次数及成功闭环次数选择关键帧来删除。

进一步的，所述S400中对每一候选帧i对应的关联结果T_pci(即该候选帧i到该普通帧F的位姿变换关系T_pci)，执行如下操作来进行错误检测：

S430，根据候选帧i的位姿T_ki和关联结果T_pci计算该普通帧F的绝对位姿T_c，即：T_c＝T_k*T_pc。

S440，如果候选帧i属于局部候选帧集合，则根据该普通帧的绝对位姿T_c计算移动机器人行驶速度，并判断该行驶速度是否小于或等于最大行驶速度，若判断结果为否，则确定关联结果T_pci错误，不接受关联结果T_pci。反之，确定关联结果T_pci正确。详细的说，移动机器人的行驶速度可根据当前普通帧F的绝对位姿T_c及上一普通帧的绝对位姿计算得到，最大行驶速度由移动机器人生产厂商确定。

具体实施时，本步骤还包括：计算该普通帧的预测位姿T_predict的置信度，判断该普通帧预测位姿T_predict的置信度是否大于预定值，判断该普通帧F的绝对位姿T_c是否在普通帧预测位姿T_predict的误差范围内(比如，若设定预测位姿T_predictc的角度误差范围为±5°，若输入的预测位姿T_predict的角度为10°，而绝对位姿T_c的角度指示为16°，则超过了误差范围。若绝对位姿T_c的角度指示为14°，则符合预测位姿T_predict的角度误差范围)，只要有一个判断结果为否，则确定关联结果T_pci错误，反之，确定关联结果T_pci正确。

详细的说，该普通帧的预测位姿T_predict的置信度计算方式如下：利用光流估计出当前时刻摄像机相对于上一时刻摄像机的运动状态，若此运动状态与预测位姿T_predict指示的运动状态冲突，相应地降低该普通帧的预测位姿T_predict的置信度；或者当移动机器人被抱起或打滑时、或IMU指示的角度与底盘的光电编码器的计算角度冲突时，预测位姿T_predict的置信度也应降低。例如置信度是从0到1之间的数值，默认是1，根据冲突的程度相应降低该数值，如果不冲突，则置信度为1。

S450，如果候选帧i属于全局候选帧集合，则进行如下判断：

S451，确定该普通帧的预测位姿T_predict的置信范围，判断该普通帧的绝对位姿T_c是否在该普通帧的预测位姿T_predict的置信范围内，若判断结果为否，则确定关联结果T_pci错误，不接受该关联结果T_pci，同时候选帧i的错误闭环次数加1，若判断结果为是，执行S452。

详细的说，T_predict的置信范围可以设置为一关于传感器精度(所述传感器包括IMU和/或底盘的光电编码器；传感器精度，比如IMU的精度在出厂设置时是0.05°/s)、和/或当前时刻与上一次闭环时刻的时间差、和/或普通帧的预测位姿T_predict本身置信度的函数，该函数例如为传感器精度、当前时刻与上一次闭环时刻的时间差以及累计误差容忍度系数(可根据经验确定累计误差容忍度系数，本发明对其具体取值不做限定)的乘积，还可以为传感器精度、当前时刻与上一次闭环时刻的时间差、普通帧的预测位姿T_predict本身置信度的乘积。具体实施时，可根据需求合理设计该函数，本发明对该函数涉及的参数及其数量、函数表示形式及具体取值不做限定。其中，上次闭环时刻指的是上次确定形成闭环的时刻，若还未形成闭环，上次闭环时刻可以取0。以IMU为例，假设IMU的角度精度为0.05°/s，这是传感器出厂的精度，当前时刻与上一次闭环时刻的时间差为10秒，则IMU在此期间的累积误差为10*0.05＝0.5°，相应地乘以累计误差容忍度比如1.5，则由此确定的置信度范围为±(0.5*1.5＝0.75)°。

S452，将候选帧i暂存在闭环候选区，判断接下来连续n个普通帧(例如连续3帧，该参数根据移动机器人中摄像机安装方式以及运动方式等设定)是否都关联到候选帧i，且n个普通帧中的每一帧的绝对位姿均符合其预测位姿T_predict的置信度范围，若判断结果为是，则确定关联结果T_pci正确，作为成功的闭环，候选帧i的成功闭环次数加1，若判断结果为否，则确定关联结果T_pci错误，从闭环候选区剔除候选帧i，同时候选帧i的不稳定闭环次数加1。

更进一步的，为了提高错误检测的速度及精度，所述S430之前还包括：

S410，根据候选帧i、该普通帧F、关联结果T_pci及相机内参矩阵统计重投影误差的均方根，若均方根大于预定阈值，则确定关联结果T_pci错误，不接收关联结果T_pci，若均方根小于或等于预定阈值，则继续执行S420。重投影误差的均方根用于检测关联结果的质量，如果均方根大于预定阈值，则证明关联结果不满足质量。

S420，判断该普通帧的绝对位姿T_c指示的状态与其他传感器指示的状态是否冲突，如果冲突，则确定关联结果T_pci错误，若不冲突，则继续执行S430或S440。比如，T_c指示移动机器人离开地面了，而来自车轮离地检测的传感器并未触发，又比如，T_c指示移动机器人倾斜程度超过了来自倾角传感器的值。

本发明一实施例中，关键帧(KeyFrame)至少包括3d-2d点集合、3d点坐标及其绝对位姿。3d表示的是某个空间点在关键帧坐标系下的三维点，2d表示的是该空间点在图像坐标系下的二维点，3d-2d点集合指的是3d点与2d点的对应关系，可用索引关系表示。S600中，根据临时区中的历史普通帧及其预测位姿、该普通帧F及其预测位姿创建新关键帧的过程包括：

S610，根据临时区中历史普通帧的预测位姿及该普通帧F的预测位姿从临时区中查找出满足预定位姿条件的m个历史普通帧。详细的说，预定位姿条件例如为两帧基线距离大于或等于10厘米并且两帧相对转角在5度以内，其中，两帧基线距离及两帧相对转角根据临时区中历史普通帧的预测位姿及该普通帧F的预测位姿计算得到，本发明对预定位姿条件的具体设定值不做限定。m可以为一个也可以为多个，本发明对其具体取值不做限定。

S620，对查找出的每一历史普通帧i执行如下操作：

S621，搜索该历史普通帧i与普通帧F之间的匹配特征点，得到2d-2d点集合，即该历史普通帧i中的2d点与普通帧F中的2d点的对应关系，判断匹配特征点个数是否大于第三阈值(如80，本发明对其具体取值不做限定，可根据定位精度确定)，若判断结果为是，则执行S622，否则对下一历史普通帧执行操作。具体实施时，可利用极限约束来搜索两帧之间的匹配特征点，本发明对确定匹配特征点的方式不做限定。

S622，利用相机信息(相机模型和参数)、该历史普通帧i与普通帧F各自的预测位姿计算上述匹配特征点的3d坐标，得到3d-2d点集合，判断3d坐标个数是否大于第四阈值(如80或75，第四阈值应等于或小于第三阈值)，若判断结果为是，则执行S623，否则对下一历史普通帧执行操作。

S623，利用Bundle Adjustment方法对匹配特征点的3d坐标和该普通帧F的预测位姿进行优化以创建新关键帧。

详细的说，将该普通帧F的预测位姿、该历史普通帧i的预测位姿、S621得到的2d-2d点集合及S622得到的3d-2d点集合作为输入，利用Bundle Adjustment方法构造优化模型，对所述匹配特征点的3d坐标、该普通帧F的预测位姿及该历史普通帧i的预测位姿进行优化调整，得到优化后的3d-2d点集合、3d点坐标及绝对位姿(预测位姿优化后的值)，优化后的inliers(内点，也就是优化完成后符合优化结果的匹配特征点对)超过一定数量，如60个(该数量可以设定小于或等于第四阈值)，则判定创建新的关键帧成功，将优化后的3d-2d点集合、3d点坐标及绝对位姿作为关键帧的参量，流程结束，否则对下一历史普通帧执行操作。理论上，调整后的3d点坐标精度会比调整前高，调整后的位姿也比预测位姿精度高，具体实施时，为了降低计算复杂度及算力，还可固定该历史普通帧i的预测位姿，不对其进行调整。

本发明一实施例中，出于节省内存和提高位姿精度的考虑，临时区有如下几种情形被清空：当创建出新关键帧、有正确关联结果、系统出现异常任一情况发生时，清空所述临时区。进一步的，还可设定临时区的存储容量，存储容量满时删除临时区内最早存入的普通帧，和/或设定普通帧的有效时间间隔，删除超过有效时间间隔的普通帧。

本发明一实施例中，后端模块根据前端给出的后端数据库复杂度控制的参考建议(关键帧的错误闭环次数、不稳定闭环次数及成功闭环次数)优化后端数据库中的关键帧的过程包括：

遍历所述后端数据库中的关键帧：如果某个关键帧的错误闭环次数大于第五阈值，则删除该关键帧；如果某个关键帧的不稳定闭环次数大于第六阈值，则降低后端数据库图中所有关联到该关键帧的边的权重，其中，后端数据库图由顶点(node)及边(edge)构成，顶点表示一帧(关键帧或历史普通帧)的位姿，边连接两个顶点，表示两个顶点之间的位姿变换关系。

本发明一实施例中，关键帧数据库还可以在空间层面网格化，以为后端模块控制关键帧数据库的复杂度提供依据。例如，后端模块设定每个网格内允许同时存在的关键帧的最大数量，当某个网格内的关键帧数量超过该最大数量时，根据关键帧的被观测信息(成功闭环次数、不稳定闭环次数及错误闭环次数)和质量，选择最差的一个删除。

网格化的方法有很多，本发明对此不作限定。一具体实施例中，系统初始化时，将摄像机置于世界坐标系原点，以此为基础，将世界平面网格化(根据使用条件，可以是平面上的2d网格，也可以是空间上的3d网格)，以2d网格为例：网格具有大小和方向属性，根据结构上摄像机光轴与机器人运动方向的夹角、摄像机视场角、摄像机模型以及所选用的特征点的匹配性能来确定。比如移动机器人在水平面移动，使用一颗上视的鱼眼镜头，经测试，使用ORB特征点时，移动机器人在原地旋转所有角度下都能与第一帧有很好的匹配率，平移超过1米时，特征的匹配率开始下降，则网格大小可以设定为1米x1米，方向设定为360度。

本发明一实施例中，后端模块采用图优化的方式来对历史普通帧和关键帧的位姿进行优化。已知地，图由顶点(node)和边(edge)构成，在本发明的图中，node代表每一帧的位姿，edge连接两个node，表示两个node之间的位姿变换关系，构成node之间的约束。详细的说，edge分为预测边和观测边，预测边由前述的预测位姿T_predict构成，相应的信息矩阵就是T_predict的置信度；观测边由前述的绝对位姿T_pc构成，观测的质量构成了观测边的信息矩阵。由各个node代表的位姿可以计算出的两个node之间的位姿变化关系，连接这两个node之间的edge也表示了这两个node之间位姿变化关系，这两个位姿变化关系之间的误差构成了残差项，全局优化的目标是，通过调整各个node的位姿，使得整个图里的总体误差和最小。

后端模块将前端模块的正确关联结果关联，进行整体优化。值得注意的是，后端模块可以每一次都调用优化过程，也可以只在含有全局候选帧集合的关联结果时才调用整体优化过程，还可以由前端模块根据实际情况指定是否调用优化过程。

由前述图的构造过程可知，随着时间的推移，图里的边和顶点会越来越多，如果计算资源有限，后端模块需要负责控制后端数据库的复杂度，将算力需求控制在一定范围内。可以使用如下的方法控制后端数据库的复杂度并不是所有情况下都需要删除图里的node，可以设置一定的条件，例如，当普通帧的数量与关键帧的数量比超过一定阈值时，考虑删除node。当需要删除node时，可以选择图中连接强度最弱的一个node来删除，这里的连接强度可以表示为与该node连接的所有其他node之间的连通性。例如，有两个node，记为nodeA和nodeB，对于nodeA，有另外四个node与其相连，构成4条edge，但这四个node之间没有任何连接关系。对于nodeB，也有另外四个node与其相连，构成4条edge，且这四个node之间互相都有edge连接，此时称nodeA在图里的连接强度弱于nodeB，相比之下，可以选择nodeA来删除。

具体实施时，删除node的同时，可以将与该node连接的所有边直接简单地删除，也可以考虑将其信息融合在相关的edge中。例如，现在要删除nodeB，与nodeB连接的有nodeA和nodeC，nodeA与nodeB之间由边e₁连接，其代表的nodeA和nodeB之间的位姿变换矩阵为T_pc1，对应的旋转矩阵为R₁，相应的协方差矩阵为V₁，nodeB与nodeC之间由边e₂连接，其代表的nodeA和nodeB之间的位姿变换矩阵为T_pc2，对应的旋转矩阵为R₂，相应的协方差矩阵为V₂，删除nodeB的同时，e₁和e₂需要融合成新的边e₃，其位姿变换矩阵为：T_cp3＝T_cp1*T_pc2，协方差矩阵为：连接到nodeA和nodeC。

本发明提供的基于数据关联及错误检测的SLAM系统和构造数据关联的方法，不依赖于环境纹理，对帧率也没有太高要求，没有复杂的初始化和重定位机制，能为机器人提供全局不发散的定位，并且可以实时运行在低端嵌入式平台上。

本领域内的技术人员应明白，本发明的实施例可提供为方法、系统、或计算机程序产品。因此，本发明可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本发明可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。

本发明是参照根据本发明实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。

这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。

以上所述仅用于说明本发明的技术方案，任何本领域普通技术人员均可在不违背本发明的精神及范畴下，对上述实施例进行修饰与改变。因此，本发明的权利保护范围应视权利要求范围为准。

Claims (10)

1.一种基于数据关联及错误检测的SLAM系统，其特征在于，包括前端模块，关键帧数据库；所述前端模块用于构造数据关联，关键帧数据库为关键帧的集合；

前端模块构造数据关联的过程包括：

S100，接收数据，其中，数据至少包括一普通帧及该普通帧的预测位姿T_predict；

S200，从关键帧数据库中查找出与该普通帧相似的n个候选帧，将n个候选帧分类为全局候选帧集合和局部候选帧集合；

S300，计算每一候选帧到该普通帧的位姿变换关系T_pc，如果T_pc计算成功，则确定该普通帧关联到了该候选帧，T_pc作为关联结果；

S400，按候选帧的分类对关联结果作错误检测以滤除错误关联结果；

S500，判断经过所述S400之后是否有正确的关联结果，若判断结果为是，则返回执行S100，若判断结果否，则执行S600；

S600，根据临时区中的历史普通帧及其预测位姿、该普通帧及其预测位姿创建新关键帧，若创建出了新关键帧，则将新关键帧存储于关键帧数据库，若未创建出新关键帧，则将该普通帧存储于临时区。

2.如权利要求1所述的系统，其特征在于，所述系统还包括后端模块，

所述S500中，判断结果为是时，还将该普通帧、该普通帧的预测位姿T_predict、正确的关联结果发送至后端模块；

所述S600中，创建出新关键帧时，还将关键帧发送至后端模块；

所述后端模块用于接收前端模块发送的数据，并将其存储于后端数据库，用于优化后端数据库中的关键帧。

3.如权利要求2所述的系统，其特征在于，所述S200中，查找候选帧之前还包括：根据后端数据库更新关键帧数据库。

4.如权利要求1至3任一项所述的系统，其特征在于，所述S100中接收到数据之后还包括：

判断接收到的数据是否满足预定需求，若不满足，则丢弃接收到的数据，重新接收新的数据，否则，执行S200。

5.如权利要求4所述的系统，其特征在于，所述S100判断接收到的数据是否满足预定需求的过程包括：

S110，检测接收到的数据是否合法；

S120，提取该普通帧的特征点，检测特征点的个数是否满足需求；

S130，利用该普通帧估计光照，检测光照是否满足需求；

若S110至S130均满足，则接收到的数据满足预定需求，否则，接收到的数据不满足预定需求。

6.如权利要求4所述的系统，其特征在于，所述S200中将n个候选帧分类为全局候选帧集合和局部候选帧集合的过程包括：

计算每一候选帧的位姿与该普通帧的预测位姿T_predict的相对位姿；

将大于第一阈值的相对位姿对应的候选帧划分至全局候选帧集合中，其余候选帧划分至局部候选帧集合中。

7.如权利要求4所述的系统，其特征在于，所述S200中将n个候选帧分类为全局候选帧集合和局部候选帧集合的过程包括：

计算候选帧的生成时间与该普通帧的生成时间的时间差，或候选帧上一次位姿被调整的时间与该普通帧的生成时间的时间差；

将大于第二阈值的时间差对应的候选帧划分至全局候选帧集合中，其余候选帧划分至局部候选帧集合中。

8.如权利要求4所述的系统，其特征在于，所述S400中对关联结果作错误检测的同时还统计关键帧的错误闭环次数、不稳定闭环次数及成功闭环次数。

9.如权利要求8所述的系统，其特征在于，所述S400中对每一候选帧i对应的关联结果T_pci，执行如下操作来进行错误检测：

S430，根据候选帧i的位姿T_ki和关联结果T_pci计算该普通帧的绝对位姿T_c；

S440，如果候选帧i属于局部候选帧集合，则根据该普通帧的绝对位姿T_c计算移动机器人的行驶速度，判断该行驶速度是否小于或等于最大行驶速度，若判断结果为否，则确定关联结果T_pci错误，反之，确定关联结果T_pci正确；

S450，如果候选帧i属于全局候选帧集合，则进行如下判断：

S451，确定该普通帧的预测位姿T_predict的置信范围，判断该普通帧的绝对位姿T_c是否在该普通帧的预测位姿T_predict的置信范围内，若判断结果为否，则确定关联结果T_pci错误，同时候选帧i的错误闭环次数加1，若判断结果为是，执行S452；

S452，将候选帧i暂存在闭环候选区，判断接下来连续n个普通帧是否都关联到候选帧i，且n个普通帧中的每一帧的绝对位姿均符合其预测位姿T_predict的置信范围，若判断结果为是，则确定关联结果T_pci正确，候选帧i的成功闭环次数加1，若判断结果为否，则确定关联结果T_pci错误，从闭环候选区剔除候选帧i，同时候选帧i的不稳定闭环次数加1。

10.一种构造数据关联的方法，其特征在于，适用于SLAM系统的前端模块，包括：

S100，接收数据，其中，数据至少包括一普通帧及该普通帧的预测位姿T_predict；

S200，从关键帧数据库中查找出与该普通帧相似的n个候选帧，将n个候选帧分类为全局候选帧集合和局部候选帧集合；

S300，计算每一候选帧到该普通帧的位姿变换关系T_pc，如果T_pc计算成功，则确定该普通帧关联到了该候选帧，T_pc作为关联结果；

S400，按候选帧的分类对关联结果作错误检测以滤除错误关联结果；

S500，判断经过所述S400之后是否有正确的关联结果，若判断结果为是，则返回执行S100，若判断结果否，则执行S600；

S600，根据临时区中的历史普通帧及其预测位姿、该普通帧及其预测位姿创建新关键帧，若创建出了新关键帧，则将新关键帧存储于关键帧数据库，若未创建出新关键帧，则将该普通帧存储于临时区。