CN114119744B - 一种构建点云地图的方法、装置、设备及存储介质 - Google Patents
一种构建点云地图的方法、装置、设备及存储介质 Download PDFInfo
- Publication number
- CN114119744B CN114119744B CN202111316500.2A CN202111316500A CN114119744B CN 114119744 B CN114119744 B CN 114119744B CN 202111316500 A CN202111316500 A CN 202111316500A CN 114119744 B CN114119744 B CN 114119744B
- Authority
- CN
- China
- Prior art keywords
- data
- point cloud
- moment
- pose
- cloud data
- 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
- 238000000034 method Methods 0.000 title claims abstract description 72
- 230000008859 change Effects 0.000 claims abstract description 51
- 230000033001 locomotion Effects 0.000 claims description 36
- 230000010354 integration Effects 0.000 claims description 13
- 238000004590 computer program Methods 0.000 claims description 10
- 238000005259 measurement Methods 0.000 claims description 5
- 238000004422 calculation algorithm Methods 0.000 description 17
- 230000008569 process Effects 0.000 description 12
- 238000004891 communication Methods 0.000 description 6
- 238000009826 distribution Methods 0.000 description 6
- 238000010586 diagram Methods 0.000 description 4
- 238000012545 processing Methods 0.000 description 4
- 230000009471 action Effects 0.000 description 3
- 238000010276 construction Methods 0.000 description 3
- 238000006073 displacement reaction Methods 0.000 description 3
- 230000004048 modification Effects 0.000 description 3
- 238000012986 modification Methods 0.000 description 3
- 230000005540 biological transmission Effects 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 230000003287 optical effect Effects 0.000 description 2
- 230000009467 reduction Effects 0.000 description 2
- 238000007792 addition Methods 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 230000008878 coupling Effects 0.000 description 1
- 238000010168 coupling process Methods 0.000 description 1
- 238000005859 coupling reaction Methods 0.000 description 1
- 238000013500 data storage Methods 0.000 description 1
- 238000009795 derivation Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 239000000835 fiber Substances 0.000 description 1
- 238000001914 filtration Methods 0.000 description 1
- 230000006870 function Effects 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 239000011159 matrix material Substances 0.000 description 1
- 230000002093 peripheral effect Effects 0.000 description 1
- 238000011084 recovery Methods 0.000 description 1
- 239000004065 semiconductor Substances 0.000 description 1
- 239000007787 solid Substances 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 238000012546 transfer Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/70—Determining position or orientation of objects or cameras
- G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01D—MEASURING NOT SPECIALLY ADAPTED FOR A SPECIFIC VARIABLE; ARRANGEMENTS FOR MEASURING TWO OR MORE VARIABLES NOT COVERED IN A SINGLE OTHER SUBCLASS; TARIFF METERING APPARATUS; MEASURING OR TESTING NOT OTHERWISE PROVIDED FOR
- G01D21/00—Measuring or testing not otherwise provided for
- G01D21/02—Measuring two or more variables by means not covered by a single other subclass
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/10—Image acquisition modality
- G06T2207/10028—Range image; Depth image; 3D point clouds
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/10—Image acquisition modality
- G06T2207/10032—Satellite or aerial image; Remote sensing
- G06T2207/10044—Radar image
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Theoretical Computer Science (AREA)
- Navigation (AREA)
Abstract
本申请实施例公开了一种构建点云地图的方法、装置、设备及存储介质。该方法包括：获取子图点云数据、预设时间段内每个时刻的点云数据和位姿、轮速计数据、IMU数据和GNSS数据；确定预设时间段内第一时刻与第二时刻之间的位姿变化量；计算第二时刻的匹配位姿；确定预设时间段内的关键帧点云数据和第二时刻的关键帧点云数据的回环相对位姿；计算多个约束数据，并根据匹配位姿和多个约束数据，计算得到第二时刻的关键帧位姿，根据第二时刻的关键帧位姿，拼接第二时刻的关键帧点云数据，得到点云地图。本申请实施例提供的方法能够构建高精度的点云地图。
Description
技术领域
本申请涉及点云地图技术领域，尤其涉及一种构建点云地图的方法、装置、设备及存储介质。
背景技术
随着自动驾驶技术的发展，自动驾驶系统对地图精度的要求越来越高，现有的地图已不能满足需求。
通常首先采用激光雷达、轮速计等多传感器采集的数据构建点云地图，其中，激光雷达转动一周指向同一个位置的时间差导致雷达数据不匹配，也就是产生运动畸变，从而造成激光雷达采集的点云数据不准确，通常采用里程计等传感器采集的数据对点云数据去运动畸变；再对轮速计采集的数据做预积分，得到轮速惯性预积分结果，采用轮速惯性预积分结果对当前帧点云数据的位姿进行预测，得到位姿的预测解；然后，利用点云匹配方法将当前帧点云数据与子图点云中的点云数据进行匹配，得到位姿的匹配解，也就是点云的先验位姿；根据轮速计采集的数据与回环约束作为约束项求解用于构建地图的点云位姿，回环约束指当前帧点云数据与当前帧之前的一帧点云数据之间的相对位姿；最后基于点云位姿和点云构建点云地图。
由于现有的构建点云地图的方法中，激光雷达以外的传感器采集的数据仅用于上述构建点云地图过程中的去运动畸变和计算先验位姿，且求解用于构建地图的点云位姿时采用的约束数据较少，造成构建的点云地图精度低。
发明内容
本申请实施例提供一种构建点云地图的方法、装置、设备及存储介质，能够构建高精度点云地图。
第一方面，本申请实施例提供一种构建点云地图的方法，该方法包括：
获取子图点云数据、预设时间段内每个时刻的点云数据和位姿、轮速计数据、惯性测量单元(Inertial Measurement Unit，IMU)数据和全球卫星导航系统(GlobalNavigation Satellite System，GNSS)数据，子图点云数据包括预设时间段内的关键帧点云数据，点云数据包括关键帧点云数据；
根据轮速计数据、IMU数据，确定预设时间段内第一时刻与第二时刻之间的位姿变化量；
根据子图点云数据、第一时刻的位姿、第二时刻的点云数据和位姿变化量，计算第二时刻的匹配位姿；
根据预设时间段内的关键帧点云数据、第二时刻的关键帧点云数据，确定预设时间段内的关键帧点云数据和第二时刻的关键帧点云数据的回环相对位姿，回环相对位姿表征预设时间段内与第二时刻的关键帧点云数据的相似度满足预设条件的关键帧点云数据，与第二时刻的关键帧点云数据的第一相对位姿；
根据第一时刻的关键帧点云数据、第二时刻的关键帧点云数据和回环相对位姿，计算回环约束数据，回环约束数据表征回环相对位姿对应的误差数据；根据第一时刻的关键帧点云数据、第二时刻的关键帧点云数据和位姿变化量，计算预积分约束数据，预积分约束数据表征位姿变化量对应的误差数据；根据第一时刻的关键帧点云数据和第二时刻的关键帧点云数据，计算激光里程计约束数据，激光里程计约束数据表征第一时刻的关键帧点云数据和第二时刻的关键帧点云数据的第二相对位姿对应的误差数据；GNSS数据包括第二时刻的关键帧点云数据的全局位姿约束数据，根据第一时刻的关键帧点云数据和全局位姿约束数据，计算GNSS约束数据，GNSS约束数据表征全局位姿约束数据对应的误差数据；根据第一时刻的位姿中的旋转数据和匹配位姿中的旋转数据，计算运动约束数据，运动约束数据表征第一时刻的位姿中的旋转数据和匹配位姿中的旋转数据之间的相对误差数据；
根据匹配位姿、回环约束数据、预积分约束数据、激光里程计约束数据、GNSS约束数据和运动约束数据，计算得到第二时刻的关键帧位姿；
根据第二时刻的关键帧位姿，拼接第二时刻的关键帧点云数据，得到点云地图。
在一种可能的实现方式中，获取预设时间段内每个时刻的点云数据、轮速计数据、IMU数据和GNSS数据，包括：
获取预设时间段内每个时刻的点云数据、初始轮速计数据、初始IMU数据和初始GNSS数据；
对点云数据、初始轮速计数据、初始IMU数据和所初始述GNSS数据时间对齐，并对初始轮速计数据、初始IMU数据和初始GNSS数据中的至少一项插值，得到每个时刻相对应的点云数据、轮速计数据、IMU数据和GNSS数据。
在一种可能的实现方式中，获取预设时间段内第二时刻的点云数据，包括：
获取第二时刻的初始点云数据；
根据位姿变化量校正初始点云数据，得到第二时刻的点云数据。
在一种可能的实现方式中，根据子图点云数据、第一时刻的位姿、第二时刻的点云数据和位姿变化量，计算第二时刻的匹配位姿，包括：
根据位姿变化量和第一时刻的位姿，确定第二时刻的预测位姿；
根据预测位姿，匹配子图点云数据和第二时刻的点云数据，得到第二时刻的匹配位姿。
在一种可能的实现方式中，根据预设时间段内的关键帧点云数据、第二时刻的关键帧点云数据，确定预设时间段内的关键帧点云数据和第二时刻的关键帧点云数据的回环相对位姿，包括：
将预设时间段内的关键帧点云数据中与第二关键帧点云数据的相似度满足预设条件的关键帧点云数据，确定为回环关键帧点云数据；
根据第二时刻的关键帧点云数据和回环关键帧点云数据，计算第二时刻的关键帧点云数据和回环关键帧点云数据的回环相对位姿。
第二方面，本申请实施例提供了一种构建点云地图的装置，该装置包括：
获取模块，用于获取子图点云数据、预设时间段内每个时刻的点云数据和位姿、轮速计数据、惯性测量单元IMU数据和全球卫星导航系统GNSS数据，子图点云数据包括预设时间段内的关键帧点云数据，点云数据包括关键帧点云数据；
确定模块，用于根据轮速计数据、IMU数据，确定预设时间段内第一时刻与第二时刻之间的位姿变化量；
计算模块，用于根据所述子图点云数据、所述第一时刻的位姿、所述第二时刻的点云数据和位姿变化量，计算所述第二时刻的匹配位姿；
确定模块，还用于根据预设时间段内的关键帧点云数据、第二时刻的关键帧点云数据，确定预设时间段内的关键帧点云数据和第二时刻的关键帧点云数据的回环相对位姿，回环相对位姿表征预设时间段内与第二关键帧点云数据的相似度满足预设条件的关键帧点云数据，与第二时刻的关键帧点云数据的第一相对位姿；
计算模块，还用于根据第一时刻的关键帧点云数据、第二时刻的关键帧点云数据和回环相对位姿，计算回环约束数据，回环约束数据表征回环相对位姿对应的误差数据；还用于根据第一时刻的关键帧点云数据、第二时刻的关键帧点云数据和位姿变化量，计算预积分约束数据，预积分约束数据表征位姿变化量对应的误差数据；还用于根据第一时刻的关键帧点云数据和第二时刻的关键帧点云数据，计算激光里程计约束数据，激光里程计约束数据表征第一时刻的关键帧点云数据和第二时刻的关键帧点云数据的第二相对位姿对应的误差数据；GNSS数据包括第二时刻的关键帧点云数据的全局位姿约束数据，根据第二时刻的关键帧点云数据和全局位姿约束数据，计算GNSS约束数据，GNSS约束数据表征全局位姿约束数据对应的误差数据；根据第一时刻的位姿中的旋转数据和匹配位姿中的旋转数据，计算运动约束数据，运动约束数据表征第一时刻的位姿中的旋转数据和匹配位姿中的旋转数据之间的相对误差数据；还用于根据匹配位姿、回环约束数据、预积分约束数据、激光里程计约束数据、GNSS约束数据和运动约束数据，计算得到第二时刻的关键帧位姿；
拼接模块，用于根据第二时刻的关键帧位姿，拼接第二时刻的关键帧点云数据，得到点云地图。
在一种可能的实现方式中，获取模块，具体用于：
获取预设时间段内每个时刻的点云数据、初始轮速计数据、初始IMU数据和初始GNSS数据；
对点云数据、初始轮速计数据、初始IMU数据和初始GNSS数据时间对齐，并对初始轮速计数据、初始IMU数据和初始GNSS数据中的至少一项插值，得到每个时刻相对应的点云数据、轮速计数据、IMU数据和GNSS数据。
在一种可能的实现方式中，获取模块，具体用于：
获取第二时刻的初始点云数据；
根据位姿变化量校正初始点云数据，得到第二时刻的点云数据。
第三方面，本申请实施例提供了一种电子设备，该设备包括：处理器以及存储有计算机程序指令的存储器；处理器执行计算机程序指令时，实现如第一方面或者第一方面的任一可能实现方式中的方法。
第四方面，本申请实施例提供了一种计算机可读存储介质，该计算机可读存储介质上存储有计算机程序指令，计算机程序指令被处理器执行时实现如第一方面或者第一方面的任一可能实现方式中的方法。
本申请实施例的构建点云地图的方法、装置、设备及存储介质，先获取了子图点云数据、预设时间段内每个时刻的点云数据、轮速计数据、IMU数据和GNSS数据，根据轮速计数据和IMU数据，确定预设时间段内第一时刻与第二时刻之间的位姿变化量；然后，根据子图点云数据、第一时刻的位姿、第二时刻的点云数据和位姿变化量，计算第二时刻的匹配位姿，也就是第二时刻的点云数据相对于子图点云数据的位姿；然后，计算预积分约束数据和运动约束数据在内的多个数据，并根据匹配位姿和多个约束数据计算第二时刻的关键帧位姿，由于计算第二时刻的关键帧位姿时采用了由多个传感器的数据计算的多个约束数据，所以第二时刻的关键帧位姿具有较高的精准性；最后，根据第二时刻的关键帧位姿，拼接第二时刻的关键帧点云数据，得到点云地图，由于第二时刻的关键帧位姿具有较高的精准性，所以根据第二时刻的关键帧位姿拼接第二时刻的关键帧点云数据，得到的点云地图的也具有较高的精度，实现了构建高精度点云地图。
附图说明
为了更清楚地说明本申请实施例的技术方案，下面将对本申请实施例中所需要使用的附图作简单的介绍，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1是本申请实施例提供的一种构建点云地图的方法流程示意图；
图2是本申请实施例提供的一种获取的预设时间段内每个时刻的点云数据、初始轮速计数据、初始IMU数据和初始GNSS数据的示意图；
图3是本申请实施例提供的一种构建点云地图的装置结构示意图；
图4是本申请实施例提供的电子设备的硬件结构示意图。
具体实施方式
下面将详细描述本申请的各个方面的特征和示例性实施例，为了使本申请的目的、技术方案及优点更加清楚明白，以下结合附图及具体实施例，对本申请进行进一步详细描述。应理解，此处所描述的具体实施例仅被配置为解释本申请，并不被配置为限定本申请。对于本领域技术人员来说，本申请可以在不需要这些具体细节中的一些细节的情况下实施。下面对实施例的描述仅仅是为了通过示出本申请的示例来提供对本申请更好的理解。
需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括……”限定的要素，并不排除在包括要素的过程、方法、物品或者设备中还存在另外的相同要素。
现有的构建点云地图的方法中，通过激光雷达和轮速计等传感器采集数据，激光雷达以外的传感器采集的数据仅用于去激光雷达采集的点云数据的运动畸变，和计算激光雷达当前采集的当前帧点云数据与子图点云中的点云数据的先验位姿。虽然通过激光雷达和轮速计等传感器采集了数据，但是在构建点云地图的过程中对激光雷达以外的传感器采集的数据利用较少，且对激光雷达以外的传感器在求解用于构建点云地图的点云位姿采用的约束数据较少，造成构建的点云地图精度低。
本申请实施例提供一种构建点云地图的方法，通过激光雷达、轮速计、IMU和GNSS采集点云数据、位姿等，不仅在去激光雷达采集的点云数据的运动畸变和计算激光雷达当前采集的当前帧点云数据与子图点云中的点云数据的先验位姿中，还充分使用每个传感器采集的数据计算项约束数据，使求解的用于构建点云地图的点云位姿更精准，从而构建出精度更高的点云地图。
本申请实施例提供的方法执行主体包括可以进行数据传输和数据处理的服务器、电脑等设备。
下面将结合图1详细阐述本申请一个实施例提供的一种构建点云地图的方法。如图1所示，该方法可以包括S110-S170。
S110，获取初始子图点云数据、预设时间段内每个时刻的点云数据、轮速计数据、IMU数据和GNSS数据。
汽车或机器人等载体上安装的激光雷达、轮速计、IMU和GNSS设备，分别获取激光雷达、轮速计、IMU和GNSS设备在预设时间段内每个时刻采集的点云数据、轮速计数据、IMU数据和GNSS数据，预设时间段可以是当前时刻以及当前时刻之前的一段时间。
子图点云数据指局部地图的点云数据，可以包括预设时间段内的关键帧点云数据，是预设时间段内的每个时刻的关键帧点云数据的集合；点云数据包括关键帧点云数据，一个时刻的关键帧点云数据表示该时刻的符合预设条件的点云数据。
S120，根据轮速计数据、IMU数据，确定预设时间段内第一时刻与第二时刻之间的位姿变化量。
位姿表示载体的位置和姿态，位姿变化量表示预测的载体的位置变化量和姿态变化量。
在一些实施例中，第二时刻为预设时间段内的任一时刻，第一时刻为预设时间段内第二时刻的上一个时刻，根据第一时刻的轮速计数据中的线速度数据和IMU数据中的角速度数据进行预积分，得到载体从第一时刻到第二时刻位移的预积分结果和姿态的预积分结果，位移的预积分结果表示位置变化量，姿态的预积分结果表示姿态变化量。
在一个示例中，第一时刻为预设时间段内的第k个时刻，第一时刻的IMU数据的角速度如下式所示：
其中，wk表示第k时刻的IMU数据的角速度，wxk表示第k时刻的IMU数据在x轴方向的角速度，wyk表示第k时刻的IMU数据在y轴方向的角速度，wzk表示第k时刻的IMU数据在z轴方向的角速度，bxg表示陀螺仪在x轴方向的偏置，byg表示陀螺仪在y轴方向的偏置，bzg表示陀螺仪在z轴方向的偏置。
第一时刻的轮速计数据的线速度如下式所示：
其中，vk表示第k时刻的轮速计数据的线速度，vxk表示第k时刻的轮速计数据在x轴方向的线速度，第k时刻的轮速计数据在y轴方向的线速度和在y轴方向的线速度均记为0。
采用公式(二)和公式(三)，根据第一时刻的轮速计数据的线速度数据和IMU数据的角速度数据进行预积分。
其中，Rz、Ry、Rx分别表示绕z轴、y轴、x轴旋转一定角度所对应位姿的旋转矩阵。ΔRij为第i时刻到第j时刻姿态的预积分结果，i、j均为大于0的正整数、且i＝j-1；tk表示第k个时刻，tk-1表示第k-1个时刻。
其中，Δtij表示第i时刻到第j时刻位移的预积分结果。
S130，根据子图点云数据、第一时刻的位姿、第二时刻的点云数据和位姿变化量，计算第二时刻的匹配位姿。
先基于第一时刻的位姿和位姿变化量，确定第二时刻的预测位姿，然后根据预测位姿，通过调整第二时刻的点云数据和子图点云数据，匹配第二时刻的点云数据和子图点云数据，完成匹配后，得到第二时刻的匹配位姿，也就是第二时刻的点云相对于子图点云的位姿。
在一个实施例中，可以采用第二时刻的点云数据中满足预设规则的关键帧点云数据，替换子图点云数据中的关键帧点云数据，得到更新后的子图点云数据，以用于将第二时刻之后时刻的点云数据与子图点云数据匹配，从而得到第二时刻之后时刻的点云数据相对于子图点云数据的位姿。
在一个示例中，将第二时刻的点云数据中，与第二时刻的上一时刻的关键帧点云数据的点云数据的距离差异不超过距离差异的阈值、角度差异不超过角度差异的阈值和时间差异不超过时间差异的阈值的点云数据，作为第二时刻的关键帧点云数据。将第二时刻的关键帧点云数据添加到子图点云数据中，并剔除子图点云数据子中的第二时刻之前的关键帧点云数据，得到更新后的子图点云数据。
S140，根据预设时间段内的关键帧点云数据、第二时刻的关键帧点云数据，确定预设时间段内的关键帧点云数据和第二时刻的关键帧点云数据的回环相对位姿。
回环相对位姿表征预设时间段内与第二时刻的关键帧点云数据的相似度满足预设条件的关键帧点云数据，与第二时刻的关键帧点云数据的第一相对位姿，也可以称为回环约束。
在一些实施例中，根据预设时间段内的关键帧点云数据、第二时刻的关键帧点云数据，确定预设时间段内的关键帧点云数据和第二时刻的关键帧点云数据的回环相对位姿，包括：
首先，将预设时间段内的关键帧点云数据中与第二时刻的关键帧点云数据的相似度满足预设条件的关键帧点云数据，确定为回环关键帧点云数据。
在一个示例中，计算预设时间段内每个时刻的GNSS数据的位置与第二时刻的GNSS数据的位置的距离，将距离小于预设阈值的时刻对应的关键帧点云数据作为回环关键帧点云数据。
在一个示例中，计算预设时间段内每个时刻的关键帧点云数据与第二时刻的关键帧点云数据的相似度，将相似度大于预设阈值的时刻对应的关键帧点云数据作为回环关键帧点云数据。
然后，根据第二关键帧点云数据和回环关键帧点云数据，计算第二时刻的关键帧点云数据和回环关键帧点云数据的回环相对位姿。
回环相对位姿表征第二时刻的关键帧点云数据和回环关键帧点云数据之间的相对位姿。
在一个示例中，采用匹配的方法，根据第二关键帧点云数据和回环关键帧点云数据，计算第二时刻的关键帧点云数据和回环关键帧点云数据的回环相对位姿。匹配的方法可以是采用迭代最近点ICP算法、ICP的衍生算法(如GICP、SICP等)、NDT算法、NDT的衍生算法(如NDT-OMP算法、NDT-GPU算法)等。
S150，根据第一时刻的关键帧点云数据、第二时刻的关键帧点云数据和回环相对位姿，计算回环约束数据；根据第一时刻的关键帧点云数据、第二时刻的关键帧点云数据和位姿变化量，计算预积分约束数据；根据第一时刻的关键帧点云数据和第二时刻的关键帧点云数据，计算激光里程计约束数据；GNSS数据包括第二时刻的关键帧点云数据的全局位姿约束数据，根据第二时刻的关键帧点云数据和全局位姿约束数据，计算GNSS约束数据；根据第一时刻的位姿中的旋转数据和匹配位姿中的旋转数据，计算运动约束数据；
首先，根据第一时刻的关键帧点云数据、第二时刻的关键帧点云数据和回环相对位姿，计算回环约束数据。
回环约束数据表征回环相对位姿对应的误差数据。
在一个示例中，计算回环约束数据的公式如下所示。
其中，ELC表示回环约束数据，表示第i个时刻的关键帧点云数据和第j个时刻的关键帧点云数据的回环相对位姿，Ti表示第i个时刻的关键帧点云数据的位姿，Tj表示，第j个时刻的关键帧点云数据的位姿，i、j分别为大于0的正整数，log(*)∨表示进行李代数对数映射。
然后，根据第一时刻的关键帧点云数据、第二时刻的关键帧点云数据和位姿变化量，计算预积分约束数据。
预积分约束数据表征位姿变化量对应的误差数据。
在一个示例中，计算预积分约束数据的公式如下所示。
其中，EPI表示预积分约束数据数据，表示预积分数据，Tk表示第k个时刻的关键帧点云数据的位姿，Tk-1表示第k-1个时刻的关键帧点云数据的位姿，k为大于0的正整数。
然后，根据第一时刻的关键帧点云数据和第二时刻的关键帧点云数据，计算激光里程计约束数据。
激光里程计约束数据表征第一时刻的关键帧点云数据和第二时刻的关键帧点云数据的第二相对位姿对应的误差数据。
在一个实施例中，先根据第一时刻的关键帧点云数据和第二时刻的关键帧点云数据，计算根据第一时刻的关键帧点云数据和第二时刻的关键帧点云数据的第二相对位姿，然后根据第二时刻的关键帧点云数据和第二相对位姿计算激光里程计约束数据。
在一个示例中，计算激光里程计约束数据的公式如下所示。
其中，ELidar表示激光里程计约束数据，表示第二相对位姿，Tk表示第k个时刻的关键帧点云数据的位姿，Tk-1表示第k-1个时刻的关键帧点云数据的位姿，k为大于0的正整数。
然后，GNSS数据包括第二时刻的关键帧点云数据的全局位姿约束数据，根据第二时刻的关键帧点云数据和全局位姿约束数据，计算GNSS约束数据。
GNSS约束数据表征全局位姿约束数据对应的误差数据。
全局位姿约束数据用于防止位姿的累计误差过大。
在一个示例中，计算GNSS约束数据的公式如下所示。
其中，EGNSS表示GNSS约束数据，表示全局位姿约束数据，Tk表示第k个时刻的关键帧点云数据的位姿。
最后，根据第一时刻的位姿中的旋转数据和匹配位姿中的旋转数据，计算运动约束数据。
运动约束数据表征第一时刻的位姿中的旋转数据和匹配位姿中的旋转数据之间的相对误差数据。
在一个示例中，计算运动约束数据的公式如下所示。
其中，EM表示运动约束数据，toEuler(*)|pitch,roll表示对选准矩阵进行欧拉角转换，取其中的俯仰角和横滚角。
在一些示例中，运动约束数据是基于载体处于平面或近平面运动的假设下添加的，需要根据实际的使用场景修改。
上面计算得到回环约束数据、预积分约束数据、激光里程计约束数据、GNSS约束数据和运动约束数据后，下面执行S170。
S160，根据匹配位姿、回环约束数据、预积分约束数据、轮速计约束数据、GNSS约束数据和运动约束数据，计算得到第二时刻的关键帧位姿。
第二时刻的关键帧位姿表征第二时刻的的关键帧的位姿。
在一些实施例中，根据匹配相对位姿、回环约束数据、预积分约束数据、激光里程计约束数据、GNSS约束数据和运动约束数据，计算第二时刻的关键帧位姿的方法如下。
其中，J(T*)表示第二时刻的关键帧位姿。
本步骤采用匹配相对位姿、回环约束数据、预积分约束数据、激光里程计约束数据、GNSS约束数据和运动约束数据，计算得到高精准度的第二时刻的关键帧位姿。
S170，根据第二时刻的关键帧位姿，拼接第二时刻的关键帧点云数据，得到点云地图。
根据第二时刻的关键帧位姿，拼接第二时刻的关键帧点云数据，得到第二时刻的完整的点云地图。
在一些实施例中，先将第二时刻的关键帧点云数据进行坐标系变换，将第二时刻的关键帧点云数据从激光雷达坐标系转换为世界坐标系。
在一个示例中，第二时刻的关键帧点云数据从激光雷达坐标系转换为世界坐标系的方法如下式所示。
其中与/>分别表示第k时刻的关键帧点云数据中的点云数据在激光雷达坐标系与世界坐标系中的空间位置坐标。Tk表示第k时刻的关键帧点云数据的位姿。
拼接点云数据指将世界坐标系的第二时刻的关键帧点云数据添加到全局地图点云集合，全局地图点云可表示为集合N为大于或等于0的正整数。
在一些实施例中，在得到点云地图后，对点云地图做体素滤波，生成用于高精定位的点云地图数据文件，以降低地图的数据存储量。
本申请先获取了子图点云数据、预设时间段内每个时刻的点云数据、轮速计数据、IMU数据和GNSS数据，根据轮速计数据和IMU数据，确定预设时间段内第一时刻与第二时刻之间的位姿变化量；然后，根据子图点云数据、第一时刻的位姿、第二时刻的点云数据和位姿变化量，计算第二时刻的匹配位姿，也就是第二时刻的点云数据相对于子图点云数据的位姿；然后，计算预积分约束数据和运动约束数据在内的多个数据，并根据匹配位姿和多个约束数据计算第二时刻的关键帧位姿，由于计算第二时刻的关键帧位姿时采用了由多个传感器的数据计算的多个约束数据，所以第二时刻的关键帧位姿具有较高的精准性；最后，根据第二时刻的关键帧位姿，拼接第二时刻的关键帧点云数据，得到点云地图，由于第二时刻的关键帧位姿具有较高的精准性，所以根据第二时刻的关键帧位姿拼接第二时刻的关键帧点云数据，得到的点云地图的精度较高，实现了构建高精度点云地图。
在一些实施例中，获取预设时间段内每个时刻的点云数据、轮速计数据、IMU数据和GNSS数据，包括：
获取预设时间段内每个时刻的点云数据、初始轮速计数据、初始IMU数据和初始GNSS数据；
对点云数据、初始轮速计数据、初始IMU数据和初始GNSS数据时间对齐，并对初始轮速计数据、初始IMU数据和初始GNSS数据中的至少一项插值，得到每个时刻相对应的点云数据、轮速计数据、IMU数据和GNSS数据。
初始轮速计数据指从轮速计获取的数据，初始IMU数据指从IMU获取的数据，初始GNSS数据指从GNSS获取的数据。
在一个示例中，获取的预设时间段内每个时刻的点云数据、初始轮速计数据、初始IMU数据和初始GNSS数据如图2所示，其中201表示预设时间段内每个时刻的点云数据，其中k表示第k时刻，k+1表示第k+1时刻，202表示初始轮速计数据、初始IMU数据和初始GNSS数据，对点云数据、初始轮速计数据、初始IMU数据和始述GNSS数据时间对齐后，在一些时刻可能没有对应的初始轮速计数据、初始IMU数据或初始GNSS数据，所以对初始轮速计数据、初始IMU数据或初始GNSS数据插值，得到插值之后的数据203，从而得到每个时刻对应的点云数据、轮速计数据、IMU数据和GNSS数据。
在一个示例中，采用线性差值的方法进行差值，如下公式所示。
其中，初始轮速计数据、初始IMU数据或初始GNSS数据的第i个数据与第j个数据之间存在的一个时刻获取到的点云数据，i、j均为大于0的正整数、且i＝j-1，第i个数据时间为ti，数据为vti；第j个数据时间为tj，数据为vtj，获取到点云数据时间为ti+△ti，vti+△ti为插值的数据。
本申请实施例提供的方法对点云数据、初始轮速计数据、初始IMU数据和初始GNSS数据进行时间对齐和插值，减少了因多个不同的传感器数据采集时间差异过大造成的精度降低。
在一些实施例中，获取预设时间段内第二时刻的点云数据，包括：
首先，获取第二时刻的初始点云数据。
获取第二时刻的激光雷达发送的初始点云数据，由于激光雷达转动一周指向同一个位置的时间差导致雷达数据不匹配，也就是产生运动畸变。
然后，根据位姿变化量校正初始点云数据，得到第二时刻的点云数据。
在一个示例中，预设时间段内的起始时刻的从激光雷达获取的点云数据的位姿为T0，如下式所示。
其中，R0表示起始时刻的姿态，T0表示起始时刻的位置。
预设时间段内第i个时刻的点云数据的位姿为Ti，如下式所示。
其中，Ri表示第i个时刻的姿态，Ti表示第i个时刻的位置。
第i个时刻的初始点云数据为Pi，如下式所示。
Pi＝[pix piy piz]T
其中，pix表示x轴方向的坐标，piy表示y轴方向的坐标，piz表示z轴方向的坐标。
去运动畸变后的第i个时刻的点云数据为，如下式所示。
本申请实施例提供的方法采用位姿变化量对第二时刻的初始点云数据去运动畸变，得到第二时刻的点云数据，提高了第二时刻的点云数据的精准度。
在一些实施例中，根据子图点云数据、第一时刻的位姿、第二时刻的点云数据和位姿变化量，计算第二时刻的匹配位姿，包括：
首先，根据位姿变化量和第一时刻的位姿，确定第二时刻的预测位姿。
预测位姿表征预测得到第二时刻的位姿。
然后，根据预测位姿，匹配子图点云数据和第二时刻的点云数据，得到第二时刻的匹配位姿。
在一个示例中，匹配的方法可以是采用迭代最近点(Iterative Closest Point，ICP)算法、ICP的衍生算法、正态分布变换(Normal Distributions Transform，NDT)算法、NDT的衍生算法等，ICP的衍生算法如：广义迭代最近点算法(Generalized IterativeCloset Point，GICP)、尺度迭代最近点算法(Scaled Iterative Closest Point，SICP)。
NDT的衍生算法如：正态分布变换-正交匹配追踪算法(Normal DistributionsTransform-Orthogonal Matching Pursuit，NDT-OMP)、正态分布变换-图像处理单元算法(Normal Distributions Transform-Graphic Processing Unit，NDT-GPU)等。
基于预测位姿，采用匹配算法匹配子图点云数据和第二时刻的点云数据，直至子图点云数据和第二时刻的点云数据拼接在一起，通过匹配完成拼接后，通过统计匹配过程中子图点云数据和第二时刻的点云数据的位姿变化量，便得到第二时刻的匹配位姿。
本申请是实施例提供的方法能够通过位姿变化量和第一时刻的位姿，预测第二时刻的位姿，根据预测的位姿调整子图点云数据和第二时刻的点云数据的位姿，以匹配子图点云数据和第二时刻的点云数据，这样能够减少匹配过程中的计算量和所需的时间，通过匹配完成拼接后，通过统计匹配过程中子图点云数据和第二时刻的点云数据的位姿变化量，便得到第二时刻的位姿。
本申请实施例还提供一种构建点云地图的装置，如图3所示，构建点云地图的装置300可以包括获取模块310、确定模块320、计算模块330和拼接模块340。
获取模块310，用于获取子图点云数据、预设时间段内每个时刻的点云数据和位姿、轮速计数据、IMU数据和GNSS数据，子图点云数据包括预设时间段内的关键帧点云数据，点云数据包括关键帧点云数据；
确定模块320，用于根据轮速计数据、IMU数据，确定预设时间段内第一时刻与第二时刻之间的位姿变化量；
计算模块330，用于根据子图点云数据、第一时刻的位姿、第二时刻的点云数据和位姿变化量，计算第二时刻的匹配位姿；
确定模块320，还用于根据预设时间段内的关键帧点云数据、第二时刻的关键帧点云数据，确定预设时间段内的关键帧点云数据和第二时刻的关键帧点云数据的回环相对位姿，回环相对位姿表征预设时间段内与第二时刻的关键帧点云数据的相似度满足预设条件的关键帧点云数据，与第二时刻的关键帧点云数据的第一相对位姿；
计算模块330，还用于根据第一时刻的关键帧点云数据、第二时刻的关键帧点云数据和回环相对位姿，计算回环约束数据，回环约束数据表征回环相对位姿对应的误差数据；根据第一时刻的关键帧点云数据、第二时刻的关键帧点云数据和位姿变化量，计算预积分约束数据，预积分约束数据表征位姿变化量对应的误差数据；还用于根据第一时刻的关键帧点云数据和第二时刻的关键帧点云数据，计算激光里程计约束数据，激光里程计约束数据表征第一时刻的关键帧点云数据和第二时刻的关键帧点云数据的第二相对位姿对应的误差数据；GNSS数据包括第二时刻的关键帧点云数据的全局位姿约束数据，根据第二时刻的关键帧点云数据和全局位姿约束数据，计算GNSS约束数据，GNSS约束数据表征全局位姿约束数据对应的误差数据；根据第一时刻的位姿中的旋转数据和匹配位姿中的旋转数据，计算运动约束数据，运动约束数据表征第一时刻的位姿中的旋转数据和匹配位姿中的旋转数据之间的相对误差数据；还用于根据匹配位姿、回环约束数据、预积分约束数据、激光里程计约束数据、GNSS约束数据和运动约束数据，计算得到第二时刻的关键帧位姿；
拼接模块340，用于根据第二时刻的关键帧位姿，拼接第二时刻的关键帧点云数据，得到点云地图。
本申请实施例体用的装置先获取了初始子图点云数据、预设时间段内每个时刻的点云数据、轮速计数据、IMU数据和GNSS数据，根据轮速计数据和IMU数据，确定预设时间段内第一时刻与第二时刻之间的位姿变化量；然后，根据子图点云数据、第一时刻的位姿、第二时刻的点云数据和位姿变化量，计算第二时刻的匹配位姿，也就是第二时刻的点云数据相对于子图点云数据的位姿，；然后，计算预积分约束数据和运动约束数据在内的多个数据，并根据匹配位姿和多个约束数据计算第二时刻的关键帧位姿，由于计算第二时刻的关键帧位姿时采用了由多个传感器的数据计算的多个约束数据，所以第二时刻的关键帧位姿具有较高的精准性；最后，根据第二时刻的关键帧位姿，拼接第二时刻的关键帧点云数据，得到点云地图，由于第二时刻的关键帧位姿具有较高的精准性，所以根据第二时刻的关键帧位姿拼接第二时刻的关键帧点云数据，得到的点云地图的精度较高，实现了构建高精度点云地图。
在一些实施例中，获取模块310，可以具体用于：
取预设时间段内每个时刻的点云数据、初始轮速计数据、初始IMU数据和初始GNSS数据；
对点云数据、初始轮速计数据、初始IMU数据和初始GNSS数据时间对齐，并对初始轮速计数据、初始IMU数据或初始GNSS数据中的至少一项插值，得到每个时刻相对应的点云数据、轮速计数据、IMU数据和GNSS数据。
本申请实施例提供的装置对点云数据、初始轮速计数据、初始IMU数据和初始GNSS数据进行时间对齐和插值，减少了因多个传感器的数据采集时间差异过大造成的精度降低。
在一些实施例中，获取模块310，还可以具体用于：
获取第二时刻的初始点云数据；
根据位姿变化量校正初始点云数据，得到第二时刻的点云数据。
本申请实施例提供的装置采用位姿变化量对第二时刻的初始点云数据去运动畸变，得到第二时刻的点云数据，提高了第二时刻点云数据的精准度。
在一些实施例中，计算模块330，可以具体用于：
根据位姿变化量和第一时刻的位姿，确定第二时刻的预测位姿；根据预测位姿，匹配子图点云数据和第二时刻的点云数据，得到第二时刻的匹配位姿。
本申请实施例提供的装置采用的位姿变化量是根据轮速计数据和IMU数据计算得到的，具有较高的精准度，所以根据位姿变化量和第一时刻的位姿得到的预测位姿据有较高的精度，从而根据预测位姿计算得到的匹配位姿也具备较高的精准度。
在一些实施例中，确定模块320，可以具体用于：
将预设时间段内的关键帧点云数据中与第二时刻的关键帧点云数据的相似度满足预设条件的关键帧点云数据，确定为回环关键帧点云数据；根据第二时刻的关键帧点云数据和回环关键帧点云数据，计算第二时刻的关键帧点云数据和回环关键帧点云数据的回环相对位姿。
本申请实施例提供的装置计算得到第二时刻的关键帧点云数据和回环关键帧点云数据的回环相对位姿，即回环约束，为计算回环约束数据提供依据。
本申请实施例提供的构建点云地图的装置执行图1所示的方法中的各个步骤，并能够达到构建高精度点云地图的技术效果，为简洁描述，再此不在详细赘述。
图4示出了本申请实施例提供的一种电子设备的硬件结构示意图。
在电子设备可以包括处理器401以及存储有计算机程序指令的存储器402。
具体地，上述处理器401可以包括中央处理器(CPU)，或者特定集成电路(Application Specific Integrated Circuit，ASIC)，或者可以被配置成实施本申请实施例的一个或多个集成电路。
存储器402可以包括用于数据或指令的大容量存储器。举例来说而非限制，存储器402可包括硬盘驱动器(Hard Disk Drive，HDD)、软盘驱动器、闪存、光盘、磁光盘、磁带或通用串行总线(Universal Serial Bus，USB)驱动器或者两个或更多个以上这些的组合。在合适的情况下，存储器402可包括可移除或不可移除(或固定)的介质。在合适的情况下，存储器402可在综合网关容灾设备的内部或外部。在特定实施例中，存储器402是非易失性固态存储器。在特定实施例中，存储器402包括只读存储器(ROM)。在合适的情况下，该ROM可以是掩模编程的ROM、可编程ROM(PROM)、可擦除PROM(EPROM)、电可擦除PROM(EEPROM)、电可改写ROM(EAROM)或闪存或者两个或更多个以上这些的组合。
处理器401通过读取并执行存储器402中存储的计算机程序指令，以实现图1所示实施例中的任意一种构建点云地图的方法。
在一个示例中，电子设备还可包括通信接口403和总线410。其中，如图4所示，处理器401、存储器402、通信接口403通过总线410连接并完成相互间的通信。
通信接口403，主要用于实现本申请实施例中各模块、装置、单元和/或设备之间的通信。
总线410包括硬件、软件或两者，将电子设备的部件彼此耦接在一起。举例来说而非限制，总线可包括加速图形端口(AGP)或其他图形总线、增强工业标准架构(EISA)总线、前端总线(FSB)、超传输(HT)互连、工业标准架构(ISA)总线、无限带宽互连、低引脚数(LPC)总线、存储器总线、微信道架构(MCA)总线、外围组件互连(PCI)总线、PCI-Express(PCI-X)总线、串行高级技术附件(SATA)总线、视频电子标准协会局部(VLB)总线或其他合适的总线或者两个或更多个以上这些的组合。在合适的情况下，总线410可包括一个或多个总线。尽管本申请实施例描述和示出了特定的总线，但本申请考虑任何合适的总线或互连。
该电子设备可以执行本申请实施例中的构建点云地图的方法，从而实现结合图1描述的构建点云地图的方法。
另外，结合上述实施例中的构建点云地图的方法，本申请实施例可提供一种计算机可读存储介质来实现。该计算机可读存储介质上存储有计算机程序指令；该计算机程序指令被处理器执行时实现上述实施例中的任意一种构建点云地图的方法。
需要明确的是，本申请并不局限于上文所描述并在图中示出的特定配置和处理。为了简明起见，这里省略了对已知方法的详细描述。在上述实施例中，描述和示出了若干具体的步骤作为示例。但是，本申请的方法过程并不限于所描述和示出的具体步骤，本领域的技术人员可以在领会本申请的精神后，作出各种改变、修改和添加，或者改变步骤之间的顺序。
以上所述的结构框图中所示的功能块可以实现为硬件、软件、固件或者它们的组合。当以硬件方式实现时，其可以例如是电子电路、专用集成电路(ASIC)、适当的固件、插件、功能卡等等。当以软件方式实现时，本申请的元素是被用于执行所需任务的程序或者代码段。程序或者代码段可以存储在机器可读介质中，或者通过载波中携带的数据信号在传输介质或者通信链路上传送。“机器可读介质”可以包括能够存储或传输信息的任何介质。机器可读介质的例子包括电子电路、半导体存储器设备、ROM、闪存、可擦除ROM(EROM)、软盘、CD-ROM、光盘、硬盘、光纤介质、射频(RF)链路，等等。代码段可以经由诸如因特网、内联网等的计算机网络被下载。
还需要说明的是，本申请中提及的示例性实施例，基于一系列的步骤或者装置描述一些方法或系统。但是，本申请不局限于上述步骤的顺序，也就是说，可以按照实施例中提及的顺序执行步骤，也可以不同于实施例中的顺序，或者若干步骤同时执行。
以上所述，仅为本申请的具体实施方式，所属领域的技术人员可以清楚地了解到，为了描述的方便和简洁，上述描述的系统、模块和单元的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。应理解，本申请的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本申请揭露的技术范围内，可轻易想到各种等效的修改或替换，这些修改或替换都应涵盖在本申请的保护范围之内。
Claims (10)
1.一种构建点云地图的方法，其特征在于，所述方法包括：
获取子图点云数据、预设时间段内每个时刻的点云数据和位姿、轮速计数据、惯性测量单元IMU数据和全球卫星导航系统GNSS数据，所述子图点云数据包括所述预设时间段内的关键帧点云数据，所述点云数据包括所述关键帧点云数据；
根据所述轮速计数据、所述IMU数据，确定所述预设时间段内第一时刻与第二时刻之间的位姿变化量；
根据所述子图点云数据、所述第一时刻的位姿、所述第二时刻的点云数据和所述位姿变化量，计算所述第二时刻的匹配位姿；
根据所述预设时间段内的关键帧点云数据、所述第二时刻的关键帧点云数据，确定所述预设时间段内的关键帧点云数据和所述第二时刻的关键帧点云数据的回环相对位姿，所述回环相对位姿表征所述预设时间段内与所述第二时刻的关键帧点云数据的相似度满足预设条件的关键帧点云数据，与所述第二时刻的关键帧点云数据的第一相对位姿；
根据所述第一时刻的关键帧点云数据、所述第二时刻的关键帧点云数据和所述回环相对位姿，计算回环约束数据，所述回环约束数据表征所述回环相对位姿对应的误差数据；根据所述第一时刻的关键帧点云数据、所述第二时刻的关键帧点云数据和所述位姿变化量，计算预积分约束数据，所述预积分约束数据表征所述位姿变化量对应的误差数据；根据所述第一时刻的关键帧点云数据和所述第二时刻的关键帧点云数据，计算激光里程计约束数据，所述激光里程计约束数据表征所述第一时刻的关键帧点云数据和所述第二时刻的关键帧点云数据的第二相对位姿对应的误差数据；所述GNSS数据包括所述第二时刻的关键帧点云数据的全局位姿约束数据，根据所述第二时刻的关键帧点云数据和所述全局位姿约束数据，计算GNSS约束数据，所述GNSS约束数据表征全局位姿约束数据对应的误差数据；根据所述第一时刻的位姿中的旋转数据和所述匹配位姿中的旋转数据，计算运动约束数据，所述运动约束数据表征所述第一时刻的位姿中的旋转数据和所述匹配位姿中的旋转数据之间的相对误差数据；
根据所述匹配位姿、所述回环约束数据、所述预积分约束数据、所述激光里程计约束数据、所述GNSS约束数据和所述运动约束数据，计算得到所述第二时刻的关键帧位姿；
根据所述第二时刻的关键帧位姿，拼接所述第二时刻的关键帧点云数据，得到点云地图。
2.根据权利要求1所述的方法，其特征在于，所述获取预设时间段内每个时刻的点云数据、轮速计数据、IMU数据和GNSS数据，包括：
获取所述预设时间段内每个时刻的点云数据、初始轮速计数据、初始IMU数据和初始GNSS数据；
对所述点云数据、所述初始轮速计数据、所述初始IMU数据和所初始述GNSS数据时间对齐，并对所述初始轮速计数据、所述初始IMU数据或所述初始GNSS数据中的至少一项插值，得到每个时刻相对应的所述点云数据、所述轮速计数据、所述IMU数据和所述GNSS数据。
3.根据权利要求1所述的方法，其特征在于，获取预设时间段内第二时刻的点云数据，包括：
获取所述第二时刻的初始点云数据；
根据所述位姿变化量校正所述初始点云数据，得到所述第二时刻的点云数据。
4.根据权利要求1所述的方法，其特征在于，所述根据所述子图点云数据、所述第一时刻的位姿、所述第二时刻的点云数据和所述位姿变化量，计算所述第二时刻的匹配位姿，包括：
根据所述第一时刻的位姿和所述位姿变化量，确定所述第二时刻的预测位姿；
根据所述预测位姿，匹配所述子图点云数据和所述第二时刻的点云数据，得到所述第二时刻的匹配位姿。
5.根据权利要求1所述的方法，其特征在于，所述根据所述预设时间段内的关键帧点云数据、所述第二时刻的关键帧点云数据，确定所述预设时间段内的关键帧点云数据和所述第二时刻的关键帧点云数据的回环相对位姿，包括：
将所述预设时间段内的关键帧点云数据中与所述第二时刻的关键帧点云数据的相似度满足预设条件的关键帧点云数据，确定为回环关键帧点云数据；
根据所述第二时刻的关键帧点云数据和所述回环关键帧点云数据，计算所述第二时刻的关键帧点云数据和所述回环关键帧点云数据的回环相对位姿。
6.一种构建点云地图的装置，其特征在于，所述装置包括：
获取模块，用于获取子图点云数据、预设时间段内每个时刻的点云数据和位姿、轮速计数据、IMU数据和GNSS数据，所述子图点云数据包括所述预设时间段内的关键帧点云数据，所述点云数据包括所述关键帧点云数据；
确定模块，用于根据所述轮速计数据、所述IMU数据，确定所述预设时间段内第一时刻与第二时刻之间的位姿变化量；
计算模块，用于根据所述子图点云数据、所述第一时刻的位姿、所述第二时刻的点云数据和所述位姿变化量，计算所述第二时刻的匹配位姿；
所述确定模块，还用于根据所述预设时间段内的关键帧点云数据、所述第二时刻的关键帧点云数据，确定所述预设时间段内的关键帧点云数据和所述第二时刻的关键帧点云数据的回环相对位姿，所述回环相对位姿表征所述预设时间段内与所述第二时刻的关键帧点云数据的相似度满足预设条件的关键帧点云数据，与所述第二时刻的关键帧点云数据的第一相对位姿；
所述计算模块，还用于根据所述第一时刻的关键帧点云数据、所述第二时刻的关键帧点云数据和所述回环相对位姿，计算回环约束数据，所述回环约束数据表征所述回环相对位姿对应的误差数据；还用于根据所述第一时刻的关键帧点云数据、所述第二时刻的关键帧点云数据和所述位姿变化量，计算预积分约束数据，所述预积分约束数据表征所述位姿变化量对应的误差数据；还用于根据所述第一时刻的关键帧点云数据和所述第二时刻的关键帧点云数据，计算激光里程计约束数据，所述激光里程计约束数据表征所述第一时刻的关键帧点云数据和所述第二时刻的关键帧点云数据的第二相对位姿对应的误差数据；所述GNSS数据包括所述第二时刻的关键帧点云数据的全局位姿约束数据，还用于根据所述第二时刻的关键帧点云数据和所述全局位姿约束数据，计算GNSS约束数据，所述GNSS约束数据表征全局位姿约束数据对应的误差数据；还用于根据所述第一时刻的位姿中的旋转数据和所述匹配位姿中的旋转数据，计算运动约束数据，所述运动约束数据表征所述第一时刻的位姿中的旋转数据和所述匹配位姿中的旋转数据之间的相对误差数据；还用于根据所述匹配位姿、所述回环约束数据、所述预积分约束数据、所述激光里程计约束数据、所述GNSS约束数据和所述运动约束数据，计算得到所述第二时刻的关键帧位姿；
拼接模块，用于根据所述第二时刻的关键帧位姿，拼接所述第二时刻的关键帧点云数据，得到点云地图。
7.根据权利要求6所述的装置，其特征在于，所述获取模块，具体用于：
获取所述预设时间段内每个时刻的点云数据、初始轮速计数据、初始IMU数据和初始GNSS数据；
对所述点云数据、所述初始轮速计数据、所述初始IMU数据和所初始述GNSS数据时间对齐，并对所述初始轮速计数据、所述初始IMU数据或所初始述GNSS数据中的至少一项插值，得到每个时刻相对应的所述点云数据、所述轮速计数据、所述IMU数据和所述GNSS数据。
8.根据权利要求6所述的装置，其特征在于，所述获取模块，具体用于：
获取所述第二时刻的初始点云数据；
根据所述位姿变化量校正所述初始点云数据，得到所述第二时刻的点云数据。
9.一种电子设备，其特征在于，所述设备包括：处理器以及存储有计算机程序指令的存储器；所述处理器执行所述计算机程序指令时实现如权利要求1-5任意一项所述的构建点云地图方法。
10.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质上存储有计算机程序指令，所述计算机程序指令被处理器执行时实现如权利要求1-5任意一项所述的构建点云地图方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111316500.2A CN114119744B (zh) | 2021-11-08 | 2021-11-08 | 一种构建点云地图的方法、装置、设备及存储介质 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111316500.2A CN114119744B (zh) | 2021-11-08 | 2021-11-08 | 一种构建点云地图的方法、装置、设备及存储介质 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN114119744A CN114119744A (zh) | 2022-03-01 |
| CN114119744B true CN114119744B (zh) | 2024-05-14 |
Family
ID=80377592
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111316500.2A Active CN114119744B (zh) | 2021-11-08 | 2021-11-08 | 一种构建点云地图的方法、装置、设备及存储介质 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114119744B (zh) |
Families Citing this family (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114897942B (zh) * | 2022-07-15 | 2022-10-28 | 深圳元戎启行科技有限公司 | 点云地图的生成方法、设备及相关存储介质 |
| CN115586511B (zh) * | 2022-11-25 | 2023-03-03 | 唐山百川工业服务有限公司 | 一种基于阵列立柱的激光雷达二维定位方法 |
Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109814572A (zh) * | 2019-02-20 | 2019-05-28 | 广州市山丘智能科技有限公司 | 移动机器人定位建图方法、装置、移动机器人和存储介质 |
| CN112304307A (zh) * | 2020-09-15 | 2021-02-02 | 浙江大华技术股份有限公司 | 一种基于多传感器融合的定位方法、装置和存储介质 |
| CN112862894A (zh) * | 2021-04-12 | 2021-05-28 | 中国科学技术大学 | 一种机器人三维点云地图构建与扩充方法 |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2019000417A1 (en) * | 2017-06-30 | 2019-01-03 | SZ DJI Technology Co., Ltd. | SYSTEMS AND METHODS OF GENERATING CARDS |
-
2021
- 2021-11-08 CN CN202111316500.2A patent/CN114119744B/zh active Active
Patent Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109814572A (zh) * | 2019-02-20 | 2019-05-28 | 广州市山丘智能科技有限公司 | 移动机器人定位建图方法、装置、移动机器人和存储介质 |
| CN112304307A (zh) * | 2020-09-15 | 2021-02-02 | 浙江大华技术股份有限公司 | 一种基于多传感器融合的定位方法、装置和存储介质 |
| CN112862894A (zh) * | 2021-04-12 | 2021-05-28 | 中国科学技术大学 | 一种机器人三维点云地图构建与扩充方法 |
Non-Patent Citations (1)
| Title |
|---|
| 基于关键帧的点云建图方法;胡向勇;洪程智;吴世全;;热带地貌;20200625(第01期);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN114119744A (zh) | 2022-03-01 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN114119744B (zh) | 一种构建点云地图的方法、装置、设备及存储介质 | |
| CN111121768B (zh) | 一种机器人位姿估计方法、装置、可读存储介质及机器人 | |
| KR101985344B1 (ko) | 관성 및 단일 광학 센서를 이용한 슬라이딩 윈도우 기반 비-구조 위치 인식 방법, 이를 수행하기 위한 기록 매체 및 장치 | |
| WO2012033807A1 (en) | Automatic correction of past position errors for location and inventory tracking | |
| EP3852065A1 (en) | Data processing method and apparatus | |
| CN105136145A (zh) | 一种基于卡尔曼滤波的四旋翼无人机姿态数据融合的方法 | |
| CN113933818A (zh) | 激光雷达外参的标定的方法、设备、存储介质及程序产品 | |
| CN104677361B (zh) | 一种综合定位的方法 | |
| CN109059907A (zh) | 轨迹数据处理方法、装置、计算机设备和存储介质 | |
| CN114179825B (zh) | 多传感器融合获取量测值置信度方法及自动驾驶车辆 | |
| CN114136315B (zh) | 一种基于单目视觉辅助惯性组合导航方法及系统 | |
| CN114019954B (zh) | 航向安装角标定方法、装置、计算机设备和存储介质 | |
| CN111721305B (zh) | 定位方法和装置、自动驾驶车辆、电子设备和存储介质 | |
| CN115060257B (zh) | 一种基于民用级惯性测量单元的车辆变道检测方法 | |
| CN111024067B (zh) | 一种信息处理方法、装置、设备及计算机存储介质 | |
| CN114593735B (zh) | 一种位姿预测方法及装置 | |
| CN117367419A (zh) | 机器人定位方法、装置和计算可读存储介质 | |
| CN114323007A (zh) | 一种载体运动状态估计方法及装置 | |
| CN111580139B (zh) | 卫星导航数据的有效性判断方法及装置、电子设备 | |
| CN113252023A (zh) | 基于里程计的定位方法、装置及设备 | |
| CN116380070A (zh) | 一种基于时间戳优化的视觉惯性定位方法 | |
| CN109074407A (zh) | 多源数据建图方法、相关装置及计算机可读存储介质 | |
| CN118482743B (zh) | Imu零偏估计方法、装置及电子设备 | |
| CN116912310B (zh) | 相机位姿估计方法、装置、计算机设备及介质 | |
| CN115164886B (zh) | 车载gnss/ins组合导航系统比例因子误差补偿方法 |
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 |