激光SLAM_Backup

激光SLAM-论文

本章主要介绍另一种基于 Levenberg-Marquardt的激光 SLAM算法。
图优化中所说的“图”是由节点和弧组成的，节点代表某一时刻的位姿，弧代表位姿之间的约束。
HectorSLAM 主要功能是导航定位，其主体是二维激光，生成的地图为二维地图；Cartographer 算法对里程计的要求相对较高，

激光按照测量方式大致可以分为基于脉冲式、相位差和三角测距原理三种类型；
ROS的优势：（1）ROS 提供一种跨平台、模块化的软件通讯机制，利用节点的概念来表示一个应用程序（进程），不同节点之间通过定义好格式的消息、服务和动作来实现通讯，这样的设计保证了程序的高并行性和拓展性，对于程序的维护和二次开发来说是很大的便利。（2）ROS 提供一系列的开源工具用于开发人员设计开发，提高了开发效率，比如可视化工具 rviz、坐标系工具 tf 和节点工具 rqt_graph 等。（3）ROS 集成了众多优秀的开源算法库。比如：OpenCV（机器视觉开源项目）、PCL（点云数据处理库）、Gazebo（实现动力学仿真、传感器仿真的仿真平台）等。
状态估计与观测更新。这部分是 SLAM 算法中最关键部分，常用的有基于滤波器和基于
非线性优化的方法。基于滤波器的方法主要有基于 EKF 的 EKF-SLAM [29] 和基于 PF 的
RBPF-SLAM，基于非线性优化方法的主要有高斯牛顿法和图优化法等。

采用欧拉角表示方法表示机体的转动时，很重要的一点是确定三个姿态角的转动顺序，常用的顺序是起始阶段机体处于与地面水平的位置，然后按照偏航、俯仰、横滚的顺序进行转动，最后得到一个从导航坐标系转到机体坐标系的转动矩阵。
ICP 算法的实质是求解非线性最小二乘问题。其主要思想就是通过不断的迭代来求解两个点云之间的最优运动变换。
ICP 的算法过程可以描述如下：
步骤 1：对 A、B 两片点云进行粗拼接，使得两片点云的初始位置误差不会太大；
步骤 2：确定两片点云中的对应最近匹配点和设定最小阈值 e ；
步骤 3：求解使得对应最近匹配点欧氏距离最小的运动变换，即公式（5-7）中的 T ；
步骤 4：比较此时f(T)与阈值 e 之间的大小，若f(T)<e，则停止迭代，反之则返回步骤 3，用 T 更新位姿，求新位姿下的匹配点，然后继续求解；



本文选用的 IMU 内部集成了三轴加速度计、三轴陀螺仪和三轴磁力计。对于加速度计，虽然可以通过反三角函数计算得到姿态角，但是由于测量的加速度本身具有比较大的高频噪声（如图 6-2 所示），动态响应较差，在旋转的瞬间计算得到的姿态角往往会有比较大的误差；对于陀螺仪，其动态响应良好，虽然可以通过按时间积分角速度来计算姿态角，但由于积分误差会随着时间积累（ADIS16480 的偏置稳定度在 6.25度 /小时，也就是每分钟的累积误差在 0.1 左右），系统长时间运行就会造成相当大的累积误差；所以常用的做法是对加速度计和陀螺仪的数据进行融合得到比较精确的横滚角和俯仰角。由于加速度计只能计算出两个轴方向的姿态，所以需要引入磁力计，与陀螺仪进行融合来估计偏航角。磁力计的特性基本与加速度计基本一致，动态响应较差，具有比较大的高频噪声，也不能单独用于姿态解算。

激光SLAM-深蓝学院

第1节 激光SLAM简要介绍

第2节 传感器数据处理I：里程计运动模型及标定
传感器：
惯性测量单元（IMU）：测角度
轮式里程计（Wheel Odometry）:测距离
激光雷达（Lidar）


第3节 传感器数据处理II：激光雷达数学模型和运动畸变去除

