一种基于视觉测量信息的密集编队控制方法与流程

本发明属于多智能体系统协同控制技术领域，特别涉及一种基于视觉测量信息的密集编队控制方法。

背景技术：

基于几何构型约束的编队是一种常见的多智能体系统协同方式。如在自然界，雁群常呈“人”字形飞行，即实现了头雁对整个雁群的有效领导，又减小了雁群中老弱病残者的体力消耗；海豚群捕食时常对鱼群形成一个包围圈，不断缩小包围半径以提高海豚捕食成功的概率。受自然界这些生物集群自然现象启发，近些年无人机、无人车和无人艇等人造智能体广泛采用编队协同形式，以提高搜索、侦查和监测等应用场景下的任务半径，提高执行效率。

编队的几何构型约束本质上是对编队各智能体相对位置关系的描述，因此，编队常通过控制各智能体间相对位置来使实现期望构型的控制。实际控制中，各智能体首先借助于惯导系统或者gps系统来测量自身位置信息，然后各智能体借助于通信获取的相邻智能体位置信息来计算相对位置信息，最后各智能体结合期望构型约束下的期望相对位置信息计算控制指令调整自身位置，最终使得相对位置信息等于期望值。可以看出，各智能体自身定位精度对编队控制精度有重要影响。但是惯导系统存在累积偏差，系统工作时间越长位置测量精度越低；gps系统测量同样存在偏差，特别是当编队任务环境存在遮挡物时，由于不能正常接受gps信号各智能体将无法准确获悉自身位置信息。因此，当编队密集度较高、相邻智能体间期望相对距离较小时，惯导系统或者gps系统的定位偏差将会对编队控制精度有重要影响。

技术实现要素：

本发明提出一种基于视觉测量信息的密集编队控制方法，能够在编队智能体自身定位精度较低情况下，提高编队控制的精度。

本发明包括下列步骤：

s1：设计编队期望构型，确定各智能体相邻智能体及其相对于相邻智能体的夹角约束；

s2：根据各智能体需满足夹角约束，确定各智能体需测量视线角的智能体，以及需发送和接收所测量视线信息的智能体；

s3：各智能体利用所测量视线角信息计算其与相邻两智能体的夹角，并将该夹角信息传递给相邻智能体；

s4：基于指定夹角约束的期望值和当前实际值，各智能体计算速度控制指令；

s5：各智能体采用速度控制指令调整自身位置；

s6：若时间到达指定的预定的编队控制任务时间，则编队控制结束，否则返回步骤s3。

进一步地，所述s1中具体步骤为：

假设三维空间下运动的编队由n个智能体组成，各智能体依次编号为1、2、…、n；编队采用主从式拓扑结构，记1号智能体为一级主智能体，2号智能体为二级智能体，3号智能体为三级主智能体，其余智能体为从智能体。其中2号主智能体相对1号和3号智能体有夹角约束φ123，3号智能体相对于1号和2号智能体存在夹角约束φ132；各从智能体l相对于智能体i、j和k均存在夹角约束φikl、φilj和φjlk，其中l≥4，i、j、k＜l；实际任务执行过程中，编队中的夹角约束φ123和φ132由3号主智能体执行，φikl、φilj和φjlk由l号主智能体执行；

若编队仅限于二维平面下运动，则只需定义1号和2号智能体为主智能体，其余智能体为从智能体。各从智能体k相对于智能体i和j号智能体存在夹角约束φijk和φikj，其中k≥3，i、j＜k，且两夹角约束均由智能体k调整自身位置来满足；二维平面下运动的编队是在1号2号主智能体的带领下完成多样性任务，其余智能体则以保持指定编队构型为目的。

进一步地，所述s2中，

