CN114995425B - 一种基于时间最优与能量最优的无人艇自主跟踪方法 - Google Patents

Abstract

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

Description

一种基于时间最优与能量最优的无人艇自主跟踪方法

技术领域

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

背景技术

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

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

发明内容

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

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

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

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

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

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

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

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

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

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

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

当无人艇处于速度跟踪阶段时，执行步骤S2.1～S2.5；

S2.1建立基于速度跟踪阶段跟踪总时间、速度跟踪阶段无人艇加速度、控制能量代价权重因子以及时间代价权重因子的第一目标函数，根据无人艇运动学模型、以及速度跟踪阶段无人艇初始状态和末端状态求解第一目标函数，得到速度跟踪阶段最优跟踪总时间和速度跟踪阶段跟踪轨迹；

S2.2对速度跟踪阶段跟踪轨迹进行可行性判断，具体为，将不与障碍物发生碰撞且每一点速度不超过无人艇最大速度的跟踪轨迹判断为可行，否则判断为不可行；如果跟踪轨迹判断为可行，使无人艇按照所述速度跟踪阶段最优跟踪时间和跟踪轨迹进行跟踪，如果跟踪轨迹判断为不可行，执行步骤S2.3；

S2.3执行步骤S1.1～S1.6，基于步骤S1.6所得最优跟踪轨迹和无人艇速度约束得到速度跟踪阶段跟踪总时间采样集合；

S2.4建立基于速度跟踪阶段跟踪总时间、速度跟踪阶段无人艇加速度、控制能量代价权重因子以及时间代价权重因子的第二目标函数，求得速度跟踪阶段跟踪总时间采样集合中每一时间采样点的第二目标函数值，根据每一时间采样点的第二目标函数值确定速度跟踪阶段次优跟踪总时间；

S2.5根据速度跟踪阶段次优跟踪总时间计算得到速度规划后的每一轨迹点速度。

进一步的，所述稳定跟踪模型如下：

时无人艇处于绕后跟踪阶段；/>时无人艇处于速度跟踪阶段；

其中，为无人艇处于绕后跟踪阶段或速度跟踪阶段时的末端速度，v_max为无人艇最大速度，d_l＝d-d_h，d为无人艇与目标的距离，d_h为保持距离，v_obs为目标当前速度，a_max为无人艇最大加速度，ξ为调节因子。

进一步的，所述步骤S1.1中，无人艇运动模型如下：

q_i＝(p_i,v_i,θ_i)；

其中，q_i为无人艇当前时刻i的状态，p_i＝(x_i,y_i)为无人艇当前时刻i的位置，θ_i为无人艇当前时刻i的航向角，(x_i+1,y_i+1),θ_i+1分别为无人艇下一时刻i+1的位置和航向角，v_i为当前时刻i无人艇的速度，φ为无人艇的转向角，R为无人艇的转弯半径，L为无人艇艇长，Δθ表示无人艇在步长时间Δt内航向角的变化量。

进一步的，所述步骤S1.4中，建立控制能量代价函数g_e、时间代价函数g_t和平滑代价函数g_s如下：

g_t＝Δt

其中，a_i表示当前时刻i无人艇的加速度，Δt为当前时刻i的步长时间，p_i-2、p_i-1、p_i分别表示i-2时刻，i-1时刻以及当前时刻i无人艇的位置。

进一步的，所述步骤S1.4中，无人艇由起点位置到当前位置的代价函数如下：

其中，g_i＝ηg_e+μg_t+λg_s，η、μ、λ分别为控制能量代价、时间代价以及平滑代价的权重因子，n为以起点序列号为0时，当前点的序列号，T_g为从无人艇从起点到当前点需要花费的总时间。

进一步的，所述步骤S1.5中，启发函数h为：

h＝d_dubin

其中，d_dubin为根据Dubin曲线模型计算得到的无人艇当前点到终点的路径长度；

所述步骤S1.6中，估值函数f＝g+h；

其中，g为无人艇由起点位置到当前位置的代价函数；

所述步骤S1.6中，基于hybrid A*方法搜索得到估值函数f估值最小的路径，即为最优跟踪轨迹。

进一步的，所述步骤S2.1中，第一目标函数为：

