一种抑制里程计高度漂移的激光SLAM方法

一种抑制里程计高度漂移的激光slam方法
技术领域
1.本发明属于自动驾驶技术领域，具体涉及一种抑制里程计高度漂移的激光slam方法。


背景技术：

2.随着自动驾驶技术的蓬勃发展，越来越多的企业开始将自动驾驶技术在各不相同的场景中进行应用，在这其中高精度地图是实现自动驾驶必须解决的关键问题，高精度地图不仅能够帮助车辆完成自动驾驶功能，也能够提高车辆的安全性。为了能够构建出高精度的地图，建图与定位(simultaneous localization and mapping)技术成为行业内的核心关注技术。目前主流高精度地图构建方法中使用的传感器有单目相机、双目相机、深度相机、激光雷达和毫米波雷达等，其中，激光雷达相较于其他传感器具有抗干扰能力强、探测距离远、精度高、实时性好等特点，因此广泛应用在高精度地图重建中。但是仅依靠激光雷达的现有slam算法也暴露出垂直分辨率低，易在起伏路段甚至平地上出现高度漂移的现象。
3.为解决传统slam算法的问题，zhang等提出loam算法，采用一个二维激光雷达和惯性测量单元(imu)的环境采集装置，点云匹配采用基于角、点和平面特征的匹配方法，相比于传统点云配准算法如迭代最近点(iterative closet point，icp)和正态分布变换(normal distributions transform，ndt)等提升了建图精度和匹配效率。此外loam算法使用匀速模型对点云进行运动补偿并通过松耦合的形式融合两类传感器数据提升激光雷达里程计精度，但未考虑对初始的激光点云进行数据预处理，同时也未考虑在后端部分加入回环检测进行位姿矫正，导致算法的鲁棒性不强，在建立的地图过程中容易发生漂移和重影。shan等提出轻量级lego-loam，首先在前端对点云进行预处理，通过对激光雷达线束的角度检测分割出地面点与非地面点并对非地面点进行聚类操作，大大提高点云配准效率和降低数据计算量。之后在后端部分添加基于半径搜索的回环检测算法，矫正了部分错误的点云位姿，但是在实际环境中存在回环检测效率低、鲁棒性不强、里程计高度漂移的问题。


技术实现要素：

