一种面向海洋探测场景的AUV集群分层式敏捷协同控制方法

本公开实施例涉及水下机器人，尤其涉及一种面向海洋探测场景的auv集群分层式敏捷协同控制方法。

背景技术：

1、海洋作为地球上最大的战略地理单元，占据着地球丰富的资源，在人类社会中有着至关重要的地位。随着自主水下航行器（autonomous underwater vehicle，auv）技术的不断发展，人类在海洋立体空间的水下活动逐步增多。海洋水文环境恶劣且复杂多变，单auv海洋探测技术有着探测面积有限、续航能力不足、信息处理效率低下等缺点，已经无法满足当前工程复杂应用的需求。

2、多auv系统在空间、时间和功能上均有其分布特点，充分克服了单auv低探测范围、低探测能力等缺点。多auv海洋探测在水文信息采集、水下应急目标搜寻、海域大面积探测等领域有着广阔的应用前景。而多auv系统面向的海洋探测场景通常位于海洋深处，光线有限，水深较大，环境条件极端，目标区域大，故所需集群编队平台数量多，探测数据庞大，可靠性不足，且存在大量时滞问题。auv种类复杂多样，数目庞大，单体探测能力差异大，探测得到的海洋信息存在不完整性、不确定性，制约着系统的探测效率。此外，探测系统的网络结构复杂，探测、控制等信息的传递存在时延高，丢包率高、效率低下等缺陷，制约着系统的整体效能。

技术实现思路

1、为了避免现有技术的不足之处，本发明提供一种面向海洋探测场景的auv集群分层式敏捷协同控制方法，用以解决现有技术中存在多auv系统所需集群编队平台数量多，探测数据庞大，可靠性不足，且存在大量时滞问题；且探测、控制等信息的传递存在时延高，丢包率高、效率低下等缺陷的问题。

2、根据本公开实施例，提供一种面向海洋探测场景的auv集群分层式敏捷协同控制方法，该方法包括：

3、基于海洋探测场景和各个auv的特性，建立auv集群分层协同控制架构；其中，auv集群包括若干个auv子集群，一个auv子集群包括一个领导auv和若干个跟随auv；

4、基于auv集群分层协同控制架构和探测任务，建立若干个探测编队；其中，一个探测编队与一个auv子集群相对应；

5、针对一个探测编队，构建auv运动学模型，并计算各个跟随auv的实际位置与理想位置在载体坐标系下的位置误差和角度误差；

6、将跟随auv的实际位置与理想位置在载体坐标系下的误差输入至编队控制器中计算得到期望角速度和期望速度，编队控制器根据期望角速度和期望速度控制各个跟随auv，使各个跟随auv和领导auv达到渐进稳定。

7、进一步的，该方法还包括：

8、针对海洋探测场景的网络特征，建立分层式集群敏捷协同网络模型，并采用基于水声modem的通信协议；其中，领导auv与各个跟随auv之间、各个领导auv之间、auv集群与协作节点之间均利用分层式集群敏捷协同网络模型和基于水声modem的通信协议进行通讯。

9、进一步的，基于水声modem的通信协议依次包括头、源地址、源角色、源层级、任务状态、通信状态、位姿信息、协同数据和尾。

10、进一步的，auv集群分层协同控制架构的结构为分层式结构，在分层式结构中，领导auv为跟随auv的层级控制中心，领导auv与各个跟随auv相互通信，各个跟随auv之间相互通信，auv集群分层协同控制架构的控制架构包括信息网络层、数据支持层和控制规划层。

11、进一步的，构建auv运动学模型，并计算各个跟随auv的实际位置与理想位置在载体坐标系下的误差的步骤中，包括：

12、针对一个探测编队，根据auv集群中领导auv和各个跟随auv在北东坐标系下的位置姿态信息，及领导auv和各个跟随auv在载体坐标系下的速度信息和角速度信息，构建auv运动学模型；

13、基于auv运动学模型，设定领导 auv与虚拟领导auv的理想距离和理想角度，并计算虚拟领导auv在北东坐标系下的位置姿态信息；

14、根据虚拟领导auv在北东坐标系下的位置姿态信息和跟随auv在北东坐标系下的位置姿态信息，得到虚拟领导auv和跟随auv在北东坐标系下的位置误差和角度误差；

15、以虚拟领导auv的位置为坐标原点，建立载体坐标系；

16、根据虚拟领导auv和跟随auv在北东坐标系下的位置误差和角度误差，得到跟随auv的实际位置与理想位置在载体坐标系下的位置误差和角度误差。

17、进一步的，auv运动学模型的表达式为：

18、（1）

19、式中，为在北东坐标系下第 i个auv的位置姿态信息，为的微分，为北东坐标系下第 i个auv的 x轴坐标，为北东坐标系下第 i个auv的 y轴坐标，为北东坐标系下第 i个auv的偏航角，为第 i个auv在载体坐标系下的速度信息和角速度信息，为第个auv在载体坐标系下的 x轴速度，为第 i个auv在载体坐标系下的 y轴速度，为第 i个auv在载体坐标系下的偏航角速度，为从北东坐标系到载体坐标系的转换矩阵，其中，