其中，以速度跟踪阶段起始时刻为0，T_t为速度跟踪阶段完成时的时刻，即速度跟踪阶段跟踪总时间，a为速度跟踪阶段无人艇的加速度，η、μ分别为控制能量代价和时间代价权重因子；

所述步骤S2.1中，根据无人艇运动学模型、以及速度跟踪阶段无人艇初始状态和末端状态，采用庞特里亚金最小值方法对目标函数进行闭式求解。

进一步的，所述步骤S2.3的具体步骤如下：

S2.3.1设无人艇执行步骤S1.6所得最优跟踪轨迹的速度v(t)与时间t满足如下关系式：

v(t)＝αt²+βt+γ，t∈[0,T_t]

其中，α、β、γ为待求解的系数，以速度跟踪阶段起始时刻为0，T_t为速度跟踪阶段完成时的时刻，即速度跟踪阶段跟踪总时间；

S2.3.2建立无人艇执行步骤S1.6所得最优跟踪轨迹的轨迹长度s(t)与时间t的关系式：

其中，κ为待求解的系数；

S2.3.3构建约束条件：

其中，v₀为无人艇在速度跟踪阶段初始时刻的速度，v_t为无人艇在速度跟踪阶段的末端速度，s_t为最优跟踪轨迹总长度；

S2.3.4根据步骤S2.3.3所构建的约束条件对步骤S2.3.1和S2.3.2的关系式进行求解，得到：

v(t)＝αt²+βt+v₀

S2.3.5估计速度跟踪阶段跟踪总时间T_t的区间为[T₁,T₂]：

S2.3.6采用二分法确定在区间[T₁,T₂]中满足无人艇速度约束的区间[T′₁,T′₂]，[T′₁,T′₂]即速度跟踪阶段跟踪总时间区间；

S2.3.7在区间[T′₁,T′₂]中确定n_st个跟踪采样时间，得到速度跟踪阶段跟踪总时间采样集合各跟踪采样时间间隔大小的设置规律为，由区间[T′₁,T′₂]边缘至步骤S2.1中所得速度跟踪阶段最优跟踪总时间，间隔逐渐减小。

进一步的，所述步骤S2.4中，第二目标函数f₂为：

其中，为速度跟踪阶段跟踪总时间采样集合中每一时间采样点，a为速度跟踪阶段无人艇的加速度，η、μ分别为控制能量代价和时间代价权重因子；

所述步骤S2.4中，将第二目标函数值最小时对应的跟踪采样时间作为速度跟踪阶段次优跟踪总时间。

进一步的，所述步骤S2.5中，根据速度跟踪阶段次优跟踪总时间得到速度规划后的每一轨迹点速度：

其中，T_sot ^*为根据步骤S2.4得到的速度跟踪阶段次优跟踪总时间。

进一步的，步骤S1.2的具体步骤为：

S1.2.1在当前时刻i，对下一时刻的加速度与步长时间进行采样，得到采样加速度集合和采样步长时间集合；

S1.2.2根据采样加速度集合、采样步长时间集合和当前时刻i无人艇的速度得到采样速度集合，并舍弃不处于0到最大速度之间的元素；

S1.2.3在当前时刻i，对下一时刻i+1无人艇的转向角进行采样，得到采样转向角集合；

S1.2.4将无人艇当前时刻状态、采样速度集合和采样转向角集合中的元素代入无人艇运动模型，得到一系列无人艇下一时刻i+1的状态集合，进而得到无人艇当前时刻的预期跟踪轨迹集合，即当前时刻到下一时刻间跟踪轨迹的集合。

本发明与现有技术相比具有如下有益效果：

(1)本发明建立了稳定跟踪模型，将自主跟踪过程划分为绕后跟踪和速度跟踪两个阶段，通过对这两个阶段分别进行考虑，从而解决进入稳定跟踪状态花费时间长和跟踪不稳定这两个问题；

(2)本发明通过对hybrid A*方法的代价函数进行改进，综合考虑控制能量代价、时间代价以及平滑代价，使跟踪轨迹具有快速执行、平滑可靠、平稳舒适的优点；

(3)本发明针对速度跟踪阶段设计了一步求解和采样求解两种方式，在提高算法收敛速度的同时从速度层面对时间和能量代价进行优化，解决跟踪过程速度不稳定，加速度不连续等问题。

