复杂环境下的高精度地图构建方法、装置和计算机设备

本申请涉及地图生成技术领域，特别是涉及一种复杂环境下的高精度地图构建方法、装置和计算机设备。

背景技术：

近年来，无人驾驶技术取得了飞速的进步。其中，高精度地图的构建和应用起到非常重要的作用。与传统地图相比，高精度地图不仅具有更高的准确性，而且能够提供更丰富的环境信息，为导航、规划等模块提供更大的帮助。

高精度地图需要同时满足全局准确性和局部准确性。在全局层面，地图要与全局坐标系统一致；在局部层面，地图需要完全捕获周围环境的局部信息，地图精度应达到厘米级。为了满足这两个要求，地图构建系统应将全局导航方法与局部导航方法相结合。其中，全局导航信息可以从全球卫星定位系统获得(如北斗，gnss，gps等)，局部导航信息可以从imu、车轮编码器以及雷达里程计等途径获得。但是，在实际应用过程中，上述导航信息都可能出现退化的情况，因此导致构建的地图精确度不够高。

技术实现要素：

基于此，有必要针对上述技术问题，提供一种能够解决地图精确度不够高的复杂环境下的高精度地图构建方法、装置和计算机设备。

一种复杂环境下的高精度地图构建方法，其特征在于，所述方法包括：

获取激光点云数据，对所述激光点云数据进行预处理，得到点云匹配因子和预处理点云数据；

获取imu导航数据以及gnss导航数据，根据所述imu导航数据以及gnss导航数据，分别得到imu边观测因子和gnss点观测因子；

对所述点云匹配因子、所述imu边观测因子和所述gnss点观测因子进行退化性分析，基于退化性分析结果设置表征不确定性的信息矩阵并构建因子图；

对所述因子图进行优化，得到激光点云数据中每个关键帧的六自由度位姿信息，根据所述六自由度位姿信息对所述预处理点云数据进行拼接，得到高精度地图。

在其中一个实施例中，还包括：根据预设的位移量，确定所述激光点云数据中的关键帧；采用点云帧内补偿算法对所述激光点云数据中由车体自运动形成的点云畸变进行处理，以及对所述激光点云数据中动态场景的点云噪声进行处理。

在其中一个实施例中，确定复杂环境的环境类型，根据环境类型确定退化分析类型；获取退化分析的退化指标，以及获取六自由度对角矩阵；根据所述退化指标、所述六自由度对角矩阵以及所述退化分析类型，得到点云匹配因子的第一信息矩阵。

在其中一个实施例中，根据gnss导航数据对应的输出定位方差和可观测卫星数目，确定gnss点观测因子对应的第二信息矩阵；采用运动模型估计方式进行量化分析，得到imu边观测因子的第三信息矩阵；根据所述第一信息矩阵、所述第二信息矩阵以及所述第三信息矩阵，构建因子图。

在其中一个实施例中，高精度地图为二维高精度地图或三维高精度地图。

一种复杂环境下的高精度地图构建装置，所述装置包括：

数据获取模块，用于获取激光点云数据，对所述激光点云数据进行预处理，得到点云匹配因子和预处理点云数据；

因子提取模块，用于获取imu导航数据以及gnss导航数据，根据所述imu导航数据以及gnss导航数据，分别得到imu边观测因子和gnss点观测因子；

因子图构建模块，用于对所述点云匹配因子、所述imu边观测因子和所述gnss点观测因子进行退化性分析，基于退化性分析结果设置表征不确定性的信息矩阵并构建因子图；

地图生成模块，用于对所述因子图进行优化，得到激光点云数据中每个关键帧的六自由度位姿信息，根据所述六自由度位姿信息对所述预处理点云数据进行拼接，得到高精度地图。

一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，所述处理器执行所述计算机程序时实现以下步骤：

获取激光点云数据，对所述激光点云数据进行预处理，得到点云匹配因子和预处理点云数据；

获取imu导航数据以及gnss导航数据，根据所述imu导航数据以及gnss导航数据，分别得到imu边观测因子和gnss点观测因子；

对所述点云匹配因子、所述imu边观测因子和所述gnss点观测因子进行退化性分析，基于退化性分析结果设置表征不确定性的信息矩阵并构建因子图；

