CN111383324B - 点云地图的构建方法、装置、计算机设备和存储介质 - Google Patents
点云地图的构建方法、装置、计算机设备和存储介质 Download PDFInfo
- Publication number
- CN111383324B CN111383324B CN201811642897.2A CN201811642897A CN111383324B CN 111383324 B CN111383324 B CN 111383324B CN 201811642897 A CN201811642897 A CN 201811642897A CN 111383324 B CN111383324 B CN 111383324B
- Authority
- CN
- China
- Prior art keywords
- point cloud
- state
- cloud data
- vehicle body
- optimal
- 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
Links
- 238000010276 construction Methods 0.000 title claims abstract description 32
- 238000000034 method Methods 0.000 claims abstract description 76
- 238000007499 fusion processing Methods 0.000 claims abstract description 24
- 230000006870 function Effects 0.000 claims description 112
- 238000005457 optimization Methods 0.000 claims description 14
- 238000004590 computer program Methods 0.000 claims description 13
- 230000001133 acceleration Effects 0.000 claims description 12
- 238000010586 diagram Methods 0.000 description 11
- 238000004422 calculation algorithm Methods 0.000 description 7
- 230000008569 process Effects 0.000 description 7
- 238000007500 overflow downdraw method Methods 0.000 description 6
- 238000013480 data collection Methods 0.000 description 4
- 238000004364 calculation method Methods 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 238000012804 iterative process Methods 0.000 description 2
- 238000005259 measurement Methods 0.000 description 2
- 238000012545 processing Methods 0.000 description 2
- 230000001360 synchronised effect Effects 0.000 description 2
- 238000009825 accumulation Methods 0.000 description 1
- 238000013528 artificial neural network Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 238000002939 conjugate gradient method Methods 0.000 description 1
- 238000001914 filtration Methods 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 238000011478 gradient descent method Methods 0.000 description 1
- 239000004973 liquid crystal related substance Substances 0.000 description 1
- 239000011159 matrix material Substances 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 238000013519 translation Methods 0.000 description 1
Images
Classifications
-
- 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
-
- 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
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/88—Lidar systems specially adapted for specific applications
- G01S17/93—Lidar systems specially adapted for specific applications for anti-collision purposes
- G01S17/931—Lidar systems specially adapted for specific applications for anti-collision purposes of land vehicles
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- General Physics & Mathematics (AREA)
- Electromagnetism (AREA)
- Computer Networks & Wireless Communication (AREA)
- Computer Graphics (AREA)
- Geometry (AREA)
- Software Systems (AREA)
- Theoretical Computer Science (AREA)
- Navigation (AREA)
- Traffic Control Systems (AREA)
Abstract
本申请涉及一种点云地图的构建方法、装置、计算机设备和存储介质。所述方法包括：获取点云数据及对应的车体传感器数据；对所述车体传感器数据进行融合处理，得到所述车体传感器数据的最优估计状态及对应的不确定度信息；根据所述最优估计状态及对应的不确定度信息构建所述点云数据对应的点云地图。采用本方法能够对所有点云数据的状态根据不确定度信息进行整体优化，降低所有状态有不确定度，尤其是长时间缺乏位置观测场景(如隧道场景)的状态的不确定度，提高整体定位精度；显著降低由于长时间缺乏位置观测信息，定位精度不够引起的高精地图构建误差。
Description
技术领域
本申请涉及计算机技术领域，特别是涉及一种点云地图的构建方法、装置、计算机设备和存储介质。
背景技术
高精地图技术可以应用于移动机器人、自动驾驶领域中，如配合激光雷达(LiDAR)、相机等传感器可实现移动机器人的厘米级定位。
现有的构建高精地图的方法主要基于非线性优化方法，通过传感器的状态构建代价函数，然后优化该代价函数得到高精地图，但是，该方法在长时间缺乏位置传感器观测及特征不明显的场景，如在较长的隧道，由于初始值不确定度过大，导致非线性优化容易收敛到错误的局部解，从而使得构建的高精地图存在较大误差。
发明内容
基于此，有必要针对上述技术问题，提供一种能够降低地图误差的点云地图的构建方法、装置、计算机设备和存储介质。
一种点云地图的构建方法，所述方法包括：
获取点云数据及对应的车体传感器数据；
对所述车体传感器数据进行融合处理，得到所述车体传感器数据的最优估计状态及对应的不确定度信息；
根据所述最优估计状态及对应的不确定度信息构建所述点云数据对应的点云地图。
在其中一个实施例中，所述对所述车体传感器数据进行融合处理，得到所述车体传感器数据的最优估计状态及对应的不确定度信息，包括：
通过贝叶斯估计方法，得到所述车体传感器数据的最优估计状态；
根据所述车体传感器数据建立不确定度模型；
根据所述不确定度模型，采用贝叶斯估计方法，获得所述最优估计状态对应的不确定度信息。
在其中一个实施例中，所述根据所述最优估计状态及对应的不确定度信息构建所述点云数据对应的点云地图，包括：
采用最优估计状态及对应的不确定度信息以及点云数据间的约束关系构建代价函数；
采用非线性优化方法优化所述代价函数，得到点云数据的最优状态；
根据所述点云数据的最优状态调整点云数据，构建点云地图。
在其中一个实施例中，所述采用最优估计状态及对应的不确定度信息以及点云数据间的约束关系构建代价函数，包括：
将所述最优估计状态作为点云数据间匹配的初始值，利用点云数据间的约束关系和所述最优状态及对应的不确定度信息构建代价函数。
在其中一个实施例中，所述将所述最优估计状态作为点云数据间匹配的初始值，利用点云数据间的约束关系和所述最优状态及对应的不确定度信息构建代价函数，包括：
将点云数据对应的最优估计状态作为初始值，根据待优化状态、所述点云数据对应的最优估计状态和对应的不确定度信息构建第一代价函数；
利用点云数据间的约束关系，通过相邻的两帧点云数据的待优化状态及对应的不确定度信息构建第二代价函数；
将所述第一代价函数及第二代价函数累加，得到最终代价函数。
在其中一个实施例中，所述车体传感器数据包括车体位置、车体速度、车体加速度、车体角速度和车体前进方向速度；所述最优估计状态包括最优估计位置及最优估计姿态；最优状态包括最优位置及最优姿态；待优化状态包括待优化位置及待优化姿态。
一种点云地图的构建装置，所述装置包括：
数据获取模块，用于获取点云数据及对应的车体传感器数据；
融合处理模块，用于对所述车体传感器数据进行融合处理，得到所述车体传感器数据的最优估计状态及对应的不确定度信息；
构建模块，用于根据所述最优估计状态及对应的不确定度信息构建所述点云数据对应的点云地图。
一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，所述处理器执行所述计算机程序时实现以下步骤：
获取点云数据及对应的车体传感器数据；
对所述车体传感器数据进行融合处理，得到所述车体传感器数据的最优估计状态及对应的不确定度信息；
根据所述最优估计状态及对应的不确定度信息构建所述点云数据对应的点云地图。
一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现以下步骤：
获取点云数据及对应的车体传感器数据；
对所述车体传感器数据进行融合处理，得到所述车体传感器数据的最优估计状态及对应的不确定度信息；
根据所述最优估计状态及对应的不确定度信息构建所述点云数据对应的点云地图。
上述点云地图的构建方法、装置、计算机设备和存储介质，通过获取点云数据及对应的车体传感器数据；对车体传感器数据进行融合处理，得到车体传感器数据的最优估计状态及对应的不确定度信息；根据最优估计状态及对应的不确定度信息构建点云数据对应的点云地图。对所有点云数据的状态根据不确定度信息进行整体优化，降低所有状态有不确定度，尤其是长时间缺乏位置观测场景(如隧道场景)的状态的不确定度，提高整体定位精度；显著降低由于长时间缺乏位置观测信息，定位精度不够引起的高精地图构建误差。
附图说明
图1是一个实施例的一种点云地图的构建方法的流程示意图；
图2是一个实施例的一种获得不确定度信息步骤的流程示意图；
图3是一个实施例的一种构建点云地图步骤的流程示意图；
图4是一个实施例的一种获得代价函数步骤的流程示意图；
图5是一个实施例的一种点云地图的构建装置的结构框图；
图6是一个实施例的一种融合处理模块的结构框图；
图7是一个实施例的一种构建模块的结构框图；
图8是一个实施例的一种函数构建子模块的结构框图；
图9是一个实施例的一种计算机设备的内部结构图。
具体实施方式
为了使本申请的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本申请进行进一步详细说明。应当理解，此处描述的具体实施例仅仅用以解释本申请，并不用于限定本申请。
在一个实施例中，如图1所示，提供了一种点云地图的构建方法，包括以下步骤：
步骤S201，获取点云数据及对应的车体传感器数据；
本实施例可应用于终端或者服务器中，本实施例对此不作限制，其中，终端可以是个人计算机、笔记本电脑、智能手机、平板电脑等，服务器可以用独立的服务器或者是多个服务器组成的服务器集群来实现；该方法可应用于自动驾驶或移动机器人的定位场景中。
本实施例中，可以首先获取到点云数据及对应的车体传感器数据；在一种具体示例中，可以获取到点云数据采集车辆在行进过程中的点云数据及对应的车体传感器数据；进一步地，该点云数据采集车辆可以采集在隧道、矿道等长时间无GPS(GlobalPositioning System，全球定位系统)位置观测的环境中的点云数据及对应的车体传感器数据。
从硬件的角度而言，该点云数据采集车辆可以包括激光雷达及多种传感器，该传感器可以包括GPS单元、惯性测量单元(IMU，Inertial Measurement Unit)及轮速计等，本实施例对此不作限制。
具体而言，该激光雷达可以用于采集点云数据，点云数据可以包括空间三维坐标信息及反射强度信息，当点云数据采集车辆通过彩色图像采集单元采集颜色信息时，该点云数据还可以包括颜色信息，点云数据可以通过激光雷达等设备获得，以PCD(PointCloud，点云)文件的格式进行储存。
另一方面，该GPS单元可以用于采集车体位置及车体速度，具体地，该GPS单元是指全球定位系统中的用户设备部分，即GPS信号接收机，其主要功能是捕获到按一定卫星截止角所选择的待测卫星的卫星信号，并跟踪该待测卫星的运行。当GPS信号接收机捕获到跟踪的卫星信号后，即可测量出接收天线至卫星的伪距离和距离的变化率，解调出卫星轨道参数等数据。根据上述的数据，GPS信号接收机中的微处理器就可按定位解算方法进行定位计算，计算出用户所在地理位置的位置、速度、时间等信息；其中，该位置可以包括经纬度、高度；本实施例中的GPS单元可以安装于点云数据采集车辆中，用于采集车体位置及车体速度，该车体位置可以包括经纬度、高度。
进一步地，IMU可以用于采集车体加速度和车体角速度，IMU是测量物体的角速度以及加速度的装置。一般的，一个IMU可以包含三个单轴的加速度计和三个单轴的陀螺，加速度计检测物体在载体坐标系统独立三轴的加速度信号，而陀螺检测车体相对于导航坐标系的角速度信号，测量车体在三维空间中的角速度和加速度。本实施例中的IMU可以安装于点云数据采集车辆中，用于采集车体加速度和车体角速度。
具体应用中，该轮速计可以用于采集轮速，即车体前进方向速度；轮速计是用于测量汽车车轮转速的传感器，其种类主要包括磁电式轮速传感器、霍尔式轮速传感器。
举例而言，当点云数据采集车辆在一段较长隧道中行进时(如在3km的隧道中)，通过激光雷达、GPS单元、惯性测量单元及轮速计，分别采集多帧点云数据及其对应的车体传感器数据，如车体位置、车体速度、车体加速度、车体角速度、车体前进方向速度等。
步骤S202，对车体传感器数据进行融合处理，得到车体传感器数据的最优估计状态及对应的不确定度信息；
进一步应用到本实施例中，针对车体传感器数据进行融合处理，得到该车体传感器数据的最优估计状态及对应的不确定度信息。
针对车体传感器数据的融合处理的具体方式，可以通过多传感器数据融合方法进行融合处理，该多传感器数据融合方法因信息融合的不同层次对应不同的算法，其可以包括加权平均融合法、卡尔曼滤波法、贝叶斯估计方法、概率论方法、模糊逻辑推理法、人工神经网络法、D-S证据理论法等，本实施例对此不作限制。
具体地，将该车体传感器数据输入至多传感器数据融合方法对应的算法模型中，得到输出的车体传感器数据的最优估计状态；举例而言，将车体传感器数据作为贝叶斯估计方法对应的算法模型的输入，得到输出的最优估计状态，即将上述的车体位置、车体速度、车体加速度、车体角速度、车体前进方向速度等输入至贝叶斯估计方法对应的算法模型，获得输出的最优估计状态；
或者，将车体传感器数据作为卡尔曼滤波对应的算法模型的输入，得到输出的最优估计状态，即将上述的车体位置、车体速度、车体加速度、车体角速度、车体前进方向速度等输入至所卡尔曼滤波对应的算法模型，获得输出的最优估计状态。
进一步地，针对不确定度信息的获得方式，首先可以根据建立车体传感器数据对应的不确定度模型，将该不确定度模型与贝叶斯估计方法结合，计算出该车体传感器数据的不确定度信息。
需要说明的是，上述的进行融合处理及获得不确定度信息的步骤可以以离线的方式在硬件设备端执行，提高数据处理效率，节省数据处理资源。
该最优估计状态可以包括最优估计位置、最优估计姿态及最优估计速度等，因该车体传感器数据与最优估计状态具有对应的关系，而该车体传感器数据与点云数据具有对应关系，即该最优估计状态可以为点云数据对应的最优估计状态。
需要说明的是，该融合处理所需要的车体传感器数据可以包括所有时刻的全部传感器采集得到的车体传感器数据，举例而言，所有时刻可以是指点云数据采集车辆的开始采集时间点及结束采集时间点中间的所有时刻，例如，开始采集时间点为时刻1，结束采集时间点为时刻T，则采集时刻1至时刻T中所有时刻的车体传感器数据，将其进行融合处理，得到最优估计状态及对应的不确定度信息。
步骤S203，根据最优估计状态及对应的不确定度信息构建点云数据对应的点云地图。
实际应用到本实施例中，在获得最优估计状态及对应的不确定度信息后，采用该最优估计状态及对应的不确定度信息构建点云数据对应的点云地图。
具体而言，采用最优估计状态及不确定度信息构建代价函数；优化代价函数获得点云数据的最优状态，根据最优状态调整点云数据，构建得到点云地图。
在构建代价函数后，可以采用非线性优化方法针对代价函数进行优化，得到点云数据的最优状态；需要说明的是，该非线性优化方法可以包括梯度下降法(GradientDescent)、牛顿法(Newton's method)、拟牛顿法(Quasi-Newton Methods)、高斯牛顿法(Gauss-Newton)、共轭梯度法(Conjugate Gradient)等，本实施例对此不作限制。
优化代价函数的过程是一个迭代的过程，将代价函数进行迭代，直到函数收敛，得到点云数据的最优状态(可以包括最优位置及最优姿态)，完成连续的多帧点云数据构建点云地图的操作，在针对代价函数进行迭代的过程，实际是不断调整点云数据的状态(包括位置及姿态)的过程，最终得到点云数据的最优状态，根据最优状态调整点云数据，构建得到点云地图。经整体优化后，构建准确的高精地图，如在应用于隧道的高精地图构建时，可以明显提高隧道地图的构建精度。
根据本实施例提供的点云地图的构建方法，获取点云数据及对应的车体传感器数据；对车体传感器数据进行融合处理，得到车体传感器数据的最优估计状态及对应的不确定度信息；根据最优估计状态及对应的不确定度信息构建点云数据对应的点云地图。对所有点云数据的状态根据不确定度信息进行整体优化，降低所有状态有不确定度，尤其是长时间缺乏位置观测场景(如隧道场景)的状态的不确定度，提高整体定位精度；显著降低由于长时间缺乏位置观测信息，定位精度不够引起的高精地图构建误差。
在另一个实施例中，参照图2，示出了本实施例的一种获得不确定度信息步骤的流程示意图，对车体传感器数据进行融合处理，得到车体传感器数据的最优估计状态及对应的不确定度信息，包括以下子步骤：
子步骤S11，通过贝叶斯估计方法，得到车体传感器数据的最优估计状态；
子步骤S12，根据车体传感器数据建立不确定度模型；
子步骤S13，根据不确定度模型，采用贝叶斯估计方法，获得最优估计状态对应的不确定度信息。
本实施例中，可以首先通过贝叶斯估计方法，得到车体传感器数据的最优估计状态，即通过多传感器数据融合方法中的贝叶斯估计方法将车体传感器数据进行融合处理，得到车体传感器数据；
具体如下，通过贝叶斯估计方法，利用全部的采集得到的车体传感器数据，得到车体在每个时刻状态的最优估计状态，贝叶斯估计方法对应的算法模型如下：
p(zk|y1:T)
其中，zk表示k时刻的最优估计状态(包括最优估计位置、最优估计速度、最优估计姿态等)，y1:T表示时刻1到时刻T所有时刻采集的所有的车体传感器数据，则时刻1到时刻T所在的时间段表示采集数据的最大时间长度。通过上述的模型可获得车体位置，姿态等的最优估计状态。
进一步地，还可以根据车体传感器数据建立不确定度模型，该不确定度模型可以为传感器噪声模型，IMU与GPS的传感器噪声模型由厂商或技术文档提供，轮速计通过采集的数据辨识该噪声模型，本实施例对此不作限制。
实际应用中，还可以根据不确定度模型，采用贝叶斯估计方法，获得最优估计状态对应的不确定度信息。
具体地，可以提取出协方差矩阵中的不确定度信息，将该不确定度信息作为最优估计状态对应的不确定度信息。
在另一个实施例中，参照图3，示出了本实施例的一种构建点云地图步骤的流程示意图，根据最优估计状态及对应的不确定度信息构建点云数据对应的点云地图，包括以下子步骤：
子步骤S21，采用最优估计状态及对应的不确定度信息以及点云数据间的约束关系构建代价函数；
子步骤S22，采用非线性优化方法优化代价函数，得到点云数据的最优状态；
子步骤S23，根据点云数据的最优状态调整点云数据，构建点云地图。
实际应用到本实施例中，首先采用最优估计状态及对应的不确定度信息以及点云数据间的约束关系构建代价函数；再通过非线性优化方法优化代价函数，得到点云数据的最优状态；而后根据点云数据的最优状态调整点云数据，构建点云地图。
在构建代价函数后，可以采用非线性优化方法中的高斯牛顿法针对代价函数进行优化，得到点云数据的最优状态。
即通过高斯牛顿法将代价函数进行迭代，直到函数收敛，得到点云数据的最优状态，该最优状态可以包括最优位置及最优姿态，完成点云数据构建点云地图的操作，在针对代价函数进行迭代的过程后，最终得到点云数据的最优状态，根据最优状态调整点云数据，构建得到点云地图。经整体优化后，构建准确的高精地图，如在应用于长时间缺乏位置观测场景的高精地图的构建时，可以明显提高相应场景的地图的构建精度。
在另一个实施例中，采用最优估计状态及对应的不确定度信息以及点云数据间的约束关系构建代价函数，包括：将最优估计状态作为点云数据间匹配的初始值，利用点云数据间的约束关系和最优状态及对应的不确定度信息构建代价函数。
具体而言，可以将最优估计状态作为点云数据间匹配的初始值，利用点云数据间的约束关系和最优状态及对应的不确定度信息构建代价函数；即将上述的zk作为点云数据间匹配的初始值，利用点云数据间的约束关系和最优状态及对应的不确定度信息构建代价函数。
在另一个实施例中，参照图4，示出了本实施例的一种获得代价函数步骤的流程示意图，将最优估计状态作为点云数据间匹配的初始值，利用点云数据间的约束关系和最优状态及对应的不确定度信息构建代价函数，包括以下子步骤：
子步骤S31，将点云数据对应的最优估计状态作为初始值，根据待优化状态、点云数据对应的最优估计状态和对应的不确定度信息构建第一代价函数；
子步骤S32，利用点云数据间的约束关系，通过相邻的两帧点云数据的待优化状态及对应的不确定度信息构建第二代价函数；
子步骤S33，将第一代价函数及第二代价函数累加，得到最终代价函数。
具体应用中，首先可以构建第一代价函数，将点云数据对应的最优估计状态作为初始值，根据待优化状态、点云数据对应的最优估计状态和对应的不确定度信息可以组成第一代价函数；
具体地，该第一代价函数可以为：ek(xk,zk)TΩek(xk,zk)，其中，zk为上述的最优估计状态，而xk为上述的待优化状态，Ω为加权系数，该加权系数为不确定度的倒数，ek(xk,zk)函数表示待xk与zk之间的约束关系(即误差函数)，在此处，k表示每帧点云数据的采集时刻或点云的编号；每帧的点云数据在ENU(East-North-Up，东-北-天)坐标系中的初始值为上述的zk，第一代价函数可以让每两帧点云数据经过初始值的平移跟旋转后，使该两帧点云数据的位置与姿态大致对齐；
进一步地，还可以通过点云数据间相同区域的约束关系，根据相邻的两帧点云数据的待优化状态对应的不确定度信息构建第二代价函数；
该第二代价函数可以为：ek(xk,xk-1)TΩek(xk,xk-1)，该xk可以为相邻两帧点云数据中其中一帧的待优化状态，而xk-1为相邻两帧点云数据中另外一帧的待优化状态，Ω为加权系数，该加权系数为不确定度的倒数；该第二代价函数可以通过相邻两帧点云数据之间相同区域的约束关系，微调相邻两帧点云数据的相对平移(即调整位置)和相对旋转(即调整姿态)，以达到两帧点云数据的最佳匹配。
而后，将该第一代价函数及第二代价函数进行累加，完成代价函数的构建，得到最终代价函数；完成整体的最终代价函数构建后，利用非线性优化方法(如高斯牛顿法)优化，使整体代价函数最小，得到点云数据的最优状态；根据点云数据的最优状态调整点云数据，从而完成点云地图构建。
以下的公式一中的F(x)表示最终代价函数，表示将第一代价函数及第二代价函数进行累加，构建得到最终代价函数；以下的公式二中的x*表示最终代价函数最小时的最优状态，即每帧点云数据的最优位置跟最优姿态。
在公式一及公式二中，zk为上述的最优估计状态，xk为上述的待优化状态，Ω为加权系数，该加权系数为不确定度的倒数，ek(xk,zk)函数表示待xk与zk之间的约束关系(即误差)，k表示每帧点云数据的采集时刻或点云的编号，F(x)表示最终代价函数，x*表示最终代价函数最小时的最优状态。
需要说明的是，虽然上述的第一代价函数及第二代价函数是分步骤构建，而在具体应用中，其可以通过最终代价函数可以是通过应用程序根据第一代价函数及第二代价函数同时构建完成的，上述的分步骤构建是为了更好说明本实施例的最终代价函数的构建过程。
为了使本领域技术人员更好地理解本实施例，以下用一个具体示例进行说明。
1、若点云数据采集车辆采集到3帧点云数据及对应的车体传感器数据；
2、通过多传感器数据融合方法将车体传感器数据进行融合处理，得到最优估计状态及对应的不确定度信息；
3、根据上述的最优估计状态为初始值，根据待优化状态、点云数据对应的最优估计状态和对应的不确定度信息构建该3帧点云数据的第一代价函数，该3帧点云数据的第一代价函数可以包括e1(x1,z1)TΩe1(x1,z1)、e2(x2,z2)TΩe2(x2,z2)、e3(x3,z3)TΩe3(x3,z3)；
4、通过相邻的两帧点云数据的待优化状态及对应的不确定度信息构建3帧点云数据的第二代价函数；该3帧点云数据的第二代价函数可以包括e2(x2,x1)TΩe2(x2,x1)、e3(x3,x2)TΩe3(x3,x2)；
5、将第一代价函数及第二代价函数累加，得到最终代价函数，即该F(x)为e1(x1,z1)TΩe1(x1,z1)、e2(x2,z2)TΩe2(x2,z2)、e3(x3,z3)TΩe3(x3,z3)、e2(x2,x1)TΩe2(x2,x1)、e3(x3,x2)TΩe3(x3,x2)的和；
6、将最终代价函数F(x)根据高斯牛顿法优化，使整体代价函数最小，得到点云数据的最优状态；而后根据点云数据的最优状态调整点云数据，从而完成点云地图构建。
在另一个实施例中，车体传感器数据包括车体位置、车体速度、车体加速度、车体角速度和车体前进方向速度；最优估计状态包括最优估计位置及最优估计姿态；最优状态包括最优位置及最优姿态；待优化状态包括待优化位置及待优化姿态。
需要说明的是，在对车体传感器数据进行融合处理，得到车体传感器数据的最优估计状态及对应的不确定度信息的步骤中，得到的最优估计状态可以包括最优估计位置、最优估计姿态及最优估计速度等，但是，对于点云地图的构建，可以只通过最优估计位置、最优估计姿态及对应的不确定度信息进行最终代价函数的构建，即对于点云数据构建点云地图的过程而言，可以只关注位置与姿态，而不关注速度，优化该最终代价函数，从而完成点云地图的构建。
应该理解的是，虽然图1-4的流程图中的各个步骤按照箭头的指示依次显示，但是这些步骤并不是必然按照箭头指示的顺序依次执行。除非本文中有明确的说明，这些步骤的执行并没有严格的顺序限制，这些步骤可以以其它的顺序执行。而且，图1-4中的至少一部分步骤可以包括多个子步骤或者多个阶段，这些子步骤或者阶段并不必然是在同一时刻执行完成，而是可以在不同的时刻执行，这些子步骤或者阶段的执行顺序也不必然是依次进行，而是可以与其它步骤或者其它步骤的子步骤或者阶段的至少一部分轮流或者交替地执行。
在一个实施例中，如图5所示，提供了一种点云地图的构建装置，包括：数据获取模块301、融合处理模块302和构建模块303，其中：
数据获取模块301，用于获取点云数据及对应的车体传感器数据；
融合处理模块302，用于对所述车体传感器数据进行融合处理，得到所述车体传感器数据的最优估计状态及对应的不确定度信息；
构建模块303，用于根据所述最优估计状态及对应的不确定度信息构建所述点云数据对应的点云地图。
在其中一个实施例中，参照图6，示出了本实施例的一种融合处理模块302的结构框图，所述融合处理模块302包括：
状态获取子模块3021，用于通过贝叶斯估计方法，得到所述车体传感器数据的最优估计状态；
模型建立子模块3022，用于根据所述车体传感器数据建立不确定度模型；
信息获取子模块3023，用于根据所述不确定度模型，采用贝叶斯估计方法，获得所述最优估计状态对应的不确定度信息。
在其中一个实施例中，参照图7，示出了本实施例的一种构建模块303的结构框图，所述构建模块包括：
函数构建子模块3031，用于采用最优估计状态及对应的不确定度信息以及点云数据间的约束关系构建代价函数；
最优状态获取子模块3032，用于采用非线性优化方法优化所述代价函数，得到点云数据的最优状态；
地图构建子模块3033，用于根据所述点云数据的最优状态调整点云数据，构建点云地图。
在其中一个实施例中，所述函数构建子模块3031用于将所述最优估计状态作为点云数据间匹配的初始值，利用点云数据间的约束关系和所述最优状态及对应的不确定度信息构建代价函数。
在其中一个实施例中，参照图8，示出了本实施例的一种函数构建子模块3031的结构框图，所述函数构建子模块包括：
第一函数构建单元30311，用于将点云数据对应的最优估计状态作为初始值，根据待优化状态、所述点云数据对应的最优估计状态和对应的不确定度信息构建第一代价函数；
第二函数构建单元30312，用于利用点云数据间的约束关系，通过相邻的两帧点云数据的待优化状态及对应的不确定度信息构建第二代价函数；
累加单元30313，用于将所述第一代价函数及第二代价函数累加，得到最终代价函数。
在其中一个实施例中，所述车体传感器数据包括车体位置、车体速度、车体加速度、车体角速度和车体前进方向速度；所述最优估计状态包括最优估计位置及最优估计姿态；最优状态包括最优位置及最优姿态；待优化状态包括待优化位置及待优化姿态。
关于点云地图的构建装置的具体限定可以参见上文中对于点云地图的构建方法的限定，在此不再赘述。上述点云地图的构建装置中的各个模块可全部或部分通过软件、硬件及其组合来实现。上述各模块可以硬件形式内嵌于或独立于计算机设备中的处理器中，也可以以软件形式存储于计算机设备中的存储器中，以便于处理器调用执行以上各个模块对应的操作。
上述提供的点云地图的构建装置可用于执行上述任意实施例提供的点云地图的构建方法，具备相应的功能和有益效果。
在一个实施例中，提供了一种计算机设备，该计算机设备可以是终端，其内部结构图可以如图9所示。该计算机设备包括通过系统总线连接的处理器、存储器、网络接口、显示屏和输入装置。其中，该计算机设备的处理器用于提供计算和控制能力。该计算机设备的存储器包括非易失性存储介质、内存储器。该非易失性存储介质存储有操作系统和计算机程序。该内存储器为非易失性存储介质中的操作系统和计算机程序的运行提供环境。该计算机设备的网络接口用于与外部的终端通过网络连接通信。该计算机程序被处理器执行时以实现一种点云地图的构建方法。该计算机设备的显示屏可以是液晶显示屏或者电子墨水显示屏，该计算机设备的输入装置可以是显示屏上覆盖的触摸层，也可以是计算机设备外壳上设置的按键、轨迹球或触控板，还可以是外接的键盘、触控板或鼠标等。
本领域技术人员可以理解，图9中示出的结构，仅仅是与本申请方案相关的部分结构的框图，并不构成对本申请方案所应用于其上的计算机设备的限定，具体的计算机设备可以包括比图中所示更多或更少的部件，或者组合某些部件，或者具有不同的部件布置。
在一个实施例中，提供了一种计算机设备，包括存储器和处理器，存储器中存储有计算机程序，该处理器执行计算机程序时实现上述各实施例中的点云地图的构建的方法中的步骤。
在一个实施例中，提供了一种计算机可读存储介质，其上存储有计算机程序，计算机程序被处理器执行时实现上述各实施例中的点云地图的构建的方法中的步骤。
本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一非易失性计算机可读取存储介质中，该计算机程序在执行时，可包括如上述各方法的实施例的流程。其中，本申请所提供的各实施例中所使用的对存储器、存储、数据库或其它介质的任何引用，均可包括非易失性和/或易失性存储器。非易失性存储器可包括只读存储器(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 (10)
1.一种点云地图的构建方法，其特征在于，包括：
获取点云数据及与所述点云数据对应的车体传感器数据；
对所述车体传感器数据进行融合处理，得到所述车体传感器数据的最优估计状态及对应的不确定度信息；
采用最优估计状态及对应的不确定度信息以及点云数据间的约束关系构建代价函数；
采用非线性优化方法优化所述代价函数，得到点云数据的最优状态；
根据所述点云数据的最优状态调整点云数据，构建所述点云数据对应的点云地图。
2.根据权利要求1所述的方法，其特征在于，所述对所述车体传感器数据进行融合处理，得到所述车体传感器数据的最优估计状态及对应的不确定度信息，包括：
通过贝叶斯估计方法，得到所述车体传感器数据的最优估计状态；
根据所述车体传感器数据建立不确定度模型；
根据所述不确定度模型，采用贝叶斯估计方法，获得所述最优估计状态对应的不确定度信息。
3.根据权利要求1所述的方法，其特征在于，所述采用最优估计状态及对应的不确定度信息以及点云数据间的约束关系构建代价函数，包括：
将所述最优估计状态作为点云数据间匹配的初始值，利用点云数据间的约束关系和所述最优状态及对应的不确定度信息构建代价函数。
4.根据权利要求3所述的方法，其特征在于，所述将所述最优估计状态作为点云数据间匹配的初始值，利用点云数据间的约束关系和所述最优状态及对应的不确定度信息构建代价函数，包括：
将点云数据对应的最优估计状态作为初始值，根据待优化状态、所述点云数据对应的最优估计状态和对应的不确定度信息构建第一代价函数；
利用点云数据间的约束关系，通过相邻的两帧点云数据的待优化状态及对应的不确定度信息构建第二代价函数；
将所述第一代价函数及第二代价函数累加，得到最终代价函数。
5.根据权利要求1至4任一项所述的方法，其特征在于，所述车体传感器数据包括车体位置、车体速度、车体加速度、车体角速度和车体前进方向速度；所述最优估计状态包括最优估计位置及最优估计姿态；最优状态包括最优位置及最优姿态；待优化状态包括待优化位置及待优化姿态。
6.一种点云地图的构建装置，其特征在于，包括：
数据获取模块，用于获取点云数据及与所述点云数据对应的车体传感器数据；
融合处理模块，用于对所述车体传感器数据进行融合处理，得到所述车体传感器数据的最优估计状态及对应的不确定度信息；
构建模块，用于根据所述最优估计状态及对应的不确定度信息构建所述点云数据对应的点云地图；
所述构建模块包括：
函数构建子模块，用于采用最优估计状态及对应的不确定度信息以及点云数据间的约束关系构建代价函数；
最优状态获取子模块，用于采用非线性优化方法优化所述代价函数，得到点云数据的最优状态；
地图构建子模块，用于根据所述点云数据的最优状态调整点云数据，构建所述点云数据对应的点云地图。
7.根据权利要求6所述的装置，其特征在于，所述融合处理模块包括：
状态获取子模块，用于通过贝叶斯估计方法，得到所述车体传感器数据的最优估计状态；
模型建立子模块，用于根据所述车体传感器数据建立不确定度模型；
信息获取子模块，用于根据所述不确定度模型，采用贝叶斯估计方法，获得所述最优估计状态对应的不确定度信息。
8.根据权利要求6所述的装置，其特征在于，所述函数构建子模块还用于将所述最优估计状态作为点云数据间匹配的初始值，利用点云数据间的约束关系和所述最优状态及对应的不确定度信息构建代价函数。
9.一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，其特征在于，所述处理器执行所述计算机程序时实现权利要求1至5中任一项所述的点云地图的构建方法的步骤。
10.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现权利要求1至5中任一项所述的点云地图的构建方法的步骤。
Priority Applications (3)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201811642897.2A CN111383324B (zh) | 2018-12-29 | 2018-12-29 | 点云地图的构建方法、装置、计算机设备和存储介质 |
| US17/419,430 US20220057517A1 (en) | 2018-12-29 | 2019-12-18 | Method for constructing point cloud map, computer device, and storage medium |
| PCT/CN2019/126328 WO2020135183A1 (zh) | 2018-12-29 | 2019-12-18 | 点云地图的构建方法、装置、计算机设备和存储介质 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201811642897.2A CN111383324B (zh) | 2018-12-29 | 2018-12-29 | 点云地图的构建方法、装置、计算机设备和存储介质 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN111383324A CN111383324A (zh) | 2020-07-07 |
| CN111383324B true CN111383324B (zh) | 2023-03-28 |
Family
ID=71128369
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201811642897.2A Active CN111383324B (zh) | 2018-12-29 | 2018-12-29 | 点云地图的构建方法、装置、计算机设备和存储介质 |
Country Status (3)
| Country | Link |
|---|---|
| US (1) | US20220057517A1 (zh) |
| CN (1) | CN111383324B (zh) |
| WO (1) | WO2020135183A1 (zh) |
Families Citing this family (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113963027B (zh) * | 2021-10-28 | 2022-09-09 | 广州文远知行科技有限公司 | 不确定性检测模型的训练、不确定性的检测方法及装置 |
| CN114549605B (zh) * | 2021-12-31 | 2023-08-04 | 广州景骐科技有限公司 | 基于点云匹配的图像优化方法、装置、设备及存储介质 |
| CN116067379B (zh) * | 2023-03-07 | 2023-06-30 | 青岛慧拓智能机器有限公司 | 一种基于动态点云的长隧道场景下多传感器融合定位方法 |
| CN116664416B (zh) * | 2023-04-17 | 2024-09-06 | 重庆大学 | 雷达点云数据处理方法、装置、电子设备及存储介质 |
Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107909612A (zh) * | 2017-12-01 | 2018-04-13 | 驭势科技（北京）有限公司 | 一种基于3d点云的视觉即时定位与建图的方法与系统 |
| CN108765481A (zh) * | 2018-05-25 | 2018-11-06 | 亮风台（上海）信息科技有限公司 | 一种单目视频的深度估计方法、装置、终端和存储介质 |
Family Cites Families (14)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104236548B (zh) * | 2014-09-12 | 2017-04-05 | 清华大学 | 一种微型无人机室内自主导航方法 |
| US9715016B2 (en) * | 2015-03-11 | 2017-07-25 | The Boeing Company | Real time multi dimensional image fusing |
| KR101725478B1 (ko) * | 2015-05-21 | 2017-04-11 | 주식회사 맥스트 | 3d 점군 인식 기반의 증강 현실 출력 방법과 이를 수행하기 위한 장치 및 시스템 |
| CN108693543B (zh) * | 2017-03-31 | 2022-11-22 | 法拉第未来公司 | 用于检测信号欺骗的方法及系统 |
| CN107796397B (zh) * | 2017-09-14 | 2020-05-15 | 杭州迦智科技有限公司 | 一种机器人双目视觉定位方法、装置和存储介质 |
| CN108445480B (zh) * | 2018-02-02 | 2022-05-03 | 重庆邮电大学 | 基于激光雷达的移动平台自适应扩展目标跟踪系统及方法 |
| CN108921893B (zh) * | 2018-04-24 | 2022-03-25 | 华南理工大学 | 一种基于在线深度学习slam的图像云计算方法及系统 |
| CN108759833B (zh) * | 2018-04-25 | 2021-05-25 | 中国科学院合肥物质科学研究院 | 一种基于先验地图的智能车辆定位方法 |
| CN108717712B (zh) * | 2018-05-29 | 2021-09-03 | 东北大学 | 一种基于地平面假设的视觉惯导slam方法 |
| CN108958266A (zh) * | 2018-08-09 | 2018-12-07 | 北京智行者科技有限公司 | 一种地图数据获取方法 |
| CN109059942B (zh) * | 2018-08-22 | 2021-12-14 | 中国矿业大学 | 一种井下高精度导航地图构建系统及构建方法 |
| US11682129B2 (en) * | 2018-09-12 | 2023-06-20 | Toyota Motor Europe | Electronic device, system and method for determining a semantic grid of an environment of a vehicle |
| US11494937B2 (en) * | 2018-11-16 | 2022-11-08 | Uatc, Llc | Multi-task multi-sensor fusion for three-dimensional object detection |
| US10991156B2 (en) * | 2018-12-05 | 2021-04-27 | Sri International | Multi-modal data fusion for enhanced 3D perception for platforms |
-
2018
- 2018-12-29 CN CN201811642897.2A patent/CN111383324B/zh active Active
-
2019
- 2019-12-18 WO PCT/CN2019/126328 patent/WO2020135183A1/zh active Application Filing
- 2019-12-18 US US17/419,430 patent/US20220057517A1/en active Pending
Patent Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107909612A (zh) * | 2017-12-01 | 2018-04-13 | 驭势科技（北京）有限公司 | 一种基于3d点云的视觉即时定位与建图的方法与系统 |
| CN108765481A (zh) * | 2018-05-25 | 2018-11-06 | 亮风台（上海）信息科技有限公司 | 一种单目视频的深度估计方法、装置、终端和存储介质 |
Also Published As
| Publication number | Publication date |
|---|---|
| WO2020135183A1 (zh) | 2020-07-02 |
| US20220057517A1 (en) | 2022-02-24 |
| CN111383324A (zh) | 2020-07-07 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111383324B (zh) | 点云地图的构建方法、装置、计算机设备和存储介质 | |
| CN108731670B (zh) | 基于量测模型优化的惯性/视觉里程计组合导航定位方法 | |
| CN110146909B (zh) | 一种定位数据处理方法 | |
| CN110873888B (zh) | 定位方法、定位装置、定位设备和计算机存储介质 | |
| US11227168B2 (en) | Robust lane association by projecting 2-D image into 3-D world using map information | |
| CN111241224B (zh) | 目标距离估计的方法、系统、计算机设备和存储介质 | |
| CN110296702A (zh) | 视觉传感器与惯导紧耦合的位姿估计方法及装置 | |
| CN113405545B (zh) | 定位方法、装置、电子设备及计算机存储介质 | |
| CN113252048B (zh) | 一种导航定位方法、导航定位系统及计算机可读存储介质 | |
| CN112985391B (zh) | 一种基于惯性和双目视觉的多无人机协同导航方法和装置 | |
| CN114264301B (zh) | 车载多传感器融合定位方法、装置、芯片及终端 | |
| CN113566850B (zh) | 惯性测量单元的安装角度标定方法、装置和计算机设备 | |
| CN113763548B (zh) | 基于视觉-激光雷达耦合的贫纹理隧洞建模方法及系统 | |
| CN115326084A (zh) | 车辆定位方法、装置、计算机设备、存储介质 | |
| CN114061611A (zh) | 目标对象定位方法、装置、存储介质和计算机程序产品 | |
| CN114061570A (zh) | 车辆定位方法、装置、计算机设备和存储介质 | |
| Hong et al. | Visual inertial odometry using coupled nonlinear optimization | |
| CN113965646A (zh) | 定位控制方法及装置、电子设备、存储介质 | |
| CN110375740B (zh) | 车辆导航方法、装置、设备和存储介质 | |
| CN114897942B (zh) | 点云地图的生成方法、设备及相关存储介质 | |
| CN114001730B (zh) | 融合定位方法、装置、计算机设备和存储介质 | |
| CN115900732A (zh) | 基于路侧相机和车载单元的组合导航方法及系统 | |
| CN115984463A (zh) | 一种适用于狭窄巷道的三维重建方法和系统 | |
| CN113034538A (zh) | 一种视觉惯导设备的位姿跟踪方法、装置及视觉惯导设备 | |
| CN117308925B (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 |