CN115164887A - 基于激光雷达与惯性组合的行人导航定位方法和装置 - Google Patents

基于激光雷达与惯性组合的行人导航定位方法和装置 Download PDF

Info

Publication number
CN115164887A
CN115164887A CN202211044235.1A CN202211044235A CN115164887A CN 115164887 A CN115164887 A CN 115164887A CN 202211044235 A CN202211044235 A CN 202211044235A CN 115164887 A CN115164887 A CN 115164887A
Authority
CN
China
Prior art keywords
point cloud
key frame
pedestrian
current moment
closed
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
CN202211044235.1A
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.)
National University of Defense Technology
Original Assignee
National University of Defense 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 National University of Defense Technology filed Critical National University of Defense Technology
Priority to CN202211044235.1A priority Critical patent/CN115164887A/zh
Publication of CN115164887A publication Critical patent/CN115164887A/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/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G01C21/1652—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with ranging devices, e.g. LIDAR or RADAR
    • 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

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)
  • Traffic Control Systems (AREA)

Abstract

本申请涉及一种基于激光雷达与惯性组合的行人导航定位方法及装置。所述方法包括：通过基于卡尔曼滤波融合激光雷达以及其内置惯性测量单元采集的三维点云以及惯性数据得到当前时刻的位姿，通过设置在行人足部的惯性测量单元提取行人步态信息并且在非零速时刻构造点云关键帧，持续零速状态则不构造关键帧，大大减少了算法算力，保证算法运行效率，接着基于位置与点云描述符的闭环检测算法有效判断闭环约束的有无，既能提高闭环检测的准确度，也能对里程计估计的位姿加以优化更新，提高激光雷达与惯性组合定位方法的精度。

Description

