CN111459161A - 一种多机器人系统人为干预控制方法 - Google Patents

Abstract

本发明公开了一种多机器人系统人为干预控制方法，运动干预意图场网络可以快速传播操作员的干预意图，加快多机器人系统对操作员输入的响应速度；通过选取不同的意图场参数，控制操作员对多机器人系统的输入影响范围；利用分组控制意图场网络，操作员可以自由改变多机器人系统的编队分组，而且这一过程是完全分布式的，这是本方法相比现有方法的优势；利用意图场网络，每个机器人都可以获知与其对应的人为干预意控制量，并通过非线性权重函数，机器人可以动态计算编队控制与人为干预的加权系数，实现编队控制和人为干预之间的动态权衡。

Description

一种多机器人系统人为干预控制方法

技术领域

本发明属于机器人控制技术领域，具体涉及一种多机器人系统人为干预控制方法。

背景技术

多机器人系统是指由多个机器人组成的、具有一定协作能力的系统。多机器人系统可以用于仓储物理、物品搬运、区域警戒、搜索搜救、灯光表演等各类场合。对于多机器人系统而言，实现机器人编队是一个基本的问题。目前国内外在编队算法的研究上已经有了很多的进展，如刚性编队、相对航向编队等。国内外现有的理论研究主要集中于多机器人自主控制方面，然而，实际环境中(如野外、灾区、战场等)，由于环境、任务的复杂性，多机器人系统在任务进行、运动控制中，都需要操作员对其进行监管、控制，进而保证机器人的安全。在对多机器人系统进行人为干预方面，遥控是最基本的手段。这方面现有的技术手段包括：Antonio Franchi等人研究的多机器人双边遥控系统(Franchi A,Secchi C,Ryll M,等.Shared Control:Balancing Autonomy and Human Assistance with a Group ofQuadrotor UAVs[J].IEEE Robotics&Automation Magazine,2012,19(3):57–68)、Liu Y-C等人研究的基于矩控制的多机器人遥控系统(Liu Y-C.Task-space coordinationcontrol of bilateral human–swarm systems[J].Journal of the FranklinInstitute,2015,352(1):311–331)、Magnus Egerstedt等人研究的基于动态覆盖算法的多机器人遥控系统(Lee S G,Diaz-Mercado Y,Egerstedt M.Multirobot Control UsingTime-Varying Density Functions[J].IEEE Transactions on Robotics,2015,31(2):489–493)。然而，这些方法虽然各自有其特色之处，它们都不能同时兼顾快速响应、分布式控制、灵活队形干预这三个目标，这使得其在实际应用中有所局限。

发明内容

有鉴于此，本发明的目的是提供一种多机器人系统人为干预控制方法，操作员可以在机器人编队行进中对机器人进行实时干预，可以通过干预改变机器人的编队队形，也可以通过干预改变使机器人分成(或合并为)若干小组，可以保证多机器人系统在人为干预下也能保持稳定。

1、一种多机器人系统人为干预控制方法，其特征在于，包括：

步骤一、假设有n个机器人要完成一项编队任务，机器人分别编号为1，2，……，n；设机器人i的位置为q_i∈R²；假设机器人之间可进行通讯，用无向图G(V,E)表示这个通讯拓扑，其中V＝{1,…,n}表示各机器人节点，(i,j)∈E表示机器人i和机器人j之间可以直接进行通讯；对于机器人i，若(i,j)∈E则称机器人j是机器人i的邻居，记机器人i的所有邻居的集合为N_i；设有一个或多个操作员对机器人进行管控，操作员可以向至少一个机器人发送运动干预信号和分组控制信号；在分组控制信号作用下，机器人之间将分成若干个小分组，用无向图G_F(V,E_F)描述这种分组，其中，若(i,j)∈E_F，则说明机器人i和机器人j处于同一分组；设机器人的期望队形由偏移向量{d_i}_i∈V决定，记机器人的编队误差为x_i＝q_i-d_i，则当对任意的机器人i,j，满足x_i-x_j＝0时，认为此时机器人完成了编队；

步骤二、更新运动干预意图场状态，具体为：

操作员向任意机器人i发送运动干预信号记作u_l,i，运动干预信号将传入运动干预意图网络F_L，机器人i在此网络中的状态记为h_l,i；设每隔dT时间，各机器人即与其邻居机器人进行一次通讯，将自身的状态发送给其所有的邻居节点，同时接收其邻居的状态；记k次通讯前，机器人i的状态为h_l,i(k)，h_l,i的更新过程如下：

1)在k＝0时，h_l,i(0)＝0；

