CN117007061A - 一种用于无人驾驶平台的基于地标的激光slam方法 - Google Patents

Abstract

本发明涉及一种用于无人驾驶平台的基于地标的激光SLAM方法，属于无人驾驶技术领域。本发明通过激光雷达的点云数据预处理、针对非地面点云建立上下层凸包、评估凸包相似性并利用改进迭代步长的坐标轮换法求解目标优化函数，从而得到无人驾驶平台非平面的位姿态变化量。针对地面点云，通过前后两帧中两个平面法向量之间的角度变化，确定无人驾驶平台的俯仰角变化量，联合求解出垂直方向变化量，从而得到六自由度位姿变化量。最终将地标点云和地面点云转换到全局坐标系下，并对地图进行占据栅格体素化。相比于现有技术，本发明在定位方面精度更高，并且能够实现对物体的分割，提供更丰富的环境信息，进而提高机器人的感知和交互能力。

Description

一种用于无人驾驶平台的基于地标的激光SLAM方法

技术领域

本发明属于无人驾驶技术领域，涉及一种用于无人驾驶平台的基于地标的激光SLAM方法。

背景技术

无人驾驶技术的实现主要依赖于对环境的精准定位以及对周围环境的高效感知。当前，广泛使用的定位技术包括卫星定位系统和组合导航定位系统。然而，卫星定位系统在高楼林立或树木茂盛的环境下，其定位信号易受到干扰，信号弱化或阻塞，这将会导致定位误差较大，甚至会失效。因此，如何在不依赖卫星定位系统的情况下完成无人驾驶平台的精准定位，成为了一项严峻的挑战。

为了解决这一问题，许多研究人员和工程师开始尝试使用车载激光雷达技术进行定位和导航。车载激光雷达是一种能够精确感知车辆周围环境，且有响应速度快、测距距离长等优点，使其在无人驾驶平台上受到了广泛关注。激光SLAM(同时定位和地图构建)是一种被广泛应用于无人驾驶平台自主导航和定位的技术。激光SLAM通过激光雷达获取车辆周围环境的三维点云数据，并通过跟踪这些点云数据的变化，计算出车辆的移动距离和方向，从而实现无人驾驶车辆的自主定位和导航。

然而，现有的激光SLAM定位方法主要存在以下两个问题：

(1)定位精度有限：传统的激光SLAM方法依赖的里程计常常会产生较大的累积误差，从而导致定位精度有限。

(2)地图信息不足：在构建环境地图时，传统的激光SLAM方法通过点云描述环境，但往往缺乏对实体物体的分割，导致建立的地图信息不够丰富，交互能力弱。

发明内容

有鉴于此，本发明的目的在于提供一种用于无人驾驶平台的基于地标的激光SLAM方法，提升定位精度并建立信息丰富的环境地图，从而为无人驾驶技术的实现提供更强的支持。

为达到上述目的，本发明提供如下技术方案：

一种用于无人驾驶平台的基于地标的激光SLAM方法，该方法包括以下步骤：

S1：对激光雷达的三维点云数据进行预处理；针对单帧的三维点云数据的预处理包括：地面和非地面点云分割、非地面点云聚类、去除统计学上的离群点；针对前后两帧去除离群点后的类进行地标的初始配对，得到非地面点云中的地标；

S2：针对非地面点云，每一对完成初始配对的地标进行分层，根据地标中三维点云z的均值对地标分为上下两层，并利用Graham扫描法分别建立上下层凸包；

S3：通过使用旋转曲线函数差值和霍夫距离建立一对凸包相似性的综合评价指标，对上下层凸包的相似性进行评估；根据综合评价指标的大小选择出上下层中最相似的一对凸包；

S4：针对完成初始配对的地标，通过使所有最相似凸包对的重叠面积之和最大、构建以无人驾驶平台非平面的位姿变化量的目标优化函数；

S5：使用改进迭代步长的坐标轮换法求解目标优化函数，完成无人驾驶平台非平面的位姿变化量的求解；

S6：针对地面点云，选择激光雷达原点某一邻域范围内的前视点云和后视点云分别进行平面拟合；