基于激光雷达与惯性组合的行人导航定位方法和装置
技术领域
本申请涉及行人导航技术领域，特别是涉及一种基于激光雷达与惯性组合的行人导航定位方法和装置。
背景技术
行人导航是一种提供当前行人位置并进行路径规划的导航技术。摆脱对于外界信号的依赖，实现行人定位的自主性是极为重要的。当前，行人自主导航定位技术主要是基于惯性测量单元（Inertial Measurement Unit，IMU）的定位技术。随着微机械技术的飞速发展，IMU的成本大大降低，基于微小惯性测量单元（Micro-IMU,MIMU）的行人导航***也趋于成熟。
基于IMU的行人导航***主要采用惯导解算的方式，根据内置陀螺仪与加速度计的输出数据计算得到当前行人位置及姿态数据。但由于累积误差的存在，IMU惯导解算得到的位置结果也越来越不可靠。所以IMU常结合其他辅助信息来修正累积误差，提高定位精度。在不引入其他传感器的前提下，零速修正是修正IMU误差廉价且有效的一种方式。行人运动过程可以被分解为一个个步态周期的组合，在一个步态周期内，行人足部接触至地面时，脚部与地面处于相对静止的状态这时传感器对地面的相对速度为零。所以，利用该步态特征就可以对IMU的导航误差进行有效的估计和修正。
随着计算机视觉技术的飞速发展，低成本、小型化的激光雷达也被用于行人导航，因此又被称为激光SLAM（Simultaneous Localization And Mapping，同步定位与建图）。激光雷达可以精确的测量雷达本身与周围环境目标的距离信息，探测距离可达上百米。激光SLAM不仅可以修正IMU惯导解算的位姿误差，提高定位精度，其记录的三维点云信息还可以用来重构周围环境，生成的点云地图也能为行人导航提供物理参考。但由于误差随时间的累积，激光SLAM估计的位姿也不准确，进而无法得到全局一致的轨迹和地图。所以有必要在激光SLAM***中添加闭环约束，用于估计局部特征匹配引起的长期累积漂移。闭环检测是通过对任意两个位置之间的相似性进行度量来识别之前已访问过位置的能力。一旦检测到闭环，就可以根据检测到的闭环点把偏差校正到合适的位置。因此，准确的闭环判断对于提高激光SLAM的适应性和可靠性尤为重要。
现有的闭环检测方法主要集中在两个方面：一是基于里程计的几何关系。若位姿估计结果与历史某一位姿结果相符合，则产生闭环关系。但累积误差的存在使得位姿估计结果并不一定准确，显然该方法并不能直接用于闭环检测。二是根据两帧点云的相似性来确定闭环关系。Bosse等人通过从3D点云中直接提取关键点并使用手工制作的3D描述符对其进行描述，实现了位置识别；Magnusson等人利用正态分布变换（Normal DistributionTransform，NDT）描述3D点云的外观，根据NDT描述符的直方图判断两帧点云的相似性。除此以外，基于学习的方法近年来也被用于闭环检测。De等人提出的SegMatch通过匹配建筑物、车辆等语义特征来实现位置识别；Angelina等人通过从端到端网络中提取全局描述符来实现位置识别，但它需要结合PointNet和NetVLAD进行训练。基于学习的方法虽然避免了手工制作描述符的繁琐性，但它的计算量比较庞大，性能也过于依赖训练过程中的数据集。
考虑到固态激光雷达的小视场角和特殊扫描模式，机械雷达360度视场角的点云描述符并不适用于此，所以有必要选择一种合适的点云描述符对环境特征进行描述。同时，随着时间的累积，历史关键帧数据对于算法运行也是比较庞大的算力，影响激光雷达与惯性组合定位算法的实时性。而现有的闭环检测大多是开环误差较大的情况下使用，如果开环误差比较小，直接匹配历史位姿显然也是可行的。而如果只根据点云关键帧进行匹配，通过相似性判断闭环也有不可避免的缺陷，在一些结构化场景中容易造成错误的闭环。
发明内容
基于此，有必要针对上述技术问题，提供一种能够减少算法算力的同时提高定位精准度的基于激光雷达与惯性组合的行人导航定位方法和装置。
一种基于激光雷达与惯性组合的行人导航定位方法，所述方法包括：
获取激光雷达以及其内置惯性测量单元分别在当前时刻采集的三维点云以及第一惯性数据；
获取布置在行人足部的惯性测量单元在当前时刻采集的第二惯性数据；
利用迭代卡尔曼滤波融合所述三维点云以及第一惯性数据得到当前时刻的行人位姿；
根据所述第二惯性数据判断行人当前是否处于运动状态，若处于运动状态则根据所述三维点云构造点云关键帧，并计算与所述点云关键帧对应的关键帧描述符；
根据当前时刻构造的点云关键帧以及历史时刻构造的点云关键帧进行行人位置的闭环检测，若判断当前时刻所在的位置在历史时刻所在的位置附近，则基于点云描述符进行闭环检测，若检测到闭环则对闭环间隔内的位姿进行优化更新，并输出优化后当前时刻的位姿，若没有检测到闭环则直接输出当前时刻的位姿。
在其中一实施例中，所述根据所述第二惯性数据判断行人当前是否处于运动状态包括：
所述第二惯性数据包括加速度，根据所述加速度的模值、方差的周期性变化以及预设的第一阈值判断行人是否处于运动状态。
在其中一实施例中，若处于运动状态则根据所述三维点云构造点云关键帧包括：
若当前时刻处于运动状态，则以该时刻为起始时刻累积预设帧数的原始三维点云，并将累积的预设帧数原始三维点云进行拼接，构造所述点云关键帧。
在其中一实施例中，所述计算所述点云关键帧对应的关键帧描述符包括：对所述点云关键帧进行分割，并利用2D直方图对分割后的点云关键帧进行描述得到所述关键帧描述符。
在其中一实施例中，所述根据当前时刻构造的点云关键帧以及历史时刻构造的点云关键帧进行行人位置的闭环检测包括：
根据当前时刻构造的点云关键帧以及历史时刻构造的点云关键帧对应的位姿计算两个点云关键帧之间的路径距离以及欧式距离；
根据所述欧式距离与路径距离之间的比值与预设的第二阈值进行对比，若小于所述第二阈值，则判断当前时刻所在的位置在历史时候所在的位置附近。
在其中一实施例中，基于点云描述符的进行闭环检测包括：根据当前时刻以及历史时刻分别对应的关键帧描述符计算对应的两个点云关键帧之间的相似度，若所述相似度大于预设的第三阈值，则判断检测到了闭环。
一种基于激光雷达与惯性组合的行人导航定位装置，所述装置包括：
第一数据获取模块，用于获取激光雷达以及其内置惯性测量单元分别采集的当前时刻的三维点云以及第一惯性数据；
第二数据获取模块，用于获取布置在行人足部的惯性测量单元采集的当前时刻的第二惯性数据；
当前时刻位姿估计模块，用于利用迭代卡尔曼滤波融合所述三维点云以及第一惯性数据得到当前时刻的位姿；
关键帧描述符计算模块，用于根据所述第二惯性数据判断行人当前是否处于运动状态，若处于运动状态则根据所述三维点云构造点云关键帧，并计算所述点云关键帧对应的关键帧描述符；
双重闭环检测模块，用于根据当前时刻构造的点云关键帧以及历史时刻构造的点云关键帧进行行人位置的闭环检测，若判断当前时刻所在的位置在历史时候所在的位置附近，则基于点云描述符的进行闭环检测，若检测到闭环则对闭环间隔内的位姿进行优化更新，并输出优化后当前时刻的位姿，若没有检测到闭环则直接输出当前时刻的位姿。
一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，所述处理器执行所述计算机程序时实现以下步骤：
获取激光雷达以及其内置惯性测量单元分别在当前时刻采集的三维点云以及第一惯性数据；
获取布置在行人足部的惯性测量单元在当前时刻采集的第二惯性数据；
利用迭代卡尔曼滤波融合所述三维点云以及第一惯性数据得到当前时刻的行人位姿；
根据所述第二惯性数据判断行人当前是否处于运动状态，若处于运动状态则根据所述三维点云构造点云关键帧，并计算与所述点云关键帧对应的关键帧描述符；
根据当前时刻构造的点云关键帧以及历史时刻构造的点云关键帧进行行人位置的闭环检测，若判断当前时刻所在的位置在历史时刻所在的位置附近，则基于点云描述符进行闭环检测，若检测到闭环则对闭环间隔内的位姿进行优化更新，并输出优化后当前时刻的位姿，若没有检测到闭环则直接输出当前时刻的位姿。
一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现以下步骤：
获取激光雷达以及其内置惯性测量单元分别在当前时刻采集的三维点云以及第一惯性数据；
获取布置在行人足部的惯性测量单元在当前时刻采集的第二惯性数据；
利用迭代卡尔曼滤波融合所述三维点云以及第一惯性数据得到当前时刻的行人位姿；
根据所述第二惯性数据判断行人当前是否处于运动状态，若处于运动状态则根据所述三维点云构造点云关键帧，并计算与所述点云关键帧对应的关键帧描述符；
根据当前时刻构造的点云关键帧以及历史时刻构造的点云关键帧进行行人位置的闭环检测，若判断当前时刻所在的位置在历史时刻所在的位置附近，则基于点云描述符进行闭环检测，若检测到闭环则对闭环间隔内的位姿进行优化更新，并输出优化后当前时刻的位姿，若没有检测到闭环则直接输出当前时刻的位姿。
上述基于激光雷达与惯性组合的行人导航定位方法及装置，通过基于卡尔曼滤波融合激光雷达以及其内置惯性测量单元采集的三维点云以及惯性数据得到当前时刻的位姿，通过设置在行人足部的惯性测量单元提取行人步态信息并且在非零速时刻构造点云关键帧，持续零速状态则不构造关键帧，大大减少了算法算力，保证算法运行效率，接着基于位置与点云描述符的闭环检测算法有效判断闭环约束的有无，既能提高闭环检测的准确度，也能对里程计估计的位姿加以优化更新，提高激光雷达与惯性组合定位方法的精度。
附图说明
图1为一个实施例中行人导航定位方法的流程示意图；
图2为一个实施例中行人导航定位方法的实施流程示意图；
图3为一个实施例中行人步态辅助的点云关键帧构造方法的流程示意图；
图4为一个实施例中基于位置和点云描述符相似度的闭环检测方法的流程示意图；
图5为一个实施例中路径距离与欧氏距离计算示意图；
图6为一个实施例中行人导航定位装置的结构框图；
图7为一个实施例中计算机设备的内部结构图。
具体实施方式
为了使本申请的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本申请进行进一步详细说明。应当理解，此处描述的具体实施例仅仅用以解释本申请，并不用于限定本申请。
如图1所示，提供了一种基于激光雷达与惯性组合的行人导航定位方法，包括以下步骤：
步骤S100，获取激光雷达以及其内置惯性测量单元在当前时刻的分别采集的三维点云以及第一惯性数据；
步骤S110，获取布置在行人足部的惯性测量单元在当前时刻采集的的第二惯性数据；
步骤S120，利用迭代卡尔曼滤波融合三维点云以及第一惯性数据得到当前时刻的位姿；
步骤S130，根据第二惯性数据判断行人当前是否处于运动状态，若处于运动状态则根据三维点云构造点云关键帧，并计算与点云关键帧对应的关键帧描述符；
步骤S140，根据当前时刻构造的点云关键帧以及历史时刻构造的点云关键帧进行行人位置的闭环检测，若判断当前时刻所在的位置在历史时刻所在的位置附近，则基于点云描述符的进行闭环检测，若检测到闭环则对闭环间隔内的位姿进行优化更新，并输出优化后当前时刻的位姿，若没有检测到闭环则直接输出当前时刻的位姿。
在本实施例中，步骤S100和步骤S110均为获取数据模块，其中激光雷达、激光雷达的内置惯性测量单元以及布置在行人足下的惯性测量单元是实时获取数据的，并在每一时刻利用步骤S120到S140中的方法对数据进行处理从而可以得到每个时刻行人的最优位姿，其中位姿包括位置坐标以及运动方向。
如图2所示，还提供了一种实施本方法的步骤流程图，接下来以该图为例对数据处理过程进行阐述。
在步骤S120中，首先通过激光雷达和其内置的惯性测量单元获取的数据进行处理从而得到获取数据这一时刻行人的位姿，从图2中可以看到，由激光雷达-惯性里程计模块进行计算得到。
具体的，首先对第一惯性数据（也就是IMU数据）进行预处理：对IMU进行状态递推估计，遵循如下公式：
Figure 32238DEST_PATH_IMAGE001
（1）
符合说明：
Figure 900444DEST_PATH_IMAGE002
表示真实状态量，
Figure 149022DEST_PATH_IMAGE003
表示估计状态量，
Figure 747494DEST_PATH_IMAGE004
表示真实状态量与估计状态量之间的状态误差量。
在公式（1）中，下标（如
Figure 30708DEST_PATH_IMAGE005
）为IMU测量值的索引；
Figure 571279DEST_PATH_IMAGE006
为***状态量，包含载体的位置、姿态、速度、IMU的噪声偏差以及重力矢量；
Figure 307154DEST_PATH_IMAGE007
，即在一次雷达扫描中相邻两次IMU采样时间之差（IMU采样间隔）；
Figure 974896DEST_PATH_IMAGE008
是IMU的测量数据；
Figure 112616DEST_PATH_IMAGE009
是IMU的测量噪声；函数
Figure 59975DEST_PATH_IMAGE010
是一个关于
Figure 17567DEST_PATH_IMAGE011
、
Figure 223420DEST_PATH_IMAGE012
、
Figure 464914DEST_PATH_IMAGE013
的函数，
Figure 363600DEST_PATH_IMAGE014
是一种表示状态递推估计的自定义运算。
所以，误差状态动态模型为：
Figure 808488DEST_PATH_IMAGE015
（2）
在公式（2）中，
Figure 323693DEST_PATH_IMAGE016
代表***真实状态量；
Figure 701585DEST_PATH_IMAGE017
为***递推得到的状态估计量；
Figure 505593DEST_PATH_IMAGE018
表示求取两者误差量的自定义运算，
Figure 703356DEST_PATH_IMAGE019
即为
Figure 251012DEST_PATH_IMAGE020
与
Figure 467099DEST_PATH_IMAGE021
的误差量。
将IMU测量噪声
Figure 176429DEST_PATH_IMAGE022
的协方差记为Q，则状态递推估计的协方差
Figure 595909DEST_PATH_IMAGE023
迭代公式为：
Figure 697988DEST_PATH_IMAGE024
（3）
在公式（3）中，
Figure 519314DEST_PATH_IMAGE025
表示
Figure 665124DEST_PATH_IMAGE026
的协方差，
Figure 837479DEST_PATH_IMAGE027
是
Figure 976206DEST_PATH_IMAGE028
关于
Figure 917617DEST_PATH_IMAGE029
时的偏微分，
Figure 499908DEST_PATH_IMAGE030
是
Figure 893980DEST_PATH_IMAGE031
关于
Figure 334932DEST_PATH_IMAGE032
时的偏微分。
接着，对三维点云数据进行预处理：激光雷达在行进扫射过程中会发生点云畸变，并且采样频率远小于IMU的采样频率，所以可利用状态递推估计公式反向进行点云去畸变，如下式：
Figure 599692DEST_PATH_IMAGE033
（4）
通过公式（4）即可得到一帧点云中每个特征点的位姿，但只是相对于雷达扫描该点时刻的位姿。为方便数据处理，可通过公式（5）把激光雷达一次扫描周期内的点云转换到扫描结束时刻（也即
Figure 352884DEST_PATH_IMAGE034
时刻）雷达坐标系下：
Figure 483520DEST_PATH_IMAGE035
（5）
在公式（5）中，
Figure 980360DEST_PATH_IMAGE036
为激光雷达扫描时刻的索引；
Figure 365205DEST_PATH_IMAGE037
表示
Figure 289299DEST_PATH_IMAGE038
时刻激光雷达扫描的点云在该时刻激光雷达坐标系下的点云坐标；
Figure 408696DEST_PATH_IMAGE039
表示由右下角标坐标系到左上角标坐标系的坐标变换矩阵，
Figure 443648DEST_PATH_IMAGE040
为IMU坐标系，
Figure 214158DEST_PATH_IMAGE041
为激光雷达坐标系；
Figure 43573DEST_PATH_IMAGE042
表示由反向递推估计得到的坐标变换矩阵（
Figure 148802DEST_PATH_IMAGE043
时刻激光雷达坐标系到
Figure 987445DEST_PATH_IMAGE044
时刻激光雷达坐标系）。
最后，利用预处理后的IMU数据以及三维点云数据进行卡尔曼滤波状态更新。
具体的，为了将计算得到的残差与从IMU数值中传播得到的预估状态
Figure 612461DEST_PATH_IMAGE045
和协方差
Figure 612778DEST_PATH_IMAGE046
融合，需要线性化将残差与真实状态
Figure 956035DEST_PATH_IMAGE047
和测量噪声联系起来的测量模型：
Figure 80592DEST_PATH_IMAGE048
（6）
在公式（6）中，
Figure 560115DEST_PATH_IMAGE049
为测量点
Figure 996913DEST_PATH_IMAGE050
的定向噪声，
Figure 811154DEST_PATH_IMAGE051
为测量点
Figure 257179DEST_PATH_IMAGE052
对应于全局坐标系下的真实特征点坐标，
Figure 591208DEST_PATH_IMAGE053
为特征点
Figure 198907DEST_PATH_IMAGE054
所在的相应平面（或边）的法向量（或边缘方向），
Figure 251176DEST_PATH_IMAGE055
为IMU坐标系到全局坐标系的坐标变换矩阵，
Figure 720466DEST_PATH_IMAGE056
表示
Figure 174581DEST_PATH_IMAGE058
与
Figure 953181DEST_PATH_IMAGE060
之间的映射关系。
利用在
Figure 758326DEST_PATH_IMAGE061
处的一阶近似，上公式（6）简化为：
Figure 529842DEST_PATH_IMAGE062
（7）
在公式（7）中，
Figure 838464DEST_PATH_IMAGE063
是残差，定义为点云特征在全局坐标系下的坐标估计与地图上最近的平面或边缘的距离；
Figure 53544DEST_PATH_IMAGE064
是
Figure 814827DEST_PATH_IMAGE065
关于
Figure 140766DEST_PATH_IMAGE066
的雅克比矩阵；
Figure 51697DEST_PATH_IMAGE067
源自于测量噪声
Figure 437679DEST_PATH_IMAGE068
。
采用迭代卡尔曼滤波器公式如下：
卡尔曼增益：
Figure 951837DEST_PATH_IMAGE069
（8）
状态估计：
Figure 815888DEST_PATH_IMAGE070
（9）
在公式（8）和公式（9）中，
Figure 82790DEST_PATH_IMAGE071
为卡尔曼滤波迭代次数；
Figure 905252DEST_PATH_IMAGE072
为
Figure 641127DEST_PATH_IMAGE073
关于
Figure 308869DEST_PATH_IMAGE074
的偏微分，
Figure 181010DEST_PATH_IMAGE075
，
Figure 659527DEST_PATH_IMAGE076
为雅可比矩阵，
Figure 148277DEST_PATH_IMAGE077
是协方差矩阵，
Figure 354131DEST_PATH_IMAGE078
是单位矩阵。
由以上更新的估计
Figure 346357DEST_PATH_IMAGE079
之后用于迭代状态更新。一直持续该过程，直到下式收敛：
Figure 494311DEST_PATH_IMAGE080
（10）
在公式（10）中，
Figure 939199DEST_PATH_IMAGE081
为给定的一个阈值。
收敛后，可得到最优状态估计与协方差：
Figure 948743DEST_PATH_IMAGE082
（11）
在步骤S130中，在构造点云关键帧时，仅在行人运动时进行构建，这样能够保证闭环精度的同时大大减少计算量，并在一定程度上减少错误闭环。
如图3所示，提供了一种结合行人步态特征构造点云关键帧的方法。
首先根据置于行人足部的惯性测量单元采集得到的第二惯性数据基于零速修正模块判断出当前时刻行人是静止状态还是运动状态。
具体的，第二惯性数据包括加速度，根据加速度的模值、方差的周期性变化以及预设的第一阈值判断行人是否处于运动状态。
首先对加速度
Figure 61055DEST_PATH_IMAGE083
的模值进行计算：
Figure 612866DEST_PATH_IMAGE084
（12）
然后根据公式（12）计算一定窗口范围内的加速度方差，同时除去重力影响，这样能够突出足部的周期性变化规律。一定窗口范围内加速度的方差
Figure 76208DEST_PATH_IMAGE085
可以按照以下式子计算得到：
Figure 623864DEST_PATH_IMAGE086
（13）
在公式（13）中，
Figure 590683DEST_PATH_IMAGE087
为当前窗口范围内的平均加速度值，可通过
Figure 831172DEST_PATH_IMAGE088
计算得到，
Figure 765499DEST_PATH_IMAGE089
为加速度数据时刻的索引，
Figure 116846DEST_PATH_IMAGE090
为选取的窗口大小。
在通过预设的阈值
Figure 938171DEST_PATH_IMAGE091
，根据下式即可判断当前时刻为零速时刻或迈步时刻（也就是足部离地）：
Figure 615140DEST_PATH_IMAGE092
（14）
考虑到小型固态激光雷达独特的非重复扫描方式，一帧点云无法充分描述场景信息，所以直接使用某一帧原始点云作为关键帧并不可取。但将多帧点云拼接起来作为一个关键帧，可以利用固态激光雷达非重复式扫描这一特性获得更丰富的场景信息，更容易提取环境特征，从而提高闭环检测的准确度。
一个点云关键帧由
Figure 521916DEST_PATH_IMAGE093
（可取
Figure 162107DEST_PATH_IMAGE094
）帧原始点云
Figure 369098DEST_PATH_IMAGE096
合成，即利用激光雷达-惯性里程计模块估计的位姿将原始点云拼接在一起，因此关键帧点云
Figure 685809DEST_PATH_IMAGE098
可表示为：
Figure 79882DEST_PATH_IMAGE099
（15）
另一方面，考虑到历史关键帧数据对算法运行效率的影响，需要对关键帧（也就是指的点云关键帧）的选择加以取舍。在足部安装的IMU可以通过零速修正模块得到行人步态信息，从而判断行人运动状态。当处于持续静止状态时，激光雷达扫描得到的点云数据并不会有明显变化，此时反复构造多个关键帧并不会提升闭环的精度，反而导致计算量大大增加。只有当行人运动时，点云数据才会发生比较大的变化。所以可结合行人的零速步态信息判断是否构造关键帧。
当行人处于零速步态时，并不会触发构造关键帧。一旦检测到非零速步态信息，也就是当前时刻处于运动状态时，则以该时刻为起始时刻开始构造关键帧，累积采集
Figure 22299DEST_PATH_IMAGE100
（可取
Figure 818217DEST_PATH_IMAGE101
）帧原始点云即拼接成完整的关键帧，再计算其描述符。
在本实施例中，计算点云关键帧对应的关键帧描述符包括：对点云关键帧进行分割，并利用2D直方图对分割后的点云关键帧进行描述得到关键帧描述符。
具体的，当检测到非零速时刻后，每接收一帧点云数据就对该帧点云所有的点进行遍历，以该点为起始点创建小立方体或将该点加入到已创建的小立方体中。
创建小立方体的规则如下：如果一个点
Figure 836988DEST_PATH_IMAGE102
不属于任何一个小立方体，那么将该点作为初始点，创建一个新的立方体，其尺寸是固定的，X、Y、Z轴方向上的长度分别为
Figure 718356DEST_PATH_IMAGE103
、
Figure 949618DEST_PATH_IMAGE104
、
Figure 613424DEST_PATH_IMAGE105
，可设置为0.8m。立方体中心点坐标
Figure 537517DEST_PATH_IMAGE106
由该初始点在世界坐标系中的位置确定：
Figure 906182DEST_PATH_IMAGE107
（16）
假设小立方体
Figure 941134DEST_PATH_IMAGE108
中有共有
Figure 695332DEST_PATH_IMAGE109
个点，根据所有点的坐标计算得到立方体的均值和协方差：
均值：
Figure 55906DEST_PATH_IMAGE110
（17）
协方差：
Figure 911867DEST_PATH_IMAGE111
（18）
遍历构成关键帧的每一帧点云后，最终可以将关键帧分割为若干个立方体。一个立方体可以用中心点
Figure 484931DEST_PATH_IMAGE112
、均值
Figure 109947DEST_PATH_IMAGE113
、协方差
Figure 126576DEST_PATH_IMAGE114
以及其中所有点坐标的集合
Figure 469832DEST_PATH_IMAGE115
表示为
Figure 112166DEST_PATH_IMAGE116
。如此一来便将关键帧中所有的点云划分为了若干小立方体。关键帧的描述符也可以由所有小立方体特征的集合表示，计算关键帧描述符即是计算所有小立方体特征的过程。
接着，对每个立方体，根据其中包含的所有点的位置信息按照以下策略确定它的特征类型和特征方向。
首先，将创建立方体过程中得到的协方差矩阵
Figure 591689DEST_PATH_IMAGE117
进行特征值分解，按照特征值大小顺序排列得到三个特征值：
Figure 277754DEST_PATH_IMAGE118
每个立方体的特征类型根据特征值的情况来确定：
a. 如果
Figure 842728DEST_PATH_IMAGE119
远大于
Figure 23174DEST_PATH_IMAGE120
，则认为该立方体的特征是面特征，其特征方向
Figure 622782DEST_PATH_IMAGE121
是特征向量矩阵的第三列，也就是面的法向量。
b. 如果不是面特征，且
Figure 230481DEST_PATH_IMAGE122
远大于
Figure 30553DEST_PATH_IMAGE123
则认为该立方体是线特征，其特征方向
Figure 749111DEST_PATH_IMAGE124
是特征向量矩阵的第一列，也就是线的方向。
c. 如果一个立方体既不是面特征也不是线特征，则记为没有特征。
同时关键帧的特征描述符还应具有旋转不变性，可通过前面计算得到的所有特征方向
Figure 468805DEST_PATH_IMAGE125
乘上一个旋转矩阵R来旋转它，以期望在一个关键帧中大多数立方体的特征方向都位于X轴上，其余部分特征方向大多位于Y轴上。
对于一个没有进行旋转变化的特征方向向量
Figure 247405DEST_PATH_IMAGE126
，经过旋转变换后其特征方向向量可以表示为：
Figure 36238DEST_PATH_IMAGE127
（19）
在公式（19）中，
Figure 824066DEST_PATH_IMAGE128
表示沿
Figure 132688DEST_PATH_IMAGE129
向量X轴正方向的单位向量。
该旋转变换本质上是将特征方向向量投影到世界坐标系下的X轴正方向上，旋转变换的欧拉角可以表示为：
Figure 82189DEST_PATH_IMAGE130
（20）
在公式（20）中，
Figure 843472DEST_PATH_IMAGE131
为俯仰角、
Figure 920143DEST_PATH_IMAGE132
为偏航角，二者的取值范围都在0°到180°之间。
将水平角按照每3度划分一个区间，每个区间对应一个元素，这个元素就是该区间内特征的数目，所有的这些元素就组成了该关键帧对应的2D直方图特征，也即最终构建的关键帧描述符。
在步骤S140中，为了在保证闭环精度的同时进一步减少计算量，闭环检测算法在计算描述符相似度之前，添加了基于位置的闭环检测，对关键帧进行初步比较，只有位置满足阈值条件才会进一步地计算相似度，在提高闭环检测效率的同时降低了计算量，减少了对计算设备的性能要求，增强了闭环检测算法的实时性。通过引入闭环优化算法可以有效消除传感器运动造成的累积误差，进一步提高激光雷达惯性组合定位方法的精度。
如图4所示，提供了一种闭环检测方法，在该方法中，首先根据当前时刻构造的点云关键帧以及历史时刻构造的点云关键帧对应的位姿计算两个点云关键帧之间的路径距离以及欧式距离，根据欧式距离与路径距离之间的比值与预设的第二阈值进行对比，若小于第二阈值，则判断当前时刻所在的位置在历史时候所在的位置附近。
由于里程计的位置误差较大并且会随时间不断累积，导航结果会随行走路径的增加而迅速发散，事先设定的两关键帧距离的阈值就会失效。但基于多个关键帧的位置信息，分别计算两关键帧之间的路径距离和欧式距离，将欧式距离和路径距离的比值作为判断条件，也能改善这一现象。
具体的，欧氏距离通过两个关键帧在世界坐标系下的位置直接计算，图5中的
Figure 348850DEST_PATH_IMAGE133
即为
Figure 469253DEST_PATH_IMAGE134
时刻构造的关键帧和
Figure 983411DEST_PATH_IMAGE135
时刻构造的关键帧之间的欧氏距离：
Figure 96730DEST_PATH_IMAGE136
（21）
在公式（21）中，
Figure 114364DEST_PATH_IMAGE137
为
Figure 671247DEST_PATH_IMAGE138
时刻激光雷达在全局坐标系下的位置。
具体的，路径距离通过两个关键帧之间所有关键帧的欧氏距离累加得到，图5中
Figure 407122DEST_PATH_IMAGE140
时刻构造的关键帧和
Figure 74864DEST_PATH_IMAGE141
时刻构造的关键帧之间的路径距离
Figure 960387DEST_PATH_IMAGE142
为：
Figure 688172DEST_PATH_IMAGE143
（22）
对于欧氏距离与路径距离的比值
Figure 645763DEST_PATH_IMAGE144
，当这一比值小于设定的阈值
Figure 117196DEST_PATH_IMAGE145
时，则判断激光雷达在
Figure 624269DEST_PATH_IMAGE146
时刻位于
Figure 257376DEST_PATH_IMAGE147
时刻所在的位置附近。如此方可进行下一步的基于点云描述符的闭环检测算法。
在通过基于行人位置的闭环检测后，需要进一步判断两关键帧的描述符是否相似。
在步骤S130中，中已经使用2D直方图描述了关键帧，因此只需要计算关键帧对应直方图的相似度即可判断两关键帧是否形成闭环。
两个2D直方图
Figure 967843DEST_PATH_IMAGE148
与
Figure 977387DEST_PATH_IMAGE149
的相似性
Figure 89700DEST_PATH_IMAGE150
可以通过下式进行计算：
Figure 910020DEST_PATH_IMAGE151
（23）
在公式（23）中，
Figure 842203DEST_PATH_IMAGE152
是
Figure 921018DEST_PATH_IMAGE153
的平均值，而
Figure 622258DEST_PATH_IMAGE154
就是元素
Figure 846434DEST_PATH_IMAGE156
的索引。
如果两关键帧之间的相似度
Figure 797073DEST_PATH_IMAGE157
高于预设的阈值，则认为是检测到了闭环。当检测到闭环信息之后，则在后端优化模块中添加闭环约束，对闭环间隔内的位姿进行优化更新，否则就直接输出激光雷达-惯性里程计估计的位姿。
上述基于激光雷达与惯性组合的行人导航定位方法，通过基于卡尔曼滤波融合激光雷达以及其内置惯性测量单元采集的三维点云以及惯性数据得到当前时刻的位姿，通过设置在行人足部的惯性测量单元提取行人步态信息并且在非零速时刻构造点云关键帧，持续零速状态则不构造关键帧，大大减少了算法算力，保证算法运行效率，接着基于位置与点云描述符的闭环检测算法有效判断闭环约束的有无，既能提高闭环检测的准确度，也能对里程计估计的位姿加以优化更新，提高激光雷达与惯性组合定位方法的精度。
应该理解的是，虽然图1的流程图中的各个步骤按照箭头的指示依次显示，但是这些步骤并不是必然按照箭头指示的顺序依次执行。除非本文中有明确的说明，这些步骤的执行并没有严格的顺序限制，这些步骤可以以其它的顺序执行。而且，图1中的至少一部分步骤可以包括多个子步骤或者多个阶段，这些子步骤或者阶段并不必然是在同一时刻执行完成，而是可以在不同的时刻执行，这些子步骤或者阶段的执行顺序也不必然是依次进行，而是可以与其它步骤或者其它步骤的子步骤或者阶段的至少一部分轮流或者交替地执行。
在一个实施例中，如图6所示，提供了一种基于激光雷达与惯性组合的行人导航定位装置，包括：第一数据获取模块600、第二数据获取模块610、当前时刻位姿估计模块620、关键帧描述符计算模块630和双重闭环检测模块640，其中：
第一数据获取模块600，用于获取激光雷达以及其内置惯性测量单元分别采集的当前时刻的三维点云以及第一惯性数据；
第二数据获取模块610，用于获取布置在行人足部的惯性测量单元采集的当前时刻的第二惯性数据；
当前时刻位姿估计模块620，用于利用迭代卡尔曼滤波融合所述三维点云以及第一惯性数据得到当前时刻的位姿；
关键帧描述符计算模块630，用于根据所述第二惯性数据判断行人当前是否处于运动状态，若处于运动状态则根据所述三维点云构造点云关键帧，并计算所述点云关键帧对应的关键帧描述符；
双重闭环检测模块640，用于根据当前时刻构造的点云关键帧以及历史时刻构造的点云关键帧进行行人位置的闭环检测，若判断当前时刻所在的位置在历史时候所在的位置附近，则基于点云描述符的进行闭环检测，若检测到闭环则对闭环间隔内的位姿进行优化更新，并输出优化后当前时刻的位姿，若没有检测到闭环则直接输出当前时刻的位姿。
关于基于激光雷达与惯性组合的行人导航定位装置的具体限定可以参见上文中对于基于激光雷达与惯性组合的行人导航定位方法的限定，在此不再赘述。上述基于激光雷达与惯性组合的行人导航定位装置中的各个模块可全部或部分通过软件、硬件及其组合来实现。上述各模块可以硬件形式内嵌于或独立于计算机设备中的处理器中，也可以以软件形式存储于计算机设备中的存储器中，以便于处理器调用执行以上各个模块对应的操作。
在一个实施例中，提供了一种计算机设备，该计算机设备可以是服务器，其内部结构图可以如图7所示。该计算机设备包括通过***总线连接的处理器、存储器、网络接口和数据库。其中，该计算机设备的处理器用于提供计算和控制能力。该计算机设备的存储器包括非易失性存储介质、内存储器。该非易失性存储介质存储有操作***、计算机程序和数据库。该内存储器为非易失性存储介质中的操作***和计算机程序的运行提供环境。该计算机设备的数据库用于存储关键帧数据。该计算机设备的网络接口用于与外部的终端通过网络连接通信。该计算机程序被处理器执行时以实现一种基于激光雷达与惯性组合的行人导航定位方法。
本领域技术人员可以理解，图7中示出的结构，仅仅是与本申请方案相关的部分结构的框图，并不构成对本申请方案所应用于其上的计算机设备的限定，具体的计算机设备可以包括比图中所示更多或更少的部件，或者组合某些部件，或者具有不同的部件布置。
在一个实施例中，提供了一种计算机设备，包括存储器和处理器，存储器中存储有计算机程序，该处理器执行计算机程序时实现以下步骤：
获取激光雷达以及其内置惯性测量单元分别采集的当前时刻的三维点云以及第一惯性数据；
获取布置在行人足部的惯性测量单元采集的当前时刻的第二惯性数据；
利用迭代卡尔曼滤波融合所述三维点云以及第一惯性数据得到当前时刻的位姿；
根据所述第二惯性数据判断行人当前是否处于运动状态，若处于运动状态则根据所述三维点云构造点云关键帧，并计算所述点云关键帧对应的关键帧描述符；
根据当前时刻构造的点云关键帧以及历史时刻构造的点云关键帧进行行人位置的闭环检测，若判断当前时刻所在的位置在历史时候所在的位置附近，则基于点云描述符的进行闭环检测，若检测到闭环则对闭环间隔内的位姿进行优化更新，并输出优化后当前时刻的位姿，若没有检测到闭环则直接输出当前时刻的位姿。
在一个实施例中，提供了一种计算机可读存储介质，其上存储有计算机程序，计算机程序被处理器执行时实现以下步骤：
获取激光雷达以及其内置惯性测量单元分别采集的当前时刻的三维点云以及第一惯性数据；
获取布置在行人足部的惯性测量单元采集的当前时刻的第二惯性数据；
利用迭代卡尔曼滤波融合所述三维点云以及第一惯性数据得到当前时刻的位姿；
根据所述第二惯性数据判断行人当前是否处于运动状态，若处于运动状态则根据所述三维点云构造点云关键帧，并计算所述点云关键帧对应的关键帧描述符；
根据当前时刻构造的点云关键帧以及历史时刻构造的点云关键帧进行行人位置的闭环检测，若判断当前时刻所在的位置在历史时候所在的位置附近，则基于点云描述符的进行闭环检测，若检测到闭环则对闭环间隔内的位姿进行优化更新，并输出优化后当前时刻的位姿，若没有检测到闭环则直接输出当前时刻的位姿。
本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一非易失性计算机可读取存储介质中，该计算机程序在执行时，可包括如上述各方法的实施例的流程。其中，本申请所提供的各实施例中所使用的对存储器、存储、数据库或其它介质的任何引用，均可包括非易失性和/或易失性存储器。非易失性存储器可包括只读存储器（ROM）、可编程ROM（PROM）、电可编程ROM（EPROM）、电可擦除可编程ROM（EEPROM）或闪存。易失性存储器可包括随机存取存储器（RAM）或者外部高速缓冲存储器。作为说明而非局限，RAM以多种形式可得，诸如静态RAM（SRAM）、动态RAM（DRAM）、同步DRAM（SDRAM）、双数据率SDRAM（DDRSDRAM）、增强型SDRAM（ESDRAM）、同步链路（Synchlink） DRAM（SLDRAM）、存储器总线（Rambus）直接RAM（RDRAM）、直接存储器总线动态RAM（DRDRAM）、以及存储器总线动态RAM（RDRAM）等。
以上实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。
以上所述实施例仅表达了本申请的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本申请构思的前提下，还可以做出若干变形和改进，这些都属于本申请的保护范围。因此，本申请专利的保护范围应以所附权利要求为准。

