一种双无人艇编队通过长直狭窄河道的运动控制方法

本发明属于多无人艇控制领域，更具体地，涉及一种双无人艇编队通过长直狭窄河道的运动控制方法。

背景技术：

1、近年来，随着智能化机器人技术的发展，无人艇的研究已成为当下研究的热点。目前大多数研究聚焦于单个无人艇的控制上，但是单个无人艇受到自身条件的限制，难以完成较为复杂的任务，应用范围有限。通过多个无人艇的协同控制可以极大地提高工作效率，扩展作业范围，完成更为复杂的任务。

2、同时多无人艇在具有边界限制的河道或者峡湾工作时，现有技术难以进行有效的协同控制，尤其是在多个集群同时通过狭窄河道时，现有技术往往难以完成该情景下的任务。

技术实现思路

1、针对现有技术的缺陷和改进需求，本发明提供了一种双无人艇编队通过长直狭窄河道的运动控制方法，其目的在于提出一种运动控制策略，避免碰撞，以有效完成双无人艇编队通过长直狭窄河道的任务。

2、为实现上述目的，按照本发明的一个方面，提供了一种双无人艇编队通过长直狭窄河道的运动控制方法，包括：

3、实时基于每个无人艇编队中每个无人艇当前的位置pi及其航速vi，以及该无人艇在该无人艇编队中当前的各邻居位置pj及其航速vj，结合该无人艇所在编队的虚拟领导者的当前位置pr以及速度vr，通过改进的蜂拥算法，计算该无人艇的编队控制力ufi；

4、在预先确定的各侧河道曲线上确定横坐标与每个无人艇当前位置的横坐标相同的点，作为该无人艇在各河道上的斥点；基于每个无人艇对应的两侧河道斥点，通过各侧河道对应的斥力势分段函数，计算该无人艇与河道间的总斥力，作为该无人艇的河道控制力uri，其中所述斥力势分段函数是通过设定无人艇与河岸的安全距离与危险距离得到；

5、基于每个无人艇当前的位置pi以及其在另一个无人艇编队中的邻居无人艇的位置pj′，通过编队间的斥力势函数，计算该无人艇与其在另一无人艇编队中各邻居无人艇之间的总斥力，作为该无人艇的编队间控制力uoi；

6、将每个无人艇的编队控制力ufi、河道控制力uri和编队间控制力uoi矢量相加，作为该无人艇的总控制量，以更新该无人艇下一时刻的速度和位置，实现双无人艇编队通过长直狭窄河道的运动控制。

7、本发明的有益效果是：本发明提出在每个无人艇的运动控制中，总的运动控制输入包含三个部分，分别对应于无人艇编队控制，无人艇与河道之间的避碰控制以及不同无人艇集群之间的避碰控制。首先，在无人艇通过长直狭窄河道时，同一个集群中的无人艇会依据编队控制控制量形成晶格编队，其次，对于编队中的无人艇，河道会产生相应的河道斥点，运用河道分段斥力势函数，依据距离河岸的位置产生不同大小的斥力uri，将无人艇编队限制在长直狭窄的河道之中，完成多无人艇协同编队通过长直狭窄河道；另外，运用编队间的斥力势函数可以得到编队间的斥力uoi，实现不同编队之间无人艇的避碰。本发明方法获取的信息均比较容易采集，利用无人艇和邻居的相关信息，完成了编队任务；依据设计出的河道分段斥力势函数，实现在狭窄长直地形约束下编队顺利通过；运用势场法设计出编队间的斥力势函数，实现了双编队之间的避碰。该发明扩展了无人艇的应用范围，提高了无人艇完成任务的效率，落实了多智能体理论的应用，具有广泛的前景。

8、进一步，每个无人艇编队的虚拟领导者的初始位置为该无人艇编队中各无人艇初始的位置的平均值，在每次运动控制中，通过该虚拟领导者的动力学模型更新其位置；

9、每个无人艇编队的虚拟领导者的速度为该无人艇编队的目标速度，取值为预设固定值。

10、本发明的进一步有益效果是：虚拟领导者的位置与速度的确定方法快捷高效。

11、进一步，每侧河道的河道曲线采用如下方式构建得到：

12、均匀地采集该侧河道m个点，得到各点的位置坐标；将每侧河道的m个点进行曲线拟合，得到该侧河道对应的河道曲线。

13、本发明的进一步有益效果是：该方法简单高效。