附图说明

图1为本发明一种基于时间最优与能量最优的无人艇自主跟踪方法的流程示意图；

图2为本发明中目标跟踪模式中的过程示意图。

具体实施方式

下面通过对本发明进行详细说明，本发明的特点和优点将随着这些说明而变得更为清楚、明确。

在这里专用的词“示例性”意为“用作例子、实施例或说明性”。这里作为“示例性”所说明的任何实施例不必解释为优于或好于其它实施例。尽管在附图中示出了实施例的各种方面，但是除非特别指出，不必按比例绘制附图。

如图1所示，本发明公开了一种基于时间最优与能量最优的无人艇自主跟踪方法，针对无人艇从发现目标后到达到稳定跟踪的过程，构建无人艇运动学模型、稳定跟踪模型，并将自主跟踪过程分为绕后跟踪和速度跟踪两个部分分别进行求解。绕后跟踪过程中希望无人艇在确保路径平滑平稳的同时能尽快跟上目标，缩短和目标的距离。因此基于hybridA*算法在连续坐标系下进行启发式搜索，并根据无人艇运动学模型，生成不同长度的曲线完成路径节点的扩张和控制空间的采样，以得到动力可行的路径，确保路径平滑。此外通过考虑无人艇时间、能量以及平滑代价对代价函数进行改进，令无人艇能尽快跟踪上目标，减少到达速度跟踪状态的时间，解决进入速度跟踪状态花费时间长的问题。而在速度跟踪阶段，需要对无人艇速度进行调整，以确保尽快达到并维持稳定跟踪状态。在这个阶段将路径规划问题转化为优化问题。根据目标船只速度信息，得到末端跟踪状态(位置、速度信息)，并考虑时间、能量最优的方式，让跟踪轨迹平滑舒适，解决进入稳定跟踪状态后跟踪不稳定、加速度不连续的问题。本发明可以应用于无人艇的路径规划领域，可以实时得到一条最优平滑平稳轨迹，并实现对目标的自主跟踪，能够直接给控制器提供无人艇速度、方向、时间以及路径点信息。

首先，根据跟踪目标的速度以及梯形速度规划和冗余性考量，建立稳定跟踪模型，确定无人艇末端速度

其中为无人艇末端速度，v_max为无人艇最大速度，d为无人艇与目标船只的距离，d_h为保持距离，指无人艇在追踪过程中与目标应当保持的距离。d_l为实际跟踪距离，d_l＝d-d_h，v_obs为跟踪目标当前速度，a_max为无人艇最大加速度，ξ为调节因子。

这一阶段认为是绕后跟踪阶段；将/>这一阶段认为是速度跟踪阶段。

步骤S10，建立无人艇运动模型。

q_i＝(p_i,v_i,θ_i)

其中：q_i为无人艇当前第i时刻的状态，p_i＝(x_i,y_i)为无人艇当前第i时刻的位置，x_i为无人艇在横轴的位置，y_i为无人艇在纵轴的位置，θ_i为无人艇的航向角，i为时刻序列。(x_i+1,y_i+1,v_i+1,θ_i+1)为无人艇第i+1时刻的状态。v_i为无人艇的速度。φ为无人艇的转向角，R为无人艇的转弯半径，L为无人艇艇长。Δθ表示无人艇在步长时间Δt内航向角的变化量。

步骤S20，在当前时刻，对无人艇下一时刻速度、步长时间、转向角进行采样，得到采样运动参数集合；根据无人艇当前时刻状态、采样运动参数集合以及无人艇运动模型计算得到一系列下一时刻无人艇的状态。在当前状态与下一时刻状态间进行采样从而得到无人艇当前时刻的预期跟踪轨迹集合；

步骤S30，安全性检查，根据无人艇下一时刻状态量以及运动学模型可以算出无人艇的采样轨迹，对采样轨迹进行安全性检查，若与无人艇的障碍物模型存在交集，则舍弃该采样轨迹。

步骤S40，建立代价函数。

从无人艇控制能量最优、时间最优、以及防止路径抖动三个方面分别建立控制能量代价函数g_e，时间代价函数g_t、平滑代价函数g_s。

