CN116399324A - 建图方法、装置及控制器、无人驾驶车辆 - Google Patents

Abstract

本申请涉及建图方法、装置及控制器、无人驾驶车辆，该方法包括：在车辆行驶时，获取激光雷达采样的原始点云、IMU采集的位姿数据以及GPS测量的位置数据；根据位姿数据对原始点云去畸变；基于去畸变的点云匹配获得激光雷达的位姿估计；将位姿估计之间的IMU积分，获得相邻两帧的第一相对位姿；将位置数据、位姿估计和第一相对位姿输入因子图中优化，得到初始优化点云地图；将初始优化点云地图和预设的标定板对车辆的当前状态进行回环检测；根据检测结果获得第二相对位姿；将第二相对位姿输入因子图中优化，获得最终优化点云地图。通过放置特征性强的标定板作为回环检测的特征匹配，有助于回环检测，并建立高精度点云地图，帮助车辆进行更好的路径规划。

Description

建图方法、装置及控制器、无人驾驶车辆

技术领域

本申请涉及计算机视觉技术领域，尤其涉及一种建图方法、装置及控制器、无人驾驶车辆。

背景技术

随着计算机技术的发展，高精度点云地图的应用越来越广泛，且对点云地图的精度要求越来越高。由于无人驾驶车辆行驶的速度快且周围环境复杂，使得目前已有的建图的精度不高。同时因为行驶距离过远，累计误差大，最终的点云地图偏移过大，使得无人驾驶基于点云地图的定位以及感知的过程中不够准确，容易造成不安全的隐患。

发明内容

本申请实施方式主要解决的技术问题是如何优化现有的高精度地图方案，从而提高无人驾驶车辆的安全性。

第一方面，本身实施例提供了一种建图方法，应用于无人驾驶车辆，所述无人驾驶车辆上设置有激光雷达、IMU和GPS，所述方法包括：在所述无人驾驶车辆行驶时，获取所述激光雷达采样的原始点云、所述IMU采集的位姿数据以及所述GPS测量的位置数据；根据所述位姿数据对所述原始点云进行去畸变处理，以获得去畸变的点云；基于所述去畸变的点云进行匹配获得所述激光雷达的位姿估计；将所述激光雷达的位姿估计之间的所述IMU的运动信息进行积分，以获得相邻两帧之间的第一相对位姿；将所述GPS测量的位置数据、所述激光雷达的位姿估计以及所述第一相对位姿输入因子图进行优化操作，得到初始优化点云地图；基于所述初始优化点云地图和预设的标定板对所述无人驾驶车辆的当前状态进行回环检测；根据所述回环检测的结果获得相邻两帧之间的第二相对位姿；将所述第二相对位姿输入所述因子图进行优化操作，获得最终优化点云地图。

在一些实施例中，所述根据所述位姿数据对所述原始点云进行去畸变处理，以获得去畸变的点云，包括：根据所述原始点云确定基准点和畸变点；采用所述位姿数据对所述原始点云中的点进行运动补偿，获得每个点的插值位姿数据；根据所述畸变点的时间戳与所述基准点的时间戳之间的插值位姿数据，计算所述畸变点与所述基准点的空间位姿关系；根据所述空间位姿关系将所述畸变点转换至所述基准点的坐标系下，以获得去畸变的点云。

在一些实施例中，所述将所述GPS测量的位置数据、所述激光雷达的位姿估计以及所述第一相对位姿输入因子图进行优化操作，得到初始优化点云地图，包括：基于所述激光雷达的位姿估计，获取当前雷达采样时刻的激光雷达点云，和从上一雷达采样时刻到所述当前雷达采样时刻之间的每一IMU采样时刻的所有IMU数据；根据所述所有所述IMU数据，以及所述上一雷达采样时刻的状态计算所述无人驾驶车辆在所述当前雷达采样时刻的当前状态预测值；获取所述当前雷达采样时刻对应的所述GPS测量的位置数据；根据所述位置数据对所述当前状态预测值进行修正，获得当前状态估计值；根据所述当前状态估计值和所述第一相对位姿，对所述所有IMU数据进行反向传播，以校正所述激光雷达的位姿估计，获得初始优化点云地图。

在一些实施例中，当所述无人驾驶车辆行驶的预设路段放置有预设数目的标定板时，所述基于所述初始优化点云地图和预设的标定板对所述无人驾驶车辆的当前状态进行回环检测，包括：当所述无人驾驶车辆行驶至所述预设路段时，根据所述预设数目的标定板获取所述无人驾驶车辆当前的索引；获取所述无人驾驶车辆当前状态的上一次行驶至所述预设路段时所记录的索引；比较所述当前的索引和所述上一次对应记录的索引，当两次记录的索引的内容相同或误差在预设范围内时，则确定所述无人驾驶车辆的当前状态为闭环行驶；否则，确定所述无人驾驶车辆的当前状态为非闭环行驶。

