一种基于自组织编队行为的集群机器人轨迹跟踪方法与流程

本发明涉及群集机器人的一种轨迹跟踪技术，特别是一种基于自组织编队行为的集群机器人轨迹跟踪方法。

背景技术：

集群机器人自组织编队行为是移动机器人路径规划的一个重要分支。移动机器人模拟生物行为，自组织完成集群编队的轨迹跟踪，势必会对人类的生产生活产生一定的影响。现阶段，群集机器人编队及部署问题，已成为微小型机器人领域，或无线传感器网络领域的重要方向。目前，国内研究的群集机器人编队问题，都需要较强计算能力的处理器和强大的传感器系统，一个机器人会配备多个传感器协同进行工作。然而，对于传感能力单一，且计算能力较弱的群粒子机器人，编队控制的实现，就变的相当困难。因此，有必要设计一种基于自组织行为的群集机器人编队跟踪控制方法，以便解决微小型群集机器人自组织编队行为的轨迹跟踪问题。

技术实现要素：

本发明所要解决的技术问题是提供一种群集机器人，在无集中控制和领导机器人的情况下，自组织编队行为的轨迹跟踪方法，能够有效的实现微小型群集机器人，在自组织编队行为的约束下，群体机器人完成指定轨迹的跟踪任务。

一种基于自组织编队行为的集群机器人轨迹跟踪方法，包括以下步骤：

步骤1：向集群机器人发送所需跟踪的轨迹信息；

步骤2：将集群机器人组成六边形结构，放在指定路径起点，并初始化集群机器人信息；

步骤3：六边形结构的集群机器人之间通过相互传输信息，依据各自不同运动状态和梯度，对各个单机器人的运动状态进行自组织的周期性编队控制，依据所需跟踪的轨迹信息进行轨迹跟踪；

所述集群机器人之间相互传输信息是指集群机器人中单个机器人将自身位置坐标信息、ID信息、运动状态以及梯度信息发送给其他机器人，并获取与其他机器人之间的距离；

机器人的运动状态包括停止状态、准备状态、行进状态以及种子状态；

处于种子状态的机器人梯度为1，集群机器人中距离种子机器人最近的三个机器人的梯度为2，剩余机器人的梯度为3；

所述种子状态是指处于种子状态的机器人不运动，且作为其他行进状态机器人停止运动的标志物，当运动机器人与处于种子状态的机器人距离小于设定停靠距离时，停止运动；步骤4：判断集群机器人是否到达所需跟踪的轨迹终点，若到达，则集群机器人陷入死锁状态，程序停止运行，控制结束；若未到达，则返回步骤3。

所述对各个单机器人的运动状态和行为进行自组织的周期性编队控制的具体过程如下：

在每个工作周期中，首先从集群机器人中根据覆盖所需跟踪的轨迹区域类型计算优先值，并根据优先值和梯度对种子机器人进行判断，种子机器人应为上一周期梯度为3且有最大优先值的机器人，选定种子机器人后重置种子机器人梯度为1，种子机器人状态为种子状态；

接着，按照机器人梯度计算方法获取其余机器人梯度，即将集群机器人中距离种子机器人最近的三个机器人的梯度设置为2，剩余机器人的梯度设置为3；

然后，令梯度为2的机器人保持在停止状态；梯度为3的机器人状态为准备状态，按照机器人的ID顺序依次进入行进状态，当处于行进状态的机器人与种子机器人之间的距离小于设定的停靠阈值时，处于行进状态的机器人进入停止状态，测量计算自身所在坐标；

最后，当种子机器人周围的6个处于停止状态的机器人与种子机器人之间的距离均小于停靠阈值时，种子机器人从种子状态进入停止状态；

等待下一周期启动，重新选择种子机器人；

在第一个工作周期开始前，从集群机器人中任意选取一个机器人作为种子机器人，放置在所需跟踪的轨迹起点上。

在每个工作周期中，有1个机器人处于种子状态，有3个机器人处于行进状态，剩余机器人处于停止状态；

每个处于行进状态的机器人均是从停止状态，转到准备状态后再进入行进状态，当处于行进状态的机器人与处于种子状态的机器人距离小于设定阈值时，再转为停止状态；

