CN112362072B - 一种复杂城区环境中的高精度点云地图创建系统及方法 - Google Patents

Abstract

本发明公开了一种复杂城区环境中的高精度点云地图创建系统及方法，该系统主要包括：里程计模块、回环检测模块、后端优化模块和地图生成模块。该方法的主要思路是，里程计模块利用语义分割将点云分为地面点云和障碍物点云，并在点云配准中引入双向对应并构建基于L1范数的鲁棒目标函数；利用全局特征和局部特征双阶段匹配的方法有效检测回环，其中全局特征用于快速剔除错误回环，局部特征用于精确筛选回环；后端优化模块构造基于L1范数的鲁棒目标函数，以克服回环和GPS包含外点的情况；地图生成模块对地面点云和障碍物点云分别处理，提升了地图标注效率和在线定位的鲁棒性。

Description

一种复杂城区环境中的高精度点云地图创建系统及方法

技术领域

本发明属于智能交通领域与高精度建图系统领域，特别涉及一种复杂城区环境中的高精度点云地图创建系统及方法。

背景技术

高精度建图是无人驾驶的核心模块。通过提前构建环境的精确三维描述，可以最大限度地减少无人驾驶在线感知模块的不确定性，提升整个无人驾驶系统的可靠性。另一方面，高精度点云地图是高精度建图的基础，通过点云地图可以进一步构造特征级地图如车道线/道路边界/交通灯地图，和拓扑级地图如导航地图等。

给定由激光雷达采集的点云帧序列、GPS和IMU信息，高精度建图方法通常包括位姿计算和地图生成两个步骤。然而，上述两个步骤受城区环境和激光传感器的扫描方式影响。位姿计算通常包括里程计、回环检测和后端优化，里程计使用相邻帧点云推算位姿，其核心点云配准算法易受动态障碍物、遮挡的影响，回环检测常常使用GPS快速剔除错误回环，但城区环境下GPS信号易受高楼、隧道、林荫道影响，后端优化基于高斯噪声假设融合里程计、回环检测和GPS信息，但不可靠的GPS和误检的回环会导致后端优化失效；地图生成上，由于城区环境的复杂性和激光雷达的扫描方式，导致生成的点云地图地面点比例很高，影响地图标注和在线定位算法的可靠性。

综上所述，目前的建图系统无法完成开放式、复杂城区下的建图任务，不具备在技术研发和应用上的推广。

发明内容

本发明的目的在于提供一种复杂城区环境中的高精度点云地图创建系统及方法，以解决上述问题。

为实现上述目的，本发明采用以下技术方案：

一种复杂城区环境中的高精度点云地图创建系统，包括里程计模块M1、回环检测模块M2、后端优化模块M3和地图生成模块M4；里程计模块M1、回环检测模块M2均连接到后端优化模块M3，后端优化模块M3连接地图生成模块M4，里程计模块M1连接回环检测模块M2；

里程计模块M1输入点云帧序列和IMU，输出障碍物点云、地面点云和里程计信息；回环检测模块M2输入障碍物点云帧序列和IMU，输出回环信息；后端优化模块M3输入里程计模块、回环检测模块和GPS，输出最优位姿；地图生成模块M4输入最优位姿、障碍物点云和地面点云，输出高精度点云地图。

进一步的，里程计模块M1包括语义分割模块M11和点云配准模块M12；语义分割模块M11用于将输入点云划分为空间栅格，通过栅格特征将点云划分为地面和障碍物两类；点云配准模块M12给定连续两帧、经语义分割的点云。

进一步的，回环检测模块M2包括全局特征筛选模块M21和局部特征筛选模块M22；全局特征筛选模块M22包括全局特征构建模块和特征匹配模块，全局特征构建模块的目标是对每帧点云构建全局特征；全局特征筛选模块M21计算每帧点云对应的全局特征，通过全局特征之间的匹配筛选得到回环的索引集合。

进一步的，一种复杂城区环境中的高精度点云地图创建方法，包括以下步骤：

步骤1，语义分割模块M11采用栅格图算法将单帧点云分割为地面点云/>阳非地面点云/>

