【技术实现步骤摘要】
本专利技术涉及自主建图和定位，具体涉及一种基于激光salm的地图更新系统及其方法。
技术介绍
1、传统slam(同步定位与建图)框架下，slam只用作构建全局地图，当构建好环境地图后，随后的定位采用粒子滤波等其他方式，在周围环境变化不大的时候，定位不会失效，但是在环境变化大的情况下，传统的激光匹配地图的原理就会失效，这将导致定位丢失。
2、而在长期建图与定位中，slam在整个生命周期运行，能够保证定位是实时在最新的地图上进行，为了防止子图的数量和节点的数量无限制的增加，地图的更新通常会有边缘化这一步骤，边缘化保证了删除节点后，剩下的节点没有信息损失。边缘化过程中，涉及到信息矩阵的求逆，但矩阵的求逆是一个复杂的运算，除了耗时之外，还面临着矩阵退化、奇异矩阵等因素的干扰，让边缘化的过程并不能总是顺利的进行。还有一些是通过在已有地图中进行定位，当检测到环境变化的时候，进行运行建图程序来通过建图程序更新地图，这种做法会导致新建的地图与原来老的地图之间存在偏差，长期更新下去会导致误差的累积，同时累积的误差不能够及时的消除掉，这对定位来说无疑是一个巨大的灾难。
技术实现思路
1、本专利技术的目的在于提供一种基于激光salm的地图更新系统及其方法，该方法提出了一种约束传播的方法，避免了边缘化的同时，尽量降低了信息的损失，达到了降低算力的同时提高了系统的健壮性。
2、为实现上述目的，本专利技术采用如下技术方案：
3、一种基于激光salm的地图更新方法，包括如下步
4、(1)先使用slam构建全局地图，全局地图是通过若干个子图构成，然后根据激光雷达的实时扫描获取激光关键帧，若干个激光关键帧构成一个新的子图，最后更新全局地图中的子图；
5、(2)将子图加入全局地图，同时进行子图删除检测，若存在某个子图的覆盖范围被新的子图完全覆盖，进行全局地图更新；
6、(3)根据更新后全局地图的大小设置节点栅格分辨率生成若干个栅格节点；
7、(4)回环检测，若激光关键帧与当前轨迹匹配，则保留当前约束；若与旧轨迹匹配，则约束传播到栅格节点；若与原始轨迹匹配，则保留当前约束；
8、约束传播到栅格节点是指：将轨迹中的关键帧和子图约束传播到栅格节点下，同时约束传播过程中，对约束权重进行衰减；
9、(5)交叠区域判别，判断更新的子图能否完全覆盖旧子图的区域；若能覆盖，则删除旧子图；反之，则保留旧子图；
10、(6)提取马尔可夫毯，遍历所有约束和节点，找到和删除节点、删除子图有直接连接关系的节点和子图、以及约束，将其加入到马尔可夫毯中；
11、(7)传播约束，对马尔可夫毯中不删除的变量进行约束传播；
12、若删除变量不是当前轨迹的，则直接删除；若删除变量是当前轨迹的，进行分类处理，具体如下：
13、若约束另一端时间戳小于待删除变量，则直接删除约束；
14、若约束另一端时间戳大于待删除变量，且当前子图没回环，则进行约束传播，传播到上一个子图；
15、若约束另一端时间戳大于待删除变量，且当前子图有回环，则进行约束传播，传播到栅格节点；
16、(8)节点删除、约束更新，将需要删除的子图以及其对应的约束、节点全部删除；再将约束传播得到新约束加入约束列表中；
17、(9)全局优化，根据当前轨迹的约束优化当前轨迹下所有的激光关键帧和子图；
18、
19、式中，为优化后的激光关键帧集合，为优化后的子图集合，f为代价函数，为当前的关键帧集合，为当前的子图集合，为栅格节点集合；
20、代价函数f为
21、
22、式中，为激光里程计所给出的约束，为回环检测的约束，为激光关键帧到栅格节点的约束，为子图到栅格节点的约束，为约束传播过程中由传播生成的子图间约束，x为激光关键帧，s为栅格节点，m为子图，z为约束关系，ω为对应约束的信息矩阵，为齐次坐标变换，齐次坐标变换
23、(10)对更新的地图进行保存，后续重复上述步骤；保证地图的实时更新。
24、进一步地，所述约束传播为：
25、
26、式中，为第i个栅格节点s下轨迹j中第y个关键帧x的位姿；为轨迹j-1下第n个子图m到第i个栅格节点s的坐标变换，坐标变换通过第i个栅格节点s和第n个子图m的全局位姿获得，即通过坐标变换对约束进行坐标变化得到约束传播的最终结果。
27、进一步地，约束传播过程中权重衰减大小跟约束传播前后距离变化相关，
28、
29、式中，ωg为对应约束的信息矩阵，ωc为回环检测约束的信息矩阵，其中‖t‖d为计算加权距离，t为两个坐标系的变换关系，包含(x,y,θ)，为轨迹j-1下第n个子图m与轨迹j下第y个激光关键帧x的约束关系，为轨迹j-1下第n个子图m到第i个栅格节点s的坐标变换。
30、本专利技术还提供了一种基于激光salm的地图更新系统包括：
31、建图定位模块，生成关键帧，构建地图局部地图，返回激光里程计位姿，再根据全局优化的结果计算得到全局地图下的定位输出；
32、回环检测模块，利用生成的关键帧进行回环检测，当检测到回环信息时，获取对应的回环约束，并根据约束优化全局地图；
33、地图更新模块，用于地图更新与约束重建，它包括用于检测是否有需要删除子图的子图删除检测单元、用于在删除子图后重新计算马尔可夫毯中其他节点之间的约束关系的约束计算单元、用于删除节点、更新约束的更新单元。
34、进一步地，所述约束计算单元是基于栅格节点的约束传播，会将约束直接传播到距离最近的节点，约束传播的同时，降低其权重，当某个两个节点存在多个约束时，保留权重高的约束。
35、本专利技术的有益效果为：本专利技术在回环检测后面添加了一个地图更新模块，从而实现长期建图与定位以及地图更新；设计出基于栅格节点的约束传播，会将约束直接传播到距离最近的节点，有效降低了约束传播的信息损失。约束传播的同时，我们会降低其权重，当某个两个节点存在多个约束时，保留权重高的约束。
本文档来自技高网...【技术保护点】
1.一种基于激光SALM的地图更新方法，其特征在于，包括如下步骤：
2.根据权利要求1所述的一种基于激光SALM的地图更新方法，其特征在于，所述约束传播为：
3.根据权利要求2所述的一种基于激光SALM的地图更新方法，其特征在于，约束传播过程中权重衰减大小跟约束传播前后距离变化相关，
4.一种基于激光SALM的地图更新系统，其特征在于，包括：
5.根据权利要求4所述逇一种基于激光SALM的地图更新系统，其特征在于，所述约束计算单元是基于栅格节点的约束传播，会将约束直接传播到距离最近的节点，约束传播的同时，降低其权重，当某个两个节点存在多个约束时，保留权重高的约束。
【技术特征摘要】
1.一种基于激光salm的地图更新方法，其特征在于，包括如下步骤：
2.根据权利要求1所述的一种基于激光salm的地图更新方法，其特征在于，所述约束传播为：
3.根据权利要求2所述的一种基于激光salm的地图更新方法，其特征在于，约束传播过程中权重衰减大小跟约束传播前后距离变化相关，<...
【专利技术属性】
技术研发人员：周军，陈航，龙羽，徐菱，
申请(专利权)人：成都睿芯行科技有限公司，
类型：发明
国别省市：
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。