2)在k＞0时：

2.1)向所有的邻居机器人j∈N_i，发送h_l,i(k)；

2.2)从各邻居机器人j接收状态h_l,j(k)；

2.3)按如下方式计算自身状态的增量dh_l,i(k)：

其中，a₀＞0,a_c＞0,a_t＞0是设计参数；当且仅当机器人i收到了操作员发送了运动干预信号时，δ_i(k)＝1；在机器人i没有收到运动干预信号时，δ_i(k)＝0；φ_∈是死区函数：

其中，∈＞0是设计参数，用于调整操作员运动干预信号的影响范围，当∈越大，则干预信号u_l,i(k)的影响越局限在机器人i的附近；

2.4)利用dh_l,i(k)计算下一时刻的状态：

h_l,i(k+1)＝h_l,i(k)+dh_l,i(k)dT

其中dT是干预意图网络F_L周期性进行通讯的时间间隔；

步骤三、更新分组控制意图场状态及分组拓扑：

操作员向任意机器人i发送分组控制信号，记作u_g,i(k)，收到分组控制信号的机器人称为领航者；分组控制信号将传入分组控制意图网络F_G，机器人i在此网络中的状态由三元组(h_g,i,r_g,i,n_g,i)表示，其分别表示机器人i所在分组的运动速度、机器人i对周围机器人的影响力、离机器人i最近的领航者机器人的编号；设每隔固定时间，各机器人即与其邻居机器人进行一次通讯，将自身在分组控制意图网络F_G中状态发送给其所有的邻居节点，同时接收其邻居的状态；记k次通讯前，机器人i的状态为(h_g,i(k),r_g,i(k),n_g,i(k))，其状态的更新过程如下：

1)在k＝0时，初始化为：

h_g,i(0)＝0,r_g,i(0)＝0,n_g,i(0)＝0；

2)在k＞0时：

2.1)向所有的邻居机器人j发送(h_g,i(k),r_g,i(k),n_g,i(k))，其中，j∈N_i；

2.2)从各邻居机器人j处接收其状态(h_g,j(k),r_g,j(k),n_g,j(k))；

2.3)若机器人i此时收到了操作员的分组控制信号u_g,i(k)，则令：

h_g,i(k+1)＝u_g,i(k),r_g,i(k+1)＝R_M,n_g,i(k+1)＝i

其中，R_M＞0为设计参数，R_M大于通讯拓扑图G的直径；

2.4)若机器人i此时未收到u_g,i(k)，则：

2.4.1)计算邻居范围内对自己最大的影响力

其中，

是机器人j对i的影响力；d_ij指的是机器人j对i的影响力的衰减程度：对j≠i，令d_ij＝1；对于d_ii，令d_ii＝δ_r；δ_r＞0；

2.4.2)计算自己邻居范围内的最大影响力节点p_i(k)；

2.4.3)若

则按如下方式更新状态：

h_g,i(k+1)＝h_g,p(k),

n_g,i(k+1)＝n_g,p(k)

2.4.4)若

则按如下方式更新状态：

h_g,i(k+1)＝0,r_g,i(k+1)＝0,n_g,i(k+1)＝0

3)、针对任意机器人i，更新其所在分组拓扑：

在第k次，对于机器人i所在分组的每个邻居j，若n_g,i(k)≠n_g,j(k)，且‖h_g,i(k)-h_g,j(k)‖≥δ_M，且‖r_g,i(k)-r_g,j(k)‖≤1，则将邻居j从该分组中去除；δ_M＞0；

在第k次，若机器人j不在机器人i所在分组，且n_g,i(k)＝n_g,j(k)，则将机器人j加入机器人i所在分组；

其他情况下，k+1次维持k次的分组状态；

步骤四、设z_i(t)是机器人i的积分器系统在t时刻的状态，在每一时刻t，机器人系统的期望速度u_i(t)按如下方式计算：

1)、计算组内编队控制量u_s,i(t)：

设S_i为当前时刻与机器人i在同一小组内的机器人邻居集合；令k_P＞0，k_I＞0为设计参数，则u_s,i(t)的计算方法为：

x_i(t)表示t时刻的编队误差；

其中，积分器z_i的更新方法为：

2)、计算组间距离保持控制量{u_ij(t)}：

针对是机器人i的邻居，但不与机器人i在同一组的机器人j，j∈N_i\S_i，组间距离保持控制量的计算方法为：

u_ij(t)＝k_P(x_j(t)-x_i(t))

3)、计算最终控制量：

