机器人SLAM技术及其ROS系统应用 第2版 课件全套 第1-5章 绪论、SLAM算法简介与实现-基于视觉的SLAM技术

第一章绪论目录CONTENTS01SLAM算介02SLAM数学模型03SLAM分类04ROS简介05基于ROS系统的SLAM技术06SLAM发展前景01SLAM简介SLAM（SimultaneousLocalizationAndMapping）中文称“同步定位及建图”是目前在机器人定位方面的热门研究课题。到目前为止，也取得了不少进展。SLAM问题可以描述为:机器人在未知环境中从一个未知位置开始移动,在移动过程中根据位置估计和传感器数据进行自身定位,同时建造增量式地图一般来说，机器人自主移动到指定位置的过程可以分解为这三个任务:（1）路径规划（2）定位（3）建图。而SLAM的目的就是解决“定位”与“地图构建”这两个问题定位(localization):机器人必须知道自己在环境中位置。建图(mapping):机器人必须记录环境中特征的位置（如果知道自己的位置）02SLAM数学模型

由于噪声的存在，将SLAM问题数学建模成一个状态估计问题，通过带有噪声的观测数据，估计状态变量。03SLAM分类SLAM算法输出为一张地图，地图可分为度量图，拓扑图，混合图或语义图。度量图可以将环境表示为栅格地图，特征地图或几何信息地图。地图度量地图栅格地图特征地图几何信息地图拓扑地图混合地图栅格地图表示法即将整个环境分为若干相同大小的栅格，对于每个栅格各指出其中是否存在障碍物。栅格地图●可以持续地获取环境信息特征地图通过指定的标记点、特征等精确度量位置来表示环境。几何信息地图表示法是指机器人收集对环境的感知信息，从中提取更为抽象的几何特征，例如线段或曲线，使用这些几何信息描述环境。特征地图几何信息地图●直观形象，便于导航●对其传感器要求较高●相对数据存储量和运算量比较小拓扑地图抽象度高，特别在环境大而简单时。这种方法将环境表示为一张拓扑意义中的图(graph)，图中的节点对应于环境中的一个特征状态、地点。如果节点间存在直接连接的路径则相当于图中连接节点的弧。●紧凑，需要的存储空间小●传感器信息模糊很难构建大环境下的地图，也可能产生的路径不是最佳路径拓扑地图混合地图包含地图的拓扑和度量信息。例如，一些度量图通过拓扑关系连接起来。混合地图中的每个节点可以是小型度量地图，或者是某个地方的定性信息，也可两者都有。●根据用途不同可以访问不同级别的信息●典型如语义地图实现人与机器人在概念上就能交互混合地图04ROS简介

ROS是面向机器人的开源的元操作系统（meta-operatingsystem）。它能够提供类似传统操作系统的诸多功能，如硬件抽象、底层设备控制、常用功能实现、进程间消息传递和程序包管理等。此外，它还提供相关工具和库，用于获取、编译、编辑代码以及在多个计算机之间运行程序完成分布式计算。

ROS的基础框架ROS的总体设计一个目标•提高机器人研发中的软件复用率五个特点•点对点设计

•多语言支持

•架构精简、集成度高

•组件化工具包丰富

•免费并且开源四位一体•ROS=通讯机制+开发工具+应用功能+

生态系统05

基于ROS系统的SLAM技术基于激光的SLAM技术最底层机器人本身的电机驱动和控制部分中间通信层通信通路决策层例Gmapping包1、基于激光的SLAM技术Gmapping包是在ROS里对开源社区openslam下Gmapping算法的C++实现，该算法采用一种高效的Rao-Blackwellized粒子滤波将收取到的激光测距数据最终转换为栅格地图。粒子滤波其核心思想是通过从后验概率（观测方程）中抽取的随机状态粒子来表达其分布，是一种循序重要性采样法。粒子滤波主要步骤如下：

（1）初始化阶段

（2）转移阶段

（3）决策阶段

（4）重采样阶段

（5）滤波

（6）地图生成2、基于视觉的SLAM技术

根据相机工作方式的不同，可分为以下三种:单目只能反映三维场景的二维图像双目通过左右眼图像的差异来判断场景中物体的远近，能从直接提取完整的特征数据。RGB-D可同时获取图像彩色信息和深度信息。

视觉SLAM经典方法的比较激光SLAM与视觉SLAM的优劣势06SLAM发展前景

视觉与惯性导航融合SLAM和深度学习技术结合。基于超宽带（ultrawideband,UWB）的定位技术基于线/面特征的

SLAM动态场景下的SLAM多机器人的SLAM。。。SLAM算法简介与实现第二章目录CONTENTS01SLAM算法简介02基于矢量的SLAM经典算法03基于随机有限集的SLAM算法01SLAM算法简介1.SLAM算法分类基于矢量的SLAM算法：应用最早和最广泛的算法，它将每一时刻机器人的位姿信息和地图特征都表示成地图、轨迹联合状态矢量(即向量序列)的形式，并通过递归公式对地图、轨迹联合状态后验概率密度进行递归估计。基于扩展卡尔曼滤波（ExtendedKalmanFilter,EKF）的EKF-SLAM算法和基于序贯蒙特卡罗的FastSLAM算法都属于比较典型的基于矢量的SLAM算法。基于随机有限集的SLAM算法：这类算法与传统的算法相比，最大的不同之处是以随机有限集的形式来代替矢量表示。RFS是集合形式的随机变量，其元素的数量和顺序都是随机的。利用RFS理论对机器人SLAM问题进行建模，即用随机有限集合而不是矢量形式来表示每一时刻的地图特征状态和观测信息，这使得多特征－多观测状态通过这种方式得到更加有效的表达。2.1基于矢量的SLAM算法特点机器人采用条件概率表示置信度，置信度是以获得数据为条件的关于状态变量的后验概率，用

