一种激光雷达SLAM算法与惯导融合定位的方法

摘要

本发明率先提出一种基于特征概率栅格地图的激光雷达SLAM算法‑CPFG(Closet Probability and Feature Grid，最近邻概率特征栅格)算法。该算法利用三维激光雷达数据，实时创建和更新线、面及高斯分布特征以及占据概率的栅格地图，并结合鲁棒化后的马氏距离作为优化函数进行实时位姿估计，该算法主要分为三步：点云预处理，点云与特征概率栅格地图的匹配及位姿估计，特征概率栅格地图的更新。本发明的激光雷达SLAM算法与目前几个主流算法相比，在实时性和定位精度方面有更好的表现。然后本发明融合了惯导的姿态信息，将激光雷达SLAM的高位移精度与惯导低姿态漂移的特性相结合，其相对定位精度可以达到千分之一左右，在无人驾驶定位领域具有广泛的使用前景。

说明书

技术领域

本发明涉及无人车技术领域，尤其涉及一种基于特征概率栅格地图的激光雷达SLAM算法-CPFG(Closet Probability and Feature Grid，最近邻概率特征栅格)算法，然后与惯导融合定位。

背景技术

SLAM问题最早起源于1986年，到目前为止已经有30多年的历史。Durrant-Whyte和Bailey等人对SLAM的发展进行总结，前20年SLAM的研究主要基于滤波器理论。1988年SmithR等人提出了EKF-SLAM，首先将扩展卡尔曼滤波应用于SLAM问题，该算法需要构建特征地图，并同时估计每个特征的位置和车辆的位置，计算复杂度较高且鲁棒性不足。Montemerlo等人提出了FastSLAM，最先使用了粒子滤波的方法进行位姿估计，其将SLAM问题分解为环境特征位置估计问题以及机器人定位问题，较大程度提高了算法的实时性。Grisetti G等人结合栅格的思想以及RBPF(RaoBlackwellized Patricle Filter)算法提出了Gmapping开源框架，其首先采用了自适应重采样的思想来缓解粒子耗散问题，并将位姿估计与栅格地图更新分离，算法的精确度、实时性和鲁棒性均有较大程度提高，因此该框架受到当时2D雷达SLAM研究人员的广泛关注和使用。后来出现了基于优化的SLAM算法，Kohlbrecher等人提出的Hector-SLAM，其通过构建目标函数进行最小化，将点云匹配问题转换成非线性最小二乘优化问题来求解。随着传感器技术的进步，多线激光雷达出现，相比之前的2D单线雷达，在垂直方向不同角度安装有多个雷达发射器和接收器，因此可以获得更丰富、立体的环境信息。基于此，基于3D激光雷达的SLAM逐渐出现并成为研究热点。Zhang J等人提出了LOAM(Lidar Odometry and Mapping)算法，其使用多线激光雷达(或增加旋转电机的单线激光雷达)采集点云数据，然后提取角点、面特征，利用点到线、点到面ICP算法，使用非线性优化的方法进行位姿估计。LOAM采用了双层匹配策略。第一层为相邻帧之间的匹配，因为两帧之间涉及到的特征点较少，且点云排列有规律，特征关联和位姿优化均能节省很多时间，因此有较高的运算效率。但也因雷达点云的稀疏性，导致关联特征点较少，且提取的特征有较大误差，因此精度相对较低。第二层需要维护特征地图，特征较多，特征关联过程花费时间也较多，因此与第一层相反，其运算时间较长，精度较高。通过两层匹配方案的搭配，LOAM可以以较高的频率(10Hz)输出较为准确的定位结果。后来Zhang J等人又在LOAM的基础上提出了V-LOAM的算法，将视觉与激光雷达信息融合，首先用视觉里程计初步估计位姿变换，然后再此位姿估计基础上，用激光里程计优化位姿估计结果，进一步提高了定位精度。2017年Zhang J等又提出了LVIO算法，将IMU、视觉、雷达信息进行融合，进一步提高了定位的稳定性和精度。

虽然目前基于激光雷达车辆定位的研究比较多，但针对无人驾驶所面对的复杂环境的需求，仍有较大的挑战：

1)要想满足无人驾驶车辆快速决策和规划的需求，对算法有较高的实时性要求。

2)要实现对车辆精确稳定的规划、控制，需要较高的车辆定位稳定性和精度。

发明内容

鉴于上述的分析，本发明旨在提供一种基于概率栅格地图的激光雷达里程计算法，并且兼顾实时性和精度，然后以该算法为基础，又进一步提出了融合惯导姿态信息的激光雷达里程计，以进一步提高算法的定位精度。

