CN109710724A - 一种构建点云地图的方法和设备 - Google Patents

Abstract

本发明公开了一种构建点云地图的方法和设备，涉及自动驾驶技术领域，用以解决目前构建点云地图时需要进行全局优化，累计误差较大，计算速度慢的问题，本发明方法包括：确定第N帧图像对应的里程计信息；根据连续M帧图像中的最后一帧图像以及M帧图像对应的里程计信息对连续M帧图像中的点云数据进行坐标转换后得到连续M帧图像的点云地图坐标；根据连续M帧图像的点云地图坐标确定点云地图，其中任意相邻两次确定的点云地图中，后一次确定的点云地图使用的连续M帧图像中的第一帧图像为前一次确定的点云地图使用的连续M帧图像中的第二帧图像。由于本发明中不需要进行全局优化，计算速度更快，且不会产生累积误差。

Description

一种构建点云地图的方法和设备

技术领域

本发明涉及自动驾驶技术领域，特别涉及一种构建点云地图的方法和设备。

背景技术

随着科技的不断发展和进步，计算机技术、现代传感技术和人工智能技术等逐渐应用到了汽车领域中，具有环境感知、路径规划、辅助驾驶等功能的智能车辆应运而生。通过对智能车辆进行控制，可以使智能车辆自动按照预先制定的行驶路径安全行驶，实现无人驾驶。其中，生成车辆行驶环境的点云地图是无人驾驶技术中的一个重要内容。

相关技术中，主要是采用视觉传感器生成车辆行驶环境的点云地图，视觉传感器获取车辆周边物体在不同视角下的图像，然后基于三角测量原理计算图像像素间的位置偏差获得物体的三维信息，之后，再根据物体的三维信息确定该物体与视觉传感器之间的实际距离，进而生成点云地图。然而这种生成方式需要执行多次图像处理操作，工作量较大，操作较繁琐。

现有技术中除了采用视觉传感器生成点云地图的方式外，对基于激光雷达扫描得到的三维点云数据构建的点云地图也进行了研究，用无人驾驶汽车搭载的激光雷达设备采集多种传感器数据，包括GPS(Global Positioning System，全球定位系统)数据、LiDAR（Light Detection And Ranging，激光雷达）数据等生成三维点云地图，利用相机数据和IMU数据生成视觉特征地图，最后利用GPS数据完成控制，同时进行全局优化。在构建点云地图时，需要进行全局优化，产生的累积误差大，计算速度慢。

综上所述，目前构建点云地图时需要进行全局优化，累计误差较大，计算速度慢。

发明内容

本发明提供一种构建点云地图的方法和设备，用以解决现有技术中存在目前构建点云地图时需要进行全局优化，累计误差较大，计算速度慢的问题。

第一方面，本发明实施例提供的一种构建点云地图的方法包括：

确定通过激光雷达扫描车辆周边道路确定的第N帧图像对应的用于表示车辆行驶状态的里程计信息，其中N为正整数；

根据连续M帧图像中的最后一帧图像以及所述M帧图像对应的所述里程计信息对所述连续M帧图像中的点云数据进行坐标转换后得到所述连续M帧图像的点云地图坐标，其中M为正整数；

根据所述连续M帧图像的点云地图坐标确定点云地图，其中任意相邻两次确定的点云地图中，后一次确定的点云地图使用的连续M帧图像中的第一帧图像为前一次确定的点云地图使用的连续M帧图像中的第二帧图像。

上述方法，在构建点云地图时，只需对激光雷达采集到的图像中的连续M帧图像进行坐标转换处理则可生成局部点云地图，不需要进行全局优化，相比于现有技术生成点云地图时根据采集到的所有图像生成点云地图，计算速度更快，并且不会产生累积误差。

在一种可能的实现方式中，通过下列方式确定所述第N帧图像对应的用于表示车辆行驶状态的里程计信息：

若所述第N帧图像对应的惯性测量单元IMU（Inertial measurement unit，惯导测量单元)数据集中的所有IMU数据都处于非失锁状态，则根据所述第N帧图像对应的IMU数据集确定所述第N帧图像对应的所述里程计信息，其中所述非失锁状态表示测量得到所述IMU数据集时IMU数据的状态；或

若所述第N帧图像对应的IMU数据集中的至少一个IMU数据处于失锁状态，则根据所述第N帧图像中的点云数据确定所述第N帧图像对应的所述里程计信息；

其中，所述第N帧图像对应的IMU数据集是在确定所述第N帧图像的同时测量得到的。

上述方法，根据IMU数据直接确定里程计信息，其中IMU可以提供车辆的位姿信息，构建的点云地图用于自动驾驶的局部感知， IMU数据集中的所有IMU数据都不处于失锁状态，保证了IMU数据的准确性；当第N帧图像对应的IMU数据集中的至少一个IMU数据处于失锁状态时，表明IMU数据不太准确，此时可以依据第N帧图像中的点云数据确定所述第N帧图像对应的里程计信息；在GPS信号不准的情况下依然可以构建有效准确的点云地图，提高了地图精度。

在一种可能的实现方式中，所述根据所述第N帧图像中的点云数据确定所述第N帧图像对应的所述里程计信息，包括：

确定所述第N帧图像中的点云数据对应的点的曲面特征，其中所述曲面特征表示所述第N帧图像中的任意一个点与所述任意一个点周围最近邻K个点欧式距离的和，K为正整数；

根据所述点的曲面特征通过非极大值抑制对所述第N帧图像中的点云数据对应的点进行筛选，并将筛选后剩余的点对应的点云数据作为最佳点云数据；

通过迭代最近点ICP（Iterative Closest Point迭代最近点)算法对所述最佳点云数据进行点云匹配确定第N-1帧图像中与所述最佳点云数据匹配的点云数据；

根据所述第N帧图像中的最佳点云数据以及所述第N-1帧图像中与所述最佳点云数据匹配的点云数据确定RT（Rotational translation，旋转平移)矩阵，并将所述RT矩阵作为所述里程计信息。

