【代码】局部路径规划DWA算法详解。

粒子滤波是一种基于蒙特卡洛模拟的非线性滤波方法，其核心思想是用随机采样的粒子表达概率密度分布。粒子滤波的三个重要步骤为：1）粒子采样，从建议分布中抽取一组粒子；2）粒子加权，根据观测概率分布，重要性分布以及贝叶斯公式计算每个粒子的权值；3）估计输出，输出系统状态的均值协方差等。此外 ，为了应对粒子退化现象，还采用了重采样等策略。

快速扩展随机树(RRT)算法，是近十几年应用比较广泛的一种运动规划算法。它的大致原理为：原始的RRT算法通过一个初始点作为根节点，通过随机采样，增加叶子节点的方式，生成一个随机扩展树，当随机树中的叶子节点包含了目标点或进入了目标区域，便可以在随机树中通过回溯的方式，找到这条从初始点到目标点的路径。

在本文中，我们提出了一个通过smoothing and mapping实现的紧耦合激光惯性里程计框架，来解决上面提到的问题。首先我们假设一个非线性运动模型用于点云运动畸变的矫正，主要是使用IMU测量值估计雷达的运动。除了用于矫正点云之外，基于IMU估计的运动还可作为激光里程计优化的初值。然后再基于激光里程计估计因子图中IMU的零偏。在机器人的轨迹估计时，基于因子图，我们可以有效的使用雷达和imu的

ORB-SLAM2 适用于单目，双目和RGB-D相机，包括闭环、重定位等功能，可用于室内环境、工业环境、飞行的无人机、行驶的汽车等。