[发明专利]实现基于粒子滤波SLAM算法的方法及系统

说明书

技术领域

本发明涉及数字图象处理技术领域，尤其是涉及实现基于粒子滤波SLAM算法的方法及系统。

背景技术

基于粒子滤波的SLAM(Simultaneous Localization And Mapping，即时定位与地图构建)是一种强调实时性与准确性的算法，其在匹配优化等方面运算量很大，现行的基于软件的SLAM方案对硬件配置的要求很高。现行的SLAM方案一般只能顺序执行。例如，对于一个多粒子的SLAM算法来说，软件的处理方式就要依据粒子数量进行同等数量的循环处理。而一次的处理就需要经过运动位姿的匹配优化、将点插入地图，权重的计算等一系列处理，而循环多次其处理时间将大大增加。如果使用高性能的CPU当然可以解决这个问题，但是硬件成本也一样提高了。对于一个强调实时性的多传感器系统来说，处理时间十分重要。

发明内容

有鉴于此，本发明的目的在于提供实现基于粒子滤波SLAM算法的方法及系统，以降低成本和提高数据处理效率。

第一方面，本发明实施例提供了一种实现基于粒子滤波SLAM算法的方法，包括：

获取激光雷达数据和里程计数据；

根据原始地图的多个原始位姿信息，对所述里程计数据进行运动估计，同时得到多个第一位姿信息；

根据所述原始地图，对所述多个第一位姿信息同时进行匹配优化，得到多个第二位姿信息，并根据所述多个第二位姿信息与所述激光雷达数据计算得到第一权重值；

根据所述第一权重值对所述原始地图进行粒子滤波，得到多个更新粒子数据组；

根据所述多个更新粒子数据组，对所述原始地图进行更新，生成更新地图。

结合第一方面，本发明实施例提供了第一方面的第一种可能的实施方式，其中，所述原始地图包括多个原始粒子数据组，所述根据所述第一权重值对所述原始地图进行粒子滤波，得到多个更新粒子数据组包括：

根据所述第一权重值设置第一阈值和第二阈值；

判断每个所述原始粒子数据组是否满足预设条件，其中，预设条件包括第一预设条件、第二预设条件或第三预设条件；

如果每个所述原始粒子数据组满足所述第一预设条件，则将满足所述第一预设条件的原始粒子数据组进行淘汰，其中，所述第一预设条件为每个所述原始粒子数据组对应的第二权重值小于所述第一阈值；

如果每个所述原始粒子数据组满足所述第二预设条件，则将满足所述第二预设条件的原始粒子数据组按照预设的次数进行复制，其中，所述第二预设条件为每个所述原始粒子数据组对应的第二权重值大于所述第二阈值；

如果每个所述原始粒子数据组满足所述第三预设条件，则将满足所述第三预设条件的原始粒子数据组进行保留，其中，所述第三预设条件为每个所述原始粒子数据组对应的第二权重值位于所述第一阈值与第二阈值之间；

将复制的所述原始粒子数据组和保留的所述原始粒子数据组构成所述多个更新粒子数据组。

结合第一方面的第一种可能的实施方式，本发明实施例提供了第一方面的第二种可能的实施方式，其中，所述多个原始粒子数据组包括所述多个原始位姿信息和原始激光雷达数据。

结合第一方面的第一种可能的实施方式，本发明实施例提供了第一方面的第三种可能的实施方式，其中，所述预设次数是根据淘汰的原始粒子数据组的个数进行设置。

结合第一方面，本发明实施例提供了第一方面的第四种可能的实施方式，其中，所述方法还包括：

如果不存在所述原始地图，则直接根据所述激光雷达数据和所述里程计数据生成地图。

第二方面，本发明实施例还提供一种实现基于粒子滤波SLAM算法的系统，包括：数据接收模块、运动估计模块、匹配优化模块、粒子滤波模块和更新地图模块；

所述数据接收模块，用于获取激光雷达数据和里程计数据，并将所述里程计数据发送给所述运动估计模块，将所述激光雷达数据发送给所述匹配优化模块；

所述运动估计模块，用于根据原始地图的多个原始位姿信息，对所述里程计数据进行运动估计，同时得到多个第一位姿信息；

所述匹配优化模块，用于根据所述原始地图，对所述多个第一位姿信息同时进行匹配优化，得到多个第二位姿信息，并根据所述多个第二位姿信息与所述激光雷达数据计算得到第一权重值；

所述粒子滤波模块，用于根据所述第一权重值对所述原始地图进行粒子滤波，得到多个更新粒子数据组；

所述更新地图模块，用于根据所述多个更新粒子数据组，对所述原始地图进行更新，生成更新地图。