步骤2，点云配准模块M12输入两帧点云和IMU信息，输出两帧点云之间的刚性变换/>

步骤3，对输入的障碍物点云帧序列全局特征筛选模块M21计算每帧点云对应的全局特征h_i，通过全局特征之间的匹配筛选得到回环的索引集合/>

步骤4，局部特征筛选模块M22通过局部特征匹配对回环索引做精确地筛选，并计算回环对应的刚性变换；

步骤5，后端优化模块M3构造基于里程计、回环检测和稀疏GPS信号的目标函数，对回环检测和GPS施加基于L1范数的鲁棒函数以增强算法对外点的鲁棒性，优化目标函数得到点云序列的最优位姿；

步骤6，地图生成模块M4由最优位姿、地面点云帧序列和障碍物点云帧序列生成地面点云地图M_grd和障碍物点云地图M_obs，分别处理后合并为点云地图M。

进一步的，语义分割模块M11将输入点云划分为空间栅格，具体过程如下：

语义分割模块M11将原始的无序点云分为地面点云和障碍物点云；考虑到三维激光的扫描特性，给定水平角分辨率d_θ和径向距离分辨率d_r，可将无序点云分割为多个互不相交的栅格：

其中单个栅格是三维点集对/>中的单个点x_m，其栅格坐标计算如下：

其中是向上取整操作，θ_m，r_m是x_m的水平角和径向距离；

针对单个栅格计算其Z轴的高度差特征：

根据上述特征，构造地面点云和障碍物点云/>其中ε_h是阈值；按比例r构造新的点云/>

进一步的，点云配准模块M12给定连续两帧、经语义分割的点云和/>以及相应的IMU信息u_x∈SE(3)和u_y∈SE(3)，需要求解两帧点云之间的相对刚性变换，n..是法向量；采用基于双向对应的迭代最近点配准算法：对应关系求解和刚性变换求解；采用IMU计算刚性变换的初值/>具体过程如下：

对应关系求解即给定当前的刚体变换通过最近邻搜索构建/>和/>之间的对应关系，表示为二元索引/>每对索引表示/>和/>是空间上的同一个点；同理，从反方向通过最近邻构建/>和/>的对应关系/>双向对应即同时满足上述两种对应关系二元索引/>通过双向对应，两帧点云表示为一一对应的点云：

进一步的，刚性变换求解即根据对应关系求解最优刚性变换：

其中p∈(0，1]是基于l_p范数的鲁棒估计，鲁棒估计可以应对交通场景中的遮挡和动态障碍物，极大地增强点云配准算法的精度；取p＝1.0；

上式变换为如下的带约束的优化问题：

其中z_i是约束变量，是约束向量，上式通过增广拉格朗日法高效求解。

进一步的，全局特征筛选模块M22包括全局特征构建模块和特征匹配模块；全局特征构建模块的目标是对每帧点云构建全局特征，该全局特征可以快速计算且对刚性变换具有不变性；

和里程计模块类似，将点云按笛卡尔栅格图的方式分割为多个栅格：

将每个栅格拟合成高斯分布，其均值和协方差矩阵为：

由于协方差矩阵提取全局特征，将点云的NDT表征记作针对每个协方差矩阵∑_i，提取全局特征如下：

f_1，i＝(λ₃-λ₂)/λ₃

f_2，i＝(λ₂-λ₁)/λ₃

f_3，i＝λ₁/λ₃

f_4，i＝λ₁/(λ₁+λ₂+λ₃)

f_5，i＝(λ₃-λ₁)/λ₃

其中λ₁≤λ₂≤λ₃是协方差矩阵的特征值，是Sigmoid函数；根据上式计算点云/>的八个特征向量/>然后对每个向量在[0，1]区间上计算直方图/>K直方图的区间数；并将直方图聚合为全局特征如下：

基于全局特征的回环筛选通过全局特征之间的匹配快速去除错误回环；假设有N帧点云帧序列对应的全局特征回环筛选的具体过程如下：

通过相邻帧构建训练集；

时域上相邻的索引集合为d_f是阈值；由此计算全局特征的残差集合：

其中是全局特征之间的距离；

