【技术实现步骤摘要】
本专利技术属于室内移动机器人定位，更具体地说，是一种针对移动机器人定位信息丢失时采用图像模板匹配与粒子滤波相结合的重定位方法。
技术介绍
1、移动机器人的发展与广泛应用推动了相关技术的创新和改进，其中，定位技术作为核心技术之一，是机器人进行建图与导航的基础。室内移动机器人定位技术主要为基于信号的定位技术和基于传感器的定位技术，前者可利用蓝牙或wifi信号进行定位，但信号强度容易受到环境干扰和障碍物影响，造成定位准确度下降，往往应用于一些简单环境且对定位精度要求不高的场景；后者作为当前移动机器人采用的主流定位方式，以激光雷达或摄像头为环境感知器件，实时获取环境信息，其中激光雷达因其不受光照条件影响、定位算法对计算能力要求较小以及准确度高而广泛被室内场景中采用。
2、使用激光雷达为感知器件进行定位，经典的方法有基于粒子滤波的自适应蒙特卡洛算法(adaptive monte carlo localization,amcl)，通过粒子在地图中的分布以及信息的观测来更新粒子的分布，得到当前机器人最可能存在的位置，当定位丢失时可通过在整个地图播撒粒子，不断更新观测来收敛到正确位置完成重定位，但是在较大的场景中往往需要大量粒子，计算复杂度较高，收敛速度慢，且极容易得到错误结果。另一种常用的方法是基于特征提取和非线性优化的方法，通过特征提取，建立当前姿态与估计姿态残差方程求解得到最优的位姿，在定位丢失时则需要与全局地图中存取的姿态进行方程建立和求解，计算复杂度较高，耗时较长，并且二维环境下激光雷达获取的结构信息较为简单，特征较少，容
3、综上所述，在实际应用中需要一种精确度和速度都有要求的重定位方法，以同时满足平台计算能力的需求并且使机器人在定位丢失后较为快速的重新获取当前所在位置，恢复正常的工作状态。
技术实现思路
1、为了克服现有在室内环境下重定位方式存在的耗时较长且精确度较低的不足，本专利技术提出一种模板匹配与粒子滤波相结合的室内机器人重定位方法，使用二维激光雷达作为传感器件，采用图像模板匹配进行粗匹配，根据粗匹配结果使用amcl算法快速收敛得到精确定位的方法，适用于室内场景下，移动机器人在无先验信息条件下任意位置的重定位，具有更为快速准确的重定位效果。
2、本专利技术所采取的技术方案为：
3、一种模板匹配与粒子滤波相结合的室内机器人重定位方法，包括以下步骤：
4、步骤1)使用基于激光测距的二维建图算法对室内场景进行地图构建，得到栅格地图及其图像；
5、步骤2)读取当前位置的激光雷达扫描帧，获取雷达坐标系到全局地图坐标系的坐标转换，并将雷达扫描帧中点云数据的坐标通过该变换得到在全局地图坐标系中的坐标；
6、步骤3)对步骤2)中点云数据根据密度进行聚类，分成三个子点云数据，若存在子点云中点的数量小于设定值，最终得到的子点云数量可以少于三个；
7、步骤4)通过栅格地图坐标系和全局地图坐标系的关系以及栅格地图坐标系和图像坐标系的关系，将步骤2)和步骤3)中处理后的点云数据转换为一个完整的扫描帧图像和与子点云对应数量的子图像；
8、步骤5)获取机器人坐标系到全局地图坐标系的坐标变换，得到机器人在全局地图中的坐标；
9、步骤6)分别以步骤4)中的完整扫描帧图像和子图像为模板在步骤1)得到的图像中进行搜索，最终的图像匹配结果通过步骤5)中得到的坐标和像素之间的关系，转换为机器人位姿；
10、步骤7)丢弃步骤6)中匹配度量值小于设定最低阈值(weak_match_value)的结果，保留匹配度量值大于weak_match_value中排列前n的结果，n为设定数，并对这些数据根据匹配度量值大小设置不同的权重；
11、步骤8)将步骤7)中得到的机器人可能位姿结果和对应权重发送至amcl算法，amcl算法按权重分配粒子，初始化粒子滤波器；
12、步骤9)让机器人原地旋转或者移动，使粒子收敛，得到机器人此时的正确位姿。
13、进一步，所述步骤1)的过程如下：在移动机器人上基于ros系统部署cartographer建图算法，使用二维激光雷达感知环境信息，结合轮式里程计和imu持续的跟踪机器人位姿变化，并通过扩展卡尔曼滤波(extended kalman filter，ekf)去除噪声，为cartographer算法提供较好的初值，然后再进一步通过前端匹配得到更准确的机器人位姿，将观测数据加入局部地图；在完成局部地图构建后，cartographer算法使用图优化的方法将局部地图拼入全局地图，并且提供了回环检测功能，在移动机器人遍历环境之后完成栅格地图构建并进行保存。
14、再进一步，所述步骤2)中，读取当前位姿激光雷达扫描帧scan_msg＝[r1,r2,…,rn]，原始的雷达点云数据只有距离信息和角度信息，对于每个点ri需要转换为其在雷达坐标系下的坐标，公式如下：
15、
16、其中为点ri在雷达坐标系下坐标，rangei为该点的雷达的测距距离，anglemin为当前激光雷达帧发射第一个点的角度，angle_increment为激光雷达角分辨率；
17、通过ros中维护坐标转换关系的tf树获取雷达坐标系到全局地图坐标系的变换t，然后计算得到雷达点云数据在全局地图下对应的坐标，并以点云格式存储至cloud_msg＝[c1,c2,…,cn]，计算方式如下:
18、
19、其中为点ci扩展后的坐标向量，变换矩阵r为二维旋转矩阵，t为三维平移向量，为激光雷达坐标系下的坐标向量扩展向量。
20、更进一步，所述步骤3)中，由于激光雷达扫描获取点云具有时序性，连续的点一定在时序上连续，按照顺序进行计算划分，对于点ci，计算前一个点ci-1到点ci的欧式距离d，当d小于设定阈值时加入到该子集中，否则以下一个点ci+1为新子集的第一个点，继续计算点ci+2到点ci+1的距离，同样按照以上方式添加至同一个子集或者创建新的子集，直至每个点都加入一个子集，取点的数量前三的子集进行保存，若其中点的数量小于25，则删除该子集。
21、所述步骤4)中，创建存放图像的像素矩阵，将cloud_msg和步骤3)生成的几个子集中的点通过栅格地图坐标系、全局地图坐标系和图像坐标系的关系构建与像素矩阵中像素点的一一对应，将点云数据转换为图像，转换关系如下：
22、
23、
24、其中为步骤2)中获取的全局地图下点的坐标向量，为栅格地图坐标系下该点的坐标向量，为最终得到的图像矩阵中对应的像素坐标向量，resolution为栅格地图分辨率，originx全局地图坐标系原点在栅格地图上的横坐标，originy全局地图坐标系原点在栅格地图上的纵坐标，mapheight为地图的高度。
25、进一步，对转换后的图像进行降噪处理，首先通过对图像膨胀操作可以使目标形状的边界更加清晰、连续，继本文档来自技高网...
【技术保护点】
1.一种模板匹配与粒子滤波相结合的室内机器人重定位方法，其特征在于：所述重定位方法包括以下步骤：
2.如权利要求1所述的一种模板匹配与粒子滤波相结合的室内机器人重定位方法，其特征在于：所述步骤1)的过程如下：在移动机器人上基于ROS系统部署Cartographer建图算法，使用二维激光雷达感知环境信息，结合轮式里程计和IMU持续的跟踪机器人位姿变化，并通过扩展卡尔曼滤波去除噪声，为Cartographer算法提供较好的初值，然后再进一步通过前端匹配得到更准确的机器人位姿，将观测数据加入局部地图；在完成局部地图构建后，Cartographer算法使用图优化的方法将局部地图拼入全局地图，并且提供了回环检测功能，在移动机器人遍历环境之后完成栅格地图构建并进行保存。
3.如权利要求1或2所述的一种模板匹配与粒子滤波相结合的室内机器人重定位方法，其特征在于：所述步骤2)中，读取当前位姿激光雷达扫描帧scan_msg＝[r1,r2,…,rn]，原始的雷达点云数据只有距离信息和角度信息，对于每个点ri需要转换为其在雷达坐标系下的坐标，公式如下：
4.如权利要
5.如权利要求3所述的一种模板匹配与粒子滤波相结合的室内机器人重定位方法，其特征在于：所述步骤4)中，创建存放图像的像素矩阵，将cloud_msg和步骤3)生成的几个子集中的点通过栅格地图坐标系、全局地图坐标系和图像坐标系的关系构建与像素矩阵中像素点的一一对应，将点云数据转换为图像，转换关系如下：
6.如权利要求3所述的一种模板匹配与粒子滤波相结合的室内机器人重定位方法，其特征在于：所述步骤5)中，通过ROS中维护坐标转换关系的tf树获取机器人坐标系到全局地图坐标系的坐标变换，并使用变换矩阵和机器人的坐标替换式(2)中的变换矩阵与激光雷达点坐标，计算得到机器人在全局地图中的坐标。
7.如权利要求5所述的一种模板匹配与粒子滤波相结合的室内机器人重定位方法，其特征在于：所述步骤6)使用OpenCV中的模板匹配函数，分别以步骤4)中得到的点云图像为模板在步骤1)中得到的地图图像中进行搜索，得到多个匹配结果序列，由于通过点云转换得到的图像亮度一致，且不存在尺度不一致的问题，因此模板匹配采用最简单快速的平方差匹配模式，匹配度量值公式如下：
8.如权利要求1或2所述的一种模板匹配与粒子滤波相结合的室内机器人重定位方法，其特征在于：所述步骤7)将步骤6)中得到的不同匹配结果序列分别按照匹配度大小进行排序，设置匹配度最低阈值weak_match_value和强匹配阈值strong_match_value，该值的设定需要针对实际环境应用中多次测试来得到一个较优的设定，对于每个匹配结果序列取出排列前n的结果所述步骤7)的过程如下：
9.如权利要求1或2所述的一种模板匹配与粒子滤波相结合的室内机器人重定位方法，其特征在于：所述步骤8)中，当AMCL算法收到步骤7)发来的结果时，进行粒子滤波器的初始化：将粒子总数按照(7-1)得到的比例关系，设置用于全局粒子和用于粗匹配结果粒子的数目，继而根据匹配结果设置不同的概率分布，创建以匹配结果中机器人位置为均值、具有相同协方差矩阵的高斯概率分布；然后根据(7-2)得到的比重将用于粗匹配结果粒子样本分发给各个分布，添加到管理粒子的数据结构中。
10.如权利要求1或2所述的一种模板匹配与粒子滤波相结合的室内机器人重定位方法，其特征在于：所述步骤9)中，使机器人进行移动，AMCL算法根据观测数据更新每一个粒子的粒子权重，逐步迭代收敛至方差较小的高斯后验分布，其均值即最终的机器人的位置。
...【技术特征摘要】
1.一种模板匹配与粒子滤波相结合的室内机器人重定位方法，其特征在于：所述重定位方法包括以下步骤：
2.如权利要求1所述的一种模板匹配与粒子滤波相结合的室内机器人重定位方法，其特征在于：所述步骤1)的过程如下：在移动机器人上基于ros系统部署cartographer建图算法，使用二维激光雷达感知环境信息，结合轮式里程计和imu持续的跟踪机器人位姿变化，并通过扩展卡尔曼滤波去除噪声，为cartographer算法提供较好的初值，然后再进一步通过前端匹配得到更准确的机器人位姿，将观测数据加入局部地图；在完成局部地图构建后，cartographer算法使用图优化的方法将局部地图拼入全局地图，并且提供了回环检测功能，在移动机器人遍历环境之后完成栅格地图构建并进行保存。
3.如权利要求1或2所述的一种模板匹配与粒子滤波相结合的室内机器人重定位方法，其特征在于：所述步骤2)中，读取当前位姿激光雷达扫描帧scan_msg＝[r1,r2,…,rn]，原始的雷达点云数据只有距离信息和角度信息，对于每个点ri需要转换为其在雷达坐标系下的坐标，公式如下：
4.如权利要求1或2所述的一种模板匹配与粒子滤波相结合的室内机器人重定位方法，其特征在于：所述步骤3)中，由于激光雷达扫描获取点云具有时序性，连续的点一定在时序上连续，按照顺序进行计算划分，对于点ci，计算前一个点ci-1到点ci的欧式距离d，当d小于设定阈值时加入到该子集中，否则以下一个点ci+1为新子集的第一个点，继续计算点ci+2到点ci+1的距离，同样按照以上方式添加至同一个子集或者创建新的子集，直至每个点都加入一个子集，取点的数量前三的子集进行保存，若其中点的数量小于25，则删除该子集。
5.如权利要求3所述的一种模板匹配与粒子滤波相结合的室内机器人重定位方法，其特征在于：所述步骤4)中，创建存放图像的像素矩阵，将cloud_msg和步骤3)生成的几个子集中的点通过栅格地图坐标系、全局地图坐标系和图像坐标系的关系构建与像素矩阵中像素点的一一对应，将点云数据转换为图像，转换关系如下：...
【专利技术属性】
技术研发人员：欧县华，吴鸿志，朱骞，徐韩，
申请(专利权)人：浙江润琛科技有限公司，
类型：发明
国别省市：
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。