CN113720324A - 一种八叉树地图构建方法及系统 - Google Patents

一种八叉树地图构建方法及系统 Download PDF

Info

Publication number
CN113720324A
CN113720324A CN202111002549.0A CN202111002549A CN113720324A CN 113720324 A CN113720324 A CN 113720324A CN 202111002549 A CN202111002549 A CN 202111002549A CN 113720324 A CN113720324 A CN 113720324A
Authority
CN
China
Prior art keywords
point cloud
map
coordinate system
pose
mobile robot
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.)
Pending
Application number
CN202111002549.0A
Other languages
English (en)
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.)
University of Shanghai for Science and Technology
Original Assignee
University of Shanghai for Science and Technology
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 University of Shanghai for Science and Technology filed Critical University of Shanghai for Science and Technology
Priority to CN202111002549.0A priority Critical patent/CN113720324A/zh
Publication of CN113720324A publication Critical patent/CN113720324A/zh
Pending legal-status Critical Current

Links

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/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
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C22/00—Measuring distance traversed on the ground by vehicles, persons, animals or other moving solid bodies, e.g. using odometers, using pedometers
    • 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/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
    • 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
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/24—Classification techniques
    • G06F18/243—Classification techniques relating to the number of classes
    • G06F18/24323—Tree-organised classifiers

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • General Physics & Mathematics (AREA)
  • Data Mining & Analysis (AREA)
  • Electromagnetism (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Theoretical Computer Science (AREA)
  • Bioinformatics & Cheminformatics (AREA)
  • General Engineering & Computer Science (AREA)
  • Evolutionary Computation (AREA)
  • Evolutionary Biology (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Bioinformatics & Computational Biology (AREA)
  • Automation & Control Theory (AREA)
  • Artificial Intelligence (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明公开了一种八叉树地图构建方法及系统。该方法包括：根据激光雷达和里程计数据，采用激光图优化SLAM算法确定关键帧以及关键帧下的移动机器人位姿；采集环境点云信息，并将点云数据在相机坐标系下的坐转换到世界坐标系下；将坐标转换后的点云信息进行拼接，得到三维点云地图；基于三维点云地图，对激光图优化SLAM算法进行回环检测和后端优化，得到更新后的移动机器人位姿和更新后的三维点云地图；对更新后的三维点云地图进行滤波，去除更新后的三维点云地图的离群点；将滤波后的三维点云地图转化为八叉树地图。相对于点云地图，本发明构建的八叉树地图易于存储，占用内存空间更小，灵活方便，可直接用于导航。

Description

一种八叉树地图构建方法及系统
技术领域
本发明涉及移动机器人三维地图构建技术领域，特别是涉及一种八叉树地图构建方法及系统。
背景技术
随着应用场景和任务的要求，二维地图已经不能满足任务要求，三维地图更能够详细描述三维空间环境，因此对三维地图的需求也越来越多，三维地图的应用不仅用于移动机器人的定位，还需要其他的应用需求，比如三维重建，导航，避障等。三维地图的表现形式主要有点云和八叉树两种。其中，点云地图构建方式主要有3D激光SLAM、视觉SLAM、2D激光与视觉结合的SLAM。其中，视觉SLAM对光照和环境敏感，对计算机算力要求高，且计算位姿不如激光精确。3D激光SLAM成本高昂，且需要对大量点云进行匹配等操作，对计算机性能要求高。相比于视觉和3D激光构建点云地图的方法，2D激光与相机结合的方式更合适，2D激光图优化SLAM的可靠性和精度更高，例如Cartographer和Karto图优化SLAM等算法已经成为了较为成熟的2D激光的建图算法，更适用于日常环境和大场景建图等。在2D激光图优化SLAM基础上，采用相机的点云信息进行点云地图构建更具有实际意义。
但三维点云地图是稀疏的，移动机器人并不能知道三维点云地图是否可以通过。同时点云地图还提供了不必要的细节。例如窗帘的褶皱等我们不需要关系的细节，把这种信息放在地图里浪费空间。因此需要转换为一种压缩的、可更新的和分辨率可调的八叉树地图。
发明内容
为了解决上述问题，本发明提供了一种八叉树地图构建方法及系统。
为实现上述目的，本发明提供了如下方案：
一种八叉树地图构建方法，包括：
根据激光雷达和里程计数据，采用激光图优化SLAM算法确定关键帧以及关键帧下的移动机器人位姿；
采集环境点云信息，并将点云数据在相机坐标系下的坐转换到世界坐标系下；
将坐标转换后的点云信息进行拼接，得到三维点云地图；
基于三维点云地图，对激光图优化SLAM算法进行回环检测和后端优化，得到更新后的移动机器人位姿和更新后的三维点云地图；
对更新后的三维点云地图进行滤波，去除更新后的三维点云地图的离群点；
将滤波后的三维点云地图转化为八叉树地图。
可选地，采用激光图优化SLAM算法确定关键帧，具体包括：
若当前帧为第一帧，则不进行扫描匹配；
若当前帧不是第一帧，则判断当前帧与上一关键帧的采集时间间隔、移动距离或航向角的差值是否超过设定的阈值；
若是，则判断当前帧为关键帧；
若否，则舍弃当前帧。
可选地，所述将点云数据在相机坐标系下的坐转换到世界坐标系下，具体包括：
获取相机坐标系与移动机器人坐标系的位姿转换关系；
根据所述移动机器人位姿和所述位姿转换关系，将点云数据在相机坐标系下的坐转换到世界坐标系下。
可选地，所述点云坐标转换公式为：
Figure BDA0003236018970000021
其中，[xw,yw,zw]表示转换后点云坐标，
Figure BDA0003236018970000022
表示移动机器人位姿，
Figure BDA0003236018970000023
表示相机坐标系与移动机器人坐标系的位姿转换关系，[xc,yc,zc]表示点云数据在相机坐标系下的坐标，w表示世界坐标系，r表示机器人坐标系，c表示相机坐标系。
可选地，所述后端优化的过程如下：
设c＝(c1,c2,...,cn)是需要优化的节点的位姿，因移动机器人在二维平面内运动，所以有：
Figure BDA0003236018970000031
其中，
Figure BDA0003236018970000032
表示第i节点处机器人在地图中的位置，θi表示第i节点处机器人在地图中的方向角；T表示矩阵的转置；
第i个节点和第j个节点之间建立的边约束所对应的相对变换关系为：
Figure BDA0003236018970000033
Ri为与θi相关的余弦公式矩阵：
Figure BDA0003236018970000034
当前边的误差函数eij表示为：
Figure BDA0003236018970000035
其中，
Figure BDA0003236018970000036
为观测量，hij为期望值；
总误差函数F(c)表示为：
Figure BDA0003236018970000037
式中，Ωij为表示边误差所占权重的信息矩阵；Fij表示第i节点和第j节点之间的误差；
极小化总误差函数消除累积误差，得到更新后的机器人位姿c*，从而构建最优的地图：
Figure BDA0003236018970000038
本发明还提供了一种八叉树地图构建系统，包括：
关键帧以及关键帧下的移动机器人位姿确定模块，用于根据激光雷达和里程计数据，采用激光图优化SLAM算法确定关键帧以及关键帧下的移动机器人位姿；
坐标转换模块，用于采集环境点云信息，并将点云数据在相机坐标系下的坐转换到世界坐标系下；
拼接模块，用于将坐标转换后的点云信息进行拼接，得到三维点云地图；
回环检测和后端优化模块，用于基于三维点云地图，对激光图优化SLAM算法进行回环检测和后端优化，得到更新后的移动机器人位姿和更新后的三维点云地图；
滤波模块，用于对更新后的三维点云地图进行滤波，去除更新后的三维点云地图的离群点；
转化模块，用于将滤波后的三维点云地图转化为八叉树地图。
可选地，所述坐标转换模块具体包括：
位姿转换关系获取单元，用于获取相机坐标系与移动机器人坐标系的位姿转换关系；
坐标转换单元，用于根据所述移动机器人位姿和所述位姿转换关系，将点云数据在相机坐标系下的坐转换到世界坐标系下。
根据本发明提供的具体实施例，本发明公开了以下技术效果：
1)本发明基于激光图优化SLAM算法获取关键帧位姿，相对于视觉图优化SLAM，精度更高，对计算资源要求更低，且理论充分，实际可行；
2)拼接形成的点云地图，不需要复杂的匹配计算，通过坐标转换即可实现拼接得到点云地图；
3)最终的八叉树地图易于存储，占用内存空间更小，灵活方便，可用于后续的移动机器人导航等。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
图1为本发明实施例八叉树地图构建的流程图；
图2为本发明实施例仿真实验平台；
图3为本发明实施例仿真实验环境；
图4为本发明实施例所用2D激光图优化SLAM算法流程图；
图5为本发明实施例所用2D激光图优化SLAM算法中回环检测示意图；
图6为本发明实施例处理点云地图离群点所用半径滤波；
图7为本发明实施例构建的无颜色八叉树地图；
图8为本发明实施例构建的有颜色八叉树地图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。
如图1所示，本发明提供的一种八叉树地图构建方法，包括以下步骤：
步骤1：根据激光雷达和里程计数据，采用激光图优化SLAM算法确定关键帧以及关键帧下的移动机器人位姿。
控制移动机器人运动并根据传感器的输入运行激光图优化SLAM算法，过程中，不断根据激光图优化SLAM算法关键帧选取规则筛选关键帧，计算并保存SLAM计算出的此刻移动机器人世界坐标系下位姿索引。
本实施例中，使用安装Hokuyo二维激光雷达和Kinect相机的两轮驱动移动机器人Turtlebot作为实验平台，如图2所示。采用Gazebo仿真环境进行仿真建模，如图3所示。基于2D激光图优化SLAM算法Karto图优化SLAM，输入传感器原始数据，运行得到Karto图优化SLAM算法的激光关键帧信息和移动机器人的位姿信息，并同时触发Kinect相机采集点云数据。
数据的采集与2D图优化SLAM算法处理：通过键盘控制Turtlebot在仿真环境中移动，运动时同时采集IMU、里程计和激光雷达原始数据并传给2D激光Karto图优化SLAM算法，算法的流程如图4所示，此时根据激光图优化SLAM的关键帧筛选要求：第一，若当前帧为第一帧，则不用进行扫描匹配；第二，判断当前帧数据采集时间与上一关键帧采集时间间隔、移动距离、航向角的差值是否超过设定的阈值，若满足其中条件之一，则判断为关键帧，若不满足，则舍弃此帧，继续判断下一帧是否为关键帧。
在判断为关键帧，得到关键帧下的移动机器人位姿，保存此刻的位姿索引值便于后续的更新。
步骤2：采集环境点云信息，并将点云数据在相机坐标系下的坐转换到世界坐标系下。
触发Kinect相机采集此时的环境点云信息并进行实时的坐标转换，过程如下：
根据在图优化SLAM算法中保存的Turtlebot机器人在世界坐标系的位姿信息、相机坐标系下点云坐标，以及相机在机器人坐标系下的坐标，可以实现将点云坐标转换到世界坐标系下，进行点云拼接，获得三维点云地图。
Kinect相机安装于移动机器人上方固定位置处，可以通过测量直接得出相机坐标系与移动机器人坐标系的位姿转换关系。Kinect相机可以直接采集到点云数据在相机坐标系下的坐标([xc,yc,zc])，根据步骤1的中得到的激光关键帧下的移动机器人在世界坐标系下的位姿
Figure BDA0003236018970000061
以及Kinect相机与移动机器人的坐标转换关系
Figure BDA0003236018970000062
经过一系列的坐标转换可以得到三维点云在世界坐标系下的坐标([xw,yw,zw])。由于经过筛选后的激光关键帧可以代表某一时间段某一区域的环境特征，因此相对应的，当前关键帧的移动机器人位姿下采集的Kinect点云数据可以实现对当前区域的完全表示而没有过多的重叠。具体坐标变换的公式为：
Figure BDA0003236018970000071
其中，
Figure BDA0003236018970000072
Figure BDA0003236018970000073
和
Figure BDA0003236018970000074
是3×3的旋转矩阵，
Figure BDA0003236018970000075
和
Figure BDA0003236018970000076
是3×1的位移矩阵。
步骤3：将坐标转换后的点云信息进行拼接，得到三维点云地图。对采集的所有帧点云实时的进行上述转换，可逐步完成所有点云在世界坐标系下的拼接，构建实时点云地图。
步骤4：基于三维点云地图，对激光图优化SLAM算法进行回环检测和后端优化，得到更新后的移动机器人位姿和更新后的三维点云地图。
通过观测实时拼接的点云地图，直到机器人在环境中运动扫描形成闭环。此时，激光图优化SLAM回环检测和后端优化结束，更新对应索引的机器人位姿，从而更新转换的点云地图。
实时的拼接查看点云地图便于控制移动机器人的运动轨迹和方向，构建一个充分完整的点云地图。同时，由于误差的累积，激光图优化SLAM前端匹配之后的位姿与真实的位姿存在误差，因此需要回环检测和后端优化来消除误差，所以，可以控制Turtlebot在仿真环境中来回移动后回到出发位置形成闭环约束。体现在图4算法中为：若机器人回到原来的位置，则会回环检测成功，经过图4所示的回环检测和后端优化流程，最终输出得到用于构建地图的当前关键帧下的优化后的移动机器人位姿。其中，回环检测示意图如图5所示，以半径r为搜索半径范围，并判断搜索到的节点链中的位姿节点数量是否满足阈值要求，若满足则回环检测成功，继续进行后端优化，修正位姿。
设c＝(c1,c2,...,cn)是需要优化的节点的位姿，因移动机器人在二维平面内运动，所以有
Figure BDA0003236018970000077
其中，
Figure BDA0003236018970000078
表示第i节点处机器人在地图中的位置，θi表示第i节点处机器人在地图中的方向角；T表示矩阵的转置。第i个节点和第j个节点之间建立的边约束所对应的相对变换关系为：
Figure BDA0003236018970000081
Ri为与θi相关的余弦公式矩阵：
Figure BDA0003236018970000082
这条边的误差函数可表示为：
Figure BDA0003236018970000083
其中，
Figure BDA0003236018970000084
为观测量，hij为期望值。所有边约束之和，即总误差函数可表示为：
Figure BDA0003236018970000085
式中，Ωij为表示边误差所占权重的信息矩阵，即ci和cj之间协方差矩阵的逆，信息权重越大，表示测量越准确。Fij表示第i节点和第j节点之间的误差。极小化总误差函数可消除累积误差，得到最可能的机器人位姿c*，从而构建最优的地图：
Figure BDA0003236018970000086
图优化问题转变为了非线性最小二乘问题，对于目标函数的求解，目前常用的方法是高斯牛顿、Levenberg-Marquart(LM)等算法。优化后根据步骤1中保存的所有的位姿索引的更新位姿对之前的点云坐标转换进行更新，从而更新点云地图。
步骤5：对更新后的三维点云地图进行滤波，去除更新后的三维点云地图的离群点。
滤波方法可以采用的方法有统计滤波器或半径滤波器等。本例中使用半径滤波器，通过设定滤波半径d，计算每个点在其半径范围内的其他点的个数。半径范围内少于某一设定阈值a的点将被滤除。如图6所示，半径d内少于2个的点将被删除。针对本例，经过试验设定d＝18cm，a＝3时效果最佳。
步骤6：将滤波后的三维点云地图转化为八叉树地图。
利用八叉树地图构建库，将滤波后的三维点云地图转换为八叉树地图。优选的，如果需要将三维点的RGB颜色信息在八叉树地图中显示，给八叉树地图添加颜色信息，可进行进一步的处理。
安装编译Octomap库，将保存的pcd点云文件转换为octomap文件。此时octomap里只存储了点的空间信息，没有颜色信息，如图7所示，在rviz软件中进行显示。由于pcd点云文件中存储有点的RGB颜色信息，因此，如果需要给Octomap添加颜色，则可以利用Octomap库中提供的ColorOcTree类中的integrateNodeColor函数，将转换后的八叉树地图赋予彩色信息。然后再将带有颜色信息的八叉树地图保存为.ot文件。显示结果如图8所示。此时，转换完成的八叉树地图可用于导航等任务中。
本发明提供的方法主要包括五个部分：基于激光图优化SLAM算法的关键帧提取与相机点云数据采集；点云在相机坐标系与世界坐标系的位姿转换与拼接实时获取三维点云地图；激光图优化SLAM回环检测与后端优化，更新移动机器人位姿；点云地图的离群点滤波处理；点云地图转换为八叉树地图。第一部分，关键帧提取通过设置时间、距离、角度阈值来选取，关键帧选取成功后采集该时刻相机的点云数据。第二部分，由于相机与移动机器人安装位置固定，且机器人在世界坐标系下的位姿通过激光图优化SLAM算法可得，因此可将点云在相机坐标系的坐标转换为在世界坐标系的坐标。通过控制移动机器人在环境中运动，上述算法可持续运行，从而完成多个关键帧的点云拼接，实时构建点云地图。第三部分，通过后续的匹配和后端优化等算法得到优化后的关键帧对应的机器人位姿以及更新后的点云地图并保存。第四部分，鉴于相机点云数据离群点数据相对不准确，影响地图精度和美观性，因此，采用了滤波算法对离群点进行去除。第五部分，相对于八叉树地图，点云地图提供了大量不需要的细节，因此占据大量的存储空间，显示时对造成地图的明显重叠，难以用于导航等。因此需要将点云地图转化为八叉树地图。
本发明基于激光图优化SLAM筛选关键帧并计算和优化位姿，方法简单，地图构建快速，在现实中机器人平台中验证了该方法的可靠性，仅依靠激光图优化SLAM和相机采集到点云信息拼接得到的三维点云地图是稀疏的，机器人不能根据点云地图知道哪里可以到达，哪里不能到达。同时点云地图中还包含很多不必要的信息，比如窗帘的褶皱等，点云地图的保存占有了大量的内存，将点云保存成八叉树地图能够节省内存，并且八叉树地图可以描述机器人在地图中的可达性。
本发明还提供了一种八叉树地图构建系统，包括：
关键帧以及关键帧下的移动机器人位姿确定模块，用于根据激光雷达和里程计数据，采用激光图优化SLAM算法确定关键帧以及关键帧下的移动机器人位姿；
坐标转换模块，用于采集环境点云信息，并将点云数据在相机坐标系下的坐转换到世界坐标系下；
拼接模块，用于将坐标转换后的点云信息进行拼接，得到三维点云地图；
回环检测和后端优化模块，用于基于三维点云地图，对激光图优化SLAM算法进行回环检测和后端优化，得到更新后的移动机器人位姿和更新后的三维点云地图；
滤波模块，用于对更新后的三维点云地图进行滤波，去除更新后的三维点云地图的离群点；
转化模块，用于将滤波后的三维点云地图转化为八叉树地图。
其中，所述坐标转换模块具体包括：
位姿转换关系获取单元，用于获取相机坐标系与移动机器人坐标系的位姿转换关系；
坐标转换单元，用于根据所述移动机器人位姿和所述位姿转换关系，将点云数据在相机坐标系下的坐转换到世界坐标系下。
本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的系统而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。
本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。

