(12)发明专利申请
(10)申请公布号 CN 112362072 A(43)申请公布日 2021.02.12
(21)申请号 202011288187.1(22)申请日 2020.11.17
(71)申请人 西安恒图智源信息科技有限责任公
司
地址 710000 陕西省西安市沣东新城协同
创新港研发办公大楼A座4楼405-1(72)发明人 薛杜娟 王迪
(74)专利代理机构 西安通大专利代理有限责任
公司 61200
代理人 安彦彦(51)Int.Cl.
G01C 21/32(2006.01)G01S 17/89(2020.01)G01S 19/14(2010.01)
权利要求书5页 说明书12页 附图4页
CN 112362072 A(54)发明名称
一种复杂城区环境中的高精度点云地图创建系统及方法(57)摘要
本发明公开了一种复杂城区环境中的高精度点云地图创建系统及方法，该系统主要包括：里程计模块、回环检测模块、后端优化模块和地图生成模块。该方法的主要思路是，里程计模块利用语义分割将点云分为地面点云和障碍物点云，并在点云配准中引入双向对应并构建基于L1范数的鲁棒目标函数；利用全局特征和局部特征双阶段匹配的方法有效检测回环，其中全局特征用于快速剔除错误回环，局部特征用于精确筛选回环；后端优化模块构造基于L1范数的鲁棒目标函数，以克服回环和GPS包含外点的情况；地图生成模块对地面点云和障碍物点云分别处理，提升了地图标注效率和在线定位的鲁棒性。
CN 112362072 A
权 利 要 求 书
1/5页
1.一种复杂城区环境中的高精度点云地图创建系统，其特征在于，包括里程计模块M1、回环检测模块M2、后端优化模块M3和地图生成模块M4；里程计模块M1、回环检测模块M2均连接到后端优化模块M3，后端优化模块M3连接地图生成模块M4，里程计模块M1连接回环检测模块M2；
里程计模块M1输入点云帧序列和IMU，输出障碍物点云、地面点云和里程计信息；回环检测模块M2输入障碍物点云帧序列和IMU，输出回环信息；后端优化模块M3输入里程计模块、回环检测模块和GPS，输出最优位姿；地图生成模块M4输入最优位姿、障碍物点云和地面点云，输出高精度点云地图。
2.根据权利要求1所述的一种复杂城区环境中的高精度点云地图创建系统，其特征在于，里程计模块M1包括语义分割模块M11和点云配准模块M12；语义分割模块M11用于将输入点云划分为空间栅格，通过栅格特征将点云划分为地面和障碍物两类；点云配准模块M12给定连续两帧、经语义分割的点云。
3.根据权利要求1所述的一种复杂城区环境中的高精度点云地图创建系统，其特征在于，回环检测模块M2包括全局特征筛选模块M21和局部特征筛选模块M22；全局特征筛选模块M22包括全局特征构建模块和特征匹配模块，全局特征构建模块的目标是对每帧点云构建全局特征；全局特征筛选模块M21计算每帧点云对应的全局特征，通过全局特征之间的匹配筛选得到回环的索引集合。
4.一种复杂城区环境中的高精度点云地图创建方法，其特征在于，基于权利要求1至3任意一项所述的一种复杂城区环境中的高精度点云地图创建系统，包括以下步骤：
步骤1，语义分割模块M11采用栅格图算法将单帧点云点云
步骤2，点云配准模块M12输入两帧点云
和IMU信息，输出两帧点云之间的刚性变换
分割为地面点云
和非地面
步骤3，对输入的障碍物点云帧序列全局特征筛选模块M21计算每帧点云对应的
全局特征hi，通过全局特征之间的匹配筛选得到回环的索引集合
步骤4，局部特征筛选模块M22通过局部特征匹配对回环索引
做精确地筛选，并计算
回环对应的刚性变换；
步骤5，后端优化模块M3构造基于里程计、回环检测和稀疏GPS信号的目标函数，对回环检测和GPS施加基于L1范数的鲁棒函数以增强算法对外点的鲁棒性，优化目标函数得到点云序列的最优位姿；
步骤6，地图生成模块M4由最优位姿、地面点云帧序列和障碍物点云帧序列生成地面点云地图Mgrd和障碍物点云地图Mobs，分别处理后合并为点云地图M。
5.根据权利要求4所述的一种复杂城区环境中的高精度点云地图创建方法，其特征在于，语义分割模块M11将输入点云划分为空间栅格，具体过程如下：
语义分割模块M11将原始的无序点云
分为地面点云和障碍物点云；考虑
到三维激光的扫描特性，给定水平角分辨率dθ和径向距离分辨率dr，可将无序点云分割为多
2
CN 112362072 A
权 利 要 求 书
2/5页
个互不相交的栅格：
其中单个栅格是三维点集对中的单个点xm，其栅格坐标计算如下：
其中
针对单个栅格
是向上取整操作，θm,rm是xm的水平角和径向距离；
计算其Z轴的高度差特征：
根据上述特征，构造地面点云中ε按比例r构造新的点云h是阈值；
和障碍物点云其
6.根据权利要求4所述的一种复杂城区环境中的高精度点云地图创建方法，其特征在于，点云配准模块M12给定连续两帧、经语义分割的点云
和
以及相应的IMU信息ux∈SE(3)和uy∈SE(3)，需要求解两帧点云
之间的相对刚性变换，n·采用基于双向对应的迭代最近点配准算法：对应关系·是法向量；求解和刚性变换求解；采用IMU计算刚性变换的初值
对应关系求解即给定当前的刚体变换系，表示为二元索引方向通过最近邻构建和对应关系二元索引
具体过程如下：
和
之间的对应关
通过最近邻搜索构建
和
每对索引表示的对应关系
是空间上的同一个点；同理，从反双向对应即同时满足上述两种
通过双向对应，两帧点云表示为一一对应的点云：
进一步的，刚性变换求解即根据对应关系求解最优刚性变换：
其中p∈(0,1]是基于lp范数的鲁棒估计，鲁棒估计可以应对交通场景中的遮挡和动态
障碍物，极大地增强点云配准算法的精度；取p＝1.0；
上式变换为如下的带约束的优化问题：
3
CN 112362072 A
权 利 要 求 书
3/5页
其中zi是约束变量，是约束向量，上式通过增广拉格朗日法高效求解。
7.根据权利要求4所述的一种复杂城区环境中的高精度点云地图创建方法，其特征在
于，全局特征筛选模块M22包括全局特征构建模块和特征匹配模块；全局特征构建模块的目标是对每帧点云构建全局特征，该全局特征可以快速计算且对刚性变换具有不变性；
和里程计模块类似，将点云
按笛卡尔栅格图的方式分割为多个栅格：
将每个栅格拟合成高斯分布，其均值和协方差矩阵为：
由于协方差矩阵提取全局特征，将点云的NDT表征记作阵Σi，提取全局特征如下：
f1,i＝(λλλ3-2)/3f2,i＝(λλλ2-1)/3f3,i＝λλ1/3f4,i＝λλλλ1/(1+2+3)f5,i＝(λλλ3-1)/3
针对每个协方差矩
其中λλλ1≤2≤3是协方差矩阵的特征值，点云图
的八个特征向量
是Sigmoid函数；根据上式计算
然后对每个向量在[0,1]区间上计算直方
K直方图的区间数；并将直方图聚合为全局特征如下：
基于全局特征的回环筛选通过全局特征之间的匹配快速去除错误回环；假设有N帧点云帧序列对应的全局特征
回环筛选的具体过程如下：
4
CN 112362072 A
权 利 要 求 书
4/5页
通过相邻帧构建训练集；时域上相邻的索引集合为算全局特征的残差集合：
df是阈值；由此计
其中d(·,·):
是全局特征之间的距离；
符合某一分布
通
阈值学习；由于相邻帧的全局特征比较类似，假设
*
过最大似然法可以学习到参数θ，认为正确的回环应该在1-α之内，阈值dLC通过下式求解：
输出回环索引；测试索引集合必须满足时间差足够大，对应IMU的距离足够小：
其中tm,xt,m分别对应第m帧点云的时间戳和IMU的平移向量，tn,xt,n同理；tthr,dthr是时间和距离阈值；最终的回环索引为：
8.根据权利要求4所述的一种复杂城区环境中的高精度点云地图创建方法，其特征在于，局部特征筛选模块M22通过局部特征匹配，对回环做进一步的筛选，并给出回环之间的精确刚性变换；从回环索引中抽取两帧点云特征筛选具体过程如下：
关键点提取；给定点云
按空间分辨率均匀降采样得到关键点
均匀降采样会改
变点云的几何形状，为减缓几何形变，必须将关键点投影到原点云上，通过最近邻搜索可以保证上述要求；点云的关键点提取和的同理；
局部特征计算；采用点云中常用的RoPS特征，对关键点计算邻域、构建局部坐标系和特征描述子，
生成局部特征
其中pi是关键点，Ci是局部坐标系，
的每个点依次
和
相应的局部
fi是特征描述子；局部坐标系和特征描述子仅有点云的局部形状决定，对刚性变换具有不变性；点云
的局部特征计算和的同理；
和
通过特征描述子之间的最近邻搜
针对每个匹配对，
刚性变换计算；给定两帧点云的局部特征
索，构建局部特征之间的一一对应关系，将其写做计算该匹配对对应的刚性变换：
t＝pY,i-RpX,i
5
CN 112362072 A
权 利 要 求 书
5/5页
该刚性变换的质量评价采用两帧点云之间的重叠率，参照里程计模块，计算基于双向对应的对应关系
输入点云可以表示为一一对应的关系，重叠率计算如下：
和
其中是指示函数，当且仅当输入的条件为真时，返回1，否则返回0；εr是距离阈值；
由局部特征筛选得到的回环索引如下：
9.根据权利要求4所述的一种复杂城区环境中的高精度点云地图创建方法，其特征在于，后端优化模块M3通过因子图融合里程计、回环和稀疏GPS信息，得到精确的位姿序列，为地图生成模块提供输入；为克服回环和稀疏GPS信息可能存在的外点，在目标函数中引入基于L1范数的鲁棒估计；假设输入点云帧序列为
里程计模块的输出为
稀疏GPS信号为
的索引集合；所构造的目标函数如下：
待优化的位姿序列为回环检测模块的输出为
是差分GPS
其中，Ω··是对应测量方程的信息矩阵，可以由点云配准算法或GPS接收机计算或读出，
是马氏距离，
是基于L1范数的鲁棒函数。
10.根据权利要求4所述的一种复杂城区环境中的高精度点云地图创建方法，其特征在于，地图生成模块M4利用最优位姿序列、地面点云序列和障碍物点云序列生成适用于在线定位的高精度点云地图；具体过程如下：
语义地图生成；里程计模块将每帧点云分割为地面点云和障碍物点云，根据最优位姿将上述两类点云拼接为路面点云地图Mgrd和障碍物点云地图Mobs；
地图处理；人工删除掉障碍物点云地图Mobs会有动态障碍物形成的拖影；将地面点云地图随机降采样到和Mobs点数相同；
语义地图合并；将上一步中的两类点云地图合并成点云地图M，并计算M中每个点的法向量、协方差矩阵几何信息。
6
CN 112362072 A
说 明 书
1/12页
一种复杂城区环境中的高精度点云地图创建系统及方法
技术领域
[0001]本发明属于智能交通领域与高精度建图系统领域，特别涉及一种复杂城区环境中的高精度点云地图创建系统及方法。
背景技术
[0002]高精度建图是无人驾驶的核心模块。通过提前构建环境的精确三维描述，可以最大限度地减少无人驾驶在线感知模块的不确定性，提升整个无人驾驶系统的可靠性。另一方面，高精度点云地图是高精度建图的基础，通过点云地图可以进一步构造特征级地图如车道线/道路边界/交通灯地图，和拓扑级地图如导航地图等。[0003]给定由激光雷达采集的点云帧序列、GPS和IMU信息，高精度建图方法通常包括位姿计算和地图生成两个步骤。然而，上述两个步骤受城区环境和激光传感器的扫描方式影响。位姿计算通常包括里程计、回环检测和后端优化，里程计使用相邻帧点云推算位姿，其核心点云配准算法易受动态障碍物、遮挡的影响，回环检测常常使用GPS快速剔除错误回环，但城区环境下GPS信号易受高楼、隧道、林荫道影响，后端优化基于高斯噪声假设融合里程计、回环检测和GPS信息，但不可靠的GPS和误检的回环会导致后端优化失效；地图生成上，由于城区环境的复杂性和激光雷达的扫描方式，导致生成的点云地图地面点比例很高，影响地图标注和在线定位算法的可靠性。[0004]综上所述，目前的建图系统无法完成开放式、复杂城区下的建图任务，不具备在技术研发和应用上的推广。
发明内容
[0005]本发明的目的在于提供一种复杂城区环境中的高精度点云地图创建系统及方法，以解决上述问题。
[0006]为实现上述目的，本发明采用以下技术方案：
[0007]一种复杂城区环境中的高精度点云地图创建系统，包括里程计模块M1、回环检测模块M2、后端优化模块M3和地图生成模块M4；里程计模块M1、回环检测模块M2均连接到后端优化模块M3，后端优化模块M3连接地图生成模块M4，里程计模块M1连接回环检测模块M2；[0008]里程计模块M1输入点云帧序列和IMU，输出障碍物点云、地面点云和里程计信息；回环检测模块M2输入障碍物点云帧序列和IMU，输出回环信息；后端优化模块M3输入里程计模块、回环检测模块和GPS，输出最优位姿；地图生成模块M4输入最优位姿、障碍物点云和地面点云，输出高精度点云地图。[0009]进一步的，里程计模块M1包括语义分割模块M11和点云配准模块M12；语义分割模块M11用于将输入点云划分为空间栅格，通过栅格特征将点云划分为地面和障碍物两类；点云配准模块M12给定连续两帧、经语义分割的点云。[0010]进一步的，回环检测模块M2包括全局特征筛选模块M21和局部特征筛选模块M22；全局特征筛选模块M22包括全局特征构建模块和特征匹配模块，全局特征构建模块的目标
7
CN 112362072 A
说 明 书
2/12页
是对每帧点云构建全局特征；全局特征筛选模块M21计算每帧点云对应的全局特征，通过全局特征之间的匹配筛选得到回环的索引集合。[0011]进一步的，一种复杂城区环境中的高精度点云地图创建方法，包括以下步骤：
[0012]
步骤1，语义分割模块M11采用栅格图算法将单帧点云分割为地面点云步骤2，点云配准模块M12输入两帧点云步骤3，对输入的障碍物点云帧序列
阳非
地面点云
[0013]
和IMU信息，输出两帧点云之间的刚性全局特征筛选模块M21计算每帧点云对
做精确地筛选，并
变换
[0014]
应的全局特征hi，通过全局特征之间的匹配筛选得到回环的索引集合
[0015]
步骤4，局部特征筛选模块M22通过局部特征匹配对回环索引
计算回环对应的刚性变换；[0016]步骤5，后端优化模块M3构造基于里程计、回环检测和稀疏GPS信号的目标函数，对回环检测和GPS施加基于L1范数的鲁棒函数以增强算法对外点的鲁棒性，优化目标函数得到点云序列的最优位姿；[0017]步骤6，地图生成模块M4由最优位姿、地面点云帧序列和障碍物点云帧序列生成地面点云地图Mgrd和障碍物点云地图Mobs，分别处理后合并为点云地图M。[0018]进一步的，语义分割模块M11将输入点云划分为空间栅格，具体过程如下：
[0019]
语义分割模块M11将原始的无序点云分为地面点云和障碍物点
云；考虑到三维激光的扫描特性，给定水平角分辨率dθ和径向距离分辨率dr，可将无序点云分割为多个互不相交的栅格：
[0020][0021]
其中单个栅格是三维点集对中的单个点xm，其栅格坐标计算如
下：
[0022][0023][0024][0025][0026][0027]
其中
针对单个栅格
是向上取整操作，θrm是xm的水平角和径向距离；m，
计算其Z轴的高度差特征：
根据上述特征，构造地面点云
其中ε按比例r构造新的点云h是阈值；
和障碍物点云
[0028]
进一步的，点云配准模块M12给定连续两帧、经语义分割的点云
和
以及相应的IMU信息ux∈SE(3)和uy
∈SE(3)，需要求解两帧点云之间的相对刚性变换，n..是法向量；采用基于双向对应的迭代
8
CN 112362072 A
说 明 书
3/12页
最近点配准算法：对应关系求解和刚性变换求解；采用IMU计算刚性变换的初值具体过程如下：
[0029]对应关系求解即给定当前的刚体变换关系，表示为二元索引反方向通过最近邻构建和对应关系二元索引
[0030][0031][0032][0033][0034]
通过最近邻搜索构建
和
和之间的对应
每对索引表示的对应关系
是空间上的同一个点；同理，从双向对应即同时满足上述两种
通过双向对应，两帧点云表示为一一对应的点云：
进一步的，刚性变换求解即根据对应关系求解最优刚性变换：
其中p∈(0，1]是基于lp范数的鲁棒估计，鲁棒估计可以应对交通场景中的遮挡和动态障碍物，极大地增强点云配准算法的精度；取p＝1.0；[0035]上式变换为如下的带约束的优化问题：
[0036][0037][0038][0039]
其中zi是约束变量，是约束向量，上式通过增广拉格朗日法高效求解。
进一步的，全局特征筛选模块M22包括全局特征构建模块和特征匹配模块；全局特
征构建模块的目标是对每帧点云构建全局特征，该全局特征可以快速计算且对刚性变换具有不变性；
和里程计模块类似，将点云
按笛卡尔栅格图的方式分割为多个栅格：
[0040][0041][0042][0043][0044][0045]
将每个栅格拟合成高斯分布，其均值和协方差矩阵为：
由于协方差矩阵提取全局特征，将点云的NDT表征记作针对每个协方
差矩阵∑i，提取全局特征如下：
[0046]f1，λλλi＝(3-2)/3[0047]f2，λλλi＝(2-1)/3[0048]f3，λλi＝1/3[0049]f4，λλλλi＝1/(1+2+3)
9
CN 112362072 A[0050][0051][0052]
说 明 书
4/12页
f5，λλλi＝(3-1)/3
[0053][0054]
其中λλλ1≤2≤3是协方差矩阵的特征值，
的八个特征向量
是Sigmoid函数；根据上式
然后对每个向量在[0，1]区间上计算
计算点云直方图
[0055][0056]
K直方图的区间数；并将直方图聚合为全局特征如下：
基于全局特征的回环筛选通过全局特征之间的匹配快速去除错误回环；假设有N
回环筛选的具体过程如下：
帧点云帧序列对应的全局特征
[0057][0058]
通过相邻帧构建训练集；时域上相邻的索引集合为
df是阈值；由
此计算全局特征的残差集合：
[0059][0060][0061]
其中是全局特征之间的距离；
符合某一分布
*
通过最大似然法可以学习到参数θ，认为正确的回环应该在1-α之内，阈值dLC
阈值学习；由于相邻帧的全局特征比较类似，假设
通过下式求解：
[0062][0063][0064][0065]
输出回环索引；测试索引集合必须满足时间差足够大，对应IMU的距离足够小：
其中tm，xt，tn，xt，tthr，dthrm分别对应第m帧点云的时间戳和IMU的平移向量，n同理；
是时间和距离阈值；最终的回环索引为：
进一步的，局部特征筛选模块M22通过局部特征匹配，对回环做进一步的筛选，并
和
[0066][0067]
给出回环之间的精确刚性变换；从回环索引中抽取两帧点云
相应的局部特征筛选具体过程如下：
[0068]
关键点提取；给定点云按空间分辨率均匀降采样得到关键点均匀降采样
会改变点云的几何形状，为减缓几何形变，必须将关键点投影到原点云上，通过最近邻搜索可以保证上述要求；点云的关键点提取和的同理；
10
CN 112362072 A[0069]
说 明 书
5/12页
局部特征计算；采用点云中常用的RoPS特征，对关键点的每个点
依次计算邻域、构建局部坐标系和特征描述子，
[0070]
生成局部特征其中pi是关键点，Ci是局部坐标
系，fi是特征描述子；局部坐标系和特征描述子仅有点云的局部形状决定，对刚性变换具有
不变性；点云的局部特征计算和的同理；
[0071]
刚性变换计算；给定两帧点云的局部特征和通过特征描述子之间的最近邻
针对每个匹配
搜索，构建局部特征之间的一一对应关系，将其写做对，计算该匹配对对应的刚性变换：
[0072][0073][0074]
t＝pY，i-RpX，i
该刚性变换的质量评价采用两帧点云之间的重叠率，参照里程计模块，计算基于
输入点云可以表示为一一对应的关系，
和
双向对应的对应关系
重叠率计算如下：
[0075][0076]
其中是指示函数，当且仅当输入的条件为真时，返回1，否则返回0；εr是距离阈
值；由局部特征筛选得到的回环索引如下：
[0077][0078]
进一步的，后端优化模块M3通过因子图融合里程计、回环和稀疏GPS信息，得到精
确的位姿序列，为地图生成模块提供输入；为克服回环和稀疏GPS信息可能存在的外点，在
待优化的位姿回环检测模块的输
是差
目标函数中引入基于L1范数的鲁棒估计；假设输入点云帧序列为序列为出为
里程计模块的输出为
稀疏GPS信号为
分GPS的索引集合；所构造的目标函数如下：
[0079][0080]
其中，Ω..是对应测量方程的信息矩阵，可以由点云配准算法或GPS接收机计算或
是马氏距离，
是基于L1范数的鲁棒函数。
读出，
[0081]
进一步的，地图生成模块M4利用最优位姿序列、地面点云序列和障碍物点云序列生成适用于在线定位的高精度点云地图；具体过程如下：[0082]语义地图生成；里程计模块将每帧点云分割为地面点云和障碍物点云，根据最优位姿将上述两类点云拼接为路面点云地图Mgrd和障碍物点云地图Mobs；[0083]地图处理；人工删除掉障碍物点云地图Mobs会有动态障碍物形成的拖影；将地面点云地图随机降采样到和Mobs点数相同；
11
CN 112362072 A[0084]
说 明 书
6/12页
语义地图合并；将上一步中的两类点云地图合并成点云地图M，并计算M中每个点
的法向量、协方差矩阵几何信息。[0085]与现有技术相比，本发明有以下技术效果：
[0086]本发明与常用的基于差分GPS的建图系统相比，算法性能不受复杂城区环境GPS信号差的影响，因此可以广泛应用于无人驾驶高精度建图、智能车辅助驾驶等领域。[0087]本发明与其他基于激光点云、GPS和IMU的定位系统相比，里程计模块M1中所用的点云配准模块M12，利用双向对应和L1鲁棒估计，克服了常见点云配准算法易受动态障碍物和遮挡影响的弱点。
[0088]本发明与其他基于激光点云、GPS和IMU的定位系统相比，所用回环检测模块M2采用全局和局部特征双阶段筛选的机制，可有效提升回环检测效率；所用回环检测模块M2无需使用GPS信号，可完成复杂城区环境内大范围建图任务。[0089]本发明与其他基于激光点云、GPS和IMU的定位系统相比，地图生成算法利用语义分割生成地面点云地图和障碍物点云地图，其中障碍物点云地图由于没有地面点影响，动态障碍物所造成的拖影可以被人轻易检测到，有效提升了地图标注系统的效率；地面点云地图没有障碍物点，可以灵活地随机降采样以满足在线定位算法的需求。附图说明
[0090]图1为本发明的系统整体框架图；[0091]图2为栅格划分示意图；
[0092]图3为语义分割结果示意图；[0093]图4为点云配准流程图；[0094]图5为回环检测流程图；[0095]图6为全局特征结果示意图；[0096]图7为全局特征筛选结果示意图；[0097]图8为局部特征筛选结果示意图；[0098]图9为后端优化结果示意图；[0099]图10为地图生成结果示意图；
[0100]图11为本发明所生成的高精度点云地图。
具体实施方式
[0101]以下结合附图对本发明进一步说明：[0102]参见图1，为本发明的系统整体框架示意图，包括里程计模块M1、回环检测模块M2、后端优化模块M3和地图生成模块M4；里程计模块输入点云帧序列和IMU，输出障碍物点云、地面点云和里程计信息；回环检测模块输入障碍物点云帧序列和IMU，输出回环信息；后端优化模块输入里程计模块、回环检测模块和GPS，输出最优位姿；地图生成模块输入最优位姿、障碍物点云和地面点云，输出高精度点云地图；
[0103]里程计模块M1包括语义分割模块M11和点云配准模块M12；
[0104]回环检测模块M2包括全局特征筛选模块M21和局部特征筛选模块M22；[0105]具体包括以下步骤：
12
CN 112362072 A[0106]
说 明 书
7/12页
步骤1，语义分割模块M11采用栅格图算法将单帧点云分割为地面点云步骤2，点云配准模块M12输入两帧点云步骤3，对输入的障碍物点云帧序列
和非
地面点云
[0107]
和IMU信息，输出两帧点云之间的刚性全局特征筛选模块M21计算每帧点云对
做精确地筛选，并
变换
[0108]
应的全局特征hi，通过全局特征之间的匹配筛选得到回环的索引集合
[0109]
步骤4，局部特征筛选模块M22通过局部特征匹配对回环索引
计算回环对应的刚性变换；
[0110]步骤5，后端优化模块M3构造基于里程计、回环检测和稀疏GPS信号的目标函数，对回环检测和GPS施加基于L1范数的鲁棒函数以增强算法对外点的鲁棒性，优化目标函数得到点云序列的最优位姿；[0111]步骤6，地图生成模块M4由最优位姿、地面点云帧序列和障碍物点云帧序列生成地面点云地图Mgrd和障碍物点云地图Mobs，分别处理后合并为点云地图M；[0112]参见图2，为本发明的栅格划分示意图，其中极坐标栅格图用于语义分割模块M11，笛卡尔栅格图用于全局特征筛选模块M21。
[0113]
极坐标栅格图计算如下：假设原始的无序点云为给定水平角分
辨率dθ和径向距离分辨率dr，可将无序点云分割为多个互不相交的栅格：
[0114][0115]
其中单个栅格是三维点集对中的单个点xm，其栅格坐标计算如
下：
[0116][0117][0118][0119][0120][0121]
其中是向上取整操作，θrm是xm的水平角和径向距离。m，
同极坐标栅格图类似，可以将点云转化为笛卡尔栅格图如下：
参见图3，为本发明的语义分割结果示意图。由图可知，语义分割可以准确地将点
计算
云划分为地面点云和障碍物点云。具体计算过程如下：针对单个栅格其Z轴的高度差特征：
[0122][0123]
根据上述特征，可构造地面点云
其中εh是阈值。
和障碍物点云
[0124]
参见图4，为本发明的点云配准流程图。点云配准模块M12给定连续两帧、经语义分
和
13
割的点云以及相应的IMU信息ux∈SE
CN 112362072 A
说 明 书
8/12页
(3)和uy∈SE(3)，需要求解两帧点云之间的相对刚性变换，n..是法向量。本发明采用基于双向对应的迭代最近点配准算法。该算法分为两步，对应关系求解和刚性变换求解。由于配准算法是局部算法，需要提供较好的初值，本发明采用IMU计算刚性变换的初值具体过程如下：
[0125]对应关系求解即给定当前的刚体变换关系，表示为二元索引反方向通过最近邻构建和对应关系二元索引
[0126][0127][0128][0129][0130]
通过最近邻搜索构建
和
和之间的对应
每对索引表示的对应关系
是空间上的同一个点；同理，从双向对应即同时满足上述两种
通过双向对应，两帧点云可以表示为一一对应的点云：
进一步的，刚性变换求解即根据对应关系求解最优刚性变换：
其中p∈(0，1]是基于lp范数的鲁棒估计，鲁棒估计可以应对交通场景中的遮挡和动态障碍物，可以极大地增强点云配准算法的精度。本发明取p＝1.0。[0131]上式可以变换为如下的带约束的优化问题：
[0132][0133][0134]
其中zi是约束变量，是约束向量，上式可通过增广拉格朗日法高效求解。
构造拉格朗日目标函数如下：
[0135][0136]
其中是拉格朗日乘子，μ是增广拉格朗日法的稀疏。增广拉格朗日法迭
代执行以下三步：
[0137][0138][0139][0140]
前两个是优化问题，最优一个是拉格朗日乘子更新。针对第一个优化问题，可将拉
格朗日函数整理为关于Z的函数：
[0141][0142]
可以看出，为最大化拉格朗日函数，仅需求解优化问题：
14
CN 112362072 A[0143]
说 明 书
9/12页
针对欧式空间中的任意向量
Operator)，且该近端估计有解析解：
[0144][0145]
上式等价于L1范数的近端估计(Proximal
[0146][0147]
因此，优化问题有解析解。
针对第二个优化问题，将拉格朗日函数整理为关于刚性变换的表达式，进一步的化简如下：
[0148][0149]
上式是标准的基于点到面距离的点云配准目标函数，已有很多成熟的求解器可
用。
参见图5，为本发明的回环检测流程图。回环检测模块M2包括全局特征筛选模块
M21和局部特征筛选模块M22；全局特征筛选模块M21通过全局特征匹配快速剔除错误回环；局部特征筛选模块M22通过局部特征对上一步中的回环做判断，并给出回环对应的刚性变换。
[0151]全局特征筛选模块M21包括全局特征构建模块和特征匹配模块；全局特征构建模块的目标是对每帧点云构建全局特征，该全局特征可以快速计算且对刚性变换具有不变性；[0152]和里程计模块类似，可以将点云按笛卡尔栅格图的方式分割为多个栅格：
[0153][0154][0155][0156][0157][0150]
将每个栅格拟合成高斯分布，其均值和协方差矩阵为：
由于协方差矩阵可以提取全局特征，将点云的NDT表征记作针对每个
协方差矩阵∑i，提取全局特征如下：[0158]f1，λλλi＝(3-2)/3[0159]f2，λλλi＝(2-1)/3[0160]f3，λλi＝1/3[0161]f4，λλλλi＝1/(1+2+3)[0162]f5，λλλi＝(3-1)/3
15
CN 112362072 A[0163][0164]
说 明 书
10/12页
[0165][0166]
其中λλλ1≤2≤3是协方差矩阵的特征值，
的八个特征向量
是Sigmoid函数。根据上式
然后对每个向量在[0，1]区间上计
可计算点云算直方图
[0167][0168]
K直方图的区间数。并将直方图聚合为全局特征如下：
基于全局特征的回环筛选通过全局特征之间的匹配快速去除错误回环。假设有N
回环筛选的具体过程如下：
帧点云帧序列对应的全局特征
[0169]
通过相邻帧构建训练集。时域上相邻的索引集合为
df是阈值。由此计算全局特征的残差集合：
[0170][0171][0172]
其中是全局特征之间的距离。
符合某一分布
*
通过最大似然法可以学习到参数θ，可以认为正确的回环应该在1-α之内，阈
阈值学习；由于相邻帧的全局特征比较类似，假设
值dLC可通过下式求解：
[0173][0174][0175][0176]
输出回环索引。测试索引集合必须满足时间差足够大，对应IMU的距离足够小：
其中tm，xt，tn，xt，tthr，dthrm分别对应第m帧点云的时间戳和IMU的平移向量，n同理。
是时间和距离阈值。最终的回环索引为：
进一步的，局部特征筛选模块M22通过局部特征匹配，对回环做进一步的筛选，并
和
[0177][0178]
给出回环之间的精确刚性变换。从回环索引中抽取两帧点云
相应的局部特征筛选具体过程如下：
[0179]
关键点提取。给定点云按空间分辨率均匀降采样得到关键点均匀降采样
会改变点云的几何形状，为减缓几何形变，必须将关键点投影到原点云上，通过最近邻搜索可以保证上述要求。点云的关键点提取和的同理。
[0180]
局部特征计算。本发明采用点云中常用的RoPS特征，对关键点的
每个点依次计算邻域、构建局部坐标系和特征描述子，生成局部特征
16
CN 112362072 A
说 明 书
11/12页
其中pi是关键点，Ci是局部坐标系，fi是特征描述子。局
部坐标系和特征描述子仅有点云的局部形状决定，对刚性变换具有不变性。点云的局部
特征计算和的同理。
[0181]
刚性变换计算。给定两帧点云的局部特征和通过特征描述子之间的最近邻
针对每个匹
搜索，可以构建局部特征之间的一一对应关系，将其写做配对，可以计算该匹配对对应的刚性变换：
[0182][0183][0184]
t＝pY，i-RpX，i
该刚性变换的质量评价采用两帧点云之间的重叠率，参照里程计模块，计算基于
输入点云可以表示为一一对应的关系，
和
双向对应的对应关系
重叠率计算如下：
[0185][0186]
其中是指示函数，当且仅当输入的条件为真时，返回1，否则返回0；εr是距离阈
值。
[0187][0188]
由局部特征筛选得到的回环索引如下：
参见图6，为本发明的全局特征结果示意图。第一行是激光雷达扫描得到的点云，
第二行是各个点云对应的全局特征。可以看出，点云A和点云B对应于不同视角下的相同场景，其全局特征较为相似。点云C和其他两帧点云在空间上是不同的场景，，其全局特征和A、B特征相差较大，说明本发明所提出的全局特征具有刚性变换不变性，可以有效地筛选回环。
[0190]参见图7，为本发明的全局特征筛选结果示意图。可以看出，通过阈值学习得到的阈值可以剔除掉绝大多数错误回环，有效提升了回环检测算法的效率。[0191]参见图8，为本发明的局部特征筛选结果示意图。第一行是点云以及局部特征之间的匹配连线，每张子图中的两帧点云来自于激光雷达不同时刻的采集数据，第二行是利用局部特征对齐后的点云。第一列的场景平移量约为18米，第二列场景旋转角度约为180度，在较大的平移和旋转下，局部特征筛选仍然可以恢复出正确的刚性变换。[0192]参见图9，为本发明的后端优化结果示意图。可以看出，融合里程计、回环和稀疏GPS信号后，位姿的累计误差明显地变小，里程计所认为的两条路被正确合并为同一条路。[0193]参见图10，为本发明的地图生成结果示意图。传统的方法不区分地面和障碍物，所生成的点云动态障碍物和地面交叠，导致地图标注无法保证效率和准确度。本发明中的地图生成模块M4分别生成地面点云和障碍物点云，可以看出，障碍物点云由于没有地面点，动态障碍物如车和人等可以清晰被人分辨出来，有利于地图的标注。[0194]参见图11，为本发明所生成的高精度点云地图。可以看出，在高动态、开放式的复杂校园和园区环境中，本发明所生成的高精度点云地图细节较为精确，满足无人驾驶导航
17
[0189]
CN 112362072 A
说 明 书
12/12页
和在线定位需求。
[0195]以上内容是结合具体的优选实施方式对本发明所作的进一步详细说明，不能认定本发明的具体实施方式仅限于此，对于本发明所属技术领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干简单的推演或替换，都应当视为属于本发明由所提交的权利要求书确定专利保护范围。
18
CN 112362072 A
说 明 书 附 图
1/4页
图1
图2
图3
19
CN 112362072 A
说 明 书 附 图
2/4页
图4
图5
图6
20
CN 112362072 A
说 明 书 附 图
3/4页
图7
图8
图9
21
CN 112362072 A
说 明 书 附 图
4/4页
图10
图11
22
因篇幅问题不能全部显示，请点此查看更多更全内容