CN102073320B - 基于轨道扩展的多机器人的寻迹编队控制方法 - Google Patents

Abstract

本发明公开一种基于轨道扩展的多机器人寻迹编队控制方法，包括如下步骤：a)对于平面中一组目标轨道，将目标轨道沿其中心指向轨道上各点的向量扩展为关于轨道函数等值轨道簇，并确定机器人的可运动范围；b)由轨道函数计算寻迹误差，设计机器人的虚拟角速度使寻迹误差达到设计要求；c)由轨道函数和目标轨道对应的参数计算机器人沿轨道运动的广义弧长及其导数，由通信得到的邻居信息，设计机器人的控制力实现编队；d)计算真实与虚拟角速度的误差，设计机器人的控制力矩完成寻迹；e)通过伺服系统完成机器人的运动控制。本发明对非完整约束的动力学机器人和简单凸闭轨道尤其适用。该方法简单可靠、精度较高，可用于多机器人最优化信息采集等。

Description

基于轨道扩展的多机器人的寻迹编队控制方法

技术领域

本发明涉及一种基于轨道扩展的多机器人寻迹编队控制方法。

背景技术

机器人的寻迹控制技术因其控制律比较简单、控制系统具有好的鲁棒性以及易于工程实践等特点被广泛应用于军事、灾难搜救以及信息采集等各种领域中。所谓机器人的寻迹控制，是指在控制系统的驱动下，机器人从任意初始位置驶入预先规划好的轨迹，并沿此轨迹运动或者最终运动到目的地。例如：轮式机器人按预定的路径去搜救幸存者、水下机器人运动于椭圆轨道采集海洋信息等。近些年来，随着灵敏传感器的快速发展，人们可以将传感器安装于多个移动机器人，从而构成移动传感器网络像生物体那样通过编队来共同采集某一区域的信息。为了能够在时间和空间上实现移动传感器网络测量的最优化，需要根据所测区域时间和空间的变化来规划每个机器人的运动轨迹，并且在规划好的轨迹上协调机器人间的编队运动，这样的控制任务称为寻迹编队控制问题。

当前，已有的寻迹方法都是针对单个机器人的，多机器人的寻迹编队控制研究甚少。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)包括如下步骤：

a1)将目标轨道沿其中心指向轨道上各点的向量扩展为等值轨道簇；

a2)根据曲线的正则条件，确定对应机器人的可运动范围；

a3)在可运动范围上构建轨道函数，使得每条等值轨道可由轨道函数取不同的值来表示。

其中所述步骤b)包括如下步骤：

b1)由机器人的位置和轨道函数，计算轨道函数值与期望值间的位置寻迹误差；

b2)由轨道函数得到轨道的切向量，计算机器人的运动方向与轨道切方向间的角度寻迹误差；

b3)由位置寻迹误差和角度寻迹误差，设计机器人的虚拟角速度使使得寻迹误差减少到满足的设计要求，同时保证机器人始终在可运动范围内运动。

其中所述步骤c)包括如下步骤：

c1)规定好目标轨道对应的参数和弧长计算的起始点，由此确定等值轨道簇中其他轨道的弧长计算的起始点；

c2)由轨道函数和目标轨道对应的参数，计算机器人沿轨道从起始点到当前位置间的弧长；

c3)根据队形要求确定广义弧长与弧长间的函数关系，计算广义弧长及其对时间的导数；

c4)由通信的相邻运动体的广义弧长及其导数，设计机器人的控制力完成编队。

其中所述步骤d)包括如下步骤：

d1)由机器人的真实角速度，计算其与步骤b)得出的虚拟角速度间的角速度误差；

d2)由寻迹误差及其随时间的变化以及步骤c)得出的机器人的控制力，计算虚拟角速度对时间的导数；

d3)根据角速度误差和虚拟角速度的导数，设计机器人的控制力矩使角速度误差减少到满足的设计要求从而最终完成寻迹。

