【技术实现步骤摘要】
本专利技术涉及机器人，特别涉及一种激光slam自适应自动更新地图的方法及系统。
技术介绍
1、现有搭载激光雷达的机器人在已知地图上通过激光和地图匹配，实现实时定位，但是如果地图环境发生变化，无法满足当前激光和已知地图的匹配，会发生定位不准或甚至定位丢失，所以需要保证地图实时更新和当前环境一致，实现稳定定位。
2、目前基于已有地图应对变化环境的2d激光slam定位方法有：1、手动更新地图，手动操作将变化的环境重新扫描，更新至地图中保存。2、使用高精度的里程计、imu、相机、gps等其他辅助传感器提高定位精度。3、环境中添加反光板、二维码或者磁条等标签，辅助定位。
3、 现有技术有如下缺点：1、手动更新地图虽然可以解决局部环境变化的问题，但是如果环境经常变换的场景，需要人工经常手动操作地图更新与保存，耗费人力和时间 。2、使用高精度里程计和imu提高定位精度，成本较高，使用相机视觉辅助需要进行单独的适配，视觉相关算法需要占用极大的计算资源，gps在室内无法满足完全覆盖，不能良好工作。3、在环境中添加反光板、二维码或磁条等标签时，需要对环境进行改造，操作不便。
技术实现思路
1、基于此，有必要提供一种可稳定定位的激光slam自适应自动更新地图的方法。
2、同时，提供一种可稳定定位的激光slam自适应自动更新地图的系统。
3、一种激光slam自适应自动更新地图的方法，包括：
4、加载原始地图：加载原始地图，转换成栅格地图，进入定位
5、激光初始定位：接收当前激光数据，确定初始位姿；
6、实时位姿跟踪：根据激光帧间匹配计算位姿，并融合里程计传感器，生成当前位姿；
7、关键帧保存：对计算出的当前位姿进行判断，判断是否保存为关键帧，当当前位姿与上一关键帧位移、角度差大于设定阈值时，将该帧数据存储为最新关键帧；
8、优化位姿：将最新关键帧和地图进行匹配，若满足匹配要求，则构建误差项，构成位姿图优化，求解使得误差项最小的待优化关键帧位姿，公式如下：
9、，式中i为关键帧id，为信息矩阵，代表该误差项的权重；
10、更新地图：将求解出使得误差项最小的关键帧位姿作为当前地图坐标系下的准确位姿，将激光点云按照优化后的位姿插入至栅格地图中，更新地图。
11、在优选实施例中，还包括：位姿输出：根据优化后的位姿对外实时发布pose，如果没有达到累计关键帧的阈值，则对外发布实时位姿跟踪步骤中的当前位姿；
12、定时保存更新地图：定时保存更新后的地图，编译加载更新后的地图。
13、在优选实施例中，所述更新地图还包括：更新地图后，清空优化后的关键帧点云和位姿数据。
14、在优选实施例中，所述激光初始定位步骤中，初始位姿的确定包括将激光点云进行栅格滤波，将原始栅格地图按照由粗到细的分辨率生成组地图，从系统最粗的分辨率地图开始匹配，首先将最粗分辨率的地图分成四份，形成四个格子，将这四个格子的局部地图与当前滤波后激光帧计算匹配度，选出score最高的区域，记录为当前best_score，将当前best_score区域使用高一层分辨率的地图maplowest+1继续分四块，在分块后的局部地图中继续计算出最高的score，以此类推，得到原地图分辨率下匹配的best_score，此时计算出best_score对应的位姿就是激光在地图中的位姿，将进行初始位姿的确定的这帧激光保存为第一帧关键帧。
15、在优选实施例中，所述实时位姿跟踪步骤中，所述激光帧间匹配包括：点云的帧间匹配，所述点云的帧间匹配包括：设上一帧栅格滤波后的点云为，当前栅格滤波后的点云为 ，求解残差 ：
16、
17、式中i为点云的顺序id，n为点云数量，r为旋转矩阵，t为平移矩阵，构建最小二乘问题，求使得误差平方和最小的r和t，
18、，
19、通过里程计增量数据获得和，根据实际激光帧间匹配和里程计精度，确定里程计权重ω（），求解出最优和，进一步计算当前位姿，
20、，
21、，
22、，
23、式中为旋转的权重，为平移权重，为单位矩阵， 是上一帧激光的位姿。
24、在优选实施例中，所述关键帧保存步骤中，利用运动滤波器对计算出的当前位姿进行判断，判断是否保存为关键帧，位移判断计算公式如下：
25、；
26、角度判断公式如下：
27、，
28、式中、、分别为实时位姿跟踪步骤中计算出来当前位姿的x、y和角度，、、分别为上一次保存的关键帧位姿的x、y和角度；若当前位姿的位移、角度判断小于设定阈值则返回实时位姿跟踪步骤继续计算最新位姿。
29、在优选实施例中，所述优化位姿步骤中，将最新关键帧和地图进行匹配的匹配方法包括：使用局部分支定界搜索最优解，使用最优解为计算精确位姿的初始解，关键帧保存的激光点云与当前地图的相对位姿，地图和点云的残差如下所示，满足残差最小时的即为精确位姿
30、
31、式中 k 为当前待匹配的点云个数， 为使用双立方插值对的平滑函数，对栅格地图进行插值，激光点云，为将 的所有点转化至当前地图坐标系下，若激光点云与地图的最小残差小于设定阈值则为匹配，若该最小残差大于或者等于阈值则判断为不匹配，如不匹配则记录相对应的关键帧点云、编号、位姿，等待下一个关键帧进入匹配。
32、一种激光slam自适应自动更新地图的系统，包括：
33、加载原始地图模块：加载原始地图，转换成栅格地图，进入定位；
34、激光初始定位模块：接收当前激光数据，确定初始位姿；
35、实时位姿跟踪模块：通过激光帧间匹配计算位姿，并融合里程计传感器数据计算位姿，生成当前位姿；
36、关键帧保存模块：利用运动滤波器对计算出的当前位姿进行判断，判断是否保存为关键帧，当当前位姿与上一关键帧位移、角度差大于设定阈值时，将该帧数据存储为最新关键帧；
37、优化位姿模块：将最新关键帧和地图进行匹配，若满足匹配要求，则构建误差项，构成位姿图优化，求解使得误差项最小的待优化关键帧位姿，公式如下
38、，式中i为关键帧id，为信息矩阵，代表该误差项的权重；
39、更新地图模块：将求解出使得误差项最小的关键帧位姿作为当前地图坐标系下的准确位姿，将激光点云按照优化后的位姿插入至栅格地图中，更新地图。
40、在优选实施例中，还包括：位姿输出模块：根据优化后的位姿对外实时发布pose，如果没有达到累计关键帧的阈值，则对外发布实时位姿跟踪步骤中的当前位姿；
41、定时保存更新地图模块：定时保存更新后的地图，编译加载更新后的地图。
42、在优选实施例中，所述更新地图还包括：更新地图后，清空优化后的关键帧点云和位姿数据。
43、在优选实本文档来自技高网...
【技术保护点】
1.一种激光slam自适应自动更新地图的方法，其特征在于，包括：
2.根据权利要求1所述的激光slam自适应自动更新地图的方法，其特征在于，还包括：位姿输出：根据优化后的位姿对外实时发布pose，如果没有达到累计关键帧的阈值，则对外发布实时位姿跟踪步骤中的当前位姿；
3.根据权利要求1所述的激光slam自适应自动更新地图的方法，其特征在于，所述激光初始定位步骤中，初始位姿的确定包括将激光点云进行栅格滤波，将原始栅格地图按照由粗到细的分辨率生成组地图，从系统最粗的分辨率地图开始匹配，首先将最粗分辨率的地图分成四份，形成四个格子，将这四个格子的局部地图与当前滤波后激光帧计算匹配度，选出score最高的区域，记录为当前best_score，将当前best_score区域使用高一层分辨率的地图maplowest+1继续分四块，在分块后的局部地图中继续计算出最高的score，以此类推，得到原地图分辨率下匹配的best_score，此时计算出best_score对应的位姿就是激光在地图中的位姿，将进行初始位姿的确定的这帧激光保存为第一帧关键帧。
4.根据权
5.根据权利要求1至3任意一项所述的激光slam自适应自动更新地图的方法，其特征在于，所述关键帧保存步骤中，利用运动滤波器对计算出的当前位姿进行判断，判断是否保存为关键帧，位移判断计算公式如下：
6.根据权利要求1至3任意一项所述的激光slam自适应自动更新地图的方法，其特征在于，所述优化位姿步骤中，将最新关键帧和地图进行匹配的匹配方法包括：使用局部分支定界搜索最优解，使用最优解为计算精确位姿的初始解，关键帧保存的激光点云与当前地图的相对位姿，地图和激光点云的残差如下所示，满足残差最小时的即为精确位姿，
7.一种激光slam自适应自动更新地图的系统，其特征在于，包括：
8.根据权利要求7所述的激光slam自适应自动更新地图的系统，其特征在于，还包括：位姿输出模块：根据优化后的位姿对外实时发布pose，如果没有达到累计关键帧的阈值，则对外发布实时位姿跟踪步骤中的当前位姿；
9.根据权利要求7至8任意一项所述的激光slam自适应自动更新地图的系统，其特征在于，所述激光初始定位模块中，初始位姿的确定包括将激光点云进行栅格滤波，将原始栅格地图按照由粗到细的分辨率生成组地图，从系统最粗的分辨率地图开始匹配，首先将最粗分辨率的地图分成四份，形成四个格子，将这四个格子的局部地图与当前滤波后激光帧计算匹配度，选出score最高的区域，记录为当前best_score，将当前best_score区域使用高一层分辨率的地图maplowest+1继续分四块，在分块后的局部地图中继续计算出最高的score，以此类推，得到原地图分辨率下匹配的best_score，此时计算出best_score对应的位姿就是激光在地图中的位姿，将进行初始位姿的确定的这帧激光保存为第一帧关键帧；
10.根据权利要求7至8任意一项所述的激光slam自适应自动更新地图的系统，其特征在于，所述关键帧保存模块中，利用运动滤波器对计算出的当前位姿进行判断，判断是否保存为关键帧，位移判断计算公式如下：；
...【技术特征摘要】
1.一种激光slam自适应自动更新地图的方法，其特征在于，包括：
2.根据权利要求1所述的激光slam自适应自动更新地图的方法，其特征在于，还包括：位姿输出：根据优化后的位姿对外实时发布pose，如果没有达到累计关键帧的阈值，则对外发布实时位姿跟踪步骤中的当前位姿；
3.根据权利要求1所述的激光slam自适应自动更新地图的方法，其特征在于，所述激光初始定位步骤中，初始位姿的确定包括将激光点云进行栅格滤波，将原始栅格地图按照由粗到细的分辨率生成组地图，从系统最粗的分辨率地图开始匹配，首先将最粗分辨率的地图分成四份，形成四个格子，将这四个格子的局部地图与当前滤波后激光帧计算匹配度，选出score最高的区域，记录为当前best_score，将当前best_score区域使用高一层分辨率的地图maplowest+1继续分四块，在分块后的局部地图中继续计算出最高的score，以此类推，得到原地图分辨率下匹配的best_score，此时计算出best_score对应的位姿就是激光在地图中的位姿，将进行初始位姿的确定的这帧激光保存为第一帧关键帧。
4.根据权利要求1至3任意一项所述的激光slam自适应自动更新地图的方法，其特征在于，所述实时位姿跟踪步骤中，所述激光帧间匹配包括：点云的帧间匹配，所述点云的帧间匹配包括：设上一帧栅格滤波后的点云为，当前栅格滤波后的点云为 ，求解残差 ：
5.根据权利要求1至3任意一项所述的激光slam自适应自动更新地图的方法，其特征在于，所述关键帧保存步骤中，利用运动滤波器对计算出的当前位姿进行判断，判断是否保存为关键帧，位移判断计算公式如下：
6.根据权利要求1至3任意一项所述的激光...
【专利技术属性】
技术研发人员：王运志，
申请(专利权)人：江苏中科重德智能科技有限公司，
类型：发明
国别省市：
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。