CN113538410A - 一种基于3d激光雷达和uwb的室内slam建图方法 - Google Patents

一种基于3d激光雷达和uwb的室内slam建图方法 Download PDF

Info

Publication number
CN113538410A
CN113538410A CN202110900474.1A CN202110900474A CN113538410A CN 113538410 A CN113538410 A CN 113538410A CN 202110900474 A CN202110900474 A CN 202110900474A CN 113538410 A CN113538410 A CN 113538410A
Authority
CN
China
Prior art keywords
uwb
slam
laser
key frame
point
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
Application number
CN202110900474.1A
Other languages
English (en)
Other versions
CN113538410B (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.)
Guangdong University of Technology
Original Assignee
Guangdong University of 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 Guangdong University of Technology filed Critical Guangdong University of Technology
Priority to CN202110900474.1A priority Critical patent/CN113538410B/zh
Publication of CN113538410A publication Critical patent/CN113538410A/zh
Application granted granted Critical
Publication of CN113538410B publication Critical patent/CN113538410B/zh
Priority to US17/826,794 priority patent/US11543530B1/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

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
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/0002—Inspection of images, e.g. flaw detection
    • 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
    • G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
    • G01S13/02—Systems using reflection of radio waves, e.g. primary radar systems; Analogous systems
    • G01S13/0209—Systems with very large relative bandwidth, i.e. larger than 10 %, e.g. baseband, pulse, carrier-free, ultrawideband
    • 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
    • G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
    • G01S13/86—Combinations of radar systems with non-radar systems, e.g. sonar, direction finder
    • G01S13/865—Combination of radar systems with lidar systems
    • 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
    • G01S17/42—Simultaneous measurement of distance and other co-ordinates
    • 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/89—Lidar systems specially adapted for specific applications for mapping or imaging
    • G01S17/894—3D imaging with simultaneous measurement of time-of-flight at a 2D array of receiver pixels, e.g. time-of-flight cameras or flash lidar
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T17/00—Three dimensional [3D] modelling, e.g. data description of 3D objects
    • G06T17/05—Geographic models
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/50—Depth or shape recovery
    • G06T7/521—Depth or shape recovery from laser ranging, e.g. using interferometry; from the projection of structured light
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10032—Satellite or aerial image; Remote sensing
    • G06T2207/10044—Radar image

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Electromagnetism (AREA)
  • Theoretical Computer Science (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Geometry (AREA)
  • Software Systems (AREA)
  • Optics & Photonics (AREA)
  • Quality & Reliability (AREA)
  • Computer Graphics (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Length Measuring Devices By Optical Means (AREA)

Abstract

本发明公开了一种基于3D激光雷达和UWB的室内SLAM建图方法，在室内场景中部署UWB定位系统，并通过携带3D激光雷达传感器的机器人探索该室内场景区域，利用融合激光雷达数据和UWB数据的SLAM算法生成所探索区域的地图。具体包括以下步骤：S1、确定3D激光SLAM坐标系和UWB定位坐标系之间的相对位姿变换；S2、利用UWB数据为激光里程计的帧间匹配提供初始值；S3、利用UWB数据为SLAM后端位姿图添加约束；S4、进行基于曲率特征编码的回环检测。本发明突破室内场景的光照条件限制，并消除SLAM的累计误差，从而构建高质量的室内地图。

Description

一种基于3D激光雷达和UWB的室内SLAM建图方法
技术领域
本发明涉及状态估计与地图构建的技术领域，尤其涉及到一种基于3D激光雷达和UWB的室内SLAM建图方法。
背景技术
室内建图技术在现代社会中得到了广泛应用，特别是在物流、救援、军事、安保等领域。室内地图的构建方法有很多种，可以从静态和动态的角度进行划分。
静态方法主要是在一个固定的室内环境中，静态部署激光雷达或视觉相机等具有测距功能的传感器在当前室内环境的各个子区域，利用每个子区域的独立传感器数据，可以构建该子区域的相应地图；最后再将各个子区域的子地图按照各个独立传感器的空间位置关系，进行拼接，得到完整的室内地图。
静态的室内地图构建方法具有简单和精确等优点；然而静态方法需要多个传感器部署在不同区域，成本较高，且需要将多个子地图拼接，也比较耗费时间和人力，不适合应用在探索救援、军事行动等紧急的场景。
动态的室内地图构建主要是基于即时定位与地图构建技术(simultaneouslocalization and mapping，SLAM)。当携带传感器的人或机器人在自由探索室内场景时，利用实时的传感器数据和SLAM算法，即可快速生成已探索区域的地图，同时还确定了传感器在该地图中的当前位置和历史轨迹。使用SLAM技术构建室内地图具有快速、灵活和低成本等优点。
在SLAM技术中，地图构建和实时定位两者是相辅相成的：只有实现精确的定位，才能将当前时刻的传感器数据(比如包含了空间测距信息的3D激光点云数据)，从当前传感器坐标系下，投影到地图坐标系中，进行融合，从而更新生成的地图信息；另一方面，需要精确的地图数据与当前实时的传感器数据进行数据关联(data association)，才可以实现精确的传感器定位。因此，控制SLAM过程的定位误差非常重要，否则会导致累计误差的增长，最终降低室内地图构建的精度。
此外，利用SLAM技术构建地图也需要考虑选择合适的传感器设备。室内环境可能存在光照条件的影响，使用视觉相机作为传感器可能会受到严重限制；另一方面，室内环境由于遮挡的原因，也无法通过融合GPS数据来实现消除SLAM累计误差。
发明内容
本发明的目的在于克服现有技术的不足，提供一种基于3D激光雷达和UWB的室内SLAM建图方法，突破室内场景的光照条件限制，并消除SLAM的累计误差，从而构建高质量的室内地图。
为实现上述目的，本发明所提供的技术方案为：
一种基于3D激光雷达和UWB的室内SLAM建图方法，在室内场景中部署UWB定位系统，并通过携带3D激光雷达传感器的机器人探索该室内场景区域，利用融合激光雷达数据和UWB数据的SLAM算法生成所探索区域的地图；
具体包括以下步骤：
S1、确定3D激光SLAM坐标系和UWB定位坐标系之间的相对位姿变换；
S2、利用UWB数据为激光里程计的帧间匹配提供初始值；
S3、利用UWB数据为SLAM后端位姿图添加约束；
S4、进行基于曲率特征编码的回环检测。
进一步地，所述步骤S1确定3D激光SLAM坐标系和UWB定位坐标系之间的相对位姿变换的具体过程如下：
S1-1、将UWB标签节点放置于3D激光雷达上，使得两种传感器看作重合，并携带这两种传感器在室内环境中探索；
S1-2、在SLAM算法中融入UWB数据之前，分别利用UWB定位算法和仅使用激光数据的SLAM，在由各个UWB基站节点构成的包络面区域内，获取传感器分别在UWB坐标系(U)下和3D激光SLAM坐标系下(L)的三维运动轨迹坐标点集PU和PL：
Figure BDA0003199598800000031
Figure BDA0003199598800000032
式(1)和(2)中，
Figure BDA0003199598800000033
和
Figure BDA0003199598800000034
为对应的三维运动轨迹坐标点；
S1-3、根据相同的时间戳，从步骤S1-2得到的两个三维运动轨迹坐标点集中均匀采样p个点，构成以下两个点集，UWB定位采样点集XU和3D激光SLAM定位采样点集XL：
Figure BDA0003199598800000035
Figure BDA0003199598800000036
式(3)和(4)中，矩阵中每一列为对应的三维运动轨迹坐标点；
S1-4、对步骤S1-3采样得到的两个点集XU和XL进行稀疏化处理；
S1-5、利用步骤S1-4得到的两个点集XU和XL，根据Umeyama定理，求出UWB坐标系(U)和3D激光SLAM坐标系(L)之间的相对位姿变换。
进一步地，三维运动轨迹坐标点集PU和PL根据时间戳采样的具体过程如下：
采用线性插值的方法，针对每一个提取的SLAM定位坐标点pointL(tk)∈PL的时间戳tk，从PL采样计算得到对应的UWB定位坐标点pointU(tk)：
Figure BDA0003199598800000037
式(5)中，tmin为最小时间差阈值，当满足|td-tk|≤tmin时，认为时间td和tk足够接近；t1，t2则为不满足tmin阈值要求时，采用线性插值计算所需的两个时间戳。
进一步地，所述步骤S1-5的具体过程如下：
S1-5-1、求出式(3)和(4)两个点集的均值xU，mean和xL，mean，并对该两个矩阵进行去心化操作，即每一列减去对应的均值，得到对应的矩阵
Figure BDA0003199598800000041
和
Figure BDA0003199598800000042
S1-5-2、对矩阵
Figure BDA0003199598800000043
进行奇异值分解，即：
Figure BDA0003199598800000044
S1-5-3、求矩阵
Figure BDA0003199598800000045
的特征值，根据特征值的范围，选取相应的S矩阵：
Figure BDA0003199598800000046
式(8)中，I为单位矩阵；
S1-5-4、出两个坐标系之间的位姿变换，包括旋转矩阵R和位移向量tl：
R＝USVT (9)
tl＝xU，mean-RxL，mean (10)
将位于UWB坐标系下的点投影到3D激光SLAM坐标系下，有：
Figure BDA0003199598800000047
通过式(11)即可获取包括3D激光SLAM坐标系下的UWB定位轨迹坐标点，UWB基站坐标点在内的数据。
进一步地，所述步骤S2利用UWB定位给出的两个相邻激光点云帧对应的UWB定位位置xT1和xT2，求出该两个位置之间的空间距离，即可得出两帧之间位姿变换的位移分量的初始值：tr＝xT2-xT1。
进一步地，所述步骤S3利用UWB数据为SLAM后端位姿图添加约束的具体过程如下：
利用获取得到的旋转矩阵R和位移向量tl，将位于UWB坐标系下的UWB基站节点投影到3D激光SLAM坐标系中，获取各个UWB基站节点在3D激光SLAM坐标系下的坐标；
在探索室内区域的过程中，UWB标签节点会与UWB基站节点实时通信；
若当前传感器的位置处于由各个UWB基站节点构成的包络面内，则将UWB系统解算出的定位位置坐标xuwb作为定位观测约束加入到SLAM后端位姿图中：
e1＝||xuwb-xt|| (13)
式(13)中xt为t时刻的SLAM关键帧位姿Pt的位置分量；
若当前传感器的位置处于由各个UWB基站节点构成的包络面区域外，则按以下方法处理：
获取得到测距信息；随后，根据UWB信号强度阈值、移动标签与基站之间的距离阈值要求，对原始的测距信息过滤:
dt＝(d1，d2，...dj)T (14)
式(14)中，dt表示t时刻，移动标签节点和j个有效基站节点的测量距离；
将t时刻测得的最近的基站坐标xmean作为路标加入到SLAM的后端位姿图，并根据测量距离dmean,为t时刻的SLAM关键帧位姿Pt构建观测约束，用于后端图优化：
Figure BDA0003199598800000051
其中，
Figure BDA0003199598800000052
e1和e2均为UWB观测约束。
进一步地，所述步骤S4进行基于曲率特征编码的回环检测，包括以下步骤：
对关键帧点云进行遍历，计算该帧点云中，每个点的曲率,记为C：
Figure BDA0003199598800000061
式(20)中，K表示与当前激光点pointi位于同一行的左侧和右侧的激光点集合，ri代表点pointi到激光雷达中心O的距离,dj代表K中除去pointi的其他点到激光雷达中心O的距离，λ代表比例系数，根据实际K集合的大小而调整，使求出的曲率大小处在一个固定区间范围内；
对曲率根据阈值进行过滤和取整操作：
Figure BDA0003199598800000062
式(21)中，Cthreshold为设定的阈值；
对关键帧点云所在的三维空间，根据水平面的分辨率进行划分，得到N个子区域：
Figure BDA0003199598800000063
式(22)中，L1和L2分别表示水平面上，距离激光雷达中心O的最大距离和最小距离；α为角度间隔，l为径向单位长度；
根据划分的空间区域生成一个规格为
Figure BDA0003199598800000064
的编码矩阵M，编码矩阵M的每个元素值为对应子区域中的最大的曲率值C，没有点云的子区域，用0值填充到矩阵的相应位置；
在SLAM算法运行的过程中，将生成的每个关键帧的对应的编码矩阵M存入数据库中；
当SLAM算法根据当前关键帧检测是否出现回环情况时，在历史关键帧的编码矩阵数据库中查找一个与当前关键帧的编码矩阵最相似的编码矩阵；
过程包括：
为编码矩阵M生成一个行数相同的编码列向量v，该向量中的每一个元素为矩阵对应行的元素最大值Cmax；
在编码矩阵数据库中查找候选矩阵时，先比较列向量v的相似度：
diff＝||vcurrent-vsefect|| (23)
式(23)中，vcurrent表示当前时刻关键帧的编码矩阵对应的编码列向量，vselect代表候选关键帧的编码矩阵对应的编码列向量；当diff小于设定阈值时，即可初步挑选出候选关键帧对应的编码矩阵Mselect；
接着确定Mselect和当前时刻的关键帧的编码矩阵Mcurrent之间的列平移数shift，求取shift的方式为从Mcurrent中随机抽取一个列向量，所在的列记为jcurrent，然后遍历Mselect中的每一个列向量，找到一个最相似的列向量，记所在的列为jselect，则有：
shift＝jselect-jcurrent (25)
随后将Mselect根据shift进行平移，得到
Figure BDA0003199598800000071
比较
Figure BDA0003199598800000072
和Mcurrent两个矩阵的相似度s：
Figure BDA0003199598800000073
式(26)中，
Figure BDA0003199598800000074
和
Figure BDA0003199598800000075
为对应编码矩阵的其中一个列向量，当s满足小于设定阈值条件时，认为两个编码矩阵对应的关键帧点云相似，即当前关键帧的位置与候选历史关键帧的位置接近，从而实现位置识别功能，符合添加回环约束的条件；
将当前关键帧点云和查找到的历史关键帧点云进行点云匹配，求出两个关键帧雷达坐标系之间的相对位姿变换，并将其添加到SLAM后端位姿图中作为约束。
与现有技术相比，本方案原理及优点如下：
1)在传统3D激光SLAM的基础上，在室内场景中融合了相对可靠的UWB传感器数据，为激光SLAM的激光里程计提供匹配初始值，有利于快速收敛；并为SLAM的后端位姿图添加了可靠的观测约束，弥补了室内场景缺乏GPS观测约束的不足，有利于减小室内3D激光SLAM的累计误差，特别是在不能出现回环检测的场景时(无法提供有效的回环检测约束，激光里程计带来的累计误差会越来越大)，UWB观测约束能够提高算法的定位精度和鲁棒性，从而进一步提高激光SLAM构建的室内地图的准确性。
2)激光SLAM算法在使用前，不需要通过测量的手段获取部署的每个UWB基站节点相对于初始时刻的激光雷达的空间位置(即不需要在算法前通过手动测量手段获取UWB坐标系和初始激光SLAM坐标系之间的位姿变换)，可以在算法运行后根据获取的激光SLAM和UWB定位的轨迹坐标点求取两个坐标系之间的相对位姿变换，从而将UWB基站坐标等UWB数据投影到激光SLAM坐标系中使用。这样能够充分适应不同的室内使用场景，避免了手动测量，节省了人力和时间，特别是对紧急的场景具有重要意义。
3)基于激光点云的曲率特征的回环检测方式，避免了传统激光SLAM仅基于关键帧位置的直接回环检测带来的弊端；充分利用了当前关键帧点云和历史关键帧点云的空间信息，通过place recognition的方式实现更加精确的回环检测，能够为SLAM后端位姿图添加更加可靠的回环约束，从而缩小激光SLAM的累计误差，进一步提高构建的室内地图的准确度；此外，本方案提出的基于点云曲率和空间信息的特征编码矩阵，有利于数据的存储，以及提高候选关键帧的查找效率。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的服务作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1为本发明一种基于3D激光雷达和UWB的室内SLAM建图方法的原理流程图；
图2为传统激光雷达SLAM算法框架图；
图3为本发明提出的SLAM算法框架图；
图4为本发明提出的轨迹点采样示意图；
图5为本发明提出的SLAM算法的后端位姿图；
图6为本发明提出的点云特征编码示意图。
具体实施方式
下面结合具体实施例对本发明作进一步说明：
如图1所示，本实施例所述的一种基于3D激光雷达和UWB的室内SLAM建图方法，在室内场景中部署UWB定位系统，并通过携带3D激光雷达传感器的机器人探索该室内场景区域，利用融合激光雷达数据和UWB数据的SLAM算法生成所探索区域的地图；
具体包括以下步骤：
S1、确定3D激光SLAM坐标系和UWB定位坐标系之间的相对位姿变换；
本实施例在室内场景中部署UWB定位系统，并通过携带3D激光雷达传感器的人或机器人探索该室内区域，利用融合激光雷达数据和UWB数据的SLAM算法生成所探索区域的地图。
在所使用的UWB定位系统中，包含多个传感器节点，这些节点主要分为两个角色，即固定的基站节点和和可移动的标签节点。利用基站节点的坐标初始化后，根据标签和各个基站之间的测距信息即可解算得到标签节点在UWB全局坐标系下的坐标。
为了将UWB数据融入到3D激光SLAM算法中，需要在系统运行前，确定激光雷达SLAM坐标系和UWB坐标系之间的相对位姿变换，随后才可以将UWB解算的数据从UWB坐标系下，投影到激光SLAM坐标系，再进行融合。
一般情况下，激光SLAM坐标系可以设定为初始时刻下的3D激光雷达坐标系。因此，可以在初始时刻下，测量3D激光雷达与UWB各个基站节点之间的距离信息，从而解算得到UWB坐标系和激光SLAM坐标系之间的位姿变换。
然而在实际应用中，更加一般的情况是人们无法确切知道各个基站节点的放置的确切位置，不能手动指定它们的坐标，同时也缺乏测量手段测量得到3D激光雷达与各个基站节点之间的空间相对几何信息，即只能通过各个基站节点之间的测距信息初始化得到UWB坐标系，而无法获取初始时刻3D激光雷达坐标系与UWB坐标系之间的相对位姿变换。
针对事先不知道UWB坐标系和3D激光SLAM坐标系之间相对位姿变换的场景，本实施例提供了以下解决方案：
S1-1、将UWB标签节点放置于3D激光雷达上，使得两种传感器看作重合，并携带这两种传感器在室内环境中探索；
S1-2、在SLAM算法中融入UWB数据之前，分别利用UWB定位算法(如TOA算法)和仅使用激光数据的SLAM，在由各个UWB基站节点构成的包络面区域内(如图4所示，即A0～A3代表的UWB基站节点包围的虚线区域内)，获取传感器分别在UWB坐标系(U)下和3D激光SLAM坐标系下(L)的三维运动轨迹坐标点集PU和PL：
Figure BDA0003199598800000101
Figure BDA0003199598800000102
式(1)和(2)中，
Figure BDA0003199598800000103
和
Figure BDA0003199598800000104
为对应的三维运动轨迹坐标点。
图4中，OU代表UWB定位坐标系的原点，OL代表SLAM坐标系的原点。由于本实施例的3D激光雷达和UWB标签节点放置在一起，故可看成重合，它们的运动轨迹是一致的。
S1-3、两个坐标点集PU和PL在获取足够多的点后，根据相同的时间戳，从步骤S1-2得到的两个三维运动轨迹坐标点集中均匀采样p个点，构成以下两个点集，UWB定位采样点集XU和3D激光SLAM定位采样点集XL：
Figure BDA0003199598800000111
Figure BDA0003199598800000112
式(3)和(4)中，矩阵中每一列为对应的三维运动轨迹坐标点；
上述两个点集根据时间戳采样的具体方法如下：
由于3D激光雷达的工作频率(一般为10Hz)相比于UWB定位更低，且由于SLAM采用基于关键帧的位姿图优化，输出轨迹点频率会更低(如1Hz)，因此本实施例采用线性插值的方法，针对每一个提取的SLAM定位坐标点pointL(tk)∈PL的时间戳tk，从PL采样计算得到对应的UWB定位坐标点pointU(tk)：
Figure BDA0003199598800000113
式(5)中，tmin为最小时间差阈值，当满足|td-tk|≤tmin时，认为时间td和tk足够接近；t1，t2则为不满足tmin阈值要求时，采用线性插值计算所需的两个时间戳。
S1-4、上述工作完成了两个点集根据时间的对齐采样工作。进一步，为了提高后续求出的像个坐标系之间的相对位姿变换的精度，本实施例还考虑上述点集在所处的空间的均匀分布，需要进一步对步骤S1-3采样得到的两个点集XU和XL进行稀疏化处理；即时间相邻的两个采样点之间的距离应该满足大于设定的阈值：
|point(k)-point(k+1)|≥dthreshold (6)
式(6)中，dthreshold为设定的距离阈值。
S1-5、利用步骤S1-4得到的两个点集XU和XL，根据Umeyama定理，求出UWB坐标系(U)和3D激光SLAM坐标系(L)之间的相对位姿变换，具体过程如下：
S1-5-1、求出式(3)和(4)两个点集的均值xU，mean和xL，mean，并对该两个矩阵进行去心化操作，即每一列减去对应的均值，得到对应的矩阵
Figure BDA0003199598800000121
和
Figure BDA0003199598800000122
S1-5-2、对矩阵
Figure BDA0003199598800000123
进行奇异值分解，即：
Figure BDA0003199598800000124
S1-5-3、求矩阵
Figure BDA0003199598800000125
的特征值，根据特征值的范围，选取相应的S矩阵：
Figure BDA0003199598800000126
式(8)中，I为单位矩阵；
S1-5-4、出两个坐标系之间的位姿变换，包括旋转矩阵R和位移向量tl：
R＝USVT (9)
tl＝xU，mean-RxL，mean (10)
将位于UWB坐标系下的点投影到3D激光SLAM坐标系下，有：
Figure BDA0003199598800000127
通过式(11)即可获取包括3D激光SLAM坐标系下的UWB定位轨迹坐标点，UWB基站坐标点在内的数据。
S2、利用UWB数据为激光里程计的帧间匹配提供初始值；
在3D激光SLAM中，激光里程计的任务是通过匹配相邻两帧的激光点云，从而求出两帧之间传感器的位姿变换。
通常情况下，激光里程计采用的点云匹配算法主要分有ICP、NDT以及基于特征点的LOAM算法。无论是哪种算法，在帧间匹配时，都必须要在3D激光雷达工作周期内，得出收敛的匹配结果。
为了减小匹配时间，加快达到收敛，一种方法是提供相对准确的匹配初始值。
本实施例利用UWB定位给出的两个相邻激光点云帧对应的UWB定位位置xT1和xT2，求出该两个位置之间的空间距离，即可得出两帧之间位姿变换的位移分量的初始值：
tr＝xT2-xT1 (12)。
S3、利用UWB数据为SLAM后端位姿图添加约束，具体过程如下：
利用上述获取得到的旋转矩阵R和位移向量tl，将位于UWB坐标系下的UWB基站节点投影到3D激光SLAM坐标系中，获取各个UWB基站节点在3D激光SLAM坐标系下的坐标；
在探索室内区域的过程中，UWB标签节点会与UWB基站节点实时通信；
若当前传感器的位置处于由各个UWB基站节点构成的包络面内(图4所示)，则UWB系统给出的定位数据会比较准确，此时将UWB系统解算出的定位位置坐标xuwb作为定位观测约束加入到如图5所示的SLAM后端位姿图中：
e1＝||xuwb-xt|| (13)
式(13)中xt为t时刻的SLAM关键帧位姿Pt的位置分量；
若当前传感器的位置处于图4所示的包络面的区域外，则按以下方法处理：
获取得到测距信息；随后，根据UWB信号强度阈值、移动标签与基站之间的距离阈值要求，对原始的测距信息过滤:
dt＝(d1,d2,...dj)T (14)
式(14)中，dt表示t时刻，移动标签节点和j个有效基站节点的测量距离；
如图5所示，将t时刻测得的最近的基站坐标xmean作为路标(landmark)加入到SLAM的后端位姿图，并根据测量距离dmean,为t时刻的SLAM关键帧位姿Pt构建观测约束，用于后端图优化：
Figure BDA0003199598800000131
其中，
Figure BDA0003199598800000141
上述的e1或e2即为图5所示的位姿图中的虚线边，代表UWB观测约束。在SLAM算法的实时运行中，后端位姿图会以一定频率完成图优化，并更新优化后的关键帧位姿，从而提高SLAM的定位精度，进一步提高本发明的室内地图构建的精确度。
S4、进行基于曲率特征编码的回环检测；
传统激光SLAM算法(如LeGO-LOAM等算法)常用的回环检测方式是直接法：即根据当前时刻的关键帧位姿Pcurrent的位置分量xcurrent，在历史关键帧数据库中，找到一个距离xcurrent最近的目标关键帧位姿Ptarget，且它们的空间位置距离满足阈值要求：
||xcurrent-xtarget||≤dth (17)
随后，利用两个关键帧位姿对应的关键帧激光点云，通过点云匹配的方式(如利用ICP或NDT算法)求出两个关键帧之间的相对位姿变换Tmeasure，随后将Tmeasure添加到激光SLAM算法的后端位姿图中，构成回环约束：
eloop＝||Tmeasure-Td|| (18)
其中有：
Td＝Ptarget -1Pcurrent (19)
Td即直接用两个关键帧计算的相对位姿变换。
然而，传统激光SLAM采用的直接法存在以下缺陷：由于SLAM算法存在累计误差，仅仅依赖于当前关键帧的空间位置寻找目标关键帧，可能会出现以下情况：
1)候选关键帧位姿Pselect，本身不应该作为目标关键帧位姿Ptargrt，但由于存在误差的关系，导致计算得到||xcurrent-xselect||≤dth，从而被错误用于构建回环约束，可能会造成SLAM的误差进一步增大。
2)由于关键帧位姿存在误差，导致无法根据当前关键帧位置找到本应该属于距离阈值要求内的候选历史关键帧，即无法找到满足式(13)的目标关键帧(虽然实际是存在的)，从而无法及时添加回环约束到后端位姿图中用于优化关键帧位姿。
对此，本实施例提供一种基于曲率特征编码的回环检测方式，而不依赖于关键帧位姿，仅利用激光点云实现寻找合适的候选历史关键帧，从而比较两个关键帧点云的相似度，实现位置识别，为SLAM后端位姿图添加可靠的回环约束。
进行基于曲率特征编码的回环检测，包括以下步骤：
对关键帧点云进行遍历，计算该帧点云中，每个点的曲率,记为C：
Figure BDA0003199598800000151
式(20)中，K表示与当前激光点pointi位于同一行的左侧和右侧的激光点集合，ri代表点pointi到激光雷达中心O的距离,dj代表K中除去pointi的其他点到激光雷达中心O的距离，λ代表比例系数，根据实际K集合的大小而调整，使求出的曲率大小处在一个固定区间范围内，如[0,10]；
对曲率根据阈值进行过滤和取整操作：
Figure BDA0003199598800000152
式(21)中，Cthreshold为设定的阈值；
如图6所示，本实施例对关键帧点云所在的三维空间，根据水平面的分辨率进行划分，得到N个子区域：
Figure BDA0003199598800000153
式(22)中，L1和L2分别表示水平面上，距离激光雷达中心O的最大距离和最小距离；α为角度间隔，l为径向单位长度；
如图6所示，本实施例根据划分的空间区域生成一个规格为
Figure BDA0003199598800000154
的编码矩阵M，编码矩阵M的每个元素值为对应子区域中的最大的曲率值C，没有点云的子区域，用0值填充到矩阵的相应位置；
在SLAM算法运行的过程中，将生成的每个关键帧的对应的编码矩阵M存入数据库中；
当SLAM算法根据当前关键帧检测是否出现回环情况时，在历史关键帧的编码矩阵数据库中查找一个与当前关键帧的编码矩阵最相似的编码矩阵；
过程包括：
为编码矩阵M生成一个行数相同的编码列向量v，该向量中的每一个元素为矩阵对应行的元素最大值Cmax；
在编码矩阵数据库中查找候选矩阵时，先比较列向量v的相似度：
diff＝||vcurrent-vselect|| (23)
式(23)中，vcurrent表示当前时刻关键帧的编码矩阵对应的编码列向量，vselect代表候选关键帧的编码矩阵对应的编码列向量；当diff小于设定阈值时，即可初步挑选出候选关键帧对应的编码矩阵Mselect；
接着还需要确定Mselect和当前时刻的关键帧的编码矩阵Mcurrent之间的列平移数shift，这是因为当回到历史探索过的区域时，可能是不同的方向(例如采用相反的方向回到之前探索过的区域)，这样就造成了两个编码矩阵存在一定的列位移，即两个矩阵的元素存在以下关系：
Mselect(i，j)＝Mcurrent(i，j+shift) (24)
对此，本实施例求取shift的方式为从Mcurrent中随机抽取一个列向量，所在的列记为jcurrent，然后遍历Mselect中的每一个列向量，找到一个最相似的列向量，记所在的列为jselect，则有：
shift＝jselect-jcurrent (25)
随后将Mselect根据shift进行平移，得到
Figure BDA0003199598800000161
比较
Figure BDA0003199598800000162
和Mcurrent两个矩阵的相似度s：
Figure BDA0003199598800000171
式(26)中，
Figure BDA0003199598800000172
和
Figure BDA0003199598800000173
为对应编码矩阵的其中一个列向量，当s满足小于设定阈值条件时，认为两个编码矩阵对应的关键帧点云相似，即当前关键帧的位置与候选历史关键帧的位置接近，从而实现位置识别功能，符合添加回环约束的条件；
将当前关键帧点云和查找到的历史关键帧点云进行点云匹配，求出两个关键帧雷达坐标系之间的相对位姿变换，并将其添加到SLAM后端位姿图中作为约束。
以上所述之实施例子只为本发明之较佳实施例，并非以此限制本发明的实施范围，故凡依本发明之形状、原理所作的变化，均应涵盖在本发明的保护范围内。

