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

第3章同时定位与制图（SLAM）算法3.1同时定位与制图算法介绍移动机器人的定位和地图创建是机器人领域的热点研究问题，也是导航中重要环节。对于已知环境中的机器人自主定位和已知机器人位置的地图创建已经有了一些实用的解决方法。然而在很多环境中机器人不能利用全局定位系统进行定位,而且事先获取机器人工作环境的地图很困难，甚至是不可能的。这时机器人需要在自身位置不确定的条件下，在完全未知环境中，创建地图同时利用该地图进行自主定位和导航。这就是移动机器人的同时定位与地图创建(SLAM)问题，也称为CML(ConcurrentMappingandLocalization)。该研究领域的代表人物有Smith，Self和Cheeseman[23]。由于SLAM算法具有重要的理论与应用价值，被很多学者认为是实现真正全自主移动机器人的关键。近几年来，其研究取得了很大的进展，并已应用于各种不同的环境，如：室内环境[24][25]、水下[26][27]以及室外环境[28][29]。3.1.1SLAM算法性质SLAM算法有很多重要的属性，影响着地图特征和机器人位置估计中的不确定性。包括：状态估计的收敛性，估计过程的一致性，状态协方差矩阵更新的计算复杂度[23]。收敛性：SLAM算法具有三个重要的收敛性，这三个关键的收敛结果是：A.地图协方差矩阵的任意子矩阵地行列式随着每一次观测单调下降；B.在极限情况下，随着观测数量的增加，特征估计变得完全相关。C.地图的精度与第一个特征被观测时机器人的位置精度有关。这些结果表明：随着观测数量的增加，地图估计的不确定性将降低到有限的误差范围内，地图特征的关系将是完全确定的。一致性：为了维护SLAM算法估计的一致性，对状态协方差矩阵进行更新维护是必要的。既然对环境的观测是相对机器人的，所以机器人估计中的任何误差和地图估计中的误差使绝对相关的。在没有其它外部的关于特征和机器人位置信息情况下，为了使系统状态估计的误差在有限的范围内，保持状态估计之间的一致性是很必要的。所以就必须维护机器人状态与环境特征之间的协方差矩阵。计算复杂度：SLAM算法应用到大规模环境时的一个重要局限性是计算环境特征之间，特征与机器人之间的相关信息时的计算负担。由于特征数量很大，协方差矩阵的更新维护导致了SLAM算法的计算复杂性。对于那些包含上万个特征的环境，计算负担使得系统协方差的更新变的难以执行。所以需要一个有效的方法来提高算法的计算效率。3.1.2SLAM算法分类根据所依据的理论基础的不同，SLAM可以分为以下几种：（1）基于扩展卡尔曼滤波（EKF）的CML/SLAM这是最常用的一种SLAM方法，适合解决非线性系统的估计问题。该方法用平面坐标表示机器人和环境特征的位置，将机器人运动与环境特征的关系描述为两个非线性模型：机器人运动模型和观测模型.通过这两个模型，运用扩展卡尔曼理论的思想来实现。主要包括预测与更新两个阶段[26][28]。（2）基于概率的CML/SLAM尽管不如EKF那样流行，但由于用概率表达机器人定位问题的不确定性非常自然合理，基于概率的CML也吸引了很多人的目光。其中，较流行是最大相似性(MaximumLikelihoodEstimation，MLE)方法。一种非常有效的最大相似性估计算法称为Baum-Welch(或α-β)算法[30]，基于这种方法的机器人制图与定位问题可看作是机器人位置与环境特征位置的最大相似性估计问题。该算法包括两步：E-Step(expectation)和M-Step(maximization)。（3）基于粒子滤波器(particlefilter)的CML/SLAM粒子滤波器定位也称为MonteCarlo定位[31][32]，其基本思想是用一组滤波器来估计机器人的可能位置(处于该位置的概率)，每个滤波器对应一个位置，利用观测对每个滤波器进行加权传播，从而使最有可能的位置的概率越来越高。（4）基于空间扩展信息滤波器的SLAM[33]该方法由SebastianThrun等人提出，是对EKF算法的改进。它不再用协方差矩阵表示空间信息的相关性，取而代之用空间信息矩阵来表示空间信息间的内在固有的关系，并且使用网状数据结构仅维护邻近的环境特征(地图)。（5）基于集合理论估计的SLAM[34]基于集合理论的方法不假设噪声服从某种分布，而只假设噪声是有界的。具体方法是：定义一个可行状态集合(feasiblestateset)和一个观测集合(measurementset)，前者表示机器人和环境特征的状态估计，后者表示符合条件(观测误差小于边界)的状态集合。算法首先根据机器人本身的状态方程计算可行状态集合，然后根据观测值计算观测集合，最后取两个集合的交集作为某时刻的经估计校正后的机器人与环境特征的状态集合(地图)。此外根据所制地图的不同，SLAM又可以分为：基于栅格的SLAM[35]；基于特征的SLAM[28][29][36]以及基于拓扑结构的SLAM[37]。在基于栅格的SLAM算法中，环境表示为一组具有特定分辨度的栅格的组合。每个栅格中有特定概率值来表示其被占据的可能性。使用基于栅格SLAM算法的时候，假设各个栅格的状态变量是相互独立的，而没有考虑栅格之间的关联性。另外，当机器人采用该种SLAM算法运行较长一段路程后，很难再回到出发点。这是因为该方法中仅在局部坐标中考虑了估计的不确定性，却没考虑局部坐标与全局坐标中的相互联系。总的说来，基于栅格的SLAM方法可以提供较为丰富的环境描述，对于局部环境下的规划与导航有很大帮助，但是通过该方法无法获得一致性较强的全局地图。基于特征地图的SLAM算法是将环境表示为一组组参数化的特征值，比如说点，线，角等。这里的特征指的是环境中那些突显于背景的、易于传感器分辨检测的且可以通过参数描述的东西。使用这种方法的时候，必须对环境中不同类型的特征分别建立测量模型以便准确提取。基于特征的SLAM算法是最为流行的一种，尽管在环境地图描述中仅仅用到特征的位置值，但是还可以通过大小，颜色等信息进行匹配等。这种SLAM算法常常是基于卡尔曼滤波的。使用这种方法时，状态向量不仅包括机器人的位姿信息而且还有特征的位置信息。随着新特征的不断提取与确认，状态向量将不断的变大。因为用于描述环境的特征值的测量都是相对机器人的，所以对环境特征测量的不确定性是与机器人位姿估计的不确定性息息相关的。可以在理论上证明，随着时间的不断推移、测量的不断进行，地图中的特征将是完全相关的，也就是说此时随意给定一个特征的绝对坐标值，将会得到一个精准的地图。在环境特征容易识别的场合下，该算法运用很好，但是对于特性不是太明显的非结构化环境中，算法的运用遇到难题。在基于拓扑结构的SLAM算法中使用“图”来描述环境。具体说是通过节点和弧线来描述环境。每个节点表示环境中突显的地方，边用来表示相邻节点间的相对位置。对于纯粹的拓扑地图来说，无须知道每个节点的具体位置，它只用来表示节点间的连通性。但是该方法也有很大的弊端，比如当环境稍微复杂一些时，其对环境的识别能力会明显下降且无法识别相近的环境。尽管SLAM算法的理论基础已经被很好的研究，但是要将其更好的运用于实际，特别是大型的非结构化环境，仍有大量理论和实际的问题需要解决包括：计算效率、地图管理、局部地图与全局地图之间的协调融合、数据匹配以及传感器管理等[38][39]。3.2基于特征SLAM算法构架简介上文中讲到根据所得到地图类型的不同，可以将SLAM算法化分为基于栅格的SLAM；基于特征的SLAM以及基于拓扑结构的SLAM三大类。其中，基于特征的SLAM是运用最广的一类，下文将对该类SLAM算法进行详细阐述。基于特征的SLAM算法架构由图3-1表示。由图3-1可以看出，整个算法实际上是由三部分构成即：预测——实践——更新。首先要建立机器人的动力学模型以及运动过程中的观测模型。某一时刻k，在给定控制参数,v后，根据动力学模型可以预测机器人在1k时刻的位姿状态；同时结合观测模型还可以预测在1k时刻机器人对旧（已经核实的）特征iF的观测值)|1(kkdi；在实践环节中，即在时刻k到时刻1k的过程中，机器人基本上要完成三大工作：环境特征的提取和合理性检验，以及相关数据的匹配。从而获得对iF特征的实际测量)1|1(kkdi，同时筛选出新的特征1iF。将对特征iF的估计测量和实际测量的差异值（残差）d、机器人位姿估计以及旧的地图一起作为输入参数，通过一定滤波算法即可更新机器人位姿，进而得到新特征1iF的位置信息并将其融入到旧地图中生成新地图。总而言之，该方法要求机器人的位置和所有地图特征的位置存放在同一个状态向量中，并且这个向量是增广的。当观测到环境中新的特征时，就要把它作为新的地图特征添加到状态向量中。然后通过滤波就可以构建一个收敛的地图。在极限情况下，随着对环境特征的重复观测，地图特征之间的相对关系变得完全确定，并且地图的不确定性控制在一个小的范围内。这个误差范围与机器人初始位置的不确定性有关。图3-1基于特征SLAM算法架构3.3传感器的选择及特征提取3.3.1传感器选择由图3-1可以看出，移动机器人的自主导航中必须以有效而可靠的环境感知为基础，运用一种或多种传感器的组合，配以信息融合技术，从而对周围环境进行认识。机器人自主导航中常用的传感器主要可以分为两大类：其中之一是内部传感器，它是机器人本身运动的感知，用来检测机器人组成部件内部状态，是实现闭环控制、伺服控制动作的不可缺少的装置。这类传感器有：惯导（INS），陀螺仪，里程计等。惯导提供加速度信息，速度计测量速度，陀螺仪测量角速度，里程计测量距离。这种传感器通常与机器人本身的运动模型一起对机器人的位置进行预测估计。由于这类传感器存在着随时间积累的漂移误差，所以不能直接用于长期定位。内部传感器记录了机器人的运动信息，能提供很高的采样频率而不依赖环境特征。另一类传感器称作外部传感器。机器人外部传感器是用来检测机器人与对象物体外部状态的传感器。它使机器人能及时了解工作环境和对象，并视其情况来调整自己的对策，以提高机器人的适应性和智能化水平。如全球定位系统GPS）。通过对已知位置的GPS卫星的观测来计算机器人当前的位置信息。其它常用的外部传感器有：激光雷达，毫米波雷达，声纳，视觉相机CCD等。激光测距雷达（Laserrangefinder），运用激光二极管光脉冲获取目标的距离信息。距离的测量一般基于TOF（timeofflight）技术或者相移（phase-shift）技术。利用Doppler效应，也可以测量目标运动的速度信息。在基于TOF的测距中，发射、接收短的激光脉冲并测量它返回时的时间，通过特定计算就可以得到距离；在基于相移的测距中，发射、接收连续的光波并通过比较返回信号与参考信号的相位来计算距离[40][41]。采用激光测距雷达的好处在于：速度快、测距精度高、相对于声纳传感器而言，角分辨率高、而且激光波束窄、测量距离长；但是其价格高、回波响应与目标的表面材料属性有关且有镜面反射和漫反射现象发生。毫米波雷达（millimeterwaveradar，MMWR），通过发射接收电磁回波来测量至目标对象的距离。雷达天线的尺寸依赖于所使用的频率。频率越高，天线越小。运用的电磁波通常是短脉冲或调制的连续波。毫米波雷达的优点在于：可以适用于任何的天气条件，而且测量距离长、精度也比较高；其缺点是：成本高、尺寸大，在同样的天气条件下，比激光雷达衰减的更快。声纳传感器（Sonar）具有成本低、波束覆盖范围宽的特点，但角度分辨率低,不够精确，且容易产生虚假和多重反射回波信号，增加特征匹配的难度。尽管如此，很多导航设计中都用到了声纳系统[42][43]。超声波测距是基于超声波在介质中的传播速度特性实现的非接触式距离测量，其原理是通过超声换能器发射超声波，其在空中传播至被测物体，经反射后由超声换能器接收反射回波信号，确定出超声脉冲从发射到接收的渡越时间T，在已知超声波声速V的前提下，则可以计算出被测物体离超声换能器的距离L=VT/2，完成