本发明的目的主要是通过以下技术方案实现的：

一种基于特征概率栅格地图的激光雷达SLAM算法-CPFG(Closet Probabilityand Feature Grid，最近邻概率特征栅格)算法，包括以下步骤：

数据预处理、特征概率地图更新、点云与地图匹配并更新位姿。

数据预处理是对激光雷达点云进行初步处理，降采样及分类等；

特征概率地图更新部分主要作用是进行地图的管理以及点云分布特征的提取、栅格的更新等；

点云匹配及位姿更新是通过每帧点云与特征地图之间进行数据关联，然后利用匹配算法，估计当前位姿。

本发明有益效果如下：

本发明提出的基于特征概率栅格地图的激光雷达SLAM算法，在越野数据集上，综合多个场景的试验结果显示，相较其他几个雷达里程计算法(LOAM、cartographer、NDT)，本发明提出的算法在多数场景下都具有最高的定位精度(1％左右)。而在世界知名公开数据集KITTI上，本发明提出的CPFG算法的定位精度也处于前列，尤其是实时性方面，是目前同等精度(0.9％以内)的激光雷达里程计算法中运算时间最短的算法。最后针对卫星干扰环境的无人车辆定位问题进行算法验证，首先对融合惯导信息的激光雷达里程计定位精度进行评估，融合惯导姿态后的激光雷达里程计定位精度可达0.1％。在无人驾驶领域具有广泛的使用前景。

本发明的其他特征和优点将在随后的说明书中阐述，并且，部分的从说明书中变得显而易见，或者通过实施本发明而了解。本发明的目的和其他优点可通过在所写的说明书、权利要求书、以及附图中所特别指出的结构来实现和获得。

附图说明

附图仅用于示出具体实施例的目的，而并不认为是对本发明的限制，在整个附图中，相同的参考符号表示相同的部件。

图1为本发明激光雷达里程计算法流程图；

图2为本发明多线激光雷达数据获取及点云分类示意图；

图3为本发明与惯导姿态深度融合的激光雷达里程计示意图；

图4为本发明惯导信息融合的优化模型示意图。

具体实施方式

下面结合附图来具体描述本发明的优选实施例，其中，附图构成本申请一部分，并与本发明的实施例一起用于阐释本发明的原理。

本发明的一个具体实施例，公开了一种基于特征概率栅格地图的激光雷达里程计算法，如图1所示，包括以下步骤：

数据预处理、特征概率地图更新、点云与地图匹配并更新位姿。

数据预处理是对激光雷达点云进行初步处理，降采样及分类等；

特征概率地图更新部分主要作用是进行地图的管理以及点云分布特征的提取、栅格的更新等；

点云匹配及位姿更新是通过每帧点云与特征地图之间进行数据关联，然后利用匹配算法，估计当前位姿。

具体的，点云预处理包括点云下采样和分类以及车辆运动导致的点云畸变矫正：

1)点云下采样和分类

本研究使用的是Velodyne生产的HDL64激光雷达，该雷达有64对激光发射器、接收器。该雷达支持不同的旋转频率，这里我们选用10Hz旋转频率，此时雷达的水平角分辨率为0.1728°，每帧点云数量约为133333。该雷达垂直方向不同雷达线束之间的夹角不同，最小角分辨率0.3333°。为了满足无人驾驶汽车的需要，该雷达向下发射的激光束要多于向上的，以便能获取更多地面上的环境信息。该雷达为机械式旋转雷达，这里认为雷达每转一圈获得的点云为一帧。由于点云数量较大，在匹配和地图更新之前均需进行点云的下采样，减少点云数目，降低运算成本。为了适应无人驾驶环境各种不同场景，本研究使用自适应分辨率下采样的方法。同时预先设置一个最低分辨率和一个下采样后的点云最低数量限制，如果利用初始分辨率下采样后满足最低点云数量限制，则下采样完成，否则提高分辨率，重新进行下采样操作，直到满足最低数量限制为止。每次调整完下采样分辨率后，按照该分辨率建立一个空的三维栅格地图，然后将点云顺序投影到栅格内，同一个栅格内的点云只保留一个点，以此来降低点云数量。

因为本研究提出的算法需要将每帧雷达点云来自不同激光放射器的点区分开，因此需要给不同的激光发射器产生的点分配不同的线(laser)编号。附图4图所示为多线激光雷达数据获取及点云分类示意图，C为车辆，O为激光雷达，a、b、c分别为同一时刻激光雷达的不同激光发射器产生的激光，则1、2、3为获得的对应反射点的线编号(lasernum)。同时为区分来自不同帧的点云，在时间上给不同帧的点云也分配不同的帧编号(scannum)。