其中所述步骤e)包括如下步骤：

由上位机将机器人的控制力和力矩发送给下位机中，通过伺服系统来完成机器人的运动控制。

有益效果：本发明与现有技术相比，其有益效果是：本方法具有简单可靠、精度较高以及便于实际运用的特点，可用于多机器人最优化信息采集等复杂任务；本发明对非完整约束的动力学机器人和简单凸闭轨道尤其适用。

附图说明

图1为通信拓扑对应的连通图；

图2为三个机器人以三角形队形编队运动于各自的目标轨道上；

图3为三个机器人运动于各自同心圆轨道并保持他们的位置与圆心在一条直线上；

图4为由目标轨道扩展出的等值轨道簇；

图5为单个机器人在等值轨道簇中的运动；

图6为基于轨道扩展的多机器人的寻迹编队控制设计流程图。

以上的图中：V₁、V₂、V₃和V₄分别表示机器人1、2、3和4；C_i0、C_j0和C_k0分别表示机器人i、j和k对应的目标轨道；h_i、h_j和h_k分别为机器人i、j和k对应的期望队形向量；

和

分别为轨道C_i0、C_j0和C_k0上弧长计算的起始点；s_i、s_j和s_k分别为轨道C_i0、C_j0和C_k0上的弧长；ξ_i、ξ_j和ξ_k分别为C_i0、C_j0和C_k0上的广义弧长；η_i、η_j和η_k分别为沿轨道运动时ξ_i、ξ_j和ξ_k对时间的导数；R_i、R_j和R_k分别为机器人i、j和k的目标圆轨道的半径；ρ_i：轨道C_i0的中心

指向轨道上各点的向量；λ_i1和λ_i2分别为沿ρ_i平移的不同长度；C_i1和C_i2分别为目标轨道C_i0沿向量ρ_i平移λ_i1和λ_i2得到的轨道；和分别为C_i1和C_i2上弧长计算的起始点；

是轨道切方向与横轴间的夹角；f_i：轨道函数；

机器人i的运动方向；与

垂直的方向；

轨道的切向量；轨道的法向量；α_i：与间的夹角。

具体实施方式

下面结合附图，通过一个最佳实施例，对本发明技术方案进行详细说明，但是本发明的保护范围不局限于所述实施例。

考虑平面中由n个机器人组成的寻迹编队控制系统，机器人在Frenet-Serret坐标系中的动力学方程为

其中z_i＝[z_xi，z_yi]^T表示机器人i重心的位置坐标，υ_i为重心平移的线速度的大小，ω_i是角速度的大小，

表示机器人的单位速度方向，

为与单位速度方向垂直的方向并且

满足右手定则，m_i为机器人的质量，I_i为转动惯量，F_i为机器人的控制力输入，N_i为控制力矩输入，i＝1，K，n。

多机器人在寻迹编队运动中，机器人间的通信是必不可少的，这里我们用无向图G＝(V，E)来描述，其中V＝{V₁，V₂，K，V_n}为节点集，

为有向边的集合。如果存在一条边连接节点V_i和V_k表明机器人i和k可以交换信息，它们互为相邻节点(即，邻居)。机器人i的相邻节点集合用N_i表示。当图中任意两个节点间都存在着一条路径，那么图是连通的。这里的两个节点

和

间的路径是指由不同节点

和边

构成的图。图的邻接矩阵A＝[a_ij]可以定义为a_ij＞0当且仅当(V_i，V_j)∈E时，其他a_ij＝0。图1为4个机器人间的通信拓扑对应的连通图。设计时，我们一旦规定好多机器人间通信关系，那么以后每一个时刻机器人i的N_i都是不变的，且对应的无向图是连通的。

设机器人i的目标轨道C_i0是平面中一条曲率κ_i0为正的简单凸闭曲线。该目标轨道可以在以C_i0的中心

为原点的参考坐标系中表示为光滑的映射：

ρ_i：[0，2π)→²，φ_i  ρ_i(φ_i)，其中光滑函数φ_i：