上述方法，根据激光雷达扫描得到的图像中的点云数据的曲面特征，采用非极大值抑制的方式对第N帧图像中的点云数据对应的点进行筛选，得到曲面特征大的边缘点以及曲面特征小的平面点，将其他的点筛除，大大减少了ICP来进行点云匹配时的计算量，降低了耗时，便于满足实时性需要，出现的累计漂移误差小，根据所述第N帧图像中的最佳点云数据以及所述第N-1帧图像中与所述最佳点云数据匹配的点云数据确定RT矩阵，即确定出相邻两帧图像中点的对应关系，将RT矩阵作为所述里程计信息，以便进行坐标转换，实现了在IMU失锁的状态下里程计信息的确定。

在一种可能的实现方式中，所述根据第N帧图像对应的IMU数据集确定所述第N帧图像对应的所述里程计信息，包括：

根据第N帧图像对应的第一位移平移、第二位移平移及旋转量确定RT矩阵，并将所述RT旋转平移矩阵作为所述里程计信息；

其中，所述第一位移平移为所述第N帧图像对应的IMU数据集中所有加速度的和与所述IMU数据测量时间间隔的乘积，所述第二位移平移为所述第N帧图像对应的IMU数据集中所有速度的和与所述IMU数据测量时间间隔的乘积，所述旋转量为根据所述第N帧图像对应的IMU数据集中的IMU数据的时间戳确定的最后一次测量得到的航向角。

上述方法，根据第N帧图像对应的IMU数据集中的加速度、速度等信息确定出车的位移平移，以及根据IMU数据集中的航向角确定出车的旋转量，后根据位移平移以及旋转量计算得到RT矩阵，将所述RT矩阵作为里程计信息，以表示车的行驶状态，采用高精度IMU数据便于构建高精度点云地图。在确定里程计信息时不依赖于GPS数据（经纬度绝对坐标）。

在一种可能的实现方式中，所述根据连续M帧图像中的最后一帧图像以及所述M帧图像对应的所述里程计信息对所述连续M帧图像中的点云数据进行坐标转换后得到所述连续M帧图像的点云地图坐标，包括：

以激光雷达在所述连续M帧图像中的最后一帧图像中的位置为坐标原点，根据所述M帧图像对应的所述里程计信息对所述连续M帧图像中的点云数据进行坐标转换。

上述方法，以激光雷达在所述连续M帧图像中的最后一帧图像中的位置为坐标原点进行坐标转换得到的坐标系是相对坐标系，根据里程计信息对激光雷达扫描得到的点云数据进行坐标转换，将连续M帧图像中的点云数据转换到统一坐标系下，相比于现有技术中生成点云地图时是一个全局地图坐标系（坐标原点为某个经纬度坐标），本发明生成的点云地图更加适用于环境感知。

第二方面，本发明实施例提供的一种构建点云地图的设备包括：至少一个处理单元以及至少一个存储单元，其中，所述存储单元存储有程序代码，当所述程序代码被所述处理单元执行时，使得所述存储单元执行下列过程：

确定通过激光雷达扫描车辆周边道路确定的第N帧图像对应的用于表示车辆行驶状态的里程计信息，其中N为正整数；

根据连续M帧图像中的最后一帧图像以及所述M帧图像对应的所述里程计信息对所述连续M帧图像中的点云数据进行坐标转换后得到所述连续M帧图像的点云地图坐标，其中M为正整数；

根据所述连续M帧图像的点云地图坐标确定点云地图，其中任意相邻两次确定的点云地图中，后一次确定的点云地图使用的连续M帧图像中的第一帧图像为前一次确定的点云地图使用的连续M帧图像中的第二帧图像。

在一种可能的实现方式中，所述处理单元还用于通过下列方式确定所述第N帧图像对应的用于表示车辆行驶状态的里程计信息：

若所述第N帧图像对应的惯性测量单元IMU数据集中的所有IMU数据都处于非失锁状态，则根据所述第N帧图像对应的IMU数据集确定所述第N帧图像对应的所述里程计信息，其中所述非失锁状态表示测量得到所述IMU数据集时IMU数据的状态；或

若所述第N帧图像对应的IMU数据集中的至少一个IMU数据处于失锁状态，则根据所述第N帧图像中的点云数据确定所述第N帧图像对应的所述里程计信息；

其中，所述第N帧图像对应的IMU数据集是在确定所述第N帧图像的同时测量得到的。

在一种可能的实现方式中，所述处理单元具体用于：

确定所述第N帧图像中的点云数据对应的点的曲面特征，其中所述曲面特征表示所述第N帧图像中的任意一个点与所述任意一个点周围最近邻K个点欧式距离的和，K为正整数；

根据所述点的曲面特征通过非极大值抑制对所述第N帧图像中的点云数据对应的点进行筛选，并将筛选后剩余的点对应的点云数据作为最佳点云数据；

通过ICP算法对所述最佳点云数据进行点云匹配确定第N-1帧图像中与所述最佳点云数据匹配的点云数据；

根据所述第N帧图像中的最佳点云数据以及所述第N-1帧图像中与所述最佳点云数据匹配的点云数据确定RT矩阵，并将所述RT矩阵作为所述里程计信息。

在一种可能的实现方式中，所述处理单元具体用于：

根据第N帧图像对应的第一位移平移、第二位移平移及旋转量确定RT矩阵，并将所述RT矩阵作为所述里程计信息；

其中，所述第一位移平移为所述第N帧图像对应的IMU数据集中所有加速度的和与所述IMU数据测量时间间隔的乘积，所述第二位移平移为所述第N帧图像对应的IMU数据集中所有速度的和与所述IMU数据测量时间间隔的乘积，所述旋转量为根据所述第N帧图像对应的IMU数据集中的IMU数据的时间戳确定的最后一次测量得到的航向角。

在一种可能的实现方式中，所述处理单元具体用于：

以激光雷达在所述连续M帧图像中的最后一帧图像中的位置为坐标原点，根据所述M帧图像对应的所述里程计信息对所述连续M帧图像中的点云数据进行坐标转换。

第三方面，本发明实施例还提供一种构建点云地图的设备，该设备包括第一确定模块、坐标转换模块和第二确定模块：

第一确定模块，用于确定通过激光雷达扫描车辆周边道路确定的第N帧图像对应的用于表示车辆行驶状态的里程计信息，其中N为正整数；