在一些实施例中，所述当所述无人驾驶车辆行驶至所述预设路段时，根据所述预设数目的标定板获取所述无人驾驶车辆当前的索引，包括：当所述无人驾驶车辆行驶至所述预设路段时，接收标定板检测范围和检测频率指令；根据所述标定板检测范围和检测频率指令对所述初始优化点云地图中的点云进行滤波处理，获取处理后的在所述标定板检测范围内的点云；根据所述点云的强度信息提取大于预设阈值强度的点云；将提取的所述点云进行霍夫直线检测，并选取符合预设条件的三根直线；从所述三根直线中分别选取一个点，计算是否满足三角形构成的条件，若满足，则记录所述无人驾驶车辆的当前雷达里程计的索引。

在一些实施例中，所述根据所述回环检测的结果获得相邻两帧之间的第二相对位姿具体包括：当所述回环检测的结果为确定所述无人驾驶车辆的当前状态为闭环行驶时，则记录相邻两帧点云之间的第二相对位姿。

第二方面，本申请实施例提供了一种建图装置，应用于无人驾驶车辆，所述无人驾驶车辆上设置有激光雷达、IMU和GPS，所述装置包括：参数获取模块，用于在所述无人驾驶车辆行驶时，获取所述激光雷达采样的原始点云、所述IMU采集的位姿数据以及所述GPS测量的位置数据；畸变处理模块，用于根据所述位姿数据对所述原始点云进行去畸变处理，以获得去畸变的点云；位姿估计获取模块，用于基于所述去畸变的点云进行匹配获得所述激光雷达的位姿估计；第一相对位姿获取模块，用于将所述激光雷达的位姿估计之间的所述IMU的运动信息进行积分，以获得相邻两帧之间的第一相对位姿；初始优化点云地图获取模块，用于将所述GPS测量的位置数据、所述激光雷达的位姿估计以及所述第一相对位姿输入因子图进行优化操作，得到初始优化点云地图；回环检测模块，用于基于所述第一目标点云和预设的标定板对所述无人驾驶车辆的当前状态进行回环检测；第二相对位姿获取模块，用于根据所述回环检测的结果获得相邻两帧之间的第二相对位姿；最终优化点云地图获取模块，用于将所述第二相对位姿输入所述因子图进行优化操作，获得最终优化点云地图。

第三方面，本申请实施例提供了一种控制器，包括：至少一个处理器；与所述至少一个处理器通信连接的存储器；其中，所述存储器存储有可被所述至少一个处理器执行的指令，所述指令被所述至少一个处理器执行，以使所述至少一个处理器能够执行如第一方面所述的建图方法。

第四方面，本申请实施例提供了一种无人驾驶车辆，包括：激光雷达、IMU、GPS以及如第三方面所述的控制器。

第四方面，本申请实施例提供了一种非易失性计算机可读存储介质，所述非易失性计算机可读存储介质存储有计算机可执行指令，当所述计算机可执行指令被控制器执行时，使所述控制器执行如第一方面所述的建图方法。

本申请实施例的有益效果：区别于相关技术的情况，本申请提供的一种建图方法、装置及控制器、无人驾驶车辆，建图方法应用于无人驾驶车辆，所述无人驾驶车辆上设置有激光雷达、IMU和GPS，该方法包括：在所述无人驾驶车辆行驶时，获取所述激光雷达采样的原始点云、所述IMU采集的位姿数据以及所述GPS测量的位置数据；根据所述位姿数据对所述原始点云进行去畸变处理，以获得去畸变的点云；基于所述去畸变的点云进行匹配获得所述激光雷达的位姿估计；将所述激光雷达的位姿估计之间的所述IMU的运动信息进行积分，以获得相邻两帧之间的第一相对位姿；将所述GPS测量的位置数据、所述激光雷达的位姿估计以及所述第一相对位姿输入因子图进行优化操作，得到初始优化点云地图；基于所述初始优化点云地图和预设的标定板对所述无人驾驶车辆的当前状态进行回环检测；根据所述回环检测的结果获得相邻两帧之间的第二相对位姿；将所述第二相对位姿输入所述因子图进行优化操作，获得最终优化点云地图。本申请以解决现有技术中无人驾驶基于点云地图的定位以及感知的过程中不够准确，回环检测易出现错误的问题，通过放置特征性强的标定板作为回环检测的特征匹配，有助于回环检测的成功率，并显著减少建图过程中的误差，有利于建立高精度点云地图，帮助无人驾驶车辆进行更好的定位及路径规划。

附图说明

一个或多个实施例通过与之对应的附图进行示例性说明，这些示例性说明并不构成对实施例的限定，附图中具有相同参考数字标号的元件表示为类似的元件，除非有特别申明，附图中的图不构成比例限制。

