高效鲁棒的基于多传感器的SLAM协调方法及系统

摘要

本发明公开了一种高效鲁棒的基于多传感器的SLAM协调方法，包括：协调各个传感器之间的时间戳和相对位置的同步；通过多个传感器分别获取机器人周围环境的上一帧和当前帧的空间坐标点集；根据上一帧和当前帧的空间坐标点集来计算机器人的位姿和位姿对应的权重；根据权重对所有传感器对应的位姿进行加权平均得到机器人真正的位姿。多个传感器即可以协调工作，由于多个传感器的视角范围广，可以减少盲区，以提高障碍物检测能力；而且，可以通过多个传感器获取更多的环境数据，所以可以极大地降低出现数据采集失败的情况，所以多传感器协调工作的策略可以提高即时定位与地图构建的成功率，并提高机器人成功采集数据的速度，以提高定位效率。

说明书

技术领域

本发明涉及通信技术领域，更具体的说，涉及高效鲁棒的基于多传感器 的SLAM协调方法及系统。

背景技术

机器人上通常会设置有传感器用于即时定位与地图构建，例如设置2D激 光器或3D体感摄影机Kinect等传感器。现有机器人上仅设置有单一的传感器， 从而通过单一的匹配策略来实现机器人的即时定位与地图构建，例如仅设置 有激光传感器或3DKinect。

在对现有技术的研究和实践过程中，本发明的发明人发现现有技术存在 以下问题：

由于通过单一的传感器能够获取到的环境数据较少，并且盲点极大，即 使在室内比较规则的环境下，也会经常出现数据采集失败的情况，所以单一 的匹配策略使得即时定位与地图构建的失败率增加；而且，单一的匹配策略 受环境变化影响较大，机器人只有在数据采集成功的情况下才能够继续行进， 所以如果机器人在当前的位置上采集数据失败的话，那么只能在当前的位置 上继续采集，知道数据采集成功为止，所以机器人的速度无法保障，从而导 致机器人定位的效率低。

因此，如何提高即时定位与地图构建的成功率，提高机器人成功采集数 据的速度，提高定位效率，成为目前最需要解决的问题。

发明内容

有鉴于此，本发明的设计目的在于，提供一种高效鲁棒的基于多传感器 的SLAM协调方法及系统，以提高即时定位与地图构建的成功率，提高机器 人的速度。

本发明实施例是这样实现的：

一种高效鲁棒的基于多传感器的SLAM协调方法，包括：

协调各个传感器之间的时间戳和相对位置的同步；

通过多个传感器分别获取机器人周围环境的上一帧和当前帧的空间坐标 点集；

根据所述上一帧和当前帧的空间坐标点集来计算所述机器人的位姿和所 述位姿对应的权重；

根据所述权重对所有传感器对应的位姿进行加权平均得到机器人真正的 位姿。

优选地，在根据所述上一帧和当前帧的空间坐标点集来计算所述机器人 的位姿和所述位姿对应的权重的步骤中，具体为：

当计算所述机器人的位姿的算法中仅包括基于特征点匹配算法时，则根 据所述上一帧的空间坐标点集计算第一特征点集，根据所述当前帧的空间坐 标点集计算第二特征点集，对所述第一特征点集和所述第二特征点集通过描 述符进行匹配，得到匹配点集，根据所述匹配点集计算位姿，并根据所述匹 配点集作为该传感器对应的位姿的权重。

优选地，在根据所述空间坐标点集来计算所述机器人的位姿和所述位姿 对应的权重的步骤中，具体为：

当计算所述机器人的位姿的算法中仅包括基于扫描匹配算法时，则对所 述上一帧的空间坐标点集和所述当前帧的空间坐标点集进行匹配，得到匹配 点集，根据匹配点集计算位姿，并利用所述传感器对应的上一帧的空间坐标 点集和当前帧的空间坐标点集的差值作为该传感器对应的位姿的权重。

优选地，在根据所述空间坐标点集来计算所述机器人的位姿和所述位姿 对应的权重的步骤中，具体为：

根据所述空间坐标点集通过基于扫描匹配算法和基于特征点匹配算法来 计算所述机器人的位姿和所述位姿对应的权重；

当所述空间坐标点集通过特征点匹配算法计算位姿时，则根据所述上一 帧的空间坐标点集计算第一特征点集，根据所述当前帧的空间坐标点集计算 第二特征点集，对第一特征点集和第二特征点集通过描述符进行匹配，得到 匹配点集，根据匹配点集计算位姿，并利用所述第一特征点集和第二特征点 集的差值作为该传感器对应的位姿的权重；