坐标转换模块，用于根据连续M帧图像中的最后一帧图像以及所述M帧图像对应的所述里程计信息对所述连续M帧图像中的点云数据进行坐标转换后得到所述连续M帧图像的点云地图坐标，其中M为正整数；

第二确定模块，用于根据所述连续M帧图像的点云地图坐标确定点云地图，其中任意相邻两次确定的点云地图中，后一次确定的点云地图使用的连续M帧图像中的第一帧图像为前一次确定的点云地图使用的连续M帧图像中的第二帧图像。

第四方面，本申请还提供一种计算机存储介质，其上存储有计算机程序，该程序被处理单元执行时实现第一方面所述方法的步骤。

另外，第二方面至第四方面中任一种实现方式所带来的技术效果可参见第一方面中不同实现方式所带来的技术效果，此处不再赘述。

附图说明

为了更清楚地说明本发明实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简要介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域的普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1为本发明实施例提供的一种构建点云地图的方法示意图；

图2为本发明实施例提供的一种激光雷达扫描得到的点云图像示意图；

图3为本发明实施例提供的一种坐标转换前的坐标系示意图；

图4为本发明实施例提供的一种坐标转换后的坐标系示意图；

图5为本发明实施例提供的一种构建点云地图时使用的连续帧图像示意图；

图6为本发明实施例提供的一种构建点云地图的完整方法流程图；

图7为本发明实施例提供的一种构建点云地图的设备示意图；

图8为本发明实施例提供的另一种构建点云地图的设备示意图。

具体实施方式

为了使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明作进一步地详细描述，显然，所描述的实施例仅仅是本发明一部份实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范围。

下面对文中出现的一些词语进行解释：

1、本发明实施例中术语“和/或”，描述关联对象的关联关系，表示可以存在三种关系，例如，A和/或B，可以表示：单独存在A，同时存在A和B，单独存在B这三种情况。字符“/”一般表示前后关联对象是一种“或”的关系。

2、本发明实施例中术语“点云数据”是指透过3D（Three Dimensional，三维)扫描仪扫描资料以点的形式记录，每一个点包含有三维坐标，有些可能含有颜色信息（RGB，红绿蓝)或反射强度信息（Intensity)。

3、本发明实施例中术语“失锁状态”表示GPS卫星信号不好，GPS由于本身故障或者安装位置不当，无法抓到4颗以上卫星。

4、本发明实施例中术语“IMU”的基本工作原理是以牛顿力学定律为基础，通过测量载体在惯性参考系的加速度，将它对时间进行积分，且把它变换到导航坐标系中，就能够得到在导航坐标系中的速度、偏航角和位置等信息。

本发明实施例描述的应用场景是为了更加清楚的说明本发明实施例的技术方案，并不构成对于本发明实施例提供的技术方案的限定，本领域普通技术人员可知，随着新应用场景的出现，本发明实施例提供的技术方案对于类似的技术问题，同样适用。其中，在本发明的描述中，除非另有说明，“多个”的含义是两个或两个以上。

如图1所示，本发明实施例的一种构建点云地图的方法，具体包括以下步骤：

步骤100：确定通过激光雷达扫描车辆周边道路确定的第N帧图像对应的用于表示车辆行驶状态的里程计信息，其中N为正整数；

步骤101：根据连续M帧图像中的最后一帧图像以及所述M帧图像对应的所述里程计信息对所述连续M帧图像中的点云数据进行坐标转换后得到所述连续M帧图像的点云地图坐标，其中M为正整数；

步骤102：根据所述连续M帧图像的点云地图坐标确定点云地图，其中任意相邻两次确定的点云地图中，后一次确定的点云地图使用的连续M帧图像中的第一帧图像为前一次确定的点云地图使用的连续M帧图像中的第二帧图像。

通过上述方案，在构建点云地图时，只需对激光雷达采集到的图像中的连续M帧图像进行处理则可生成局部点云地图，不需要进行全局优化，相比于现有技术生成点云地图时根据采集到的所有图像生成点云地图，计算速度更快，并且不会产生累积误差，适用于环境感知。

相比于现有技术中构建的全局点云地图，本发明的实现方法对存储空间要求低，根据连续M帧点云数据和M个里程计信息确定点云地图，采用渐进更新方式来更新维护点云地图，当最新的一帧数据（点云数据和里程计信息）生成，则加入M帧序列里面，同时把历史最早的一帧从序列里面删除，保证序列里面的数量都是稳定的M帧。本发明根据连续M帧图像确定的点云地图而是一个局部的点云地图，不存在随时间变化不断增长的情况，局部感知信息增强，有利于自动驾驶的局部感知。

在本发明实施例中，点云数据是指在一个三维坐标系统中的一组向量的集合。这些向量通常以X，Y，Z三维坐标的形式表示，而且一般主要用来代表一个物体的外表面形状。不仅如此，除（X，Y，Z)代表的几何位置信息之外，点云数据还可以表示一个点的RGB颜色，灰度值，深度，分割结果等。

例如，Pi={Xi，Yi，Zi}表示空间中的一个点，（i=1，2，3，……，n)，则Point Cloud={P1，P2，P3，……，Pn}表示一组点云数据。

现有技术中，大多数点云数据是由3D扫描设备产生的，例如激光雷达(2D/3D)，立体摄像头(stereo camera)，越渡时间相机(time-of-flight camera)。这些设备用自动化的方式测量在物体表面的大量的点的信息，然后用某种数据文件输出点云数据。这些点云数据就是扫描设备所采集到的。点云数据是3D激光雷达扫描仪的基本输出。这通用的点云数据文件形式是3D坐标文件(经常指一个xyz文件)。这些文件是ASCII（American StandardCode for Information Interchange，美国信息交换标准代码)，因此可以被所有的后处理软件读取。

在本发明实施例中，IMU数据集中的IMU数据包括但不限于下列的部分或全部：

速度、加速度、航向角。

在本发明实施例中，采用的装置是velodyne-32线激光雷达和诺瓦泰高精度惯导单元IMU，IMU可达到厘米级定位精度。通过激光雷达扫描车辆周边道路后输出的就是第N帧图像中的点云数据，与第N帧图像对应的IMU数据集表示在确定第N帧图像的同时通过IMU测量得到的IMU数据组成的集合，例如激光雷达数据输出频率是10赫兹，IMU输出的频率是100赫兹，则输出第N帧图像中的点云数据的时间内对应输出10组IMU数据（假设一组IMU包括一个加速度，一个速度以及一个航向角)，这10组IMU数据（30个IMU数据)组成一个IMU数据集，即与第N帧图像对应的IMU数据集。