Claims (7)

1.一种八叉树地图构建方法，其特征在于，包括：
根据激光雷达和里程计数据，采用激光图优化SLAM算法确定关键帧以及关键帧下的移动机器人位姿；
采集环境点云信息，并将点云数据在相机坐标系下的坐转换到世界坐标系下；
将坐标转换后的点云信息进行拼接，得到三维点云地图；
基于三维点云地图，对激光图优化SLAM算法进行回环检测和后端优化，得到更新后的移动机器人位姿和更新后的三维点云地图；
对更新后的三维点云地图进行滤波，去除更新后的三维点云地图的离群点；
将滤波后的三维点云地图转化为八叉树地图。
2.根据权利要求1所述的八叉树地图构建方法，其特征在于，采用激光图优化SLAM算法确定关键帧，具体包括：
若当前帧为第一帧，则不进行扫描匹配；
若当前帧不是第一帧，则判断当前帧与上一关键帧的采集时间间隔、移动距离或航向角的差值是否超过设定的阈值；
若是，则判断当前帧为关键帧；
若否，则舍弃当前帧。
3.根据权利要求1所述的八叉树地图构建方法，其特征在于，所述将点云数据在相机坐标系下的坐转换到世界坐标系下，具体包括：
获取相机坐标系与移动机器人坐标系的位姿转换关系；
根据所述移动机器人位姿和所述位姿转换关系，将点云数据在相机坐标系下的坐转换到世界坐标系下。
4.根据权利要求3所述的八叉树地图构建方法，其特征在于，所述点云坐标转换公式为：
Figure FDA0003236018960000021
其中，[xw,yw,zw]表示转换后点云坐标，
Figure FDA0003236018960000022
表示移动机器人位姿，
Figure FDA0003236018960000023
表示相机坐标系与移动机器人坐标系的位姿转换关系，[xc,yc,zc]表示点云数据在相机坐标系下的坐标，w表示世界坐标系，r表示机器人坐标系，c表示相机坐标系。
5.根据权利要求1所述的八叉树地图构建方法，其特征在于，所述后端优化的过程如下：
设c＝(c1,c2,...,cn)是需要优化的节点的位姿，因移动机器人在二维平面内运动，所以有：
Figure FDA0003236018960000024
其中，
Figure FDA0003236018960000025
表示第i节点处机器人在地图中的位置，θi表示第i节点处机器人在地图中的方向角；T表示矩阵的转置；
第i个节点和第j个节点之间建立的边约束所对应的相对变换关系为：
Figure FDA0003236018960000026
Ri为与θi相关的余弦公式矩阵：
Figure FDA0003236018960000027
当前边的误差函数eij表示为：
Figure FDA0003236018960000028
其中，
Figure FDA0003236018960000029
为观测量，hij为期望值；
总误差函数F(c)表示为：
Figure FDA00032360189600000210
式中，Ωij为表示边误差所占权重的信息矩阵；Fij表示第i节点和第j节点之间的误差；
极小化总误差函数消除累积误差，得到更新后的机器人位姿c*，从而构建最优的地图：
Figure FDA0003236018960000031
6.一种八叉树地图构建系统，其特征在于，包括：
关键帧以及关键帧下的移动机器人位姿确定模块，用于根据激光雷达和里程计数据，采用激光图优化SLAM算法确定关键帧以及关键帧下的移动机器人位姿；
坐标转换模块，用于采集环境点云信息，并将点云数据在相机坐标系下的坐转换到世界坐标系下；
拼接模块，用于将坐标转换后的点云信息进行拼接，得到三维点云地图；
回环检测和后端优化模块，用于基于三维点云地图，对激光图优化SLAM算法进行回环检测和后端优化，得到更新后的移动机器人位姿和更新后的三维点云地图；
滤波模块，用于对更新后的三维点云地图进行滤波，去除更新后的三维点云地图的离群点；
转化模块，用于将滤波后的三维点云地图转化为八叉树地图。
7.根据权利要求6所述的八叉树地图构建系统，其特征在于，所述坐标转换模块具体包括：
位姿转换关系获取单元，用于获取相机坐标系与移动机器人坐标系的位姿转换关系；
坐标转换单元，用于根据所述移动机器人位姿和所述位姿转换关系，将点云数据在相机坐标系下的坐转换到世界坐标系下。
CN202111002549.0A 2021-08-30 2021-08-30 一种八叉树地图构建方法及系统 Pending CN113720324A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111002549.0A CN113720324A (zh) 2021-08-30 2021-08-30 一种八叉树地图构建方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111002549.0A CN113720324A (zh) 2021-08-30 2021-08-30 一种八叉树地图构建方法及系统

