服务机器人路径规划与路径跟踪

服务机器人路径规划与路径跟踪摘要：服务机器人由于所处的环境不可能是完全确定的，使得机器人导航问题在服务型机器人应用中越发关键。本文讨论了环境部分未知的情况下的导航问题中的全局路径规划，根据环境中的静态已知障碍物利用蚁群算法找出全局最优路径，采用了链接图来建立环境全局地图，将工作空间转换为带权图的形式，然后使用蚁群算法对带权图进行搜索，得到从起始点到目标的一条全局最优路径。关键词：蚁群算法；路径跟踪；避障Service Robots path planning and path followingAbstract: Because the working environment of the service robot cant be completely determined,the navigation is more important to resarch for service robot than other robots. The thesis proposes a solution to the robot navigation in part unknowen environment.The solution is the global path-planning. The main purpose is to lay out a global path in the static global environment using Ant Colony Algorithm. MAKLINK Graph is used to build the global working evrionment,which converts the workspace to weighted graph,then Ant Colony Algorithm is used to search the weighted graph and get the global optimal path from the begin position to the goal position.Key words: Ant Colony Algorithm; Path following; obstacle avoidance家庭服务机器人是为了倡导机器人技术步入人们的生活以及机器人技术的实用化普及化，最终实现自主机器人与人类真正意义上的人机协作和自然交互(如语音交互、姿态交互)。机器人在家庭环境中快速、准确地导航是机器人进入家庭、提供各类服务的前提。区别于工业机器人所处的准备好的确定性环境，家庭环境变动性大，由不规则形状的家具、相对狭窄的门和频繁活动于其间的人类构成，且不能因为机器人的进入，大规模改动家庭环境，从而给生活主体人类带来不便。因此寻找实用且效率高的环境获取方法，同时在不大量增加传感器数量的前提下，实现精确定位，从而支持家庭服务机器人快速、准确导航成为一个极富挑战性的课题。在移动机器人路径规划理论和方法的研究中，确定性环境的路径规划问题即离线的全局路径规划方已取得了大量的研究和应用成果。使用链接图将全局确定性环境转换成带权值的图，而对于带权图求两固定点之间的最短距离可以用已经成熟的遗传算法以及蚁群算法求解1。但是，对于存在未知障碍物的环境下的路径规划虽然也取得了一定的研究成果，但是还没有完善的体系结构，仍然有不少关键理论和技术问题急需解决。要想让机器人进入服务领域，必须解决环境部分未知情况下的路径规划，因为服务机器人的服务对象是人，而有人存在的环境必然是变化的，比如人的行走，人往环境中移入或移除物体。1 机器人路径规划1.1 路径规划的基本概念路径规划是移动机器人导航中最重要的任务之一，对移动机器人路径规划系统的主要要求是2：（1） 在环境地图中寻找一条路径，保证机器人沿该路径移动时不与外界发生碰撞；（2） 能够处理用传感器感知的环境模型中的不确定因素和路径执行中出现的误差；（3） 通过使机器人避开外界物体而使其对机器人传感器感知范围的影响降到最小；（4） 能够按照需要找到最优路径。不论采用何种导航方式，智能移动机器人主要完成路径规划，定位和避障等任务。根据机器人对环境信息的了解程度，机器人路径规划可以划分为：环境信息完全已知的全局路径规划和环境信息完全未知或部分未知，需要通过传感器对障碍物位置、形状或尺寸进行探测的局部路径规划。全局路径规划的任务是根据先验地理环境信息找出从起始点到目标点的符合一定性能的可行或最优路径。局部路径规划使得机器人在沿全局路径的行进过程中，根据传感器不断感知的周围环境信息和自身的状态信息，规划出一条无障碍、可通行的局部路径。在实际的工作中，环境是由已知的静态障碍物和每天可能变化位置的静态障碍物（比如桌椅摆放的位置）以及动态障碍物（比如在房间中行走的人）所组成。1.2 路径规划的分类及现状从到目前为止的研究来看,移动机器人路径规划方法主要可以分为以下三种类型3：（1） 基于事例的学习规划方法基于事例的学习规划方法依靠过去的经验进行学习及问题求解，一个新的事例可以通过修改事例库中与当前情况相似的旧的事例来获得。将其应用于移动机器人的路径规划中可以描述为：首先，利用路径规划所用到的或已产生的信息建立一个事例库,库中的任一事例包含每一次规划时的环境信息和路径信息，这些事例可以通过特定的索引取得：随后，将由当前规划任务和环境信息产生的事例与事例库中的事例进行匹配，以寻找出一个最优匹配事例，然后对该事例进行修正，并以此作为最后的结果。移动机器人导航需要良好的自适应性和稳定性，而基于事例的方法能满足这个需求。Ram A将基于事例的在线匹配和增强式学习相结合，提高了机器人的自适应性能，较好地适应了环境的变化。利用基于事例的方法时要注意保持事例库中的事例数量，以防止增加机器人在线规划时间或产生信息爆炸问题。MarefatM把基于事例的方法作为一个特征辅助规划与全局规划结合从而提高了全局规划的效率。Kruusmaa M4通过创建种群事例库在理论上覆盖了关于路径搜寻问题所有可能的路径解空间，克服了启发式搜索方法在此方面的缺陷。（2） 基于环境模型的规划方法该方法首先需要建立一个关于机器人运动环境的环境模型。在很多时候由于移动机器人的工作环境具有不确定性（包括非结构性、动态性等），使得移动机器人无法建立全局环境模型，而只能根据传感器信息实时地建立局部环境模型，因此局部模型的实时性、可靠性成为影响移动机器人是否可以安全、连续、平稳运动的关键。环境建模的方法基本上可以分为两类：网络/图建模方法、基于网格的建模方法。前者主要包括自由空间法、顶点图像法、广义锥法等，利用它们在进行路径规划时可得到比较精确的解，但所耗费的计算量相当大，不适合于实际的应用。而后者在实现上要简单许多，所以应用比较广泛，其典型代表就是四叉树建模法及其扩展算法（如基于位置码四叉树建模法、Framed-quadtrees建模法等5）。基于环境模型的规划方法根据掌握环境信息的完整程度可以细分为环境信息完全已知的全局路径规划和环境信息完全未知或部分未知的局部路径规划。由于环境模型是已知的，全局路径规划的设计标准是尽量使规划的效果达到最优。在此领域已经有了许多成熟的方法，包括可视图法、切线图法、Voronoi图法、拓扑法、惩罚函数法、栅格法等。前4种方法都是采用基于图论的思想，将目标、机器人及其工作空间用一个连接图表示，如此一来,路径规划问题就转化为在图上寻找一条从起始节点到目标节点的路线。惩罚函数法将路径规划这个有约束的问题（受到障碍物的限制）转化为一个无约束最优化问题，再求解就可得出解答。栅格法用网格描述机器人的工作环境，根据栅格的可信度值可确定出障碍物的分布，此时通过避障规划就可得到无碰路径。（3） 基于行为的路径规划方法基于行为的方法由Brooks8在他著名的包容式结构中建立，它是一门从生物系统收到启发而产生的用来设计自主机器人的技术，它采用类似动物进化的自底向上的原理体系，尝试从简单的智能体来建立一个复杂的系统。将其用于解决移动机器人路径规划问题是一种新的发展趋势，它把导航问题分解为许多相对独立的行为单元，比如跟踪、避碰、目标制导等。这些行为单元是一些由传感器和执行器组成的完整的运动控制单元，具有相应的导航功能，各行为单元所采用的行为方式各不相同，这些单元通过相互协调工作来完成导航任务。基于行为的方法大体可以分为反射式行为、反应式行为、慎思行为3种类型。反射式行为类似于青蛙的膝跳反射，是一种瞬间的应激性本能反应，它可以对突发性情况作出迅速反应，如移动机器人在运动中紧急停止等，但该方法不具备智能性，一般是与其他方法结合使用。基于反应式行为的方法是由Brooks最先提出，它直接读取传感器数据来规划下一步的动作，可以稳定及时地对不可预知的障碍和环境变化作出反应，但由于缺乏全局环境知识，因此所产生的动作序列可能不是全局最优的，不适合于复杂环境下移动机器人的路径规划，机器人的沿墙走行为策略是其典型的代表。慎思行为利用已知的全局环境模型为智能体系统到达某个特定目标提供最优动作序列，适合于复杂静态环境下的规划，移动机器人在运动中的实时重规划就是一种慎思行为，机器人可能出现倒退的动作以走出危险区域，但由于慎思规划需要一定的时间去执行，所以它对于环境中不可预知的改变反应较慢。反应式行为、慎思行为可以通过传感器数据、全局知识、反应速度、推理论证能力和计算的复杂性这几方面来加以区分。近来，在慎思行为的发展中出现了一种类似于人的大脑记忆的陈述性认知行为，基于此的规划不仅仅依靠传感器和已有的先验信息，还取决于所要到达的目标。比如对于距离较远且暂时不可见的目标，有可能存在一个行为分叉点，即有几种行为可供采用，机器人要择优选择，这种决策性行为就是陈述性认知行为。将它用于路径规划中能使移动机器人具有更高的智能，但由于决策的复杂性，该方法难以用于实际之中，这方面工作还有待进一步研究。2 全局路径规划关于路径规划的一种流传甚广但是不准确的说法是，路径规划本质上是由一些碰撞检测或避障组成的。实际上，路径规划不仅如此。它包含如下方面7：在移动障碍物之间计算出无碰路径；协调几个机器人之间的运动；获取物体之间的精确关系；分析基于传感信息所做的运动策略的不稳定性；处理物理模型的特性以及机器人对物体的抓取。基本的运动规划问题描述如下：令A是在欧式空间W中运动的一个机器人，W就是机器人A的工作空间，可以描述为，N=2或3；令是空间W中分布的静态障碍物。假设的几何特性以及的位置已知的话，再进一步假设A的运动没有运动学限制。那么运动规划问题就是，给定A在W中起始位置和方向以及目标点位置和方向，计算出从起始点到目标点并且不碰到的一系列连续的线段。2.1 全局地图的创建全局规划的第一步就是要建立全局地图，获得在机器人工作空间W中的精确描述。机器人根据在W中的描述，规划出一条无碰路径。全局地图的构建方法分为自由空间法和构造空间法8。2.1.1 自由空间法自由空间法(Free Space Approach)9：采用预先定义的如广义锥形和凸多边形等基本形状构造自由空间，并将自由空间表示为连通图，然后通过搜索连通图来进行路径规划。此方法比较灵活，即使起始点和目标点改变，也不必重构连通图。但是算法的复杂程度与障碍物的多少成正比，且不能保证任何情况下都能获得最短路径。因而该方法仅适用于路径精度要求不高，机器人速度不快的场合。2.1.2 构造空间法构造空间是指在一个适当的空间（机器人的构造空间）中将机器人看成一个点，然后将障碍物映射进该空间中。也就是说将障碍物根据机器人的几何尺寸进行相应的扩展，扩展后，机器人就可以看成一个质点了。如图1所示：其中左边的为实际工作空间，右边的为构造空间。图1 构造空间Fig.1 Structural atial对于构造空间，可以进一步划分为可视图法（Visibility Graph）、沃罗诺伊图法（Voronoi Diagram）和栅格法（Grids）。可视图法是将机器人视为一点，将机器人、目标点和扩展后的多边形障碍物的各个顶点进行组合连接，要求机器人和障碍物各顶点之间、目标点和障碍物各顶点之间以及各障碍物顶点与顶点之间的连线均不能穿越障碍物，即直线是可视的。如图2所示。搜索最优路径的问题变为求这些可视直线的最短距离问题，可运用优化算法比如Dijkstra算法，简化可视图，缩短搜索时间。图2 可视图Fig.2 Visual chart假设最优路径是一条固定起始点的绳子，那么拉动绳子处于目标点的一端，当拉不动时得到的就是最优路径。由此可知，全局最优路径必然经过障碍物顶点，因此，可视图法有可能求得全局最优解。但是根据可视图法求得的最优路径距离障碍物太近甚至接触，当机器人的轮子打滑或是定位不准的时候就会发生碰撞。常用的解决方法是：把机器人扩展的半径增加，当然，这样牺牲了可视图路径规划的最优长度。另外，可视图法由于建图的时候将起点和目标点考虑进去，因此当起点或目标点发生改变的时候就要重新构造可视图，比较麻烦。可视图还有一个缺点就是搜索时间太长，已经有人证明对于有N条线段的环境来说，时间复杂度是。对于带有圆弧的障碍物，可视图法失效，于是产生了沃罗诺伊图法。Voronoi是在19世纪初由俄国数学家J.Voronoi首先提出的一个几何概念。与可视图相比，Voronoi图倾向于使图中机器人与障碍物之间的距离最大化。对自由空间中的各点，计算它到最近障碍物的距离。在离两个或多个障碍物等距离的点上，这种距离图就有陡的山脊，Voronoi图就是由这些陡的山脊点所形成的边缘组成。当构造空间的障碍物都是多边形时，Voronoi图由直线和抛物线组成。如图3所示。图3 沃罗诺伊图Fig.3 Voronoi chart在有限距离的定位传感器情况下，Voronoi图有一个重要的弱点。因为这种路径规划算法使机器人与物体之间的距离最大化，机器人上的任何短距离传感器会有感知不到周围环境的危险。但Voronoi图由一个重要的难得优点，即Voronoi图具有可执行性。通过Voronoi图规划，给定一个特殊的已规划路径，配备有距离传感器的机器人可以使用简单的控制规则，跟踪在物理世界中Voronoi图的边缘。这些规则与创建Voronoi图的规则相匹配：即机器人使其传感器值的局部极小值最大化。这种控制系统会自然地使机器人呢保持在Voronoi图的边缘上，所以基于Voronoi图的运动减少了编码器的不准确性。栅格法是由Howden W.E.在1968年提出的，他在进行路径规划时采用了栅格表示地图。栅格法将机器人的工作空间划分成一系列具有二值信息的网络单元。若对环境中的某个方向不敏感，则可以将该方向的栅格长度增加，使得单元栅格的形状为长方形。另一种栅格表示法是基于四叉树的表示方法，基于环境中非相似区域不断递归分解。这一方法的好处是在一定程度上节约了存储空间。栅格大小的选取直接影响到控制算法的性能。栅格选得小，环境分辨率高，但抗干扰能力弱，环境信息存储量大，决策速度慢；栅格选得大，抗干扰能力强，环境信息存储量小，决策速度快，但分辨率下降，在密集障碍物环境中发现路径的能力减弱。栅格大小的选取也与传感器的性能有关。如果传感器精度高而且速度快，栅格可以选得小些。2.2 链接图法环境建模链接图是一种改进的自由空间法，它可以减少搜索的复杂性。建立链接图的时候基于以下假设：1、多边形障碍物的高度要平行于Z轴，整个路径存在于XY平面内。2、将障碍物的边界依据机器人的最大尺寸和机器人正常感知所需的最小范围进行扩展，从而可以将机器人相应的简化为一个质点。链接图将环境中的障碍物以其顶点表示，假设第i个障碍物有个顶点，则可表示为,而整个环境可以表示为,其中WSB为环境的边界。环境的改变只是W中相应元素的增减。要想构造链接图，先得定义自由链接线（free link）。自由链接线满足以下三个条件：（1）链接线的两端必须是两个多边形障碍物的两个顶点或者其中一个是障碍物顶点而另一个在环境的边界上。（2）每条自由链接线是相邻两个自由凸区域的公共界线。（3）自由链接线不能穿越环境中的任何障碍物。自由链接线的计算服从以下规则：(1)、从障碍物顶点做到环境边界的垂线，并将所有障碍物顶点两两连接。(2)、将第1步得到的线段按长度的大小从小到达排列。(3)、选择排序好的线段中的第一条。(4)、检测该线段是否与任何障碍物的边界相交。如果相交的话，则该线段不能作为自由链接线，忽略它，并从排好序的线段中取出下一条进行相应的处理。如果不与任何障碍物的边界相交，则进入第5步。(5)、检查这条链接线与障碍物顶点所产生的两个外角。 a)如果两个外角都小于或等于180度，则这条链接线是该顶点的最佳链接线。删去该顶点的其它链接线，然后转向第8步。 b)如果这条链接线的某个外角大于180度，则将这条链接线加入到该顶点的候选链接线。(6)、检查该顶点的所有候选链接线中是否还有外角大于180度，如果还有的话，转第4步，否则转下一步。(7)、删除该顶点的冗余链接线。(8)、对所有的顶点重复第17步。当从上述步骤中得到自由链接线之后，取每条链接线的中点并连接起来，则得到了我们所要的链接图。环境地图的自由链接线如图4所示，图中的黑点表示自由链接线的中点。相应的链接图如图5所示。图4 环境中的自由链接线Fig.4 Unconstrained threaded line in the environment图5 链接图Fig.5 Threaded chart假设S和G分别是机器人的起始点和目标点，首先看S和G之间的直接连线是否穿越障碍物，若不穿越，则就得到了最优路径；若穿越，则将S和G加入链接图。加入S和G以后的链接图如下：连接S和它所处的自由区域的相邻中点并对G做同样的处理。3 使用蚁群算法求解最短路径将机器人的起始点和目标点加入链接图以后，就可以开始我们的搜索过程了。将链接图的各个顶点以及起始点和目标点之间的连线赋上权值。如果顶点之间有连线，则权值为相应连线的实际长度，如果两点之间没有连线，则权值为无穷大。经过上述赋予权值的过程后，我们就将实际工作环境的静态已知部分转换为带权图。而对于带权图求两点之间的最短距离可以使用蚁群算法。3.1 蚁群算法的基本原理蚁群算法是最近几年才由意大利学者Dorigo,Manierio,Collorni等人提出的一种新型的模拟进化算法10。在自然界中蚂蚁几乎是瞎子，却能发现食物与蚁穴之间最短的距离。生态学的研究表明，蚂蚁是借助信息素（pheromone）来实现这一点的。信息素是蚂蚁分泌的一种化学物质，蚂蚁在寻找食物过程中会分泌这种物质。蚁群在寻找食物时，会有一些蚂蚁四处游荡，当蚂蚁发现食物并返回巢穴的过程中会在途中留下信息素。如果假定所有蚂蚁以相同的速度前进的话，则第一只返回巢穴的蚂蚁发现的路径是当前所有蚂蚁发现的路径中最好的。较短的路径上信息素会比较浓一些，而较浓的信息素能吸引更多的蚂蚁走这条路径，从而发现比较好的路径。我们引用Dorigo所举的例子来具体说明蚁群系统的原理(Colorni etal,1991)，(张纪会等，1999)。如图6所示，设A是巢穴，E是食物源，HC为一障碍物。由于有障碍物存在，蚂蚁只能由H或C由A到达E，或由E到达A，各点之间的距离如图所示。设每个时间单位有30只蚂蚁由A到达B，有30只蚂蚁由E到达D点，蚂蚁过后留下的信息素为1。为方便，设该物质停留时间为1。在初始时刻，由于路径BH、BC、DH、DC上均无信息素存在，位于B和D的蚂蚁可以随机选择路径。从统计的角度可以认为它们以相同的概率选择BH、BC、DH、DC。经过一个时间单位后，路径BCD上的信息素浓度是BHD上的2倍。t=1时刻，将有20只蚂蚁由B和D到达C，有10只蚂蚁由B和D到达H。随着时间的推移，蚂蚁将会以越来越大的概率选择路径BCD，最终完全选择路径BCD，从而找到由蚁巢到食物源的最短路径。由此可见，蚂蚁个体之间的信息交换是一个正反馈过程。图6 蚁群系统示意图Fig.6 The diagrammatic drawing of Ant System3.2 蚁群算法的基本原理我们已经使用链接图法将机器人的已知工作环境转换成了带权图，现在开始讲述蚁群算法在带权图搜索上的应用。假设带权图中共有n个结点，机器人起始位置位于结点0，目标点位于结点n-1，并以符号m表示蚂蚁个数，表示结点i和j之间的距离，也就是带权图上两点之间的权值。表示t时刻路径ij上的信息素浓度，表示t时刻位于结点i上的蚂蚁个数。蚂蚁一开始都位于结点0，权值小于无穷大的路径上设置初始信息浓度C。蚂蚁从结点0开始按概率选择下一结点。下一结点i被选中的概率正比于路径上的信息素浓度以及启发信息，本文中将启发信息选取为反比于两节点之间的路径段的权值。假设第k只蚂蚁选择了结点i作为下一结点后，将结点i放入该蚂蚁的禁忌表中，然后第k只蚂蚁就继续从结点i开始按同样的方法选择下一结点，如此继续下去，直到该蚂蚁到达目标节点n-1。当所有蚂蚁都到达目标结点后，就完成了一个搜索周期。接下来更新每个路径段上的信息素并清空每只蚂蚁的禁忌表。当这些做完了以后，将所有蚂蚁位置重置为结点0，然后开始下一个周期的搜索。如此往复，直到最终达到搜索终止条件。路径段上的信息素随着时间的推移不断挥发，当完成一次搜索后，蚂蚁又为走过的路径段上增加一定数量的信息素，使得被较多蚂蚁走过的路径段上留下更多的信息素，而较少蚂蚁走过的路径段上信息素由于挥发作用而不断减少，这称为信息素的更新。每只蚂蚁的下一步可选结点为与当前结点直接相连的并且未放入禁忌表中的结点。禁忌表是为了防止蚂蚁往回走。信息素浓度的更新方式如公式1所示： （1）其中为挥发系数，为第t次循环完成后在路径段ij上增加的信息素浓度。为开始第t次循环时路径段ij上的信息素浓度，为完成第t次循环后路径段ij上的信息素浓度。其中的计算公式如2式所示： （2）其中为第k只蚂蚁在第k次循环中为路径段ij增加的信息素浓度。根据的计算方式以及何时更新可以得到三种不同的算法模型：ANT-density, ANT-quantity和ANT-cycle。三种模型的表达式分别如下3，4，5所示： （3） （4） （5）在第t次循环中，第k只蚂蚁从结点i转移到结点j的概率为，其计算公式如6式所示： （6）其中，表示蚂蚁k下一步允许选择的结点集合；C表示带权图所有结点集合；禁忌表表示蚂蚁k当前循环中走过的结点集合；启发信息的计算方式如式7所示；参数表示信息素浓度的重要性，越大表示信息素浓度在蚂蚁路径选择中所起的作用越大，蚂蚁更倾向于选择有更多蚂蚁走过的路径，蚂蚁之间的协作性就越强；参数表示启发信息的重要程度，越大表示启发信息在蚂蚁路径选择中所起的作用越大，蚂蚁更倾向于选择下一路径段最短的结点，蚁群算法就越接近与贪心算法。 （7）蚁群算法的基本流程如下：（1） 将所有蚂蚁的初始位置设置为机器人起始点0，并且对所有的可行路径（即权值小于正无穷的路径）赋予初始信息浓度C，即令。（2） 对每一只蚂蚁搜索可行路径，根据每一条可行路径上的信息素浓度和启发信息计算得到蚂蚁选择该路径作为下一条路径段得概率，蚂蚁根据概率来选择下一条路径段。并将选择的路径段的另一个结点放入该蚂蚁的禁忌表。（3） 当所有蚂蚁到达目标点后更新全局信息素和局部信息素，然后清空每只蚂蚁的禁忌表。（4） 根据搜索到的路径长度是否明显减少或是否达到最大搜索次数来判断是否结束搜索。如果搜索结束条件未达到，则转向第2步进行下一个周期的搜索。由算法复杂性理论可知，m只蚂蚁要遍历n个结点，如果搜索次数为N，那么算法的时间复杂度是。4 路径跟踪4.1 路径跟踪的概念路径跟踪是要让机器人沿一条预先指定的路径移动。在这里预先指定的路径即是前面使用蚁群算法规划出来的全局最优路径。全局最优路径由一系列首尾相连的线段组成，每条线段用该线段的两个端点来表示。在这里介绍了两种路径跟踪方法：Follow the Carrot和Pure Pursuit，这两种跟踪方法都是使用一个始终处于全局最优路径上的Carrot点来引导机器人运动。这两种方法简单而有效。4.2 Follow the Carrot方法在Follow the Carrot方法中，将Carrot设置为路径段上距离机器人一定距离的点，这个距离通常称之为Look Ahead Distance，跟踪策略就是机器人一直朝着Carrot点的方向前进。Follow the Carrot方法中，最关键的就是Carrot的求取。当得到了Carrot点以后之间连接机器人和Carrot点，连线的方向就是机器人下一步前进的方向。要求取Carrot点首先得找到机器人在路径上的最近点pnear，然后从最近点pnear开始沿着路径段前进长度为Look Ahead Distance的距离，此时得到的点就是我们想要的Carrot点了。机器人到路径段上的距离d的定义如图7所示。图7 机器人到路径段上的距离Fig.7 The distance of robot geting to the path segment图中P表示机器人当前位置的中心点，和分别表示全局最优路径上的一条路径段的两个端点，表示路径段上距离P最近的点，表示机器人到路径段上的距离。d的计算方法如下所示： （8）其中，w表示向量，v表示向量，的计算如式9，表示两点之间的欧式距离。在式8的前两种情况下，分别等于和。 （9）求取机器人到全局最优路径的所有路径段上的距离，并找出其中最小的一个，距离最小的一个对应的就是我们所需要的，如图8所示。图8 找出点Fig.8 Find out the point of 黑点表示机器人，P，Q，R是机器人分别到三条路径段得距离，由于Q到机器人的距离最短，因此在图中情况下，就是Q点。由点求取Carrot点的方法如图9所示。图9 求取Carrot点Fig.9 Work out the point of Carrot 在上图中，从点开始沿着路径段前进Look Ahead Distance的距离就得到了Carrot点。如果Pnear到的距离小于Look Ahead Distance，则进入下一条路径段并前进在中不足的长度。如果一直到最后一条路径段都没有达到Look Ahead Distance的长度，则将Carrot点设置为最后一条路径段的末端端点。表示机器人当前方位角，为Carrot点与机器人中心点之间的连线与X轴的夹角，机器人的转向角，转向角保证在和之间。4.3 Pure Pursuit方法Pure Pursuit是Follow the Carrot的改进版本，它们二者十分相似，主要区别在于Follow the Carrot方法是机器人沿直线趋向于Carrot点，而Pure Pursuit方法是以去圆弧的方式趋向于Carrot点。机器人所走的圆弧是唯一的，它保证机器人在每一时刻的前进方向都是圆弧的切线方向。这种沿圆弧运动的方式使得机器人的转向更加平滑，减少了震荡的可能性。Pure Pursuit算法描述如下：(1)、获取机器人的当前位置。(2)、按照跟Follow the Carrot同样的方式找出Pnear点。(3)、按照和Follow the Carrot同样的方式找到Carrot点。(4)、将Carrot点坐标用机器人的局部坐标系表示，因为计算圆弧的曲率的公式只有在机器人局部坐标系下才有效。机器人局部坐标系的y轴选取为当前前进方向。(5)、计算曲率，其中r是机器人的转向半径。(6)、计算转向角。如图10所示，XY表示全局坐标系，表示机器人局部坐标系，表示Carrot点在局部坐标系下的坐标，VCP表示机器人中心点，O表示圆弧的圆心位置，D为机器人中心点与Carrot点之间的距离。转向角计算公式的推导过程如下： （10） （11） （12）将式11代入12式中展开并代入10式化简得到 （13）由式（13）可得，由于所以可求出 （14） （15）式（14）中的Length表示机器人的长度，至此可以求出转向角了。图10 Pure Pursuit 计算转向角的示意图Fig.10 Calculate the angle of turn with Pure Pursuit5 总结机器人导航问题一直是机器人应用中的热点和难点问题，特别是对于服务类型的机器人来说。虽然全局环境已知的情况下，机器人的路径规划问题已经研究的比较透彻了，但是对于环境部分未知的情况下还没有一个普适性的算法。机器人的一个重要应用就在与服务领域，而服务机器人的工作环境大部分都是不断改变的，因此，研究不确定环境下的导航问题具有十分重要的意义。本文简单介绍了机器人的发展历史，国内外在移动服务机器人方面的研究现状，概括性地描述了移动机器人导航的研究方法。主要介绍了全局地图的构造方法、现有的主要几种全局路径规划方法：自由空间法和构造空间法。着重介绍了自由