[发明专利]一种基于动态规划的水下航行器路径规划方法

权利要求书

1.一种基于动态规划的水下航行器路径规划方法，其特征在于步骤如下：

步骤1、将路径规划空间栅格化：将规划区域的横向等间距分为n份，纵向等间距分为m份，这样就形成了n×m个点，以每列的点为一个搜索阶段，按顺序组成n个阶段；以第一个阶段的第一个点为搜索起始点，以最后阶段最后一个点为终点；

步骤2：在航行器起始点开始，根据已知的洋流模型，获取在初始时刻t_start、初始点Search_start_point所在位置的洋流速度V_{ocean_current_start}，利用速度合成模型判断两个搜索点之间能否通过；

速度合成模型为：Δ=(Vroute0T·Vcoean_current)2+vvehicle2-Vocean_currentT·Vocean_current]]>

如果Δ≤0，两点之间路径不可通；

如果Δ＞0，计算两个搜索点之间的速度

如果v_route≤0，两点之间路径不可通；

式中：表示两个搜索点之间的单位矢量；v_vehicle为航行器的速度，V_{ocean_current}是洋流在某一时刻的速度矢量；

如果v_route＞0，v_route为两个搜索点之间的速度；

步骤3：计算以为粗略估计的时间t_{rough_route}；

步骤4：以t_{rough_route}+t_start为时间值从洋流模型中获取在初始点的下一个搜索点的V_{ocean_current_end}速度；

步骤5：以(V_{ocean_current_start}+V_{ocean_current_end})/2作为V_ocean，再次计算vroute=Vroute0T·Vocean_current+Δ,]]>以及得到两个阶段搜索点之间的相对t_{rough_route}更精确的时间t_{accu_route}，以t_{accu_route}为搜索权重；以此计算一个阶段的所有点到下一个阶段的所有点之间的时间，并把该时间作为两点间的搜索权重；然后以t_{accu_route}+t_start作为Search_end_point这一点的时刻值，在起始时刻为0的条件下，应用动态规划算法算出最初起始点到这一搜索点的最早时刻，然后将其作为下一个搜索点的t_start，以此作为获取时变洋流速度的变量之一；在下一步搜索中，以Search_end_point为下一步的初始点，以t_{accu_route}+t_start作为初始点所在时刻，直至Search_end_point为航行器的规划终点；

步骤6、应用动态规划算法找出从初始点到第i阶段所有点之间的距离：f[s_i,j]表示从第i-1阶段所有点中找出使f[s_i-1,k]+w(s_i-1,k,s_i,j)最小的点，作为f[s_i,j]：

f[s_i,j]＝min{f[s_i-1,k]+w(s_i-1,k,s_i,j)}

其中：w(s_i-1,k,s_i,j)表示第i-1阶段的第k个点到第i阶段的第j个点所耗费时间；

然后开始进行第i到i+1阶段的搜索，首先通过2-5步骤计算出i阶段所有点到i+1阶段所有点之间的时间，然后找出初始点到第i+1阶段所有点之间的距离，往后递推计算，直至到最后阶段，最后f[s_40,30]即为所求最短路径；

步骤7：将每个搜索阶段中找出的一个点，从起始点到最终点首位顺序连接出一条线段，为规划路径，然后根据步骤2-5求得两个点之间所需时间。