对所述因子图进行优化，得到激光点云数据中每个关键帧的六自由度位姿信息，根据所述六自由度位姿信息对所述预处理点云数据进行拼接，得到高精度地图。

一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现以下步骤：

获取激光点云数据，对所述激光点云数据进行预处理，得到点云匹配因子和预处理点云数据；

获取imu导航数据以及gnss导航数据，根据所述imu导航数据以及gnss导航数据，分别得到imu边观测因子和gnss点观测因子；

对所述点云匹配因子、所述imu边观测因子和所述gnss点观测因子进行退化性分析，基于退化性分析结果设置表征不确定性的信息矩阵并构建因子图；

对所述因子图进行优化，得到激光点云数据中每个关键帧的六自由度位姿信息，根据所述六自由度位姿信息对所述预处理点云数据进行拼接，得到高精度地图。

上述复杂环境下的高精度地图构建方法、装置、计算机设备和存储介质，因子图采用概率框架，可以有效地处理不同导航源信息的不确定性，并且能够以灵活的方式整合不同的因子。我们全面分析了每种因子的状态，并且针对不同因子提出对应的退化状态估计方法。然后，根据退化程度调整因子对应的信息矩阵：具有较高置信度的因子获得更高的权重，在优化过程中不易被调整；置信度较低的因子在很大程度上被削弱，从而大大的提高了地图构建的精度。

附图说明

图1为一个实施例中复杂环境下的高精度地图构建方法的示意性流程图；

图2为一个实施例中位姿图优化框架的示意图；

图3为一个实施例中高精度建图结果示意图；其中，(a)表示三维地图，(b)表示二维障碍地图，(c)表示二维反射率地图；

图4为一个实施例中复杂环境下的高精度地图构建装置的结构框图；

图5为一个实施例中计算机设备的内部结构图。

具体实施方式

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

在一个实施例中，如图1所示，提供了一种复杂环境下的高精度地图构建方法，包括以下步骤：

步骤102，获取激光点云数据，对激光点云数据进行预处理，得到点云匹配因子和预处理点云数据。

激光点云数据是激光雷达获取得到的数据，通过对激光点云数据的预处理，即可以得到预处理点云数据，也可以通过帧内和帧间的信息，得到点云匹配因子。

步骤104，获取imu导航数据以及gnss导航数据，根据imu导航数据以及gnss导航数据，分别得到imu边观测因子和gnss点观测因子。

步骤106，对点云匹配因子、imu边观测因子和gnss点观测因子进行退化性分析，基于退化性分析结果设置表征不确定性的信息矩阵并构建因子图。

步骤108，对因子图进行优化，得到激光点云数据中每个关键帧的六自由度位姿信息，根据六自由度位姿信息对预处理点云数据进行拼接，得到高精度地图。

上述复杂环境下的高精度地图构建方法中，因子图采用概率框架，可以有效地处理不同导航源信息的不确定性，并且能够以灵活的方式整合不同的因子。我们全面分析了每种因子的状态，并且针对不同因子提出对应的退化状态估计方法。然后，根据退化程度调整因子对应的信息矩阵：具有较高置信度的因子获得更高的权重，在优化过程中不易被调整；置信度较低的因子在很大程度上被削弱，从而大大的提高了地图构建的精度。

在其中一个实施例中，在本发明中应用的位姿图优化框架进行解释，在该位姿图优化框架中，不同于传统的因子，本发明是对各个因子进行退化分析后，具有一定的不确定性的因子，具体过程如下：

该框架输入关键帧位姿节点信息、由传感器获得的关联位姿节点的约束以及每个约束对应的信息矩阵(也称为置信度矩阵)，输出优化后的位姿，本发明中采用的约束包括以下三类：

(1)由gnss、gps或北斗卫星导航系统提供的全局观测约束(也称为点约束)，记为zk。点约束可以通过传感器直接获得，在差分状态良好的情况下，gnss可以提供厘米级的观测精度。

(2)局部相邻帧约束(也称短边约束)，记为局部边约束又可以分为三种，第一种是由码盘/imu递推得到局部车体运动估计，记为第二种是由相邻帧点云之间的匹配(scan-to-scan)获得的位姿约束，记为第三种是由当前帧点云与邻近子图之间的匹配(scan-to-submap)获得的位姿约束，记为

(3)闭环约束，采用时间相距较远且空间邻近的点云之间的匹配获得的位姿约束(也称长边约束)，记为