Claims (7)

1.一种基于3D激光雷达和UWB的室内SLAM建图方法，其特征在于，在室内场景中部署UWB定位系统，并通过携带3D激光雷达传感器的机器人探索该室内场景区域，利用融合激光雷达数据和UWB数据的SLAM算法生成所探索区域的地图；
具体包括以下步骤：
S1、确定3D激光SLAM坐标系和UWB定位坐标系之间的相对位姿变换；
S2、利用UWB数据为激光里程计的帧间匹配提供初始值；
S3、利用UWB数据为SLAM后端位姿图添加约束；
S4、进行基于曲率特征编码的回环检测。
2.根据权利要求1所述的一种基于3D激光雷达和UWB的室内SLAM建图方法，其特征在于，所述步骤S1确定3D激光SLAM坐标系和UWB定位坐标系之间的相对位姿变换的具体过程如下：
S1-1、将UWB标签节点放置于3D激光雷达上，使得两种传感器看作重合，并携带这两种传感器在室内环境中探索；
S1-2、在SLAM算法中融入UWB数据之前，分别利用UWB定位算法和仅使用激光数据的SLAM，在由各个UWB基站节点构成的包络面区域内，获取传感器分别在UWB坐标系(U)下和3D激光SLAM坐标系下(L)的三维运动轨迹坐标点集PU和PL：
Figure FDA0003199598790000011
Figure FDA0003199598790000012
式(1)和(2)中，
Figure FDA0003199598790000013
和
Figure FDA0003199598790000014
为对应的三维运动轨迹坐标点；
S1-3、根据相同的时间戳，从步骤S1-2得到的两个三维运动轨迹坐标点集中均匀采样p个点，构成以下两个点集，UWB定位采样点集XU和3D激光SLAM定位采样点集XL：
Figure FDA0003199598790000021
Figure FDA0003199598790000022
式(3)和(4)中，矩阵中每一列为对应的三维运动轨迹坐标点；
S1-4、对步骤S1-3采样得到的两个点集XU和XL进行稀疏化处理；
S1-5、利用步骤S1-4得到的两个点集XU和XL，根据Umeyama定理，求出UWB坐标系(U)和3D激光SLAM坐标系(L)之间的相对位姿变换。
3.根据权利要求2所述的一种基于3D激光雷达和UWB的室内SLAM建图方法，其特征在于，三维运动轨迹坐标点集PU和PL根据时间戳采样的具体过程如下：
采用线性插值的方法，针对每一个提取的SLAM定位坐标点pointL(tk)∈PL的时间戳tk，从PL采样计算得到对应的UWB定位坐标点pointU(tk)：
Figure FDA0003199598790000023
式(5)中，tmin为最小时间差阈值，当满足|td-tk|≤tmin时，认为时间td和tk足够接近；t1，t2则为不满足tmin阈值要求时，采用线性插值计算所需的两个时间戳。
4.根据权利要求2所述的一种基于3D激光雷达和UWB的室内SLAM建图方法，其特征在于，所述步骤S1-5的具体过程如下：
S1-5-1、求出式(3)和(4)两个点集的均值xU，mean和xL，mean，并对该两个矩阵进行去心化操作，即每一列减去对应的均值，得到对应的矩阵
Figure FDA0003199598790000024
和
Figure FDA0003199598790000025
S1-5-2、对矩阵
Figure FDA0003199598790000031
进行奇异值分解，即：
Figure FDA0003199598790000032
S1-5-3、求矩阵
Figure FDA0003199598790000033
的特征值，根据特征值的范围，选取相应的S矩阵：
Figure FDA0003199598790000034
式(8)中，I为单位矩阵；
S1-5-4、出两个坐标系之间的位姿变换，包括旋转矩阵R和位移向量tl：
R＝USVT (9)
tl＝xU，mean-RxL，mean (10)
将位于UWB坐标系下的点投影到3D激光SLAM坐标系下，有：
Figure FDA0003199598790000035
通过式(11)即可获取包括3D激光SLAM坐标系下的UWB定位轨迹坐标点，UWB基站坐标点在内的数据。
5.根据权利要求1所述的一种基于3D激光雷达和UWB的室内SLAM建图方法，其特征在于，所述步骤S2利用UWB定位给出的两个相邻激光点云帧对应的UWB定位位置xT1和xT2，求出该两个位置之间的空间距离，即可得出两帧之间位姿变换的位移分量的初始值：tr＝xT2-xT1。
6.根据权利要求4所述的一种基于3D激光雷达和UWB的室内SLAM建图方法，其特征在于，所述步骤S3利用UWB数据为SLAM后端位姿图添加约束的具体过程如下：
利用获取得到的旋转矩阵R和位移向量tl，将位于UWB坐标系下的UWB基站节点投影到3D激光SLAM坐标系中，获取各个UWB基站节点在3D激光SLAM坐标系下的坐标；
在探索室内区域的过程中，UWB标签节点会与UWB基站节点实时通信；
若当前传感器的位置处于由各个UWB基站节点构成的包络面内，则将UWB系统解算出的定位位置坐标xuwb作为定位观测约束加入到SLAM后端位姿图中：
e1＝||xuwb-xt|| (13)
式(13)中xt为t时刻的SLAM关键帧位姿Pt的位置分量；
若当前传感器的位置处于由各个UWB基站节点构成的包络面区域外，则按以下方法处理：
获取得到测距信息；随后，根据UWB信号强度阈值、移动标签与基站之间的距离阈值要求，对原始的测距信息过滤：
dt＝(d1，d2，...dj)T (14)
式(14)中，dt表示t时刻，移动标签节点和j个有效基站节点的测量距离；
将t时刻测得的最近的基站坐标xmean作为路标加入到SLAM的后端位姿图，并根据测量距离dmean，为t时刻的SLAM关键帧位姿Pt构建观测约束，用于后端图优化：
Figure FDA0003199598790000041
其中，
Figure FDA0003199598790000042
e1和e2均为UWB观测约束。
7.根据权利要求1所述的一种基于3D激光雷达和UWB的室内SLAM建图方法，其特征在于，所述步骤S4进行基于曲率特征编码的回环检测，包括以下步骤：
对关键帧点云进行遍历，计算该帧点云中，每个点的曲率，记为C：
Figure FDA0003199598790000051
式(20)中，K表示与当前激光点pointi位于同一行的左侧和右侧的激光点集合，ri代表点pointi到激光雷达中心0的距离，dj代表K中除去pointi的其他点到激光雷达中心0的距离，λ代表比例系数，根据实际K集合的大小而调整，使求出的曲率大小处在一个固定区间范围内；
对曲率根据阈值进行过滤和取整操作：
Figure FDA0003199598790000052
式(21)中，Cthreshold为设定的阈值；
对关键帧点云所在的三维空间，根据水平面的分辨率进行划分，得到N个子区域：
Figure FDA0003199598790000053
式(22)中，L1和L2分别表示水平面上，距离激光雷达中心0的最大距离和最小距离；α为角度间隔，l为径向单位长度；
根据划分的空间区域生成一个规格为
Figure FDA0003199598790000054
的编码矩阵M，编码矩阵M的每个元素值为对应子区域中的最大的曲率值C，没有点云的子区域，用0值填充到矩阵的相应位置；
在SLAM算法运行的过程中，将生成的每个关键帧的对应的编码矩阵M存入数据库中；
当SLAM算法根据当前关键帧检测是否出现回环情况时，在历史关键帧的编码矩阵数据库中查找一个与当前关键帧的编码矩阵最相似的编码矩阵；
过程包括：
为编码矩阵M生成一个行数相同的编码列向量v，该向量中的每一个元素为矩阵对应行的元素最大值Cmax；
在编码矩阵数据库中查找候选矩阵时，先比较列向量v的相似度：
diff＝||vcurrent-vselect|| (23)
式(23)中，vcurrent表示当前时刻关键帧的编码矩阵对应的编码列向量，vselect代表候选关键帧的编码矩阵对应的编码列向量；当diff小于设定阈值时，即可初步挑选出候选关键帧对应的编码矩阵Mselect；
接着确定Mselect和当前时刻的关键帧的编码矩阵Mcurrent之间的列平移数shift，求取shift的方式为从Mcurrent中随机抽取一个列向量，所在的列记为jcurrent，然后遍历Mselect中的每一个列向量，找到一个最相似的列向量，记所在的列为jselect，则有：
shift＝jselect-jcurrent (25)
随后将Mselect根据shift进行平移，得到
Figure FDA0003199598790000061
比较
Figure FDA0003199598790000062
和Mcurrent两个矩阵的相似度s：
Figure FDA0003199598790000063
式(26)中，
Figure FDA0003199598790000064
和
Figure FDA0003199598790000065
为对应编码矩阵的其中一个列向量，当s满足小于设定阈值条件时，认为两个编码矩阵对应的关键帧点云相似，即当前关键帧的位置与候选历史关键帧的位置接近，从而实现位置识别功能，符合添加回环约束的条件；
将当前关键帧点云和查找到的历史关键帧点云进行点云匹配，求出两个关键帧雷达坐标系之间的相对位姿变换，并将其添加到SLAM后端位姿图中作为约束。
CN202110900474.1A 2021-08-06 2021-08-06 一种基于3d激光雷达和uwb的室内slam建图方法 Active CN113538410B (zh)

