面向海洋机器人护航的全局刚性图编队集结方法及系统

本发明属于多智能体协同控制，尤其涉及海洋机器人编队集结方法。

背景技术：

1、面向的海洋作业任务越发复杂，单个海洋机器人已经无法满足规模庞大、任务复杂的海洋作业需求。而海洋机器人编队比单个海洋机器人具有更强的鲁棒性、容错性、机动性和灵活性，可以有效地克服单艇能力不足的缺陷，大幅度扩大作业范围，显著提高作业效率，以相对较少的代价成本完成更复杂的作业任务。相比于常规舰艇编队执行护航任务，采用海洋机器人编队执行护航任务可以极大程度地降低护航作业成本、提高护航效率、减少人员伤亡。因此研究海洋机器人护航编队控制技术具有巨大的研究价值和应用空间。

2、根据智能体获取邻居信息的不同类型，多智能体编队控制方式主要可以分为三类：基于邻居间位置信息、基于邻居间距离信息和基于邻居间方位信息的编队控制。基于位置信息的编队控制是编队控制领域中常用的方法之一，其可以细分为基于绝对位置和相对位置信息的编队控制两种。而基于距离信息的编队控制方法，理论较为成熟，使用最为广泛。

3、名称为《基于网络化微型无人机的空中护航任务》的论文提出一种面向空中护航任务的编队控制方法，该方法将护航任务简化为一个具有领导-跟随结构的多智能体系统的编队控制问题。护航目标被视为领导者，而无人机被视为跟随者，使无人机跟踪护航目标的速度，并与护航目标一起形成稳定的编队队形。公开号为cn115237130a、名称为“基于虚拟领航-跟随者的多机器人编队控制方法”的专利提供一种跟随者跟踪虚拟领航者的编队控制方法，该方法通过跟随机器人与虚拟领航机器人之间的期望相对距离和期望方位角，对跟随机器人的线速度和角速度进行控制，使其收敛到保持队形所需的期望相对距离和期望相对方位角。但是该方法是基于相对位置的编队控制，需要估计机器人邻居节点的距离和局部方位信息来获取相对位置信息，这无疑增加了传感器系统软硬件层面的复杂程度，不利于护航编队系统的实现。

4、专利号为cn110262553b、名称为“基于位置信息的固定翼无人机编队飞行方法”的专利提供一种多无人机的编队集结控制方法，该方法通过预定的队形解算目标位置，再通过僚机的实际位置和目标位置偏差，计算僚机的目标姿态、速度和高度，从而实现多无人机按照预定队形进行集结飞行。公开号为cn108829113a、名称为“一种多机器人编队自适应零空间行为融合方法”的专利提供一种多机器人的编队集结控制方法，该方法使用机器人当前位置距离队形中心的实际位置偏差与期望位置偏差，定义多机器人的编队队形，实现编队集结控制。但是这种基于绝对位置的编队集结方法，需要假定智能体在全局坐标系下可获取自身绝对位置和感知邻居位置信息，编队集结点与队形中心的相对位置是预先设定的，这一约束使得该方法过于保守。当多机器人初始位置随机分布时，往往存在多机器人趋向集结点会舍近求远的现象，编队集结过程不够灵活。并且基于最优理论集结点分配算法需要离线计算，不适用于工程实践。

5、公开号为cn112904847a、名称为“基于最小弱刚性的二维编队通信拓扑生成和表达方法”的专利提供一种基于刚性二维编队拓扑的生成和表达方法，该方法通过构建最小弱刚框架实现通信拓扑生成，并且法将相对位移内积为约束条件利用队形图中的夹角来确定所需的队形，可快速完成二维编队的最小弱刚性约束。但是该方法采用最小弱刚框架，不能保障框架拓扑图的唯一性。并且该方法只进行了刚性二维队形生成的研究，没有涉及到刚性编队控制方法。

6、公开号为cn107632614a、名称为“一种基于刚性图理论的多无人机编队自组织协同控制方法”的专利提供一种基于刚性图理论的多无人机编队集结控制方法，该方法基于刚性图构建无人机编队目标刚性网络拓扑结构，通过定义各无人机个节点间的距离来构成期望的队形，实现多无人机编队集结控制。这种基于距离信息的编队控制方法的优势在于不需要坐标系的存在，可以很好的解决由于需要坐标系而产生的局限性问题。但是该方法采用最小刚性图构建队形拓扑结构，可能存在等价框架的产生，不能保障队形框架拓扑图的唯一性。全局性图与最小刚性图相比，更容易避免陷入局部最优。并且，该方法没有考虑编队护航问题，即多机器人集结形成编队队形的同时追踪护航目标。