Publications (1)

Publication Number Publication Date
CN113720324A true CN113720324A (zh) 2021-11-30

Family

ID=78678889

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111002549.0A Pending CN113720324A (zh) 2021-08-30 2021-08-30 一种八叉树地图构建方法及系统

Country Status (1)

Country Link
CN (1) CN113720324A (zh)

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114459467A (zh) * 2021-12-30 2022-05-10 北京理工大学 一种未知救援环境中基于vi-slam的目标定位方法
CN114593738A (zh) * 2022-05-09 2022-06-07 山东大学 基于八叉树搜索反光柱的机器人全局定位方法及系统
CN114708392A (zh) * 2022-03-22 2022-07-05 重庆大学 一种基于闭环轨迹的八叉树地图构建方法
CN115601519A (zh) * 2022-11-07 2023-01-13 南京理工大学（Cn） 面向低通信带宽下远程操控的操控端建图方法
CN116030213A (zh) * 2023-03-30 2023-04-28 千巡科技(深圳)有限公司 一种多机云边协同地图创建与动态数字孪生方法及系统
CN117761717A (zh) * 2024-02-21 2024-03-26 天津大学四川创新研究院 一种自动回环三维重建系统及运行方法

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104850615A (zh) * 2015-05-14 2015-08-19 西安电子科技大学 一种基于g2o的SLAM后端优化算法方法
CN110264563A (zh) * 2019-05-23 2019-09-20 武汉科技大学 一种基于orbslam2的八叉树建图方法
CN112785702A (zh) * 2020-12-31 2021-05-11 华南理工大学 一种基于2d激光雷达和双目相机紧耦合的slam方法
CN112862894A (zh) * 2021-04-12 2021-05-28 中国科学技术大学 一种机器人三维点云地图构建与扩充方法

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104850615A (zh) * 2015-05-14 2015-08-19 西安电子科技大学 一种基于g2o的SLAM后端优化算法方法
CN110264563A (zh) * 2019-05-23 2019-09-20 武汉科技大学 一种基于orbslam2的八叉树建图方法
CN112785702A (zh) * 2020-12-31 2021-05-11 华南理工大学 一种基于2d激光雷达和双目相机紧耦合的slam方法
CN112862894A (zh) * 2021-04-12 2021-05-28 中国科学技术大学 一种机器人三维点云地图构建与扩充方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
洪运志;吴怀宇;陈志环;: "室内环境下深度相机V-SLAM的稠密建图", 计算机工程与设计, no. 09 *
陈慧岩等: "《无人驾驶车辆理论与设计 慕课版》", 北京理工大学出版社, pages: 119 *