表示在执行完后，进行观测之前所计算的后验概率密度表示为：该概率也常被称为预测。是基于以前状态的后验，在综合时刻的观测之前，预测了时刻的状态。EKF优势：●既简单又高效的计算速率缺陷：●计算复杂度过高。其复杂度与环境中路标（特征）数量的平方成正比

●存在数据关联问题无法处理虚警、漏检和观测的不确定性等问题。FastSLAM优势：●使用个独立的EKF滤波器完成地图特征的估计，其计算复杂度降低为

●适合大范围场景中的应用缺陷：●必须解决新的特征观测和地图中已有特征之间的数据关联问题2.2基于随机有限集的SLAM算法

随机有限集理论扩大了随机变量统计理论的应用范围，实现了随机变量统计理论到随机集统计理论的推广。用RFS处理SLAM问题，也就是将地图特征和传感器观测信息都表示成随机有限集合的形式。优点：●避免数据关联●避免特征管理●误差定量分析科学3.1贝叶斯滤波算法第一步为预测，通过基于状态的置信度和控制来计算状态的先验置信度第二步为更新，用已经观测到的观测的概率乘以预测置信度。利用归一化常数对结果进行归一化，最后推导出最后的后验置信度基本的贝叶斯滤波算法先验：全概率公式：后验：贝叶斯公式：1：AlgorithmKalman_filter:2:3:4:5:6:7:return1：AlgorithmExtended_Kalman_filter:2:3:4:5:6:7:returnKF算法EKF算法3.2卡尔曼滤波算法3.3粒子滤波算法1：AlgorithmParticle_filter:2:3:fortodo4:sample5:6:7:endfor8:fortodo9:drawwithprobability10:addto11:endfor12:return用粒子集来表示置信度由粒子集递归地构建粒子集02基于矢量的SLAM经典算法1.EKF-SLAM算法基本原理EKF-SLAM算法估计机器人位姿的同时，还对估计路径中所遇到的所有地标点的坐标进行估计。将包含机器人位姿和地图的状态矢量称为联合状态矢量，并将该矢量定义为。用均值和方差来描述机器人在时刻的联合状态，该联合状态矢量由下式给出：该状态矢量是维的，式中、、为机器人在时刻的位姿坐标；和为第个地标的坐标，；为该地标的签名。

EKF-SLAM算法流程图（1）预测机器人从时刻到时刻的状态变化可由非线性函数来描述：

和为状态向量；为控制向量，包含平移速度和旋转速度随机变量是一个高斯随机向量，表示状态转移的不确定性预测机器人运动时，状态矢量根据标准带噪声速度模型的变化如下

为时刻到时刻的变化量，、、为机器人在时刻的位姿坐标；为控制误差的协方差，表示服从均值为0，方差为

的高斯分布。其中将函数在机器人时刻的状态后验均值处一阶泰勒展开可以得出在时刻机器人预测的联合状态的均值和协方差（2）观测更新阶段观测模型：

为在时刻探测到的第个地标的坐标，为它的签名

为地标相对机器人的距离；为地标相对机器人的角度；为地标签名；为观测误差的协方差如果路标是第一次观测到，可表示为之前被观测到，那么时刻对路标的观测数据可以表示为：

是机器人在时刻的预测位姿；、为路标的坐标，为的地标签名。在预测的状态处一阶泰勒展开，得：相应的卡尔曼增益为：机器人位姿和地图特征的联合状态矢量进行更新：2.FastSLAM算法基本原理粒子滤波器（ParticleFilter）用离散的随机采样点（粒子）来表征状态向量的后验概率密度函数，它对于非线性非高斯系统的估计问题具有普遍适用性。FastSLAM算法使用粒子滤波器估计路径的后验，使用EKF估计地图特征的位置。因为因式分解，FastSLAM算法能为每个特征维持独立的EKF。每个独立的EKF是以机器人路径为条件的，每个粒子拥有它自己的EKF集合，地图里的一个特征对应一个EKF。FastSLAM算法中的粒子可以表示为：

