CN113252051A - 一种地图构建方法及装置 - Google Patents

一种地图构建方法及装置 Download PDF

Info

Publication number
CN113252051A
CN113252051A CN202010087732.4A CN202010087732A CN113252051A CN 113252051 A CN113252051 A CN 113252051A CN 202010087732 A CN202010087732 A CN 202010087732A CN 113252051 A CN113252051 A CN 113252051A
Authority
CN
China
Prior art keywords
map
sub
pose
frame
point cloud
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
CN202010087732.4A
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.)
Beijing Tusimple Technology Co Ltd
Original Assignee
Beijing Tusimple Technology Co Ltd
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 Beijing Tusimple Technology Co Ltd filed Critical Beijing Tusimple Technology Co Ltd
Priority to CN202010087732.4A priority Critical patent/CN113252051A/zh
Publication of CN113252051A publication Critical patent/CN113252051A/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/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
    • G01C21/30—Map- or contour-matching
    • G01C21/32—Structuring or formatting of map data
    • 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

Abstract

本申请提供了一种地图构建方法及装置，涉及高精度地图技术领域。本申请实施例能够实时获得可移动物体上搭载的传感器采集的传感器数据，并进行各帧与第一子地图的配准，确定各帧与第一子地图的第一相对位姿关系。确定与第一子地图相关的第二子地图，将第一子地图与第二子地图进行配准，并结合第一子地图的初始位姿，建立位姿约束关系。对位姿约束关系进行全局优化，确定各第一子地图的子地图优化位姿。根据上述的第一相对位姿关系和子地图优化位姿，确定各第一子地图中各帧数据对应的帧优化位姿。根据帧优化位姿和各帧数据，进行地图构建，形成全局地图。本申请可以提高地图构建的一致性，并且受激光雷达等传感器的标定精度的影响较小。

Description

