（计算机应用技术专业论文）多机器人协作及环境建模技术研究.pdf

i i i i iiiiii ii i i ill lll l i y 18 0 9 3 8 1 d e g r e eo fd e n g o tc o l l a b o r a t i o na n d m o d e l i n g 1 一 c a n d i d a t e ：z h a n gz i y i n g s u p e r v i s o r ：p r o f z h a n gr u b o a c a d e m i cd e g r e ea p p l i e df o r ：d o c t o ro fe n g i n e e r i n g s p e c i a l t y ：c o m p u t e ra p p l i c a t i o nt e c h n o l o g y d a t eo fs u b m i s s i o n ：o c t o b e r , 2 0 0 9 d a t eo fo r a le x a m i n a t i o n ：d e c e m b e r , 2 0 0 9 u n i v e r s i t y ：h a r b i ne n g i n e e r i n gu n i v e r s i t y 户 l 。 哈尔滨工程大学 学位论文原创性声明 本论文的所有工作，是在导师的指导下，由 。有关观点、方法、数据和文献的引用已在 文献相对应。除文中已注明引用的内容外， 他个人或集体已经公开发表的作品成果。对 本文的研究做出重要贡献的个人和集体，均已在文中以明确方式 标明。本人完全意识到本声明的法律结果由本人承担。 、一 作者( 签字) ：弓杉方醒 日期：2 万己秒年搠钿 哈尔滨工程大学 学位论文授权使用声明 本人完全了解学校保护知识产权的有关规定，即研究生在校 攻读学位期间论文工作的知识产权属于哈尔滨工程大学。哈尔滨 工程大学有权保留并向国家有关部门或机构送交论文的复印件。 本人允许哈尔滨工程大学将论文的部分或全部内容编入有关数据 库进行检索，可采用影印、缩印或扫描等复制手段保存和汇编本 学位论文，可以公布论文的全部内容。同时本人保证毕业后结合 学位论文研究课题再撰写的论文一律注明作者第一署名单位为哈 尔滨工程大学。涉密学位论文待解密后适用本声明。+ 本论文 0 ( 2 5 ) 若时间增量缸足够小，则可采用圆弧模型的近似模型一直线模来描述： 图2 7 机器人运动模型 f i g2 7r o b o tm o t i o nm o d e l 为了简化形式，减少计算量，本文以直线模型为主，同时在位姿方向角推 算过程中使用圆弧模型【1 4 9 1 。在此基础上，考虑里程计系统误差，机器人的运 动模型可以表示为： 厂( 置，a ，) + w f = 幺 毋 比 + 蚓 q 。7 2 5 机器人感知模型 对不确定性进行更新的方法需要借助传感器模型，本文采用声纳传感器 获取外部环境信息，而栅格地图通过贝叶斯方法进行更新。 m c s 坐鹋丝鹋够 + + + 儿 幺 第2 章多机器人体系结构与协作探索技术 束声纳的声波范围分为三个区域，如图2 1 2 所示【15 0 1 。图中r 为最大 离，为声纳视野半角，口和，分别为栅格的距离和偏角。 1 ，一， 一 1 l l一 一一 _ 一q fr 、 一 一 _ 一 1 、lj l ， 一 i ll i量 1 1l r 。 i 一f r - lk【一r一 k j ， l j 憎、 r 1i i ， ， c 1、j，l 一 7 爿 k-一， 、yt鞫 f | lf | i l、rl j 、l擤 七、，f 。 li，i 、jl 。 ll ，l ， v 疆 l 一 t， j o 、r ； 图2 8 声纳传感器模型 f i g2 8s o n a rs e n s o rm o d e l 区域i ：存在障碍物可能性大的区域，该区域栅格被占用的可能性大。 区域：存在障碍物可能性小的区域，该区域栅格为空的可能性大。 区域i i i ：被障碍物遮挡的区域，栅格的概率未知。 为了表示地图栅格的不确定性，需要将传感器的读数转化为概率表示。 对于区域i 的栅格单元下式表示可信度： 对于区域i i 的栅格单元下式表示可信度： x 0 9 8 、c 竿) + ( 等) p ( v a c a n c y ) = 竺_ 上 p ( o c c u p i e d ) = 1 - p ( v a c a n c y ) 例如：声纳声纳的返回值为3 m ，容错度为0 2 5 m ；栅格距机器人 ，= 1 6 m ，口= 0 。，由i i 区公式可得： 口一 一 )笠姗 1 2 聊兰嘶 ，一 1 l l l i 、，、， d ， 抬 缈 p 托 印 船 尸 p 用 被 其 条 假 2 6 本章小结 为了使得多机器人能够有效地完成不同的任务，本章从多机器人系统的 体系结构出发介绍了各种单机器人和多机器人在不同任务下的控制结构；然 后，结合本文工作，设计了合理的单个和多个机器人系统的控制结构，并介 绍了机器人的自主规划技术、环境建模技术以及多机器人的运动协调和任务 分配技术；最后，介绍了机器人运动和感知模型，并对贝叶斯更新规则进行 了必要的说明，为多机器人协作探索任务的研究奠定了一定的理论和实践基 础。 第3 章环境探索中的多机器人运动协调机制研究 第3 章环境探索中的多机器人运动协调机制研究 多机器人探索中上层协作行为的基础是机器人之间的运动协调。其最基 本的要求就是机器人能够安全、可靠地到达目标区域( 或者称探索区域) ，并 且能够维持协作的基本位置关系需求。只有安全及时地到达目标位置，机器 人才能有可能进行进一步的协作。通常，为了维持机器人协作的可能性，通 常要求机器人保持一定空间位置关系进行探索环境，这就需要解决多机器人 的编队问题。总的来说，为了更好地协作探索，多机器人之间的运动协调问 题需要解决以下问题：多机器人的队形控制、机器人的动态和静态避障以及 机器人的路线跟踪等行为。 在未知环境中协作探索的过程中，多机器的编队探索不但可以通过个体 间的信息融合来获得更加准确有效的环境信息；而且可以解决机器人个体无 法并行执行任务的问题，提高了工作效率；同时编队控制还可以通过队形约 束在一定程度上防止死锁和停滞现象的发生，增加了系统的鲁棒性。 在编队移动过程中遇到障碍物时，机器人的运动规划通常把探测到的障 碍物都当作静态的来处理。而在实际应用中，机器人常常工作于动态的、不 确定的环境中，其中一般包含位置未知的静止障碍物和运动轨迹不定的动态 障碍物。当机器人遇到动态障碍物时，容易与其发生冲突或碰撞。因此，未 知环境下的机器人实时运动规划应当包括对静态和动态障碍物的有效避碰。 在多机器人队形控制过程中，已经对静态障碍物的避碰行为做了一定的研究。 因此，为了提高机器人对运动障碍物的安全避碰能力，本章在解决基于约束 控制的多机器人编队控制问题后，进一步对机器人的动态避碰行为进行了研 究。 3 1 多机器人编队问题 多机器人在探索过程中，通过队形控制可以大大提高机器人探索环境的 覆盖率。但是，实现多机器人的编队，仍然需要解决许多问题：多机器人体 系结构、队形种类、参考点的选择及性能评估。 哈尔滨工程大学博士学位论文 3 1 1 多机器人编队的体系结构 多机器人体系结构是实现信息关系传递和控制的基础，这里采用的 智能体的方法实现多机器人的体系结构。其核心思想是把多机器人系统 成多个有自治能力的智能子系统【1 5 1 1 ，如图3 1 所示。 图3 1 多机器人系统体系结构 f i g 3 1t h e a r c h i t e c t u r eo fm u l t i - r o b o ts y s t e m 图中给出了多移动机器人编队系统模块的结构，整个系统分成软件和硬 件两个部分。软件部分包括机器人定位、通讯和运动控制，硬件部分包括网 络、传感器和电机系统。 3 1 2 多机器人编队的队形 多机器人编队首先需要考虑的问题是选择何种队形。适合的编队不但能 保持队形移动的整体性，而且有利于降低能量的消耗。常用的编队队形有线 形、柱形、v 形和菱形等，如图3 2 所示。 第3 章环境探索中的多机器人运动协调机制研究 o ooo o o o o 。 o ooo o oo o ( 曲线形他) 柱形( 菱形( mv 形 图3 2 机器人编队基本队形 f i g 3 2t h eb a s i c f o r m a t i o n s 3 1 3 编队中参考点的选取 多机器人系统在编队过程中，必须选择参考点来实现相关队形的约束。 在多数情况下，多机器人系统中的个体是通过与参考点保持相对位置来实现 期望的队形。通常情况下可以根据不同的需求，选择领航、中心和邻居三种 参考点，如图3 3 所示。 ( 1 ) 领航参考( b ) 中心参考( c ) 邻居参考 图3 3 队形参考点的选择 f i g 3 3t h er e f e r e n c ep o i n ts e l e c t i o ni nf o r m a t i o nc o n t r o l 3 2 基于约束控制的多机器人分布式编队方法 在多机器人协作探索的过程中，可能形成不同的协作机器人子群体( 后 面的章节中称为“簇 ) 。当主动机器人簇在探索当前区域的过程中，为了保 证簇内结构的维持，机器人应当能够保持相对的位置关系。在大量机器人存 在的协作系统中，采用集中控制的方法会使得整个系统的灵活性、可扩展性 和适应性变得较差，而且系统中通信量会随着机器人数量的增加呈指数增加。 为此，从增加系统的可靠性和减少系统的通信量出发，本章采用分布式控制 哈尔滨工程大学博士学位论文 方法。考虑到机器人传感器信息的不完全性，这里假设机器人的距离传感器 可以获得其他机器人的相对距离( 可通过机器人相互定位算法来实现) 、速度 和航行角度。在机器人的局部感知下，可以设计分布式多机器人编队控制。 由于感知信息的不完全性和不确定性，在机器人编队时需要通过必要的通信 来弥补感知的不足，如机器人的编号等信息。因此，本章的多机器人编队控 制方法是在局部感知和通信相结合的交互方式下实现的。局部感知用于感知 其他机器人的相对位置和角度；通信用于给出队形信息和指令等信息。 3 2 1 多机器人编队的行为和结构 3 2 1 1 参考点的选择 如前面讨论，编队控制需要一个参考点。由于分簇协作下管理方式要求 父节点机器人能够管理自己的子节点的行为( 第四章详细介绍) ，因此机器人 在编队中参考点的选取必须包含自己的父节点，如需要其他参考点，则可以 选择相同父节点下的其他节点。由于多机器人的编队采用基于局部感知和通 信相结合的通信方法，因此父节点的确定可以使用有限的通信来实现。 3 2 1 2 簇头机器人运动控制 整个簇的移动受到簇头( 或者称领队) 的带动。而在簇内机器人的树形 管理结构中，父节点的移动带动其子节点。因此，需要对簇头机器人的移动 进行控制，可以使得整体按照希望的路线进行移动。 只要保证簇头可以向着目标点( 或者边界点、目标区域) 移动，整个机 器人簇就可以很容易的沿着指定路线到达目标位置。因此，需要建立机器人 的运动模型。 在如图3 4 所示的机器人系统，其运动学方程设计如下： j = v c o s p 夕= v s i n o ( 3 - 1 ) 矽：缈 其中，0 ，y ，秒) 分别表示机器人的位姿信息，即坐标和方向。，和彩分别为机 器人的平移速度和角度。 图3 4 机器人运动模型 f i g 3 4t h ek i n e m a t i c sm o d e lo f r o b o t 图中0 为参考原点，两轮中心( 即机器人旋转中心) 到参考原点o 的 距离为d 。9 和r 分别为目标g 相对于机器人的相对角度伊和相对距离，这 两个变量则可有如下的微分方程描述： ，；= d c o s i n 够一v c o s 伊 ( 3 2 ) 西：v s i n c p 一彩d c o c o s(3-3)-f 驴= 一一彩。一 t ， 其中，1 ，和国分别为机器人的平移速度和角度。为了使得机器人簇能够到达 目标点，因此控制的最终目标为：，= 0 ，缈= 0 。而为了保证控制的合理性， 可以将控制策略定为如下线性方程： 户= 一以r( 3 4 ) = 一如缈 ( 3 - 5 ) 其中五，以为调整参数，二者均为正的固定值。通过对式( 3 - 2 ) 和式( 3 5 ) 2 行求 解，可以得到当前时刻机器人的控制输入1 ，和缈： ( ，一d c o s c p ) + d 够如s i n 缈 r c o s d 缈：；qs i nc p + 2 2 c p c o sc p ， ，c o s q o d 实际环境中，机器人的平移速度和角速度具有一定的限制， 人的机械特性所决定的。因此可以做出如下的约束： 4 1 ( 3 6 ) ( 3 - 7 ) 这是由机器 哈尔滨1 二程大学博士学位论文 i ，嘣v 1 ，懈 ，= v - - v r e a x v 1 ，眦 ( 3 - 8 ) 卜v 麟， 国眦 国= 国 一缈。缈缈蕊 ( 3 - 9 ) 【国嗍c o 一功麟 3 2 1 3 保持队形 为了在机器人队形保持行为中使用机器人的运动控制模型，需要将机器 人之间的位置约束引入到机器人的运动控制模型中。在，一矽的控制方法中， 机器人的位姿信息为( x i , y ，谚) ，i = 1 , 2 ，定义机器人的参考原点的坐标。机器 人的转动中心位于两驱动轮的中心位置。在图3 5 中，磙示参考点到转动中 心的距离。由于参考点的正方向已经给出，因此途中情况下的d 都是负值。 另外机器人的正前方为速度的正方向，逆时针旋转为角速度的正方向。 y 0 x 图3 5 机器人队形保持，一够控制 f i g 3 5t h e ，一缈c o n t r o lo f f o r m a t i o nc o n t r o l 在无障碍的环境中，机器人采用，一矽的控制方法，通过约束父节点和子 节点之间相对距离和相对角度就可以实现整簇机器人的整体移动，并可以保 证一定的几何约束。但是，当环境中出现障碍物时，一矽的控制方法只考虑 参考自己与参考机器人的关系，无法将环境信息引入到编队的控制约束方程 中。因此，为了机器人有效地避障，还需引入其他的避障规则，这增加了机 第3 章环境探索中的多机器人运动协调机制研究 器人编队控制的复杂性。为此，本章采用了基于，一，控制方法的多机器人队 形保持及静态避障行为，将队形保持和静态避障在相同的约束方程下实现， 简化机器人编队控制难度。这在后面的章节会详细介绍。 3 2 1 4 机器人的避障行为 在机器人的移动过程中，会碰到不同类型的障碍物。前面已经提到，机 器人的，一z 控制可以将障碍物等静态环境信息考虑到机器人的运动控制方程 中，这避免了引入新的编队算法，使得机器人编队控制结构变得更加复杂。 另外，在实际的复杂环境探索过程中，如果机器人遇到动态障碍物( 如人、 非本簇机器人等) ，该方法会由于移动障碍物的信息不完全而使得机器人碰到 障碍。为此，仍需要提高机器人动态避障能力，需要设计机器人的动态避障 算法。 3 2 1 5 机器人编队控制结构 通常，机器人控制结构是用来控制机器人个体行为的“大脑”。智能系统 控制结构来自两个相反的哲学思想：( 1 ) 经典的人工智能方法注重集中处理 和“自上而下的推理。( 2 ) “自底向上”的设计方法，如基于行为的控制结 构。由于本文的机器人交互方式采用局部感知和通信结合的方法，注重局部 机器人个体间的相互作用，因此采用更为简单的自底向上的设计思路更加适 厶 口。 在探索任务中，机器人的编队问题要求成队的机器人按照指定的路线， 移动到目标位置。在移动的过程中，要求机器人能够避开静态或者动态障碍 物，并具有恢复、改变队形的能力。因此，为保证机器人等够满足任务需求， 在基于行为的控制方法中，需要设计以下行为：向目标移动、队形保持、静 态动态避障、路线跟随。 如前所述，机器人的向目标移动行为比较简单，按照机器人的运动控制 模型就可以简单地控制机器人到达目标位置。它保证了机器人簇能够到达目 标位置。队形保持行为根据约束方程保证了机器人之间的相对位置关系，为 机器人协作探索提供了保障。避障行为使得机器人在遇到障碍物时能够绕过 障碍物或者沿着障碍物的轮廓进行移动，有助于实现在不与障碍物碰撞的前 提下对障碍物的完整探索。路线跟踪行为保障了机器人能够按照一定的路线 4 3 哈尔滨工程大学博士学位论文 进行探索移动，在保证队形同时完成有效地环境探索。 因此，这里设计采用基于行为的控制方法，采用行为选择的包容式控制 结构，如图3 6 所示： 图3 6 行为选择的包容式结构 f i g 3 6t h es u b s u m p t i o na r c h i t e c t u r eo f b e h a v i o rs e l e c t i o n 图中s 表示抑制。因此，在机器人控制过程中，通过仲裁器，来判断行 为的优先级，并根据预先设定的优先级来选择优先级最高的行为，从而避免 了不同行为间的冲突。 3 2 2 基于约束控制的队形保持 与，一矽控制方法不同的是，z z 的控制要求机器人的参考机器人数量为 2 ，即机器人要保证同两个参考机器人之间的相对距离为期望值。 在控制过程中，机器人f 需要的信息是自己本地的状态( x iy ，只，v ，q ) 和 参考机器人的信息o ，y j , 秒f ，v ，彩，) u f ) 。其中， ，y ，) 为机器人在坐标系 中的坐标；矽，为机器人的方向角；1 ，和缈，分别为机器人的速度和角速度。 在机器人簇中，当前机器人的参考机器人有两个，一个是其父节点机器人， 另一个是其相邻节点的机器人。当其相邻节点的机器人不存在时，系统会配 置一个“虚拟机器人”在其相邻节点，并按照队形约束给出当前机器人的约 束控制方程。 在假设如图3 7 所示的三个机器人队伍中。d 表示参考点和转动中心的 距离。机器人3 同机器人1 和2 的参考原点之间的距离f 1 3 和，：，分别表示机器 人之间的距离，因为这里规定所有机器人的参考原点都位于机器人的相同位 置。该信息可以动过机器人的距离传感器获得，这样就避开了机器人位置信 ，和仍，分别为机 器人的移动方向 图3 7 机器人队形保持，一，控制 f i g 3 7t h e l lc o n t r o lo ff o r m a t i o nc o n t r o l 在，一，控制算法中，控制目标就是要维持机器人之间的相对参数( ，1 ，1 2 ，) 固定在期望值( 学，z ；3 ) 。为了运算方便并不失一般性，对各个机器人都有 d i | - di = 1 , 2 ，通过分析图3 7 ，可以得到得到如下的运动学方程： z 1 3 2 v 3c o s y i + d s i n y l v 1c o s q ) 1 3 乞32y 3c o s y 2 + d c 0 3s i n y 2 - - y 2c o s 缈2 3 岛= 功3 其中乃= 幺一岛+ 仍3 ，儿= 岛一岛+ 仍3 。 ( 3 1 0 ) ( 3 1 1 ) ( 3 一1 2 ) 由于这里的控制目标是：i ，一口卜。和i 乞，一巧3i 专0 ，所以系统的控制 变量约束方程为： 3 = 口i ( 学- 1 1 3 ) ( 3 - 1 3 ) 如3 = 口l ( 巧3 一乞3 ) ( 3 1 4 ) 其中，q ，口：均为正数。 因而，= - - y p a 从上述的四个式子中得到机器人3 的控制输入变量( c o ，y 3 ) ： 1 国1 = d s i n ( y l y 2 ) k 。( 艺3 1 1 3 ) c 。s 儿+ v 。c 。s 伊1 3c o s ：一口：( 巧，一1 2 3 ) c o s 厂。一v ：c o s p ：，c o s y 。】 4 5 哈尔滨 程大学博十学位论文 仇：一鱼垡2 二! ! ! ! ! 塑丝二丝垡窆二1 2 i 2 1 1 呈丝兰塑! 亟! ! 垫丝二兰竺! 丝j ! 垫堑 s 证一儿) ( 3 1 5 ) 定理3 1 ：假设个机器人的系统中，系统移动机器人运动学方程为公 式( 3 1 ) ，按照公式( 3 1 5 ) 控制规则描述的三个机器人编号分别为f ，_ ，k ，参考 机器人f ，j 速度有下界，；，v j ，当前机器尼与参考机器人的初始相对朝向 有界，即怜( 气) 一以o 。) i i 丑万，i b ( 气) - o k ( t 。) 8 如万有界，以及相对方向 幺一秒，、相对速度u 一，有界，则这三个机器人组成的队形是稳定的，系统 输出3 、1 2 3 会收敛到期望值1 5 2 1 。 证明：定义系统偏差e = ，p 成，0 ，p o 肚】， 其中e 惫= l ：一t 鼍。e 俅= l 营一t 穰e 袅= 9 i 一9 t = e j e t 。 由控制目标公式( 3 - 1 3 ) 、( 3 一1 4 ) 可以看出缸和，肛是指数稳定的。故三个机 器人队形的稳定性等价于机器人k 与参考机器人i ，歹的方向偏差的有界性。 首先分析8 呈。由于e ：= b 一吼，可知 = q 一国t 。 将公式( 3 1 5 ) 下标的1 、2 、3 分别替换为f ，j ，k ，带入上式并经过计算 可以得到 吾：= 一号s ；np呈+缈，一二皇二呈宅嘉罢霉笔萋亏鼍鲁兰舌乏孑兰之亳专等三三宝丢挚 一 丝垒竺! 垒竺壁垒竺! 垒 d ( 幺一a s ) c o s ( q , s t 一缈 ，) + s i n ( f p s k 一) 用符号f ( e 昱) 表示上式等号右边的2 、3 、4 项。则上式的标称系统是在 r ( 口萎) = 0 前提下，给出 叠：= 一导s i n 2 墨 由于两个参考机器人按照既定的路线移动，则两个参考机器人相对方向 毋一秒，、相对速度_ 一y ，有界条件能够满足，因此有 怜一巴0 毛，及一_ 忙印 命题中假设 。) i i 五万。则 上式说明随着时间的演化，系统中机器人k 同其中的一个参考机器人i 航 向角会稳定地保持在一定的偏差内。 同理，可以证明机器人k 同其中的一个参考机器人，航向角会稳定地保 持在一定的偏差f ，内。 得证。 根据定理3 1 ，可以得到机器人k 同两个参考机器人f ，之间的距离会逐 渐收敛到目标值，并且航向偏差也会保持在一定的范围内，只要机器人f 和 机器人，航向一致，三者就能形成稳定的编队。 从公式( 3 1 5 ) 可以知道，一，控制方法仅仅需要参考机器人的相对距离和 相对角度信息就可以实现，这些信息可以通过机器人传感器获得，并不需要 机器人实时的通信，这大大减少了机器人之间的通信量，增加了系统的可扩 展性。 3 2 3 基于约束控制的静态避障 在复杂环境中，障碍物的存在对多机器人的队形结构具有一定的影响。 由于前面采用基于约束控制的方法实现机器人的队形保持，为了方便机器人 的控制，可以采用同样的约束方程实现多机器人的避障行为。这也是本章选 用，一，编队控制方法的原因。 由于在，一z 控制中，跟随机器人受到两个参考机器人的约束进行移动。 当机器人遇到障碍物时，为了能够将，一z 控制思想应用到机器人避障中。这 里将障碍物的边缘垂直与机器人的移动方向上的点虚拟为该机器人的一个参 过程中，如果远离障碍物到达一定程度，此时机器人的避障行为不再产生作 用，此时机器人仍然会恢复避障之前的参考机器人，保证整体的移动效果。 参 人 图3 8 机器人静态避障示意图 f i g 3 8t h ev i e wo fs t a t i co b s t a c l ea v o i d a n c e 因此，机器人的静态避障采用前述的，一z 控制方法就可以实现，此时机 器人的参考机器人仍然为2 个。假设当前机器人编号为2 ，参考机器人编号 为1 ，虚拟机器人编号下标为d 。不同的是机器人同虚拟参考机器人的距离 代表了机器人同障碍物的距离，即 1 0 2 = v 2s i n y o d c 0 2c o s y 。 其中y 。= 岛一0 3 + 仍，儿= 一i ，表示虚拟机器与当前机器人方向相同，保 z ： 证机器人能够绕开障碍物的边界移动。 则按照公式( 3 1 5 ) 可以将机器人的两个控制输入更新为： 4 8 第3 章环境探索中的多机器人运动协调机制研究 ( 3 - 1 6 ) 制输入，机器人2 能够通过沿着障碍物的边缘避开障碍物， 形移动。 在机器人避障时虚拟机器人的产生不需要机器人之间的通信，仅仅采用 机器人的距离传感器信息即可构造。该避障方法使得机器人将环境信息考虑 到队形控制约束方程中，采用相同的控制模型就可以同时完成机器人的队形 保持和避障行为，大大简化了机器人的控制复杂性。 3 2 4 路线跟踪行为 在机器人协作探索的任务过程中，为了能够在探索区域中覆盖更多的区 域，通常在探索区域会按照一定的路线进行探索，如除草机路线。当机器人 遇到障碍物或受到环境的影响，会偏离当前的预定路线。为了使得机器人能 够按照原有的路线进行移动，需要设计机器人的路线跟踪行为。 机器人的路线跟踪行为的目标是控制机器人能够在使得其与既定路线距 离以及方向误差保持在一定的范围内。该行为保障了机器人能够按照预定的 顺序经过最近的子目标点。这意味着机器人会沿着与其既定路线尽可能近的 平行线移动。图3 9 给出了机器人路线跟踪行为的控制过程。其中，d 表示 机器人与当前既定路线的距离，汐表示机器人与既定路线的相对角度。0 表 示控制规则的控制方向。路线跟踪的目的就是在机器人距离既定路线较远时， 机器人控制推进力的距离( 大小) 成分产生主动的作用，使得机器人向该路 线靠近。当机器人距离既定路线太近时，机器人的控制推进力的方向起到主 要作用，使得机器人的移动方向平行于既定路线。 故此，可以设计机器人运动方向的控制规则： 0 = 秘d + 吃p ( 3 1 7 ) 其中，吃和屯是适应性调节因子，根据环境状况动态变化。 另外，路径跟踪算法不仅需要调节机器人的运动方向，需要改变机器人 4 9 图3 9 路线跟踪原理图 f i g 3 9t h ep r i n c i p l eo fp a t ht r a c k i n g 3 3 基于速度空间的机器人动态避障方法 前面的多机器人编队控制中，机器人的避障行为采用，一，的方法，基本 能够解决静态环境下避障要求。但是在复杂环境探索过程中，如果机器人遇 到动态障碍物( 如人、机器人等) ，该方法会由于移动障碍物的信息不完全而 使得机器人碰到障碍。为此仍需要提高机器人动态避障能力。 3 3 1 速度避障的基本思想 对移动的两个物体a 和b ，如图3 1 0 所示。假设在t 时刻其速度分别为圪 和k ，而且他们空间中的位置分别为x 。和x 。，则每个运动物体的状态可采 用其参考点的空间坐标及速度表示。因此，物体的运动可描述为如下的形式： s ( t ) = x ( f ) ，v ( 力】 ( 3 - 1 9 ) 其中矢量x ( t ) = 【x ( f ) ，j ，( f ) 】r 为参运动考点的位置坐标，v ( t ) 为速度矢量。在 以下分析中，把传感器检测得到的运动障碍物简化为质点，并设当前时刻为 5 0 第3 章环境探索中的多机器人运动协调机制研究 t ，机器人和所有运动障碍物的速度在规划的时间问隔丁的时间内保持不变， 而是在r 时刻后发生改变。 a ( 丘) k l 由 b ( x b ) 图3 1 0 由位置和速度矢量表示的状态空间 f i g 3 10s t a t es p a c ee x p r e s s e db yp o s i t o na n dv e l o c i t yv e c t o r 在图3 1 1 中，全局坐标系中，当前t 时刻移动机器人r 位于点 x ，= ( ，y ，) ，其速度矢量为v ，。同时，运动障碍物d 位于点x 。= ( x o ，y 。) ， 速度为v 0 。由于机器人的位置以其中心点的位置计算，因此需要将障碍物进 行膨化，则可以将机器人模型化一个质点来分析系统的动态变化。简单地， 障碍物d 根据机器人的半径进行“膨化”，其半径延伸为r 。，并称“膨化”后 的障碍物为机器人的一个扩展重构障碍物e o 。定义z 。，和乙，是机器人与障碍 物e o 两侧切线方向的射线。 图3 1 1 基于相对速度信息的避碰规划 f i g 3 11d y n a m i cc o l l i s i o na v o i d a n c ep l a n n i n gb a s e do nr e l a t i v ev e l o c i t y 为了使得机器人能够躲避运动的障碍物，需要声明以下几点定义： ( 1 ) 相对速度 5 1 图3 1 2 两个目标间的碰撞关系 f i g 3 12c o l l i s i o ng e o m e t r yb e t w e e nt w oo b j e c t s 如图3 1 2 所示，a 、b 为两个匀速移动的物体，速度分别圪和，方向 角分别为口和。两者之间连线的动态变化可以用以下的运动学方程表示为： 一= 户= e o s ( p - o ) 一c o s ( c r 一目) ( 3 _ 2 1 ) = r o = s i n ( f l 一目) 一s i n ( a 一目) ( 3 _ 2 2 ) 其中昨和分别表示b 对于a 在两者连线上的相对速度和相对角度。在每 个瞬时时刻，点目标的速度、口和可以假定为常数，因此给出式( 3 2 1 ) 和式 ( 3 2 2 ) 的微分方程可以得到 = o v bs i n ( 一0 ) 一o v s i n ( 口一0 ) = o v o ( 3 2 3 ) = - 0 c o s ( p 一目) + o v ac o s ( 口一0 ) = 一目 ( 3 - 2 4 ) 第3 章环境探索中的多机器人运动协调机制研究 由式( 3 - 2 3 ) 和式( 3 2 4 ) 可以得到 + = 0 ( 3 2 5 ) 对式( 3 2 5 ) 求解微分方程，可以得到 咋+ 嘭= c( 3 2 6 ) 其中c 是常数，说明一和圪在其构成的空间中，始终在以原点为圆心， 嘿+ 嘿为半径的圆上，如图3 1 3 所示。和o 为某一时刻z 。时的相对 速度和相对角度值。 k 厂、。 。 图3 1 3 圪，形的运动轨迹 f i g 3 1 3a t y p i c a l ( ，) t r a j e c t o r y 引理3 1 ：在和构成的空间中，满足= 0 的点的集合为该平面的 稳态点集。 证明：由于= 厂痧，当= o 时，意味着痧= 0 ，代入公式( 3 - 2 3 ) 和式( 3 2 4 ) 可以知道吃= 0 ，吃= 0 。 引理3 2 ：如果两个物体分别以恒定的速度( 包括大小和方向) 移动，则 两者相互碰撞的充要条件是= 0 ，以及。 0 。 证明：充分性。由引理。= 0 意味着对任何时间空间中的点为稳定点， 即巧和不变，因此对任意时刻都有巧= o 和杉 0 。另外，= 0 说明两 点连线不会发生旋转，形 0 说明两点连线会逐渐变短。因此说明两点会发 生碰撞。 必要性。假设两点在时n t 发生碰撞。在初始时刻到达t 的过程中( 这个 过程中的任意时刻表示为t ) ，两点之间的距离逐渐变短，这意味着 ( f + ) t 。的这种情况，两点的连线都会与t 。时刻两点的连线平行，因此 有( f ) = 0 。因此由引理3 1 可以知道o = 0 ，以o 0 。 得证。 图3 1 4 目标间的避碰关系 f i g 3 1 4c o l l i s i o ng e o m e t r yb e t w e e n ap o i n ta n dac i r c l e 如图3 1 4 所示，d 为一个运动的点，r 为中线点在尸、半径为尺的圆。 0 和r 的速度分别为v o 和诈，方向角分别为口和。对圆上的任意一点c ， 假设o p 与c o 之间的夹角为( 符号逆时针为正，顺时针为负) ，o p 与水平 轴夹角口，可以得到c o 连线上的相对成分 v , ( 0 6 3 = 咋e o s ( 7 一( 护+ ) ) 一v oc o s ( o ! 一( p + 矽) ) ( 3 2 7 ) v o ( o c ) = 咋s i n ( 7 一( 秒+ ) ) 一v os i n ( a 一( 汐+ 矽) ) ( 3 2 8 ) 引理3 3 ：对移动的物体0 ，如果它将与圆r 相撞，当且仅当在圆上存在 一点c ，且c o 穿过圆r ，有昨( d c ) 0 及v o ( o c ) = 0 。 证明：同引理3 2 。 另外，令a 和b 分别为0 作r 的切线的两个切点，o a 与o b 连线上的相 对成分分别为v , ( o a ) 、v ( o a ) 和v r ( 0 8 ) 、( ) ，且洲与同o p 之间 的夹角大小为y ，同样符号逆时针为正，顺时针为负。这样可以得到如下结 论： 引理3 4 ：在任何时间，如果有v o ( o a ) v o ( 0 8 ) 0 ，则在圆1 1 存在只存 在一个点c ，使得( d c ) = 0 。 证明：考虑任意一点c ，在彳和b 弧线之间，则有一y ，。由公式 ( 3 2 8 ) 可以知道，p c ) 是的连续函数。因此由中值定理， ( o a ) v ( o b ) 0 意味着至少存在一个点c 使得( d c ) = 0 。 第3 章环境探索中的多机器人运动协调机制研究 证明唯一性。令g ( o c ) - - - 0 ，带入公式( 3 - 2 7 ) ， tan矽：vrsin(fl-0)-vos i n ( a - o ) ( 3 2 9 ) 咋e o s ( ，一秒) 一v oe o s ( a 一目) 、。 t a i l 是周期为万的周期函数，假设( d c ) = o 出现在 一三， z f 7 ， n 痧y ，而y = a r c s i n ( 竺) 。由于，r ，因此0 。，s9 0 。在 石 一号，三 区间a r c s i n ( 工) 是单调函数，因此仅仅存在一个使得( d c ) = o 。 得证。 将v o ( o a ) v o ( o b ) 0 中的v o ( o a ) 和v o ( o b ) 分别用公式( 3 - 2 7 ) 进行替 换，这里分别用y 和一y 代替，因此可以得到 咋s i n ( p 一( p + r ) ) s i n ( p 一( 口一y ) ) 一v o v r s i n ( a 一( 0 + y ) ) s i n ( p 一( 护一7 ) ) + s i n ( p 一( 0 + d ) s i n ( a 一( 秒一厂) ) 】 ( 3 - 3 0 ) + 吃s i n ( a 一( o + d ) s i n ( a - ( 0 一厂) ) 0 由罩j ( 卯) = 咋c o s ( 一9 ) 一v o c o s ( d r 一0 ) 。i v o ( o p ) = 咋s i n ( f l 一目) 一v os i n ( a ：一目) | c o s ( 2 7 ) = 1 2 ( s i nr ) 2 = ( ，2 2 r 2 ) r 2 ，使用三角函数积化和差公式可 以得到 厂2 曙( 卯) r 2 哆( 0 p ) + 曙( 卯) 】 ( 3 3 1 ) 由于p 是圆r 的圆心，为了简单可以省略标记o p 。 得证。 引理3 5 ：如果一个点和一个半径为r 的圆都以恒定的速度移动，如果 有一个时刻满足公式( 3 31 ) ，则在以后的时刻都会满足公式( 3 31 ) 不变化。 证明： 令 厂( f ) = ，2 瑶( 一r 2 哆( ) + 曙( d p ) 】 ( 3 - 3 2 ) 对公式( 3 3 2 ) 作关于时间t 的导数： 当机器人在冲突区域中，则采用局部规划产生的“避障规则”进行障碍 物的规避，否则机器人继续按照原有的航行路线移动，不需要产生避障行为。 ( 3 ) 速度空间 与传统的避障算法不同，基于速度空间的机器人动态避障方法需要构建 速度空间。最终，从速度空间中选取合适的速度矢量，来达到机器人避障的 效果。前面已经给出了在相对速度前提下，机器人与障碍物碰撞的条件。因 如 度子空间 可以说，如果机器人的速度满足圪c r ，即机器人速度为其避碰子空间 中的元素，机器人将与障碍物e o 发生碰撞，否则不会发生碰撞。 图3 1 1 为机器人在速度空间中的单个运动障碍物情况下的动态避障示意 图。而图3 1 5 则给除了多运动障碍物存在情况下的避障示意图，其中障碍物 1 的速度为q ，机器人与它之间的相对速度为v 三，相应的碰撞相对速度子 空间为c - z 1 。障碍物2 的速度为v ：，机器人与它之间的相对速度为v 三，相 应的碰撞相对速度子空间为c z ，。根据前面的分析，可以看出吒c z l 并且 v 三c z ：( 即机器人与障碍物1 和障碍物2 的相对速度分别位于相互的碰 撞相对速度子空间中) ，如果在下一个时刻v r 保持不变( 即v 二与v 三保持不 变) ，则机器人将与障碍物l 和障碍物2 发生碰撞。如果把叱与v 三在速度 空间中的点调整到c z 。和c z ，之区域之外，则机器人能避开与二者即将发生 的碰撞。 机器人 图3 1 5 多动态障碍物的避碰规划示意图 f i g 3 15d y n a m i cc o l l i s i o na v o i d a n c ep l a n n i n gw i t hm u l t i o b s t a c l e 通过以上分析，可以得出以下结论： 5 7 即运动方向和速度。在室内环境中，运动障碍物包括可以同当前机器人交流 的其他移动机器人和无法同机器人交流的人、移动物体等。因此，机器人获 得运动障碍物运动状态的方式可以有两种：( 1 ) 通信；( 2 ) 感知。通过通信的方 式获得的是其他移动机器人的运动状态信息，这种方法很容易实现，不需要 介绍。通过感知不仅可以获得动态障碍物的运动状态，也可以获得运动机器 人运动状态。 根据声纳传感器特征，需要将机器人感知范围内的读数信息做一定处理， 转化为障碍物合理表示，使得机器人能够根据前述的速度避障基本思想实现 有效的动态避障。 为了获取局部环境信息，需要对基于声纳传感器扫描得到的局部环境信 息进行提取，为机器人建立准确的环境模型做好准备。图3 1 6 中g ，y ，) 为机 器人的中心位置坐标，o 为机器人的方向角，足为机器人自身半径。假设机r 器人距离传感器扫描范围为2 妒( 妒 9 0 0 ，这里假定扫描范围1 8 0 0 ) ，并且角度 扫描间隔为妒，则共可以得到。个扫描点： 虬：翌+ 1 ( 3 3 6 ) 。 a c o 第3 章环境探索中的多机器人运动协调机制研究 图3 1 6 机器人局部环境示意图 f i g 3 16t h el o c a le n v i r o n m e n to fm o b i l er o b o t 在这个点中，每个点都对应移动读数，即声纳传感器的距离信息。定 义从坐标系中x 轴正向开始，逆时针对扫描点依次进行标记为 0 ，2 ，f ，一，z 。，其中扫描点f f 以扫描角仍和相应的测量距离d ，来表示，因 此t 的极坐标形式可以写成如下形式： 厶= 勋f ，d f ) | 缈 = ( f 一1 ) 汐，d j d 。璃。，0 f m ( 3 - 3 7 ) 其中d 嘲。为最大测量距离上限，若d ， d m 毂时，( 纪，d ，) 可以视为一个障碍物 点，否则认为没有障碍物；仍为相对于坐标系x 轴的偏角，当纷= b 时与 机器人同向。 扫描数据也可以用简单的向量进行表示，即 l = 【d l ，d 2 ，d i ，j 。】7 吼“。 l 表示了整个扫描空间中所有扫描点的距离信息。这样就省去了各个扫 描数据的角度信息，减少了存储空间。 则扫描数据的每个点对应的坐标可以计算为： y 篓三y ：d d ：i s i n 义( 丢男。 z m c 3 3 8 ， 【f =，+f 够) 。 、7 为了近似的表示障碍物的位置信息，在l 中读取连续d ， d 嗽的数值， 并记录连续数值的个数。假设连续d ， d 瑚。数值读数个数为n ，分别为 5 9 见：a r c 伽丛血 ( 3 3 9 ) x t x t + 上述公式仅仅是将两次测量的障碍物信息进行计算，并没有确定出两次 测量的信息是否来自一个障碍物。为此，需要增加一个判定条件来判定这两 个读数是否来自同一个障碍物。假定过去测量的障碍物速度为圪，给定一个 速度变化的允许误差占，则如果 l 圪一v o 峰占 则认为两次的读数信息来自同一个障碍物。此类方法可以扩展到多个障碍物 的判定上。 3 3 3 速度空间的避碰规划 前面定义了机器人动态避障的基本思想，为了能够实现该思想，需要给 出机器人动态避障规则。为不失一般性，考虑多个运动障碍物的情况进行描 述机器人的动态避障规则。 根据引理3 7 ，对于机器人与个运动的障碍物 q ，o n ，分别计算 出o i ，f = 1 , 2 ，对应的e o , 、c z ，) 及c r g ，则机器人与多个运动障碍物的避 碰条件为： 6