Claims (7)

1.基于激光雷达与惯性组合的行人导航定位方法，其特征在于，所述方法包括：
获取激光雷达以及其内置惯性测量单元分别在当前时刻采集的三维点云以及第一惯性数据；
获取布置在行人足部的惯性测量单元在当前时刻采集的第二惯性数据；
利用迭代卡尔曼滤波融合所述三维点云以及第一惯性数据得到当前时刻的行人位姿；
根据所述第二惯性数据判断行人当前是否处于运动状态，若处于运动状态则根据所述三维点云构造点云关键帧，并计算与所述点云关键帧对应的关键帧描述符；
根据当前时刻构造的点云关键帧以及历史时刻构造的点云关键帧进行行人位置的闭环检测，若判断当前时刻所在的位置在历史时刻所在的位置附近，则基于点云描述符进行闭环检测，若检测到闭环则对闭环间隔内的位姿进行优化更新，并输出优化后当前时刻的位姿，若没有检测到闭环则直接输出当前时刻的位姿。
2.根据权利要求1所述的行人导航定位方法，其特征在于，所述根据所述第二惯性数据判断行人当前是否处于运动状态包括：
所述第二惯性数据包括加速度；
根据所述加速度的模值、方差的周期性变化以及预设的第一阈值判断行人是否处于运动状态。
3.根据权利要求1所述的行人导航定位方法，其特征在于，若处于运动状态则根据所述三维点云构造点云关键帧包括：
若当前时刻处于运动状态，则以该时刻为起始时刻累积预设帧数的原始三维点云，并将累积的预设帧数原始三维点云进行拼接，构造所述点云关键帧。
4.根据权利要求1所述的行人导航定位方法，其特征在于，所述计算所述点云关键帧对应的关键帧描述符包括：对所述点云关键帧进行分割，并利用2D直方图对分割后的点云关键帧进行描述得到所述关键帧描述符。
5.根据权利要求1所述的行人导航定位方法，其特征在于，所述根据当前时刻构造的点云关键帧以及历史时刻构造的点云关键帧进行行人位置的闭环检测包括：
根据当前时刻构造的点云关键帧以及历史时刻构造的点云关键帧对应的位姿计算两个点云关键帧之间的路径距离以及欧式距离；
根据所述欧式距离与路径距离之间的比值与预设的第二阈值进行对比，若小于所述第二阈值，则判断当前时刻所在的位置在历史时刻所在的位置附近。
6.根据权利要求4所述的行人导航定位方法，其特征在于，基于点云描述符的进行闭环检测包括：
根据当前时刻以及历史时刻分别对应的关键帧描述符计算对应的两个点云关键帧之间的相似度，若所述相似度大于预设的第三阈值，则判断检测到了闭环。
7.基于激光雷达与惯性组合的行人导航定位装置，其特征在于，所述装置包括：
第一数据获取模块，用于获取激光雷达以及其内置惯性测量单元分别在当前时刻采集的三维点云以及第一惯性数据；
第二数据获取模块，用于获取布置在行人足部的惯性测量单元在当前时刻采集的第二惯性数据；
当前时刻位姿估计模块，用于利用迭代卡尔曼滤波融合所述三维点云以及第一惯性数据得到当前时刻的行人位姿；
关键帧描述符计算模块，用于根据所述第二惯性数据判断行人当前是否处于运动状态，若处于运动状态则根据所述三维点云构造点云关键帧，并计算与所述点云关键帧对应的关键帧描述符；
双重闭环检测模块，用于根据当前时刻构造的点云关键帧以及历史时刻构造的点云关键帧进行行人位置的闭环检测，若判断当前时刻所在的位置在历史时刻所在的位置附近，则基于点云描述符进行闭环检测，若检测到闭环则对闭环间隔内的位姿进行优化更新，并输出优化后当前时刻的位姿，若没有检测到闭环则直接输出当前时刻的位姿。
CN202211044235.1A 2022-08-30 2022-08-30 基于激光雷达与惯性组合的行人导航定位方法和装置 Pending CN115164887A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211044235.1A CN115164887A (zh) 2022-08-30 2022-08-30 基于激光雷达与惯性组合的行人导航定位方法和装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211044235.1A CN115164887A (zh) 2022-08-30 2022-08-30 基于激光雷达与惯性组合的行人导航定位方法和装置