1.1)利用加权函数λ_G(·,.)融合编队控制信号u_s,i(t)、{u_ij(t)}与分组控制信号h_g,i(k)，得到u_G,i(t)，其过程为：

1.11)利用零阶保持器，将离散时间信号h_g,i(k)转为连续时间信号h_g,i(t)；

1.12)利用非线性加权函数λ_G(.,.)，针对是机器人i的邻居，但不与机器人i在同一组的机器人j，计算加权系数λ_G,i,j(t)：

λ_G,i,j(t)＝λ_G(max{‖h_g,i(t)‖,‖h_g,j(t)‖},‖u_ij(t)‖)

1.13)计算u_G,i(t)：

1.2)利用加权函数λ_L(.,.)融合运动干预信号u_l,i(t)与u_G,i(t)，得到u_L,i(t)，其过程为：

1.21)利用λ_L(.,.)，计算加权系数λ_L,i(t)：

λ_L,i(t)＝λ_L(‖h_l,i(t)‖,‖u_G,i(t)‖)

1.22)利用λ_L,i(t)进行融合，计算u_L,i(t)：

u_L,i(t)＝(1-λ_L,i(t)/u_G,i(t)+λ_L,i(t)v_L,i(t)

其中，

∈＞0，v₀＞0是设计参数；

最终机器人i的控制量即为u_i(t)＝u_L,i(t)。

较佳的，λ_L和λ_G都可取为下列函数形式之一：

形式1、λ(u,x)＝Ku/(Ku+x+∈)，参数K＞0，∈＞0；

形式2、λ(u,x)＝1-exp(-(Ku^p)/(x^p+∈))，参数K＞0，∈＞0，p＞0。

较佳的，所述步骤2.4.2)中，计算自己邻居范围内的最大影响力节点p_i(k)的方法为：若机器人j∈N_i∪{i}满足

那么令p_i(k)＝j，即令j是最大影响力节点；当有多个机器人同时满足

时，选取其中编号最小的机器人为最大影响力节点。

相比于现有多机器人系统人机协同控制方法，本发明具有如下有益效果：

1、运动干预意图场网络可以快速传播操作员的干预意图，加快多机器人系统对操作员输入的响应速度；通过选取不同的意图场参数，控制操作员对多机器人系统的输入影响范围。前者使本系统比经典的领航-跟随者系统响应更快，后者对操作员影响范围的控制则是现有方法中所缺少的。

2、利用分组控制意图场网络，操作员可以自由改变多机器人系统的编队分组。而且这一过程是完全分布式的，这是本方法相比现有方法的优势。

3、利用意图场网络，每个机器人都可以获知与其对应的人为干预意控制量，并通过非线性权重函数，机器人可以动态计算编队控制与人为干预的加权系数，实现编队控制和人为干预之间的动态权衡。

附图说明

图1为本发明的方法流程图。

具体实施方式

下面结合附图并举实施例，对本发明进行详细描述。

在说明各部分的构成前，先对要控制的多机器人系统做一定的说明。假设有n个机器人要完成一项编队任务，机器人分别编号为1，2，……，n。设机器人i的位置为q_i∈R²，其满足简化的动力学方程：

其中u_i是待计算的控制量。假设机器人之间可利用无线网络等方式进行通讯。用无向图G(V,E)表示这个通讯拓扑，其中V＝{1,…,n}表示各机器人节点，(i,j)∈E表示机器人i和机器人j之间可以直接进行通讯。对于机器人i，若(i,j)∈E则称机器人j是机器人i的邻居，记机器人i的所有邻居的集合为N_i。设有一个或多个操作员对机器人进行管控，操作员可以通过手柄、触屏等交互设备向一个或少数几个机器人发送两种遥控信号：运动干预信号、分组控制信号。在分组控制信号作用下，机器人之间将分成若干个小分组。用图G_F(V,E_F)描述这种分组，其中，若(i,j)∈E_F，则说明机器人i和机器人j处于同一分组。设机器人的期望队形由偏移向量{d_i}_i∈V决定，按通行做法，记机器人的编队误差为x_i＝q_i-d_i，则当对任意的i,j，满足x_i-x_j＝0时，认为此时机器人完成了编队。

以下说明本发明控制器的各个部分：

一、用于传播、建模操作员控制意图的两个意图场网络

1.1运动干预意图网络F_L

