基于激光雷达地室内移动机器人SLAM方法研究与实现

基于激光雷达地室内移动机器人SLAM方法研究与实现
摘要
SLAM技术又叫CML技术，一种及时定位与地图构建技术，具体可以说成机器人进入一个陌生环境后，机器人通过激光雷达或者视觉相机、里程计等手段获取外界信息，实现对外界环境的感知，从而达到定位机器人自身，构建地图以实现机器人能自主移动的问题。

在本文，我们使用了RPLIDAR A1激光雷达，结合机器人轮式编码里程计和一个九轴IMU来实现机器人对外界环境的感知。

在这个过程中，首先机器人移动所产生的数据，如线速度，航向角由轮式里程计获得，然后通过STM32位驱动板发布到机器人底盘节点中，同样IMU惯性导航单元感知外部的角速度，加速度，通过驱动板发布到机器人地盘节点中，激光雷达则负责构建周边环境的点云图像，采取周边墙壁障碍物的特征点来实现对周边地图的构建。

在这个过程中各个部分并不是单独工作的，而是数据联合工作的，机器人需要定位自身才能构建更精准的周边地图，而机器人定位自身又需要构建周边的地图，这个问题不妨可以说成是“鸡”生“蛋”还是“蛋”生“鸡”的关系，机器人的定位需要周边地图，周边地图也需要机器人的定位。

我们主要工作是采用EKF算法来讲机器人发布的里程计数据和IMU数据进
行数据融合，因为机器人在实际运动过程中经常会发生滑动，比如在机器人急停时会有刹车制动滑动，在机器人转向时会发生侧滑等等，所以，单一的里程计数据或者单一的IMU数据误差会很大，测得的数据也会非常不准确，这就会造成机器人构建的周边雷达点云地图和实际地图会有较大偏差，所以我们采用了EKF 算法来融合IMU和里程计数据，来实现IMU数据和里程技术的相互矫正，使得机器人定位更加精确，构建周边地图更加准确。

我们采用了Gmapping算法来构建周边地图，配合EKF融合数据来实现精确定位和构建更好的地图。

关键词：SLAM，EKF数据融合，激光雷达
第一章绪论
1.1 课题背景及研究的目的和意义
近年来，随着计算机技术、视觉技术以及人工智能的飞速发展，不仅使得机器人从军事领域逐步融入到民用领域的各方面，也让中国机器人的发展迈向了一个新的台阶，近几年，中国机器人市场规模逐年快速增长，如图 1.1，从2014年的34.8亿美元一路增长到2019年86.8亿美元，占世界机器人市场规模的29.52%。

同时由于人工成本持续上涨、人口老龄化程度增大、服务机器人产业的兴起，可以预期，中国机器人产业将会更进一步蓬勃发展。

与此同时，机器人在工作方式上也产生了巨大的变革，从传统的手动遥控机器人到现在的机器人自主化控制。

机器人在逐渐替代人类完成一些比较危险和重复的机械性工作，解放人类的双手，最大化保障人类的安全性的同时，提高产品质量稳定性，提升生产效率，为企业生产和供应链管理提供了新的思路，一定程度上推动了一些传统行业的进步与发展以及新兴产业的诞生。

机器人的智能化与自主化发展离不开导航定位技术。

未知环境下智能机器人如何能自主移动并安全抵达目标点，是智能机器人的基本能力，也是发展机器人产业的关键技术。

目前，智能机器人的自主移动能力已经受到越来越多相关领域专家的关注与重视，而解决机器人自主移动的困难点和焦点在于机器人定位，也就是机器人系统知道“我在哪”、“我去哪”。

以前的导航定位系统主要采用 GPS 的方式来实现，然而，当机器人进入某些特殊环境工作时，易受到环境的干扰使得GPS信号变弱或者失效，这一缺陷使得机器人在某些特殊环境下无法工作。