当所述空间坐标点集通过扫描匹配算法计算位姿时，则对上一帧的空间 坐标点集和当前帧的空间坐标点集进行匹配，得到匹配点集，根据匹配点集 计算位姿，并利用所述传感器对应的上一帧的空间坐标点集和当前帧的空间 坐标点集的差值作为该传感器对应的位姿的权重。

一种高效鲁棒的基于多传感器的SLAM协调系统，包括：

协调模块，用于协调各个传感器之间的时间戳和相对位置的同步；

传感器，用于通过多个传感器分别获取机器人周围环境的上一帧和当前 帧的空间坐标点集；

即时定位和地图构建模块，用于根据所述上一帧和当前帧的空间坐标点 集来计算所述机器人的位姿和所述位姿对应的权重；

位姿计算模块，用于根据所述权重对所有传感器对应的位姿进行加权平 均得到机器人真正的位姿。

优选地，所述即时定位和地图构建模块，具体用于当计算所述机器人的 位姿的算法中仅包括基于特征点匹配算法时，则根据所述上一帧的空间坐标 点集计算第一特征点集，根据所述当前帧的空间坐标点集计算第二特征点集， 对所述第一特征点集和所述第二特征点集通过描述符进行匹配，得到匹配点 集，根据所述匹配点集计算位姿，并根据所述匹配点集作为该传感器对应的 位姿的权重。

优选地，所述即时定位和地图构建模块，具体用于当计算所述机器人的 位姿的算法中仅包括基于扫描匹配算法时，则对所述上一帧的空间坐标点集 和所述当前帧的空间坐标点集进行匹配，得到匹配点集，根据匹配点集计算 位姿，并利用所述传感器对应的上一帧的空间坐标点集和当前帧的空间坐标 点集的差值作为该传感器对应的位姿的权重。

优选地，所述即时定位和地图构建模块，具体用于根据所述空间坐标点 集通过基于扫描匹配算法和基于特征点匹配算法来计算所述机器人的位姿和 所述位姿对应的权重；当所述空间坐标点集通过特征点匹配算法计算位姿时， 则根据所述上一帧的空间坐标点集计算第一特征点集，根据所述当前帧的空 间坐标点集计算第二特征点集，对第一特征点集和第二特征点集通过描述符 进行匹配，得到匹配点集，根据匹配点集计算位姿，并利用所述第一特征点 集和第二特征点集的差值作为该传感器对应的位姿的权重；当所述空间坐标 点集通过扫描匹配算法计算位姿时，则对上一帧的空间坐标点集和当前帧的 空间坐标点集进行匹配，得到匹配点集，根据匹配点集计算位姿，并利用所 述传感器对应的上一帧的空间坐标点集和当前帧的空间坐标点集的差值作为 该传感器对应的位姿的权重。

与现有技术相比，本实施例提供的技术方案具有以下优点和特点：

在本发明提供的方案中，在协调各个传感器之间的时间戳和相对位置同 步之后，多个传感器即可以协调工作，由于多个传感器的视角范围广，可以 减少盲区，以提高障碍物检测能力；而且，可以通过多个传感器获取更多的 环境数据，所以可以极大地降低出现数据采集失败的情况，所以多传感器协 调工作的策略可以提高即时定位与地图构建的成功率，并提高机器人成功采 集数据的速度，以提高定位效率。因此，本发明具有提高即时定位与地图构 建的成功率和提高定位效率等优点。

附图说明

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

图1为本发明所提供的一种高效鲁棒的基于多传感器的SLAM协调方法 的流程图；

图2为本发明所提供的另一种高效鲁棒的基于多传感器的SLAM协调方 法的流程图；

图3为本发明所提供的又一种高效鲁棒的基于多传感器的SLAM协调方 法的流程图；

图4为本发明所提供的又一种高效鲁棒的基于多传感器的SLAM协调方 法的流程图；

图5为本发明所提供的一种高效鲁棒的基于多传感器的SLAM协调系统 的模块图；

图6为本发明所提供的雷达的协调示意图；

图7为本发明所提供的Kinect传感器的协调示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行 清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而 不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做 出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本发明实施例提供了一种高效鲁棒的基于多传感器的SLAM协调方法， 以提高即时定位与地图构建的成功率，提高机器人的运行速度。由于上述高 效鲁棒的基于多传感器的SLAM协调方法的具体实现存在多种方式，下面通 过具体实施例进行详细说明：

实施例一

请参见图1所示，图1所示的为一种高效鲁棒的基于多传感器的SLAM 协调方法，该方法包括：

步骤S11、协调各个传感器之间的时间戳和相对位置的同步；

其中，SLAM的应为全称为simultaneous localization and mapping，中文解释为 即时定位与地图构建，或并发建图与定位。传感器包括很多个种类，例如， 雷达和Kinect等传感器，其中，Kinect为3D体感摄影机。