是轨道切方向与横轴间的夹角，T为一维环空间(即：如果

我们视

和

等价)，并且ρ_i(φ_i)对所有的φ_i满足||ρ_i(φ_i)||≠0且v_i＝||dρ_i(φ_i)/dφ_i||＞0。目标轨道的Frenet表述为

其中

和

分别为轨道的单位切向量和法向量。本发明的目的就是根据通信得到的相邻机器人的信息，设计每个机器人的控制力和控制力矩使其运动在目标轨道的同时，机器人间保持一定的队形。

在本发明中，对于沿着目标轨道运动的各机器人之间的队形位置关系采用如下方式规定：设目标轨道C_i0上的固定点

为弧长计算的起始点，s_i是从起始点

沿着目标轨道到机器人位置间的路程(即弧长)，广义弧长ξ_i＝τ_i(s_i)是关于s_i一个具有二阶连续导数的函数，并且对于所有的s_i满足

以及各机器人之间保持队形位置关系是指：

τ_i(s_i)-τ_j(s_j)＝0。

如果机器人i的目标轨道是由参考轨道平移期望的队形向量h_i而得到的(如图2所示)，规定的弧长计算的起始点满足

那么我们可以简单的定义弧长s_i即为广义弧长ξ_i，弧长s_i和机器人沿目标轨道的速度达到一致就能保证多机器人以期望的队形z_i(t)-z_j(t)＝h_i-h_j编队运动。对于更复杂的情况，弧长和机器人沿着目标轨道的速度达到一致并不能保证多机器人以期望的队形编队运动。图3所示的是三个机器人运动在不同半径R_i的同心圆轨道上并且保持与圆心在同一条直线上的例子，显然只有当ξ_i＝s_i/R_i及其导数η_i达到一致时才能保证多机器人以期望的队形编队运动。因此，本方法选择用沿着轨道运动的广义弧长ξ_i达到一致、其导数η_i达到一致或给定值η^*(t)来描述多机器人的编队运动，这里我们要求一致值和η^*(t)是一个对时间t一致有界非0的信号。

本发明中，控制器的设计思想是先将目标轨道C_i0沿着其中心

指向轨道上各点的向量ρ_i扩展为关于轨道函数f_i(z)的等值轨道簇，并由曲线的正则性确定对应机器人i的可运动范围Ω_i(如图4所示)。通过设计机器人的力矩使得初始位于Ω_i中的机器人i始终在Ω_i中运动，与此同时，轨道函数值f_i(z_i)与期望值间的位置寻迹误差d_i以及机器人的运动方向与轨道的切方向间的夹角(即，角度寻迹误差)α_i减少到满足设计的要求(如图5所示)。为了设计机器人的控制力矩，本发明中我们是分两步来完成：第一步，设计虚拟角速度使得寻迹误差减少到满足设计的要求，同时保证机器人不离开Ω_i；第二步，设计机器人的控制力矩使得真实角速度和虚拟角速度间的误差减少到满足设计的要求。当机器人运动在各自的目标轨道上，多机器人间的编队运动就退化为机器人沿轨道运动的位置与速度达到一致。根据通信得到相邻机器人的信息，设计机器人的控制力使得沿轨道运动的广义弧长ξ_i(t)达到一致、其导数η_i(t)达到一致值或期望值η^*(t)来实现多机器人的在轨编队运动。

图6是本发明的设计流程图，由模块P1、P2、P3、P4和P5构成，各模块叙述如下：

1)模块P1

由于本发明设计寻迹编队控制器的前提是将每条目标轨道扩展为由不同轨道函数值表示的等值轨道簇，模块P1用于得到等值轨道簇、轨道函数以及对应机器人的可运动范围，具体按下列步骤实现：

第一步：在C_i0附近，将C_i0上的每一点沿其中心

指向轨道上各点的向量ρ_i平移实数λ_ik得到扩展轨道C_ik，即