操作员可以通过交互设备向任意机器人i发送运动干预信号，记作u_l,i∈R²。通过这一方式，操作员可以牵拉机器人i及其周围的机器人，使之暂时偏离期望的编队队形。运动干预信号将传入运动干预意图网络F_L，机器人i在此网络中的状态记为h_l,i∈R²。干预意图网络F_L是周期性运行的网络，设每隔dT时间，各机器人即与其邻居机器人进行一次通讯，将自身的状态发送给其所有的邻居节点，同时接收其邻居的状态。记第k次进行通讯前，机器人i的状态为h_l,i(k)，以机器人i为例，h_l,i∈R²的更新过程如下：

1)在k＝0时，h_l,i(0)＝0。

2)在k＞0时刻，

2.1)向所有的邻居机器人j∈N_i，发送h_l,i(k)。

2.2)从各邻居机器人j∈N_i处接收h_l,j(k)。

2.3)按如下方式计算自身状态的增量dh_l,i(k)：

其中，a₀＞0,a_c＞0,a_t＞0是设计参数；δ_i(k)＝1当且仅当机器人i收到了操作员发送了运动干预信号u_l,i(k)，在机器人i没有收到改信号时δ_i(k)＝0；φ_∈是如下定义的死区函数：

其中，‖·‖表示取模；∈＞0是设计参数，用于调整操作员运动干预信号的影响范围，当∈越大，则干预信号u_l,i(k)的影响越局限在机器人i的附近。

2.4)利用dh_l,i(k)计算下一时刻的h_l,i：h_l,i(k+1)＝h_l,i(k)+dh_l,i(k)dT，其中dT是干预意图网络F_L周期性进行通讯的时间间隔。

1.2分组控制意图网络F_G

操作员可以通过交互设备向任意机器人i发送分组控制信号，记作u_g,i∈R²；收到分组控制信号的机器人称为领航者；通过这一方式，操作员可以将机器人分成若干个小组，并控制每一组的运动。分组控制信号将传入分组控制意图网络F_G，机器人i在此网络中的状态由三元组(h_g,i,r_g,i,n_g,i)∈R²×R×Z表示，h_g,i表示机器人i所在分组的期望运动速度；r_g,i表示机器人i的影响力；它影响邻居机器人j时的影响力是r_g,i-d_ij，即影响力要随距离衰减(同样的，别人影响它时，影响力也随距离衰减)；它影响自己时的影响力是r_g,i-d_ii，即影响力要随时间衰减，一般而言，可令影响力随距离衰减的量d_ij＝1，同时令d_ii＝δ_r，δ_r＞0是某个固定的参数。

n_g,i表示离机器人i最近的领航者机器人的编号，n_g,i＝0表示机器人i当前无领航者。与运动干预意图网络F_L类似，分组控制意图网络F_G也是周期性运行的网络，设每隔固定时间，各机器人即与其邻居机器人进行一次通讯，将自身的状态发送给其所有的邻居节点，同时接收其邻居的状态。记第k次进行通讯前，机器人i的状态为(h_g,i(k),r_g,i(k),n_g,i(k))，以机器人i为例，其状态的更新过程如下：

1)在k＝0时，初始化为：

h_g,i(0)＝0,r_g,i(0)＝0,n_g,i(0)＝0

2)在k＞0时：

2.1)向所有的邻居机器人j∈N_i，发送(h_g,i(k),r_g,i(k),n_g,i(k))。

2.2)从各邻居机器人j∈N_i处接收(h_g,j(k),r_g,j(k),n_g,j(k))。

2.3)若机器人i此时收到了操作员的分组控制信号u_g,i(k)，则令

h_g,i(k+1)＝u_g,i(k),r_g,i(k+1)＝R_M,n_g,i(k+1)＝i

其中，R_M＞0为设计参数，一般设计为大于图G直径的正数。

2.4)若机器人i此时未收到u_g,i(k)，则

2.4.1)计算邻居范围内对自己最大的影响力

其中，

是机器人j对i的影响力；d_ij指的是机器人j对i的影响力的衰减程度：对j≠i，令d_ij＝1；对于d_ii，令d_ii＝δ_r；δ_r＞0是一设计参数。

2.4.2)计算自己邻居范围内的最大影响力节点p_i(k)：若机器人j∈N_i∪{i}满足

那么机器人令p_i(k)＝j，即令j是最大影响力节点；当有多个机器人同时满足

时，选取其中编号最小的机器人为最大影响力节点，用公式描述，即如下：

上式中，min()是一个机器人编号集合，在这个集合里的机器人，它们对机器人i的影响力

都是最大的。

2.4.3)若

则按如下方式更新状态：

h_g,i(k+1)＝h_g,p(k),

n_g,i(k+1)＝n_g,p(k)

2.4.4)若