正常情况下，传感器获取的每一帧观测信息上面都会有一个时间戳，以 表示该帧观测信息的观测时间，每个传感器的时间戳可能都不相同，所以需 要对这些传感器的时间戳进行同步。具体的实现方式可以为，将所有传感器 的时间都统一设置成一个时间，保证所有的传感器在某一时刻下同时采集观 测信息，以便于后续步骤对同一时刻下每个传感器获取的观测信息进行使用。

步骤S12、通过多个传感器分别获取机器人周围环境的上一帧和当前帧的 空间坐标点集；

其中，传感器每次获取的环境信息为由多个空间坐标点构成的空间坐标 点集，每个传感器设置在机器人上面的位置都不同，所以设置在机器人上的 每个传感器获取的信息均为不同角度的环境信息。本步骤需要获取传感器采 集的上一帧和当前帧的空间坐标点集。

步骤S13、根据所述上一帧和当前帧的空间坐标点集来计算所述机器人的 位姿和所述位姿对应的权重；

其中，计算机器人的位姿的方式有很多种，主要包括两种方法，具体为 Scan-matching和Feature-matching。Scan-matching是基于扫描的匹配算法，简 称为扫描匹配算法，该算法在2D传感器的情况下计算的速度快，鲁棒性强， 但是3D情况下速度非常慢，最大的缺点是很难实现闭环；Feature-matching 是基于特征点匹配算法，简称为特征点匹配算法，该算法的计算速度较慢， 但是可以利用图优化技术实现闭环。扫描匹配算法和特征点匹配算法均有各 自的优点和缺陷，所以将多个传感器均采用不同的匹配算法来得到的位姿， 可以兼具上述两种算法的优点，摒弃两种算法的缺点。

对于位姿的权重，由于计算扫描匹配算法和特征匹配算法的权重的方式 各不相同，下文会通过更具体的实施例来进行介绍。此处需要说明的是，位 姿的权重代表着该位姿的重要性，对于权重越大的位姿，说明该位姿的重要 程度就越大，所以该位姿的参考价值就越大。

步骤S14、根据所述权重对所有传感器对应的位姿进行加权平均得到机器 人真正的位姿。

其中，通过上面的步骤进行处理后，每个传感器都会得到一个对应的位 姿和该位姿对应的权重。假设，传感器的数量为2个，第一传感器的位姿为 P1，P1对应的权重为W1，第二传感器的位姿为P2，P2对应的权重为W2， 假设W1具体为20%，W2具体为80%，所以机器人真正的位姿为 P=P1×20%+P2×80%，通过上述方案可以了解到，机器人真正的位姿是通过每 个传感器得到的位姿和该位姿所占权重计算而得到的，所以相对于现有技术 采用的单一传感器的策略，本发明计算位姿的方法成功率更高，采集速度更 快。

在图1所示的实施例中，在协调各个传感器之间的时间戳和相对位置同 步之后，多个传感器即可以协调工作，由于多个传感器的视角范围广，可以 减少盲区，以提高障碍物检测能力；而且，可以通过多个传感器获取更多的 环境数据，所以可以极大地降低出现数据采集失败的情况，所以多传感器协 调工作的策略可以提高即时定位与地图构建的成功率，并提高机器人成功采 集数据的速度，以提高定位效率。因此，本发明具有提高即时定位与地图构 建的成功率和提高定位效率等优点。

在实施例一中的步骤S13中提到，计算机器人的位姿的方式有很多种， 下面的实施例将对计算机器人的位姿的各种情况进行详细的介绍。

实施例二

请参见图2所示，图2所示的为另一种高效鲁棒的基于多传感器的SLAM 协调方法，本实施例提供的方案主要是介绍对于至少有两个传感器的情况， 其中，所有的传感器均采用特征点匹配算法计算位姿的方案，该方法包括：

步骤S21、协调各个传感器之间的时间戳和相对位置的同步；

其中，SLAM的应为全称为simultaneous localization and mapping，中文解释为 即时定位与地图构建，或并发建图与定位。

正常情况下，传感器获取的每一帧观测信息上面都会有一个时间戳，以 表示该帧观测信息的观测时间，每个传感器的时间戳可能都不相同，所以需 要对这些传感器的时间戳进行同步。具体的实现方式可以为，将所有传感器 的时间都统一设置成一个时间，保证所有的传感器在某一时刻下同时采集观 测信息，以便于后续步骤对同一时刻下每个传感器获取的观测信息进行使用。

步骤S22、通过多个传感器分别获取机器人周围环境的上一帧和当前帧的 空间坐标点集；