以同时满足以下三个条件作为种子机器人：

1)当前机器人梯度为3；

2)当前机器人的优先值在所有梯度值为3的机器人中最大；

3)所有梯度值为3的机器人的优先值不完全相同；

所述机器人的优先值是指机器人覆盖所需跟踪的轨迹区域类型；

覆盖的轨迹为线条或弧线时，为线优先值；覆盖的轨迹存在拐点时，为点优先值；不存在轨迹覆盖时，为非优先值；

其中，线优先值大于点优先值，点优先值大于非优先值。

所述机器人覆盖所需跟踪的轨迹区域是指机器人的工作负责区域与与所需跟踪的轨迹的重合区域；

所述机器人的工作负责区域是以(x_o+1.155R,y_o+R)，(x_o-1.155R,y_o+R)，(x_o+1.155R,y_o-R)，(x_o-1.155R,y_o-R)四点组成的矩形区域；

其中，机器人圆面圆心点坐标为(x_o,y_o)，机器人半径为R。

当处于准备状态的机器人与其他正处于行进状态的机器人之间的距离小于设定的间隔阈值时，停止进入行进状态，转入停止状态，等待下一个工作周期。

处于行进状态的机器人沿边缘行走，以顺时针方向，围绕集群机器人，以与集群机器人间距为0.309倍的机器人半径距离进行移动。

所述机器人自身位置坐标信息采用如下定位方法进行计算获得：

$({\hat{x}}_{self},{\hat{x}}_{self})={\min }_{x_{self},y_{self}}\left(\underset{N}{\Σ}\right|D_N-\sqrt{{(x_n-x_{self})}^2+{(y_n-y_{self})}^2}\left|\right)$

其中，x_self，y_self分别表示为机器人真实的横、纵坐标；分别表示为机器人的横、纵坐标计算值；x_n，y_n分别表示为其他6个机器人中的第n个机器人测量计算所得的横、纵坐标；D_N表示为该机器人与通讯范围内第n机器人之间的测量距离。

所述步骤2中将集群机器人组成六边形结构，是指由7个单体机器人组成的指定六边形结构，由6个单体机器人环绕1个单体机器人而组成，且相邻单体机器人之间的间距为0.309倍机器人半径。

有益效果

本发明提供了一种基于自组织编队行为的集群机器人轨迹跟踪方法，该方法中的主要步骤为：步骤1：从控制器向机器人个体发送路径信息；步骤2：将由多个单体机器人组成的指定六边形结构排放在指定的路径起点，并初始化路径信息；步骤3：根据从控制器接收到的路径信息、六边形结构内其他单体机器人传输信息、状态和梯度对单体机器人的运动状态进行控制，从而实现对群机器人运动和编队的周期控制；步骤4：到达终点，系统陷入死锁状态，程序停止运行，控制结束。相较于能够实现群机器人自编队功能的机器人需要有较强计算能力的处理器和强大的传感器系统而言，该基于自组织编队行为的群集机器人轨迹跟踪方法，面向微小型群集机器人，对机器人硬件的要求低，硬件方面只需要红外传感器，计算能力较弱的处理器和电机。与其他复杂传感器的编队系统相比，更加轻便，有着更好的操控性。

在编队队形方面，运动时因为采用的是沿边缘行进行为算法，所以在该模型中每个周期结束之后都能够保证队形的统一，相比其他编队算法，该算法有更好的编队形状的稳定性。

在定位方面，因为该模型采用的是一种六边形的结构，在三边定位时不会出现因为机器人共线，而导致三边定位法的失败。所以在定位方面相比其他的队形结构，该六边形结构有着更加稳定的效果和更精确的测量。并且因为负责区域的设置，若某一时刻某一个机器人出现定位的误差，只要计算后的机器人负责区域覆盖真实的坐标，则在编队表现上就不会出现偏差，还会按照正常的轨迹行进。所以该系统在定位方面有着很强的容错性。

附图说明

图1为本发明整体流程图；

图2为本发明中央处理器与个体机器人关系图；

图3为本发明编队六边形结构及负责区域示意图；

图4为本发明单机器人部分硬件组成图；

图5为本发明运动状态转换图；

