（精密仪器及机械专业论文）全向移动足球机器人的运动控制及路径规划(精密仪器及机械专业优秀论文).pdf

摘要 摘要 本文针对存在参数不确定性及不确定性扰动的全向移动足球机器人模型，提 出具有方差约束和闭环极点约束的鲁棒控制方法。该方法对状态协方差进行最小 化优化处理，使控制系统更能有效地抑制扰动的影响，同时约束系统闭环极点区 域，使控制系统具有良好的动态特性，最后还对控制器的能量进行最小化优化。 本文基于线性矩阵不等式( l m i ) 方法，用m a t l a b 的求解器得出上述优化问题的 结果，进而得出鲁棒控制器，通过仿真说明了此方法具有良好的控制效果。 本文同时提出了一种适用于为快速移动的机器人做动态路经规划的方法，称 为改进型的v f h 木方法。该算法充分考虑了障碍物的速度，在生成机器人的极坐标 柱状图时加入了障碍物的速度因素，同时在生成搜索树不同深度的节点时，也预 估了障碍物可能到达的新位置，该方法规划的路径更为安全可靠。同时该方法在 生成搜索树时，根据环境中障碍物密度的不同，能自动调整搜索步长，故其不需 要很多的搜索步数就可以达到良好的效果，这也加快了算法的速度。 关键词：线性矩阵不等式，方差约束，鲁棒性，改进型v f h ，i c ，动态路径规划。 a b s t r a c t a b s t r a c t t bd e a lw i t hp a r a m e t r i cu n c e r t a i n t i e sa 1 1 du n c e n a i nd i s t u l 七a n c et ot l l e o m n i d i r e c t i o n a ls o c c e rr o b o tm o d e l ，t h i sp a p e rr e c o m m e n d sar o b u s tc o n t r o l m e t h o d 讯t hv 撕a n c ec o n s t r a i m sa i l db o u n d e dc l o s e d l o o pp o l e sr e g i o n t i l i s m e m o dm i n i m i z e st h ec o v a r i a i l c eo fs t a t e s ，s om ec o n t r o ls y s t e mc a l li n h i b i tm e i m p a c to fd i s t u r b a n c em o r ee 伍c i e n t ，a n dt oa c h i e v eg o o dd y n a m i cc h a r a c t e r i s t i c s ， t l l i sm e t h o da l s ob o u n d sc l o s e d - l o o pp o l e sr e g i o no ft h es y s t e m ，t h e nm i n i m i z e s e n e 唱yc o s to ft l l ec o n t r o l l e r o nt h eb a s i so fl m ia p p r o a c h ，t h e s eo p t i m i z a t i o n p r o b l e m sa r es o l v e du s i n gm a t l a bs o l v e r ，a j l dar o b u s tc o n t r o l l e ri so b t a i n e d t h e s i m u l a t i o nr e s u l ts h o w st 1 1 ee m c i e n c yo ft h em e t h o d t h i sp a p e ra l s op u tf o n a r dan e wm e t h o do fd y n a m i cp a t hp l a n n i n gf o rf a s t m o v i n gr o b o t _ i m p r o v e dv f h 木m e t h o d t h em e t h o dp r o p o s e db yt h i sp a p e r 如l l y c o n s i d e r e dt h ev e l o c i t yo fo b s t a c l e s ，n o to n l yp u tt h ev e l o c i t yo fo b s t a c l e st om e r o b o tp o l a rc o o r d i n a t eh i s t o g r 锄，w h e nc r e a t i n gn o d ew i t hd i 虢r e n td e p t hf o r s e a r c h i n gn e e ，i ta l s oe s t i m a t e dp o s s i b l en e wp o s i t i o n sf o rt h eo b s t a c l e s ，p a t l l p l a n e db yt m sm e t h o di sm o r eo fs a f e t ya n dr e l i a b l e a tt h es 锄et i m e ，w h e ni t 。s c r e a t i n gt h es e a r c h i n gt r e e ，i tc a na d j u s ts e a r c h i n gs t e pd i s t a n c ea c c o r d i n gt o o b s t a c l ed e n s i t yi nt h ee n v i r o m i l e n t ，c a ng e tag o o dr e s u l t 谢t h o u tt o om a n y s e a r c h i n gs t e p s ，a n da l s os p e e d su pt h ea l g o r i n l m k e y w o r d s ：l i n e a rm a t r i xi n e q u a l i t y v a r i a n c ec o n s t r a i n t ，r o b u s t n e s s ，i m p r o v e d v f h 木，d y n a m i cp a mp l a n n i n g 论文原创i 生和授权使用声明 本人声明所呈交的学位论文，是本人在导师指导下进行研究工作 所取得的成果。除已特别加以标注和致谢的地方外，论文中不包含任 何他人已经发表或撰写过的研究成果。与我一同工作的同志对本研究 所做的贡献均已在论文中作了明确的说明。 本人授权中国科学技术大学拥有学位论文的部分使用权，即：学 校有权按有关规定向国家有关部门或机构送交论文的复印件和电子 版，允许论文被查阅和借阅，可以将学位论文编入有关数据库进行检 索，可以采用影印、缩印或扫描等复制手段保存、汇编学位论文。 保密的学位论文在解密后也遵守此规定。 作者签名： 年月 日 第一章引言 第一章引言 1 1 小型组足球机器人系统简介 1 1 1 系统概述 r o b o c u p 小型组机器人足球赛是“机器人足球世界杯”( r o b o c u p ) 的组成部 分之一。因为r 0 b o c u p 的长期目标是建立能与人类足球队相抗衡的两腿机器人 足球队，所以作为一个重要的过渡，小型组机器人足球赛就有着特殊的重要意义。 小型组机器人足球的研究还为计算机、自动控制、传感、无线通讯、精密机械， 仿生材料，多主体系统中的合作、决策、实时规划和机器学习等众多学科提供了 一个良好的研究背景。r o b o c u p 小型组机器人足球赛实际上是高技术的对抗，它 从一个侧面反映了一个国家精密机械、信息与自动化领域基础研究和高技术发展 的水平。 小型组足球机器人系统又分为半自主和全自主两种形式，比赛队双方各有5 名队员，使用桔黄色标准高尔夫球，比赛在一块2 9 水2 4 m 2 的毡子场地上进行。 分为上下两节，各十分钟，中间休息十分钟，总共3 0 分钟。机器人必须自主控 制，系统由摄像头，图像处理系统，计算机操作系统，无线通信系统，m p u 系统， 决策系统，机器人姿态控制系统及机器人车体等组成。 图( 1 1 1 ) 小型组足球机器人比赛系统 f i g ( 1 1 1 ) f - 1 8 0c o m p e t i t i o ns y s t e m 第一章引言 足球机器人更新换代，现在已普遍采用全向移动结构代替以往的两轮结构。 全向移动机器人不具有非完整性约束，它在一个平面上具有全自由度，即可以朝 各个方向运动，同时可以自由转动。一种典型的全向移动机构如图( 1 1 2 ) 所 示的小型组足球机器人的行走机构。该种行走机构由若干个沿圆周分布的十字正 交轮组成，每个车轮都由独立的驱动电机控制，所有车轮的驱动力矢量和构成了 小车的驱动力，同理，所有车轮的速度的矢量和构成了小车的整体速度( 如果车 轮不存在打滑现象) 。 图( 1 1 2 ) 全向移动足球机器人行走机构 f i g ( 1 1 2 ) m o t i o ns t m c t u r eo fo m n i - d i r e c t i o n a l r o b o t s 1 1 2 主要模块及功能 该机器人系统主要包含以下三个模块： 1 ) 决策模块 决策模块主要是指机器人的a i 模块，它就是机器人系统的大脑。它要做的 工作是根据球场上的形势指挥各个机器人执行相应的动作指令。一般来说决策模 块是运行在p c 机上的一套程序。 2 ) 执行模块 执行模块是指机器人的行为控制模块，主要包括机器人的运动控制模块和路 径规划模块。运动控制模块负责对机器人的位置、速度及加速度进行控制，而路 径规划模块则决定沿着什么样的路径到达目的地。打个通俗的比方，前者就好比 教小孩怎么站立行走，而后者则是在他能站立行走之后教他怎么朝某个特定地点 走过去。 执行模块接收决策模块发出的指令并执行这些指令。对于无线系统，执行系 2 第一章引言 统往往也捆绑了一套能和决策模块通信的无线通信系统。 3 ) 反馈模块 反馈模块是指机器人系统的视觉模块，它是决策模块和执行模块联系的纽 带。在小型组足球机器人系统中，视觉模块由两个摄像头和相应的图像处理程序 组成。视觉模块能告诉决策模块场上的形势，比如各个机器人的速度和位置，球 的速度和位置等信息，以便决策模块能及时的根据场上形势做出战略部署，同时 通过反馈模块，决策模块也能监视到执行模块对命令的执行情况。反馈模块是机 器人闭环系统中很重要的一环，直接关系到系统的可控性及稳定性等性能。在小 型组足球机器人系统中，视觉系统每秒大概处理2 5 帧图像，以便决策系统能及 时了解到场上的形势。 图( 1 1 3 ) 反映了上述三个模块的关系 图( 1 1 3 ) 机器人系统中各个模块之间的关系 f i g ( 1 1 3 ) r e l a t i o n s h i p 锄o n gm o d u l e so f r o b o ts y s t e m 1 2移动机器人运动控制的发展现状 当前很多机器人的运动控制都是基于运动学的，而没有考虑动力学特性。 当机器人系统的机械惯量较小，互相耦合较小( 比如有较多移动副) ，驱动器具有 较大功率，能快速动作；而机器人运作速度不高，精度要求较低时，可以采用运 动学控制的办法。运动学控制的优点是：控制系统结构简单，容易实时控制，但 该控制方法一般不考虑机器人的动力学特性，因此在一些动力学特性比较明显的 第一章引言 1 )全局路径规划方法 全局路径规划的方法有：拓扑法、可视图法和栅格法。拓扑法是将规划空间 分割成具有拓扑特征子空间，并建立拓扑网络，在拓扑网络上寻找起始点到目标 点的拓扑路径，最终由拓扑路径求出几何路径。其缺点是建立拓扑网络的过程相 当复杂，特别在增加障碍物时如何有效地修正已经存在的拓扑网络及如何提高图 形速度是有待解决的问题。 可视图法视机器人为一点，将机器人、目标点和多边形障碍物的各顶点进行 组合连接，要求机器人和障碍物各顶点之间、目标点和障碍物各项点之间以及各 障碍物顶点与顶点之间的连线，均不能穿越障碍物，即直线是可视的。搜索最优 路径的问题就转化为从起始点到目标点经过这些可视直线的最短距离问题。运用 优化算法，可删除一些不必要的连线以简化视图，缩短搜索时问。该法能够求得 最短路径，但假设机器人的尺寸大小忽略不计，使得机器人通过障碍物顶点时离 障碍物太近甚至接触并且搜索时间长。 栅格法是由w 。e h o w d e n 在1 9 6 8 年提出的。栅格法将机器人工作环境分解成 一系列具有二值信息的网格单元，工作空间中障碍物的位置和大小一致，并且在 机器人运动过程中，障碍物的位置和大小不发生变化。用尺寸相同的栅格对机器 人的二维工作空间进行划分，栅格的大小以机器人自身的尺寸为准。若某个栅格 范围内不含任何障碍物，则称此栅格为自由栅格；反之，称为障碍栅格。自由空 间和障碍物均可表示为栅格块的集成。栅格的标识方法有两种：直角坐标法和序 号法。多采用四叉树或八叉树表示工作环境，并通过优化算法完成路径搜索。该 方法以栅格为单位记录环境信息，栅格粒度越小，障碍物的表示会越精确，但同 时会占用大量的存储空问，算法的搜索范围将按指数增加。栅格的粒度太大，规 划的路径会很不精确。所以栅格粒度的大小的确定，是栅格法的主要问题。 2 ) 局部路径规划方法 局部路径规划的主要方法有：人工势场法、遗传算法和模糊逻辑算法。人工 势场法是由k h a t i b 提出的一种虚拟力澍引。人工势场法是传统算法中较成熟且高 产的规划方法。这种方法的基本思想是把机器人在环境中的运动视为一种在抽象 的人造受力场中的运动，即在环境中建立人工势场的负梯度方向指向系统的运动 控制方向。目标点对移动机器人产生引力，障碍物对机器人产生斥力，其结果是 使机器人沿“势峰”间的“势谷”前进。引力和斥力的合力作为机器人的加速力 来控制机器入的运动方向和计算机器人的位置。这类方法突出的优点是系统的路 径生成与控制直接与环境实现了闭环，从而大大加强了系统的适应性与避障性 能。但是人工势场法也存在几个主要的缺陷：1 7 j 第一章引言 ( 1 ) 陷阱区域。 ( 2 ) 在相近的障碍物之间不能发现路径。 ( 3 ) 在障碍物前振荡。 ( 4 ) 在狭窄通道中摆动。 为了克服这些缺点，b o r e n s t e i n 和k 0 r e n 于1 9 9 1 年提出一种基于矢量场的方法 - v f h ( v e c t o rf i e l dh i s t o g r 唧) 方法【1 0 】，这种方式能有效地解决上述问题，但 该算法也因为局部特性而存在一些不可避免的缺点，之后一些学者又对其做了不 少改进。 遗传算法( g a ) 是一种基于自然选择和自然遗传的全局优化算法，它采用从自 然界选择、遗传操作中抽象出来的几个算子，对参数编码的字符串进行遗传操作， 每一字符串对应于一个可行解，这种遗传操作是对多个可行解组成的群体进行 的，故在进化过程中可以并行地对解空间的不同区域进行搜索，可使搜索趋于全 局最优解而不会陷于局部极小解。正是由于这种内在的优良特性，g a 可广泛应用 于各种优化问题。遗传算法的操作算法有： ( 1 ) 复制或选择算子。 ( 2 ) 交叉算子。 ( 3 ) 变异算子。 可见g a 的主要优点是：采用群体方式对目标函数空间进行多线索的并行搜 索，可同时对多个可行解进行检查，交叉算子、变异算子可以使可行解之间交换 信息从而产生新的可行解，不会陷入局部极小点；g a 只需要可行解目标函数的 值，而不需要其他信息，对目标函数的连续性、可微性没有要求，因此使用方便； 解的选择和产生采用概率方式，因此具有较强的适应能力和鲁棒性。它通过对随 机产生的多条路径进行选择、交叉、变异、优化组合，利用遗传算法的优胜劣汰、 适者生存的自然选择原理，选择出适应值达到一定标准的一条优化路径。利用遗 传算法解决机器人动态环境中路径规划问题，可以避免困难的理论推导，直接获 得问题的最优解。但遗传算法运算速度不快，进化众多的规划要占用较大的存储 空间和运算时间。 基于实时传感信息的模糊逻辑算法参考人的驾驶经验，通过查表得到规划信 息，实现局部路径规划，计算量不大，易做到边规划边跟踪，能满足实时性要求。 该方法克服了势场法易产生的局部极小问题，适用于时变未知环境下的路径规 划，实时性较好。 3 ) 基于行为的路径规划方法 基于行为的移动机器人路径规划方法是移动机器人路径规划问题研究中的 6 第二章基于l m i 的鲁棒控制器设计 g ( x ，掰) = 厂( x ，甜) 一彳x b “ 厂( x ，甜) = 办( x ，“) 一c k d 甜 进一步可以将方程( 2 1 2 ) 写成如下等价形式： 戈= 彳x + b “+ w 1 y = q + d “+ ( 2 1 3 ) ( ，w 2 ) = ( g ( x ，材) ，r ( x ，铭) ) 设g 是由( 2 1 3 ) 前两个方程确定的映射：对给定的初始条件x ( 0 ) ， ( w 1 ，m ，甜) h ( x ，“，y ) 。q 是由( 2 3 ) 第三个方程确定的映射：( 毛“) h ( w l ，心) 。因 此，( 2 1 3 ) 式描述的系统可以用图2 1 1 来表示。 图( 2 1 1 ) 系统分解 f i g ( 2 1 1 ) d e c o m p o s i t i o no f s y s t e m 容易看到，g 是系统的线性部分，9 是静态的非线性映射。这样就将系统的 非线性部分分离出来，归入到映射q 中，非线性部分和线性部分通过反馈关联联 系起来。 在上面介绍的系统分解中，将一个复杂的非线性系统分解成一个线性部分g 和一个非线性部分q 的关联。其中的q 不仅是非线性的，还可能包含动态特性， 因此它往往是比较复杂的。如果能找到一种更容易处理的对象来替代映射口，则 这样的分解会使得整个系统的分析交得更加方便和容易，其意义也才会显现出 来。以下介绍具有这样性质的替代映射q 的一个映射集合。 集合是具有以下性质的一个映射集合：如果对某个输入p ，9 = q ( p ) ，则 存在一个映射人，使得g = 人( p ) 。如果集合具有这样的性质，则： ( p ，g ) ：g = q ( p ) ) c ( p ，口) ：v a ，g = a ( p ) ) 显然，上式左边的集合定义了一个函数，而右边由j 产生的输入输出对一般 9 第二章基于l m i 的鲁棒控制器设计 来说只是一种关系。这两个集合相差的程度确定了由来替代q 后所产生的保守程度。 之所以引进集合是因为一般来说集合中的映射比映射q要简单得多。例 如，当q是一个非线性映射时，可以用一组线性映射来产生所有可能的满足 g=q(p)的输入输出对(p，g)。因此，我们可以用简单的线性映射集合来替代这个非线性映射q 。 现在利用集合来分析图( 2 1 1 ) 描述的系统。设s ( g ，q ) 表示图( 2 1 1 )中的映射：“h y，如果用映射集s(g，)=s(g，人)：人)来替代映射s(g，q)， s ( g ，9 ) 所描述的系统也必定具有这样的性质。然而，反之却未必成立。因此，应用这样的方法来分析系统的性质具有一定的保守性。 在系统分析中，窭 尹旨饿；匿季吖丞繇眭提畦企等式方一法给羹蒸篙喘侧翼穆?五霞镭臻灞船撕 鞘娄需锴翟磊墼蒂桫蔫篓壁!垂嘏薹“酯耋磋盟；蠢馨驯娶羹墅蒜；等发覆薹鹭 副i理澎擂酒蹲理蓁得到薹y俎薹是否精屠手塞苇篓霁潞羹鬻引“蠢蕨鼍答籀僦塔崔缘高暴崩筒镯誊得到个控j 制鲞；薷墨是特别有用的。2 2基于l 川方法的鲁棒方差控制 2 2 1 模型建立 一个一般的具有参数不确定性和不确定扰动的系统模型如下所示： 戈( f ) = ( 彳+彳) x ( f ) + ( b + b ) “( f ) + g 钆( f ) ( 2 2 1 ) 其中：x ( f ) r ”是系统的状态向量，“( f ) r ”是控制输入，w ( f ) r 是外部扰动， 么，b ，c 是适当维数的常数矩阵，w ( f ) 是一个具有单位方差的零均值白噪声过 程。且w ( f ) 和初始状态x ( o ) 是不相关的。削和衄是反映系统模型中参数不确定 性的位置矩阵，并假定其具有一下的结构形式： 【鲋，舳 - 脚 l，2 】 ( 2 2 2 ) 其中：f r h 7是满足卯7 。，的不确定矩阵，m ，1 和2 是一直常数矩阵。 2 2 2 鲁棒性能分析 记d ( g ，) 是 222鲁棒性能分析 记d ( g ，) 是左半开负平面中的中心在一g + j 0 ( g o ) ，半径，( 厂 o 和对称矩阵尸，使得如下线性矩阵不等式成立： 一r p p 篮+ q p p n ? l 彳尸+ g 尸 一胪+ 鸟r - 1 g g7 + 占删7 o l o ( 2 2 4 ) l l 尸 。 一占，l 吼f 仃2 ，( 江l ，2 ，胛) 1 2 第二章基于l m f 的鲁棒控制器设计 考虑一个具有以下状态空间实现的输出动态反馈控制器： 毫( f ) = 4 t ( f ) + 眈y ( f ) 甜( f ) = e t ( f ) + d c 少( ，) 2 2 1 0 其中：t ( f ) r ”是控制器的状态，4 ，e ，c c 及眈是待定的适当维数实矩阵。 将控制器( 2 2 1 0 ) 应用于系统( 2 。2 9 ) ，得到的闭环系统是 夏( f ) = ( 彳+ a 删) i ( ，) + d w ( ，) z ( r ) = 西( f ) 2 川 其中： 即，= 刚，才= 彳嚣g 斜去柳露= 阳 霄= l + 2 砬c 1 2 c 】，e = 【c 2o 】 这里要研究的问题是：对给定的系统( 2 2 9 ) 、圆盘d ( g ，) 和一个给定的对 称矩阵u ，设计一个输出反馈控制器( 2 2 1 0 ) ，使得对所有允许的不确定性，闭 环系统满足以下条件： ( i ) 所有的闭环极点均在圆盘d ( 9 ，j ) ( i i ) 闭环稳态输出协方差满足 l i n l 研z ( f ) z m ) 】 o 和一个对 称正定矩阵户以及一个对称矩阵u ，使得下列矩阵不等式 一r pp a l + q pp n |qq 毒 一r pqdm 毒奄 一6 ioo 木奉木 一嘲一1 ，o 水 枣木水 一s 一， 1 4 o ( 2 2 1 3 ) 第二章基丁l m i 的鲁棒控制器设计 r 茹! o 旺2 “) k r 一尸j 钏 ( 2 2 “) 其中，幸表示由矩阵的对称性得到的子块。则对所有允许的不确定性，系统的所 有极点均位于圆盘d ( g ，) 。进而，系统( 2 2 9 ) 为稳态状态协方差矩阵 l i m e z ( r ) z7 ( f ) 】存在，且满足x o ，使得具有未知变量x 、y 、五、雪、e 和d 的线性矩阵不等式 一r x r i 岔x + c j 移+ q x 岔+ q 分b 1 + q i 峨+ c j d 1n ：q0 幸 一r y 彳r + 翻7 + c 7 b 7 + g 】，m q + 0 7 ； oo 卑卑 一r xr iqx dx m 木| c牛 一厂】，0上)彳 幸宰 木木 一占，o 0 木奉幸幸木 一厂口一， o 乖事 木木幸奉 一占一。， ( 2 2 1 6 ) 一u c 2 c 2 】，l 串 一x 一，i o( 2 2 1 7 ) 木 ，i c 一】厂l 是可行的，则系统( 2 2 9 ) 存在形如( 2 2 1 0 ) 的输出反馈鲁棒协方差控制器。 其中謇代表出矩阵的对称性得到的矩阵子块。 对于给定的占，矩阵不等式( 2 2 1 6 ) 和( 2 2 1 7 ) 是可行的，则从其任意一 个可行解可以按以下步骤求得系统( 2 2 9 ) 的一个输出反馈鲁棒协方差控制器： 1 通过矩阵，一肼的奇异值分解，确定两个可逆矩阵s 和丁； 2 通过( 2 2 1 5 ) 确定控制器参数4 、最、e 和q 。 2 2 5 状态反馈控制器优化设计 前面得到状态反馈控制器的线性矩阵不等式组。通过在以上的线性矩阵不等 式中添加其他的一些约束条件，可以有效地求取具有其他性能要求的鲁棒方差控 1 6 0 第二章基丁l m i 的鲁棒控制器设计 制律。在实际应用中，具有最小能量的控制器是最有意义的。其性能指标是：【2 】 ，( “) = s u p l i m e ( 甜7 ( f ) 尺“( f ) ) 】l 2 fl f 一 j 其中r 是给定的正定加权矩阵。由“( f ) = 即一x ( f ) 且x i 实验表明，下面一系列参数对目标导向机器人来说是可行的： l = 5 ，心= 2 ，3 = 2 卜5 ，从= 1 ，以= 1 2 ) 启发函数 启发函数办( c ) 是从节点_ 到目标点的最小的代价估计。如果一个函数从来不 会过高地估计节点到目标点的代价，它就可以作为一个启发函数。在本方法中， 使用如下函数作为启发函数： 忍( _ ) = 乃( 以( t ，堡) + 从( t ，q 一。) ) ( 3 2 8 ) “ 这个启发函数仅仅考虑了与下个分支相关的代价，而没有考虑机器人当前速 度方向的影响。因此这个启发函数不是最优的，因为它低估了机器人到达终点节 点的代价。然而，这个启发函数是允许的，并且是比较有效的。 一个更好的，同时也考虑了机器人运动方向影响的启发函数如下： 曩( 吩) = 五( “( 屯j t ) + “( 毛，鱼) + “( 墨，q 一。) ) ( 3 2 9 ) 口 其中： 3 6 第三章基于矢量场的路径规划 后。= 一a r c t a ni 幽l lt l 一誓 这个启发函数相对前一个来说较优，但是计算量更大。然而，它依然不是最 优的，因为它也仅仅考虑与下一个相联系的分支的代价。如果要得出最优的启发 函数值，只有从当前点到目标节点全部展开( 不要建立极坐标柱状图和决定候选 前进方向) ，并且在每个节点处都把目标方向作为候选方向。把得出的相应的所 有代价相加，我们就得到了最优的启发函数值，但这样做计算量太大了。 以上两个启发函数不同点在于一个对算法速度和算法性能之间的一个权衡， 然而，启发函数对于机器人的该朝哪个方向运动并没有影响。 3 2 4 算法结论 搜索路径的好坏主要取决于搜索深度以该参数是与向前搜索的路径总长 度成正比的。图( 3 2 5 ) 表示了不同搜索深度的算法所得出的路径。图( 3 2 6 ) 表示出了在一些关键点的搜索树。 图( 3 2 5 ) a 显示了搜索深度为1 时所得到的轨迹。如图所示，在躲避了前 两个障碍物后，机器人陷入了一个死角，此时算法有一半的概率作出错误的选择， 即图中所示的让机器人朝左行进，进而陷入该死角，此时机器人只能后退寻找其 他路径。 图( 3 2 5 ) b 显示了搜索深度为2 时所得到的轨迹。此时机器人有了向前索 索机制，它不会再像a ) 中那样陷入第一个死角，因为算法在遇到第一扇水平墙 壁时让机器人做出了正确的方向的选择，即向右行进。图( 3 2 6 ) a 显示了在这 个关键点生成的搜索树。在绕过第一扇水平墙壁后，算法引导机器人进入一个死 胡同，图( 3 2 5 ) b ，v f h 幸算法检测到机器人陷入了一个死胡同，此时机器人必 须后退或者拐弯( 如果机器人的最小拐弯半径允许) 。 图( 3 2 5 ) c 显示了搜索深度为5 时所得到的轨迹。由于搜索深度足够大， 机器人在未进入死胡同前算法就已经探测到了死胡同，而使机器人正确地向左拐 弯避过死胡同。图( 3 2 6 ) b 显示了在这个关键点生成的搜索树。v f h 木能避免 陷入的死胡同的深度是与算法的搜索深度成f 比的。很明显，如果要使机器人不 陷入死胡同，机器人传感器的探测范围必修大于死胡同的深度，除非机器人具有 全局的环境地图。可见，只要传感器的探测范围允许，加上足够的搜索深度，一 般的死胡同v f h 母算法是可以避过的。 图( 3 2 5 ) c 显示了搜索深度为1 0 时所得到的轨迹。在这种情况下，由于 搜索深度足够大，算法在很早的时候就已经探测到如果机器人向右拐弯的话所得 第三章基予矢量场的路径规划 到的路径会更优。除此之外，算法也在很早的时候就已经探测到了死胡同。图 ( 3 ，2 。6 ) c 显示了在这两个关键点时生成的搜索树。 图( 3 2 5 ) 显示了搜索深度越大，v f h 宰算法工作的越好。但是增加搜索深 度的代价是增加算法运算时间。图( 3 2 7 ) 显示了不同搜索深度v f h 半的运算时 间，c p u 工作在1 0 0 m h z 。表中第2 列为不同搜索深度算法运算的平均时间，第 三列是算法运算的最大时间，从6 m s 到2 4 2 m s 。总的来说，v f h 幸算法速度在搜 索步数保持比较小时还是比较快的。甚至当搜索步数达到1 0 时，算法的运算的 平均时间也可以达到比较小的数值3 0 m s 。然而，最关键的数值还是算法运算的 最大时间，这也就限制了搜索步数的大小。通过应用附加的分支因子简化和优化 软件，最大的执行时间将会变小。 图( 3 2 5 ) 不同搜索深度的轨迹a ) 唿= 1 ，b ) 门g = 2 ，c ) n g = 5 ，d ) 即g = 1 0 f i g ( 3 2 5 ) t r 萄e c t o r i e so f d i f 诧r e n ts e a r c hd e p t h s a ) 唿= l ，b ) = 2 ，c ) 唿= 5 ，d ) 唯= 1 0 吲 a )b )c ) 图( 3 2 6 ) 不同搜索深度的搜索树a ) = 2 ，b ) 住= 5 ，c ) 唿= 1 0 提示：黑色表示机器人选择的路径，灰色表示展开的那些代价比黑色路径火的搜索树的 ，l；卜、 -t 第三章基于矢量场的路径规划 畦，：机器人的几何中心位置与障碍物的距离 么( 乃) ：矢量历的方向 k ：砖一吃的大小 口，6 由下式决定：【7 】 口一6 ( 掣) = 1 ( 3 3 3 ) 么 其中：w 。时与障碍物相关的一个常数【引。 可以看出，与v f h 木相比，障碍物向量的大小不再仅仅是距离z ，的函数， 同时也是机器人速度口和障碍物速度露的函数。 生成极坐标柱状图的目的是使机器人在障碍物堆中能找到前进的缺口，它告 诉机器人哪个方向被阻挡而那个方向可以通行。如图( 3 3 1 ) ，由于障碍物有一 定的速度屹，障碍物阻挡角度范围不再是v f h 乖方法给出的9 ，而变成了彰， 用区间表示为【所，九】。可以看出，现在的阻挡角度在原来的基础上沿着障碍物速 度方向做了一定角度的偏移，且范围也发生了变化。图( 3 3 1 ) 中的虚线圆直径 为柳器人 图( 3 3 1 ) 对具有速度的障碍物生成柱状图 f i g ( 3 3 1 ) c r e a t ep o l a rh i s t o g r a mf o rm o v i n go b s t a c l e 当前速度大小。根据几何关系可以得到区间，以】。极坐标柱状图由下式表述： 彤= 忽，- ， ( 3 3 4 ) 其中：红。，= 1 ，秒【乃，以】 如，= o ，其他。 由此对于图( 3 3 1 ) 的情况，可以生成如图( 3 3 2 ) 所示的极坐标柱状图。 该图不仅记录下了当前障碍物阻挡角和障碍物矢量大小，同时也记录下了阻挡区 第三章基于矢量场的路径规划 搜索步长和极坐标柱状图日，的关系由下式给出： l 氏瓠，i f七厶侈以戤 办= 尼彤，i f “i n 尼彤 心+ 鸬 ( 3 3 7 ) 与v f h 謇不同，本方法代价函数也考虑了障碍物速度因素，有了该种机制，机器 人就会尽量避免选择会与障碍物相碰的方向前进。 4 2 第二章基丁欠量场的路径规划 2 ) 分支因子 通过简化搜索树分支因子6 ，可以使搜索算法更快并且所需要的存储空间更 小。通过去除多余的节点，分支因子可以得到简化。 每个节点都有和当前候选方向数目相同的下属节点。因为规划步长z 通常 很小，一些下属节点的规划位置和方向可能是一样的。所有超过统的候选方向都 将具有相同的规划位置和方向，同样，所有超过舅的候选方向也将具有相同的规 划位置和方向。为了简化分支因子，除了代价最低的候选方向外的其他侯选方向 的节点都将被去除。这种方法的有效性很容易得到解释。考虑一个节点，它具有 一些超过谚候选方向，因为他们的规划位置和方向都相同，他们也具有相同的极 坐标柱状图，于是，为这些候选方向生成的搜索树也将一模一样，仅仅是代价函 数的第三项会有所不同。如果从一个候选方向生成的搜索树的节点有最小的代 价，它通常会比其他相应的候选方向节点代价要小。因为这种消除方法，分支因 子6 很少大于三。 另一个在算法速度上的改进就是只展开主节点含有多于一个候选方向的搜 索树，当主节点仅有一个候选方向就没有必要展开搜索树了，因为这个时候机器 人只有一个前进方向。 3 ) 衰减因子 节点有一个重要的参数是衰减因子旯。给不同深度的节点的代价函数不同 的加权因子，而不是相同的，这样做有以下几点理由。 首先，没有衰减因子力，所有的分支将具有相同的加权值，避障算法将不 会像设计的那样发挥它的作用。如图( 3 3 4 ) 的一个例子，搜索深度刀2 = 7 ，t 表示目标方向。没有衰减因子力，b 路径的代价函数将比a 路径小，在这种情 况下，机器人将继续向右移动而没有向目标方向前进。同样，没有衰减因子力， 避障算法将趋于寻找一种在未受到障碍物影响的情况下就停下来的路径。在图 ( 3 3 4 ) 所示的情况下，如果搜索步数再增加1 ，路径b 的代价函数将会增加很 多。然而，就算路径b 的代价函数比a 大，算法将会选择c 路径，因为它的代 价函数更小，又会再一次陷入还没有遇到障碍物就停下来的困境。随着衰减因子 的引入，最后几步的代价函数将被削减很多，因此他们的影响对整个路径的影响 变小，结果，路径a 的代价将比b 小，从而使机器人选择正确的路径。 第二章基丁矢量场的路径规划 州i 帝+ 图( 3 4 ) 需要有衰减因子的情况 f i g ( 3 3 4 ) s i t u a t i o nt h a tr e q u i r e st h ed i s c o u n tf a c t o r 其次，衰减因子在某种程度上补偿了地图信息的不确定性。相比机器人而言， 环境信息的不确定因素很多，在加上机器人传感器和地图处理的误差，实际输入 给算法的地图网格可能跟真实的有出入。向前规划的越远，这种出入就可能越大， 衰减因子在一定程度上能弥补这种误差带来的影响。 最后，不使用衰减因子，就相当于使衰减因子等于1 。有了衰减因子，就等 于我们对于不同深度的节点的代价多了一层控制，从而，也使我们对算法多了一 层控制，调节衰减因子的值有可能使算法的性能更优越。 3 3 5 结果及仿真 实验在小型组足球机器人运动控制仿真平台上进行。 搜索步数以。= 5 ，= 2 4 ，“；。= 砟，其中4 为机器人的直径。衰减因子 允= o 8 。代价函数系数分别为： i = 6 ，鸬= 3 ，鸬= 2 ，段= 3 卜5 ，以= 2 ，以= 1 ，“= 2 仿真环境如图( 3 3 5 ) 所示，有5 个障碍物机器人( 白色) ，其中1 号和5 号机 器人静止不动，2 ，3 ，4 号沿y 轴做往复运动。图( 3 3 5 ) 给出了行进过程中的 4 个截图。黑色线表示当前机器人选出的最优路径，灰色线表示机器人已经走过 的路径。可以看出机器人在靠近运动的障碍物时前进路线仍然很平滑，没有出现 急拐弯或者和障碍物相撞的情形。 4 5 c：，；，；。j， 第三章基丁矢量场的路径规划 r b ) ( d ) 图( 3 3 5 ) 路径规划仿真 f i g ( 3 3 5 ) p a t hp l a n n i n gs i m u l a t i o n 图( 3 3 6 ) 显示了在路径规划中机器人的速度曲线。图中实线表示x 方 向速度，点划线表示y 方向的速度。 第三章基于矢量场的路径规划 3 3 6 结论 图( 3 3 6 ) 机器人的速度曲线 f i g ( 3 3 6 ) v e l o c i t yc u r v eo f t h er o b o t 改进型的v f h 幸路径规划方法，在障碍物快速移动的场合中是很有效的。同 时，该方法加快了路径规划的速度，这对那些需要把大量时间放在系统决策的场 合，比如足球机器人比赛中是很重要的。 4 7 第四章小型组足球机器人运动控制仿真平台简介 第四章小型组足球机器人运动控制仿真平台简介 4 1 仿真平台概述 为了验证机器人鲁棒运动控制及路径规划的可行性和效果，笔者同时开发了 一个小型组足球机器人运动控制仿真系统一s s l s i m u l a t o r 。本章简单介绍一下该 仿真平台。 仿真系统开发平台是v i s u a l n e t 。图( 4 1 1 ) 是仿真系统的主界面。仿真平 台集成了两个模块，一个是鲁棒运动控制模块，另一个是路径规划器模块。运动 控制模块基于动力学模型对机器人的运动进行仿真，包括动态点跟踪，动态轨迹 跟踪等。路径规划器是一个基于改进型v f h 木方法的规划器，其功能是基于当前 环境对机器人进行动态路径规划，引导机器人沿着安全的路径到达目标点。 图( 4 1 1 ) 仿真平台主界面 f i g ( 4 1 1 ) m a i ni n t e r f a c eo ft h es i m u l a t i o np l a t f o m 4 2 仿真平台功能 仿真系统具有以下功能： 4 8 一 第四章小型组足球机器人运动控制仿真平台简介 4 ) 显示运动轨迹。 5 ) 机器人参数自定义设置。 图( 4 2 3 ) 机器人参数设置界面 f i g ( 4 2 3 ) i n t e r f a c eo f t h er o b o t sp a r 锄e t e r ss e t t i n g 6 ) 比赛环境自定义设置。 默认设置i 。- - _ 。_ - 。_ _ - 1 _ - 。- _ _ _ - _ - - - 一 取涓 自 楚一j i _ - - - - - - - _ _ - _ _ - - _ _ _ _ _ - _ - 一_ _ _ _ _ _ - - _ _ 一 图( 4 2 4 ) 场地环境设置界面 f i g ( 4 2 4 ) i n t e r f a c eo ft h ef i e l dp a r a m e t e r ss e t t i n g 7 ) 路径规划功能。 第四章小型组足球机器人运动控制仿真平台简介 窭置匠墨焉翻趣疆翟嚣施耸4 、 s 支。o _ 鲁4 谯母鬯仍- 锰) 再逗，圮- a 二 t- 口t 膏勋j d 窖日t p 叠t 协鼻疆最 - i 一目摹 鱼! 苎! 苎l 曰。却u 罩f “a “t i “s ：h n t - t 仿鼻捷矗：一止仿i h 阔oo 8 图( 4 2 5 ) 路径规划界面 f i g ( 4 2 5 ) i n t e r f a c eo f p a t hp l a n n i n g 8 ) 机器人力、位置、速度等参数曲线绘制。 鞠翟露辍溲露嬲翰嬲獭溯骚缀缀戮搿誊鞘籀臻镳i 镶尊 s 文件q ) m 氆口m 勘稿唾曲l 比曩屯) 工 q ) - 口也 掣骑m ) d 瞎口。， l _ l 罟t 协真扭最 ， ：卜“- 二岔霉一 塑! 苎塑i 氆。州；f 卅轴巾f 甜j 0 曲r 0 ， 何 扶杰：辱止协封阍：5 2 0 壮 三i 到到 一埘 删 o 0 e o o o 0e 量 触 i 一一 厂一： r l 班一 厂 刊爿 且重! ：i 凹机70 - 性 捧甜面艟 t 位置羟捌 剖到 0e o 矾翘 t r i 啦硎