[发明专利]基于自适应蒙特卡洛和特征匹配的全局融合定位方法

摘要

本发明公开了一种基于自适应蒙特卡洛和特征匹配的全局融合定位方法，其包括提取栅格地图中的地图线特征存储至地图特征数据库中；提取非第二帧激光雷达扫描图的扫描图线特征，采用第一帧激光雷达扫描图计算的估算位姿对自适应蒙特卡洛的粒子集位姿初始化；对于第二帧激光雷达扫描图，采用里程计数据、运动模型、激光雷达扫描图和测量模型更新粒子的位姿和权重；对于余下帧激光雷达扫描图，采用激光雷达扫描图计算的估算位姿更新粒子集中部分粒子的初始位姿，之后更新相应粒子的权重，同时采用里程计数据、运动模型、激光雷达扫描图和测量模型更新粒子集中余下部分粒子的位姿和权重，最后将两个更新过位姿和权重的粒子集合并成新的粒子集。

主权项

1.基于自适应蒙特卡洛和特征匹配的全局融合定位方法，其特征在于，包括：S1、获取机器人在检测环境行走时生成的栅格地图，并提取栅格地图中的地图线特征存储至地图特征数据库中；S2、当不存在机器人的激光雷达扫描图和里程计数据输入时，退出全局融合定位；S3、当存在机器人的激光雷达扫描图和里程计数据输入时，进入步骤S4；S4、判断当前激光雷达扫描图是否为第一帧、第二帧或余下帧，若是第一帧进入步骤S5，若是第二帧进入步骤S9，若是余下帧进入步骤S12；S5、提取激光雷达扫描图中的扫描图线特征存储至地图特征模板中，查找地图特征数据库与地图特征模板中长度差小于长度阈值的地图线特征和扫描图线特征；S6、计算长度差小于长度阈值的地图线特征和扫描图线特征重合时，激光雷达坐标系与栅格地图坐标系的转换关系，并利用转换关系平移和旋转激光雷达坐标系中原点计算得到候选位姿；S7、计算候选位姿上的所有全局扫描数据点到栅格地图中最近栅格占据点的距离的累加作为候选位姿的匹配拟合度，并将匹配拟合度最小的候选位姿作为估算位姿；S8、采用第一帧激光雷达扫描图计算的估算位姿对自适应蒙特卡洛的粒子集χ_t位姿初始化，其中粒子用于评估机器人的位姿，之后返回步骤S4；S9、根据机器人的里程计数据和运动模型，更新粒子的位姿；根据激光雷达扫描图和测量模型，计算粒子的权重；S10、根据粒子集χ_t中粒子的权重和位姿，计算粒子位姿的期望作为机器人的估计位姿，之后计算粒子权重的平均值、长期均值、短期均值及长期均值与短期均值的相对差值；S11、根据相对差值与零的大小关系，将粒子集χ_t划分成粒子集和粒子集并对粒子集进行重采样，之后返回步骤S4；S12、采用步骤S9更新粒子集中粒子的位姿和权重；S13、根据当前激光雷达扫描图执行步骤S5至步骤S7，并采用得到的估算位姿更新粒子集中粒子的位姿，之后采用当前激光雷达扫描图和测量模型，计算粒子集中粒子的权重；S14、采用更新了权重和位姿的粒子集和粒子集合并的新粒子集更新粒子集χ_t，之后返回步骤S10。