其中，传感器每次获取的环境信息为由多个空间坐标点构成的空间坐标 点集，每个传感器设置在机器人上面的位置都不同，所以设置在机器人上的 每个传感器获取的信息均为不同角度的环境信息。本步骤需要获取传感器采 集的上一帧和当前帧的空间坐标点集。

步骤S23、当计算所述机器人的位姿的算法中仅包括基于特征点匹配算法 时，则根据所述上一帧的空间坐标点集计算第一特征点集，根据所述当前帧 的空间坐标点集计算第二特征点集；

其中，在将上一帧的空间坐标点集计算第一特征点集和根据当前帧的空 间坐标点集计算第二特征点集的步骤中的计算均采用flirt算法。

步骤S24、对所述第一特征点集和所述第二特征点集通过描述符进行匹配 得到匹配点集；

其中，特征点包括坐标信息和描述符信息，通常情况下判断两个特征点 是否相同，是利用特征点对应的描述符进行的匹配的。例如，如果特征点A 和特征点B的描述符相同，那么我们可以判断特征点A和特征点B相同；如 果特征点A和特征点B的描述符不同，那么我们可以判断特征点A和特征点 B不同。第一特征点集包括一些特征点，这些特征点是通过上一帧的空间坐 标集计算出来的；第二特征点集也包括一些特征点，这些特征点是通过当前 帧的空间坐标集计算出来的。所以在对第一特征点集和第二特征点集进行匹 配时，需要匹配出第一特征点集和第二特征点集中的相同的特征点，即两个 特征点集中匹配符相同的特征点，并将匹配出的一些特征点作为匹配点集。 例如，第一特征点集和第二特征点集均包括10个特征点，这两个特征点集在 通过描述符进行匹配后，判断出第一特征点集中有8个特征点的描述符与第 二特征点集中的8个特征点的描述符相同，所以可以说明这8个描述符相同 的特征点是相匹配的，从而将这8个特征点作为匹配点集，以备后续步骤使 用。

步骤S25、根据所述匹配点集计算位姿；

其中，当传感器的数量具体为两个时，那么这两个传感器对应的位姿均 采用特征点匹配算法进行计算，所以就利用匹配点集中的特征点并采用特征 点匹配算法来计算该传感器对应的位姿。

需要说明的是，本实施例中开始已经限定在至少两个传感器的情况下， 所以在步骤S25中，计算得到的结果实际上是至少两个传感器中每个传感器 对应的位姿，但是本实施例仅仅是举例说明两个传感器的情况，所以并不局 限于两个传感器，也可以为多个传感器。

步骤S26、根据所述匹配点集作为该传感器对应的位姿的权重。

其中，假设传感器的数量为2个，包括第一传感器和第二传感器，第一 传感器对应的是第一匹配点集，第一匹配点集中包括10个特征点；第二传感 器对应的是第二匹配点集，包括20个特征点。第一传感器的权重为第一匹配 点集的特征点占据第一匹配特征点集与第二匹配特征点集之和的比例，第二 传感器的权重为第二匹配点集的特征点占据第一匹配特征点集与第二匹配特 征点集之和的比例。具体为，第一传感器的位姿的权重为10÷（10+20）=1/3， 第二传感器的位姿的权重为20÷（10+20）=2/3，所以第二传感器的位姿的权 重相对于第一传感器的位姿的权重更大一些，说明第二传感器的位姿的可靠 性更大一些。

步骤S27、根据所述权重对所有传感器对应的位姿进行加权平均得到机器 人真正的位姿。

其中，通过上面的步骤进行处理后，每个传感器都会得到一个对应的位 姿和该位姿对应的权重。假设，传感器的数量为2个，第一传感器的位姿为 P1，P1对应的权重为W1，第二传感器的位姿为P2，P2对应的权重为W2， 假设W1具体为1/3，W2具体为2/3，所以机器人真正的位姿为 P=P1×1/3+P2×2/3，通过上述方案可以了解到，机器人真正的位姿是通过每个 传感器得到的位姿和该位姿所占权重计算而得到的，所以相对于现有技术采 用的单一传感器的策略，本发明计算位姿的方法成功率更高，采集速度更快。

在图2所示的实施例中，本实施例与实施例一不同在于，本实施例提供 的方案主要是针对至少有两个传感器的情况，其中，所有的传感器均采用特 征点匹配算法计算位姿的方案。因此，本发明提供的实施例同样具有提高即 时定位与地图构建的成功率和提高定位效率等优点。

实施例三

请参见图3所示，图3所示的为另一种高效鲁棒的基于多传感器的SLAM 协调方法，本实施例提供的方案主要是介绍对于至少有两个传感器的情况， 其中，所有的传感器均采用扫描匹配算法计算位姿的方案，该方法包括：

