一种基于船舶避碰的路径规划和跟踪控制方法及装置

本发明涉及船舶路径规划，具体涉及一种基于船舶避碰的路径规划和跟踪控制方法及装置。

背景技术：

1、随着经济全球化的发展，航运业已成为全球贸易的支柱。近年来，我国船舶水上交通运输行业面临着数字化和智能化转型的挑战，而在船舶智能化研究中，船舶避碰路径规划跟踪控制的研究具有重要意义。船舶避碰路径规划的研究主要分为传统算法和智能算法，智能算法可适用于船舶动态航行复杂的环境，且具有较好的鲁棒性和分布式计算能力，但存在需要大量的训练样本和训练时间、维度灾难等缺陷。传统算法虽然存在规划路径不平滑、容易陷入局部最优等不足，但是也具有结构简单，在规划路径的实时避障中有较好的效果等优势。

2、在现有技术中，针对船舶轨迹跟踪控制的研究主要通过将一种控制方法与一种船舶运动模型结合的方式实现，其中，控制方法包括了模糊控制方法、pid控制方法等，船舶运动模型包括了nomoto模型、mmg模型等。现今许多轨迹跟踪方法存在容易与环境中的障碍物发生避碰风险的不足。

3、因此，急需提出一种基于船舶避碰的路径规划和跟踪控制方法及装置，解决现有技术中的轨迹跟踪方法容易与环境中的障碍物发生避碰风险的技术问题。

技术实现思路

1、有鉴于此，有必要提供一种基于船舶避碰的路径规划和跟踪控制方法及装置，用以解决现有技术中的轨迹跟踪方法容易与环境中的障碍物发生避碰风险的技术问题。

2、一方面，本发明提供了一种基于船舶避碰的路径规划和跟踪控制方法，包括：

3、获取本船舶、目的地和障碍物的位置；

4、根据所述本船舶的位置、所述目的地的位置、所述障碍物的位置和四元船舶领域对人工势场算法进行改进，根据改进后的人工势场算法，输出所述本船舶的避碰路径点；

5、根据最小二乘法对所述避碰路径点进行拟合，得到所述本船舶距离所述目的地的避碰路径轨迹；

6、根据所述避碰路径轨迹对所述本船舶进行控制，得到所述本船舶在各个时刻的目标航向和目标位置，根据所述目标航向和所述目标位置对所述本船舶进行跟踪。

7、在一些可能的实现方式中，所述人工势场算法包括引力势场函数、引力函数、斥力势场函数和斥力函数；

8、所述根据所述本船舶的位置、所述目的地的位置、所述障碍物的位置和四元船舶领域对人工势场算法进行改进，包括：

9、根据所述本船舶和所述目的地的位置，确定第一距离，根据所述本船舶和所述障碍物的位置，确定第二距离；

10、根据所述本船舶的位置、所述目的地的位置、所述第一距离对所述引力势场函数和所述引力函数进行改进；

11、根据所述本船舶的位置、所述障碍物的位置、所述第一距离、所述第二距离、所述四元船舶领域对所述斥力势场函数和所述斥力函数进行改进。

12、在一些可能的实现方式中，所述根据改进后的人工势场算法，输出所述本船舶的避碰路径点，包括：

13、根据改进后的所述引力势场函数和所述引力函数，计算所述本船舶所受的引力合力；

14、根据改进后的所述斥力势场函数和所述斥力函数，计算所述本船舶所受的斥力合力；

15、根据所述引力合力和所述斥力合力，计算所述本船舶的所受合力；

16、根据所述所受合力，得到所述本船舶在所述所受合力的作用下，朝所述目的地航行，并达到所述目的地过程中的避碰路径点。

17、在一些可能的实现方式中，所述根据改进后的所述斥力势场函数和所述斥力函数，计算所述本船舶所受的斥力合力之后，还包括：

18、当所述本船舶在航行过程中存在多个所述障碍物时，将每个所述障碍物的所述斥力合力相加，得到所述本船舶的总斥力合力。

19、在一些可能的实现方式中，所述根据最小二乘法对所述避碰路径点进行拟合，得到所述本船舶距离所述目的地的避碰路径轨迹，包括：

