一种基于激光SALM的地图更新系统及其方法

专利主权项内容

1.一种基于激光SALM的地图更新方法，其特征在于，包括如下步骤：(1)先使用SLAM构建全局地图，全局地图是通过若干个子图构成，然后根据激光雷达的实时扫描获取激光关键帧，若干个激光关键帧构成一个新的子图，最后更新全局地图中的子图；(2)将子图加入全局地图，同时进行子图删除检测，若存在某个子图的覆盖范围被新的子图完全覆盖，进行全局地图更新；(3)根据更新后全局地图的大小设置节点栅格分辨率生成若干个栅格节点；(4)回环检测，若激光关键帧与当前轨迹匹配，则保留当前约束；若与旧轨迹匹配，则约束传播到栅格节点；若与原始轨迹匹配，则保留当前约束；约束传播到栅格节点是指：将轨迹中的关键帧和子图约束传播到栅格节点下，同时约束传播过程中，对约束权重进行衰减；(5)交叠区域判别，判断更新的子图能否完全覆盖旧子图的区域；若能覆盖，则删除旧子图；反之，则保留旧子图；(6)提取马尔可夫毯，遍历所有约束和节点，找到和删除节点、删除子图有直接连接关系的节点和子图、以及约束，将其加入到马尔可夫毯中；(7)传播约束，对马尔可夫毯中不删除的变量进行约束传播；若删除变量不是当前轨迹的，则直接删除；若删除变量是当前轨迹的，进行分类处理，具体如下：若约束另一端时间戳小于待删除变量，则直接删除约束；若约束另一端时间戳大于待删除变量，且当前子图没回环，则进行约束传播，传播到上一个子图；若约束另一端时间戳大于待删除变量，且当前子图有回环，则进行约束传播，传播到栅格节点；(8)节点删除、约束更新，将需要删除的子图以及其对应的约束、节点全部删除；再将约束传播得到新约束加入约束列表中；(9)全局优化，根据当前轨迹的约束优化当前轨迹下所有的激光关键帧和子图；式中，为优化后的激光关键帧集合，/>为优化后的子图集合，f为代价函数，χ为当前的关键帧集合，/>为当前的子图集合，/>为栅格节点集合；代价函数f为式中，为激光里程计所给出的约束，/>为回环检测的约束，/>为激光关键帧到栅格节点的约束，/>为子图到栅格节点的约束，/>为约束传播过程中由传播生成的子图间约束，x为激光关键帧，s为栅格节点，m为子图，z为约束关系，Ω为对应约束的信息矩阵，为齐次坐标变换，齐次坐标变换/>(10)对更新的地图进行保存，后续重复上述步骤；保证地图的实时更新。。数据由马 克 团 队整理