S7：通过前后两帧中两个平面法向量之间的角度变化，确定无人驾驶平台的俯仰角变化量；

S8：根据非平面的位姿态变化量和俯仰角变化量联合求解出垂直方向变化量，得到前后两帧点云匹配得到的六自由度位姿变化量；

S9：根据解算出的六自由度位姿变化量，将地标点云和地面点云转换到全局坐标系下，并对地图进行占据栅格体素化。

可选的，所述S1包括：

S1-1：对单帧的激光雷达三维点云数据进行预处理，具体包括：使用Ray GroundFilter方法实现地面和非地面点云的分割，对分割后的非地面点云使用欧式聚类的方法进行空间聚类，对聚类后的点云使用基于统计点云的方法去除类中的离群点，得到去除离群点的类的集合；

S1-2：针对前后两帧地标的初始配对，具体包括：分别求解k-1和k时刻类的重心位置，分别计算k-1时刻中、类的重心到k时刻类的重心距离的最小值，并对距离最小值进行平均；如果在k-1和k时刻的地标中，某对类的重心之间的距离的最小值小于平均值，则认为该对类满足地标配对，称这两个类分别是k-1和k时刻中的一个地标。

可选的，所述S2包括：

S2-1：对完成初始配对的地标进行分层；具体包括：计算地标对点云在z方向的和、并对其进行平均，得到该地标对的分层阈值；地标的垂直方向的均值小于分层阈值定义为第一层分层类；反之，称为第二层分层类；

S2-2：将第一层分层类和第二层分层类分别投影到x-o-y平面上，使用Graham扫描法算法实现上下层凸包的建立；k-1时刻第一层分层类对应的凸包是k-1时刻第二层分层类对应的凸包是/>k时刻第一层分层类对应的凸包是/>k时刻第二层分层类对应的凸包是/> 和/>是第一层凸包对；/>和/>是第二层凸包对。

可选的，所述S3包括：

S3-1：求凸包的旋转曲线函数；具体包括：求凸包/>的边长并将边长进行归一化，选择距离原点最近的凸包顶点作为初始点，选初始点两侧边的外角作为初始角度，沿凸包边缘逆时针方向求下一个拐点两侧边的外角；根据凸包归一化的边长和外角可以得到凸包的转角函数曲线，其中转角函数曲线的横坐标是是每一个归一化的边长值的累加，横坐标的取值范围[0,1]，纵坐标是凸包顶点外角的累加，纵坐标的取值范围[0,2π]；

S3-2：求凸包旋转曲线函数差值；用旋转曲线函数的差值衡量凸包和/>的相似性，即/>得到衡量凸包/>和/>的相似程度的差值指标/>同理，可得凸包/>和/>的相似程度的差值指标/>

S3-3：求解凸包之间的霍夫距离；针对凸包和/>计算/>中的顶点到中顶点最近点的距离，然后找出这些距离中的最大值，得到有向Hausdorff距离同理，针对凸包/>和/>计算/>中的顶点到/>中顶点最近点的距离，然后找出这些距离中的最大值，得到有向Hausdorff距离/>

S3-4：求解凸包对的相似性综合评价指标；根据相似程度的差值指标和有向Hausdorff距离/>计算凸包对/>和/>的相似性的综合评价指标/>其中，α＝0.5，β＝0.5；同理，根据相似程度的差值指标/>和有向Hausdorff距离计算凸包对/>和/>的相似性的综合评价指标其中，α＝0.5，β＝0.5；

S3-5：根据综合评价指标选取最相似的对凸包；在综合评价指标和/>中选取数值小的作为地标相似性的综合评价指标同时，选取与之对应的凸包/>和/>解算非平面的位姿变化量，其中sel∈{fir,sec}。

可选的，所述S4包括：

S4-1：计算凸包经过ΔT的欧式变换后与凸包/>的重叠面积其中，变化量ΔT＝(Δt_x,Δt_y,Δt_θ)/>是无人驾驶平台位姿的变化量，Δt_x和Δt_y分别表示在横纵坐标的平移变化量，Δt_θ表示航向角的变化量，/>表示/>经过ΔT的欧式变换后形成的凸包，表示两个凸包相交形成的逆时针方向有序的凸包；Area(·)表示计算两个凸包重叠面积的函数；