为粒子的索引；为机器人的路径估计；和为第个粒子的第个特征位置的均值和协方差。FastSLAM1.0算法流程图（1）通过采样新位姿扩展路径后验第个粒子对应的位姿为：（2）更新观察到的特征估计特征更新后的值与新位姿一起被增加到临时的粒子集合中对于没有观测到的特征，其后验保持不变：对于观测到的特征，使用贝叶斯准则更新：FastSLAM算法以EKF-SLAM同样的方法线性化感知模型新的均值和协方差可以用EKF测量更新得到：重复步骤（1）、（2）次，产生个粒子的临时集合。（3）重采样临时集合的粒子建议分布服从如下分布：重采样过程导致了目标分布和建议分布的不同，重采样重要性系数由目标分布和建议分布的商给出：进一步变换。在变换时，忽略与传感器测量值不相关的变量：FastSLAM算法采用与步骤（2）使用的测量更新完全相同的线性近似，重要性系数如下所示：FastSLAM2.0在很大程度上与FastSLAM1.0类似，但在对位姿采样时，建议分布考虑测量。03基于随机有限集的SLAM算法随机有限集最早起源于随机集理论，主要是指有限集统计(finitesetstatistics，FISST)理论。随机集是指取值为集合的随机元，是概率论中随机变量(或随机向量)概念的推广，实际上就是元素及其个数都是随机变量的集合。随机变量处理的是随机点值函数，而随机集处理的是随机集值函数。1.随机有限集随机集理论是点变量(向量)统计学向“集合变量”统计学的一种推广。该理论能够解决复杂环境下信息融合、多目标跟踪的各种问题，是目前信息融合和多目标跟踪研究领域最受关注的方向之一。利用随机有限集理论，可以将多目标问题中的探测、跟踪、属性识别等问题统一起来，并能解决多目标状态的后验估计、多目标信息融合算法的性能评估等棘手问题。2.基于随机有限集的SLAM（1）基于RFS的SLAM模型表示用RFS的形式来表示每一时刻的地图特征状态和观测信息：

表示时刻地图中特征的数量，表示地图中的第个特征或路标把传感器的观测信息以RFS的形式进行表示：

表示机器人已知的位姿，表示传感器对地图特征进行观测产生的RFS，表示虚警的RFS（2）基于RFS的SLAM问题描述在SLAM中都以RFS的形式表示，，，联合估计的后验概率密度可表示为在贝叶斯下的递推过程如下所示：●根据之前的机器人的位姿和控制输入量进行预测：●根据当前观测的测量集合进行更新：

是一组集合（2.1）（2.2）3.PHD-SLAM算法基本原理处理基于集合估计的一个近似方法是利用概率假设密度（probabilityhypothesisdensity,PHD）。PHD滤波器跟踪整体特征的建图，并在进行新测量时尝试检测和跟踪单个特征。利用标准条件概率，可将式(2.1)和式(2.2)分解：强度函数PHD的一步预测可表示为：PHD的更新可表示为：

，是在机器人位姿对特征检测到的概率，是对特征检测的似然函数，是PHD随机有限集的杂波强度。对进行集合积分：在时刻，可用一组粒子集合来表示其密度，经机器人位姿、特征PHD预测，特征PHD更新，机器人位姿更新等环节，实现PHD-SLAMGM-PHDSLAM方法步骤：（1）每个粒子的GM-PHD特征地图1）预测通过时刻的观测数据，使用逆空间测量模型计算新生高斯均值和方差，并平均分配权重为

，，分别表示新生高斯的权重，均值和协方差，为观测的方差每个粒子的新生GM-PHD：