g_t＝Δt

其中：a_i表示第i时刻无人艇的加速度，p_i-2、p_i-1、p_i分别表示第i-2时刻，i-1时刻以及第i时刻无人艇的位置，Δt为该时刻的步长时间。

最终得到第i-1时刻到第i时刻的时间内的路径代价函数g_i：

从起点到当前点的代价函数为：

其中：n为当前点的序列号，T_g为从无人艇从起点到当前点需要花费的总时间，η、μ、λ分别为控制能量代价、时间代价以及平滑代价的权重因子。

步骤S50，建立启发函数h。

考虑到无人艇的非完整性约束以及海面障碍物只要都是船只和浮标等单个障碍物，直接基于Dubin曲线模型构建启发函数，确保路径尽快收敛。

h＝d_dubin

其中d_dubin为根据Dubin曲线模型计算得到的无人艇当前点到终点的路径长度。

步骤S60，建立估值函数f

f＝g+h

其中g为代价函数，h为启发函数。

步骤S70，根据估值函数进行打分，基于hybrid A*方法搜索得到估值最小的路径点，最终得到一条运动学可行路径。

步骤S80，一步求解实现速度规划。

在速度跟踪阶段，考虑到速度跟踪的精度，将跟踪问题转化为OBVP(OptimalBoundary Value Problem)进行求解。已知末端跟踪状态和开始跟踪点状态，按如下方式建立目标函数f₁，即希望在从开始跟踪点到末端跟踪点的过程中跟踪时间和控制能量能达到最优。

其中T_t为速度跟踪阶段完成跟踪时间，a为速度跟踪阶段无人艇的加速度。

基于无人艇运动学模型和速度跟踪阶段初始和末端状态，采用庞特里亚金最小值原理对上式进行闭式求解，求解得到最优跟踪时间T_t ^*和跟踪轨迹。

步骤S90，可行性判断。由于求解OBVP得到多项式轨迹时，没有对障碍物和速度进行约束，因此需要对求出的跟踪轨迹进行安全性检查，检查轨迹是否与障碍物产生碰撞。此外还需要进行速度约束检查，检查轨迹每一点的速度是否超过速度最大值v_max。若安全性检查或动力学检查未通过，则认为该轨迹不可行，一步求解跟踪轨迹失败，此时继续根据估值函数进行启发式路径搜索，即继续进行后续步骤。若可行性判断通过，根据跟踪轨迹可以计算得到每一点的速度、时间、位置、方向信息，将上述信息发给控制器实现无人艇的自主跟踪，完成跟踪规划，不再进行后续步骤。

步骤S100，采样速度规划。当完成路径搜索后，需要在搜索得到路径基础上进行速度规划以确保跟踪过程的稳定可靠性。

设无人艇执行该跟踪轨迹的速度与时间满足如下关系。

v(t)＝αt²+βt+γ，t∈[0,T_t]

其中，α、β、γ为待求解的系数，t为无人艇执行跟踪轨迹的时间，T_t为速度跟踪阶段完成跟踪轨迹的时间。

同理可以求得轨迹长度和时间的关系。

其中κ为待求解的系数。

搜索得到的运动学可行路径时自带粗略速度信息，将该路径点的速度信息作为跟踪初始时刻的无人艇速度v₀，根据稳定跟踪模型可以得到末端跟踪速度v_t，跟踪初始时刻跟踪轨迹长度为0，根据搜索得到的运动路径可以计算得到跟踪轨迹长度s_t。

根据上述约束，可求解得到：

v(t)＝αt²+βt+v₀

已知s_t和无人艇的跟踪初始速度以及速度最大值v_max，可以粗略算出无人艇执行完跟踪轨迹的时间区间[T₁,T₂]。

步骤S110，时间采样。首先使用二分法先尽快找到满足速度约束的时间区间。由于在一步求解过程(即步骤S80)中已经求出最优时间T_t ^*，但T_t ^*不满足速度或者障碍物约束。因此在T_t ^*附近设置较小的采样间隔，时间区间边缘位置设置较大的采样间隔。通过采样得到一组采样时间集合且/>其中n_st为跟踪采样时间集合中的元素个数，/>为速度跟踪阶段跟踪总时间采样集合中每一时间采样点。