阈值学习；由于相邻帧的全局特征比较类似，假设符合某一分布/>通过最大似然法可以学习到参数θ^*，认为正确的回环应该在1-α之内，阈值d_LC通过下式求解：

输出回环索引；测试索引集合必须满足时间差足够大，对应IMU的距离足够小：

其中t_m，x_t，m分别对应第m帧点云的时间戳和IMU的平移向量，t_n，x_t，n同理；t_thr，d_thr是时间和距离阈值；最终的回环索引为：

进一步的，局部特征筛选模块M22通过局部特征匹配，对回环做进一步的筛选，并给出回环之间的精确刚性变换；从回环索引中抽取两帧点云和/>相应的局部特征筛选具体过程如下：

关键点提取；给定点云按空间分辨率均匀降采样得到关键点/>均匀降采样会改变点云的几何形状，为减缓几何形变，必须将关键点投影到原点云上，通过最近邻搜索可以保证上述要求；点云/>的关键点提取和/>的同理；

局部特征计算；采用点云中常用的RoPS特征，对关键点的每个点依次计算邻域、构建局部坐标系和特征描述子，

生成局部特征其中p_i是关键点，C_i是局部坐标系，f_i是特征描述子；局部坐标系和特征描述子仅有点云的局部形状决定，对刚性变换具有不变性；点云/>的局部特征计算和/>的同理；

刚性变换计算；给定两帧点云的局部特征和/>通过特征描述子之间的最近邻搜索，构建局部特征之间的一一对应关系，将其写做/>针对每个匹配对，计算该匹配对对应的刚性变换：

t＝p_Y，i-Rp_X，i

该刚性变换的质量评价采用两帧点云之间的重叠率，参照里程计模块，计算基于双向对应的对应关系输入点云可以表示为一一对应的关系，/>和重叠率计算如下：

其中是指示函数，当且仅当输入的条件为真时，返回1，否则返回0；ε_r是距离阈值；由局部特征筛选得到的回环索引如下：

进一步的，后端优化模块M3通过因子图融合里程计、回环和稀疏GPS信息，得到精确的位姿序列，为地图生成模块提供输入；为克服回环和稀疏GPS信息可能存在的外点，在目标函数中引入基于L1范数的鲁棒估计；假设输入点云帧序列为待优化的位姿序列为/>里程计模块的输出为/>回环检测模块的输出为/>稀疏GPS信号为/>是差分GPS的索引集合；所构造的目标函数如下：

其中，Ω..是对应测量方程的信息矩阵，可以由点云配准算法或GPS接收机计算或读出，是马氏距离，/>是基于L1范数的鲁棒函数。

进一步的，地图生成模块M4利用最优位姿序列、地面点云序列和障碍物点云序列生成适用于在线定位的高精度点云地图；具体过程如下：

语义地图生成；里程计模块将每帧点云分割为地面点云和障碍物点云，根据最优位姿将上述两类点云拼接为路面点云地图M_grd和障碍物点云地图M_obs；

地图处理；人工删除掉障碍物点云地图M_obs会有动态障碍物形成的拖影；将地面点云地图随机降采样到和M_obs点数相同；

语义地图合并；将上一步中的两类点云地图合并成点云地图M，并计算M中每个点的法向量、协方差矩阵几何信息。

与现有技术相比，本发明有以下技术效果：

本发明与常用的基于差分GPS的建图系统相比，算法性能不受复杂城区环境GPS信号差的影响，因此可以广泛应用于无人驾驶高精度建图、智能车辅助驾驶等领域。

本发明与其他基于激光点云、GPS和IMU的定位系统相比，里程计模块M1中所用的点云配准模块M12，利用双向对应和L1鲁棒估计，克服了常见点云配准算法易受动态障碍物和遮挡影响的弱点。

本发明与其他基于激光点云、GPS和IMU的定位系统相比，所用回环检测模块M2采用全局和局部特征双阶段筛选的机制，可有效提升回环检测效率；所用回环检测模块M2无需使用GPS信号，可完成复杂城区环境内大范围建图任务。

