人工力矩队形控制方法的改进

1、摘要组群机器人系统的队形控制是一个具有广阔应用前景和富有挑战性的研究方向，作为一个具有广阔应用前景和富有挑战性的研究方向，目前受到普遍重视。然而目前虽然有很多的队形控制方法，但却没有通用且特有效的方法，针对这一问题，人们提出了许多的解决方法，其中基于人工力矩的队形控制方法，作为一种最近提出的队形控制方法，不但通用性强，控制灵活，而且具有稳定性。本文的主要工作有：1） 定义 了感 觉障 碍 区间 、 无偏 障碍点 等 新概念 和一种 新的 人 工力矩 排斥矩，得到了求无偏障碍点感觉排斥线段方向角的定理和方法。2） 给出 了一 种新 的 系统 模 型 人才 市 场式组群机器人系统 模型，用来动态确

2、定元素间leader, follower关系。3）将人工力矩法做了有效改进，给出了改进后各机器人的运动模型。4）设计了一个多机器人系统的仿真平台，并应用该平台对改进后的人工力矩法作了仿真。理论和仿真结果表明，改进后的人工力矩法，控制更灵活，系统更稳定，机器人的避障与容错能力也得到了显著提高。关键词： 群体机器人; 社会势场 ; 形成控制 ;人工力矩法; 对行为的控制improvement of the artificial moment method for multi-robot formation control abstract for swarm-robot formation con

3、trol, a useful and challengeable subject, has attracted many attentions. many methods, such as the artificial moments method, have developed to solve the problem. although the artificial moments method is a general, flexible and stable method. the main contributions of this paper are the following：1

4、) many new conceptions such as sensing-obstacle region, non-deflection point, are defined. a new type of artificial moment: repulsion moment is given. theorems for calculating the direction angle of sensing repulsion segments are concluded. 2) a novel model: talent-market multi-robot model is given,

5、 which can determine leader-follower relations between elements of the system. 3) the articial moments method is improved, and motion controllers for robots are designed. 4） a simulation platform for multi-robot systems is designed, on which a simulation for the improved artificial moment method is

6、carried out.the theory and simulation results show that the improved artificial moment method is more flexible and stable. the ability of robots obstacle-avoidence is also enhanced. key words:robotics, formation control, artificial moments method, generalized quadratic, obstacle avoidence 目录摘要abstra

7、ct 第 1 章绪论1 1.1 选题意义1 1.2 现有组群队形控制方法简介3 1.3 本章 小 结5 第 2 章基于人工力矩的多机器人队形控制6 2.1 人工力矩法6 2.2 系统模型与相关定8 2.3 可行变化方向的计算 14 2.4 仿真 .17 2.5 本章 小 结22 第 3 章人工力矩法的改进与应用 23 3.1 感觉障碍区间、排斥矩及相关概念23 3.2 无偏障碍点感觉排斥线段方向角的计算26 3.3 人才市场式多机器人系统模型31 3.4 人工力矩法的改进 36 结论与展望 41 致谢 42 参考文献 43 附录 a 外文原文45 附录 b 中文翻译46 第 1 章绪论1.1选

8、 题 意 义机器人作为人类20 世纪最伟大的发明之一，在短短的40 年内发生了日新月异的变化。发展机器人技术已成为高技术领域中具有代表性的战略目标之一。机器人技术的出现和发展，不但使传统的工业生产面貌发生了根本性变化，而且将对人类社会产生深远的影响。随着社会生产技术的飞速发展，机器人的应用领域不断扩展。从自动化生产线到海洋资源的探索，乃至太空作业等领域，机器人可谓是无处不在。就目前的机器人技术水平而言，单机器人在信息的获取、处理及控制能力等方面都是有限的。对于复杂的工作任务及多变的工作环境，单机器人的能力尤显不足。于是人们考虑由多个机器人组成的群体系统通过协调、协作来完成单机器人无法或难以完成

9、的工作。所谓队形控制就是指多个移动机器人在前进的过程中, 保持某种队形, 同时又要适应环境( 例如障碍物) 约束的控制技术。群体机器人系统比单机器人系统具有更强的优越性。体现在：能力倍增、提高效率、节约成本、操作简便等等。本文基于稳定性这一系统目标，研究了组群机器人队形控制系统的设计。本文之所以选择这一课题是因为复杂组群机器人系统的队形控制，是二十世纪九十年代以来一个备受重视的研究方向，是因为组群机器人之间形成并保持一定的队形具有很多的优点，如：1）能充分获取当前的环境信息，对于实现侦查、搜寻、排雷、安全巡逻等有利；2）在对抗性环境中能增强组群机器人系统抵抗外界进攻的能力；3）在一些具体任务中

10、，形成并保持一定队形可使组群机器人系统更加有效地完成任务，缩短任务的执行时间，提高系统的效率；稳定性是该类系统设计的首要目标。研究基于稳定性的条件下，该类系统的设计及分析技术，不但对一般复杂系统的设计有借鉴作用，还对飞行器或卫星等的编队有指导意义，因为飞行器或卫星也可以看作机器人。所以研究该类系统的设计、分析技术，本身是非常有意义的。而稳定性，则是该类系统设计必须首先达到的目标。然而现在虽然有很多的队形控制方法，但却没有一般且特有效的方法，一些方法甚至不能保证系统的稳定性，从而使得灵活的队形控制难以实现，所以基于稳定性这一系统目标，本文对该课题也进行了比较深入的研究，其核心工作是以稳定性作为系