图1是是本申请实施例提供的一种建图方法的流程示意图；

图2是本申请实施例提供的一种标定板放置的场景示意图；

图3是本申请实施例提供的一种因子图调节标定板参数的示意图；

图4是本申请实施例提供一种标定板检测结果示意图；

图5是本申请实施例提供的一种建图装置的结构示意图；

图6是本申请实施例提供的一种控制器的结构示意图；

图7是本申请实施例提供的一种无人驾驶车辆的结构示意图。

具体实施方式

为了使本申请的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本申请进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本申请，并不用于限定本申请。

需要说明的是，如果不冲突，本申请实施例中的各个特征可以相互组合，均在本申请的保护范围之内。另外，虽然在装置示意图中进行了功能模块的划分，在流程图中示出了逻辑顺序，但是在某些情况下，可以以不同于装置示意图中的模块划分，或流程图中的顺序执行所示出或描述的步骤。

除非另有定义，本说明书所使用的所有的技术和科学术语与属于本申请的技术领域的技术人员通常理解的含义相同。在本申请的说明书中所使用的术语只是为了描述具体的实施方式的目的，不是用于限制本申请。

请参阅图1，图1是本申请实施例提供的一种建图方法的流程示意图，该建图方法应用于无人驾驶车辆，所述无人驾驶车辆上设置有激光雷达、IMU和GPS，该方法包括以下步骤：

步骤S1：在所述无人驾驶车辆行驶时，获取所述激光雷达采样的原始点云、所述IMU采集的位姿数据以及所述GPS测量的位置数据。

激光雷达采样的原始点云是指由三维激光雷达设备扫描得到的空间点的数据集，每个点云都包含了三维坐标信息，有的还包含颜色信息、反射强度信息、回波次数信息等。

IMU全称是惯性导航***，可以利用惯性测量元件(陀螺仪、加速度计)测量载体相对惯性空间的角运动参数和线运动参数，在给定运动初始条件下，经导航解算得到载体速度、位置及姿态和航向。

GPS全称是全球定位***，是一种以人造地球卫星为基础的高精度无线电导航的定位***，它在全球任何地方以及近地空间都能够提供准确的地理位置、车行速度及精确的时间信息。

本申请实施例在无人驾驶车辆上安装激光雷达、IMU和GPS，以实时采集得到车辆外部环境的三维的激光雷达点云、IMU采集的位姿数据和GPS测量的位置数据。需要说明的是激光雷达和GPS分别是以一个较低的频率提供激光雷达点云数据和位置数据，IMU是以一个较高的频率提供位姿数据，将激光雷达采集到点云数据的时刻记为雷达采样时刻，将GPS采集的位置数据记为GPS采样时刻，IMU采集到位姿数据的时刻记为IMU采样时刻，则两个雷达采样时刻之间，会存在多个IMU采样时刻，两个GPS采样时刻之间，也会存在多个IMU采样时刻。

步骤S2：根据所述位姿数据对所述原始点云进行去畸变处理，以获得去畸变的点云。

先对激光雷达采样的原始点云中的部分噪点进行滤除，由于激光雷达在高速旋转采集数据的过程中，车辆也在运动，就会造成采集到的点云存在运动畸变，因此需要对原始点云进行去畸变处理。IMU的位姿数据可以为点云的去畸变提供一个转换的关系，将当前时刻的雷达点转换到特定的雷达位姿上，用以消除点云运动畸变。

在一些实施例中，所述根据所述位姿数据对所述原始点云进行去畸变处理，以获得去畸变的点云，包括：根据所述原始点云确定基准点和畸变点，采用所述位姿数据对所述原始点云中的点进行运动补偿，获得每个点的插值位姿数据，根据所述畸变点的时间戳与所述基准点的时间戳之间的插值位姿数据，计算所述畸变点与所述基准点的空间位姿关系，根据所述空间位姿关系将所述畸变点转换至所述基准点的坐标系下，以获得去畸变的点云。

基准点是指原始点云中一类点云可以作为其它点云参考或依据来进行校准的点云称为基准点，亦称参考点。

畸变点是指在采集过程中存在运动畸变的点云，在本申请中，除了基准点外的点都是畸变点。

时间戳是指激光雷达采集每个点云时的记录的时刻。

高频率的IMU采集的数据可以为原始点云的去畸变，提供一个转换的关系，通过这种转换关系可以将畸变点转换至基准点的坐标下，以获得去畸变的点云。例如通过四元数来确定转换关系，记录某一帧雷达中，第一个点云被采集的第一时间戳以及最后一个点云被采集的第二时间戳，并记录第一时间戳到第二时间戳这个时间段内所有的IMU的数据，利用IMU提供的四元数位姿数据，对原始点云中的每个点的位姿按照时间戳进行球面差值，获得每个点的差值位姿数据，根据畸变点的时间戳和基准点的时间戳的插值位姿数据，计算畸变点和基准点的空间为准关系，通过该空间位置关系将原始点云中的畸变点转换至基准点的坐标系下，即可消除因激光雷达的运动，造成点云运动形成的畸变点。