本发明与其他基于激光点云、GPS和IMU的定位系统相比，地图生成算法利用语义分割生成地面点云地图和障碍物点云地图，其中障碍物点云地图由于没有地面点影响，动态障碍物所造成的拖影可以被人轻易检测到，有效提升了地图标注系统的效率；地面点云地图没有障碍物点，可以灵活地随机降采样以满足在线定位算法的需求。

附图说明

图1为本发明的系统整体框架图；

图2为栅格划分示意图；

图3为语义分割结果示意图；

图4为点云配准流程图；

图5为回环检测流程图；

图6为全局特征结果示意图；

图7为全局特征筛选结果示意图；

图8为局部特征筛选结果示意图；

图9为后端优化结果示意图；

图10为地图生成结果示意图；

图11为本发明所生成的高精度点云地图。

具体实施方式

以下结合附图对本发明进一步说明：

参见图1，为本发明的系统整体框架示意图，包括里程计模块M1、回环检测模块M2、后端优化模块M3和地图生成模块M4；里程计模块输入点云帧序列和IMU，输出障碍物点云、地面点云和里程计信息；回环检测模块输入障碍物点云帧序列和IMU，输出回环信息；后端优化模块输入里程计模块、回环检测模块和GPS，输出最优位姿；地图生成模块输入最优位姿、障碍物点云和地面点云，输出高精度点云地图；

里程计模块M1包括语义分割模块M11和点云配准模块M12；

回环检测模块M2包括全局特征筛选模块M21和局部特征筛选模块M22；

具体包括以下步骤：

步骤1，语义分割模块M11采用栅格图算法将单帧点云分割为地面点云/>和非地面点云/>

步骤2，点云配准模块M12输入两帧点云和IMU信息，输出两帧点云之间的刚性变换/>

步骤3，对输入的障碍物点云帧序列全局特征筛选模块M21计算每帧点云对应的全局特征h_i，通过全局特征之间的匹配筛选得到回环的索引集合/>

步骤4，局部特征筛选模块M22通过局部特征匹配对回环索引做精确地筛选，并计算回环对应的刚性变换；

步骤5，后端优化模块M3构造基于里程计、回环检测和稀疏GPS信号的目标函数，对回环检测和GPS施加基于L1范数的鲁棒函数以增强算法对外点的鲁棒性，优化目标函数得到点云序列的最优位姿；

步骤6，地图生成模块M4由最优位姿、地面点云帧序列和障碍物点云帧序列生成地面点云地图M_grd和障碍物点云地图M_obs，分别处理后合并为点云地图M；

参见图2，为本发明的栅格划分示意图，其中极坐标栅格图用于语义分割模块M11，笛卡尔栅格图用于全局特征筛选模块M21。

极坐标栅格图计算如下：假设原始的无序点云为给定水平角分辨率d_θ和径向距离分辨率d_r，可将无序点云分割为多个互不相交的栅格：

其中单个栅格是三维点集对/>中的单个点x_m，其栅格坐标计算如下：

其中是向上取整操作，θ_m，r_m是x_m的水平角和径向距离。

同极坐标栅格图类似，可以将点云转化为笛卡尔栅格图如下：

参见图3，为本发明的语义分割结果示意图。由图可知，语义分割可以准确地将点云划分为地面点云和障碍物点云。具体计算过程如下：针对单个栅格计算其Z轴的高度差特征：

根据上述特征，可构造地面点云和障碍物点云/>其中ε_h是阈值。

参见图4，为本发明的点云配准流程图。点云配准模块M12给定连续两帧、经语义分割的点云和/>以及相应的IMU信息u_x∈SE(3)和u_y∈SE(3)，需要求解两帧点云之间的相对刚性变换，n..是法向量。本发明采用基于双向对应的迭代最近点配准算法。该算法分为两步，对应关系求解和刚性变换求解。由于配准算法是局部算法，需要提供较好的初值，本发明采用IMU计算刚性变换的初值/>具体过程如下：

对应关系求解即给定当前的刚体变换通过最近邻搜索构建/>和/>之间的对应关系，表示为二元索引/>每对索引表示/>和/>是空间上的同一个点；同理，从反方向通过最近邻构建/>和/>的对应关系/>双向对应即同时满足上述两种对应关系二元索引/>通过双向对应，两帧点云可以表示为一一对应的点云：