S4-2：构建无条件约束的目标函数其中，N是前后两帧配对的路标的个数；/>表示在变化量ΔT的情况下，每一对凸包重叠面积之和，G(ΔT)是目标函数值；

S4-3：构建最优迭代步长其中，α表示在迭代循环中步长的整数倍系数，ceil(·)表示大于等于函数值(·)的取整函数，表示第N对相似凸包对/>和/>中面积最小的凸包面积之和。

可选的，所述S5包括：

S5-1：采用坐标轮换法，依次沿三个正交坐标轴的方向进行目标函数最大值的搜索其中搜索方向取坐标轴方向，即

S5-2：若进行下一轮搜索；否则，/>T^*即为k-1时刻到k时刻无人驾驶平台运动位姿态的变化量。

可选的，所述S6包括：

S6-1：针对地面点云，选择激光雷达原点某一邻域范围内的前视点云和后视点云分别进行RANSCAN平面拟合；其中前视点云表示是在激光雷达坐标系下x大于0的点云，后视点云表示是在x小于0的点云；前视点云和后视点云拟合出的平面对应的法向量分别为n_f(a_f,b_f,c_f)和n_l(a_l,b_l,c_l)。

可选的，所述S7包括：

S7-1：连续解算这两个法向量的夹角，同时要求夹角的角度必须是锐角；

S7-2：当夹角满足设定的阈值大小条件，并且在连续的变化中存在最大值时，通过这两个法向量之间的角度差，确定无人驾驶平台的俯仰角变化量其中，“·”表示向量的点积，||n||代表向量n的模，即向量的长度。

可选的，所述S8包括：

S8-1：假设无人驾驶平台侧倾角的变化量是

S8-2：根据空间位姿的变化量的关系解算出垂直方向变化量Δz_k；其中，Δz_k是垂直方向变化量，Δψ_k是俯仰角变化量，Δθ_k是偏航角的变化量，Δx_k和Δy_k分别为横纵坐标的平移变化量。

可选的，所述S9包括：

S9-1：根据解算出的六自由度位姿变化量，将地标点云和地面点云转换到全局坐标系下，并对地图进行占据栅格体素化。

本发明的有益效果在于：

(1)提高定位精度：通过使用基于地标的方法，可以显著降低因里程计产生的累积误差，从而提高了定位精度。这使得无人驾驶车辆在进行自主导航时，能够更精准地确定自身的位置，从而提高了驾驶的安全性和效率。

(2)构建更加信息丰富的地图：相较于传统的激光SLAM方法仅通过点云描述环境，本方案能够对实体物体进行更细致的分割，并据此建立丰富的地图信息。这样一来，不仅提高了地图的信息含量，也增强了与环境的交互能力。

本发明的其他优点、目标和特征在某种程度上将在随后的说明书中进行阐述，并且在某种程度上，基于对下文的考察研究对本领域技术人员而言将是显而易见的，或者可以从本发明的实践中得到教导。本发明的目标和其他优点可以通过下面的说明书来实现和获得。

附图说明

为了使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明作优选的详细描述，其中：

图1为本发明流程图；

图2为基于KITTI00、KITTI02以及KITTI03数据集，本发明所解算的轨迹与真实轨迹的对比；

图3为基于KITTI04、KITTI05、KITTI06以及KITTI07数据集，本发明所解算的轨迹与真实轨迹的对比；

图4为基于KITTI08、KITTI09以及KITTI10数据，本发明解算的轨迹与真实轨迹的对比。

具体实施方式

以下通过特定的具体实例说明本发明的实施方式，本领域技术人员可由本说明书所揭露的内容轻易地了解本发明的其他优点与功效。本发明还可以通过另外不同的具体实施方式加以实施或应用，本说明书中的各项细节也可以基于不同观点与应用，在没有背离本发明的精神下进行各种修饰或改变。需要说明的是，以下实施例中所提供的图示仅以示意方式说明本发明的基本构想，在不冲突的情况下，以下实施例及实施例中的特征可以相互组合。