当激光雷达数据输出频率是10赫兹，IMU输出的频率是50赫兹，则输出第N帧图像中的点云数据的时间内对应输出5组IMU数据（假设一组IMU包括一个加速度，一个速度以及一个航向角)，这5组IMU数据（15个IMU数据)组成一个IMU数据集，即与第N帧图像对应的IMU数据集。

在实施中，确定第N帧图像对应的用于表示车辆行驶状态的里程计信息的方式有很多种，本发明实施例中根据第N帧图像对应的IMU数据集是否处于非失锁状态来进一步确定计算里程计信息的方式。

可选的，判断IMU数据集是否处于非失锁状态，即判断IMU数据集中的IMU数据是否都不处于失锁状态（其中非失锁状态表示测量得到IMU数据集时其中的IMU数据的状态)。

具体的，判断第N帧图像对应的IMU数据集中的IMU数据是否都不处于失锁状态，根据诺瓦泰组合惯导单元的输出信号来确定，其中每输出一组IMU数据对应一组输出信号，假设一组输出信号为（a，b)，其中a表示GPS状态，b表示系统状态，若a为4，b为1时，则表示这一组IMU数据不处于失锁状态；若a为3，b为1，此时表示这一组IMU数据处于失锁状态。进一步，在判断某一帧图像对应的IMU数据集是否处于非失锁状态时，需保证IMU数据集中的IMU数据都不处于失锁状态，若一个IMU数据集包含30个IMU数据，则当这30个IMU数据都不处于失锁状态（即这30个数据都处于非失锁状态时)，此时IMU数据集处于非失锁状态；若这30个数据中有6个数据处于失锁状态，其余24个数据都不处于失锁状态，此时IMU数据集处于失锁状态。

在本发明实施例中，判断第N帧图像对应的IMU数据集中的IMU数据的状态后，需要计算用于表示车辆行驶状态（即车辆的位移平移、旋转量等)的里程计信息，在本发明实施例中用RT矩阵作为里程计信息。

具体的，根据第N帧图像对应的IMU数据集是否处于非失锁状态确定计算里程计信息的方式是有两种情况：

计算方式一、若确定第N帧图像对应的IMU数据集中的所有IMU数据都处于非失锁状态，则根据第N帧图像对应的IMU数据集确定第N帧图像对应的里程计信息。

具体的，根据第N帧图像对应的第一位移平移、第二位移平移及旋转量确定RT矩阵，并将所述RT矩阵作为所述里程计信息；其中，所述第一位移平移为所述第N帧图像对应的IMU数据集中所有加速度的和与所述IMU数据测量时间间隔的乘积，所述第二位移平移为所述第N帧图像对应的IMU数据集中所有速度的和与所述IMU数据测量时间间隔的乘积，所述旋转量为根据所述第N帧图像对应的IMU数据集中的IMU数据的时间戳确定的最后一次测量得到的航向角，即根据IMU航向角和速度加速度信息，得到里程计信息

假设激光雷达数据输出频率是10赫兹，IMU输出的频率是100赫兹，首先要对激光雷达和IMU做时间同步，激光雷达输出两帧图像点云数据之间的里程计实际就是100ms时间间隔之内IMU的位移平移和旋转量，位移平移就是将IMU输出的速度和加速度信息计算得到，旋转量是通过IMU输出的航向角确定的。

假设用Offx表示第一位移平移，Offy表示第二位移平移，θ表示旋转量，则通过下列公式计算得到：

其中，V _x ，V _y 分别表示一组IMU数据中的加速度和速度，T为IMU数据测量时间间隔（即输出相邻两组IMU数据时的时间间隔)，yaw是一个IMU数据集中IMU最后一次输出的航向角（可以根据IMU数据的时间戳确定)，T=10ms。

例如，一个IMU数据集包含10组IMU数据（30个IMU数据）：

{(V _x 1，V _y 1，yaw1)；(V _x 2，V _y 2，yaw2)；…；(V _x 10，V _y 10，yaw10)}。

其中(V _x 1，V _y 1，yaw1)的时间戳为t1，(V _x 2，V _y 2，yaw2)的时间戳为t2，…，(V _x 10，V _y 10，yaw10)的时间戳为t10，且t1<t2<…<t10，则(V _x 10，V _y 10，yaw10)即这一IMU数据集中最后一次测量输出的IMU数据，最后一次输出的航向角即yaw10，T=t2-t1=t3-t2=t4-t3=…=t10-t9。

则Offx=（V _x 1+V _x 2+V _x 3+…+V _x 10）*T；Offy=（V _y 1+V _y 2+V _y 3+…+V _y 10）*T；θ=yaw10。

在本发明实施例中，每一帧激光雷达都保存对应的位移平移和旋转量作为当前的里程计，用RT矩阵表示，其中R表示旋转矩阵，T表示平移矩阵，在本发明实施例中，对第N帧图像中的点云数据进行坐标转换时，可以先旋转再平移，需要保证的是，连续N帧图像的坐标旋转过程保持一致，不同的是，不同帧图像对应不同的点云数据以及不同的RT矩阵。

例如，对第N帧图像中的点云数据进行坐标转换实质也是对每一个点的坐标进行转换，假设第N帧图像中某一点P（x，y，z），则对P进行旋转变换时的方式有很多种，以绕z轴旋转为例，将点P（x，y，z）绕z轴旋转θ角得到点P′(x′，y′，z′)，由于是绕z轴进行的旋转，因此z坐标保持不变，y和x组成的yox（O是坐标原点）平面上进行的是一个二维的旋转，于是有：

x′= ysinθ+xcosθ；y′=ycosθ−xsinθ；z′=z。

用列向量表示为P=[x；y；z；1]，P′=[x′；y′；z′；1]，则RP=P′，其中R为4*4矩阵，且R矩阵是根据旋转量θ确定的，写成4*4矩阵的形式为：R=[cosθ，sinθ，0，0；-sinθ，cosθ，0，0；0，0，1，0；0，0，0，1]。