11、统设计的首要目标，首先提出了一种新的队形控制方法人工力矩法，据该方法而设计了一个离散的非线性的队形控制系统。然后基于liapunov稳定性的一般理论，通过变换，把原系统分解成三个子系统，通过构造出各子系统的李亚普诺夫函数而证明了原系统的稳定性。最后定义了距离最优对应，给出了一种求距离最优对应的有效方法淘汰法，并据距离最优对应和淘汰法而给出了组群机器人系统的动态模型，从而增加了队形控制的灵活性和系统的稳定性。本文的成果在一定程度上将有利于非线性定常系统稳定性理论、组群机器人队形控制理论及相关理论的发展。1.2 现有组群队形控制方法简介自从20 世纪60 年代初，第一台工业机器人发明以来，机器人的

12、发展已经经历了近半个世纪，机器人技术也得到了巨大的发展。随着人类对机器人要求的不断提高，机器人生产线的出现和柔性制造系统的应用，人们已经不再局限于单个机器人，而对由多个机器人组成的系统越来越感兴趣。而队形 控 制 问 题 作 为组 群多 机器 人 系 统典 型性 和代 表性 的研 究问 题之 一和一 个具有广阔应用前景和富有挑战性的研究方向，自二十世纪九十年代以来受到了众多研究者的关注和重视，产生了多种控制方法，这些方法基本上可以划分成如下几类：1） leader-follower方法该方法的基本思想是将队形控制问题转化为follower跟踪 leader 的位置和方向问题，优点是队形的运动完

13、全由leader 的轨迹确定，控制简单，缺点是 leader 不容易得到follower的跟踪误差反馈，同时一个leader 的错误，则可能导致整个任务失败。2）基于行为的队形控制方法该方法的基本思想是机器人的控制器是由一系列行为组成，每个行为有自己的目标或任务，其输入可以是机器人的传感信息，也可以是系统中其他行为的输出；其输出或送到机器人的效应器以控制机器人的运动或作为其他行为的输入，从而构成了互相交互的行为网络。该方法的优点是并行性、分布性和实时性好；缺点是难以明确设计出能合成指定队形的局部基本行为和局部控制规则，队形控制系统的稳定性也得不到保证。3） virtual structure

14、法该方法的基本思想是将机器人团队的队形看作是一个刚体的虚拟结构，每个机器人是虚拟结构上相对位置固定的一点。当队形移动时，机器人跟踪对应刚体固定点的运动即可。该方法的优点是可以容易指定机器人群体的行为，并可以进行队形反馈，取得较高精度的轨迹跟踪效果；缺点是当前采用集中式控制，要求队形按一个虚拟结构运动，缺乏灵活性和适应性。4）基于图论的队形控制方法该方法的基本思想是首先利用图上的节点表示机器人的动力学或动力学特性，节点之间的边表示机器人相互之间的约束；然后再利用图论和控制理论知识对以图形表示的队形进行稳定性分析，进而得到控制策略。该方法的优点是利用图能表示任意队形，且有图论成熟的形式化理论作为基

15、础；缺点是主要限于仿真研究，实现比较困难。5）人工势场法该方法的基本思想是借鉴物理学方面的概念，环境中的障碍物对机器人产生排斥力，目标点对机器人产生吸引力，在合力的作用下，机器人沿最小化势能的方向运动。该方法的优点是计算简单，便于实现实时控制；缺点是势场函数的设计比较困难等。另外，还有其他一些类型的方法，如基于队形向量的控制方法等，同时在机器人的队形控制过程中，各种方法也往往是综合运用，限于篇幅，本文不再详细介绍。1.3 本章小结本章主要是 为了讲解选 题的意 义和 现有组群队 形控制方法 简介 ， 现 有 群组队形的 控制方法主要 有 5 中 ，分 别为leader-follower方 法；

16、基于行 为的队形控 制方法； virtual structure 法； 基于 图论 的队形控制 方法；人工 势场法。第 2 章基于人工力矩的多机器人队形控制2.1 人工力矩法设计一个队形控制系统，稳定性是系统设计必须首先要达到的目标（以能否形成目标队形作为系统的稳定性标志）。又根据吸引线段式主- 从队形图和定义1.4 知：只有当r中所有普通机器人所受的合力矩都达到最大值时，所有机器人的基本运动方向线的方向才都相同，各普通机器人才都恰位于其leader线段的终点上，机器人群r才具有指定队形的吸引线段式主- 从队形图，即具有目标队形，所以为了保证整个队形控制系统能稳定地形成目标队形，各普通机器人，