其中，附图仅用于示例性说明，表示的仅是示意图，而非实物图，不能理解为对本发明的限制；为了更好地说明本发明的实施例，附图某些部件会有省略、放大或缩小，并不代表实际产品的尺寸；对本领域技术人员来说，附图中某些公知结构及其说明可能省略是可以理解的。

本发明实施例的附图中相同或相似的标号对应相同或相似的部件；在本发明的描述中，需要理解的是，若有术语“上”、“下”、“左”、“右”、“前”、“后”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此附图中描述位置关系的用语仅用于示例性说明，不能理解为对本发明的限制，对于本领域的普通技术人员而言，可以根据具体情况理解上述术语的具体含义。

如图1所示，本发明一种用于无人驾驶平台的基于地标的激光SLAM方法包括以下步骤：

S1，对激光雷达的三维点云数据进行预处理。针对单帧的三维点云数据的预处理包括：地面和非地面点云分割、非地面点云聚类、去除统计学上的离群点。针对前后两帧去除离群点后的类进行地标的初始配对，得到非地面点云中的地标。

S2，针对非地面点云，每一对完成初始配对的地标进行分层，根据地标中三维点云z的均值对地标分为上下两层，并利用Graham扫描法分别建立上下层凸包。

S3，通过使用旋转曲线函数差值和霍夫距离建立一对凸包相似性的综合评价指标，对上下层凸包的相似性进行评估。根据综合评价指标的大小选择出上下层中最相似的一对凸包。

S4，针对完成初始配对的地标，通过使所有最相似凸包对的重叠面积之和最大、构建以无人驾驶平台非平面的位姿变化量的目标优化函数。

S5，使用改进迭代步长的坐标轮换法求解目标优化函数，从而完成无人驾驶平台非平面的位姿变化量的求解。

S6，针对地面点云，选择激光雷达原点某一邻域范围内的前视点云和后视点云分别进行平面拟合。

S7，通过前后两帧中两个平面法向量之间的角度变化，从而确定无人驾驶平台的俯仰角变化量。

S8，根据非平面的位姿态变化量和俯仰角变化量联合求解出垂直方向变化量，进而得到前后两帧点云匹配得到的六自由度位姿变化量。

S9，根据解算出的六自由度位姿变化量，将地标点云和地面点云转换到全局坐标系下，并对地图进行占据栅格体素化。

S1具体包括

1-1对单帧的激光雷达三维点云数据进行预处理，具体包括：使用Ray GroundFilter方法实现地面和非地面点云的分割，对分割后的非地面点云使用欧式聚类的方法进行空间聚类，对聚类后的点云使用基于统计点云的方法去除类中的离群点，得到去除离群点的类的集合。

1-2针对前后两帧地标的初始配对，具体包括：分别求解k-1和k时刻类的重心位置，分别计算k-1时刻中、类的重心到k时刻类的重心距离的最小值，并对距离最小值进行平均。如果在k-1和k时刻的地标中，某对类的重心之间的距离的最小值小于平均值，则认为该对类满足地标配对，称这两个类分别是k-1和k时刻中的一个地标。

S2具体包括

2-1对完成初始配对的地标进行分层。具体包括：计算地标对点云在z方向的和、并对其进行平均，得到该地标对的分层阈值。地标的垂直方向的均值小于分层阈值定义为第一层分层类；反之，称为第二层分层类。

2-2将第一层分层类和第二层分层类分别投影到x-o-y平面上，使用Graham扫描法算法实现上下层凸包的建立。k-1时刻第一层分层类对应的凸包是k-1时刻第二层分层类对应的凸包是/>k时刻第一层分层类对应的凸包是/>k时刻第二层分层类对应的凸包是/> 和/>是第一层凸包对；/>和/>是第二层凸包对。

S3具体包括

3-1求凸包的旋转曲线函数。具体包括：求凸包/>的边长并将边长进行归一化，选择距离原点最近的凸包顶点作为初始点，选初始点两侧边的外角作为初始角度，沿凸包边缘逆时针方向求下一个拐点两侧边的外角。根据凸包归一化的边长和外角可以得到凸包的转角函数曲线，其中转角函数曲线的横坐标是是每一个归一化的边长值的累加，横坐标的取值范围[0,1]，纵坐标是凸包顶点外角的累加，纵坐标的取值范围[0,2π]。