步骤S31、协调各个传感器之间的时间戳和相对位置的同步；

其中，SLAM的应为全称为simultaneous localization and mapping，中文解释为 即时定位与地图构建，或并发建图与定位。

正常情况下，传感器获取的每一帧观测信息上面都会有一个时间戳，以 表示该帧观测信息的观测时间，每个传感器的时间戳可能都不相同，所以需 要对这些传感器的时间戳进行同步。具体的实现方式可以为，将所有传感器 的时间都统一设置成一个时间，保证所有的传感器在某一时刻下同时采集观 测信息，以便于后续步骤对同一时刻下每个传感器获取的观测信息进行使用。

步骤S32、通过多个传感器分别获取机器人周围环境的上一帧和当前帧的 空间坐标点集；

其中，传感器每次获取的环境信息为由多个空间坐标点构成的空间坐标 点集，每个传感器设置在机器人上面的位置都不同，所以设置在机器人上的 每个传感器获取的信息均为不同角度的环境信息。本步骤需要获取传感器采 集的上一帧和当前帧的空间坐标点集。

步骤S33、当计算所述机器人的位姿的算法中仅包括基于扫描匹配算法 时，则对所述上一帧的空间坐标点集和所述当前帧的空间坐标点集进行匹配， 得到匹配点集；

其中，空间坐标点包括坐标信息和描述符信息，通常情况下判断两个空 间坐标点是否相同，是利用空间坐标点对应的描述符进行的匹配的。例如， 如果空间坐标点A和空间坐标点B的描述符相同，那么我们可以判断空间坐 标点A和空间坐标点B相同；如果空间坐标点A和空间坐标点B的描述符不 同，那么我们可以判断空间坐标点A和空间坐标点B不同。在对上一帧的空 间坐标点集和当前帧的空间坐标点集进行匹配时，需要匹配出上一帧的空间 坐标点集和当前帧的空间坐标点集中的相同的空间坐标点，即两个空间坐标 点集中匹配符相同的空间坐标点，并将匹配出的一些空间坐标点作为匹配点 集。例如，第一空间坐标点集和第二空间坐标点集均包括10个空间坐标点， 这两个空间坐标点集在通过描述符进行匹配后，判断出上一帧的空间坐标点 集中有8个空间坐标点的描述符与当前帧的空间坐标点集中的8个空间坐标 点的描述符相同，所以可以说明这8个描述符相同的空间坐标点是相匹配的， 从而将这8个空间坐标点作为匹配点集，以备后续步骤使用。

步骤S34、根据所述匹配点集计算位姿；

其中，当传感器的数量具体为两个时，那么两个传感器对应的位姿均采 用扫描匹配算法进行计算，所以就利用匹配点集中的空间坐标点并采用扫描 匹配算法来计算该传感器对应的位姿。

需要说明的是，本实施例中开始已经限定在至少两个传感器的情况下， 所以在步骤S34中，计算得到的结果实际上是至少两个传感器中每个传感器 对应的位姿，但是本实施例仅仅是举例说明两个传感器的情况，所以并不局 限于两个传感器，也可以为多个传感器。

步骤S35、利用所述传感器对应的上一帧的空间坐标点集和当前帧的空间 坐标点集的差值作为该传感器对应的位姿的权重。

其中，假设传感器的数量为2个，包括第一传感器和第二传感器，第一 传感器上一帧的空间坐标点集的空间坐标点的数量为20个，第一传感器当前 帧的空间坐标点集的空间坐标点的数量为40个，所以第一传感器对应的上一 帧的空间坐标点集和当前帧的空间坐标点集的差值为20；第二传感器上一帧 的空间坐标点集的空间坐标点的数量为20个，第二传感器当前帧的空间坐标 点集的空间坐标点的数量为50个，所以第二传感器对应的上一帧的空间坐标 点集和当前帧的空间坐标点集的差值为30。具体为，第一传感器得到的差值 为20，第二传感器得到的差值为30，所以第一传感器的位姿的权重为20÷ （20+30）=2/5，第二传感器的位姿的权重为30÷（20+30）=3/5，所以第二传 感器的位姿的权重相对于第一传感器的位姿的权重更大一些，说明第二传感 器的位姿的可靠性更大一些。

步骤S36、根据所述权重对所有传感器对应的位姿进行加权平均得到机器 人真正的位姿。