为达到编队下的指定夹角约束，对于三维空间下运动的编队，为提供3号智能调整自身位置需要的信息，需要2号智能体能测得夹角φ123的大小，3号智能体能测得夹角φ132的大小，同时2号智能体应能将φ123发送给3号智能体；l号智能体配有视觉传感器以测得φilj和φjlk的大小，其中l≥4，k号智能体配有视觉传感器以测得夹角φikl的大小、k号智能体能与l号智能体通信以将φikl的值发送给它；

对于二维平面下运动的编队，结合指定的夹角约束知，l号智能体为实现夹角约束φijk和φikj，其中l≥3，则要求j号和k号智能体配有视觉传感器测得夹角φijk和φikj的实际大小，同时j号智能体将φijk发送给k号智能体；

各智能体关于相邻两智能体的夹角具体测量如下：记i号智能体视觉传感器的测量坐标系为oi-xiyizi，相邻j号和k号智能体在该坐标系的视线方位角和高低角分别记为：σij、θij、σik和θik，j号和k号智能体相对于i号智能体的单位视线方向向量可表示为：

则j号和k号智能体关于智能体i智能体的夹角可表示为：

二维平面内，各智能体关于相邻两智能体的夹角可视为其视线角的差值：

φjik＝θij-θik3

进一步地，所述s4中具体步骤为：

假设各智能体运动特性可用一阶微分环节近似：

其中pi为各智能体位置，ui为各智能体控制输入；对三维空间下运动的编队有二维平面下运动的编队有pi、

对于三维空间下运动的编队，3号智能体为满足夹角约束φ123和φ132，采用控制律形式为：

其中和表示夹角约束φ123和φ132在期望编队构型下的期望值，e31和e32表示1号智能体和2号智能体相对于3号智能体的视线单位方向向量在3号智能体测量坐标系下的表示，e21和e23表示1号和3号智能体相对于2号智能体的视线单位方向向量在3号智能体测量坐标系下的表示，pe32表示：

式中表示单位对角阵。

记：

结合pe32定义知表达式pe32(e21+e23)对应的物理含义为向量e21+e23在向量e32垂直方向上的分量，即u3,1表示垂直于向量e32的控制加速度。由u3,2的形式知其表示沿向量e32方向的控制加速度；可通过对智能体3施加作用力u3,1使得夹角φ123调整到期望值；通过对智能体3施加控制力u3,2使得在不改变夹角φ123的情况下调整φ132的大小；式5所示控制律可以解耦的形式分别调整夹角φ123和φ132的大小。

对于l(l≥4)号智能体为满足夹角约束φikl、φilj和φjlk，可采用控制律形式如下：

式中和分别表示φikl、φilj和φjlk的期望值，表示垂直于j、l和k号智能体所在平面的单位方向向量，可表示为：

其中符合“×”表示向量的叉乘。

记：

其中ul,1表示在i、k和l号智能体所在平面内且垂直于向量ekl的控制力；ul,2表示垂直于i、k和l号智能体所在平面内的控制力；ul,3表示沿向量elk方向的控制力；可通过对l号智能体施加控制力ul,1调整夹角φikl的大小；可通过对l号智能体施加控制力ul,2使得在不改变夹角φikl的情况下调整夹角φjkl的大小；可通过对l号智能体施加控制力ul,2使得在不改变夹角φikl和φjkl大小的情况下调整φilk的大小；式8可以解耦的形式调整夹角φikl、φjkl和φilk的大小。

对于二维平面下运动的编队，智能体k为满足其相对于相邻智能体i号和j号需满足夹角约束φijk和φikj，k≥3，i,j＜k，采用如下控制律：

本发明提出的基于视觉测量信息的编队控制方法。由于视觉传感器测量量是相对物理量，直接体现了智能体间的相对几何关系，相对于惯导和gps系统视觉传感器具有智能体间间距越小测量精度越高的特性，因此本发明能够在编队智能体自身定位精度较低情况下，提高编队控制的精度。

附图说明

图1本发明编队控制方法流程图；

图2主智能体间的几何约束；

图3从智能体给定的几何约束；

图4主智能体间的几何约束；

图5从智能体给定的几何约束；