14、进一步，每个无人艇编队中第i个无人艇的编队控制力ufi的计算方式为：

15、

16、式中，mi为每个无人艇编队中第i个无人艇的质量；α1、α2为编队控制系数，给定值；ni表示所述第i个无人艇在其所在编队中的邻居无人艇的总个数；均大于0；φα表示α动作函数；pj表示所述第i个无人艇在其所在无人艇编队中当前的第j个邻居无人艇的位置；pi表示所述第i个无人艇的位置；vj表示所述第i个无人艇在其所在无人艇编队中第j个邻居无人艇的航速；vi表示所述第i个无人艇的航速；c1、c2为目标追踪系数；pr表示第i个无人艇所在无人艇编队虚拟领导者的位置；vr表示第i个无人艇所在无人艇编队虚拟领导者的速度即该编队的目标速度；

17、φα(x)＝ρh(x/||r||σ)φ(x-||d||σ)；x＝||pj-pi||σ

18、

19、式中，d为编队形成时编队中的无人艇之间的期望距离；r表示无人艇编队中无人艇与其在同一编队中邻居间的临界距离，均为给定值；

20、aij(p)＝ρh(p)∈[0,1],j≠i，p＝||pj-pi||σ/||r||σ；

21、

22、式中，h∈(0,1)，给定值。

23、进一步，第i个无人艇与其对应的第k侧河道斥点之间的斥力势分段函数为：

24、

25、式中，dsaf为河道安全距离；ddan为河道危险距离；dsaf≥ddan，均为给定值；xk＝||pi-pick||，pick表示第i个无人艇在第k侧河道的斥点位置，k表示两侧河道的编号，取值为1或2；λ1、λ2、θ均为势场系数，λ2>λ1>0，θ>0；creri,k表示第i个无人艇与其对应的第k侧河道斥点之间的斥力势；

26、则第i个无人艇的河道控制力

27、本发明的进一步有益效果是：本方法设置了河道安全距离以及河道危险距离，运用不同的势场系数λ1、λ2表示当船体靠近河岸时根据其与河岸距离的不同其势场强弱不同，对应的就会产生斥力的大小不同。通过分段函数考虑了无人艇与河岸之间距离的多种情况，根据碰撞危险程度的高低产生不同的力。

28、进一步，编队间的斥力势函数为：

29、

30、式中，η>0是势场系数，xd＝||pi-p′j||-l，表示第i个无人艇与其在另一个编队中邻居无人艇之间的位置距离与第i个无人艇船体长度l之差，r2为第i个无人艇与其在另一个编队中邻居无人艇的临界距离，为预设值；xr＝||pi-pr||，表示第i个无人艇与其所在无人艇编队的虚拟领导者之间的位置距离；crepi表示编队间的斥力势；n′i表示第i个无人艇在另一无人艇编队中的邻居无人艇的总个数；

31、则第i个无人艇的编队间控制力

32、本发明的有益效果是：本发明一方面考虑了编队中无人艇的长度，即xd＝||p1i-p2j||-l，避免船体质心虽然没有到同一地点，但是船体发生碰撞的情况。另一方面，在势场函数中加入了编队目标项，保证了两个编队的无人艇相互之间避免发生碰撞的同时，使无人艇编队在编队控制的基础上对目标进行有效的跟随和追踪，反映在uoi中为uoi中的第二项。

33、本发明还提供一种双无人艇编队通过狭窄河道的运动控制系统，包括处理器和机器可读存储介质，所述机器可读存储介质存储有能够被所述处理器执行的机器可执行指令，所述处理器执行所述机器可执行指令以实现如上所述的一种双无人艇编队通过狭窄河道的运动控制方法。

34、本发明还提供一种计算机可读存储介质，所述计算机可读存储介质包括存储的计算机程序，其中，在所述计算机程序被处理器运行时控制所述存储介质所在设备执行如上所述的一种双无人艇编队通过狭窄河道的运动控制方法。

35、总体而言，通过本发明所构思的以上技术方案，能够取得以下有益效果：

36、本发明提出总的运动控制输入包含三个部分，分别对应于无人艇编队控制，无人艇与河道之间的避碰控制以及不同无人艇集群之间的避碰控制，解决了双无人艇编队过长直狭窄河道难以进行协同控制的技术问题，实现了在长直狭窄地形约束下进行无人艇编队以及不同无人艇编队间的避碰。