其中，通过上面的步骤进行处理后，每个传感器都会得到一个对应的位 姿和该位姿对应的权重。假设，传感器的数量为2个，第一传感器的位姿为 P1，P1对应的权重为W1，第二传感器的位姿为P2，P2对应的权重为W2， 假设W1具体为2/5，W2具体为3/5，所以机器人真正的位姿为 P=P1×2/5+P2×3/5，通过上述方案可以了解到，机器人真正的位姿是通过每个 传感器得到的位姿和该位姿所占权重计算而得到的，所以相对于现有技术采 用的单一传感器的策略，本发明计算位姿的方法成功率更高，采集速度更快。

在图3所示的实施例中，本实施例与实施例一不同在于，本实施例提供 的方案主要是针对至少有两个传感器的情况，其中，所有的传感器均采用特 扫描匹配算法计算位姿的方案。因此，本发明提供的实施例同样具有提高即 时定位与地图构建的成功率和提高定位效率等优点。

实施例四

请参见图4所示，图4所示的为又一种高效鲁棒的基于多传感器的SLAM 协调方法，本实施例提供的方案主要是介绍对于至少有两个传感器的情况， 其中，至少有一个传感器采集的数据利用扫描匹配算法计算位姿，至少有一 个传感器采用的数据利用特征点匹配算法计算位姿，该方法包括：

步骤S41、协调各个传感器之间的时间戳和相对位置的同步；

其中，SLAM的应为全称为simultaneous localization and mapping，中文解释为 即时定位与地图构建，或并发建图与定位。

正常情况下，传感器获取的每一帧观测信息上面都会有一个时间戳，以 表示该帧观测信息的观测时间，每个传感器的时间戳可能都不相同，所以需 要对这些传感器的时间戳进行同步。具体的实现方式可以为，将所有传感器 的时间都统一设置成一个时间，保证所有的传感器在某一时刻下同时采集观 测信息，以便于后续步骤对同一时刻下每个传感器获取的观测信息进行使用。

步骤S42、通过多个传感器分别获取机器人周围环境的上一帧和当前帧的 空间坐标点集；

其中，传感器每次获取的环境信息为由多个空间坐标点构成的空间坐标 点集，每个传感器设置在机器人上面的位置都不同，所以设置在机器人上的 每个传感器获取的信息均为不同角度的环境信息。本步骤需要获取传感器采 集的上一帧和当前帧的空间坐标点集。

步骤S43、根据所述空间坐标点集通过基于扫描匹配算法和基于特征点匹 配算法来计算所述机器人的位姿和所述位姿对应的权重；

其中，在多个传感器获取的数据中，至少有一个传感器获得的数据是通 过基于扫描匹配算法计算位姿和该位姿对应的权重，至少有一个传感器获得 的数据是通过基于特征点匹配算法计算位姿和该位姿对应的权重。基于扫描 匹配算法的速度较快，而且基于特征点匹配算法能够实现闭环，所以本实施 例可以兼顾高速与闭环这两种非常重要的SLAM算法特性。

步骤S44、当所述空间坐标点集通过特征点匹配算法计算位姿时，则根据 所述上一帧的空间坐标点集计算第一特征点集，根据所述当前帧的空间坐标 点集计算第二特征点集，对第一特征点集和第二特征点集通过描述符进行匹 配，得到匹配点集，根据匹配点集计算位姿，并利用所述第一特征点集和第 二特征点集的差值作为该传感器对应的位姿的权重；

其中，在将上一帧的空间坐标点集计算第一特征点集和根据当前帧的空 间坐标点集计算第二特征点集的步骤中的计算均采用flirt算法。

特征点包括坐标信息和描述符信息，通常情况下判断两个特征点是否相 同，是利用特征点对应的描述符进行的匹配的。例如，如果特征点A和特征 点B的描述符相同，那么我们可以判断特征点A和特征点B相同；如果特征 点A和特征点B的描述符不同，那么我们可以判断特征点A和特征点B不同。 第一特征点集包括一些特征点，这些特征点是通过上一帧的空间坐标集计算 出来的；第二特征点集也包括一些特征点，这些特征点是通过当前帧的空间 坐标集计算出来的。所以在对第一特征点集和第二特征点集进行匹配时，需 要匹配出第一特征点集和第二特征点集中的相同的特征点，即两个特征点集 中匹配符相同的特征点，并将匹配出的一些特征点作为匹配点集。

例如，第一特征点集和第二特征点集均包括10个特征点，这两个特征点 集在通过描述符进行匹配后，判断出第一特征点集中有8个特征点的描述符 与第二特征点集中的8个特征点的描述符相同，所以可以说明这8个描述符 相同的特征点是相匹配的，从而将这8个特征点作为匹配点集，以备后续步 骤使用。

在根据所述匹配点集计算位姿的步骤中，利用匹配点集中的特征点并采 用特征点匹配算法来计算该传感器对应的位姿。