时刻的地图PHD是高斯混合的形式：最终得：2）修正将机器人先前的位姿估计转化为新位姿的预测样本：在新的观测中，权重会因漏检率而减少，于是有：在PHD校正中引入计算因子：更新GM-PHD被检测部分的权重：将漏检和加权预测的高斯分量合并成高斯量，得到更新后的高斯PHD：选择高斯权重较高的分量，删除剩余的高斯分量。最后，可以得到基于高斯加权的GM-PHD地图：（2）机器人轨迹更新根据机器人的控制输入，预测时刻机器人的位姿：根据观测信息计算新的权重，并归一化：根据规划权重对所有个粒子重采样：最后得出粒子的后验集和对应的地图PHD：PHD-SLAM算法流程图基于ROS系统的SLAM技术第三章目录CONTENTS01ROS系统02基于ROS系统的机器人实践03基于ROS系统的SLAM开源方案01ROS系统ROS从某种意义上说是一个操作系统（OS），它能够提供类似操作系统所能提供的功能，包括硬件抽象描述、底层启动管理、公共功能的执行、程序间消息的传递以及程序发行包管理，它也能提供工具程序和库，用于获取、建立、编写和运行多机整合的程序。ROS的首要设计目标是在机器人研发领域提高代码的复用率。ROS系统的应用1.ROS的起源2.ROS的五个特点（1）点对点设计节点单元分布式网络RPC+TCP/UDP通信系统（2）多语言支持支持Python、C++、Java等编程语言语言无关的接口定义（3）精简与集成每个功能节点可以单独编译集成众多开源项目接口统一、提高软件复用性（4）工具包丰富3D可视化工具—rviz物理仿真环境—gazebo数据记录工具—rosbagQT工具箱—rqt_*rvizrqt_plot（5）免费且开源BSD许可，可修改、可复用、可商用软件包数量指数级增加，良好的生态系统3.ROS文件系统级文件系统级是用于描述可以在硬盘上查找到的代码及可执行文件程序，在这一级中将使用一组概念来解释ROS的内部组成、文件架构以及工作所需的核心文件。ROS文件系统主要概念（1）功能包（package）：功能包是ROS软件组织的基本形式，是构成ROS的基本单元。一个功能包可以包含多个可执行文件（节点）。（2）功能包清单（packagemanifest）：记录功能包的基本信息。（3）综合功能包（Metapackage）：也称元功能包，将几个具有某些功能的功能包组织在一起。（4）综合功能包清单（Metapackagemanifest）：会包含运行时所需要依赖的功能包或者声明一些引用的标签。（5）消息类型（.msg）：消息是ROS节点之间发布/订阅的通信消息。（6）服务类型（.srv）：服务类型定义了ROS服务器/客户端通信模型下的请求与应答数据类型。文件系统级4.ROS计算图级计算图级体现的是进程与系统之间的通信，描述程序是如何运行的。ROS会创建一个连接所有进程的网络，子系统中的任何节点都可以访问此网络，并通过该网络与其他节点交互，获取其他节点发布的消息，并将自身的数据发布到网络上。计算图级（1）节点（Node）:执行运算任务的进程。在ROS系统中，节点是最小的进程单元。一个软件包里面可以有多个可执行文件，可执行文件在运行之后就成了一个进程（这个进程便是一个节点）。（2）节点管理器(NodeMaster):master在整个网络通信架构里相当于管理中心，管理着各个node。node首先在master处进行注册，之后master会将该node纳入整个ROS程序中。（3）消息（Message）:节点之间最重要的通信机制就是基于发布/订阅模型的消息通信。消息包含一个节点发送到其他节点的数据信息，节点是通过消息实现彼此的逻辑联系与数据交换。（4）主题（Topic）:单向异步通信机制，传输消息(Message)。在这种机制中，Message以一种发布/订阅的方式进行传递(Publish-Subscribe).每个Topic的消息类型都是强类型，发布到其上的Message都必须与Topic的ROS消息类型匹配，而且节点只能接收类型匹配的消息。单向异步通信机制（5）服务（Service）:双向同步通信机制，ROS中称其为“服务”，传输请求/应答数据，是一个request-reply模型.与主题不同的是，ROS中只允许有一个节点提供指定命名的服务。这种机制不仅可以发送消息，还存在反馈。双向同步通信机制（6）参数服务器（ParameterServer）:参数服务器能够是数据通过关键词存储在一个系统的核心位置（节点管理器）。通过使用参数就能够在运行时配置节点或改变节点的工作任务。（7）消息记录包（Bag）:用于记录和回放ROS消息数据的文件格式，保存在.bag文件中。Bag是一种用于存储数据的重要机制，它可以获取并且记录各种难以收集的传感器数据，程序可以通过Bag反复获取实验数据，常用于调试算法。rosparam支持参数6.ROS开源社区级（1）发行版（Distribution）：ROS发行版包括一系列带有版本号、可以直接安装的功能包。（2）软件源（Repository）：ROS依赖于共享网络上的开源代码，不同的组织机构可以开发或者共享自己的机器人软件。（3）ROS维基（ROSwiki）：记录ROS信息文档的主要论坛。（4）邮件列表（Mailinglist）：交流ROS更新的主要渠道，同时也可以交流ROS开发的各种疑问。（5）ROS问答（ROSAnswer）：咨询ROS相关问题的网站。（6）博客（Blog）：发布ROS社区中的新闻、图片、视频等，网址是/news。ROS开源社区ROS系统实现三个层次02基于ROS系统的机器人实践

TurtleBot第一代发布于2010年，两年后发布了第二代产品。前两代TurtleBot都使用iRobot的机器人作为底盘，在底盘上可以装载激光雷达、Kinect等传感器，使用PC搭载基于ROS的控制系统。

TurtleBot3彻底颠覆了原有TurtleBot的外形设计，成本进一步降低，而且模块化更强，可以根据开发者的需求自由改装。TurtleBot2主要结构示意图在本书中，主要介绍的版本是TurtleBot2，TurtleBot2主要构成如图所示，其主要包含三个部分，Kobuki底盘、主机以及传感器Kinect。Kobuki底盘：Kobuki是Yujin公司开发的移动机器人底盘，机器人接口控制板有32个内置传感器，两个驱动轮，2个从动轮共四个可移动小轮，110度/秒单轴陀螺仪，一对编码器，一个可以扩大的输入输出端口和一个后挡板。开放式接口可以直接实现对机器人的移动、声音、显示以及输入传感器的操作。Kobuki底盘

最初TurtleBot2所带的传感器是微软在2009年发布的kinectV1深度传感器在2014年微软公司发布了Kinect的二代产品。现在的TurtleBot2所配备的传感器Kinect2.0。

kinect1Kinect2Kinect1，彩色摄像头为640*480，深度摄像头为320*240，可以同时检测到2名用户的姿势可以检测到人体20个关节点检测范围为0.8m-4.0m角度（水平57°垂直43°）Kinect2，彩色摄像头为1920*1080，深度摄像头为512*424，可以同时检测到6名用户的姿势可以检测到人体25个关节点检测范围为0.5m-4.5m角度（水平70°垂直60°）03基于ROS系统的SLAM开源方案基于ROS的开源SLAM方案就非常多，总共可以分为两个大类，分别是基于激光雷达的开源方案以及基于视觉的开源方案。1.基于激光雷达的SLAM算法（1）HectorSLAMHectorSLAM是一种结合了鲁棒性较好的扫描匹配（scanmatching）算法的2D激光SLAM方法，同时在该方案中使用了惯性传感系统的导航技术。要求：高更新频率、低测量噪声的激光扫描仪，不需要里程计特点：内存中有不同的地图，同时使用之前估计的姿势来更新它们，计算成本很低，且地图总是保持一致。HectorSLAM多分辨率地图（2）Gmapping这是一种基于Rao-Blackwellized的粒子滤波SLAM方法。基于粒子滤波的算法用许多加权粒子表示路径的后验概率，每个粒子都给出一个重要性因子。特点：●通常需要大量的粒子才能获得比较好的的结果，从而增加该算法的计算复杂性。