Priority Applications (2)

Application Number Priority Date Filing Date Title
CN202110900474.1A CN113538410B (zh) 2021-08-06 2021-08-06 一种基于3d激光雷达和uwb的室内slam建图方法
US17/826,794 US11543530B1 (en) 2021-08-06 2022-05-27 Indoor SLAM method based on 3D lidar and UWB

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110900474.1A CN113538410B (zh) 2021-08-06 2021-08-06 一种基于3d激光雷达和uwb的室内slam建图方法

Publications (2)

Publication Number Publication Date
CN113538410A true CN113538410A (zh) 2021-10-22
CN113538410B CN113538410B (zh) 2022-05-20

Family

ID=78122043

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110900474.1A Active CN113538410B (zh) 2021-08-06 2021-08-06 一种基于3d激光雷达和uwb的室内slam建图方法

Country Status (2)

Country Link
US (1) US11543530B1 (zh)
CN (1) CN113538410B (zh)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114419187A (zh) * 2021-12-23 2022-04-29 北京百度网讯科技有限公司 地图构建方法、装置、电子设备和可读存储介质
CN114488112A (zh) * 2021-12-23 2022-05-13 西安电子科技大学 一种基于不同体制雷达的波形与点迹融合检测方法
CN114814868A (zh) * 2022-04-06 2022-07-29 广东工业大学 一种双手爪攀爬机器人系统及其同时定位与建图方法