2)车辆运动导致的点云畸变矫正

机械式旋转雷达由于其旋转一周需要一段时间，而此类雷达相关的研究一般都会将旋转一周的点云当作一帧点云来处理。而此时如果仍然通过第二章第一节中介绍的雷达模型来计算点云位置，由于车辆运动将会导致激光雷达坐标系的原点一直在改变，从而导致计算得到的点云与真实值存在偏差，即产生畸变。在车辆高速行驶时的点云畸变尤其明显。

为了矫正车辆运动导致的点云畸变，首先要获取此时的车辆运动速度和角速度，然后根据已知的车辆运动来估计点云畸变的偏差，从而对其进行矫正。车辆速度(和角速度)的获取方式主要有两种：一种是通过外部传感器来获取，如惯导、车量轮速计等；第二种是用激光雷达里程计的位姿来估计当前车辆的速度(和角速度)。因为本章主要介绍的是纯激光雷达里程计，因此这里选用第二种方法来获取当前车辆的速度(和角速度)。而使用激光雷达里程计的位姿来估计速度又可衍生出两种：一种是利用前几帧的位姿来估计当前车辆速度(和角速度)；另一种是在迭代优化位姿的过程中也迭代地不断更新用于点云畸变矫正的车辆速度(和角速度)。经试验，这两种方式的定位精度相差不大，但第二种会导致位姿波动较大，因此最终选择了利用前几帧的位姿来估计当前车辆速度(和角速度)。获得车速v，角速度ω之后，即可通过以下公式来对点云进行矫正，将点云都转换到该帧点云参考时间戳t时刻下的车辆坐标系。设原始点为P

其中函数AngletoMatrix()代表欧拉角到旋转矩阵的转换函数。对点云中每个点都经过上述几步计算后，即可将当前帧所有非参考时间的点云转换到参考时间下的坐标系中，即实现了点云畸变的矫正。

3)栅格地图数据结构

三维栅格地图是将一片范围内的空间分成很多固定大小的体元，用于存放该位置的点云或提取的点云高层次信息。而用怎样的数据结构可以消耗较低的时间复杂度和空间复杂度来存储三维栅格地图是一个很重要的问题。目前管理三维栅格地图比较常用的数据结构是八叉树(octree)。

八叉树是用于描述和管理三维空间数据的一种树状数据结构，是二叉搜索树和2D四元树在三维空间上的扩展。八叉树的父节点是一个较大的立方体，而每下一层的子节点，相当于上一次父节点空间以其中心为分叉中心，均匀切分成八块大小相同的立方体，如此不断循环直到指定最大深度或子节点体积元素的分辨率达到最低阈值为止。八叉树的每个节点主要分三个状态：空节点、被占据节点、非空但未被占据的节点。空节点是指该节点为空指针，未开辟存储空间，这类节点出现在非最深层的节点中；被占据节点是指该节点的空间已经被开辟且以经被点云占据；非空但未被占据的节点是指存储空间已经开辟，但未被点云占据，这类节点出现在最深层的叶节点中。

在无人驾驶SLAM应用中，在位姿估计的同时需要同时进行栅格地图的创建和更新，由于随着车辆行驶，地图会逐渐增大，因此需要使用可以不断扩展其空间范围的八叉树来管理。给定一个点云，构建八叉树的步骤如下：

1)设定最深层节点分辨率、根节点初始大小和位置。

2)将每个点放入栅格地图中。

3)判断点是否超出根节点所限定的范围，如果超出，需要在当前根节点上层扩展更大的立方体体元节点作为新的根节点，重复验证根节点的范围，并扩展根节点，直到该点满足根节点的范围限制。

4)利用点的坐标，逐层索引，遇到没达到最大递归深度的空节点则开辟新的空间，并对该节点代表的立方体体元进行八等份，建立八个新的子节点，如此递归索引，直到最大深度的节点，将点插入到该节点中。

用八叉树来管理栅格地图，主要有两个优势：一是八叉树只会在被占据的栅格处开辟空间，因此可以很大程度上降低计算机内存占用；二是树状的数据结构搜索时间只与最大深度D呈线性关系，具有较低的时间复杂度，假设最大深度节点的体元分辨率为0.5m，八叉树的最大覆盖范围为500m，则该八叉树的最大深度为D＝log