●与PF重采样过程相关的粒子退化耗尽问题降低了算法的准确性。30粒子80粒子Gmapping基于不同粒子数的地图（3）CartographerCartographer是谷歌推出的一套基于图优化的SLAM算法，该算法以栅格的形式来建立地图，并且将局部匹配直接表示成一个非线性优化问题，后端用图来优化。特点：

●目标是降低计算资源消耗、实时优化并且不追求高精度。●整个算法的核心代码依赖很少Cartographer地图2.基于视觉的SLAM算法MonoSLAM以扩展卡尔曼滤波为后端，追踪前端非常稀疏的特征点。

●第一个实时的视觉SLAM系统

●应用场景窄，路标数量有限，稀疏特征点非常容易丢失PTMA实现了跟踪与建图过程的并行化，跟踪部分需要实时响应图像数据，而对地图的优化则没必要实时的计算。也就意味着后端优化可以在后端进行，然后在必要的时候进行线程同步即可。 ●首次将前后端的概念区分出来；引入了关键帧机制 ●应用场景小，跟踪容易丢失ORB-SLAM代表着特征点SLAM的一个高峰，支持单目、双目、RGB-D三种模式，整个系统围绕ORB特征进行计算，包括视觉里程计与回环检测的ORB字典。●ORB回环检测可以有效的降低算法运行过程中的累积误差，并且在丢失后可以迅速找回●创新式的使用了三个线程完成SLAM

基于特征估计的激光SLAM技术第四章目录CONTENTS01rviz和Gazebo工具02激光雷达传感器03基于激光的Gmapping算法04基于激光的HectorSLAM算法05基于激光的Cartographer算法01rviz和Gazebo工具

机器人系统中存在大量数据，这些数据在计算过程中往往都处于数据形态，比如图像数据中0~255的RGB值。但是这种数据形态的值往往不利于开发者去感受数据所描述的内容，所以常常需要将数据可视化显示，例如机器人模型的可视化、图像数据的可视化、地图数据的可视化等。rviz是ROS可视化（ROSvisualization）的缩写，是ROS中强有力的3D可视化工具。它使得用户能够查看模拟机器人模型、来自机器人传感器的传感器日志信息，并且重放已记录的传感器信息。用户能够对机器人应用程序进行调试，调试内容涵盖从传感器输入到计划动作(或计划外动作)的整个过程。rvizrviz主界面划分为以下几个主要的显示区域:1.中心窗口2.

Displays控制面板3.工具栏4.Views控制面板5.Time控制面板6.主窗口菜单栏。常见的display类型类型描述消息类型Axes显示坐标系－Gamera从相机视角显示图像sensor_msgs/Imagesensor_msgs/CameraInfoGrid显示网格－Image显示图像sensor_msgs/ImageLaserScan显示激光雷达数据sensor_msgs/LaserScanImage显示图像sensor_msgs/ImagePointClode2显示点云数据sensor_msgs/PointCloud2Odomerty显示里程计数据nav_msgs/OdometryPointClode2显示点云数据sensor_msgs/PointCloud2RobotModel显示机器人模型－PointClode2显示TF树－rviz的视角1.Orbital轨道摄像机视角，围绕一个焦点旋转，Ｌ:单击并拖动围绕焦点旋转Ｍ:在相机向上和向右向量形成平面，按下中键并拖动焦点R:单击拖动鼠标放大/缩小焦点向上拖动放大，向下拖动缩小滚轮:放大缩小焦点2.FPS第一视角FPVＬ:单击选中对象，并拖动旋转Ｍ:移动沿着相机向上、向右向量形成的平面移动R:右击并拖动，前后移动，上拖向前移动，下拖向后移动滚轮:前后移动3.top-downOrthographic自上而下的正视图Ｌ:单击并拖动绕Ｚ轴旋转Ｍ:在ＸＹ平面移动R:右击并拖动，缩放图像滚轮:缩放图像4.ＸＹOrthographic限制在XY平面GazeboGazebo是一款3D动态模拟器，能够在复杂的室内和室外环境中准确有效地模拟机器人群。与游戏引擎提供高保真度的视觉模拟类似，Gazebo提供高保真度的物理模拟，其提供一整套传感器模型，以及对用户和程序非常友好的交互方式。1.Gazebo的典型用途●测试机器人算法●设计机器人●用现实场景进行回归测试2.Gazebo的一些主要特点●包含多个物理引擎●包含丰富的机器人模型和环境库●包含各种各样的传感器●程序设计方便和具有简单的图形界面3.系统要求Gazebo官方建议，Gazebo目前最好在Ubuntu或者其他的Linux发行版上运行。同时您的计算机需要具有以下功能：●专用GPU：Nvidia卡往往在Ubuntu中运行良好●至少是IntelI5或同等产品的CPU●至少500MB的可用磁盘空间●安装尽可能高版本的UbuntuTrusty02激光雷达传感器激光雷达探测原理激光雷达（Lidar）类似于雷达（Radar），但是分辨率更高，因为激光的波长大约比无线电的波长小10万倍。可以用来区分真实移动中的行人和人物海报、在三维立体空间中建模、检测静态物体、精确测距。激光雷达是通过发射激光束来探测目标位置、速度等特征量的雷达系统，具有测量精度高、方向性好等优点。隼鸟2号LiDAR结合光学相机影像获取龙宫全球三维地形信息iPadPro探测点云示意图激光雷达优点●具有极高的分辨率●抗干扰能力强●获取的信息量丰富●可全天时工作激光雷达缺点●工作时受天气和大气影响大。激光一般在晴朗的天气里衰减较小，传播距离较远。而在大雨、浓烟、浓雾等坏天气里，衰减急剧加大，传播距离大受影响。●激光雷达的波束极窄,在空间搜索目标非常困难，直接影响对非合作目标的截获概率和探测效率,只能在较小的范围内搜索、捕获目标，激光雷达RPLIDARA1高度60毫米直径98.5毫米重量170克测量半径范围0.15-12米扫描测距角度360度测量频率最大可达每秒8000次扫描频率5.5赫兹。激光雷达RPLIDARA2高度41毫米直径190毫米重量76克测量半径范围0.15-18米扫描测距角度360度测量频率最大可达每秒8000次扫描频率15赫兹。激光雷达RPLIDARA1/2是思岚科技（SLAMTEC）有限公司的产品,能够准确测量试场中物体轮廓边沿与设备间的相对距离，可快速获得环境轮廓信息，这些轮廓信息组成所谓的点云并绘制出3D环境地图，可以帮助机器人实现自主构建地图、实时路径规划与自动避开障碍物。03基于激光的Gmapping算法Gmapping是一种基于Rao-Blackwellized粒子滤波器并适用于网格地图的SLAM算法，也是一种有效解决同时定位和建图的算法，它将定位和建图分离，并且每一个粒子都携带一幅地图。

