matlab代做基于SLAM的移动机器人导航算法研究解析

第3章 同时定位与制图（SLAM）算法

3.1 同时定位与制图算法介绍

移动机器人的定位和地图创建是机器人领域的热点研究问题，也是导航中重要环节。 对于已知环境中的机器人自主定位和已知机器人位置的地图创建已经有了一些实用的解决方法。 然而在很多环境中机器人不能利用全局定位系统进行定位,而且事先获取机器人工作环境的地图很困难，甚至是不可能的。这时机器人需要在自身位置不确定的条件下，在完全未知环境中，创建地图同时利用该地图进行自主定位和导航。 这就是移动机器人的同时定位与地图创建(SLAM) 问题，也称为CML (Concurrent Mapping and Localization) 。该研究领域的代表人物有Smith，Self 和Cheeseman [23] 。由于SLAM算法具有重要的理论与应用价值，被很多学者认为是实现真正全自主移动机器人的关键。近几年来，其研究取得了很大的进展，并已应用于各种不同的环境，如： 室内环境[24 ][25] 、水下[26 ][27] 以及室外环境[28 ][29]。

3.1.1 SLAM算法性质

SLAM算法有很多重要的属性，影响着地图特征和机器人位置估计中的不确定性。包括：状态估计的收敛性，估计过程的一致性，状态协方差矩阵更新的计算复杂度[23]。

收敛性：SLAM算法具有三个重要的收敛性，这三个关键的收敛结果是：A.地图协方差矩阵的任意子矩阵地行列式随着每一次观测单调下降；B.在极限情况下，随着观测数量的增加，特征估计变得完全相关。C.地图的精度与第一个特征被观测时机器人的位置精度有关。这些结果表明：随着观测数量的增加，地图估计的不确定性将降低到有限的误差范围内，地图特征的关系将是完全确定的。

一致性：为了维护SLAM算法估计的一致性，对状态协方差矩阵进行更新

- 1 - - -

维护是必要的。既然对环境的观测是相对机器人的，所以机器人估计中的任何误差和地图估计中的误差使绝对相关的。在没有其它外部的关于特征和机器人位置信息情况下，为了使系统状态估计的误差在有限的范围内，保持状态估计之间的一致性是很必要的。所以就必须维护机器人状态与环境特征之间的协方差矩阵。

计算复杂度：SLAM算法应用到大规模环境时的一个重要局限性是计算环境特征之间，特征与机器人之间的相关信息时的计算负担。由于特征数量很大，协方差矩阵的更新维护导致了SLAM算法的计算复杂性。对于那些包含上万个特征的环境，计算负担使得系统协方差的更新变的难以执行。所以需要一个有效的方法来提高算法的计算效率。

3.1.2 SLAM算法分类

根据所依据的理论基础的不同，SLAM可以分为以下几种： （1）基于扩展卡尔曼滤波（EKF）的CML/ SLAM

这是最常用的一种SLAM方法，适合解决非线性系统的估计问题。该方法用平面坐标表示机器人和环境特征的位置，将机器人运动与环境特征的关系描述为两个非线性模型：机器人运动模型和观测模型.通过这两个模型，运用扩展卡尔曼理论的思想来实现。主要包括预测与更新两个阶段[26][28]。 （2）基于概率的CML/ SLAM

尽管不如EKF 那样流行，但由于用概率表达机器人定位问题的不确定性非常自然合理，基于概率的CML 也吸引了很多人的目光。其中，较流行是最大相似性(Maximum Likelihood Estimation ，MLE)方法。一种非常有效的最大相似性估计算法称为Baum-Welch (或α-β) 算法[30]，基于这种方法的机器人制图与定位问题可看作是机器人位置与环境特征位置的最大相似性估计问题。该算法包括两步：E-Step (expectation) 和M-Step (maximization)。 （3）基于粒子滤波器( particle filter) 的CML/SLAM