一种地图构建方法及装置
技术领域
本申请涉及高精度地图技术领域，尤其涉及一种地图构建方法及装置。
背景技术
目前，随着自动驾驶技术、智能机器人技术的发展，如何保证自动驾驶车辆和智能机器人的精确行驶成为了一个热点问题。在自动驾驶技术中，一般会应用高精度地图，其不同于传统的导航地图，高精度地图包含大量的驾驶辅助信息，最重要的信息是依托道路网的精确三维表征，例如交叉路口布局和路标位置等。另外，高精度地图还包含很多语义信息，地图上可能会报告通信交通灯上不同颜色的含义，它可能指示道路的速度限制，以及左转车道开始的位置等。高精地图最重要的特征之一是精度，高精度地图能使自动驾驶车辆等达到厘米级的精度，这对确保自动驾驶车辆的安全至关重要。
高精度地图在被使用前，需要预先构建，方法一般有多种，例如可以使用高精度的组合导航系统获取自动驾驶车辆的位姿，同时结合多线激光雷达的采集数据构建高精度地图，又例如可以采用SLAM技术(Simultaneous Localization and Mapping，即时定位与地图构建)来进行高精度地图构建。然而上述传统的高精度地图构建方法普遍存在地图一致性较差，受激光雷达等传感器的标定精度的影响较大的问题。
本申请实施例旨在提出一种不同于上述构建方式的高精度地图的构建方法。
发明内容
本申请的实施例提供一种地图构建方法及装置，能够克服现有技术中地图构建方法存在的地图一致性较差，受激光雷达等传感器的标定精度的影响较大的问题。
为达到上述目的，本申请的实施例采用如下技术方案：
本申请实施例的第一方面，提供一种地图构建方法，包括：
实时获得可移动物体上搭载的传感器采集的传感器数据，确定预设帧长度的第一子地图中的各帧数据；
根据各帧数据和所述第一子地图中的若干帧数据，进行各帧与所述第一子地图的配准，确定各帧与所述第一子地图的第一相对位姿关系；所述若干帧数据为所述第一子地图中第一帧到各帧前一帧的数据；
根据各第一子地图对应的初始位姿确定与各第一子地图相关的第二子地图，将各第一子地图和与其相关的第二子地图进行配准，建立各第一子地图和第二子地图的第一位姿约束关系，并建立各第一子地图和各第一子地图对应的初始位姿的第二位姿约束关系；
根据所述第一位姿约束关系和第二位姿约束关系进行全局优化，确定各第一子地图的子地图优化位姿；
根据所述第一相对位姿关系和所述子地图优化位姿，确定各第一子地图中各帧数据对应的帧优化位姿；
根据所述帧优化位姿和各帧数据，进行地图构建，形成全局地图。
本申请实施例的第二方面，提供一种地图构建装置，包括：
数据采集单元，用于实时获得可移动物体上搭载的传感器采集的传感器数据，确定预设帧长度的第一子地图中的各帧数据；
帧与子地图配准单元，用于根据各帧数据和所述第一子地图中的若干帧数据，进行各帧与所述第一子地图的配准，确定各帧与所述第一子地图的第一相对位姿关系；所述若干帧数据为所述第一子地图中第一帧到各帧前一帧的数据；
子地图与子地图配准单元，用于根据各第一子地图对应的初始位姿确定与各第一子地图相关的第二子地图，将各第一子地图和与其相关的第二子地图进行配准，建立各第一子地图和第二子地图的第一位姿约束关系，并建立各第一子地图和各第一子地图对应的初始位姿的第二位姿约束关系；
全局优化单元，用于根据所述第一位姿约束关系和第二位姿约束关系进行全局优化，确定各第一子地图的子地图优化位姿；
帧优化位姿确定单元，用于根据所述第一相对位姿关系和所述子地图优化位姿，确定各第一子地图中各帧数据对应的帧优化位姿；
全局地图构建单元，用于根据所述帧优化位姿和各帧数据，进行地图构建，形成全局地图。
本申请实施例的第三方面，提供一种计算机可读存储介质，包括程序或指令，当所述程序或指令在计算机上运行时，实现上述第一方面所述的地图构建方法。
本申请实施例的第四方面，提供一种包含指令的计算机程序产品，当所述计算机程序产品在计算机上运行时，使得所述计算机执行如上述第一方面所述的地图构建方法。
本申请实施例的第五方面，提供一种芯片系统，包括处理器，所述处理器与存储器的耦合，所述存储器存储有程序指令，当所述存储器存储的程序指令被所述处理器执行时实现上述第一方面所述的地图构建方法。
本申请实施例的第六方面，提供一种电路系统，所述电路系统包括处理电路，所述处理电路配置为执行如上述第一方面所述的地图构建方法。
本申请实施例的第七方面，提供一种计算机服务器，包括存储器，以及与所述存储器通信连接的一个或多个处理器；
所述存储器中存储有可被所述一个或多个处理器执行的指令，所述指令被所述一个或多个处理器执行，以使所述一个或多个处理器实现如上述第一方面所述的地图构建方法。
本申请实施例提供的一种地图构建方法及装置，能够实时获得可移动物体上搭载的传感器采集的传感器数据，并进行各帧与第一子地图的配准，确定各帧与第一子地图的第一相对位姿关系。之后在确定与第一子地图相关的第二子地图后，能够将第一子地图与第二子地图进行配准，并结合第一子地图的初始位姿，建立位姿约束关系。从而在对位姿约束关系进行全局优化后，可以确定各第一子地图的子地图优化位姿。进而可以根据上述的第一相对位姿关系和子地图优化位姿，确定各第一子地图中各帧数据对应的帧优化位姿。进而可以根据帧优化位姿和各帧数据，进行地图构建，形成全局地图。这样，通过本申请实施例可以克服现有技术中地图构建方法存在的地图一致性较差，受激光雷达等传感器的标定精度的影响较大的问题。
附图说明
为了更清楚地说明本申请实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本申请的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
图1为本申请实施例提供的一种地图构建方法的结构示意图一；
图2为本申请实施例中的车辆上设置激光雷达的示意图；
图3为本申请实施例提供的一种地图构建方法的结构示意图二；
图4为本申请的一实施例中的全局位姿关系图；
图5为本申请实施例中的地图车得到的待验证全局地图的细节示意图一；
图6为本申请实施例中的地图车得到的待验证全局地图的细节示意图二；
图7为本申请实施例中的卡车得到的待验证全局地图的细节示意图一；
图8为本申请实施例中的卡车得到的待验证全局地图的细节示意图二；
图9为本申请实施例提供的一种地图构建装置的结构示意图。
具体实施方式
下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。
需要说明的是，本申请的说明书和权利要求书及上述附图中的术语“第一”、“第二”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的本申请的实施例。此外，术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或单元的过程、方法、系统、产品或设备不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。
为了使本领域的技术人员更好的了解本申请，下面先对本申请实施例中出现的部分技术术语进行解释如下：
可移动物体：是指车辆、移动机器人、飞行器等可进行地图采集的物体，可移动物体上可以搭载各类型传感器，如激光雷达、相机等。
GNSS：Global Navigation Satellite System，全球导航卫星系统。
GPS：Global Positioning System，全球定位系统。
IMU：Inertial Measurement Unit，惯性测量单元，是测量物体三轴姿态角(或角速率)以及加速度的装置。
高精度地图：不同于传统的导航地图，高精度地图包含大量的驾驶辅助信息，最重要的信息是依托道路网的精确三维表征，例如交叉路口布局和路标位置等。另外，高精度地图还包含很多语义信息，地图上可能会报告通信交通灯上不同颜色的含义，它可能指示道路的速度限制，以及左转车道开始的位置等。高精地图最重要的特征之一是精度，高精度地图能使车辆达到厘米级的精度，这对确保自动驾驶汽车的安全至关重要。
建图(Mapping)：根据估计得到的车辆或移动机器人实时位姿以及激光雷达等视觉传感器的采集数据，构建出描述当前场景的高精度地图。
位姿(Pose)：位置和朝向的总称，包含6个自由度，其中包括3个位置自由度和3个朝向自由度。朝向的3个自由度通常用俯仰角(Pitch)、翻滚角(Roll)、偏航角(Yaw)来表示。
位姿估计(Pose Estimation)：根据车辆或移动机器人所携带的传感器感知到的环境信息，估计出车辆或机器人的实时位姿。
帧(Frame)：传感器完成一次观测所接收到的测量数据，如相机的一帧数据为一张图片，激光雷达的一帧数据为一组激光点云。
子地图(Submap)：全局地图由若干个子地图组成，每个子地图包含连续多帧的观测结果。
配准(Registration)：对同一区域在不同时刻、不同位置的观测结果进行匹配，得到两个观测时刻间的相对位姿关系。
图优化(Graph Optimization)：把优化问题表示为图的形式，图由节点和边组成，优化过程即通过调整节点的位姿使得最大程度满足边的约束。
KD-Tree(K-dimensional树)：是一种分割K维数据空间的数据结构。主要应用于多维空间关键数据的搜索(如：范围搜索和最近邻搜索)。KD-Tree是二进制空间分割树的特殊的情况。在计算机科学里，KD-Tree是在K维欧几里德空间组织点的数据结构。
NDT：Normal Distributions Transform，即正态分布变换算法，是一种配准算法，它应用于三维点的统计模型，使用标准最优化技术来确定两个点云间的最优的匹配。
SVD：Singular Value Decomposition，奇异值分解，是线性代数中一种重要的矩阵分解，奇异值分解则是特征分解在任意矩阵上的推广。在信号处理等领域有重要应用。
NovAtel：精密全球导航卫星系统(GNSS)及其子系统领域中，处于领先地位的产品与技术供应商。本申请实施例中表示NovAtel的组合导航系统。
在本申请的一些实施例中，术语“车辆”广泛地解释为包括任何移动物体，包括例如飞行器、船只、航天器、汽车、卡车、厢式货车、半挂车、摩托车、高尔夫球车、越野车辆、仓库运输车辆或农用车以及行驶在轨道上的运输工具，例如电车或火车以及其它有轨车辆。本申请中的“车辆”通常可以包括：动力系统、传感器系统、控制系统、外围设备和计算机系统。在其它实施例中，车辆可以包括更多、更少或者不同的系统。
其中，动力系统是为车辆提供动力运动的系统，包括：引擎/马达、变速器和车轮/轮胎、能源单元。
控制系统可以包括控制车辆及其组件的装置的组合，例如转向单元、节气门、制动单元。
外围设备可以是允许车辆与外部传感器、其它车辆、外部计算设备和/或用户进行交互的设备，例如无线通信系统、触摸屏、麦克风和/或扬声器。
基于上述描述的车辆，自动驾驶车辆中还配置有传感器系统和自动驾驶控制装置。
传感器系统可以包括用于感测车辆所处环境的信息的多个传感器，以及改变传感器的位置和/或方向的一个或多个致动器。传感器系统可以包括全球定位系统传感器、惯性测量单元、无线电检测和测距(RADAR)单元、相机、激光测距仪、光检测和测距(LIDAR)单元和/或声学传感器等传感器的任何组合；传感器系统还可以包括监视车辆内部系统的传感器(例如O2监视器、燃油表、引擎温度计等)。
自动驾驶控制装置可以包括一个处理器和存储器，存储器中存储有至少一条机器可执行指令，处理器执行至少一条机器可执行指令实现包括地图引擎、定位模块、感知模块、导航或路径模块、以及自动控制模块等的功能。地图引擎和定位模块用于提供地图信息和定位信息。感知模块用于根据传感器系统获取到的信息和地图引擎提供的地图信息感知车辆所处环境中的事物。导航或路径模块用于根据地图引擎、定位模块和感知模块的处理结果，为车辆规划行驶路径。自动控制模块将导航或路径模块等模块的决策信息输入解析转换成对车辆控制系统的控制命令输出，并通过车载网(例如通过CAN总线、局域互联网络、多媒体定向系统传输等方式实现的车辆内部电子网络系统)将控制命令发送给车辆控制系统中的对应部件，实现对车辆的自动控制；自动控制模块还可以通过车载网来获取车辆中各部件的信息。
一般情况下，高精度地图在被使用前，需要预先构建，方法一般有多种：
例如方式一：可以使用高精度的组合导航系统获取自动驾驶车辆的位姿，同时结合多线激光雷达的采集数据构建高精度地图。即使用自动驾驶车辆上的多台激光雷达设备实时扫描周围场景，获得点云数据，同时使用由高精度GPS和IMU组成的组合导航系统测量自动驾驶车辆的实时位姿，并根据组合导航系统与激光雷达之间的外参标定得到的位姿关系，计算出激光雷达的实时位姿，之后将点云数据按照激光雷达的位姿进行叠加，得到全局的高精度地图。
又例如方式二：可以采用SLAM技术来进行高精度地图构建。即不断的将当前帧所采集到的激光雷达点云数据与前一时刻或前几个时刻的激光点云数据进行配准，从而增量式的估计当前车辆的实时位姿，并构建全局的高精度地图。
而目前，评价高精度地图构建的优劣的标准一般有两种——地图局部一致性和地图全局一致性。其中，地图全局一致性是指：地图构建时所产生的每一个点与真实的点的坐标相比较，误差如果小于一定阈值，则可以认为全局一致性较好。地图局部一致性是指：表示地图构建后每个局部区域的点云数据的清晰程度，如果地图局部一致性较差，则例如车辆道路上的隔离护栏、车道线等不清晰，地面存在的厚度较大(地图局部一致性高的情况下，地面应没有厚度，或者厚度较小)，一般情况下，路面上的地图构建可以以点云数据所表达的地面的厚度、路边标志物的清晰程度来评价地图局部一致性的优良。
目前大规模场景下(例如大范围的城市地图、长度较长的高速公路网络地图)的建图通常采用上述方式一的构建方法，该方式一的构建方法建图效率较高、地图全局一致性好，但受限于GPS测量，得到的车辆位姿通常存在噪声，同时车辆轨迹也不够平滑，从而导致得到的地图局部一致性较差。小范围场景下(例如园区、地下停车场)通常采用上述方式二的构建方法进行建图，该方式二得到的地图局部一致性良好，但是由于激光点云配准不可避免的存在累计误差，同时方式二缺少GPS提供的绝对约束，所以得到的地图全局一致性较差。
可见，综合上述方式一和方式二，目前的地图的构建面临多个问题：
(1)地图局部一致性较差：高精度组合导航系统提供了较为准确的全局位姿，但多次采集的数据间不可避免的存在偏差，在有些环境下(GPS信号弱的地区)组合导航系统甚至会定位失败，所以地图局部一致性会因此而降低。
(2)地图全局一致性较差：方式二采用基于激光点云配准的激光SLAM算法不可避免的会存在累计误差，传统算法是通过回环检测(又称闭环检测，是指机器人识别曾到达某场景，使得地图闭环的能力)来减小累计误差的影响，但在跨度范围大的场景下，如高速场景中，累计误差会迅速增加，而且高速场景极少存在可用的闭环，从而导致地图全局一致性急剧下降。
(3)无论方式一还是方式二，其建图方法依赖高精度的传感器(激光雷达)，同时需要精确标定不同传感器之间的外参。(传统建图方法较为依赖传感器的外参标定，即GPS与激光雷达的位置关系，两个激光雷达之间的位置关系)。
本申请实施例旨在提出一种不同于上述方式一和方式二的构建方式的高精度地图的构建方法，以克服现有技术中地图构建方法存在的地图一致性较差，受激光雷达等传感器的标定精度的影响较大的问题。
如图1所示，本申请实施例提供一种地图构建方法，包括：
步骤101、实时获得可移动物体上搭载的传感器采集的传感器数据，确定预设帧长度的第一子地图中的各帧数据。
步骤102、根据各帧数据和第一子地图中的若干帧数据，进行各帧与第一子地图的配准，确定各帧与第一子地图的第一相对位姿关系。
其中，该若干帧数据为第一子地图中第一帧到各帧前一帧的数据。
步骤103、根据各第一子地图对应的初始位姿确定与各第一子地图相关的第二子地图，将各第一子地图和与其相关的第二子地图进行配准，建立各第一子地图和第二子地图的第一位姿约束关系，并建立各第一子地图和各第一子地图对应的初始位姿的第二位姿约束关系。
步骤104、根据第一位姿约束关系和第二位姿约束关系进行全局优化，确定各第一子地图的子地图优化位姿。
步骤105、根据第一相对位姿关系和子地图优化位姿，确定各第一子地图中各帧数据对应的帧优化位姿。
步骤106、根据帧优化位姿和各帧数据，进行地图构建，形成全局地图。
为了使本领域的技术人员更好的了解本申请，下面结合附图、实例等对本申请实施例做更为详细的阐述。值得说明的是，本申请实施例中的可移动物体可以是指车辆、移动机器人、飞行器等可进行地图采集的物体，可移动物体上可以搭载各类型传感器，例如在本申请的一实施例中，传感器可以为激光雷达，则相应的传感器数据为激光点云数据，各帧数据为激光雷达采集的各帧激光点云数据。例如，如图2所示，在一车辆20(可以为自动驾驶车辆，也可以为地图采集车等有人驾驶车辆)的两侧或车辆顶部等处可以设置用于进行周围环境感知的激光雷达21，具体的激光雷达21在车辆20上的安装此处不再赘述。
如图3所示，本申请实施例提供一种地图构建方法，包括：
步骤301、控制可移动物体上搭载的传感器进行数据采集工作。
例如，控制车辆上搭载的激光雷达采集各帧激光点云数据；又例如控制车辆上搭载的组合导航系统采集组合导航系统位置处的位姿，位姿可以包括位置(如GPS)和姿态(如俯仰角(Pitch)、翻滚角(Roll)、偏航角(Yaw))。
步骤302、实时获得可移动物体上搭载的传感器采集的传感器数据，确定预设帧长度的第一子地图中的各帧数据。
例如，在车辆上搭载的激光雷达采集各帧激光点云数据时，一帧激光点云数据是指激光雷达向周围发射激光，采集一周(360°)得到的激光点云数据。此处，子地图是相对于全局地图的概念，全局地图由若干个子地图组成，每个子地图包含连续多帧，即可以为子地图预设帧长度，如设置第一子地图的帧长度为A帧，则每个第一子地图在传感器如激光雷达采集A帧激光点云数据后，形成该第一子地图的全部帧数据。例如，A可以根据需求设置为20、30、40等，但不仅局限于此。另外，例如车辆上搭载的组合导航系统可以采集组合导航系统位置处的位姿，组合导航系统对应的一帧传感器数据可以是指对应于采集一帧激光点云数据时的组合导航系统位置处的位姿。
之后，在如下步骤303至步骤305处需要对激光雷达采集各帧激光点云数据进行预处理，这是由于在一些车辆行驶较快的场景(如高速场景)，存在车速快、周围车辆情况复杂的问题，从而得到的原始激光点云数据不可避免的存在测距误差以及动态车辆点云混杂的问题。因此需要通过步骤303至步骤305来进行预处理，以便于后续的数据处理。
步骤303、将各帧激光点云数据进行运动补偿，确定各帧激光点云数据中的点的运动补偿后的位置。
此处，该步骤303可以采用如下方式实现：
获得采集一帧激光点云数据的开始时刻和结束时刻的激光雷达位姿(此处，可以预先对激光雷达与组合导航系统之间的位姿进行标定，从而在实时得到组合导航系统位姿的情况下，可以得到每一时刻激光雷达的位姿)。在采集一帧激光点云数据的开始时刻和结束时刻之间进行时间戳插值得到一帧激光点云数据中采集每个点的时刻对应的激光雷达位姿。根据采集每个点的时刻对应的激光雷达位姿，以及每个点在激光雷达坐标系下的坐标(激光雷达在扫描外部环境时，通过得到的激光点云数据可以直接得到各点在激光雷达坐标系下的坐标)，确定每个点的运动补偿后的位置。
步骤304、通过各帧激光点云数据进行动态目标检测，从各帧激光点云数据中确定干扰物体对应的点，并将干扰物体对应的点剔除。
一般情况下，该干扰物体可以为预先设置的运动物体，例如预先标注的车辆、行人、动物等，但不仅局限于此。这样在样本上标注了各种运动物体之后，通过样本来训练神经网络，从而训练好的神经网络可以用于对各帧激光点云数据进行动态目标检测，从而从各帧激光点云数据中确定干扰物体对应的点，进而将干扰物体对应的点剔除，其具体过程可以如文献：Zhang,Ji,and Sanjiv Singh."Low-drift and real-time lidar odometryand mapping."Autonomous Robots 41.2(2017):401-416.所示，此处不再赘述。
步骤305、对进行运动补偿和动态目标检测及剔除后的各帧激光点云数据进行特征提取，确定满足预先设置的特征提取策略的特征点。
此处，该步骤305可以采用如下方式实现，例如：
在进行运动补偿和动态目标检测及剔除后的一帧激光点云数据中获得一个线束上的任一点及该线束上的任一点预设范围内的若干点。此处，由于激光雷达测量得到的激光点是按照线束排列的，所以可以按照线束为每一个激光点找到其预设范围内的若干点，如在线束上其左右两侧的若干激光点(如左右两侧各取5个激光点，但不仅局限于此)。
根据该任一点在激光雷达坐标系下的坐标和该线束上的任一点预设范围内的若干点在激光雷达坐标系下的坐标，确定该任一点处的曲率。例如，可以采用如下曲率计算公式来确定该任一点处的曲率：
Figure BDA0002382026260000101
其中，c表示
Figure BDA0002382026260000102
点处的曲率；
Figure BDA0002382026260000103
分别表示当前帧中第k条线上的第i、j个点在激光雷达坐标系下的坐标，S表示第i个点左右两侧若干个点组成的点集，|S|表示该点集所包含点的个数。
根据预先设置的曲率阈值，在一点的曲率大于曲率阈值时，将该一点作为线特征点，在一点的曲率小于曲率阈值时，将该一点作为平面特征点。
根据预先设置的特征点数目阈值，从所述曲率阈值开始剔除曲率阈值两侧曲率对应的线特征点和平面特征点，直至剩余的线特征点和平面特征点的数目之和小于等于所述特征点数目阈值，并获得该剩余的线特征点和平面特征点。即相当于曲率越接近曲率阈值的点其线特征和平面特征不突出，容易出现误判断的情况，因此需要将曲率接近曲率阈值的点剔除，仅保留曲率远离曲率阈值的点，从而限制特征点的数目。例如可以对线特征点和面特征点按照曲率进行排序，在提取得到的线特征点和面特征点数目超过特征点数目阈值时，按照曲率舍弃曲率较小的线特征点以及曲率较大的面特征点。例如，曲率阈值两侧分别有10000个点，共计20000个点，而特征点数目阈值为5000，则可以将曲率阈值两侧各自7500个点剔除。
步骤306、获得采集一帧激光点云数据对应的激光雷达位姿，作为配准位姿初始值。
其中，一帧激光点云数据中包括上述步骤305中的剩余的线特征点和平面特征点。
该步骤306可以采用如下方式实现，例如：
通过组合导航系统获得采集一帧激光点云数据对应的组合导航系统位姿。
根据组合导航系统位姿和预先获得的激光雷达与组合导航系统的外参标定结果，确定该一帧激光点云数据对应的激光雷达位姿。此处，外参标定结果可以体现组合导航系统位姿与激光雷达位姿的对应关系，从而在得到组合导航系统位姿的情况下，可以解算出对应的激光雷达位姿。
步骤307、根据预先采用KD-Tree算法为每个点建立的数据索引，搜索得到每个点对应的若干近邻点。
其中若干近邻点为第一子地图中的若干帧数据中的点。例如，该若干近邻点为第一子地图中第一帧到当前帧前一帧的点。如一个第一子地图中要记录30帧数据，当前帧为第9帧，则该若干近邻点为第一子地图中第1帧到第8帧的点。
在步骤307之后继续执行步骤308和步骤309。
步骤308、对一帧激光点云数据中的线特征点，根据该线特征点对应的配准位姿初始值和对应的若干近邻点，建立线特征点误差函数。
该步骤308可以采用如下方式实现，例如：
根据一帧激光点云数据中的线特征点
Figure BDA0002382026260000121
对应的若干近邻点(例如搜索得到5个近邻点，但不仅局限于此)拟合得到一条直线(例如通过最小二乘法来将若干近邻点拟合得到直线)，将线特征点
Figure BDA0002382026260000122
与该直线的距离函数作为线特征点误差函数。
其中，线特征点误差函数为：
Figure BDA0002382026260000123
其中，
Figure BDA0002382026260000124
和
Figure BDA0002382026260000125
为该直线上的任意两点，R和t表示配准位姿初始值，其中R表示配准位姿初始值中的姿态，t表示配准位姿初始值中的位置。
步骤309、对一帧激光点云数据中的面特征点，根据该面特征点对应的配准位姿初始值和对应的若干近邻点，建立面特征点误差函数。
此处，步骤309可以采用如下方式实现，例如：
根据一帧激光点云数据中的面特征点
Figure BDA0002382026260000126
对应的若干近邻点(例如搜索得到5个近邻点，但不仅局限于此)拟合得到一个平面Ax+By+Cz+D＝0(例如采用SVD分解方式，根据该若干近邻点得到平面)，将面特征点
Figure BDA0002382026260000127
与该平面的距离函数作为面特征点误差函数；其中，k表示一帧激光点云数据中的第k条线，i表示第i个点，A、B、C和D表示拟合得到的平面的参数；
Figure BDA0002382026260000128
为面特征点在激光雷达坐标系下的坐标。
该面特征点误差函数为：
Figure BDA0002382026260000129
其中，R和t表示配准位姿初始值，其中R表示配准位姿初始值中的姿态，t表示配准位姿初始值中的位置；n表示矩阵：n＝(A,B,C)。
在步骤308和步骤309之后执行步骤310。
步骤310、根据线特征点误差函数和面特征点误差函数，确定帧至子地图配准的误差函数。
此处，步骤310可以采用如下方式实现，例如：
根据线特征点误差函数
Figure BDA00023820262600001210
和面特征点误差函数
Figure BDA0002382026260000131
确定帧至子地图配准的误差函数
Figure BDA0002382026260000132
其中，nline表示该一帧激光点云数据中的线特征点数目，nplane表示该一帧激光点云数据中的面特征点数目；
Figure BDA0002382026260000133
表示一帧激光点云数据中的第i个线特征点的线特征点误差，
Figure BDA0002382026260000134
表示一帧激光点云数据中的第i个面特征点的面特征点误差。
步骤311、对帧至子地图配准的误差函数进行求解，得到一帧激光点云数据对应的第一激光雷达优化位姿，并确定第一激光雷达优化位姿与第一子地图对应位姿的第一相对位姿关系。
其中，第一子地图对应位姿为第一子地图第一帧激光点云数据对应的激光雷达位姿。
此处，步骤311中对帧至子地图配准的误差函数进行求解，得到一帧激光点云数据对应的第一激光雷达优化位姿，可以采用如下方式，例如：
对帧至子地图配准的误差函数
Figure BDA0002382026260000135
进行迭代求解(例如采用高斯牛顿法，或者LM算法(Levenberg-Marquard)来进行迭代求解，但不仅局限于此)，得到Eframe取得最小值时对应的姿态R和位置t，作为一帧激光点云数据对应的第一激光雷达优化位姿。
这样，本申请实施例采用帧(Frame)至子地图(Submap)的配准，能够避免由于帧所包含的信息比较有限，帧至帧(Frame-to-Frame)的配准方式会导致累计误差迅速增加的问题。另外，本申请实施例此处的帧至子地图的配准采用的是基于激光点云的配准算法，其对配准位姿初始值的精度要求不高，因此对激光雷达与组合导航系统的外参标定精度要求不高，在标定精度较低时，依然可以得到准确的配准结果，即一帧激光点云数据对应的第一激光雷达优化位姿以及第一相对位姿关系。
步骤312、根据各第一子地图对应的初始位姿确定与各第一子地图相关的第二子地图，将各第一子地图和与其相关的第二子地图进行配准，建立各第一子地图和第二子地图的第一位姿约束关系，并建立各第一子地图和各第一子地图对应的初始位姿的第二位姿约束关系。
其中，该步骤312可以采用如下两种方式：
方式一：
其中，根据各第一子地图对应的初始位姿确定与各第一子地图相关的第二子地图，包括对每个第一子地图执行以下步骤：
根据第一子地图对应的初始位姿，确定与第一子地图相邻的上一个子地图作为第二子地图；第一子地图对应的初始位姿为第一子地图第一帧激光点云数据对应的激光雷达位姿。此处，在方式一中考虑的是与第一子地图相邻的上一个子地图，而未考虑存在闭环检测的情况。
另外，将各第一子地图和与其相关的第二子地图进行配准，建立各第一子地图和第二子地图的第一位姿约束关系，包括对每个第一子地图执行以下步骤：
根据第一子地图对应的初始位姿和第二子地图对应的初始位姿，采用NDT算法得到第一子地图和第二子地图对应的位姿变换关系
Figure BDA0002382026260000141
其中，第二子地图对应的初始位姿为第二子地图第一帧激光点云数据对应的激光雷达位姿。此处，通过NDT算法得到第一子地图和第二子地图对应的位姿变换关系
Figure BDA0002382026260000142
的方式为：首先选择一个子地图的点云进行三维栅格划分，之后为每一个栅格计算一个正态分布，然后对另一个子地图中的激光点做降采样，并将所有降采样后的点按照组合导航系统提供的初始位姿变换投影到三维栅格中，同时计算出每个点的概率密度函数，并计算所有概率密度函数乘积的最大似然，从而得到两个子地图之间的位姿变换关系。
建立第一子地图和第二子地图的第一位姿误差函数：
Figure BDA0002382026260000143
其中，
Figure BDA0002382026260000144
表示第i个子地图和第i+1个子地图对应的位姿变换关系，
Figure BDA0002382026260000145
表示第i个子地图和第i+1个子地图的第一位姿误差，Xi表示第i个子地图对应的初始位姿，Xi+1表示第i+1个子地图对应的初始位姿，Xi和Xi+1可以通过上述步骤311中的第一激光雷达优化位姿的确定方式得到。
另外，建立各第一子地图和各第一子地图对应的初始位姿的第二位姿约束关系，包括对每个第一子地图执行以下步骤：
建立第一子地图和第一子地图对应的初始位姿的第二位姿误差函数：
Figure BDA0002382026260000146
其中，Xi表示第i个子地图对应的初始位姿，
Figure BDA0002382026260000147
表示根据组合导航系统位姿和预先获得的激光雷达与组合导航系统的外参标定结果，确定得到的第i个子地图对应的激光雷达位姿。
方式二：
其中，根据各第一子地图对应的初始位姿确定与各第一子地图相关的第二子地图，包括对每个第一子地图执行以下步骤：
根据第一子地图对应的初始位姿，确定与第一子地图相邻的上一个子地图以及与第一子地图存在闭环的子地图作为第二子地图；与第一子地图存在闭环的子地图对应的初始位姿与第一子地图对应的初始位姿之间的距离小于等于预先设置的距离阈值；所述第一子地图对应的初始位姿为第一子地图第一帧激光点云数据对应的激光雷达位姿；与第一子地图存在闭环的子地图对应的初始位姿为与第一子地图存在闭环的子地图第一帧激光点云数据对应的激光雷达位姿。此处，在方式二中不仅考虑的是与第一子地图相邻的上一个子地图，并且还需要考虑存在闭环检测的情况，即将与第一子地图存在闭环的子地图同样作为第二子地图。
另外，将各第一子地图和与其相关的第二子地图进行配准，建立各第一子地图和第二子地图的第一位姿约束关系，包括对每个第一子地图执行以下步骤：
根据第一子地图对应的初始位姿和第二子地图对应的初始位姿，采用NDT算法得到第一子地图和第二子地图对应的位姿变换关系
Figure BDA0002382026260000151
和
Figure BDA0002382026260000152
其中，第二子地图对应的初始位姿为第二子地图第一帧激光点云数据对应的激光雷达位姿；
Figure BDA0002382026260000153
为相邻的第i个子地图和第i+1个子地图对应的位姿变换关系；
Figure BDA0002382026260000154
为第i个子地图和与第i子地图存在闭环的第j个子地图对应的位姿变换关系。
建立第一子地图和第二子地图的第一位姿误差函数：
Figure BDA0002382026260000155
以及
Figure BDA0002382026260000156
其中，
Figure BDA0002382026260000157
表示第i个子地图和第i+1个子地图的第一位姿误差，Xi表示第i个子地图对应的初始位姿，Xi+1表示第i+1个子地图对应的初始位姿；
Figure BDA0002382026260000158
表示第i个子地图和第j个子地图的第一位姿误差，Xj表示第j个子地图对应的初始位姿。此处，Xi、Xi+1和Xj可以通过上述步骤311中的第一激光雷达优化位姿的确定方式得到。
另外，建立各第一子地图和各第一子地图对应的初始位姿的第二位姿约束关系，包括对每个第一子地图执行以下步骤：
建立第一子地图和第一子地图对应的初始位姿的第二位姿误差函数：
Figure BDA0002382026260000159
其中，Xi表示第i个子地图对应的初始位姿，
Figure BDA00023820262600001510
表示根据组合导航系统位姿和预先获得的激光雷达与组合导航系统的外参标定结果，确定得到的第i个子地图对应的激光雷达位姿。
步骤313、根据第一位姿约束关系和第二位姿约束关系进行全局优化，确定各第一子地图的子地图优化位姿。
其中，对应于上述步骤312中的方式一和方式二，该步骤313也可以通过如下方式一和方式二实现，其中方式一是步骤312中的方式一对应的后续处理过程，方式二是步骤312中的方式二对应的后续处理过程。
方式一：
其中，根据第一位姿约束关系和第二位姿约束关系进行全局优化，确定各第一子地图的子地图优化位姿，可以采用如下方式：
根据第一位姿误差函数：
Figure BDA0002382026260000161
和第二位姿误差函数
Figure BDA0002382026260000162
确定第一全局误差函数：
Figure BDA0002382026260000163
其中，i表示第i个子地图；n表示子地图数目；Ωi(i+1)和Ωii为预先设置的信息矩阵，表示误差置信度。
对第一全局误差函数进行迭代求解(例如采用高斯牛顿法进行迭代求解)，确定E1最小时各第一子地图的子地图优化位姿。
方式二：
其中，根据第一位姿约束关系和第二位姿约束关系进行全局优化，确定各第一子地图的子地图优化位姿，可以采用如下方式：
根据第一位姿误差函数：
Figure BDA0002382026260000164
以及
Figure BDA0002382026260000165
和第二位姿误差函数
Figure BDA0002382026260000166
确定第二全局误差函数：
Figure BDA0002382026260000167
其中，i表示第i个子地图，j表示第j个子地图；n表示子地图数目；Ωi(i+1)、Ωij和Ωii为预先设置的信息矩阵，表示误差置信度。
对所述第二全局误差函数进行迭代求解(例如采用高斯牛顿法进行迭代求解)，确定E2最小时各第一子地图的子地图优化位姿。
此处的全局优化所采用的方式一般为图优化，图优化是指把优化问题表示为图的形式，图由节点和边组成，优化过程即通过调整节点的位姿使得最大程度满足边的约束。如图4所示，上述第二全局误差函数可以以全局位姿关系图表示，其中，虚线表示车辆行驶的路线，S表示子地图(例如当Si为第一子地图时，Si-1和Sk为与第一子地图Si相关的第二子地图)，G表示第一子地图和第一子地图对应的初始位姿的第二位姿约束关系，实线表示各节点之间的约束关系，即上述的第一位姿误差函数：
Figure BDA0002382026260000171
以及
Figure BDA0002382026260000172
和第二位姿误差函数
Figure BDA0002382026260000173
步骤314、根据第一激光雷达优化位姿与第一子地图对应位姿的第一相对位姿关系，确定子地图优化位姿对应的采集一帧激光点云数据对应的激光雷达的帧优化位姿。
从第一相对位姿关系中可以得到第一激光雷达优化位姿与第一子地图对应位姿的相对关系，从而在确定各第一子地图的子地图优化位姿时，可以通过该第一相对位姿关系得到子地图优化位姿对应的采集一帧激光点云数据对应的激光雷达的帧优化位姿。
步骤315、根据帧优化位姿和各帧激光点云中的点在激光雷达坐标系下的坐标，将各帧激光点云中的点映射到世界坐标系下。
步骤316、将映射到世界坐标系下的激光点云中的点进行叠加，形成全局地图。
为验证本申请实施例中上述步骤301至步骤316所提方法的有效性及泛化能力，本申请的一实施例中使用定位精度高的地图车、定位精度相对较低的卡车进行了建图实验，并从地图的全局一致性、局部一致性对建图结果进行分析。此外，为增强实验结果的说服力，本申请实施例在车辆上装备了高精度的组合导航系统(例如NovAtel)，以组合导航系统输出的结果(通过组合导航系统的定位最终得到的参考全局地图)作为真实值，并将参考全局地图与本申请实施例形成的全局地图(称为待验证全局地图)相对比。
如图5和图6所示，对于地图车得到的待验证全局地图，其道路结构清晰，车道线、护栏轮廓规整，地面厚度较小，因此待验证全局地图的局部一致性较好。
如图7和图8所示，对于卡车得到的待验证全局地图，其道路结构清晰，车道线、护栏轮廓规整，地面厚度较小，因此待验证全局地图的局部一致性较好。
另外，如图9所示，本申请实施例还提供一种地图构建装置，包括：
数据采集单元41，用于实时获得可移动物体上搭载的传感器采集的传感器数据，确定预设帧长度的第一子地图中的各帧数据。
帧与子地图配准单元42，用于根据各帧数据和第一子地图中的若干帧数据，进行各帧与第一子地图的配准，确定各帧与所述第一子地图的第一相对位姿关系；若干帧数据为所述第一子地图中第一帧到各帧前一帧的数据。
子地图与子地图配准单元43，用于根据各第一子地图对应的初始位姿确定与各第一子地图相关的第二子地图，将各第一子地图和与其相关的第二子地图进行配准，建立各第一子地图和第二子地图的第一位姿约束关系，并建立各第一子地图和各第一子地图对应的初始位姿的第二位姿约束关系。
全局优化单元44，用于根据第一位姿约束关系和第二位姿约束关系进行全局优化，确定各第一子地图的子地图优化位姿。
帧优化位姿确定单元45，用于根据第一相对位姿关系和子地图优化位姿，确定各第一子地图中各帧数据对应的帧优化位姿。
全局地图构建单元46，用于根据帧优化位姿和各帧数据，进行地图构建，形成全局地图。
另外，本申请实施例还提供一种计算机可读存储介质，包括程序或指令，当程序或指令在计算机上运行时，实现上述图1或图3所述的地图构建方法。
另外，本申请实施例还提供一种包含指令的计算机程序产品，当计算机程序产品在计算机上运行时，使得计算机执行如上述图1或图3所述的地图构建方法。
另外，本申请实施例还提供一种芯片系统，包括处理器，处理器与存储器的耦合，存储器存储有程序指令，当存储器存储的程序指令被所述处理器执行时实现上述图1或图3所述的地图构建方法。
另外，本申请实施例还提供一种电路系统，电路系统包括处理电路，处理电路配置为执行如上述图1或图3的地图构建方法。
另外，本申请实施例还提供一种计算机服务器，包括存储器，以及与存储器通信连接的一个或多个处理器；
存储器中存储有可被一个或多个处理器执行的指令，所述指令被一个或多个处理器执行，以使一个或多个处理器实现如上述图1或图3所述的地图构建方法。
本申请实施例提供的一种地图构建方法及装置，能够实时获得可移动物体上搭载的传感器采集的传感器数据，并进行各帧与第一子地图的配准，确定各帧与第一子地图的第一相对位姿关系。之后在确定与第一子地图相关的第二子地图后，能够将第一子地图与第二子地图进行配准，并结合第一子地图的初始位姿，建立位姿约束关系。从而在对位姿约束关系进行全局优化后，可以确定各第一子地图的子地图优化位姿。进而可以根据上述的第一相对位姿关系和子地图优化位姿，确定各第一子地图中各帧数据对应的帧优化位姿。进而可以根据帧优化位姿和各帧数据，进行地图构建，形成全局地图。这样，通过本申请实施例可以克服现有技术中地图构建方法存在的地图一致性较差，受激光雷达等传感器的标定精度的影响较大的问题。
本领域内的技术人员应明白，本申请的实施例可提供为方法、系统、或计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。
本申请是参照根据本申请实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
本申请中应用了具体实施例对本申请的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本申请的方法及其核心思想；同时，对于本领域的一般技术人员，依据本申请的思想，在具体实施方式及应用范围上均会有改变之处，综上所述，本说明书内容不应理解为对本申请的限制。