步骤S3：基于所述去畸变的点云进行匹配获得所述激光雷达的位姿估计。

激光雷达的位姿估计是通过匹配算法处理得到的雷达帧间匹配的相对位姿。匹配算法可以利用ICP(Iterative Closest Point，迭代最近点算法)和NDT(NormalDistributed Transform，正态分布变换算法)，但是以上算法的在匹配时不具有实时性，因此，在本申请中，可充分利用车辆行驶过程中地面的特点，来分割点云中的显示的地面，求解每个点的曲率信息，将去畸变的点云根据曲率来提取点云的线特征与面特征，将曲率大的作为线特征，将曲率小的作为面特征，为了避免一个地方的特征密集，可以将一周的点云数据划分为多份，并在各个区域根据曲率的大小来提取各区域的特征，例如选取的角可以是墙角，以及平面点可以是墙面。

步骤S4：将所述激光雷达的位姿估计之间的所述IMU的运动信息进行积分，以获得相邻两帧之间的第一相对位姿。

具体地，可以将相邻的雷达帧之间的IMU记录下来，对六轴IMU的输出数据进行预积分，进一步地，是对IMU的输出的角速度进行预积分，求得相邻帧之间的欧拉角变换，为了方便插值以及防止欧拉角出现万向锁的问题，将其转换为四元数，将该四元数作为相邻雷达帧之间的匹配的第一相对位姿，通过提供较准确的第一相对位姿，能更好，更快的加速相邻帧之间的特征匹配的收敛速度以及匹配精度。

步骤S5：将所述GPS测量的位置数据、所述激光雷达的位姿估计以及所述第一相对位姿输入因子图进行优化操作，得到初始优化点云地图。

G2O(General Graphic Optimization，图优化)是一种基于图优化的C++框架，主要用来优化非线性误差函数。

因子图是指对函数因子分解的表示图，一般内含两种顶点，变量顶点和函数顶点，边线表示顶点之间的函数关系。

在本申请中将雷达的位姿作为G2O优化库中因子图的顶点，激光雷达的位姿估计以及所述第一相对位姿作为G2O优化库中因子图的二元边。由于每次匹配可能会存在微小的误差，且每次匹配是建立在前一次匹配的基础上在进行匹配，由于车辆行驶距离较远，累计误差变大，会造成输出的点云地图的偏移过大，因此，可将GPS测量的位置数据作为因子图中雷达的位姿顶点的一元边导入到因子图中进行优化，用以得到更准确的点云地图。

GPS测量的位置数据可通过直接获取GPS输出的数据，将GPS测量的经度，纬度以及高度，转化为本地东北天坐标系，以获得此时车辆的位置数据。

在一些实施例中，基于所述激光雷达的位姿估计，获取当前雷达采样时刻的激光雷达点云，和从上一雷达采样时刻到所述当前雷达采样时刻之间的每一IMU采样时刻的所有IMU数据，根据所述所有所述IMU数据，以及所述上一雷达采样时刻的状态计算所述无人驾驶车辆在所述当前雷达采样时刻的当前状态预测值，获取所述当前雷达采样时刻对应的所述GPS测量的位置数据，根据所述位置数据对所述当前状态预测值进行修正，获得当前状态估计值，根据所述当前状态估计值和所述第一相对位姿，对所述所有IMU数据进行反向传播，以校正所述激光雷达的位姿估计，获得初始优化点云地图。

步骤S6：基于所述初始优化点云地图和预设的标定板对所述无人驾驶车辆的当前状态进行回环检测。

标定板是一种独立设计具有可识别的强特征标识的物体。

回环检测又称为闭环检测，是指无人驾驶车辆识别曾经到达某场景，使得地图闭环的能力。回环检测成功，能显著的减小累计的误差，无人驾驶车辆能更准确的进行定位以及感知等工作；而回环检测错误，非但不能消除误差，还会使得点云地图变得更加糟糕，甚至整个***崩溃。

可以理解的是，回环检测需要判断车辆是否到达之前已经来过的地方，需要知道车辆不同时刻是否返回到之前行驶过得同一个地方。

在上述过程中，车辆在行驶过程中会遇到很多动态障碍物，比如有红绿灯的十字路口，大卡车路过等复杂的环境中，或者场景单一的道路，比如建筑构造相同的小区或者厂房，通过常用的基于里程计的欧式距离，或者GPS的欧式距离来寻找回环边时，第一次经过识别的特征与第二次经过识别的特征会出现很大的变动，或者识别的相同特征过多，导致检测不到闭环或者得出错误匹配。为了避免上述情况的发生，在需要多次经过的交叉路口、十字路口等地方放置预设的标定板，根据现场各个地方放置的标定板的情况，动态调节检测的范围以及检测频率，以有效的提高检测的速度以及检测的准确度。其中标定板的数量可以是一个或多个，具体可根据实际情况而定。

