基于粒子滤波SLAM的地图构建方法及装置制造方法及图纸

本发明专利技术公开了基于粒子滤波SLAM的地图构建方法及装置，通过对激光传感器数据通过迭代最近点算法进行扫描匹配，判断激光传感器数据是否适用；扫描匹配失败，则采用里程计数据，扫描匹配成功，则采用激光传感器数据；结合激光传感器数据和里程计数据进行采样并更新地图，避免数据单一，激光传感器数据具有精度更高、鲁棒性更强的特点，能提高有效粒子数，减少重采样；通过判断有效粒子数是否满足条件决定是否进行重采样，进一步减少重采样，且重采样采用自适应重采样；多方面改进使性能提高。

Method and device of map construction based on particle filter slam

【技术实现步骤摘要】
基于粒子滤波SLAM的地图构建方法及装置
本专利技术涉及导航领域，特别是基于粒子滤波SLAM的地图构建方法及装置。
技术介绍
SLAM技术(Simultaneouspositioningandmapbuilding)即是同时定位与地图创建技术。SLAM技术可以在部分已知或未知环境中利用载有的传感器感知环境并同时建图和定位。传统的SLAM技术只采用里程计运动模型的数据来采样，当遇到地面不平、轮子打滑的情况时，里程计运动模型准确性较低，导致采样得到的样本只能覆盖状态空间区域的一小部分，导致粒子之间权重差异大，得到的有效粒子数少。由于有效粒子数少，则需多次重采样，导致粒子耗散以及多样性降低，使性能降低。
技术实现思路
本专利技术的目的在于至少解决现有技术中存在的技术问题之一，提供基于粒子滤波SLAM的地图构建方法及装置。本专利技术解决其问题所采用的技术方案是：本专利技术的第一方面，基于粒子滤波SLAM的地图构建方法，包括以下步骤：获取步骤：获取激光传感器数据p(zt|xt，m)和里程计数据p(xt|xt-1，ut-1)：匹配步骤：通过迭代最近点算法对所述激光传感器数据进行扫描匹配；更新步骤：若扫描匹配失败，采用所述里程计数据更新地图；若扫描匹配成功，采用所述激光传感器数据更新地图；重采样步骤：计算有效粒子数Neff，当所述有效粒子数小于粒子总数的一半时，进行自适应重采样补充所述激光传感器数据和所述里程计数据，否则直接返回获取步骤；其中，所述采用里程计数据更新地图包括以下步骤：根据所述里程计数据从t-1时刻的粒子集合Yt-1采样获取t时刻的粒子集合Yt；计算t时刻的粒子集合Yt中的每个粒子的权重；根据设定的第一阈值对t时刻的粒子集合Yt中的粒子筛选以生成第一最终粒子集，并使第一最终粒子集的所有粒子具有相同的权重；根据第一最终粒子集的每个粒子中包含的运动轨迹信息和观测信息生成地图；其中，所述采用激光传感器数据更新地图包括以下步骤：将所述激光传感器数据融合至建议分布，并从融合激光传感器数据的建议分布中采样获取t时刻的粒子集合Yt，其中融合激光传感器数据的建议分布为计算t时刻的粒子集合Yt中的每个粒子的权重，其中权重为根据设定的第二阈值对t时刻的粒子集合Yt中的粒子筛选以生成第二最终粒子集，并使第二最终粒子集的所有粒子具有相同的权重；根据第二最终粒子集的每个粒子中包含的运动轨迹信息和观测信息生成地图。根据本专利技术的第一方面，所述通过迭代最近点算法对所述激光传感器数据进行扫描匹配具体为：设所述激光传感器数据的点集P为P＝{p1，p2，...，pm)和参考点集Q为Q＝{q1，q2，...，qn}，计算所述激光传感器数据的点集P与所述参考点集Q间的变换T＝{R，t}，使存在最优解，其中N为最优解下成功配对的点对数。根据本专利技术的第一方面，比较N与设定的第三阈值；若N大于第三阈值，则扫描匹配成功；否则扫描匹配失败。根据本专利技术的第一方面，所述重采样步骤中的有效粒子数Neff为根据本专利技术的第一方面，根据粒子中包含的运动轨迹信息和观测信息生成地图具体为：通过占据栅格地图算法生成二维平面栅格图，其中所述二维平面栅格图表示为x1t为运动轨迹信息，z1t为观测信息。本专利技术的第二方面，基于粒子滤波SLAM的地图构建装置，包括：获取模块，用于获取激光传感器数据p(zt|xt，m)和里程计数据p(xt|xt-1，ut-1)；匹配模块，用于通过迭代最近点算法对所述激光传感器数据进行扫描匹配；更新模块，用于根据扫描匹配结果更新地图；所述更新模块包括第一更新子模块和第二更新子模块；所述第一更新子模块用于在扫描匹配结果为失败时，采用所述里程计数据更新地图；所述第二更新子模块用于在扫描匹配结果为成功时，采用所述激光传感器数据更新地图；重采样模块，用于计算有效粒子数Neff，当所述有效粒子数小于粒子总数的一半时，进行自适应重采样补充所述激光传感器数据和所述里程计数据，否则直接返回获取模块执行；其中，所述第一更新子模块包括：第一粒子初采样模块，用于根据所述里程计数据从t-1时刻的粒子集合Yt-1采样获取t时刻的粒子集合Yt；第一权重计算模块，用于计算t时刻的粒子集合Yt中的每个粒子的权重；第一粒子重采样模块，用于根据设定的第一阈值对t时刻的粒子集合Yt中的粒子筛选以生成第一最终粒子集，并使第一最终粒子集的所有粒子具有相同的权重；第一地图生成模块，用于根据第一最终粒子集的每个粒子中包含的运动轨迹信息和观测信息生成地图；其中，所述第二更新子模块包括：第二粒子初采样模块，用于将所述激光传感器数据融合至建议分布，并从融合激光传感器数据的建议分布中采样获取t时刻的粒子集合Yt，其中融合激光传感器数据的建议分布为第二权重计算模块，用于计算t时刻的粒子集合Yt中的每个粒子的权重，其中权重为第二粒子重采样模块，用于根据设定的第二阈值对t时刻的粒子集合Yt中的粒子筛选以生成第二最终粒子集，并使第二最终粒子集的所有粒子具有相同的权重；第二地图生成模块，用于根据第二最终粒子集的每个粒子中包含的运动轨迹信息和观测信息生成地图。根据本专利技术的第二方面，所述匹配模块具体配置为：设所述激光传感器数据的点集P为P＝{p1，p2，...，pm}和参考点集Q为Q＝{q1，q2，...，qn}，计算所述激光传感器数据的点集P与所述参考点集Q间的变换T＝{R，t}，使存在最优解，其中N为最优解下成功配对的点对数。根据本专利技术的第二方面，所述更新模块包括判定模块；所述判定模块用于比较N与设定的第三阈值，若N大于第三阈值，则判定为扫描匹配成功，否则判定为扫描匹配失败。根据本专利技术的第二方面，所述重采样模块中的有效粒子数Neff具体为根据本专利技术的第二方面，在所述第一地图生成模块和所述第二地图生成模块中，通过占据栅格地图算法生成二维平面栅格图，其中所述二维平面栅格图表示为x1t为运动轨迹信息，z1t为观测信息。上述技术方案至少具有以下的有益效果：对激光传感器数据通过迭代最近点算法进行扫描匹配，判断激光传感器数据是否适用；扫描匹配失败，则采用里程计数据，扫描匹配成功，则采用激光传感器数据；结合激光传感器数据和里程计数据进行采样并更新地图，避免数据单一，激光传感器数据具有精度更高、鲁棒性更强的特点，能提高有效粒子数，减少重采样；通过判断有效粒子数是否满足条件决定是否进行重采样，进一步减少重采样，且重采样采用自适应重采样；多方面改进使性能提高。本专利技术的附加方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本专利技术的实践了解到。附图说明下面结合附图和实例对本专利技术作进一步说明。图1是本专利技术实施例图基于粒子滤波SLAM的地图构建方法的流程图；图2是本专利技术实施例图基于粒子滤波SL本文档来自技高网...