C_ik(φ_i，λ_ik)＝C_i0(φ_i)+λ_ikρ_i0(φ_i).

第二步：由扩展轨道的切向量

以及正则条件

我们选择机器人i的可运动范围Ω_i为平面中所有满足-1＜λ_ik＜ε＜∞的轨道上点的集合。

第三步：由于集合Ω_i中的每一点都属于Ω_i中的一条扩展轨道，我们可以在Ω_i上构建轨道函数

f_i：Ω_i→(-1，ε)

并且满足

进而，Ω_i可以表示为Ω_i＝{z∈i²|-1＜f_i(z)＜ε}。C_ik是关于轨道函数f_i的一条等值平面凸闭轨道，即z＝[z_x，z_y]^T∈C_ik，则f_i(z)＝λ_ik；当z∈C_i0，f_i(z)＝0。

例如：(倾斜)超椭圆曲线

${\mathrm{\ρ}}_i\left({\mathrm{\φ}}_i\right)={[a{\left(\cos {\mathrm{\φ}}_i\right)}^{\frac{1}{p}}+\mathrm{\μb}{\left(\sin {\mathrm{\φ}}_i\right)}^{\frac{1}{p}},b{\left(\sin {\mathrm{\φ}}_i\right)}^{\frac{1}{p}}]}^T$

其中μ∈i，a≥b＞0，p＝1，3，5...。对于μ＝0且a＞b(特别的，a＝b)，p＝1表示椭圆(特别的，圆)而p≥3表示圆角矩形(特别的，圆角正方形)。如果μ≠0且p＞1，则表示圆角平行四边形。按照模块P1的轨道扩展方式，上述超椭圆曲线可以扩展为关于轨道函数

$f_i\left(z\right)={({\left(\frac{z_x-\mathrm{\μ}{z}_y}{a}\right)}^{2p}+{\left(\frac{z_y}{b}\right)}^{2p})}^{\frac{1}{2p}}-1$

的等值轨道簇。

2)模块P2

模块P2是用来设计虚拟角速度使得寻迹误差减少到满足设计的要求，同时保证机器人始终在可运动范围内运动，具体的设计步骤如下：

第一步：由轨道函数f_i和机器人的当前位置z_i，计算寻迹位置误差d_i(t)

d_i＝f_i(z_i)-0＝f_i(z_i).

第二步：由轨道函数f_i得到轨道的切向量

${\stackrel{r}{T}}_i=\left[\begin{array}{cc}0& 1\\ -1& 0\end{array}\right]{\stackrel{r}{N}}_i$

其中

为轨道的法向量，由此计算寻迹角度误差α_i(t)∈(-π，π]

${\mathrm{\α}}_i=\arccos ({\stackrel{r}{x}}_i\·{\stackrel{r}{T}}_i).$

第三步：设计机器人的虚拟角速度

${\stackrel{\‾}{\mathrm{\ω}}}_i={\mathrm{\υ}}_i(k_{\mathrm{ai}}\cos {\mathrm{\α}}_i+k_{\mathrm{bi}}\sin {\mathrm{\α}}_i+2{\mathrm{\ψ}}_i\left(d_i\right)||\▿f_i|\left|{\cos }^2\frac{{\mathrm{\α}}_i}{2}\right)+k_1\sin \frac{{\mathrm{\α}}_i}{2}$

其中

$k_{\mathrm{ai}}=\frac{1}{\left|\right|\▿f_i\left|\right|}{\stackrel{r}{T}}_i\·{\▿}^2{f}_i{\stackrel{r}{T}}_i,$

$k_{\mathrm{bi}}=-\frac{1}{\left|\right|\▿f_i\left|\right|}{\stackrel{r}{T}}_i\·{\▿}^2{f}_i{\stackrel{r}{N}}_i,$

ψ_i(d_i)＝1/(ε-d_i)-1/(d_i+1)+ln(1+d_i)-ln(ε-d_i)-1/ε+1+lnε，