7、综上所述，对于护航编队集结控制问题，基于位置的编队集结控制方法如虚拟领航-跟随法，存在集结策略不灵活、效率低等问题，而基于距离的编队控制方法如基于刚性图的编队控制方法，没有考虑追踪目标问题，因此以上都无法满足编队护航任务需求。

技术实现思路

1、本发明是为了解决基于位置的编队集结控制方法存在集结策略不灵活、效率低的问题，而基于距离的编队控制方法没有考虑追踪目标的问题，现提供面向海洋机器人护航的全局刚性图编队集结方法及系统。

2、面向海洋机器人护航的全局刚性图编队集结方法，包括以下步骤：

3、步骤一：建立海洋机器人护航任务中期望集结队形的全局刚性框架f*：

4、f*＝(g,p*)，

5、其中，全局刚性框架的无向图g表示海洋机器人编队执行护航任务的期望集结队形，p*为所有海洋机器人的期望位置，为第n个海洋机器人的期望位置，n为参与护航任务的海洋机器人总数；

6、步骤二：以海洋机器人的期望位置与实际位置的边函数误差z＝φ(p*)-φ(p)＝0为控制目标，建立各海洋机器人的全局刚性框架集结控制模型：

7、

8、其中，φ(p*)和φ(p)分别表示p*和p的边函数，p为所有海洋机器人的实际位置，p＝[p1,p2,...,pn]，ui为第i个海洋机器人的全局刚性框架集结控制律，s为反馈误差且有s＝ui-uf，ks和kf分别为s和z的正向控制增益，表示的转置矩阵，为的刚度矩阵，和分别为第i个和第j个海洋机器人的期望位置，h为修正指令，1n＝[1,2,...,n]t，i＝1,2,...,n，j＝1,2,...,n，i≠j；

9、步骤三：令步骤二所述修正指令h通过虚拟领航者追踪护航目标来修正海洋机器人编队的期望速度，则修正指令h的表达式为：

10、

11、其中，kr和kv分别为ur和qv的正向控制增益，为的对称雅克比矩阵，pv为虚拟领航者与护航目标之间的位置误差、且有pv＝pr-pt，qv为虚拟领航者与护航目标之间的速度误差、且有pr和ur分别为虚拟领航者的位置和速度，pt和分别为护航目标的位置和速度；

12、步骤四：将步骤三所述修正指令h代入步骤二所述各海洋机器人的全局刚性框架集结控制模型中，获得基于虚拟领航者追踪护航目标的全局刚性框架集结控制律：

13、

14、步骤五：将步骤四获得的全局刚性框架集结控制律作为大地坐标系下海洋机器人的期望速度向量，并利用该期望速度向量计算海洋机器人的期望速度和期望航向，

15、

16、

17、其中，ui＝[uix,uiy]t，uix和uiy分别为第i个海洋机器人在大地坐标系下x轴和y轴坐标的分速度，ui为第i个海洋机器人的期望速度，ψi为第i个海洋机器人的期望航向角；

18、步骤六：利用海洋机器人的期望速度和期望航向角控制参与护航任务的海洋机器人，从而实现海洋机器人护航任务的全局刚性图编队集结。

19、进一步的，上述步骤一中，所述全局刚性框架的无向图g具有3-连通性，且其表达式为：

20、g＝(v,e)，

21、其中，v＝{1,2,...,n}为期望集结队形中所有海洋机器人的集合，为两个海洋机器人之间连线的集合。

22、进一步的，上述步骤一中所述全局刚性框架f*的刚度矩阵r(p*)的秩为：rank[r(p*)]＝2n-3，

23、所述刚度矩阵r(p*)的表达式为：

24、

25、进一步的，上述步骤二中，所述φ(p*)和φ(p)的表达式分别如下：

26、φ(p)＝[||p1-p2||2,||p1-p3||2,...,||p1-pn||2,||p2-p3||2,...,||p2-pn||2,...,||pn-1-pn||2]，

27、

28、进一步的，上述步骤三中，虚拟领航者的位置pr表达式为：

