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

权利要求书

1.一种实现基于粒子滤波SLAM算法的方法，其特征在于，包括：

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

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

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

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

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

2.根据权利要求1所述的实现基于粒子滤波SLAM算法的方法，其特征在于，所述原始地图包括多个原始粒子数据组，所述根据所述第一权重值对所述原始地图进行粒子滤波，得到多个更新粒子数据组包括：

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

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

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

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

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

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

3.根据权利要求2所述的实现基于粒子滤波SLAM算法的方法，其特征在于，所述多个原始粒子数据组包括所述多个原始位姿信息和原始激光雷达数据。

4.根据权利要求2所述的实现基于粒子滤波SLAM算法的方法，其特征在于，所述预设次数是根据淘汰的原始粒子数据组的个数进行设置。

5.根据权利要求1所述的实现基于粒子滤波SLAM算法的方法，其特征在于，所述方法还包括：

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

6.一种实现基于粒子滤波SLAM算法的系统，其特征在于，包括：数据接收模块、运动估计模块、匹配优化模块、粒子滤波模块和更新地图模块；

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

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

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

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

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