Gmapping算法流程图重要性重采样（SamplingImportanceResampling,SIR）滤波器

Gmapping算法的实现步骤1）第i个粒子在k时刻的预测位姿可以由k-1

时刻的位姿和控制输入通过运动模型得出。2）机器人基于地图

，从预测的位姿处执行扫描匹配算法。扫描匹配执行的搜索区域被限定在附近的一个有限区域内。如果扫描匹配失败，则根据运动模型计算位姿和权重（忽略步骤3和4）。3）通过扫描匹配器在位姿周围选取一组采样点，其中是扫描匹配器通过将当前观测值与已构建的地图进行匹配，找到最有可能的位姿。基于这些点，通过逐点评估采样位姿中的目标分布来计算建议的平均值和协方差矩阵。在此阶段计算加权系数。4）根据改进的建议分布得出粒子i

的新位姿，位姿服从高斯分布的形式。5）更新粒子权重。6）根据机器人位姿和最新观测，更新粒子i的地图。7）得到新粒子样本后，根据阈值执行重采样步骤。04基于激光的HectorSLAM算法HectorSLAM使用一种快速在线学习的占用栅格地图系统，它是结合了激光雷达系统和基于惯性传感的三维姿态估计系统的鲁棒扫描匹配方法。通过使用快速近似的地图梯度和多分辨率栅格，实现了可靠的定位和建图。HectorSLAM功能包使用高斯牛顿的方法，不需要里程计数据，只根据激光信息便可构建地图。

SLAM一般分为前端和后端系统。SLAM前端用于实时在线估计机器人运动，后端用于对位姿图进行优化。前端快速扫描匹配步骤用于位姿估计，而较慢的后端建图步骤在后台或远程计算机上运行。HectorSLAM方法主要关注前端系统，没有提供相应的位姿图优化，主要是由于这种优化在真实环境下建图是不需要的，因为HectorSLAM方法对于机器人执行任务来说已经足够精确。HectorSLAM算法原理HectorSLAM系统需要用到6自由度运动的平台，这不同于其它2D网格SLAM算法所假设的3自由度运动，因此系统必须估计由平台的平移和旋转组成的全6自由度状态。为此，该系统由两个主要组件组成，导航滤波器子系统融合来自惯性测量单元和其他可用传感器的信息，形成一致的3D解决方案，而2DSLAM子系统用于提供地平面内的位姿和航向信息。这两种估计都是单独更新的，并且只是松散耦合的，因此它们随着时间的推移保持同步。Hectorslam算法的实现步骤（1）在2DSLAM中，为了能够表示任意环境，HectorSLAM使用占用栅格地图，占用栅格地图的离散特性限制了地图表达精度，也不允许直接计算插值或导数。给定一个连续的地图坐标，占用值以及梯度可以如图用四个近邻点来近似，沿着x轴和y轴进行线性插值，然后得到:导数可以近似为（2）扫描匹配是将激光扫描相互对齐或与现有地图对齐的过程，HectorSLAM实质上是一个将扫描末端点与已知地图对准优化方法,其基本思路是使用高斯-牛顿法。通过解最小二乘函数，使得激光扫描与地图有最佳的对齐：代表机器人的位姿，代表i号激光束在机器人姿态下扫描点的世界坐标，其坐标变化为：根据优化测量误差来实现对的估计：对做一阶泰勒展开得：对上式求偏导并设置为，可得上述最小化问题的高斯牛顿方程：其中（3）HectorSLAM使用扩展卡尔曼滤波器(EKF)。在扫描时的卡尔曼估计可以用均值和协方差来表示，位姿估计