假设图2中的约束之间相互独立，基于贝叶斯推理，全概率的表达式为：

其中，x代表待优化的节点位姿，z代表全局观测约束，e代表局部相邻帧约束和闭环约束，进一步的，对应的状态转移矩阵可以描述为：

其中，xi和xj表示节点位姿，表示用于连接两个节点约束，表示状态转移过程函数，表示的噪声，若为零均值高斯噪声，其协方差矩阵表示为指约束类型，p(zk|xk)对应的观测模型为：

zk＝hk(xk)+wk

其中，zk代表gnss/ins观测到的关联xk的全局约束，hk代表观测方程，wk为对应的观测噪声，设置为协方差为λk的零均值噪声。

因此，结合上式，可以将位姿图优化问题转化为最小二乘问题为：

其中，将λ-1/2简记为ω，ωe和ωz分别表示边观测约束和点观测约束的信息矩阵。ω表征了因子的置信度水平，可以通过标准差的逆矩阵求解。为了简化计算，ω通常表示为6×6的对角阵，换言之，节点位姿x的具有独立的六个自由度分量为横向位置、纵向位置、高度、横滚角、俯仰角和偏航角。

基于以上实施例的说明，本发明采用的是表征不确定性的信息矩阵并构建因子图，因此可以提高建图的鲁棒性。

对于步骤102，在其中一个实施例中，对所述激光点云数据进行预处理，包括：根据预设的位移量，确定所述激光点云数据中的关键帧；采用点云帧内补偿算法对激光点云数据中运动形成的点云畸变进行处理，以及对激光点云数据中动态场景的点云噪声进行处理。

对于步骤106，在其中一个实施例中，对点云匹配因子、imu边观测因子和gnss点观测因子进行退化性分析，包括：确定复杂环境的环境类型，根据环境类型确定退化分析类型；获取退化分析的退化指标，以及获取六自由度对角矩阵；根据退化指标、六自由度对角矩阵以及退化分析类型，得到点云匹配因子的第一信息矩阵。

本发明中，在得到激光点云数据，扫描匹配的目的是估计目标点云p和参考点云q之间的相对变换t。根据参考点云q的不同类型，扫描匹配因子可分为如下三种形式：当q是相邻的关键帧点云，该因子为帧到帧(scan-to-scan)匹配因子；当q是相邻的局部子图，该因子为帧到子图(scan-to-subscan)匹配因子；当q是重新访问的历史点云，该因子为闭环(loop-closure)匹配因子。点云匹配约束可以通过icp、ndt、csm以及loam等扫描匹配算法求解，为了兼顾匹配算法的效率和精度，采用基于loam的点云匹配方法。该方法首先从p和q中提取特征点，然后分别将边缘特征和平面特征与对应的直线和平面进行关联，构建目标函数，最后通过迭代求解最优变换矩阵。

匹配算法的优化目标是求解最优变换矩阵：

t＝[r|t]

使得以下能量函数最小：

其中，se和sp分别表示成功关联的边缘特征点集合和平面特征点集合，di表示特征点到直线的距离，dh表示特征点到平面的距离。

最优匹配t求解可以表示为：

由此将点云配准问题转化为非线性优化问题，采用levenberg-marquardt方法迭代求解：

δ＝-(j^tj+λdiag(j^tj))^-1j^td

t←t+δ

其中，为nf×6的雅各比矩阵，nf为成功关联的特征点数目，d为nf×1的矩阵，上述优化求解过程以两个线程同时进行，第一个线程以高频模式求解相邻帧的变换矩阵(scan-to-scan)；第二个线程以低频模式求解当前帧到累积子图的变换矩阵(scan-to-submap)。前者保证了算法的实时性，后者提高了匹配的精度。

在进行退化性分析时，j^tj的最小特征值可以表征为匹配的退化程度，该退化指标被定义为：

d＝λm+1

其中，λm表示j^tj的最小特征值。

在本发明中，应用于多种匹配算法，且充分考虑了匹配过程的迭代情况，该方法的思路是：统计更新向量δ在优化过程中的振荡情况，对于退化情况，δ的波动较大，反之波动较小，因此记录每次迭代的更新向量δk，统计其协方差矩阵∑δ，∑δ的求解方式如下：

统计n次迭代情况，计算其协方差矩阵如下：