在利用所述第一特征点集和第二特征点集的差值作为该传感器对应的位 姿的权重的步骤中，第一传感器的第一特征点集的特征点的数量为20个，第 一传感器的第二特征点集的特征点的数量为40个，所以第一传感器对应的第 一特征点集和第二特征点集的差值为20。

步骤S45、当所述空间坐标点集通过扫描匹配算法计算位姿时，则对上一 帧的空间坐标点集和当前帧的空间坐标点集进行匹配，得到匹配点集，根据 匹配点集计算位姿，并利用所述传感器对应的上一帧的空间坐标点集和当前 帧的空间坐标点集的差值作为该传感器对应的位姿的权重。

其中，空间坐标点包括坐标信息和描述符信息，通常情况下判断两个空 间坐标点是否相同，是利用空间坐标点对应的描述符进行的匹配的。例如， 如果空间坐标点A和空间坐标点B的描述符相同，那么我们可以判断空间坐 标点A和空间坐标点B相同；如果空间坐标点A和空间坐标点B的描述符不 同，那么我们可以判断空间坐标点A和空间坐标点B不同。在对上一帧的空 间坐标点集和当前帧的空间坐标点集进行匹配时，需要匹配出上一帧的空间 坐标点集和当前帧的空间坐标点集中的相同的空间坐标点，即两个空间坐标 点集中匹配符相同的空间坐标点，并将匹配出的一些空间坐标点作为匹配点 集。例如，第一空间坐标点集和第二空间坐标点集均包括10个空间坐标点， 这两个空间坐标点集在通过描述符进行匹配后，判断出上一帧的空间坐标点 集中有8个空间坐标点的描述符与当前帧的空间坐标点集中的8个空间坐标 点的描述符相同，所以可以说明这8个描述符相同的空间坐标点是相匹配的， 从而将这8个空间坐标点作为匹配点集，以备后续步骤使用。

在根据所述匹配点集计算位姿的步骤中，利用匹配点集中的空间坐标点 并采用扫描匹配算法来计算该传感器对应的位姿。

