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

权利要求书

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根据速度跟踪阶段次优跟踪总时间计算得到速度规划后的每一轨迹点速度。

2.根据权利要求1所述的一种基于时间最优与能量最优的无人艇自主跟踪方法，其特征在于，所述稳定跟踪模型如下：

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

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

3.根据权利要求1所述的一种基于时间最优与能量最优的无人艇自主跟踪方法，其特征在于，所述步骤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内航向角的变化量。