基于粒子滤波的远程定位方法技术

本发明专利技术公开了一种基于粒子滤波的远程定位方法，包括以下步骤：基于视觉位姿进行粒子群状态初始化；基于优化后轮式里程计进行粒子位姿预测；基于激光匹配进行粒子状态微调；基于激光观测信息进行粒子状态更新；基于视觉位姿和机器人系统当前定位信息判断机器人是否被绑架，如果被绑架，重新开始执行基于视觉位姿进行粒子群状态初始化；如果没有被绑架，判断视觉位姿是否可信，如果视觉位姿可信，结合视觉位姿、激光观测信息和二维激光似然域地图进行粒子权重更新；如果视觉位姿信息不可信，仅仅基于激光观测信息和二维激光似然域地图进行粒子权重更新；根据更新的粒子权重对粒子状态进行加权平均，计算机器人基于多传感器融合算法得出的最终位姿。

Remote location method based on particle filter

【技术实现步骤摘要】
基于粒子滤波的远程定位方法
本专利技术属于移动机器人定位
，具体涉及一种基于粒子滤波的远程定位方法。
技术介绍
以视觉导航机器人不在少数，视觉导航是依靠在机器人上安装一个摄像头，将看到的信息收集，经过将这些信息算法处理，得出环境模型，再而选择一条最佳路线，实现机器人的移动。视觉导航技术的优势在于较为成熟、技术成本低、借助大数据就能够解决技术路线，因此视觉导航在机器人市场中占据一席之地。但是视觉导航机器人的问题也很明显，视觉测距的前提要测的准，而在实际操作中，视觉测距经常会出现位置漂移的情况，在机器人的摄像头获取环境信息后计算出距离，期间若是数据处理和算法跟不上，距离数据就会形成偏差。激光导航的工作原理是通过激光获取环境信息，测量激光从发出到接收的时间计算出自身距离前方障碍物的距离，通过算法的处理，同样得出环境模型，在不断的扫描测距中获得定位和行走路线。激光雷达作为机器人技术中的后起之秀，激光雷达因为成本的高昂起步较慢，但随着低成本激光雷达的研发，激光雷达机器人的性能更加优异，激光雷达具有高精度、高分辨率的优势，在地图的精度和定位上更精准。如上分析，激光的信息精确度高，并且对光照鲁棒，基于激光得到的里程计，置信度高；但是激光的信息量较少，特征少，导致在结构化重复性程度高的环境中容易出现定位失效的情况。视觉信息的特征丰富，容易通过闭环检测来减少累计误差，但是其精度不高，并且对光照变化剧烈的情况不鲁棒，容易受图像遮挡，运动物体干扰或者快速移动导致系统失效。激光和可见光的优缺点形成互补关系，因此考虑将激光信息和视觉信息进行融合。
技术实现思路
鉴于以上存在的技术问题，本专利技术用于提供一种基于粒子滤波的远程定位方法，针对室内环境中的机器人定位展开研究，采用多传感器融合的方式来解决在重复性结构多的环境中可能出现的定位失效问题。为解决上述技术问题，本专利技术采用如下的技术方案：一种基于粒子滤波的远程定位方法，包括以下步骤：基于视觉位姿进行粒子群的状态初始化；基于优化后轮式里程计进行粒子位姿预测；基于激光匹配进行预测粒子状态的微调；基于激光观测信息进行粒子状态更新；基于视觉位姿和机器人系统当前定位信息判断机器人是否被绑架，如果判断得到机器人被绑架，重新开始执行基于视觉位姿进行粒子群的状态初始化；如果机器人没有被绑架，判断视觉位姿是否可信，如果视觉位姿可信，如果视觉位姿可信，结合视觉位姿、激光观测信息和二维激光似然域地图进行粒子权重更新；如果视觉位姿信息不可信，不结合视觉权重进行粒子权重更新，仅仅基于激光观测信息和二维激光似然域地图进行粒子权重更新；；根据更新的粒子权重对粒子状态进行加权平均，计算机器人基于多传感器融合算法得出的最终位姿。优选地，基于视觉位姿进行粒子群状态初始化具体为：基于视觉SLAM获得环境的视觉地图，加载视觉地图，远程操控机器人移动，当机器人在运动过程中视觉回环成功后，返回当前相机位姿xvisual，以xvisual为均值，基于服从式的高斯概率分布采样粒子{x(i)}，对粒子群进行初始化，粒子分布服从高斯分布，每个粒子都携带有机器人的位姿信息，粒子群的位姿均值为系统粒子滤波的最后输出机器人位姿xPF。优选地，基于优化后轮式里程计进行粒子位姿预测具体为：基于实现的系统粒子群{x(i)}的初始化，移动机器人在运动的过程中粒子群也需实时移动，将优化后的轮式里程计作为机器人的控制输入信息ut-1，假设当前时刻为t，机器人在t-1时刻的粒子群为基于式其中代表基于里程计的运动更新，预测下一时刻粒子群的位置。优选地，基于激光匹配进行粒子状态微调具体为：根据预测得到的粒子群在t时刻的状态由于轮式里程计存在误差，需要对粒子群中的每一个粒子进行位置微调，在移动过程中基于激光似然域模型，基于当前激光观测信息和粒子所处的地图进行匹配，计算得到当前粒子的置信度；以每个粒子以预测值为初值，在一定范围内对粒子进行移动，计算各个位置的粒子置信度，保留置信度最高的粒子，替换预测得到的粒子群，得到微调的粒子群优选地，基于激光观测信息进行粒子状态更新具体为：对于粒子群中每个粒子在其周围一定范围的区域内有规则地选取K个粒子，对每个粒子点求取其置信度其中基于机器人的概率运动模型计算得到，基于激光地图的似然域模型计算得到，基于这K个点的置信度计算近似的后验分布，最终计算得到粒子状态的后验分布从该后验分布中重新采样得到t时刻最终的粒子群优选地，判断机器人是否被绑架具体为：对当前的激光传感器信息zt地图m，计算基于多传感器融合算法得到的机器人位姿xPF的置信度和基于视觉SLAM得到的机器人位姿xvisual的置信度若则认为此时机器人已经被绑架，对粒子群基于视觉位姿重新初始化；若则认为此时视觉位姿信息失效，进行粒子权重更新；若则认为此时视觉位姿信息有效，进行粒子权重更新。优选地，若则认为此时视觉位姿信息失效，进行粒子权重更新具体为：优选地，若则认为此时视觉位姿信息有效，进行粒子权重更新具体为：其中为视觉权重，xvisual为视觉SLAM系统输出的位姿，按照如下公计算：采用本专利技术具有如下的有益效果：通过本专利技术实施例的基于粒子滤波的远程定位方法，通过基于优化后轮式里程计进行粒子位姿预测、基于激光匹配进行粒子状态微调以及基于激光观测信息进行粒子状态更新，并结合视觉信息与激光观测信息进行权重更新，实现机器人位姿信息的实时更新，提高定位精度。附图说明图1为本专利技术实施例的基于粒子滤波的远程定位方法的步骤流程图；图2为本专利技术实施例的基于粒子滤波的远程定位方法中基于视觉位姿的粒子初始化；图3为本专利技术实施例的基于粒子滤波的远程定位方法中基于轮式里程计的采样模型；图4为本专利技术实施例的基于粒子滤波的远程定位方法中粒子重采样过程示意图。具体实施方式下面将结合本专利技术实施例中的附图，对本专利技术实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本专利技术一部分实施例，而不是全部的实施例。基于本专利技术中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本专利技术保护的范围。根据视觉和激光信息各自优缺点，激光的信息精确度高，并且对光照鲁棒，基于激光得到的里程计，置信度高；但是激光的信息量较少，特征少，导致在结构化重复性程度高的环境中容易出现定位失效的情况。视觉信息的特征丰富，容易通过闭环检测来减少累计误差，但是其精度不高，并且对光照变化剧烈的情况不鲁棒，容易受图像遮挡，运动物体干扰或者快速移动导致系统失效。激光和可见光的优缺点形成互补关系，故本专利技术实施例中将激光信息和视觉信息进行融合。在基于滤波的方式中，同时定位与建图的核心思想是根据系统得到的传感器信息z1:t来估计环境地图m和机器人位姿联合后验概率密度函数p(x1:t,m|z1:t,u0:t)，被本文档来自技高网...

