基于滑动窗口位姿图优化的机器人抗干扰定位方法及装置制造方法及图纸

本发明专利技术提供一种基于滑动窗口位姿图优化的机器人抗干扰定位方法，包括：获取激光数据，机器人进行重定位：根据机器人是否有初始位姿判断搜索范围，确定机器人的当前位姿；跟踪机器人移动过程中的实时位姿：通过帧间匹配方法计算机器人的实时定位位姿，并输出定位结果；判断在跟踪过程中是否累计足够关键帧，若足够，则删除最早关键帧，进行最新关键帧保存；判断机器人当前定位状态，若当前定位丢失，重新获取最新的准确定位，更新定位，以最新位姿继续位姿跟踪；否则，进行内点筛选和累计误差消除，使得机器人能够在地图内持续输出位姿，实时准确定位。本发明专利技术的有益效果是能够实现无需较长时间运动进行全局重定位，提高抗干扰能力。力。力。

【技术实现步骤摘要】
基于滑动窗口位姿图优化的机器人抗干扰定位方法及装置


[0001]本专利技术属于自主导航与定位
，尤其是涉及一种基于滑动窗口位姿图优化的机器人抗干扰定位方法及装置。

技术介绍

[0002]机器人的精准定位，是机器人完成自主导航的必要前提。现有技术常常通过安装磁钉、磁条反光板等标记物来间接判断机器人位姿，但是该方法需要对已有环境进行改造，不适应大多数场景。此外，通过激光SLAM(simultaneous localization and mapping)构建先验地图，基于已有先验地图采用自适应蒙特卡洛算法(adaptive Monte Carlo Localization；AMCL)进行机器人定位，该算法采取粒子滤波的方式，计算量较大且所输出的估算结果不唯一，在无先验位姿前提下全局重定位较为困难或者需要机器人运动较长时间等缺点，实际使用效果不佳，并且在部分发生变化的区域，定位精度变差，使用原生cartographer位姿图优化的方式进行机器人定位，计算量较大，对于低成本的机器人不适用。

技术实现思路

[0003]鉴于上述问题，本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于滑动窗口位姿图优化的机器人抗干扰定位方法，其特征在于：包括，获取激光数据，机器人进行重定位：根据所述机器人是否有初始位姿判断搜索范围，确定所述机器人的当前位姿；跟踪所述机器人移动过程中的实时位姿：通过帧间匹配方法计算所述机器人的实时定位位姿，并输出定位结果；判断在跟踪过程中是否累计足够关键帧，若足够，则删除最早关键帧，进行最新关键帧保存；判断所述机器人当前定位状态，若当前定位丢失，重新获取最新的准确定位，更新定位，以最新位姿继续位姿跟踪；否则，进行内点筛选和累计误差消除，使得所述机器人能够在地图内持续输出位姿，实时准确定位。2.根据权利要求1所述的基于滑动窗口位姿图优化的机器人抗干扰定位方法，其特征在于：所述获取激光数据，机器人进行重定位步骤中，当所述机器人没有初始位姿时，搜索全部子图，所述机器人与所有的子图进行遍历匹配，匹配分数最高的子图为所述机器人的当前位姿，计算公式为：其中，ρ为损失函数，E为残差，为第i个子图的在世界坐标系下的位姿，为第j个激光帧在世界坐标系下的位姿，∑
ij
为第j个激光帧插入第i个子图的质量，ε
ij
为第j个激光帧插入第i个子图的相对位姿。3.根据权利要求2所述的基于滑动窗口位姿图优化的机器人抗干扰定位方法，其特征在于：所述获取激光数据，机器人进行重定位步骤中，当所述机器人有初始位姿时，通过搜索先验位姿附近的子图计算机器人的当前位姿，搜索判断公式为：其中，distance
init
为先验位姿与子图位姿的距离，x
initial
为初始位姿的横坐标，y
initial
为初始位姿的纵坐标，x
submap
为子图的横坐标，y
submap
为子图的纵坐标。4.根据权利要求1
‑
3任一项所述的基于滑动窗口位姿图优化的机器人抗干扰定位方法，其特征在于：所述通过帧间匹配方法计算所述机器人的实时定位位姿，并输出定位结果步骤中，从当前位姿开始建立新的定位子图，当有新的激光帧输入后与所述从当前位姿开始建立新的定位子图进行匹配，输出实时位姿，计算公式为：其中，K为当前点云个数，M
smooth
为一个使用双立方插值对的平滑函数，h
k
为激光点云，T
ε
是将h
k
的所有点转化至当前定位子图坐标系下。5.根据权利要求4所述的基于滑动窗口位姿图优化的机器人抗干扰定位方法，其特征在于：所述判断在跟踪过程中是否累计足够关键帧，若足够删除最早关键帧，进行最新关键帧保存步骤中，当最新激光帧匹配的位姿和上一关键帧的位移差或角度差大于...

【专利技术属性】
技术研发人员：王运志，
申请(专利权)人：火种源码中山科技有限公司，
类型：发明
国别省市：