进一步的，刚性变换求解即根据对应关系求解最优刚性变换：

其中p∈(0，1]是基于l_p范数的鲁棒估计，鲁棒估计可以应对交通场景中的遮挡和动态障碍物，可以极大地增强点云配准算法的精度。本发明取p＝1.0。

上式可以变换为如下的带约束的优化问题：

其中z_i是约束变量，是约束向量，上式可通过增广拉格朗日法高效求解。构造拉格朗日目标函数如下：

其中是拉格朗日乘子，μ是增广拉格朗日法的稀疏。增广拉格朗日法迭代执行以下三步：

前两个是优化问题，最优一个是拉格朗日乘子更新。针对第一个优化问题，可将拉格朗日函数整理为关于Z的函数：

可以看出，为最大化拉格朗日函数，仅需求解优化问题：

针对欧式空间中的任意向量上式等价于L1范数的近端估计(ProximalOperator)，且该近端估计有解析解：

因此，优化问题有解析解。

针对第二个优化问题，将拉格朗日函数整理为关于刚性变换的表达式，进一步的化简如下：

上式是标准的基于点到面距离的点云配准目标函数，已有很多成熟的求解器可用。

参见图5，为本发明的回环检测流程图。回环检测模块M2包括全局特征筛选模块M21和局部特征筛选模块M22；全局特征筛选模块M21通过全局特征匹配快速剔除错误回环；局部特征筛选模块M22通过局部特征对上一步中的回环做判断，并给出回环对应的刚性变换。

全局特征筛选模块M21包括全局特征构建模块和特征匹配模块；全局特征构建模块的目标是对每帧点云构建全局特征，该全局特征可以快速计算且对刚性变换具有不变性；

和里程计模块类似，可以将点云按笛卡尔栅格图的方式分割为多个栅格：

将每个栅格拟合成高斯分布，其均值和协方差矩阵为：

由于协方差矩阵可以提取全局特征，将点云的NDT表征记作针对每个协方差矩阵∑_i，提取全局特征如下：

f_1，i＝(λ₃-λ₂)/λ₃

f_2，i＝(λ₂-λ₁)/λ₃

f_3，i＝λ₁/λ₃

f_4，i＝λ₁/(λ₁+λ₂+λ₃)

f_5，i＝(λ₃-λ₁)/λ₃

其中λ₁≤λ₂≤λ₃是协方差矩阵的特征值，是Sigmoid函数。根据上式可计算点云/>的八个特征向量/>然后对每个向量在[0，1]区间上计算直方图/>K直方图的区间数。并将直方图聚合为全局特征如下：

基于全局特征的回环筛选通过全局特征之间的匹配快速去除错误回环。假设有N帧点云帧序列对应的全局特征回环筛选的具体过程如下：

通过相邻帧构建训练集。时域上相邻的索引集合为d_f是阈值。由此计算全局特征的残差集合：

其中是全局特征之间的距离。

阈值学习；由于相邻帧的全局特征比较类似，假设符合某一分布/>通过最大似然法可以学习到参数θ^*，可以认为正确的回环应该在1-α之内，阈值d_LC可通过下式求解：

输出回环索引。测试索引集合必须满足时间差足够大，对应IMU的距离足够小：

其中t_m，x_t，m分别对应第m帧点云的时间戳和IMU的平移向量，t_n，x_t，n同理。t_thr，d_thr是时间和距离阈值。最终的回环索引为：

进一步的，局部特征筛选模块M22通过局部特征匹配，对回环做进一步的筛选，并给出回环之间的精确刚性变换。从回环索引中抽取两帧点云和/>相应的局部特征筛选具体过程如下：

关键点提取。给定点云按空间分辨率均匀降采样得到关键点/>均匀降采样会改变点云的几何形状，为减缓几何形变，必须将关键点投影到原点云上，通过最近邻搜索可以保证上述要求。点云/>的关键点提取和/>的同理。