【技术保护点】
1.一种基于粒子滤波的远程定位方法，其特征在于，包括以下步骤：/n基于视觉位姿进行粒子群的状态初始化；/n基于优化后轮式里程计进行粒子位姿预测；/n基于激光匹配进行预测粒子状态的微调；/n基于激光观测信息进行粒子状态更新；/n基于视觉位姿和机器人系统当前定位信息判断机器人是否被绑架，如果判断得到机器人被绑架，重新开始执行基于视觉位姿进行粒子群的状态初始化；如果机器人没有被绑架，判断视觉位姿是否可信，如果视觉位姿可信，如果视觉位姿可信，结合视觉位姿、激光观测信息和二维激光似然域地图进行粒子权重更新；如果视觉位姿信息不可信，不结合视觉权重进行粒子权重更新，仅仅基于激光观测信息和二维激光似然域地图进行粒子权重更新；；/n根据更新的粒子权重对粒子状态进行加权平均，计算机器人基于多传感器融合算法得出的最终位姿。/n

【技术特征摘要】
1.一种基于粒子滤波的远程定位方法，其特征在于，包括以下步骤：
基于视觉位姿进行粒子群的状态初始化；
基于优化后轮式里程计进行粒子位姿预测；
基于激光匹配进行预测粒子状态的微调；
基于激光观测信息进行粒子状态更新；
基于视觉位姿和机器人系统当前定位信息判断机器人是否被绑架，如果判断得到机器人被绑架，重新开始执行基于视觉位姿进行粒子群的状态初始化；如果机器人没有被绑架，判断视觉位姿是否可信，如果视觉位姿可信，如果视觉位姿可信，结合视觉位姿、激光观测信息和二维激光似然域地图进行粒子权重更新；如果视觉位姿信息不可信，不结合视觉权重进行粒子权重更新，仅仅基于激光观测信息和二维激光似然域地图进行粒子权重更新；；
根据更新的粒子权重对粒子状态进行加权平均，计算机器人基于多传感器融合算法得出的最终位姿。


