专利名称:基于轨道扩展的多机器人的寻迹编队控制方法
技术领域:
本发明涉及一种基于轨道扩展的多机器人寻迹编队控制方法。
背景技术:
机器人的寻迹控制技术因其控制律比较简单、控制系统具有好的鲁棒性以及易于 工程实践等特点被广泛应用于军事、灾难搜救以及信息采集等各种领域中。所谓机器人的 寻迹控制,是指在控制系统的驱动下,机器人从任意初始位置驶入预先规划好的轨迹,并沿 此轨迹运动或者最终运动到目的地。例如轮式机器人按预定的路径去搜救幸存者、水下机 器人运动于椭圆轨道采集海洋信息等。近些年来,随着灵敏传感器的快速发展,人们可以将 传感器安装于多个移动机器人,从而构成移动传感器网络像生物体那样通过编队来共同采 集某一区域的信息。为了能够在时间和空间上实现移动传感器网络测量的最优化,需要根 据所测区域时间和空间的变化来规划每个机器人的运动轨迹,并且在规划好的轨迹上协调 机器人间的编队运动,这样的控制任务称为寻迹编队控制问题。当前,已有的寻迹方法都是针对单个机器人的,多机器人的寻迹编队控制研究 甚少。Princeton大学的Leonard教授指导的团队采用沿曲线法向量扩展的方法将简单 闭曲线扩展为一组关于轨道函数的闭曲线设计了多个质点机器人在单条简单闭曲线上 白勺寻队(F. Zhang et al, “ Control of coordinated patternsfor ocean sampling, “ Int. J. Control, vol. 80, pp. 1186-1199,2007) 但是,该方法对一些简单 闭曲线(如椭圆、圆角正方形以及圆角平行四边形等)是很难求出轨道函数显示表达 式,而轨道函数在控制律的设计中是必不可少的,这使得某些情况下寻迹编队控制很难实 现。其次,该方法将通信拓扑限制为双向链式结构、运动中质点机器人的速度要求始终不 能为0并且控制律设计中需要用到邻居的加速度信息,这在一定程度上也限制了该方法 在实际中的运用。中国专利“基于投影的多运动体的协同路径跟踪控制方法”(专利号 ZL200810196368. 4)采用基于沿着横轴或纵轴投影的方法设计多质点机器人的协同路径跟 踪控制律,由于该方法需要在目标路径上一直能够找到运动体的唯一的投影点,因而不适 应目标路径是闭曲线的情况。此外,实际中应用较多的轮式机器人含有非完整约束(如不 能侧滑等),而上述两种方法针对的模型都是满足牛顿第二定律的质点运动,因而不能直接 用于真实机器人的控制。实际上,最优化信息采集中的目标轨道多为简单凸闭曲线。例如,海洋探测中设定 的轨道通常为边很长且接近直线的超椭圆曲线(包括椭圆、圆角正方形以及圆角平行四边 形等),同样的情况也出现于多个轮式机器人协同采集某一陆地的信息。与此同时,机器人 的运动通常满足非完整约束的动力学方程。因此,设计多个满足非完整约束的动力学机器 人在一组简单凸闭轨道上的寻迹编队控制方法将更加具有现实意义。但目前还不存在此类 控制方法。
发明内容
发明目的本发明的目的在于针对现有技术的不足,提供一种方法简单可靠、精度 较高,可用于多机器人最优化信息采集等复杂任务的基于轨道扩展的多机器人寻迹编队控 制方法。技术方案本发明所述的本发明是一种基于轨道扩展的多机器人寻迹编队控制 方法,其中所述的机器人的运动满足非完整约束的动力学方程并且目标轨道是简单凸闭曲 线,具体包括如下步骤a)对于平面中一组目标轨道,将目标轨道沿其中心指向轨道上各点的向量扩展为 关于轨道函数等值轨道簇,并确定机器人的可运动范围;b)由轨道函数计算寻迹误差,设计机器人的虚拟角速度使寻迹误差达到设计要求;c)由轨道函数和目标轨道对应的参数计算机器人沿目标轨道运动的广义弧长及 其导数,由通信得到的邻居信息,设计机器人的控制力来实现编队;d)计算真实与虚拟角速度间的误差,设计机器人的控制力矩完成寻迹;e)通过伺服系统完成机器人的运动控制。其中所述步骤a)包括如下步骤al)将目标轨道沿其中心指向轨道上各点的向量扩展为等值轨道簇;a2)根据曲线的正则条件,确定对应机器人的可运动范围;a3)在可运动范围上构建轨道函数,使得每条等值轨道可由轨道函数取不同的值 来表不。其中所述步骤b)包括如下步骤bl)由机器人的位置和轨道函数,计算轨道函数值与期望值间的位置寻迹误差;b2)由轨道函数得到轨道的切向量,计算机器人的运动方向与轨道切方向间的角
度寻迹误差;b3)由位置寻迹误差和角度寻迹误差,设计机器人的虚拟角速度使使得寻迹误差 减少到满足的设计要求,同时保证机器人始终在可运动范围内运动。其中所述步骤C)包括如下步骤cl)规定好目标轨道对应的参数和弧长计算的起始点,由此确定等值轨道簇中其 他轨道的弧长计算的起始点;c2)由轨道函数和目标轨道对应的参数,计算机器人沿轨道从起始点到当前位置 间的弧长;c3)根据队形要求确定广义弧长与弧长间的函数关系,计算广义弧长及其对时间 的导数;c4)由通信的相邻运动体的广义弧长及其导数,设计机器人的控制力完成编队。其中所述步骤d)包括如下步骤dl)由机器人的真实角速度,计算其与步骤b)得出的虚拟角速度间的角速度误 差;d2)由寻迹误差及其随时间的变化以及步骤C)得出的机器人的控制力,计算虚拟 角速度对时间的导数;d3)根据角速度误差和虚拟角速度的导数,设计机器人的控制力矩使角速度误差减少到满足的设计要求从而最终完成寻迹。其中所述步骤e)包括如下步骤由上位机将机器人的控制力和力矩发送给下位机中,通过伺服系统来完成机器人 的运动控制。有益效果本发明与现有技术相比,其有益效果是本方法具有简单可靠、精度较 高以及便于实际运用的特点,可用于多机器人最优化信息采集等复杂任务;本发明对非完 整约束的动力学机器人和简单凸闭轨道尤其适用。
图1为通信拓扑对应的连通图;图2为三个机器人以三角形队形编队运动于各自的目标轨道上;图3为三个机器人运动于各自同心圆轨道并保持他们的位置与圆心在一条直线 上;图4为由目标轨道扩展出的等值轨道簇;图5为单个机器人在等值轨道簇中的运动;图6为基于轨道扩展的多机器人的寻迹编队控制设计流程图。以上的图中Vi、V2、V3和V4分别表示机器人1、2、3和4 ;Ci0, Cjo和Cktl分别表示机 器人i、j和k对应的目标轨道;hp hj和hk分别为机器人i、j和k对应的期望队形向量; zI、。和4分别为轨道ci(1、Cjo和Cktl上弧长计算的起始点;S” Sj和%分别为轨道Ci(1、Cjo 和Cktl上的弧长;ξ i、ξ j和Ik分别为Ci(1、Cj(1和Cktl上的广义弧长;n i、n j和η k分别为沿 轨道运动时ξ i、ξ ^和ξ k对时间的导数;氏、民和&分别为机器人i、j和k的目标圆轨道 的半径;P i 轨道Citl的中心4指向轨道上各点的向量;λ n和λ 12分别为沿P i平移的不同 长度;Cil和Ci2分别为目标轨道Citl沿向量ρ i平移λ n和λ i2得到的轨道;4和4分别为 Cil和Ci2上弧长计算的起始点%是轨道切方向与横轴间的夹角;fi 轨道函数Λ:机器人i
的运动方向丄:与&垂直的方向:轨道的切向量A :轨道的法向量;α i ★.与力间的夹角。
具体实施例方式下面结合附图,通过一个最佳实施例,对本发明技术方案进行详细说明,但是本发 明的保护范围不局限于所述实施例。考虑平面中由n个机器人组成的寻迹编队控制系统,机器人在Frenet-Serret坐
标系中的动力学方程为
斧=^y1mA= F1
^A=N1其中Zi= [zxi,zyi]T表示机器人i重心的位置坐标,、为重心平移的线速度的大 小,Qi是角速度的大小,^ =[cos&sin^.f表示机器人的单位速度方向,&为与单位速度方向 垂直的方向并且( )满足右手定则,Hii为机器人的质量,Ii为转动惯量,Fi为机器人的控 制力输入,Ni为控制力矩输入,i = 1,K,η。多机器人在寻迹编队运动中,机器人间的通信是必不可少的,这里我们用无向图 G= (V, Ε)来描述,其中V= IV1, V2, K,VJ为节点集,EcVxV为有向边的集合。如果 存在一条边连接节点Vi和Vk表明机器人i和k可以交换信息,它们互为相邻节点(即, 邻居)。机器人i的相邻节点集合用Ni表示。当图中任意两个节点间都存在着一条路
径,那么图是连通的。这里的两个节点χ和λ(间的路径是指由不同节点fX^X^K,Yj和边
以,^办,力其权?力构成的图。图的邻接矩阵A= ^ij]可以定义为当且 仅当(Vi, Vj) e E时,其他= O。图1为4个机器人间的通信拓扑对应的连通图。设计 时,我们一旦规定好多机器人间通信关系,那么以后每一个时刻机器人i的Ni都是不变的, 且对应的无向图是连通的。设机器人i的目标轨道Citl是平面中一条曲率Kitl为正的简单凸闭曲 线。该目标轨道可以在以Citl的中心A为原点的参考坐标系中表示为光滑的映射 Α:
Ti(Si)-Tj(Sj)=0。如果机器人i的目标轨道是由参考轨道平移期望的队形向量Iii而得到的(如图2 所示),规定的弧长计算的起始点满足4 _<。= H1 -Hj,那么我们可以简单的定义弧长Si即 为广义弧长ξ i,弧长Si和机器人沿目标轨道的速度释达到一致就能保证多机器人以期望 的队形Zi(t)-z“t) =Iii-I^编队运动。对于更复杂的情况,弧长和机器人沿着目标轨道的 速度达到一致并不能保证多机器人以期望的队形编队运动。图3所示的是三个机器人运动 在不同半径氏的同心圆轨道上并且保持与圆心在同一条直线上的例子,显然只有当Ii =
及其导数Jli达到一致时才能保证多机器人以期望的队形编队运动。因此,本方法选 择用沿着轨道运动的广义弧长Ii达到一致、其导数Hi达到一致或给定值n*(t)来描述多 机器人的编队运动,这里我们要求一致值和是一个对时间t 一致有界非ο的信号。本发明中,控制器的设计思想是先将目标轨道Citl沿着其中心A指向轨道上各点 的向量Pi扩展为关于轨道函数fi (Z)的等值轨道簇,并由曲线的正则性确定对应机器人i 的可运动范围Qi(如图4所示)。通过设计机器人的力矩使得初始位于Qi中的机器人i 始终在Qi中运动,与此同时,轨道函数值& (Zi)与期望值间的位置寻迹误差Cli以及机器人 的运动方向与轨道的切方向间的夹角(即,角度寻迹误差)α 1减少到满足设计的要求(如 图5所示)。为了设计机器人的控制力矩,本发明中我们是分两步来完成第一步,设计虚 拟角速度使得寻迹误差减少到满足设计的要求,同时保证机器人不离开Qi ;第二步,设计 机器人的控制力矩使得真实角速度和虚拟角速度间的误差减少到满足设计的要求。当机器 人运动在各自的目标轨道上,多机器人间的编队运动就退化为机器人沿轨道运动的位置与 速度达到一致。根据通信得到相邻机器人的信息,设计机器人的控制力使得沿轨道运动的 广义弧长ii(t)达到一致、其导数ια)达到一致值或期望值来实现多机器人的 在轨编队运动。图6是本发明的设计流程图,由模块P1、P2、P3、P4和P5构成,各模块叙述如下1)模块 Pl由于本发明设计寻迹编队控制器的前提是将每条目标轨道扩展为由不同轨道函 数值表示的等值轨道簇,模块Pl用于得到等值轨道簇、轨道函数以及对应机器人的可运动 范围,具体按下列步骤实现第一步在Ci0附近,将Citl上的每一点沿其中心《指向轨道上各点的向量P i平移 实数Xik得到扩展轨道Cik,即Cik ( Φ Xik) = (^(ΦΟ + λ^Ρ ^1(Cti).第二步由扩展轨道的切向量九=巧(1 + 4)力。以及正则条件九类0,我们选择机器 人i的可运动范围Qi为平面中所有满足-1 < Aik< ε <⑴的轨道上点的集合。第三步由于集合Qi中的每一点都属于Qi中的一条扩展轨道,我们可以在Qi 上构建轨道函数fi Ω j ^ (-1,ε )并且满足V/类0。进而,Qi可以表示为Qi = {z G i2卜1 <Α(Ζ) < ε }。Cik是关 于轨道函数fi的一条等值平面凸闭轨道,即Z = [zx,Zy]T e CikJU fi(z) = λ ik ;当z e Ci0, fi(z) = 0。例如(倾斜)超椭圆曲线
权利要求
1.一种基于轨道扩展的多机器人的寻迹编队控制方法,其特征在于该方法中机器人 的运动需满足非完整约束的动力学方程并且目标轨道是简单凸闭曲线,该方法包括如下步 骤a)对于平面中一组目标轨道,将目标轨道沿其中心指向轨道上各点的向量扩展为关于 轨道函数等值轨道簇,并确定机器人的可运动范围;b)由轨道函数计算寻迹误差,设计机器人的虚拟角速度使寻迹误差达到设计要求;c)由轨道函数和目标轨道对应的参数计算机器人沿轨道运动的广义弧长及其导数,由 通信得到的邻居信息,设计机器人的控制力来实现编队;d)计算真实与虚拟角速度的误差,设计机器人的控制力矩完成寻迹;e)通过伺服系统完成机器人的运动控制。
2.根据权利要求1所述的基于轨道扩展的多机器人的寻迹编队控制方法,其特征在 于其中所述步骤a)包括如下步骤al)将目标轨道沿其中心指向轨道上各点的向量扩展为等值轨道簇; a2)根据曲线的正则条件,确定对应机器人的可运动范围;a3)在可运动范围上构建轨道函数,使得每条等值轨道可由轨道函数取不同的值来表示。
3.根据权利要求1所述的基于轨道扩展的多机器人的寻迹编队控制方法,其特征在 于其中所述步骤b)包括如下步骤bl)由机器人的位置和轨道函数,计算轨道函数值与期望值间的位置寻迹误差; b2)由轨道函数得到轨道的切向量,计算机器人的运动方向与轨道切方向间的角度寻迹误差;b3)由位置寻迹误差和角度寻迹误差,设计机器人的虚拟角速度使得寻迹误差减少到 满足的设计要求,同时保证机器人始终在可运动范围内运动。
4.根据权利要求1所述的基于轨道扩展的多机器人的寻迹编队控制方法,其特征在 于其中所述步骤c)包括如下步骤cl)规定好目标轨道对应的参数和弧长计算的起始点,由此确定等值轨道簇中其他轨 道的弧长计算的起始点;c2)由轨道函数和目标轨道对应的参数,计算机器人沿轨道从起始点到当前位置间的 弧长;c3)根据队形要求确定广义弧长与弧长间的函数关系,计算广义弧长及其对时间的导数;c4)由通信得到的相邻机器人的广义弧长及其导数,设计机器人的控制力完成编队。
5.根据权利要求1所述的基于轨道扩展的多机器人的寻迹编队控制方法,其特征在 于其中所述步骤d)包括如下步骤dl)由机器人的真实角速度,计算其与步骤b)得出的虚拟角速度间的角速度误差; d2)由寻迹误差及其对时间的导数以及步骤c)得出的机器人的控制力,计算虚拟角速 度对时间的导数;d3)根据角速度误差和虚拟角速度的导数,设计机器人的力矩使得角速度误差减少到 满足的设计要求从而最终完成寻迹。
6.根据权利要求1所述的基于轨道扩展的多机器人的寻迹编队控制方法,其特征在 于其中所述步骤e)包括如下步骤由上位机将机器人的控制力和力矩发送给下位机中,通过伺服系统来完成机器人的运 动控制。
全文摘要
本发明公开一种基于轨道扩展的多机器人寻迹编队控制方法,包括如下步骤a)对于平面中一组目标轨道,将目标轨道沿其中心指向轨道上各点的向量扩展为关于轨道函数等值轨道簇,并确定机器人的可运动范围;b)由轨道函数计算寻迹误差,设计机器人的虚拟角速度使寻迹误差达到设计要求;c)由轨道函数和目标轨道对应的参数计算机器人沿轨道运动的广义弧长及其导数,由通信得到的邻居信息,设计机器人的控制力实现编队;d)计算真实与虚拟角速度的误差,设计机器人的控制力矩完成寻迹;e)通过伺服系统完成机器人的运动控制。本发明对非完整约束的动力学机器人和简单凸闭轨道尤其适用。该方法简单可靠、精度较高,可用于多机器人最优化信息采集等。
文档编号G05D1/00GK102073320SQ201010552508
公开日2011年5月25日 申请日期2010年11月19日 优先权日2010年11月19日
发明者田玉平, 陈杨杨 申请人:东南大学