[发明专利]一种基于时间最优与能量最优的无人艇自主跟踪方法

说明书

本发明公开了一种基于时间最优与能量最优的无人艇自主跟踪方法，首先建立稳定跟踪模型，使无人艇进入绕后跟踪阶段或速度跟踪阶段，绕后跟踪阶段包括：建立无人艇运动模型，根据无人艇当前时刻状态集合确定无人艇下一时刻状态集合，进而得到预期跟踪轨迹集合，从无人艇控制能量最优、时间最优、以及防止路径抖动三个方面分别建立代价函数，结合启发函数搜索后得到最优跟踪轨迹；速度跟踪阶段包括：一步求解进行速度规划，并对规划结果进行可行性判断，可行时完成规划，不可行时，通过采样求解的方法重新规划。本发明能够实时规划出包含时间、速度、位置、方向信息的轨迹，确保路径安全平滑，平稳可靠，并能尽快追踪上目标进入稳定跟踪状态。

技术领域

本发明属于无人艇路径规划领域，具体涉及一种基于时间最优与能量最优的无人艇的目标跟踪方法。

背景技术

随着无人驾驶系统和人工智能技术的快速发展，继无人机和无人车后，无人艇(Unmanned Surface Vehicle,USV)在军用和民用领域中越来越受欢迎，在海上搜救、海事取证、环境监测和敌方侦察等各种海洋应用中发挥着重要作用，而若要在上述应用场景中真正发挥出无人艇的价值，自主跟踪就是无人艇需要具备的一个基础能力。

路径规划技术一直是水面无人艇领域中的核心技术之一，是从有人走向无人的关键一步，是无人艇智能化的重点发展方向。目前路径规划领域有多个研究方向，如基于图搜索的方法、基于采样的方法、基于数值优化的方法以及智能算法等。但多数路径规划主要侧重于避碰，而忽视了如何在避碰的同时实现对目标的自主跟踪，导致规划过程中没有综合考虑时间序列信息以及速度信息，从而降低跟踪过程中的鲁棒性。

发明内容

本发明的目的在于克服上述缺陷，提供一种基于时间最优与能量最优的无人艇自主跟踪方法，首先建立稳定跟踪模型，并根据稳定跟踪模型使无人艇进入绕后跟踪阶段或速度跟踪阶段，绕后跟踪阶段的执行步骤包括：建立无人艇运动模型，根据无人艇当前时刻状态集合确定无人艇下一时刻状态集合，进而得到无人艇当前时刻的预期跟踪轨迹集合，从无人艇控制能量最优、时间最优、以及防止路径抖动三个方面分别建立代价函数，结合启发函数搜索后得到最优跟踪轨迹；速度跟踪阶段的执行步骤包括：一步求解进行速度规划，并对规划结果进行可行性判断，当结果可行时完成规划，当结果不可行时，通过采样求解的方法重新规划。本发明针对无人艇自主跟踪场景，能够实时规划出一条包含时间、速度、位置、方向信息的轨迹，确保路径安全平滑，平稳可靠，并能尽快追踪上目标进入稳定跟踪状态。

为实现上述发明目的，本发明提供如下技术方案：

一种基于时间最优与能量最优的无人艇自主跟踪方法，包括如下步骤：

建立稳定跟踪模型，并根据稳定跟踪模型确定无人艇处于绕后跟踪阶段或速度跟踪阶段；

当无人艇处于绕后跟踪阶段时，执行步骤S1.1～S1.6：

S1.1建立无人艇运动模型，所述无人艇运动模型用于根据无人艇当前时刻状态和下一时刻运动参数确定无人艇下一时刻状态；所述无人艇的状态包括无人艇的位置、速度及航向角，所述运动参数包括加速度、步长时间和转向角；

S1.2在当前时刻，对无人艇下一时刻的运动参数进行采样，得到采样运动参数集合；根据无人艇当前时刻状态、采样运动参数集合以及无人艇运动模型得到无人艇当前时刻的预期跟踪轨迹集合；

S1.3舍弃预期跟踪路径集合中与障碍物产生碰撞的轨迹；

S1.4根据无人艇当前时刻的预期跟踪轨迹集合建立控制能量代价函数、时间代价函数和平滑代价函数，并根据控制能量代价函数、时间代价函数和平滑代价函数得到无人艇由起点位置到当前位置的代价函数；

S1.5根据Dubin模型建立启发函数；

S1.6根据无人艇由起点位置到当前位置的代价函数以及启发函数得到估值函数；并根据估值函数搜索得到最优跟踪轨迹；