为了弥补机器人基于 GPS导航定位系统的不足，目前已经有了一些成熟方案，如：通过无线网络进行指纹算法定位；安装传感器人工进行路标点定位；安装多个摄像头进行视觉辅助定位等，虽然这些方法针对行业需求在某种程度上提供了相关的解决方案，但是这些辅助型设备的大量使用既不方便后期维护，也会增加场地设备安装和布线的复杂程度。

更为重要的是，一旦机器人进入到一个未知环境中，以上方法都不可行。

采用SLAM技术作为机器人的自主导航方式，机器人可以在没有外界GPS信号提供的基础之上，实现周围环境的感知并且根据感知到的环境来进行自身的定位。

SLAM中的定位与建图功能不仅能够提供机器人自主移动所需要的精确位置，而且还能够创建出具有拓扑功能的连续一致性地图，地图是移动机器人自主导航的基础，能够限制机器人长期移动工作下的定位误差，在机器人实际工作的过程中，里程计的误差会随着时间积累，使用地图机器人就可以重置定位来使误差消
失。

SLAM技术遍布机器人导航、自动控制、人工智能的各种领域，尤其在无人驾驶、室内测绘、智能服务机器人等方面应用广泛。

如图1-2。

（a）（b）
图1-2 SLAM的应用。

（a）无人驾驶汽车；（b）扫地机器人
1.2 SLAM的发展和研究现状
SLAM发展历史
SLAM领域在过去三十年的时间中取得了瞩目的成就，众多科研人员将SLAM 当做学术前沿来进行研究。

在1986年到2004年之间，被称为SLAM的“传统时代”，在这期间，SLAM问题被提出，并转换为一个状态估计问题，利用扩展卡尔曼滤波、粒子滤波及最大似然估计等方法求解。

SLAM技术的起源可以追溯至20世纪80年代，在1986年，美国加州的机器人自动化大会上SLAM算法被首次提出，该算法当时第一次被Cheeseman, Self以及Smith三人提出。

与此同时，在1986年，概率统计的概念被引入至同时定位与地图构建算法中。

SLAM研究的历程中，最开始时期的研究都是基于概率统计的仿法，例如Gutmann以及Leonard 的相关研究，是基于卡尔曼滤波的SLAM算法，但此算法只对于线性系统有作用。

在1990年，Cheeseman及Smith通过扩展卡尔曼滤波将SLAM算法延伸到非线性系统，解决了KF对于现行系统的限制。

之后，基于粒子滤波的SLAM算法被提出。

粒子滤波在EKF的基础上进一步进行了扩展，不要求高斯系统且为非线性，提高了算法的适用性。

粒子滤波的思想在初始时，首先按照一定的先验概率分布信息随机性的产生指定数量的粒子，然后进入更新，根据自身的状态转移方程来更新对粒子的位置姿态信息，接下来通过评价函数来评价粒子的可信度对其权重进行计算并进行重采样，最后进入下一时刻的评估。

而在2004年到2015年这段时间SLAM的基本特性，包括观测性、收敛性和一致性，成为研究的主流，被称作“算法分析时代”。

在这十几年的时间里，基于稀疏法即特征点法的SLAM算法成为主流。