δ＝[δ1,δ2,···,δn]

∑δ＝δ·δ^t

其中，表示第k次迭代过程中六自由度位姿更新量，∑δ可以简写为对角矩阵：

对于步骤106，在其中一个实施例中，基于退化性分析结果设置表征不确定性的信息矩阵并构建因子图，包括：根据gnss导航数据对应的输出定位方差和可观测卫星数目，确定gnss点观测因子对应的第二信息矩阵；采用运动模型估计方式进行量化分析，得到imu边观测因子的第三信息矩阵；根据第一信息矩阵、第二信息矩阵以及第三信息矩阵，构建因子图。

具体的，当其中一个自由度出现退化时，∑δ′中的分量会增大，因此也容易被整合到图优化框架中，再次基础上，将d与∑δ′结合，实现自动调节的，具体第一信息矩阵如下：

其中，当d≥dt时，s(x)＝1，反之，s(x)＝-1，dt为阈值。

具体的，对于40线和64线激光雷达，dt可以设置为300，对于16线激光雷达，dt可以设置为200，r1和r2可以分别设置为0.75和0.25，ξe为归一化函数，c∈{1,2,3}。

但是，本发明的具体设置不限制于此，还与环境类型有关，例如在越野场景下，为了提高点云匹配精度和鲁棒性，采用loam和csm相结合的方式，得到第一信息矩阵为：

sc表示由csm得到的匹配得分值。

对于第二信息矩阵，在其中一个实施例中，的全局观测信息为gnss/ins组合系统输出的6自由度全局位姿，其观测方程已知。然而，gnss/ins容易被高楼和树木遮挡信号，导致定位精度降低。除了6自由度全局位姿外，gnss/ins组合设备同时输出定位方差和可观测的卫星数目，输出定位方差和可观测的卫星数目与定位精度相关，因此，计算得到的第二信息矩阵如下：

其中，为预先设置好的阈值，例如取值可以是10，ξz是归一化函数。i为6×6对角矩阵，定义如下：

i＝diag(ηt,ηt,ηt,ηr,ηr,ηr)

其中，ηt表示平移权重，ηr表示旋转权重，二者满足ηr＝5·ηt。

经过因子图优化后的位姿x^*将预处理后的点云拼接成全局一致地图。优化后的位姿输出是三维的，因此，该系统可以同时生成高精度的三维地图和二维灰度图。为了提高地图的存取效率和方便地图更新，将整个地图依据全局位置划分成小块，每个小块采用单独的文件存储。对于三维高精度地图，采用八叉树的结构存储数据，并通过二进制流实现对八叉树的编码，有效地降低了地图存储的占用空间。对于二维灰度图，采用二维占据栅格的存储结构，根据栅格内存放的数据，可以分为二维高度地图(高程信息)和二维反射率地图(反射强度信息)，如图3所示。

应该理解的是，虽然图1流程图中的各个步骤按照箭头的指示依次显示，但是这些步骤并不是必然按照箭头指示的顺序依次执行。除非本文中有明确的说明，这些步骤的执行并没有严格的顺序限制，这些步骤可以以其它的顺序执行。而且，图1的至少一部分步骤可以包括多个子步骤或者多个阶段，这些子步骤或者阶段并不必然是在同一时刻执行完成，而是可以在不同的时刻执行，这些子步骤或者阶段的执行顺序也不必然是依次进行，而是可以与其它步骤或者其它步骤的子步骤或者阶段的至少一部分轮流或者交替地执行。

在一个实施例中，如图4所示，提供了一种复杂环境下的高精度地图构建装置，包括：数据获取模块402、因子提取模块404、因子图构建模块406和地图生成模块408，其中：

数据获取模块402，用于获取激光点云数据，对所述激光点云数据进行预处理，得到点云匹配因子和预处理点云数据；

因子提取模块404，用于获取imu导航数据以及gnss导航数据，根据所述imu导航数据以及gnss导航数据，分别得到imu边观测因子和gnss点观测因子；

因子图构建模块406，用于对所述点云匹配因子、所述imu边观测因子和所述gnss点观测因子进行退化性分析，基于退化性分析结果设置表征不确定性的信息矩阵并构建因子图；