在本发明实施例中，平移矩阵T是根据位移平移确定的，假设第N帧图像中某一点Q（x，y，z），则对Q进行平移变换后得到Q′（x′，y′，z′），三个坐标轴的移动分量分别为dx=Offx，dy=Offy，dz=0，则：

x′= x + Offx；y′= y + Offy；z′= z 。

用列向量表示为Q=[x；y；z；1]，Q′=[x′；y′；z′；1]，则TQ=Q′，其中T=[1，0，0，Offx；0，1，0，Offy；0，0，1，0；0，0，0，1]。

在本发明实施例中，对点P先旋转后平移可以表示为：T*R*P。

上述方法，根据IMU数据确定里程计信息，其中IMU数据可以提供车辆的位姿信息，并且IMU数据都处于非失锁状态，此外测量精度可达厘米级提高了构建点云地图的精度。

计算方式二、若确定第N帧图像对应的IMU数据集中的至少一个IMU数据处于失锁状态，则根据第N帧图像中的点云数据确定第N帧图像对应的里程计信息。

具体的，确定所述第N帧图像中的点云数据对应的点的曲面特征，其中所述曲面特征表示所述第N帧图像中的任意一个点与所述任意一个点周围最近邻K个点欧式距离的和，K为正整数；根据所述点的曲面特征通过非极大值抑制对所述第N帧图像中的点云数据对应的点进行筛选，并将筛选后剩余的点对应的点云数据作为最佳点云数据；通过迭代最近点ICP算法对所述最佳点云数据进行点云匹配确定第N-1帧图像中与所述最佳点云数据匹配的点云数据；根据所述第N帧图像中的最佳点云数据以及所述第N-1帧图像中与所述最佳点云数据匹配的点云数据确定RT矩阵，并将所述RT矩阵作为所述里程计信息。

如图2所示，其中在计算点A的曲面特征时，首先确定与点A最近邻的五个点，分别为图X中的点：F、A、E、B、C，则计算这5个点与点A的欧式距离的和，将这5个点与点A的欧式距离的和作为点A的曲面特征，通过非极大值抑制的方式对第N帧图像中的点云数据相对应的点进行筛选得到曲面特征较大的边缘点（例如位于路边的点）以及曲面特征较小的平面点（例如位于路面的点），例如通过激光雷达扫描得到的第N帧图像中有1000个点，通过筛选后剩余100个点，用非极大值抑制来保证采样点的均匀性，这样可以大大减小ICP的计算量；将这100个点相对应的点云数据作为第N帧图像中的最佳点云数据，之后进行特征匹配，采用的是scan-to-scan的匹配，将这100个最佳点云数据作为一个点集，通过ICP算法进行配准，点集配准采用法方向最近邻点来进行配准，搜索第N-1帧图像中与第N帧图像筛选得到的这100个点相匹配的100个点，即配准点集，其中N为不小于2的正整数。

在确定RT矩阵时，采用罗德里杰斯公式将旋转矩阵R展开然后求导，求解雅克比矩阵，最后使用LM（Levenberg-Marquardt，列文伯格-马夸尔特法)的非线性方法进行优化得到RT矩阵。其中LM是使用最广泛的非线性最小二乘算法，利用梯度求最大(小)值的算法，形象的说，属于“爬山”法的一种。它同时具有梯度法和牛顿法的优点。当λ很小时，步长等于牛顿法步长，当λ很大时，步长约等于梯度下降法的步长。

在求解RT矩阵时，将旋转量θ以及位移平移Offx、Offy作为变量，旋转平移前的坐标为第N-1帧图像中点云配准得到的100个点的坐标，旋转平移后的坐标即第N帧图像中的最佳点云数据（点云数据即点的坐标），通过优化问题求解的方式，找到最优的θ，以及Offx、Offy，例如采用计算方式一中的4*4的R矩阵以及T矩阵（区别在于计算方式二中的θ、Offx、Offy为变量，不是定量，需要通过罗德里杰斯公式、LM算法等求解）最后确定RT矩阵。

上述方法，在进行当前帧和上一帧的点云特征匹配时，基于曲面特征的采样使用一些具有明显特征的点集来进行配准，大量减少了对应点的数目，包括边缘点特征、平面点特征，采用ICP匹配方法，最后将求得的RT矩阵作为里程计信息。

需要说明的是，本发明实施例中所列举的根据IMU数据集和/或点云数据计算里程计信息的方式只是举例说明，任何一种根据IMU数据集和/或点云数据计算里程计信息的方式都适用于本发明实施例。

在本发明实施例中，确定里程计信息后，进行坐标转换的具体方式为：根据连续M帧图像中的最后一帧图像以及所述M帧图像对应的所述里程计信息对所述连续M帧图像中的点云数据进行坐标转换后得到所述连续M帧图像的点云地图坐标，其中M为正整数；即将连续M帧图像的点云地图坐标进行统一映射，连续M帧图像生成一次点云地图。在进行坐标转换时，以激光雷达在连续M帧图像中的最后一帧图像中的位置为坐标原点，根据M帧图像对应的里程计信息对连续M帧图像中的点云数据进行坐标转换。

例如，根据连续5帧图像的点云地图坐标确定点云地图时，则对第1~5帧图像中的点云数据进行坐标转换得到点云地图坐标后统一进行一次映射，并且映射到统一坐标系中，根据映射得到的坐标系生成点云地图。

具体的，根据连续5帧图像（第1帧、第2帧、第3帧、第4帧、第5帧）确定点云地图，以激光雷达在第5帧图像中的位置为坐标原点，根据每一帧图像对应的里程计信息对这连续5帧图像的点云数据进行坐标转换，例如，第1帧图像对应的里程计信息为RT1、第2帧图像对应的里程计信息为RT2、第3帧图像对应的里程计信息为RT3、第4帧图像对应的里程计信息为RT4、第5帧图像对应的里程计信息为RT5，则以激光雷达在第5帧图像中的位置为坐标原点，根据RT1对第1帧图像中的点云数据进行坐标转换，根据RT2对第2帧图像中的点云数据进行坐标转换，根据RT3对第3帧图像中的点云数据进行坐标转换，根据RT4对第4帧图像中的点云数据进行坐标转换，根据RT5对第5帧图像中的点云数据进行坐标转换。