17、须要沿着能使自身所受合力矩增大的位置和方向变化。这就是人工力矩法控制机器人队形的基本原理。进一步介绍该方法以前，在本章中先作如下三个假设。假设 1 r中每个普通机器人都有全方位感应器，能感知它的leader和follower当前时刻的位置，其中虚拟机器人也可被它的follower感知到；假设2 r中每个普通机器人都有基本的通信功能，由此能得到自己的leader(follower)线 段 的 长度 及 对 应 的吸 引 角 ， 当 前 时 刻自 己leader和follower们的基本运动方向线的方向和领队r0运动步幅等信息，另外还有必需的数据处理和驱动等功能；假设 3 环境为理想环境，即不存在

18、各种静态、动态障碍物；机器人在一个时间段( tk, tk+1 内的变化是理想变化，即不会出现超出它的能力的变化要求，也不会在变化中与其他机器人碰撞。根据上述有关定义和假设，则人工力矩法可详述如下。假设目标队形已 预 先 给定， 并 得到了 目 标 队 形的一 个吸引线段式主 - 从队形图。设time= tk| k=0，1， 为离散时间点集，则在一个时间段 ( tk ，tk+1 内，任意机器人的基本运动方向线的方向都不改变，位置沿着一条直线匀速变化。在时刻tk向时间段 ( tk ，tk+1 过渡的瞬间，各机器人的基本运动方向线的方向和运动状态才可能变化。由于领队r0的任务是引导普通机器人群的运动

19、，所以 r0在每个时间段内总沿着需要的方向、以需要的步幅运动，而不受其他机器人的影响。设 s 0 表示 r0的基本运动步幅，0( k) 、s0( k) 为 r0的 pmdline 的方向角和基本运动步幅的变化量（由高层控制器决定），则r0运动控制器的数学模型为)1(sin)()()1()1(cos)()() 1()()(dgl)1(00000000000kksskykykksskxkxkkk（2.10 ）定义 2 （可行变化方向）设 rj为任意普通机器人，它的leader为 ri，m （ m 0）个 follower分别为1hr，2hr，mhr，同时假设p( k+1)=p( k) ，xpj(

20、k+1)= xpj( k) ， ypj( k+1)= ypj( k)( p=i ， h1， hm) ，)()()1()()()1()()(dgl)1(kykykykxkxkxkkkjjjjjjjjj（2.11 ）如果对任意小的正数0， 按 （2.11 ） 式求得 (j( k+1) ， xj( k+1) ， yj( k+1)t和按（ 2.9 ）式求得pj( k+1) 、 pj( k) 后，有pj( k+1) pj(k) ，则称 (j( k) ，xj( k) ，yj( k)t为 rj的可行变化方向，为步长因子。根据人工力矩法的基本原理，普通机器人，为了保证整个队形控制系统的 稳 定 性 ， 首 先

21、应 沿着 它的 可 行 变化 方向 变化 位置 和基 本运 动方 向线的 方向，以使自身所受的合力矩能不断增大。在沿着可行变化方向运动的同时，它还应努力与系统保持速度匹配，所以对任意普通机器人rj，其运动控制器的数学模型为)()1(sin)()()1()()1(cos)()() 1()()(dgl)1(00kykksskykykxkksskxkxkkkjjjjjjjjjjj， （j =1, , n） （ 2.12 ）其中 (j( k) ，xj( k) ，yj( k)t、分别为rj的可行变化方向及步长因子。注 1.7 各个机器人在各自的队形控制器的作用下，都完成了一个时间段的变化，则称队形行进了

22、一步。重复上述过程，直到任务结束。2.2 系统模型与相关定义为了能方便地描 述 吸 引线段 式 主- 从队形 图，下面首先给出一 条有向线到另一条有向线的角的定义。定义 2.1一条有向线（射线、有向线段）绕着它的端点旋转到与另一条 有 向 线 的 方 向相 同时 所转 的 绝 对值 最小 的有 向角 称为 该有 向线 到另一 条有向线的角；一条有向线到全局坐标系x 轴的正半轴的角称为该有向线的方向角。注 2.1规定一条有向线到另一条有向线的角，如果是沿顺时针方向旋转成的则为正角，沿逆时针方向旋转成的则为负角。注 2.2 设ij表示有向线li到 lj的角，则规定 -ij。设i为有向线li的方向角

23、，j为有向线lj的方向角，函数dgl( x)=时当时当|)(sign2|xxxxx (2.1) 则据定义2.1 有ij=dgl(i-j) (2.2) 定义 2.2设在一个2 维平面中，有一个虚拟机器人r0（可用机器人群围捕的敌人、引导机器人群运动的光点等移动物体、机器人群形成队形的目标 地 点 或 指 定 的 运 动 路 线等 担 当 ， 具 体 由 外 界 控 制 者 指 定 ） 和n 个 节 点r1/， rn/。集合r/= r0，r1/， rn/ ， r/ 中任意元素rp/用其位置所在点来表示，并仍记为rp/； r/和其中元素rp/ 有如下特征：1） r0在 r/ 中没有leader ，其

24、他元素在r/ 中都有且只有一个leader ，它则为leader的 follower；整个集合r/，如果依照leader 、follower 关系对其中元素进行划分，则是一棵以r0为根的树 ; 2） rp/有且只 有一条以 它所在位 置 为端点 的射线，称为基本 运动方向线( 简称为pmdline) ，记为op/；3）如果 rp/有一个follower（leader） ，则对应它的follower(leader)，rp/有一条以它所在位置为起点，具有给定长度且到op/的角（此角称为吸引角）也为给定值的有向线段，称为主（从）吸引线段。 如果 rp/有 m个 follower，则对应每个follo

25、wer，它都有一条主吸引线段，所以共有m条主吸引线段。设r/中所有元素的主吸引线段组成的集合为ls，则ls中任意两条线段，如果不考虑它们的端点（起点与终点），则没有公共点。设ri/是rj/ 的 leader ，aij/是ri/ 对应rj/ 的主吸引线段，则称aij/是rj/的 leader线段；同时设ij是 aij/的吸引角，线段bji/是 rj/ 对应 ri / 的从吸引线段，ji是 bji/的吸引角，则 |aij/ |=| bji/ | ，|ij-ji|=4）在任意时刻，r/ 中所有元素的pmdline 的方向都相同；除r0外，所有元素都恰位于其leader线段的终点上。那么 r/ 中的所

26、有元素在该平面内形成的图即为一个吸引线段式主- 从队形图 ( 简称为alfgraph) 。如在图1 中，集合 r/= r0r1/, r2/, r3/, r4/ 中的元素形成的alfgraph如图2.1 （a1）所示；其中节点r2/的完整模型如图2.1 （a2）所示，它的pmdline 为 o2/，从吸引线段为b21/，其终点为b21/；它有两条主吸引线段，分别为 a23/、a24/，对应的吸引角分别是23、24，终点分别是a23/、a24/。（ a1） alfgraph （a2）r2/的模型图图 1 alfgraph 图及其中节点r2/的模型根据定义2.2 ，则本章的组群机器人系统模型可描述如

27、下：设在该平面中，除了集合r/ 外，还有一个集合r= r0，r1， rn ，其中 r1， rn都为普通机器人，r0即为r/中虚拟机器人，并称为领队；r 中元素都用一个圆来表示，圆心为该元素的位置；r 中元素除都有且只有一条pmdline 外（可在普通机器人上，装一根指针类的物体，以指示该机器人的pmdline 的方向，通常情况下，r0的 pmdline 的方向为其运动方向），还有如下特征：1） 、r 中每个元素在r/中都有一个节点与之对应，且是一一对应（ r0看作与自身对应）；2） 、设 ri与 ri / 对应，那么r i不但有与ri /相同数目的主（从）吸引线段，且ri与 ri /的主（从）

28、吸引线段一一对应，对应主（从）吸引线段的长度、吸引角分别相等；3） 、同时设rj与 rj /对应、 ri/的 follower是 rj/，则 ri的 follower是 rj,ri是rj的 leader ；再设rj/的 leader线段为aij /，aij 是ri所有且与aij / 对应的主吸引线段，则 aij是 ri对应 rj的主吸引线段，同时称aij为 rj的 leader线段，rj的对应ri的从吸引线段bji称为ri的 follower线段。注 2.3 因为领队的作用特殊，其微小的扰动可能引起队形较大的扰动，所以在吸引线段 式主 - 从队形 图 中，领 队是由行 为不受扰动影 响的虚拟机

29、器人担当。注 2.4 一个吸引线段式主- 从队形图唯一地确定了一个机器人队形，而任意一个机器人队形都可用一个吸引线段式主- 从队形图来表示，但不唯一，选择时以方便实际应用为标准。定义 2.3 设机器人rj与节点rj /对应，则rj /称为 rj的队形位置。根据定义和吸引线段式主- 从队形图的定义知：虽然一个吸引线段式主-从队形图的位置 和方 向随 着 r0的位 置及其 基本运动方向线的 方向的变化而变化，但其基本形状却是固定不变的，由r0和各个节点的模型及它们之间的leader 、follower关系唯一确定，所以它不但能表示队形运动方向不变的队形，而且能表示队形运动方向随时间变化的队形。另外

30、为了方便，在此对后文中的有关符号作如下规定：ri是 r 中元素，rj是 r中普通机器人， 如果 ri是 rj的 leader(follower)， 则 ri对应 rj的主 ( 从 )吸引线段的终点为aij（bij） ，在全局坐标系中，( xij( k), yij( k) 表示点aij（bij）在 tk时刻的实际位置坐标，i( k) 、( xi( k), yi( k) 、 （xi( k) ，yi( k) ）分别表示ri在 tk时刻 pmdline 的方向角、实际位置和队形位置的坐标。定义2.4 （主吸引矩、从吸引矩）设1、2、3、 、 d 都为正常数，且12都远小于2，32，、1，函数 agl(

31、x)=xd2（ 2.4 ） apow(x)=时当时当时当2|22|)cos(|22)cos(112211xxxxxx（ 2.5 ）bpow( x)=时当时当时且当时当|)(|)2(|)(|2|)(|2(2|)(|)(cos(|2333222222xsignxxsignxxsignxxsignxxxsignxx（ 2.6 ）则 1 ）设 ri是 rj的 leader ，apij( k)=apowji(k)+apowagl(xj( k)- xij( k)+apowagl(yj( k)- yij( k) (2.7) 则 apij( k) 称为 tk时刻 ri对 rj的主吸引矩。2）设 rh是 rj的

32、 follower，bphj( k)=bpowjh(k)+bpowagl(xj( k)- xhj( k)+bpowagl(yj( k)- yhj( k) (2.8) 则 bphj( k) 称为 tk时刻 rh对 rj的从吸引矩。注 2.5 函数 y1= apow( x) 、y2= bpow( x) 的图象如图2.2 所示。图 2 函数 y1=apow( x) 、 y2= bpow( x) 的图象主吸引矩和从吸 引矩都 为 人工力 矩 。设rj为任 意普通机器人，则 它在tk时刻受到的主吸引矩和所有从吸引矩的和为rj在 tk时刻受到的合力矩，记作 pj( k) 。设 rj的 leader为 ri

33、，m （ m 0）个 follower分别为1hr，2hr，mhr，则据（ 1.7 ） 、 （1.8 ）式得 pj(k)= apij (k)+)(1kpbjshms（2.9 ）规定当m =0 时，)(1kpbjshms=0，即当m =0 时， pj( k)= apij ( k) 。2.3 可行变化方向的计算据（ 1.12 ）式知，运用人工力矩法控制机器人队形，需要确定各普通机器人的可行变化方向。根据文6 定义1.4知， rj可找到很多的可行变化方向， 但如 果 要 使机 器人 群 能 以尽可 能快 的 速 度形 成需 要 的队 形 ， 则 需对rj的可行变化方向进行优化，以使pj( k+1)-

34、 pj( k) 的值尽可能大。设 f ( x, y, z ) 是以 x、 y、 z 为自变量的函数，根据梯度的性质知：f ( x, y,z) 沿 着 关 于 自 变 量x 、 y 、 z的 梯 度 方 向 变 化 ， 则 可 在 变 化 量222)()()(zyx相同 的情 况 下 ， f ( x+ x, y+ y, z + z)- f ( x, y, z ) 的值最大。对于rj可行变化方向的优化，自然也可利用梯度的这一性质，但问题是函数pj( k) 应该把什么作为它的自变量？如果以j( k) 、xj( k) 、 yj( k) 作为 pj( k) 的自变量， 则可在变化量222)()()(ky

35、kxkjjj相同的情况下， pj( k+1)- pj( k) 的值最大。但这样有一个显然的问题，即j( k) 是无量纲的，而 xj( k) 、 yj( k) 是有量纲的。 设1j( k)=0.1 ，1xj( k)=0.1米，1yj( k)=0.2米; 2j( k)=0.1 ，2xj( k)=10 厘米，2yj( k)=20 厘米，显然两次的变化量相同，212121)()()(kykxkjjj222222)()()(kykxkjjj；如果设xj( k)=aglxj( k) ，yj( k)=aglyj( k) ，把j( tk) 、xj( k) 、yj( k) 作为pj( k) 的自变量，则不会出现

36、这类问题，所以本章把pj( k) 看成是关于自变量j( k) 、 xj( k) 、yj( k) 的函数。设y 表示 y 的梯度，则据（ 2.9 ）式得：pj( k)=apij( k)+)(1kpbjshms (2.13) 设函数 ader(x)= 时当时当时当2|)sign(2|)(sin|111xxxxxx，（2.14 ）bder( x)=其他情况时当时且当0|)sign(|2)sign(2|)sign(|)sign(sin(32222xxxxxxxx (2.15) 则得apij( k)=)()(aderagl()()()()(aderagl()()()()(aderdgl()()(kyky

37、kykapkxkxkxkapkkkkapijjjijijjjijijjij (2.16) jshbp( k)=)()(bderagl()()()()(bderagl()()()()(bderdgl()()(kykykykbpkxkxkxkbpkkkkbpjshjjjshjshjjjshshjjjsh, （s=1, , m ） (2.17) 据（ 1.13 ） 、 （1.16 ） 、 （1.17 ）式得pj( k) 关于变量j(k) 、xj( k) 、yj( k)的梯度方向为pj( k)=)()()()()()(kykpkxkpkkpjjjjjj=)()(agl(bder)()(aderagl(

38、)()(agl(bder)()(aderagl()()(dgl()()(aderdgl(111kykykykykxkxkxkxkkbderkkjshjmsijjjshjmsijjshjmsij (2.18) 根据定义2.4的假设，令rj在时间段 ( tk, tk+1 内沿着方向pj( k) 变化，则得)()1()()1()()1(kykykxkxkkjjjjjj=)()(agl(bder)()(aderagl()()(agl(bder)()(aderagl()()(bderdgl()()(aderdgl(111kykykykykxkxkxkxkkkkjshjmsijjjshjmsijjshjm

39、sij其中为步长因子。整理后得)()1()()1()()1(kykykxkxkkjjjjjj=)()(agl(bder)()(aderagl(2)()(agl(bder)()(aderagl(2)()(bderdgl()()(aderdgl(111kykykykydkxkxkxkxdkkkkjshjmsijjjshjmsijjshjmsij即优化后普通机器人rj的可行变化方向为)()()(kykxkjjj=)()(agl(bder)()(aderagl(2)()(agl(bder)()(aderagl(2)()(bderdgl()()(aderdgl(111kykykykydkxkxkxkxd

40、kkkkjshjmsijjjshjmsijjshjmsij (2.19) 得到 可 行变化 方 向后 ，进 一步 需要 确 定步 长因 子， 对此通 常 采用 直接给定的方法确定，但给定的须满足(1+ m ) 1 （2.20 ）其中 m为 follower数最多的一个机器人的follower数。2.4 仿真为检验人工力矩法的效果，我们运用matlab仿真形成了人字形队形和实现了长方形队形顺时针转/2，其运动轨迹和结果分别如图2.4-2.6 所示。图 2.4 是人字形队形形成过程图，其目标队形的吸引线段式主-从队形图如图2.3 所示，其中a01=1，01= ；a12=2，12=17/18；a23

41、=2，23=5 /6；a25=2，25= - 5 /6；a34=3，34=3/4；a56=3，56= -3/4。参数 d=1.2 ， =0.2 ，=0.3，1= /90，2= /90，3=3.23 ， s =0.25 ，0(k)=s0(k) 0。各机器人的初始位置和基本运动方向线的方向都如图所示，其中小的圆点表示各机器人的初始位置，带箭头的射线表示各机器人的基本运动方向线。图 3 目 标“人”字 形 队形的alfgraph （ a1）运动4 步 后的队形状 态（ a2）运 动 10 步后 的队形状态（ a3） 运动15 步 后的队 形状 态（ a4） 运动28 步 后的 队形状 态图 4 人

42、字形队形形 成 过程图图 2.5、图 2.6 是行距为3，列距为2 的长方形队形顺时针转/2 的过程图， r0的基本运动方向线的方向角每步转/90，变化45 步后不再变化。两次参数值相同的参数有d=1.2 ， =0.19 ，1= /90，2= /60，3=3.22 ，s =0.22 ，s0(k)0。不同的是在图2.5 中，参数=0，即各机器人只受主吸引矩的作用；而在图2.6 中，参数=0.3，即各机器人不但要受主吸引矩的作用，还要受从吸引矩的作用（如果一个普通机器人有follower ） 。各 机 器 人 的 初 始 位 置 和 基 本 运动 方向 线的 方向 是一 个排 列 好 的 长 方

43、形队形。（ b1 ） 运动30 步 后的 队形状态（ b2） 运动50 步后 的队 形状 态（ b3） 运动60 步 后的 队形状态（ b4） 运动80 步 后的 队形状 态图 5 长方 形队 形顺 时针 转/2 的过程图 (=0)（ c1） 运动30 步 后的队形状 态（ c2）运动50 步后的队形状态（ c3 ）运动60 步后 的队形状态（ c4）运动80 步后的队形状态图 6 长方 形队 形顺 时针 转/2 的过程图 (=0.3) 从图 2.4 到图2.6 可以看出，运用人工力矩法，可精确地得到任意的机器人队形；同时从图2.5 和图 2.6 可以看出，加上从吸引矩后，各普通机器人在跟随其

44、leader 运动的同时，还能照顾其follower ，从而能在一定程度上防止一个机器人与其follower靠得太近或离得太远，有利于保持队形的整体和谐。2.5 本章小 结本章对基于人工力矩控制机器人队形的方法作了比较详细的介绍。首先介绍了机器人的基本运动方向线等概念，根据这些概念，得到了一种新的队形表示模型：吸 引线 段式主 - 从 队形图 ，同时还 定义了两种人 工力矩：主吸引矩和从吸引矩，其中主吸引矩用来表示leader对 follower的作用大小，而从吸引矩则用来表示follower对 leader的作用大小。 以上述概念为基础，本章介绍了一种新的多机器人队形控制方法：人工力矩法，及

45、据人工力矩法而设计的离散的非线性的队形控制系统。为了加快收敛速度，又对队形控制器系统中普通机器人的可行变化方向进行了优化。然后通过变换，把原队形控制系统分解成三个子系统，通过分别构造各个子系统的liapunov函数，并利用离散系统的李亚普诺夫稳定性定理，证明了优化可行变化方向后的队形控制系统具有稳定性。第 3 章人工力矩法的改进与应用3.1 感觉障碍区间、排斥矩及相关概念为叙述方便，先将后文中的有关符号规定如下：1） ab表示以点a 为起点，点b 为终点的有向线段；2）1、2、3（123）分别表示机器人的位置安全距离（rj到一物体的距离如果小于1，则rj会感到安全受到威胁）、方向安全距离（如果

46、rj到一物体的距离小于2且不是背着该物体运动，则即使到该物体的距离大于1，rj也会感到安全受到威胁）、识别距离（如果一物体到rj的距离大于3，则不能被rj发现）。定义 3.1 设 m为障碍物边缘曲线上的一个点（后文同），如果m到 rj的距离小于1（2）且与rj的连线段不经过任何物体内部，则m为rj的位置（方向）障碍点；设以m为起点，过rj且长度等于1（2）的有向线段的终点为e，则称以rj为起点，以e 为终点的有向线段为m的位置（方向）排斥线段， e 为吸引点。如在图1 中，点 d 既是 rj的位置障碍点，又是rj的方向障碍点，它的位置排斥线段为1erj，对应的吸引点为e1，方向排斥线段是2er

47、j，对应的吸引点是e2；点b1是 rj的方向障碍点，但不是位置障碍点；而点a，虽然到rj的距离小于2，但它与rj的连线段经过障碍物的内部，所以不是rj的方向障碍点。图 1 障碍点、感觉障碍点和感觉障碍区间注 3.1 1 ）位置障碍点和方向障碍点统称为障碍点；2）如果一个点是位置障碍点，则一定是方向障碍点，但反过来不成立。定义 3.2 设 b1，b2 是障碍物边缘曲线上的一个闭区间，如果该区间满足下述条件，则 b1，b2 为 rj的障碍区间。1） b1、b2是 rj的方向障碍点；2） b1，b2 内的其他点，如果与rj的连线段不经过障碍物的内部，则是rj的方向障碍点，且其中至少有一点是rj的位置

48、障碍点；3）对任意的区间 c1， c2 b1，b2 ， c1， c2 内的点不同时满足上述两个条件。如在图2.1 中，区间 b1，b2 是 rj的障碍区间，而区间 b1，m 、 b1，a不是 rj的障碍区间。注 3.2为方便，后文总假设 b1，b2 是 rj的感觉障碍区间。定义3.3 设 d 为 b1， b2 内的方向障碍点，将 b1， b2 内所有方向障碍点的方向排斥线段，按照向量求和的法则相加，如果和向量的方向与d的方向排斥线段的方向相同，则d的方向排斥线段为 b1， b2 的方向线段。结论3.1 设 d 的方向排斥线段所在的直线为l1，直线l2与 l1交于 rj且 l2 l1，则 d 的

49、方向排斥线段是 b1， b2 方向线段的充要条件是 b1， b2内所有方向障碍点的方向排斥线段沿l2方向的分量的和为零（如在图1 中，点 d的感觉排斥线段2re 沿 l2方向的分量为/2re） 。定义 3.4设 p1、p2为 b1，b2 边缘曲线c 上距 rj最近的点，他们的位置排斥线段的长度为pd， b1，p1） 、 （p2，b2 内的点到rj的距离都大于pd, ，则称以rj为起点，方向与 p1，p2 的方向线段的方向相同，长度等于（1- pd）的有向线段为 b1， b2 的位置线段。结论 3.2如果区间 p1， p2 退化成一个点p， 则 p的位置排斥线段为 b1，b2 的位置线段； 如果

50、曲线c关于 b1， b2 的位置线段所在的直线对称，则 b1，b2 的方向线段与位置线段的方向相同。定义 3.5设表示 b1，b2 ，j( k) 、xj ( k) 、yj ( k) 分别为rj在 tk时刻基本运动方向线的方向角及所在位置的横、纵坐标；m j ( k) 为 tk时刻的无偏障碍点的感觉排斥线段的方向角；xn j ( k) 、yn j ( k) 为 tk时刻中距rj最近点n的排斥线段的吸引点的横、纵坐标；d、都为正常数，函数 agl(x)=xd（ 3.1 ）rp j ( k) = -2(dgl(j( k)j( k)2+(agl(xj( k)- xnjk)2+(agl(yj( k)-

51、ynj(k)2（ 3.2 ）则称 rp j ( k) 为 tk时刻感觉障碍区间对 rj的排斥矩。根据排斥矩的定义，结合第 2 章定义的主吸引矩、从吸引矩，则知道在rj的活动空间里，共存在三种人工力矩。rj在 tk时刻受到的主吸引矩、所有从吸引矩、所有排斥矩的和为rj在 tk时刻受到的合力矩，记作pj( k) 。设 rj的 leader为 ri，m （ m 0）个 follower分别为1hr、2hr、mhr，l ( l 0）个感觉障碍区间分别为1、2、l ，tk时刻 ri对 rj的主吸引矩为 apij( k) ， rh对 rj的从吸引矩为bphj( k) ，则 pj( k)= apij ( k

52、)+)(1kpbjshms+)(1kprjsls（ 3.3 ）3.2 无偏障碍点感觉排斥线段方向角的计算定理3.1 设 b1，b2 边缘曲线c上的每一点都是rj的感觉障碍点，d是 c上任意一点，d到 rj的距离为r ，d的感觉排斥线段的方向角为，点 m为 b1，b2 无偏障碍点，m的感觉排斥线段的方向角设为m j，则cjmdsr)sin()(2=0 (3.4) 证明如图 1， 设 c上任意点d的感觉排斥线段为2re ， 作 re通过点d 且|re|=2，则 |2re |= |（ re- rd） | 根据结论2.1 ，如果规定l2向右的方向为正方向（规定左方向为正方向可以得到类似结果），则2re

53、 沿 l2方向的分量/2re= |2re |sin(m j -)=|(re- rd)| sin(m j -) =(2- r )sin(m -)= -(2- r)sin(-m j) 用分点b1=d0d1 dn=b2将曲线c分成 n 等分， 每段的长度设为，i、ri分别为分点di的感觉排斥线段的方向角及到rj的距离， e2i是 di的感觉排斥线段的吸引点。则 b1，b2 内所有点的感觉排斥线段沿l2方向的分量的和为limn|02niiresin(mj -i)=limnnijmiir02)sin()(，因为点m为 b1， b2 的无偏障碍点，所以根据结论3.1 得：limn|02niiresin(m

54、j -i) =limnnijmiir02)sin()(=0 limnnijmiir12)sin()(=0limnnijmiir12)sin()(=0 cjmdsr)sin()(2=0，所以命题成立，证毕。根据定理3.1 ，得到以下推论：推论 3.1设 c1、 cm是 c 上 m段互不相交的曲线段且它们上面的每一点都是rj的感觉障碍点，除c1、 cm外， c上其他点都不是rj的感觉障碍点，则)sin()(12micjmidsr=0 （3.5 ）定理 3.2设 b1，b2 的边缘曲线c上的每一点都是rj的感觉障碍点，c的方程为 f ( x, y) =0 （3.6 ）b1、b2的感觉排斥线段的方向角

55、分别为1、2；c上任意点d的感觉排斥线段的方向角为，到 rj的距离为r ，且当d 沿着曲线c 由 b1运动到b2时，是由1连续运动到2；m为 b1，b2 的无偏障碍点，它的感觉排斥线段的方向角为mj；设（ x，y）为点d的坐标，ddyyddxx，则1）当21222)cos()(dyxr=0 时，mj = （/2 ） ；2）当21222)cos()(dyxr0 时mj =arctan(2121222222)cos()()sin()(dyxrdyxr) 或mj=arctan(2121222222)cos()()sin()(dyxrdyxr)。证明设 r j的坐标为（x0，y0） ，据题设有)sin

56、()sin()cos()cos(0000ryryyrxrxx（3.7 ）将（ 3.7 ）代入（ 3.6 ）得f(x0 - rcos(), y0-rsin()=0 (3.8) 据（ 3.8 ）可求得r 为关于的函数，不失一般，设r =g() ；将 r =g()代入（ 7）得x、 y 也是关于的函数，所以将曲线积分转化成关于的一元函数积分得cjmdsr)sin()(2=21222)sin()(dyxrjm（ 3.9 ）因为 m为 b1， b2 的无偏障碍点，所以据定理3.1 21222)sin()(dyxrjm=0cos(mj)21222)sin()(dyxr-sin(mj)21222)cos()

57、(dyxr=0 21212121210)cos()(0)sin()()cos(0)cos()()cos()()sin()()tan(222222222222222时当时当dyxrdyxrdyxrdyxrdyxrjmjm(3.10) 据（ 3.10 ）和有向线段方向角的定义求得mj 即得结论，所以命题成立，证毕。据定理3.2 ，得推论3.2 。推论 3.2如果当d沿着曲线c 由点 b1运动到b2，不是由1连续运动到2，而其他条件都满足定理2 的要求，设函数dgl( x)=时当时当|)(sign2|xxxxx（3.11 ）3=1+dgl(2-1) (3.12) 则 1）当31222)cos()(d

58、yxr=0 时，m j =/2 ；2）当31222)cos()(dyxr0 时m j =arctan(3131222222)cos()()sin()(dyxrdyxr) 或m j =arctan(3131222222)cos()()sin()(dyxrdyxr)根据定理3.2 ，得到求m j近似值的数值计算方法如下：一 、 用 分 点1=01n=2将 区间 1，2 分 成n 等分 ； 设 函 数h()=(2 r )sin()22yx， g()=(2 r )cos()22yx， 函数 h() 、g() 在点i（0i n）的值分别记为hi、gi。二、对给定的i（ i =0，1， n）1） 据 （

59、3.8 ） 式 可 求 得ri， 同 时 利 用 隐 函 数 的 求 导 方 法 求 得iiddrr；2）据（ 3.7 ）式有iiiiiiiiiiiirrddyyrrddxx)sin()cos()cos()sin(（3.13 ）将 ri和ir 代入（ 3.13 ）即得ix 、iy 。3）将i、ri、ix 、iy 代入相应的函数求出hi、gi。三、运用求定积分近似值的梯度法12，得1) 2122)sin()(dyxrn)(12( h0+hn)/2+11niih ；2）2122)cos()(dyxrn)(12( g0+gn)/2+11niig ；四、据定理3.2 得：1）如果 ( g0+gn)/2

60、+11niig =0，则m j = （/2 ） ；2）如果 ( g0+gn)/2+11niig0，则m j =arctan(1101102/ )(2/)(niinniinggghhh) 或m j =arctan(1101102/ )(2/)(niinniinggghhh)。注 2.3 1)感觉障碍点的感觉排斥线段的方向角等于以该点为起点，以rj为终点的有向线 段的方 向角； 2） 实际问 题中，为简化计算，可用分段的直线段来代替曲线段c 。3.3 人才市场式多机器人系统模型介绍人才市场式多机器人系统模型以前，先做如下假设：假设1 r 中机器人都有基本的通信功能，由此能得到吸引线段式主 -从队形