【技术保护点】
1.基于粒子滤波SLAM的地图构建方法，其特征在于，包括以下步骤：/n获取步骤：获取激光传感器数据p(z

【技术特征摘要】
1.基于粒子滤波SLAM的地图构建方法，其特征在于，包括以下步骤：
获取步骤：获取激光传感器数据p(zt|xt，m)和里程计数据p(xt|xt-1，ut-1)；
匹配步骤：通过迭代最近点算法对所述激光传感器数据进行扫描匹配；
更新步骤：若扫描匹配失败，采用所述里程计数据更新地图；
若扫描匹配成功，采用所述激光传感器数据更新地图；重采样步骤：计算有效粒子数Neff，当所述有效粒子数小于粒子总数的一半时，进行自适应重采样补充所述激光传感器数据和所述里程计数据，否则直接返回获取步骤；
其中，所述采用里程计数据更新地图包括以下步骤：
根据所述里程计数据从t-1时刻的粒子集合Yt-1采样获取t时刻的粒子集合Yt；
计算t时刻的粒子集合Yt中的每个粒子的权重；
根据设定的第一阈值对t时刻的粒子集合Yt中的粒子筛选以生成第一最终粒子集，并使第一最终粒子集的所有粒子具有相同的权重；
根据第一最终粒子集的每个粒子中包含的运动轨迹信息和观测信息生成地图；
其中，所述采用激光传感器数据更新地图包括以下步骤：
将所述激光传感器数据融合至建议分布，并从融合激光传感器数据的建议分布中采样获取t时刻的粒子集合Yt，其中融合激光传感器数据的建议分布为
计算t时刻的粒子集合Yt中的每个粒子的权重，其中权重为wt(i)＝p(zt|mt-1(i)，xt-1(i)，ut-1)·wt-1(i)；
根据设定的第二阈值对t时刻的粒子集合Yt中的粒子筛选以生成第二最终粒子集，并使第二最终粒子集的所有粒子具有相同的权重；
根据第二最终粒子集的每个粒子中包含的运动轨迹信息和观测信息生成地图。


2.根据权利要求1所述的基于粒子滤波SLAM的地图构建方法，其特征在于，所述通过迭代最近点算法对所述激光传感器数据进行扫描匹配具体为：设所述激光传感器数据的点集P为P＝{p1，p2，...，pm}和参考点集Q为Q＝{q1，q2，...，qn}，计算所述激光传感器数据的点集P与所述参考点集Q间的变换T＝{R，t}，使存在最优解，其中N为最优解下成功配对的点对数。


3.根据权利要求2所述的基于粒子滤波SLAM的地图构建方法，其特征在于，比较N与设定的第三阈值；若N大于第三阈值，则扫描匹配成功；否则扫描匹配失败。


4.根据权利要求1所述的基于粒子滤波SLAM的地图构建方法，其特征在于，所述重采样步骤中的有效粒子数Neff为


5.根据权利要求1所述的基于粒子滤波SLAM的地图构建方法，其特征在于，根据粒子中包含的运动轨迹信息和观测信息生成地图具体为：通过占据栅格地图算法生成二维平面栅格图，其中所述二维平面栅格图表示为x1t为运动轨迹信息，z1t为观测信息。


6.基于粒子滤波SLAM的地图构建装置，其特征在于，包括：
获取模块，用于获取激光传感器数据p(zt|xt，m)和里程计数据p(xt|x...

【专利技术属性】
技术研发人员：黄辉，邹安安，邹媛媛，胡鹏，蔡庆荣，江励，王宏民，王琼瑶，汤健华，梁艳阳，王广龙，
申请(专利权)人：五邑大学，
类型：发明
国别省市：广东;44