如图3所示，为激光雷达扫描的第5帧图像，扫描得到第5帧图像是构建的是如图所示坐标系，坐标原点为O，其中x轴、y轴、z轴表示如图3所示方向，其中点P坐标为（x，y，z），其中激光雷达在第5帧图像中的位置为图3中Q点位置处，进行坐标转换后，绕z轴旋转角度为90度，且y、x方向有平移，旋转平移后坐标原点为O′（Q点），如图4所示，点P坐标为（x′，y′，z′），通过坐标转换可以使得激光雷达在图像中的位置为坐标原点，建立相对坐标系。

可选的，在本发明实施例中，可以对点云地图进行动态更新。

具体的，按照每帧图像的生成顺序，根据每连续M帧图像的点云地图坐标确定一次点云地图，其中M为正整数；其中任意相邻两次确定的点云地图中，后一次确定的点云地图使用的连续M帧图像中的第一帧图像为前一次确定的点云地图使用的连续M帧图像中的第二帧图像。

例如，采用上述坐标转换的方法，每映射一次坐标（对连续13帧图像的坐标进行统一映射），确定一次点云地图（例如以1~13帧图像生成了一次点云地图），之后使用图像累计加1帧（第14帧图像），判断使用的图像是否达到13帧，如果是，则把第1帧图像中的点云数据删除，把第2~14帧图像中的点云数据进行坐标转换得到的点云地图坐标进行映射；否则的话，则把当前帧图像中的点云数据进行坐标转换得到点云地图坐标，图像帧数累计加1，继续进行下一帧的坐标转换。如图5所示，在第一次确定点云地图时使用的是第1~13帧图像的点云地图坐标，在第二次确定点云地图时使用的是第2~14帧图像的点云地图坐标，在第三次确定点云地图时使用的是第3~15帧图像的点云地图坐标，实现了一种渐进式更新的局部点云地图，并且仅存储M帧图像数据，降低对数据存储的要求。

如图6所示，本发明实施例提供的一种构建点云地图的完整方法包括：

步骤600、接收激光雷达扫描车辆周边道路确定的第N帧图像以及IMU同时测量得到的第N帧图像对应的IMU数据集；

步骤601、判断第N帧图像对应的IMU数据集中的所有IMU数据是否都处于失锁状态，如果是，则执行步骤603，否则执行步骤602；

步骤602、根据第N帧图像对应的IMU数据集确定第N帧图像对应的用于表示车辆行驶状态的里程计信息；

步骤603、根据第N帧图像中的点云数据确定第N帧图像对应的里程计信息；

步骤604、根据连续M帧图像中的最后一帧图像以及所述M帧图像对应的所述里程计信息对所述连续M帧图像中的点云数据进行坐标转换后得到所述连续M帧图像的点云地图坐标；

步骤605、根据连续M帧图像的点云地图坐标确定点云地图，其中任意相邻两次确定的点云地图中，后一次确定的点云地图使用的连续M帧图像中的第一帧图像为前一次确定的点云地图使用的连续M帧图像中的第二帧图像。

基于相同的发明构思，本发明实施例中还提供了一种构建点云地图的设备，由于该设备即是本发明实施例中的方法中的设备，并且该设备解决问题的原理与该方法相似，因此该设备的实施可以参见方法的实施，重复之处不再赘述。

如图7所示，本发明实施例还提供一种构建点云地图的设备，该设备包括：至少一个处理单元700、以及至少一个存储单元701，其中，所述存储单元701存储有程序代码，当所述程序代码被所述处理单元700执行时，使得设备执行下列过程：

确定通过激光雷达扫描车辆周边道路确定的第N帧图像对应的用于表示车辆行驶状态的里程计信息，其中N为正整数；

根据连续M帧图像中的最后一帧图像以及所述M帧图像对应的所述里程计信息对所述连续M帧图像中的点云数据进行坐标转换后得到所述连续M帧图像的点云地图坐标，其中M为正整数；

根据所述连续M帧图像的点云地图坐标确定点云地图，其中任意相邻两次确定的点云地图中，后一次确定的点云地图使用的连续M帧图像中的第一帧图像为前一次确定的点云地图使用的连续M帧图像中的第二帧图像。

可选的，所述处理单元700还用于通过下列方式确定所述第N帧图像对应的用于表示车辆行驶状态的里程计信息：

若所述第N帧图像对应的惯性测量单元IMU数据集中的所有IMU数据都处于非失锁状态，则根据所述第N帧图像对应的IMU数据集确定所述第N帧图像对应的所述里程计信息，其中所述非失锁状态表示测量得到所述IMU数据集时IMU数据的状态；或

若所述第N帧图像对应的IMU数据集中的至少一个IMU数据处于失锁状态，则根据所述第N帧图像中的点云数据确定所述第N帧图像对应的所述里程计信息；

其中，所述第N帧图像对应的IMU数据集是在确定所述第N帧图像的同时测量得到的。

可选的，所述处理单元700具体用于：

确定所述第N帧图像中的点云数据对应的点的曲面特征，其中所述曲面特征表示所述第N帧图像中的任意一个点与所述任意一个点周围最近邻K个点欧式距离的和，K为正整数；

根据所述点的曲面特征通过非极大值抑制对所述第N帧图像中的点云数据对应的点进行筛选，并将筛选后剩余的点对应的点云数据作为最佳点云数据；

通过ICP算法对所述最佳点云数据进行点云匹配确定第N-1帧图像中与所述最佳点云数据匹配的点云数据；

根据所述第N帧图像中的最佳点云数据以及所述第N-1帧图像中与所述最佳点云数据匹配的点云数据确定RT矩阵，并将所述RT矩阵作为所述里程计信息。

可选的，所述处理单元700具体用于：

根据第N帧图像对应的第一位移平移、第二位移平移及旋转量确定RT矩阵，并将所述RT矩阵作为所述里程计信息；