传统的八叉树虽然有较高的空间利用率和相对较低的时间复杂度，但是在实际计算机运行时，因为需要在堆上频繁的分配很多的小存储空间来存储各个节点，这对计算机来说需要消耗较大的时间。同时考虑到实际无人车场景中，可占据状态的栅格大多都聚集在相邻区域。因此本研究在传统八叉树的基础上进行扩展，由原先每次分割为分成八个体元空间扩展为分割为8*8*8个体元，这样相邻节点内存分配时由多次创建8单元的小空间变成一次创建512单元的大空间。避免了内存的零散化和频繁的内存分配操作，从而提高了地图的创建效率，这是一种空间换时间的方法，可以在内存充足而又有较强实时性要求的场景下使用。

4)特征概率栅格的更新

本研究提出的特征概率栅格由特征描述和占据栅格概率两部分组成。在每帧点云匹配结束后，进行点云的插入和特征地图的更新，即：

M

其中M

特征描述部分将栅格特征分为点云分布特征、线特征、面特征。本文的点云分布特征是用高斯分布来近似点云分布，即X～N(μ，∑)。其中μ代表栅格内点云的均值，∑代表点云的协方差矩阵。

相比相机获取的图像数据，激光雷达点云具有较强的稀疏性，尤其是在垂直方向每个激光放射器之间都有较大的角度间隔，而且由于雷达光束呈放射状射出，会导致距离雷达越远的点云越稀疏。因此每次进行栅格更新时，有些栅格内的点云数量较少，无法真实地反映该栅格内物体的表面分布。

特征地图的栅格内的多个点都由同一帧点云的同一对激光收发器获得，可以看出此时如果计算点云分布，将会获得近似直线的分布，这与地表实际情况(平面)不一致，故此时不应该进行特征提取。而随着车辆行驶，等到来自不同的发射器的越来越多帧的点云不断地被填充到栅格内，此时再开始进行特征提取及更新。随着栅格内的点云越来越多，提取的点云分布特征也将能越来越真实地反映物体的形状。

因此本研究在进行点云分布特征提取时，为了避免因点云的稀疏性而导致提取到错误的特征，需要点云的编号(线编号及帧编号)种类达到预设的最低阈值，才开始进行特征的更新。如下式：

下式为栅格内点云分布特征的计算方法。

n代表点的数量，P

cov(a，b)代表协方差的计算公式：

其中sum(ab)，sum(a)，sum(b)都可以通过单独的存储空间存储，每融合一帧点云只需更新累加一次坐标值，避免了点云坐标的重复求和，从而很大程度上提高了算法效率。

∑为实对称矩阵，可以进行特征值分解，如下式：

∑＝V

Λ代表特征值矩阵，为对角矩阵，V为特征值对应的特征向量矩阵。其中λ

通过点云分布进行特征表示，在匹配时总是会用到在三个不同方向的约束，其会在应用于真正的线、面特征时产生特征衰退的现象，带来了冗余的约束。面特征只有法线方向的约束，如果用点云分布特征将会增加其他方向的约束，这相当于在优化中引入了噪声，将会降低位姿估计的精度和稳定性。因此针对提取到的点云分布特征，仍需进一步判断是否为线、面特征。因为特征值代表了不同方向上点云分布的方差，因此λ

占据栅格概率部分利用贝叶斯理论进行栅格的概率更新。

其中p(m

5)点云匹配与位姿估计

点云匹配的第一步是进行数据关联，本研究通过计算当帧点云的每个采样点与周围最近八个特征栅格的点云分布均值的距离，从而找到最近邻特征栅格。上一节提到的线、面及点云分布特征都可以统一用高斯分布的形式来表示，可以表达该栅格内点云所在位置的概率分布，协方差矩阵的特征值大小可以表示点云在对应特征向量方向的方差。点云分布的方向性及方差的差异代表不同方向上栅格对点云有不同的约束，如对于直线来说，其沿直线方向无约束；对平面来说，其只有平面法向方向的约束。为统一这种具有方向异性的约束，可以用马氏距离来建立优化函数，即不同方向有不同的优化权重。为了提高优化的稳定性，需要对特征矩阵使用非线性函数

对该特征值矩阵的逆进行归一化如下。

然后以马氏距离的作为误差函数，得到下式所示的代价函数。

N为当前帧待匹配点的数量，X

为了进一步提高算法的稳定性，降低外点对优化结果的影响，这里使用M估计对代价函数的形状进行修改，采用柯西(Cauchy)鲁棒代价函数，降低较大误差的权重。

在本研究中每个采样点对应的误差函数u

同时因为占据概率高的栅格说明被点云击中的概率更高，其点云分布也将有更高的置信度，因此将概率作为附加标量权重值：

c

最终获得新的代价函数如下：

