CN116734871A - 地图构建方法、定位方法、数据采集车辆及车辆 - Google Patents

地图构建方法、定位方法、数据采集车辆及车辆 Download PDF

Info

Publication number
CN116734871A
CN116734871A CN202310318366.2A CN202310318366A CN116734871A CN 116734871 A CN116734871 A CN 116734871A CN 202310318366 A CN202310318366 A CN 202310318366A CN 116734871 A CN116734871 A CN 116734871A
Authority
CN
China
Prior art keywords
radar
dimensional grid
grid
point
coordinates
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
CN202310318366.2A
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.)
Faw Nanjing Technology Development Co ltd
FAW Group Corp
Original Assignee
Faw Nanjing Technology Development Co ltd
FAW Group Corp
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 Faw Nanjing Technology Development Co ltd, FAW Group Corp filed Critical Faw Nanjing Technology Development Co ltd
Priority to CN202310318366.2A priority Critical patent/CN116734871A/zh
Publication of CN116734871A publication Critical patent/CN116734871A/zh
Pending legal-status Critical Current

Links

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
    • G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
    • G01S13/88—Radar or analogous systems specially adapted for specific applications
    • G01S13/89—Radar or analogous systems specially adapted for specific applications for mapping or imaging
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T17/00—Three dimensional [3D] modelling, e.g. data description of 3D objects
    • G06T17/05—Geographic models
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T17/00—Three dimensional [3D] modelling, e.g. data description of 3D objects
    • G06T17/20—Finite element generation, e.g. wire-frame surface description, tesselation
    • 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)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • General Physics & Mathematics (AREA)
  • Software Systems (AREA)
  • Geometry (AREA)
  • Theoretical Computer Science (AREA)
  • Computer Graphics (AREA)
  • Automation & Control Theory (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Electromagnetism (AREA)
  • Radar Systems Or Details Thereof (AREA)

Abstract

本发明公开了一种地图构建方法、定位方法、数据采集车辆及车辆，其中，定位方法包括：获取点云数据，其中，点云数据是通过雷达扫描真实世界得到的，点云数据中包括多个第一雷达点；确定每个第一雷达点在站心坐标系下的坐标得到第一雷达点坐标，其中，站心坐标系的原点为雷达的中心；确定每个第一雷达点在站心坐标系下的法向量得到第一雷达点法向量；根据第一雷达点坐标和第一雷达点法向量构建一级三维网格；根据一级三维网格构建二级三维网格；根据一级三维网格和二级三维网格确定目标地图。本发明解决了相关技术中车辆在进行定位时定位精度差的技术问题。

Description

地图构建方法、定位方法、数据采集车辆及车辆
技术领域
本发明涉及智能驾驶技术领域，具体而言，涉及一种地图构建方法、定位方法、数据采集车辆及车辆。
背景技术
即时定位与地图构建(Simultaneous Localization and Mapping，SLAM)技术，是自动驾驶领域应用较广的定位方案之一，通过建立高精度地图，并在已有的地图中进行定位。SLAM技术在自动驾驶领域的具体应用可以分为建图和定位两部分。现有技术中都是采用卡尔曼滤波进行定位，因此如何构建误差模型则成为一个比较难处理的技术问题。目前在构建测量方程和预测方程时，大多数的技术方案都是在理想情况下估计的，因此无法预估其准确性，从而导致了车辆定位精度差的技术问题。
针对上述的问题，目前尚未提出有效的解决方案。
发明内容
本发明实施例提供了一种地图构建方法、定位方法、数据采集车辆及车辆，以至少解决相关技术中车辆在进行定位时定位精度差的技术问题。
根据本发明其中一实施例，提供了一种地图构建方法，包括：获取点云数据，其中，点云数据是通过雷达扫描真实世界得到的，点云数据中包括多个第一雷达点，确定每个第一雷达点在站心坐标系下的坐标得到第一雷达点坐标，其中，站心坐标系的原点为雷达的中心，确定每个第一雷达点在站心坐标系下的法向量得到第一雷达点法向量，根据第一雷达点坐标和第一雷达点法向量构建一级三维网格，根据一级三维网格构建二级三维网格，根据一级三维网格和二级三维网格确定目标地图。
可选地，地图构建的方法还包括：确定每个第一雷达点的曲率，将每个第一雷达点的曲率与预设曲率阈值进行对比得到对比结果，响应于对比结果表示第一雷达点的曲率小于预设曲率阈值，确定第一雷达点所在平面，根据第一雷达点所在平面，确定第一雷达点法向量。
可选地，地图构建的方法还包括：根据第一雷达点的多个空间最近邻点和第一雷达点确定第一雷达点的曲率，其中，空间最近邻点为雷达同一扫描线束上相邻的点。
可选地，地图构建的方法还包括：在站心坐标系下划分多个预设一级三维网格，根据第一雷达点坐标确定第一雷达点所属的预设一级三维网格得到匹配网格，将第一雷达点投影至匹配网格，根据匹配网格中包含的第一雷达点坐标和第一雷达点法向量，确定匹配网格的网格坐标和网格法向量，根据网格坐标和网格法向量，构建一级三维网格。
根据本发明其中一实施例，提供了一种定位方法，包括：获取待定位主体的初始位姿和待定位主体周围环境的当前点云数据，根据初始位姿，加载目标地图，其中，目标地图通过上述实施例中的地图构建方法构建得到，目标地图中包括一级三维网格和二级三维网格，根据当前点云数据，从目标地图中确定目标一级三维网格，其中，目标一级三维网格为当前点云数据对应的一级三维网格，根据当前点云数据和目标一级三维网格，利用欧式距离公式对初始位姿进行优化得到目标位姿。
可选地，定位方法还包括：根据初始位姿中的位置坐标，确定待定位主体当前所属的二级三维网格，加载目标地图中的待定位主体当前所属的二级三维网格作为目标二级三维网格，根据当前点云数据，从目标地图中确定目标一级三维网格，根据当前点云数据，从目标二级三维网格中确定目标一级三维网格。
可选地，定位方法还包括：当前点云数据中包括多个第二雷达点一级三维网格中包括网格坐标和网格法向量，每个第二雷达点有一个目标一级三维网格与之对应，根据当前点云数据和目标一级三维网格，利用欧式距离公式对初始位姿进行优化得到目标位姿包括：根据预设调整策略和欧式距离公式，对初始位姿进行迭代调整，使欧式距离之和小于预设距离阈值并且记录当前位姿作为目标位姿，其中，欧式距离之和为每个第二雷达点到与之对应的网格坐标所在平面的欧式距离之和。
可选地，定位方法还包括：将第二雷达点在雷达坐标系下的坐标转化为站心坐标系下的坐标得到第二雷达点坐标，针对每个第二雷达点，根据第二雷达点坐标、目标一级三维网格中的网格坐标和目标一级三维网格中的网格法向量，利用欧式距离公式确定第二雷达点到与之对应的网格坐标所在平面的欧式距离。
根据本发明其中一实施例，还提供了一种数据采集车辆，包括存储器和处理器，存储器中存储有计算机程序，处理器被设置为运行计算机程序以执行上述任一项实施例所述的地图构建方法。
根据本发明其中一实施例，还提供了一种车辆，包括存储器和处理器，存储器中存储有计算机程序，处理器被设置为运行计算机程序以执行上述任一项实施例中的定位方法。
在本发明实施例中，采用获取点云数据，其中，点云数据是通过雷达扫描真实世界得到的，点云数据中包括多个第一雷达点，确定每个第一雷达点在站心坐标系下的坐标得到第一雷达点坐标，其中，站心坐标系的原点为雷达的中心，确定每个第一雷达点在站心坐标系下的法向量得到第一雷达点法向量，根据第一雷达点坐标和第一雷达点法向量构建一级三维网格，再根据一级三维网格构建二级三维网格，通过一级三维网格和二级三维网格确定目标地图，达到了可以通过一级三维网格和二级三维网格对车辆进行自主定位的目的，从而实现了提高车辆定位精度的技术效果，进而可以解决相关技术中车辆在进行定位时定位精度差的技术问题。
附图说明
此处所说明的附图用来提供对本发明的进一步理解，构成本申请的一部分，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。在附图中：
图1是根据本发明其中一实施例的地图构建方法的流程图；
图2是根据本发明其中一实施例的定位方法的流程图；
图3是根据本发明其中一实施例的法向量地图构建方法的流程图；
图4是根据本发明其中一实施例的雷达匹配定位方法的流程图。
具体实施方式
为了使本技术领域的人员更好地理解本发明方案，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分的实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本发明保护的范围。
需要说明的是，本发明的说明书和权利要求书及上述附图中的术语“第一”、“第二”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的术语在适当情况下可以互换，以便这里描述的本发明的实施例能够以除了在这里图示或描述的那些以外的顺序实施。此外，术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或单元的过程、方法、系统、产品或设备不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。
根据本发明实施例，提供了一种地图构建方法的实施例，需要说明的是，在附图的流程图示出的步骤可以在包含至少一组计算机可执行指令的计算机系统中执行，并且，虽然在流程图中示出了逻辑顺序，但是在某些情况下，可以以不同于此处的顺序执行所示出或描述的步骤。
该方法实施例还可以在包含存储器和处理器的电子装置、类似的控制装置或者车载终端中执行。以车载终端为例，车载终端可以包括一个或多个处理器和用于存储数据的存储器。可选地，上述车载终端还可以包括用于通信功能的通信设备以及显示设备。本领域普通技术人员可以理解，上述结构描述仅为示意，其并不对上述车载终端的结构造成限定。例如，车载终端还可包括比上述结构描述更多或者更少的组件，或者具有与上述结构描述不同的配置。
处理器可以包括一个或多个处理单元。例如：处理器可以包括中央处理器(central processing unit，CPU)、图形处理器(graphics processing unit，GPU)、数字信号处理(digital signal processing，DSP)芯片、微处理器(microcontroller unit，MCU)、可编程逻辑器件(field－programmable gate array，FPGA)、神经网格处理器(neural-network processing unit，NPU)、张量处理器(tensor processing unit，TPU)、人工智能(artificial intelligent，AI)类型处理器等的处理装置。其中，不同的处理单元可以是独立的部件，也可以集成在一个或多个处理器中。在一些实例中，电子装置也可以包括一个或多个处理器。
存储器可用于存储计算机程序，例如存储本发明实施例中的地图构建方法对应的计算机程序，处理器通过运行存储在存储器内的计算机程序，从而实现上述的地图构建方法。存储器可包括高速随机存储器，还可包括非易失性存储器，如一个或者多个磁性存储装置、闪存、或者其他非易失性固态存储器。在一些实例中，存储器可进一步包括相对于处理器远程设置的存储器，这些远程存储器可以通过网格连接至电子装置。上述网格的实例包括但不限于互联网、企业内部网、局域网、移动通信网及其组合。
通信设备用于经由一个网格接收或者发送数据。上述的网格具体实例可包括移动终端的通信供应商提供的无线网格。在一个实例中，通信设备包括一个网格适配器(network interface controller，NIC)，其可通过基站与其他网格设备相连从而可与互联网进行通讯。在一个实例中，通信设备可以为射频(radio frequency，RF)模块，其用于通过无线方式与互联网进行通讯。在本方案的一些实施例中，通信设备用于与手机、平板等移动设备连接，可以通过移动设备向车载终端发送指令。
显示设备可以为触摸屏式的液晶显示器(liquid crystal display，LCD)和触摸显示器(也被称为“触摸屏”或“触摸显示屏”)。该液晶显示器可使得用户能够与车载终端的用户界面进行交互。在一些实施例中，上述车载终端具有图形用户界面(graphical userinterface，GUI)，用户可以通过触摸触敏表面上的手指接触和/或手势来与GUI进行人机交互，此处的人机交互功能可以包括车辆挡位切换功能，用于执行上述人机交互功能的可执行指令被配置/存储在一个或多个处理器可执行的计算机程序产品或可读存储介质中。
图1是根据本发明其中一实施例的地图构建方法的流程图，如图1所示，该方法包括以下步骤：
步骤S102，获取点云数据，其中，点云数据是通过雷达扫描真实世界得到的，点云数据中包括多个第一雷达点。
可选地，本实施例的执行主体为即时定位与地图构建(SLAM)系统，，其中，其他电子设备、处理器也可以作为执行主体，在此不作具体限定。
在本公开上述步骤S102提供的技术方案中，即时定位与地图构建系统在车辆行驶过程中可以采用雷达对真实世界进行扫描得到的点云数据，然后再依据点云数据构建地图，其中获取到的点云数据中包括多个第一雷达点。
具体的，即时定位与地图构建技术是一种在自动驾驶领域应用较广的定位技术之一，其可以实现通过建立高精度地图，并在已有的地图中进行定位。即时定位与地图构建技术在自动驾驶领域的具体应用主要可以分为建图和定位两部分。
可选地，由雷达扫描得到的点云数据的数量可以由技术人员提前进行预设，一般来说，预设值可以取经验数值。值得注意的是，在雷达扫描过程中，点云数据的预设数值越大，则构建生成的地图越精确。
步骤S104，确定每个第一雷达点在站心坐标系下的坐标得到第一雷达点坐标，其中，站心坐标系的原点为雷达的中心。
在本公开上述步骤S104提供的技术方案中，在得到第一雷达点后，还应将第一雷达点投射到站心坐标系内得到对应的第一雷达点坐标，以便后续对第一雷达点坐标进行操作，其中，应注意的是，投射时站心坐标系的原点则为雷达的中心。
具体的，站心坐标系是以测站为原点的坐标系，主要用于了解以观察者为中心的其他物体的运动规律。
步骤S106，确定每个第一雷达点在站心坐标系下的法向量得到第一雷达点法向量。
在本公开上述步骤S106提供的技术方案中，在将第一雷达点投射到站心坐标系时，除了上述的获取到第一雷达点的坐标之外，还应获取到第一雷达店在站心坐标系下的法向量，即第一雷达点法向量。
步骤S108，根据第一雷达点坐标和第一雷达点法向量构建一级三维网格。
在本公开上述步骤S108提供的技术方案中，即时定位与构建系统可以以站心坐标系的参考点为中心，再将多个第一雷达点坐标和第一雷达点法向量匹配到划分好的多个一级三维网格中以构建多个一级三维网格。
可选地，举例说明，即时定位与地图构建系统可以构建范围100km的不同网格尺寸的网格图，其中，系统可以按照预设尺寸来划分一级三维网格，例如，预设值为0.3米，值得注意的的是，上述尺寸预设值可以基于构建范围和地图精度要求灵活变化，由此增加了此系统的鲁棒性。
步骤S110，根据一级三维网格构建二级三维网格。
在本公开上述步骤S110提供的技术方案中，在构建了多个一级三维网格后，还应将过个一级三维网格按照预设值进行进一步的汇总，由此构建二级三维网格。
具体的，每个二级三维网格中会存在若干个一级三维网格，其中每个二级三维网格的尺寸也可以按照预设值进行划分，举例说明，一个300米的二级三维网格中，可以具有多个尺寸为0.3米的一级三维网格。
值得注意的是，对一级三维网格和二级三维网格尺寸值的选取与后续对车辆进行定位时的精度呈正相关，当尺寸预设值越小时，则后续定位会越精确；反之，尺寸预设值越大，则后续定位会精确度会较低。
步骤S112，根据一级三维网格和二级三维网格确定目标地图。
在本公开上述步骤S112提供的技术方案中，即时定位与地图构建系统可以将多个雷达点投射在一级三维网格和二级三维网格之中，再计算每个三维网格中的平均坐标和平均法向量作为该三维网格的索引值，再根据每个三维网格的内容和索引值确定目标地图。
具体的，上述索引值为查询到每个三维网格的值，值得注意的是，二级三维网格的索引值应为一个区间，而一级三维网格的索引值可以为具体数值。
可选地，当一个一级三维网格中有多个雷达点时，则对一个一级三维网格中包含的所有雷达点的信息取平均值，包括位置坐标和法向量，该平均值可以作为这个一级三维网格的位置坐标和法向量。
可选地，即时定位与地图构建系统可以将二级三维网格的索引值、一级三维网格的索引值和一级三维网格的内容存入法向量文件，以供定位阶段提供匹配信息，此外，即时定位与地图构建系统也可以将上述需储存的内容存储至专用于储存的系统中，以供后续车辆定位操作时使用。
上述步骤S102至步骤S112，如图3所示，可以获知，在本公开中，采用获取点云数据，其中，点云数据是通过雷达扫描真实世界得到的，点云数据中包括多个第一雷达点，确定每个第一雷达点在站心坐标系下的坐标得到第一雷达点坐标，其中，站心坐标系的原点为雷达的中心，确定每个第一雷达点在站心坐标系下的法向量得到第一雷达点法向量，根据第一雷达点坐标和第一雷达点法向量构建一级三维网格，再根据一级三维网格构建二级三维网格，通过一级三维网格和二级三维网格确定目标地图，达到了可以通过一级三维网格和二级三维网格对车辆进行自主定位的目的，从而实现了提高车辆定位精度的技术效果，进而可以解决相关技术中车辆在进行定位时定位精度差的技术问题。
容易注意到的是，在本公开实施例中，在地图构建时采用了基于雷达点匹配不同尺寸的三维网格的方式来进行地图的构建，其中三维网格的尺寸可以基于不同的扫描范围和需求精度进行灵活的变化，由此解决了相关技术中定位精度差的技术问题。
同尺寸下面对该实施例的上述方法进行进一步地详细介绍。
作为一种可选的实施方式，步骤S106，确定每个第一雷达点的曲率，将每个第一雷达点的曲率与预设曲率阈值进行对比得到对比结果，响应于对比结果表示第一雷达点的曲率小于预设曲率阈值，确定第一雷达点所在平面，根据第一雷达点所在平面，确定第一雷达点法向量。
在该实施例中，即时定位与地图构建系统可以根据点云在自身雷达坐标系下的位置，计算每一个雷达点的曲率，将每个第一雷达点的曲率与预设曲率阈值进行对比得到对比结果，当该雷达点的曲率小于预设曲率阈值时，则此点就是平面点，因此可以确定该点所在平面，再将该雷达点所处平面的法向量作为该雷达点的法向量。
具体的，当确定该雷达点为平面点时，则需进一步确定该点上下左右四个点，其中，该点的右、中、上点，左、中、上点，左、中、下点，右、中、下点分别可以构成四个平面，再计算上述四个平面的法向量的平均值作为该雷达点的法向量。
具体的，该雷达点的上下左右四个点的法向量公式如下：
N右中上＝[(P右-P中)X(P上-P中)]normal
N左中上＝[(P左-P中)X(P上-P中)]normal
N左中下＝[(P左-P中)X(P下-P中)]normal
N右中下＝[(P右-P中)X(P下-P中)]normal
其中，N右中上、N左中上、N左中下、N右中下分别代表由右、中、上点构成的平面的单位法向量、左、中、上点构成的平面的单位法向量、左、中、下点构成的平面的单位法向量、右、中、下点构成的平面的单位法向量。
P代表该雷达点在自身雷达坐标系下的坐标，X代表叉乘，normal代表归一化即求单位法向量，N代表当前雷达点的法向量。
作为一种可选的实施方式，确定每个第一雷达点的曲率包括：根据第一雷达点的多个空间最近邻点和第一雷达点确定第一雷达点的曲率，其中，空间最近邻点为雷达同一扫描线束上相邻的点。
在该实施例中，即时定位与地图构建系统在确定每个第一雷达点时包括以下步骤：当前雷达点曲率值为当前雷达点与其左右10个空间最近邻点(同一扫描线束上相邻的点)深度值之差的累加和的平方，其中深度值表示该雷达点在自身雷达坐标下下的欧式距离，粗糙度则表示该雷达点的曲率。
具体的，上述曲率的具体计算公式如下：
Cur＝(Δd)2
其中，Ri代表左右10个相邻点的深度值，R代表当前点的深度值，Cur代表当前雷达点的曲率。
作为一种可选的实施方式，根据第一雷达点坐标和第一雷达点法向量构建一级三维网格包括：在站心坐标系下划分多个预设一级三维网格，根据第一雷达点坐标确定第一雷达点所属的预设一级三维网格得到匹配网格，将第一雷达点投影至匹配网格，根据匹配网格中包含的第一雷达点坐标和第一雷达点法向量，确定匹配网格的网格坐标和网格法向量，根据网格坐标和网格法向量，构建一级三维网格。
在该实施例中，根据第一雷达点坐标和第一雷达点法向量构建一级三维网格包括以下步骤：即时定位与地图构建系统可以根据预设值首先在站心坐标系内划分多个一级三维网格，再根据第一雷达点的坐标与划分好的多个一级三维网格进行匹配，匹配到一级三维网格后，将该雷达点投影到该匹配网格中，根据匹配网格中包含的第一雷达点坐标和第一雷达点法向量来确定该匹配网格的网格坐标和网格法向量，最终根据上述网格坐标和网格法向量构建一级三维网格。
具体的，上述确定匹配网格的网格坐标和网格法向量的具体公式如下：
其中，指的是匹配网格的平均位置，/>指的是匹配网格的平均法向量。Ti指的是在匹配网格中雷达点的法向量，Tp指的是在匹配网格中雷达点的位置，m代表该匹配网格中雷达点的数目。
可选地，每个一级三维网格中可能包含多个雷达点，此时，该一级三维网格中网格坐标和网格法向量即为该网格中所有雷达点的坐标和法向量的平均值。
根据本发明实施例，还提供了定位方法的实施例，需要说明的是，在附图的流程图示出的步骤可以在包含至少一组计算机可执行指令的计算机系统中执行，并且，虽然在流程图中示出了逻辑顺序，但是在某些情况下，可以以不同于此处的顺序执行所示出或描述的步骤。
该方法实施例还可以在包含存储器和处理器的电子装置、类似的控制装置或者车载终端中执行。以车载终端为例，车载终端可以包括一个或多个处理器和用于存储数据的存储器。可选地，上述车载终端还可以包括用于通信功能的通信设备以及显示设备。本领域普通技术人员可以理解，上述结构描述仅为示意，其并不对上述车载终端的结构造成限定。例如，车载终端还可包括比上述结构描述更多或者更少的组件，或者具有与上述结构描述不同的配置。
图2是根据本发明其中一实施例的定位方法的流程图，如图2所示，该方法包括以下步骤：
步骤S202，获取待定位主体的初始位姿和待定位主体周围环境的当前点云数据。
在本公开上述步骤S202提供的技术方案中，如图4所示，即时定位与地图构建系统可以根据卫星导航定位技术(GNSS)传感器获取到待定位车辆的初始位姿点和当前待定位车辆主体周围环境的点云数据。
可选地，卫星导航定位技术是指采用导航卫星对地面、海洋、空中和空间用户进行导航定位的技术，一般应用于各种定位场景，具有较为准确的精度。
可选地，获取当前待定位主体周围环境的点云数据时，可以向即时定位与地图构建系统中的储存模块内发送获取请求，也可以发出控制命令使即时定位与地图构建系统将地图构建时所采集的环境点云数据发送至当前定位模块，在此不作具体限定。
步骤S204，根据初始位姿，加载目标地图，其中，目标地图通过权利要求1中的地图构建方法构建得到，目标地图中包括一级三维网格和二级三维网格。
在本公开上述步骤S204提供的技术方案中，如图4所示，在获取到当前待定位车辆的初始位姿点后，即时定位与地图构建系统应加载当前初始位姿点周围一定范围内的地图数据，即为目标地图，其中，目标地图内包括多个一级三维网格和二级三维网格。
可选地，当即时定位与地图构建系统在加载当前初始位姿点周围一定范围的内的地图数据时，选取的范围大小可以为预设的经验数值，也可以根据周围环境的变化进行适应性的修改。
步骤S206，根据当前点云数据，从目标地图中确定目标一级三维网格，其中，目标一级三维网格为当前点云数据对应的一级三维网格。
在本公开上述步骤S206提供的技术方案中，如图4所示，即时定位与地图构建系统可以根据当前的点云数据，在上述加载的目标地图中确定出离当前点云数据对应的一级三维网格，即为目标一级三维网格。
可选地，当前点云数据中可以具有多个雷达点，其中每个雷达点都需进行匹配，确定对应的一级三维网格。
步骤S208，根据当前点云数据和目标一级三维网格，利用欧式距离公式对初始位姿进行优化得到目标位姿。
在本公开上述步骤S208提供的技术方案中，如图4所示，即时定位与地图构建系统可以将点云数据中的所有雷达点与对应的一级三维网格进行匹配，根据匹配到的一级三维网格的网格坐标和网格法向量确定每个雷达点到该网格点平面的距离，再根据欧式距离公式对待定位车辆的初始位姿的所有雷达点进行迭代优化得到最优的目标位姿。
作为一种可选的实施方式，步骤S204，根据初始位姿中的位置坐标，确定待定位主体当前所属的二级三维网格，加载目标地图中的待定位主体当前所属的二级三维网格作为目标二级三维网格，根据当前点云数据，从目标地图中确定目标一级三维网格包括，根据当前点云数据，从目标二级三维网格中确定目标一级三维网格。
在该实施例中，根据待定位车辆的初始位姿的每个雷达点的坐标确定当前待定位车辆所属的二级三维网格，再加载当前待定位车辆对应的二级三维网格作为目标三维网格，再根据当前点云数据中的每个雷达点的坐标确定从所属的二级三维网格中进一步确定其所属的一级三维网格。
具体的，在目标二级三维网格中确定目标一级三维网格时，即时定位与地图构建系统应将距离其雷达点最近的一级三维网格确定为目标一级三维网格。
可选的，当雷达点投射在多个一级三维网格的边界线上时，应先计算该雷达点到每个一级三维网格的平面距离，再将每个获得的距离值与预设阈值进行比较，当距离值大于预设值时，则排除该距离值对应的一级三维网格。若多个距离值均小于该预设阈值，则对该雷达点重新确定坐标进行投射。
作为一种可选的实施方式，步骤S208，当前点云数据中包括多个第二雷达点，一级三维网格中包括网格坐标和网格法向量，每个第二雷达点有一个目标一级三维网格与之对应，根据当前点云数据和目标一级三维网格，利用欧式距离公式对初始位姿进行优化得到目标位姿包括：根据预设调整策略和欧式距离公式，对初始位姿进行迭代调整，使欧式距离之和小于预设距离阈值并且记录当前位姿作为目标位姿，其中，欧式距离之和为每个第二雷达点到与之对应的网格坐标所在平面的欧式距离之和。
在该实施例中，待定位车辆中的点云数据中包含多个第二雷达点，而每个第二雷达点均有一个目标一级三维网格模型和其相对应，同时每个目标一级三维网格中包含网格坐标和网格法向量，根据当前点云数据和目标一级三维网格，利用欧式距离公式对初始位姿进行优化得到目标位姿包括以下步骤：即时定位和地图构建系统可以根据预设的调整策略和欧式距离公式，对待定位车辆的初始位姿进行迭代调整，以使其欧式距离之和小于预设距离阈值时，记录当前待定位车辆的位姿点作为目标位姿点，其中，欧式距离之和为每个第二雷达点到与之对应的网格坐标所在平面的欧式距离之和。
可选地，对待定位车辆的初始位姿进行迭代调整时，可以选取多个雷达点进行迭代优化，值得注意的是，当选取的雷达点越多时，其迭代优化的结果精度越高，最终对车辆定位的位姿点定位也越准确。
作为一种可选的实施方式，每个第二雷达点到与之对应的网格坐标所在平面的欧式距离的确定包括：将第二雷达点在雷达坐标系下的坐标转化为站心坐标系下的坐标得到第二雷达点坐标，针对每个第二雷达点，根据第二雷达点坐标、目标一级三维网格中的网格坐标和目标一级三维网格中的网格法向量，利用欧式距离公式确定第二雷达点到与之对应的网格坐标所在平面的欧式距离。
在该实施例中，每个第二雷达点到与之对应的网格坐标所在平面的欧式距离的确定包括以下步骤：即时定位与地图构建系统可以将第二雷达点在雷达坐标系下的坐标转化为站心坐标系下的坐标得到第二雷达点坐标，再基于第二雷达点坐标、目标一级三维网格中的网格坐标和网格法向量，运用欧式距离公式确定第二雷达点到与其对应的网格所在平面的欧式距离。
具体的，上述欧式距离的公式如下：
其中R代表雷达点到一级三维网格点所在平面的欧式距离，代表一级三维网格的平面法向量，/>代表在站心参考坐标系下当前雷达点的坐标位置,欧式距离等于平面法向量与雷达点位置的点积。
TLIDAR-ENU代表雷达坐标系到站心坐标系的转换关系，也就是上述方法中提到的T。
代表雷达点在自身雷达坐标系下的坐标位置。
为所有雷达点到level1网格点所在平面的欧式距离的最小值。
具体的，当前待定位车辆的位姿T就是当前时刻雷达坐标系相对于站心参考坐标系下的位姿变换关系，因为参考站心坐标点是已知的，进而可以得到当前雷达点的精确位置，进而可以得到当前待定位车辆的准确位姿点。
通过以上实施方式的描述，本领域的技术人员可以清楚地了解到根据上述实施例的方法可借助软件加必需的通用硬件平台的方式来实现，当然也可以通过硬件，但很多情况下前者是更佳的实施方式。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质(如ROM/RAM、磁碟、光盘)中，包括若干指令用以使得一台终端设备(可以是手机，计算机，服务器，或者网格设备等)执行本发明各个实施例的方法。
本发明的实施例还提供了一种数据采集车辆。
可选地，在本实施例中，上述数据采集车辆中的处理器可以被设置为运行计算机程序以执行以下步骤：
步骤S102，获取点云数据，其中，点云数据是通过雷达扫描真实世界得到的，点云数据中包括多个第一雷达点。
步骤S104，确定每个第一雷达点在站心坐标系下的坐标得到第一雷达点坐标，其中，站心坐标系的原点为雷达的中心。
步骤S106，确定每个第一雷达点在站心坐标系下的法向量得到第一雷达点法向量。
步骤S108，根据第一雷达点坐标和第一雷达点法向量构建一级三维网格。
步骤S110，根据一级三维网格构建二级三维网格。
步骤S112，根据一级三维网格和二级三维网格确定目标地图。
可选地，本实施例中的具体示例可以参考上述实施例及可选实施方式中所描述的示例，本实施例在此不再赘述。
本发明的实施例还提供了一种车辆。
可选地，在本实施例中，上述车辆可以被设置为存储用于执行以下步骤的计算机程序：
步骤S102，获取点云数据，其中，点云数据是通过雷达扫描真实世界得到的，点云数据中包括多个第一雷达点。
步骤S104，确定每个第一雷达点在站心坐标系下的坐标得到第一雷达点坐标，其中，站心坐标系的原点为雷达的中心。
步骤S106，确定每个第一雷达点在站心坐标系下的法向量得到第一雷达点法向量。
步骤S108，根据第一雷达点坐标和第一雷达点法向量构建一级三维网格。
步骤S110，根据一级三维网格构建二级三维网格。
步骤S112，根据一级三维网格和二级三维网格确定目标地图。
可选地，本实施例中的具体示例可以参考上述实施例及可选实施方式中所描述的示例，本实施例在此不再赘述。
在本发明的上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述的部分，可以参见其他实施例的相关描述。
在本申请所提供的一些实施例中，应该理解到，所揭露的技术内容，可通过其它的方式实现。其中，以上所描述的装置实施例仅仅是示意性的，例如单元的划分，可以为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，单元或模块的间接耦合或通信连接，可以是电性或其它的形式。
作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。
另外，在本发明各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。
集成的单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可为个人计算机、服务器或者网格设备等)执行本发明各个实施例方法的全部或部分步骤。而前述的存储介质包括：U盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、移动硬盘、磁碟或者光盘等各种可以存储程序代码的介质。
以上仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (10)