在利用所述传感器对应的上一帧的空间坐标点集和当前帧的空间坐标点 集的差值作为该传感器对应的位姿的权重的步骤中，假设第二传感器上一帧 的空间坐标点集的空间坐标点的数量为20个，第二传感器当前帧的空间坐标 点集的空间坐标点的数量为50个，所以第二传感器对应的上一帧的空间坐标 点集和当前帧的空间坐标点集的差值为30。假设第一传感器对应的第一特征 点集和第二特征点集的差值为20。具体为，第一传感器得到的差值为20，第 二传感器得到的差值为30，所以第一传感器的位姿的权重为20÷(20+30）=2/5， 第二传感器的位姿的权重为30÷（20+30）=3/5，所以第二传感器的位姿的权 重相对于第一传感器的位姿的权重更大一些，说明第二传感器的位姿的可靠 性更大一些。

步骤S46、根据所述权重对所有传感器对应的位姿进行加权平均得到机器 人真正的位姿。

其中，通过上面的步骤进行处理后，每个传感器都会得到一个对应的位 姿和该位姿对应的权重。假设，传感器的数量为2个，第一传感器的位姿为 P1，P1对应的权重为W1，第二传感器的位姿为P2，P2对应的权重为W2， 假设W1具体为2/5，W2具体为3/5，所以机器人真正的位姿为 P=P1×2/5+P2×3/5，通过上述方案可以了解到，机器人真正的位姿是通过每个 传感器得到的位姿和该位姿所占权重计算而得到的，所以相对于现有技术采 用的单一传感器的策略，本发明计算位姿的方法成功率更高，采集速度更快。

在图4所示的实施例中，本实施例与实施例一不同在于，本实施例提供 的方案为至少有两个传感器的情况，其中，至少有一个传感器采集的数据利 用扫描匹配算法计算位姿，至少有一个传感器采用的数据利用特征点匹配算 法计算位姿，因此，基于扫描匹配算法的速度较快，而且基于特征点匹配算 法能够实现闭环，所以本实施例可以兼顾高速与闭环这两种非常重要的SLAM 算法特性。本发明提供的实施例同样具有提高即时定位与地图构建的成功率 和提高定位效率等优点。

实施例五

请参见图5所示，图5所示的为一种高效鲁棒的基于多传感器的SLAM 协调系统，该系统包括：协调模块11，用于协调各个传感器之间的时间戳和 相对位置的同步；传感器12，用于通过多个传感器分别获取机器人周围环境 的上一帧和当前帧的空间坐标点集；即时定位和地图构建模块13，用于根据 所述上一帧和当前帧的空间坐标点集来计算所述机器人的位姿和所述位姿对 应的权重；位姿计算模块14，用于根据所述权重对所有传感器对应的位姿进 行加权平均得到机器人真正的位姿。

在图5所示的实施例中，当计算所述机器人的位姿的算法中仅包括基于 特征点匹配算法时，即时定位和地图构建模块13，具体用于根据所述上一帧 的空间坐标点集计算第一特征点集，根据所述当前帧的空间坐标点集计算第 二特征点集，对所述第一特征点集和所述第二特征点集通过描述符进行匹配， 得到匹配点集，根据所述匹配点集计算位姿，并根据所述匹配点集作为该传 感器对应的位姿的权重。

在图5所示的实施例中，当计算所述机器人的位姿的算法中仅包括基于 扫描匹配算法时，所述即时定位和地图构建模块13，具体用于则对所述上一 帧的空间坐标点集和所述当前帧的空间坐标点集进行匹配，得到匹配点集， 根据匹配点集计算位姿，并利用所述传感器对应的上一帧的空间坐标点集和 当前帧的空间坐标点集的差值作为该传感器对应的位姿的权重。

在图5所示的实施例中，当计算所述机器人的位姿的算法中包括基于扫 描匹配算法和基于特征点匹配算法时，即时定位和地图构建模块13，具体用 于根据所述空间坐标点集通过基于扫描匹配算法和基于特征点匹配算法来计 算所述机器人的位姿和所述位姿对应的权重；当所述空间坐标点集通过特征 点匹配算法计算位姿时，则根据所述上一帧的空间坐标点集计算第一特征点 集，根据所述当前帧的空间坐标点集计算第二特征点集，对第一特征点集和 第二特征点集通过描述符进行匹配，得到匹配点集，根据匹配点集计算位姿， 并利用所述第一特征点集和第二特征点集的差值作为该传感器对应的位姿的 权重；当所述空间坐标点集通过扫描匹配算法计算位姿时，则对上一帧的空 间坐标点集和当前帧的空间坐标点集进行匹配，得到匹配点集，根据匹配点 集计算位姿，并利用所述传感器对应的上一帧的空间坐标点集和当前帧的空 间坐标点集的差值作为该传感器对应的位姿的权重。

实施例六

请参见图6所示，图6所示的为设置在机器人上的两个传感器，这两个 传感器均为雷达，包括雷达21和雷达22，由于两个雷达设置在不同水平面上， 所以两个雷达将获取不同的环境信息，从而获取了更多的环境信息，降低了 即时定位和地图构建的失败率。而且，雷达22获取的信息用快速的扫描匹配 算法进行计算，而雷达21则使用特征点匹配算法进行计算，从而使得该系统 在即时定位和地图构建的过程中既能够实现高速，又能实现闭环。

实施例七

请参见图7所示，图7所示的为设置在机器人上的两个传感器，两个传 感器均为3D传感器，具体为Kinect传感器，包括Kinect传感器31和Kinect 传感器32。由于Kinect传感器的视野比较局限，所以利用多个Kinect传感器 可以极大地增加数据获取的能力，降低了即时定位和地图构建的失败率。而 且，Kinect传感器32获取的数据利用精确但速度慢的SURF-Based SLAM算 法，Kinect传感器31则用快速但是误差较大的Orb-Based SLAM算法，所以 本系统既保证了速度，又提高了精度。其中，SURF-Based SLAM算法与 Orb-Based SLAM算法都属于特征点匹配算法，但是前者提取速度慢，精度高， 而后者提取速度快，精度低。

综上所述，本发明提供的方案具有很多优点，首先，由于多个传感器的 视角范围广，可以减少盲区，以提高障碍物检测能力；而且，可以通过多个 传感器获取更多的环境数据，所以可以极大地降低出现数据采集失败的情况， 所以多传感器协调工作的策略可以提高即时定位与地图构建的成功率，举例 来讲，设有2组SLAM模块，工作时每帧SLAM成功率分别P1、P2。两个 SLAM模块都成功时，该模块负责数据融合矫正，若其一失败，则以另一个 的数据弥补。通过我们这个模块合理并联后，整体SLAM成功率即可达 1-(1-P1)*(1-P2)，即P1+P2-P1*P2。另外，基于扫描匹配算法的速度较快，而 且基于特征点匹配算法能够实现闭环，所以本实施例可以兼顾高速与闭环这 两种非常重要的SLAM算法特性。

需要说明的是，图1至图7所示的实施例只是本发明所介绍的优选实施 例，本领域技术人员在此基础上，完全可以设计出更多的实施例，因此不在 此处赘述。

对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见 的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下， 在其它实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例， 而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。