控制参数k₁＞0。函数ψ_i(d_i)是用于保证初始位于Ω_i的机器人i始终在Ω_i中运动且最终运动于目标轨道。

3)模块P3

如图5所示，机器人i沿轨道切方向的运动可以改变机器人沿当前轨道运动的弧长s_i。与此同时，机器人i投影到轨道法向量上的运动可以改变平面中机器人所在的轨道(即轨道函数值f_i(z_i)发生变化)，同样也可以改变弧长。因此，本方法选择目标轨道对应的参数φ_i来刻画机器人运动在Ω_i中所有轨道上的弧长。

模块P3根据通信得到邻居的广义弧长ξ_j(t)和η_j(t)来设计机器人i的控制力从而实现编队，具体的设计步骤如下：

第一步：规定好目标轨道对应的参数φ_i和弧长计算的起始点

由此我们可以选择等值轨道簇中其他轨道上弧长计算的起始点

使得

和

对应的参数值是一致的。

第二步：由轨道函数和弧长参数φ_i，计算沿机器人当前轨道从起始点

到位置z_i间的弧长s_i

$s_i(f_i,{\mathrm{\φ}}_i){\∫}_{{\mathrm{\φ}}_{i0}^{*}}^{{\mathrm{\φ}}_i}\frac{\∂s_i(f_i,\mathrm{\τ})}{\∂\mathrm{\τ}}\mathrm{d\τ}$

其中

为起始点

对应的参数值；

第三步：由队形的要求设计好广义弧长函数ξ_i，即ξ_i是关于s_i一个具有二阶连续导数的函数，并且对于所有的s_i满足

且

由弧长s_i计算广义弧长ξ_i(s_i)。于是，广义弧长对时间的导数为

考虑到机器人最终会运动于目标轨道，因此我们选择η_j(t)为α_i＝0时的

即：

${\mathrm{\η}}_i=\frac{\∂{\mathrm{\ξ}}_i}{\∂s_i}{\mathrm{\υ}}_i.$

这样做的好处就是可以简化计算量和控制律的设计。

第四步：根据信息交互所得到的相邻运动体的广义弧长ξ_j和η_j，设计机器人的控制力F_i(t)

$F_i=m_i{\left(\frac{\∂{\mathrm{\ξ}}_i}{\∂s_i}\right)}^{-1}(-(\cos {\mathrm{\α}}_i+\frac{\∂s_i(f_i,{\mathrm{\φ}}_i)}{\∂f_i}||\▿f_i|\left|\sin {\mathrm{\α}}_i\right)(\underset{j=1}{\overset{n}{\mathrm{\Σ}}}{a}_{\mathrm{ij}}({\mathrm{\ξ}}_i-{\mathrm{\ξ}}_j)+\frac{{\∂}^2{\mathrm{\ξ}}_i}{\∂s_i^2}{\mathrm{\υ}}_i^2)-k_2\underset{j=1}{\overset{n}{\mathrm{\Σ}}}{a}_{\mathrm{ij}}({\mathrm{\η}}_i-{\mathrm{\η}}_j)).$

其中控制参数k₂＞0。

对于运动体的沿着目标轨道的速度达到期望值η^*(t)的编队要求，设计机器人的控制力F_i(t)为

4)模块P4

模块P4根据真实角速度和虚拟角速度来设计机器人的控制力矩使得两者间的角速度误差减少到满足设计的要求从而最终实现寻迹。设计按下列步骤实现：

第一步：由机器人的真实角速度ω_i(t)，计算其与模块P2得出的虚拟角速度的角速度误差ω_ie(t)

${\mathrm{\ω}}_{\mathrm{ie}}={\mathrm{\ω}}_i-{\stackrel{\‾}{\mathrm{\ω}}}_i.$

第二步：由寻迹误差及其随时间的变化以及步骤c)得出的机器人的控制力F_i(t)，计算虚拟角速度随时间的变化

其中