请参阅图2，图2是本申请实施例提供的一种标定板放置的场景示意图，如图2所示，无人驾驶车辆10在行驶的过程中，经过同一个地方设置有框中标定板20、框中标定板30和框中标定板40，通过设置这些标定板，动态调节检测的范围以及检测频率，以有效的提高回环检测的速度以及检测的准确度。

在一些实施例中，当所述无人驾驶车辆行驶的预设路段放置有预设数目的标定板时，所述基于所述初始优化点云地图和预设的标定板对所述无人驾驶车辆的当前状态进行回环检测，包括：当所述无人驾驶车辆行驶至所述预设路段时，根据所述预设数目的标定板获取所述无人驾驶车辆当前的索引，获取所述无人驾驶车辆当前状态的上一次行驶至所述预设路段时所记录的索引，比较所述当前的索引和所述上一次对应记录的索引，当两次记录的索引的内容相同或误差在预设范围内时，则确定所述无人驾驶车辆的当前状态为闭环行驶；否则，确定所述无人驾驶车辆的当前状态为非闭环行驶。

索引是指无人驾驶车辆运动到特定位置时对该位置的车辆做的标记。例如，当无人驾驶车辆经过第一个十字路口时，可标记车辆的索引为01，第二次经过该十字路口时，标记车辆的索引为02，通过索引可以调取车辆经过该十字路口时的相关数据信息。

在一些实施例中，所述当所述无人驾驶车辆行驶至所述预设路段时，根据所述预设数目的标定板获取所述无人驾驶车辆当前的索引，包括，当所述无人驾驶车辆行驶至所述预设路段时，接收标定板检测范围和检测频率指令，根据所述标定板检测范围和检测频率指令对所述初始优化点云地图中的点云进行滤波处理，获取处理后的在所述标定板检测范围内的点云，根据所述点云的强度信息提取大于预设阈值强度的点云，将提取的所述点云进行霍夫直线检测，并选取符合预设条件的三根直线，从所述三根直线中分别选取一个点，计算是否满足三角形构成的条件，若满足，则记录所述无人驾驶车辆的当前雷达里程计的索引。

请参阅图3，图3是本申请实施例提供的一种因子图调节标定板参数的示意图，如图3所示，当无人驾驶车辆行驶至预设路段时，接收标定板检测范围和检测频率指令，整个参数调节需要使得包围盒A在前后左右上下方向能够包围住标定板，本申请中标定板的参数支持动态调整，在建图过程中包围盒A会随着最新的雷达里程计进行运动，需要说明的是，标定板的参数的调整会影响包围盒A的范围大小，进而会影响计算的速度，当标定板的参数调整范围适中，检测回环的速度会加快，标定板的参数调整范围过大，检测回环的计算的时间适当延长，标定板的参数调整范围过小，没有包围住所有的标定板则检测不到标定板，标定板的作用失效。

标定板的个数为三块时，通过在点云地图中框选的三块标定板的范围，具体地可参阅图3中的包围盒A，在调节好需要检测的范围以及检测频率后无需检测，首先提取包围盒A范围内的当前帧的点云，由于标定板的固有特性，即发射在标定板上的激光雷达点，得到的点云强度比其它地方都得要高，即可获取高强度的点云，根据点云的强度信息，设定预设阈值强度提取需要的点云，其中，预设阈值强度的设定需要大于普通点云的强度，小于或等于标定板得到的点云强度。提取需要的点云，通过霍夫直线检测来拟合出多条直线，选取三条直线上的各一个点，计算是否满足构成三角形的条件，若满足，则记录无人驾驶车辆的当前雷达里程计的索引。

具体地，请参阅图4，图4是本申请实施例提供一种标定板检测结果示意图，如图4所示，其中，无人驾驶车10a周围的检测到的标定板分别为框中标定板20a、框中标定板30a和框中标定板40a。

步骤S7：根据所述回环检测的结果获得相邻两帧之间的第二相对位姿。

在一些实施例中，所述根据所述回环检测的结果获得相邻两帧之间的第二相对位姿具体包括：当所述回环检测的结果为确定所述无人驾驶车辆的当前状态为闭环行驶时，则记录相邻两帧点云之间的第二相对位姿。

当无人驾驶车辆运动到放置标定板的位置时，记录第一次的雷达里程计的索引，当第二次经过时，同样记录此时雷达里程计的索引，将两次经过标定板位置时的两帧点云的特征进行ICP算法匹配，即可得到两帧之间无累计误差的第二相对位姿。

