基于空间分解的水下航行器三维路径规划方法

本发明涉及基于空间分解的水下航行器三维路径规划方法，属于水下航行器路径规划领域。

背景技术：

1、近年来，随着计算机、人工智能等技术的发展，水下航行器越来越被广泛用于水下工业、军事等领域。在水下执行任务的过程中，航行器需要根据周围环境信息规划出一条安全、高效的可行路径，水下三维路径规划方法研究成为水下航行器自主技术的关注重点。

2、动态窗口算法(dynamic window approaches,dwa)是基于预测控制理论的一种局部规划算法，具有计算量小，反应迅速、可操作性强等特点。dwa算法将避障问题描述为速度空间中带约束的优化问题，其可以在运动学和动力学约束以及障碍物安全距离约束下形成动态窗口并在速度空间中采样多组速度数据，然后根据这些数据模拟出未来一段时间内的运动轨迹，最后利用评价函数对所有模拟轨迹进行评分并选取最优轨迹进行运动，从而实现动态避障。

3、然而现有对于dwa算法规划路径的研究大多基于二维平面展开，同时dwa算法存在易陷入局部最优解等问题。

技术实现思路

1、针对现有dwa算法规划路径易陷入局部最优解，对三维空间路径规划适用性差的问题，本发明提供一种基于空间分解的水下航行器三维路径规划方法。

2、本发明的一种基于空间分解的水下航行器三维路径规划方法，包括：

3、根据高程地图数据，确定航行器任务区间的高程地图障碍物，并根据任务区间确定环境边界；

4、采用墙体障碍物标记环境边界，确定环境边界内动态障碍物；

5、对所有墙体障碍物、高程地图障碍物与动态障碍物进行基于障碍物点的膨化处理，并基于膨化处理后动态障碍物确定动态障碍物安全范围；由膨化处理后墙体障碍物、膨化处理后高程地图障碍物和动态障碍物安全范围确定航行器当前路径规划三维空间；

6、在航行器当前路径规划三维空间内采用dwa算法进行路径规划，包括：

7、根据航行器当前位置点、目标位置点、高程地图障碍物位置以及动态障碍物安全范围，确定dwa算法的权重参数；

8、再根据航行器当前位置点的深度构建水平面，并建立以航行器当前位置点和目标位置点连接形成的直线确定的垂直于所述水平面的垂直剖面；

9、分别获取水平面和垂直剖面上的所有障碍物点；基于获取的障碍物点，采用dwa算法分别计算航行器在水平面和垂直剖面由当前位置点到目标位置点的避碰路径一步规划结果；将两个避碰路径一步规划结果进行耦合，得到耦合后一步规划路径；使航行器按照耦合后一步规划路径航行后，更新航行器当前位置点和动态障碍物安全范围，再继续采用dwa算法进行路径规划，直到到达目标位置点；

10、在采用dwa算法进行路径规划的过程中，当dwa算法陷入局部最优解时，采用人工势场法算法进行路径辅助规划，直到脱离dwa算法的局部最优解位置，再采用dwa算法继续进行路径规划。

11、根据本发明的基于空间分解的水下航行器三维路径规划方法，确定环境边界内动态障碍物包括确定动态障碍物初始位置和运动速度。

12、根据本发明的基于空间分解的水下航行器三维路径规划方法，所述膨化处理的方法为：以障碍物点为中心，基于预设半径得到球面障碍物点集；由所有球面障碍物点集形成球面障碍物边界。

13、根据本发明的基于空间分解的水下航行器三维路径规划方法，确定dwa算法的权重参数的方法为：

14、若航行器当前位置点和目标位置点之间的连线上存在高程地图障碍物或动态障碍物安全范围，则第一标志位置0；否则置1；

15、若航行器当前位置点探测范围内存在高程地图障碍物，则第二标志位置0；否则置1；

16、若两个标志位均为1，则在dwa算法中，选择航向权重高的权重参数；否则选择障碍物距离权重高的权重参数。

17、根据本发明的基于空间分解的水下航行器三维路径规划方法，将垂直剖面的剖面方程表示为：

18、ax+by+cz+d＝0，

19、式中x表示垂直剖面的x轴坐标，y表示垂直剖面的y轴坐标，z表示垂直剖面的z轴坐标，a为x轴系数，b为y轴系数，c为z轴系数，d为常数项；