其中，所述第一位移平移为所述第N帧图像对应的IMU数据集中所有加速度的和与所述IMU数据测量时间间隔的乘积，所述第二位移平移为所述第N帧图像对应的IMU数据集中所有速度的和与所述IMU数据测量时间间隔的乘积，所述旋转量为根据所述第N帧图像对应的IMU数据集中的IMU数据的时间戳确定的最后一次测量得到的航向角。

可选的，所述处理单元700具体用于：

以激光雷达在所述连续M帧图像中的最后一帧图像中的位置为坐标原点，根据所述M帧图像对应的所述里程计信息对所述连续M帧图像中的点云数据进行坐标转换。

基于相同的发明构思，本发明实施例中还提供了一种构建点云地图的设备，由于该设备即是本发明实施例中的方法中的设备，并且该设备解决问题的原理与该方法相似，因此该设备的实施可以参见方法的实施，重复之处不再赘述。

如图8所示，本发明实施例还提供一种构建点云地图的设备，该设备包括：第一确定模块800、坐标转换模块801和第二确定模块802。

第一确定模块800：用于确定通过激光雷达扫描车辆周边道路确定的第N帧图像对应的用于表示车辆行驶状态的里程计信息，其中N为正整数；

坐标转换模块801：用于根据连续M帧图像中的最后一帧图像以及所述M帧图像对应的所述里程计信息对所述连续M帧图像中的点云数据进行坐标转换后得到所述连续M帧图像的点云地图坐标，其中M为正整数；

第二确定模块802：用于根据所述连续M帧图像的点云地图坐标确定点云地图，其中任意相邻两次确定的点云地图中，后一次确定的点云地图使用的连续M帧图像中的第一帧图像为前一次确定的点云地图使用的连续M帧图像中的第二帧图像。

可选的，所述第一确定模块800还用于通过下列方式确定所述第N帧图像对应的用于表示车辆行驶状态的里程计信息：

若所述第N帧图像对应的惯性测量单元IMU数据集中的所有IMU数据都处于非失锁状态，则根据所述第N帧图像对应的IMU数据集确定所述第N帧图像对应的所述里程计信息，其中所述非失锁状态表示测量得到所述IMU数据集时IMU数据的状态；或

若所述第N帧图像对应的IMU数据集中的至少一个IMU数据处于失锁状态，则根据所述第N帧图像中的点云数据确定所述第N帧图像对应的所述里程计信息；

其中，所述第N帧图像对应的IMU数据集是在确定所述第N帧图像的同时测量得到的。

可选的，所述第一确定模块800具体用于：

确定所述第N帧图像中的点云数据对应的点的曲面特征，其中所述曲面特征表示所述第N帧图像中的任意一个点与所述任意一个点周围最近邻K个点欧式距离的和，K为正整数；

根据所述点的曲面特征通过非极大值抑制对所述第N帧图像中的点云数据对应的点进行筛选，并将筛选后剩余的点对应的点云数据作为最佳点云数据；

通过迭代最近点ICP算法对所述最佳点云数据进行点云匹配确定第N-1帧图像中与所述最佳点云数据匹配的点云数据；

根据所述第N帧图像中的最佳点云数据以及所述第N-1帧图像中与所述最佳点云数据匹配的点云数据确定RT矩阵，并将所述RT矩阵作为所述里程计信息。

可选的，所述第一确定模块800具体用于：

根据第N帧图像对应的第一位移平移、第二位移平移及旋转量确定RT矩阵，并将所述RT矩阵作为所述里程计信息；

其中，所述第一位移平移为所述第N帧图像对应的IMU数据集中所有加速度的和与所述IMU数据测量时间间隔的乘积，所述第二位移平移为所述第N帧图像对应的IMU数据集中所有速度的和与所述IMU数据测量时间间隔的乘积，所述旋转量为根据所述第N帧图像对应的IMU数据集中的IMU数据的时间戳确定的最后一次测量得到的航向角。

可选的，所述坐标转换模块801具体用于：

以激光雷达在所述连续M帧图像中的最后一帧图像中的位置为坐标原点，根据所述M帧图像对应的所述里程计信息对所述连续M帧图像中的点云数据进行坐标转换。

本发明实施例还提供一种计算机可读非易失性存储介质，包括程序代码，当所述程序代码在计算终端上运行时，所述程序代码用于使所述计算终端执行上述本发明实施例构建点云地图的方法的步骤。

以上参照示出根据本申请实施例的方法、装置（系统)和/或计算机程序产品的框图和/或流程图描述本申请。应理解，可以通过计算机程序指令来实现框图和/或流程图示图的一个块以及框图和/或流程图示图的块的组合。可以将这些计算机程序指令提供给通用计算机、专用计算机的处理器和/或其它可编程数据处理装置，以产生机器，使得经由计算机处理器和/或其它可编程数据处理装置执行的指令创建用于实现框图和/或流程图块中所指定的功能/动作的方法。

相应地，还可以用硬件和/或软件（包括固件、驻留软件、微码等)来实施本申请。更进一步地，本申请可以采取计算机可使用或计算机可读存储介质上的计算机程序产品的形式，其具有在介质中实现的计算机可使用或计算机可读程序代码，以由指令执行系统来使用或结合指令执行系统而使用。在本申请上下文中，计算机可使用或计算机可读介质可以是任意介质，其可以包含、存储、通信、传输、或传送程序，以由指令执行系统、装置或设备使用，或结合指令执行系统、装置或设备使用。

显然，本领域的技术人员可以对本发明进行各种改动和变型而不脱离本发明的精神和范围。这样，倘若本发明的这些修改和变型属于本发明权利要求及其等同技术的范围之内，则本发明也意图包含这些改动和变型在内。

Claims (10)

1.一种构建点云地图的方法，其特征在于，该方法包括：

确定通过激光雷达扫描车辆周边道路确定的第N帧图像对应的用于表示车辆行驶状态的里程计信息，其中N为正整数；

根据连续M帧图像中的最后一帧图像以及所述M帧图像对应的所述里程计信息对所述连续M帧图像中的点云数据进行坐标转换后得到所述连续M帧图像的点云地图坐标，其中M为正整数；

根据所述连续M帧图像的点云地图坐标确定点云地图，其中任意相邻两次确定的点云地图中，后一次确定的点云地图使用的连续M帧图像中的第一帧图像为前一次确定的点云地图使用的连续M帧图像中的第二帧图像。