3-2求凸包旋转曲线函数差值。用旋转曲线函数的差值衡量凸包和/>的相似性，即/>得到衡量凸包/>和/>的相似程度的差值指标/>同理，可得凸包/>和/>的相似程度的差值指标

3-3求解凸包之间的霍夫距离。针对凸包和/>计算/>中的顶点到中顶点最近点的距离，然后找出这些距离中的最大值，得到有向Hausdorff距离同理，针对凸包/>和/>计算/>中的顶点到/>中顶点最近点的距离，然后找出这些距离中的最大值，得到有向Hausdorff距离/>

3-4求解凸包对的相似性综合评价指标。根据相似程度的差值指标和有向Hausdorff距离/>计算凸包对/>和/>的相似性的综合评价指标其中，α＝0.5，β＝0.5。同理，根据相似程度的差值指标/>和有向Hausdorff距离/>计算凸包对/>和/>的相似性的综合评价指标其中，α＝0.5，β＝0.5。

3-5根据综合评价指标选取最相似的对凸包。在综合评价指标和中选取数值小的作为地标相似性的综合评价指标/>同时，选取与之对应的凸包/>和/>解算非平面的位姿变化量，其中sel∈{fir,sec}。

S4具体包括

4-1计算凸包经过ΔT的欧式变换后与凸包/>的重叠面积其中，变化量ΔT＝(Δt_x,Δt_y,Δt_θ)/>是无人驾驶平台位姿的变化量，Δt_x和Δt_y分别表示在横纵坐标的平移变化量，Δt_θ表示航向角的变化量，/>表示/>经过ΔT的欧式变换后形成的凸包，表示两个凸包相交形成的逆时针方向有序的凸包；Area(·)表示计算两个凸包重叠面积的函数。

4-2构建无条件约束的目标函数其中，N是前后两帧配对的路标的个数；/>表示在变化量ΔT的情况下，每一对凸包重叠面积之和，G(ΔT)是目标函数值。

4-3构建最优迭代步长其中，α表示在迭代循环中步长的整数倍系数，ceil(·)表示大于等于函数值(·)的取整函数，/>表示第N对相似凸包对/>和/>中面积最小的凸包面积之和。

S5具体包括

5-1采用坐标轮换法，依次沿三个正交坐标轴的方向进行目标函数最大值的搜索其中搜索方向取坐标轴方向，即

5-2若进行下一轮搜索；否则，/>T^*即为k-1时刻到k时刻无人驾驶平台运动位姿态的变化量。

S6具体包括

6-1针对地面点云，选择激光雷达原点某一邻域范围内的前视点云和后视点云分别进行RANSCAN平面拟合。其中前视点云表示是在激光雷达坐标系下x大于0的点云，后视点云表示是在x小于0的点云。前视点云和后视点云拟合出的平面对应的法向量分别为n_f(a_f,b_f,c_f)和n_l(a_l,b_l,c_l)。

S7具体包括

7-1连续解算这两个法向量的夹角，同时要求夹角的角度必须是锐角。

7-2当夹角满足设定的阈值大小条件，并且在连续的变化中存在最大值时，通过这两个法向量之间的角度差，确定无人驾驶平台的俯仰角变化量其中，“·”表示向量的点积，||n||代表向量n的模，即向量的长度。

S8具体包括

8-1假设无人驾驶平台侧倾角的变化量是

8-2根据空间位姿的变化量的关系解算出垂直方向变化量Δz_k。其中，Δz_k是垂直方向变化量，Δψ_k是俯仰角变化量，Δθ_k是偏航角的变化量，Δx_k和Δy_k分别为横纵坐标的平移变化量。

S9具体包括

9-1根据解算出的六自由度位姿变化量，将地标点云和地面点云转换到全局坐标系下，并对地图进行占据栅格体素化。