最终问题转换为求解上述代价函数的最小值，可以使用第二章的非线性优化算法来求解，如Levenberg-Marquard算法。

上述为激光雷达里程计算法的步骤，但是因为激光雷达里程计算法需要依赖足够数量的几何特征，而在一些特征较少的环境单纯依靠激光雷达来进行定位，会有匹配失效的风险，难以输出稳定的定位结果。同时单纯靠激光里程计来定位，在长时间的导航过程中，由于误差的积累，精度难以达到无人车辆定位的需要。这就需要融合其他传感器的信息来保证它的稳定性和定位精度。

6)初始位姿估计

初始位姿估计的目的是为后续点云匹配和位姿优化提供所需的初值。用于初始位姿估计的方法主要分为需要其他传感器和不需要其他传感器的方法。常用方法包括：基于匀速运动模型、DR(Dead Reckoning，航迹推算)等。

A：匀速运动模型

匀速运动模型是指不依赖于其他传感器信息，通过匀速假设依据车辆前一时刻的运动状态来估计当前时刻的车辆位姿的方法。假设当前时刻之前相距时间Δt

其中toVec()表示从变换矩阵转换为轴角

设上一时刻与当前时刻的时间差为Δt

其中toMatrix()表示从轴角及位移转换为变换矩阵。

B：航迹推算

航迹推算是利用车辆上车载里程计、惯导等来获取车辆的航向和速度信息，然后通过不断积分求和，推算出当前车辆所在的位置。因为车载里程计、惯导等都属于内部传感器，因此基本不会受到环境的影响，但由于其误差会随着积分不断积累，因此一般会用来作为辅助定位源。

航迹推算公式如下：

其中(x

7)与惯导姿态深度融合

在实际应用中，用只依赖于激光雷达的里程计进行定位时，车辆行驶较远距离后，其角度误差会有较大程度的积累，定位误差也会不断发散；同时在一些特征较少的环境也存在定位失效的风险。因此为保证定位的精度和鲁棒性需要融合其他传感器的信息，本研究将惯导的姿态信息与雷达里程计进行深度融合，在保证定位鲁棒性的同时极大地提高了车辆的定位精度。惯性导航系统是将陀螺仪与IMU集成在同一个载体上，在无卫星时其通过对陀螺仪输出的角速度积分获得航向信息，并与IMU的加速度方向相结合估计载体的俯仰角和侧倾角；而后通过对IMU输出的加速度信息，经过二次积分获得载体的位置；而在有卫星信号时又可以利用经纬度信息进行位置和航向矫正。由于目前惯导所用的陀螺仪一般都具有较高的精度，漂移较小，因而即使在无卫星信号环境下在较长时间内其姿态输出仍具有较高的精度。但是惯导所用的IMU输出的加速度信息在经过二次积分后，累计误差较为明显，因此在无卫星环境下，其得到的位置信息可信度较低。

在卫星信号受到干扰的环境下，惯导的角度仍具有较高的精度，但其位移精度会随着时间推移有较大的累计误差；而基于匹配算法的激光雷达里程计则相反，其角度精度有所不足，但位移精度较高。因此本研究考虑将二者优势相结合，利用图优化的思想，建立统一的误差模型，进行最优化估计，从而获得较高精度的车辆位姿。

如附图4所示，G代表大地坐标参考系，M代表三维子地图位姿，I代表高精度惯导的姿态(不包括位置)，X为待估计的车辆位姿，G与M及G与I之间为刚性连接，有固定的位姿关系，M与X及I与X之间为待优化的约束边。

将雷达里程计误差模型与惯导姿态误差模型融合建立如下优化函数：

其中，T＝|q t|，q代表车辆旋转，t代表车辆位移，q

最后使用Levenberg-Marquardt算法来解决上述非线性最小化优化问题，从而获得车辆位姿。

综上所述，本发明提出的基于特征概率栅格地图的激光雷达SLAM算法，在越野数据集上，综合多个场景的试验结果显示，相较其他几个雷达里程计算法(LOAM、cartographer、NDT)，本发明提出的算法在多数场景下都具有最高的定位精度(1％左右)。而在世界知名公开数据集KITTI上，本发明提出的CPFG算法的定位精度也处于前列，尤其是实时性方面，是目前同等精度(0.9％以内)的激光雷达里程计算法中运算时间最短的算法。最后针对卫星干扰环境的无人车辆定位问题进行算法验证，对融合惯导信息的激光雷达里程计定位精度进行评估，融合惯导姿态后的激光雷达里程计定位精度可达0.1％。在无人驾驶领域具有广泛的使用前景。

以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。