2.如权利要求1所述的方法，其特征在于，通过下列方式确定所述第N帧图像对应的用于表示车辆行驶状态的里程计信息：

若所述第N帧图像对应的惯性测量单元IMU数据集中的所有IMU数据都处于非失锁状态，则根据所述第N帧图像对应的IMU数据集确定所述第N帧图像对应的所述里程计信息，其中所述非失锁状态表示测量得到所述IMU数据集时IMU数据的状态；或

若所述第N帧图像对应的IMU数据集中的至少一个IMU数据处于失锁状态，则根据所述第N帧图像中的点云数据确定所述第N帧图像对应的所述里程计信息；

其中，所述第N帧图像对应的IMU数据集是在确定所述第N帧图像的同时测量得到的。

3.如权利要求2所述的方法，其特征在于，所述根据所述第N帧图像中的点云数据确定所述第N帧图像对应的所述里程计信息，包括：

确定所述第N帧图像中的点云数据对应的点的曲面特征，其中所述曲面特征表示所述第N帧图像中的任意一个点与所述任意一个点周围最近邻K个点欧式距离的和，K为正整数；

根据所述点的曲面特征通过非极大值抑制对所述第N帧图像中的点云数据对应的点进行筛选，并将筛选后剩余的点对应的点云数据作为最佳点云数据；

通过迭代最近点ICP算法对所述最佳点云数据进行点云匹配确定第N-1帧图像中与所述最佳点云数据匹配的点云数据；

根据所述第N帧图像中的最佳点云数据以及所述第N-1帧图像中与所述最佳点云数据匹配的点云数据确定旋转平移RT矩阵，并将所述RT矩阵作为所述里程计信息。

4.如权利要求2所述的方法，其特征在于，所述根据第N帧图像对应的IMU数据集确定所述第N帧图像对应的所述里程计信息，包括：

根据第N帧图像对应的第一位移平移、第二位移平移及旋转量确定RT矩阵，并将所述RT矩阵作为所述里程计信息；

其中，所述第一位移平移为所述第N帧图像对应的IMU数据集中所有加速度的和与所述IMU数据测量时间间隔的乘积，所述第二位移平移为所述第N帧图像对应的IMU数据集中所有速度的和与所述IMU数据测量时间间隔的乘积，所述旋转量为根据所述第N帧图像对应的IMU数据集中的IMU数据的时间戳确定的最后一次测量得到的航向角。

5.如权利要求1所述的方法，其特征在于，所述根据连续M帧图像中的最后一帧图像以及所述M帧图像对应的所述里程计信息对所述连续M帧图像中的点云数据进行坐标转换后得到所述连续M帧图像的点云地图坐标，包括：

以激光雷达在所述连续M帧图像中的最后一帧图像中的位置为坐标原点，根据所述M帧图像对应的所述里程计信息对所述连续M帧图像中的点云数据进行坐标转换。

6.一种构建点云地图的设备，其特征在于，该设备包括：至少一个处理单元以及至少一个存储单元，其中，所述存储单元存储有程序代码，当所述程序代码被所述处理单元执行时，使得所述存储单元执行下列过程：

确定通过激光雷达扫描车辆周边道路确定的第N帧图像对应的用于表示车辆行驶状态的里程计信息，其中N为正整数；

根据连续M帧图像中的最后一帧图像以及所述M帧图像对应的所述里程计信息对所述连续M帧图像中的点云数据进行坐标转换后得到所述连续M帧图像的点云地图坐标，其中M为正整数；

根据所述连续M帧图像的点云地图坐标确定点云地图，其中任意相邻两次确定的点云地图中，后一次确定的点云地图使用的连续M帧图像中的第一帧图像为前一次确定的点云地图使用的连续M帧图像中的第二帧图像。

7.如权利要求6所述的设备，其特征在于，所述处理单元还用于通过下列方式确定所述第N帧图像对应的用于表示车辆行驶状态的里程计信息：

若所述第N帧图像对应的惯性测量单元IMU数据集中的所有IMU数据都处于非失锁状态，则根据所述第N帧图像对应的IMU数据集确定所述第N帧图像对应的所述里程计信息，其中所述非失锁状态表示测量得到所述IMU数据集时IMU数据的状态；或

若所述第N帧图像对应的IMU数据集中的至少一个IMU数据处于失锁状态，则根据所述第N帧图像中的点云数据确定所述第N帧图像对应的所述里程计信息；

其中，所述第N帧图像对应的IMU数据集是在确定所述第N帧图像的同时测量得到的。

8.如权利要求7所述的设备，其特征在于，所述处理单元具体用于：

确定所述第N帧图像中的点云数据对应的点的曲面特征，其中所述曲面特征表示所述第N帧图像中的任意一个点与所述任意一个点周围最近邻K个点欧式距离的和，K为正整数；

根据所述点的曲面特征通过非极大值抑制对所述第N帧图像中的点云数据对应的点进行筛选，并将筛选后剩余的点对应的点云数据作为最佳点云数据；

通过ICP算法对所述最佳点云数据进行点云匹配确定第N-1帧图像中与所述最佳点云数据匹配的点云数据；

根据所述第N帧图像中的最佳点云数据以及所述第N-1帧图像中与所述最佳点云数据匹配的点云数据确定RT矩阵，并将所述RT矩阵作为所述里程计信息。

9.如权利要求7所述的设备，其特征在于，所述处理单元具体用于：

根据第N帧图像对应的第一位移平移、第二位移平移及旋转量确定RT矩阵，并将所述RT矩阵作为所述里程计信息；

其中，所述第一位移平移为所述第N帧图像对应的IMU数据集中所有加速度的和与所述IMU数据测量时间间隔的乘积，所述第二位移平移为所述第N帧图像对应的IMU数据集中所有速度的和与所述IMU数据测量时间间隔的乘积，所述旋转量为根据所述第N帧图像对应的IMU数据集中的IMU数据的时间戳确定的最后一次测量得到的航向角。

10.如权利要求6所述的设备，其特征在于，所述处理单元具体用于：

以激光雷达在所述连续M帧图像中的最后一帧图像中的位置为坐标原点，根据所述M帧图像对应的所述里程计信息对所述连续M帧图像中的点云数据进行坐标转换。