【技术实现步骤摘要】
本专利技术属于定位，更具体地，本专利技术涉及一种融合栅格和均值点云地图的slam定位方法及系统。
技术介绍
1、随着激光agv在自动化仓储物流领域应用中越来越广泛，各个应用场景对于激光agv的定位导航精度要求也越来越高，这个精度非常依赖于激光定位导航算法。
2、现有的激光slam算法主要是点云到点云的scan_to_scan匹配和点云到地图的scan_to_map匹配算法。scan_to_scan算法主要有icp、ndt及相关改进算法，这些算法的点云地图受限于雷达单点测距的误差，导致点云地图边界较厚，定位精度降低。scan_to_map算法主要有gmapping、hector、cartographer及相关改进算法，这些算法基于栅格地图进行定位，但精度受限于栅格的尺寸。
3、为了解决上述问题，现有的方案主要包含如下两种：1)融合使用二维码或反光柱等标识。在原本栅格或点云slam定位基础上，在需要高精度定位的区域融合使用二维码或反光柱标识进行精定位。2)切换使用栅格或点云地图匹配。在普通区域使用栅格地图匹配，在高精度区域使用均值化的点云地图匹配。然而两种方法都存在一些问题，第一种需要加装特殊标记物，工程实施效率较低，第二种只对地图均值化，但自身点云的厚度会带来较大绝对误差，而且单纯切换使用，无法避免单一方式陷入局部极值的缺陷。
技术实现思路
1、本专利技术提供一种融合栅格和均值点云地图的slam定位方法，旨在改善上述问题。
2、本专利技术是这样实现
3、检测机器人当前是否处于高精定位区域，若检测结果为是，则将当前帧均值化的点云与slam占据栅格地图、多帧均值化的点云地图进行匹配，匹配出机器人当前的位姿。
4、进一步的，在机器人进行定位前，需要先构建地图，地图的构建过程具体如下：
5、(1)通过激光雷达建立占据栅格地图m；
6、(2)在占据栅格地图中划定高精度定位区域，计算高精定位区域转换至世界坐标下；
7、(3)在世界坐标系下对高精定位区域进行膨胀，计算膨胀后的高精定位区域与占据栅格地图的交集，确定均值点云地图的栅格区域；
8、(4)将均值点云地图的栅格区域转换至世界坐标系下，得到均值点云地图区域map_m_rg；
9、(5)通过多帧点云均值化构建均值点云地图区域map_m_rg内的均值点云地图mean_m。
10、进一步的，均值点云地图mean_m的构建过程具体如下：
11、(1)在当前雷达位姿pose_c下，针对每个扫描角度扫描k次，获取同一角度下的k个激光帧，每个激光帧中存在m个pi点，统计每个扫描角度下k个pi点的均值mean_pi和标准差std_pi：
12、(2)当标准差std_pi超过阈值k_std时，删除2*std_pi之外的点集，执行步骤(1)，若标准差std_pi仍超过阈值k_std，则不再保留当前角度下的第i个点；在标准差std_pi低于阈值k_std时，则当前角度下第i个点为可用点，保留其均值点mean_pi；
13、(3)基于移动机器人当前的位姿将可用点的均值点变换到世界坐标系，取位于均值点云地图区域map_m_rg内的均值点；
14、(4)计算所有角度下的可用点的均值点，即可得到均值点云地图mean_m。
15、进一步的，步骤(4)之后还包括：
16、(5)使用当前帧的均值化点云means和当前均值点云地图meanm进行scan_to_map匹配，获取机器人当前的定位位姿；
17、(6)机器人在高精定位区域的移动过程中，通过当前定位位姿重新构建一个新的均值点云地图meanm_new，
18、(7)将均值点云地图meanm_new和现有的均值点云meanm滤波融合，更新当前均值点云地图；
19、(8)循环执行步骤(5)～(6)，最终完成高精定位区域的均值点云地图mean_m的构建。
20、进一步的，当前帧均值化的点云means的构建方法具体如下：
21、(1)计算当前激光帧s的第i个点pi转换到栅格坐标系，计算其栅格的整数坐标p_mi(r,c)；
22、(2)将整数坐标(r,c)相同的点认定为处在同一个栅格内的点，当落入一个栅格内的点数n小于栅格内点数的预设值k_num，则直接删除该栅格。当落入一个栅格内的点数n大于等于预设值k_num，直接统计该栅格内点集的均值，并保留均值点pi_mean：
23、(3)统计均值点的总点数p_num，当点数p_num小于最少保留点数k_min_num，将栅格分辨率c_res缩小一倍到res/4.0，再执行步骤(1)，直到点数符合最少保留点数要求，得到当前帧激光帧的均值化点云means。
24、进一步的，基于当前帧均值点云means的定位方法具体如下：
25、构建双重残差约束，包括：基于当前激光点云和占据栅格地图m构建的概率残差约束，基于单帧均值化点云和均值点云地图mean_m构建的点到点的距离残差约束。构建双重残差约束的非线性最小二乘目标函数，通过ceres最小化目标函数残差，来求解高精的定位位姿tξ。
26、进一步的，目标函数如下所示：
27、
28、其中，meansi表示当前激光帧的均值点云means中第i个点，tξmeansi表示基于位姿tξ将均值点云means中第i个点meansi投影至均值点云地图mean_m；ξ表示高精定位位姿的向量形式，tξ表示高精度定位位姿的矩阵形式；sj表示当前激光帧s中第j个点，tξsj表示基于位姿tξ将当前激光帧s中第j个点投影至世界坐标系，g(tξsj)表示将tξsj转换至栅格坐标系下，m(g(tξsj))表示g(tξsj)所在栅格在占据栅格地图m中的占据概率；w1和w2为权重系数。
29、进一步的，若机器人当前不位于高精定位区域时，则基于当前采集到的点云帧与slam占据栅格地图进行匹配，匹配出机器人当前的位姿。
30、本专利技术是这样实现的，一种融合栅格和均值点云地图的slam定位系统，所述系统包括：
31、设于移动机器人上的激光雷达，与激光雷达通讯连接的处理单元；
32、处理单元基于上述融合栅格和均值点云地图的slam定位方法来进行定位。
33、本专利技术实现了在不同定位精度需求的环境下，实现不同的定位精度，实现了在高精定位需求下不需要加装辅助定位装置，利用双残差约束实现融合定位,提高了定位精度和稳定性。
本文档来自技高网...【技术保护点】
1.一种融合栅格和均值点云地图的SLAM定位方法，其特征在于，所述方法包括如下步骤：
2.如权利要求1所述融合栅格和均值点云地图的SLAM定位方法，其特征在于，在机器人进行定位前，需要先构建地图，地图的构建过程具体如下：
3.如权利要求2所述融合栅格和均值点云地图的SLAM定位方法，其特征在于，均值点云地图mean_M的构建过程具体如下：
4.如权利要求3所述融合栅格和均值点云地图的SLAM定位方法，其特征在于，步骤(4)之后还包括：
5.如权利要求3所述融合栅格和均值点云地图的SLAM定位方法，其特征在于，当前帧均值化的点云meanS的构建方法具体如下：
6.如权利要求3所述融合栅格和均值点云地图的SLAM定位方法，其特征在于，基于当前帧均值点云meanS的定位方法具体如下：
7.如权利要求6所述融合栅格和均值点云地图的SLAM定位方法，其特征在于，目标函数如下所示：
8.如权利要求1所述融合栅格和均值点云地图的SLAM定位方法，其特征在于，若机器人当前不位于高精定位区域时，则基于当前采集到的点
9.一种融合栅格和均值点云地图的SLAM定位系统，其特征在于，所述系统包括：
...【技术特征摘要】
1.一种融合栅格和均值点云地图的slam定位方法，其特征在于，所述方法包括如下步骤：
2.如权利要求1所述融合栅格和均值点云地图的slam定位方法，其特征在于，在机器人进行定位前，需要先构建地图，地图的构建过程具体如下：
3.如权利要求2所述融合栅格和均值点云地图的slam定位方法，其特征在于，均值点云地图mean_m的构建过程具体如下：
4.如权利要求3所述融合栅格和均值点云地图的slam定位方法，其特征在于，步骤(4)之后还包括：
5.如权利要求3所述融合栅格和均值点云地图的slam定位方法，其特征在于，当前帧均值...
【专利技术属性】
技术研发人员：郝奇，陈智君，郑亮，赵法龙，方靖森，曹雏清，赵立军，
申请(专利权)人：长三角哈特机器人产业技术研究院，
类型：发明
国别省市：
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。