步骤S120，筛选速度跟踪阶段次优跟踪总时间T_sot ^*，对跟踪采样时间集合中的每一个采样时间计算时间与能量代价值，即目标函数f₂的值。

其中f₂为目标函数。

选取目标函数f₂的值最小时的时间采样量作为完成跟踪轨迹的时间T_sot ^*。最终得到速度为：

根据上式可以算出每一点的速度、时间信息，将速度、时间、位置、方向信息发给控制器实现无人艇的自主跟踪。

如图2所示为采用本发明方法实现的跟踪轨迹示意，可以看出，无人艇在由左右两侧进行跟踪时均能实现稳定跟踪。

以上结合具体实施方式和范例性实例对本发明进行了详细说明，不过这些说明并不能理解为对本发明的限制。本领域技术人员理解，在不偏离本发明精神和范围的情况下，可以对本发明技术方案及其实施方式进行多种等价替换、修饰或改进，这些均落入本发明的范围内。本发明的保护范围以所附权利要求为准。

本发明说明书中未作详细描述的内容属本领域技术人员的公知技术。

Claims (7)

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

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

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

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

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

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

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

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

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

当无人艇处于速度跟踪阶段时，执行步骤S2.1～S2.5；

S2.1建立基于速度跟踪阶段跟踪总时间、速度跟踪阶段无人艇加速度、控制能量代价权重因子以及时间代价权重因子的第一目标函数，根据无人艇运动学模型、以及速度跟踪阶段无人艇初始状态和末端状态求解第一目标函数，得到速度跟踪阶段最优跟踪总时间和速度跟踪阶段跟踪轨迹；

S2.2对速度跟踪阶段跟踪轨迹进行可行性判断，具体为，将不与障碍物发生碰撞且每一点速度不超过无人艇最大速度的跟踪轨迹判断为可行，否则判断为不可行；如果跟踪轨迹判断为可行，使无人艇按照所述速度跟踪阶段最优跟踪时间和跟踪轨迹进行跟踪，如果跟踪轨迹判断为不可行，执行步骤S2.3；

S2.3执行步骤S1.1～S1.6，基于步骤S1.6所得最优跟踪轨迹和无人艇速度约束得到速度跟踪阶段跟踪总时间采样集合；

S2.4建立基于速度跟踪阶段跟踪总时间、速度跟踪阶段无人艇加速度、控制能量代价权重因子以及时间代价权重因子的第二目标函数，求得速度跟踪阶段跟踪总时间采样集合中每一时间采样点的第二目标函数值，根据每一时间采样点的第二目标函数值确定速度跟踪阶段次优跟踪总时间；

S2.5根据速度跟踪阶段次优跟踪总时间计算得到速度规划后的每一轨迹点速度；

所述稳定跟踪模型如下：

时无人艇处于绕后跟踪阶段；/>时无人艇处于速度跟踪阶段；

其中，为无人艇处于绕后跟踪阶段或速度跟踪阶段时的末端速度，v_max为无人艇最大速度，d_l＝d-d_h，d为无人艇与目标的距离，d_h为保持距离，v_obs为目标当前速度，a_max为无人艇最大加速度，ξ为调节因子；

所述步骤S1.1中，无人艇运动模型如下：

q_i＝(p_i,v_i,θ_i)；

其中，q_i为无人艇当前时刻i的状态，p_i＝(x_i,y_i)为无人艇当前时刻i的位置，θ_i为无人艇当前时刻i的航向角，(x_i+1,y_i+1),θ_i+1分别为无人艇下一时刻i+1的位置和航向角，v_i为当前时刻i无人艇的速度，φ为无人艇的转向角，R为无人艇的转弯半径，L为无人艇艇长，Δθ表示无人艇在步长时间Δt内航向角的变化量；

所述步骤S1.4中，建立控制能量代价函数g_e、时间代价函数g_t和平滑代价函数g_s如下：

g_t＝Δt

其中，a_i表示当前时刻i无人艇的加速度，Δt为当前时刻i的步长时间，p_i-2、p_i-1、p_i分别表示i-2时刻，i-1时刻以及当前时刻i无人艇的位置。

