无人驾驶--实时定位与地图构建（SLAM）仿真与实战（附源码）

无人驾驶–实时定位与地图构建（SLAM）仿真与实战（附源码）

一个SLAM的技术小结，供自己回顾也为后人学习提供参考。
另外建了一个无人驾驶方面的微信交流群，有兴趣的同行或者专家学者可以加我微信：wxl609278502 请注明： 姓名-单位/学校/科研院所

项目技术描述

硬件平台：智能车平台、多线激光雷达、工控机、stm32、车身转角传感器和轮速传感器
软件平台：Ubuntu16.04、ROS
技术实现：通过激光雷达的点云数据和车身传感器数据，使用扩展卡尔曼滤波算法，实现实时定位与地图构建的matlab仿真验证与整车工程化实现（Ubuntu+ROS）。

SLAM概述

SLAM，全称是Simultaneous Localization and Mapping，即同时定位与建图。所以我们能看到SLAM的主要工作是定位以及建图。SLAM目前有很多实现方法，主要分为激光雷达和视觉传感器两个方向。。本文讨论的是激光雷达SLAM技术。
EKF 方法是解决 SLAM 问题的一种经典方法，其应用依赖于运动模型和观测模型的高斯噪声假设。在 SLAM 问题首次提出不久后，Smith 和 Cheesman及 Durrant-Whyte对机器人和路标间的几何不确定性关系进行了研究。Smith 等利用 EKF 方法提出的数学公式至今仍在广泛使用。基于 EKF 的 SLAM 方法提出后，许多研究者对该方法进一步改进,Moutarlier 等采用 EKF 方法实现了第一个 SLAM 系统。基于 EKF-SLAM 框架，一些研究人员在不同机器人系统上完成了许多开发工作。
EKF-SLAM 的主要优点是它能够以在线的方式估计地图的全后验概率。到目前为止，EKF（或其扩展方法，如高斯混合模型）方法和 RBPF（Rao-Blackwellized Particle Filte）方法是仅有的能够估计地图全后验概率的方法。估计全后验概率有很多优点：除了获得最可能的地图和机器人位姿，EKF 在地图中维护的不确定性在机器人利用地图导航的时候有极大的好处。除此以外，EKF可以使机器人位姿和所创建的环境地图依概率 1 收敛到真实值，其收敛程度由机器人初始不确定性和传感器观测的不确定性决定。

SLAM可以分为定位问题和建图问题：
定位问题：

1. 已知路标的位置的地图m，
2. 从无人车的位置得到路标的测量值Zk (距离和方位角），
3. 根据路标的测量值Zk和路标的位置,计算出无人车的坐标Zk。
   
   建图问题：
4. 提供车辆的坐标Xk,
5. 通过激光雷达获得路标相对车辆的测量值Zk，
6. 根据无人车的坐标Xk和路标的测量值Zk，计算路标的坐标，实现路标地图的构建。
   
   定位与建图融合：
7. 定位与建图是一个耦合问题，可以从单次测量中推断出两个量，
8. 只有同时考虑定位和建图的方案才能解决SLAM问题。

EKF-SLAM基本算法流程

1.车辆的预测模型（车辆运动学建模）
目标：根据车辆在时刻k的车速和方向盘转交V和y，估计车辆在k + 1 时刻的位置和航向角。具体模型和公式推导如下图所示。

车辆运动学方程：

因此，车辆k+1时刻的预测方程为：

2.路标的预测模型
我们认定路标点的坐标相对地面坐标系是不变的，及k时刻和k+1时刻的路标点坐标位置一致。


3.整个系统（车辆预测+路标预测）的预测方程

4.观测模型

本实验中使用的激光雷达返回的是障碍物的距离和角度，因此观测方程需要从笛卡尔坐标系转换到极坐标系，因此观测方程如下所示：

5.EKF公式推导（直接上ppt,卡尔曼算法原理不清楚的就去查论文吧）