1.一种地图构建方法，其特征在于，包括：
获取点云数据，其中，所述点云数据是通过雷达扫描真实世界得到的，所述点云数据中包括多个第一雷达点；
确定每个所述第一雷达点在站心坐标系下的坐标得到第一雷达点坐标，其中，所述站心坐标系的原点为所述雷达的中心；
确定每个所述第一雷达点在所述站心坐标系下的法向量得到第一雷达点法向量；
根据所述第一雷达点坐标和所述第一雷达点法向量构建一级三维网格；
根据所述一级三维网格构建二级三维网格；
根据所述一级三维网格和所述二级三维网格确定目标地图。
2.根据权利要求1所述的地图构建方法，其特征在于，所述确定每个所述第一雷达点在所述站心坐标系下的法向量得到第一雷达点法向量包括：
确定每个所述第一雷达点的曲率；
将每个所述第一雷达点的曲率与预设曲率阈值进行对比得到对比结果；
响应于所述对比结果表示所述第一雷达点的曲率小于所述预设曲率阈值，确定所述第一雷达点所在平面；
根据所述第一雷达点所在平面，确定所述第一雷达点法向量。
3.根据权利要求2所述的地图构建方法，其特征在于，所述确定每个所述第一雷达点的曲率包括：
根据所述第一雷达点的多个空间最近邻点和所述第一雷达点确定所述第一雷达点的曲率，其中，所述空间最近邻点为所述雷达同一扫描线束上相邻的点。
4.根据权利要求3所述的地图构建方法，其特征在于，所述根据所述第一雷达点坐标和所述第一雷达点法向量构建一级三维网格包括：
在所述站心坐标系下划分多个预设一级三维网格；
根据所述第一雷达点坐标确定所述第一雷达点所属的预设一级三维网格得到匹配网格；
将所述第一雷达点投影至所述匹配网格；
根据所述匹配网格中包含的所述第一雷达点坐标和所述第一雷达点法向量，确定所述匹配网格的网格坐标和网格法向量；
根据所述网格坐标和所述网格法向量，构建所述一级三维网格。
5.一种定位方法，其特征在于，包括：
获取待定位主体的初始位姿和所述待定位主体周围环境的当前点云数据；
根据所述初始位姿，加载目标地图，其中，所述目标地图通过上述权利要求1中所述的地图构建方法构建得到，所述目标地图中包括一级三维网格和二级三维网格；
根据所述当前点云数据，从所述目标地图中确定目标一级三维网格，其中，所述目标一级三维网格为所述当前点云数据对应的所述一级三维网格；
根据所述当前点云数据和所述目标一级三维网格，利用欧式距离公式对所述初始位姿进行优化得到目标位姿。
6.根据权利要求5所述的定位方法，其特征在于，所述根据所述初始位姿，加载目标地图包括：
根据所述初始位姿中的位置坐标，确定所述待定位主体当前所属的二级三维网格；
加载所述目标地图中的所述待定位主体当前所属的二级三维网格作为目标二级三维网格；
根据所述当前点云数据，从所述目标地图中确定目标一级三维网格包括：
根据所述当前点云数据，从所述目标二级三维网格中确定所述目标一级三维网格。
7.根据权利要求5所述的定位方法，其特征在于，所述当前点云数据中包括多个第二雷达点，所述一级三维网格中包括网格坐标和网格法向量，每个所述第二雷达点有一个所述目标一级三维网格与之对应；
所述根据所述当前点云数据和所述目标一级三维网格，利用欧式距离公式对所述初始位姿进行优化得到目标位姿包括：
根据预设调整策略和所述欧式距离公式，对所述初始位姿进行迭代调整，使欧式距离之和小于预设距离阈值并且记录当前位姿作为目标位姿，其中，所述欧式距离之和为每个所述第二雷达点到与之对应的所述网格坐标所在平面的欧式距离之和。
8.根据权利要求7所述的定位方法，其特征在于，每个所述第二雷达点到与之对应的所述网格坐标所在平面的欧式距离的确定包括：
将所述第二雷达点在雷达坐标系下的坐标转化为站心坐标系下的坐标得到第二雷达点坐标；
针对每个所述第二雷达点，根据所述第二雷达点坐标、所述目标一级三维网格中的网格坐标和所述目标一级三维网格中的网格法向量，利用所述欧式距离公式确定所述第二雷达点到与之对应的所述网格坐标所在平面的欧式距离。
9.一种数据采集车辆，包括存储器和处理器，其特征在于，所述存储器中存储有计算机程序，所述处理器被设置为运行所述计算机程序以执行上述权利要求1至4任一项中所述的地图构建方法，其中，权利要求1至4任一项中所述的地图构建方法构建的目标地图用于权利要求5至8中任一项所述的定位方法。
10.一种车辆，包括存储器和处理器，其特征在于，所述存储器中存储有计算机程序，所述处理器被设置为运行所述计算机程序以执行上述权利要求5至8任一项中所述的定位方法，其中，权利要求5至8任一项中所述的定位方法使用权利要求1至4任一项中所述的地图构建方法构建的目标地图。
CN202310318366.2A 2023-03-28 2023-03-28 地图构建方法、定位方法、数据采集车辆及车辆 Pending CN116734871A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310318366.2A CN116734871A (zh) 2023-03-28 2023-03-28 地图构建方法、定位方法、数据采集车辆及车辆

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310318366.2A CN116734871A (zh) 2023-03-28 2023-03-28 地图构建方法、定位方法、数据采集车辆及车辆

Publications (1)

Publication Number Publication Date
CN116734871A true CN116734871A (zh) 2023-09-12

Family

ID=87913981

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310318366.2A Pending CN116734871A (zh) 2023-03-28 2023-03-28 地图构建方法、定位方法、数据采集车辆及车辆

Country Status (1)

Country Link
CN (1) CN116734871A (zh)

Similar Documents

Publication Publication Date Title
US11360216B2 (en) Method and system for positioning of autonomously operating entities
CN111126182B (zh) 车道线检测方法、装置、电子设备及存储介质
EP2555166A1 (en) Space error parameter for 3D buildings and terrain
CN114387319B (zh) 点云配准方法、装置、设备以及存储介质
CN111125283A (zh) 电子地图构建方法、装置、计算机设备和存储介质
CN113359782A (zh) 一种融合lidar点云与图像数据的无人机自主选址降落方法
CN114926549B (zh) 三维点云处理方法、装置、设备以及存储介质
CN110954114B (zh) 电子地图的生成方法、装置、终端及存储介质
CN115578433A (zh) 图像处理方法、装置、电子设备及存储介质
AU2022427045A1 (en) Method and device for optimizing three-dimensional map display
CN116052097A (zh) 一种地图要素检测方法、装置、电子设备和存储介质
CN114528941A (zh) 传感器数据融合方法、装置、电子设备及存储介质
CN110770540B (zh) 用于构建环境模型的方法和装置
CN113640810A (zh) 空间车位检测方法、设备、存储介质和程序产品
JP2023503750A (ja) ロボットの位置決め方法及び装置、機器、記憶媒体
CN117330083A (zh) 机器人定位方法、机器人和可读存储介质
CN116734871A (zh) 地图构建方法、定位方法、数据采集车辆及车辆
KR20230006628A (ko) 이미지 처리 방법과 장치, 전자 기기, 저장 매체 및 컴퓨터 프로그램
CN109816726A (zh) 一种基于深度滤波器的视觉里程计地图更新方法和系统
CN115790621A (zh) 高精地图更新方法、装置及电子设备
CN114882115A (zh) 车辆位姿的预测方法和装置、电子设备和存储介质
CN113808196A (zh) 平面融合定位方法、装置、电子设备及存储介质
CN111210297A (zh) 一种上车点的划分方法及装置
CN112308766A (zh) 一种图像数据展示方法、装置、电子设备及存储介质
US20230354258A1 (en) Data processing method and apparatus

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