CN104536442B - 一种基于动态规划的水下航行器路径规划方法 - Google Patents

Abstract

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

Description

一种基于动态规划的水下航行器路径规划方法

技术领域

本发明属于水下航行器路径规划技术领域，具体涉及一种基于动态规划的水下航行器路径规划方法。

背景技术

路径规划是一种在某一特定环境下，从设定的起始点到终点规划出一条可行的路径的方法，这条路径一定是在某种约束下的最优或次优解，这种约束可以是时间最优或威胁最小或能耗最少等条件。路径规划已经应用于各种场合，例如移动机器人路径规划、无人机路径规划，水下航行器路径规划。路径规划可以综合考虑各种环境状况，例如面临的各种障碍、威胁，规划出一条时间最优或者能耗最优的路径，对于水下航行器路径规划也已经有很多研究，然而对于低速水下航行器，例如水下滑翔机，在海洋中远比其它的水下航行器受洋流的影响大，因此很有可能受洋流的运动而偏离既定的航迹很远，而且洋流速度的大小、方向也不是固定不变的，而是随时间及各种因素状况不断调整的。

发明内容

为了避免现有技术的不足之处，本发明提出一种基于动态规划的水下航行器路径规划方法，针对传统水下航行器路径规划未能考虑低速水下航行器受洋流影响较大的情况，根据可预测或已知的时变洋流设计了适用于单个水下航行器路径规划方法。

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

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

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

速度合成模型为：

如果Δ≤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_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_n,m]即为所求最短路径；

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

本发明提出的一种基于动态规划的水下航行器路径规划方法，对于低速水下航行器受到洋流影响较大，很大程度上会收到洋流的影响而偏离预定航迹很远，而现有的水下航行器都没能够考虑这一影响，而且洋流是复杂、时变的。

本发明在洋流的未来状况已知的情况下，先将所要规划的区域栅格化然后利用速度合成模型来计算搜索点之间所耗费的时间值，并以时间值作为权重，应用动态规划算法在二维平面内规划出一条可达最优路径。本发明能够根据已知的洋流的情况，利用动态规划快速规划出一条可行路径，仿真验证了该算法具有良好的效果。

附图说明

图1即为速度矢量合成模型图

图2-6分别为下列时刻的搜索结果及洋流状况图，图中密集的小箭头即为洋流的速度矢量。

图2 t＝56.0386s 20

图3 t＝80.3424s

图4 t＝108.4267s

图5 t＝131.9892s

图6 t＝213.3822s

具体实施方式

现结合实施例、附图对本发明作进一步描述：

利用本发明实现路径规划的具体步骤如下：

步骤1：将路径规划空间栅格化。将规划区域的横向等间距分为40份，纵向等间距分为30份，这样就形成了1200个点，在实际应用中可根据情况进行更改。每列的点为一个搜索阶段，按顺序组成了40个阶段，起始点设为第一个阶段的第一个一个点，终点设为最后阶段最后一个点。搜索过程即为从每个搜索阶段中找出一个点，从起始点到最终点首位顺序连接出一条线段，即为规划路径。然后根据步骤2-5求得两个点之间所需时间；(1)将起始点与终点所在水平空间区域栅格化，得到栅格化的点，并设起始点的时刻为0；

将栅格化后的点按照空间或时间特征，划分为若干组合，每个组合作为一个搜索阶段，最终的目的是从每个阶段选择出一个点，作为决策点，然后将所有阶段所选出的决策点前后相连组成一条线，即为规划出的路径。要注意的是，这若干个阶段一定是有序的，即由前一个阶段转向下一个阶段(即无后向性)，否则无法用动态规划求解。例如，可以选择起始点为一阶段，终点为一阶段，中间每一列所有的点作为一个阶段，由起始阶段向最终阶段递推；

步骤2：在洋流速度大小、方向均为时变且已知的情况下，并且航行器速度大小已确定、方向可调，利用速度合成模型计算所有相邻阶段内的点与点之间的速度大小、方向，判断能否从此阶段的一点转移到相邻阶段的另一点，如果可行，利用两点间的距离除以速度得到转移状态点之间所耗费时间。

速度合成是从一个阶段的搜索起始点到下一阶段的搜索终点，同时下一阶段的搜索终点即为下一个搜索阶段的搜索起始点，由搜索起始点到达搜索终点所耗用时间为 表示第i个搜索阶段的路径长度，表示第i个搜索阶段的速度大小，可由下列公式求得：

判别式

如果Δ≤0，则表示路径不可达，即两点之间路径不可通，如果Δ>0，则带入下式计算：

如果得出v_route≤0，则路径也不可达，表示两个搜索点之间的单位矢量。v_vehicle为航行器的速度，假设航行器速度大小不变，方向可调。V_{ocean_current}是洋流在某一时刻的速度矢量，既包括速度大小，也包括方向，且均为已知。图1所示即为向量合成示意图，即v_vehicle，V_{ocean_current}两个向量合成两个搜索点之间速度向量，方向即向量所在方向，大小即为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_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为航行器的规划终点；

应用动态规划算法找出从初始点到第i阶段所有点之间的距离。公式可以表示为f[s_i,j]＝min{f[s_i-1,k]+w(s_i-1,k,s_i,j)}，其中f[s_i,j]表示从起始点到第i阶段的第j个点的路径长度，w(s_i-1,k,s_i,j)表示第i-1阶段的第k个点到第i阶段的第j个点所耗费时间。公式即表示从第i-1阶段所有点中找出使f[s_i-1,k]+w(s_i-1,k,s_i,j)最小的点，作为f[s_i,j]，然后开始进行第i到i+1阶段的搜索，首先通过2-5步骤计算出i阶段所有点到i+1阶段所有点之间的时间，然后找出初始点到第i+1阶段所有点之间的距离，往后递推计算，直至到最后阶段，最后f[s_40,30]即为所求最短路径。

本方法由终点开始递归，逐步递归到起始点，δ(v)是与u相邻的上一个阶段的所有的点，w(v,u)即为两个搜索点之间的时间，MinDista(u)即为从初始点到u这一点之间所经过的时间长度，即以总时间为权重，最后取使MinDista(v_终)最小的路径节点为最优路径。

具体实施方式

初始条件：

在单个航行器路径仿真实例中，路径规划起点为(0，0)，终点为(200，150)，处在整个矩形仿真区域的主对角线两个端点，在整个仿真区域中，有四个涡流，中心分别为(150，125)，(70，65)，(150，100)，(70，40)，涡流中心速度分别为2.5m/s，3.5m/s，2.5m/s，4m/s，流速随着距中心的距离增大而减小。这四个涡流中心不移动， 此外，有在t＝0时刻中心点为(200，0)的速度为3m/s的涡流在仿真区域的对角线方向移动，中心移动速度为1m/s。在仿真区域的右上角，有一个长方形的障碍物，四个角的坐标为(190，105)，(200，105)，(200，130)，(190，230)。

本发明的单个路径实例仿真步骤如下：

(1)将空间栅格化，计算每一个点的横纵坐标，再根据可预测的洋流状况，画出洋流的速度矢量图,设初始时刻t＝0；

(2)从初始阶段开始递推计算起始点到下一个阶段的所有点的距离、速度、时间，并将其存储下来，再从下一个阶段递推计算其下一个阶段所用时间，依次往后，直至最后的终点；

(3)使用动态规划算法从终点倒推至起点，寻找使MinDista(v_终)的路径节点，将其记下来，并完成绘图，图2到图6即为搜索结果。

Claims (1)

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

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

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

速度合成模型为：

如果Δ≤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_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_n,m]即为所求最短路径；

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