标签：Scan -- Pn 理论 地图 学习 slam 优化 激光雷达
内容作为激光slam框架概念版 学习参考，转自知乎网友 https://www.zhihu.com/column/c_1007202713288966144
前言：
开通这个专栏的目的是为了传播激光雷达SLAM相关工作，为激光雷达SLAM做出一份贡献，我会尽量以最短时间内把所有研究工作展现出来。
- 首先要明白，目前来说激光雷达SLAM是非常成熟的，已经不是什么特别高科技的东西，不管是理论研究和实际商业落地都是成熟和成功的。目前很多算法都是好几年前甚至一二十年前的东西，这个是很显然的，因为商业上成功的东西，其实它们在实验室里已经待了很长时间了。我在2011左右研究方向为4G通信信号处理相关的内容，但是那时发表关于4G有关的论文是比较困难的了（可能因为本人水平有限的原因吧），因为那时通信的论文热点是关于5G的东西，可想而知。
- 既然是激光雷达SLAM，可以只需要一个激光雷达传感器就行了，不需要IMU等等，可能在实际商业产品中需要IMU、超声波、惯导、红外等其它传感器和方法，但是我把注意力永远放在激光雷达上。
- 需要一台笔记本（或者台式）（我的配置：win 7 64位，CPU为i3 2.4GHz，内存4G），如果你的配置比我好那就更好了。需要一个激光传感器，没有也没事，我会给你激光雷达传感器原始数据，那么你只需要一台电脑了。
- 专栏主要围绕三大主题展开：Scan to Scan Match； Scan to Map Match； 图优化理论（闭环或者回环检测）。主要就是基于优化理论的，本人认为这些属于state of the art，搞懂这些基本可以完成激光雷达SLAM相关项目了，因为这些理论是完备的充分的，涉及到了方方面面。基于概率统计理论的方法基本不涉及，扩展卡尔曼滤波EKF、粒子滤波等不讨论。不是说不讨论的东西就是不好的，我只是想用更简洁的方式去描述激光雷达SLAM导航，简洁不是简单，我只是把注意力放在优化理论上，专注一点，深入挖掘，毕竟人的时间精力有限。想起刚开始入手激光雷达SLAM项目的时候，就是从网上下载了Probablistic Robotics （概率机器人），被称为圣经的书（听说有第二版，我看的是最早的不到500页的PDF），里面全部是是基于概率统计理论方法，看了前面没几章，大致了解了SLAM的大概，给了一个入门作用，发现书中后半部分理论越来越深奥不可测，只好放弃另辟蹊径。书中的理论简单仿真实现过几个，后来越来越感到力不从心，感觉自己智商差书的作者太大了，只能放弃，现在当作茶余饭后为了激发灵感而随手翻阅。真正给我曙光的是发现了基于优化理论的SLAM新大陆，可能缘由我在大学时代研究凸优化理论（Convex Theory）和熟悉矩阵理论有关，发现本人对于非线性优化较敏感，看了一篇又一篇基于优化理论的SLAM论文，豁然开朗，原来其实就是这么回事。
- 专栏中涉及的理论或者论文都会一一注明，追根到底，而且都是推导验证仿真测试过的，不仅仅只是复制黏贴一大推公式，更加关注的是理论的说明，公式的Matlab实现，是讲的明白的，更是实际验证正确的。
第一篇：激光雷达
激光雷达选用北阳（HOKUYO）的产品：UTM-30LX-EW，工业级别，可用于室外，
价格为人民币3万一千块。
主要性能指标：1、数据传输为以太网。2、测量距离极限最大为60米，一般有效测量范围为30米之内。最小精度为正负3厘米，一般为5厘米。3、扫描范围270度，间隔0.25度一个激光测量数据，每帧激光雷达数据分割为1080个数据点。4、扫描频率最大40Hz，每隔25ms一帧雷达数据。
如还需了解相关特性可从官网上查询（需要注册），以下图片是激光雷达、官网相关资料和官方软件使用界面。
官方软件有保存测量数据功能，我们后期各种仿真测试都是基于原始测量的存储文档，以后落地为C++时需要实时读取以太网数据包中激光雷达数据即可（只要后期C++优化处理恰当，就可以实时读取、测量计算）。下图为保存的原始数据内容，可以Txt格式打开，因为是ASCII格式，Matlab可以很容易读取里面的数据（仿真测试阶段使用），测量数据单位为mm。
开篇结束语：
前言和第一篇到此为止，我主要讲了以后要做的三大任务：Scan to Scan Match； Scan to Map Match； 图优化理论（闭环或者回环检测）。主要就是基于优化理论的。另外说明了一下使用的激光雷达，以后开始就是正式的核心内容了，我会详细展开。
------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
分割线
------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
参考思岚科技官网的激光雷达模块说明了解基本参数：http://www.slamtec.com/cn/Lidar/A3
------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
分割线
------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
第二篇:Scan to Scan（上）
本篇主要讲述栅格地图构建和Scan to Scan方法。
- 栅格地图或者说是概率占据栅格地图初步
在Matlab中创建一个1000*1000大小的二维数组，如果数组中每个元素取值为0到255的整数（灰度图），再把这个数组用Matlab中的图片转换函数把数组转化为BMP图片（也可以其它你自己觉得合适的格式），这样我们就手动构建了一张地图了。直接用Windows自带的画图软件打开，可以在图上自己手动修改，这样就是手动地图修改，保存后在用Matlab读取这张BMP图片，转换为1000*1000的二维数组，这样就是地图的读取。说明：你也可以生成500*500，或者10000*10000大小，根据实际需求设置。图片中每个像素点代表一个单位距离，假如一个像素点代表0.05m，那么1000*1000大小的图片代表50m*50m大小的地图，以此类推，一个像素点也可以代表1m单位距离（距离分辨率小了）或者0.01m单位距离（距离分辨率大了）。在灰度图中0值为黑色，255为白色，可以假定0值为障碍物和255值为空（也可以反过来，随自己喜好），其它值代表这个位置是障碍物的概率或者是空的概率的大小，比如128代表既不是空也不是障碍物或者说两者的情形概率相等，比如200代表是空的概率大点，比如50代表是障碍物的概率大点。
下图为全是0值的1000*1000像素大小的地图，这里假定0值为空。
下图为自己随意画的几条线代表障碍物，假定255白色为障碍物。
下两幅图为实际利用激光雷达SLAM方法生成一个大的房间的地图，两幅地图本质一样，只是表达方式取值不一样，第二幅只显示了障碍物和空地（没探索到的地方也表示空了）。
- 对栅格地图的进一步说明
栅格地图理论可以从 概率机器人这本书中或者网上很方便的搜索到，我这边在仔细说明一下，以我自己实际应用的方式班门弄斧。
到此为止，栅格地图的大体已介绍完毕，而且这些也基本足够了，除非特别提到，后面将不再提起，因为如何画地图在激光雷达SLAM中只是很简单的一件事。
第二篇结束语：
其实到目前为止，你可以直接激光雷达数据地图构建了。怎么弄！假设第一帧激光雷达数据为原点[0，0，0]，通过Trim-PLICP得到第二帧和第一帧相对偏移量[x1,y1,theta1],那么第二点位置坐标为[x1,y1,theta1],第三帧相对第二帧偏移[x2,y2,theta2],那么第三点坐标为[x1+x2,y1+y2,theta1+theta2],……，以此类推求得所有经过的点坐标，又由于该点对应的激光雷达扫描数据也是知道的，就可以建图了。这就是增量建图原理。不过你马上就会提出问题，对的，这样累计误差会越来越大。虽然两帧之间匹配是很准确的，但是还是会有计算误差，积累起来到最后误差是可观的，假如你的地图好几百米的长宽。那么怎么办，很简单，专栏第三部分要讲图优化理论，就是为了消除回环或者闭环误差，我会到时详细求解。
另外再提一点，点云匹配方法一定要掌握，因为这是非常精确的求相对位置的方法。虽然可能它的运算量较大，但是在车速较慢，求相对偏移时是很好的，而且对于我们后面图优化理论约束条件的建立是很重要的。
最后再说一下，非线性优化迭代方法一定要掌握两三种，它是核导弹，查阅优化相关文献资料的得力助手，学了它，你查阅论文资料再也不会摸不着头脑了，而且有些论文你一天时间就可以把它模型仿真出来。在后面Scan to map 和图优化理论中，我基本上两三天看完一篇论文的大概，再花一天时间把它仿真出来。
第三篇:Scan to Map Match
本篇主要讲述Scan to map方法
- Scan to map ，即激光雷达扫描数据直接与地图进行匹配，得到实际位置坐标[x,y,theta]。这种方式一边计算位置，一边把新扫描到的数据及时加入到先前地图中。我这边主要参考论文：A Flexible and Scalable SLAM System with Full 3D Motion Estimation。这里我关心的只是二维激光雷达数据定位和建图，文献中的3D姿态估计等不涉及，只关心二维的。其实这篇文献就是Hector的理论基础。Google的Cartographer中前半部分submap建立就是用的这个方法，唯一区别就是原文献的双线性插值变成了Google的双三次插值，这么做应该是让数据更加smooth，插值方式可以随自己喜好，也可以选用其它数值计算相关书籍中的方法，这个主要是为了让数值计算更加稳定，避免出现计算崩溃，数据越smooth，计算可靠性越好。本篇讲诉比较简短，因为实现太简单了，编程很容易实现，这个前提是你要掌握非线性优化理论相关知识，因为文献中用了高斯牛顿迭代法求解。这里又出现了优化迭代方法，我在第二篇中也提出了一种高斯牛顿法简化原文献的计算思路。
- 第二篇中的Scan to scan 主要用于精确的相对位置求解，那么scan to map用来做什么呢。因为Scan to map 对于地图表示要求较高，一旦地图建错，后面将越来越错，就算地图不建错，它也是有误差在的。但是假如一张精确的地图已知，我们可以用Scan to map 给个大致定位，这个才是主要目标。而且它的定位计算量较小，因此在车速较高时也是可以达到实时的。下图就是我在十几米长宽的房间实现的一张小地图，一边Scan to map，一边把新的激光雷达数据加入地图中。地图50m*50m。
第三篇结束语：
本篇我讲诉比较简短，只选取了一篇代表性文献资料进行实现，因为在SLAM中相关方法没有太大的区别，而且Scan to map 在slam中不是特别有主导地位，比如说用来建图还是有误差，后端还是主要图优化来建图；它主要还是在前端作用，用来即时定位，这就是它的存在吧，因为计算量还是比较小的相比Scan to Scan。以后开始我要着重讲诉图优化原理方法，把图优化理论用到激光雷达slam中可以说是激光雷达slam到了成熟期，理论上基本已经可以不需要再去研究了，最多只是计算效率怎么提升下，因为它把地图误差消除处理的非常好，回环误差或者闭环误差在图优化理论下根本无处可躲。现在主流slam都转向了视觉，更是由于神经网络，深度学习等等这几年的再一次火热，视觉slam成为了最新研究热点，我目前不会讲诉相关视觉slam，主要还是以激光雷达为主，也许以后再有机会研究讨论下。
第四篇:激光雷达slam图优化理论
本篇开始，我要大张旗鼓地介绍激光雷达SLAM图优化理论，并以此理论设计后端地图优化方法，这样，关于激光雷达地图构建的理论就基本介绍完毕了，剩下的就是实际应用问题。本篇主要参考文献资料：
Globally Consistent Range Scan Alignment for Environment Mapping.
Efficient Sparse Pose Adjustment for 2D Mapping.
A Tutorial on Graph-Based SLAM.
- 图优化理论中，本质还是优化理论，主要涉及最小均方二次优化方法。这里的图与组合数学的图论还是有些区别，这里的图只是一个形象的比喻，本质是优化理论。自动导航中的路径规划Planning概念就比较深入讲解组合数学中图论的相关知识了，后期实际应用时我会专门详细说明一下相关概念，目前还是以优化理论为主导进行介绍。
2、
激光雷达扫描位置从P0开始，沿着P1，P2，……，一直到Pn-1，Pn。结束点Pn在起始点P0附近。Pn的坐标位置可以通Pn-1获得，当然也可以通过P0获得。显然由于误差存在问题，Pn分别从Pn-1和P0获得的坐标不一样。如通过Scan to scan，Pn通过P0获得的坐标准确点，通过Pn-1获得的坐标误差大点，因为累计误差较大；如通过Scan to map获得坐标，由于地图信息在不停地变换，因此P0、Pn两点地图信息和P0、P1、P2、…、Pn-1、Pn所有点地图信息也不一样，因此得到的Pn点坐标也不可能重合。
那么怎么办，简单方法就是可以通过Pn-1和P0的加权平均得到Pn的位置坐标。这样，Pn-1的位置坐标也要更新了，同样Pn-2,Pn-3等等都需要重新更新一下。因此我们需要一个统一的方法去处理如上的问题。
第四篇结束语：
利用图优化理论去消除位置坐标误差方法是非常完备的，这里的消除不是说没有误差，而是降到最低。一般具体应用时，我们一般采集好激光雷达数据，进行地图构建，地图构建中利用图优化理论降低回环误差，修正位置坐标，而且位置之间相互约束关系也根据实际情况求解，对于大范围地图，这个过程可能会消耗两三个小时。你可能会说，那么慢怎么实时。这里说明一下，首先工业级别激光雷达数据量比几百块或者上千块的电子消费级别（如扫地机器人，服务机器人）激光雷达大的多，同样的算法需要处理更多数据；一般实时指后期定位导航实时，前提是地图事先生成好了，我们在第一次建图时还是以准确建图为目标，而且一般地图肯定还需要进行人为修改后才会正式使用，这个是工业坏境中的应用。
图优化理论是强大的，我在实际测试数据仿真时在位置坐标中加入高斯噪声，优化出来的实际位置还很准确，如下两图对比：
下图位置坐标加入高斯噪声建图：
下图在加入高斯噪声位置坐标进行图优化计算后建图：
可以看到，好像滤波一样，把高斯噪声滤除了，第一次实际看到数据结果时还是蛮惊讶的，感叹图优化理论的强大。
最后说明：我目前为止只讲了优化的相关理论。为了简单说明，环境假设静态不变。但是后期实际应用，如动态路径规划，地图环境改变时导航定位等，我会另外抽空具体讲解一些应对方法。尤其是动态路径规划，实际坏境有静态的路径巡航，当然更多的是动态的巡航，而且在地图坏境变化时，比如环境改变，有人员在场内运动，这时的导航和定位环境更加恶劣，对算法可靠性要求更加大了，比起静态环境。这时也许需要其它传感器来辅助等等。
到目前为止三大部分讲解完毕，讲了皮毛而已，优化理论很广很强大，还需继续深入研究，有机会再探讨。
标签：Scan,--,Pn,理论,地图,学习,slam,优化,激光雷达 来源： https://blog.csdn.net/GGY1102/article/details/110005906
本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享； 2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关； 3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关； 4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除； 5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。