局部特征计算。本发明采用点云中常用的RoPS特征，对关键点的每个点依次计算邻域、构建局部坐标系和特征描述子，生成局部特征/>其中p_i是关键点，C_i是局部坐标系，f_i是特征描述子。局部坐标系和特征描述子仅有点云的局部形状决定，对刚性变换具有不变性。点云/>的局部特征计算和/>的同理。/>

刚性变换计算。给定两帧点云的局部特征和/>通过特征描述子之间的最近邻搜索，可以构建局部特征之间的一一对应关系，将其写做/>针对每个匹配对，可以计算该匹配对对应的刚性变换：

t＝p_Y，i-Rp_X，i

该刚性变换的质量评价采用两帧点云之间的重叠率，参照里程计模块，计算基于双向对应的对应关系输入点云可以表示为一一对应的关系，/>和重叠率计算如下：

其中是指示函数，当且仅当输入的条件为真时，返回1，否则返回0；ε_r是距离阈值。

由局部特征筛选得到的回环索引如下：

参见图6，为本发明的全局特征结果示意图。第一行是激光雷达扫描得到的点云，第二行是各个点云对应的全局特征。可以看出，点云A和点云B对应于不同视角下的相同场景，其全局特征较为相似。点云C和其他两帧点云在空间上是不同的场景，，其全局特征和A、B特征相差较大，说明本发明所提出的全局特征具有刚性变换不变性，可以有效地筛选回环。

参见图7，为本发明的全局特征筛选结果示意图。可以看出，通过阈值学习得到的阈值可以剔除掉绝大多数错误回环，有效提升了回环检测算法的效率。

参见图8，为本发明的局部特征筛选结果示意图。第一行是点云以及局部特征之间的匹配连线，每张子图中的两帧点云来自于激光雷达不同时刻的采集数据，第二行是利用局部特征对齐后的点云。第一列的场景平移量约为18米，第二列场景旋转角度约为180度，在较大的平移和旋转下，局部特征筛选仍然可以恢复出正确的刚性变换。

参见图9，为本发明的后端优化结果示意图。可以看出，融合里程计、回环和稀疏GPS信号后，位姿的累计误差明显地变小，里程计所认为的两条路被正确合并为同一条路。

参见图10，为本发明的地图生成结果示意图。传统的方法不区分地面和障碍物，所生成的点云动态障碍物和地面交叠，导致地图标注无法保证效率和准确度。本发明中的地图生成模块M4分别生成地面点云和障碍物点云，可以看出，障碍物点云由于没有地面点，动态障碍物如车和人等可以清晰被人分辨出来，有利于地图的标注。

参见图11，为本发明所生成的高精度点云地图。可以看出，在高动态、开放式的复杂校园和园区环境中，本发明所生成的高精度点云地图细节较为精确，满足无人驾驶导航和在线定位需求。

以上内容是结合具体的优选实施方式对本发明所作的进一步详细说明，不能认定本发明的具体实施方式仅限于此，对于本发明所属技术领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干简单的推演或替换，都应当视为属于本发明由所提交的权利要求书确定专利保护范围。

Claims (6)

1.一种复杂城区环境中的高精度点云地图创建方法，其特征在于，基于一种复杂城区环境中的高精度点云地图创建系统，所述系统包括：

里程计模块M1、回环检测模块M2、后端优化模块M3和地图生成模块M4；里程计模块M1、回环检测模块M2均连接到后端优化模块M3，后端优化模块M3连接地图生成模块M4，里程计模块M1连接回环检测模块M2；

里程计模块M1输入点云帧序列和IMU，输出障碍物点云、地面点云和里程计信息；回环检测模块M2输入障碍物点云帧序列和IMU，输出回环信息；后端优化模块M3输入里程计模块、回环检测模块和GPS，输出最优位姿；地图生成模块M4输入最优位姿、障碍物点云和地面点云，输出高精度点云地图；

里程计模块M1包括语义分割模块M11和点云配准模块M12；语义分割模块M11用于将输入点云划分为空间栅格，通过栅格特征将点云划分为地面和障碍物两类；点云配准模块M12给定连续两帧、经语义分割的点云；