Claims (28)

1.一种地图构建方法，其特征在于，包括：
实时获得可移动物体上搭载的传感器采集的传感器数据，确定预设帧长度的第一子地图中的各帧数据；
根据各帧数据和所述第一子地图中的若干帧数据，进行各帧与所述第一子地图的配准，确定各帧与所述第一子地图的第一相对位姿关系；所述若干帧数据为所述第一子地图中第一帧到各帧前一帧的数据；
根据各第一子地图对应的初始位姿确定与各第一子地图相关的第二子地图，将各第一子地图和与其相关的第二子地图进行配准，建立各第一子地图和第二子地图的第一位姿约束关系，并建立各第一子地图和各第一子地图对应的初始位姿的第二位姿约束关系；
根据所述第一位姿约束关系和第二位姿约束关系进行全局优化，确定各第一子地图的子地图优化位姿；
根据所述第一相对位姿关系和所述子地图优化位姿，确定各第一子地图中各帧数据对应的帧优化位姿；
根据所述帧优化位姿和各帧数据，进行地图构建，形成全局地图。
2.根据权利要求1所述的地图构建方法，其特征在于，所述传感器为激光雷达，所述传感器数据为激光点云数据，所述各帧数据为激光雷达采集的各帧激光点云数据。
3.根据权利要求2所述的地图构建方法，其特征在于，在实时获得可移动物体上搭载的传感器采集的传感器数据，确定预设帧长度的第一子地图中的各帧数据之后，包括：
将各帧激光点云数据进行运动补偿，确定各帧激光点云数据中的点的运动补偿后的位置；
通过各帧激光点云数据进行动态目标检测，从各帧激光点云数据中确定干扰物体对应的点，并将所述干扰物体对应的点剔除；
对进行运动补偿和动态目标检测及剔除后的各帧激光点云数据进行特征提取，确定满足预先设置的特征提取策略的特征点。
4.根据权利要求3所述的地图构建方法，其特征在于，所述将各帧激光点云数据进行运动补偿，确定各帧激光点云数据中的点的运动补偿后的位置，包括对每一帧激光点云数据执行以下步骤：
获得采集一帧激光点云数据的开始时刻和结束时刻的激光雷达位姿；
在采集一帧激光点云数据的开始时刻和结束时刻之间进行时间戳插值得到一帧激光点云数据中采集每个点的时刻对应的激光雷达位姿；
根据采集每个点的时刻对应的激光雷达位姿，以及每个点在激光雷达坐标系下的坐标，确定每个点的运动补偿后的位置。
5.根据权利要求3所述的地图构建方法，其特征在于，所述干扰物体为预先设置的运动物体。
6.根据权利要求3所述的地图构建方法，其特征在于，所述对进行运动补偿和动态目标检测及剔除后的各帧激光点云数据进行特征提取，确定满足预先设置的特征提取策略的特征点，包括：
在进行运动补偿和动态目标检测及剔除后的一帧激光点云数据中获得一个线束上的任一点及该线束上的任一点预设范围内的若干点；
根据该任一点在激光雷达坐标系下的坐标和该线束上的任一点预设范围内的若干点在激光雷达坐标系下的坐标，确定该任一点处的曲率；
根据预先设置的曲率阈值，在一点的曲率大于所述曲率阈值时，将该一点作为线特征点，在一点的曲率小于所述曲率阈值时，将该一点作为平面特征点；
根据预先设置的特征点数目阈值，从所述曲率阈值开始剔除曲率阈值两侧曲率对应的线特征点和平面特征点，直至剩余的线特征点和平面特征点的数目之和小于等于所述特征点数目阈值，并获得该剩余的线特征点和平面特征点。
7.根据权利要求6所述的地图构建方法，其特征在于，所述根据各帧数据和所述第一子地图中的若干帧数据，进行各帧与所述第一子地图的配准，确定各帧与所述第一子地图的第一相对位姿关系，包括对每一帧激光点云数据执行以下步骤：
获得采集一帧激光点云数据对应的激光雷达位姿，作为配准位姿初始值；其中，一帧激光点云数据中包括所述剩余的线特征点和平面特征点；
根据预先采用KD-Tree算法为每个点建立的数据索引，搜索得到每个点对应的若干近邻点；其中所述若干近邻点为第一子地图中的若干帧数据中的点；
对一帧激光点云数据中的线特征点，根据该线特征点对应的配准位姿初始值和对应的若干近邻点，建立线特征点误差函数；
对一帧激光点云数据中的面特征点，根据该面特征点对应的配准位姿初始值和对应的若干近邻点，建立面特征点误差函数；
根据所述线特征点误差函数和面特征点误差函数，确定帧至子地图配准的误差函数；
对所述帧至子地图配准的误差函数进行求解，得到一帧激光点云数据对应的第一激光雷达优化位姿，并确定第一激光雷达优化位姿与第一子地图对应位姿的第一相对位姿关系；所述第一子地图对应位姿为第一子地图第一帧激光点云数据对应的激光雷达位姿。
8.根据权利要求7所述的地图构建方法，其特征在于，所述获得采集一帧激光点云数据对应的激光雷达位姿，作为配准位姿初始值，包括：
通过组合导航系统获得采集一帧激光点云数据对应的组合导航系统位姿；
根据所述组合导航系统位姿和预先获得的激光雷达与组合导航系统的外参标定结果，确定该一帧激光点云数据对应的激光雷达位姿。
9.根据权利要求7所述的地图构建方法，其特征在于，所述对一帧激光点云数据中的线特征点，根据该线特征点对应的配准位姿初始值和对应的若干近邻点，建立线特征点误差函数，包括：
根据一帧激光点云数据中的线特征点
Figure FDA0002382026250000031
对应的若干近邻点拟合得到一条直线，将线特征点
Figure FDA0002382026250000032
与该直线的距离函数作为线特征点误差函数；
所述线特征点误差函数为：
Figure FDA0002382026250000033
其中，
Figure FDA0002382026250000034
和
Figure FDA0002382026250000035
为该直线上的任意两点，R和t表示配准位姿初始值，其中R表示配准位姿初始值中的姿态，t表示配准位姿初始值中的位置。
10.根据权利要求9所述的地图构建方法，其特征在于，所述对一帧激光点云数据中的面特征点，根据该面特征点对应的配准位姿初始值和对应的若干近邻点，建立面特征点误差函数，包括：
根据一帧激光点云数据中的面特征点
Figure FDA0002382026250000036
对应的若干近邻点拟合得到一个平面Ax+By+Cz+D＝0，将面特征点
Figure FDA0002382026250000037
与该平面的距离函数作为面特征点误差函数；其中，k表示一帧激光点云数据中的第k条线，i表示第i个点，A、B、C和D表示拟合得到的平面的参数；
Figure FDA0002382026250000041
为面特征点在激光雷达坐标系下的坐标；
所述面特征点误差函数为：
Figure FDA0002382026250000042
其中，R和t表示配准位姿初始值，其中R表示配准位姿初始值中的姿态，t表示配准位姿初始值中的位置；n表示矩阵：n＝(A,B,C)。
11.根据权利要求10所述的地图构建方法，其特征在于，根据所述线特征点误差函数和面特征点误差函数，确定帧至子地图配准的误差函数，包括：
根据所述线特征点误差函数
Figure FDA0002382026250000043
和面特征点误差函数
Figure FDA0002382026250000044
确定帧至子地图配准的误差函数
Figure FDA0002382026250000045
其中，nline表示该一帧激光点云数据中的线特征点数目，nplane表示该一帧激光点云数据中的面特征点数目；
Figure FDA0002382026250000046
表示一帧激光点云数据中的第i个线特征点的线特征点误差，
Figure FDA0002382026250000047
表示一帧激光点云数据中的第i个面特征点的面特征点误差。
12.根据权利要求11所述的地图构建方法，其特征在于，对所述帧至子地图配准的误差函数进行求解，得到一帧激光点云数据对应的第一激光雷达优化位姿，包括：
对所述帧至子地图配准的误差函数
Figure FDA0002382026250000048
进行迭代求解，得到Eframe取得最小值时对应的姿态R和位置t，作为一帧激光点云数据对应的第一激光雷达优化位姿。
13.根据权利要求2所述的地图构建方法，其特征在于，所述根据各第一子地图对应的初始位姿确定与各第一子地图相关的第二子地图，包括对每个第一子地图执行以下步骤：
根据第一子地图对应的初始位姿，确定与第一子地图相邻的上一个子地图作为第二子地图；所述第一子地图对应的初始位姿为第一子地图第一帧激光点云数据对应的激光雷达位姿。
14.根据权利要求13所述的地图构建方法，其特征在于，所述将各第一子地图和与其相关的第二子地图进行配准，建立各第一子地图和第二子地图的第一位姿约束关系，包括对每个第一子地图执行以下步骤：
根据第一子地图对应的初始位姿和第二子地图对应的初始位姿，采用NDT算法得到第一子地图和第二子地图对应的位姿变换关系
Figure FDA0002382026250000051
其中，所述第二子地图对应的初始位姿为第二子地图第一帧激光点云数据对应的激光雷达位姿；
建立第一子地图和第二子地图的第一位姿误差函数：
Figure FDA0002382026250000052
其中，
Figure FDA0002382026250000053
表示第i个子地图和第i+1个子地图对应的位姿变换关系，
Figure FDA0002382026250000054
表示第i个子地图和第i+1个子地图的第一位姿误差，Xi表示第i个子地图对应的初始位姿，Xi+1表示第i+1个子地图对应的初始位姿。
15.根据权利要求14所述的地图构建方法，其特征在于，所述建立各第一子地图和各第一子地图对应的初始位姿的第二位姿约束关系，包括对每个第一子地图执行以下步骤：
建立第一子地图和第一子地图对应的初始位姿的第二位姿误差函数：
Figure FDA0002382026250000055
其中，Xi表示第i个子地图对应的初始位姿，
Figure FDA0002382026250000056
表示根据组合导航系统位姿和预先获得的激光雷达与组合导航系统的外参标定结果，确定得到的第i个子地图对应的激光雷达位姿。
16.根据权利要求15所述的地图构建方法，其特征在于，所述根据所述第一位姿约束关系和第二位姿约束关系进行全局优化，确定各第一子地图的子地图优化位姿，包括：
根据所述第一位姿误差函数：
Figure FDA0002382026250000057
和第二位姿误差函数
Figure FDA0002382026250000058
确定第一全局误差函数：
Figure FDA0002382026250000059
其中，i表示第i个子地图；n表示子地图数目；Ωi(i+1)和Ωii为预先设置的信息矩阵，表示误差置信度；
对所述第一全局误差函数进行迭代求解，确定E1最小时各第一子地图的子地图优化位姿。
17.根据权利要求2所述的地图构建方法，其特征在于，所述根据各第一子地图对应的初始位姿确定与各第一子地图相关的第二子地图，包括对每个第一子地图执行以下步骤：
根据第一子地图对应的初始位姿，确定与第一子地图相邻的上一个子地图以及与第一子地图存在闭环的子地图作为第二子地图；所述与第一子地图存在闭环的子地图对应的初始位姿与所述第一子地图对应的初始位姿之间的距离小于等于预先设置的距离阈值；所述第一子地图对应的初始位姿为第一子地图第一帧激光点云数据对应的激光雷达位姿；所述与第一子地图存在闭环的子地图对应的初始位姿为与第一子地图存在闭环的子地图第一帧激光点云数据对应的激光雷达位姿。
18.根据权利要求17所述的地图构建方法，其特征在于，所述将各第一子地图和与其相关的第二子地图进行配准，建立各第一子地图和第二子地图的第一位姿约束关系，包括对每个第一子地图执行以下步骤：
根据第一子地图对应的初始位姿和第二子地图对应的初始位姿，采用NDT算法得到第一子地图和第二子地图对应的位姿变换关系
Figure FDA0002382026250000061
和
Figure FDA0002382026250000062
其中，所述第二子地图对应的初始位姿为第二子地图第一帧激光点云数据对应的激光雷达位姿；
Figure FDA0002382026250000063
为相邻的第i个子地图和第i+1个子地图对应的位姿变换关系；
Figure FDA0002382026250000064
为第i个子地图和与第i子地图存在闭环的第j个子地图对应的位姿变换关系；
建立第一子地图和第二子地图的第一位姿误差函数：
Figure FDA0002382026250000065
以及
Figure FDA0002382026250000066
其中，
Figure FDA0002382026250000067
表示第i个子地图和第i+1个子地图的第一位姿误差，Xi表示第i个子地图对应的初始位姿，Xi+1表示第i+1个子地图对应的初始位姿；
Figure FDA0002382026250000068
表示第i个子地图和第j个子地图的第一位姿误差，Xj表示第j个子地图对应的初始位姿。
19.根据权利要求18所述的地图构建方法，其特征在于，所述建立各第一子地图和各第一子地图对应的初始位姿的第二位姿约束关系，包括对每个第一子地图执行以下步骤：
建立第一子地图和第一子地图对应的初始位姿的第二位姿误差函数：
Figure FDA0002382026250000069
其中，Xi表示第i个子地图对应的初始位姿，
Figure FDA00023820262500000610
表示根据组合导航系统位姿和预先获得的激光雷达与组合导航系统的外参标定结果，确定得到的第i个子地图对应的激光雷达位姿。
20.根据权利要求19所述的地图构建方法，其特征在于，所述根据所述第一位姿约束关系和第二位姿约束关系进行全局优化，确定各第一子地图的子地图优化位姿，包括：
根据所述第一位姿误差函数：
Figure FDA00023820262500000611
以及
Figure FDA00023820262500000612
和第二位姿误差函数
Figure FDA00023820262500000613
确定第二全局误差函数：
Figure FDA0002382026250000071
其中，i表示第i个子地图，j表示第j个子地图；n表示子地图数目；Ωi(i+1)、Ωij和Ωii为预先设置的信息矩阵，表示误差置信度；
对所述第二全局误差函数进行迭代求解，确定E2最小时各第一子地图的子地图优化位姿。
21.根据权利要求7所述的地图构建方法，其特征在于，所述根据所述第一相对位姿关系和所述子地图优化位姿，确定各第一子地图中各帧数据对应的帧优化位姿，包括：
根据第一激光雷达优化位姿与第一子地图对应位姿的第一相对位姿关系，确定子地图优化位姿对应的采集一帧激光点云数据对应的激光雷达的帧优化位姿。
22.根据权利要求21所述的地图构建方法，其特征在于，所述帧数据包括各帧激光点云中的点在激光雷达坐标系下的坐标；
所述根据所述帧优化位姿和各帧数据，进行地图构建，形成全局地图，包括：
根据所述帧优化位姿和各帧激光点云中的点在激光雷达坐标系下的坐标，将各帧激光点云中的点映射到世界坐标系下；
将映射到世界坐标系下的激光点云中的点进行叠加，形成全局地图。
23.一种地图构建装置，其特征在于，包括：
数据采集单元，用于实时获得可移动物体上搭载的传感器采集的传感器数据，确定预设帧长度的第一子地图中的各帧数据；
帧与子地图配准单元，用于根据各帧数据和所述第一子地图中的若干帧数据，进行各帧与所述第一子地图的配准，确定各帧与所述第一子地图的第一相对位姿关系；所述若干帧数据为所述第一子地图中第一帧到各帧前一帧的数据；
子地图与子地图配准单元，用于根据各第一子地图对应的初始位姿确定与各第一子地图相关的第二子地图，将各第一子地图和与其相关的第二子地图进行配准，建立各第一子地图和第二子地图的第一位姿约束关系，并建立各第一子地图和各第一子地图对应的初始位姿的第二位姿约束关系；
全局优化单元，用于根据所述第一位姿约束关系和第二位姿约束关系进行全局优化，确定各第一子地图的子地图优化位姿；
帧优化位姿确定单元，用于根据所述第一相对位姿关系和所述子地图优化位姿，确定各第一子地图中各帧数据对应的帧优化位姿；
全局地图构建单元，用于根据所述帧优化位姿和各帧数据，进行地图构建，形成全局地图。
24.一种计算机可读存储介质，其特征在于，包括程序或指令，当所述程序或指令在计算机上运行时，实现权利要求1至22任一项所述的地图构建方法。
25.一种包含指令的计算机程序产品，其特征在于，当所述计算机程序产品在计算机上运行时，使得所述计算机执行如权利要求1至22任一项所述的地图构建方法。
26.一种芯片系统，其特征在于，包括处理器，所述处理器与存储器的耦合，所述存储器存储有程序指令，当所述存储器存储的程序指令被所述处理器执行时实现权利要求1至22任一项所述的地图构建方法。
27.一种电路系统，其特征在于，所述电路系统包括处理电路，所述处理电路配置为执行如权利要求1至22任一项所述的地图构建方法。
28.一种计算机服务器，其特征在于，包括存储器，以及与所述存储器通信连接的一个或多个处理器；
所述存储器中存储有可被所述一个或多个处理器执行的指令，所述指令被所述一个或多个处理器执行，以使所述一个或多个处理器实现如权利要求1至22任一项所述的地图构建方法。
CN202010087732.4A 2020-02-11 2020-02-11 一种地图构建方法及装置 Pending CN113252051A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010087732.4A CN113252051A (zh) 2020-02-11 2020-02-11 一种地图构建方法及装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010087732.4A CN113252051A (zh) 2020-02-11 2020-02-11 一种地图构建方法及装置