图6对智能体3施加控制力效果示意图；

图7对l号智能体施加控制力效果示意图

图8三维空间下的编队控制效果；

图9二维平面下的编队控制效果。

具体实施方式

下面将结合附图对本发明进行详细的阐述。

为避免惯导和gps系统自身定位偏差对密集编队控制精度的影响，本发明提出了一种基于视觉测量信息的编队控制方法。视觉传感器测量量是相对物理量，即视线角信息，直接体现了智能体间的相对几何关系，且相对于惯导和gps系统视觉传感器具有智能体间间距越小测量精度越高的特性。基于此特性，本发明提出了一种基于视觉测量信息的编队控制方法。发明首先给出了一种适用于视觉测量信息实现编队期望构型控制的智能体间关联结构，然后给出了编队的测量和通信配置要求，最后给出了一种各智能体利用视觉测量信息实现编队期望构型的控制策略。

如图1所示，本发明方法有如下步骤：

第一步：设计编队期望构型，确定各智能体相邻智能体及其相对于相邻智能体的夹角约束；

第二步：利用各智能体需满足夹角约束，确定各智能体需测量视线角的智能体，以及需发送和接收所测量视线信息的智能体，即信息测量和交互拓扑结构；

第三步：各智能体利用所测量视线角信息计算其与相邻两智能体的夹角，并根据信息交互拓扑结构将该夹角信息传递给相邻智能体；

第四步：基于指定夹角约束的期望值和当前实际值，各智能体利用式5、8和11计算速度控制指令；

第五步：各智能体采用速度控制指令调整自身位置；

第六步：若时间到达指定的预定的编队控制任务时间，则编队控制结束，否则返回第三步。

一.定义编队几何构型约束

假设三维空间下运动的编队由n个智能体组成，各智能体依次编号为1、2、…、n。编队采用主从式拓扑结构，记1号智能体为一级主智能体，2号智能体为二级智能体，3号智能体为三级主智能体，其余智能体为从智能体。其中2号主智能体相对1号和3号智能体有夹角约束φ123，3号智能体相对于1号和2号智能体存在夹角约束φ132，如图2所示；各从智能体l(l≥4)相对于智能体i、j和k(i、j、k＜l)均存在夹角约束φikl、φilj和φjlk，如图3所示。实际任务执行过程中，编队中的夹角约束φ123和φ132由3号主智能体执行，φikl、φilj和φjlk由l号主智能体执行。考虑到编队在三维空间下运动，因此编队中的1号和2号主智能体实际运动过程中无需考虑编队构型约束，可自由运动，带领编队中其余智能体完成多样性任务；3号主智能体实际运动考虑2个约束，还存在一个自由度，可结合实际任务需要配合1号和2号主智能体做一定适应性变化；其余智能体均存在3个约束，主要以维持指定的编队构型为控制目的。

若编队仅限于二维平面下运动，则只需定义1号和2号智能体为主智能体，其余智能体为从智能体。各从智能体k(k≥3)相对于智能体i和j(i、j＜k)号智能体存在夹角约束φijk和φikj，且两夹角约束均由智能体k调整自身位置来满足。可以看出，二维平面下运动的编队是在1号2号主智能体的带领下完成多样性任务，其余智能体则以保持指定编队构型为目的。二.确定各智能体测量和通信需求

结合具体任务，三维空间下的编队由1号、2号和3号主智能体相互配合控制任务流程，二维空间下的编队则由1号和2号主智能体配合控制任务流程。因此，任务执行前需对以上智能体的航迹进行规划或者指定任务目的。因此，需要结合具体任务确定1、2和3号智能体的测量需求，如对于循迹飞行任务，则应配有定位测量装置。

