激光追踪算法

A. SLAM与VSLAM有什么区别

一、成熟度不同

激光 SLAM 比 VSLAM 起步早，在理论、技术和产品落地上都相对成熟。基于视觉的 SLAM 方案目前主要有两种实现路径，一种是基于 RGBD 的深度摄像机，比如 Kinect；还有一种就是基于单目、双目或者鱼眼摄像头的。

VSLAM 目前尚处于进一步研发和应用场景拓展、产品逐渐落地阶段。

二、应用场景

从应用场景来说，VSLAM 的应用场景要丰富很多。VSLAM 在室内外环境下均能开展工作，但是对光的依赖程度高，在暗处或者一些无纹理区域是无法进行工作的。而激光 SLAM 目前主要被应用在室内，用来进行地图构建和导航工作。

三、地图精度不同

激光 SLAM 在构建地图的时候，精度较高；

VSLAM，比如常见的，用的非常多的深度摄像机 Kinect，（测距范围在 3-12m 之间），地图构建精度约 3cm；所以激光 SLAM 构建的地图精度一般来说比 VSLAM 高，且能直接用于定位导航。

四、易用性不同

激光 SLAM 和基于深度相机的 VSLAM 均是通过直接获取环境中的点云数据，根据生成的点云数据，测算哪里有障碍物以及障碍物的距离。

但是基于单目、双目、鱼眼摄像机的 VSLAM 方案，则不能直接获得环境中的点云，而是形成灰色或彩色图像，需要通过不断移动自身的位置，通过提取、匹配特征点，利用三角测距的方法测算出障碍物的距离。

B. 基于激光雷达的SLAM和路径规划算法研究与实现

本文仅供学习使用，并非商业用途，全文是针对哈尔滨工业大学刘文之的论文《移动机器人的路径规划与定位技术研究》进行提炼与学习。论文来源中国知网，引用格式如下：
[1]刘文之. 基于激光雷达的SLAM和路径规划算法研究与实现[D].哈尔滨工业大学,2018.

相关坐标系转换原理已经在前一篇文章写完了，直接上转换方程。

这里他的运动模型选择的是基于里程计的运动模型，还有一种基于速度的运动模型，其实都差不多，整体思想都一样。里程计是通过计算一定时间内光电编码器输出脉冲数来估计机器人运动位移的装置，主要是使用光电码盘。根据光电码盘计算出此时轮子的速度，然后通过已知的轮子半径来获得单位时间 每个轮子 的位移增量。

高等数学可知单位时间位移增量就是速度，对速度在一定时间上进行积分就得到这一段时间所走过的路程。

根据上图，我们可以求出来机器人航向角角速度、圆弧运动半径和机器人角度变化量，由此可以解的机器人在当前时刻的位姿。

实际上也是有误差，所以单独依靠里程计会与实际结果产生较大误差，所以必须引入其他的外部传感器对外部环境的观测来修正这些误差，从而提高定位精度。

首先肯定需要将激光雷达所测得的端点坐标从极坐标、机器人坐标中转换到世界坐标中。

这张略过，暂时不需要看这个

路径规划算法介绍：

因为该算法会产生大量的无用临时途径，简单说就是很慢，所以有了其他算法。

了解两种代价之后，对于每一个方块我们采用预估代价与当前路径代价相加的方法，这样可以表示每一个路径点距离终点的距离。在BFS搜索过程的基础上，优先挑选总代价最低的那个路径进行搜索，就可以少走不少弯路。（算法讲解 https://www.bilibili.com/video/BV1bv411y79P?from=search&seid=3623681329596549549 ）

在局部路径规划算法之中，我们选用DWA算法（dynamic window approach），又叫动态窗口法。动态窗口法主要是在速度(v, w)空间中采样多组速度，并模拟机器人在这些速度下一定时间内的轨迹。在得到多组轨迹后，对这些轨迹进行评价，选取最优的轨迹所对应的速度来驱动机器人运动。
state sampling就是按照之前给出的全局路径规划，无论是Dijkstra还是A* 都可以方便的得到state sampling，DWA算法所需要提前建立的action sampling有两种：

但是无论是什么情况，上述所做的工作就是把机器人的位移转化到世界坐标中来，而不是机器人坐标系。速度采样结束之后，只需要对小车的轨迹进行评判，就可以得到最优解了。下面介绍速度采样的办法。

对速度进行采样一般有以下三个限制：

当确定了速度范围之后，就需要根据速度分辨率来对小车速度离散化，在每一时刻将小车在不同直线速度角速度组合下所即将要行驶的距离都可视化出来。

其中每一条轨迹都是很多小直线连接起来的。

需要用评价函数来对上述轨迹进行选择，选择最适合的轨迹

最后为了让三个参数在评价函数里所发挥的作用均等，我们使用归一化处理来计算权重。

算法流程整体如下：

C. 激光雷达SLAM包括以下几种方案:

激光雷达SLAM包括以下几种方案:

直接法, 直接法的典型代表是ICP和NDT系列，ICP可以直接计算出激光的位姿，多帧可以构成联合优化，这种方案比较简单但是比较有效，常用于激光雷达的多程对齐。

基于特征的匹配(LO)，该方案的典型代表是LOAM和后续的改进方案A-LOAM/F-LOAM,该方案通过寻找线面特征，并通过特征匹配来计算帧间的位姿，多个位姿可以做BA优化。

多传感器融合的方案。该方案的典型代表是LIO-Mapping,LINS和LIO-SAM.LIO-Mapping算法借鉴VINS-Mono的预积分和后端优化，前端视觉里程计改成激光里程计。

基于栅格的，该方案的代表是谷歌开源的cartography，这种方案在室内的机器人定位比较有优势。