则按如下方式更新状态：

h_g,i(k+1)＝0,r_g,i(k+1)＝0,n_g,i(k+1)＝0

二、基于意图场网络的编队分组模块

利用意图场F_L、F_G传播、生成的操作员控制意图，分组编队模块计算并更新多机器人系统的编队拓扑G_F(V,E_F)。这一模块与分组控制意图网络F_G同步运行，在更新完分组控制意图网络F_G中的状态后，将更新当前时刻的G_F(V,E_F)。这一更新过程是分布式的，不需要某个中心节点进行协调。对于机器人i而言，其更新方式如下：

1)在k＝0时，令E_F(0)＝E

2)在k＞0时，针对每个邻居j∈N_i，按如下方法进行决策。

2.1)若(i,j)∈E_F(k)，且n_g,i(k)≠n_g,j(k)，且‖h_g,i(k)-h_g,j(k)‖≥δ_M，且‖r_g,i(k)-r_g,j(k)‖≤1，则将边(i,j)从E_F(k+1)中去除。δ_M＞0是设计参数。

2.2)若

且n_g,i(k)＝n_g,j(k)，则将边(i,j)加入E_F(k+1)中。

2.3)其他情况下，若(i,j)∈E_F(k)，则(i,j)∈E_F(k+1)；若

则

三、基于比例积分控制的编队控制模块

如前所述，机器人i的位置记作为q_i，其在期望队形中所处的位置由d_i指定，x_i＝q_i-d_i是机器人i的编队误差向量。设z_i∈R²是机器人i的积分器系统，在每一时刻，编队控制模块计算如下的控制量：

1)组内编队控制量u_s,i(t)。设S_i＝{j∈N_i：(i,j)∈E_F}为当前时刻与机器人i在同一小组内的i的邻居集合。令k_P＞0，k_I＞0为设计参数，则u_s,i(t)的计算方法为：

其中，积分器z_i的更新方法为：

2)组间距离保持控制量{u_ij(t)}。对于j∈N_i\S_i，即所有是机器人i的邻居，但不与机器人i在同一组的机器人j，组间距离保持控制量的计算方法为：

u_ij(t)＝k_P(x_j(t)-x_i(t)),j∈N_i\S

四、基于非线性加权函数的共享控制模块

共享控制模块是本控制器最终的控制量计算模块。以机器人i为例，这一控制模块将融合组内编队控制量u_s,i(t)、组间距离保持控制量{u_ij(t)}、分组控制意图h_g,i(k)、运动干预意图h_l,i(k)，并计算得到最终的控制量u_i(t)。这一模块利用了非线性加权函数λ_L(·,·)、λ_G(·,·)对这些控制量进行融合。以下给出一些可选的加权函数形式，λ_L和λ_G都可取为下列函数形式之一：

形式1、λ(u,x)＝Ku/(Ku+x+∈)，参数K＞0，∈＞0。

形式2、λ(u,x)＝1-exp(-(Ku^p)/(x^p+∈))，参数K＞0，∈＞0，p＞0。

以机器人i为例，共享控制模块的计算最终控制量的过程如下。

1)利用加权函数λ_G(·,·)融合编队控制信号u_s,i(t)、{u_ij(t)}与分组控制信号h_g,i(k)，得到u_G,i(t)，其过程为：

1.1)利用零阶保持器，将离散时间信号h_g,i(k)转为连续时间信号h_g,i(t)

1.2)利用λ_G(·,·)，对每个j∈N_i\S_i，计算加权系数λ_G,i,j(t)：

λ_G,i,j(t)＝λ_G(max{‖h_g,i(t)‖,‖h_g,j(t)‖},‖u_ij(t)‖)

1.3)利用{λ_G,i,j(t)}进行融合，计算u_G,i(t)：

2)利用加权函数λ_L(·,·)融合运动干预信号u_l,i(t)与u_G,i(t)，得到u_L,i(t)，其过程为：

2.1)利用λ_L(·,·)，计算加权系数λ_L,i(t)：

λ_L,i(t)＝λ_L(‖h_l,i(t)‖,‖u_G,i(t)‖)

2.2)利用λ_L,i(t)进行融合，计算u_L,i(t)：

u_L,i(t)＝(1-λ_L,i(t)/u_G,i(t)+λ_L,i(t)v_L,i(t)

其中，

∈＞0时未避免“除以0”所用的小常数，v₀＞0是设计参数，表示机器人在人运动干预下的期望运动速度。

3)最终机器人i的控制量即为u_i(t)＝u_L,i(t)。

实施例：

