移动机器人SLAM技术 课件 【ch04】移动机器人定位

移动机器人定位第四章大学生礼仪高等院校公共课系列精品教材01环境地图构建方法PARTONE01环境地图构建方法

(1)定位。通过己构建并存储的环境地图即可获得移动机器人的位置信息，无须再次运行系统进行建图。（2）导航。导航是指移动机器人能够在地图中进行路径规划，从任意两个地图点间寻找路径，然后控制自己运动到目标点的过程。但该用途需要构建稠密地图才可实现。（3）避障。避障也是移动机器人经常碰到的一个问题。它与导航类似，但更注重局部的、动态的障碍物的处理。同样地，仅有特征点，我们无法判断某个特征点是否为障碍物，所以也需要稠密地图。（4）重建。可利用SLAM获得周围环境的重建效果，使其他人能够远程地观看我们重建得到的三维物体或场景一．一三维的视频通话或网上购物等。这种地图亦是稠密的，并且我们还对它的外观有一些要求。（5）交互。交互主要指人与地图之间的互动。例如，在增强现实中，我们会在房间里放置虚拟物体，并与这些虚拟物体之间有一些互动。01环境地图构建方法

因移动机器人可能涉足的环境多种多样，目前还没有找到能够适用于所有环境的统一方法。针对此种情况，人们设计了不同的环境地图，以使其能够适用于不同的场合，典型地图类型如图。01几何特征地图

几何特征地图是由环境中的路标所组成的，每个路标的特征用点、线、面等几何原型来近似。几何特征地图能够为移动机器人的定位提供所需要的度量信息，并且该地图所需的存储量比较小，有利于移动机器人的位姿估计及环境中的目标识别。该类方法的问题主要有如何从移动机器人所收集的环境信息中找出几何特征，并且如何按照当前移动机器人的位姿和所观测到的环境中的路标在几何特征地图中寻找对应的位置。提取几何特征需要对所观测到的信息进行额外的处理，并且需要使用一定数量的观测数据才能得到想要的结果。假如是在室内环境下，通常可以将环境中的桌子、墙壁、楼梯定义为更加抽象化的面、角、边等几何特征。一般情况下，室外环境的几何特征不易提取。01几何特征地图

基于特征的地图(Feature-basedMaps)使用全局坐标表示环境中的一些基本几何图元。由于几何特征地图既能提供规划及定位所需要的度量信息，存储量又相对较小，因此它经常出现在许多基于视觉SLAM的研究中。在这类研究中，移动机器人一般只在一定范围空旷的区域内（无障碍）活动。几何特征地图一般用来表征路标等移动机器人感兴趣的特征信息，其优点是表示方法较紧凑，且便于定位和目标识别，但如何从自然环境中提取稳定的特征信息是其需要解决的问题之一01拓扑地图

更加抽象的环境地图形式将空间环境以拓扑结构图的形式进行描述：拓扑地图中的节点表示环境当中的特殊位置点，其中的边表示节点之间的连接关系。拓扑地图一般以图表的形式出现，所需要的存储空间相对较小，利用拓扑地图进行路径规划的效率会很高，其比较适用于大规模的环境地图构建。然而由于没有精确的尺度信息，所以该地图不适用于移动机器人的定位。当移动机器人所处的环境中存在两个相似的特征时，依据拓扑地图将难以确定该特征是否属于同一节点。01点云地图是三维空间中离散的一组点除了包含基本的三维坐标x、y、z外，还可以具有r、g、b的彩色信息，那么，点云中的每一个点都具有[r,g,b,x,y,z]]的形式。首先，点云地图是一种度量地图，能够清所地描述环境中物体间的位置关系，并且在相对稠密的点云地图中，物体的表面信息能够被较好地描述。点云地图的状态表达相对简单，存在点云的地方就是被占据的，反之，则为空闲。01点云地图利用RGB-D相机进行稠密建图是相对容易的。不过，根据地图形式的不同，也存在着若干种不同的主流建图方式。最直观、最简单的方法就是根据估算的相机位姿，将RGB-D数据转化为点云，然后进行拼接，最后得到一个由离散的点组成的点云地图。所谓点云地图，就是由一组离散的点表示的地图。最基本的点包含x、y、z三维坐标，也可以带有r、g、b的彩色信息。由于RGB-D相机提供了彩色图和深度图，很容易根据相机内参数来计算RGB-D点云。如果通过某种手段得到了相机的位姿，那么只要直接把点云进行加和，就可以获得全局的点云。01点云地图深度相机经过标定得到相机内参数和畸变系数后，对彩色图像和深度图像进行配准和去畸变。在已知彩色图像像素点坐标和对应深度值时，即可根据式(4-1)得到该点在相机坐标系下的坐标。01点云地图式中，

为彩色图像的像素坐标；d为深度图像对应的深度值：s为比例系数。对图像中每个像素点利用式(4-2)计算其空间点在相机坐标系下的坐标，即可得到整张图像的点云。根据关键帧的彩色图像、深度图像可得到单帧点云如图4.4所示，全局地图由单帧点云拼接而成。已知空间中一点在世界坐标系下的坐标为P,在相机坐标系下的坐标为R,对应图像中的像素坐标为pu,则它们存在关系式中，X为相机内参数矩阵，s为比例系数，Tcw为从世界坐标系转换到相机坐标系的变换矩阵。在视觉SLAM中，相机的位姿为相机相对于世界坐标系的坐标和相机坐标系相对于世界坐标系的旋转，故Tcw为通过视觉SLAM系统得到的相机位姿。一般情况下，将相机输入视觉SLAM系统的第一帧数据对应的坐标系作为世界坐标系，且将该帧设置为关键帧。因此，点云的拼接过程可以描述如下。01点云地图假设利用第i帧关键帧生成的点云为Cloud,第1顿关键帧的相机位姿为Ti,第j帧关键帧生成的点云为Cloudi,,第j帧关键帧的相机位姿为Tj,拼接这两顿点云的过程如下。01点云地图点云地图如图4.5所示，它为我们提供了比较基本的可视化地图，让我们能够大致了解环境的样子。它以三维方式存储，使得我们能够快速地浏览场景的各个角落，乃至在场景中进行漫游。点云地图的一大优势是可以直接由RGB-D图像高效地生成，不需要额外处理。它的滤波操作非常直观，处理效率尚能接受。但是点云地图也存在缺陷，在点云地图中，我们虽然有了三维结构，亦进行了体素滤波以调整分辨率，但点云地图通常规模很大，所以一PCD文件也会很大。一张640×480的图像，会产生30万个空间点，需要大量的存储空间，其中有很多不必要的细节，即使经过一些滤波之后，PCD文件也是很大的。除非我们降低分辨率，否则在有限的内存中，无法建模较大的环境。然而降低分辨率会导致地图质量下降，且点云地图无法实现是否占据的描述，因此无法应用于导航等功能。01八叉树地图八叉树地图是一种灵活的、能随时进行压缩的地图形式，其通过八叉树结构来描述空间环境。八叉树地图如图4.6所示，又称为Octomap,是一种栅格地图，以八叉树(Octree)结构的形式存储并基于占据概率的形式表示。同时八叉树地图是一种灵活的、可压缩的，又能随时更新的地图形式，更适合于移动机器人的环境地图描述。01八叉树地图八叉树地图像是由很多个小方块组成的。当其分辨率较高时，方块很小；当其分辨率较低时，方块很大。每个方块表示该格被占据的概率，因此可以通过查询某个方块或点“是否可以通过”,从而实现不同层次的导航。简而言之，环境较大时采用较低分辨率，而较精细的导航可采用较高分辨率，十分方便。八叉树结构通过对三维空间的几何实体进行体元剖分，从而形成一个具有根节点的方向图。其中图4.7(a)所示为八叉树在空间中的表现形式，可以看出利用八叉树可以任意地分割空间，满足不同的地图精度要求。图4.7(b)所示为八叉树的数据结构，树根可以不断地往下扩展，每次分成八个枝，直到叶子为止。叶子节点代表了分辨率最高的情况，图中的照方块代表的就是叶子节点。01八叉树地图如果对每个叶子节点分配一个概率，则父节点的概率为其所有子节点的概率之和。对于已经确定被占据的节点，其概率为1,相应地，确定未被占据的节点的概率为0。对一个叶子节点n,在1,2,…的不同时刻，有观测数据

那么叶子节点的概率为01八叉树地图根据式（4．11）可知，在三维空间中的任何点，新的观测可以直接加到之前的观测上，所有地图的更新十分简便快速，同时也说明父节点的概率就是子节点的概率和。因此，基于传感器获得的观测数据，可以对八叉树结构的节点不断地进行更新，计算各节点的占据概率，完成八叉树地图的构建，八叉树地图如图4·8所示。01精格地图栅格地图是二维地图，反映了空间中某一平面的信息，主要思想是将平面划分为同等大小的栅格单元，每个栅格单元代表平面上的一个区域，并用栅格单元是否被占据来描述环境信息。此种方法采用概率值来表达环境模型的不确定性。栅格地图相比于上述几种环境地图，能够提供更为精确的度量信息，而且该地图非常直观且容易处理，因此广泛应用于许多移动机器人系统中，直到现在仍受到很多研究者的欢迎。01精格地图栅格地图中的每个栅格单元，其状态可以分为已占据状态和未占据状态。已占据状态代表该栅格单元中存在障碍物，与此相反，未占据状态代表在该栅格单元中没有障碍物存在。移动机器人在未知环境中运动时，以一个栅格作为最小移动单位，并综合考虑栅格地图中的所有栅格的状态，从而对行进路线进行决策。图4．9所示为移动机器人在室内环境下进行自主探索所建立的栅格地图。在该地图中，灰色部分代表未占据区域，即在该区域中没有障碍物，黑色部分代表己占据区域，即有墙体等障碍物的存在。栅格地图虽然有很多优占但也不可避免地存在一些问题。栅格地图的准确性严重依赖里程计的精度，同时栅格地图的存储和维护所需要的数据量比较大，对大规模场景的环境地图构建问题而言，难以满足其实时性要求。因此使用栅格地图时，需要综合考虑计算机的计算能力及实际运行的环境大小等。02定位问题的分类PARTONE02局部定位与全局定位影响定位困难程度的第1个方面是定位问题是以最初运行期间可供使用信息的类型为特征的。位置跟踪假定移动机器人的初始(Initial)位姿己知，通过适应移动机器人的运动噪声来定位移动机器人。此类噪声的影响通常很微弱，因此位置跟踪经常依赖位姿误差小的假设。位置的不确定性经常用单峰分布（如高斯分布）来近似。位置跟踪问题是一个局部问题，因为不确定性是局部的，并且局限于移动机器人真实位姿附近的区域。全局定位认为移动机器人的初始位姿未知。移动机器人最初放置在环境中的某个地方，但是缺少它的位置信息。全局定位不能假定位姿误差的有界性，正如后面章节将介绍的，，使用单峰分布通常是不合适的：全局定位比位置跟踪更困难，事实上它包括了位置跟踪。02局部定位与全局定位绑架移动机器人问题是全局定位问题的一个变种，但是它更加困难。在运行过程中，移动机器人被绑架瞬间移动到其他位置。绑架移动机器人问题比全局定位问题更困难，因为移动机器人可能相信自己在那儿，尽管它不是在那里，而在全局定位中，移动机器人不知道自己在哪儿。有人可能认为实际上移动机器人很少被绑架，然而，这个问题的现实意义来自最先进定位算法的观察，算法并不能保证永不失效，具备从失效中恢复的能力对于真正的自主移动机器人来说是必不可少的。通过绑架移动机器人可以测试一个定位算法，可以用来衡量该算法从全局定位失效中恢复的能力。02静态环镜定位与动态环境定位静态环境静态环境，指仅有的变量（状态）是移动机器人位姿的环境。换句话说，静态环境里只有移动机器人是移动的，环境里其他目标永远保持在同一位置。静态环境具有一些很好的数学特性使得移动机器人服从高效概率估计。动态环境，存在除移动机器人外位置或配置随时间变化的物体。特别有趣的是，变化在整个时间上持续，并对一个以上传感器的读数产生影响。不可测量的改变当然与定位无关，那些只影响一个测量的变化最好被当作噪声对待。更多持久变化的示例是，人、日光（对安装摄像机的移动机器人来说）、可移动的家具、门。很明显，大多数真实的环境是动态的，状态变化发生在不同的速度范围内。动态环境02被动定位与主动定位主动定位(ActiveLocation)的情况。主动定位算法控制移动机器人，以便最小化定位误差/最小化定位不良的移动机器人进入一个危险地方引起的花费。主动定位技术受到的一个主要限制是需要全过程地控制移动机器人。因此，只有主动定位技术是不够的。当执行其他任务而不是定位时，移动机器人也必须能自我定位。一些主动定位技术建立在被动定位技术之上。其他的一些主动定位技术，当管理一个移动机器人时，会把任务性能目标和定位目标结合起来。被动定位(PassiveLocation)的情况。定位模块仅观察移动机器人的运行，移动机器人通过其他方式控制，并且移动机器人运动不是为了定位，而是可能随意移动或执行它每天的任务。02单机器人定位与多机器人定位单机器人定位（Single-robotLocalization)是定位研究最常用的方法，它仅仅处理单一机器人。单机器人定位便于在单一机器人平台上收集所有数据，并且不存在通信问题。多机器人定位(Multi-robotLocalization)问题来源于机器人团队：每一机器人能独立地定位自身，因此多机器人定位问题可以通过单机器人定位解决。如果机器人之间能相互探测，定位有可能做得更好。这是因为如果两个机器人的相对位置信息可供使用，一个机器人的看法可以用于影响另一个机器人的看法。多机器人定位问题引出了一些有趣的、有意义的问题，即置信表示问题与两者之间的通信属性问题。上述4个方面捕获了移动机器人定位问题的4个重要特性，还有其他特性会影响定位问题的难度，如移动机器人测量提供的信息和运动过程中信息的丢失，而且，对称环境比非对称环境更加困难，因为其具有更高的模糊性。03定位的常用方法PARTONE03GPS定位GPS是一种可以授时和测距的空间交会定点的导航系统，可向全球用户提供连续、实时、高精度的三维位置、三维速度和时间信息。GPS最初主要用于军事，如为陆海空三军提供实时、全天候和全球性的导航服务，并用于情报收集、核爆监测、应急通信和爆破定位等方面。03GPS定位一个简单的GPS由空间卫星部分、地面接收器和用户接收机三部分构成。其中空间卫星部分由24颗在离地面12000km的高空上，以12小时为一个周期环绕地球运行的GPS卫星组成。任意时刻，在地面上的任意一点都可以同时观测到4颗以上的GPS卫星，地面接收器在计算出GPS卫星的位置后，利用三边法计算出地面接收器的经纬度和高度。03GPS定位GPS相对定位，亦称差分GPS定位，是目前GPS定位中精度最高的一种定位方法，其基本原理示意图如图4卫所示，将两台用户接收机分别安置在基线的两端，并同步观测相同的GPS卫星，以确定基线端点（测站点）在WGS-84坐标系中的相对位置或基线向量。03基于地图的定位1·使用拓扑地图和直接推理的定位方法此类方法都使用拓扑地图表示结构化的外界环境，在定位过程中往往采用直接式的推理方法，以广义Voronoi图（GeneralizedVoronoiGraph,GVG）方法为典型。如图4．12所示，广义Voronoi图由一系列的直线和抛物线构成，直线段由两个障碍物的顶点或两个障碍物的边定义生成，直线段上的所有点必须与障碍物的顶点或障碍物的边距离相等。抛物线段由一个障碍物的顶点和一个障碍物的边定义生成，同样要求抛物线段上的所有点与障碍物的顶点和障碍物的边有相同距离。03基于地图的定位2·使用几何特征地图和直接推理的定位方法几何特征地图和直接推理的定位方法大多通过判断移动机器人自身与环境中的己知点的相对位置进行定位。如何在有噪声的情况下有效识别尽量多的路标并用于定位，一直是研宄关注的焦点之一。几何特征地冬在描述具体环境特征上较拓扑地图更为有效。基于传感器融合的三角形方法是一种典型的使用几何特征地图和直接推理的定位方法，其意图在于通过在移动机器人移动过程中对距离传感器的读数进行分析，找出环境中的一些固定点作为特征点，用这些点和地图中的参考点集进行比较，从而估计移动机器人可能的位置。03基于地图的定位识别特征点使用三角形融合算法完成，其核心思想是从移动机器人的激光、声呐等测距波束在不同位置获得的周边测距信息中提取环境中固定物体产生的反射，经过时间序列上的筛选和综合之后，移动机器人就可以有效跟踪环境中的特征点相对自身的位置变化。前面识别的特征点，定位的推理用一个选举的过程来进行，目的是找出一个平移和旋转变换使当前识别获得的特征点集与预先存储的参照点集（往往是环境中有转角特征的点，可以人工标出，也可以通过图像处理技术从地图中自动获得）有最大重合度，而这个平移和旋转变换就是移动机器人的位置和朝向。此方法由两部分构成，识别特征点和利用识别结果推理定位。03基于地图的定位3·使用栅格地图和概率推理的定位方法随着对移动机器人定位问题的研究深入，定位过程中遇到的不确定性越来越受到重视，当根据传感器信息建立的环境模型与配置的环境地图进行匹配时，由于传感器误差等不确定性因素，可能出现一个对象符合数个匹配的情况。在这种情况下，一般采用基于概率推理的定位方法来消除匹配的不明确性。在近几年的研究中，应用这种推理的定位方法往往使用基于栅格描述的环境地图，下节将着重介绍该部分方法。03基于概率方法的定位在移动机器人定位中，移动机器人的位姿是马尔可夫定位方法的研究对象。移动机器人在空间中的运动可以用离散的数学模型来描述，因而可以通过离散的马尔可夫过程（马尔可夫链）来表示移动机器人在各个时间点的每个位置的概率分布。具体地，利用先验环境地图信息、位姿的当前估计和传感器的观测值等输入信息，将其经过一定的处理和变换，更新各个时间点的每个位姿的概率分布，从而产生更加准确的移动机器人当前的位姿估计结果。马尔可夫定位的关键是移动机器人运动过程的无后效性假设，即移动机器人当前时刻的状态仅与前一时刻的状态和动作有关，与其过去的状态和动作与未来的状态和动作无关，并且移动机器人在过去状态下的传感器测量数据与当前状态下的传感器测量数据无关。在马尔可夫定位中，环境状态即是移动机器人在环境中的位姿，通常以离散变量表示。1·马尔可夫定位03基于概率方法的定位，另外，Bel(Xk)表示移动机器人在k时刻位于X处的可信度。根据移动机器人传感器的感知数据Zk所采取的动作Ak确定移动机器人位于环境空间位置的可能性大小Bel(Xk)。该可信度代表了移动机器人位于整个环境空间的概率分布情况(这里用可信度概念而不直接用概率表示，是为了强调这是对移动机器人状态的估计)。这里的Xk;可以是(X,Y,O)空间中的任一位置或拓扑空间中的某一节点。当移动机器人运动或接收到新的传感器输入数据时，状态将被更新。假设在每一个离散时刻k,移动机器人收到一次测量数据Zk,并给出一次动作命令Ak。马尔可夫定位的任务是随着移动机器人的运动，不断地更新移动机器人位于位置Xk的可信度，并以可信度最大的位置作为对移动机器人实际位姿的估计。03基于概率方法的定位03基于概率方法的定位马尔可夫定位方法与卡尔曼滤波定位方法的不同之处在于,马尔可夫定位方法不使用高斯分布表示概率密度,而是将整个状态空间离散化,直接以每个(离散后的)空间单元的概率密度来表示状态分布,因此在各个时间记录的就不仅仅是单纯的姿态数学期望和置信度方差,而是整个空间中的姿态概率分布。这种表示方法的优越性在于首先可以表示定位过程中的歧义或多义的情况,其次可以表示移动机器人对自身位置一无所知的情况,这为处理移动机器人在运动过程中突然被转移的情况和定位失败的恢复提供了较好的解决途径。03基于概率方法的定位2.卡尔曼滤波定位马尔可夫定位模型可以表示移动机器人位置的任何概率密度函数,该方法比较通用但效率低。移动机器人定位的关键不是概率密度曲线的精确复制,而是与鲁棒定位相关的传感器融合问题。卡尔曼滤波是实现多种传感器信息融合的一个高效技术,不管获得的数据是否准确,利用线性的系统状态方程和观测方程就可以得到一个全局最优的状态估计。设一个线性离散时间系统可以用如下状态方程和观测方程表示。03基于概率方法的定位尔曼滤波定位极具有效性，其递推特性使得系统处理数据不需要大量的数据存储和计算，因此在移动机器人定位领域得到了广泛的应用。经典卡尔曼滤波定位实现移动机器人的自定位，需要严格的运动模型匹配，但其具有不能处理非线性估计问题的缺点，因而需要采用扩展卡尔曼滤波定位更好地解决移动机器人在实际情况下的定位问题。03基于概率方法的定位扩展卡尔曼滤波(ExtendedKalmanFilter,EKE)定位方法为了处理移动机器人定位过程中的不确定性，移动机器人的位姿使用高斯分布来描述，而在移动机器人运动过程中位姿的更新表现为移动机器人位姿空间中位姿向量分布的数学期望和方差的更新。这个更新过程中用的是扩展卡尔曼滤波，其包括两个方面：扫描匹配和基于扩展卡尔曼滤波的位姿计算。扫描匹配的目的在于获得根据移动机器人的传感器读数推断的当前位置假设坐标和此假设坐标的误差矩阵。基于扩展卡尔曼滤波的位置计算按照不同的信息来源（外部测距读数或内部里程计读数）计