Families Citing this family (14)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US12004046B2 (en) 2021-09-13 2024-06-04 Motorola Mobility Llc Object tracking based on UWB tags
US11990012B2 (en) 2021-11-29 2024-05-21 Motorola Mobility Llc Object contextual control based on UWB radios
US20230168343A1 (en) * 2021-11-29 2023-06-01 Motorola Mobility Llc Object and Environment Dimensioning Based on UWB Radios
US12069120B2 (en) 2021-11-29 2024-08-20 Motorola Mobility Llc Digital media playback based on UWB radios
US12063059B2 (en) 2022-01-20 2024-08-13 Motorola Mobility Llc UWB accessory for a wireless device
CN116242372B (zh) * 2023-01-03 2024-07-19 东南大学 一种gnss拒止环境下的uwb-激光雷达-惯导融合定位方法
CN116299383B (zh) * 2023-02-14 2024-02-02 北京建筑大学 多传感器融合综合管廊巡检小车定位方法
CN115965756B (zh) * 2023-03-13 2023-06-06 安徽蔚来智驾科技有限公司 地图构建方法、设备、驾驶设备和介质
CN116908869B (zh) * 2023-04-14 2024-07-19 南京航空航天大学 一种大工作空间下多目标的多感知融合定位方法
CN116930864B (zh) * 2023-06-27 2024-02-23 中铁第四勘察设计院集团有限公司 一种室内外无缝统一基准构建方法及装置
CN117761717B (zh) * 2024-02-21 2024-05-07 天津大学四川创新研究院 一种自动回环三维重建系统及运行方法
CN118334259B (zh) * 2024-04-11 2024-10-15 西安工业大学 融合增强多线激光雷达与imu的建图方法及系统
CN118225074B (zh) * 2024-05-23 2024-08-06 武汉理工大学 一种散货船清舱机器人自适应地图更新方法及装置
CN118463996B (zh) * 2024-07-09 2024-10-01 杭州智元研究院有限公司 去中心化的多机器人协同定位方法及系统

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109275093A (zh) * 2018-10-08 2019-01-25 高子庆 基于uwb定位与激光地图匹配的定位方法和移动终端
CN110849367A (zh) * 2019-10-08 2020-02-28 杭州电子科技大学 基于融合uwb的视觉slam的室内定位与导航的方法
CN111540005A (zh) * 2020-04-21 2020-08-14 南京理工大学 基于二维栅格地图的回环检测方法
CN112540345A (zh) * 2020-11-18 2021-03-23 广东工业大学 一种基于高斯分布检测uwb质量的双模型定位方法及系统
US20210158560A1 (en) * 2019-11-22 2021-05-27 Beijing Xiaomi Mobile Software Co., Ltd. Method and device for obtaining localization information and storage medium
CN112907491A (zh) * 2021-03-18 2021-06-04 中煤科工集团上海有限公司 一种适用于井下巷道的激光点云回环检测方法及系统
CN112950781A (zh) * 2021-03-19 2021-06-11 中山大学 特种场景的多传感器动态加权融合的点云地图构建方法

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20220187841A1 (en) * 2020-12-10 2022-06-16 AI Incorporated Method of lightweight simultaneous localization and mapping performed on a real-time computing and battery operated wheeled device

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109275093A (zh) * 2018-10-08 2019-01-25 高子庆 基于uwb定位与激光地图匹配的定位方法和移动终端
CN110849367A (zh) * 2019-10-08 2020-02-28 杭州电子科技大学 基于融合uwb的视觉slam的室内定位与导航的方法
US20210158560A1 (en) * 2019-11-22 2021-05-27 Beijing Xiaomi Mobile Software Co., Ltd. Method and device for obtaining localization information and storage medium
CN111540005A (zh) * 2020-04-21 2020-08-14 南京理工大学 基于二维栅格地图的回环检测方法
CN112540345A (zh) * 2020-11-18 2021-03-23 广东工业大学 一种基于高斯分布检测uwb质量的双模型定位方法及系统
CN112907491A (zh) * 2021-03-18 2021-06-04 中煤科工集团上海有限公司 一种适用于井下巷道的激光点云回环检测方法及系统
CN112950781A (zh) * 2021-03-19 2021-06-11 中山大学 特种场景的多传感器动态加权融合的点云地图构建方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
徐晓苏等: "基于二次解析的UWB室内定位高度方向优化方法", 《中国惯性技术学报》 *

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114419187A (zh) * 2021-12-23 2022-04-29 北京百度网讯科技有限公司 地图构建方法、装置、电子设备和可读存储介质
CN114488112A (zh) * 2021-12-23 2022-05-13 西安电子科技大学 一种基于不同体制雷达的波形与点迹融合检测方法
CN114419187B (zh) * 2021-12-23 2023-02-24 北京百度网讯科技有限公司 地图构建方法、装置、电子设备和可读存储介质
CN114814868A (zh) * 2022-04-06 2022-07-29 广东工业大学 一种双手爪攀爬机器人系统及其同时定位与建图方法

