（精密仪器及机械专业论文）足球机器人运动控制算法研究及控制系统设计.pdf

申晷辩擘技术大学颈士学经论文 摘要 本文主要介绍了对遥轮足球机器人运动控制簿法的磷究。首先，本文建立了 四轮垒向足球机器人运动学和动力学模型。通过对机器人运动学和动力学模型的 分析，对其控制方程进行合理简化，提出了四轮垒向足球机器人的运动控制算法。 该算法可激霹枫器人闵对完或位鼗揎鞠囊遥度控翊，著量控制辩闭最谎。该算法 分为两个部分：机器人速度轨迹生成算法和机器人速度轨迹跟随算法。机器人速 度轨迹生成算法基于玎关控制( 又称b a n g b a n f f 控制) 原理，它根据机器人当前 逝置、萎翦运度、爨搽点位置和酲标袭速度，雯戏一条扶“当蘸点”到“莲标点” 的时间镀优的速度轨迹。机器人逑度轨迹生成算法 用模糊p i d 复合算法实时控 制机器人四组驱动轮速度。通过四蛆驱动轮的速度矢量合成，使得机器人跟随设 定的速度轨迹。实验液醍该速度控制箕法完全满怒控制足球机器人运动快遽、准 礁、灵活的控制要隶。 本文还对四轮足球机器人的控制系统进行了研究和设计。控制系统采用盘一 从式双d s p 为核心的设计方案。系统中，主d s p 对从d s p 进行管理和控制、双 d s p 醚台，完成速震鞭隧控制、无线通讯、传感器数据读取等任务。该方案稻捷 统方寨相比，更加稳定、实用、可靠，功能更强。 本文还介绍了作者在1 0 0 l o o n l i n 微小型越障机器人控制上的研究工作。微 小型越簿巍嚣a 静蓉籍系统峦决策级、行为级魏羲好级三个熙次组成。本文对霉亍 为级和执行级进行了煎点研究。行为级包括越障机器人的行进、越障、自饿复控 制。执行级使用p i d 控制方法来进行前、后摆臂角度进行控制，使用模糊p i d 复 台算法对左、右理带避雩亍速度控制。越障机嚣人舒进圣亍为控制君眭撬行级的控涮邦 分移植了足球机器人的运动控铡冀法。本文着重对两者的差异和算弦的变动进行 了叙述。 中国科学技术大学硕士学位论文 a b s t r a c t t h er o b o to f r o b o c u pg a m e sm u s tt r a v e l si nac o n f u s e dd y n a m i cc i r c u m s t a n c ef r e e l y a n da c h i e v ea na c c u r a t ep o s i t i o n i n gi nt h ec o m p e t i t i o n i nt h i sp a p e r ，t h ek i n e m a t i c s e q u a t i o n sa n dt h ed y n a m i ce q u a t i o n so ft h eh o l o n o m i co m n i d i r e c t i o n a lw h e e l e d m o b i l er o b o ta r ep r e s e n t e d a n a l y s i n gt h o s ee q u a t i o n s ，t h i sp a p e rp r o p o s eam o t i o n c o n t r o lm e t h o df o rt h er o b o t t h em e t h o di n c l u d e st w op a n s ，v e l o c i t yt r a j e c t o r y g e n e r a t i o n ( v t g ) a n dv e l o c i t yt r a j e c t o r yt r a c k i n g ( v t t ) t h ev t gi s b a s e do n b a n g b a n gc o n t r o la n dg e n e r a t e sav e l o c i t yt r a j e c t o r yf r o ms t a r t p o i n tt oe n d p o i n t f o r t h er o b o t t h ev t ti sb a s e do np i dc o n t r o la n dc o n t r o l st h ev e l o c i t yo ft h er o b o t t r a c k i n gt h ev e l o c i t yt r a j e c t o r yg e n e r a t e db yv t g t h i sp a p e rp r e s e n t st h ed e s i g no fr o b o ts o c c e r sc i r c u i ts y s t e m t h ec o r eo ft h e c i r c u i ti sd u a ld s p , t m s 3 2 0 l f 2 4 0 7 a o n ed s pi st h em a s t e r , a n dt h eo t h e ri ss l a v e r y u n d e rt h ec o n t r o lo f t h em a s t e r , t h es l a v e r yd s pr e a l i z e st h em o t i o nc o n t r o la l g o r i t h m t h e nt h ep a p e rd e s c r i b e st h er e s e a r c ho nt h e10 0 x l0 0 m mm i c r oo v e r o b s t a c l er o b o t t h ec o n t r o ls y s t e mi n c l u d et h r e es t r a t u m s ：d e c i s i o n m a k i n gs t r a t u m ，c o r r e s p o n da c t i o n s t r a t u m ，e x e c u t es t r a t u m t h em o t i o nc o n t r o la n de x e c u t es t r a t u mu s et h ea l g o r i t h mo f r o b o c u pr o b o t t h i sp a p e rd e s c r i b e st h ed i f f e r e n c e se s p e c i a l l y - i i - 中国科学技术大学硕士学位论文 第1 章绪论 1 1 机器人足球比赛及研究意义 机器人足球赛是由硬件或仿真机器人进行的足球赛，比赛规则与人类正规的 足球赛类似。机器人足球队的研制涉及自动控制、精密机械、传感与感知融合、 计算机、无线通讯和仿生材料等众多学科的前沿研究与综合集成。机器人足球是 人工智能和机器人学新的标准问题，是连接基础研究与实际应用的中介和桥梁， 是展示信息自动化前沿研究成果的窗口和促进产、学、研结合的新途径。 目前，举办足球机器人比赛的国际组织有两个：机器人足球世界杯( r o b o c u p ) 和国际机器人足球联合会( f i r a ) 。 其中r o b o c u p 机器人足球世界杯赛及学术大会( t h er o b o tw o r l dc u ps o c c e r g a m e sa n dc o n f e r e n c e s ) 是国际上级别最高、规模最大、影响最广泛的机器人 足球赛事和学术会议，每年举办一次。 r o b o c u p 机器人足球赛最重要的目的是检验信息自动化前沿研究特别是 多智能体系统( m u l t i - - a g e n ts y s t e m ) 研究的最新成果，交流新思想和新进展， 从而更好的推动基础研究和应用基础研究及其成果转化。通过竞赛，各种不同的 新思想、新原理和新技术可以得到客观的评价。虽然机器人足球只有短短几年历 史，它在推动产、学、研结合方面的显著作用和巨大潜力已经表现出来。因而 r o b o c u p 机器人足球世界杯赛和学术大会受到了世界各国、特别是美、日、德等 发达国家的高度重视。我国在这一方向上起步较晚，更需要奋起直追1 1 2 r o b o c u p 小型组国内外研究概况 r o b o c u p 共分仿真组、小型组，中型组，四腿组、救援机器人、两腿机器人 六个正式比赛类别。另外还有面向中小学生的初级组竞赛( r o b o c u pj u n i o r ) 。 小型组的比赛是由两队各5 名机器人球员在一个铺满绿色地毯垫的长方型 场地上进行的，机器人的任务是将“足球( 橘红色的高尔夫球) ”攻入对方的球 门同时防止对方攻入自己的球门。 在比赛过程中双方的参赛人员完全不得干预比赛，机器人足球队必须通过自 身的人工智能程序完全自主的理解和执行裁判的判罚，而不是通过遥控。足球机 器人必须自主的完成发边界球、死球后的重新开球、球门球、角球、任意球、点 1 - 中国科学技术大学硕士学位论文 球等复杂的球队整体战术。 足球机器人比赛中双方对抗激烈、赛场情况瞬间万变。要想取得胜利，机器 人足球队必须拥有高度的智能，能够对场上局势和出现的变化作出正确、及时的 感知，并作出迅速和准确的反应。而且，机器人足球队需要有足够的智能来进行 各个队员之间的相互配合，执行复杂的整体战术。这些都是当今世界人工智能领 域的尖端问题，它在工业自动化和国防等领域中有着重要的应用。 小型组足球机器人不仅仅是一个人工智能的问题，它是一个复杂的系统，涉 及到广泛的学科门类，具有相当的难度。它包括：精密机械及驱动、自动控制、 无线通讯、机器人视觉及传感、多智能体控制。 本文所述的研究工作对象是中国科大蓝鹰小型组机器人足球队，它成立于 2 0 0 0 年，是国内的第一只r o b o c u p 小型组机器人足球队，现已发展到了第三代。 球队多次参加国内外机器人大赛，获得了好的成绩。 1 3 论文主要研究工作 本论文的主要研究工作集中在足球机器人运动控制算法上，并针对运动控制 算法的要求为足球机器人设计了控制电路系统。此外以这些研究为基础，作者将 研究工作延伸到了对1 0 0xl o o m m 微小型越障机器人的控制方法研究和控制系统 设计上。 机器人足球系统是个理想的研究平台。作者根据控制算法需要设计了合适的 控制系统来进行控制算法的研究，利用比赛来不断验证和改进算法。在不断的研 究和改进过程中，形成了一套完整的运动控制算法。该算法应用在第三代四轮全 向足球机器人系统上，参加了国内和国际上的多次比赛。经过实际比赛的验证， 该运动控制算法取得良好的效果。 机器人的运动控制有一定的相通性，也有不同点。作者利用足球机器人控制 算法研究和控制系统设计上的积累，为8 6 3 项目“1 0 0xl o o m m 微小型越障机器 人”设计了控制系统，并对它的控制算法进行研究和应用。该项目已成功通过专 家组验收。 该论文主要内容为如下： 第一章介绍了r o b o c u p 小型组机器人的研究背景，意义。介绍了本队的研究 工作以及本文所述的研究工作在全系统中的位置和意义。 第二章是本文研究对象的四轮足球机器人系统概述。 一2 - 中国科学技术大学硕士学位论文 第三章是本文的核心。它介绍了详细研究出来的运动控制算法。该算法包括 速度轨迹生成( v e l o c i t yt r a j e c t o r yg e n e r a t i o n ) 算法和速度轨迹跟随 ( v e l o c i t yt r a j e c t o r yt r a c k i n g ) 算法两部分。 其中，速度轨迹生成算法( v t g ) 根据人工智能系统为单个机器人规划的“任 务”控制机器人到底一定位置保持一定的姿态。所谓“任务 就是机器人的目的 点和到达目的点时的瞬时速度、瞬时角速度、机器人正对的方向。该算法根据机 器人当前位置、当前速度、目标点位置和目标点速度，生成一条从“当前点”到 “目标点”的时间最优的速度轨迹。 速度轨迹跟随算法( v t t ) 实时控制机器人速度，使机器人自身速度跟随速度 轨迹生成算法生成的速度轨迹，即可完成机器人的运动控制。 最后列出了根据本文提出的速度轨迹生成( v t g ) 算法和速度轨迹跟随算法 ( v t t ) 来实验。实验证明了算法的有效性。 第四章是本文的另一个重要章节，它介绍了四轮全向足球机器人的控制系统 的设计，包括采用主从工作模式的双d s p 控制系统的硬件和软件设计。还介绍了 足球机器人驱动及传感电路的设计。 第五章重点介绍了对1 0 0 l o o m m 微小型越障机器人控制算法的研究工作。 微小型越障机器人的控制系统由决策级、行为级和执行级三个层次组成。本文对 行为级和执行级进行了详细研究。执行级使用p i d 控制方法来进行前后摆臂角度 进行控制，使用模糊p i d 复合算法对左右履带进行速度控制。从而解决了执行级 的控制任务。行为级针对不同的行为给出了特定的控制策略。 最后，本文第六章对研究工作进行了总结，对未来的研究工作做了进一步的 展望。 一3 - 中国科学技术大学硕士学位论文 第2 章四轮全向足球机器人系统概述 2 1 机器人足球队系统工作流程 足球机器人系统是一个典型的多智能体系统( m u l t i - a g o n ts y s t e m ) ，球队拥 有五个足球机器人。五个机器人在场t 具有再自的角色如：前锋、后卫、守门 员，在比赛中分别承担不同的任务。比赛中，球队的五个足球机器人要根据场上 的局势，通过互相配合的方式完成给定的任务，与对方对抗。 图2lr o b o c u p 小型组机器人足球队工作场景示意图 图2 2 机器人足球队控制系统总体框图 图2 1 为r o b o c u p 小型组机器人足球队工作场景示意图。图22 为机器人 足球队工作流程图。比赛中，整个系统按照如下流程工作： 1 比赛的时候，悬挂于赛场m - 方的c c d 摄像机将比赛场地上的信息采集下 来输送给计算机内的图像处理子系统。 2 计算机( p c ) 内的图像处理子系统通过对球场图象进行处理，得到本方 球员、对方球员以及球的位置、姿态、速度等信息的全局坐标。 一d 中国科学技术大学硕士学位论文 3 这些坐标信息被送入装载在计算机中的控制软件中，控制软件对这些信 息进行分析、处理，根据球队战术的要求为每一个机器人分配“任务”， 生成各自的速度轨迹。足球机器人的速度轨迹以指令的形式通过无线通 讯系统发送给各个足球机器人。 4 单个的足球机器人通过自身装备的无线通讯模块接收速度轨迹指令，并 由装载在机器人主控m c u 上的控制程序实时控制机器人达到指定的速 度。这样球队的所有足球机器人就可以在整个控制系统的“指挥”下相 互配合，完成比赛。 计算机内的图像处理子系统2 5 帧秒采样一次，然后整个系统就按照上面所 述的工作流程走一遍，根据新的图像信息给机器人新的控制指令。 比赛中的“球”和比赛双方共十个足球机器人都是高速运动的，整个足球机 器人系统工作的环境是高度动态的。而且，“球”和对方机器人的动作都变化快 速、不可预测的。这些客观条件决定了足球机器人队的控制具有很大难度。 整支机器人足球队的控制系统需要能够根据场上的形式实时的安排整支球 队战术和配合，给每个足球机器人规划“任务”。由于场上的局势是实时变化的， 所以我们的系统需要高速的运作。每个机器人的“任务”也是实时变化的。这个 部分功能需要多智能体协调控制算法，属于人工智能的范畴。 我们的四轮全向足球机器人具有特殊的驱动起构，具备平面上全自由度运动 的能力，可以沿任意的复杂曲线运动，其最大速度达到3 m s 。传统的两轮移动 机器人的运动控制模式和方法根本不适用。面对实时变化的“任务”，具有如何 快速、精确地控制全向机器人值得深入研究。 2 2 视觉系统介绍 视觉部分负责识别场上双方机器人和球在场地坐标系内的位置和方向，这些 信息是决策系统的输入，信息准确性非常重要。 视觉子系统的软件流程如图2 - 3 所示。在设计视觉系统需要考虑的问题如 下：( a ) 图像增强。滤除噪声，提高质量；( b ) 图像恢复。对于几何畸变的矫正； ( c ) 色标设计与辨识算法研究：( d ) 颜色模型的选择与亮度不变性的考虑；( e ) 颜 色分割与特征提取算法；( f ) 视觉系统处理信息的速度；( g ) 辨识精度。 目前，较好的校准方法由t s a i 等提出，通过校准径向畸变、切向畸变、畸 变中心和焦距等参数获得较高的校准精度。较好的色标设计方法是有由c a r n e g i e 5 一 中国科学技术大学硕士学位论文 m e ll o n 大学提出的色标设计方法h 儿引，在小型足球机器人中被普遍采用，其优点 是可以利用各色标之间几何关系确定姿态，且精度高。 荤牛 i 以一定步长扫捕豳像撤獬设讣的色标校扳计 i 抖找队标和球 算机器人位簧和方向 争 叫埘方机器人的位簧卜+ 1r 图2 3 视觉处理流程图 2 3 足球机器人控制系统总体结构 比赛中，机器人足球队的控制系统根据其功能层 次，可以划分为决策控制和运动控制两个层次： 如上层为决策控制部分，它是一个多智能体系统 ( m u l t i a g e n ts y s t e m ) 的协调控制系统。它的主要功 能是预测“球”和场上对手的运动，组织安排球队级 的战术和配合，利用多智能体协调控制算法为每个机 器人安排各自的“任务”。所谓“任务”即为机器人应 该到达的位置及姿态。 决策控制部分采用分层控制处理结构，由信息处 理层、协调战术层、任务层和动作层组成，如图2 4 所示。 信息处理层：处理场上基本信息，经过预测分析， 得出综合信息并更新旧的公共信息库为后续决策提供 知识。 协调战术层：根据公共信息库的知识，从战术库 图2 - 4 足球机器人 运动控制总体结构图一 中国科学技术大学硕士学位论文 中选择有利于我方的战术，更新球员角色分配表。 任务层：根据角色分配表中自己的角色，确定自己的动作类型与目标坐标， 更新私有信息库。 动作层：根据私有信息库的知识，结合场上现实情形，选择合适的执行方案 并调用基本动作实现。 底层为足球机器人运动控制部分，它的功能是根据多智能体协调控制部分的 给定的“任务”控制机器人运动。足球机器人运动控制部分也可分为两个部分： 速度轨迹生成( v e l o c i t yt r a j e c t o r yg e n e r a t i o n ) 部分：它针对决策控制部 分制定的“任务”，根据机器人当前位置、当前速度、目标点位置和目标点速度， 为足球机器人生成一条从“当前点”到“目标点”的时间最优的速度轨迹。 速度轨迹跟随( v e l o c i t yt r a j e c t o r yt r a c k i n g ) 部分：它控制足球机器人的 驱动机构，实时控制足球机器人的速度跟随生成的速度轨迹，使足球机器人完成 规定的“任务”。 2 4 决策控制系统介绍 决策系统是一个多智能体系统( m u l t i a g e n ts y s t e m ) 的协调控制系统。它主 要包括：信息预测处理，多智能体( m a s ) 协调策略、角色任务分配执行、机器人 路径规划、机器人运动控制这几个主要部分，还有很多和其他系统的接口。图2 5 显示的是决策系统的基本组成与运行流程。 图2 5 决策控制系统运行流程 由于整个机器人系统运作过程中会产生信息传递延时，视觉信息不能真实的 反映当前机器人和球的位置坐标和方向，需要对位置信息进行预测。一般采用的 7 一 中国科学技术大学硕士学位论文 办法有扩展卡尔曼滤波、线性模型、b p 神经网络等方法。 段时间内或者有一定目的的机器人队伍整体决策称为战术，根据小剧的隋 况采崩不同的战术。每个机器人都会分配到不同的任务，并且每个战术都有它的 应用条什，通过战术选择算法在比赛中决策系统能自适应对手进行战术选择，对 任何对手都能以最高效率进行比赛。 路径规划的问题是机器人如何避升所有的障碍从一点走到另一点，常用算法 有：基于格点的算法，人t 势场法，d e l a u n f l y 三角法，几何方法，随机扩展搜 索算法等。 为了使决策程序的开发能不受其他子系统的进度影响，有必要开发个能模 拟真实比赛情况的仿真软件对机械，视觉，通讯剖分都进行真实的模拟。仿真 平台是小型组决策程序调试的一个重要工具。它是代替真实机器人系统的一个模 拟器，在真实机器人系统或视觉系统不可用的状况下，使用仿真平台可以方便的 调试a i 决策系统。特别是在真实机器人还不够成熟稳定的隋况，调试决策程序 的战术是非常困难的，这时候用较为理想的仿真平台就能很好实现战术调试，能 方便的看到决策程序的效果甚至做分析。仿真平台通过网络接口与决策系统交 互，进行比赛，如图2 - f , 。 图2 - 6 决策控制系统仿真平台 2 5 无线通讯系统介绍 在小型足球机器人系统当中无线通讯系统有着相当重要的作用，担负着将决 策指令及时准确的送达在场r 的每一个机器人的任务。无线通讯系统是衔接决策 和机器人之间联系的不可缺少的一环，它必须保证从土机端和机器人之间的数据 传送是可靠的，从而使得机器人比赛能够je a n 流畅进行。由于比赛双方都有多个 一8 一 中圆科学技术大学硕士学位论文 机器人同时在场地上高速运动，要求无线通讯的稳定要好，有一定的抗干扰能力。 无线通讯系统的性能，相当程度上直接影响着机器人的场上表现。 我们的无线通讯部分采用计算机串口和无线通讯系统接口，原因是通过计算 机串口传送数据非常直接利简单，没有延时。无线通信系统可以允许双向通信， 如图2 7 所示，对于一帧数据整个通信延时不超过8 m s ，其中i v 行( 从主机到所 有机器人) 的数据量不超过3 2 k b p s ，上行( 从每个机器人到主机) 的数据量不 超过2 0 k b p s 。 2 6 机械结构介绍 l 、机器人总体结构 图27 无线通讯系统接口 图2 8 足球机器人总体结构图 图2 8 是本文所介绍的四轮伞向足球机器人。它具自特殊的驱动轮设计及 布置方式，因此它具备平面上的全自由度运动能力。机器人总体结构从l ：往r 分 为三部分：最上部分足上色标板，用于机器人视觉系统的识别机器人位置、方向 及编号：中间安装的是控制电路板；最下面是机器人的机构部分其主要由车体 底盘、驱动机构、带球机构、击挑球机构组成。 9 一 中国科学技术 学颂士学位论文 图29 足球机器人驱动轮设计及布置图 1 驱动机构 足球机器人之所以能全向运动在于它特殊的驱动轮布置方式和驱动轮机构。 如图29 所示，足球机器人的驱动轮机构是圆形大轮的圆周上密布小滚动轮子 构成，整个驱动轮可以沿同向与轴向两个方向做滚动，其中周向受电机驱动，轴 向可自由运动。四组驱动轮及电机成一定角度布置存一个恻周r ，通过各驱动轮 的速度矢量合成，机器人可以在平面上实现全自由度运动。 全向足球机器人可以很方便的实现任意方向运动。特别是a ；需转弯可在运动 过程中自由随时姿态( 机器人前方朝向) 。全向足球机器人在比赛过程中可以精 确的实现各种复杂的路径运动。 2 、带球机构 比赛规则规定，带球机构与球面的接触面积不得超过球面面积的1 3 。根据 这一要求，选择使用带球棍旋转摩擦球使球跟随机器人运动，从而来实现带球功 能。顺时引高速旋转的带球棍依靠摩擦力带动小球做逆时针旋转，小球受到的地 而的摩擦力与带球棍的摩擦力的合力指向机器人本体，当足球机器人前进、后退、 转弯时，小球始终在带球棍的带动下跟着足球机器人运动，图21 0 为带球机构 简图。 图2 1 0 带球机构简图 带球一直是机械部分的一个问题它涉及到力学、利料、机械等多个方面。 因为机器人的行走机构换了一种全向轮，虽然抓地力更强，但是也带来了抖动， 一l o 中国科学技术大学硕士学位论文 给带球机构的设计带来了很大的麻烦。针对以上问题，带球机构主要设计方案： ( 1 ) 设计了悬挂式的带球机构；( 2 ) 带球杆的位置不是固定的，可以上下调整， 在不同的场地比赛时地面摩擦力不同，调整带球杆的位置相当于调整球和带球杆 的摩擦力，以适应场地的摩擦力，就能获得最佳的带球效果。 3 、击球机构 踢球在足球机器人比赛中相当重要，击球机构是完成射门和传球的工具。目 前的击球方式大致有以下几种：弹簧储能击球、齿轮齿条击球、气动击球、弓弩 式击球、电磁铁击球。 实践证明弹簧储能击球效果不好，在击球之前通过电机将弹簧压缩储能，击 球时弹簧瞬间释放能量，先前弹簧储存的弹性势能全部转化为施加给小球的冲 量。弹簧击球瞬间的爆发力与弹簧的压缩量和弹簧刚度成正比，弹簧的压缩量越 大则击球的爆发力越大，也会增加给小球冲量的持续时间t ；但增加弹簧压缩量 使击球机构尺寸增大，可靠性降低。弹簧的刚度越大则击球的爆发力就越大，而 小球受到冲量的时间就会减小。气动和弓弩式击球机构设计制造复杂，稳定性不 好。 电磁铁机构是目前足球机器人击球机构中用的最为普遍的一种机构，它的优 点是力量大，动作频率高，反应速度快，机械机构相对简单可靠。基于以上的分 析，采用电磁铁击球机构。设计方案采用2 0 0 v 左右的电压驱动电磁铁来击球， 利用电磁铁将存储在电容中巨大的能量释放出来转递给球，可以使球的速度达到 5 m s 左右，并且能通过控制放电时间来控制出球的速度。因此，可以使用击球 机构来实现不同力度的“传球”和“射门”。 中国科学技术大学硕士学位论文 第3 章四轮全向足球机器人运动控制算法研究 3 1 运动控制模式 足球机器人运动控制就是按照多智能体协调控制软件为每个足球机器人设 定的“任务”控制机器人完成该任务。 因为我们的足球机器人是四轮全向足球机器人，它具有新的驱动结构，控制 难度较高，所以也需要选择合适的运动控制模式和运动控制算法。 从运动控制模式上内在层次，运动控制算法可分为下面为两个部分： 速度轨迹生成算法。它根据“任务”为足球机器人生成一条速度轨迹，得到 的是机器人车体的控制速度矢量和角速度矢量。所谓速度轨迹即机器人在特定的 时间点应该达到的速度。 机器人速度跟随算法。它根据速度轨迹生成算法设定的速度矢量，参照足球 机器人的自身参数控制，控制足球机器人使机器人在特定的时刻达到速度轨迹设 定的机器人整体速度、角速度。 结合足球机器人系统整体工作流程，足球机器人运动控制过程如下： ( 1 ) 整个足球机器人视觉系统以每秒2 5 帧的速度处理图像信息。 ( 2 ) 视觉信息每更新一次，决策控制部分就执行一次工作流程，为各机器人 分配“任务”。也就是说，机器人“任务”信息也是以每秒2 5 次( 4 0 毫秒) 的 速度来更新的。 ( 3 ) 机器人“任务”每更新一次，机器人运动控制系统就使用速度轨迹生成 算法为五个机器人各自独立的生成新的速度轨迹，并通过无线通讯系统以指令的 形式发送给机器人主控系统。也就是说，速度轨迹生成算法也是每秒2 5 次工作 一次，将机器人要保持的整体速度发送给机器人。 ( 4 ) 机器人以每秒2 5 次的频率接收到传送来的指令，并使用运动控制算法的 速度轨迹跟随算法控制机器人达到指令要求的速度值。4 0 毫秒后，收到下一条 指令后，机器人的速度控制量随新的指令而变化。 3 2 速度轨迹生成( v e l o c i t yt r a j e c t o r yg e n e r a t i o n ) 算法 本节要讨论的速度轨迹生成( v e l o c i t yt r a j e c t o r yg e n e r a t i o n ) 算法( 简 称：v t g ) 基于b a n g - - b a n g 控制，它对多智能体协调控制子系统指定的足球机器 1 2 中国科学技术大学硕士学位论文 人初始位置、初始速度、末位置、末速度生成一条时间最优的速度轨迹，使得足 球机器人能以最短的时间到达末位置并保持指定的末速度。 3 2 1 运动控制模型 图3 一l 足球机器人运动学示意图 如图3 1 所示，足球机器人运动控制系统需要控制的偏差为机器人位置点 的x 坐标、y 坐标、机器人正对方向与垂直方向夹角臼、x 方向速度x 、y 方向速 度y 和白转角速度臼。 图3 2 足球机器人速度合成图 如图3 2 ，足球机器人共有四组驱动轮，成一定角度分布在一个圆上。机器 人的速度和角速度通过四组驱动轮速度的矢量合成得到。通过控制四组驱动轮速 度成特定关系，机器人可以在平面上按照任意轨迹行走。 足球机器人每组驱动轮使用一个直流电机驱动，又因为机器人速度由驱动轮 速度合成得到，因此机器人x 方向速度x 、加速度x ，y 方向速度y 、加速度y ， 角速度口、角加速度口会受到驱动轮电机动力学参数约束。分析足球机器人控制 系统，得到其控制用的运动合成方程如下： - 1 3 - 中国科学技术大学硕士学位论文 国l 缈2 国3 缈4 c 。s ( 妙 c 。s ( 妙 c 。s ( 妙 c 。s ( 妙 “n ( 妙。) 夕 “n ( 妙：) 夕 “n ( 妙，) 夕 一s i n ( 5 u 、 t ( 3 1 ) 国一表示各驱动轮的转动角速度 沙一表示各驱动轮与x 方向夹角 j 一驱动轮所在圆周半径 儿、y 。、一机器人自身坐标系下的x 、y 方向速度和角速度 由于系统过于复杂，很难得到实际可用的对可解耦的控制方程。机器人四组 驱动轮对称分布，使用相同的驱动电机。分析公式( 3 1 ) ，可以得到机器人在自 身坐标系下的各个方向的动力学参数几乎没有差别。因此可以在实际工程中加以 简化的认为：在x 方向、y 方向以及转动角方向0 三个方向上的控制量是独立的。 因此，在速度轨迹生成算法中，对于足球机器人x 方向、y 方向以及角方向0 上 的控制量，分别在三个方向各自独立进行控制。 3 2 2 控制方程解析 度) 。 通过3 2 1 节的分析，利用简化模型得到司以解析的控制方程如f ： x + k ，x 2 q x ( f ) ( 3 2 ) y + k 。y 2 q y ( t ) ( 3 3 ) 0 + k 口0 2 q e ( f ) ( 3 4 ) q x ( f ) 、q y ( t ) 、q o ( f ) 分别为x 方向、y 方向和角方向p 上的控制作用( 加速 k ，、k ，、k 。分别为三个方向上的粘滞阻尼系数。 x 方向、y 方向和角方向0 上的控制作用的约束为： 一a ，吼o ) a ， 一a y 以( f ) a ， ( 3 5 ) ( 3 6 ) 1 4 厶p厶：、厶厶p 、ili，、ll-，、ii-，、i_-， 中国科学技术大学硕士学位论文 一a 口q o ( t ) a 口 ( 3 7 ) 彳，、彳y 、a 口为x 、y 、秒方向上的最大控制作用( 最大加速度) 。 方程( 3 8 ) 、( 3 9 ) 、( 3 1 0 ) 是方程( 3 5 ) 、( 3 6 ) 、( 3 7 ) 典型的边界条件： 删= x o ，嘶) 2 x t ，x ( o ) 飞。，x ( f ，) = o ( 3 8 ) 嗣2 y o ，飑) 2 野y ( o ) 2 v y 。，y ( f ，) - o ( 3 9 ) = f 5 6 ，电) = 徘秒( o ) 2 0 9 0 ，0 ( f ，) = 0 ( 3 1 0 ) x ( 0 ) 、y ( 0 ) 、口( o ) 分别为三个方向上的初始位置。 x ( t r ) 、y ( t r ) 、o ( tr ) 分别为三个方向上的末位置。 x ( 0 ) 、y ( 0 ) 、0 ( 0 ) 分别为三个方向上的初始速度。 工( r ，) 、y ( t ，) 、0 ( f ，) 分别为三个方向上的末速度。 要得到时间最优的速度轨迹生成算法就需要求解机器人满足( 3 8 ) 、( 3 9 ) 、 ( 3 1 0 ) 条件的时间最优控制解。由此得到的x ( t ) 、y ( t ) 和0 ( t ) 经过积分就是生 成的时间最优的速度轨迹：( t ) ：，( t ) 、；( t ) 。 应用极小值原理很难求出具体的时间最优控制x ( t ) 、y ( t ) 和0 ( t ) 。在实际 工程上应用一种特殊情况：即各个控制量都取控制域的边界值，而且不断的从一 个边界切换到另一个边界从而构成最强的控制作用，即b a n g - b a n g 控制。 3 2 3 算法原理 下面将使用b a n g b a n g 控制的原理来求解，得到速度轨迹生成算法。 因为这三组控制量的控制方程和约束条件类似，故使用变量z 代替x 进行演 算。对两外两个变量y 和0 的求解过程和x 相同，只需要将z 分别换成y 和0 即 可。 根据b a n g b a n g 控制原理，即总是采用最强的控制作用( 最大加速度) 。也就 是控制作用 g ；( r ) l 三k a ： ( 3 1 1 ) - 15 中国科学技术大学硕士学位论文 a ：为常数，代表此方向上的最大控制作用( 最大加速度) 。 k 为粘滞阻尼系数。 因此，公式( 3 2 ) 、( 3 5 ) 、( 3 8 ) 转化为： z + k z 34 0 f f l z + 后z2 4岛* 芎 z ( o ) = z o ，z ( 9 ) = z i ，z ( 0 ) 。v o ，z ( 0 ) = 0 初始时刻为0 ，时刻为正负最强控制作用切换点 t ，时刻为末时刻 根据方程( 3 1 2 ) 、( 3 1 3 ) 、( 3 1 4 ) 可以求得： 当0 f f 1 ，为正最强控制作用段。 却冲岫哮一爷一争+ 睾 嵋) = 知= ( v ：o 一务却+ 睾 当 f + 之= 0 ，为负最强控制作用段。 z ( f ) = 1 ，( f ) = ( 3 1 2 ) ( 3 1 3 ) ( 3 1 4 ) ( 3 1 5 ) 寺( 坼砌叫“l ，- f - 1 ) + 。 ( 3 1 6 ) 知却如叫棚睾 因为机器人的位移和速度是连续的，所以在扛 时方程( 3 i 5 ) 、( 3 1 6 ) 是相 等，因此得到方程： p 嘶唼一抄- 爰i t l + z o - - 万a 2 + 睾吨嘶叫，睾 c v o 一争电1 + 鲁= c e - k t - 1 ) - 争 由上述方程( 3 1 7 ) 、( 3 1 8 ) 可以计算得到： ( 3 1 7 ) ( 3 1 8 ) 。1 6 中国科学技术大学硕士学位论文 f - = t 2 - 百k c ：其中c _ - - i v o + z 0 - - z f ( 3 1 9 ) 乞_ l n ( 1 s g n ( 争) 历) 其中d = l + e 竺a , f 鲁一1 ( 3 2 。) 托以j 则由基于b a n g b a n g 的速度轨迹生成算法的点到点的最小时间为： o 晌= + t 2 = 2 i n ( 1 + 历) 一 ( 3 2 1 ) 在足球机器人的控制系统中，需要生成一条离散的速度轨迹。即利用上面推 导的公式，计算出从当前点到目标点的最小时间和临界点，判断每个离散点处于 系统正最大控制作用时刻还是负最大控制时刻。再代入公式( 3 1 2 ) 、( 3 1 3 ) 积分 就可以计算出每一条位置、速度，这样就生成了一条时间最优的速度轨迹。 根据上节所述的轨迹生成算法原理，我们可以为四轮全向足球机器人生成实 际的速度轨迹曲线。根据机器人当前时刻的速度、位置信息和多智能体协调子系 统实时生成的目标点位置、到达目标点的末速度等信息，应用上面所述的速度轨 迹生成方程计算出每一个时刻的v ，、v ，、，这样就是一条可以用来控制足球 机器人的离散的速度轨迹。机器人速度轨迹跟随子系统实时控制机器人速度，使 机器人每个时刻的自身速度达到v t g 生成的速度轨迹曲线的对应时刻的值，这样 就可实现机器人的运动控制。 3 2 4 各方向上速度轨迹的时间同步 在上节的算法原理中，机器人在x 方向、y 方向以及转动角方向口的速度矢 量的独立规划生成速度轨迹的。各方向上生成的速度轨迹与起始点与目标点的距 离、初时刻速度、末时刻速度、最大控制量( 最大加速度) 值相关，到达目标点 的最小时间也与这些值相关。 在实际的足球机器人控制中，为了使机器人在运动平稳、灵活，我们必须使 这三个方向上的速度轨迹在时间上同步。如果不同步，在实际的控制中就会出现 一个方向上先达到目标值，然后机器人保持此方向上的值不变，在其它方向上的 继续运动达到设定值的动作。这样机器人的运动从总体上就不是条平滑的曲 线，极大的影响机了器人的寻轨性。为了各方向上的速度轨迹达到时间上的同步， 也就是要使得各方向上计算的最小时间t f 戚。相等。这个最小时间f ，嘶。也就是我们 在这个方向上生成的速度轨迹的设定时间。 一17 中国科学技术大学硕士学位论文 根据公式( 3 2 0 ) ，我们可知起始点与目标点的距离、初时刻速度、末时刻速 度这些边界条件都是已经给定了的，是“任务”设定的和客观实在的数据。所以 必须通过设定各方向的控制量( 各方向的加速度值) 来调节生成的速度轨迹中运 动到目标点的最小时间( 速度轨迹设定时间) ，使得各方向上的速度轨迹同步。 事实上，x 方向、y 方向上的速度轨迹必须时间上统一，转动角方向0 与x 方向、 y 方向不是必须统一，但是统一的话会使得控制更优。因此，我们在实际算法上 使的x 方向、y 方向严格统一，角方向0 上的速度轨迹以x 方向、y 方向上同步 的结果来求解。下面我们来对x 方向、y 方向来做同步处理。 由公式( 3 1 7 ) 和( 3 1 8 ) ，我们可知，如果将控制作用减小，生成的速度轨迹 的设定时间t f 。i 。就会增大；如果将控制作用增大，生成的速度轨迹设定时间t f 。；。 就会减小。 我们为了即满足x 方向和y 方向上的时间统一，又使得这个统一的速度轨迹 时间值最小。势必会采用使得一个方向使用最大控制作用，即使用足球机器人此 方向上的最大加速度值最为此方向上的控制作用；而另一个方向上的控制作用减 小到合适的值，使得x 方向和y 方向上的速度轨迹设定时间上相等。 由上节讨论可知，速度轨迹时间执行时间依靠边界条件和控制量，也就是： t 止( 1 4 1 ) = t f ( z f ，v o ，a z ) ( 3 2 2 ) k ( f 4 i ) = 白( 1 4 i ) ( 3 2 3 ) 由于x 和y 方向还必须满足： 4 2 + 4 2 = l ( 3 2 4 ) k ( 一。( i 瓜i ) = 0 ( 3 2 5 ) 因为t p ( 1 4 i ) 在1 4 l ( o ，1 区间是严格单调递减，而ol i l 一4 2i ) 在此区间是 严格单调递增的，所以o ( 1 4 i ) 一o i l 一4 2 ”是严格单调递减的。因此，只存在 唯一的1 4 i 满足( 3 2 5 ) 。只需要计算出这个k i ，即能生成x 和y 方向同步的速 中国科学技术大学硕士学位论文 度轨迹。 3 2 5 速度轨迹生成算法流程 根据速度轨迹生成算法原理，再将在x 方向、y 方向以及转动角方向口进行 时间同步计算，即可得到各方向上实际可用的速度轨迹。生成的各方向上的速度 轨迹将传送给速度轨迹跟随部分，作为控制的输入量。具体的速度轨迹生成工作 流程如图3 - 3 所示： 。攀粼剃孵驰器瓣攀，i 、善l “i ，。：露豁枣理鲟舔。l 潮遴滕麓璧$ 囊壤黧囊筏黔i l i + 1 鬻鼗辫冀 瓤l ，喜谚i 国进行同皴螋璐一l 鎏黪 j 一褥趱瓣攀麟鬻 l 辫嘲l 箍鬻潲鼗颇爱涵：。 。：。、 。 。1 。 i 躲攀嘲鞭 戮酾鹱醚 r ：嬲鞠黪j i -瓴， 协、_ 、 图3 - 3 速度轨迹生成算法流程 3 2 6 速度轨迹生成算法实验 下面根据本文提出的速度轨迹生成( v t g ) 算法来实验。实验要求机器人在 x 方向行进0 8 米，自转9 0 度。机器人初始速度不为零，要求到达目标点的末 速度为零。实验中使用机器人驱动电机的码盘信号来测算机器人的速度，使用全 局的图像来实时测算机器人的位移。 实验主要参数如下： 起点状态( 速度单位m s ，位移单位m ，角度为r a d s ) ： v ，- - - - 0 0 2 ，v 。20 ，v o = 0 ，x = o ，y = o ，0 = 0 19 中国科学技术大学硕士学位论文 终点状态( 速度单位m s ，位移单位m ，角度为r a d s ) ： v ，2 0 ，v y 20 ，v e = 0 ，x 2 0 8 ，y ：o ，0 ：9 0 系统的固有参数： 帧速率2 5 ，最大加速度2 s 2 ，最大速度1 6 0 m s ，最 大角速度2 3 r a d s ，最大角加速4 1r a d j 2 ，采样点数n = 6 0 。 一 羹 ” i 。 _ ， ? ：，。：椭- 一” 图3a ) 速度一时间曲线b ) 位移一时间曲线 i- 。” 羹 墨” 。 - - - 。 羹- 一亲摔j r - _ 桑，点 c ) 角速度一时i 司曲线d ) 角度- 时i 司曲线 图3 4 运动控制实验数据曲线 使用该算法，得到实验结果如图3 - 4 所示： 图a ) 、b ) 中，t ：o 7 1 m 时刻是x 方向的正负最强控制作用切换点。0 0 7 1 m ， 机器人采用正最大加速度。t ：o 7 1 m 时刻，机器人达到实验过程的最大速度 1 4 9 m s ( 因为实验距离短，机器人未达到自身最大速度1 6 0 m s ) 。0 7 1 m 一0 9 7 m 采用负最大加速度，机器人减速。t ：o