一种无人艇编队跟踪及避障控制方法

本发明属于无人艇编队运动控制，具体涉及一种无人艇编队跟踪及避障控制方法。

背景技术：

1、无人艇编队作为一种能够实现自主航行的水上智能装备，因其机动灵活、经济高效，能够增强人类面对未知水域的主动性，并代替人类执行危险水上作业任务而备受关注。无人艇编队在军用和民用领域均发挥作用。在民用领域，无人艇编队可用于水文勘探、环境维保、灾后搜救等，能够降低劳务支出并提高作业效率；在军用领域，无人艇编队可用于反潜反导、水域封锁、区域巡逻、突击作战等，能够减少人员伤亡并提高军队作战效费比。

2、目前，无人艇编队避障及跟踪控制的研究主要分三大部分。第一部分，采用如动态窗口法和模型预测控制等最优控制方法，将安全避碰、高精度跟踪等需求设置为损失函数中的一项，实时求解最优控制律，实现编队跟踪及避碰。然而，无论是还是方法均存在优化目标权重固定、计算复杂的问题；第二部分，采用多智能体强化学习控制方法，将安全避碰、高精度跟踪等需求设计为奖励函数，将编队机动定义为行为，在损失函数迭代收敛后，实现编队跟踪及避碰。此方法在损失函数迭代收敛后的结构化环境中效果较好，但也存在最优控制方法的缺陷，且在非结构化环境时，需要重新学习避碰和跟踪需求的特征，可能产生跟踪失败甚至无法避碰的严重后果；第三部分，采用人工势场法，将障碍物特征用斥力势场表征，所有障碍物的斥力合矢量用于设计制导律或直接参与控制律计算。此方法设计灵活，能根据不同需求改进，因而备受学者青睐。

3、近年来，学者们提出多种结合人工势场法的避碰控制方法。部分研究只涉及静态障碍物避碰，在无人艇编队航行时，易受助航浮标、编外他艇影响，需针对动态障碍物设计避碰控制方法；另一部分学者参考其他智能体的避碰控制方法，设计了考虑动态障碍物的人工势场法，但没有考虑到国际海上避碰规则，不符合航行规范；少数学者考虑存在动、静态障碍物的开阔水域下符合的无人艇编队跟踪及避碰，但尚未满足编队在不同水域执行任务的需求。然而，随着作业任务的复杂和多样化，需要满足多种水域通航（开阔水域、狭长水域等）的队形改变策略，以提高编队面对不同水域的适应能力，据笔者所知，这方面的研究尚处于基础阶段。

4、除了避碰决策和编队策略，编队跟踪控制器的设计也对跟踪和避碰效果有着重要影响。部分学者研究路径规划和避碰决策算法，不考虑无人艇的跟踪能力和控制器的设计。然而，无人艇模型不确定性、输入饱和与环境干扰等内、外部不确定因素，可能导致编队无法实现对路径规划和避碰决策层给出的期望状态的跟踪。目前，综合考虑避碰决策、编队策略及跟踪控制的无人艇编队跟踪及避障控制解决方案研究少，理论支撑不足。

技术实现思路

1、为了解决上述技术问题，本发明提出了一种基于改进人工势场法和队形变换策略的无人艇编队跟踪及避障鲁棒控制方法，用于解决存在多种未知障碍物和不同水域下的无人艇编队跟踪及避障控制问题。

2、在第一方面，本发明提供了一种无人艇编队跟踪及避障控制方法，包括：

3、步骤（1）获取无人艇运动状态信息和周围环境障碍物信息，建立无人艇编队操纵运动模型；

4、步骤（2）根据无人艇当前位置和速度状态以及目标点位置状态和速度状态对无人艇所受吸引力势场进行建模，得到吸引力函数；

5、步骤（3）根据动态障碍物与无人艇距离和无人艇状态信息基于国际海上避碰规则计算得到排斥力函数；

6、步骤（4）根据起点和终点以及改进的人工势场法获取的合力，规划避开障碍物的可行路径，并生成路径上的目标航路点；

7、步骤（5）利用障碍物产生的合斥力势能计算危险评价函数，根据危险评价函数对编队队形进行调整；

8、步骤（6）基于反步法和神经网络算法的编队跟踪控制指令并输出。

9、进一步地，步骤（1）中建立无人艇编队操纵运动模型包括：

10、建立惯性坐标系向量表示第艘无人艇位置和艏向角状态，建立附体坐标系，向量表示第艘无人艇纵向、横向、转艏速度状态；其中附体坐标系以领导无人艇重心为原点，纵向前进方向为x轴，垂直于纵向前进方向为y轴，建立坐标系附体坐标系；基于惯性坐标系和附体坐标系建立无人艇编队操纵运动模型；

11、建立无人艇编队操纵运动模型：

12、

13、

14、其中，为附加质量惯性矩阵，为阻尼矩阵，为科氏力矩阵，、、分别为无人艇附加质量惯性矩阵、阻尼矩阵和科氏力矩阵的标称模型矩阵，为模型不确定；为环境扰动，为坐标系转换矩阵；为输入力和力矩矩阵。

15、进一步地，步骤（1）进一步包括：

16、

17、

18、

19、

20、上述矩阵满足以下性质：

21、

22、

23、其中，s矩阵为：

24、。