4.针对现有技术的上述不足，本发明提供一种抑制里程计高度漂移的激光slam方法，以解决上述技术问题。
5.本发明提供一种抑制里程计高度漂移的激光slam方法，包括：
6.获取经过预处理的各类点云，提取特征并计算激光雷达里程计位移，并采用卡尔曼滤波优化位移，根据优化后的雷达里程计位移确定雷达里程计位姿，采用坡度限制算法对发生高度漂移的雷达里程计进行优化，得到更为准确的雷达里程计位姿；
7.将所述各类点云和里程计信息转换到要建立的点云地图上，提取关键帧并对所述关键帧的点云进行特征提取，在求解出关键帧里程计位姿后采用历史位姿对高度进行平滑操作，对所述雷达里程计位姿和点云地图进行优化，保存关键帧信息和里程计位姿并将加
入到因子图，得到关键帧里程计位姿因子；
8.提取基于半径搜索的scan context回环检测方法中的回环检测因子，与所述关键帧里程计位姿因子加入到isam2因子图中，通过对因子图进行图优化实现对里程计位姿和点云地图进行矫正，得到全局激光雷达里程计6-dof位姿信息及三维点云地图。
9.进一步的，所述获取经过预处理的各类点云，提取特征并计算激光雷达里程计位移，包括：
10.对经过预处理的各类点云数据进行特征提取，采用平滑度原则提取点云的平面点和边缘点；其中用于区分平面点和边缘点的待评估局部曲面的平滑度c为：
[0011][0012]
上式中，s为计算点邻域的点集合，rj、ri为集合s中点pj、pi对应的像素点到传感器的欧几里得距离；
[0013]
通过平面点特征的变化求解出里程计在k时刻相对于k-1时刻的[δtz,δθ
roll
,δθ
pich
]，其中，δtz为z轴位移单位时刻增量，δθ
roll
为横滚角单位时刻增量，δθ
pich
为俯仰角单位时刻增量；通过边缘点特征的变化求解出[δt
x
,δty,δθ
yaw
]，其中，δt
x
为x轴位移单位时刻增量，δty为y轴位移单位时刻增量，δθ
yaw
为偏航角单位时刻增量，得到里程计的增量6-dof位姿。
[0014]
进一步的，所述采用卡尔曼滤波优化位移，根据优化后的雷达里程计位移确定雷达里程计位姿，包括：
[0015]
对高度相关的δtz、δθ
roll
、δθ
pich
进行卡尔曼滤波优化，其中卡尔曼滤波器系统状态方程为xk＝x
k-1
+u
k-1
+w
k-1
；卡尔曼滤波器观测方程为：zk＝xk+vk；
[0016]
xk是状态向量，即过滤前位姿δtz、δθ
roll
、δθ
pich
，u
k-1
是k-1时刻状态控制向量，w
k-1
是系统k时刻的噪声，服从高斯分布：w
k-1
～n(0,q)，q是系统噪声协方差矩阵；zk是k时刻观测向量，即过滤后的位姿输出值，vk是k时刻观测的噪声，服从高斯分布：vk～n(0,r)，r是观测噪声的协方差矩阵；
[0017]
将优化后的里程计增量3-dof位姿[δt
x
,δty,δtz]转为平移矩阵，3-dof位姿[δθ
roll
,δθ
pich
,δθ
yaw
]转为旋转矩阵；
[0018]
通过不断的累计便可求得世界坐标系下任意时刻里程计3-dof位姿的旋转矩阵：
[0019][0020]
其中，为雷达坐标系l下k时刻里程计的增量3-dof位姿相对于k-1时刻的旋转矩阵，为世界坐标系w下k时刻里程计的增量3-dof位姿相对于原点的旋转矩阵；世界坐标系w下k-1时刻里程计的增量3-dof位姿相对于原点的旋转矩阵；
[0021]
通过反解得到世界坐标系下k时刻里程计3-dof位姿同时结合平移矩阵的累计求得
[0022]
进一步的，所述采用坡度限制方法对发生高度漂移的雷达里程计进行优化，得到更为准确的雷达里程计位姿，包括：
[0023]
根据下式计算里程计从k-1时刻到k时刻的坡度gr：
[0024][0025]
获取待建图的道路设计坡度且
[0026]
通过判断里程计从k-1时刻到k时刻的坡度gr和待建图的道路设计坡度的大小，确定是否发生高度漂移，其中，在时确定发生高度漂移；当时确定没有发生高度漂移；
[0027]
在确定发生高度漂移的情况下，对进行限制，令gr等于然后反解gr得到新并用新替换
[0028]
进一步的，所述在求解出关键帧里程计位姿后采用历史位姿对高度进行平滑操作，对所述雷达里程计位姿和点云地图进行优化，包括：
[0029]
采用关键帧的平面特征优化里程计位姿的采用边缘特征优化里程计位姿的
[0030]
将经过优化的点云和里程计信息不断的加入到地图队列中之后采用scan tomap的方式进行不断的迭代优化，优化方式为采用历史关键帧对关键帧的高度进行平滑滤波操作，具体包括：采集历史关键帧里程计的6-dof位姿信息加入到地图队列中，在发现k-1和k时刻的里程计位姿的高度差超出预设值后进行介入，利用以下公式使得k时刻里程计的高度由当前关键帧高度和历史关键帧高度组成：
[0031][0032]
其中η＝a+b+c+d，a、b、c、d为权重参数。
[0033]
进一步的，所述提取基于半径搜索的scan context回环检测方法中的回环检测因子，包括：
[0034]
在接收到来自雷达的一帧完整扫描数据后，查看其是否为关键帧，然后查询当前关键帧的半径内有无历史关键帧，若有则作为候选历史关键帧，提取此过程的半径搜索回环因子；
[0035]
将每一帧的3d点云展开为2d图像，然后建立scan context，通过引入旋转不变性的描述符ring key进行两阶段分层搜索，构建用于扫描索引key的kd树，利用最近邻域搜索算法快速找到候选候选历史关键帧与当前关键帧进行匹配，计算其ring key相似度，找出最相似的候选历史关键帧，提取此过程的scan context回环因子。
[0036]
进一步的，所述通过对因子图进行图优化实现对里程计位姿和点云地图进行矫正，得到全局激光雷达里程计6-dof位姿信息及三维点云地图，包括：
[0037]
构建优化位姿和轨迹的约束项，通过icp迭代当前关键帧、闭环关键帧的位姿变换及增量地图配准，并将姿态估计表述为最大后验问题，然后基于高斯噪声协方差，转换问题为非线性最小二乘问题，在贝叶斯树基础下利用isam2对轨迹和位姿等部分非线性因子进
行平滑及线性化，得到最终精确的全局轨迹及位姿。
[0038]
本发明的有益效果在于：本发明提供的抑制里程计高度漂移的激光slam方法，针对现有的激光slam算法在建图过程中容易发生里程计的高度漂移现象以及造成的地图扭曲现象，仅使用激光雷达就可以实现在复杂地形下的高精度建图，通过算法优化提升并实现高精度的里程计位姿和无扭曲的点云地图的效果，而非采用传统的多传感器融合的形式，以坡度限制算法和高度平滑算法为核心，融合卡尔曼滤波、回环检测和图优化等方法，为slam领域提供了一种新的解决思路。
[0039]
此外，本发明实现了在工厂等环境中可以舍弃imu、相机等传感器，仅通过激光雷达实现低成本的获取周围环境信息，有利于实现机器人大范围应用。此外，本发明设计原理可靠，结构简单，具有非常广泛的应用前景。
附图说明
[0040]
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，对于本领域普通技术人员而言，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
[0041]
图1是本发明一个实施例的方法的示意性流程图。
[0042]
图2是本发明一个实施例的数据处理方法的示意性流程图。
[0043]
图3是本发明一个实施例的前端雷达里程计位姿计算的示意性流程图。
[0044]
图4为本发明实施例提供的点云分割算法的示意图。
[0045]
图5为本发明实施例提供的里程计位姿坡度的示意图。
[0046]
图6为本发明一个实施例的后端雷达建图的示意性流程图。
具体实施方式
[0047]
为了使本技术领域的人员更好地理解本发明中的技术方案，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本发明保护的范围。
[0048]
目前多线激光雷达的垂直分辨率低，现有slam算法在室外起伏路段甚至在平路上进行建图时里程计容易出现高度漂移现象，导致地图出现扭曲和重影等问题，对此本发明实施例提出了一种抑制里程计高度漂移的激光slam方法。如图1所示，该方法包括：
[0049]
s1、数据预处理：在得到激光雷达采集的环境数据后，由分割算法标记出地面点云和非地面点云，之后由聚类算法对非地面点云进行聚类操作，最后将分割的地面点云和聚类后的非地面点云传输到s2。
[0050]
s2、前端雷达里程计位姿计算：获取经过预处理的各类点云，提取特征并计算激光雷达里程计位移，并采用卡尔曼滤波优化位移，根据优化后的雷达里程计位移确定雷达里程计位姿，采用坡度限制算法对发生高度漂移的雷达里程计进行优化，得到更为准确的雷达里程计位姿；
[0051]
s3、后端雷达建图：将所述各类点云和里程计信息转换到要建立的点云地图上，提
取关键帧并对所述关键帧的点云进行特征提取，在求解出关键帧里程计位姿后采用历史位姿对高度进行平滑操作，对所述雷达里程计位姿和点云地图进行优化，保存关键帧信息和里程计位姿并将加入到因子图，得到关键帧里程计位姿因子；
[0052]
s4、回环检测与因子图优化：提取基于半径搜索的scan context回环检测方法中的回环检测因子，与所述关键帧里程计位姿因子加入到isam2因子图中，通过对因子图进行图优化实现对里程计位姿和点云地图进行矫正，得到全局激光雷达里程计6-dof位姿信息及三维点云地图。
[0053]
本发明实施例提供的方法，包括数据预处理、前端雷达里程计、后端雷达建图和回环检测等环节，该方法以坡度限制算法和高度平滑算法为核心，融合卡尔曼滤波、回环检测和图优化等方法，对发生高度漂移的激光雷达里程计进行优化，使得系统能够输出高精度的里程计位姿和无扭曲的点云地图。
[0054]
为了便于对本发明的理解，下面以对上述s1～s4进行展开说明，对本发明提供的抑制里程计高度漂移的激光slam方法做进一步的描述。
[0055]
如图2所示，数据预处理包括：
[0056]
s11：获取激光雷达采集的环境数据；
[0057]
s12：由分割算法标记出所述环境数据的地面点云和非地面点云；
[0058]
s13：由聚类算法对所述非地面点云进行聚类操作；
[0059]
s14：将分割得到的地面点云和聚类后的非地面点云作为所述经过预处理的各类点云。
[0060]
具体地，由于市面上采用的是多线激光雷达并且是将雷达固定安装于车辆上，那么激光雷达距离地面的高度为恒定的，因此分割算法可以通过激光雷达的线数实现。如图3所示，本实施例采用的rs-16激光雷达垂直方向有16个发射器，垂直角分辨率则为0.2
°
，水平视场角为360
°
，水平角分辨率为0.2
°
，那么各发射器每圈返回点数为1800个。设定在t时刻获取的点云p
t
＝{p1,p2,p3,...,pn}，pi是t时刻的点云p
t
中的一个点，1≤i≤n。将该点云投影到一个距离图像中，则其分辨率为1800
×
16。rs-16激光雷达垂直方向扫描范围是[-15
°
，15
°
]，那么地面出现在[-15
°
，-1
°
]之间，本实施例设定地面出现在第7线之下，对0-7线束的点云求解上下两线之间的俯仰角度(即图3中的角α)并进行地平面估计，本实施例规定α角度在10
°
范围内的点云为地面点，其他点云则设定为非地面点。至此，实现了获取激光雷达采集的环境数据，由分割算法标记出所述环境数据的地面点云和非地面点云。
[0061]
聚类操作采用bfs(breadth-first-search)聚类算法对非地面点进行聚类操作，以pi为中心向外搜索。本实施例设定如果聚类点云超过30个则标定为一个可用聚类；如果聚类点云小于30但大于等于5，则统计其垂直方向上的点云数，如果大于3则也标定为一个可用聚类。通过聚类操作可以有效降低点云中动态物体(行人、车辆)和树叶等小物体的干扰，然后可以对聚类后的非地面点和分割出的地面点进行下一步的处理。因此，本发明实施例提供的数据预处理方法可以实现降低点云数据量，提高后续算法的运算速度。
[0062]
如图4所示，前端雷达里程计环节，主要目的是提取里程计6-dof位姿数据。具体包括：
[0063]
s21：采用平滑度原则进行特征提取。
[0064]
首先对经过预处理的各类点云数据进行特征提取，依据平滑度原则将特征点划分
为两大类：平面点和边缘点，因此本发明进行特征提取时采用平滑度原则提取点云的平面点和边缘点。
[0065]
其中，用于区分平面点和边缘点的待评估局部曲面的平滑度c为：
[0066][0067]
上式中，s为计算点邻域的点集合，在本实施例中设置s为10。rj、ri为集合s中点pj、pi对应的像素点到传感器的欧几里得距离。
[0068]
s22：通过平面点特征和边缘点特征计算里程计的增量6-dof位姿。
[0069]
之后通过平面点特征的变化求解出里程计在k时刻相对于k-1时刻的[δtz,δθ
roll
,δθ
pich
]，其中，δtz为z轴位移单位时刻增量，δθ
roll
为横滚角单位时刻增量，δθ
pich
为俯仰角单位时刻增量，通过边缘点特征的变化求解出[δt
x
,δty,δθ
yaw
]，其中，δt
x
为x轴位移单位时刻增量，δty为y轴位移单位时刻增量，δθ
yaw
为偏航角单位时刻增量，这样就可以得到里程计的增量6-dof位姿[δtz,δt
x
,δty,δθ
yaw
,δθ
roll
,δθ
pich
]。
[0070]
s23：对里程计的增量6-dof位姿进行卡尔曼滤波优化。
[0071]
由于是简单的特征点粗匹配，同时激光雷达在水平面上的误差较小，主要在高度上有漂移的情况，故不对水平位置相关的δt
x
、δty、δθ
yaw
进行优化，只对高度相关的δtz、δθ
roll
、δθ
pich
进行卡尔曼滤波优化：
[0072]
xk＝x
k-1
+u
k-1
+w
k-1
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(2)
[0073]
zk＝xk+vkꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(3)
[0074]
其中，公式(2)为卡尔曼滤波器系统状态方程，公式(3)为卡尔曼滤波器观测方程。xk是k时刻的状态向量，即过滤前位姿δtz、δθ
roll
、δθ
pich
，u
k-1
是k-1时刻状态控制向量，u
k-1
是k-1时刻的状态控制向量，w
k-1
是k-1时刻的控制系统的噪声，服从高斯分布：w
k-1
～n(0,q)，q是系统噪声协方差矩阵，通过传感器校准离线获得。zk是k时刻的观测向量，即过滤后的位姿输出值，vk是k时刻的观测的噪声，服从高斯分布：vk～n(0,r)，r是观测噪声的协方差矩阵。
[0075]
本实施例设定在对δtz处理时q为0.5、r为1；对δθ
roll
、δθ
pich
处理时q为0.001、r为0.005。
[0076]
s24：根据卡尔曼滤波优化后的里程计增量3-dof位姿计算里程计6-dof位姿。
[0077]
将卡尔曼滤波优化后的里程计增量3-dof位姿[δt
x
,δty,δtz]转为平移矩阵，将3-dof位姿[δθ
roll
,δθ
pich
,δθ
yaw
]转为旋转矩阵。由于里程计的起始点为世界坐标系的原点，那么通过公式(4)不断的累计便可求得世界坐标系下任意时刻里程计3-dof位姿的旋转矩阵：
[0078][0079]
其中，为雷达坐标系l下k时刻里程计的增量3-dof位姿相对于k-1时刻的旋转矩阵，为世界坐标系w下k时刻里程计的增量3-dof位姿相对于原点的旋转矩阵；世界坐标系w下k-1时刻里程计的增量3-dof位姿相对于原点的旋转矩阵；通过反解得到世界坐标系下k时刻里程计3-dof位姿同时结合平移矩阵的累计求得
至此，求出前端里程计的6-dof位姿，本发明实施例提供的激光slam方法，解决了里程计在高度上有漂移的情况，减少了激光雷达里程计的位姿误差。
[0080]
但前端里程计推测的位姿容易受到地形的影响发生高度漂移，为了降低里程计在高度上的漂移，通常道路在建设时会规定一系列建设指标，其中坡度就是重要的一项指标，且普通高速公路的最大坡度指标为5％，而前端里里程计在发生高度漂移后的轨迹坡度值不在指标范围内。
[0081]
为解决上述问题，本发明实施例还包括：
[0082]
s25：采用坡度限制的方法对建图过程中的里程计进行优化。具体如下：
[0083]
如图5所示，a点为k-1时刻里程计位姿b点为k时刻位姿bc为里程计的垂直位移，ac为水平位移。
[0084]
路面坡度即坡面的垂直高度δh和水平距离δl的百分比，用gr表示：
[0085][0086]
结合图5和公式(5)可得到里程计从k-1时刻到k时刻的坡度。
[0087][0088]
其中，为k时刻x轴位姿，为k-1时刻x轴位姿；z轴、y轴同理，在此不再赘述。
[0089]
获取待建图的道路设计坡度且通过判断里程计从k-1时刻到k时刻的坡度gr和待建图的道路设计坡度的大小，确定是否发生高度漂移，其中，在时确定发生高度漂移；当时确定没有发生高度漂移；在确定发生高度漂移的情况下，对进行限制，令gr等于然后反解gr得到新并用新替换
[0090]
如图6所示，后端雷达建图的目的是建立地图并确定图因子。具体包括：
[0091]
s31：提取关键帧并获取关键帧的里程计位姿。
[0092]
首先将预处理后的各类点云和前端里程计信息转换到要建立的地图上，然后依据一系列的历史雷达帧提取出关键帧，之后采用关键帧的平面特征优化里程计位姿的[tz,θ
roll
,θ
pich
]信息，采用边缘特征优化[t
x
,ty,θ
yaw
]信息。
[0093]
将经过优化的点云和里程计信息不断的加入到地图队列中之后采用scan to map的方式进行不断的迭代优化。虽然经过scan to map的优化后的里程计位姿相较于前端里程计更为精准，但是这一结果建立在前端里程计不发生高度漂移的情况下，如果前端给定的数据有误，即使经过scan to map的优化，里程计依然会出发现高度漂移的现象，因此本发明实施例采用平滑滤波的思想，对发生高度漂移的里程计部分进行平滑优化。
[0094]
s32：利用高度平滑滤波算法对关键帧的里程计位姿进行平滑优化。具体地，高度平滑滤波算法如下：
[0095]
首先需要不断采集历史关键帧里程计的6-dof位姿信息将其加入到地图队列中，然后在发现k-1和k时刻的里程计位姿的高度差超出0.1m后进行介入，利用公式(7)使得k时刻里程计的高度由当前帧高度和历史帧高度组成：
[0096][0097]
其中η＝a+b+c+d。
[0098]
关于权重a、b、c、d的取值，经过多次实验发现，在不同道路环境下选取不同的权重参数得到的建图效果最好。在较为平坦道路环境中，取a、b、c、d为9、5、3、1；在较为崎岖的道路环境中，取a、b、c、d为3、5、3、1。在经过高度平滑滤波后，便可以抑制里程计高度漂移的发生，进而减少地图扭曲现象。最后保存经过优化的关键帧和里程计因子，让其用于后续的回环检测和因子图优化方法。
[0099]
本发明实施例提供的回环检测方法与因子图优化方法，主要是为了抑制slam过程中出现的里程计高度漂移现象，实现在复杂环境下也可以完成回环检测并实现高效稳定建图。
[0100]
具体地，回环检测线程在接收到来自激光雷达的一帧完整环境数据后，历史关键帧半径搜索的回环检测算法会先通过查看其是否为关键帧，然后查询当前关键帧的半径(可以为距离当前关键帧7m或过去30s内)有无历史关键帧；将每一帧的3d点云展开为2d图像，然后建立scan context，通过引入旋转不变性的描述符ring key进行两阶段分层搜索，构建用于扫描索引key的kd树，使用最近邻域搜索算法找到候选历史关键帧与当前关键帧进行匹配，通过计算其点云相似度(例如ring key相似度)找出最相似的关键帧，实现回环检测。
[0101]
本发明实施例提供的回环检测方法，与仅能在简单环境实现回环的历史关键帧半径搜索回环方法不同，scan context回环检测方法可以实现复杂环境下的回环，并在运行速度上更快。本发明实施例提供的回环检测方法以scan context回环为主，关键帧半径搜索回环为辅，在经过两种回环方法的匹配后，两种回环因子会加入到因子图中进行进一步优化。本发明实施例提供的激光slam方法具有回环检测矫正了部分偏差的轨迹，使得全局里程计没有那么大的偏差，与真实轨迹的贴合程度更高。
[0102]
因子图优化方法的实施，首先对先验因子进行初始化，在因子图中逐步添加雷达里程计因子、半径搜索回环因子、scan context回环因子，构建优化位姿和轨迹的约束项，之后通过icp迭代当前关键帧、闭环关键帧的位姿变换及增量地图配准，并将姿态估计表述为最大后验(map)问题，然后基于高斯噪声协方差，转换问题为非线性最小二乘问题，在贝叶斯树基础下利用isam2对轨迹和位姿等部分非线性因子进行平滑及线性化，得到最终精确的全局轨迹及位姿。因本发明实施例在lego-loam原有回环检测部分中引入了空间点云描述符scan context的回环因子，组成了基于历史关键帧半径搜索因子和scan context因子的回环检测算法，并通过isam2进行全局一致性优化。
[0103]
尽管通过参考附图并结合优选实施例的方式对本发明进行了详细描述，但本发明并不限于此。在不脱离本发明的精神和实质的前提下，本领域普通技术人员可以对本发明的实施例进行各种等效的修改或替换，而这些修改或替换都应在本发明的涵盖范围内/任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应所述以权利要求的保护范围为准。