直接得出，融合结果如下：观测器矩阵C将整个状态空间投影到SLAM系统的三维子空间中，参数用来调整SLAM更新的效果。于是，进一步有：05基于激光的Cartographer算法基于激光的SLAM方法中，扫描--扫描匹配（Scan-to-scanmatching）经常用于计算相对姿态变化。然而，扫描--扫描匹配本身会很快积累误差。扫描--地图匹配（Scan-to-mapmatching）有助于限制这种误差积累，可使用高斯-牛顿方法在线性插值图上寻找局部最优解。当高精度频率激光雷达提供良好的位姿初始估计时，局部优化的扫描--地图匹配是有效和鲁棒的。

像素精度的扫描匹配方法，可以进一步减少了局部误差积累。解决剩余局部误差累积的两种常见方法是粒子滤波和基于图形的SLAM。Cartographer是谷歌推出的一套基于图优化的SLAM算法。Cartographer的设计目的是在计算资源有限的情况下，实时获取相对较高精度的2D地图。考虑到基于模拟策略的粒子滤波方法在较大环境下对内存和计算资源的需求较高，Cartographer采用基于图优化方法。Cartographer是一个实时的室内建图算法，能生成分辨率的栅格地图。在前端将最新的激光雷达扫描数据在相邻的子图上(整个地图的一小块)完成扫描匹配，得到一个在短时间内准确的最佳插入位置(位姿)后，将扫描插入到子图中。扫描匹配中，位姿估计的误差会在整个地图中随时间逐渐累积，在后端中,通过回环检测加约束进行优化消除误差。Cartographer算法框架图Cartographer算法原理Cartographer系统将独立的局部和全局方法综合来实现二维的定位与地图构建。在局部方法中，每个连续的扫描都与世界坐标系上的一小块区域（称为子图）进行匹配，使用非线性优化将扫描与子图对齐，此过程被称为扫描匹配。子图的构建就是将扫描结果不断与子图的坐标系对齐的迭代过程。将扫描原点设为，雷达扫描点记为，。变换表示的是位姿位于扫描帧中的位置转换到子图帧中的位置，于是，对于一个雷达扫描点，转换到子图帧中可以用下式表示：连续帧扫描可以用来构建一个子图，这些子图采用概率网格

:的形式，r是网格地图的分辨率网格点和相关像素

网格点所对应计算的值代表存在障碍物的概率。对于每个网格点定义相应的像素，它是由离网格点最近的所有点组成的。每一个像素是尺寸为的方格，它代表对应范围内的所有点，扫描数据点是实数，概率网格的尺寸是整数。每当扫描要插入到概率网格地图时，都要确定命中（hits）的点和未命中（misses）的点集。命中就是网格里有雷达数据点，未命中就是没雷达数据点。每一个Hit的点（阴影且画的），对应的最近邻格子点加入到命中点集合；每一个Miss的点（阴影），雷达原点到扫描点中间的格子点全部加到未命中点集合，如果这个点已经在命中点集中则排除。与hit和miss相关的扫描和像素每个还未观察到的网格，可以自定义赋值为一个或者值（一般用0.5，即不知道这个网格有没有被占用）。已经观察到的网格，它的概率更新为：每个网格的命中概率更新公式为：一帧扫描插入子图之前，首先要对这帧扫描的位姿进行优化。Cartographer使用的是基于Ceres库的扫描匹配。该优化问题可以描述成一个非线性最小二乘问题：与扫描匹配一样，环路闭合优化也可描述为一个非线性最小二乘问题，每间隔几秒，就用Ceres库来计算出一个解：残差的计算：当扫描匹配将不正确的约束添加到优化问题时，损失函数（例如Huberloss）可以用于减少异常值的影响。使用基于分支定界（Branch-and-bound）扫描匹配方法，得到精确到像素级的最优位姿估计，核心公式如下：分支定界算法的基本步骤分支定界法是求解整数规划问题的最常用算法，这种方法不但可以求解纯整数规划，还可以求解混合整数规划问题。分支定界法采用搜索与迭代的方法，选择不同的分支变量和子问题进行分支。基于视觉的SLAM技术第五章目录CONTENTS01经典视觉SLAM框架02MonoSLAM算法03ORB-SLAM204多机器人视觉SLAM技术01经典视觉SLAM框架视觉SLAM框架前端：VO后端：Optimization回环检测Loop