粒子滤波器定位也称为Monte Carlo 定位[31][32]，其基本思想是用一组滤波器来估计机器人的可能位置(处于该位置的概率)，每个滤波器对应一个位置，利用观测对每个滤波器进行加权传播，从而使最有可能的位置的概率越来越高。 （4）基于空间扩展信息滤波器的SLAM[33]

- 2 - - -

该方法由Sebastian Thrun 等人提出，是对EKF 算法的改进。它不再用协方差矩阵表示空间信息的相关性，取而代之用空间信息矩阵来表示空间信息间的内在固有的关系，并且使用网状数据结构仅维护邻近的环境特征(地图)。 （5）基于集合理论估计的SLAM[34]

基于集合理论的方法不假设噪声服从某种分布，而只假设噪声是有界的。 具体方法是：定义一个可行状态集合(feasible state set) 和一个观测集合(measurement set) ，前者表示机器人和环境特征的状态估计，后者表示符合条件(观测误差小于边界)的状态集合。算法首先根据机器人本身的状态方程计算可行状态集合，然后根据观测值计算观测集合，最后取两个集合的交集作为某时刻的经估计校正后的机器人与环境特征的状态集合(地图)。

此外根据所制地图的不同，SLAM又可以分为：基于栅格的SLAM[35]；基于特征的SLAM[28][29][36]以及基于拓扑结构的SLAM[37]。

在基于栅格的SLAM算法中，环境表示为一组具有特定分辨度的栅格的组合。每个栅格中有特定概率值来表示其被占据的可能性。使用基于栅格SLAM算法的时候，假设各个栅格的状态变量是相互独立的，而没有考虑栅格之间的关联性。另外，当机器人采用该种SLAM算法运行较长一段路程后，很难再回到出发点。这是因为该方法中仅在局部坐标中考虑了估计的不确定性，却没考虑局部坐标与全局坐标中的相互联系。总的说来，基于栅格的SLAM方法可以提供较为丰富的环境描述，对于局部环境下的规划与导航有很大帮助，但是通过该方法无法获得一致性较强的全局地图。

基于特征地图的SLAM算法是将环境表示为一组组参数化的特征值，比如说点，线，角等。这里的特征指的是环境中那些突显于背景的、易于传感器分辨检测的且可以通过参数描述的东西。使用这种方法的时候，必须对环境中不同类型的特征分别建立测量模型以便准确提取。基于特征的SLAM算法是最为流行的一种，尽管在环境地图描述中仅仅用到特征的位置值，但是还可以通过大小，颜色等信息进行匹配等。这种SLAM算法常常是基于卡尔曼滤波的。使用这种方法时，状态向量不仅包括机器人的位姿信息而且还有特征的位置信息。随着新特征的不断提取与确认，状态向量将不断的变大。因为用于描述环境的特征值的测量都是相对机器人的，所以对环境特征测量的不确定性是与机器人位姿估计的不确定性息息相关的。可以在理论上证明，随着时间的不断推移、测量的不断进行，地图中的特征将是完全相关的，也就是说此时随意给定一个

- 3 - - -

特征的绝对坐标值，将会得到一个精准的地图。在环境特征容易识别的场合下，该算法运用很好，但是对于特性不是太明显的非结构化环境中，算法的运用遇到难题。

在基于拓扑结构的SLAM算法中使用“图”来描述环境。具体说是通过节点和弧线来描述环境。每个节点表示环境中突显的地方，边用来表示相邻节点间的相对位置。对于纯粹的拓扑地图来说，无须知道每个节点的具体位置，它只用来表示节点间的连通性。但是该方法也有很大的弊端，比如当环境稍微复杂一些时，其对环境的识别能力会明显下降且无法识别相近的环境。

尽管SLAM算法的理论基础已经被很好的研究，但是要将其更好的运用于实际，特别是大型的非结构化环境，仍有大量理论和实际的问题需要解决包括：计算效率、地图管理、局部地图与全局地图之间的协调融合、数据匹配以及传感器管理等[38][39]。

3.2 基于特征SLAM算法构架简介