基于面元，该方案的典型代表是suma。

基于语义信息，该方案的典型代表是segmap和suma++。

高精地图的生产包括语义信息的提取(通常来自视觉，部分也可来自激光),单程LIO(gnss+imu+dmi+lidar/visual odometry),多程对齐。说一下每一部分的难点吧:

图像语义信息的提取。图像的语义信息包括车道线，杆状物体，牌子，地面车信等；在高速场景，物体遮挡较少，物体检测的准确率是可以做到95%以上的；在城市道路，由于树木的遮挡(对牌子和杆状物体的遮挡), 车辆遮挡(对车道线和地面车信的遮挡),目前的检测是很难做到90%以上的。

点云的语义信息提取。点云对于特殊材料反射率较大(如车道线),基本上二分就可以解决很多问题，对于高速场景，车道线磨损并不严重，语义信息提取较容易；对于城市道路车道线磨损严重，且存在老旧的车道线的干扰，语义信息的提取是很难做到90%以上的。

LIO. 高精采集车一般配备激光雷达，相机，imu, dmi和RTK等设备，对单程轨迹来说，是可以采用多传感器融合的。对于高速场景，建筑物遮挡较少，RTK信号较好，后结算的RTK的精度是可以做到30cm以内的(在山区和隧道场景例外)，自动化比较难做，这个和场景有关系。

多程融合。主要看人眼去分辨点云是否对齐，当然是可以建立小规模数据集进行评测，自动化率非常低。

总的来说，目前高精地图的生产是很难做到自动化的，主要原因是场景复杂,corner case太多，绝对精度和相对精度都很难满足要求。

总结一下，目前激光SLAM的直接法比较简单，可以用于激光里程计的多程对齐或回环检测; 纯LO的算法目前工业圈很少使用，一般采用多传感器融合的方案，毕竟激光雷达都用了，也不差IMU这样的传感器。多传感器融合的方案主要用于高精地图的制作，自动驾驶领域一般采用高精地图作为先验来定位，不会同时定位和建图。基于栅格的可以用于移动机器人，在室内环境栅格假设大多数还是有效的，在室外场景的话，一般会采用NDT Map来存储地图。基于语义信息和面元的话，个人不太了解，在工业圈的使用也不多。

D. 激光雷达SLAM算法

机器人研究的问题包含许许多多的领域，我们常见的几个研究的问题包括：建图(Mapping)、定位(Localization)和路径规划（Path Planning），如果机器人带有机械臂，那么运动规划（Motion Planning）也是重要的一个环节，SLAM需要机器人在未知的环境中逐步建立起地图，然后根据地区确定自身位置，从而进一步定位。

ROS系统通常由大量节点组成，其中任何一个节点均可以通过发布/订阅的方式与其他节点进行通信。举例来说，机器人上的一个位置传感器如雷达单元就可以作为ROS的一个节点，雷达单元可以以信息流的方式发布雷达获得的信息，发布的信息可以被其他节点如导航单元、路径规划单元获得。

ROS的通信机制：

ROS（机器人操作系统）中SLAM的一些功能包，也就是一些常用的SLAM算法，例如Gmapping、Karto、Hector、Cartographer等算法。我们不会去关注算法背后的数学原理，而是更注重工程实现上的方法，告诉你SLAM算法包是如何工作的，怎样快速的搭建起SLAM算法。

地图 ： ROS中的地图很好理解，就是一张普通的灰度图像，通常为pgm格式。这张图像上的黑色像素表示障碍物，白色像素表示可行区域，灰色是未探索的区域

地图在ROS中是以Topic的形式维护和呈现的，这个Topic名称就叫做 /map ，由于 /map 中实际上存储的是一张图片，为了减少不必要的开销，这个Topic往往采用锁存（latched）的方式来发布。地图如果没有更新，就维持着上次发布的内容不变，此时如果有新的订阅者订阅消息，这时只会收到一个 /map 的消息，也就是上次发布的消息；只有地图更新了（比如SLAM又建出来新的地图），这时 /map 才会发布新的内容。 这种方式非常适合变动较慢、相对固定的数据（例如地图），然后只发布一次，相比于同样的消息不定的发布，锁存的方式既可以减少通信中对带宽的占用，也可以减少消息资源维护的开销。

Gmapping ，Gmapping算法是目前基于激光雷达和里程计方案里面比较可靠和成熟的一个算法，它基于粒子滤波，采用RBPF的方法效果稳定，许多基于ROS的机器人都跑的是gmapping_slam。

gmapping的作用是根据激光雷达和里程计（Odometry）的信息，对环境地图进行构建，并且对自身状态进行估计。因此它得输入应当包括激光雷达和里程计的数据，而输出应当有自身位置和地图。

论文支撑：R-LINS: A Robocentric Lidar-Inertial State Estimator for Robust and Efficient Navigation

6轴 IMU：高频，聚焦自身运动，不采集外界环境数据
3D LiDAR：低频，聚焦车体运动，采集外界环境数据

R-LINS使用以上两种传感器来估计机器人的运动姿态， 对于任一传感器而言，单独的依靠自己的数据是很难实现地图构建的， 比如纯雷达模型使用的传感器是激光雷达，可以很好的探测到外界的环境信息。但是，同样的，也会受到这些信息的干扰，再长时间的运算中会产生一定的累计误差。为了防止这种误差干扰到后续的地图构建中，需要使用另一种传感器来矫正机器人自身的位姿信息， 即IMU传感器，IMU传感器由于是自身运动估计的传感器，所以，采集的都是自身运动的姿态信息。可以很好的矫正激光雷达里程计的位姿信息。所以，通常使用激光雷达和惯导来进行数据融合，实现姿态信息的矫正。

一共分为三大块：