20、根据剖面方程的系数a、b、c及常数项d计算障碍物点与垂直剖面的距离d：

21、

22、式中xob为障碍物点的x轴坐标，yob为障碍物点的y轴坐标，zob为障碍物点的z轴坐标；

23、根据高程地图障碍物与垂直剖面的距离与当前位置点探测范围的比较结果，确定第二标志位的标志结果。

24、根据本发明的基于空间分解的水下航行器三维路径规划方法，避碰路径一步规划方法包括对航行器的当前运行速度进行采样：

25、航行器的水平面运行速度vh_m限制为：

26、vh_m＝{(vh,ωh)|vh∈[vh_min,vh_max],ωh∈[ωh_min,ωh_max]}，

27、式中vh为水平面运行线速度，ωh为水平面运行角速度，vh_min为水平面运行最小线速度，vh_max为水平面运行最大线速度，ωh_min为水平面运行最小角速度，ωh_max为水平面运行最大角速度；

28、设定采样周期为δt，得到动态矩形窗口vh_d：

29、

30、式中vh_c为航行器当前水平面运行线速度，ahv_max为航行器水平面最大线加速度，ωh_c为航行器当前水平面运行角速度，ahw_max为航行器水平面最大角加速度；

31、由此得到满足动态避障的水平面运行速度vh_a：

32、

33、式中dist(vh,ωh)为距离评价子函数，表示当前水平面运行速度下的规划路径与水平面内障碍物点的最小距离；

34、最后得到航行器的水平面运行速度采样空间vh_r：

35、vh_r＝vh_m∩vh_d∩vh_a。

36、根据本发明的基于空间分解的水下航行器三维路径规划方法，避碰路径一步规划方法还包括对航行器的避碰路径一步规划进行预测：

37、

38、式中xh_k为航行器第k步的当前位置点x轴坐标，yh_k为航行器第k步的当前位置点y轴坐标，θh_k为航行器第k步的当前位置点方位角；dt为步长时间。

39、根据本发明的基于空间分解的水下航行器三维路径规划方法，采用评价函数计算避碰路径一步规划的每一条预测模拟路径的得分g(vh,ωh)：

40、g(vh,ωh)＝α·normal_head(vh,ωh)+β·normal_dist(vh,ωh)+γ·normal_vel(vh,ωh)，

41、式中α为方位角权重，normal_head(vh,ωh)为方位角评价子函数head(vh,ωh)的归一化值，β为距离权重，normal_dist(vh,ωh)为距离评价子函数dist(vh,ωh)的归一化值，γ为速度权重，normal_vel(vh,ωh)为速度评价子函数vel(vh,ωh)的归一化值；

42、根据得分g(vh,ωh)分别确定水平面和垂直剖面内由当前位置点到目标位置点的避碰路径一步规划最优结果。

43、根据本发明的基于空间分解的水下航行器三维路径规划方法，

44、

45、式中head(i)表示第i条预测模拟路径的方位角评价子函数值，n为周期内水平面运行速度采样空间约束条件下的预测模拟路径总数；dist(i)表示第i条预测模拟路径的距离评价子函数值，vel(i)表示第i条预测模拟路径的速度评价子函数值。

46、根据本发明的基于空间分解的水下航行器三维路径规划方法，采用加权平均方法对两个避碰路径一步规划最优结果进行耦合；所述加权平均方法的权重根据水平面和垂直剖面的位置坐标量级差别进行设置。

47、本发明的有益效果：本发明方法实现了dwa算法在水下航行器三维路径规划中的应用。

48、本发明方法在高程地图数据基础上，以添加墙体障碍物方式实现环境边界限制，添加动态障碍物以实现动态实时避碰，且通过判断航行器当前位置、目标点位置与障碍物之间关系设置不同权重值以实现dwa算法权重的自适应更改；通过航行器当前位置与目标点位置将三维空间分解为水平面与垂直剖面，分别在两个面采用dwa算法进行实时动态避碰并将结果进行耦合，从而实现了dwa算法在三维空间规划路径，提高dwa算法在三维空间适应性，减少了计算复杂度；本发明方法在dwa算法陷入局部最优解时，通过采用三维apf算法的方式，实现航行器脱离陷入局部最优解的位置，再继续采用dwa算法规划路径，降低了dwa算法局限性。