上文中讲到根据所得到地图类型的不同，可以将SLAM算法化分为基于栅格的SLAM；基于特征的SLAM以及基于拓扑结构的SLAM三大类。其中，基于特征的SLAM是运用最广的一类，下文将对该类SLAM算法进行详细阐述。基于特征的SLAM算法架构由图3-1表示。

由图3-1可以看出，整个算法实际上是由三部分构成即：预测——实践——更新。首先要建立机器人的动力学模型以及运动过程中的观测模型。某一时刻k，在给定控制参数v,?后，根据动力学模型可以预测机器人在k?1时刻的位姿状态；同时结合观测模型还可以预测在k?1时刻机器人对旧（已经核实的）特征Fi的观测值di(k?1|k)；在实践环节中，即在时刻k到时刻k?1的过程中，机器人基本上要完成三大工作：环境特征的提取和合理性检验，以及相关数据的匹配。从而获得对Fi特征的实际测量di(k?1|k?1)，同时筛选出新的特征Fi?1。将对特征Fi的估计测量和实际测量的差异值（残差）?d、机器人位姿估计以及旧的地图一起作为输入参数，通过一定滤波算法即可更新机器人位姿，进而得到新特征Fi?1的位置信息并将其融入到旧地图中生成新地图。总而言之，该方法要求机器人的位置和所有地图特征的位置存放在同一个状态向量中，并且这个向量是增广的。当观测到环境中新的特征时，就要把它作为新的地图特征添加

- 4 - - -

到状态向量中。然后通过滤波就可以构建一个收敛的地图。在极限情况下，随着对环境特征的重复观测，地图特征之间的相对关系变得完全确定，并且地图的不确定性控制在一个小的范围内。这个误差范围与机器人初始位置的不确定性有关。

图3-1 基于特征SLAM算法架构

3.3传感器的选择及特征提取

3.3.1传感器选择

由图3-1可以看出，移动机器人的自主导航中必须以有效而可靠的环境感知为基础，运用一种或多种传感器的组合，配以信息融合技术，从而对周围环境进行认识。机器人自主导航中常用的传感器主要可以分为两大类：

其中之一是内部传感器，它是机器人本身运动的感知，用来检测机器人组成部件内部状态，是实现闭环控制、伺服控制动作的不可缺少的装置。这类传感器有：惯导（INS），陀螺仪，里程计等。惯导提供加速度信息，速度计测量速度，陀螺仪测量角速度，里程计测量距离。这种传感器通常与机器人本身的

- 5 - - -

运动模型一起对机器人的位置进行预测估计。由于这类传感器存在着随时间积累的漂移误差，所以不能直接用于长期定位。内部传感器记录了机器人的运动信息，能提供很高的采样频率而不依赖环境特征。

另一类传感器称作外部传感器。机器人外部传感器是用来检测机器人与对象物体外部状态的传感器。它使机器人能及时了解工作环境和对象，并视其情况来调整自己的对策，以提高机器人的适应性和智能化水平。如全球定位系统GPS）。通过对已知位置的GPS卫星的观测来计算机器人当前的位置信息。其它常用的外部传感器有：激光雷达，毫米波雷达，声纳，视觉相机CCD等。

激光测距雷达（Laser rangefinder），运用激光二极管光脉冲获取目标的距离信息。距离的测量一般基于TOF（time of flight）技术或者相移（phase-shift）技术。利用Doppler效应，也可以测量目标运动的速度信息。在基于TOF的测距中，发射、接收短的激光脉冲并测量它返回时的时间，通过特定计算就可以得到距离；在基于相移的测距中，发射、接收连续的光波并通过比较返回信号与参考信号的相位来计算距离[40][41]。采用激光测距雷达的好处在于：速度快、测距精度高、相对于声纳传感器而言，角分辨率高、而且激光波束窄、测量距离长；但是其价格高、回波响应与目标的表面材料属性有关且有镜面反射和漫反射现象发生。

毫米波雷达（millimeter wave radar，MMWR），通过发射接收电磁回波来测量至目标对象的距离。雷达天线的尺寸依赖于所使用的频率。频率越高，天线越小。运用的电磁波通常是短脉冲或调制的连续波。毫米波雷达的优点在于：可以适用于任何的天气条件，而且测量距离长、精度也比较高；其缺点是：成本高、尺寸大，在同样的天气条件下，比激光雷达衰减的更快。