图6为机器人沿边缘行进的运动轨迹图；

图7为本发明优先值计算示意图；

图8为本发明编队一周期内运动示意图，其中，(a)为运动起始状态图，(b)为机器人梯度图，(c)为机器人编号状态说明图，(d)为R₁机器人起始运动图，(e)为R₂机器人起始运动图，(f)为R₁机器人停靠图，(g)为R₃机器人起始运动图，(h)为机器人停靠图，(i)为一周期运动后机器人梯度图，(j)为新一周期机器人梯度图；

图9为基于自组织编队行为的集群机器人轨迹跟踪实验过程截图，其中，(a)为起点处编队运动截图，(b)为拐点处编队运动截图，(c)为拐点后在直线路径上编队运动截图，(d)为终点处编队运动截图

具体实施方式

以下将结合附图和具体实施对本发明做进一步详细说明。

本发明所述的一种自组织编队行为的集群机器人轨迹跟踪方法总体流程图，如图1所示。实现步骤包括：

步骤1：向集群机器人个体发送所需跟踪的轨迹信息。

如图2所示。控制器仅包含通过红外信号发射器向各个终端机器人个体发送地图信息的功能，不包含在运行期间进行通信并向机器人发送动态指令的功能。从控制器向机器人个体发送的路径信息为单一起点，单一终点，路径无岔路的线型地图信息。路径无岔路的含义为群机器人不能在岔路口解体沿不同的方向行进。

步骤1.1：预先通过接口将控制算法文件存入ROM存储器中，由中央控制器通过红外装置向7个机器人发送地图路径信息，如图7。

步骤1.2：7个机器人接收到由中央控制器发送过来的地图信息和初始定位信息存储在ROM存储器中。

步骤2：将由集群机器人组成六边形结构，排放在指定路径起点，并初始化机器人信息。

单体机器人能够通过红外发射器，向通讯范围内的其他机器人交换位置坐标信息，ID信息，所处状态信息，梯度信息。并可通过红外发射器计算其与通讯范围内其他机器人之间的距离。即初始化信息包括位置坐标信息，ID信息，所处状态信息，梯度信息，通讯范围内与其他机器人之间的距离。

7个机器人按照指定六边形的方式摆放在图形预设起点处，并设置第一代种子状态机器人R_s，如图8(a)所示路径L为中央控制器发送至单体机器人的路径信息。

步骤3：根据路径跟踪信息，实现六边形结构的群集机器人间的信息传输。根据不同群集机器人的运动状态梯度，完成对单机器人的运动状态的编队控制，实现对群集机器人编队与轨迹跟踪的周期控制。

群集机器人间的信息传输具体包括：单机器人能够通过红外发射器，向通讯范围内的其他单体机器人发送其位置坐标信息、ID信息、所处状态信息、梯度信息。并可通过红外发射器，计算其与通讯范围内其他机器人之间的距离。

编队轨迹跟踪控制算法是通过控制单机器人进入不同的运动状态，完成轨迹跟踪。其中不同运动状态包括停止状态、准备状态、行进状态、种子状态。

7个机器人同时开始执行控制算法文件，并同时时刻通过红外发射器和红外接收器与其他6个机器人进行通信，并将其他机器人的ID信息、状态信息、梯度信息、定位信息，和自身的ID信息，状态信息，梯度信息实时更新存储在RAM中。通过红外发射装置向外广播信息RAM中的自身信息，并在停止后同时向外广播ROM中的定位信息。

步骤3.1：在预先设置种子状态机器人R_s后，其他6个机器人在RAM中检测到存在种子状态机器人且自身状态为停止状态，则除种子状态机器人以外的机器人进行梯度计算，R_s机器人梯度为1，紧密围绕R_s机器人的单体机器人梯度为2，其他三个机器人梯度为3，如图8(b)所示。

步骤3.2：如图8(c)所示，梯度为2的单体机器人R₄、R₅、R₆会滞留在停止状态。而梯度为3的跟随机器人R₁、R₂、R₃进入准备状态，并开始计算ID。ID计算结果为：R₁＝0.832，R₂＝0.518，R₃＝0.331，则运动先后顺序为：R₁，R₂，R₃。