在本研究中，我们将本发明的方法与多种SLAM算法(包括LeGO-LOAM[1]T.X.Shanand B.Englot,"LeGO-LOAM:Lightweight and Ground-Optimized Lidar Odometry andMapping on Variable Terrain,"in 25th IEEE/RSJ International Conference onIntelligent Robots and Systems(IROS),pp.4758-4765,2018，SuMa F-to-F[2]Chen X,Milioto A,Palazzolo E,et al."Suma++:Efficient lidar-based semantic slam"inIEEE/RSJ International Conference on Intelligent Robots and Systems(IROS),pp.4530-4537,2019，MULLS-LO(s1)[3]Pan Y,Xiao P,He Y,et al."MULLS:VersatileLiDAR SLAM via multi-metric linear least square"in 2021IEEE InternationalConference on Robotics andAutomation(ICRA),pp 11633-11640,2021，ICP-Point2Point[4]P.J.Besl and N.D.McKay,"A method for registration of 3-Dshapes,"IEEE Transactions on Pattern Analysis and Machine Intelligence,pp.239-256,1992，ICP-point2plane[5]K.L.J.C.H.Low,"Linear Least-SquaresOptimization for Point-to-Plane ICP Surface Registration,"Chapel Hill,University of North Carolina,pp.1-3,2004，GICP[6]A.Segal,D.Hhnel,and S.Thrun,"Generalized-ICP,"Robotics:Science and Systems,pp.1-8,2009，Velas et al.[7]M.Velas,M.Spanel,M.Hradis,and A.Herout,"CNN for IMU assisted odometryestimation using velodyne LiDAR,"in 2018IEEE International ConferenceonAutonomous Robot Systems and Competitions(ICARSC),pp.71-77,2018和LOAM_Velodyne[8]J.Zhang and S.Singh,"LOAM:Lidar Odometry and Mapping in Real-time,"Robotics:Science and Systems,pp.1-9,2014)进行了比较。我们使用KITTI数据集的里程计序列00和02-10进行评估。数据集的点云数据由频率为10Hz的Velodyne HDL-64ES2 LIDAR扫描仪采集，包括住宅区、城市环境和校园等多种场景。为评估算法的性能，我们主要考虑两个核心度量：平移误差(trel)和旋转误差(rrel)。平移误差代表系统估计的车辆位置和实际位置之间的差距，旋转误差则评估系统估计的车辆方向和实际方向之间的差距。通过对比实验，我们发现本发明提出的算法在以上两项度量上总体上优于对比的SLAM算法。具体数据可以参见表1。

表1本发明SLAM算法与现有SLAM方法的精度比较

从这些结果来看，我们的方法在估计无人驾驶平台位置和方向方面的精度显著提高。由此，我们有理由相信，本发明的方法将在未来的无人驾驶技术发展中起到关键性的推动作用。

图2至图4直观地展示了本发明在KITTI数据集上的表现。X轴代表纵向(前进和后退)距离，Y轴代表横向(左右)距离。其中，真实轨迹由蓝线表征，代表传感器平台的实际路径，而我们算法解算出的估计轨迹由红线表征。

具体来说，图2、图3和图4分别在KITTI00，02，03、KITTI04-07和KITTI08-10数据集上展示了本发明解算的轨迹与真实轨迹的对比。这些图形清晰展示了估计轨迹与真实轨迹之间的显著一致性，强调了本发明的高精度性能。

综上所述，本发明提供的基于地标的激光SLAM定位方法能够有效解决当前激光SLAM定位技术存在的问题，它不仅提高了无人驾驶平台的定位精度，而且构建了信息更丰富的地图。这种高精度定位和信息丰富的地图将有助于无人驾驶技术的进一步发展和应用。

最后说明的是，以上实施例仅用以说明本发明的技术方案而非限制，尽管参照较佳实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，可以对本发明的技术方案进行修改或者等同替换，而不脱离本技术方案的宗旨和范围，其均应涵盖在本发明的权利要求范围当中。

Claims (10)

1.一种用于无人驾驶平台的基于地标的激光SLAM方法，其特征在于：该方法包括以下步骤：

S1：对激光雷达的三维点云数据进行预处理；针对单帧的三维点云数据的预处理包括：地面和非地面点云分割、非地面点云聚类、去除统计学上的离群点；针对前后两帧去除离群点后的类进行地标的初始配对，得到非地面点云中的地标；