20、根据所述避碰路径点的经纬度坐标的变化趋势，预设初始函数；

21、根据所述避碰路径点的经纬度坐标和所述初始函数，确定近似函数；

22、当所述近似函数满足预设条件时，将所述近似函数确定为所述避碰路径轨迹。

23、在一些可能的实现方式中，所述当所述近似函数满足预设条件时，将所述近似函数确定为所述避碰路径轨迹，包括：

24、将所述初始函数在所述各个时刻的纵坐标值分别与所述近似函数在所述各个时刻的纵坐标值进行计算，得到所述各个时刻对应的残差值；

25、将所述各个时刻对应的所述残差值进行计算，得到平方和残差值；

26、当所述平方和残差值小于预设阈值时，将所述近似函数确定为所述避碰路径轨迹。

27、在一些可能的实现方式中，所述当所述近似函数满足预设条件时，将所述近似函数确定为所述避碰路径轨迹之后，还包括：

28、当所述平方和残差值大于等于所述预设阈值时，重新预设所述初始函数，返回“根据所述避碰路径点的经纬度坐标的变化趋势，预设初始函数”的步骤。

29、在一些可能的实现方式中，所述根据所述避碰路径轨迹对所述本船舶进行控制，得到所述本船舶在各个时刻的目标航向和目标位置，包括：

30、根据船舶四自由度mmg模型坐标系和pid控制方法，构建所述本船舶在航行过程中对航向进行计算的运动模型；

31、将所述避碰路径轨迹在所述各个时刻的航向确定为目标航向；

32、通过所述运动模型，将所述本船舶在当前时刻的当前航向转至所述当前时刻对应的目标航向，得到所述当前时刻对应的舵角和航向差；

33、根据所述舵角和所述航向差，得到所述当前时刻对应的目标位置；

34、判断所述避碰路径轨迹是否存在下一时刻对应的目标航向；

35、若是，则将所述下一时刻确定为所述当前时刻，并返回“通过所述运动模型，将所述本船舶在当前时刻的当前航向转至所述当前时刻对应的目标航向，得到所述当前时刻对应的舵角和航向差”的步骤；

36、若否，则得到所述本船舶在各个时刻的目标航向和目标位置。

37、在一些可能的实现方式中，所述船舶四自由度mmg模型坐标系包括横荡、纵荡、横摇和艏摇四个自由度。

38、另一方面，本发明还提供了一种基于船舶避碰的路径规划和跟踪控制装置，包括：

39、位置获取模块，用于获取本船舶、目的地和障碍物的位置；

40、路径点确定模块，用于根据所述本船舶的位置、所述目的地的位置、所述障碍物的位置和四元船舶领域对人工势场算法进行改进，根据改进后的人工势场算法，输出所述本船舶的避碰路径点；

41、路径轨迹确定模块，用于根据最小二乘法对所述避碰路径点进行拟合，得到所述本船舶距离所述目的地的避碰路径轨迹；

42、船舶控制模块，用于根据所述避碰路径轨迹对所述本船舶进行控制，得到所述本船舶在各个时刻的目标航向和目标位置，根据所述目标航向和所述目标位置对所述本船舶进行跟踪。

43、采用上述实施例的有益效果是：本发明提供的基于船舶避碰的路径规划和跟踪控制方法，通过改进人工势场算法，得到本船舶避开障碍物到达目的地的过程中的避碰路径点，解决现有技术中目的地距离本船过远造成引力过大、避开障碍物后无法自主回归计划航线的问题，提高本船舶航行的安全性。进一步的，通过最小二乘法对避碰路径点进行拟合，使避碰路径轨迹保持平滑、安全，从而实现本船舶避碰的路径规划。还通过避碰路径轨迹对本船舶进行控制，确定本船舶在各个时刻的目标航向和目标位置，从而对本船舶进行避碰路径的跟踪，避碰路径轨迹为本船舶避开障碍物的路径轨迹，所以在对避碰路径进行跟踪时，可以避免本船舶与障碍物发生避碰，降低了本船舶与障碍物发生避碰的风险，提高了本船舶的安全性。