29、

30、面向海洋机器人护航的全局刚性图编队集结系统，包括：

31、框架建立单元：建立海洋机器人护航任务中期望集结队形的全局刚性框架f*：

32、f*＝(g,p*)，

33、其中，全局刚性框架的无向图g表示海洋机器人编队执行护航任务的期望集结队形，p*为所有海洋机器人的期望位置，为第n个海洋机器人的期望位置，n为参与护航任务的海洋机器人总数；

34、模型建立单元：以海洋机器人的期望位置与实际位置的边函数误差z＝φ(p*)-φ(p)＝0为控制目标，建立各海洋机器人的全局刚性框架集结控制模型：

35、

36、其中，φ(p*)和φ(p)分别表示p*和p的边函数，p为所有海洋机器人的实际位置，p＝[p1,p2,...,pn]，ui为第i个海洋机器人的全局刚性框架集结控制律，s为反馈误差且有s＝ui-uf，ks和kf分别为s和z的正向控制增益，表示的转置矩阵，为的刚度矩阵，和分别为第i个和第j个海洋机器人的期望位置，h为修正指令，1n＝[1,2,...,n]t，i＝1,2,...,n，j＝1,2,...,n，i≠j；

37、修正指令建立单元：令模型建立单元所述修正指令h通过虚拟领航者追踪护航目标来修正海洋机器人编队的期望速度，则修正指令h的表达式为：

38、

39、其中，kr和kv分别为ur和qv的正向控制增益，为的对称雅克比矩阵，pv为虚拟领航者与护航目标之间的位置误差、且有pv＝pr-pt，qv为虚拟领航者与护航目标之间的速度误差、且有pr和ur分别为虚拟领航者的位置和速度，pt和分别为护航目标的位置和速度；

40、控制律设计单元：将修正指令建立单元所述修正指令h代入模型建立单元所述各海洋机器人的全局刚性框架集结控制模型中，获得基于虚拟领航者追踪护航目标的全局刚性框架集结控制律：

41、

42、速度航向计算单元：将控制律设计单元获得的全局刚性框架集结控制律作为大地坐标系下海洋机器人的期望速度向量，并利用该期望速度向量计算海洋机器人的期望速度和期望航向，

43、

44、

45、其中，ui＝[uix,uiy]t，uix和uiy分别为第i个海洋机器人在大地坐标系下x轴和y轴坐标的分速度，ui为第i个海洋机器人的期望速度，ψi为第i个海洋机器人的期望航向角；

46、编队集结单元：利用海洋机器人的期望速度和期望航向角控制参与护航任务的海洋机器人，从而实现海洋机器人护航任务的全局刚性图编队集结。

47、进一步的，上述全局刚性框架的无向图g具有3-连通性，且其表达式为：

48、g＝(v,e)，

49、其中，v＝{1,2,...,n}为期望集结队形中所有海洋机器人的集合，为两个海洋机器人之间连线的集合；

50、全局刚性框架f*的刚度矩阵r(p*)的秩为：rank[r(p*)]＝2n-3，

51、所述刚度矩阵r(p*)的表达式为：

52、

53、进一步的，上述φ(p*)和φ(p)的表达式分别如下：

54、φ(p)＝[||p1-p2||2,||p1-p3||2,...,||p1-pn||2,||p2-p3||2,...,||p2-pn||2,...,||pn-1-pn||2]，

55、

56、虚拟领航者的位置pr表达式为：

57、

58、一种计算机可读的存储介质，所述存储介质存储有计算机程序，所述计算机程序被执行时实现如上述面向海洋机器人护航的全局刚性图编队集结方法。

59、电子设备，包括存储介质、处理器以及存储在所述存储介质中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序实现如上述面向海洋机器人护航的全局刚性图编队集结方法。

60、本发明所述面向海洋机器人护航的全局刚性图编队集结方法与现有的编队集结控制方法相比，有益效果是：

61、采用了全局刚性拓扑图保证了海洋机器人护航编队框架的唯一性，更容易避免陷入局部最优。同时，引入虚拟领航者的编队控制思想，使得海洋机器人编队中心与护航目标保持位置一致。设计了可在线计算的基于距离信息的编队集结控制器，使得海洋机器人护航编队可以快速灵活、安全、高效地集结成护航编队队形，满足海洋机器人护航编队集结阶段的任务需求。