技术特征：
1.一种抑制里程计高度漂移的激光slam方法，其特征在于，包括：获取经过预处理的各类点云，提取特征并计算激光雷达里程计位移，并采用卡尔曼滤波优化位移，根据优化后的雷达里程计位移确定雷达里程计位姿，采用坡度限制算法对发生高度漂移的雷达里程计进行优化，得到更为准确的雷达里程计位姿；将所述各类点云和里程计信息转换到要建立的点云地图上，提取关键帧并对所述关键帧的点云进行特征提取，在求解出关键帧里程计位姿后采用历史位姿对高度进行平滑操作，对所述雷达里程计位姿和点云地图进行优化，保存关键帧信息和里程计位姿并将加入到因子图，得到关键帧里程计位姿因子；提取基于半径搜索的scan context回环检测方法中的回环检测因子，与所述关键帧里程计位姿因子加入到isam2因子图中，通过对因子图进行图优化实现对里程计位姿和点云地图进行矫正，得到全局激光雷达里程计6-dof位姿信息及三维点云地图。2.根据权利要求1所述的方法，其特征在于，所述获取经过预处理的各类点云，提取特征并计算激光雷达里程计位移，包括：对经过预处理的各类点云数据进行特征提取，采用平滑度原则提取点云的平面点和边缘点；其中用于区分平面点和边缘点的待评估局部曲面的平滑度c为：上式中，s为计算点邻域的点集合r
j
、r
i
，为集合s中点p
j
、p
i
对应的像素点到传感器的欧几里得距离；通过平面点特征的变化求解出里程计在k时刻相对于k-1时刻的[δt
z
,δθ
roll
,δθ
pich
]，其中，δt
z
为z轴位移单位时刻增量，δθ
roll
为横滚角单位时刻增量，δθ
pich
为俯仰角单位时刻增量；通过边缘点特征的变化求解出[δt
x
,δt
y
,δθ
yaw
]，其中，δt
x
为x轴位移单位时刻增量，δt
y
为y轴位移单位时刻增量，δθ
yaw
为偏航角单位时刻增量。3.根据权利要求2所述的方法，其特征在于，所述采用卡尔曼滤波优化位移，根据优化后的雷达里程计位移确定雷达里程计位姿，包括：对高度相关的δt
z
、δθ
roll
、δθ
pich
进行卡尔曼滤波优化，其中，卡尔曼滤波器系统状态方程为x
k
＝x
k-1
+u
k-1
+w
k-1
；卡尔曼滤波器观测方程为：z
k
＝x
k
+v
k
；x
k
是k时刻状态向量，即过滤前位姿δt
z
、δθ
roll
、δθ
pich
，u
k-1
是k-1时刻状态控制向量，w
k-1
是系统k时刻的噪声，服从高斯分布：w
k-1
～n(0,q)，q是系统噪声协方差矩阵；z
k
是k时刻观测向量，即过滤后的位姿输出值，v
k
是k时刻观测的噪声，服从高斯分布：v
k
～n(0,r)，r是观测噪声的协方差矩阵。将优化后的里程计增量3-dof位姿[δt
x
,δt
y
,δt
z
]转为平移矩阵，3-dof位姿[δθ
roll
,δθ
pich
,δθ
yaw
]转为旋转矩阵；通过不断的累计便可求得世界坐标系下任意时刻里程计3-dof位姿的旋转矩阵：其中，为雷达坐标系l下k时刻里程计的增量3-dof位姿相对于k-1时刻的旋转矩阵，为世界坐标系w下k时刻里程计的增量3-dof位姿相对于原点的旋转矩阵；世界坐标
系w下k-1时刻里程计的增量3-dof位姿相对于原点的旋转矩阵；通过反解得到世界坐标系下k时刻里程计3-dof位姿同时结合平移矩阵的累计求得4.根据权利要求3所述的方法，其特征在于，所述采用坡度限制方法对发生高度漂移的雷达里程计进行优化，得到更为准确的雷达里程计位姿，包括：根据下式计算里程计从k-1时刻到k时刻的坡度g
r
：获取待建图的道路设计坡度且通过判断里程计从k-1时刻到k时刻的坡度g
r
和待建图的道路设计坡度的大小，确定是否发生高度漂移，其中，在时确定发生高度漂移；当时确定没有发生高度漂移；在确定发生高度漂移的情况下，对k时刻的z轴位移进行限制，令g
r
等于然后反解g
r
得到k时刻的z轴新位移并用新替换5.根据权利要求1所述的方法，其特征在于，所述在求解出关键帧里程计位姿后采用历史位姿对高度进行平滑操作，对所述雷达里程计位姿和点云地图进行优化，包括：采用关键帧的平面特征优化里程计位姿的采用边缘特征优化里程计位姿的将经过优化的点云和里程计信息不断的加入到地图队列中之后采用scan tomap的方式进行不断的迭代优化，优化方式为采用历史关键帧对关键帧的高度进行平滑滤波操作，具体包括：采集历史关键帧里程计的6-dof位姿信息加入到地图队列中，在发现k-1和k时刻的里程计位姿的高度差超出预设值后进行介入，利用以下公式使得k时刻里程计的高度由当前关键帧高度和历史关键帧高度组成：其中η＝a+b+c+d，a、b、c、d为权重参数。6.根据权利要求1所述的方法，其特征在于，所述提取基于半径搜索的scan context回环检测方法中的回环检测因子，包括：在接收到来自雷达的一帧完整扫描数据后，查看其是否为关键帧，然后查询当前关键帧的半径内有无历史关键帧，若有则作为候选历史关键帧，提取此过程的半径搜索回环因子；将每一帧的3d点云展开为2d图像，然后建立scan context，通过引入旋转不变性的描述符ring key进行两阶段分层搜索，构建用于扫描索引key的kd树，利用最近邻域搜索算法快速找到候选候选历史关键帧与当前关键帧进行匹配，计算其ring key相似度，找出最相
似的候选历史关键帧，提取此过程的scan context回环因子。7.根据权利要求1所述的方法，其特征在于，所述通过对因子图进行图优化实现对里程计位姿和点云地图进行矫正，得到全局激光雷达里程计6-dof位姿信息及三维点云地图，包括：构建优化位姿和轨迹的约束项，通过icp迭代当前关键帧、闭环关键帧的位姿变换及增量地图配准；并将姿态估计表述为最大后验问题，然后基于高斯噪声协方差，转换问题为非线性最小二乘问题，在贝叶斯树基础下利用isam2对轨迹和位姿等部分非线性因子进行平滑及线性化，得到最终精确的全局轨迹及位姿。

技术总结
本发明提供一种抑制里程计高度漂移的激光SLAM方法，包括：获取经过预处理的各类点云，采用卡尔曼滤波优化位移，根据优化后的雷达里程计位移确定雷达里程计位姿，采用坡度限制算法对发生高度漂移的雷达里程计进行优化；将所述各类点云和里程计信息转换到要建立的点云地图上，提取关键帧并对所述关键帧的点云进行特征提取，在求解出关键帧里程计位姿后采用历史位姿对高度进行平滑操作，提取基于半径搜索的Scan Context回环检测方法中的回环检测因子，通过对因子图进行图优化，得到全局激光雷达里程计6-DOF位姿信息及三维点云地图。本发明以坡度限制算法和高度平滑算法为核心，实现高精度的里程计位姿和无扭曲的点云地图的效果。果。果。


技术研发人员：秦毫 邹艳丽 庾国梁 刘会鹏 谭宇飞
受保护的技术使用者：广西师范大学
技术研发日：2023.07.26
技术公布日：2023/10/15