Cited By (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114459467A (zh) * 2021-12-30 2022-05-10 北京理工大学 一种未知救援环境中基于vi-slam的目标定位方法
CN114459467B (zh) * 2021-12-30 2024-05-03 北京理工大学 一种未知救援环境中基于vi-slam的目标定位方法
CN114708392A (zh) * 2022-03-22 2022-07-05 重庆大学 一种基于闭环轨迹的八叉树地图构建方法
CN114708392B (zh) * 2022-03-22 2024-05-14 重庆大学 一种基于闭环轨迹的八叉树地图构建方法
CN114593738A (zh) * 2022-05-09 2022-06-07 山东大学 基于八叉树搜索反光柱的机器人全局定位方法及系统
CN114593738B (zh) * 2022-05-09 2022-07-26 山东大学 基于八叉树搜索反光柱的机器人全局定位方法及系统
CN115601519A (zh) * 2022-11-07 2023-01-13 南京理工大学（Cn） 面向低通信带宽下远程操控的操控端建图方法
CN115601519B (zh) * 2022-11-07 2024-04-05 南京理工大学 面向低通信带宽下远程操控的操控端建图方法
CN116030213A (zh) * 2023-03-30 2023-04-28 千巡科技(深圳)有限公司 一种多机云边协同地图创建与动态数字孪生方法及系统
CN117761717A (zh) * 2024-02-21 2024-03-26 天津大学四川创新研究院 一种自动回环三维重建系统及运行方法
CN117761717B (zh) * 2024-02-21 2024-05-07 天津大学四川创新研究院 一种自动回环三维重建系统及运行方法

Similar Documents

Publication Publication Date Title
CN113720324A (zh) 一种八叉树地图构建方法及系统
CN111536964B (zh) 机器人定位方法及装置、存储介质
CN111429574B (zh) 基于三维点云和视觉融合的移动机器人定位方法和系统
CN112634451B (zh) 一种融合多传感器的室外大场景三维建图方法
CN107160395B (zh) 地图构建方法及机器人控制系统
CN111402339B (zh) 一种实时定位方法、装置、系统及存储介质
Borrmann et al. Globally consistent 3D mapping with scan matching
CN108537876A (zh) 基于深度相机的三维重建方法、装置、设备及存储介质
CN110298914B (zh) 一种建立果园中果树冠层特征地图的方法
CN109163722B (zh) 一种仿人机器人路径规划方法及装置
KR20220025028A (ko) 시각적 비콘 기반의 비콘 맵 구축 방법, 장치
CN113593017A (zh) 露天矿地表三维模型构建方法、装置、设备及存储介质
Pan et al. Gem: online globally consistent dense elevation mapping for unstructured terrain
CN110992424B (zh) 基于双目视觉的定位方法和系统
CN111709988A (zh) 一种物体的特征信息的确定方法、装置、电子设备及存储介质
CN108367436A (zh) 针对三维空间中的物体位置和范围的主动相机移动确定
CN116380039A (zh) 一种基于固态激光雷达和点云地图的移动机器人导航系统
CN114091515A (zh) 障碍物检测方法、装置、电子设备和存储介质
CN114429432B (zh) 一种多源信息分层融合方法、装置及存储介质
CN114494150A (zh) 一种基于半直接法的单目视觉里程计的设计方法
CN117765524A (zh) 一种基于多视图的三维目标检测方法
CN111815684B (zh) 基于统一残差模型的空间多元特征配准优化方法及装置
CN110864670B (zh) 目标障碍物位置的获取方法和系统
CN112348854A (zh) 一种基于深度学习视觉惯性里程检测方法
CN116520302A (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