声纳传感器（Sonar）具有成本低、波束覆盖范围宽的特点，但角度分辨率低,不够精确，且容易产生虚假和多重反射回波信号，增加特征匹配的难度。尽管如此，很多导航设计中都用到了声纳系统[42][43]。超声波测距是基于超声波在介质中的传播速度特性实现的非接触式距离测量，其原理是通过超声换能器发射超声波，其在空中传播至被测物体，经反射后由超声换能器接收反射回波信号，确定出超声脉冲从发射到接收的渡越时间T，在已知超声波声速V的前提下，则可以计算出被测物体离超声换能器的距离L=VT/2，完成障碍物及其位置的确定。

立体视觉系统（Stereo-vision system）运用单个或多个相机得到深度信息。

- 6 - - -

得到的距离图像的质量依赖于相机标定、光照条件以及基线情况。目前，视觉已经在机器人导航中广泛运用[44][45]。视觉可以提供更为丰富的信息，但是数据处理工作也相对庞大，而且视觉图像受光照条件影响比较大。

所以在选择传感器时要权衡各类传感器的弊益。一般来说，使用单一传感器进行定位制图的可靠性较差。利用多传感信息融合技术是定位与制图技术的发展方向。在本研究中主要涉及声纳传感器。声纳传感器的弱势将通过软硬件进行补偿。

3.3.2特征提取

特征提取是SLAM算法得以实施的前提条件。对于不同的传感器、不同的特征类型，都会有不同的特征提取算法。合理的特征提取算法可以提高SLAM算法的质量。由于本文侧重于SLAM算法中如何运用特征，这一问题的理论研究，故没有过多考虑特征本身的提取算法。对于从声纳传感器返回值中提取点、直线特征的算法，可以参考文献[46]。

3.4噪音模型建立

上文提到，在机器人的自主导航研究中要利用各种传感器对机器人本身运动或者外界环境特征进行测量。这些测量通常会受到各种噪音的干扰，是固有不确定性的。常用一个噪音模型来表示这重不确定性。其中，最常用的噪音模型是白色高斯噪音（WGN）。

另外在进行各种建模时，为了得到完全精确的模型需要很多参数，并且是非线性的。为了表示的方便通常只是用一个近似模型。在这个近似的过程中也会引入误差，该误差也是用高斯函数来表示的。

3.5滤波技术简介

由图3-1可以看出，滤波技术在基于特征SLAM算法中有着相当重要的地位，是算法的根基所在。所谓滤波就是从混合在一起的诸多信号中提取出所需要的信号。信号是传递和运载信息的时间和空间函数。有一类信号的变化规律

- 7 - - -

是既定的，如调幅广播中的载波信号、阶跃信号、脉宽固定的矩形脉冲信号等，它们都具有确定的频谱，这类信号称之为确定性信号。对于这类信号可根据各信号频带的不同，设置具有相应频率特性的滤波器如：低通、高通、带通、带阻滤波器，使有用的信号无衰减的通过，使干扰信号受到抑制。这类滤波器可以使用物理方法实现即：模拟滤波器，也可以用计算机通过算法实现即：数字滤波器。对确定性信号的滤波处理也称常规滤波。

另一类信号没有既定的变化规律，在相同的初始条件和环境条件下，信号的每次实现都是不一样的，如陀螺漂移、海浪、惯性系统的导航输出误差等，它们没有确定的频谱，这类信号称之为随机信号。随机信号没有确定的频谱，无法用常规滤波提取或抑制信号，但随机信号具有确定的功率谱，可以根据有用信号和干扰信号的功率谱来设计滤波器。维纳滤波是解决此类问题的方法之一。但是设计维纳滤波器时需要做功率谱分解，只有当被处理的信号为平稳的，干扰信号和有用信号均为一维，且功率谱为有理分式时，维纳滤波器的传递函数才可以用伯特-香农设计法较容易的求解出。否则设计维纳滤波器时会遇到诸多困难。维纳滤波除设计思想与常规滤波不同之外，对信号做抑制和选通这一点是相似的。此外常用的还有卡尔曼滤波。它是从与被提取信号有关的“量测量”中通过算法估计出所需的信号。其中，被估计的信号是由白噪声激励引起的随机响应，激励源与响应之间的传递结构（系统方程）已知，量测量与被估计量之间的函数关系（量测方程）也已知。下面将着重介绍卡尔曼滤波。