Publications (1)

Publication Number Publication Date
CN113252051A true CN113252051A (zh) 2021-08-13

Family

ID=77219597

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010087732.4A Pending CN113252051A (zh) 2020-02-11 2020-02-11 一种地图构建方法及装置

Country Status (1)

Country Link
CN (1) CN113252051A (zh)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114199233A (zh) * 2021-11-08 2022-03-18 北京旷视科技有限公司 位姿确定方法及可移动设备
CN114549605A (zh) * 2021-12-31 2022-05-27 广州景骐科技有限公司 基于点云匹配的图像优化方法、装置、设备及存储介质
CN115493603A (zh) * 2022-11-17 2022-12-20 安徽蔚来智驾科技有限公司 地图对齐方法、计算机设备及计算机可读存储介质

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114199233A (zh) * 2021-11-08 2022-03-18 北京旷视科技有限公司 位姿确定方法及可移动设备
CN114199233B (zh) * 2021-11-08 2024-04-05 北京旷视科技有限公司 位姿确定方法及可移动设备
CN114549605A (zh) * 2021-12-31 2022-05-27 广州景骐科技有限公司 基于点云匹配的图像优化方法、装置、设备及存储介质
CN114549605B (zh) * 2021-12-31 2023-08-04 广州景骐科技有限公司 基于点云匹配的图像优化方法、装置、设备及存储介质
CN115493603A (zh) * 2022-11-17 2022-12-20 安徽蔚来智驾科技有限公司 地图对齐方法、计算机设备及计算机可读存储介质
CN115493603B (zh) * 2022-11-17 2023-03-10 安徽蔚来智驾科技有限公司 地图对齐方法、计算机设备及计算机可读存储介质