2.如权利要求1所述的基于粒子滤波的远程定位方法，其特征在于，基于视觉位姿进行粒子群状态初始化具体为：
基于视觉SLAM获得环境的视觉地图，加载视觉地图，远程操控机器人移动，当机器人在运动过程中视觉回环成功后，返回当前相机位姿xvisual，以xvisual为均值，基于服从式的高斯概率分布来采样粒子{x(i)}，对粒子群进行初始化，粒子分布服从高斯分布，每个粒子都携带有机器人的位姿信息，粒子群的位姿均值为系统粒子滤波的最后输出机器人位姿xPF。


3.如权利要求2所述的基于粒子滤波的远程定位方法，其特征在于，基于优化后轮式里程计进行粒子位姿预测具体为：
基于实现的系统粒子群{x(i)}的初始化，移动机器人在运动的过程中粒子群也需实时移动，将优化后的轮式里程计作为机器人的控制输入信息ut-1，假设当前时刻为t，机器人在t-1时刻的粒子群为基于式其中代表基于里程计的运动更新，预测下一时刻粒子群的位置。


4.如权利要求3所述的基于粒子滤波的远程定位方法，其特征在于，基...

【专利技术属性】
技术研发人员：曹衍龙，孙安顺，杨将新，曹彦鹏，何泽威，董献瑞，薛小杰，刘佳炜，
申请(专利权)人：浙江大学，
类型：发明
国别省市：浙江;33