步骤S8：将所述第二相对位姿输入所述因子图进行优化操作，获得最终优化点云地图。

将ICP算法匹配之后得第二相对位姿，作为因子图中雷达里程计顶点的二元边导入到因子图中进行优化，获得最终优化点云地图。

本申请提供的建图方法应用于无人驾驶车辆，所述无人驾驶车辆上设置有激光雷达、IMU和GPS，该方法包括：在所述无人驾驶车辆行驶时，获取所述激光雷达采样的原始点云、所述IMU采集的位姿数据以及所述GPS测量的位置数据；根据所述位姿数据对所述原始点云进行去畸变处理，以获得去畸变的点云；基于所述去畸变的点云进行匹配获得所述激光雷达的位姿估计；将所述激光雷达的位姿估计之间的所述IMU的运动信息进行积分，以获得相邻两帧之间的第一相对位姿；将所述GPS测量的位置数据、所述激光雷达的位姿估计以及所述第一相对位姿输入因子图进行优化操作，得到初始优化点云地图；基于所述初始优化点云地图和预设的标定板对所述无人驾驶车辆的当前状态进行回环检测；根据所述回环检测的结果获得相邻两帧之间的第二相对位姿；将所述第二相对位姿输入所述因子图进行优化操作，获得最终优化点云地图。本申请以解决现有技术中无人驾驶基于点云地图的定位以及感知的过程中不够准确，回环检测易出现错误的问题，通过放置特征性强的标定板作为回环检测的特征匹配，有助于回环检测，并显著减少建图过程中的误差，有利于建立高精度点云地图，帮助无人驾驶车辆进行更好的定位及规划路径。

请参阅图5，图5是本申请实施例提供的一种建图装置的结构示意图，应用于无人驾驶车辆，无人驾驶车辆上设置有激光雷达、IMU和GPS，该装置100包括：参数获取模块101、畸变处理模块102、位姿估计获取模块103、第一相对位姿获取模块104、初始优化点云地图获取模块105、回环检测模块106、第二相对位姿获取模块107、最终优化点云地图获取模块108。

其中，参数获取模块101，用于在所述无人驾驶车辆行驶时，获取所述激光雷达采样的原始点云、所述IMU采集的位姿数据以及所述GPS测量的位置数据。

畸变处理模块102，用于根据所述位姿数据对所述原始点云进行去畸变处理，以获得去畸变的点云。

位姿估计获取模块103，用于基于所述去畸变的点云进行匹配获得所述激光雷达的位姿估计。第一相对位姿获取模块104，用于将所述激光雷达的位姿估计之间的所述IMU的运动信息进行积分，以获得相邻两帧之间的第一相对位姿。初始优化点云地图获取模块105，用于将所述GPS测量的位置数据、所述激光雷达的位姿估计以及所述第一相对位姿输入因子图进行优化操作，得到初始优化点云地图。回环检测模块106，用于基于所述第一目标点云和预设的标定板对所述无人驾驶车辆的当前状态进行回环检测。第二相对位姿获取模块107，用于根据所述回环检测的结果获得相邻两帧之间的第二相对位姿。最终优化点云地图获取模块108，用于将所述第二相对位姿输入所述因子图进行优化操作，获得最终优化点云地图。

需要说明的是，上述建图装置可执行本申请实施例所提供的建图方法，具备执行方法相应的功能模块和有益效果。未在建图装置实施例中详尽描述的技术细节，可参见本申请实施例所提供的建图方法。

在本申请实施例中，建图装置亦可以由硬件器件搭建成的，例如，建图装置可以由一个或两个以上的芯片搭建而成，各个芯片可以互相协调工作，以完成上述各个实施例所阐述的应用于建图方法。再例如，建图装置还可以由各类逻辑器件搭建而成，诸如由通用处理器、数字信号处理器(DSP)、专用集成电路(ASIC)、现场可编程门阵列(FPGA)、单片机、ARM(Acorn RISC Machine)或其它可编程逻辑器件、分立门或晶体管逻辑、分立的硬件组件或者这些部件的任何组合而搭建成。

本申请实施例中的建图装置可以为具有操作***的装置。该操作***可以为安卓(Android)操作***，可以为ios操作***，还可以为其他可能的操作***，本申请实施例不作具体限定。

需要说明的是，本申请实施例中的一种建图装置中的各个模块、单元之间的信息交互、执行过程等内容，由于与本申请方法实施例基于同一构思，具体内容同样适用于建图装置。本申请实施例中的各个模块能作为单独的硬件或软件来实现，并且可以根据需要使用单独的硬件或软件来实现各个单元的功能的组合。

本申请还提供了一种控制器，请参阅图6，图6是本申请实施例提供的一种控制器的结构示意图。该控制器200，包括至少一个处理器201，以及与所述至少一个处理器201通信连接的存储器202，其中，所述存储器202存储有可被所述至少一个处理器201执行的指令，所述指令被所述至少一个处理器201执行，以使所述至少一个处理器201能够执行上述任意方法实施例中的建图方法。所述处理器201和所述存储器202可以通过总线或者其他方式连接，图6中以通过总线连接为例。