25、进一步地，步骤（2）进一步包括：

26、首先，对无人艇所受吸引力势场进行建模，势能函数与无人艇当前位置和速度状态以及目标点位置状态和速度状态有关，该函数与无人艇和目标点的相对位置和相对速度有关；

27、表示无人艇自身的位置和速度，跟踪的航路点目标位置是，跟踪的航路点速度的速度为，吸引力势场公式由合速度模值和欧式距离的公式共同构成如下：

28、

29、

30、

31、其中，和是人为调整的势场增益系数，和分别是无人艇在惯性坐标系下的横向和纵向坐标，和分别是目标点在惯性坐标系下的横向和纵向坐标，和分别是无人艇的纵向速度和横线速度，和分别是目标点的纵向速度和横线速度，是无人艇合速度，是目标点合速度；

32、对求梯度可得吸引势场力：

33、

34、其中是由相对位置产生的吸引矢量力的方向单位向量，是由相对速度产生的吸引矢量力的方向单位向量。

35、进一步地，步骤（3）进一步包括：

36、利用国际海上避碰规则将动态障碍物所在区域划分为三部分，分别是危险避碰区，避碰操作区，碰撞检测区，根据动态障碍物所在区的区域建立障碍物排斥势场：

37、

38、其中，和是排斥势能正系数，无人艇自身和障碍物之间的相对距离表示为，和分别代表划分的三个区域的半径，是碰撞风险评估角，由无人艇和障碍物的相对速度角和相对位置角之差构成，和避碰区域临界角，，用于计算向量和的角度；

39、对求负梯度，可以将排斥力表示为：

40、

41、其中：

42、

43、

44、

45、

46、

47、

48、

49、其中，和分别是障碍物与无人艇相对位置矢量的单位向量和相对速度矢量的单位向量，，，和表示由目标点产生的部分吸引力，、、、、表示障碍物产生的斥力。

50、进一步地，步骤（4）进一步包括：

51、首先，根据起点和终点以及改进的人工势场法获取的合力，设计一条由起点到终点的满足对静态障碍物避碰的可行路径，并生成路径上的目标航路点：

52、

53、其中是由斥力和吸引力组成的合力的矢量方向与x轴方向的夹角，是第艘无人艇的设定航速；

54、其次，根据无人艇与跟踪的目标航路点的距离对目标航路点进行平滑处理，分别表示无人艇与目标航路点的欧式距离以及无人艇采用平滑航路点的阈值距离；

55、如果，无人艇的期望路径航路点坐标及艏向角计算公式如下：

56、

57、如果，无人艇的期望路径航路点坐标及艏向角计算公式如下：

58、

59、最后，在无人艇趋近目标航路点的过程中，无人艇根据由斥力和吸引力组成的合力的矢量方向调整无人艇的期望纵向和横向位置坐标。

60、进一步地，步骤（5）进一步包括：

61、利用障碍物产生的合斥力势能作为输入，经危险评价函数计算得到队形变换系数b，根据队形变换系数计算第个跟随无人艇的自适应编队方程：

62、

63、其中，表示第个跟随无人艇与领导者无人艇在纵向方向的距离，表示第个跟随无人艇与领导者无人艇在横向方向的距离，其中分别表示与领导者无人艇的纵向速度方向相反的单位方向向量，表示与领导者无人艇的纵向速度方向垂直的单位方向向量。

64、进一步地，步骤（6）进一步包括：

65、设为第个无人艇的期望位置和艏向角，定义位置误差系统，当前其导数为：

66、

67、其中为虚拟控制输入向量，为了稳定位置误差系统，定义虚拟控制向量的中间控制量为：

68、

69、其中是正定设计参数矩阵，定义速度向量和虚拟中间控制量的误差，则当前导数公式可表示为：

70、

71、对求导并代入运动模型公式：

72、

73、引入三个网络近似和补偿无人艇的未知动力学和受到的环境干扰：

74、

75、其中是网络的输入向量，是干扰的未知上界值，是基函数向量，理想的常数权重矩阵为：

76、；

77、其中，是无人艇的未知动力学和环境干扰的近似误差向量，设，分别为的估计，设计控制律如下：

78、

79、其中是正定常数矩阵，设计神经网络权重参数更新律为：

80、

81、权重矩阵估计值的具体形式为：

82、

83、其中和分别是正定设计矩阵和设计常数。

84、在本发明的第二方面，本发明公开了一种计算机设备，其特征在于，包括存储器、处理器和存储在存储器中可供处理器运行的程序指令，所述处理器执行所述程序指令以实现第一方面所述的方法。

85、在本发明的第三方面，本发明公开了一种计算机可读存储介质，存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现第一方面所述的方法。

86、本发明涉及一种无人艇编队跟踪及避障控制方法，属于无人艇编队运动控制技术领域，本发明通过传感器测量和岸基发送的障碍物、无人艇编队、航道状态信息以及期望目标点，基于改进人工势场法的避碰制导律满足避碰要求并符合国际海上避碰规则来规划可行路径，并基于危险度评价函数的队形变换策略来规划无人艇编队方案，最终基于神经网络估计器和反步控制器实时生成控制律并由执行器执行，驱动无人艇编队跟踪期望目标点，在具有避碰风险时，优先实现编队避碰，在无避碰风险时，实现编队跟踪和队形保持。