地图生成模块408，用于对所述因子图进行优化，得到激光点云数据中每个关键帧的六自由度位姿信息，根据所述六自由度位姿信息对所述预处理点云数据进行拼接，得到高精度地图。

在其中一个实施例中，数据获取模块402还用于根据预设的位移量，确定所述激光点云数据中的关键帧；采用点云帧内补偿算法对所述激光点云数据中运动形成的点云畸变进行处理，以及对所述激光点云数据中动态场景的点云噪声进行处理。

在其中一个实施例中，因子图构建模块406还用于确定复杂环境的环境类型，根据环境类型确定退化分析类型；获取退化分析的退化指标，以及获取六自由度对角矩阵；根据所述退化指标、所述六自由度对角矩阵以及所述退化分析类型，得到点云匹配因子的第一信息矩阵。

在其中一个实施例中，因子图构建模块406还用于根据gnss导航数据对应的输出定位方差和可观测卫星数目，确定gnss点观测因子对应的第二信息矩阵；采用运动模型估计方式进行量化分析，得到imu边观测因子的第三信息矩阵；根据所述第一信息矩阵、所述第二信息矩阵以及所述第三信息矩阵，构建因子图。

在其中一个实施例中，所述高精度地图为二维高精度地图或三维高精度地图。

关于复杂环境下的高精度地图构建装置的具体限定可以参见上文中对于复杂环境下的高精度地图构建方法的限定，在此不再赘述。上述复杂环境下的高精度地图构建装置中的各个模块可全部或部分通过软件、硬件及其组合来实现。上述各模块可以硬件形式内嵌于或独立于计算机设备中的处理器中，也可以以软件形式存储于计算机设备中的存储器中，以便于处理器调用执行以上各个模块对应的操作。

在一个实施例中，提供了一种计算机设备，该计算机设备可以是终端，其内部结构图可以如图5所示。该计算机设备包括通过系统总线连接的处理器、存储器、网络接口、显示屏和输入装置。其中，该计算机设备的处理器用于提供计算和控制能力。该计算机设备的存储器包括非易失性存储介质、内存储器。该非易失性存储介质存储有操作系统和计算机程序。该内存储器为非易失性存储介质中的操作系统和计算机程序的运行提供环境。该计算机设备的网络接口用于与外部的终端通过网络连接通信。该计算机程序被处理器执行时以实现一种复杂环境下的高精度地图构建方法。该计算机设备的显示屏可以是液晶显示屏或者电子墨水显示屏，该计算机设备的输入装置可以是显示屏上覆盖的触摸层，也可以是计算机设备外壳上设置的按键、轨迹球或触控板，还可以是外接的键盘、触控板或鼠标等。

本领域技术人员可以理解，图5中示出的结构，仅仅是与本申请方案相关的部分结构的框图，并不构成对本申请方案所应用于其上的计算机设备的限定，具体的计算机设备可以包括比图中所示更多或更少的部件，或者组合某些部件，或者具有不同的部件布置。

在一个实施例中，提供了一种计算机设备，包括存储器和处理器，该存储器存储有计算机程序，该处理器执行计算机程序时实现上述实施例中方法的步骤。

在一个实施例中，提供了一种计算机可读存储介质，其上存储有计算机程序，计算机程序被处理器执行时实现上述实施例中方法的步骤。

本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一非易失性计算机可读取存储介质中，该计算机程序在执行时，可包括如上述各方法的实施例的流程。其中，本申请所提供的各实施例中所使用的对存储器、存储、数据库或其它介质的任何引用，均可包括非易失性和/或易失性存储器。非易失性存储器可包括只读存储器(rom)、可编程rom(prom)、电可编程rom(eprom)、电可擦除可编程rom(eeprom)或闪存。易失性存储器可包括随机存取存储器(ram)或者外部高速缓冲存储器。作为说明而非局限，ram以多种形式可得，诸如静态ram(sram)、动态ram(dram)、同步dram(sdram)、双数据率sdram(ddrsdram)、增强型sdram(esdram)、同步链路(synchlink)dram(sldram)、存储器总线(rambus)直接ram(rdram)、直接存储器总线动态ram(drdram)、以及存储器总线动态ram(rdram)等。

以上实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。

以上所述实施例仅表达了本申请的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本申请构思的前提下，还可以做出若干变形和改进，这些都属于本申请的保护范围。因此，本申请专利的保护范围应以所附权利要求为准。