处理器201可以是通用处理器，包括中央处理器(Central Processing Unit，CPU)、网络处理器(Network Processor，NP)、硬件芯片或者其任意组合；还可以是数字信号处理器(Digital Signal Processing，DSP)、专用集成电路(Application SpecificIntegrated Circuit，ASIC)、可编程逻辑器件(programmable logic device，PLD)或其组合。上述PLD可以是复杂可编程逻辑器件(complex programmable logic device，CPLD)，现场可编程逻辑门阵列(field-programmable gate array，FPGA)，通用阵列逻辑(genericarray logic，GAL)或其任意组合。

存储器202作为一种非暂态计算机可读存储介质，可用于存储非暂态软件程序、非暂态性计算机可执行程序以及模块，如本申请实施例中的建图方法对应的程序指令/模块。处理器201通过运行存储在存储器202中的非暂态软件程序、指令以及模块，可以实现上述任一方法实施例中的建图方法，即能够实现图1的整个过程。

本申请还提供了一种无人驾驶车辆，请参阅图7，图7是本申请实施例提供的一种无人驾驶车辆的结构示意图。该无人驾驶车辆300，包括：激光雷达301、IMU302、GPS303以及上述控制器200。

本申请实施例提供了一种非易失性计算机可读存储介质，所述非易失性计算机可读存储介质存储有计算机可执行指令，例如包括程序代码的存储器，上述程序代码可由处理器执行以完成上述实施例中的建图方法。例如，该计算机可读存储介质可以是只读存储器(Read-Only Memory,ROM)、随机存取存储器(Random Access Memory，RAM)、只读光盘(Compact Disc Read-Only Memory，CDROM)、磁带、软盘和光数据存储设备等。

本申请实施例提供了一种计算机程序产品，该计算机程序产品包括一条或多条程序代码，该程序代码存储在计算机可读存储介质中。智能车辆的处理器从计算机可读存储介质读取该程序代码，处理器执行该程序代码，以完成上述实施例中提供的建图方法步骤。

通过以上的实施方式的描述，本领域普通技术人员可以清楚地了解到各实施方式可借助软件加通用硬件平台的方式来实现，当然也可以通过硬件。本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程是可以通过计算机程序来指令相关的硬件来完成，所述的程序可存储于一计算机可读取存储介质中，该程序在执行时，可包括如上述各方法的实施例的流程。其中，所述的存储介质可为磁碟、光盘、只读存储记忆体(Read-Only Memory,ROM)或随机存储记忆体(Random Access Memory,RAM)等。

最后应说明的是：以上实施例仅用以说明本申请的技术方案，而非对其限制；在本申请的思路下，以上实施例或者不同实施例中的技术特征之间也可以进行组合，步骤可以以任意顺序实现，并存在如上所述的本申请的不同方面的许多其它变化，为了简明，它们没有在细节中提供；尽管参照前述实施例对本申请进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本申请各实施例技术方案的范围。

Claims (10)

1.一种建图方法，应用于无人驾驶车辆，所述无人驾驶车辆上设置有激光雷达、IMU和GPS，其特征在于，所述方法包括：

在所述无人驾驶车辆行驶时，获取所述激光雷达采样的原始点云、所述IMU采集的位姿数据以及所述GPS测量的位置数据；

根据所述位姿数据对所述原始点云进行去畸变处理，以获得去畸变的点云；

基于所述去畸变的点云进行匹配获得所述激光雷达的位姿估计；

将所述激光雷达的位姿估计之间的所述IMU的运动信息进行积分，以获得相邻两帧之间的第一相对位姿；

将所述GPS测量的位置数据、所述激光雷达的位姿估计以及所述第一相对位姿输入因子图进行优化操作，得到初始优化点云地图；

基于所述初始优化点云地图和预设的标定板对所述无人驾驶车辆的当前状态进行回环检测；

根据所述回环检测的结果获得相邻两帧之间的第二相对位姿；

将所述第二相对位姿输入所述因子图进行优化操作，获得最终优化点云地图。

2.根据权利要求1所述的建图方法，其特征在于，所述根据所述位姿数据对所述原始点云进行去畸变处理，以获得去畸变的点云，包括：

根据所述原始点云确定基准点和畸变点；

采用所述位姿数据对所述原始点云中的点进行运动补偿，获得每个点的插值位姿数据；

根据所述畸变点的时间戳与所述基准点的时间戳之间的插值位姿数据，计算所述畸变点与所述基准点的空间位姿关系；

根据所述空间位姿关系将所述畸变点转换至所述基准点的坐标系下，以获得去畸变的点云。