3.6卡尔曼滤波简介

1960年，R.E.Kalman 在其发表的一篇著名的论文[47]中，描述了用递归的方法来解决离散数据的线性滤波问题，这便是卡尔曼滤波的雏形。简单的讲，卡尔曼滤波可以看作是一组数学方程式，通过这些方程它提供了一种有效的解决过程中的状态估计问题。该方法不仅可以对过去或现在的状态进行估计，而且可以预测未来状态，所以它在机器人自主或辅助导航研究中占有很重要的位置

[48][49]

。随后卡尔曼滤波不断发展，特别是在解决非线性滤波问题的扩展卡尔曼滤

波提出之后，该技术得到了更为广泛的应用与发展。

- 8 - - -

3.6.1线性卡尔曼滤波简介

卡尔曼滤波提供了解决离散时间控制下的状态估计问题。假设该过程可以由以下线性方程表示：

xk?Axk?1?Buk?1?wk?1， x??n （3-1）并且已知该过程中伴随着测量方程：

z??m （3-2） zk?Hxk?vk ，

其中，随即变量wk与vk分别代表过程与测量误差。假设两者相互独立，且均为白色高斯噪音，具有如下的概率分布：

p(w)~N(0,Q)， （3-3） p(v)~N(0,R), （3-4）

在实际中，过程噪音协方差矩阵Q与测量噪音协方差矩阵R都会随着时间和测量的进行而不断变化。但是在这里我们假设它们是恒定的。

由方程（3-1）可以看到，k?1时刻的状态xk?1通过n?n矩阵A与k时刻状态xk联系起来，实际中该矩阵可能是变化的，但是在这里我们假设它不变。n?l矩阵B将控制量u??l与状态量x联系起来。在测量方程（3-2）中，m?n矩阵

H将测量量zk与状态量x联系起来。在实际操作中，随着测量过程的进行，H也

是可能发生变化的，但是这里我们依旧将其视为不变。

??k我们定义x，它表示根??n为对时刻k的一个先验状态估计，称为“先验”

?k??n称为“后验”据时刻k前获得的信息对时刻k的状态进行的估计。相反，x，表示根据k时刻得到的测量zk来估计k时刻的状态。先验和后验的估计误差分别定义如下：

???k ?k，以及 ek?xk?xek?xk?x这样，就可以分别得到两者误差估计协方差分别为：

??T Pk??Eekek， （3-5）

Pk?? ?E?ee?， （3-6）

Tkk为了得到卡尔曼滤波算法的方程，首先要构造如下方程，从而将上述各量合理联系起来：

???k?x?k?k （3-7） x?Kzk?Hx???k?k其中，?zk?Hx与实际测量zk之? 称为测量残差。它表示预测测量值Hx间的差异。K为一个n?m矩阵，为卡尔曼“增益”，用来减小后验误差协方差，

?? - 9 - - -

通常由以下式子得到：

Kk?PH?kT?HP?kH?RT??1?Pk?HT HPk?HT?R （3-8）

Kk?0。由上式可以得到limKk?H?1以及lim也就是说，测量误差协方差R?Rk?0Pk?0??k越小，实际的测量zk就越准确，而预测的测量值Hx也就越不准；然而，先验??k估计误差协方差Pk?越小，实际的测量反而越不准确，预测的测量值Hx却相对

更准确。

使用卡尔曼滤波进行的过程估计是通过反馈控制来完成的。在某时刻先对过程状态进行估计，然后通过测量反馈进行更正。因此，可以将算法中的方程分为两大类即：时间更新方程和测量更新方程。时间更新方程是用来通过当前状态值以及其误差协方差获得下一时刻状态的先验估计值；而测量更新方程则是用于反馈，通过反馈回来的测量值以及先验估计来获得更为准确的后验估计。实际上，时间更新方程可以看成是“预测者”，而测量更新方程则可以看成是“矫正者”。

时 间 更 新 （ 预测者） 测 量 更 新 （ 矫正者） 图 3-2 预测——矫正算法

在实际使用滤波算法前，通常要通过测量采样分析获得测量噪音协方差R。然而过程噪音协方差Q相对来说不容易直接测得，要通过考虑多方面的不定因素进行拟订。不管在选择上述两参数时是否有足够合理的理论依据，都要根据具体的模型与目的对其进行调整。

卡尔曼滤波的整体结构如下图所示：

- 10 - - -

时间更新方程 先验估计 ??k?k?1?Buk?1 x?Ax 测量更新方程 卡尔曼增益 Kk?Pk?HTHPk?HT?R后验估计 ???k?x?k?k x?Kkzk?Hx???1先验估计误差协方差 P?APk?1A?Q ?kT??后验估计误差协方差 Pk??I?KkH?Pk?

图 3-3 线性卡尔曼滤波算法结构

3.6.2 扩展卡尔曼滤波（EKF）简介

通过上述对卡尔曼滤波的简单介绍可以看出它主要处理的是线性问题，而通常情况下，我们更多遇到的是非线性问题。这样就应该了解一下专门用于处理这类问题的扩展卡尔曼滤波算法。

仿效泰勒级数展开，在当前估计值处对过程或测量方程求偏导，从而将非线性问题线性化。同卡尔曼滤波一样，假设过程状态向量x??n，不同的是该过程是由以下非线性方程表示：

xk?f?xk?1,uk?1,wk?1?， （3-9） 同样，观测方程也是非线性的：

zk?h?xk,vk?， z??m （3-10） 其中，随机变量wk与vk分别代表的是过程与测量噪音。实际过程中，我们可能无法获知任意时刻这两中噪音的具体值，但是我们可以在忽略它们的情况下给出状态与测量向量的近似表达式如下：

?,u,0? （3-11） ~ x?f?xkk?1k?1以及 ~ zk?h?~xk,0?， （3-12）根据上述两式将状态向量和测量向量线性化

- 11 - - -

?k?1??Wwk?1xk?~xk?A?xk?1?x， （3-13） ~zk?~zk?H?xk?xk??Vvk， （3-14） 其中，xk与zk分别为实际的状态向量和测量向量，

~ x与~z分别为根据式（3-11）和（3-12）得到的状态和测量向量的估计，

kk A可以通过f对x求偏导而获得，A?i,j???f?i??x?j??k?1,uk?1,0?， ?x W可以通过f对w求偏导而获得，W?i,j?? H可以通过h对x求偏导而获得，H?i,j? V可以通过h对v求偏导而获得，V?i,j???f?i??w?j??h?i?~?xk,0?， ??x?j??h?i?~?xk,0?， ?v?j??k?1,uk?1,0?， ?x同样，可以将算法中的方程分为两大类即：时间更新方程和测量更新方程。时间更新方程是用来通过当前状态值以及其误差协方差获得下一时刻状态的先验估计值；而测量更新方程则是用于反馈，通过反馈回来的测量值以及先验估计来获得更为准确的后验估计。如图3-4示。

上文中对基于特征SLAM算法构架以及算法根基扩展卡尔曼滤波（EKF）进行了介绍，下一章中将以线特征为例对算法进行详细分析。

3.7 本章小结

同时定位与制图（SLAM）算法是本研究的重点内容。本章首先对SLAM算法的定义、性质、发展进行了介绍；剖析了基于特征SLAM算法的构架；并对基于特征SLAM算法的算法根基卡尔曼滤波进行了详细说明。

- 12 - - -

图3-4 扩展卡尔曼滤波算法方程与结构

- 13 - -

-

本文来源：https://www.bwwdw.com/article/385p.html