20、（2）

21、式中，为载体坐标系绕轴旋转与北东坐标系重合的夹角；

22、虚拟领导者的位置为：

23、（3）

24、式中，为北东坐标系下虚拟领导auv的 x轴坐标，为北东坐标系下虚拟领导auv的 y轴坐标，为北东坐标系下虚拟领导auv的偏航角，为北东坐标系下领导auv的 x轴坐标，为北东坐标系下领导auv的 y轴坐标，为北东坐标系下领导auv的偏航角，为领导auv与虚拟领导auv的理想距离，为领导auv与虚拟领导auv的理想角度；

25、用表示跟随auv的实际位置与虚拟领导auv的误差，在北东坐标系下，auv误差模型为：

26、（4）

27、式中，为北东坐标系下跟随auv的实际位置与虚拟领导auv的位置的 x轴误差，为北东坐标系下跟随auv的实际位置与虚拟领导auv的位置的 y轴误差，为北东坐标系下跟随auv的实际位置与虚拟领导auv的位置的偏航角误差，为北东坐标系下跟随auv的 x轴坐标，为北东坐标系下跟随auv的 y轴坐标，为北东坐标系下跟随auv的偏航角；

28、用表示跟随auv的理想位置与实际位置在载体坐标系下的误差，则有：

29、（5）

30、式中，为载体坐标系下跟随auv的实际位置与虚拟领导auv的位置的 x轴误差，为载体坐标系下跟随auv的实际位置与虚拟领导auv的位置的 y轴误差，为载体坐标系下跟随auv的实际位置与虚拟领导auv的位置的偏航角误差，为跟随auv载体坐标系绕轴旋转与北东坐标系重合的夹角；

31、对时间取微分，进而得到：

32、（6）

33、式中，为载体坐标系下跟随auv的实际位置与虚拟领导auv的位置的 x轴误差的微分，为载体坐标系下跟随auv的实际位置与虚拟领导auv的位置的 y轴误差的微分，为载体坐标系下跟随auv的实际位置与虚拟领导auv的位置的偏航角误差的微分，为跟随auv在载体坐标系下的x轴速度，为跟随auv在载体坐标系下的偏航角，为领导auv在载体坐标系下的x轴速度，为领导auv在载体坐标系下的偏航角。

34、进一步的，编队控制器控制的设计步骤包括：

35、使虚拟领导auv与跟随auv达到一致，即：

36、（7）

37、则令

38、（8）

39、式中，为第一参数，为第二参数，为第三参数，为第四参数，为虚拟控制变量；

40、对公式（8）取微分，带入公式（6）得：

41、（9）

42、式中，为第一参数的微分，为第二参数的微分，为第三参数的微分，为第四参数的微分，为跟随auv在载体坐标系下的偏航角的微分，为虚拟控制变量的微分；

43、令，则将公式（9）转换为：

44、（10）

45、式中，为第一参数、第二参数和第三参数对时间求导的导数，控制器参数；

46、利用反步法设计编队控制器，即首先针对，设计控制律如下：

47、（11）

48、式中，为第一正常数，为第二正常数，为第三正常数；

49、选取lyapunov函数如下：

50、（12）

51、对lyapunov函数求导，则有：

52、

53、当、和有一个不为时，；

54、基于公式（10），选取如下控制律：

55、

56、式中，为第四正常数；

57、选取lyapunov函数如下：

58、（13）

59、对lyapunov函数求导，则有：

60、

61、当有一个不为时，；

62、综上，编队控制器设计如下：

63、（14）

64、式中，为控制器参数。

65、进一步的，该方法还包括：

66、若发现可疑目标，则根据测量信息得到可疑目标的可疑目标位置和艄向角，并根据pp导引法计算得到探测编队与可疑目标的相对距离和艄向角之差；

67、根据探测编队与可疑目标的相对距离和艄向角之差，得到探测编队的理想艄向和理想位置，并将理想艄向输入至跟踪控制器中的艄向控制器中得到舵机控制力矩，理想位置输入至跟踪控制器中的速度控制器中得到推进器的推力；

68、跟踪控制器根据舵机控制力矩和推进器的推力控制探测编队，以对可疑目标进行跟踪。

69、本公开的实施例提供的技术方案可以包括以下有益效果：

70、本公开的实施例中，通过上述面向海洋探测场景的auv集群分层式敏捷协同控制方法，首先针对海洋探测场景特点和任务需求，分析auv集群的群体性能，提出了auv集群分层协同控制架构；进而，面向分层式协同控制架构，根据海上区域探测封控的高动态拓扑、高突发性探测数据等网络特征，建立了分层式集群敏捷协同网络模型；最后，auv集群分层协同控制架构与分层式集群敏捷协同网络模型，结合信息融合结果，分析协同控制架构的控制目标和控制需求，给出对应的编队控制方法。