d=c*t;距离=光速×时间;
t=Δψ/2pif; 时间=相角差（角度Δψ）/角速度（2pif）



R是旋转矩阵，旋转矩阵都是正交矩阵，即R×RT=RT×R=I

两幅点云 X和P
EM方法：A-R和T矩阵，B-两幅点云的对应点（X×R+T在P中距离最近的点）
EM方法：求解非线性MLE（极大似然估计，如果是线性的，则构造极大似然函数求导=0）的工具
EM方法流程：要求A，必须先知道B（隐条件），则先固定A去求B，再固定B求A，再固定A求B…一直迭代直到收敛。
ICP：先已知R、T（假设单位阵），求对应点（X×R+T在P中距离最近的点），然后根据对应点求R和T，再固定R和T求对应点…最终R和T收敛


激光雷达运动畸变去除：里程计获得位姿辅助方法（轮式里程计获得位移和角度）

第4节 激光SLAM的前端配准方法



如图（c）,误差函数即为Pi与Pj1的连线（向量）与Pj1与Pj2连线的法向量点乘，即PiPj1在Pj1Pj2法向量方向上的投影。（即点到线的距离）。
PL-ICP比PP-ICP好，激光点的离散性导致两次打到同一个物体的不同部位，都是真实点，pp-icp算法此时会引入误差，pl-icp此时误差为0。（比如打到一堵墙） PL-ICP在ROS-KINETIC-CSM里面有开源代码实现。如图所示，点线匹配与点点匹配相比，误差函数不同，ICP为点点距离为误差，PL-ICP为点线距离为误差。因为激光数据为离散的而墙体为连续的，所以在前后匹配的两帧之间可能扫描到的点不是一个地方而强行匹配造成误差，所以点线匹配精度更高。

分枝定界能保证一定可以找到最优解。

!

第5节 基于滤波器的激光SLAM方法(Grid-based)




KF是贝叶斯在线性高斯情况的特例。

ut可以看成里程计的数据，zt看成激光雷达的数据，为了估计状态。

g代表先验概率，f代表后验概率。重采样的目的：让粒子群更符合后验分布。
粒子滤波、贝叶斯滤波等的目的都是为了估计状态。


slam_gampping：
主要程序：
slam_gampping.cpp
void SlamGMapping::laserCallback函数
if(addScan(*scan, odom_pose))

openslam_gampping：
gridslamprocessor.cpp
bool GridSlamProcessor::processScan
gridslamprocessor.hxx
inline void GridSlamProcessor::scanMatch
scanmatcher.cpp

第6节 基于图优化的激光SLAM方法(Grid-based)























误差的权重矩阵（正式名称为信息矩阵）等于协方差矩阵的逆。所以图优化里每一条边的信息矩阵就是这些测量协方差矩阵的逆。如果协方差越小，表示这次测量越准越值得相信，信息权重就越大。
图优化SLAM能分解成俩个任务：
1.构建图，机器人位姿当做顶点，位姿关系当做边，称为前端。
2.优化图，调整机器人位姿顶点尽量满足边的约束，称为后端。
图优化过程中：先堆积数据，机器人位姿为构建的顶点。边是位姿之间的关系，可以是编码器数据计算的位姿，也可以是通过ICP匹配计算出来的位姿，还可以是闭环检测的位姿关系。

第7节 基于已知定位的建图

















mj大于某个阈值设为障碍物，小于某个阈值被认为是通行的。

https://blog.csdn.net/u011722133/article/details/51673806 Bresenham画线算法


https://zhuanlan.zhihu.com/p/21738718 构建栅格地图
https://blog.csdn.net/changer_sun/article/details/79147446 概率法——利用激光传感器构建占据栅格地图

第8节 3D激光SLAM介绍


si和di是对应点，m表示变换矩阵（T和R），M×Si-di表示两个对应点连成的向量，再点乘平面的法向量表示两个对应点连成的向量在平面法向量上的投影，即点到平面的距离。