回环检测模块M2包括全局特征筛选模块M21和局部特征筛选模块M22；全局特征筛选模块M21包括全局特征构建模块和特征匹配模块，全局特征构建模块的目标是对每帧点云构建全局特征；全局特征筛选模块M21计算每帧点云对应的全局特征，通过全局特征之间的匹配筛选得到回环的索引集合；

包括以下步骤：

步骤1，语义分割模块M11采用栅格图算法将单帧点云分割为地面点云/>和非地面点云/>

步骤2，点云配准模块M12输入两帧点云和IMU信息，输出两帧点云之间的刚性变换

步骤3，对输入的障碍物点云帧序列全局特征筛选模块M21计算每帧点云对应的全局特征h_i，通过全局特征之间的匹配筛选得到回环的索引集合/>

步骤4，局部特征筛选模块M22通过局部特征匹配对回环索引做精确地筛选，并计算回环对应的刚性变换；

步骤5，后端优化模块M3构造基于里程计、回环检测和稀疏GPS信号的目标函数，对回环检测和GPS施加基于L1范数的鲁棒函数以增强算法对外点的鲁棒性，优化目标函数得到点云序列的最优位姿；

步骤6，地图生成模块M4由最优位姿、地面点云帧序列和障碍物点云帧序列生成地面点云地图M_grd和障碍物点云地图M_obs，分别处理后合并为点云地图M；

全局特征筛选模块M21包括全局特征构建模块和特征匹配模块；全局特征构建模块的目标是对每帧点云构建全局特征，该全局特征可以快速计算且对刚性变换具有不变性；

和里程计模块类似，将点云按笛卡尔栅格图的方式分割为多个栅格：

将每个栅格拟合成高斯分布，其均值和协方差矩阵为：

由于协方差矩阵提取全局特征，将点云的NDT表征记作针对每个协方差矩阵Σ_i，提取全局特征如下：

f_1,i＝(λ₃-λ₂)/λ₃

f_2,i＝(λ₂-λ₁)/λ₃

f_3,i＝λ₁/λ₃

f_4,i＝λ₁/(λ₁+λ₂+λ₃)

f_5,i＝(λ₃-λ₁)/λ₃

其中λ₁≤λ₂≤λ₃是协方差矩阵的特征值，g(·):是Sigmoid函数；根据上式计算点云/>的八个特征向量/>然后对每个向量在[0,1]区间上计算直方图/>K直方图的区间数；并将直方图聚合为全局特征如下：

基于全局特征的回环筛选通过全局特征之间的匹配快速去除错误回环；假设有N帧点云帧序列对应的全局特征回环筛选的具体过程如下：

通过相邻帧构建训练集；

时域上相邻的索引集合为d_f是阈值；由此计算全局特征的残差集合：

其中d(·,·):是全局特征之间的距离；

阈值学习；由于相邻帧的全局特征比较类似，假设符合某一分布/>通过最大似然法可以学习到参数θ^*，认为正确的回环应该在1-α之内，阈值d_LC通过下式求解：

输出回环索引；测试索引集合必须满足时间差足够大，对应IMU的距离足够小：

其中t_m,x_t,m分别对应第m帧点云的时间戳和IMU的平移向量，t_n,x_t,n同理；t_thr,d_thr是时间和距离阈值；最终的回环索引为：

2.根据权利要求1所述的一种复杂城区环境中的高精度点云地图创建方法，其特征在于，语义分割模块M11将输入点云划分为空间栅格，具体过程如下：

语义分割模块M11将原始的无序点云分为地面点云和障碍物点云；考虑到三维激光的扫描特性，给定水平角分辨率d_θ和径向距离分辨率d_r，可将无序点云分割为多个互不相交的栅格：

其中单个栅格是三维点集对/>中的单个点x_m，其栅格坐标计算如下：

其中是向上取整操作，θ_m,r_m是x_m的水平角和径向距离；

针对单个栅格计算其Z轴的高度差特征：

根据上述特征，构造地面点云和障碍物点云/>其中ε_h是阈值；按比例r₁构造新的点云/>