Closing建图Mapping视觉传感器1.单目相机模型某一相机拍摄时，若外部所有可视景物光线只能通过相机的光心到达相机的成像平面并形成倒立缩小的成像平面点，这种相机就属于针孔模型相机几何关系:2.双目相机模型双目相机一般是由左右两个单目相机组成，两个相机水平放置，当然也还有其他放置方式，比如上下两目，本质上没有什么区别。双目相机双目相机成像原理几何关系:3.深度相机模型深度相机能够主动测量每个像素的深度信息红外结构光来测量像素距离（Kinect1代相机）通过飞行时间法原理来测量像素距离（Kinect2代相机）●在基于红外结构光的原理中，相机主要是根据返回的结构光的图案来计算物体与相机之间的距离。●基于飞行时间法原理的相机主要是通过向目标发射脉冲光，然后根据发送到返回之间光束的飞行时间来确定物体和相机之间的距离的。视觉里程计VisualOdometry相邻图像估计相机运动基本形式：通过两张图像计算运动和结构不可避免地有漂移方法●特征点法

●直接法后端优化▪滤波▪BA图优化（视觉主流方法）1.BA代价函数与图优化（1）1.投影模型第一步，把世界坐标转换到相机坐标系：第二步，将投至归一化平面，得到归一化坐标：第三步，考虑归一化坐标的径向畸变情况，可以用一个多项式函数来描述畸变前后的坐标变化：第四步，根据内参模型，计算像素坐标：（2）BA优化函数投影的整个过程看似有些复杂，可以将整个过程概括为一个观测方程，可以把它抽象成：关于观测的误差：整体的代价函数可以定义为：2.BA求解把自变量定义成所有待优化的变量：给自变量一个增量时，目标函数变为：把空间点的变量也放在一起，把相机位姿变量放到一起：公式可以简化表达：3.位姿图优化相机的位姿以及位姿节点之间相对运动的估计:按照李群的写法来表示:构建误差:将扰动项移至两侧，根据伴随性质，可以得到：稍加整理可以得到：求出误差关于两个位姿的雅可比矩阵：取近似：总体目标函数为：将扰动项移至最后，导出右乘形式的雅可比矩阵：回环检测检测机器人是否回到早先位置识别到达过的场景计算图像间的相似性方法：词袋模型建图用于导航、规划、通讯、可视化、交互等度量地图vs拓扑地图稀疏地图vs稠密地图02MonoSLAM算法MonoSLAM是由AndrewJ.Davison等人在2007年提出的一种在未知场景中恢复单目相机3D移动轨迹的实时算法。该方法的核心是在概率框架内在线创建一个稀疏持久的地图路标。▪采用一种主动的地图构建与测量方法▪使用一般的平滑相机运动模型来捕捉视频流中固有的动力学先验信息▪彻底解决单目特征初始化问题。MonoSLAM系统图MonoSLAM算法1、概率三维地图MonoSLAM是基于概率特征的建图，地图中的信息代表相机状态和所有感兴趣的特征的当前估计，还代表这些估计中的不确定性。地图可以状态向量和协方差矩阵表示：相机的状态向量：2、自然视觉路标●MonoSLAM的目标是在相机可能剧烈运动下能够重复识别一样的视觉地标●假定每一个特征都在一个局部平面上●在初始化阶段，认为局部平面的法向与特征至相机矢量平行3、系统初始化在单目相机SLAM算法中，初始化时通常在相机前面放置一个已知物体作为先验信息●在单目相机SLAM中，没有直接的方法来测量特征深度或里程信息●初始化时，已知特征信息可有助于直接进入预测、测量、更新的模式4、运动模型和预测MonoSLAM采用恒定速度、恒定角速度模型噪声为：状态更新方程为：通过雅可比矩阵计算得到过程噪声协方差：5、主动特征测量与地图更新相机的位置：图像中特征的位置：径向畸变模型：特征在图像上位置预测的不确定性用对称信息协方差矩阵表示：6、特征初始化MonoSLAM中，在新特征首个测量和确认后，在地图中初始化一条3D线并使得该特征在此线上。这是一条半无限线，起点为相机的估计位置，沿着特征点视觉方向并指向无穷远处7、地图管理●对地图中特征数量的管理，需要动态地决定何时应该识别和初始化新特征，何时需要删除某个特征。●使从任何相机位置所可见的可靠特征的数量接近预定值●只有当相机经过的区域中可见的特征数量小于阈值时，才会将特征添加到地图中8、特征方位估计▪当相机移动时，它的外观会随着视角的变化而改变。▪扭曲的量化取决于相机的初始和当前位置，特征中心的三维位置以及其局部表面的方位。▪SLAM系统提供了实时的相机位姿和3D特征位置的估计，为每一个特征点都保留了相机初始位置和局部平面方位的估计。在两个不同位置观测定向平面几何态势，于是，扭曲可由单应性来描述：平面特征表面三维方位估计流程假设外观预测能保证特征的当前图像位置能定位，下一步则是测量预测模板和当前图像之间的扭曲变化。通过假设扭曲变化小，并采用概率逆合成梯度下降图像对齐步骤进行搜索来达到全局最佳拟合。03ORBSLAM2算法ORB-SLAM2是一款基于单目、双目、RGB-D相机的SLAM系统ORB-SLAM2是在单目ORB-SLAM的基础上提出的▪回环检测▪地图重用▪重定位算法流程图ORB-SLAM主线线程1、跟踪线程跟踪线程主要负责对每一帧图像的相机位姿进行定位，同时决策何时插入新的关键帧。（1）特征提取（2）局部地图初始化（3）相机位姿计算（4）局部地图跟踪（5）是否插入关键帧判断插入关键帧的条件:1）局部建图线程闲置或距离上次插入关键帧时间超过了20帧；2）距离