本文将介绍当前移动机器人的主要定位方法类别，并详细介绍了同时定位与建图原理及基于滤波的同时定位与建图方法。
01、定位
移动机器人定位（Localization）即机器人在环境中运行时实现移动机器人相对于世界坐标系的位置及其本身的位姿的判断，是移动机器人完成自主能力的最基本问题。精确的环境模型（地图）及机器人定位有助于高效地路径规划和决策，是保障机器人安全导航的基础。
移动机器人一开始是通过自身携带的内部传感器基于航迹推算的方法进行定位，后来进一步发展到使用各种外部传感器对环境特征进行观测，从而计算出移动机器人相对于整个环境的位姿。到现在为止，形成了基于多传感器信息融合的定位方法。
如今，移动机器人的定位方法可以分为相对定位、绝对定位、组合定位三类。
相对定位的基本原理是在移动机器人位姿初始值给定的前提下，基于内部传感器信息计算出每一时刻位姿相对于上一时刻位姿的距离以及方向角的变化，进而实现位姿的实时估计，此方法又称为航迹推测（Dead Reckoning，DR），能够依据运动学模型自我推测机器人的航迹。相对定位方法常用的内部传感器主要有里程计（Odometry）、陀螺仪（Gyroscopes）、惯性传感器（IMU）。
绝对定位是指确定移动机器人在全局参考框架下的位姿信息。例如，采用导航信标（Landmark Navigation）；主动或被动标识（Active or Passive Beacons）；地图匹配（Map Matching）或全球定位系统（Global Positioning System，GPS）进行定位。由于绝对定位方法不需要时间和初始位姿，所以，不仅没有累积误差问题，还具有精度高、可靠性强等特点。同时，基于超声波、激光、卫星、WiFi、射频标签（RFID）、蓝牙（Bluetooth，BT）、超宽带（Ultra Wide Band，UWB）、计算机视觉等定位方法，也属于绝对定位范畴。位置计算方法主要有三边测量法（Trilateration）、三角测量法（Triangulation）、模型匹配算法（Model Matching）等。
组合定位则是结合相对定位与绝对定位的方法。相对定位方法存在累积误差，而绝对定位方法也存在一些不足，例如，GPS只能用于室外，信标或标识牌的建设和维护成本较高，地图匹配技术处理速度较慢等。由于单一定位方法的不足，因此移动机器人定位主要基于航迹推测与绝对信息矫正相结合的方法。
当然，不同方法对应的定位性能也有差异。以室内定位为例，其性能指标为定位精度和规模化难易程度，常用各定位方法的分布如图61所示。其中，属于相对定位范围的惯性传感器定位精度大部分在亚米级，也是最容易实现规模化的一类。属于绝对定位范围内的WiFi、RFID、超声波、蓝牙、计算机视觉、激光和超宽带的定位方法的定位精度跨越米级、亚米级以及分米级的定位精度，并且规模化实现由易到难。
■ 图6-1室内定位主要技术对比
移动机器人的环境建模依赖于机器人精确的定位，而定位又依赖于精确的环境地图，因此，机器人的定位和建图问题是一个“鸡和蛋”的问题，而同时进行定位和地图提供了解决这个问题的可能性：机器人在构建一个环境地图的同时，利用这个地图进行机器人定位，这样的方法称作同时定位与建图（Simultaneous Localization And Mapping，SLAM）。
SLAM指的是机器人在不确定自身位姿的条件下，在完全未知的环境中创建地图，同时利用地图进行自主定位。SLAM因为其重要的理论和应用价值而引起移动机器人研究人员的极大兴趣，它被认为是实现真正全自主移动机器人的关键。
SLAM问题基本包含四个方面：
（1） 如何进行环境描述，即环境地图的表示方法。
（2） 怎样获得环境信息，涉及机器人的定位与环境特征提取问题。
（3） 怎样表示获得的环境信息，并根据信息更新地图。
（4） 发展稳定、可靠的SLAM方法。
目前，SLAM按照使用的传感器可以分为两类：一类是基于激光雷达的激光SLAM（Lidar SLAM）；另一类是基于视觉的SLAM（Visual SLAM，VSLAM）。
激光SLAM中按照环境复杂程度可采用2D或3D激光雷达（也叫单线或多线激光雷达）。2D激光雷达通常用于室内机器人上（如扫地机器人），而3D激光雷达一般用于无人驾驶领域。激光雷达的出现和应用使得测量的快速性和准确性有了很大提高，信息更丰富。激光雷达采集到的物体信息表现为一系列离散的、具有准确角度和距离信息的点，被称为点云。通常，激光SLAM系统经过对不同时刻两片点云的匹配和比对，计算激光雷达相对运动的距离和姿态的改变，也就完成了对机器人自身的定位。激光雷达测距比较准确，误差模型简单，在强光直射以外的环境中工作稳定，点云的处理也比较简单。并且，点云信息本身含有直接的几何关系，可以让机器人的路径规划和导航变得直观。激光SLAM理论研究也更加成熟，落地产品更丰富。
视觉SLAM一般使用相机作为传感器，能够从环境中获取足够的、富于冗余的纹理信息，拥有超强的场景辨识本领。早期的视觉SLAM基于滤波理论，其非线性的误差模型和庞大的计算量成为它实用落地的阻碍。近年来，随着具有稀疏性的非线性优化理论（Bundle Adjustment）和相机技术、计算性能的进步，实时运行的视觉SLAM已经可以实现。视觉SLAM的优点是它利用了丰富的纹理信息，这带来了重定位、场景分类上无可比拟的巨大优势。同时，视觉信息可以较为容易地用来追踪和预测场景中的动态目标，如行人、车辆等，这对于在复杂动态场景中的应用至关重要。
如表6-1，激光SLAM和视觉SLAM各有优劣，结合二者使用，可以发挥各自的优势以取长补短。例如，视觉SLAM在纹理丰富的动态环境中运行稳定，同时可以为激光SLAM提供非常准确的点云匹配，而激光雷达又可提供精确方向和距离信息，这在正确匹配的点云上会发挥更大的威力。而在光照严重不足或纹理缺失的环境中，激光SLAM的定位工作使得视觉SLAM可以借助不多的信息完成场景记录。
表6-1激光SLAM和视觉SLAM的优缺点
近年来，SLAM导航技术已取得很大的发展，它将赋予机器人和其他智能体前所未有的行动能力，而激光SLAM与视觉SLAM必将在相互竞争和融合中发展，使机器人走出实验室和展厅，达到真正服务于人类的目的。
02、同时定位与建图
移动机器人同时定位与建图问题可以描述为：移动机器人从一个未知的位置出发，在不断运动过程中按照自身位姿估计和传感器对环境的感知构建增量式地图，同时运用该地图更新自己的定位。定位与增量式建图同时进行，而不是独立的两个阶段。
设T时刻移动机器人的位姿表示为xT。对于二维空间移动机器人，xT是三维矢量，包括平面上的二维坐标和一个单向旋转的方向值。对于三维空间移动机器人，xT是六维矢量，包括空间中的三维坐标和三个方向旋转的方向值。移动机器人的位姿序列表示如下：
其中，初始位姿x0通常作为参考点。
设uT表示机器人在T－1时刻和T时刻之间的运动，移动机器人的相对运动序列可以描述如下：
对于无噪声运动， uT将足以从初始位姿x0推导出最终位姿，这就是通常所说的航迹推算。然而，里程计是有噪声的，直接航迹推算的位姿是存在偏差的。
用m表示环境中地标、物体等的位置。环境映射m通常被假设为静态的。
移动机器人通过传感器对环境的观测z可以建立m特征与移动机器人位姿xT之间的信息。移动机器人的观测序列表示为
图6-2展示了SLAM问题的典型模型。SLAM主要解决在已知移动机器人的相对运动序列UT和移动机器人的测量序列ZT的情况下恢复环境模型m和机器人的位姿序列XT的问题。
■ 图6-2SLAM问题模型
SLAM主要有full SLAM和online SLAM两种形式。
（1） full SLAM，可描述为
full SLAM是根据移动机器人直接观察到的数据ZT以及UT计算XT和m上的联合后验概率的问题，得到的是整个地图和移动机器人轨迹的估计。full SLAM算法通常是批处理的，即同时处理所有数据。
（2） online SLAM，可描述为
online SLAM只估计目前移动机器人的位姿，而不是整个轨迹。online SLAM通常是增量式的，一次处理一个数据项。
因为自主移动机器人缺少自身位姿和环境的先验信息，仅能利用外部和内部传感器获得知识，所以环境、传感器信息及机器人运动本身都具有不确定性，因此，各类SLAM算法都是一个“估计-校正”的过程。
SLAM算法通常可分为传感器数据读取、前端里程计、后端优化、建图和回环检测5个模块，如图6-3所示。
■ 图6-3SLAM算法的一般框架
前端里程计： 通过相邻帧传感器数据，计算移动机器人的相对运动。
后端优化：接收前端局部里程计，计算出移动机器人的位姿以及回环检测信息，并对它们进行优化，获得全局一致的轨迹和地图。
回环检测：判断移动机器人是否经过先前的位置，如果检测到回环，则将回环信息提供给后端进行处理。
建图：按照移动机器人的位姿和对应的传感器数据建立地图。
通常，SLAM问题的处理办法主要有基于滤波的SLAM方法和基于图优化的SLAM方法。
1●基于滤波的SLAM方法
online SLAM通常也被称作基于滤波的SLAM方法。而基于滤波的SLAM方法基本都是以贝叶斯滤波为基础发展起来的。
贝叶斯滤波
贝叶斯滤波的目标是在已知
的情况下，得到的表达式，即在t－1时刻状态量的概率分布，以及在t时刻的ut和zt的情况下，估算出状态量在t时刻的后验概率分布，此概率称为状态的置信概率，设为bel(xt)。
首先介绍贝叶斯滤波中涉及的假设。
综上，结合贝叶斯公式可得
其中，根据Morkov性质中t时刻的观测仅与t时刻的状态相关可推出
根据全概率公式可推出
根据Markov性质中t时刻的状态由t－1时刻的状态和t时刻的动作决定可推出
以及
综上，可推导出
可以看出，最终的贝叶斯公式可分为两部分：一部分是
它基于xt-1和ut预测xt的状态，即状态观测；另一部分是ηp(xt|zt)，它基于观测zt更新状态xt，即状态更新。
另外，通常令
表示xt的预测（proposal）概率分布，还可写为
作为经典的状态推断方法，贝叶斯滤波方法是很多实用算法（例如Kalman滤波、扩展Kalman滤波、信息滤波、粒子滤波）的基础。