第七章 智能机器人自主导航与路径规划ppt课件

智能机器人原理与实践.第7章智能机器人自主导航与路径规划.7.1概述导航，最初是指对航海的船舶抵达目的地进行的导引过程。这一术语和自主性相结合，已成为智能机器人研究的核心和热点。Leonard和Durrant-Whyte将移动机器人导航定义为三个子问题：（1）“WhereamI？”——环境认知与机器人定位；（2）“WhereamIgoing？”——目标识别；（3）“HowdoIgetthere？”——路径规划。为完成导航，机器人需要依靠自身传感系统对内部姿态和外部环境信息进行感知，通过对环境空间信息的存储、识别、搜索等操作寻找最优或近似最优的无碰撞路径并实现安全运动。.7.1.1导航系统分类对于不同的室内与室外环境、结构化与非结构化环境，机器人完成准确的自身定位后，常用的导航方式主要有磁导航、惯性导航、视觉导航、卫星导航等。1.磁导航磁导航是在路径上连续埋设多条引导电缆，分别流过不同频率的电流，通过感应线圈对电流的检测来感知路径信息。2.惯性导航惯性导航是利用陀螺仪和加速度计等惯性传感器测量移动机器人的方位角和加速率，从而推知机器人当前位置和下一步的目的地。3.视觉导航依据环境空间的描述方式，可将移动机器人的视觉导航方式划分为三类。.（1）基于地图的导航：是完全依靠在移动机器人内部预先保存好的关于环境的几何模型、拓扑地图等比较完整的信息，在事先规划出的全局路线基础上，应用路径跟踪和避障技术来实现的；（2）基于创建地图的导航：是利用各种传感器来创建关于当前环境的几何模型或拓扑模型地图，然后利用这些模型来实现导航；（3）无地图的导航：是在环境信息完全未知的情况下，可通过摄像机或其他传感器对周围环境进行探测，利用对探测的物体进行识别或跟踪来实现导航。4．卫星导航移动机器人通过安装卫星信号接收装置，可以实现自身定位，无论其在室内还是室外。.7.1.2导航系统体系结构智能机器人的导航系统是一个自主式智能系统，其主要任务是如何把感知、规划、决策和行动等模块有机地结合起来。下图给出了一种智能机器人自主导航系统的控制结构。..7.2环境地图的表示构造地图的目的是用于绝对坐标系下的位姿估计。地图的表示通常有四种：拓扑图、特征图、网格图及直接表征法(Appearancebasedmethods)。不同方法具有各自的特点和适用范围，其中特征图和网格图应用较为普遍。.7.2.1　拓扑图1）基本思想地铁、公交路线图均是典型的拓扑地图实例，其中停靠站为节点，节点间的通道为边。在一般的办公环境中，拓扑单元有走廊和房间等，而打印机、桌椅等则是功能单元。连接器用于连接对应的位置，如门、楼梯、电梯等。2）特点拓扑图把环境建模为一张线图表示，忽略了具体的几何特征信息，不必精确表示不同节点间的地理位置关系，图形抽象，表示方便。.7.2.2　特征图1）基本思想结构化环境中，最常见的特征是直线段、角、边等。这些特征可用它们的颜色、长度、宽度、位置等参数表示。基于特征的地图一般用式（7.1）的特征集合表示：其中是一个特征(边、线角等)，n是地图中的特征总数。、 （7.1） .2）特点特征法定位准确，模型易于由计算机描述和表示，参数化特征也适用于路径规划和轨迹控制，但特征法需要特征提取等预处理过程，对传感器噪声比较敏感，只适于高度结构化环境。.7.2.3　网格图1）基本思想网格图把机器人的工作空间划分成网状结构，网格中的每一单元代表环境的一部分，每一个单元都分配了一个概率值，表示该单元被障碍物占据的可能性大小。2）特点网格法是一种近似描述，易于创建和维护，对某个网格的感知信息可直接与环境中某个区域对应，机器人对所测得的障碍物具体形状不太敏感，特别适于处理超声测量数据。但当在大型环境中或网格单元划分比较细时，网格法计算量迅速增长，需要大量内存单元，使计算机的实时处理变得很困难。.7.2.4　直接表征法1）基本思想通过记录来自不同位置及方向的环境外观感知数据，这些图像中包括了某些坐标、几何特征或符号信息，利用这些数据作为在这些位置处的环境特征描述。直接表征法与识别拓扑位置所采用的方法原理上是一样的，差别仅在于该法试图从所获取的传感器数据中创建一个函数关系以便更精确地确定机器人的位姿。2）特点直接表征法数据存贮量大，环境噪声干扰严重，特征数据的提取与匹配困难，其应用受到一定限制。.7.3定位定位是确定机器人在其作业环境中所处位置。机器人可以利用先验环境地图信息、位姿的当前估计以及传感器的观测值等输入信息，经过一定处理变换，获得更准确的当前位姿。移动机器人定位方式有很多种，常用的可以采用里程计、摄像机、激光雷达、声纳、速度或加速度计等。从方法上来分，移动机器人定位可分为相对定位和绝对定位两种。.7.3.1相对定位相对定位又称为局部位置跟踪，要求机器人在已知初始位置的条件下通过测量机器人相对于初始位置的距离和方向来确定当前位置，通常也称航迹推算法。相对定位只适于短时短距离运动的位姿估计，长时间运动时必须应用其它的传感器配合相关的定位算法进行校正。.1.里程计法里程计法是移动机器人定位技术中广泛采用的方法之一。在移动机器人的车轮上安装光电编码器，通过编码器记录的车轮转动圈数来计算机器人的位移和偏转角度。里程计法定位过程中会产生两种误差。.1）系统误差系统误差在很长的时间内不会改变，和机器人导航的外界环境并没有关系，主要由下列因素引起：（1）驱动轮直径不等；（2）驱动轮实际直径的均值和名义直径不等；（3）驱动轮轴心不重合；（4）驱动轮间轮距长度不确定；（5）有限的编码器测量精度；（6）有限的编码器采样频率。.2）非系统误差非系统误差是在机器人和外界环境接触过程中，由于外界环境不可预料特性引起的。主要误差来源如下：（1）轮子打滑；（2）地面不平；（3）地面有无法预料的物体（例如石块）；（4）外力作用和内力作用；（5）驱动轮和地板是面接触而不是点接触。.对于机器人定位来说，非系统误差是异常严重的问题，因为它无法预测并导致严重的方向误差。非系统误差包括方向误差和位置误差。考虑机器人的定位误差时，方向误差是主要的误差源。机器人导航过程中小的方向误差会导致严重的位置误差。轮子打滑和地面不平都能导致严重的方向误差。在室内环境中，轮子打滑对机器人定位精度的影响要比地面不平对定位精度影响要大，因为轮子打滑发生的频率更高。.3）误差补偿机器人定位过程中，需要利用外界的传感器信息补偿误差。因此利用外界传感器定位机器人时，主要任务在于如何提取导航环境的特征并和环境地图进行匹配。在室内环境中，墙壁、走廊、拐角、门等特征被广泛地用于机器人的定位研究。.2.惯性导航定位法惯性导航定位法是一种使用惯性导航传感器定位的方法。它通常用陀螺仪来测量机器人的角速度，用加速度计测量机器人的加速度。对测量结果进行一次和二次积分即可得到机器人偏移的角度和位移，进而得出机器人当前的位置和姿态。.7.3.2绝对定位绝对定位又称为全局定位，要求机器人在未知初始位置的情况下确定自己的位置。主要采用导航信标、主动或被动标识、地图匹配、卫星导航技术或概率方法进行定位，定位精度较高。这几种方法中，信标或标识牌的建设和维护成本较高，地图匹配技术处理速度慢，GPS只能用于室外，目前精度还很差，绝对定位的位置计算方法包括三视角法、三视距法、模型匹配算法等。.1.主动灯塔法主动灯塔是可以很可靠地被检测到的信号发射源，将该信号进行最少的处理就可以提供精确的定位信息。2.路标导航定位法路标导航定位法是利用环境中的路标，给移动机器人提供位置信息。路标分为人工路标和自然路标。3.地图匹配法基于地图的定位方法称为地图匹配法。机器人运用各种传感器(如超声波传感器、激光测距仪、视觉系统等)探测环境来创建它所处的局部环境地图，然后将此局部地图与存储在机器人中的己知的全局地图进行匹配。如果匹配成功，机器人就计算出自身在该环境中的位置。.4.GPS定位GPS是适用于室外移动机器人的一种全局定位系统，它是一种以空间卫星为基础的高精度导航与定位系统，是由美国国防部批准研制，为海、陆、空三军服务的一种新的军用卫星导航系统，该系统由三大部分构成：GPS卫星星座(空间部分)，地面监控部分(控制部分)和GPS信号接收机(用户部分)。GPS系统能够实施全球性、全天候、实时连续的三维导航定位服务。.7.3.3基于概率的绝对定位概率定位中最重要的马尔可夫定位和蒙特卡罗定位。马尔可夫定位和蒙特卡罗定位不仅能够实现全局定位和局部位置跟踪，而且能够解决机器人的“绑架”问题。机器人“绑架”问题是指，由于机器人容易与外界发生碰撞而使机器人在不知情(里程计没有记录)的情况下发生移动。.1.马尔可夫定位（MarkovLocalization，ML）尔可夫定位的基本思想是：机器人不知道它的确切位置，而是知道它可能位置的信度（Belief，即机器人在整个位置空间的概率分布，信度值之和为1）。马尔可夫定位的关键之处在于信度值的计算。当机器人收到外界传感器信息或者利用编码器获得机器人移动信息时，基于马尔可夫假设和贝叶斯规则，每个栅格的信度值被更新。.根据初始状态概率分布和观测数据估计系统的当前状态，其中表示机器人的位姿(由位置和方向组成)。从统计学的观点看，的估计是一个贝叶斯滤波问题，可以通过估计后验密度分布来实现。贝叶斯滤波器假设系统是一个马尔可夫过程，可以通过以下2步算得。       .1)预测通过运动模型预测系统在下一时刻的状态，即通过如下公式计算先验概率密度：（7.2）式中：称为系统的运动模型(状态转移先验密度)。   .2)更新通过观测模型利用新的观测信息更新系统的状态，即通过如下公式计算后验概率密度：（7.3）式中：称为系统的观测模型(观测密度)。当机器人获得编码器信息或者利用外界传感器感知环境后，马尔可夫定位算法必须对所有的栅格进行计算，因此需要大量的计算资源和内存，导致定位处理的实时性很差。   .2.蒙特卡罗定位（Monte-CarloLocalization，MCL）基于马尔可夫定位方法，Dellaert等人提出了蒙特卡罗定位方法。MCL也称为粒子滤波（ParticleFilter）。MCL的主要思想是用N个带有权重的离散采样来表示后验概率密度。其中是机器人在t时刻的一个可能状态；是一个非负的参数称为权重，表示t时刻机器人的状态为的概率也就是，且。       .MCL包括4个阶段：初始化，采样阶段，权重归一化和输出阶段。采样阶段是MCL的核心，它包括重采样、状态转移和权重计算3步。实际上MCL是按照提议密度分布抽取采样，然后利用权重来补偿提议密度分布与后验密度分布之间的差距。当机器人发生“绑架”时，要估计的后验密度与提议密度分布的错位很大，在取值较大区域的采样数很少，需要大量的采样才能较好地估计后验密度。   .3.卡尔曼滤波定位（KalmanFilter，KF）kalman滤波器是一个最优化自回归数据处理算法。基本思想是采用信号和噪声空间状态模型，结合当前时刻的观测值和前一时刻的估计值来更新对状态变量的估计，从而得到当前时刻的估计值。对于非线性估计问题，可以通过线性近似去解决。相应的方法有EKF(ExtendedKalmanFilter)、UKF(UnscentedKalmanFilter)等。.kalman滤波器通过预测方程和测量方程对系统状态进行估计，利用递推的方式寻找最小均方误差下的的估计值。kalman滤波的数学模型为：状态方程为：（7.4）测量方程为：(7.5）    其中，是k时刻时系统的状态，A是k-1时刻到k时刻的状态转移矩阵，是k时刻的测量值，H是观测矩阵，为系统过程噪声，为系统测量噪声，假设为高斯白噪声。   .如果不考虑观测噪声和输入信号时，则k时刻的观测值和已知的最有状态估计值，可通过一下方程进行求解最优估计值。状态预测方程：（7.6）预测状态下的协方差方程：（7.7）滤波器增益矩阵：（7.8）      .状态最优化估计方程：（7.9）状态最优化估计的协方差方程：（7.10）通过kalman滤波器的公式可以看出，只要给定了和，就可以根据k时刻的观测值，就可以通过递推计算得出k时刻的状态估计。     .下图给出了卡尔曼滤波根据所有传感器提供的信息，实现高效信息融合的一般。.kalman滤波器已经广泛应用在了各个方面，比如机器人的SLAM问题，雷达系统的跟踪的等等。下图描述了卡尔曼滤波器的机器人定位架构。.(1)位置预测：基于带有高斯误差的运动系统模型，机器人根据编码器数据，进行位置预测。(2)传感器测量：机器人收集实际的传感器数据，提取合适的环境特征，产生一个实际的位置。(3)匹配更新：机器人要在实际提取的特征和测量预测的期望特征之间，辨识最佳的信息。卡尔曼滤波器可以将所有这些匹配所提供的信息融合，递归估计更新机器人的状态。.7.4路径规划7.4.1路径规划分类路径规划本身可以分成不同的层次，从不同的方面有不同的划分。根据对环境的掌握情况，机器人的路径规划问题可以大致分为三种类型：1.基于地图的全局路径规划基于地图的全局路径规划，根据先验环境模型找出从起始点到目标点的符合一定性能的可行或最优的路径。2.基于传感器的局部路径规划基于传感器的局部路径规划，依赖传感器获得障碍物的尺寸、形状和位置等信息。环境是未知或部分未知的。.3.混合型方法混合型方法试图结合全局和局部的优点，将全局规划的“粗”路径作为局部规划的目标，从而引导机器人最终找到目标点。.7.4.2路径规划方法1.可视图法可视图（VisibilityGraph，VG）由一系列障碍物的顶点和机器人起始点及目标点用直线组合相连。要求机器人和障碍物各顶点之间、目标点和障碍物各顶点之间以及各障碍物顶点与顶点之间的连线均不能穿越障碍物，即直线是“可视的”。这样，从起始点到目标点的最优路径转化为从起始点到目标点经过这些可视直线的最短距离问题。图中粗实线即为由VG法得到的具有最短路径，但由于过于靠近障碍物，得到路径的安全性较差。可视图法适用于环境中的障碍物是多边形的情况。.可视图法路径规划如下图所示：.2.Voronoi图法Voronoi图，又叫泰森多边形图。如下图所示，它是由一组由连接两邻点直线的垂直平分线组成的连续多边形组成。.由下图可见，Voronoi图路径规划尽可能远离障碍物，从起始节点到目标节点的路径将会增长。但采用这种控制方式时，即使产生位置误差，移动机器人也不会碰到障碍物，其缺点是存在较多的突变点。.3.单元分解法如下图所示，首先把状态空间分解为与空间平行的许多矩形或立方体，称为单元（Cell），其中每个cell都标记为：1)空的：如果Cell内每一点均与状态空间的障碍物不相交；2)满的：如果Cell内每一点均与状态空间中的障碍物相交；3)混合的：如果Cell内点既有与状态空间的障碍物相交，也有不相交的。状态空间分解.单元分解法就是要寻找一条由空的Cell所组成的包含有起点和目标点的连通路径，如下图所示。如果这样的路径在初始划分的状态空间中不存在，则要找出所有混合cell，将其进一步细分，并将划分的结果进行标记，然后在空的cell中进行搜索，如此反复，直至成功。连通路径搜索.4.人工势场法传统的人工势场法把智能机器人在环境中的运动视为一种在抽象的人造受力场中的运动，目标点对智能机器人产生“引力”，障碍物对智能机器人产生“斥力”，最后通过求合力来控制智能机器人的运动。但是，由于势场法把所有信息压缩为单个合力，这样就存在把有关障碍物分布的有价值的信息抛弃的缺陷，且易陷入局部最小值。.5.A*算法1）A*算法原型Dijkstra算法的基本思想如图所示：.从初始点S到目标点E寻求最低花费路径，粗黑的箭头代表寻找到的最优路径。圆圈代表节点，圆圈中间数字代表从初始点经过最低花费的路径到达该点时的总花费，箭头上数字代表从箭头始端指向末端所需的花费，算法通过比较各条路径选择了一条最短的花费作为该点圆圈内的数字。2）A*算法流程A*算法具体引入了当前结点的估计函数f(i)，结点的评价函数可以定义为：（7.11）式中：g(i)表示从起点到当前结点的最短距离，h(i)表示从当前结点到终点的最短距离的估计值，可取结点到终点的直线和球面距离。 .若，即没有利用任何路网信息，这时的A*算法就变成了Dijkstra算法。可见，A*算法实质是Dijkstra算法的改进“算法”。对于h(i)的具体形式，也可以依据实际情况进行选择。例如，除了可以当前结点到终点的最短距离之外，还可以引入方向。A*算法本身表述起来很简单，关键是在代码优化上，基本的思路一般都是以空间(即内存的占佣)换取时间(搜索速度)，另外还有诸如多级地图精度和地图分区域搜索等一些地图预处理技术。 .6.基于模糊逻辑的路径规划模糊方法是在线规划中通常采用的一种规划方法，包括建模和局部规划。基于模糊逻辑的机器人路径规划的基本思想：各个物体的运动状态用模糊集的概念来表达，每个物体的隶属函数包含该物体当前位置、速度大小和速度方向的信息。然后通过模糊综合评价对各个方向进行综合考察，得到路径规划结果。.7.基于神经网络的路径规划Hopfiled神经网络用于机器人路径规划的基本思想：障碍物中心处的空间点其碰撞罚函数有最大值。随着空间点与障碍物中心距离的增大，其碰撞罚函数的值逐渐减小，且为单调连续变化。在障碍物区域外的空间点其碰撞罚函数的值近似为0。因此使整个能量函数E最小，便可以使该路径尽可能远离障碍物，不与障碍物相碰，并使路径的长度尽量短，即得到一条最优路径。.8.基于遗传算法的路径规划遗传算法用于机器人路径规划的基本思想：采用栅格法对机器人工作空间进行划分，用序号标识栅格，并以此序号作为机器人路径规划参数编码，统一确定其个体长度，随机产生障碍物位置及数目，并在搜索到最优路径后，再在环境空间中随机插入障碍物，模拟环境变化。但是，规划空间栅格法建模还存在缺陷，即若栅格划分过粗，则规划精度较低；若栅格划分太细，则数据量又会太大。.9.动态规划法动态规划法是解决多阶段决策优化问题的一种数值方法。动态规划算法将复杂的多变量决策问题进行分段决策，从而将其转化为多个单变量的决策问题。JeromeBarraquand等人以经典的动态规划方法为基础，对全局路径规划问题进行了研究。结论表明，动态规划算法非常适合于动态环境下的路径规划。如何改进动态规划的算法，以提高计算效率，是当前动态规划研究一项重要内容。.7.5人工势场法7.5.1人工势场法基本思想人工势场实际上是对机器人运行环境的一种抽象描述。在势场中包含斥力和引力极，不希望机器人进入的区域的障碍物属于斥力极，子目标及建议机器人进入的区域为引力极。引力极和斥力极的周围由势函数产生相应的势场。机器人在势场中具有一定的抽象势能，它的负梯度方向表达了机器人系统所受到抽象力的方向，正是这种抽象力，促使机器人绕过障碍物，朝目标前进。.7.5.2势场函数的构建传统势场法中势场的构造是应用引力与斥力共同对机器人产生作用，总的势场U可表示为：（7.12）式中：为斥力场；为引力场。势场力可表示为：（7.13）式中：为引力；为斥力；为合力，决定了智能机器人的运动。       .斥力与引力可分别表达为：（7.14）（7.15）    .在势场中智能机器人的受力图如下图所示。.当机器人到达目标，目标点对智能机器人的引力等于障碍物对其产生的斥力时，。算法也可能会产生局部极小点，在某个位置时，并未到达目标。这时需要对算法进行改进，例如引入其他的量对机器人进行控制。图下图所示，给出了一个人工势场分布示意图，从图中可以大致了解机器人在某个位置的运动趋势。.1.斥力场函数当障碍物形状规则时，障碍物的表面由隐函数来表示，则斥力函数可表示为（7.16）式中：为位置增益系数，是障碍物附近一点。势力场的影响范围局限于和两表面之间的空间。     .当障碍物形状不规则时，斥力场函数可表示为（7.17）式中：为智能机器人X与障碍物O之间的最短距离，是一个常数，代表障碍物的影响距离。  .相应地，将式（7.16）（7.17）代入到式（7.13）可求得斥力：（7.18）或（7.19） .式中：（7.20）（7.21）  .2.引力场函数目标G的势函数同样也可以基于距离的概念。目标G对智能机器人X起吸引作用，而且距离远，吸引作用越大，反之就越小。当距离为零时，智能机器人的势能为零，此时智能机器人到达终点。通常引力场函数可构建为：（7.22）式中：kg为位置增益系数；为智能机器人X与目标点之间的相对距离。   .相应地，将式（7.22）代入式（7.15）中可得到吸引力为（7.23）式中：吸引力方向指向目标点，在智能机器人到达目标的过程中，这个力线性地收敛于零。  .7.5.3人工势场法的特点1.优点人工势场法的优点是应用人工势场法规划出来的路径一般是比较平滑并且安全的，因为斥力场的作用，智能机器人总是要远离障碍物的势场范围；势场法结构简单、易于实现，所以在路径规划中被广泛地采用。2.缺点势场法的缺点是存在一个局部最优点问题。为了解决这个问题，许多学者进行了研究，如Rimon、Shahid和Khosla等。他们期望通过建立统一的势能函数来解决这一问题，但是这就要求障碍物最好是规则的，否则算法的计算量很大，有时甚至是无法计算的。.7.5.4人工势场法的改进在定义斥力场函数时，把智能机器人与目标之间的相对距离也考虑进去，从而建立一个新的斥力场函数。修改式（7.16）和式（7.17）如下：（7.24）（7.25）.式中，为智能机器人与目标点之间的距离，障碍物的影响范围在距离之内，是一个大于零的任意实数。与（7.16）和式（7.17）相比，改进的势场函数引入了智能机器人与目标的相对距离，保证了整个势场仅在目标点全局最小。通过分析取值不同时，势场函数的数学特性，证明斥力函数在目标点是可微的，在此不再赘述。    .7.5.5仿真分析假定机器人以不变的速度运动，仿真环境选择Matlab，小车的运动由合力决定。目标点（10，10）(仿真中用三角表示)，起点（0，0）(仿真中用小方框表示)，随机产生的障碍物（仿真中用小圆圈表示）。相应的参数选取为：（1）引力增益系数：2；（2）斥力增益系数：5；（3）小车运动的步长：0.5；（4）障碍物影响距离：2。.单障碍物的路径规划仿真结果如下图所示：.对多障碍物的仿真（因FIRA比赛中有5V5比赛，故障碍物选取5个）。实验中就不同给定障碍物的条件下，进行了大量的仿真。绝大部分情况下，小车均能寻找到通往目标点的路径，并且顺利绕开障碍物。说明了人工势场法的用于机器人的路径规划还是可行的。.下图给出了其中几种不同条件下的路径规划图。.下图给出了目标点与障碍物较近时的路径规划情况。图中可以看出：起初，机器人能够完成避障并向目标前进；当机器人接近目标时，机器人被推开而达不到目标点的情况，这就是所谓的局部稳定，就是指在特殊情况下，由于障碍物的位置因素使得机器人在路径中的某一点受力平衡，达到稳定，从而使该点成为势场的全局最小点，机器人陷在该点无法到达目标。.7.4栅格法7.6.1用栅格表示环境有些文献中采用正方形栅格表示环境，每个正方形栅格有一个表征值CV，表示在此方法中障碍物对于机器人的危险程度，对于高CV值的栅格位置，机器人就要优先躲避。CV值按其距车体的距离被事先划分成若干等级。每个等级对机器人的躲避方向会产生不同的影响。障碍物的位置一旦被确定，则按照一定的衰减的方式赋给障碍物本身及其周围栅格一定的值，每个栅格的值代表了该位置有障碍物的可能性。障碍物栅格的初值和递减速度完全是由路径的安全性和最优性来共同决定。下图给出一种障碍物的赋值示例，以被检测到的障碍物为中心向周围八个方向进行传播，障碍物所在的栅格值最大。.栅格值计算.7.6.2基于栅格地图的路径搜索当给定起点位置和目标位置后，应根据给定的目标点位置对整个地图进行初始化。确定初始值的各种方法都大致相同：每个栅格的初始值等于该栅格与目标栅格的横向距离加上该栅格与目标栅格纵向距离。由此形成初始地图。初始地图与障碍物地图合起来就成了路径搜索用的地图了，在这个地图上进行路径的搜索。.传统的栅格法中，路径搜索一般是将“起始点栅格”作为参考栅格，从参考栅格的八个相邻栅格中选择值最小的栅格；再将所选栅格作为新的参考栅格，重复此直到到达到了“目标栅格”。那么为了保证路径的平滑，要做一定的设置，即如果有多个可选栅格时，选择使智能机器人转动角度最小的栅格。那么此时就要记录智能机器人的移动方向。.7.6.3栅格法的特点通过研究发现栅格具有简单、实用、操作方便的特点，完全能够满足使用要求。(1)无需障碍物为规则障碍物，在动态规划中，更加不需要知道障碍物的形状、大小；(2)无需考虑运动对象的运动轨迹、数目及形状；(3)算法实现简单，在很多场合都实用；(4)只要起始点与终点之间存在通路，那么栅格就一定能找到一条路径从起始点到终点。.同时也能看到栅格大小的选择直接影响着控制算法的性能。栅格选得小，环境分辨率高，但是抗干扰能力弱，环境信息存储量大，决策速度慢；栅格选得大，抗干扰能力强，环境信息存储量小，决策速度快，但是分辨率下降，在密集障碍物环境中发现路径的能力减弱。所有单用栅格法对现在的移动机器人的研究已经行不通了。.7.7移动机器人的同步定位与地图构建机器人构建一个环境地图，并同时运用这个地图进行机器人定位，称作同时定位与建图(SimultaneousLocallzationandMapPing，SLAM）或并发定位与建图(ConcurrentLocalizationandMapping，CLM)。(1)环境建模(MaPPing)是建立机器人所工作环境的各种物体如障碍、路标等的准确的空间位置描述，即空间模型或地图。(2)定位(Localization)是确定机器人自身在该工作环境中的精确位置。精确的环境模型(地图)及机器人定位有助于高效地路径规划和决策，是保证机器人安全导航的基础。可见：定位和建图是一个“鸡和蛋”的问题，环境建模需要定位，定位又依赖于环境地图。.7.7.1SLAM的基本问题SLAM问题可以描述为：移动机器人从一个未知的位置出发，在不断运动过程中根据自身位姿估计和传感器对环境的感知构建增量式地图，同时利用该地图更新自己的定位。定位与增量式建图融为一体，而不是独立的两个阶段。.作为机器人导航领域的热点，SLAM问题的研究主要包括以下几个方面：（1）环境描述，即环境地图的表示方法。地图的表示通常可分为3类：栅格表示、几何特征表示和拓扑图表示。（2）环境信息的获取。机器人在环境中漫游并记录传感器的感知数据，涉及到机器人的定位与环境特征提取问题；（3）环境信息的表示。机器人根据环境信息更新地图，这涉及到对运动和感知不确定信息的描述和处理。（4）鲁棒的SLAM方法。.7.7.2移动机器人SLAM系统模型下图简单描述了移动机器人SLAM的系统状态。假设机器人在未知环境中移动，同时使用自身携带的传感器探测外部未知的路标信息以及自身的里程信息。表示t时刻移动机器人的位姿状态向量，表示第i个路标的位置状态向量，为机器人从t-1时刻到t时刻的输入控制向量，为t时刻观测向量。    .移动机器人SLAM系统状态图.若把t时刻移动机器人SLAM系统的状态记为，状态包含了t时刻机器人的位姿(即机器人的位置和方向)和路标的位置。从概率学来看，假定移动机器人SLAM系统是先将机器人运动到当前位置，然后进行观测，则系统当前状态与之前的系统状态、观测信息以及输入有关，即。假设系统当前的状态仅与前一时刻的系统状态和当前的输入有关，即前一时刻的系统状态已经包含了之前的系统状态、观测信息和输入，则当前系统状态的分布概率为：（7.26）在此系统状态估计上获得的观测信息的估计为（7.27）  .可以看出，公式(7.26)描述了系统状态转移概率，它与公式(7.27)共同组成了移动机器人和环境的一个隐马尔科夫模型(HiddenMarkovModel，HMM)或动态贝叶斯网络(DynamicBayesNetwork，DBN)，即移动机器人SLAM问题模型如下图所示。.7.7.3移动机器人SLAM解决方法1．SLAM解决思想对于SLAM问题，根据之前的移动机器人位姿、观测信息以及控制输入信息可以求得t时刻机器人位姿x和环境中路标位置m的联合后验概率(7.28)假定环境服从马尔科夫的前提，SLAM问题可分为预测、更新两步递归执行。.预测：依据前一时刻状态的后验信度，也即，结合运动模型来预测当前t时刻状态的先验信度。(7.29)式中：为运动模型，为后验信度。      .更新：利用感知模型，结合当前的感知测量信息来更新当前t时刻状态的后验概率分布。(7.30)为化因子，为观测模型，为先验信度。   .一般而言，SLAM由传感数据的获取、数据关联、定位和地图构建等环节构成，其典型流程如右图。图中，表示传感器测量所获取的数据，表示第k-1时刻的局部地图，表示k时刻机器人的位姿。传感数据获取包括移动机器人本身的数据采集和环境的数据。   .数据关联是SLAM的关键步骤，用于当前特征与已有特征的匹配。对于数据关联中没有关联上的特征，可先加人到临时特征存储区，并对下一次观测进行预测，在下一次观测中把观测特征与其关联。如果一个特征连续两次没有关联成功，则作为假观测从临时特征存储区剔除。如果一个特征第一次关联不成功，但第二次关联成功，则将该特征加人状态向量，同时对状态进行扩维。定位和地图构建是一个相互交互的过程，定位的结果用于地图构建，而已经构建的地图又用于机器人的定位。事实上，目前移动机器人SLAM经典的算法主要包括扩展卡尔曼滤波器、最大似然估计、粒子滤波器以及Markov定位等，而其中扩展卡尔曼滤波器EKF和粒子滤波的方法是来自于上述Bayes滤波器的估计状态后验概率分布的思想的。.2.卡尔曼滤波卡尔曼滤波在数学上是一种统计估算方法，通过处理一系列带有误差的实际量测数据而得到的物理参数的最佳估算，其思想是利用前一时刻的数据和误差信息来估计当前时刻的数据。卡尔曼滤波器用反馈控制的方法估计过程状态：滤波器估计过程某一时刻的状态，然后以(含噪声的)测量变量的方式获得反馈。因此卡尔曼滤波器可分为两个部分：预测和更新。预测负责及时向前推算当前状态变量和误差协方差估计的值，以便为下一个时间状态构造先验估计。更新方程负责反馈，也就是说，它将先验估计和新的测量变量结合以构造改进的后验估计。.3.粒子滤波粒子滤波器是一种典型的采用蒙特卡罗数值模拟求解贝叶斯滤波问题的方法，其基本思想是利用一组带有相关权值的随机样本，用这些样本的估计来表示系统状态的后验概率密度。当样本数非常大时，这种估计将等同于后验概率密度。粒子滤波通过非参数化的蒙特卡罗模拟方法来实现递推Bayes滤波，适用于任何能用状态空间模型表示的非线性系统，以及传统卡尔曼滤波无法表示的非线性系统，精度可以逼进最优估计。.7.7.4SLAM的难点和技术关键l.不确定性和计算量大的问题无论是感知外部环境的或感知机器人运动的传感器测量都带有不确定性，即测量噪声。机器人的运动控制也同样带有不确定性。各种测量误差之间并非完全独立，因此SLAM对地图的估计和对机器人的位姿估计都有很强的不确定性。为了处理不确定性，无论是扩展卡尔曼滤波方法或是粒子滤波方法，都需要很大的计算量。扩展卡尔曼滤波SLAM方法的计算量主要在于地图的更新计算，即协方差矩阵的计算；而粒子滤波SLAM方法的计算量是随着粒子数增多而增大。如何减少SLAM过程的计算量是SLAM研究的重要课题，对于动态环境下的SLAM技术更是紧迫的问题。.2.数据关联问题数据关联问题也称一致性问题，是指建立在不同时间、不同地段获得的传感器测量之间、传感器测量与地图特征之间或者地图特征之间的对应关系，以确定它们是否源于环境中同一物理实体的过程。数据关联问题是SLAM本身面临的挑战之一，其正确与否对于SLAM的状态估计至关重要。在动态环境下，由于动态目标的影响，数据关联问题就显得更为困难和重要。.3.动态目标检侧与处理问题移动机器人成功构建地图必须具备识别动态障碍物和静态障碍物的能力。目前相关的研究大部分也就如何检测动态目标而展开，但是相关方法都有如下问题：检测的准确性，阐值难以确定，动态目标在成功检测出来后，如何在SLAM过程中进行处理都是难点之一。.