CN113701760A - 基于滑动窗口位姿图优化的机器人抗干扰定位方法及装置 - Google Patents
基于滑动窗口位姿图优化的机器人抗干扰定位方法及装置 Download PDFInfo
- Publication number
- CN113701760A CN113701760A CN202111022656.XA CN202111022656A CN113701760A CN 113701760 A CN113701760 A CN 113701760A CN 202111022656 A CN202111022656 A CN 202111022656A CN 113701760 A CN113701760 A CN 113701760A
- Authority
- CN
- China
- Prior art keywords
- pose
- robot
- positioning
- current
- latest
- 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 76
- 238000005457 optimization Methods 0.000 title claims abstract description 46
- 238000012216 screening Methods 0.000 claims abstract description 22
- 230000033001 locomotion Effects 0.000 claims abstract description 10
- 230000008030 elimination Effects 0.000 claims abstract description 8
- 238000003379 elimination reaction Methods 0.000 claims abstract description 8
- 238000004364 calculation method Methods 0.000 claims description 12
- 230000006870 function Effects 0.000 claims description 7
- 238000006073 displacement reaction Methods 0.000 claims description 5
- 238000009499 grossing Methods 0.000 claims description 3
- 239000000203 mixture Substances 0.000 claims description 3
- 238000003780 insertion Methods 0.000 claims description 2
- 230000037431 insertion Effects 0.000 claims description 2
- 230000004807 localization Effects 0.000 claims description 2
- 239000002994 raw material Substances 0.000 claims 2
- 238000012163 sequencing technique Methods 0.000 claims 1
- 238000010586 diagram Methods 0.000 description 6
- 230000002159 abnormal effect Effects 0.000 description 5
- 238000010276 construction Methods 0.000 description 4
- 238000001914 filtration Methods 0.000 description 4
- 238000012217 deletion Methods 0.000 description 2
- 230000037430 deletion Effects 0.000 description 2
- 230000007613 environmental effect Effects 0.000 description 2
- 230000001960 triggered effect Effects 0.000 description 2
- 230000003044 adaptive effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000000694 effects 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
- 230000003287 optical effect Effects 0.000 description 1
- 239000002245 particle Substances 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 238000004659 sterilization and disinfection Methods 0.000 description 1
- 238000010408 sweeping Methods 0.000 description 1
- 238000013519 translation Methods 0.000 description 1
Images
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/20—Instruments for performing navigational calculations
- G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/88—Lidar systems specially adapted for specific applications
- G01S17/93—Lidar systems specially adapted for specific applications for anti-collision purposes
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Computer Networks & Wireless Communication (AREA)
- Electromagnetism (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Manipulator (AREA)
Abstract
本发明提供一种基于滑动窗口位姿图优化的机器人抗干扰定位方法，包括：获取激光数据，机器人进行重定位：根据机器人是否有初始位姿判断搜索范围，确定机器人的当前位姿；跟踪机器人移动过程中的实时位姿：通过帧间匹配方法计算机器人的实时定位位姿，并输出定位结果；判断在跟踪过程中是否累计足够关键帧，若足够，则删除最早关键帧，进行最新关键帧保存；判断机器人当前定位状态，若当前定位丢失，重新获取最新的准确定位，更新定位，以最新位姿继续位姿跟踪；否则，进行内点筛选和累计误差消除，使得机器人能够在地图内持续输出位姿，实时准确定位。本发明的有益效果是能够实现无需较长时间运动进行全局重定位，提高抗干扰能力。
Description
技术领域
本发明属于自主导航与定位技术领域，尤其是涉及一种基于滑动窗口位姿图优化的机器人抗干扰定位方法及装置。
背景技术
机器人的精准定位，是机器人完成自主导航的必要前提。现有技术常常通过安装磁钉、磁条反光板等标记物来间接判断机器人位姿，但是该方法需要对已有环境进行改造，不适应大多数场景。此外，通过激光SLAM(simultaneous localization and mapping)构建先验地图，基于已有先验地图采用自适应蒙特卡洛算法(adaptive Monte CarloLocalization；AMCL)进行机器人定位，该算法采取粒子滤波的方式，计算量较大且所输出的估算结果不唯一，在无先验位姿前提下全局重定位较为困难或者需要机器人运动较长时间等缺点，实际使用效果不佳，并且在部分发生变化的区域，定位精度变差，使用原生cartographer位姿图优化的方式进行机器人定位，计算量较大，对于低成本的机器人不适用。
发明内容
鉴于上述问题，本发明提供一种基于滑动窗口位姿图优化的机器人抗干扰定位方法及装置，以解决现有技术存在的以上或者其他前者问题。
为解决上述技术问题，本发明采用的技术方案是：一种基于滑动窗口位姿图优化的机器人抗干扰定位方法，包括，
获取激光数据，机器人进行重定位：根据机器人是否有初始位姿判断搜索范围，确定机器人的当前位姿；
跟踪机器人移动过程中的实时位姿：通过帧间匹配方法计算机器人的实时定位位姿，并输出定位结果；
判断在跟踪过程中是否累计足够关键帧，若足够，则删除最早关键帧，进行最新关键帧保存；
判断机器人当前定位状态，若当前定位丢失，重新获取最新的准确定位，更新定位，以最新位姿继续位姿跟踪；
否则，进行内点筛选和累计误差消除，使得机器人能够在地图内持续输出位姿，实时准确定位。
进一步的，获取激光数据，机器人进行重定位步骤中，当机器人没有初始位姿时，搜索全部子图，机器人与所有的子图进行遍历匹配，匹配分数最高的子图为机器人的当前位姿，计算公式为：
其中，ρ为损失函数，E为残差，
为第i个子图的在世界坐标系下的位姿，
为第j个激光帧在世界坐标系下的位姿，∑ij为第j个激光帧插入第i个子图的质量，εij为第j个激光帧插入第i个子图的相对位姿。
进一步的，获取激光数据，机器人进行重定位步骤中，当机器人有初始位姿时，通过搜索先验位姿附近的子图计算机器人的当前位姿，搜索判断公式为：
其中，distanceinit为先验位姿与子图位姿的距离，xinitial为初始位姿的横坐标，yinitial为初始位姿的纵坐标，xsubmap为子图的横坐标，ysubmap为子图的纵坐标。
进一步的，通过帧间匹配方法计算机器人的实时定位位姿，并输出定位结果步骤中，从当前位姿开始建立新的定位子图，当有新的激光帧输入后与从当前位姿开始建立新的定位子图进行匹配，输出实时位姿，计算公式为：
其中，K为当前点云个数，Msmooth为一个使用双立方插值对的平滑函数，hk为激光点云，Tε是将hk的所有点转化至当前定位子图坐标系下。
进一步的，判断在跟踪过程中是否累计足够关键帧，若足够删除最早关键帧，进行最新关键帧保存步骤中，当最新激光帧匹配的位姿和上一关键帧的位移差或角度差大于设定阈值时，将最新激光帧存储为最新关键帧。
进一步的，进行当前定位是否丢失判定时，将最新关键帧和其对应位姿与全局地图进行匹配，获取匹配度，若匹配度累计数量达到第一设定阈值，则判定当前定位丢失。
进一步的，最新关键帧和其对应位姿与全局地图进行匹配，若匹配度大于第二设定阈值时，进行内点筛选，将最新关键帧点云中各个点中的外点逐个剔除，保留内点。
进一步的，当关键帧累计到第三设定阈值后，使用确定初始位姿时的当前位姿为先验位姿构造当前定位子图与全局地图子图之间的运动约束，计算内点筛选后的点云在全局地图中的位姿，更新对应的定位子图，消除累计误差。
进一步的，当关键帧持续累计到第四预设阈值后，且连续多帧激光与原始栅格地图的灰度差小于设定评分阈值，判定此时位姿定位相对准确，删除排序最早的关键帧。
一种基于滑动窗口位姿图优化的机器人抗干扰定位装置，包括，
激光数据获取模块，用于获取激光数据，机器人根据获取的激光数据进行重定位；
当前位姿确定模块，用于根据机器人是否有初始位姿判断搜索范围，确定机器人的当前位姿；
实时位姿跟踪模块，用于跟踪机器人移动过程中的实时位姿，通过帧间匹配方法计算机器人的实时定位位姿，并输出定位结果；
最新关键帧保存模块，用于判断在跟踪过程中是否累计足够关键帧，若足够，则删除最早关键帧，进行最新关键帧保存；
当前定位状态判断模块，用于判断机器人当前定位状态，若当前定位丢失，重新获取最新的准确定位，更新定位，以最新位姿继续位姿跟踪；
内点筛选模块，用于当机器人当前定位没有丢失时，进行内点筛选；
累计误差消除模块，用于当机器人当前定位没有丢失时，进行消除累计误差，使得机器人能够在地图内持续输出位姿，实时准确定位。
一种电子设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的一个或多个程序，当一个或多个程序被处理器执行时，使得电子设备执行如上述的基于滑动窗口位姿图优化的机器人抗干扰定位方法。
一种计算机可读存储介质，计算机可读存储介质上存储有计算机指令，当计算机指令在电子设备的处理器上运行时，使得计算机存储介质所在的设备执行如上述的基于滑动窗口位姿图优化的机器人抗干扰定位方法。
由于采用上述技术方案，能够实现无需较长时间运动进行全局重定位，加入内点筛选机制，提高抗干扰能力，过滤更为精确得点云构建位子图优化消除累计误差，实时位姿跟踪过程中在当前位姿附近的滑动窗口进行准确匹配，输出实时位姿，并且加入与原始地图评分机制，判断定位出现异常时会自动触发全局重定位；
对于机器人有无先验位姿两种情况下对应搜索原地图相关的子图进行全局重定位，无需较长时间运动即可获取当前初始位姿，使用方便；通过与原始地图匹配，加入持续评分和判断定位偏移机制，当定位丢失时及时触发全局重定位就进行矫正，适应能力提高；在实时位姿跟踪过程中，具有内点筛选，使用更为精确的点云构建位姿图约束和位姿图优化来消除累计误差，对于有部分环境变化的区域，依然能够准确定位，提高抗干扰能力；只在当前位姿附近的滑动窗口进行计算，删除较早的数据，减少内存占用，降低计算量，使得能够在低性能处理器上输出准确位姿。
附图说明
图1是本发明的一实施例的流程示意图；
图2是本发明的一实施例的机器人使用cartographer开源算法建立的某办公室场景地示意图；
图3是本发明的一实施例的定位状态示意图；
图4是本发明的一实施例的全局地图的示意图；
图5是本发明的一实施例的实时位姿跟踪和对于最早的关键帧删除示意图。
具体实施方式
下面结合附图和具体实施例对本发明作进一步的说明。
图1示出了本发明的一实施例的流程图，本发明的一实施例涉及一种基于滑动窗口位姿图优化的机器人抗干扰定位方法及装置，用于机器人在未知状态下实时精准定位，能够实现无需较长时间运动进行全局重定位，具有内点筛选机制，提高抗干扰能力，过滤更为精确得点云构建位子图优化消除累计误差，实时位姿跟踪过程中在当前位姿附近的滑动窗口进行准确匹配，输出实时位姿，并且具有与原始地图评分机制，判断定位出现异常时会自动触发全局重定位。
一种基于滑动窗口位姿图优化的机器人抗干扰定位方法，该定位方法使用cartographer构图机制建立已有先验地图，其构造的先验地图由多个子图组合而成，每一个子图都含有对应于全局地图的相对位姿，可以适应于不同种类机器人(扫地机器人、消毒机器人、物流配送机器人等室内作业机器人)的自主导航需求，为导航与运动控制提供实时位姿，确保机器人能够正常工作。
该基于滑动窗口位姿图优化的机器人抗干扰定位方法，包括，
获取激光数据，机器人进行重定位：根据机器人是否有初始位姿判断搜索范围，确定机器人的当前位姿；在该步骤中，首先对机器人是否有初始位姿进行判断，当机器人没有初始位姿时，搜索全部子图，机器人与所有的子图进行遍历匹配，匹配分数最高的子图为机器人的当前位姿，计算公式为：
其中，ρ为损失函数，E为残差，为第i个子图的在世界坐标系下的位姿，为第j个激光帧在世界坐标系下的位姿，为第j个激光帧插入第i个子图的质量，为第j个激光帧插入第i个子图的相对位姿。
机器人开机没有先验位姿时处于完全未知状态，强制机器人尝试与所有的子图进行遍历匹配，得分最高的子图即为机器人当前位姿。
机器人定位使用图优化的方式，将当前的激光帧与待匹配的子图构成一个位姿约束的优化问题，最终变为最小二乘问题解决计算。对于已有的子图与当前激光帧构成的整体误差和如下公式：
其中，ρ为损失函数，用来避免异常点导致的错误匹配，具体使用的是Huber Loss，E为残差，
为第i个子图的在世界坐标系下的位姿，
为第j个激光帧在世界坐标系下的位姿，∑ij为第j个激光帧插入第i个子图的质量，即后续优化的评分值，εij为第j个scan插入第i个子图的相对位姿。
对上述约束进行优化，转化为下面式中的优化问题：
通过尝试与原有地图中多有的子图计算出当前激光帧相对位姿后，初始重定位成功，由未知状态变为已知状态。
在该步骤中，当机器人有初始位姿时，通过搜索先验位姿附近的子图计算机器人的当前位姿，当机器人开机时有先验位姿时，可以根据先验位姿采取只搜索先验位姿附近的子图，计算量会相对减少。先验位姿是其他方式提供一个离机器人真实位姿较近的参考位姿，如：用户输入、模板匹配、其他传感器等。搜索判断公式为：
其中，为先验位姿与子图位姿的距离，为初始位姿的横坐标，为初始位姿的纵坐标，为子图的横坐标，为子图的纵坐标。
当distanceinit大于界限阈值时，该子图不被构建约束加入优化问题，后续以上述的图优化的方式计算出相对准确的初始位姿，机器人由位姿状态变为已知状态。该界限阈值根据实际需求进行设定，这里不做具体要求。
跟踪机器人移动过程中的实时位姿：通过帧间匹配方法计算机器人的实时定位位姿，并输出定位结果；确定初始位姿后，从当前位姿开始建立新的定位子图，其格式由概率栅格表示，当有新的激光帧输入后，与从当前位姿开始建立新的定位子图进行匹配，输出实时位姿，计算公式为：
其中，K为当前点云个数，Msmooth为一个使用双立方插值对的平滑函数，hk为激光点云，Tε是将hk的所有点转化至当前定位子图坐标系下。
通过上述帧间匹配方法可以实时输出当前定位位姿。
判断在跟踪过程中是否累计足够关键帧，若足够，则删除最早关键帧，进行最新关键帧保存；采取空间间隔保存关键帧的储存机制，当最新激光帧匹配的位姿和上一关键帧的位移差或角度差大于设定阈值时，将最新激光帧存储为最新关键帧。该阈值可根据内存大小和精度需求调整。
判断机器人当前定位状态，若当前定位丢失，重新获取最新的准确定位，更新定位，以最新位姿继续位姿跟踪；否则，进行内点筛选和累计误差消除，使得机器人能够在地图内持续输出位姿，实时准确定位。
进行当前定位是否丢失判定时，将最新关键帧和其对应位姿与全局地图进行匹配，该匹配方法采用上述的机器人移动过程中的实时位姿跟踪方法进行匹配，获取匹配度，并输出匹配度，若匹配度累计数量达到第一设定阈值，则判定当前定位丢失；如果当前帧激光与原始栅格地图的灰度差大于设定评分阈值，则评分处于较低状态，且连续多帧激光与原始栅格地图的灰度差大于设定评分阈值，持续评分处于较低状态，匹配度持续较低，累计数量达到第一设定阈值，认为当前定位丢失，该设定评分阈值及第一设定阈值根据实际需求进行选择设定，此时，使用获取激光数据机器人进行重定位步骤中的方法重新获取最新的准确位姿，更新定位，以最新位姿继续位姿跟踪。该第一设定阈值根据实际需求进行选择设定。
如果当前帧激光与原始栅格地图的灰度差小于设定评分阈值，则评分处于较高状态，若连续多帧激光与原始栅格地图的灰度差小于设定评分阈值，匹配度持续较高，则进行后续内点筛选与累计误差消除。由于建图时和位姿跟踪时的实时环境可能部分发生变化，使用当前点云不足够准确与原始地图进行图优化不足够精确，通过当前定位是否丢失判断步骤中的匹配度判断，最新关键帧和其对应位姿与全局地图进行匹配，若匹配度大于第二设定阈值时，进行内点筛选，将最新关键帧点云中各个点中的外点逐个剔除，保留内点，其中，原始地图匹配差异较大的点云视为外点，剔除当前点云中的外点，如果剩余点的个数百分比占原始点云60％以上，则使用滤波过后内点进行后续步骤，提高抗干扰能力。该第二设定阈值根据实际需求进行选择设定。
当机器人开始移动时，通过关键帧保存步骤中匹配实时更新位姿，该方法会产生累计误差，所以，当关键帧累计到第三设定阈值后，使用确定初始位姿时的当前位姿为先验位姿构造当前定位子图与全局地图子图之间的运动约束，利用图优化方式计算内点筛选后的点云在全局地图中的位姿，更新对应的定位子图，消除累计误差。该第三设定阈值根据实际需求进行选择设置。
当关键帧持续累计到第四预设阈值后，且连续多帧激光与原始栅格地图的灰度差小于设定评分阈值，持续评分处于较高状态，判定此时位姿定位相对准确，删除排序最早的关键帧，实现只针对当前位姿附近的滑动窗口实现精准定位，维持系统内存稳定，减少计算量，使得机器人能够在地图内持续输出位姿，在性能较低的处理器上同样有效。
重复上述步骤，可实现机器人实时准确定位。
一种基于滑动窗口位姿图优化的机器人抗干扰定位装置，包括，
激光数据获取模块，用于获取激光数据，机器人根据获取的激光数据进行重定位；
当前位姿确定模块，用于根据机器人是否有初始位姿判断搜索范围，确定机器人的当前位姿；
实时位姿跟踪模块，用于跟踪机器人移动过程中的实时位姿，通过帧间匹配方法计算机器人的实时定位位姿，并输出定位结果；
最新关键帧保存模块，用于判断在跟踪过程中是否累计足够关键帧，若足够，则删除最早关键帧，进行最新关键帧保存；
当前定位状态判断模块，用于判断机器人当前定位状态，若当前定位丢失，重新获取最新的准确定位，更新定位，以最新位姿继续位姿跟踪；
内点筛选模块，用于当机器人当前定位没有丢失时，进行内点筛选；
累计误差消除模块，用于当机器人当前定位没有丢失时，进行消除累计误差，使得机器人能够在地图内持续输出位姿，实时准确定位。
将机器人放置于地图环境中，启动机器人，激光数据获取模块进行激光数据的获取，并根据该获取的激光数据进行重新定位；当前位姿确定模块获取机器人是否有初始位姿，判断机器人的当前位姿，判断搜索范围，计算当前激光点云所在位姿，确定初始位姿，并输出初始位姿，重定位成功，定位状态由未知变位已知；机器人进行移动，移动过程中，实时位姿跟踪模块跟踪机器人移动过程中的实时位姿，通过帧间匹配方法计算机器人的实时定位位姿，并输出定位结果，最新关键帧保存模块判断在跟踪过程中是否累计足够关键帧，若足够，则删除最早关键帧，进行最新关键帧保存，否则，不删除最早关键帧，当前定位状态判断模块判断机器人当前定位状态，若当前定位丢失，重新获取最新的准确定位，更新定位，以最新位姿继续位姿跟踪；否则，内点筛选模块进行内点筛选，累计误差消除模块进行消除累计误差，使得机器人能够在地图内持续输出位姿，实时准确定位。
一种电子设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的一个或多个程序，当一个或多个程序被处理器执行时，使得电子设备执行如上述的基于滑动窗口位姿图优化的机器人抗干扰定位方法。
一种计算机可读存储介质，计算机可读存储介质上存储有计算机指令，当计算机指令在电子设备的处理器上运行时，使得计算机存储介质所在的设备执行如上述的基于滑动窗口位姿图优化的机器人抗干扰定位方法。
上述的各功能模块可以集成在一个处理模块中，也可以是各个模块单独物理存在，也可以两个或两个以上模块集成在一个模块中。上述的集成的模块既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。
集成的模块如果以软件功能单元的形式实现时，可以存储在一个计算机可读取存储介质中。本申请的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机、服务器或者网络设备等)执行本申请中的实施例所述的基于滑动窗口位姿图优化的机器人抗干扰定位方法的全部或部分步骤。
前述的存储介质包括：U盘、移动硬盘、只读存储器、随机存取存储器、磁碟或者光盘等各种可以存储程序代码的介质。
下面以一个具体实施例进行说明。
图2为搭载单线激光雷达的的移动机器人使用cartographer开源算法建立的某办公室场景地图，面积约1000平方米，地图由30个子图合并构成，每个子图有对应的位姿，以机器人在下图环境中的定位过程来具体描述上述的定位方法的使用。
S1：将机器人放置于地图所示环境中，启动机器人，在初始状态未知情况下，将输入的激光点云依次尝试搜索所有原有地图中的子图进行匹配，当找到原地图中满足匹配阈值的子图，则与构成图优化问题进行计算当前激光点云所在位姿。
如果启动机器人时由人工选定方式给予初始位置，则尝试搜索先验位姿附近限定范围10m内的原始地图中的子图作为匹配对象，减少计算量，将输入的激光点云依次尝试搜索初始位姿附近10m内原有地图中的子图进行匹配，同上，后续构成图优化问题进行结算当前激光点云所在位姿。
初始位姿计算输出后，重定位成功，定位状态由未知变为已知。定位状态如图3，与地图环境匹配一致。
S2：在初始状态位姿开始建立新的定位子图，其格式由概率栅格表示，每当有新激光帧输入，与之前建立的定位子图进行匹配得到精确的实时位姿，以此类推。
S3：取空间间隔保存关键帧的储存机制，当最新激光帧匹配位姿与上一关键帧位移与角度差大于设定阈值时(在该套系统配置下平移距离大于20cm或者旋转角度大于3°)，保存一帧关键帧。
S4：最新的关键帧与包含所有原始地图子图信息的全局地图进行匹配，获取匹配度，全局地图如图4所示，如果匹配度良好时进行后续步骤，匹配度持续较低并且累计到15次后，认为当前定位丢失，使用步骤S1中的方法搜索全部子图获取重新计算机器人在地图中的位姿，更新定位。
S5：如果关键帧持续匹配度较高，则将关键帧点云中各个点与原地图匹配较差的点逐个删除，只保留匹配较好的点，视为内点，如果剩余内点个数大于原关键帧中点云个数的60％，则使用筛选过后的内点进行后续图优化操作，如果不足60％，为防止异常过滤，则旋转原关键帧点云进行后续操作。
S6：机器人移动过程中，通过步骤S2中匹配实时更新位姿，但该方法会产生累计误差，所以当关键帧个数累计到一定阈值(该套系统中为20帧)后，使用步骤S1中以当前帧位姿为初始位姿搜索附近可能匹配原地图的子图，将所有当前搜索得到得关键帧与原始地图子图所在得位姿构成图优化问题计算当前机器人最准确得位姿，以此消除累计误差。
S7：当关键帧持续累计到设定阈值(该套系统中设定为100)，并且持续评分处于较高状态，此时位姿定位比较准确，删除排序最早的关键帧，实现只针对当前位姿附近的滑动窗口实现精准位姿输出，图5为实时位姿跟踪和对于最早的关键帧删除示意图。
S8：在机器人位姿跟踪过程中，不断更新建立新的关键帧，同时删除最早得关键帧，保持新建子图保留在当前定位位姿附近，保留基于当前位姿的滑动窗口，在该窗口内的所有关键帧均与原始地图子图可能构成匹配约束，构成图优化问题求解，实时纠正可能产生的累计误差。
重复迭代上述步骤，实现机器人在当前环境中的准确定位。
由于采用上述技术方案，能够实现无需较长时间运动进行全局重定位，加入内点筛选机制，提高抗干扰能力，过滤更为精确得点云构建位子图优化消除累计误差，实时位姿跟踪过程中在当前位姿附近的滑动窗口进行准确匹配，输出实时位姿，并且加入与原始地图评分机制，判断定位出现异常时会自动触发全局重定位；
对于机器人有无先验位姿两种情况下对应搜索原地图相关的子图进行全局重定位，无需较长时间运动即可获取当前初始位姿，使用方便；通过与原始地图匹配，加入持续评分和判断定位偏移机制，当定位丢失时及时触发全局重定位就进行矫正，适应能力提高；在实时位姿跟踪过程中，具有内点筛选，使用更为精确的点云构建位姿图约束和位姿图优化来消除累计误差，对于有部分环境变化的区域，依然能够准确定位，提高抗干扰能力；只在当前位姿附近的滑动窗口进行计算，删除较早的数据，减少内存占用，降低计算量，使得能够在低性能处理器上输出准确位姿。
以上对本发明的实施例进行了详细说明，但所述内容仅为本发明的较佳实施例，不能被认为用于限定本发明的实施范围。凡依本发明申请范围所作的均等变化与改进等，均应仍归属于本发明的专利涵盖范围之内。
Claims (12)
1.一种基于滑动窗口位姿图优化的机器人抗干扰定位方法，其特征在于：包括，
获取激光数据，机器人进行重定位：根据所述机器人是否有初始位姿判断搜索范围，确定所述机器人的当前位姿；
跟踪所述机器人移动过程中的实时位姿：通过帧间匹配方法计算所述机器人的实时定位位姿，并输出定位结果；
判断在跟踪过程中是否累计足够关键帧，若足够，则删除最早关键帧，进行最新关键帧保存；
判断所述机器人当前定位状态，若当前定位丢失，重新获取最新的准确定位，更新定位，以最新位姿继续位姿跟踪；
否则，进行内点筛选和累计误差消除，使得所述机器人能够在地图内持续输出位姿，实时准确定位。
2.根据权利要求1所述的基于滑动窗口位姿图优化的机器人抗干扰定位方法，其特征在于：所述获取激光数据，机器人进行重定位步骤中，当所述机器人没有初始位姿时，搜索全部子图，所述机器人与所有的子图进行遍历匹配，匹配分数最高的子图为所述机器人的当前位姿，计算公式为：
其中，ρ为损失函数，E为残差，
为第i个子图的在世界坐标系下的位姿，
为第j个激光帧在世界坐标系下的位姿，∑ij为第j个激光帧插入第i个子图的质量，εij为第j个激光帧插入第i个子图的相对位姿。
3.根据权利要求2所述的基于滑动窗口位姿图优化的机器人抗干扰定位方法，其特征在于：所述获取激光数据，机器人进行重定位步骤中，当所述机器人有初始位姿时，通过搜索先验位姿附近的子图计算机器人的当前位姿，搜索判断公式为：
其中，distanceinit为先验位姿与子图位姿的距离，xinitial为初始位姿的横坐标，yinitial为初始位姿的纵坐标，xsubmap为子图的横坐标，ysubmap为子图的纵坐标。
4.根据权利要求1-3任一项所述的基于滑动窗口位姿图优化的机器人抗干扰定位方法，其特征在于：所述通过帧间匹配方法计算所述机器人的实时定位位姿，并输出定位结果步骤中，从当前位姿开始建立新的定位子图，当有新的激光帧输入后与所述从当前位姿开始建立新的定位子图进行匹配，输出实时位姿，计算公式为：
其中，K为当前点云个数，Msmooth为一个使用双立方插值对的平滑函数，hk为激光点云，Tε是将hk的所有点转化至当前定位子图坐标系下。
5.根据权利要求4所述的基于滑动窗口位姿图优化的机器人抗干扰定位方法，其特征在于：所述判断在跟踪过程中是否累计足够关键帧，若足够删除最早关键帧，进行最新关键帧保存步骤中，当最新激光帧匹配的位姿和上一关键帧的位移差或角度差大于设定阈值时，将所述最新激光帧存储为最新关键帧。
6.根据权利要求1-3和5任一项所述的基于滑动窗口位姿图优化的机器人抗干扰定位方法，其特征在于：进行当前定位是否丢失判定时，将所述最新关键帧和其对应位姿与全局地图进行匹配，获取匹配度，若所述匹配度累计数量达到第一设定阈值，则判定当前定位丢失。
7.根据权利要求6所述的基于滑动窗口位姿图优化的机器人抗干扰定位方法，其特征在于：所述最新关键帧和其对应位姿与全局地图进行匹配，若匹配度大于第二设定阈值时，进行所述内点筛选，将所述最新关键帧点云中各个点中的外点逐个剔除，保留内点。
8.根据权利要求7所述的基于滑动窗口位姿图优化的机器人抗干扰定位方法，其特征在于：当关键帧累计到第三设定阈值后，使用确定初始位姿时的当前位姿为先验位姿构造当前定位子图与所述全局地图子图之间的运动约束，计算内点筛选后的点云在所述全局地图中的位姿，更新对应的定位子图，消除累计误差。
9.根据权利要求7或8所述的基于滑动窗口位姿图优化的机器人抗干扰定位方法，其特征在于：当关键帧持续累计到第四预设阈值后，且连续多帧激光与原始栅格地图的灰度差小于设定评分阈值，判定此时位姿定位相对准确，删除排序最早的关键帧。
10.一种基于滑动窗口位姿图优化的机器人抗干扰定位装置，其特征在于：包括，
激光数据获取模块，用于获取激光数据，机器人根据获取的激光数据进行重定位；
当前位姿确定模块，用于根据所述机器人是否有初始位姿判断搜索范围，确定所述机器人的当前位姿；
实时位姿跟踪模块，用于跟踪所述机器人移动过程中的实时位姿，通过帧间匹配方法计算所述机器人的实时定位位姿，并输出定位结果；
最新关键帧保存模块，用于判断在跟踪过程中是否累计足够关键帧，若足够，则删除最早关键帧，进行最新关键帧保存；
当前定位状态判断模块，用于判断所述机器人当前定位状态，若当前定位丢失，重新获取最新的准确定位，更新定位，以最新位姿继续位姿跟踪；
内点筛选模块，用于当机器人当前定位没有丢失时，进行内点筛选；
累计误差消除模块，用于当机器人当前定位没有丢失时，进行消除累计误差，使得所述机器人能够在地图内持续输出位姿，实时准确定位。
11.一种电子设备，其特征在于：包括存储器、处理器及存储在存储器上并可在处理器上运行的一个或多个程序，当所述一个或多个程序被所述处理器执行时，使得所述电子设备执行如权利要求1-9任一项所述的基于滑动窗口位姿图优化的机器人抗干扰定位方法。
12.一种计算机可读存储介质，其特征在于：所述计算机可读存储介质上存储有计算机指令，当所述计算机指令在电子设备的处理器上运行时，使得所述计算机存储介质所在的设备执行如权利要求1-9任一项所述的基于滑动窗口位姿图优化的机器人抗干扰定位方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111022656.XA CN113701760B (zh) | 2021-09-01 | 2021-09-01 | 基于滑动窗口位姿图优化的机器人抗干扰定位方法及装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111022656.XA CN113701760B (zh) | 2021-09-01 | 2021-09-01 | 基于滑动窗口位姿图优化的机器人抗干扰定位方法及装置 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113701760A true CN113701760A (zh) | 2021-11-26 |
| CN113701760B CN113701760B (zh) | 2024-02-27 |
Family
ID=78658879
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111022656.XA Active CN113701760B (zh) | 2021-09-01 | 2021-09-01 | 基于滑动窗口位姿图优化的机器人抗干扰定位方法及装置 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113701760B (zh) |
Cited By (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114442808A (zh) * | 2022-01-26 | 2022-05-06 | 深圳市慧鲤科技有限公司 | 一种位姿跟踪模块的测试方法、装置、设备、系统和介质 |
| CN115035115A (zh) * | 2022-08-11 | 2022-09-09 | 聊城市宏伟电器有限公司 | 一种纸包铜扁线质量监测方法 |
| CN115290098A (zh) * | 2022-09-30 | 2022-11-04 | 成都朴为科技有限公司 | 一种基于变步长的机器人定位方法和系统 |
| CN115326051A (zh) * | 2022-08-03 | 2022-11-11 | 广州高新兴机器人有限公司 | 一种基于动态场景的定位方法、装置、机器人及介质 |
| CN115685133A (zh) * | 2022-12-30 | 2023-02-03 | 安徽蔚来智驾科技有限公司 | 自动驾驶车辆的定位方法、控制装置、存储介质及车辆 |
| CN115984516A (zh) * | 2022-12-16 | 2023-04-18 | 工业富联(杭州)数据科技有限公司 | 基于slam算法的增强现实方法及相关设备 |
| CN117451035A (zh) * | 2023-12-25 | 2024-01-26 | 江苏中科重德智能科技有限公司 | 一种激光slam自适应自动更新地图的方法及系统 |
Citations (12)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107990899A (zh) * | 2017-11-22 | 2018-05-04 | 驭势科技（北京）有限公司 | 一种基于slam的定位方法和系统 |
| CN109544636A (zh) * | 2018-10-10 | 2019-03-29 | 广州大学 | 一种融合特征点法和直接法的快速单目视觉里程计导航定位方法 |
| WO2019169540A1 (zh) * | 2018-03-06 | 2019-09-12 | 斯坦德机器人（深圳）有限公司 | 紧耦合视觉slam的方法、终端及计算机可读存储介质 |
| CN111507132A (zh) * | 2019-01-31 | 2020-08-07 | 杭州海康机器人技术有限公司 | 一种定位方法、装置及设备 |
| CN111739063A (zh) * | 2020-06-23 | 2020-10-02 | 郑州大学 | 一种基于多传感器融合的电力巡检机器人定位方法 |
| CN111915651A (zh) * | 2020-07-31 | 2020-11-10 | 西安电子科技大学 | 基于数字影像地图与特征点跟踪的视觉位姿实时估计方法 |
| CN112068154A (zh) * | 2020-09-14 | 2020-12-11 | 中科院软件研究所南京软件技术研究院 | 一种激光建图定位方法、装置、存储介质及电子设备 |
| CN112115874A (zh) * | 2020-09-21 | 2020-12-22 | 武汉大学 | 一种融合云端的视觉slam系统及方法 |
| CN112179330A (zh) * | 2020-09-14 | 2021-01-05 | 浙江大华技术股份有限公司 | 移动设备的位姿确定方法及装置 |
| CN112304307A (zh) * | 2020-09-15 | 2021-02-02 | 浙江大华技术股份有限公司 | 一种基于多传感器融合的定位方法、装置和存储介质 |
| CN113012212A (zh) * | 2021-04-02 | 2021-06-22 | 西北农林科技大学 | 一种基于深度信息融合的室内场景三维点云重建方法和系统 |
| US20210190513A1 (en) * | 2019-12-20 | 2021-06-24 | Ubtech Robotics Corp Ltd | Navigation map updating method and apparatus and robot using the same |
-
2021
- 2021-09-01 CN CN202111022656.XA patent/CN113701760B/zh active Active
Patent Citations (12)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107990899A (zh) * | 2017-11-22 | 2018-05-04 | 驭势科技（北京）有限公司 | 一种基于slam的定位方法和系统 |
| WO2019169540A1 (zh) * | 2018-03-06 | 2019-09-12 | 斯坦德机器人（深圳）有限公司 | 紧耦合视觉slam的方法、终端及计算机可读存储介质 |
| CN109544636A (zh) * | 2018-10-10 | 2019-03-29 | 广州大学 | 一种融合特征点法和直接法的快速单目视觉里程计导航定位方法 |
| CN111507132A (zh) * | 2019-01-31 | 2020-08-07 | 杭州海康机器人技术有限公司 | 一种定位方法、装置及设备 |
| US20210190513A1 (en) * | 2019-12-20 | 2021-06-24 | Ubtech Robotics Corp Ltd | Navigation map updating method and apparatus and robot using the same |
| CN111739063A (zh) * | 2020-06-23 | 2020-10-02 | 郑州大学 | 一种基于多传感器融合的电力巡检机器人定位方法 |
| CN111915651A (zh) * | 2020-07-31 | 2020-11-10 | 西安电子科技大学 | 基于数字影像地图与特征点跟踪的视觉位姿实时估计方法 |
| CN112068154A (zh) * | 2020-09-14 | 2020-12-11 | 中科院软件研究所南京软件技术研究院 | 一种激光建图定位方法、装置、存储介质及电子设备 |
| CN112179330A (zh) * | 2020-09-14 | 2021-01-05 | 浙江大华技术股份有限公司 | 移动设备的位姿确定方法及装置 |
| CN112304307A (zh) * | 2020-09-15 | 2021-02-02 | 浙江大华技术股份有限公司 | 一种基于多传感器融合的定位方法、装置和存储介质 |
| CN112115874A (zh) * | 2020-09-21 | 2020-12-22 | 武汉大学 | 一种融合云端的视觉slam系统及方法 |
| CN113012212A (zh) * | 2021-04-02 | 2021-06-22 | 西北农林科技大学 | 一种基于深度信息融合的室内场景三维点云重建方法和系统 |
Non-Patent Citations (1)
| Title |
|---|
| 侯荣波;魏武;黄婷;邓超锋;: "基于ORB-SLAM的室内机器人定位和三维稠密地图构建", 计算机应用, no. 05 * |
Cited By (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114442808A (zh) * | 2022-01-26 | 2022-05-06 | 深圳市慧鲤科技有限公司 | 一种位姿跟踪模块的测试方法、装置、设备、系统和介质 |
| CN115326051A (zh) * | 2022-08-03 | 2022-11-11 | 广州高新兴机器人有限公司 | 一种基于动态场景的定位方法、装置、机器人及介质 |
| CN115035115A (zh) * | 2022-08-11 | 2022-09-09 | 聊城市宏伟电器有限公司 | 一种纸包铜扁线质量监测方法 |
| CN115035115B (zh) * | 2022-08-11 | 2022-10-25 | 聊城市宏伟电器有限公司 | 一种纸包铜扁线质量监测方法 |
| CN115290098A (zh) * | 2022-09-30 | 2022-11-04 | 成都朴为科技有限公司 | 一种基于变步长的机器人定位方法和系统 |
| CN115984516A (zh) * | 2022-12-16 | 2023-04-18 | 工业富联(杭州)数据科技有限公司 | 基于slam算法的增强现实方法及相关设备 |
| CN115984516B (zh) * | 2022-12-16 | 2023-11-17 | 工业富联(杭州)数据科技有限公司 | 基于slam算法的增强现实方法及相关设备 |
| CN115685133A (zh) * | 2022-12-30 | 2023-02-03 | 安徽蔚来智驾科技有限公司 | 自动驾驶车辆的定位方法、控制装置、存储介质及车辆 |
| CN117451035A (zh) * | 2023-12-25 | 2024-01-26 | 江苏中科重德智能科技有限公司 | 一种激光slam自适应自动更新地图的方法及系统 |
| CN117451035B (zh) * | 2023-12-25 | 2024-02-27 | 江苏中科重德智能科技有限公司 | 一种激光slam自适应自动更新地图的方法及系统 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113701760B (zh) | 2024-02-27 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN113701760A (zh) | 基于滑动窗口位姿图优化的机器人抗干扰定位方法及装置 | |
| CN111536964B (zh) | 机器人定位方法及装置、存储介质 | |
| CN108550318B (zh) | 一种构建地图的方法及装置 | |
| CN107967457B (zh) | 一种适应视觉特征变化的地点识别与相对定位方法及系统 | |
| Rottensteiner et al. | Automatic extraction of buildings from LIDAR data and aerial images | |
| CN110849374A (zh) | 地下环境定位方法、装置、设备及存储介质 | |
| CN111563442A (zh) | 基于激光雷达的点云和相机图像数据融合的slam方法及系统 | |
| CN110717983A (zh) | 一种基于背包式三维激光点云数据的建筑物立面三维重建方法 | |
| CN112882056B (zh) | 基于激光雷达的移动机器人同步定位与地图构建方法 | |
| EP2420975A1 (en) | System and method for 3d wireframe reconstruction from video | |
| CN113483747B (zh) | 基于具有墙角信息的语义地图改进amcl定位方法及机器人 | |
| CN110223380B (zh) | 融合航拍与地面视角图像的场景建模方法、系统、装置 | |
| CN115290097B (zh) | 基于bim的实时精确地图构建方法、终端及存储介质 | |
| WO2011085435A1 (en) | Classification process for an extracted object or terrain feature | |
| CN113096181B (zh) | 设备位姿的确定方法、装置、存储介质及电子装置 | |
| CN113191412A (zh) | 设备重定位方法、装置、计算机设备和存储介质 | |
| CN116448118B (zh) | 一种扫地机器人的工作路径优化方法和装置 | |
| CN111951341A (zh) | 一种基于rgb-d slam的闭环检测改进方法 | |
| KR101766823B1 (ko) | 불규칙한 조도 변화에 강건한 영상 기반 주행거리 측정 시스템 및 방법 | |
| WO2011085433A1 (en) | Acceptation/rejection of a classification of an object or terrain feature | |
| CN116698043A (zh) | 一种室内移动机器人视觉导航方法 | |
| CN116363319A (zh) | 一种建筑物屋顶的建模方法、建模装置、设备及介质 | |
| Román et al. | Towards the automatic 3D parametrization of non-planar surfaces from point clouds in HBIM applications | |
| CN115512054A (zh) | 三维时空连续点云地图的构建方法 | |
| CN114115231B (zh) | 移动机器人空间位姿点云校正方法及系统 |
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 |