Similar Documents

Publication Publication Date Title
CN111862672B (zh) 基于顶视图的停车场车辆自定位及地图构建方法
CN107246876B (zh) 一种无人驾驶汽车自主定位与地图构建的方法及系统
JP2022019642A (ja) マルチセンサ融合に基づく測位方法及び装置
CN113819914A (zh) 一种地图构建方法及装置
CN111142091B (zh) 一种融合车载信息的自动驾驶系统激光雷达在线标定方法
CN113819905A (zh) 一种基于多传感器融合的里程计方法及装置
CN113252051A (zh) 一种地图构建方法及装置
EP4124829A1 (en) Map construction method, apparatus, device and storage medium
CN111649740B (zh) 一种基于imu进行车辆高精度定位的方法及系统
CN110119138A (zh) 用于自动驾驶车辆的自定位方法、系统和机器可读介质
CN113252022A (zh) 一种地图数据处理方法及装置
Kellner et al. Road curb detection based on different elevation mapping techniques
Moras et al. Drivable space characterization using automotive lidar and georeferenced map information
US20220035036A1 (en) Method and apparatus for positioning movable device, and movable device
Jiménez et al. Improving the lane reference detection for autonomous road vehicle control
CN111402328B (zh) 一种基于激光里程计的位姿计算方法及装置
CN111257853B (zh) 一种基于imu预积分的自动驾驶系统激光雷达在线标定方法
CN113920198B (zh) 一种基于语义边缘对齐的由粗到精的多传感器融合定位方法
Hara et al. Vehicle localization based on the detection of line segments from multi-camera images
Parra-Tsunekawa et al. A kalman-filtering-based approach for improving terrain mapping in off-road autonomous vehicles
US11315269B2 (en) System and method for generating a point cloud that includes surface normal information
CN111708010B (zh) 一种可移动设备的定位方法、装置、系统及可移动设备
CN115792894A (zh) 一种多传感器融合车辆目标跟踪方法、系统及存储介质
CN115235452A (zh) 基于uwb/imu和视觉信息融合的智能泊车定位系统及方法
CN111103578B (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