[发明专利]一种激光雷达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)要实现对车辆精确稳定的规划、控制，需要较高的车辆定位稳定性和精度。

发明内容