步骤一、确定编队队形的偏移向量{d_i}。以一个由4个机器人组成的系统为例，如果4个机器人要形成的队形是边长为1的正方形，则可令{d_i}如下：

d₁＝[1,1]^T,d₂＝[1,0]^T,d₃＝[0,1]^T,d_r＝[0,0]^T

步骤二、确定通讯拓扑图G(V,E)。对于控制器实现而言，这一步是要确定每个机器人在网络中的邻居集合N_i。仍以4个机器人组成的系统为例，如果通讯拓扑是一个正方形，则可令机器人1到4的邻居集合为：

N₁＝{2,3},N₂＝{1,4},N₃＝{1,4},N₄＝{2,3}

注意，我们要求通讯拓扑图G(V,E)是对称的，即若i是j的邻居(i∈N_j)，则也必然有j是i的邻居(j∈N_i)。

步骤三、按照通讯拓扑图G(V,E)，建立机器人之间的通讯关系。同时，按需要，可以选择一个或几个机器人与操作员终端建立通讯关系，以便于接收操作员的遥控指令。

步骤四、确定运动干预意图场的参数：令a₀＝0.02,a_c＝1,a_t＝2,∈＝0.4。不同的死区大小∈会影响操作员通过运动干预意图影响的机器人的范围。

步骤五、确定分组控制意图场的参数：以通讯拓扑图G(V,E)是由4个机器人组成的正方形为例，该图的网络直径为2，故可令R_M＝3；同时令δ_r＝0.2。

步骤六、确定编队分组模块的参数：令δ_M＝0.2。δ_M越小，则越容易将机器人分成两组。

步骤七、确定编队控制模块的参数：令k_P＝1，k_I＝1。原则上只要k_P、k_I均为正数即可。

步骤八、确定共享控制模块参数：非线性权重函数可设计为：

同时，令v₀＝1，表明机器人在运动干预信号下应以1m/s速度前进。

步骤九、确定运动干预意图场的运行周期T_L、分组控制意图场的运行周期T_G、机器人控制环路的运行周期T_C。可令

T_L＝0.1s,T_G＝0.1s,T_C＝0.1s

步骤十、根据上述确定的参数，按如下方式对机器人进行控制。以下以机器人i为例：

1)初始化：

z_i←0,h_l,i←0,h_g,i←0,r_g,i←0,n_g,i←0,S_i＝N_i

2)每隔T_L时间，更新运动干预意图场状态：

2.1)若当前收到了来自操作员的运动干预指令u_l,i，则令δ_i＝1。

2.2)按当前收到的来自邻居机器人的状态{h_l,j}，更新自己的状态

h_l,i←h_l,i+u_h,l,iT_L

2.3)向所有的邻居机器人j∈N_i发送现在自己的状态h_l,i。

3)每隔T_G时间，更新分组控制意图场状态及分组拓扑：

3.1)如果收到了来自操作员的分组控制指令u_g,i，则令

h_g,i←u_g,i,r_g,i←R_M,n_g,i←i

3.2)如果未收到操作员的分组控制指令u_g,i，则按当前收到的来自邻居机器人的状态{h_g,j,r_g,j,n_g,j}，更新自己的状态

3.2.1)计算：

3.2.2)遍历每个邻居机器人j∈N_i，计算：

3.2.3)求

求

3.2.4)如果

则令

h_g,i←h_g,p,

n_g,i←n_g,p

3.2.5)如果

则令

h_g,i←0,r_g,i←0,n_g,i←0

3.3)向所有的邻居机器人j∈N_i发送现在自己的状态h_g,i,r_g,i,n_g,i。

3.4)更新当前自己的编队分组拓扑。遍历每个邻居机器人j∈N_i：

3.4.1)若j∈S_i，且n_g,i≠n_g,j，且‖h_g,i-h_g,j‖≥δ_M，且‖r_g,i-r_g,j‖≤1,则将j从同组邻居的集合S_i中去除，即S_i←S_i\{j}。

3.4.2)若

且n_g,i(k)＝n_g,j(k)，则将j添加到同组邻居的集合S_i中，即S_i←S_i∪{j}。

4)每隔T_C时间，计算机器人的控制量，更新积分器状态：

4.1)根据当前邻居机器人的位置、状态q_j,z_j计算编队控制量u_S,i、u_ij：

u_ij←k_P(q_j-q_i+d_i-d_j),j∈N_i\S_i

4.2)根据收到的邻居意图场状态h_g,i，计算加权系数λ_G,i,j：

λ_G,i,j←λ_G(max{‖h_g,i‖,‖h_g,j‖},‖u_ij‖)

