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

摘要

本发明涉及一种基于动态规划的水下航行器路径规划方法，对于低速水下航行器受到洋流影响较大，很大程度上会收到洋流的影响而偏离预定航迹很远，而现有的水下航行器都没能够考虑这一影响，而且洋流是复杂、时变的。本发明在洋流的未来状况已知的情况下，先将所要规划的区域栅格化然后利用速度合成模型来计算搜索点之间所耗费的时间值，并以时间值作为权重，应用动态规划算法在二维平面内规划出一条可达最优路径。本发明能够根据已知的洋流的情况，利用动态规划快速规划出一条可行路径，仿真验证了该算法具有良好的效果。

主权项

一种基于动态规划的水下航行器路径规划方法，其特征在于步骤如下：步骤1、将路径规划空间栅格化：将规划区域的横向等间距分为n份，纵向等间距分为m份，这样就形成了n×m个点，以每列的点为一个搜索阶段，按顺序组成n个阶段；以第一个阶段的第一个点为搜索起始点，以最后阶段最后一个点为终点；步骤2：在航行器起始点开始，根据已知的洋流模型，获取在初始时刻tstart、初始点Search_start_point所在位置的洋流速度Vocean_current_start，利用速度合成模型判断两个搜索点之间能否通过；速度合成模型为：Δ=(Vroute0T·Vcoean_current)2+vvehicle2-Vocean_currentT·Vocean_current]]>如果Δ≤0，两点之间路径不可通；如果Δ＞0，计算两个搜索点之间的速度如果vroute≤0，两点之间路径不可通；式中：表示两个搜索点之间的单位矢量；vvehicle为航行器的速度，Vocean_current是洋流在某一时刻的速度矢量；如果vroute＞0，vroute为两个搜索点之间的速度；步骤3：计算以为粗略估计的时间trough_route；步骤4：以trough_route+tstart为时间值从洋流模型中获取在初始点的下一个搜索点的Vocean_current_end速度；步骤5：以(Vocean_current_start+Vocean_current_end)/2作为Vocean，再次计算vroute=Vroute0T·Vocean_current+Δ,]]>以及得到两个阶段搜索点之间的相对trough_route更精确的时间taccu_route，以taccu_route为搜索权重；以此计算一个阶段的所有点到下一个阶段的所有点之间的时间，并把该时间作为两点间的搜索权重；然后以taccu_route+tstart作为Search_end_point这一点的时刻值，在起始时刻为0的条件下，应用动态规划算法算出最初起始点到这一搜索点的最早时刻，然后将其作为下一个搜索点的tstart，以此作为获取时变洋流速度的变量之一；在下一步搜索中，以Search_end_point为下一步的初始点，以taccu_route+tstart作为初始点所在时刻，直至Search_end_point为航行器的规划终点；步骤6、应用动态规划算法找出从初始点到第i阶段所有点之间的距离：f[si,j]表示从第i‑1阶段所有点中找出使f[si‑1,k]+w(si‑1,k,si,j)最小的点，作为f[si,j]：f[si,j]＝min{f[si‑1,k]+w(si‑1,k,si,j)}其中：w(si‑1,k,si,j)表示第i‑1阶段的第k个点到第i阶段的第j个点所耗费时间；然后开始进行第i到i+1阶段的搜索，首先通过2‑5步骤计算出i阶段所有点到i+1阶段所有点之间的时间，然后找出初始点到第i+1阶段所有点之间的距离，往后递推计算，直至到最后阶段，最后f[s40,30]即为所求最短路径；步骤7：将每个搜索阶段中找出的一个点，从起始点到最终点首位顺序连接出一条线段，为规划路径，然后根据步骤2‑5求得两个点之间所需时间。