开源出很多SLAM算法库，与此同时，机器视觉也得到了发展，其中包括视觉里程计(VO)和运动恢复结构(SFM）。

视觉里程计通过视觉传感器感知周围的环境，以图像的形式输入信息，建立深度图，来求得机器人的具体位姿。

2004年，一种实时视觉里程计方法被Nister等人提
出，其提出的运动恢复结构模型能够在多张连续图像中回复相机位姿，达到精确定位的目的，相当于一个本地SLAM问题，该模型迅速成为了一种标准方法并沿用至今。

在SLAM的发展中，由于滤波的方法在一定程度上进行了马尔科夫性假设，该类方法却存在一定的非线性误差，故而提出了基于非线性优化的SLAM方法。

二十一世纪之后，科研人员将运动恢复结构中的光束平差法引入到SLAM问题中，它不是一个迭代的过程，而是考虑过去和现在所有的信息，然后在数据整体上更精确求解机器人位姿的方法，该方法在近些年得到了广泛应用，其适用范围更大，在机器人状态发生较大改变时也能够适用。

现在普遍认为在同等计算量的情况下，非线性优化的方法能够得到比滤波的方法得到更好的结果。

2015年至今则是鲁棒性-预测性时代robust-perception，鲁棒性、高级别的场景理解，计算资源优化，任务驱动的环境感知慢慢成为主要发展方向。

SLAM研究现状
移动机器人能够在己知或未知环境中自主运行和定位是非常重要的。

精确的位置估计在任何运动系统中都是核心，比如定位、地图构建或路径规划。

纯粹的推算方法，例如里程计，容易产生随时间无限增长的误差。

这个问题产生了各种各样的解决方案，使用不同的外部感受传感器(声纳、红外、激光、视觉等)。

在各种类型的传感器中，激光雷达在移动机器人中越来越受欢迎，激光雷达目前已用于本地化、动态地图构建或特征跟踪(例如用于避碰)等场景。

与其他传感器相比，激光雷达有许多优点:它们提供更密集和更精确的距离测量，它们具有高采样率、高角度分辨率、良好的距离分辨率。

另一个移动机器人的问题是如何准确地将雷达数据与地图和收集的信息进行匹配。

移动机器人中常用的匹配技术有两种:点匹配和特征匹配。

COX的早期工作使用距离数据来帮助机器人在一个小的多边形区域中自我定位，利用迭代最小二乘极小化方法，提出了一种先验映射中点图像与目标模型的匹配算法。

Lu and Milios的另一项工作则是将自我定位问题置于一个未知的环境中，提出近似两个连续扫描的对齐，然后通过定义和最小化扫描之间的距离迭代矫正对齐。

与直接处理原始测量点不同，基于特征的匹配首先将原始扫描数据转换为几何特征数据，然后将提取的特征用于下一步的匹配。

近年来，该方法在有限元提取、特征跟踪、机器人定位、动态地图构建等方面得到了广泛的研究和应用。

由于数据形式更紧凑，他们需要的内存更少，却能提供丰富和准确的信息。

因此，与点算法相比，参数化几何有限元算法效率更高。

在许多几何基元中，线段是最简单的，用线段来描述大多数环境是很容易的，利用从二维数据提取的直线特征，提出许多移动机器人算法。

如一种基于计算机视觉算法的线段分割法，利用先验映射作为机器人定位的方法，基于几何特征的动态地图构建算法等等。

Arras and Siegwart在地图的定位中使用了一种直线
重复的二维扫描分割方法；Jensfelt and Christensen 提出了一种利用激光扫描仪提取正交直线来获取和跟踪办公环境中移动机器人姿态技术；Pfister, Roumeliotis提出了一种基于线的地图构建中使用加权线拟合的线提取算法；Brunskill and Roy提出了一种增量概率一二值化技术来提取片段。

现阶段较为流行的算法包括:包括霍夫变换(Hough Transform ), RandomSample Consensus方法(RANSAC)、最小二乘(Least square )、分裂和合并方法(Split-and-Merge)等。

其中，霍夫变换(Hough transform algorithm)往往是最成功的应用于线性强度图像，它己经被引入机器人技术中，用于从扫描图像中提取直线NSAC-Random Sample Consensus 是一种在数据异常情况下对模型进行鲁棒拟合的算法，RANSAC的主要优点是它是一种通用的分割方法，一旦我们有了特征模型，就可以与许多类型的特征一起使用，它的实现也相对简单，这种算法在计算机视觉提取特征中非常流行。

最小二乘方法( least-squares method)来计算直线参数及其协方差矩阵，一旦我们有了一组由算法确定的线性矩阵，该方法倾向于将更多的权重放在有噪声的边缘点上该技术克服了众所周知的最小二乘方法的偏置问题。

线性回归算法(Lineregression algorithm)在Arras and Siegwart 中被提出用于基于地图的定位，该算法的核心思想来源于霍夫变换算法，该算法首先将线抽取问题转化为模型空间(线参数域)中的搜索问题，然后应用聚类层次聚类(AHC)算法构造相邻线段，该算法的一个缺点是实现起来相当复杂。

增量算法(Incremental algorithm)的主要优点是简单，它己在许多应用中得到应用，其也被称为行跟踪。

分裂-合并方法可能是最流行的线提取算法，它起源于Pavlidis and Horowitz于1974年提出的计算机视觉，它己被研究和应用于众多机器人研究。

在基于激光传感器的SLAM研究中，H Zhou等人在文章中提出了一种运动
恢复结构的方法，在这种方法中，将三维目标在空间中的目标函数进行迭代计算，以此来提高SLAM方法的鲁棒性及其处理异常值的稳定性，与此同时，将扩展卡尔曼滤波的方法与运动恢复结构的方法进行紧密的结合，利用滤波的方法来提高整个系统的可扩展性，方便激光传感器采集的信息与其他的传感器采集到的信息进行融合，从而提高系统的稳定性和鲁棒性，并且该研究在文章的最后部分对于系统的收敛性进行了验证。

苏黎世联邦理工学院的Autonomus Systems Lab团队也针对复杂环境下机器人智能感知、精确自主定位、路径规划等问题进行了深入研究。

该团队目标是创建能够在复杂多样的环境中自主运作的机器人和智能系统，包括用于感知、
定位、地图构建和路径规划的新颖方法。

2011年，Michal Jamal和Dale Schinstock等人在文章中提出了一套起降自动化物流设备导航系统，这是一个基于激光传感器的SLAM系统，通过测量相
机的位置和方向并使用来自激光传感器采集的图像信息来构建周围环境的地图，成功的构建周围环境地图并且求解位姿控制自动化物流设备运行。

根据先前的文献，已经开发了许多估计理论方法来实现SLAM。

这些方法可以分为两个主要框架:基于过滤的SLAM和基于优化的SLAM。

基于过滤的算法在SLAM问题中发挥了重要作用，例如最早的EKF-S LAM，包含噪声的无迹卡尔曼滤波器(UKF-SLAM)和粒子滤波器(PF-SLAM )。

而基于优化的算法包括束调整(BA ) ，姿势图SLAM等，基于优化的方法被证明比基于过滤的方法具有更高的一致性，这些方法已广泛应用于激光SLAM。

作为经典的滤波算法，EKF-SLAM包括两个步骤:预测步骤和校正步骤以解决SLAM问题而非线性运动和观测模型通过线性化模型通过泰勒展开近似求解。

近年来，已经开发许多EKF-SLAM的变体来解决各种环境中的SLAM问题，例如室内，室外，水下环境等。

同时，己经开发了许多方法来改进和分析关于EKF-SLA的准确性和效率的性能,在Gregor Klancar中，提出了一种用于在线估计输入和输出噪声协方差矩阵的新方法，以改善基于EKF 的定位和SLAM的收敛性和稳定性，建议在Santhanakrishnan, Rayappan中使用点特征方法来实现EKF-SLAM的有效实施。

综上所述，SLAM的研究已经有三十多年的历史，激光雷达SLAM更是研究热点，其能够有效的解决机器人自主定位的问题，具有极高的研究意义。

但目前国内对于同时定位与建图技术的研究相对于国际来说起步较晚，一些高校对于该技术进行了一定的研究，但是研究成果较世界先进成果相比还有差距，需要进一步增加科研投入，提高研究成果水平。

1.3 论文内容与章节安排
本文中一共包含四章内容，首先对对所研究内容的背景意义和相关技术进行了阐述，归纳总结了激光和视觉SLAM的发展历史和研究现状。

搭建了对于自主定位算法进行验证的实验平台，包括机器人平台、坐标系统、环境定位系统、传感系统等。

并在此基础上对ROS机器人的通讯机制和线段地标环境的线性特征的关键技术进行了研究。

紧接着本文对高精度算法进行了优化，并做了算法实验。

最后对自主定位和地图构建的关键技术进行了总结和展望。

下面为每一章的具体内容：
第一章是绪论，首先对SLAM技术的研究价值进行了调研，对SLAM技术的发展历史和研究现状进行了总结，明确了研究方向与目标。

第二章对实验平台进行搭建，会分别对载体机器人、传感器进行介绍，对于基于ROS机器人的通讯机制进行阐述。

第三章会详细说明扩展卡尔曼滤波（EKF）算法的一些原理和架构，会对激光雷达和IMU进行数据融合的方法和原理进行研究。

第四章会分别对实验平台、EKF滤波效果、地图实际构建效果和误差进行分
析，对误差来源进行探讨，并对实验进行总结归纳。

第二章实验平台设计与搭建
2.1 SLAM综述
SLAM的概念
SLAM ,也称为CML , 由Smith、Self和cheeseman于1988年提出。

即时定位与地图构建技术，或并发建图与定位技术。

问题可以描述为：将一个机器人放入未知环境中的未知位置，让机器人一边移动一边逐步描绘出此环境增量式地图，来实现机器人的自主导航和定位，所谓增量式地图是指不受障碍行进到房间可进入的每个角落。

由于其重要的理论与应用价值，被很多人认为是实现全自主移动机器人的关键。

在室内有限环境空间下，机器人的姿态位置变化非常快，需要分米级以上的精度才可能满足实时导航控制的要求。

而使用SLAM技术可以实现高精确定位，在此基础上我们将理论应用于实践，使用激光雷达、轮式里程计和IMU，使机器人在GPS弱信号的室内空间中能够完成地图构建的任务。

基于传感器的SLAM类型
基于所用传感器的不同，可以将SLAM大致分为三类（1）激光雷达SLAM；（2）视觉SLAM；（3）激光雷达和视觉融合的SLAM。

激光雷达SLAM又根据使用雷达的不同可大致分为2D SLAM和3D SLAM。

视觉雷达根据使用视觉相机的不同可分为单目SLAM、双目SLAM以及使用深度相机的RGB-D SLAM。

在SLAM技术中用于获取外部数据的传感器主要包括单目相机、双目相机、激光雷达和深度相机等。

（1）单目相机
单目相机由单个摄像头组成，体积小，画质清晰，单目相机将真实环境中的特征点映射到二维平面的过程可以简易的用几何模型来实现，如针孔模型。

（2）双目相机
双目相机分别由两个摄像头组成，两摄像头间的距离即基线长度固定的情况下组成摄像头，可以分别返回两个摄像头所获得的图像信息。

（3）深度相机
RGBD相机由一个单目相机以及一个发射结构光的传感器组成，在返回图像摄像头的图像信息的同时，可以由结构光返回的时长计算出前方物体与相机的距离，因此该摄像头可以同时返回一个深度图。

（4）激光雷达
激光雷达，通过发射激光束探测目标物体位置、速度等特征量的雷达系统。

工作原理是向物体发射激光束，然后将接收到的从物体反射回来的信号和发射信号进行对比，从而得到物体或自身状态的变化量，获得相关信息。

SLAM的传感器各有优缺点，通过表一对他们的优缺点进行对比
传感器优点缺点
单目相机模型简单，成本低无法确定深度信息
双目相机能通过视觉差计算深度信息容易受环境光影响，计算量大，要解决两个相机像素匹
配问题
深度相机能直接获得深度信息对环境要求高，测量范围窄
激光雷达精度高，方向性强得到信息类型受限，成本高
表一SLAM传感器间比较
激光SLAM技术的基本流程
机器人在运动过程中通过轮式编码器结合IMU计算得到里程计和IMU融合信息，运用机器人运动模型得到机器人的位姿初估计，然后通过机器人装载的激光雷达获取的激光数据结合观测模型对机器人位姿进行精确修正，得到机器人的精确定位，最后在精确定位的基础上，将激光数据添加到栅格地图中，反复如此。

机器人在环境中运动，最终完成整个场景地图的构建。

基本流程大致可以概括成4个模块：前端匹配、后端优化、图结构及回环检测。

图2-1 激光SLAM技术流程图
2.2 ROS架构设计
ROS 操作系统是目前在SLAM机器人方面运用最多，运用最广的操作系统之一，ROS操作系统简单轻便，更容易集成与其他机器人软件框架，ROS测试简单，扩展性强很容易在任何编程语言中执行，因此，我们选用ROS操作系统作为SLAM 机器人的操作系统。

ROS 的架构如图 2-1 所示，可以将其分为三个层次：OS 层、中间层和应用层。

图 2-2 ROS 架构
OS层
ROS 并不是一个传统意义上的操作系统，无法像 Windows、Linux 一样直接运行在计算机硬件之上，而是需要依托于Linux 系统。

所以在 OS 层，我们可以直接使用ROS 官方支持度最好的 Ubuntu 操作系统，也可以使用 macOS、Arch、Debian 等操作系统。

在本文中，我们使用了 Ubuntu 16.4 版本进行操作。

中间层
Linux 是一个通用系统，并没有针对机器人开发提供特殊的中间件，所以ROS 在中间层做了大量工作，其中最为重要的就是基于TCPROS/UDPROS 的通信系统,ROS 的通信系统基于 TCP/UDP 网络，在此之上进行了再次封装，也就是TCPROS/UDPROS。

通信系统使用发布/订阅、客户端/服务器等模型，实现多种通信机制的数据传输。

除了 TCPROS/UDPROS 的通信机制外，ROS 还提供一种进程内的通信方法—— Nodelet，可为多进程通信提供更优的数据传输方式。

在通信机制之上，ROS 提供了大量机器人开发相关的库，如数据类型定义、坐标变换、运动控制等，可供给应用层使用。

应用层
在应用层，ROS 需要运行一个管理者——Master，负责管理整个系统的正常运行。

ROS 社区内共享了大量的机器人应用功能包，这些功能包内的模块以节点为单位运行，以 ROS 标准的输入输出作为接口，开发者不需要关注模块的内部实现机制，只需要了解接口规则即可实现复用，极大地提高了开发效率。

2.3 从系统实现角度将ROS 划分成的三个层次
从系统实现的角度来看，ROS 也可以分为如图 2-2 所示的三个层次：计算图、文件系统和开源社区。

图 2-2 从系统实现角度将 ROS 划分成的三个层次
计算图
从计算图的角度来看，ROS 系统软件的功能模块以节点为单位独立运行，可以分布于多个相同或不同的主机中，在系统运行时通过端对端的拓扑结构进行连接。

（1）节点
节点就是一些执行任务的进程，一个系统由多个节点组成，也称为“模块”。

节点的引入使得基于 ROS 的系统在运行时更加具体，当许多节点同时运行时，可以很方便地将端对端的通信绘制成如图 2-3 所示的节点关系图，在这个图中进程就是图中的节点，连接关系就是节点之间的连线。

图2-3节点关系图
（2）消息
节点之间最主要的通信机制就是基于发布/订阅模型的消息（Message）通信。

每一个消息都是一种严格的数据结构，支持标准数据类型（整型、浮点型、布尔型等），也支持嵌套结构和数组（类似于 C 语言的结构体 struct），还可以根据需求由开发者自主定义。

（3）话题
消息以一种发布/订阅（Publish/Subscribe）的方式传递，如图 2-4 所示。

一个节点可以给一个给定的话题（Topic）发布消息（称为发布者/Talker），也。