3.根据权利要求1所述的建图方法，其特征在于，所述将所述GPS测量的位置数据、所述激光雷达的位姿估计以及所述第一相对位姿输入因子图进行优化操作，得到初始优化点云地图，包括：

基于所述激光雷达的位姿估计，获取当前雷达采样时刻的激光雷达点云，和从上一雷达采样时刻到所述当前雷达采样时刻之间的每一IMU采样时刻的所有IMU数据；

根据所述所有所述IMU数据，以及所述上一雷达采样时刻的状态计算所述无人驾驶车辆在所述当前雷达采样时刻的当前状态预测值；

获取所述当前雷达采样时刻对应的所述GPS测量的位置数据；

根据所述位置数据对所述当前状态预测值进行修正，获得当前状态估计值；

根据所述当前状态估计值和所述第一相对位姿，对所述所有IMU数据进行反向传播，以校正所述激光雷达的位姿估计，获得初始优化点云地图。

4.根据权利要求1至3任一项所述的建图方法，其特征在于，当所述无人驾驶车辆行驶的预设路段放置有预设数目的标定板时，所述基于所述初始优化点云地图和预设的标定板对所述无人驾驶车辆的当前状态进行回环检测，包括：

当所述无人驾驶车辆行驶至所述预设路段时，根据所述预设数目的标定板获取所述无人驾驶车辆当前的索引；

获取所述无人驾驶车辆当前状态的上一次行驶至所述预设路段时所记录的索引；

比较所述当前的索引和所述上一次对应记录的索引，当两次记录的索引的内容相同或误差在预设范围内时，则确定所述无人驾驶车辆的当前状态为闭环行驶；否则，确定所述无人驾驶车辆的当前状态为非闭环行驶。

5.根据权利要求4所述的建图方法，其特征在于，所述当所述无人驾驶车辆行驶至所述预设路段时，根据所述预设数目的标定板获取所述无人驾驶车辆当前的索引，包括：

当所述无人驾驶车辆行驶至所述预设路段时，接收标定板检测范围和检测频率指令；

根据所述标定板检测范围和检测频率指令对所述初始优化点云地图中的点云进行滤波处理，获取处理后的在所述标定板检测范围内的点云；

根据所述点云的强度信息提取大于预设阈值强度的点云；

将提取的所述点云进行霍夫直线检测，并选取符合预设条件的三根直线；

从所述三根直线中分别选取一个点，计算是否满足三角形构成的条件，若满足，则记录所述无人驾驶车辆的当前雷达里程计的索引。

6.根据权利要求4所述的建图方法，其特征在于，所述根据所述回环检测的结果获得相邻两帧之间的第二相对位姿具体包括：

当所述回环检测的结果为确定所述无人驾驶车辆的当前状态为闭环行驶时，则记录相邻两帧点云之间的第二相对位姿。

7.一种建图装置，应用于无人驾驶车辆，所述无人驾驶车辆上设置有激光雷达、IMU和GPS，其特征在于，所述装置包括：

参数获取模块，用于在所述无人驾驶车辆行驶时，获取所述激光雷达采样的原始点云、所述IMU采集的位姿数据以及所述GPS测量的位置数据；

畸变处理模块，用于根据所述位姿数据对所述原始点云进行去畸变处理，以获得去畸变的点云；

位姿估计获取模块，用于基于所述去畸变的点云进行匹配获得所述激光雷达的位姿估计；

第一相对位姿获取模块，用于将所述激光雷达的位姿估计之间的所述IMU的运动信息进行积分，以获得相邻两帧之间的第一相对位姿；

初始优化点云地图获取模块，用于将所述GPS测量的位置数据、所述激光雷达的位姿估计以及所述第一相对位姿输入因子图进行优化操作，得到初始优化点云地图；

回环检测模块，用于基于所述第一目标点云和预设的标定板对所述无人驾驶车辆的当前状态进行回环检测；

第二相对位姿获取模块，用于根据所述回环检测的结果获得相邻两帧之间的第二相对位姿；

最终优化点云地图获取模块，用于将所述第二相对位姿输入所述因子图进行优化操作，获得最终优化点云地图。

8.一种控制器，其特征在于，包括：至少一个处理器；与所述至少一个处理器通信连接的存储器；其中，所述存储器存储有可被所述至少一个处理器执行的指令，所述指令被所述至少一个处理器执行，以使所述至少一个处理器能够执行权利要求1-6任一项所述的建图方法。

9.一种无人驾驶车辆，其特征在于，包括：激光雷达、IMU、GPS以及权利要求8所述的控制器。

10.一种非易失性计算机可读存储介质，其特征在于，所述非易失性计算机可读存储介质存储有计算机可执行指令，当所述计算机可执行指令被控制器执行时，使所述控制器执行权利要求1-6任一项所述的建图方法。

Images