Also Published As

Publication number Publication date
CN113538410B (zh) 2022-05-20
US11543530B1 (en) 2023-01-03

Similar Documents

Publication Publication Date Title
CN113538410B (zh) 一种基于3d激光雷达和uwb的室内slam建图方法
Lategahn et al. Vision-only localization
CN106840148B (zh) 室外作业环境下基于双目摄像机的可穿戴式定位与路径引导方法
Lenac et al. Fast planar surface 3D SLAM using LIDAR
Qu et al. Vehicle localization using mono-camera and geo-referenced traffic signs
Scaramuzza et al. Visual odometry [tutorial]
Alonso et al. Accurate global localization using visual odometry and digital maps on urban environments
CN110361027A (zh) 基于单线激光雷达与双目相机数据融合的机器人路径规划方法
CN109781092B (zh) 一种危险化工事故中移动机器人定位与建图方法
Qu et al. Landmark based localization in urban environment
CN114088081B (zh) 一种基于多段联合优化的用于精确定位的地图构建方法
CN113447949B (zh) 一种基于激光雷达和先验地图的实时定位系统及方法
Dawood et al. Harris, SIFT and SURF features comparison for vehicle localization based on virtual 3D model and camera
CN115936029B (zh) 一种基于二维码的slam定位方法及装置
Wen et al. TM 3 Loc: Tightly-coupled monocular map matching for high precision vehicle localization
US20170108338A1 (en) Method for geolocating a carrier based on its environment
Lin et al. A sparse visual odometry technique based on pose adjustment with keyframe matching
Jo et al. New Monte Carlo localization using deep initialization: A three-dimensional LiDAR and a camera fusion approach
CN116127405A (zh) 一种融合点云地图、运动模型和局部特征的位置识别方法
Alliez et al. Real-time multi-SLAM system for agent localization and 3D mapping in dynamic scenarios
Tao et al. Automated processing of mobile mapping image sequences
Lu et al. A lightweight real-time 3D LiDAR SLAM for autonomous vehicles in large-scale urban environment
Xu et al. Bifocal-binocular visual SLAM system for repetitive large-scale environments
CN111882663A (zh) 一种融合语义信息完成视觉slam闭环检测方法
Wong et al. Single camera vehicle localization using SURF scale and dynamic time warping

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