CN110849374B - 地下环境定位方法、装置、设备及存储介质 - Google Patents

地下环境定位方法、装置、设备及存储介质 Download PDF

Info

Publication number
CN110849374B
CN110849374B CN201911217135.2A CN201911217135A CN110849374B CN 110849374 B CN110849374 B CN 110849374B CN 201911217135 A CN201911217135 A CN 201911217135A CN 110849374 B CN110849374 B CN 110849374B
Authority
CN
China
Prior art keywords
point cloud
map
pose
frame
current
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
CN201911217135.2A
Other languages
English (en)
Other versions
CN110849374A (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.)
Central South University
Original Assignee
Central South University
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 Central South University filed Critical Central South University
Priority to CN201911217135.2A priority Critical patent/CN110849374B/zh
Publication of CN110849374A publication Critical patent/CN110849374A/zh
Application granted granted Critical
Publication of CN110849374B publication Critical patent/CN110849374B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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/20—Instruments for performing navigational calculations
    • 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
    • G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
    • G01S7/48—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00
    • G01S7/4802—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00 using analysis of echo signal for target characterisation; Target signature; Target cross-section
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F16/00—Information retrieval; Database structures therefor; File system structures therefor
    • G06F16/20—Information retrieval; Database structures therefor; File system structures therefor of structured data, e.g. relational data
    • G06F16/29—Geographical information databases
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/23—Clustering techniques