步骤3.3：如图8(d)所示，机器人R₁开始进行沿边缘行进运动，顺时针绕静止不动的机器人进行运动。R₂机器人检测到与行进机器人R₁之间距离为d₁₂。且此时d₁₂<d₀，d₀为最小运动间距，则R₂保持准备状态。

步骤3.4：如图8(e)所示，此时d₁₂>d₀，则机器人R₂进入行进状态，开始进行沿边缘行进运动。此时机器人R₃与R₁的间距d₁₃<d₀，且与机器人R₂的间距d₂₃<d₀，则R₃机器人保持静止。

步骤3.5：如图8(f)所示，机器人R₁与种子状态机器人R_s之间距离d_1s≤0.309R，则机器人R₁随即进入停止状态，并执行定位行为，将自身的定位信息在ROM中进行更新。

步骤3.6：如图8(g)所示，机器人R₂沿边缘行进运动到此刻位置时d₂₃>d₀则机器人R₃进入行进状态。

步骤3.7：如图8(h)所示，机器人R₂与R₃先后运动到与种子机器人R_s间距，d_2s≤0.309R，d_3s≤0.309R，则R₂与R₃机器人先后停止运动，进入停止状态随后执行定位行为，并将自身的定位信息在ROM中进行更新。

步骤3.8：如图8(i)所示，此时7个机器人的梯度值如图所示。种子状态机器人R_s检测到自身被6个停止状态机器人紧密围绕，即通过检测RAM中信息，检测到与其余6个机器人距离全都小于0.309R，且其他机器人的状态都是停止状态，则自身将进入停止状态。

步骤3.9：如图8(j)所示，当所有机器人在RAM中检测到所有机器人状态为停止状态时，梯度为3的机器人进行优先值计算，并与其他梯度同为3的机器人进行比较。3个梯度为3的机器人中优先值最大的机器人进入种子状态。如图，机器人R_s计算梯度为线优先值，在图1(I)中梯度为3的机器人中优先值最大所以进入种子状态。其他6个机器人在RAM中检测到存在种子状态机器人，且自身为停止状态，则重新开始计算梯度判断状态。

由多个单体机器人组成的指定六边形结构及负责的区域，如图3所示。所包含的单体机器人数量为7个，且指定六边形结构为由6个单体机器人紧密环绕1个单体机器人为使得机器人所覆盖的区域无间隙，且不能重叠。需将7个机器人的排放结构按照如图3进行设置，直线l₁，l₂，l₃，l₄，l₅，l₆分别与圆相切。设中心位置的机器人与相邻机器人圆心之间的距离为D，机器人圆半径为R，机器人之间距离为d，则有：

$D=\frac{4\sqrt{3}}{3}R$

$d=D-2R=\left(\frac{4\sqrt{3}}{3}-2\right)R\&ap;0.309R$

$\frac{d}{2}\&ap;0.155R$

设中间机器人的坐标为(x_o,y_o)，则其负责的区域为由(x_o+1.155R,y_o+R)，(x_o-1.155R,y_o+R)，(x_o+1.155R,y_o-R)，(x_o-1.155R,y_o-R)4点组成的矩形区域。其他机器人的负责区域按照相同方法进行计算。该矩形形状负责区域能够无重叠覆盖七个机器人所在区域。

平铺的是指7个矩形分别覆盖了7个机器人所在区域。

单体机器人的硬件构造，如图4所示。为CPU为STM32处理器进行计算处理；ROM储存器储存由中央控制器发送的路径信息以及控制算法文件；RAM储存器，用来存储通讯范围内其他机器人的信息和状态，以及自身的状态信息；两个步进电机控制运动和转动；红外传发射器和接收器，用来进行与中央控制器和其他机器人的通信，并且能够测量与其他机器人之间的距离。

控制算法是通过控制单体机器人进入不同的状态执行不同的基本行为实现的。运动状态分为：停止状态、准备状态、行进状态、种子状态。基本行为包括：定位行为、沿边缘行进行为、计算梯度行为、计算优先值行为和计算ID行为。

根据控制算法，每个单体机器人会有一个自身运动状态周期，对于7个机器人组成的机器人群也存在一个运动周期。