4.3)计算融合了分组控制意图的控制量u_G：

4.4)计算加权系数λ_L,i：

λ_L,i←λ_L(‖h_l,i‖,‖u_G,i‖)

4.5)计算最终的机器人的控制量u_L,i，计算积分器的更新值u_z,i：

4.6)更新自己的积分器状态：

z_i←z_i+u_Z,iT_C

4.7)将速度控制量u_L,i转换为机器人的期望线速度v、期望角速度w，并用将v,w发送给机器人的下层控制器。记u_L,i＝[u_x,u_y]^T,则v,w可计算如下：

v←cos(θ)u_x+sin(θ)u_y

w←cos(θ)u_y-sin(θ)u_x

4.8)将自己的积分器状态z_i、位置q_i、编队偏移量d_i发给每一个邻居机器人j∈N_i。

综上所述，以上仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (3)

1.一种多机器人系统人为干预控制方法，其特征在于，包括：

步骤一、假设有n个机器人要完成一项编队任务，机器人分别编号为1，2，……，n；设机器人i的位置为q_i∈R²；假设机器人之间可进行通讯，用无向图G(V,E)表示这个通讯拓扑，其中V＝{1,…,n}表示各机器人节点，(i,j)∈E表示机器人i和机器人j之间可以直接进行通讯；对于机器人i，若(i,j)∈E则称机器人j是机器人i的邻居，记机器人i的所有邻居的集合为N_i；设有一个或多个操作员对机器人进行管控，操作员可以向至少一个机器人发送运动干预信号和分组控制信号；在分组控制信号作用下，机器人之间将分成若干个小分组，用无向图G_F(V,E_F)描述这种分组，其中，若(i,j)∈E_F，则说明机器人i和机器人j处于同一分组；设机器人的期望队形由偏移向量{d_i}_i∈V决定，记机器人的编队误差为x_i＝q_i-d_i，则当对任意的机器人i,j，满足x_i-x_j＝0时，认为此时机器人完成了编队；

步骤二、更新运动干预意图场状态，具体为：

操作员向任意机器人i发送运动干预信号记作u_l,i，运动干预信号将传入运动干预意图网络F_L，机器人i在此网络中的状态记为h_l,i；设每隔dT时间，各机器人即与其邻居机器人进行一次通讯，将自身的状态发送给其所有的邻居节点，同时接收其邻居的状态；记k次通讯前，机器人i的状态为h_l,i(k)，h_l,i的更新过程如下：

1)在k＝0时，h_l,i(0)＝0；

2)在k＞0时：

2.1)向所有的邻居机器人j∈N_i，发送h_l,i(k)；

2.2)从各邻居机器人j接收状态h_l,j(k)；

2.3)按如下方式计算自身状态的增量dh_l,i(k)：

其中，a₀＞0,a_c＞0,a_t＞0是设计参数；当且仅当机器人i收到了操作员发送了运动干预信号时，δ_i(k)＝1；在机器人i没有收到运动干预信号时，δ_i(k)＝0；φ_∈是死区函数：

其中，ε＞0是设计参数，用于调整操作员运动干预信号的影响范围，当∈越大，则干预信号u_l,i(k)的影响越局限在机器人i的附近；

2.4)利用dh_l,i(k)计算下一时刻的状态：

h_l,i(k+1)＝h_l,i(k)+dh_l,i(k)dT

其中dT是干预意图网络F_L周期性进行通讯的时间间隔；

步骤三、更新分组控制意图场状态及分组拓扑：

操作员向任意机器人i发送分组控制信号，记作u_g,i(k)，收到分组控制信号的机器人称为领航者；分组控制信号将传入分组控制意图网络F_G，机器人i在此网络中的状态由三元组(h_g,i,r_g,i,n_g,i)表示，其分别表示机器人i所在分组的运动速度、机器人i对周围机器人的影响力、离机器人i最近的领航者机器人的编号；设每隔固定时间，各机器人即与其邻居机器人进行一次通讯，将自身在分组控制意图网络F_G中状态发送给其所有的邻居节点，同时接收其邻居的状态；记k次通讯前，机器人i的状态为(h_g,i(k),r_g,i(k),n_g,i(k))，其状态的更新过程如下：

1)在k＝0时，初始化为：

h_g,i(0)＝0,r_g,i(0)＝0,n_g,i(0)＝0；

2)在k＞0时：

2.1)向所有的邻居机器人j发送(h_g,i(k),r_g,i(k),n_g,i(k))，其中，j∈N_i；