Publications (1)

Publication Number Publication Date
CN115164887A true CN115164887A (zh) 2022-10-11

Family

ID=83481808

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211044235.1A Pending CN115164887A (zh) 2022-08-30 2022-08-30 基于激光雷达与惯性组合的行人导航定位方法和装置

Country Status (1)

Country Link
CN (1) CN115164887A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115326068A (zh) * 2022-10-17 2022-11-11 北京理工大学 激光雷达-惯性测量单元融合里程计设计方法及***

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110887490A (zh) * 2019-11-29 2020-03-17 上海有个机器人有限公司 一种激光定位导航的关键帧选取方法、介质、终端和装置
CN113466890A (zh) * 2021-05-28 2021-10-01 中国科学院计算技术研究所 基于关键特征提取的轻量化激光雷达惯性组合定位方法和***
CN113674399A (zh) * 2021-08-16 2021-11-19 杭州图灵视频科技有限公司 一种移动机器人室内三维点云地图构建方法及***
CN114674311A (zh) * 2022-03-22 2022-06-28 中国矿业大学 一种室内定位与建图方法及***
CN114723917A (zh) * 2022-04-08 2022-07-08 广州高新兴机器人有限公司 激光里程计的位姿优化方法、装置、介质及设备
CN114782626A (zh) * 2022-04-14 2022-07-22 国网河南省电力公司电力科学研究院 基于激光与视觉融合的变电站场景建图及定位优化方法
CN114924287A (zh) * 2022-04-21 2022-08-19 深圳市正浩创新科技股份有限公司 地图构建方法、设备与介质

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110887490A (zh) * 2019-11-29 2020-03-17 上海有个机器人有限公司 一种激光定位导航的关键帧选取方法、介质、终端和装置
CN113466890A (zh) * 2021-05-28 2021-10-01 中国科学院计算技术研究所 基于关键特征提取的轻量化激光雷达惯性组合定位方法和***
CN113674399A (zh) * 2021-08-16 2021-11-19 杭州图灵视频科技有限公司 一种移动机器人室内三维点云地图构建方法及***
CN114674311A (zh) * 2022-03-22 2022-06-28 中国矿业大学 一种室内定位与建图方法及***
CN114723917A (zh) * 2022-04-08 2022-07-08 广州高新兴机器人有限公司 激光里程计的位姿优化方法、装置、介质及设备
CN114782626A (zh) * 2022-04-14 2022-07-22 国网河南省电力公司电力科学研究院 基于激光与视觉融合的变电站场景建图及定位优化方法
CN114924287A (zh) * 2022-04-21 2022-08-19 深圳市正浩创新科技股份有限公司 地图构建方法、设备与介质

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115326068A (zh) * 2022-10-17 2022-11-11 北京理工大学 激光雷达-惯性测量单元融合里程计设计方法及***