第三步：设计机器人的控制力矩N_i(t)

其中控制参数k₃＞0。

5)模块P5

模块P5将模块P3和P4的得到机器人的控制力和力矩发送给下位机，通过伺服系统完成机器人的运动控制，具体的按照以下步骤实现：

由上位机将机器人的控制力和力矩发送给下位机中，通过伺服系统来完成机器人的运动控制，并返回到模块P2。

如上所述，尽管参照特定的优选实施例已经表示和表述了本发明，但其不得解释为对本发明自身的限制。在不脱离所附权利要求定义的本发明的精神和范围前提下，可对其在形式上和细节上作出各种变化。

Claims (6)

1.一种基于轨道扩展的多机器人的寻迹编队控制方法，其特征在于：该方法中机器人的运动需满足非完整约束的动力学方程并且目标轨道是简单凸闭曲线，该方法包括如下步骤：

a)对于平面中一组目标轨道，将目标轨道沿其中心指向轨道上各点的向量扩展为关于轨道函数等值轨道簇，并确定机器人的可运动范围；

b)由轨道函数计算寻迹误差，设计机器人的虚拟角速度使寻迹误差达到设计要求；

c)由轨道函数和目标轨道对应的参数计算机器人沿轨道运动的广义弧长及其导数，由通信得到的邻居信息，设计机器人的控制力来实现编队；

d)计算真实与虚拟角速度的误差，设计机器人的控制力矩完成寻迹；

e)通过伺服系统完成机器人的运动控制。

2.根据权利要求1所述的基于轨道扩展的多机器人的寻迹编队控制方法，其特征在于：其中所述步骤a)包括如下步骤：

a1)将目标轨道沿其中心指向轨道上各点的向量扩展为等值轨道簇；

a2)根据曲线的正则条件，确定对应机器人的可运动范围；

a3)在可运动范围上构建轨道函数，使得每条等值轨道可由轨道函数取不同的值来表示。

3.根据权利要求1所述的基于轨道扩展的多机器人的寻迹编队控制方法，其特征在于：其中所述步骤b)包括如下步骤：

b1)由机器人的位置和轨道函数，计算轨道函数值与期望值间的位置寻迹误差；

b2)由轨道函数得到轨道的切向量，计算机器人的运动方向与轨道切方向间的角度寻迹误差；

b3)由位置寻迹误差和角度寻迹误差，设计机器人的虚拟角速度使得寻迹误差减少到满足的设计要求，同时保证机器人始终在可运动范围内运动。

4.根据权利要求1所述的基于轨道扩展的多机器人的寻迹编队控制方法，其特征在于：其中所述步骤c)包括如下步骤：

c1)规定好目标轨道对应的参数和弧长计算的起始点，由此确定等值轨道簇中其他轨道的弧长计算的起始点；

c2)由轨道函数和目标轨道对应的参数，计算机器人沿轨道从起始点到当前位置间的弧长；

c3)根据队形要求确定广义弧长与弧长间的函数关系，计算广义弧长及其对时间的导数；

c4)由通信得到的相邻机器人的广义弧长及其导数，设计机器人的控制力完成编队。

5.根据权利要求1所述的基于轨道扩展的多机器人的寻迹编队控制方法，其特征在于：其中所述步骤d)包括如下步骤：

d1)由机器人的真实角速度，计算其与步骤b)得出的虚拟角速度间的角速度误差；

d2)由寻迹误差及其对时间的导数以及步骤c)得出的机器人的控制力，计算虚拟角速度对时间的导数；

d3)根据角速度误差和虚拟角速度的导数，设计机器人的力矩使得角速度误差减少到满足的设计要求从而最终完成寻迹。

6.根据权利要求1所述的基于轨道扩展的多机器人的寻迹编队控制方法，其特征在于：其中所述步骤e)包括如下步骤：

由上位机将机器人的控制力和力矩发送给下位机中，通过伺服系统来完成机器人的运动控制。

Images