CN114236552B - 基于激光雷达的重定位方法及系统 - Google Patents

基于激光雷达的重定位方法及系统 Download PDF

Info

Publication number
CN114236552B
CN114236552B CN202111342049.1A CN202111342049A CN114236552B CN 114236552 B CN114236552 B CN 114236552B CN 202111342049 A CN202111342049 A CN 202111342049A CN 114236552 B CN114236552 B CN 114236552B
Authority
CN
China
Prior art keywords
point cloud
robot
key frame
pose
repositioning
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.)
Active
Application number
CN202111342049.1A
Other languages
English (en)
Other versions
CN114236552A (zh
Inventor
隋佳城
彭勇
程艳涛
俞剑敏
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Suzhou Jiuwu Intelligent Technology Co ltd
Original Assignee
Suzhou Jiuwu Intelligent Technology Co ltd
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Suzhou Jiuwu Intelligent Technology Co ltd filed Critical Suzhou Jiuwu Intelligent Technology Co ltd
Priority to CN202111342049.1A priority Critical patent/CN114236552B/zh
Publication of CN114236552A publication Critical patent/CN114236552A/zh
Application granted granted Critical
Publication of CN114236552B publication Critical patent/CN114236552B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • 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/02—Systems using the reflection of electromagnetic waves other than radio waves
    • G01S17/06—Systems determining position data of a target
    • 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/89—Lidar systems specially adapted for specific applications for mapping or imaging
    • 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
    • G01S17/931—Lidar systems specially adapted for specific applications for anti-collision purposes of land vehicles

Landscapes

  • Physics & Mathematics (AREA)
  • Engineering & Computer Science (AREA)
  • Electromagnetism (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • General Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Manipulator (AREA)

Abstract

本发明涉及一种基于激光雷达的重定位方法及系统，包括以下步骤：机器人遍历整个环境空间，通过机器人上的激光雷达传感器采集环境信息，构建完整的点云地图；机器人继续工作并记录当前时刻的点云数据和位姿信息，将当前时刻的激光点云数据和其位姿信息作为关键帧保存，直至机器人跑完点云地图，获得包含关键帧信息的点云数据库；当机器人定位丢失时，获取机器人当前时刻的点云数据，将机器人当前时刻的点云数据与点云数据库进行匹配搜索，获得搜索匹配对应的关键帧；根据搜索匹配的关键帧，计算机器人全局位姿信息以实现重定位。其重定位精度高，准确快捷，应用范围广。

Description

基于激光雷达的重定位方法及系统
技术领域
本发明涉及机器人定位技术领域，尤其是指一种基于激光雷达的重定位方法及系统。
背景技术
移动机器人在复杂环境下实行导航任务时，经常会出现其定位丢失的情形，从而导致其导航任务的失败。目前解决机器人定位丢失的方案主要有如下两种:第一、利用GPS卫星信号的重定位，当机器人定位丢失时，利用GPS信号计算出其位置信息，从而实现机器人的重新定位；第二、基于图像特征的视觉定位，当机器人定位丢失的时候，利用机器人当前状态下获取的图像特征与建图时所保存的全局图像特征库进行匹配，当匹配成功时，即可获得机器人的位姿。
上述两种方案都有其不足之处，第一种方案由于GPS信号的强弱和精度会受所处环境的影响，比如当机器人在桥梁下，隧道内，地下室及室内或处于高大树木建筑附近，其GPS信号精度都会受到不同程度的影响，因而基于GPS重定位的应用具有一定的局限性；而第二种基于图像视觉特征的方案同样会受到其所处环境的影响，在光照变化显著或者环境纹理特征缺失的情况下，会出现误匹配甚至匹配失败，都会导致重定位的失败。
发明内容
为此，本发明所要解决的技术问题在于克服现有技术中定位受GPS信号强弱和精度影响较大，以及定位受光照及环境纹理特征缺失所导致重定位失败的技术缺陷。
为解决上述技术问题，本发明提供了一种基于激光雷达的重定位方法，包括以下步骤：
S1、机器人遍历整个环境空间，通过机器人上的激光雷达传感器采集环境信息，构建完整的点云地图；
S2、机器人继续工作并记录当前时刻的点云数据和位姿信息，将当前时刻的激光点云数据和其位姿信息作为关键帧保存，直至机器人跑完S1中的点云地图，获得包含关键帧信息的点云数据库；
S3、当机器人定位丢失时，获取机器人当前时刻的点云数据，将机器人当前时刻的点云数据与点云数据库进行匹配搜索，获得搜索匹配对应的关键帧；
S4、根据搜索匹配的关键帧，计算机器人全局位姿信息以实现重定位。
作为优选的，所述S2中，所述关键帧的结构体还包括激光点云数据的scancontext描述。
作为优选的，所述S2中，机器人继续工作并记录当前时刻的点云数据和位姿信息，将当前时刻的激光点云数据和其位姿信息作为关键帧保存，包括：
获取当前时刻下机器人在地图坐标系下的位姿话题，提取当前关键帧；
计算机器人里程计位姿态与当前最近的关键帧位姿的相对变换，所述相对变换包括机器人的空间位置变换和机器人的空间旋转角度变换；
当机器人的空间位置变换大于预设距离阈值或者当机器人的空间旋转角度变换大于预设角度阈值时，创建新的关键帧；
将创建的新的关键帧赋值给当前关键帧以更新当前关键帧；
保存更新后的当前的关键帧的位姿、当前时刻的点云数据并计算其scan context描述。
作为优选的，获取当前时刻下机器人在地图坐标系下的位姿话题，提取当前关键帧；计算机器人里程计位姿态与当前最近的关键帧位姿的相对变换，所述相对变换包括机器人的空间位置变换和机器人的空间旋转角度变换，具体包括：
设机器人发布的里程计位姿为Ti，当前关键帧的位姿TKcurrent，设机器人初始里程计位姿为T0,令机器人初始关键帧位姿TK0＝T0；
当机器人在运行定位的时候，计算其中，ΔT为i时刻的机器人里程计位姿态与当前最近的关键帧位姿的相对变换,ΔT包括空间位置变换ΔP和空间旋转角度变换ΔQ。
作为优选的，所述S3包括：
S31、将当前位姿的一帧点云数据用scan context表示；
S32、在保存好的地图关键帧中scan context中搜索出相近的候选关键帧，获得最近邻的多个候选关键帧；
S33、遍历候选关键帧，选择距离最近的帧作为匹配帧并返回此关键帧。
作为优选的，所述S31和S32之间还包括：
计算scan context的每一行的均值作为RingKey；
所述S32具体包括：用保存的第一关键帧的RingKey构建Kd-tree以搜索候选关键帧。
作为优选的，所述S33具体包括：
不断对任一候选关键帧的scan context进行循环右移操作，依次计算出候选关键帧与当前scan context的距离值，选择其中距离最近的一个候选关键帧作为匹配帧，并返回此关键帧。
作为优选的，所述S4包括：
获取搜索匹配的关键帧，将所述关键帧的位姿作为下一步点云匹配的初始位姿；
计算当前的激光点云和关键帧点云的FPFH，使用截断最小乘和半定松弛算法获得激光点云和关键帧点云的相对旋转变量；
将所述相对旋转变量作为迭代最近点算法的初值，计算获得实际的激光点云和关键帧点云的相对位姿变换；
将所述相对位姿变换乘以关键帧的点云，获得全局位姿；
将全局位姿传入定位节点，所述定位节点计算全局位姿的匹配误差；
将所述匹配误差与预设误差阈值比较，若匹配误差小于预设误差阈值，则重定位成功，否则，重定位失败。
本发明还公开了一种基于激光雷达的重定位系统，包括：
建图模块，所述建图模块用于驱动机器人遍历整个环境空间，通过机器人上的激光雷达传感器采集环境信息，构建完整的点云地图；
点云数据库构建模块，所述点云数据库构建模块驱动机器人继续工作并记录当前时刻的点云数据和位姿信息，将当前时刻的激光点云数据和其位姿信息作为关键帧保存，直至机器人跑完建图模块获取的点云地图，获得包含关键帧信息的点云数据库；
搜索匹配模块，当机器人定位丢失时，所述搜索匹配模块获取机器人当前时刻的点云数据，将机器人当前时刻的点云数据与点云数据库进行匹配搜索，获得搜索匹配对应的关键帧；
重定位模块，所述重定位模块根据搜索匹配的关键帧，计算机器人全局位姿信息以实现重定位。
作为优选的，所述重定位模块具体执行以下操作：
获取搜索匹配的关键帧，将所述关键帧的位姿作为下一步点云匹配的初始位姿；
计算当前的激光点云和关键帧点云的FPFH，使用截断最小乘和半定松弛算法获得激光点云和关键帧点云的相对旋转变量；
将所述相对旋转变量作为迭代最近点算法的初值，计算获得实际的激光点云和关键帧点云的相对位姿变换；
将所述相对位姿变换乘以关键帧的点云，获得全局位姿；
将全局位姿传入定位节点，所述定位节点计算全局位姿的匹配误差；
将所述匹配误差与预设误差阈值比较，若匹配误差小于预设误差阈值，则重定位成功，否则，重定位失败。
本发明的上述技术方案相比现有技术具有以下优点：
1、本发明在定位模式下建立重定位地图，节省内存，简单高效。
2、本发明运用scan context进行点云的匹配搜索，准确快捷。
3、本发明基于截断最小二乘和FPFH特征计算匹配初值，精度更高。
4、本发明不依赖GPS信号，应用范围广。
5、本发明中的机器人不受所处环境的影响，在光照变化显著或者环境纹理特征缺失的情况下，不会出现误匹配甚至于匹配失败，重定位效果好。
附图说明
图1为本发明基于激光雷达的重定位方法的流程图；
图2为基于激光雷达重定位的系统示意图；
图3为基于激光传感器的重定位地图构建流程图；
图4为基于点云scan context的搜索匹配算法；
图5为重定位全局位姿计算流程图。
具体实施方式
下面结合附图和具体实施例对本发明作进一步说明，以使本领域的技术人员可以更好地理解本发明并能予以实施，但所举实施例不作为对本发明的限定。
参照图1-n所示，本发明公开了一种基于激光雷达的重定位方法，包括以下步骤：
步骤一、机器人遍历整个环境空间，通过机器人上的激光雷达传感器采集环境信息，构建完整的点云地图。
步骤二、机器人继续工作并记录当前时刻的点云数据和位姿信息，将当前时刻的激光点云数据和其位姿信息作为关键帧保存，直至机器人跑完步骤一中的点云地图，获得包含关键帧信息的点云数据库。其中，所述关键帧的结构体还包括激光点云数据的scancontext(上下文扫描)描述。
机器人继续工作并记录当前时刻的点云数据和位姿信息，将当前时刻的激光点云数据和其位姿信息作为关键帧保存，包括：获取当前时刻下机器人在地图坐标系下的位姿话题，提取当前关键帧；计算机器人里程计位姿态与当前最近的关键帧位姿的相对变换，所述相对变换包括机器人的空间位置变换和机器人的空间旋转角度变换；当机器人的空间位置变换大于预设距离阈值或者当机器人的空间旋转角度变换大于预设角度阈值时，创建新的关键帧；将创建的新的关键帧赋值给当前关键帧以更新当前关键帧；保存更新后的当前的关键帧的位姿、当前时刻的点云数据并计算其scan context描述。
具体的，设机器人发布的里程计位姿为Ti，当前关键帧的位姿TKcurrent，设机器人初始里程计位姿为T0,令机器人初始关键帧位姿TK0＝T0；
当机器人在运行定位的时候，计算其中，ΔT为i时刻的机器人里程计位姿态与当前最近的关键帧位姿的相对变换,ΔT包括空间位置变换ΔP和空间旋转角度变换ΔQ。
步骤三、当机器人定位丢失时，获取机器人当前时刻的点云数据，将机器人当前时刻的点云数据与点云数据库进行匹配搜索，获得搜索匹配对应的关键帧，包括：
S31、将当前位姿的一帧点云数据用scan context表示；
S32、在保存好的地图关键帧中scan context中搜索出相近的候选关键帧，获得最近邻的多个候选关键帧；
S33、遍历候选关键帧，选择距离最近的帧作为匹配帧并返回此关键帧。
其中，所述S31和S32之间还包括：计算scan context的每一行的均值作为RingKey；所述S32具体包括：用保存的第一关键帧的RingKey构建Kd-tree以搜索候选关键帧。
所述S33具体包括：不断对任一候选关键帧的scan context进行循环右移操作，依次计算出候选关键帧与当前scan context的距离值，选择其中距离最近的一个候选关键帧作为匹配帧，并返回此关键帧。
步骤四、根据搜索匹配的关键帧，计算机器人全局位姿信息以实现重定位，包括：
S41、获取搜索匹配的关键帧，将所述关键帧的位姿作为下一步点云匹配的初始位姿；
S42、计算当前的激光点云和关键帧点云的FPFH，使用截断最小乘和半定松弛算法获得激光点云和关键帧点云的相对旋转变量；
S43、将所述相对旋转变量作为迭代最近点算法的初值，计算获得实际的激光点云和关键帧点云的相对位姿变换；
S44、将所述相对位姿变换乘以关键帧的点云，获得全局位姿；
S45、将全局位姿传入定位节点，所述定位节点计算全局位姿的匹配误差；
S46、将所述匹配误差与预设误差阈值比较，若匹配误差小于预设误差阈值，则重定位成功，否则，重定位失败。
本发明还公开了一种基于激光雷达的重定位系统，包括建图模块、点云数据库构建模块、搜索匹配模块和重定位模块。
所述建图模块用于驱动机器人遍历整个环境空间，通过机器人上的激光雷达传感器采集环境信息，构建完整的点云地图。
所述点云数据库构建模块驱动机器人继续工作并记录当前时刻的点云数据和位姿信息，将当前时刻的激光点云数据和其位姿信息作为关键帧保存，直至机器人跑完建图模块获取的点云地图，获得包含关键帧信息的点云数据库。
当机器人定位丢失时，所述搜索匹配模块获取机器人当前时刻的点云数据，将机器人当前时刻的点云数据与点云数据库进行匹配搜索，获得搜索匹配对应的关键帧。
所述重定位模块根据搜索匹配的关键帧，计算机器人全局位姿信息以实现重定位，具体的：获取搜索匹配的关键帧，将所述关键帧的位姿作为下一步点云匹配的初始位姿；计算当前的激光点云和关键帧点云的FPFH，使用截断最小乘和半定松弛算法获得激光点云和关键帧点云的相对旋转变量；将所述相对旋转变量作为迭代最近点算法的初值，计算获得实际的激光点云和关键帧点云的相对位姿变换；将所述相对位姿变换乘以关键帧的点云，获得全局位姿；将全局位姿传入定位节点，所述定位节点计算全局位姿的匹配误差；将所述匹配误差与预设误差阈值比较，若匹配误差小于预设误差阈值，则重定位成功，否则，重定位失败。
下面，结合具体实施例对本发明中的技术方案做进一步说明。
1.系统组成
本发明系统主要包括以下三个模块：1.构建重定位地图模块；2.scan context匹配搜索模块；3.全局位姿匹配计算模块。其系统示意图如图2所示。
2.构建重定位地图
激光SLAM中点云地图是由一系列激光点云及其scan context描述和其在地图中所对应的位姿组成的。因此，本发明中定义的关键帧结构体包括激光点云数据与其scancontext描述和其位姿信息。
当在ROS(机器人操作系统)环境下运行本方案定位节点时，会发布当前时刻下机器人在地图坐标系下的位姿话题。设机器人发布的里程计位姿为Ti，当前关键帧的位姿TKcurrent，设机器人初始里程计位姿为T0,令机器人初始关键帧位姿为TK0＝T0。当机器人在运行定位的时候，计算其中ΔT为i时刻的机器人里程计位姿态与当前最近的关键帧位姿的相对变换。其中ΔT包括，空间位置变换ΔP和空间旋转角度变换ΔQ。
每当ΔP大于一定距离或者ΔQ大于一定角度时(角度和距离阈值可视地图大小确定)，将创建一个新的关键帧，并把刚刚创建的关键帧赋值给TKcurrent，并保存当前的关键帧位姿TKj，以及当前时刻的点云数据并计算其scan context描述。其流程图见图3。
3.scan context搜索匹配
当重定位地图构建完毕后，重定位节点运行期间导入地图。地图中关键帧位姿及其点云是一一对应的。当定位丢失后，计算过程如下：
将当前位姿的一帧点云数据用scan context表示，并在保存好的地图关键帧中scan context中搜索出相近的候选关键帧。可选地，为了加快搜索速度，在存储关键帧数据scan context时，可以计算scan context的每一行的均值作为RingKey,然后依此RingKey值构造kd-tree结构。
通常上一步会得到最近邻的几个候选关键帧，又因scan context的数据表现形式与机器人的姿态有关，所以需要不断对任一候选关键帧的scan context进行循环右移操作，依次计算出与当前scan context的距离值，然后选择其中距离最小的一个关键帧，并返回此关键帧。其流程图见图4所示。
4.点云匹配计算全局位姿
主要思想：计算当前激光点云与检索到的关键帧之间的位姿相对变换，将计算得到的相对位姿变换乘上检索到的关键帧对应的地图位姿，得到的位姿即机器人在地图上的全局位姿。参照图5所示，其主要流程如下：
A.取出步骤3搜索匹配对应的关键帧，将此关键帧的位姿作为下一步点云匹配的初始位姿。
B.对关键帧的点云和当前的激光点云计算FPFH(点云特征直方图)，使用截断最小二乘和半定松弛算法解出两帧点云的相对旋转变量。
C.以此作为初值放入ICP(Iterative Closest Point，迭代最近点)算法中，计算实际的两帧点云的相对位姿变换。
D.最后将计算后的位姿变换乘以关键帧的点云，得到的就是全局位姿。
E.将计算得到的全局位姿，传入定位节点，如果定位节点计算出此位姿匹配误差大，则说明重定位位姿不正确，则重定位失败，需要人工定位。
具体的，本发明的开发环境及数据格式如下：开发语言：C++；开发依赖开源软件库：ROS,PCL,Teaserplusplus,Eigen；数据存储格式：ros格式,txt格式。
其中一实施例具体如下：
1.在ROS(机器人操作系统)环境下运行定位节点，计算通过Eigen库计算出平移变换量t和旋转变换量r。
2.当t>2米或者r>15度，保存当前的位姿，点云数据。
3.将当前位姿的一帧点云数据用scan context表示。
4、将当前时刻的位姿，点云数据组成一个关键帧，重复2,3,4步骤，直到完成重定位建图。
5、启动重定位节点，导入建好的重定位地图，等待定位程序发出重定位请求。
6、当前位姿的一帧点云数据用scan context表示，并在保存的地图关键帧scancontext中搜索出最近的关键帧。
7、对当前时刻的点云和找到的最近关键帧提取FPFH特征，用Teaserplusplus库将FPFH特征作为输入，计算两帧点云的相对旋转量。
8、将两帧点云和步骤7计算好的相对旋转量作为初值放入ICP计算，得到的位姿乘以步骤6返回的关键帧位姿，即为全局重定位位姿。如果定位节点位姿误差大于1.5，则需要手动重定位。
本领域内的技术人员应明白，本申请的实施例可提供为方法、系统、或计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。
本申请是参照根据本申请实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
显然，上述实施例仅仅是为清楚地说明所作的举例，并非对实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式变化或变动。这里无需也无法对所有的实施方式予以穷举。而由此所引伸出的显而易见的变化或变动仍处于本发明创造的保护范围之中。

Claims (7)

1.一种基于激光雷达的重定位方法，其特征在于，包括以下步骤：
S1、机器人遍历整个环境空间，通过机器人上的激光雷达传感器采集环境信息，构建完整的点云地图；
S2、机器人继续工作并记录当前时刻的点云数据和位姿信息，将当前时刻的激光点云数据和其位姿信息作为关键帧保存，直至机器人跑完S1中的点云地图，获得包含关键帧信息的点云数据库；所述关键帧的结构体还包括激光点云数据的scan context描述；
S3、当机器人定位丢失时，获取机器人当前时刻的点云数据，将机器人当前时刻的点云数据与点云数据库进行匹配搜索，获得搜索匹配对应的关键帧；
S4、根据搜索匹配的关键帧，计算机器人全局位姿信息以实现重定位；所述S4包括：
获取搜索匹配的关键帧，将所述关键帧的位姿作为下一步点云匹配的初始位姿；
计算当前的激光点云和关键帧点云的FPFH，使用截断最小乘和半定松弛算法获得激光点云和关键帧点云的相对旋转变量；
将所述相对旋转变量作为迭代最近点算法的初值，计算获得实际的激光点云和关键帧点云的相对位姿变换；
将所述相对位姿变换乘以关键帧的点云，获得全局位姿；
将全局位姿传入定位节点，所述定位节点计算全局位姿的匹配误差；
将所述匹配误差与预设误差阈值比较，若匹配误差小于预设误差阈值，则重定位成功，否则，重定位失败。
2.根据权利要求1所述的基于激光雷达的重定位方法，其特征在于，所述S2中，机器人继续工作并记录当前时刻的点云数据和位姿信息，将当前时刻的激光点云数据和其位姿信息作为关键帧保存，包括：
获取当前时刻下机器人在地图坐标系下的位姿话题，提取当前关键帧；
计算机器人里程计位姿态与当前最近的关键帧位姿的相对变换，所述相对变换包括机器人的空间位置变换和机器人的空间旋转角度变换；
当机器人的空间位置变换大于预设距离阈值或者当机器人的空间旋转角度变换大于预设角度阈值时，创建新的关键帧；
将创建的新的关键帧赋值给当前关键帧以更新当前关键帧；
保存更新后的当前的关键帧的位姿、当前时刻的点云数据并计算其scan context描述。
3.根据权利要求2所述的基于激光雷达的重定位方法，其特征在于，获取当前时刻下机器人在地图坐标系下的位姿话题，提取当前关键帧；计算机器人里程计位姿态与当前最近的关键帧位姿的相对变换，所述相对变换包括机器人的空间位置变换和机器人的空间旋转角度变换，具体包括：
设机器人发布的里程计位姿为Ti，当前关键帧的位姿TKcurrent，设机器人初始里程计位姿为T0,令机器人初始关键帧位姿TK0＝T0；
当机器人在运行定位的时候，计算其中，ΔT为i时刻的机器人里程计位姿态与当前最近的关键帧位姿的相对变换,ΔT包括空间位置变换ΔP和空间旋转角度变换ΔQ。
4.根据权利要求1所述的基于激光雷达的重定位方法，其特征在于，所述S3包括：
S31、将当前位姿的一帧点云数据用scan context表示；
S32、在保存好的地图关键帧中scan context中搜索出相近的候选关键帧，获得最近邻的多个候选关键帧；
S33、遍历候选关键帧，选择距离最近的帧作为匹配帧并返回此关键帧。
5.根据权利要求4所述的基于激光雷达的重定位方法，其特征在于，所述S31和S32之间还包括：
计算scan context的每一行的均值作为RingKey；
所述S32具体包括：用保存的第一关键帧的RingKey构建Kd-tree以搜索候选关键帧。
6.根据权利要求4所述的基于激光雷达的重定位方法，其特征在于，所述S33具体包括：
不断对任一候选关键帧的scan context进行循环右移操作，依次计算出候选关键帧与当前scan context的距离值，选择其中距离最近的一个候选关键帧作为匹配帧，并返回此关键帧。
7.一种基于激光雷达的重定位系统，其特征在于，包括：
建图模块，所述建图模块用于驱动机器人遍历整个环境空间，通过机器人上的激光雷达传感器采集环境信息，构建完整的点云地图；
点云数据库构建模块，所述点云数据库构建模块驱动机器人继续工作并记录当前时刻的点云数据和位姿信息，将当前时刻的激光点云数据和其位姿信息作为关键帧保存，直至机器人跑完建图模块获取的点云地图，获得包含关键帧信息的点云数据库；所述关键帧的结构体还包括激光点云数据的scan context描述；
搜索匹配模块，当机器人定位丢失时，所述搜索匹配模块获取机器人当前时刻的点云数据，将机器人当前时刻的点云数据与点云数据库进行匹配搜索，获得搜索匹配对应的关键帧；
重定位模块，所述重定位模块根据搜索匹配的关键帧，计算机器人全局位姿信息以实现重定位；所述重定位模块具体执行以下操作：
获取搜索匹配的关键帧，将所述关键帧的位姿作为下一步点云匹配的初始位姿；
计算当前的激光点云和关键帧点云的FPFH，使用截断最小乘和半定松弛算法获得激光点云和关键帧点云的相对旋转变量；
将所述相对旋转变量作为迭代最近点算法的初值，计算获得实际的激光点云和关键帧点云的相对位姿变换；
将所述相对位姿变换乘以关键帧的点云，获得全局位姿；
将全局位姿传入定位节点，所述定位节点计算全局位姿的匹配误差；
将所述匹配误差与预设误差阈值比较，若匹配误差小于预设误差阈值，则重定位成功，否则，重定位失败。
CN202111342049.1A 2021-11-12 2021-11-12 基于激光雷达的重定位方法及系统 Active CN114236552B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111342049.1A CN114236552B (zh) 2021-11-12 2021-11-12 基于激光雷达的重定位方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111342049.1A CN114236552B (zh) 2021-11-12 2021-11-12 基于激光雷达的重定位方法及系统

Publications (2)

Publication Number Publication Date
CN114236552A CN114236552A (zh) 2022-03-25
CN114236552B true CN114236552B (zh) 2024-05-31

Family

ID=80749513

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111342049.1A Active CN114236552B (zh) 2021-11-12 2021-11-12 基于激光雷达的重定位方法及系统

Country Status (1)

Country Link
CN (1) CN114236552B (zh)

Families Citing this family (23)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114413882B (zh) * 2022-03-29 2022-08-05 之江实验室 一种基于多假设跟踪的全局初定位方法和装置
CN114643579B (zh) * 2022-03-29 2024-01-16 深圳优地科技有限公司 一种机器人定位方法、装置、机器人及存储介质
CN114814868B (zh) * 2022-04-06 2024-07-19 广东工业大学 一种双手爪攀爬机器人系统及其同时定位与建图方法
CN114758000B (zh) * 2022-04-20 2025-03-18 北京京东乾石科技有限公司 一种终端重定位方法和装置
CN115096308B (zh) * 2022-05-31 2025-05-02 中国科学院深圳先进技术研究院 机器人、定位方法、定位系统、定位装置以及存储介质
CN115468557B (zh) * 2022-08-12 2025-08-01 深圳市优必选科技股份有限公司 重定位方法、装置、机器人及计算机可读存储介质
CN117685945A (zh) * 2022-09-02 2024-03-12 广州视源电子科技股份有限公司 一种机器人的位姿确定方法及电子设备
CN115630185B (zh) * 2022-09-23 2024-02-02 深圳市云洲创新科技有限公司 一种重定位方法、水面航行器及存储介质
CN115436968A (zh) * 2022-09-23 2022-12-06 福建（泉州）哈工大工程技术研究院 一种基于激光雷达的位图化重定位方法
CN115655291B (zh) * 2022-09-28 2024-08-30 重庆中科汽车软件创新中心 激光slam闭环建图的方法、装置、移动机器人、设备及介质
CN115683126A (zh) * 2022-11-14 2023-02-03 中汽创智科技有限公司 一种车辆重定位方法、装置、电子设备及存储介质
CN115855082B (zh) * 2022-12-07 2023-10-20 北京理工大学 一种基于点云先验地图的双模式快速重定位方法
CN116359930A (zh) * 2023-02-24 2023-06-30 浙江安吉智电控股有限公司 停车场移动充电机器人地图定位方法、装置、设备及介质
CN116148808B (zh) * 2023-04-04 2024-01-26 江苏集萃清联智控科技有限公司 一种基于点云描述符的自动驾驶激光重定位方法和系统
CN116592888A (zh) * 2023-05-08 2023-08-15 五八智能科技(杭州)有限公司 一种巡逻机器人全局定位的方法、系统、装置和介质
CN117031481B (zh) * 2023-08-14 2024-09-03 北京数字绿土科技股份有限公司 一种基于投影3d激光点云的移动机器人重定位方法及系统
CN117516535A (zh) * 2023-11-01 2024-02-06 霞智科技有限公司 一种基于关键帧地图的三维激光雷达定位方法
CN117495968B (zh) * 2024-01-02 2024-05-17 苏州中德睿博智能科技有限公司 基于3d激光雷达的移动机器人位姿跟踪方法及装置
CN118915079B (zh) * 2024-07-23 2025-11-11 武汉新电电气股份有限公司 基于变电站三维点云地图的激光雷达重定位的系统及方法
CN119478327B (zh) * 2024-10-24 2025-09-26 广州骊珠科技有限责任公司 环境帧的回环检测方法
CN119291699B (zh) * 2024-11-11 2025-07-29 青岛蚂蚁机器人有限责任公司 基于激光雷达的重定位方法
CN120672809B (zh) * 2025-08-20 2025-10-28 齐鲁空天信息研究院 一种基于点云配准的农机机库定位方法及装置
CN120993440B (zh) * 2025-10-23 2026-01-02 成都量芯集成科技有限公司 一种适用于室内环境的基于激光雷达的重定位方法

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106296693A (zh) * 2016-08-12 2017-01-04 浙江工业大学 基于3d点云fpfh特征实时三维空间定位方法
CN111024100A (zh) * 2019-12-20 2020-04-17 深圳市优必选科技股份有限公司 一种导航地图更新方法、装置、可读存储介质及机器人
CN111239763A (zh) * 2020-03-06 2020-06-05 广州视源电子科技股份有限公司 对象的定位方法、装置、存储介质和处理器
CN112419464A (zh) * 2020-12-02 2021-02-26 中北大学 一种基于点云局部凹凸性的三维碎块拼接方法
CN112817026A (zh) * 2021-01-29 2021-05-18 西人马帝言(北京)科技有限公司 运动对象的位姿确定方法、装置、设备及存储介质
CN113129373A (zh) * 2021-04-02 2021-07-16 南京航空航天大学 一种基于卷积神经网络的室内移动机器人视觉定位方法
CN113138373A (zh) * 2021-03-31 2021-07-20 苏州玖物互通智能科技有限公司 激光雷达测量值修正方法、误差补偿模型以及激光雷达
CN113485346A (zh) * 2021-07-15 2021-10-08 上海交通大学 一种移动机器人在核事故复杂环境中的自主导航方法

Family Cites Families (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP3391339A2 (en) * 2015-12-18 2018-10-24 Iris Automation, Inc. Real-time visual situational awareness system
CN110307838B (zh) * 2019-08-26 2019-12-10 深圳市优必选科技股份有限公司 机器人重定位方法、装置、计算机可读存储介质及机器人

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106296693A (zh) * 2016-08-12 2017-01-04 浙江工业大学 基于3d点云fpfh特征实时三维空间定位方法
CN111024100A (zh) * 2019-12-20 2020-04-17 深圳市优必选科技股份有限公司 一种导航地图更新方法、装置、可读存储介质及机器人
CN111239763A (zh) * 2020-03-06 2020-06-05 广州视源电子科技股份有限公司 对象的定位方法、装置、存储介质和处理器
CN112419464A (zh) * 2020-12-02 2021-02-26 中北大学 一种基于点云局部凹凸性的三维碎块拼接方法
CN112817026A (zh) * 2021-01-29 2021-05-18 西人马帝言(北京)科技有限公司 运动对象的位姿确定方法、装置、设备及存储介质
CN113138373A (zh) * 2021-03-31 2021-07-20 苏州玖物互通智能科技有限公司 激光雷达测量值修正方法、误差补偿模型以及激光雷达
CN113129373A (zh) * 2021-04-02 2021-07-16 南京航空航天大学 一种基于卷积神经网络的室内移动机器人视觉定位方法
CN113485346A (zh) * 2021-07-15 2021-10-08 上海交通大学 一种移动机器人在核事故复杂环境中的自主导航方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
Point Cloud Registration Based on One-Point RANSAC and Scale-Annealing Biweight Estimation;Jiayuan Li;IEEE TRANSACTIONS ON GEOSCIENCE AND REMOTE SENSING;20211111;第59卷(第11期);第9716 - 9729页 *
TEASER: Fast and Certifiable Point Cloud Registration;Yang;IEEE Transactions on Robotics;20210430;第37卷(第2期);314-333 *
基于图像边缘的彩色点云配准;王琛;中国体视学与图像分析;20210625;第26卷(第2期);第155-162页 *

Also Published As

Publication number Publication date
CN114236552A (zh) 2022-03-25

Similar Documents

Publication Publication Date Title
CN114236552B (zh) 基于激光雷达的重定位方法及系统
CN112179330B (zh) 移动设备的位姿确定方法及装置
CN114187418B (zh) 回环检测方法、点云地图构建方法、电子设备及存储介质
US10803619B2 (en) Method and system for efficiently mining dataset essentials with bootstrapping strategy in 6DOF pose estimate of 3D objects
CN107796397A (zh) 一种机器人双目视觉定位方法、装置和存储介质
KR20200118677A (ko) 이미지를 기반으로 포즈 계산을 위한 지도의 최신성을 유지하는 방법 및 시스템
CN113375657B (zh) 电子地图的更新方法、装置和电子设备
EP3729225A1 (en) Interactive computer-implemented method, graphical user interface and computer program product for building a high-accuracy environment map
CN114092638B (zh) 点云地图构建方法、装置、设备、存储介质及计算机程序
CN112123343B (zh) 点云匹配方法、设备及存储介质
CN114120279A (zh) 一种交通标识牌更新方法、系统、电子设备及存储介质
CN115963508A (zh) 激光雷达和imu的紧耦合slam方法及系统
CN115482252B (zh) 基于运动约束的slam闭环检测和位姿图优化方法
CN117870646A (zh) 一种适用于地下矿环境激光slam建图的方法
CN115112122B (zh) 基于多传感器融合的定位方法、系统、设备及存储介质
CN114547223A (zh) 轨迹预测方法、轨迹预测模型的训练方法及装置
WO2023273169A1 (zh) 一种融合视觉与激光的2.5d地图构建方法
CN121311737A (zh) 地图更新方法和装置、自主移动装置、存储介质
CN110413716B (zh) 数据存储和数据查询方法、装置及电子设备
CN114419155A (zh) 一种基于激光雷达辅助的视觉建图方法
CN116592897B (zh) 基于位姿不确定性的改进orb-slam2定位方法
WO2022252482A1 (zh) 机器人及其环境地图构建方法和装置
CN115147561B (zh) 位姿图生成方法、高精地图生成方法和装置
CN112269378B (zh) 一种激光定位方法及装置
CN119687934B (zh) Agv导航方法、装置、设备、介质及程序产品

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
CB02 Change of applicant information
CB02 Change of applicant information

Address after: 215000 floor 6, building 5, building 3, Tianyun Plaza, No. 111, Wusongjiang Avenue, Guoxiang street, Wuzhong District, Suzhou City, Jiangsu Province

Applicant after: Suzhou Jiuwu interworking Intelligent Technology Co.,Ltd.

Address before: 1 / F, building B1, Dongfang Chuangzhi garden, 18 JinFang Road, Suzhou Industrial Park, 215000, Jiangsu Province

Applicant before: Suzhou Jiuwu Interchange Intelligent Technology Co.,Ltd.

CB02 Change of applicant information
CB02 Change of applicant information

Address after: 215000 floor 6, building 5, building 3, Tianyun Plaza, No. 111, Wusongjiang Avenue, Guoxiang street, Wuzhong District, Suzhou City, Jiangsu Province

Applicant after: Suzhou Jiuwu Intelligent Technology Co.,Ltd.

Address before: 215000 floor 6, building 5, building 3, Tianyun Plaza, No. 111, Wusongjiang Avenue, Guoxiang street, Wuzhong District, Suzhou City, Jiangsu Province

Applicant before: Suzhou Jiuwu interworking Intelligent Technology Co.,Ltd.

GR01 Patent grant
GR01 Patent grant