CN117451035A - 一种激光slam自适应自动更新地图的方法及系统 - Google Patents
一种激光slam自适应自动更新地图的方法及系统 Download PDFInfo
- Publication number
- CN117451035A CN117451035A CN202311789336.6A CN202311789336A CN117451035A CN 117451035 A CN117451035 A CN 117451035A CN 202311789336 A CN202311789336 A CN 202311789336A CN 117451035 A CN117451035 A CN 117451035A
- Authority
- CN
- China
- Prior art keywords
- pose
- map
- laser
- key frame
- current
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 32
- 238000006073 displacement reaction Methods 0.000 claims abstract description 20
- 238000005457 optimization Methods 0.000 claims abstract description 14
- 239000011159 matrix material Substances 0.000 claims description 22
- 238000013519 translation Methods 0.000 claims description 15
- 238000001914 filtration Methods 0.000 claims description 14
- 238000010586 diagram Methods 0.000 claims description 11
- 238000004364 calculation method Methods 0.000 claims description 9
- 238000009499 grossing Methods 0.000 claims description 6
- 238000004321 preservation Methods 0.000 claims description 4
- 238000009825 accumulation Methods 0.000 abstract description 3
- 230000006870 function Effects 0.000 description 6
- 230000008859 change Effects 0.000 description 2
- 238000010276 construction Methods 0.000 description 2
- 230000007613 environmental effect Effects 0.000 description 2
- 230000006872 improvement Effects 0.000 description 2
- 239000000463 material Substances 0.000 description 2
- 230000000007 visual effect Effects 0.000 description 2
- 230000006978 adaptation Effects 0.000 description 1
- 230000004888 barrier function Effects 0.000 description 1
- 230000010485 coping Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000008569 process Effects 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/38—Electronic maps specially adapted for navigation; Updating thereof
- G01C21/3804—Creation or updating of map data
- G01C21/3833—Creation or updating of map data characterised by the source of data
- G01C21/3841—Data obtained from two or more sources, e.g. probe vehicles
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
一种激光slam自适应自动更新地图的方法及系统包括加载原始地图，转换成栅格地图，进入定位；接收当前激光数据，确定初始位姿；根据激光帧间匹配计算位姿，并融合里程计数据计算位姿，生成当前位姿；对计算出的当前位姿进行判断，判断是否保存为关键帧，当前位姿与上一关键帧位移、角度差大于设定阈值时，将该帧数据存储为最新关键帧；将最新关键帧和地图进行匹配，若满足匹配要求，则构建误差项ei，位姿图优化；将优化的关键帧位姿作为当前地图坐标系下的准确位姿，将激光点云按照优化后的位姿插入至栅格地图中，更新地图；上述方法及系统通过关键帧累计，图优化计算出一系列关键帧准确位姿，将关键帧的点云插入地图，实现地图的准确自动更新。
Description
技术领域
本发明涉及机器人技术领域，特别涉及一种激光slam自适应自动更新地图的方法及系统。
背景技术
现有搭载激光雷达的机器人在已知地图上通过激光和地图匹配，实现实时定位，但是如果地图环境发生变化，无法满足当前激光和已知地图的匹配，会发生定位不准或甚至定位丢失，所以需要保证地图实时更新和当前环境一致，实现稳定定位。
目前基于已有地图应对变化环境的2d激光slam定位方法有：1、手动更新地图，手动操作将变化的环境重新扫描，更新至地图中保存。2、使用高精度的里程计、imu、相机、gps等其他辅助传感器提高定位精度。3、环境中添加反光板、二维码或者磁条等标签，辅助定位。
现有技术有如下缺点：1、手动更新地图虽然可以解决局部环境变化的问题，但是如果环境经常变换的场景，需要人工经常手动操作地图更新与保存，耗费人力和时间 。2、使用高精度里程计和imu提高定位精度，成本较高，使用相机视觉辅助需要进行单独的适配，视觉相关算法需要占用极大的计算资源，gps在室内无法满足完全覆盖，不能良好工作。3、在环境中添加反光板、二维码或磁条等标签时，需要对环境进行改造，操作不便。
发明内容
基于此，有必要提供一种可稳定定位的激光slam自适应自动更新地图的方法。
同时，提供一种可稳定定位的激光slam自适应自动更新地图的系统。
一种激光slam自适应自动更新地图的方法，包括：
加载原始地图：加载原始地图，转换成栅格地图，进入定位；
激光初始定位：接收当前激光数据，确定初始位姿；
实时位姿跟踪：根据激光帧间匹配计算位姿，并融合里程计传感器，生成当前位姿；
关键帧保存：对计算出的当前位姿进行判断，判断是否保存为关键帧，当当前位姿与上一关键帧位移、角度差大于设定阈值时，将该帧数据存储为最新关键帧；
优化位姿：将最新关键帧和地图进行匹配，若满足匹配要求，则构建误差项，构成位姿图优化，求解使得误差项最小的待优化关键帧位姿，公式如下：
，式中i为关键帧id，/>为信息矩阵，代表该误差项的权重；
更新地图：将求解出使得误差项最小的关键帧位姿作为当前地图坐标系下的准确位姿，将激光点云按照优化后的位姿插入至栅格地图中，更新地图。
在优选实施例中，还包括：位姿输出：根据优化后的位姿对外实时发布pose，如果没有达到累计关键帧的阈值，则对外发布实时位姿跟踪步骤中的当前位姿；
定时保存更新地图：定时保存更新后的地图，编译加载更新后的地图。
在优选实施例中，所述更新地图还包括：更新地图后，清空优化后的关键帧点云和位姿数据。
在优选实施例中，所述激光初始定位步骤中，初始位姿的确定包括将激光点云进行栅格滤波，将原始栅格地图按照由粗到细的分辨率生成组地图，从系统最粗的分辨率地图开始匹配，首先将最粗分辨率的地图分成四份，形成四个格子，将这四个格子的局部地图与当前滤波后激光帧计算匹配度，选出score最高的区域，记录为当前best_score，将当前best_score区域使用高一层分辨率的地图maplowest+1继续分四块，在分块后的局部地图中继续计算出最高的score，以此类推，得到原地图分辨率下匹配的best_score，此时计算出best_score对应的位姿就是激光在地图中的位姿，将进行初始位姿的确定的这帧激光保存为第一帧关键帧。
在优选实施例中，所述实时位姿跟踪步骤中，所述激光帧间匹配包括：点云的帧间匹配，所述点云的帧间匹配包括：设上一帧栅格滤波后的点云为，当前栅格滤波后的点云为/> ，求解残差/> ：
式中i为点云的顺序id，n为点云数量，R为旋转矩阵，t为平移矩阵，构建最小二乘问题，求使得误差平方和最小的R和t，
，
通过里程计增量数据获得和/>，根据实际激光帧间匹配和里程计精度，确定里程计权重ω（/>），求解出最优/>和/>，进一步计算当前位姿/>，
，
，
，
式中为旋转的权重，/>为平移权重，/>为单位矩阵， />是上一帧激光的位姿。
在优选实施例中，所述关键帧保存步骤中，利用运动滤波器对计算出的当前位姿进行判断，判断是否保存为关键帧，位移判断计算公式如下：
；
角度判断公式如下：
，
式中、/>、/>分别为实时位姿跟踪步骤中计算出来当前位姿的x、y和角度，/>、/>、/>分别为上一次保存的关键帧位姿的x、y和角度；若当前位姿的位移、角度判断小于设定阈值则返回实时位姿跟踪步骤继续计算最新位姿。
在优选实施例中，所述优化位姿步骤中，将最新关键帧和地图进行匹配的匹配方法包括：使用局部分支定界搜索最优解，使用最优解为计算精确位姿的初始解，关键帧保存的激光点云与当前地图的相对位姿/>，地图和点云的残差如下所示，满足残差最小时的/>即为精确位姿
式中 K 为当前待匹配的点云个数， 为使用双立方插值对的平滑函数，对栅格地图进行插值，/>激光点云，/>为将/> 的所有点转化至当前地图坐标系下，若激光点云与地图的最小残差小于设定阈值则为匹配，若该最小残差大于或者等于阈值则判断为不匹配，如不匹配则记录相对应的关键帧点云、编号、位姿，等待下一个关键帧进入匹配。
一种激光slam自适应自动更新地图的系统，包括：
加载原始地图模块：加载原始地图，转换成栅格地图，进入定位；
激光初始定位模块：接收当前激光数据，确定初始位姿；
实时位姿跟踪模块：通过激光帧间匹配计算位姿，并融合里程计传感器数据计算位姿，生成当前位姿；
关键帧保存模块：利用运动滤波器对计算出的当前位姿进行判断，判断是否保存为关键帧，当当前位姿与上一关键帧位移、角度差大于设定阈值时，将该帧数据存储为最新关键帧；
优化位姿模块：将最新关键帧和地图进行匹配，若满足匹配要求，则构建误差项，构成位姿图优化，求解使得误差项最小的待优化关键帧位姿，公式如下
，式中i为关键帧id，/>为信息矩阵，代表该误差项的权重；
更新地图模块：将求解出使得误差项最小的关键帧位姿作为当前地图坐标系下的准确位姿，将激光点云按照优化后的位姿插入至栅格地图中，更新地图。
在优选实施例中，还包括：位姿输出模块：根据优化后的位姿对外实时发布pose，如果没有达到累计关键帧的阈值，则对外发布实时位姿跟踪步骤中的当前位姿；
定时保存更新地图模块：定时保存更新后的地图，编译加载更新后的地图。
在优选实施例中，所述更新地图还包括：更新地图后，清空优化后的关键帧点云和位姿数据。
在优选实施例中，所述激光初始定位模块中，初始位姿的确定包括将激光点云进行栅格滤波，将原始栅格地图按照由粗到细的分辨率生成组地图，从系统最粗的分辨率地图开始匹配，首先将最粗分辨率的地图分成四份，形成四个格子，将这四个格子的局部地图与当前滤波后激光帧计算匹配度，选出score最高的区域，记录为当前best_score，将当前best_score区域使用高一层分辨率的地图maplowest+1继续分四块，在分块后的局部地图中继续计算出最高的score，以此类推，得到原地图分辨率下匹配的best_score，此时计算出best_score对应的位姿就是激光在地图中的位姿，将进行初始位姿的确定的这帧激光保存为第一帧关键帧；
在优选实施例中，所述实时位姿跟踪模块中，所述激光帧间匹配包括：点云的帧间匹配，所述点云的帧间匹配包括：设上一帧栅格滤波后的点云为，当前栅格滤波后的点云为/> ，求解残差/> ：
式中i为点云的顺序id，n为点云数量，R为旋转矩阵，t为平移矩阵，构建最小二乘问题，求使得误差平方和最小的R和t，
，
通过里程计增量数据获得和/>，根据实际激光帧间匹配和里程计精度，确定里程计权重ω（/>），求解出最优/>和/>， 进一步计算当前位姿/>，
，
，
，
式中为旋转的权重，/>为平移权重，/>为单位矩阵， />是上一帧激光的位姿。
在优选实施例中，所述关键帧保存模块中，利用运动滤波器对计算出的当前位姿进行判断，判断是否保存为关键帧，位移判断计算公式如下：
；
角度判断公式如下：
；
式中、/>、/>分别为实时位姿跟踪步骤中计算出来当前位姿的x、y和角度，/>、/>、/>分别为上一次保存的关键帧位姿的x、y和角度；若当前位姿的位移、角度判断小于设定阈值则返回实时位姿跟踪模块继续计算最新位姿；
在优选实施例中，所述优化位姿模块中，将最新关键帧和地图进行匹配的匹配方法包括：使用局部分支定界搜索最优解，使用最优解为计算精确位姿的初始解，关键帧保存的激光点云与当前地图的相对位姿/>，地图和激光点云的残差如下式所示，满足残差最小时的/>即为精确位姿
；
式中 K 为当前待匹配的点云个数， 为使用双立方插值对的平滑函数，对栅格地图进行插值，/>激光点云，/>为将/> 的所有点转化至当前地图坐标系下，若激光点云与地图的最小残差小于设定阈值则为匹配，若该最小残差大于或者等于阈值则判断为不匹配，如不匹配则记录相对应的关键帧点云、编号、位姿，等待下一个关键帧进入匹配。
上述激光slam自适应自动更新地图的方法及系统，通过关键帧累计，图优化计算出一系列关键帧准确位姿，将关键帧的点云插入地图，实现地图的准确自动更新，并且更新后的地图可以应用于后续定位。可以在环境经常变化的场景中。本发明在建立基础地图后无需手动更新地图，更省人力成本和时间；无需对环境进行改造添加标签、反光板或者磁条等辅助材料，部署更为方便；无需添加gps、视觉相机等辅助传感器，仅使用里程计和单线激激光，成本低；针对经常变换环境中的定位问题，有效解决变化环境中定位不准和定位丢失问题，实现稳定定位。
附图说明
图1为本发明一实施例的激光slam自适应自动更新地图的方法的流程图；
图2为本发明的一实施例的激光slam自适应自动更新地图的方法及系统的加载的原始地图示意图；
图3为本发明的一实施例的激光slam自适应自动更新地图的方法及系统的初始位姿示意图；
图4为本发明的一实施例的激光slam自适应自动更新地图的方法及系统的当前位姿的示意图；
图5为本发明的一实施例的激光slam自适应自动更新地图的方法及系统的优化后位姿的示意图；
图6为本发明的一实施例的激光slam自适应自动更新地图的方法及系统的更新后的地图的示意图。
具体实施方式
以下的实施例便于更好地理解本发明，但并不限定本发明。
如图1所示，本发明的一种激光slam（imultaneous Localization and Mapping即时定位与地图构建）自适应自动更新地图的方法，包括：
步骤S101，加载原始地图：加载原始地图，转换成栅格地图，进入定位。地图可以由gmapping、cartographer等算法建立保存，建图时不要有重影和墙壁歪斜。建图时整体系统会实时对外显示地图，可以进行观察反馈。及时回环，使用准确参数。
步骤S103，激光初始定位：接收当前激光数据，确定初始位姿。初始位姿可以通过激光与地图匹配计算确定，也可以使用手动提供的位姿。初始位姿的计算方法可以使用分支定界算法，首先将激光点云进行栅格滤波，将原始栅格地图按照由粗到细的分辨率生成一组地图，从最粗的分辨率地图开始匹配。
首先将最粗分辨率的地图分成四份，形成四个格子。这个四个格子的局部地图与当前滤波过后激光帧计算匹配度(score)，选出score最高的区域，记录为当前best_score，将当前best_score区域使用高一层分辨率的地图maplowest+1继续分四块，在分块后的局部地图中继续计算出最高的score，以此类推，得到原地地图分辨率下匹配的best_score。此时计算出best_score对应的位姿就是激光在地图中的位姿，将进行初始位姿的确定的这帧激光保存为第一帧关键帧。在一优选实施例中，最粗的分辨率可以使用3.2m。激光落入地图中的总体概率为匹配度。具体为激光占据栅格地图，每一个激光点地图中有一个概率，这些概率总和求均值。
步骤S105，实时位姿跟踪：通过激光帧间匹配计算位姿，并融合传感器数据计算位姿，生成当前位姿。传感器包括：里程计传感器。
激光点云的帧间匹配可以使用ICP算法。设上一帧栅格滤波后的点云为，当前栅格滤波后的点云为/> 。用如下公式求解残差/>
式中i为点云的顺序id（Identity document，身份标识号），n为点云数量，R为旋转矩阵，t为平移矩阵。理论上使得误差项J最接近0时，R和t为最优解。因此构建最小二乘问题，求使得误差平方和最小的R和t
通过里程计增量数据也可以获得出一组和/>。
本实施例根据实际激光匹配和里程计精度，确定里程计权重ω（），求解出最优/>和/>，进一步计算当前位姿/>，
，
，
，
式中为旋转的权重，/>为平移权重，/>为单位矩阵， />是上一帧激光的位姿。旋转的权重/>、平移权重/>可根据传感器精度进行调整。
根据里程计预测位姿，经过实时位姿跟踪的初始化，机器人初始位姿已经固定，当机器人开始运动时需要实时输出定位位姿，机器人运动时，首先通过里程计增量预测当前定位并储存，具体计算公式为：
，
，
式中和/>分别为本次里程计位姿和上一次里程计位姿。/>和/>分别为本次里程计预测的位姿和上一次对外发布的位姿/>表示上一次里程计位姿/>的逆，/>表示计算出的/>的逆。
步骤S107，关键帧保存：利用运动滤波器对计算出的当前位姿进行判断，判断是否保存为关键帧，当当前位姿与上一关键帧位移、角度差大于设定阈值时，将该帧数据存储为最新关键帧。
位移判断计算公式如下：
，
角度判断公式如下：
，
式中、/>、/>分别为步骤S105，实时位姿跟踪计算出来位姿的x、y和角度，/>、/>、/>分别为上一次保存的关键帧位姿的x、y和角度。
>0，angle>0 。阈值大小设置可根据内存大小和精度需求进行调整。
如果位移判断、角度判断小于设定阈值则返回步骤S105，实时位姿跟踪继续计算最新位姿。
如设激光雷达频率10hz，每次执行到步骤105，实时位姿跟踪步骤，会计算出一个匹配位姿，为最新匹配位姿。第一个关键帧为初始化的那一帧，第二帧就是对比第一帧，经过步骤S107关键帧保存步骤中保存的帧，第一关键帧相对第二关键帧为上一关键帧，第二关键帧相对于第三关键帧为上一关键帧，以此类推。
步骤S109，优化位姿：将最新关键帧和地图进行匹配，若满足匹配要求，则构建误差项，构成位姿图优化，求解使得误差项最小的待优化关键帧位姿，公式如下：
，
式中i为关键帧id，为信息矩阵，代表该误差项的权重。在一个实施例中，构建误差项，可以采用当前帧点云位姿与激光和地图算出的位姿差作为误差项，图优化的一个项。
匹配方式可以先使用局部分支定界搜索最优解，使用最优解作为计算精确位姿的初始解，使用如下公式，关键帧保存的激光点云与当前地图的相对位姿/>，地图和激光点云的残差如下式所示，满足残差最小时的/>即为精确位姿/>，
式中 K 为当前待匹配的点云个数； 为一个使用双立方插值对的平滑函数，对栅格地图进行插值；/>为使用三次立方差值把离散点云转化为连续函数。/>激光点云，/>是将/>的所有点转化至当前地图坐标系下。通过上述匹配方法可以实时输出关键帧在map下的位姿。精确位姿的最优解的残差值小于设置阈值则判断为匹配。
本发明一优选实施例的位姿可以用如下表达函数表示：；R为旋转矩阵，2*2的矩阵； t为平移矩阵，2*1矩阵。
若不满足匹配要求，则把相对应的关键帧点云、编号、位姿进行记录，等待下一个关键帧进入尝试匹配。
本实施例的储存的关键帧每次都要匹配，如果匹配度满足就进行优化。
步骤S111，更新地图：将求解出使得误差项最小的关键帧位姿作为当前地图坐标系下的准确位姿，将激光点云按照优化后的位姿插入至栅格地图中，更新地图。
为保证内存稳定，更新地图步骤还包括：更新地图后，清空优化后的关键帧点云和位姿数据。
本发明一实施例的激光slam自适应自动更新地图的系统，还包括：
步骤S113，位姿输出：根据优化后的位姿对外实时发布pose，如果没有达到累计关键帧的阈值，则对外发布实时位姿跟踪步骤中的当前位姿；
定时保存更新地图：定时保存更新后的地图，编译加载更新后的地图。
为了进一步说明本发明的技术方案，下面采用一具体的实施例中进行说明。
实际测试中使用 turtlrbot 底盘，万集 716 雷达，rk3399 开发板，在某地下车库中建立地图，使用激光和里程计数据做在已有地图上开启定位模式。
步骤S101，加载原始地图：加载原始地图，转换成栅格地图，开始定位模式。本实施例中，地图由cartographer算法建立，地图显示如图2所示。
步骤S103，激光初始定位：使用分支定界算法搜索地图中的最优解，地图中搜索到的最优解位姿为（1.42，-0.99，-0.81），将该帧激光点云和位姿保存为关键帧id=0；地图位姿如图3所示。
步骤S105，实时位姿跟踪：控制机器人运动，开始实时位姿跟踪，以上一帧位姿为起点，通过帧间匹配ICP算法计算R，t，同步获取里程计增量信息和/> ，由于实际机器人的底盘上里程计旋转误差较大，平移误差较小，所以设置/>，/>。
根据公式；/>计算最优/>和，进一步计算当前位姿/>，以此类推，当机器人的底盘移动至如下位置时，位姿pose为（0.28，-1.56，-2.28），如图4所示。
步骤S107，关键帧保存：计算当前帧的上一次关键帧的位姿差，使用运动滤波器判断是否作为关键帧插入，本实例运动滤波器使用的位移阈值为0.2m，旋转角度阈值为3°。如果满足条件则插入新的关键帧id=1，2 ，3……，如果不满足，则返回步骤S105，实时位姿跟踪继续进行下一帧匹配。底盘移动至上述pose（0.28，-1.56，-2.28）位姿时，已经插入了44个关键帧。
步骤S109，优化位姿：最新的关键帧和地图进行匹配，id = 43的关键帧满足与原地图匹配，构建误差项，构成位姿图优化，求解使得误差项最小id为0-43关键帧位姿作为最优解。
步骤S111，更新地图：把优化后的关键帧位姿对应的点云更新到栅格地图上，更新后及时以优化完毕的关键帧位姿和点云，保持内存稳定。如果环境变化不大时，更新后的地图和原地图基本一致，如果环境变化较大时，更新后的地图有明显改变，例如图5中地图新增了障碍物，优化后能够及时更新。
根据地图上是否有新的物体或者有没有不一致的点判断环境变化大小。如图5所示，实线框标注处是变化的地图区域，虚线框标注处为无变化的地图区域。
步骤S113，位姿输出：如果没有达到累计关键帧的阈值，则对外发布步骤S105，实时位姿跟踪步骤中的实时跟踪位姿。
步骤S115，定时保存更新地图：完成建图保存地图。最终的地图如图6所示，本次完成。
本发明一实施例的一种激光slam自适应自动更新地图的系统，包括：
加载原始地图模块：加载原始地图，转换成栅格地图，进入定位；
激光初始定位模块：接收当前激光数据，确定初始位姿；
实时位姿跟踪模块：通过激光帧间匹配计算位姿，并融合里程计传感器数据计算位姿，生成当前位姿；
关键帧保存模块：利用运动滤波器对计算出的当前位姿进行判断，判断是否保存为关键帧，当当前位姿与上一关键帧位移、角度差大于设定阈值时，将该帧数据存储为最新关键帧；
优化位姿模块：将最新关键帧和地图进行匹配，若满足匹配要求，则构建误差项，构成位姿图优化，求解使得误差项最小的待优化关键帧位姿，公式如下
，式中i为关键帧id，/>为信息矩阵，代表该误差项的权重；
更新地图模块：将求解出使得误差项最小的关键帧位姿作为当前地图坐标系下的准确位姿，将激光点云按照优化后的位姿插入至栅格地图中，更新地图；
位姿输出模块：根据优化后的位姿对外实时发布pose，如果没有达到累计关键帧的阈值，则对外发布实时位姿跟踪步骤中的当前位姿；
定时保存更新地图模块：定时保存更新后的地图，编译加载更新后的地图。
本实施例的更新地图模块还包括：更新地图后，清空优化后的关键帧点云和位姿数据。
在一优选实施例中，本实施例的所述激光初始定位模块中，初始位姿的确定包括将激光点云进行栅格滤波，将原始栅格地图按照由粗到细的分辨率生成组地图，从系统最粗的分辨率地图开始匹配，首先将最粗分辨率的地图分成四份，形成四个格子，将这四个格子的局部地图与当前滤波后激光帧计算匹配度，选出score最高的区域，记录为当前best_score，将当前best_score区域使用高一层分辨率的地图maplowest+1继续分四块，在分块后的局部地图中继续计算出最高的score，以此类推，得到原地图分辨率下匹配的best_score，此时计算出best_score对应的位姿就是激光在地图中的位姿，将进行初始位姿的确定的这帧激光保存为第一帧关键帧。
在一优选实施例中，本实施例的实时位姿跟踪模块中，所述激光帧间匹配包括：点云的帧间匹配，所述点云的帧间匹配包括：设上一帧栅格滤波后的点云为，当前栅格滤波后的点云为/> ，求解残差/> ：
，
式中i为点云的顺序id，n为点云数量，R为旋转矩阵，t为平移矩阵，构建最小二乘问题，求使得误差平方和最小的R和t，
，
通过里程计增量数据获得和/>，根据实际激光帧间匹配和里程计精度，确定里程计权重ω（/>），求解出最优/>和/>， 进一步计算当前位姿/>，
，
，
，
式中为旋转的权重，/>为平移权重，/>为单位矩阵， />是上一帧激光的位姿。
在一优选实施例中，本实施例的关键帧保存模块中，位移判断计算公式如下：1
角度判断公式如下：
，
式中、/>、/>分别为实时位姿跟踪步骤中计算出来当前位姿的x、y和角度，/>、/>、/>分别为上一次保存的关键帧位姿的x、y和角度。
若当前位姿的位移判断、角度判断小于设定阈值则返回实时位姿跟踪模块继续计算最新位姿。
在一优选实施例中，本实施例的优化位姿模块中，将最新关键帧和地图进行匹配的匹配方法包括：使用局部分支定界搜索最优解，使用最优解为计算精确位姿的初始解，关键帧保存的激光点云与当前地图的相对位姿/>，地图和激光点云的残差如下式所示，满足该残差最小时的/>即为精确位姿，
，
式中 K 为当前待匹配的点云个数， 为使用双立方插值对的平滑函数，对栅格地图进行插值，/>激光点云，/>为将/> 的所有点转化至当前地图坐标系下。
若激光点云与地图的最小残差小于设定阈值则为匹配，若该最小残差大于或者等于阈值则判断为不匹配，如不匹配则记录相对应的关键帧点云、编号、位姿，等待下一个关键帧进入匹配。
本发明的激光slam自适应自动更新地图的方法及系统可以在环境经常变化的场景中，通过关键帧累计，图优化计算出一系列关键帧准确位姿，将关键帧的点云插入地图，实现地图的准确自动更新，并且更新后的地图可以应用于后续定位。本发明在建立基础地图后无需手动更新地图，更省人力成本和时间。无需对环境进行改造添加标签、反光板或者磁条等辅助材料，部署更为方便。无需添加gps、视觉相机等辅助传感器，仅使用里程计和单线激激光，成本低。对于经常变换环境中的定位问题，有效解决变化环境中定位不准和定位丢失问题。
以上述依据本申请的理想实施例为启示，通过上述的说明内容，相关工作人员完全可以在不偏离本项申请技术思想的范围内，进行多样的变更以及修改。本项申请的技术性范围并不局限于说明书上的内容，必须要根据权利要求范围来确定其技术性范围。
Claims (10)
1.一种激光slam自适应自动更新地图的方法，其特征在于，包括：
加载原始地图：加载原始地图，转换成栅格地图，进入定位；
激光初始定位：接收当前激光数据，确定初始位姿；
实时位姿跟踪：根据激光帧间匹配计算位姿，并融合里程计传感器数据计算位姿，生成当前位姿；
关键帧保存：对计算出的当前位姿进行判断，判断是否保存为关键帧，当当前位姿与上一关键帧位移、角度差大于设定阈值时，将该帧数据存储为最新关键帧；
优化位姿：将最新关键帧和地图进行匹配，若满足匹配要求，则构建误差项，构成位姿图优化，求解使得误差项最小的待优化关键帧位姿，公式如下：
，式中i为关键帧id，/>为信息矩阵，代表该误差项的权重；
更新地图：将求解出使得误差项最小的关键帧位姿作为当前地图坐标系下的准确位姿，将激光点云按照优化后的位姿插入至栅格地图中，更新地图。
2.根据权利要求1所述的激光slam自适应自动更新地图的方法，其特征在于，还包括：位姿输出：根据优化后的位姿对外实时发布pose，如果没有达到累计关键帧的阈值，则对外发布实时位姿跟踪步骤中的当前位姿；
定时保存更新地图：定时保存更新后的地图，加载更新后的地图；
所述更新地图还包括：更新地图后，清空优化后的关键帧点云和位姿数据。
3.根据权利要求1所述的激光slam自适应自动更新地图的方法，其特征在于，所述激光初始定位步骤中，初始位姿的确定包括将激光点云进行栅格滤波，将原始栅格地图按照由粗到细的分辨率生成组地图，从系统最粗的分辨率地图开始匹配，首先将最粗分辨率的地图分成四份，形成四个格子，将这四个格子的局部地图与当前滤波后激光帧计算匹配度，选出score最高的区域，记录为当前best_score，将当前best_score区域使用高一层分辨率的地图maplowest+1继续分四块，在分块后的局部地图中继续计算出最高的score，以此类推，得到原地图分辨率下匹配的best_score，此时计算出best_score对应的位姿就是激光在地图中的位姿，将进行初始位姿的确定的这帧激光保存为第一帧关键帧。
4.根据权利要求1至3任意一项所述的激光slam自适应自动更新地图的方法，其特征在于，所述实时位姿跟踪步骤中，所述激光帧间匹配包括：点云的帧间匹配，所述点云的帧间匹配包括：设上一帧栅格滤波后的点云为，当前栅格滤波后的点云为/> ，求解残差/> ：
，
式中i为点云的顺序id，n为点云数量，R为旋转矩阵，t为平移矩阵，构建最小二乘问题，求使得误差平方和最小的R和t，
，
通过里程计增量数据获得和/>，根据实际激光帧间匹配和里程计精度，确定里程计权重ω（/>），求解出最优/>和/>， 进一步计算当前位姿/>，
，
，
，
式中为旋转的权重，/>为平移权重，/>为单位矩阵， />是上一帧激光的位姿。
5.根据权利要求1至3任意一项所述的激光slam自适应自动更新地图的方法，其特征在于，所述关键帧保存步骤中，利用运动滤波器对计算出的当前位姿进行判断，判断是否保存为关键帧，位移判断计算公式如下：
；
角度判断公式如下：
，
式中、/>、/>分别为实时位姿跟踪步骤中计算出来当前位姿的x、y和角度，/>、/>、/>分别为上一次保存的关键帧位姿的x、y和角度；若当前位姿的位移、角度判断小于设定阈值则返回实时位姿跟踪步骤继续计算最新位姿。
6.根据权利要求1至3任意一项所述的激光slam自适应自动更新地图的方法，其特征在于，所述优化位姿步骤中，将最新关键帧和地图进行匹配的匹配方法包括：使用局部分支定界搜索最优解，使用最优解为计算精确位姿的初始解，关键帧保存的激光点云与当前地图的相对位姿/>，地图和激光点云的残差如下所示，满足残差最小时的/>即为精确位姿，
，
式中 K 为当前待匹配的点云个数， 为使用双立方插值对的平滑函数，对栅格地图进行插值，/>激光点云，/>为将/>的所有点转化至当前地图坐标系下，若激光点云与地图的最小残差小于设定阈值则为匹配，若该最小残差大于或者等于阈值则判断为不匹配，如不匹配则记录相对应的关键帧点云、编号、位姿，等待下一个关键帧进入匹配。
7.一种激光slam自适应自动更新地图的系统，其特征在于，包括：
加载原始地图模块：加载原始地图，转换成栅格地图，进入定位；
激光初始定位模块：接收当前激光数据，确定初始位姿；
实时位姿跟踪模块：通过激光帧间匹配计算位姿，并融合里程计传感器数据计算位姿，生成当前位姿；
关键帧保存模块：利用运动滤波器对计算出的当前位姿进行判断，判断是否保存为关键帧，当当前位姿与上一关键帧位移、角度差大于设定阈值时，将该帧数据存储为最新关键帧；
优化位姿模块：将最新关键帧和地图进行匹配，若满足匹配要求，则构建误差项，构成位姿图优化，求解使得误差项最小的待优化关键帧位姿，公式如下：
，式中i为关键帧id，/>为信息矩阵，代表该误差项的权重；
更新地图模块：将求解出使得误差项最小的关键帧位姿作为当前地图坐标系下的准确位姿，将激光点云按照优化后的位姿插入至栅格地图中，更新地图。
8.根据权利要求7所述的激光slam自适应自动更新地图的系统，其特征在于，还包括：位姿输出模块：根据优化后的位姿对外实时发布pose，如果没有达到累计关键帧的阈值，则对外发布实时位姿跟踪步骤中的当前位姿；
定时保存更新地图模块：定时保存更新后的地图，编译加载更新后的地图；
所述更新地图还包括：更新地图后，清空优化后的关键帧点云和位姿数据。
9.根据权利要求7至8任意一项所述的激光slam自适应自动更新地图的系统，其特征在于，所述激光初始定位模块中，初始位姿的确定包括将激光点云进行栅格滤波，将原始栅格地图按照由粗到细的分辨率生成组地图，从系统最粗的分辨率地图开始匹配，首先将最粗分辨率的地图分成四份，形成四个格子，将这四个格子的局部地图与当前滤波后激光帧计算匹配度，选出score最高的区域，记录为当前best_score，将当前best_score区域使用高一层分辨率的地图maplowest+1继续分四块，在分块后的局部地图中继续计算出最高的score，以此类推，得到原地图分辨率下匹配的best_score，此时计算出best_score对应的位姿就是激光在地图中的位姿，将进行初始位姿的确定的这帧激光保存为第一帧关键帧；
所述实时位姿跟踪模块中，所述激光帧间匹配包括：点云的帧间匹配，所述点云的帧间匹配包括：设上一帧栅格滤波后的点云为，当前栅格滤波后的点云为/> ，求解残差/> ：
，
式中i为点云的顺序id，n为点云数量，R为旋转矩阵，t为平移矩阵，构建最小二乘问题，求使得误差平方和最小的R和t，
，
通过里程计增量数据获得和/>，根据实际激光帧间匹配和里程计精度，确定里程计权重ω（/>），求解出最优/>和/>，进一步计算当前位姿/>，
，
，
，
式中为旋转的权重，/>为平移权重，/>为单位矩阵， />是上一帧激光的位姿。
10.根据权利要求7至8任意一项所述的激光slam自适应自动更新地图的系统，其特征在于，所述关键帧保存模块中，利用运动滤波器对计算出的当前位姿进行判断，判断是否保存为关键帧，位移判断计算公式如下：；
角度判断公式如下：
，
式中、/>、/>分别为实时位姿跟踪步骤中计算出来当前位姿的x、y和角度，/>、/>、/>分别为上一次保存的关键帧位姿的x、y和角度；若当前位姿的位移、角度判断小于设定阈值则返回实时位姿跟踪模块继续计算最新位姿；
所述优化位姿模块中，将最新关键帧和地图进行匹配的匹配方法包括：使用局部分支定界搜索最优解，使用最优解为计算精确位姿的初始解，关键帧保存的激光点云与当前地图的相对位姿/>，地图和激光点云的残差如下所示，满足残差最小时的/>即为精确位姿
，
式中 K 为当前待匹配的点云个数，为使用双立方插值对的平滑函数，对栅格地图进行插值，/>激光点云，/>为将/>的所有点转化至当前地图坐标系下，若激光点云与地图的最小残差小于设定阈值则为匹配，若该最小残差大于或者等于阈值则判断为不匹配，如不匹配则记录相对应的关键帧点云、编号、位姿，等待下一个关键帧进入匹配。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311789336.6A CN117451035B (zh) | 2023-12-25 | 2023-12-25 | 一种激光slam自适应自动更新地图的方法及系统 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311789336.6A CN117451035B (zh) | 2023-12-25 | 2023-12-25 | 一种激光slam自适应自动更新地图的方法及系统 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN117451035A true CN117451035A (zh) | 2024-01-26 |
| CN117451035B CN117451035B (zh) | 2024-02-27 |
Family
ID=89580327
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202311789336.6A Active CN117451035B (zh) | 2023-12-25 | 2023-12-25 | 一种激光slam自适应自动更新地图的方法及系统 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117451035B (zh) |
Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111707281A (zh) * | 2020-06-30 | 2020-09-25 | 华东理工大学 | 一种基于光度信息和orb特征的slam系统 |
| CN112068154A (zh) * | 2020-09-14 | 2020-12-11 | 中科院软件研究所南京软件技术研究院 | 一种激光建图定位方法、装置、存储介质及电子设备 |
| CN112100298A (zh) * | 2020-08-17 | 2020-12-18 | 深圳市优必选科技股份有限公司 | 一种建图方法、装置、计算机可读存储介质及机器人 |
| CN112985416A (zh) * | 2021-04-19 | 2021-06-18 | 湖南大学 | 激光与视觉信息融合的鲁棒定位和建图方法及系统 |
| US20210190513A1 (en) * | 2019-12-20 | 2021-06-24 | Ubtech Robotics Corp Ltd | Navigation map updating method and apparatus and robot using the same |
| CN113701760A (zh) * | 2021-09-01 | 2021-11-26 | 火种源码(中山)科技有限公司 | 基于滑动窗口位姿图优化的机器人抗干扰定位方法及装置 |
| CN114782626A (zh) * | 2022-04-14 | 2022-07-22 | 国网河南省电力公司电力科学研究院 | 基于激光与视觉融合的变电站场景建图及定位优化方法 |
| CN115127538A (zh) * | 2022-05-16 | 2022-09-30 | 浙江华睿科技股份有限公司 | 地图更新方法、计算机设备及存储装置 |
| CN115655291A (zh) * | 2022-09-28 | 2023-01-31 | 重庆中科汽车软件创新中心 | 激光slam闭环建图的方法、装置、移动机器人、设备及介质 |
-
2023
- 2023-12-25 CN CN202311789336.6A patent/CN117451035B/zh active Active
Patent Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20210190513A1 (en) * | 2019-12-20 | 2021-06-24 | Ubtech Robotics Corp Ltd | Navigation map updating method and apparatus and robot using the same |
| CN111707281A (zh) * | 2020-06-30 | 2020-09-25 | 华东理工大学 | 一种基于光度信息和orb特征的slam系统 |
| CN112100298A (zh) * | 2020-08-17 | 2020-12-18 | 深圳市优必选科技股份有限公司 | 一种建图方法、装置、计算机可读存储介质及机器人 |
| CN112068154A (zh) * | 2020-09-14 | 2020-12-11 | 中科院软件研究所南京软件技术研究院 | 一种激光建图定位方法、装置、存储介质及电子设备 |
| CN112985416A (zh) * | 2021-04-19 | 2021-06-18 | 湖南大学 | 激光与视觉信息融合的鲁棒定位和建图方法及系统 |
| CN113701760A (zh) * | 2021-09-01 | 2021-11-26 | 火种源码(中山)科技有限公司 | 基于滑动窗口位姿图优化的机器人抗干扰定位方法及装置 |
| CN114782626A (zh) * | 2022-04-14 | 2022-07-22 | 国网河南省电力公司电力科学研究院 | 基于激光与视觉融合的变电站场景建图及定位优化方法 |
| CN115127538A (zh) * | 2022-05-16 | 2022-09-30 | 浙江华睿科技股份有限公司 | 地图更新方法、计算机设备及存储装置 |
| CN115655291A (zh) * | 2022-09-28 | 2023-01-31 | 重庆中科汽车软件创新中心 | 激光slam闭环建图的方法、装置、移动机器人、设备及介质 |
Non-Patent Citations (1)
| Title |
|---|
| 刘云根;刘金刚;: "重建误差最优化的运动捕获数据关键帧提取", 计算机辅助设计与图形学学报, no. 04, 15 April 2010 (2010-04-15) * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN117451035B (zh) | 2024-02-27 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN108717710B (zh) | 室内环境下的定位方法、装置及系统 | |
| CN112268559B (zh) | 复杂环境下融合slam技术的移动测量方法 | |
| CN113409410B (zh) | 一种基于3d激光雷达的多特征融合igv定位与建图方法 | |
| CN107167826B (zh) | 一种自动驾驶中基于可变网格的图像特征检测的车辆纵向定位系统及方法 | |
| CN112070770B (zh) | 一种高精度三维地图与二维栅格地图同步构建方法 | |
| CN112862894A (zh) | 一种机器人三维点云地图构建与扩充方法 | |
| Ros et al. | Visual slam for driverless cars: A brief survey | |
| JP2019518222A (ja) | リアルタイムオンラインエゴモーション推定を有するレーザスキャナ | |
| CN112965063B (zh) | 一种机器人建图定位方法 | |
| CN112184906B (zh) | 一种三维模型的构建方法及装置 | |
| CN109871739B (zh) | 基于yolo-sioctl的机动站自动目标检测与空间定位方法 | |
| CN114777775B (zh) | 一种多传感器融合的定位方法及系统 | |
| CN115272596A (zh) | 一种面向单调无纹理大场景的多传感器融合slam方法 | |
| CN114429432B (zh) | 一种多源信息分层融合方法、装置及存储介质 | |
| CN115962773A (zh) | 一种移动机器人同步定位与地图构建方法、装置和设备 | |
| CN113379915B (zh) | 一种基于点云融合的行车场景构建方法 | |
| CN113124854A (zh) | 一种视觉定位方法、以及地图构建方法、装置 | |
| CN113838129B (zh) | 一种获得位姿信息的方法、装置以及系统 | |
| CN115143958A (zh) | 一种基于gpu加速的多传感器融合的slam方法 | |
| CN117782063A (zh) | 一种基于图优化的可变滑动窗口的多传感器融合定位方法 | |
| CN117451035B (zh) | 一种激光slam自适应自动更新地图的方法及系统 | |
| CN115655291B (zh) | 激光slam闭环建图的方法、装置、移动机器人、设备及介质 | |
| CN110849349A (zh) | 一种基于磁传感器与轮式里程计融合定位方法 | |
| He et al. | Robust mapping and localization in offline 3D point cloud maps | |
| Hu et al. | Accurate fiducial mapping for pose estimation using manifold optimization |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |