（模式识别与智能系统专业论文）基于voronoi图的机器人局部路径规划.pdf

硕士论文基予v o r o n o i 黼韵机嚣几局部路径规划 撼要 路径攫剡最搬嚣a 学酌一个夔要漾遂，目翁豹钎究主嚣分为全黼路径规矧鞫局部 路径翅划。传统於基于v o r o n o i 鬻熊路径烧划主瑟搿予全强落径觌期，它懑予熬子构 囊空翔几辩鞫遣的方法。本文主要研究了藻于v o r o n o i 黼鲢餍帮嫱镬禳划，它铡用赞 感器的信息，采用增攫式构造地网的方法，使之遁用于周部路径规划。 垫子俦感爨兹鼹径蠼划方法中，旱斓搿究较多辫是被称露襄发式熬蔑鲻方法，且 弱鞭予平瓣的情况下。然丽，宜以来入们都不糍诞萌这蝗方法一定眺正确的发现路 径，箕完备灌无法勰决。丽本文所研究勰算法聚掰广义v o m n o i 霸( g v d ) ，杭嚣人酋 先到达g v d 韵一个等蹉边，竣整跟艨这袋边壹捌到达g v d 翡个节点，然蜃分裂 遍历经过这个节点舱边。当所肖节点都没霄衷避历瓣方向埘，算法皱繁。这拿终索条 传捷褥本文溺方法不黼予传统纂于抟港器豹潞镊蕊划方法之赶在予：它赔究备的。 此矫，本文的增餐式构造方法扩展到用于兰维空问，此时地酗的麓本构成为三缭 空阕，的j “义v o r o n o i 圈( g v g ) 。三雏空闯不嗣予 l 曼藤，由于不适邋鼹g v g 的存在， 它的麓杂性大丈增蕊。为托采愆离盼广义v o r o n o i 瀚( h o v g ) 中鹃鬻阶g v g 选以解决 连邂性阉题。文中绘爨了榻应的实验结聚。 关键词：v o z o n o i 胬，局部路径斌划，增徽式构遗 基乎v o r o n o i 图静机器人局部路托规划 a b s t r a c t p a t hp l a n n i n gi so n eo fi m p o r t a n tt o p i c si nr o b o t i c s ，w h i c hc a l lb ec l a s s i f i e di n t og l o b a l a n dl o c a lp a t hp l a n n i n g t h et r a d i t i o n a lp a t hp l a n n i n gb a s e d0 1 1v o r o n o id i a g r a mi su s e df o r g l o b a lp a t hp l m n n i n g w h i c hb e l o n g s t og e o m e t r i cm e * h o db a s e do nc o l r i g u m t i o n a c e 抽 t h i s p a p e r ，l o c a lp a t hp l a r m i n gb a s e do nv o r o n o id i g r a m i ss t u d i e d ，w h i c ha d o p t s s e n s o r b a s e di n f o r m a t i o nt ob u i l dm a pi n c r e m e n t a l l ys oa st ob ea p p l i c a b l et ol o c a lp a t h p l a n n i n g m u c hc u l t e n tw o r ki ns e n s o r - b a s e d p l a l m i n g i s h e u r i s t i ca n d a p p l i e s t o t w o d i m e n s i o n a j s p a c e s n o n e o ft h e s em e t h o d sp o s s e s s e sp r o o f so fc o r r e c t n e s s g u a r a n t e e i n gt h a iap a t hc l i l i lb ef m m d ，s ot h e i rc o m p l e t e n e s sc a l l tb ew e l ls o l v e d t h e a l g o f i t h r ad e s c l b e di nt h i sp a p e ru s e sg e n e r a l i z e dv o r o n o id i a g r m n v 磷f i l s d y ，t h e r o b o ta c c e s s e st oa ne q u i d i s t a n te d g ei nt h eg v d a n dt h e nt r a c e st h ee d g eu n t i lar e a c h e sa n o d ei nt h eg v d ，a tw h i c hp o i n ti tb r a n c h e st oe x p l o r ea l le d g e se m a n a t i n gf r o mt h a tn o d e w h e na l ln o d e sh a v en ou n e x p l o r e dd i r e c t i o n s t h ea l g o r i f l l me n d s t h i st e i n a t i o n p r o p e r t y d i f f e r e n t i a t e st h i s a l g o r i t t n nf r o mo t h e rl o c a lp a f l lp l a n n i n gt e c h n i q u e s ：i t i s c o m p l e t e t h ei n c m m e n t mc o n s t r u c t i o np r o c e d u r ei n t h i sp a p e r c a l lb e e x t e n d e di n t o t h r e e d i m e n s i o n a ls p a c e s ，a n dt h eb a s ec o m p o n e n to ft h em a ps y s t e mi sg e n e r a l i z e d v o r o n o ig r a p h ( g v g ) i nt h r e e t d i m e n s i o n a l ，t h es i t u a t i o ni nt h r e e - d i m e n s i o i z a is p a c e si s m o l ec o m p l e xt h a nt w o d i m e n s i o n a lb e c a u s eo ft h ee x i s t i n go fd i s c o n n e c t e dg v gh i 曲 o r d e rg v ge d g e si nh i g h 。r d “g e n e r a l i z e dv o r o n o ig r a p hf h g v ( 的a r ei n t r o d u c e dt o s o l v et h i sp r o b l e mc o r r e s p o n d i n ge x p e r i m e n lr e s u l t sa r eg i v e no u ti nt h i sp a p e r k e w o r s ：v o r o n o id i a g r a m ，l o c a lp a t hp l a n n i n g ，i n c r e m e n t a lc o n s t r u c t i o n i l 声明 本学位论文是我在导师的指导下取得的研究成果，尽我所知，在本学 位论文中，除了加以标注和致谢的部分外，不包含其他人已经发表或公布 过的研究成果，也不包含我为获得任何教育机构的学位或学历而使用过的 材料。与我一同工作的同事对本学位论文做出的贡献均己在论文中作了明 确的说明。 研究生签名：壑盎利聊莎年5 月i e i 学位论文使用授权声明 南京理工大学有权保存本学位论文的电子和纸质文档，可以借阅或上 网公布本学位论文的部分或全部内容，可以向有关部门或机构送交并授权 其保存、借阅或上网公布本学位论文的部分或全部内容。对于保密论文， 按保密的有关规定和程序处理。 研究生签名 恕永禾4 动p 年g 月咿h 硕士论文 基于v o r o n o i 图的机器人局部路径规划 1 绪论 _ 1 1 移动机器人路径规划技术简介 美国m i t 著名机器人科学家认为自主机器人导航应该回答三个问题【1 1 ，“w h e r e 锄i ? ”，“w h e r e 锄g o i n g ? ”，“h o ws h o u l dig ot h e r e ? ”分别描述了机器人定 位，规划和控制三个问题。随着科技的进步和人类活动领域的拓宽，机器人必将在人 类生产研究重办演着越来越重要的角色闭，作为机器人技术中的核心技术之一，路 径规划在机器人的研究中有着举足轻重的作用【3 】，对其进行全面系统的研究具有特别 重要的理论意义和现实意义。 机器人是一个综合性很强的交叉领域，涉及包括机器学习、模式识别等诸多计算 机方向和包括传感器等非计算机方向的研究领域1 4 l 。随着电子技术的飞速发展，机器 人传感器正走向成熟，计算机的计算能力正得到显著提高，移动机器人的关键技术得 到深入而广泛的研究，移动机器人正一步一步地走向人们生活的各个领域。目前，越 来越多的移动机器人已应用到物料自动传输、危险场合下的自动作业以及军事、服务 业等1 5 j 。这些应用对移动机器人的导航能力提出了更高的要求，在涉及导航技术的诸 多方面中，路径识别和路径跟踪能力是移动机器人智能化水平的重要标志。而限制其 识别能力和路径跟踪速度及精度的关键是控制策略及控制算法的实时性和稳定性旧 移动机器人的自主行为要求其能够在各种环境中为完成给定任务提供一条安全、 高效的运动路径，这正是路径规划技术的任纠7 1 路径规划技术是机器人完成任务的 安全保障，是机器人研究领域中的关键问题之一。 对机器人运动规划的研究是2 0 世纪6 0 年代出现的嘲在过去的几十年中，机 器人路径规划技术已经有了很大的发展。这个领域涉及到许多重要的数学内容，如经 典几何学、拓扑学、代数几何学、代数学和组合学等。在研究过程中，人们逐步提出 了基于各种思想的路径规划方法。 1 2 几种典型路径规划技术介绍 e 路径规划通常按照执行的层次分为全局规划和局部规划两个过程f 3 】。全局规划 的任务是根据预先学习并存储下来的环境信息离线的规划出自起始点至目标点的一 条粗略的路径【9 】。局部规划是在移动机器人的行驶过程中，以传感器探测到的局部 环境信息和机器人自身状态信息为基础，规划出短程内一段无碰撞的局部路径嘲。 全局路径规划的任务是在具有障碍物的环境内，按照一定的评价标准，寻找一 硕士论文 基于v o r o n o i 圈的机器人局部路径规划 条从起始状态到达目标状态的无碰撞路径。它是首先给出移动机器人和作业环境的 描述，然后规划连接两个指定位置之间的移动路径，最后根据所规划的路径给出控 制输出，移动机器人作出行动决策。目前常用的全局路径规划方法有：构形空间法， 栅格法，自由空间法等。 在八十年代初，l o z a n o p e r z 和w e s l e y 提出了路径规划中构形空间【1 0 】的概念。 此后，构形空间成了路径规划的重要工具，其方法是把移动机器人压缩成一个构形点， 而把障碍物拓展成充满空间的构形障碍物；路径规划问题就是寻找在构形空间中构形 点避开构形障碍物的无冲突路径【1 1 1 。但是构形空间法的主要缺点是从移动机器人空 间和障碍物空间产生构形空间并发现无冲突空间需要大量的运算，从而降低控制的实 时性。传统的v o r o n o i 图法就是它的一个典型。其在规划空间中，根据己知的障碍分 布情况，取障碍物的顶点、边界构造出障碍分布的v o r o n o i 图。连接机器人运动的 起始点以及目标点到已经构造好的v o r o n o i 图上【1 2 1 。在此基础上，基于图的搜索算 法可以找到连接起点和终点的最短路径。v o r o n o i 图的优点是生成的路径相对比较安 全，远离障碍，且路径搜索是在曲线或曲面上进行的，路径比较平滑。现在考虑基 于v o r o n o i 图的局部路径规划。目前国内学者对其研究还比较少，c m u 的h o w t ec h o s e t 一直在从事这方面的研究工作【l3 1 。本文利用传感器实时获得的信息，采用增量式构造 广义v o r o n o i 图的方法，使之适用于局部路径规划。 栅格法是由w e h o w d e n 在1 9 6 8 年提出的。该方法将机器人的工作空间分解 为多个简单的区域，一般称为栅格。由这些栅格构成一个显式的连通图，或在搜索过 程中形成隐式的连通图，然后在图上搜索一条从起始栅格到目标栅格的路径，一般来 说，路径只需用栅格的序号表示【1 4 】。按照栅格划分方式又分为确切的和不确切的两种。 美国c m u 的s t c n t z 等人使用一种带边框的四叉树结构来表示环境【1 5 1 ，是对近似栅格 法的一种改进。栅格法中，栅格粒度越小，障碍物的表示会越精确，但同时会占用大 量的存储空间，算法的搜索范围将按指数增加。栅格的粒度太大，规划的路径会很不 精确。所以栅格粒度大小的确定，是栅格法中的主要问题【1 6 】。 自由空间法的基本思想是采用预先定义的基本形状( 如广义锥形，凸多边形等) 构造自由空间，并将自由空间表示为连通图，然后通过对图的搜索来规划路径，其算 法的复杂程度往往与障碍物的个数成正比【5 l 。自由空间的优点是比较灵活，机器人的 起始点和目标点的改变不会造成连通图的重新构造，缺点是不是任何时候都可以获得 最短路径。 局部路径规划实际可理解为“制导路径规划”1 1 7 1 ，它是根据移动机器人周围的局 部环境模型，实时规划满足一定条件的可行路径【3 1 。传统上研究较多的有：人工势场 法，神经网络法，遗传算法等。 人工势场法，作为一种在线避障的方法首先由k h a t i b 提出。其基本思想是在 2 l 硕士论文 基于v o r o n o i 图的机器人局韶路径规划 机器人所处离散环境中的每一点p 赋一个势场值，该值是且标点的引力和障碍物的斥 力的叠加，机器人的路径规划就是从起始点沿着势场下降最快的方向到达目标点。这 种算法的缺点是容易产生局部最小值 神经网络法，它是一种仿效生物神经系统的信息处理方法。它的优点在于它可以 处理难以用模型或者规则描述的过程和系统，对非线性系统具有统一的描述，具有较 强的信息融合能力和系统容错能力，但是神经网络法受学习样本的影响很大，选择代 表性强的样本是十分困难的，而让样本集覆盖整个样本空间是不现实的，因而样本的 选择与设计是一大难题【1 9 1 。 遗传算法，是j h o l l a n d 在2 0 世纪6 0 年代提出的唧。它以自然遗传机制和自然 选择等生物进化理论为基础，构造了随机化搜索算法。它利用选择，交叉和变异来培 养控制机制的计算程序，在某种程度上对生物进化过程做数学方式的模拟。但是遗传 算法运算速度不快，进化众多的规划要占用大量的存储空间和运算时间。同时受个体 编码设计和遗传算子影响较大，如果选择不合适，导致进化缓慢，效果不明显，效力 低下 2 0 , 2 1 1 1 3 基于传感器的路径规划技术 传统的路径规划方法都基于事先给定的环境情况，从而能够根据已知障碍物的分 布给出整个环境的拓扑结构。然而，在现实中，使用传统规划方法的条件很苛刻。在 很多时候，机器人会遇到这些情况：( 1 ) 机器人事先并不知道目标环境的情况；( 2 ) 机器人只知道目标环境的一个粗略的状况；( 3 ) 事先给出的环境描述是不精确的甚至 是错误的；( 4 ) 目标环境还可能包含运动着的障碍物，机器人需要处理各种突发事件。 于是，基于传感器的路径规划方法被提了出来。 基于传感器的路径规划方法中，早期发展较多的是被称作启发式的规划方法四】。 一个典型的启发式的方法是基于行为的探索方法，机器人被设定一些简单行为集团】， 例如墙跟踪 2 4 】。这些简单行为的组合便可实现复杂的行为，例如探索。这种方法的一 个扩展叫做序列化【2 5 1 。基于点的探索也很普遍。被规划的环境被描述成具有一定分辨 率的点 2 0 9 。然而，这些启发式的方法不能从理论上保证能找到路径，一直以来人们都 不能证明这些方法一定能正确的发现路径，甚至不能保证在有限时间内确定这样的算 法在解决特定问题中失效。 当然，也存在许多非启发式的方法，然而许多非启发式的方法只是适用于二维的 空间1 2 ”如l u m e l s k y 的“b u g s ”算法是第一个能在平面内正确工作的基于传感器的 方法哪刀j 。但是这个方法要求自始至终明确目的点的位置，这样，机器人就不能搜寻 一个给定除位置以外的其他特征的目标。这个算法的另一个局限是它仅仅返回从起点 硕士论文基于v o r o n o i 图的机器人局部路径规划 到终点的路径，却不能返回这个环境的拓扑结构，从而不能给迸一步的探索提供帮助。 于是，路径规划近期的发展着重在于创建目标环境的“路标”1 3 0 j 一个路标系 统是一系列一维曲线的结合，并标明目标环境拓扑性质和几何性质的数据结构。路标 系统具有以下性质：可达性，可离性，以及连通性。这些性质使得机器人通过如下方 法可以构造出连接机器人自由空间中的任意两点的一条路径。首先，能够避开障碍物 到达路标系统，遍历路标系统到达目标点的附近，然后找到一条从路标系统上的一点 到目标点的无障碍路径“路标”系统的实现有不少文献对其进行专门研究。c a n n y 和l i m 的投机规划方法】就是“路标系统”一个例子。r i m o n 把该思想应用于基于传 感器的路径规划1 3 2 1 ，然而，他的方法给出的路标却不能保证是连通的。另外，从实用 的角度看，r i m o n 的方法还有两个缺陷：第一，要构造这样的路标系统，必须配备有 特殊的传感器，专门用于探测有用的“关键点”和“鞍点”，而原文也没有详细给出 实现这样的传感器的方法；第二，文中并没有详细地给出根据传感器信息高效的构造 这样的健壮的路标系统的边的方法。对于基于传感器的路径规划，一种可行的方法就 是将一个很可能正确的或“完备”的典型的路径规划策略改编为基于传感器的方法。 路标系统就是这些完备的典型方法中的一种。它的应用例子包括c a n n y 和l i m 的机 会路径规划以及k u i p e r s 和b y a n 的路标系统策略p 3 1 。 l 1 4 本文的工作介绍和结构安排 本文采用基于v o r o n o i 图的方法，它结合传感器实时获得的信息，采用增量式构 造的方法进行路径规划。其最原始的形式就是二维平面上的v o r o n o i 图( v o r o n o i d i a g r a m , 简写作、，d ) ，即平面中到两个点距离相等的点的集合【蚓，由于环境中的障 碍物都是多边形，故引入了广义v o r o n o i 图g v d ( g e n e r a l i z e dv o r o n o id i a g r a m ) 。 由于在机器人路径规划问题中，环境的中的障碍物可以抽象为凸多边形，如果能够构 造出它们的g v d ，那么机器人沿着该g v d 上的边移动，必定是远离障碍物的。第一个 运用g v d 进行路径规划的是文献 3 5 1 。之后，在文献 3 6 1 中，用g v d 方法研究了盘形机 器人在平面中的运动。但他们的方法需要事先知道环境的几何结构，而且他们的方法 也难以扩展到高维空间中去。本文主要研究了基于v o r o n o i 图的局部路径规划，它利 用传感器的信息，采用增量式构造地图的方法，使之适用于局部路径规划。在平面的 情况下，本文给出了构造该g v d 图的算法，该算法将机器人分为接近状态，探索状态， 交汇点状态，边界点状态以及回溯状态。本文将详细介绍这几个状态的含义以及各个 状态下机器人的控制算法和决策策略。同时在模拟实验中，我们发现可以去除某些多 余的边以简化机器人的跟踪过程，并且不会影响跟踪结果。于是，我们提出了简广义 v o r o n o i 图r g v d 。在r g v d 中，我们对交汇点状态的控制算法和边界点测试条件进行 4 硕士论文基于v o r o n o i 图的机器人局部路径规划 了改进在模拟系统上进行了仿真实验，系统由三个模块；实验平台、模拟环境和机 器入。其中实验平台的职责是加载环境，安置机器人并将环境和机器人的交互建立起 来；模拟环境的职责是定位机器人的位置，为机器人提供钡4 距接口；机器人职责是提 供路径规划算法，这个模块可以动态加载，从而可以在同一平台上测试多种规划算法。 实验结果充分说明了该算法的完备性。此外，本文的增量式构造方法扩展到用于三维 空间，此时地图的基本构成为三维空间中的广义v o r o n o i 图( g v g ) 三维空间不同于 平面，由于不连通的g v g 的存在，它的复杂性大大增加。为此采用高阶广义v o r o n o i 图( h g v g ) 中的高阶g v g 边以解决连通性问题。 基于传感器的路径规划的相当一部分工作是在处理与运动相关的传感信息f 3 7 l ，使 用增量式构造过程，它可以自动地决定何时感观何时移动1 3 3 捌这个算法使用距离信 息通过计算构造g v d 边。机器人每移动一小步，都可能使得机器人偏离g v d 边，我们 采用修正的方法使机器人重新回到g v d 边上。因为传感器可以提供距离数据，计算过 程可以很容易地使用传感数据来产生g v d 边的一部分。然后机器人沿着这部分边移 动，不断重复这个过程以得到接下来的部分。因此，这个增量式构造过程，自动地与 机器人的移动交汇进行。机器人跟踪一条边直到它到达g v d 的一个节点，然后分别遍 历经过这个节点的边。当所有地节点都没有未遍历的方向时，算法结束。这个结束条 件使得该基于传感器的构造过程不同于其他具备路径规划技术之处在于：它是完备 的。且时间复杂度低，实时性很好。并且生成的路径远离障碍物，生成的路经对机器 人来说是可执行的，合理性好。 本文的结构安排如下： 第l 章简单介绍了移动机器人路径规划技术和本文的主要研究内容。 第2 章介绍本文描述的局部路径规划方法的数学工具v o r o n i o 图和广义v o r o n o i 图的概念，从平面上v d 扩展到g v d ，再扩展到高维空间中的g v g 以及h g v g 。 第3 章阐述了平面情况下g v d 图的构造算法，同时针对多余v o r o n o i 边，提出了 简广义v o r o n o i 图，本章的最后展示了模拟实验的结果。 第4 章是研究三维空间基于广义v o r o n o i 图的机器人局部路径规划算法，其基本 思想仍然采用增量式构造的方法，但是由于空间的g v g 可能不连通，我们引入了高阶 g v g 边，同时描述了相应的不连通结构和连接策略，并给出了相应的实验结果。 最后总结了本文所作的工作，并对以后工作进行了展望。 硕士论文基于v o r o n o i 圈的机器人局部路径规划 2y o r o n o i 图和广义v o r o n o i 图简介 2 1 引言 v o r o n o i 图是一个关于空间划分的基础数据结构【如i ，在求解点集或其他几何对象 与距离有关的问题时有重要作用【4 i 】。早在1 8 5 0 年d i r i c h l e t 及1 9 0 8 年v o r o n o i 在其 论文中都讨论过v o r o n o i 图的概念【l 即1 0 0 年来，有很多文献对其进行了研究，详细 描述了它的扩展和各种应用。随着计算机技术的普及和发展，y o r o n o i 图的应用范围 也在不断扩大。在不同的领域，v o r o n o i 图有时也被称为t h i e s s e n 多边形、d i r i c h r i t 网格、或w i g n e r s e i t z 区域等【1 3 1 在y o r o n o i 图中，被用来划分空间的各个基本图 形元素一般被称为站点。最基本的v o r o n o i 图是以平面点集p 为站点的v o r o n o i 图， 它将平面划分成凸多边形形状的v o r o n o i 区域，p 中的每个站点p ，对应一个这样的 区域v ，使得v 内的任何点距离p ，比距离其他站点近。v o r o n o i 图的定义可以推广到 三维或高维，也可以推广到二阶或高阶( 以两个站点或多个站点为一组划分邻近区 域) ，也可以进一步推广到站点包括线段或多边形的广义v o r o n o i 。y o r o n o i 图在此领 域的作用很早就被人们发现：如果障碍物可以近似成质点，那么机器人沿着障碍物的 v o r o n o i 图的边行走是最安全的。如果障碍物不能用质点来近似，那么就应该应用广 义v o r o n o i 图( 站点为线段、多边形或多面体等) 。 、r o r o n o i 的应用非常广泛，在文献【l 习中详细描述了v o r o n o i 图在各个领域的应用， 包括几何形体重构领域，计算机图形学、图像处理与模式识别领域，物理、化学和分 子生物学领域以及机器人运动规划领域等。虽然v o r o n o i 图所适用的各个专业领域从 专业角度千差万别，都把生成各类v o r o n o i 图作为基础步骤，从v o r o n o i 图所发挥的作 用角度，可以归结为如下3 个方面：( 1 ) 把v o r o n o i 图作为表示各种元素之间关系的一 个结构，通过这个结构可以提取出重要信息；( 2 ) 把v o r o n o i 图作为一个辅助数据结构， 通过这个数据结构可以完成许多关于物体形态或邻近关系的计算任务；( 3 ) 把v o r o n o i 图作为提高某些几何算法运算速度的重要手段f 4 2 1 一般来说，二维的v o r o n o i 图可以 在o ( n i o g 行) 时间内获得，三维的v 0 r o n o i 图可以在o ( 拧2 1 时间内获得 4 2 1 。y o r o n o i 图的 性质决定了它与许多其他几何结构具有内在关系，通过v o r o n o i 图许多几何算法可以 得到快速运算。 本文研究的机器人局部路径规划之所以采用基于v o r o n o i 图的方法，正是因为 v o r o n o i 图在表示空间关系的独特优势，闰浩文和郭仁忠在其论文中对其理论依据有 严格的论述j 。 6 硕士论文基于v o m n o i 图的机器人局部路径规划 2 2v o r o n o i 图 。 l 1 l 2 p ：- b p 图2 2 1 矿( 丑) ，y ( 最) 的图示 关于平面点集的v o r o n o i 图其最基本的形式如图2 2 1 ，只，只是平面上的两个 点，称为站点，l 是线段只最的垂直平分线，即等距线，l 将平面分成两部分厶和上2 ， 位于上l 内的点碍具有这样的性质：d ( g ，置) 0 ，i l ( v y f ( 妒， 宰) ) “| l o ，g 满 足 v g ( ，) 非奇异，即l ( v g ( ) ，) ) “0 s 坛，y 易( y ) ，i l v g ( 工) 一v g ( y ) 悟厂卜y 9 ，脚s 那么对于每一个少彩( ) ，) ，迭代式少“= 少一( v g ( 少) ) 1 g ( 少) 满足 y 6 e 易( y ) ， y 6 平方收敛于y ， 叫少“一y m 少可2 ，其中口= 1 一俐 形 在我们要处理的问题中，迭代公式是 y k + l = y k 一( a ，g i ( h 她州一g l ( x + 如矿) 迭代的最终结果是点序列 卜+ a x ，广) 将收敛于修正轴与g v d 边的交点上然而，在 实现过程中，我们发现a ，g l b + 缸，y ) 在g l ( 南_ y ) 的解析式未知的情况下是难以获得 的。于是我们改用了双点弦截法，即将偏导数替换为 g l ( x + 缸，广) 一g l ( x + 缸，) 了与芦一 问题得以解决，但双点弦截法的收敛速度降为p = ( 1 + 5 ) 2 实际计算中，上述迭代无需无限进行下去。事实上，由于探测器存在误差，迭代 精度过高也没有意义。而上述收敛定理中，p 一般是很大的，在一般的探测精度误差， 舍入误差下，这种方法的鲁棒性还是很强的。一般的，设探测器误差的方差为6 ，则 迭代至 | i g l ( x - i - 缸，) 一g l ( x + h a ，4 ) i f 万 ( 3 2 2 3 ) 即可，即保证计算误差与探测器的误差在同一个数量级。最后，我们只需要将局部坐 标系转换为全局坐标系就得到了步进后的位置。 3 2 3 交汇点探测和交汇点处的决策 按3 2 2 的方法，机器人沿着g v d 边前进最终会到达g v d 边的交汇点或边界点。 1 7 硕士论文基于v o r o n o i 图的机器人局部路径规划 这一节我们讨论交汇点的情况。 1 交汇点的探测 要求机器人在前进一修正过程中恰好停在交汇点上几乎是不可能的。即使运气足 够好，在探测器存在误差的情况下，机器人也不能断定自己就是位于交汇点上。然而， 我们确实不需要在正好位于交汇点上时才能做此判断，而可以用最近物体突变法进行 探测。 如图3 2 3 1 ，机器人沿着厶前进时，它的最近障碍是e 和g ，指向最近障碍物 的单位向量巧和k 在前进过程中是渐变的，前一个位置的向量k ( x ，y ) 与前进一步后 的向量k ( x + a x ，y + a y ) 的内积是一个很接近1 的数。然而，当机器人越过交汇点时， 他突然发现：或者原来的最近障碍物g 变成了c 。( 走到了厶上) ，而向量k 突变为一； 或者发现q 变成了c ，( 走到了厶上) ，而向量k 突变为形。无论是什么情况，先前位 置的向量和前进一步后的向量所得的内积必然与1 相差很远。图3 2 3 i 中交汇点处， k 与只相差1 8 0 。，内积是一l ；而k 与一相差9 0 0 ，内积是0 。这样明显突变对检测 交汇点是很有利的。 图3 2 3 1 交汇点探测 2 交汇点处的决策 首先，机器人要判断它是否访问过这个交汇点。探测的方法可以是一个坐标匹配。 然而，实际情况中，机器人的定位系统因累积误差而使硬性坐标匹配失败。故可以引 入交汇点处的环境“签名”，诸如各最近点问夹角等特征；也可以相邻点联合匹配。 这是一个需进一步研究的问题，尚无鲁棒性好的方法，尤其在各处都“很相像”的高 度对称环境下，上面的各种方法都很难奏效。在模拟系统中，由于软件的累积误差相 当小，我们没有考虑这样的问题，仅仅使用了坐标匹配的方法。下面针对当前点是否 被访问过分别讨论。 i ) 交汇点未访问 由于是一个新的顶点，我们需要提取交汇于这个顶点的各条g v d 边的初始方向 ( 即各g v d 边在交汇点处的切向量) 。具体算法如下：得到最近的n 个等距点，并设指 1 8 硕士论文摹于v o r o n o i 图的机器人局部路径规划 向这n 个最近点的向量v 1 ，v 2 ，计算每两个向量的平分线。 然而有些平分线向量v 其实并不是我们所需要的，而是所需向量的逆向量。如图 3 1 4 ，鸭和v 1 的角平分线指向了，然而我们需要的向量应该指离心。为解决这个 问题，我们将这个向量v 与e 取内积( i = 1 ，2 ，3 n ，i k ，k + 1 ) 。如果v i ，“哆) ( v ，吃) ， 机器人将沿着v 方向离开这个交汇点，否则，它将沿着一v 方向离开。提取这些信息之 后，我们将该新节点加入到节点队列。 交汇点 图3 2 3 2 平分线不是出射方向图3 2 3 3 距离很近的交汇点 由于机器人是从另一个节点h 到达这个节点v 。的，我们可以确定当前节点的某一 条边上的邻接点是前一个点e 。 然而，如果当前交汇点距次近障碍物的距离比到最近障碍物的距离大很少时，则 在当前交汇点附近必然还有另一个交汇点，如图3 2 3 3 的m 。和j 】l 如。如果让这 样的交汇点同时存在，机器人将来回到这些交汇点中的任何一个时都不能确定自己究 竟在哪个点上，而且由于距离很近，误差足以让任两个点的实际位置和计算出的位置 颠倒，从而造成路标系统的错误。针对这个问题，我们采用了合并这样的交汇点的办 法。具体做法是，设到某个交汇点到距离最近的障碍物为c l ，g ，g ，距离为以，如 果c 4 到这个障碍物距离小于以+ 占( 6 是一个较小的正常量) ，我们认为e 也是距这 个交汇点最近的障碍物之一，这样，到c 2 ，g ，g 等距的点也被并入了当前的交汇 点。同时，由于这个交汇点实际包含了两个点，在下次机器人访问这个点时，坐标匹 配的范围也应扩大。 2 ) 交汇点己访问 如果当前交汇点已被访问过，那么他必然在节点队列中存在，我们要做的就是确 定机器人是从哪条g v d 边进入到这个交汇点的。开始，我们的算法是最大内积匹配算 法，即将机器人进入该节点前的最后一次步进的向量v 的逆向量一v 与该节点在1 ) 中 计算出的各条g v d 边的切向量作内积，取最大者作为机器人的进入边。 1 9 硕士论文 基于v o r o n o i 图的机器人局部路径规划 但在实际模拟中由于存在探测器误差，3 2 2 节的迭代也不精确的，进入该节点 前的最后一次步进向量可以看作是一个随机变量，它的数学期望是该v o r o n o i 边在该 交汇点处的切向量，但存在着随机波动在这个向量的方差较大的情况下，它与相应 的g v d 边的切向量的夹角可能很大，如图3 2 2 4 。我们的改进算法使用最近n 次步 进后位置的线性拟合。在n 的选取上，如果n 过小，则取得的向量的方差仍旧很大， 仍可能造成匹配错误；如果r l 过大，则取得的向量实际上已不是g v d 在端点处的切向 量。在模拟中，我们取n = 5 。 j 一一4 、一一一、7 七文汇点 图3 2 3 4 入射向量方差大 当机器人记录下这些信息后，它要选择一条g v d 边离开当前交汇点。如果经过当 前点所有边都己探测过，机器人便进入回溯状态( 3 2 5 节) 。 3 2 4 边界点状态 边界点的探测是很显然的，只要机器人步进到它发现最近障碍物的距离足够小就 可以断定。 边界点同样需要记录到节点队列中，由于边界点只有一条边与之相连，故进行向 量的匹配没有必要，只需记录与当前边界点邻接的是哪一个节点即可。记录下信息之 后，由于没有其它未探索边，机器人转入回溯状态( 3 2 5 节) ，表现为机器人反转方 向，按原路离开。 3 2 5 回溯状态 在无未探索边的节点或是边界点处，机器人便会转入回溯状态。 回溯第一步是确定回溯路径。我们的回溯策略是基于贪心的，即寻找最近的一个 存在未探索边的节点。使用的算法是d i j k s t r a 算法的变形。具体描述如下： 步骤l ：t _ 当前节点 ，l ( 当前节点) = 0 ，p = 所有已访问节点 一t ，对p 中每 一个顶点，令1 ( t ) = w ( 当前节点，t ) ) 。 硕士论文基于v o m n o i 图的机器人局部路径规划 步骤2 ：膏= a 玛m i l l ( ，( ，) ) ，t p 。如果x 包含未探索边，则算法结束，x 就是回溯的 终点。 步骤3 ：t = t u z ，p 。= 尸一 x ； 如果p 。= o ，所有节边都已探索过，算法结束。 步骤4 ：对任一f p ，计算，。( ，) = m i l l ，( r ) ，( x ) + ( 工，f ) ； 用r 代替丁，户。代替p ，转到步骤2 。 实际计算中，由于需要记录路径，在步骤4 重新计算z ( f ) 时，如果 ，( f ) ，( x ) + 形( f ) ，表示从顶点经x 到达t 点路径比原有的路径更短，故我们记录t 的前驱节点v r e v ( t ) = z 。当最终找到存在未探索边的节点时，按公式 i = p r e y ( v , ) 迭代每一个前驱点并压栈，直到p ，+ l 等于当前节点。此时从栈顶到栈底的所有元素就 构成了我们的回溯路径。随后的回溯从当前点开始，采用的步进算法，每当遇到个 节点，便弹出栈顶元素，选择所在节点的通向新栈顶的边继续步进，直至栈为空。此 时机器人便回到了一个包含未探索边的节点，再次转入探索状态。然而如果找不到包 含未探索边的节点，那么机器人便会宣告寻找且标点失败。 ， 3 2 6 离开状态 在实际情况中，目的地可能并不是一个给定了坐标的点，而是一个有着某种机器 人可以探测到的特征的物体。在探索过程中，一旦这样的物体进入到机器人的探测范 围，机器人就可以脱离g v d 边沿直线接近目标物体，从而完成规划。 然而，我们能保证机器人在探索过程中一定能“看到”目标物体吗? 下面将证明 这一点。设目标点是昂( 目标点不位于任何障碍物内部) ，过r 任作一条直线l 。由于 环境是有界的，l 必然与两个障碍物e 和c ，相交，交点分别为只，只。由于 只e 矿( c 1 ，p ，v c ，) ，直线必然与某条v o r o n o i 边相交。即必然在v o r o n o i 边上存在 一点q ，q 点处可以探测到目标点名。 3 3 多余v o r o n o i 边问题和简广义v o r o n o i 算法 在进行模拟时，我们发现在环境边界和障碍物凹陷的地方，g v d 边有许多分叉， 见图3 3 1 ( a ) 。这些分叉对路径规划并没有用处，因为他们都只是连通交汇点和一个 边界点；而且这样的边的遍历对机器人也是额外的负担。想象一个正i i 边形大厅， v o r o n o i 机器人进入大厅后会从中心开始“仔细察看”每一个角落，最后才“安心离 开”，见图3 3 1 ( b ) 。如果大厅是圆形的，机器人几乎无法完成对这个大厅的探索， 2 1 硕士论文 基于v o r o n o i 图的机器人局部路径规划 这里有无数个最近障碍物! 其中的原因是：我们没有充分利用探测器得到的环境信息， 仅仅利用了其中最近障碍物的几个点的信息如果我们将环境的非最近点也加以考 虑，那么这样的分叉就可以去除。据此，我们使用简广义v o r o n o i 虱的概念，并使机 器人按着这样的路标系统进行探索。这样的算法我们称之为简广义v o r o n o i 算法。 ( a ) 3 3 1 简广义v o r o n o i 图 ( b ) 图3 3 1 传统v o r o n o i 图的缺陷 我们将广义v o r o n o i 图中删去所有与边界点和相关联的边所留下的子集称为简广 义v o r o n o i 图( r e d u c e dg e n e r a l i z e dv o r o n o id i a g r a m ，r g 、d ) ，见图3 3 1 1 圆国 图3 3 1 1g v d 和r g 、，d 对比 注意到r g v d 仍旧是连通的，因为它只是消去了边界点，所以r g 、，d 仍旧可以作为机 器人探索空间的路标系统。 3 3 2 简广义v o r o n o i 机器人的交汇点状态 一般的，如果某个方向的环境对我都是可见的，那么我没有必要在这个方向上做 进一步探索，因为假如目标在这个方向上，那么我就已经“看到”了。这个方向上的 所有边界点以及其他非边界点的节点都可以去掉。那么机器人如何知道环境对它使全 硕士论文 基于v o r o n o i 圈的机嚣人局部路径规划 部可见的昵? 下面给出一个充分条件。 假设机器人位于p ，定义p 点处的环境函数e ：r r ，e ( o ；p ) 表示p 点向0 方向引 一条射线，射线遇到的第一个障碍物时经过的距离。并定于p 点处的探测函数s - r r s ( p ；p ) = e 竺p e ( 主嚣：窆 其中咒是机器人的最大探测半径显然e ( 护) 和s ( 目) 都以2 石为周期如果s ( 口；p ) 在 【e l , 岛】上是连续的，那么暖，岛】区间内的空间对机器人都是可见的如图3 3 2 1 ， 深灰色为障碍，白色为机器人可见的区域，浅灰色为机器人不可见区域。在箭头所指 的方向上，s ( 口；p ) 从上部和边界的交点处跳变到中部和边界的切点处。 图3 3 2 1 探测函数连续性和环境可见性 根据上述命题，当机器人位于交汇点时，如果它发现相邻的最近障碍物g 和气之 间的探测函数是连续的，那么c 和c 0 的平分线方向就没有必要进行探索了。 然而在实际环境中，探测函数的连续性是很难判断的，只能用差分的办法进行近 似的判定。设探测器在心，岛】上探测到的距离是s ) ，s ( 西) ，s ( 丸) ，5 ( 以。) ，s ( 岛) ， 如果记日= ，岛= 吮，v i ( o f 珂) ，有p ( 谚。) 一s 似) l o p e l 3 打开某个具体实例后界面显示，它展示 了当前的环境，以多边形表示障碍物。由于其为非凸多边形，根据我们的算法， 会将其分割按照两个障碍物处理。这点表明了本文的基于v o r o n o i 图的路径规划 算法可以处理非凸多边形的障碍物问题。界面上的s 表示机器人的起始位置，e 表示要寻找的目标。该实例中的目标点被安置在障碍物上，也就是说在自由空间 中机器人必定宣告探索失败。机器人在探索