S2：针对非地面点云，每一对完成初始配对的地标进行分层，根据地标中三维点云z的均值对地标分为上下两层，并利用Graham扫描法分别建立上下层凸包；

S3：通过使用旋转曲线函数差值和霍夫距离建立一对凸包相似性的综合评价指标，对上下层凸包的相似性进行评估；根据综合评价指标的大小选择出上下层中最相似的一对凸包；

S4：针对完成初始配对的地标，通过使所有最相似凸包对的重叠面积之和最大、构建以无人驾驶平台非平面的位姿变化量的目标优化函数；

S5：使用改进迭代步长的坐标轮换法求解目标优化函数，完成无人驾驶平台非平面的位姿变化量的求解；

S6：针对地面点云，选择激光雷达原点某一邻域范围内的前视点云和后视点云分别进行平面拟合；

S7：通过前后两帧中两个平面法向量之间的角度变化，确定无人驾驶平台的俯仰角变化量；

S8：根据非平面的位姿态变化量和俯仰角变化量联合求解出垂直方向变化量，得到前后两帧点云匹配得到的六自由度位姿变化量；

S9：根据解算出的六自由度位姿变化量，将地标点云和地面点云转换到全局坐标系下，并对地图进行占据栅格体素化。

2.根据权利要求1所述的一种用于无人驾驶平台的基于地标的激光SLAM方法，其特征在于：所述S1包括：

S1-1：对单帧的激光雷达三维点云数据进行预处理，具体包括：使用Ray GroundFilter方法实现地面和非地面点云的分割，对分割后的非地面点云使用欧式聚类的方法进行空间聚类，对聚类后的点云使用基于统计点云的方法去除类中的离群点，得到去除离群点的类的集合；

S1-2：针对前后两帧地标的初始配对，具体包括：分别求解k-1和k时刻类的重心位置，分别计算k-1时刻中、类的重心到k时刻类的重心距离的最小值，并对距离最小值进行平均；如果在k-1和k时刻的地标中，某对类的重心之间的距离的最小值小于平均值，则认为该对类满足地标配对，称这两个类分别是k-1和k时刻中的一个地标。

3.根据权利要求1所述的一种用于无人驾驶平台的基于地标的激光SLAM方法，其特征在于：所述S2包括：

S2-1：对完成初始配对的地标进行分层；具体包括：计算地标对点云在z方向的和、并对其进行平均，得到该地标对的分层阈值；地标的垂直方向的均值小于分层阈值定义为第一层分层类；反之，称为第二层分层类；

S2-2：将第一层分层类和第二层分层类分别投影到x-o-y平面上，使用Graham扫描法算法实现上下层凸包的建立；k-1时刻第一层分层类对应的凸包是k-1时刻第二层分层类对应的凸包是/>k时刻第一层分层类对应的凸包是/>k时刻第二层分层类对应的凸包是/>和/>是第一层凸包对；/>和/>是第二层凸包对。

4.根据权利要求1所述的一种用于无人驾驶平台的基于地标的激光SLAM方法，其特征在于：所述S3包括：

S3-1：求凸包的旋转曲线函数；具体包括：求凸包/>的边长并将边长进行归一化，选择距离原点最近的凸包顶点作为初始点，选初始点两侧边的外角作为初始角度，沿凸包边缘逆时针方向求下一个拐点两侧边的外角；根据凸包归一化的边长和外角可以得到凸包的转角函数曲线，其中转角函数曲线的横坐标是是每一个归一化的边长值的累加，横坐标的取值范围[0,1]，纵坐标是凸包顶点外角的累加，纵坐标的取值范围[0,2π]；

S3-2：求凸包旋转曲线函数差值；用旋转曲线函数的差值衡量凸包和/>的相似性，即/>得到衡量凸包/>和/>的相似程度的差值指标/>同理，可得凸包/>和/>的相似程度的差值指标