Similar Documents

Publication Publication Date Title
CN109974693B (zh) 无人机定位方法、装置、计算机设备及存储介质
Dellenbach et al. Ct-icp: Real-time elastic lidar odometry with loop closure
CN112985416B (zh) 激光与视觉信息融合的鲁棒定位和建图方法及***
CN109887053B (zh) 一种slam地图拼接方法及***
JP6760114B2 (ja) 情報処理装置、データ管理装置、データ管理システム、方法、及びプログラム
CN111583369B (zh) 一种基于面线角点特征提取的激光slam方法
CN107167826B (zh) 一种自动驾驶中基于可变网格的图像特征检测的车辆纵向定位***及方法
CN112230242B (zh) 位姿估计***和方法
US8437501B1 (en) Using image and laser constraints to obtain consistent and improved pose estimates in vehicle pose databases
CN109059907B (zh) 轨迹数据处理方法、装置、计算机设备和存储介质
CN113313763B (zh) 一种基于神经网络的单目相机位姿优化方法及装置
CN113466890B (zh) 基于关键特征提取的轻量化激光雷达惯性组合定位方法和***
CN115690338A (zh) 地图构建方法、装置、设备及存储介质
KR101985344B1 (ko) 관성 및 단일 광학 센서를 이용한 슬라이딩 윈도우 기반 비-구조 위치 인식 방법, 이를 수행하기 위한 기록 매체 및 장치
CN115143954B (zh) 一种基于多源信息融合的无人车导航方法
CN115479598A (zh) 基于多传感器融合的定位与建图方法及紧耦合***
CN113920198B (zh) 一种基于语义边缘对齐的由粗到精的多传感器融合定位方法
CN114001733A (zh) 一种基于地图的一致性高效视觉惯性定位算法
CN116429084A (zh) 一种动态环境3d同步定位与建图方法
CN115164887A (zh) 基于激光雷达与惯性组合的行人导航定位方法和装置
Chiu et al. Precise vision-aided aerial navigation
CN115127554A (zh) 一种基于多源视觉辅助的无人机自主导航方法与***
CN112612034B (zh) 基于激光帧和概率地图扫描的位姿匹配方法
CN112067007B (zh) 地图生成方法、计算机存储介质及电子设备
Emter et al. Simultaneous localization and mapping for exploration with stochastic cloning EKF

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
RJ01 Rejection of invention patent application after publication

Application publication date: 20221011

RJ01 Rejection of invention patent application after publication