2.2)从各邻居机器人j处接收其状态(h_g,j(k),r_g,j(k),n_g,j(k))；

2.3)若机器人i此时收到了操作员的分组控制信号u_g,i(k)，则令：

h_g,i(k+1)＝u_g,i(k),r_g,i(k+1)＝R_M,n_g,i(k+1)＝i

其中，R_M＞0为设计参数，R_M大于通讯拓扑图G的直径；

2.4)若机器人i此时未收到u_g,i(k)，则：

2.4.1)计算邻居范围内对自己最大的影响力

其中，

是机器人j对i的影响力；d_ij指的是机器人j对i的影响力的衰减程度：对j≠i，令d_ij＝1；对于d_ii，令d_ii＝δ_r；δ_r＞0；

2.4.2)计算自己邻居范围内的最大影响力节点p_i(k)；

2.4.3)若

则按如下方式更新状态：

2.4.4)若

则按如下方式更新状态：

h_g,i(k+1)＝0,r_g,i(k+1)＝0,n_g,i(k+1)＝0

3)、针对任意机器人i，更新其所在分组拓扑：

在第k次，对于机器人i所在分组的每个邻居j，若n_g,i(k)≠n_g,j(k)，且‖h_g,i(k)-h_g,j(k)‖≥δ_M，且‖r_g,i(k)-r_g,j(k)‖≤1，则将邻居j从该分组中去除；δ_M＞0；

在第k次，若机器人j不在机器人i所在分组，且n_g,i(k)＝n_g,j(k)，则将机器人j加入机器人i所在分组；

其他情况下，k+1次维持k次的分组状态；

步骤四、设z_i(t)是机器人i的积分器系统在t时刻的状态，在每一时刻t，机器人系统的期望速度u_i(t)按如下方式计算：

1)、计算组内编队控制量u_s,i(t)：

设S_i为当前时刻与机器人i在同一小组内的机器人邻居集合；令k_P＞0，k_I＞0为设计参数，则u_s,i(t)的计算方法为：

x_i(t)表示t时刻的编队误差；

其中，积分器z_i的更新方法为：

2)、计算组间距离保持控制量{u_ij(t)}：

针对是机器人i的邻居，但不与机器人i在同一组的机器人j，j∈N_i\S_i，组间距离保持控制量的计算方法为：

u_ij(t)＝k_P(x_j(t)-x_i(t))

3)、计算最终控制量：

1.1)利用加权函数λ_G(·,·)融合编队控制信号u_s,i(t)、{u_ij(t)}与分组控制信号h_g,i(k)，得到u_G,i(t)，其过程为：

1.11)利用零阶保持器，将离散时间信号h_g,i(k)转为连续时间信号h_g,i(t)；

1.12)利用非线性加权函数λ_G(·,·)，针对是机器人i的邻居，但不与机器人i在同一组的机器人j，计算加权系数λ_G,i,j(t)：

λ_G,i,j(t)＝λ_G(max{‖h_g,i(t)‖,‖h_g,j(t)‖},‖u_ij(t)‖)

1.13)计算u_G,i(t)：

1.2)利用加权函数λ_L(·,·)融合运动干预信号u_l,i(t)与u_G,i(t)，得到u_L,i(t)，其过程为：

1.21)利用λ_L(·,·)，计算加权系数λ_L,i(t)：

λ_L,i(t)＝λ_L(‖h_l,i(t)‖,‖u_G,i(t)‖)

1.22)利用λ_L,i(t)进行融合，计算u_L,i(t)：

u_L,i(t)＝(1-λ_L,i(t))u_G,i(t)+λ_L,i(t)v_L,i(t)

其中，

∈＞0，v₀＞0是设计参数；

最终机器人i的控制量即为u_i(t)＝u_L,i(t)。

2.如权利要求1所述的一种多机器人系统人为干预控制方法，其特征在于，λ_L和λ_G都可取为下列函数形式之一：

形式1、λ(u,x)＝Ku/(Ku+x+∈)，参数K＞0，∈＞0；

形式2、λ(u,x)＝1-exp(-(Ku^p)/(x^p+∈))，参数K＞0，∈＞0，p＞0。

3.如权利要求1所述的一种多机器人系统人为干预控制方法，其特征在于，所述步骤2.4.2)中，计算自己邻居范围内的最大影响力节点p_i(k)的方法为：若机器人j∈N_i∪{i}满足

那么令p_i(k)＝j，即令j是最大影响力节点；当有多个机器人同时满足

时，选取其中编号最小的机器人为最大影响力节点。

Images