Landscapes

  • Engineering & Computer Science (AREA)
  • Remote Sensing (AREA)
  • Data Mining & Analysis (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • General Engineering & Computer Science (AREA)
  • Databases & Information Systems (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Automation & Control Theory (AREA)
  • Artificial Intelligence (AREA)
  • Bioinformatics & Cheminformatics (AREA)
  • Bioinformatics & Computational Biology (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Evolutionary Biology (AREA)
  • Evolutionary Computation (AREA)
  • Optical Radar Systems And Details Thereof (AREA)

Abstract

本发明公开了一种地下环境定位方法、装置、设备及存储介质。其中，该方法包括：获取扫描点云数据的特征点云；以上一帧的位姿作为当前的初始位姿，基于当前帧的扫描点云数据的特征点云与上一帧的扫描点云数据的特征点云进行点云帧与帧匹配，确定当前的预测位姿；基于当前帧的扫描点云数据的特征点云、所述当前的预测位姿和特征地图，进行点云帧与地图匹配，得到当前的修正位姿。本发明实施例实现了基于距离权重的地下环境的精确定位，能够提高地下环境中定位的精度，满足地下装备的自主行走的无人驾驶需求。

Description

地下环境定位方法、装置、设备及存储介质
技术领域
本发明涉及定位技术领域，尤其涉及一种地下环境定位方法、装置、设备及存储介质。
背景技术
近年来，地面无人驾驶项目发展迅速，对提高整个交通行业安全及效率有重要作用。而地下封闭受限空间的特殊环境，是实现无人驾驶快速落地的首选场景，能大幅度改善作业的安全性并提高地下作业效率，进而为企业创造出更大的经济效益，其中，地下环境中的精确定位是关键。
虽然在空旷环境下基于GNSS(全球定位卫星***)的定位能实现厘米级的定位精度，但对地下矿山、地铁、管廊、消防、人防等地下特殊环境，由于无法接收到GPS(全球定位***)信号，使得地下定位困难增加。针对地下环境的定位问题，早在20世纪90年代初就已应用低频电磁引导、超声波传感测量引导、基于视觉信标导航等进行地下定位与连续跟踪。此外，基于Wi-Fi(Wireless-Fidelity，无线保真)定位、蓝牙定位、RFID(Radio FrequencyIdentification，无线射频识别)定位、UWB(Ultra Wideband)定位、红外技术、超声波等定位技术也有广泛应用。但上述定位技术都需要在地下环境中安装相应的辅助定位装置，虽能提高定位精度，但是需要增加大量的设备与维护费用，同时由于地下空间环境粗糙，容易造成定位过程中的累计误差，特别是在旋转情况下，很容易出现位姿获取失败，使地下装备自主行走过程中的安全性大大降低。
发明内容
有鉴于此，本发明实施例提供了一种地下环境定位方法、装置、设备及存储介质，旨在提高地下环境中定位的精度，满足地下装备的自主行走的无人驾驶需求。
本发明实施例的技术方案是这样实现的：
本发明实施例提供了一种地下环境定位方法，包括：
获取扫描点云数据的特征点云；
以上一帧的位姿作为当前的初始位姿，基于当前帧的扫描点云数据的特征点云与上一帧的扫描点云数据的特征点云进行点云帧与帧匹配，确定当前的预测位姿；
基于当前帧的扫描点云数据的特征点云、所述当前的预测位姿和特征地图，进行点云帧与地图匹配，得到当前的修正位姿；
其中，所述特征地图是基于实时的激光里程计及建图(LOAM，Lidar Odometry andMapping in Real-time)生成的，且所述特征地图的构建过程中，基于点云与扫描设备间的距离对扫描点云数据进行修正。
本发明实施例还提供了一种地下环境定位装置，包括：
获取模块，用于获取扫描点云数据的特征点云；
预测模块，用于以上一帧的位姿作为当前的初始位姿，基于当前帧的扫描点云数据的特征点云与上一帧的扫描点云数据的特征点云进行点云帧与帧匹配，确定当前的预测位姿；
位姿确定模块，用于基于当前帧的扫描点云数据的特征点云、所述当前的预测位姿和特征地图，进行点云帧与地图匹配，得到当前的修正位姿；
其中，所述特征地图是基于LOAM生成的，且所述特征地图的构建过程中，基于点云与扫描设备间的距离对扫描点云数据进行修正。
本发明实施例又一种地下环境定位设备，包括：处理器和用于存储能够在处理器上运行的计算机程序的存储器，其中，所述处理器，用于运行计算机程序时，执行本发明任一实施例所述方法的步骤。
本发明实施例还提供了一种存储介质，所述存储介质上存储有计算机程序，所述计算机程序被处理器执行时，实现本发明任一实施例所述方法的步骤。
本发明实施例提供的技术方案，通过基于当前帧的扫描点云数据的特征点云与上一帧的扫描点云数据的特征点云进行点云帧与帧匹配，确定当前的预测位姿；基于当前帧的扫描点云数据的特征点云、所述当前的预测位姿和特征地图，进行点云帧与地图匹配，得到当前的修正位姿，且特征地图是基于LOAM生成的，且所述特征地图的构建过程中，基于点云与扫描设备间的距离对扫描点云数据进行修正，实现了基于距离权重的地下环境的精确定位，能够提高地下环境中定位的精度，满足地下装备的自主行走的无人驾驶需求。
附图说明
图1为本发明实施例地下环境定位方法的流程示意图；
图2为本发明实施例中点云坐标系变换的示意图；
图3为本发明一实施例地下环境定位框架的示意图；
图4为本发明实施例地下环境定位装置的结构示意图；
图5为本发明实施例地下环境定位设备的结构示意图。
具体实施方式
下面结合附图及实施例对本发明再作进一步详细的描述。
除非另有定义，本文所使用的所有的技术和科学术语与属于本发明的技术领域的技术人员通常理解的含义相同。本文中在本发明的说明书中所使用的术语只是为了描述具体的实施例的目的，不是旨在于限制本发明。
相关技术中，三维激光雷达(又称为3D激光雷达)的出现，为地下环境的精确定位带来了便利，可以通过采集丰富的3D环境点云信息，实现高精度地图构建及定位的应用。发明人在实现本发明的过程中，发现三维激光雷达的采集点的距离越远，其测量误差越大，采集点距离越近，其测量误差越小。同时由于在地下环境中，其地面并不是十分平整，在三维激光雷达的移动过程中，少许的抖动更会增加远距离数据点(即点云)的测量误差。现有的SLAM(即时定位与地图构建)***在建图过程中，一般是给地图划分栅格，根据实时点云数据的不断增加，对栅格内的点值进行不断的加权平均更新，求其均值和方差，很少考虑其测量误差，虽然LOAM中对运动畸变进行了去畸处理，但是没有考虑测量本身的误差对建图的影响。
基于此，在本发明的各种实施例中，基于LOAM的地图构建过程中，基于点云与扫描设备间的距离对扫描点云数据进行修正，实现了基于距离权重的地下环境的精确定位，能够提高地下环境中定位的精度，满足地下装备的自主行走的无人驾驶需求。
本发明实施例提供了一种地下环境定位方法，如图1所示，包括：
步骤101，获取扫描点云数据的特征点云；
步骤102，以上一帧的位姿作为当前的初始位姿，基于当前帧的扫描点云数据的特征点云与上一帧的扫描点云数据的特征点云进行点云帧与帧匹配，确定当前的预测位姿；
步骤103，基于当前帧的扫描点云数据的特征点云、所述当前的预测位姿和特征地图，进行点云帧与地图匹配，得到当前的修正位姿。
其中，所述特征地图是基于实时的激光里程计及建图(LOAM)生成的，且所述特征地图的构建过程中，基于点云与扫描设备间的距离对扫描点云数据进行修正。
本发明实施例中，由于特征地图是基于点云与扫描设备的距离进行修正后生成的，基于当前帧的扫描点云数据的特征点云、所述当前的预测位姿和特征地图，进行点云帧与地图匹配，得到当前的修正位姿，实现了基于距离权重的地下环境的精确定位，能够提高地下环境中定位的精度，满足地下装备的自主行走的无人驾驶需求。
在一实施例中，扫描设备为三维激光雷达。这里，以VLP-16三维激光雷达为例，VLP-16三维激光雷达是Velodyne公司出品的三维激光雷达。其竖直分辨率约为2°，水平分辨率最大为0.4°。把单个线束称为一个Scan，对全部16线组成的一帧点云称为一个Sweep。一帧内所有的点云，都是按顺序串行扫描的，同一个时间点，只会有一次发送，紧接着一次接收。先从水平第一个角度，一般在0°左右，扫描这个水平角度上竖直方向所有16个点(对应16个SCAN)的深度，当然这16个点也是串行按顺序的，然后转到下一个水平角度，比如0.3°开始，水平分辨率0.4°，那么下个角度就是0.7°，然后1.1°，一直顺时针扫完一圈，完成一个Sweep数据的采集，其输出形式为每个点云对应的(X，Y，Z，I)，其中，(X，Y，Z)为三维坐标，I为反射强度值。
这里，地下环境定位设备获取三维激光雷达扫描得到的扫描点云数据，其中，一帧扫描点云数据即三维激光雷达扫描采集的一个Sweep数据。
本发明实施例中，地下环境定位设备获取到扫描点云数据后，对扫描点云数据基于无损卡尔曼滤波(UKF，Unscented Kalman Filter)构建特征地图、点云帧与帧匹配、点云帧与地图匹配的定位框架，实现地下环境的准确定位。其中，UKF是无损变换(UT)和标准卡尔曼(Kalman)滤波体系的结合，通过无损变换使非线性***方程适用于线性假设下的标准卡尔曼滤波体系。UKF以UT变换为基础，摒弃了对非线性函数进行线性化的传统做法，采用卡尔曼线性滤波框架，对于一步预测方程，使用UT来处理均值和协方差的非线性传递，就成为UKF算法。UKF是对非线性函数的概率密度分布进行近似，用一系列确定样本来逼近状态的后验概率密度，而不是对非线性函数进行近似，不需要求导计算Jacobian(雅克比)矩阵。UKF没有线性化忽略高阶项，因此非线性分布统计量的计算精度较高。
这里，地下环境定位设备对获取的扫描点云数据基于聚类方法进行噪声移除，提取扫描点云数据的特征点云。具体地，可以对噪声移除后的扫描点云数据采用LOAM中的特征提取方法进行特征提取，得到扫描点云数据的特征点云，比如，提取corner地图和surface地图对应的点云作为特征点云。在其他实施例中，扫描设备可以对扫描获取的扫描点云数据进行特征提取，得到扫描点云数据的特征点云并传送给地下环境定位设备，使得地下环境定位设备获取扫描点云数据的特征点云。
基于UKF的定位框架的地下环境定位包括：预测位姿阶段和修正位姿阶段。其中，预测位姿阶段即前述步骤102，修正位姿阶段即前述步骤103。
这里，可以将上一帧的位姿作为当前的初始位姿，基于当前帧的扫描点云数据的特征点云与上一帧的扫描点云数据的特征点云进行点云帧与帧匹配，确定当前的预测位姿。
实际应用中，定义三维激光雷达的位姿状态向量为：
Figure BDA0002299802280000061
其中，pt为t时刻的三维激光雷达的三维坐标，qt为t时刻的三维激光雷达的四元数。
地下环境定位设备通过在连续帧之间迭代地应用所提取的特征点云确定三维激光雷达的预测位姿。具体地，可以基于当前帧的扫描点云数据的特征点云与上一帧的扫描点云数据的特征点云进行点云帧与帧(Scan to Scan)匹配，得到当前帧与上一帧间的相对位姿；基于所述相对位姿和上一帧的位姿得到当前的预测位姿。
示例性地，可以将实时获取的当前帧的特征点云与上一相邻帧的特征点云进行匹配，来获得重新扫描帧t时刻到最近帧t-1时刻的相对位姿Δxt-1,t＝[Δpt,Δqt]T，同时t-1时刻三维激光雷达在世界坐标中的位姿为xt-1，则t时刻三维激光雷达的预测位姿计算如下：
xt＝xt-1Δxt-1,t
在一实施例中，所述基于当前帧的扫描点云数据的特征点云、所述当前的预测位姿和特征地图，进行点云帧与地图匹配，得到当前的修正位姿，包括：
以所述当前的预测位姿作为点云帧与地图匹配的初始值，对当前帧的扫描点云数据的特征点云与特征地图进行点云帧与地图(Scan to Map)匹配得到配准位姿，将所述配准位姿作为当前的修正位姿。需要说明的是，在一实施例中，当前的修正位姿可以作为下一帧的初始位姿。若上一帧为第一帧时，则当前的初始位姿可以选取扫描设备的起始位置。
实际应用中，以所述预测位姿xt作为点云帧与地图匹配的初始值，通过点云帧与地图匹配，得到配准位姿为
Figure BDA0002299802280000062
以此作为无损卡尔曼滤波的观测变量。
由于在进行点云帧与地图匹配时，需要用到特征地图。基于此，在一实施例中，所述方法还包括:
基于LOAM对已获取的扫描点云数据进行地图构建，得到初始的特征地图；
对所述初始的特征地图基于距离幂次反比法对特征点的距离赋予权值，得到修正后的特征地图。
这里，所述基于LOAM对已获取的扫描点云数据进行地图构建，得到初始的特征地图，包括：
根据第一频率对已获取的扫描点云数据基于帧间匹配，得到相邻帧的数据间匹配的拼接位姿；
根据第二频率对相邻帧的数据基于相应的拼接位姿拼接，得到初始的角corner地图和初始的面surface地图；
其中，所述第一频率大于所述第二频率。
LOAM可以使用一个三维空间中运动的两轴激光雷达来获得低漂移和低复杂度，并且不需要高精度的测距和惯性测量的实时激光里程计和地图，其核心思想是将定位和建图的分割，通过两个算法：一个是执行高频率的里程计但是低精度的运动估计(定位)，通过对每一帧激光的配准，可以得到一个精度较差的ODOM(里程计)，由于没有IMU(惯性测量单元)提供帧间匹配的初始位姿，因此对LOAM进行少许改动，运用一个匀速运动模型来计算帧与帧配准的拼接位姿(POSE)；另一个算法在一个数量级低的频率执行匹配和注册点云信息(建图和校正里程计)，通过将多帧的激光特征点云基于拼接位姿拼接，形成特征点云地图(即前述的特征地图)。具体地，可以包括：corner地图和surface地图。这样，就建立了两种特征点云地图。此外，还可以将新入的帧与特征点云地图作配准，得到更精准的拼接位姿，并基于这个更精准的拼接位姿进行拼接，得到更新的特征点云地图；将这两个算法结合就获得了高精度、实时性的激光里程计。
本发明实施例中，还需要对特征点云地图基于距离幂次反比法对特征点的距离赋予权值，得到修正后的特征地图，包括：
将初始的corner地图和初始的corner地图的三维点云转换至世界坐标系并划分至世界坐标系下的三维栅格中；
针对每个栅格，统计各栅格内点云的个数，并基于各点云与设定位置的距离，对各点云进行权值赋值，确定各栅格对应的坐标；
根据各栅格对应的坐标更新初始的corner地图和corner地图，得到修正后的特征地图。
实际应用中，对特征点云地图基于距离幂次反比法对特征点的距离赋予权值，得到修正后的特征地图，包括：
步骤1，依据得到的三维激光雷达的位姿信息，把激光雷达坐标系下的三维点云坐标转换到世界坐标系下，如图2所示。设三维激光雷达坐标系下三维点坐标为pL＝(xL,yL,zL)，三维激光雷达位姿信息用旋转矩阵TWL表示，则世界坐标系下三维点云坐标pW＝(xW,yW,zW)，满足如下公式：
pW＝TWL·pL
在三维激光雷达坐标系下，计算三维点云中任意一点距离三维激光雷达坐标原点的距离
Figure BDA0002299802280000081
步骤2，把世界坐标下的实时转换点云和局部地图点云划分到三维栅格中。当实时转换的点云为第一帧数据时，初始化点云地图，只需把第一帧转换点云划分到三维栅格中。在后续的三维激光帧中，需把转换点云和局部地图划分到三维栅格中。
针对每一个栅格，统计栅格内点云的个数n，根据激光雷达的测量特性，距离远的点其测量误差相对较大，依据上述点到激光雷达坐标系原点的距离S的计算方法，给每个点赋予相应的权值
Figure BDA0002299802280000082
本次试验p的取值为2。
根据转换后的三维点坐标和其相应的权重，计算每个栅格中此时刻最终点坐标pWC＝(xWC,yWC,zWC)，其满足如下公式：
Figure BDA0002299802280000091
根据最终点坐标pWC＝(xWC,yWC,zWC)，计算其在三维激光雷达坐标系下坐标pLC＝(xLC,yLC,zLC)，其满足如下公式：
Figure BDA0002299802280000092
并通过如下公式计算pLC与此时刻三维激光雷达坐标原点的距离：
Figure BDA0002299802280000093
步骤3，依据得到的每一时刻位姿和其三维点云信息，不断迭代以上两个步骤，可以逐步对点云地图进行更新，得到受点云噪声影响小的完整三维点云地图(即修正后的特征地图)。
如图3所示，在一实施例中，三维激光雷达采用Velodyne公司的VLP-16，地下环境精确定位框架主要分为基于3D激光雷达的离线距离权重点云地图构建和基于距离权重点云地图的实时精确定位。针对离线地图构建，首先基于LOAM生成Cornern地图和Surface地图，同时在生成地图过程中，根据3D激光雷达的数据采集特性，距离远的障碍物返回值较距离近的障碍物返回值偏移距离误差大，采用距离幂次反比法根据每个点的距离赋予不同的权值，进而对生成的特征地图进行修正，得到基于LOAM和距离幂次反比法的特征地图。针对基于距离权重地图的实时精确定位，为保证后续处理效率及定位精度，首先基于聚类方法进行噪声移除，同时采用LOAM中的特征提取方法对实时点云进行特征提取，得到特征点云；其次，基于无损卡尔曼滤波实现地下环境的实时定位。其中，在预测位姿阶段，主要通过点云帧间匹配进行位姿估计得到预测位姿；在修正位姿阶段，通过实时点云与构建的距离权重地图进行点云帧与地图，得到当前的修正位姿，最终输出实时精确位姿，同时该修正位姿也用于下一次循环的初始位姿。
实际应用中，将本发明实施例地下环境定位方法应用于在地下多个场景中进行测试，包括表面光滑直巷道、表面光滑回环巷道、粗糙非直巷道及粗糙斜坡道，通过定位误差及定位时间分析证明该算法实现地下环境的精准定位及算法的鲁棒性，并从定位误差、定位时间等方面进行对比分析，定位误差能控制在4cm左右，定位时间平均控制在60ms左右，结果表明该算法可以实现低漂移的实时定位。
为了实现本发明实施例的方法，本发明实施例还提供一种地下环境定位装置，如图4所示，该装置包括：获取模块401、预测模块402、位姿确定模块403。其中，
获取模块401，用于获取扫描点云数据的特征点云；
预测模块402，用于以上一帧的位姿作为当前的初始位姿，基于当前帧的扫描点云数据的特征点云与上一帧的扫描点云数据的特征点云进行点云帧与帧匹配，确定当前的预测位姿；
位姿确定模块403，用于基于当前帧的扫描点云数据的特征点云、所述当前的预测位姿和特征地图，进行点云帧与地图匹配，得到当前的修正位姿；
其中，所述特征地图是基于LOAM生成的，且所述特征地图的构建过程中，基于点云与扫描设备间的距离对扫描点云数据进行修正。
在一实施例中，地下环境定位装置还包括：地图构建模块404，用于：
基于LOAM对已获取的扫描点云数据进行地图构建，得到初始的特征地图；
对所述初始的特征地图基于距离幂次反比法对特征点的距离赋予权值，得到修正后的特征地图。
在一实施例中，地图构建模块404具体用于：
根据第一频率对已获取的扫描点云数据基于帧间匹配，得到相邻帧的数据间匹配的拼接位姿；
根据第二频率对相邻帧的数据基于相应的拼接位姿拼接，得到初始的角corner地图和初始的面surface地图；
其中，所述第一频率大于所述第二频率。
在一实施例中，地图构建模块404具体用于：
将初始的corner地图和初始的corner地图的三维点云转换至世界坐标系并划分至世界坐标系下的三维栅格中；
针对每个栅格，统计各栅格内点云的个数，并基于各点云与设定位置的距离，对各点云进行权值赋值，确定各栅格对应的坐标；
根据各栅格对应的坐标更新初始的corner地图和corner地图，得到修正后的特征地图。
在一实施例中，预测模块402具体用于：
基于当前帧的扫描点云数据的特征点云与上一帧的扫描点云数据的特征点云进行点云帧与帧匹配，得到当前帧与上一帧间的相对位姿；
基于所述相对位姿和上一帧的位姿得到当前的预测位姿。
在一实施例中，位姿确定模块403具体用于：
以所述当前的预测位姿作为点云帧与地图匹配的初始值，对当前帧的扫描点云数据的特征点云与特征地图进行点云帧与地图匹配得到配准位姿，将所述配准位姿作为当前的修正位姿。
实际应用时，获取模块401、预测模块402、位姿确定模块403及地图构建模块404，可以由地下环境定位装置中的处理器来实现。当然，处理器需要运行存储器中的计算机程序来实现它的功能。
需要说明的是：上述实施例提供的地下环境定位装置在进行地下环境定位时，仅以上述各程序模块的划分进行举例说明，实际应用中，可以根据需要而将上述处理分配由不同的程序模块完成，即将装置的内部结构划分成不同的程序模块，以完成以上描述的全部或者部分处理。另外，上述实施例提供的地下环境定位装置与地下环境定位方法实施例属于同一构思，其具体实现过程详见方法实施例，这里不再赘述。
基于上述程序模块的硬件实现，且为了实现本发明实施例的方法，本发明实施例还提供一种地下环境定位设备。图5仅仅示出了该设备的示例性结构而非全部结构，根据需要可以实施图5示出的部分结构或全部结构。
如图5所示，本发明实施例提供的地下环境定位设备500包括：至少一个处理器501、存储器502、用户接口503和至少一个网络接口504。地下环境定位设备500中的各个组件通过总线***505耦合在一起。可以理解，总线***505用于实现这些组件之间的连接通信。总线***505除包括数据总线之外，还包括电源总线、控制总线和状态信号总线。但是为了清楚说明起见，在图5中将各种总线都标为总线***505。
其中，用户接口503可以包括显示器、键盘、鼠标、轨迹球、点击轮、按键、按钮、触感板或者触摸屏等。
本发明实施例中的存储器502用于存储各种类型的数据以支持地下环境定位设备的操作。这些数据的示例包括：用于在地下环境定位设备上操作的任何计算机程序。
本发明实施例揭示的地下环境定位方法可以应用于处理器501中，或者由处理器501实现。处理器501可能是一种集成电路芯片，具有信号的处理能力。在实现过程中，地下环境定位方法的各步骤可以通过处理器501中的硬件的集成逻辑电路或者软件形式的指令完成。上述的处理器501可以是通用处理器、数字信号处理器(DSP，Digital SignalProcessor)，或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。处理器501可以实现或者执行本发明实施例中的公开的各方法、步骤及逻辑框图。通用处理器可以是微处理器或者任何常规的处理器等。结合本发明实施例所公开的方法的步骤，可以直接体现为硬件译码处理器执行完成，或者用译码处理器中的硬件及软件模块组合执行完成。软件模块可以位于存储介质中，该存储介质位于存储器502，处理器501读取存储器502中的信息，结合其硬件完成本发明实施例提供的地下环境定位方法的步骤。
在示例性实施例中，地下环境定位设备可以被一个或多个应用专用集成电路(ASIC，Application Specific Integrated Circuit)、DSP、可编程逻辑器件(PLD，Programmable Logic Device)、复杂可编程逻辑器件(CPLD，Complex Programmable LogicDevice)、FPGA、通用处理器、控制器、微控制器(MCU，Micro Controller Unit)、微处理器(Microprocessor)、或者其他电子元件实现，用于执行前述方法。
在一应用示例中，地下环境定位设备包括16线3D激光雷达、移动电源及笔记本电脑，该移动电源供电给16线3D激光雷达和笔记本电脑，笔记本电脑与16线3D激光雷达通信连接，笔记本电脑运行计算机程序时，实现前述任一实施例所述的地下环境定位方法。
可以理解，存储器502可以是易失性存储器或非易失性存储器，也可包括易失性和非易失性存储器两者。其中，非易失性存储器可以是只读存储器(ROM，Read Only Memory)、可编程只读存储器(PROM，Programmable Read-Only Memory)、可擦除可编程只读存储器(EPROM，Erasable Programmable Read-Only Memory)、电可擦除可编程只读存储器(EEPROM，Electrically Erasable Programmable Read-Only Memory)、磁性随机存取存储器(FRAM，ferromagnetic random access memory)、快闪存储器(Flash Memory)、磁表面存储器、光盘、或只读光盘(CD-ROM，Compact Disc Read-Only Memory)；磁表面存储器可以是磁盘存储器或磁带存储器。易失性存储器可以是随机存取存储器(RAM，Random AccessMemory)，其用作外部高速缓存。通过示例性但不是限制性说明，许多形式的RAM可用，例如静态随机存取存储器(SRAM，Static Random Access Memory)、同步静态随机存取存储器(SSRAM，Synchronous Static Random Access Memory)、动态随机存取存储器(DRAM，Dynamic Random Access Memory)、同步动态随机存取存储器(SDRAM，SynchronousDynamic Random Access Memory)、双倍数据速率同步动态随机存取存储器(DDRSDRAM，Double Data Rate Synchronous Dynamic Random Access Memory)、增强型同步动态随机存取存储器(ESDRAM，Enhanced Synchronous Dynamic Random Access Memory)、同步连接动态随机存取存储器(SLDRAM，SyncLink Dynamic Random Access Memory)、直接内存总线随机存取存储器(DRRAM，Direct Rambus Random Access Memory)。本发明实施例描述的存储器旨在包括但不限于这些和任意其它适合类型的存储器。
在示例性实施例中，本发明实施例还提供了一种存储介质，即计算机存储介质，具体可以是计算机可读存储介质，例如包括存储计算机程序的存储器502，上述计算机程序可由地下环境定位设备的处理器501执行，以完成本发明实施例方法所述的步骤。计算机可读存储介质可以是ROM、PROM、EPROM、EEPROM、Flash Memory、磁表面存储器、光盘、或CD-ROM等存储器。
需要说明的是：“第一”、“第二”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。
另外，本发明实施例所记载的技术方案之间，在不冲突的情况下，可以任意组合。
以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以权利要求的保护范围为准。

Claims (6)

1.一种地下环境定位方法，其特征在于，包括：
获取扫描点云数据的特征点云；
以上一帧的位姿作为当前的初始位姿，基于当前帧的扫描点云数据的特征点云与上一帧的扫描点云数据的特征点云进行点云帧与帧匹配，确定当前的预测位姿；
基于当前帧的扫描点云数据的特征点云、所述当前的预测位姿和特征地图，进行点云帧与地图匹配，得到当前的修正位姿；
其中，所述特征地图是基于实时的激光里程计及建图LOAM生成的，且所述特征地图的构建过程中，基于点云与扫描设备间的距离对扫描点云数据进行修正；
所述方法还包括：
基于LOAM对已获取的扫描点云数据进行地图构建，得到初始的特征地图；
对所述初始的特征地图基于距离幂次反比法对特征点的距离赋予权值，得到修正后的特征地图；
所述基于LOAM对已获取的扫描点云数据进行地图构建，得到初始的特征地图，包括：
根据第一频率对已获取的扫描点云数据基于帧间匹配，得到相邻帧的数据间匹配的拼接位姿；
根据第二频率对相邻帧的数据基于相应的拼接位姿拼接，得到初始的角corner地图和初始的面surface地图；
其中，所述第一频率大于所述第二频率；
所述对所述初始的特征地图基于距离幂次反比法对特征点的距离赋予权值，得到修正后的特征地图，包括：
将初始的corner地图和初始的corner地图的三维点云转换至世界坐标系并划分至世界坐标系下的三维栅格中；
针对每个栅格，统计各栅格内点云的个数，并基于各点云与设定位置的距离，对各点云进行权值赋值，确定各栅格对应的坐标；
根据各栅格对应的坐标更新初始的corner地图和corner地图，得到修正后的特征地图。
2.根据权利要求1所述的方法，其特征在于，所述基于当前帧的扫描点云数据的特征点云与上一帧的扫描点云数据的特征点云进行点云帧与帧匹配，确定当前的预测位姿，包括：
基于当前帧的扫描点云数据的特征点云与上一帧的扫描点云数据的特征点云进行点云帧与帧匹配，得到当前帧与上一帧间的相对位姿；
基于所述相对位姿和所述当前的初始位姿得到当前的预测位姿。
3.根据权利要求1所述的方法，其特征在于，所述基于当前帧的扫描点云数据的特征点云、所述当前的预测位姿和特征地图，进行点云帧与地图匹配，得到当前的修正位姿，包括：
以所述当前的预测位姿作为点云帧与地图匹配的初始值，对当前帧的扫描点云数据的特征点云与特征地图进行点云帧与地图匹配得到配准位姿，将所述配准位姿作为当前的修正位姿。
4.一种地下环境定位装置，其特征在于，包括：
获取模块，用于获取扫描点云数据的特征点云；
预测模块，用于以上一帧的位姿作为当前的初始位姿，基于当前帧的扫描点云数据的特征点云与上一帧的扫描点云数据的特征点云进行点云帧与帧匹配，确定当前的预测位姿；
位姿确定模块，用于基于当前帧的扫描点云数据的特征点云、所述当前的预测位姿和特征地图，进行点云帧与地图匹配，得到当前的修正位姿；
其中，所述特征地图是基于LOAM生成的，且所述特征地图的构建过程中，基于点云与扫描设备间的距离对扫描点云数据进行修正；
地图构建模块，用于：基于LOAM对已获取的扫描点云数据进行地图构建，得到初始的特征地图；对所述初始的特征地图基于距离幂次反比法对特征点的距离赋予权值，得到修正后的特征地图；
所述基于LOAM对已获取的扫描点云数据进行地图构建，得到初始的特征地图，包括：
根据第一频率对已获取的扫描点云数据基于帧间匹配，得到相邻帧的数据间匹配的拼接位姿；
根据第二频率对相邻帧的数据基于相应的拼接位姿拼接，得到初始的角corner地图和初始的面surface地图；
其中，所述第一频率大于所述第二频率；
所述对所述初始的特征地图基于距离幂次反比法对特征点的距离赋予权值，得到修正后的特征地图，包括：
将初始的corner地图和初始的corner地图的三维点云转换至世界坐标系并划分至世界坐标系下的三维栅格中；
针对每个栅格，统计各栅格内点云的个数，并基于各点云与设定位置的距离，对各点云进行权值赋值，确定各栅格对应的坐标；
根据各栅格对应的坐标更新初始的corner地图和corner地图，得到修正后的特征地图。
5.一种地下环境定位设备，其特征在于，包括：处理器和用于存储能够在处理器上运行的计算机程序的存储器，其中，
所述处理器，用于运行计算机程序时，执行权利要求1至3任一项所述方法的步骤。
6.一种存储介质，所述存储介质上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时，实现权利要求1至3任一项所述方法的步骤。
CN201911217135.2A 2019-12-03 2019-12-03 地下环境定位方法、装置、设备及存储介质 Active CN110849374B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201911217135.2A CN110849374B (zh) 2019-12-03 2019-12-03 地下环境定位方法、装置、设备及存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201911217135.2A CN110849374B (zh) 2019-12-03 2019-12-03 地下环境定位方法、装置、设备及存储介质

Publications (2)

Publication Number Publication Date
CN110849374A CN110849374A (zh) 2020-02-28
CN110849374B true CN110849374B (zh) 2023-04-18

Family

ID=69607168

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201911217135.2A Active CN110849374B (zh) 2019-12-03 2019-12-03 地下环境定位方法、装置、设备及存储介质

Country Status (1)

Country Link
CN (1) CN110849374B (zh)

Families Citing this family (28)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111442722B (zh) * 2020-03-26 2022-05-17 达闼机器人股份有限公司 定位方法、装置、存储介质及电子设备
CN111461981B (zh) * 2020-03-30 2023-09-01 北京百度网讯科技有限公司 点云拼接算法的误差估计方法和装置
CN111461980B (zh) * 2020-03-30 2023-08-29 北京百度网讯科技有限公司 点云拼接算法的性能估计方法和装置
CN111461982B (zh) * 2020-03-30 2023-09-22 北京百度网讯科技有限公司 用于拼接点云的方法和装置
CN111735451B (zh) * 2020-04-16 2022-06-07 中国北方车辆研究所 一种基于多源先验信息的点云匹配高精度定位方法
WO2021212477A1 (zh) * 2020-04-24 2021-10-28 华为技术有限公司 校正点云数据的方法和相关装置
CN114026461A (zh) * 2020-05-19 2022-02-08 深圳市大疆创新科技有限公司 构建点云帧的方法、目标检测方法、测距装置、可移动平台和存储介质
WO2021253068A1 (en) * 2020-06-18 2021-12-23 Commonwealth Scientific And Industrial Research Organisation Navigation of an underground mining machine
CN111812669B (zh) * 2020-07-16 2023-08-04 南京航空航天大学 绕机检查装置及其定位方法、存储介质
CN114814872A (zh) * 2020-08-17 2022-07-29 浙江商汤科技开发有限公司 位姿确定方法及装置、电子设备和存储介质
CN112130168B (zh) * 2020-09-11 2023-04-14 北京埃福瑞科技有限公司 一种用于折返控制的列车位置状态检测方法及***
CN112101229B (zh) * 2020-09-16 2023-02-24 云南师范大学 点云数据特征点提取方法、装置、计算机设备及存储介质
CN114526720B (zh) * 2020-11-02 2024-04-16 北京四维图新科技股份有限公司 定位的处理方法、装置、设备及存储介质
CN114088093B (zh) * 2020-11-09 2024-07-16 北京京东乾石科技有限公司 一种点云地图构建方法、装置、***及存储介质
CN113761090B (zh) * 2020-11-17 2024-04-05 北京京东乾石科技有限公司 一种基于点云地图的定位方法和装置
CN112700495A (zh) * 2020-11-25 2021-04-23 北京旷视机器人技术有限公司 位姿确定方法、装置、机器人、电子设备及存储介质
CN112630787B (zh) * 2020-12-03 2022-05-17 深圳市优必选科技股份有限公司 定位方法、装置、电子设备及可读存储介质
CN112683268A (zh) * 2020-12-08 2021-04-20 中国铁建重工集团股份有限公司 一种基于扩展卡尔曼滤波的巷道实时定位导航方法及***
CN112630745A (zh) * 2020-12-24 2021-04-09 深圳市大道智创科技有限公司 一种基于激光雷达的环境建图方法和装置
CN113155126B (zh) * 2021-01-04 2023-10-20 航天时代飞鸿技术有限公司 一种基于视觉导航的多机协同目标高精度定位***及方法
CN112883134A (zh) * 2021-02-01 2021-06-01 上海三一重机股份有限公司 数据融合建图方法、装置、电子设备及存储介质
CN112862894B (zh) * 2021-04-12 2022-09-06 中国科学技术大学 一种机器人三维点云地图构建与扩充方法
CN113219486B (zh) * 2021-04-23 2023-03-31 北京云迹科技股份有限公司 一种定位方法及装置
CN113310484B (zh) * 2021-05-28 2022-06-24 杭州艾米机器人有限公司 一种移动机器人定位方法和***
CN113503883B (zh) * 2021-06-22 2022-07-19 北京三快在线科技有限公司 采集用于构建地图的数据的方法、存储介质及电子设备
CN114820749A (zh) * 2022-04-27 2022-07-29 西安优迈智慧矿山研究院有限公司 无人车井下定位方法、***、设备及介质
CN115661798B (zh) * 2022-12-23 2023-03-21 小米汽车科技有限公司 确定目标区域的方法、装置、车辆及存储介质
CN117452429B (zh) * 2023-12-21 2024-03-01 江苏中科重德智能科技有限公司 基于多线激光雷达的机器人定位方法及***

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108225341A (zh) * 2016-12-14 2018-06-29 乐视汽车（北京）有限公司 车辆定位方法
CN109507677A (zh) * 2018-11-05 2019-03-22 浙江工业大学 一种结合gps和雷达里程计的slam方法
CN109682385A (zh) * 2018-11-05 2019-04-26 天津大学 一种基于orb特征的即时定位与地图构建的方法
CN109813310A (zh) * 2019-03-11 2019-05-28 中南大学 地下作业设备定位方法、装置、***及存储介质
CN109839112A (zh) * 2019-03-11 2019-06-04 中南大学 地下作业设备定位方法、装置、***及存储介质
CN109945856A (zh) * 2019-02-18 2019-06-28 天津大学 基于惯性/雷达的无人机自主定位与建图方法

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP3078935A1 (en) * 2015-04-10 2016-10-12 The European Atomic Energy Community (EURATOM), represented by the European Commission Method and device for real-time mapping and localization

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108225341A (zh) * 2016-12-14 2018-06-29 乐视汽车（北京）有限公司 车辆定位方法
CN109507677A (zh) * 2018-11-05 2019-03-22 浙江工业大学 一种结合gps和雷达里程计的slam方法
CN109682385A (zh) * 2018-11-05 2019-04-26 天津大学 一种基于orb特征的即时定位与地图构建的方法
CN109945856A (zh) * 2019-02-18 2019-06-28 天津大学 基于惯性/雷达的无人机自主定位与建图方法
CN109813310A (zh) * 2019-03-11 2019-05-28 中南大学 地下作业设备定位方法、装置、***及存储介质
CN109839112A (zh) * 2019-03-11 2019-06-04 中南大学 地下作业设备定位方法、装置、***及存储介质

Also Published As

Publication number Publication date
CN110849374A (zh) 2020-02-28

Similar Documents

Publication Publication Date Title
CN110849374B (zh) 地下环境定位方法、装置、设备及存储介质
CN112347840B (zh) 视觉传感器激光雷达融合无人机定位与建图装置和方法
US11295472B2 (en) Positioning method, positioning apparatus, positioning system, storage medium, and method for constructing offline map database
US10571270B2 (en) Fusion of sensor and map data using constraint based optimization
Badino et al. Visual topometric localization
Rosinol et al. Incremental visual-inertial 3d mesh generation with structural regularities
CN111402339B (zh) 一种实时定位方法、装置、***及存储介质
CN109813310B (zh) 地下作业设备定位方法、装置、***及存储介质
CN104833354A (zh) 一种多基多模组网融合室内人员导航定位***及其实施方法
Ibrahim et al. Inertial measurement unit based indoor localization for construction applications
US20200100066A1 (en) System and Method for Generating Floor Plans Using User Device Sensors
Atia et al. Particle‐Filter‐Based WiFi‐Aided Reduced Inertial Sensors Navigation System for Indoor and GPS‐Denied Environments
Niu et al. An online solution of LiDAR scan matching aided inertial navigation system for indoor mobile mapping
CN109839112B (zh) 地下作业设备定位方法、装置、***及存储介质
CN111665512A (zh) 基于3d激光雷达和惯性测量单元的融合的测距和绘图
CN114119886A (zh) 高精地图点云重建方法、装置、车辆、设备和存储介质
Jia et al. A cross-correction LiDAR SLAM method for high-accuracy 2D mapping of problematic scenario
CN111679303A (zh) 一种多源定位信息融合的综合定位方法及装置
US20220341752A1 (en) Alignment Of Map Segments
Khoshelham et al. Vehicle positioning in the absence of GNSS signals: Potential of visual-inertial odometry
CN111678513A (zh) 一种超宽带/惯导紧耦合的室内定位装置与***
Shu et al. 3D point cloud-based indoor mobile robot in 6-DoF pose localization using a Wi-Fi-aided localization system
CN111578939B (zh) 考虑采样周期随机变化的机器人紧组合导航方法及***
CA2894863A1 (en) Indoor localization using crowdsourced data
CN115984463A (zh) 一种适用于狭窄巷道的三维重建方法和***

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant