CN113759369A - 基于双多线雷达的建图方法和装置 - Google Patents

基于双多线雷达的建图方法和装置 Download PDF

Info

Publication number
CN113759369A
CN113759369A CN202010599593.3A CN202010599593A CN113759369A CN 113759369 A CN113759369 A CN 113759369A CN 202010599593 A CN202010599593 A CN 202010599593A CN 113759369 A CN113759369 A CN 113759369A
Authority
CN
China
Prior art keywords
point cloud
cloud data
point
pose
radar
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.)
Granted
Application number
CN202010599593.3A
Other languages
English (en)
Other versions
CN113759369B (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.)
Beijing Jingdong Qianshi Technology Co Ltd
Original Assignee
Beijing Jingdong Qianshi 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 Jingdong Qianshi Technology Co Ltd filed Critical Beijing Jingdong Qianshi Technology Co Ltd
Priority to CN202010599593.3A priority Critical patent/CN113759369B/zh
Publication of CN113759369A publication Critical patent/CN113759369A/zh
Application granted granted Critical
Publication of CN113759369B publication Critical patent/CN113759369B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • 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
    • 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/02—Systems using reflection of radio waves, e.g. primary radar systems; Analogous systems
    • G01S13/06—Systems determining position data of a target
    • G01S13/42—Simultaneous measurement of distance and other co-ordinates

Abstract

本发明公开了一种基于双多线雷达的建图方法和装置，涉及计算机技术领域。该方法的一具体实施方式包括：分别对两个雷达的点云数据进行处理以建立第一位姿约束，第一位姿约束包括同雷达相邻点云数据帧的位姿约束和同雷达闭环位姿约束；根据雷达间的位姿和安装关系建立第二位姿约束，第二位姿约束包括不同雷达的相邻点云数据帧的位姿约束，相邻点云数据帧为时间间隔最近的两个点云数据帧；根据第一位姿约束和第二位姿约束更新两个雷达的点云数据的位姿，并根据更新后的位姿生成点云地图。该实施方式能够使用两个雷达的数据同时建图，减小盲区影响，补全单个雷达看不到的地图细节；由于约束增加，使得地图的精度也会大大提高。

Description

基于双多线雷达的建图方法和装置
技术领域
本发明涉及计算机技术领域，尤其涉及一种基于双多线雷达的建图方法和装置。
背景技术
目前无人驾驶车辆，基于减少盲区等的考虑，一般会同时配备2个甚至更多个多线雷达，但是在基于多线雷达进行SLAM(simultaneous localization and mapping，即时定位与地图构建)建图时，往往只使用其中一个雷达的数据。
在实现本发明过程中，发明人发现现有技术中至少存在如下问题：
使用单个多线雷达的数据构建地图，虽然在算法上实现起来简单，但是雷达盲区的存在不可避免的会造成点云地图部分细节的丢失。如果单个雷达盲区过大，生成的点云地图的精度可能也会受到影响。
发明内容
有鉴于此，本发明实施例提供一种基于双多线雷达的建图方法和装置，能够使用两个雷达的数据同时建图，减小盲区影响，补全单个雷达看不到的地图细节；由于约束增加，使得地图的精度也会大大提高。
为实现上述目的，根据本发明实施例的一个方面，提供了一种基于双多线雷达的建图方法。
一种基于双多线雷达的建图方法，包括：分别对两个雷达的点云数据进行处理以建立第一位姿约束，所述第一位姿约束包括同雷达相邻点云数据帧的位姿约束和同雷达闭环位姿约束；根据雷达间的位姿和安装关系建立第二位姿约束，所述第二位姿约束包括不同雷达的相邻点云数据帧的位姿约束，所述相邻点云数据帧为时间间隔最近的两个点云数据帧；根据所述第一位姿约束和所述第二位姿约束更新两个雷达的点云数据的位姿，并根据更新后的位姿生成点云地图。
可选地，所述同雷达相邻点云数据帧的位姿约束通过以下方式建立：从同雷达的相邻点云数据帧的线点和面点中，获取距离最近的匹配线点和匹配面点，并根据所述匹配线点和所述匹配面点计算相邻两帧点云数据的位姿增量；将所述位姿增量作为初始值，将当前点云数据帧的邻近帧组成的点云地图作为局部点云地图，然后计算当前点云数据帧在所述局部点云地图中的位姿以建立同雷达相邻帧点云数据的位姿约束。
可选地，所述线点和面点通过以下方式确定：计算点云数据帧的光滑度得分；将光滑度得分大于第一阈值的点作为线点，小于第二阈值的点作为面点。
可选地，获取距离最近的匹配线点和匹配面点包括：从同雷达的点云数据中，获取与当前点云数据帧相邻的前一点云数据帧；分别从所述前一点云数据帧的线点和面点中，获取与所述当前点云数据帧的线点和面点距离最近的线点和面点，并将距离最近的当前点云数据帧的线点和前一点云数据帧的线点作为相邻点云数据帧的匹配线点，将距离最近的当前点云数据帧的面点和前一点云数据帧的面点作为相邻点云数据帧的匹配面点。
可选地，根据所述匹配线点和所述匹配面点计算相邻两帧点云数据的位姿增量包括：根据所述匹配线点，从所述前一点云数据帧的线点中获取另外一个线点，并与所述匹配线点中所述前一点云数据帧的线点构成一条直线，计算所述匹配线点中所述当前点云数据帧的线点到所述直线的第一距离；根据所述匹配面点，从所述前一点云数据帧的面点中获取另外两个面点，并与所述匹配面点中所述前一点云数据帧的面点构成一个平面，计算所述匹配面点中所述当前点云数据帧的面点到所述平面的第二距离；通过使所述第一距离和所述第二距离最小，以计算相邻两帧点云数据的位姿增量。
可选地，所述局部点云地图通过以下方式得到：将邻近帧的线点组成局部线点地图，邻近帧的面点组成局部面点地图，从而拼接得到局部点云地图。
可选地，所述同雷达闭环位姿约束通过以下方式建立：将当前点云数据帧在所述局部点云地图中的位姿与雷达的整个位姿轨迹进行比较，寻找闭环以建立闭环位姿约束，所述雷达的整个位姿轨迹由所述雷达所采集的点云数据的位姿构成。
可选地，根据更新后的位姿生成点云地图包括：根据更新后的位姿，以及雷达的点云数据，对点云数据帧进行拼接生成点云地图。
根据本发明实施例的另一方面，提供了一种基于双多线雷达的建图装置。
一种基于双多线雷达的建图装置，包括：同雷达数据处理模块，用于分别对两个雷达的点云数据进行处理以建立第一位姿约束，所述第一位姿约束包括同雷达相邻点云数据帧的位姿约束和同雷达闭环位姿约束；雷达间数据处理模块，用于根据雷达间的位姿和安装关系建立第二位姿约束，所述第二位姿约束包括不同雷达的相邻点云数据帧的位姿约束，所述相邻点云数据帧为时间间隔最近的两个点云数据帧；位姿更新模块，用于根据所述第一位姿约束和所述第二位姿约束更新两个雷达的点云数据的位姿，并根据更新后的位姿生成点云地图。
可选地，所述同雷达相邻点云数据帧的位姿约束通过以下方式建立：从同雷达的相邻点云数据帧的线点和面点中，获取距离最近的匹配线点和匹配面点，并根据所述匹配线点和所述匹配面点计算相邻两帧点云数据的位姿增量；将所述位姿增量作为初始值，将当前点云数据帧的邻近帧组成的点云地图作为局部点云地图，然后计算当前点云数据帧在所述局部点云地图中的位姿以建立同雷达相邻帧点云数据的位姿约束。
可选地，获取距离最近的匹配线点和匹配面点包括：从同雷达的点云数据中，获取与当前点云数据帧相邻的前一点云数据帧；分别从所述前一点云数据帧的线点和面点中，获取与所述当前点云数据帧的线点和面点距离最近的线点和面点，并将距离最近的当前点云数据帧的线点和前一点云数据帧的线点作为相邻点云数据帧的匹配线点，将距离最近的当前点云数据帧的面点和前一点云数据帧的面点作为相邻点云数据帧的匹配面点。
可选地，根据所述匹配线点和所述匹配面点计算相邻两帧点云数据的位姿增量包括：根据所述匹配线点，从所述前一点云数据帧的线点中获取另外一个线点，并与所述匹配线点中所述前一点云数据帧的线点构成一条直线，计算所述匹配线点中所述当前点云数据帧的线点到所述直线的第一距离；根据所述匹配面点，从所述前一点云数据帧的面点中获取另外两个面点，并与所述匹配面点中所述前一点云数据帧的面点构成一个平面，计算所述匹配面点中所述当前点云数据帧的面点到所述平面的第二距离；通过使所述第一距离和所述第二距离最小，以计算相邻两帧点云数据的位姿增量。
可选地，所述同雷达闭环位姿约束通过以下方式建立：将当前点云数据帧在所述局部点云地图中的位姿与雷达的整个位姿轨迹进行比较，寻找闭环以建立闭环位姿约束，所述雷达的整个位姿轨迹由所述雷达所采集的点云数据的位姿构成。
可选地，所述位姿更新模块还用于：根据更新后的位姿，以及雷达的点云数据，对点云数据帧进行拼接生成点云地图。
根据本发明实施例的又一方面，提供了一种基于双多线雷达的建图电子设备。
一种基于双多线雷达的建图电子设备，包括：一个或多个处理器；存储装置，用于存储一个或多个程序，当所述一个或多个程序被所述一个或多个处理器执行，使得所述一个或多个处理器实现本发明实施例所提供的基于双多线雷达的建图方法。
根据本发明实施例的再一方面，提供了一种计算机可读介质。
一种计算机可读介质，其上存储有计算机程序，所述程序被处理器执行时实现本发明实施例所提供的基于双多线雷达的建图方法。
上述发明中的一个实施例具有如下优点或有益效果：通过分别对两个雷达的点云数据进行处理以建立第一位姿约束，第一位姿约束包括同雷达相邻点云数据帧的位姿约束和同雷达闭环位姿约束；根据雷达间的位姿和安装关系建立第二位姿约束，第二位姿约束包括不同雷达的相邻点云数据帧的位姿约束，相邻点云数据帧为时间间隔最近的两个点云数据帧；根据第一位姿约束和第二位姿约束更新两个雷达的点云数据的位姿，并根据更新后的位姿生成点云地图，可以使用两个雷达的数据同时建图，能够减小盲区影响，补全单个雷达看不到的地图细节；由于约束增加，使得地图的精度也会有所提高。前期对各个雷达的数据处理相互独立，互不依赖，可以采用并行的方式，故而可以加快求解速度；后期再建立两个雷达之间数据的位姿约束，根据同雷达相邻帧位姿约束、同雷达闭环约束和不同雷达相邻帧位姿约束求解整个位姿轨迹并生成地图，可以更好地建立雷达点云数据之间的位姿约束，使得建图误差达到最小，建图精度大大提高。
上述的非惯用的可选方式所具有的进一步效果将在下文中结合具体实施方式加以说明。
附图说明
附图用于更好地理解本发明，不构成对本发明的不当限定。其中：
图1是根据本发明实施例的基于双多线雷达的建图方法的主要步骤示意图；
图2是本发明一个实施例的基于双多线雷达的建图流程示意图；
图3是本发明一个实施例的不同雷达间的位姿约束的构建原理示意图；
图4是根据本发明实施例的基于双多线雷达的建图装置的主要模块示意图；
图5是本发明实施例可以应用于其中的示例性系统架构图；
图6是适于用来实现本发明实施例的终端设备或服务器的计算机系统的结构示意图。
具体实施方式
以下结合附图对本发明的示范性实施例做出说明，其中包括本发明实施例的各种细节以助于理解，应当将它们认为仅仅是示范性的。因此，本领域普通技术人员应当认识到，可以对这里描述的实施例做出各种改变和修改，而不会背离本发明的范围和精神。同样，为了清楚和简明，以下的描述中省略了对公知功能和结构的描述。
为了解决现有技术中所存在的问题，本发明提供了一种基于双多线雷达的建图方法和装置，使用两个多线雷达的数据同时建图，能够减小盲区影响，补全单个雷达看不到的地图细节。由于约束增加，最后地图的精度也会有所提高。
图1是根据本发明实施例的基于双多线雷达的建图方法的主要步骤示意图。如图1所示，本发明实施例的基于双多线雷达的建图方法主要包括如下的步骤S101至步骤S103。
步骤S101：分别对两个雷达的点云数据进行处理以建立第一位姿约束，第一位姿约束包括同雷达相邻点云数据帧的位姿约束和同雷达闭环位姿约束；
步骤S102：根据雷达间的位姿和安装关系建立第二位姿约束，第二位姿约束包括不同雷达的相邻点云数据帧的位姿约束，相邻点云数据帧为时间间隔最近的两个点云数据帧；
步骤S103：根据第一位姿约束和第二位姿约束更新两个雷达的点云数据的位姿，并根据更新后的位姿生成点云地图。
在本发明的实施例中，前期对各个雷达的数据处理相互独立，互不依赖，可以采用并行的方式，故而可以加快求解速度；后期再建立两个雷达之间数据的位姿约束，从而引入另一个雷达数据的位姿约束，能够减小盲区影响，补全单个雷达看不到的地图细节，且由于约束增加，最后地图的精度也会有所提高。本发明根据同雷达相邻帧位姿约束、同雷达闭环约束和不同雷达相邻帧位姿约束求解整个位姿轨迹并生成地图，可以更好地建立雷达点云数据之间的位姿约束，使得建图误差达到最小，建图精度大大提高。
根据本发明的一个实施例，同雷达相邻点云数据帧的位姿约束例如可以通过以下方式建立：
从同雷达的相邻点云数据帧的线点和面点中，获取距离最近的匹配线点和匹配面点，并根据匹配线点和匹配面点计算相邻两帧点云数据的位姿增量；
将位姿增量作为初始值，将当前点云数据帧的邻近帧组成的点云地图作为局部点云地图，然后计算当前点云数据帧在局部点云地图中的位姿以建立同雷达相邻帧点云数据的位姿约束。
在本发明的实施例中，线点和面点可以通过以下方式确定：计算点云数据帧的光滑度得分；将光滑度得分大于第一阈值的点作为线点，小于第二阈值的点作为面点。其中，第一阈值和第二阈值可根据应用需要进行灵活设定。
根据本发明的另一个实施例，在获取距离最近的匹配线点和匹配面点时，具体可以是按照以下的步骤来执行：
从同雷达的点云数据中，获取与当前点云数据帧相邻的前一点云数据帧；
分别从前一点云数据帧的线点和面点中，获取与当前点云数据帧的线点和面点距离最近的线点和面点，并将距离最近的当前点云数据帧的线点和前一点云数据帧的线点作为相邻点云数据帧的匹配线点，将距离最近的当前点云数据帧的面点和前一点云数据帧的面点作为相邻点云数据帧的匹配面点。
其中，在根据所述匹配线点和所述匹配面点计算相邻两帧点云数据的位姿增量时，具体可以包括：
根据匹配线点，从前一点云数据帧的线点中获取另外一个线点，并与匹配线点中前一点云数据帧的线点构成一条直线，计算匹配线点中当前点云数据帧的线点到该直线的第一距离；
根据匹配面点，从前一点云数据帧的面点中获取另外两个面点，并与匹配面点中前一点云数据帧的面点构成一个平面，计算匹配面点中当前点云数据帧的面点到该平面的第二距离；
通过使第一距离和第二距离最小，以计算相邻两帧点云数据的位姿增量。
在本发明的实施例中，局部点云地图可以通过以下方式得到：将邻近帧的线点组成局部线点地图，邻近帧的面点组成局部面点地图，从而拼接得到局部点云地图。由于在本发明的实施例中，计算相邻帧之间的位姿关系时，是基于线点和面点来进行计算的，因此，由邻近帧的线点和面点来构成局部点云地图，可以在保证计算精度的情况下减小计算量，提高建图效率。其中，邻近帧指的是与当前点云数据帧相邻的前指定个数的点云数据帧，例如：是当前点云数据帧的前50帧，具体个数可根据计算精度的需要进行灵活设定。
根据本发明的又一个实施例，同雷达闭环位姿约束可以通过以下方式建立：将当前点云数据帧在所述局部点云地图中的位姿与雷达的整个位姿轨迹进行比较，寻找闭环以建立闭环位姿约束，雷达的整个位姿轨迹由雷达所采集的点云数据的位姿构成。由于前面在进行点云数据处理时，每次仅根据相邻帧的点云数据进行位姿计算，可能会存在误差，故而为了尽量减小误差，需要进行位姿修正。在本发明的实施例中，可根据当前点云数据帧在所述局部点云地图中的位姿与该雷达的整个位姿轨迹进行比较分析来获取闭环点，进而添加闭环位姿约束。该雷达的整个位姿轨迹由该雷达所采集的所有点云数据帧经过的历史位姿构成。通过将当前点云数据帧与该雷达所采集的所有点云数据帧进行比较，以判断是否有现实世界中的相同点或相近点(主要是根据所采集点云点的位置坐标来判断)存在，若有，则构成了闭环点。
根据本发明的再一个实施例，在根据更新后的位姿生成点云地图时，具体可以是：根据更新后的位姿，以及雷达的点云数据，对点云数据帧进行拼接或者累加生成点云地图。
图2是本发明一个实施例的基于双多线雷达的建图流程示意图。如图2所示，在本发明的实施例中，建图流程主要包括以下的步骤1至步骤8。
步骤1、首先，对两个雷达(雷达A和雷达B)的数据通道并行处理，对于每帧雷达点云数据，寻找其线点和面点。可按下式(1)求取光滑度得分，得分较大值点为线点，得分较小值点为面点。其中，可通过设定得分阈值的方式来判断得分是大还是小：
smoothnessi＝∑k＝1,2,…,5(rangei±k-rangei)2 (1)。
步骤2、根据相邻两帧(在本发明实施例介绍中，“点云数据帧”简称为“帧”)的线点和面点，分别寻找最近的匹配点，以建立相邻帧位姿约束。其中，在寻找匹配点时，以寻找匹配线点为例，从与当前帧相邻的前一帧的所有线点中，寻找与当前帧的线点距离最近的点，并将这两个距离最近的分别位于当前帧及其前一帧上的线点作为匹配线点。具体地，可通过计算当前帧上每个线点与前一帧上每个线点之间的距离，并从这些距离中选择最小的距离对应的两个线点作为匹配线点。另外，也可根据其他的算法选择匹配线点。匹配面点的选择方法类似，此处不再赘述。
步骤3、使匹配线点中位于当前帧的线点离直线的第一距离和匹配面点中位于当前帧的面点离平面的第二距离达到最小，以求取帧间位姿增量。其中，直线通过以下方式生成：假设匹配线点中位于当前帧(第k+1帧)上的线点为点Xk+1,i，位于前一帧(第k帧)上的线点为点Xk,j，从第k帧中再选取一个线点Xk,l，则由点Xk,j和点Xk,l即可表示位于前一帧上的直线；平面通过以下方式生成：假设匹配面点中位于当前帧(第k+1帧)上的面点为点Xk+1,m，位于前一帧(第k帧)上的面点为点Xk,n，从第k帧中再选取两个面点Xk,p和Xk,q，则由点Xk,n和点Xk,p、Xk,q即可表示位于前一帧上的平面(应注意，点Xk,n、点Xk,p和点Xk,q不在同一直线上)。之后，根据下式(2)计算点Xk+1,i到直线(由点Xk,j和点Xk,l表示)的第一距离dline，根据下式(3)计算点Xk+1,m到平面(由点Xk,n、点Xk,p和点Xk,q表示)的第二距离dplane：
Figure BDA0002558716280000101
Figure BDA0002558716280000102
之后，即可通过使dline和dplane达到最小，以计算第k+1帧与第k帧之间的位姿增量ΔTk+1。
步骤4、将雷达采集的点云点按下式(4)转换为地图系点云：
Figure BDA0002558716280000111
其中，
Figure BDA0002558716280000112
为雷达所采集的点云数据在雷达坐标系下所对应的坐标，Xk+1,a为位姿变换后在地图系下所对应的坐标，Tk+1为前帧在局部点云地图中的位姿。在本发明的实施例中，该位姿Tk+1的求解方式如下：
将当前帧的邻近帧(例如：当前帧的前50帧)的线点组成局部线点地图，邻近帧的面点组成局部面点地图，进而得到局部点云地图。然后，将第k+1帧与第k帧之间的位姿增量ΔTk+1作为初始值，求解当前帧在局部点云地图中的位姿Tk+1。具体地，将步骤3中得到的所有约束写成(5)式的形式，位姿变换为优化目标，每一行对应一个线点或面点，计算雅克比矩阵J，然后通过非线性优化迭代(6)式使d(包括dline和dplane)达到最小值，此时的Tk+1即为第k+1帧的位姿。
f(Tk+1)＝d (5)；
Figure BDA0002558716280000113
其中，
Figure BDA0002558716280000114
λ是优化系统。
步骤5、根据步骤4的结果，对属于同一个雷达的相邻两帧建立位姿约束。之后，将当前帧与该雷达整个位姿轨迹比较，寻找闭环，并添加闭环位姿约束。闭环位姿变换可采用ICP(Iterative Closest Point，最近点迭代)或GICP(General Iterative ClosestPoint，泛化最近点迭代)迭代求解。
步骤6、对不同雷达的相邻帧之间根据位姿和安装关系建立位姿约束。此处，相邻帧指的是时间间隔最近的两个点云数据帧。不同雷达的相邻帧在获取时，是将与采集当前帧的雷达不同的另一雷达所采集的点云数据帧，根据采集时间分别与当前帧进行比较来确定。然后，根据两个雷达之间的安装关系和位姿关系即可建立不同雷达的相邻帧之间的位姿约束。
图3是本发明一个实施例的不同雷达间的位姿约束的构建原理示意图。如图3所示，依据其中一个雷达(比如右雷达)的位姿和安装关系先得到另一雷达(比如左雷达)的目标位姿，再与左雷达实际的位姿建立位姿约束。
步骤7、将同雷达相邻帧位姿约束、同雷达闭环位姿约束和不同雷达的相邻帧之间的位姿约束形成式(7)所示的格式，基于与(6)式类似的方式，对这些位姿约束进行迭代求解，以进行位姿优化调整，并更新各个点云数据帧的位姿。
f(Ti,Tj)＝Δi,j (7)；
其中，Δi,j表示不同雷达的相邻帧之间的位姿增量。
步骤8、根据更新后的位姿和其对应的雷达数据，拼接生成点云地图。
图4是根据本发明实施例的基于双多线雷达的建图装置的主要模块示意图。如图4所示，本发明实施例的基于双多线雷达的建图装置400主要包括同雷达数据处理模块401、雷达间数据处理模块402和位姿更新模块403。
同雷达数据处理模块401，用于分别对两个雷达的点云数据进行处理以建立第一位姿约束，所述第一位姿约束包括同雷达相邻点云数据帧的位姿约束和同雷达闭环位姿约束；
雷达间数据处理模块402，用于根据雷达间的位姿和安装关系建立第二位姿约束，所述第二位姿约束包括不同雷达的相邻点云数据帧的位姿约束，所述相邻点云数据帧为时间间隔最近的两个点云数据帧；
位姿更新模块403，用于根据所述第一位姿约束和所述第二位姿约束更新两个雷达的点云数据的位姿，并根据更新后的位姿生成点云地图。
根据本发明的一个实施例，同雷达相邻点云数据帧的位姿约束例如可以通过以下方式建立：
从同雷达的相邻点云数据帧的线点和面点中，获取距离最近的匹配线点和匹配面点，并根据所述匹配线点和所述匹配面点计算相邻两帧点云数据的位姿增量；
将所述位姿增量作为初始值，将当前点云数据帧的邻近帧组成的点云地图作为局部点云地图，然后计算当前点云数据帧在所述局部点云地图中的位姿以建立同雷达相邻帧点云数据的位姿约束。
在本发明的实施例中，线点和面点是通过以下方式确定的：
计算点云数据帧的光滑度得分；
将光滑度得分大于第一阈值的点作为线点，小于第二阈值的点作为面点。
根据本发明的另一个实施例，获取距离最近的匹配线点和匹配面点具体可以包括：
从同雷达的点云数据中，获取与当前点云数据帧相邻的前一点云数据帧；
分别从所述前一点云数据帧的线点和面点中，获取与所述当前点云数据帧的线点和面点距离最近的线点和面点，并将距离最近的当前点云数据帧的线点和前一点云数据帧的线点作为相邻点云数据帧的匹配线点，将距离最近的当前点云数据帧的面点和前一点云数据帧的面点作为相邻点云数据帧的匹配面点。
根据本发明的又一个实施例，根据所述匹配线点和所述匹配面点计算相邻两帧点云数据的位姿增量包括：
根据所述匹配线点，从所述前一点云数据帧的线点中获取另外一个线点，并与所述匹配线点中所述前一点云数据帧的线点构成一条直线，计算所述匹配线点中所述当前点云数据帧的线点到所述直线的第一距离；
根据所述匹配面点，从所述前一点云数据帧的面点中获取另外两个面点，并与所述匹配面点中所述前一点云数据帧的面点构成一个平面，计算所述匹配面点中所述当前点云数据帧的面点到所述平面的第二距离；
通过使所述第一距离和所述第二距离最小，以计算相邻两帧点云数据的位姿增量。
在本发明的实施例中，局部点云地图例如可以通过以下方式得到：
将邻近帧的线点组成局部线点地图，邻近帧的面点组成局部面点地图，从而拼接得到局部点云地图。
根据本发明的又一个实施例，所述同雷达闭环位姿约束通过以下方式建立：
将当前点云数据帧在所述局部点云地图中的位姿与雷达的整个位姿轨迹进行比较，寻找闭环以建立闭环位姿约束，所述雷达的整个位姿轨迹由所述雷达所采集的点云数据的位姿构成。
根据本发明的再一个实施例，位姿更新模块403还可以用于：
根据更新后的位姿，以及雷达的点云数据，对点云数据帧进行拼接生成点云地图。
根据本发明实施例的技术方案，通过分别对两个雷达的点云数据进行处理以建立第一位姿约束，第一位姿约束包括同雷达相邻点云数据帧的位姿约束和同雷达闭环位姿约束；根据雷达间的位姿和安装关系建立第二位姿约束，第二位姿约束包括不同雷达的相邻点云数据帧的位姿约束，相邻点云数据帧为时间间隔最近的两个点云数据帧；根据第一位姿约束和第二位姿约束更新两个雷达的点云数据的位姿，并根据更新后的位姿生成点云地图，可以使用两个雷达的数据同时建图，能够减小盲区影响，补全单个雷达看不到的地图细节；由于约束增加，使得地图的精度也会有所提高。前期对各个雷达的数据处理相互独立，互不依赖，可以采用并行的方式，故而可以加快求解速度；后期再建立两个雷达之间数据的位姿约束，根据同雷达相邻帧位姿约束、同雷达闭环约束和不同雷达相邻帧位姿约束求解整个位姿轨迹并生成地图，可以更好地建立雷达点云数据之间的位姿约束，使得建图误差达到最小，建图精度大大提高。
图5示出了可以应用本发明实施例的基于双多线雷达的建图方法或基于双多线雷达的建图装置的示例性系统架构500。
如图5所示，系统架构500可以包括终端设备501、502、503，网络504和服务器505。网络504用以在终端设备501、502、503和服务器505之间提供通信链路的介质。网络504可以包括各种连接类型，例如有线、无线通信链路或者光纤电缆等等。
用户可以使用终端设备501、502、503通过网络504与服务器505交互，以接收或发送消息等。终端设备501、502、503上可以安装有各种通讯客户端应用，例如购物类应用、网页浏览器应用、搜索类应用、即时通信工具、邮箱客户端、社交平台软件等(仅为示例)。
终端设备501、502、503可以是具有显示屏并且支持网页浏览的各种电子设备，包括但不限于智能手机、平板电脑、膝上型便携计算机和台式计算机等等。
服务器505可以是提供各种服务的服务器，例如对用户利用终端设备501、502、503所浏览的购物类网站提供支持的后台管理服务器(仅为示例)。后台管理服务器可以对接收到的产品信息查询请求等数据进行分析等处理，并将处理结果(例如目标推送信息、产品信息--仅为示例)反馈给终端设备。
需要说明的是，本发明实施例所提供的基于双多线雷达的建图方法一般由服务器505执行，相应地，基于双多线雷达的建图装置一般设置于服务器505中。
应该理解，图5中的终端设备、网络和服务器的数目仅仅是示意性的。根据实现需要，可以具有任意数目的终端设备、网络和服务器。
下面参考图6，其示出了适于用来实现本发明实施例的终端设备或服务器的计算机系统600的结构示意图。图6示出的终端设备或服务器仅仅是一个示例，不应对本发明实施例的功能和使用范围带来任何限制。
如图6所示，计算机系统600包括中央处理单元(CPU)601，其可以根据存储在只读存储器(ROM)602中的程序或者从存储部分608加载到随机访问存储器(RAM)603中的程序而执行各种适当的动作和处理。在RAM 603中，还存储有系统600操作所需的各种程序和数据。CPU 601、ROM 602以及RAM 603通过总线604彼此相连。输入/输出(I/O)接口605也连接至总线604。
以下部件连接至I/O接口605：包括键盘、鼠标等的输入部分606；包括诸如阴极射线管(CRT)、液晶显示器(LCD)等以及扬声器等的输出部分607；包括硬盘等的存储部分608；以及包括诸如LAN卡、调制解调器等的网络接口卡的通信部分609。通信部分609经由诸如因特网的网络执行通信处理。驱动器610也根据需要连接至I/O接口605。可拆卸介质611，诸如磁盘、光盘、磁光盘、半导体存储器等等，根据需要安装在驱动器610上，以便于从其上读出的计算机程序根据需要被安装入存储部分608。
特别地，根据本发明公开的实施例，上文参考流程图描述的过程可以被实现为计算机软件程序。例如，本发明公开的实施例包括一种计算机程序产品，其包括承载在计算机可读介质上的计算机程序，该计算机程序包含用于执行流程图所示的方法的程序代码。在这样的实施例中，该计算机程序可以通过通信部分609从网络上被下载和安装，和/或从可拆卸介质611被安装。在该计算机程序被中央处理单元(CPU)601执行时，执行本发明的系统中限定的上述功能。
需要说明的是，本发明所示的计算机可读介质可以是计算机可读信号介质或者计算机可读存储介质或者是上述两者的任意组合。计算机可读存储介质例如可以是——但不限于——电、磁、光、电磁、红外线、或半导体的系统、装置或器件，或者任意以上的组合。计算机可读存储介质的更具体的例子可以包括但不限于：具有一个或多个导线的电连接、便携式计算机磁盘、硬盘、随机访问存储器(RAM)、只读存储器(ROM)、可擦式可编程只读存储器(EPROM或闪存)、光纤、便携式紧凑磁盘只读存储器(CD-ROM)、光存储器件、磁存储器件、或者上述的任意合适的组合。在本发明中，计算机可读存储介质可以是任何包含或存储程序的有形介质，该程序可以被指令执行系统、装置或者器件使用或者与其结合使用。而在本发明中，计算机可读的信号介质可以包括在基带中或者作为载波一部分传播的数据信号，其中承载了计算机可读的程序代码。这种传播的数据信号可以采用多种形式，包括但不限于电磁信号、光信号或上述的任意合适的组合。计算机可读的信号介质还可以是计算机可读存储介质以外的任何计算机可读介质，该计算机可读介质可以发送、传播或者传输用于由指令执行系统、装置或者器件使用或者与其结合使用的程序。计算机可读介质上包含的程序代码可以用任何适当的介质传输，包括但不限于：无线、电线、光缆、RF等等，或者上述的任意合适的组合。
附图中的流程图和框图，图示了按照本发明各种实施例的系统、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，流程图或框图中的每个方框可以代表一个模块、程序段、或代码的一部分，上述模块、程序段、或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在有些作为替换的实现中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个接连地表示的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，框图或流程图中的每个方框、以及框图或流程图中的方框的组合，可以用执行规定的功能或操作的专用的基于硬件的系统来实现，或者可以用专用硬件与计算机指令的组合来实现。
描述于本发明实施例中所涉及到的单元或模块可以通过软件的方式实现，也可以通过硬件的方式来实现。所描述的单元或模块也可以设置在处理器中，例如，可以描述为：一种处理器包括同雷达数据处理模块、雷达间数据处理模块和位姿更新模块。其中，这些单元或模块的名称在某种情况下并不构成对该单元或模块本身的限定，例如，同雷达数据处理模块还可以被描述为“分别对两个雷达的点云数据进行处理以建立第一位姿约束的模块”。
作为另一方面，本发明还提供了一种计算机可读介质，该计算机可读介质可以是上述实施例中描述的设备中所包含的；也可以是单独存在，而未装配入该设备中。上述计算机可读介质承载有一个或者多个程序，当上述一个或者多个程序被一个该设备执行时，使得该设备包括：分别对两个雷达的点云数据进行处理以建立第一位姿约束，所述第一位姿约束包括同雷达相邻点云数据帧的位姿约束和同雷达闭环位姿约束；根据雷达间的位姿和安装关系建立第二位姿约束，所述第二位姿约束包括不同雷达的相邻点云数据帧的位姿约束，所述相邻点云数据帧为时间间隔最近的两个点云数据帧；根据所述第一位姿约束和所述第二位姿约束更新两个雷达的点云数据的位姿，并根据更新后的位姿生成点云地图。
根据本发明实施例的技术方案，通过分别对两个雷达的点云数据进行处理以建立第一位姿约束，第一位姿约束包括同雷达相邻点云数据帧的位姿约束和同雷达闭环位姿约束；根据雷达间的位姿和安装关系建立第二位姿约束，第二位姿约束包括不同雷达的相邻点云数据帧的位姿约束，相邻点云数据帧为时间间隔最近的两个点云数据帧；根据第一位姿约束和第二位姿约束更新两个雷达的点云数据的位姿，并根据更新后的位姿生成点云地图，可以使用两个雷达的数据同时建图，能够减小盲区影响，补全单个雷达看不到的地图细节；由于约束增加，使得地图的精度也会有所提高。前期对各个雷达的数据处理相互独立，互不依赖，可以采用并行的方式，故而可以加快求解速度；后期再建立两个雷达之间数据的位姿约束，根据同雷达相邻帧位姿约束、同雷达闭环约束和不同雷达相邻帧位姿约束求解整个位姿轨迹并生成地图，可以更好地建立雷达点云数据之间的位姿约束，使得建图误差达到最小，建图精度大大提高。
上述具体实施方式，并不构成对本发明保护范围的限制。本领域技术人员应该明白的是，取决于设计要求和其他因素，可以发生各种各样的修改、组合、子组合和替代。任何在本发明的精神和原则之内所作的修改、等同替换和改进等，均应包含在本发明保护范围之内。

Claims (11)

1.一种基于双多线雷达的建图方法，其特征在于，包括：
分别对两个雷达的点云数据进行处理以建立第一位姿约束，所述第一位姿约束包括同雷达相邻点云数据帧的位姿约束和同雷达闭环位姿约束；
根据雷达间的位姿和安装关系建立第二位姿约束，所述第二位姿约束包括不同雷达的相邻点云数据帧的位姿约束，所述相邻点云数据帧为时间间隔最近的两个点云数据帧；
根据所述第一位姿约束和所述第二位姿约束更新两个雷达的点云数据的位姿，并根据更新后的位姿生成点云地图。
2.根据权利要求1所述的建图方法，其特征在于，所述同雷达相邻点云数据帧的位姿约束通过以下方式建立：
从同雷达的相邻点云数据帧的线点和面点中，获取距离最近的匹配线点和匹配面点，并根据所述匹配线点和所述匹配面点计算相邻两帧点云数据的位姿增量；
将所述位姿增量作为初始值，将当前点云数据帧的邻近帧组成的点云地图作为局部点云地图，然后计算当前点云数据帧在所述局部点云地图中的位姿以建立同雷达相邻帧点云数据的位姿约束。
3.根据权利要求2所述的建图方法，其特征在于，所述线点和面点通过以下方式确定：
计算点云数据帧的光滑度得分；
将光滑度得分大于第一阈值的点作为线点，小于第二阈值的点作为面点。
4.根据权利要求2所述的建图方法，其特征在于，获取距离最近的匹配线点和匹配面点包括：
从同雷达的点云数据中，获取与当前点云数据帧相邻的前一点云数据帧；
分别从所述前一点云数据帧的线点和面点中，获取与所述当前点云数据帧的线点和面点距离最近的线点和面点，并将距离最近的当前点云数据帧的线点和前一点云数据帧的线点作为相邻点云数据帧的匹配线点，将距离最近的当前点云数据帧的面点和前一点云数据帧的面点作为相邻点云数据帧的匹配面点。
5.根据权利要求4所述的建图方法，其特征在于，根据所述匹配线点和所述匹配面点计算相邻两帧点云数据的位姿增量包括：
根据所述匹配线点，从所述前一点云数据帧的线点中获取另外一个线点，并与所述匹配线点中所述前一点云数据帧的线点构成一条直线，计算所述匹配线点中所述当前点云数据帧的线点到所述直线的第一距离；
根据所述匹配面点，从所述前一点云数据帧的面点中获取另外两个面点，并与所述匹配面点中所述前一点云数据帧的面点构成一个平面，计算所述匹配面点中所述当前点云数据帧的面点到所述平面的第二距离；
通过使所述第一距离和所述第二距离最小，以计算相邻两帧点云数据的位姿增量。
6.根据权利要求2所述的建图方法，其特征在于，所述局部点云地图通过以下方式得到：
将邻近帧的线点组成局部线点地图，邻近帧的面点组成局部面点地图，从而拼接得到局部点云地图。
7.根据权利要求1所述的建图方法，其特征在于，所述同雷达闭环位姿约束通过以下方式建立：
将当前点云数据帧在所述局部点云地图中的位姿与雷达的整个位姿轨迹进行比较，寻找闭环以建立闭环位姿约束，所述雷达的整个位姿轨迹由所述雷达所采集的点云数据的位姿构成。
8.根据权利要求1所述的建图方法，其特征在于，根据更新后的位姿生成点云地图包括：
根据更新后的位姿，以及雷达的点云数据，对点云数据帧进行拼接生成点云地图。
9.一种基于双多线雷达的建图装置，其特征在于，包括：
同雷达数据处理模块，用于分别对两个雷达的点云数据进行处理以建立第一位姿约束，所述第一位姿约束包括同雷达相邻点云数据帧的位姿约束和同雷达闭环位姿约束；
雷达间数据处理模块，用于根据雷达间的位姿和安装关系建立第二位姿约束，所述第二位姿约束包括不同雷达的相邻点云数据帧的位姿约束，所述相邻点云数据帧为时间间隔最近的两个点云数据帧；
位姿更新模块，用于根据所述第一位姿约束和所述第二位姿约束更新两个雷达的点云数据的位姿，并根据更新后的位姿生成点云地图。
10.一种基于双多线雷达的建图电子设备，其特征在于，包括：
一个或多个处理器；
存储装置，用于存储一个或多个程序，
当所述一个或多个程序被所述一个或多个处理器执行，使得所述一个或多个处理器实现如权利要求1-8中任一所述的方法。
11.一种计算机可读介质，其上存储有计算机程序，其特征在于，所述程序被处理器执行时实现如权利要求1-8中任一所述的方法。
CN202010599593.3A 2020-06-28 2020-06-28 基于双多线雷达的建图方法和装置 Active CN113759369B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010599593.3A CN113759369B (zh) 2020-06-28 2020-06-28 基于双多线雷达的建图方法和装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010599593.3A CN113759369B (zh) 2020-06-28 2020-06-28 基于双多线雷达的建图方法和装置

Publications (2)

Publication Number Publication Date
CN113759369A true CN113759369A (zh) 2021-12-07
CN113759369B CN113759369B (zh) 2023-12-05

Family

ID=78785446

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010599593.3A Active CN113759369B (zh) 2020-06-28 2020-06-28 基于双多线雷达的建图方法和装置

Country Status (1)

Country Link
CN (1) CN113759369B (zh)

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108320329A (zh) * 2018-02-02 2018-07-24 维坤智能科技（上海）有限公司 一种基于3d激光的3d地图创建方法
CN108332759A (zh) * 2018-01-12 2018-07-27 浙江国自机器人技术有限公司 一种基于3d激光的地图构建方法及系统
CN108550318A (zh) * 2018-03-12 2018-09-18 浙江大华技术股份有限公司 一种构建地图的方法及装置
CN108732584A (zh) * 2017-04-17 2018-11-02 百度在线网络技术（北京）有限公司 用于更新地图的方法和装置
CN108765487A (zh) * 2018-06-04 2018-11-06 百度在线网络技术（北京）有限公司 重建三维场景的方法、装置、设备和计算机可读存储介质
CN109934920A (zh) * 2019-05-20 2019-06-25 奥特酷智能科技(南京)有限公司 基于低成本设备的高精度三维点云地图构建方法
CN110243370A (zh) * 2019-05-16 2019-09-17 西安理工大学 一种基于深度学习的室内环境三维语义地图构建方法
CN110749901A (zh) * 2019-10-12 2020-02-04 劢微机器人科技(深圳)有限公司 自主移动机器人及其地图拼接方法、装置和可读存储介质

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108732584A (zh) * 2017-04-17 2018-11-02 百度在线网络技术（北京）有限公司 用于更新地图的方法和装置
CN108332759A (zh) * 2018-01-12 2018-07-27 浙江国自机器人技术有限公司 一种基于3d激光的地图构建方法及系统
CN108320329A (zh) * 2018-02-02 2018-07-24 维坤智能科技（上海）有限公司 一种基于3d激光的3d地图创建方法
CN108550318A (zh) * 2018-03-12 2018-09-18 浙江大华技术股份有限公司 一种构建地图的方法及装置
CN108765487A (zh) * 2018-06-04 2018-11-06 百度在线网络技术（北京）有限公司 重建三维场景的方法、装置、设备和计算机可读存储介质
CN110243370A (zh) * 2019-05-16 2019-09-17 西安理工大学 一种基于深度学习的室内环境三维语义地图构建方法
CN109934920A (zh) * 2019-05-20 2019-06-25 奥特酷智能科技(南京)有限公司 基于低成本设备的高精度三维点云地图构建方法
CN110749901A (zh) * 2019-10-12 2020-02-04 劢微机器人科技(深圳)有限公司 自主移动机器人及其地图拼接方法、装置和可读存储介质

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
危双丰等: "基于激光雷达的同时定位与地图构建方法综述", 计算机应用研究, vol. 37, no. 2, pages 327 - 332 *
陈超;张伟伟;徐军;: "一种基于三维视觉的移动机器人定位与建图方法", 现代电子技术, no. 06, pages 34 - 38 *

Also Published As

Publication number Publication date
CN113759369B (zh) 2023-12-05

Similar Documents

Publication Publication Date Title
CN108629823B (zh) 多视角图像的生成方法和装置
CN113869293B (zh) 车道线识别方法、装置、电子设备和计算机可读介质
CN111815738B (zh) 一种构建地图的方法和装置
CN112652036A (zh) 道路数据的处理方法、装置、设备及存储介质
CN114120414B (zh) 图像处理方法、装置、电子设备和介质
CN111104479A (zh) 一种数据标注的方法及装置
CN110633717A (zh) 一种目标检测模型的训练方法和装置
CN110288625A (zh) 用于处理图像的方法和装置
CN112528995A (zh) 用于训练目标检测模型的方法、目标检测方法及装置
CN110211017B (zh) 图像处理方法、装置及电子设备
CN114792355A (zh) 虚拟形象生成方法、装置、电子设备和存储介质
CN114428907A (zh) 信息搜索方法、装置、电子设备及存储介质
CN110633716A (zh) 一种目标对象的检测方法和装置
CN110378904B (zh) 对点云数据进行分割的方法和装置
CN110363847B (zh) 一种基于点云数据的地图模型构建方法和装置
CN113759369B (zh) 基于双多线雷达的建图方法和装置
CN114581523A (zh) 一种用于单目3d目标检测的标注数据确定方法和装置
CN112990046B (zh) 差异信息获取方法、相关装置及计算机程序产品
CN110375752B (zh) 一种生成导航点的方法和装置
CN113778078A (zh) 定位信息生成方法、装置、电子设备和计算机可读介质
CN113761090B (zh) 一种基于点云地图的定位方法和装置
CN111161067A (zh) 一种交易路由的确定方法和装置
CN112183799A (zh) 用于合成任务单的任务分配方法及装置
CN112180910A (zh) 一种移动机器人障碍物感知方法和装置
CN110633595A (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