一种无人艇自适应导航控制方法及装置

摘要: 本发明涉及无人艇自适应导航控制领域,尤其涉及一种无人艇自适应导航控制方法及装置。背景技术:1、当前的无人艇导航技术主要依赖传统的路径规划与目标识别方法。在路径规...
一种无人艇自适应导航控制方法及装置

本发明涉及无人艇自适应导航控制领域,尤其涉及一种无人艇自适应导航控制方法及装置。

背景技术:

1、当前的无人艇导航技术主要依赖传统的路径规划与目标识别方法。在路径规划方面,多采用基于静态地图的算法进行航线设计,这种方式仅适用于预设任务和低动态环境,无法应对动态障碍和复杂海况,难以适配突发状况如漂浮物、洋流等。在目标识别方面,通常使用单一类型的传感器(如声呐或雷达)对目标进行探测与识别,导致在复杂环境下识别能力有限,目标误检率高,多目标跟踪能力不足。系统运行过程中大多需要人工介入进行目标判定和,造成反应迟缓等问题,显而易见,这会导致无人艇导航控制时出现如撞击、进入复杂海况等问题。

技术实现思路

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、基于预设约束条件对所述无人艇目标代价函数进行求解,得到无人艇最优控制序列;

31、将所述无人艇最优控制序列中的第一个控制量作为所述当前无人艇控制指令。

32、上述方案中,通过基于参考路径序列、避障代价、威胁规避代价和当前无人艇自身状态估计构建无人艇目标代价函数,并在预设约束条件下求解该代价函数得到无人艇最优控制序列,再将序列中的第一个控制量作为当前无人艇控制指令,引入了模型预测控制框架,实现了滚动时域下的最优决策,使无人艇能够预见未来有限步内的状态变化并实时调整控制量,有效应对系统动态约束和环境不确定性,提高了控制的可靠性。

33、进一步地,所述基于所述当前无人艇控制指令控制无人艇执行相应的无人艇航行动作,包括:

34、基于所述当前无人艇控制指令得到期望航速和期望舵角;

35、基于所述期望航速控制无人艇执行相应的航行速度,并基于所述期望舵角控制无人艇执行相应的航行偏转。

36、上述方案中,通过从当前无人艇控制指令中解析出期望航速和期望舵角,并分别基于期望航速控制航行速度、基于期望舵角控制航行偏转,将上层规划的控制指令直接映射为推进器与舵机的执行动作,实现了对无人艇运动的速度和方向的双通道精确控制,确保了无人艇能够准确跟踪规划路径,从而可靠地完成航行动作。

37、本发明另一实施例还提供了一种无人艇自适应导航控制装置,包括:

38、基础数据获取模块,用于获取原始观测数据、历史无人艇控制指令和历史无人艇全局状态估计;

39、当前无人艇全局状态估计获取模块,用于基于所述历史无人艇控制指令、所述历史无人艇全局状态估计和所述原始观测数据,得到当前无人艇全局状态估计;

40、自身状态和目标状态获取模块,用于基于所述当前无人艇全局状态估计得到当前无人艇自身状态估计和当前目标状态估计集合;

41、威胁评分获取模块,用于对于所述当前目标状态估计集合中的每一个当前目标状态估计,基于所述当前无人艇自身状态估计和该所述当前目标状态估计,得到该所述当前目标状态估计的威胁评分;

42、威胁评估结果获取模块,用于基于所述目标状态估计集合对应的所有所述威胁评分,得到所述威胁评估结果;

43、当前无人艇控制指令获取模块,用于基于所述当前无人艇全局状态估计、所述威胁评估结果和预设顶层任务目标,得到当前无人艇控制指令;

44、执行模块,用于基于所述当前无人艇控制指令控制无人艇执行相应的无人艇航行动作。

45、进一步地,所述历史无人艇全局状态估计包括历史无人艇自身状态估计、历史环境地图估计和历史目标状态估计集合,所述当前无人艇全局状态估计获取模块,用于基于所述历史无人艇控制指令、所述历史无人艇全局状态估计和所述原始观测数据,得到当前无人艇全局状态估计,包括:

46、基于所述历史无人艇控制指令和所述历史无人艇自身状态估计得到预测状态;

47、将所述原始观测数据与所述预测状态进行融合,得到当前无人艇自身状态估计;

48、基于所述原始观测数据对所述历史环境地图估计进行更新,得到当前环境地图估计;

49、基于所述原始观测数据对所述历史目标状态估计集合进行更新,得到当前目标状态估计集合;

50、基于所述当前无人艇自身状态估计、所述当前环境地图估计和当前目标状态估计集合,得到所述当前无人艇全局状态估计。

51、进一步地,所述基于所述原始观测数据对所述历史环境地图估计进行更新,得到当前环境地图估计,包括:

52、获取栅格区域;

53、基于所述原始观测数据和所述栅格区域构建似然比;

54、基于所述似然比对所述历史环境地图估计进行更新,得到所述当前环境地图估计。

55、进一步地,所述基于所述当前无人艇全局状态估计、所述威胁评估结果和预设顶层任务目标,得到当前无人艇控制指令,包括:

56、基于所述预设顶层任务目标生成参考路径序列;

57、基于所述威胁评估结果、所述当前无人艇自身状态估计和所述当前环境地图估计,得到避障代价和威胁规避代价;

58、基于所述参考路径序列、所述避障代价、所述威胁规避代价以及所述当前无人艇自身状态估计,得到所述当前无人艇控制指令。