单体机器人运动状态周期，如图5所示。起始为停止状态，然后根据优先值和梯度判断下一阶段状态为停止状态、种子状态或准备状态。

当种子状态机器人身边围绕停靠6个机器人时，机器人进入停止状态；

准备状态机器人，根据ID和最小运动间距，判断先行机器人，进入行进状态；

行进状态机器人，根据距离种子状态机器人的距离大小判断，是否进入停止状态。

六边形结构群机器人运动周期。在一个周期中只有3个机器人能够通过准备状态进入行进状态，有1个机器人为种子状态，其余3个为停止状态。周期描述如下：

起始7个机器人全部为停止状态。通过优先值和梯度判断种子状态机器人，并设种子状态机器人梯度为1；除种子之外6个机器人重新计算梯度；梯度为2的机器人仍保持停止状态；梯度为3的机器人进入准备阶段，并通过ID和最小运行间距判断运行先后运动顺序进入行进状态。当梯度为3的机器人以沿边缘行进运动先后停靠到种子状态机器人周围并进入停止状态后，种子状态机器人进入停止状态。此时7个机器人全部为停止状态组成指定六边形结构。

种子状态机器人进入停止状态；7个机器人全部为停止状态组成指定六边形结构。控制算法伪代码如下：

沿边缘行走，如图6所示。以顺时针方向围绕由静止机器人组成的机器人群体，各机器人间以间距为0.309R距离进行移动，并且仅在单体机器人的行进状态下执行。设置机器人运动步长为d_f，角度转动步长为α_f，并且需要记录之前一步的距离d_p，设置预期距离d_s和现有测量距离D。其中所指的距离为行进机器人与被该机器人围绕行进的机器人圆心之间的距离。在实际中d_s＝0.309R，R为单体机器人半径。伪代码如下：

If D<d_s

ifd_p<D

向前前进d_f；

else

逆时针转α_f并前进d_f；

end

else

ifd_p>D

向前前进d_f；

else

顺时针转α_f并前进d_f；

end

end

计算优先值：如图7所示，是根据自身定位行为计算定位信息和负责区域，与控制器预先向该单体机器人发送的路径信息，进行各机器人优先值计算的行为。根据机器人定位信息与给定跟踪路径的距离和位置，优先值计算结果可分为：线形优先值、拐点形优先值和非优先值。设定：线形优先值>拐点形优先值>非优先值。如图7所示，设路径由一个半圆圆弧C，两条直线L₁，L₂。L₁与L₂拐点P，起点S，终点F组成。直线路径L₁与半圆圆弧路径C平滑连接。若路径上存在一点i，且i∈A，A为一机器人的负责区域，则认为该路径∈A。

若仅L₁∈A则该机器人优先值为线优先值；

若仅L₂∈A则该机器人优先值为线优先值；

若仅P∈A则该机器人优先值为点优先值；

若仅C∈A则该机器人优先值为线优先值；

若A为则该机器人优先值为非优先值。

ID计算用来确定群运动周期开始时3个梯度为3的机器人的运动先后顺序。ID计算行为为随机一个[0,1]之间的数字作为自身的ID，ID大的先进行运动。

最小运动间距，即通过ID进行判断后机器人R_n先进入行进状态，R_m随后计入运动状态。初始时R_n与R_m之间的间距d_nm＝0，d_nm<d_o，d_o为最小运动间距，机器人R_n为行进状态，R_m为准备状态。当R_n运动一段时间后，与R_m间距满足d_nm>d_o时，R_m进入运动状态。

终点结束阶段。当全部机器人进入停止状态，梯度为3的3个机器人计算优先值时，求得的优先值均为非优先值，则不会再出现种子状态的机器人，则整个系统陷入死锁，即停止执行程序。

步骤4：步骤3.1至步骤3.9为一个运动周期，之后将会按照该运动周期的运动模式对该7个机器人组成的编队进行周期性的移动。设输入地图信息为“L”型路线，则编队运动过程如实验过程截图，图9所示，机器人显示的数字为机器人的梯度数值。直至编队运动到检测不到新种子状态机器人，即所有梯度为3的机器人计算的优先值都为非优先值的终点状态为止，系统进入死锁状态，停止运动。

以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。