为达到编队下的指定夹角约束，对于三维空间下运动的编队，为提供3号智能调整自身位置需要的信息，需要2号智能体能测得夹角φ123的大小，3号智能体能测得夹角φ132的大小，同时2号智能体应能将φ123发送给3号智能体；l(l≥4)号智能体配有视觉传感器以测得φilj和φjlk的大小，k号智能体配有视觉传感器以测得夹角φikl的大小、k号智能体能与l号智能体通信以将φikl的值发送给它。对于二维平面下运动的编队，结合指定的夹角约束知，l(l≥3)号智能体为实现夹角约束φijk和φikj，则要求j号和k号智能体配有视觉传感器测得夹角φijk和φikj的实际大小，同时j号智能体将φijk发送给k号智能体。

各智能体关于相邻两智能体的夹角具体测量如下：记i号智能体视觉传感器的测量坐标系为oi-xiyizi，相邻j号和k号智能体在该坐标系的视线方位角和高低角分别记为：σij、θij、σik和θik，如图4所示，则知j号和k号智能体相对于i号智能体的单位视线方向向量可表示为：

则j号和k号智能体关于智能体i智能体的夹角可表示为：

二维平面内，各智能体关于相邻两智能体的夹角可视为其视线角的差值，如图5所示：

φjik＝θij-θik3

第三步：确定各智能体控制策略

假设各智能体运动特性可用一阶微分环节近似：

其中pi为各智能体位置，ui为各智能体控制输入。对三维空间下运动的编队有pi、二维平面下运动的编队有pi、

对于三维空间下运动的编队，3号智能体为满足夹角约束φ123和φ132，采用控制律形式为：

其中和表示夹角约束φ123和φ132在期望编队构型下的期望值，e31和e32表示1号智能体和2号智能体相对于3号智能体的视线单位方向向量在3号智能体测量坐标系下的表示，e21和e23表示1号和3号智能体相对于2号智能体的视线单位方向向量在3号智能体测量坐标系下的表示，pe32表示：

式中表示单位对角阵。

记：

结合pe32定义知表达式pe32(e21+e23)对应的物理含义为向量e21+e23在向量e32垂直方向上的分量，即u3,1表示垂直于向量e32的控制加速度。由u3,2的形式知其表示沿向量e32方向的控制加速度。由图6可知：可通过对智能体3施加作用力u3,1使得夹角φ123调整到期望值；通过对智能体3施加控制力u3,2使得在不改变夹角φ123的情况下调整φ132的大小。因此，式5所示控制律可以解耦的形式分别调整夹角φ123和φ132的大小。

对于l(l≥4)号智能体为满足夹角约束φikl、φilj和φjlk，可采用控制律形式如下：

式中和分别表示φikl、φilj和φjlk的期望值，表示垂直于j、l和k号智能体所在平面的单位方向向量，可表示为：

其中符合“×”表示向量的叉乘。

记：

由上式中各式形式知：ul,1表示在i、k和l号智能体所在平面内且垂直于向量ekl的控制力；ul,2表示垂直于i、k和l号智能体所在平面内的控制力；ul,3表示沿向量elk方向的控制力。由图7知可通过对l号智能体施加控制力ul,1调整夹角φikl的大小；可通过对l号智能体施加控制力ul,2使得在不改变夹角φikl的情况下调整夹角φjkl的大小；可通过对l号智能体施加控制力ul,2使得在不改变夹角φikl和φjkl大小的情况下调整φilk的大小。因此，式8会以解耦的形式调整夹角φikl、φjkl和φilk的大小。

图8给出了三维空间下四智能体编队的控制效果，编队期望几何构型为正四面体，编队下的3号和4号智能体分别采用式5和式8所示控制律，可以看出在该控制律作用下编队收敛到期望构型。

对于二维平面下运动的编队，智能体k(k≥3)为满足其相对于相邻智能体i号和j号(i,j＜k)需满足夹角约束φijk和φikj，同样可采用与式5相似的控制律：

图9给出了二维平面下四智能体编队的控制效果，编队期望几何构型为正方形，编队下的3号和4号智能体均采用式11所示控制律，可以看出在该控制律作用下编队收敛到期望构型。