2.根据权利要求1所述的一种基于时间最优与能量最优的无人艇自主跟踪方法，其特征在于，所述步骤S1.4中，无人艇由起点位置到当前位置的代价函数如下：

其中，g_i＝ηg_e+μg_t+λg_s，η、μ、λ分别为控制能量代价、时间代价以及平滑代价的权重因子，n为以起点序列号为0时，当前点的序列号，T_g为从无人艇从起点到当前点需要花费的总时间。

3.根据权利要求1所述的一种基于时间最优与能量最优的无人艇自主跟踪方法，其特征在于，所述步骤S1.5中，启发函数h为：

h＝d_dubin

其中，d_dubin为根据Dubin曲线模型计算得到的无人艇当前点到终点的路径长度；

所述步骤S1.6中，估值函数f＝g+h；

其中，g为无人艇由起点位置到当前位置的代价函数；

所述步骤S1.6中，基于hybrid A*方法搜索得到估值函数f估值最小的路径，即为最优跟踪轨迹。

4.根据权利要求1所述的一种基于时间最优与能量最优的无人艇自主跟踪方法，其特征在于，所述步骤S2.1中，第一目标函数为：

其中，以速度跟踪阶段起始时刻为0，T_t为速度跟踪阶段完成时的时刻，即速度跟踪阶段跟踪总时间，a为速度跟踪阶段无人艇的加速度，η、μ分别为控制能量代价和时间代价权重因子；

所述步骤S2.1中，根据无人艇运动学模型、以及速度跟踪阶段无人艇初始状态和末端状态，采用庞特里亚金最小值方法对目标函数进行闭式求解。

5.根据权利要求1所述的一种基于时间最优与能量最优的无人艇自主跟踪方法，其特征在于，所述步骤S2.3的具体步骤如下：

S2.3.1设无人艇执行步骤S1.6所得最优跟踪轨迹的速度v(t)与时间t满足如下关系式：

v(t)＝αt²+βt+γ，t∈[0,T_t]

其中，α、β、γ为待求解的系数，以速度跟踪阶段起始时刻为0，T_t为速度跟踪阶段完成时的时刻，即速度跟踪阶段跟踪总时间；

S2.3.2建立无人艇执行步骤S1.6所得最优跟踪轨迹的轨迹长度s(t)与时间t的关系式：

其中，κ为待求解的系数；

S2.3.3构建约束条件：

其中，v₀为无人艇在速度跟踪阶段初始时刻的速度，v_t为无人艇在速度跟踪阶段的末端速度，s_t为最优跟踪轨迹总长度；

S2.3.4根据步骤S2.3.3所构建的约束条件对步骤S2.3.1和S2.3.2的关系式进行求解，得到：

v(t)＝αt²+βt+v₀

S2.3.5估计速度跟踪阶段跟踪总时间T_t的区间为[T₁,T₂]：

S2.3.6采用二分法确定在区间[T₁,T₂]中满足无人艇速度约束的区间[T₁′,T₂′]，[T₁′,T₂′]即速度跟踪阶段跟踪总时间区间；

S2.3.7在区间[T₁′,T₂′]中确定n_st个跟踪采样时间，得到速度跟踪阶段跟踪总时间采样集合各跟踪采样时间间隔大小的设置规律为，由区间[T₁′,T₂′]边缘至步骤S2.1中所得速度跟踪阶段最优跟踪总时间，间隔逐渐减小。

6.根据权利要求5所述的一种基于时间最优与能量最优的无人艇自主跟踪方法，其特征在于，所述步骤S2.4中，第二目标函数f₂为：

其中，为速度跟踪阶段跟踪总时间采样集合中每一时间采样点，a为速度跟踪阶段无人艇的加速度，η、μ分别为控制能量代价和时间代价权重因子；

所述步骤S2.4中，将第二目标函数值最小时对应的跟踪采样时间作为速度跟踪阶段次优跟踪总时间。

7.根据权利要求6所述的一种基于时间最优与能量最优的无人艇自主跟踪方法，其特征在于，所述步骤S2.5中，根据速度跟踪阶段次优跟踪总时间得到速度规划后的每一轨迹点速度：

其中，T_sot ^*为根据步骤S2.4得到的速度跟踪阶段次优跟踪总时间。