3.根据权利要求1所述的一种复杂城区环境中的高精度点云地图创建方法，其特征在于，点云配准模块M12给定连续两帧、经语义分割的点云和以及相应的IMU信息u_x∈SE(3)和u_y∈SE(3)，需要求解两帧点云之间的相对刚性变换，n_..是法向量；采用基于双向对应的迭代最近点配准算法：对应关系求解和刚性变换求解；采用IMU计算刚性变换的初值/>具体过程如下：

对应关系求解即给定当前的刚体变换通过最近邻搜索构建/>和/>之间的对应关系，表示为二元索引/>每对索引表示/>和/>是空间上的同一个点；同理，从反方向通过最近邻构建/>和/>的对应关系/>双向对应即同时满足上述两种对应关系二元索引I＝I₀∩I₁；通过双向对应，两帧点云表示为一一对应的点云：

进一步的，刚性变换求解即根据对应关系求解最优刚性变换：

其中p∈(0,1]是基于l_p范数的鲁棒估计，鲁棒估计可以应对交通场景中的遮挡和动态障碍物，极大地增强点云配准算法的精度；取p＝1.0；

上式变换为如下的带约束的优化问题：

其中z_i是约束变量，是约束向量，上式通过增广拉格朗日法高效求解。

4.根据权利要求1所述的一种复杂城区环境中的高精度点云地图创建方法，其特征在于，局部特征筛选模块M22通过局部特征匹配，对回环做进一步的筛选，并给出回环之间的精确刚性变换；从回环索引中抽取两帧点云和/>相应的局部特征筛选具体过程如下：

关键点提取；给定点云按空间分辨率均匀降采样得到关键点/>均匀降采样会改变点云的几何形状，为减缓几何形变，必须将关键点投影到原点云上，通过最近邻搜索可以保证上述要求；点云/>的关键点提取和/>的同理；

局部特征计算；采用点云中常用的RoPS特征，对关键点的每个点依次计算邻域、构建局部坐标系和特征描述子，

生成局部特征其中p_i是关键点，C_i是局部坐标系，f_i是特征描述子；局部坐标系和特征描述子仅有点云的局部形状决定，对刚性变换具有不变性；点云/>的局部特征计算和/>的同理；

刚性变换计算；给定两帧点云的局部特征和/>通过特征描述子之间的最近邻搜索，构建局部特征之间的一一对应关系，将其写做/>针对每个匹配对，计算该匹配对对应的刚性变换：

t＝p_Y,i-Rp_X,i

该刚性变换的质量评价采用两帧点云之间的重叠率，参照里程计模块，计算基于双向对应的对应关系输入点云可以表示为一一对应的关系，/>和重叠率计算如下：

其中是指示函数，当且仅当输入的条件为真时，返回1，否则返回0；ε_r是距离阈值；由局部特征筛选得到的回环索引如下：

5.根据权利要求1所述的一种复杂城区环境中的高精度点云地图创建方法，其特征在于，后端优化模块M3通过因子图融合里程计、回环和稀疏GPS信息，得到精确的位姿序列，为地图生成模块提供输入；为克服回环和稀疏GPS信息可能存在的外点，在目标函数中引入基于L1范数的鲁棒估计；假设输入点云帧序列为待优化的位姿序列为/>里程计模块的输出为/>回环检测模块的输出为/>稀疏GPS信号为/> 是差分GPS的索引集合；所构造的目标函数如下：

其中，Ω_··是对应测量方程的信息矩阵，可以由点云配准算法或GPS接收机计算或读出，是马氏距离，/>是基于L1范数的鲁棒函数。

6.根据权利要求1所述的一种复杂城区环境中的高精度点云地图创建方法，其特征在于，地图生成模块M4利用最优位姿序列、地面点云序列和障碍物点云序列生成适用于在线定位的高精度点云地图；具体过程如下：

语义地图生成；里程计模块将每帧点云分割为地面点云和障碍物点云，根据最优位姿将上述两类点云拼接为路面点云地图M_grd和障碍物点云地图M_obs；

地图处理；人工删除掉障碍物点云地图M_obs会有动态障碍物形成的拖影；将地面点云地图随机降采样到和M_obs点数相同；

语义地图合并；将上一步中的两类点云地图合并成点云地图M，并计算M中每个点的法向量、协方差矩阵几何信息。