S3-3：求解凸包之间的霍夫距离；针对凸包和/>计算/>中的顶点到/>中顶点最近点的距离，然后找出这些距离中的最大值，得到有向Hausdorff距离同理，针对凸包/>和/>计算/>中的顶点到/>中顶点最近点的距离，然后找出这些距离中的最大值，得到有向Hausdorff距离/>

S3-4：求解凸包对的相似性综合评价指标；根据相似程度的差值指标和有向Hausdorff距离/>计算凸包对/>和/>的相似性的综合评价指标其中，α＝0.5，β＝0.5；同理，根据相似程度的差值指标/>和有向Hausdorff距离/>计算凸包对/>和/>的相似性的综合评价指标其中，α＝0.5，β＝0.5；

S3-5：根据综合评价指标选取最相似的对凸包；在综合评价指标和中选取数值小的作为地标相似性的综合评价指标/>同时，选取与之对应的凸包/>和/>解算非平面的位姿变化量，其中sel∈{fir,sec}。

5.根据权利要求1所述的一种用于无人驾驶平台的基于地标的激光SLAM方法，其特征在于：所述S4包括：

S4-1：计算凸包经过ΔT的欧式变换后与凸包/>的重叠面积其中，变化量/>是无人驾驶平台位姿的变化量，Δt_x和Δt_y分别表示在横纵坐标的平移变化量，Δt_θ表示航向角的变化量，/>表示/>经过ΔT的欧式变换后形成的凸包，表示两个凸包相交形成的逆时针方向有序的凸包；Area(·)表示计算两个凸包重叠面积的函数；

S4-2：构建无条件约束的目标函数其中，N是前后两帧配对的路标的个数；/>表示在变化量ΔT的情况下，每一对凸包重叠面积之和，G(ΔT)是目标函数值；

S4-3：构建最优迭代步长其中，α表示在迭代循环中步长的整数倍系数，ceil(·)表示大于等于函数值(·)的取整函数，/>表示第N对相似凸包对/>和/>中面积最小的凸包面积之和。

6.根据权利要求1所述的一种用于无人驾驶平台的基于地标的激光SLAM方法，其特征在于：所述S5包括：

S5-1：采用坐标轮换法，依次沿三个正交坐标轴的方向进行目标函数最大值的搜索其中搜索方向取坐标轴方向，即

S5-2：若进行下一轮搜索；否则，/>T^*即为k-1时刻到k时刻无人驾驶平台运动位姿态的变化量。

7.根据权利要求1所述的一种用于无人驾驶平台的基于地标的激光SLAM方法，其特征在于：所述S6包括：

S6-1：针对地面点云，选择激光雷达原点某一邻域范围内的前视点云和后视点云分别进行RANSCAN平面拟合；其中前视点云表示是在激光雷达坐标系下x大于0的点云，后视点云表示是在x小于0的点云；前视点云和后视点云拟合出的平面对应的法向量分别为n_f(a_f,b_f,c_f)和n_l(a_l,b_l,c_l)。

8.根据权利要求1所述的一种用于无人驾驶平台的基于地标的激光SLAM方法，其特征在于：所述S7包括：

S7-1：连续解算这两个法向量的夹角，同时要求夹角的角度必须是锐角；

S7-2：当夹角满足设定的阈值大小条件，并且在连续的变化中存在最大值时，通过这两个法向量之间的角度差，确定无人驾驶平台的俯仰角变化量其中，“·”表示向量的点积，||n||代表向量n的模，即向量的长度。

9.根据权利要求1所述的一种用于无人驾驶平台的基于地标的激光SLAM方法，其特征在于：所述S8包括：

S8-1：假设无人驾驶平台侧倾角的变化量是

S8-2：根据空间位姿的变化量的关系解算出垂直方向变化量Δz_k；其中，Δz_k是垂直方向变化量，Δψ_k是俯仰角变化量，Δθ_k是偏航角的变化量，Δx_k和Δy_k分别为横纵坐标的平移变化量。

10.根据权利要求1所述的一种用于无人驾驶平台的基于地标的激光SLAM方法，其特征在于：所述S9包括：

S9-1：根据解算出的六自由度位姿变化量，将地标点云和地面点云转换到全局坐标系下，并对地图进行占据栅格体素化。