[发明专利]一种水下路径规划方法和系统

权利要求书

1.一种水下路径规划方法，其特征在于，包括：

S1.对水下潜航器进行定位并获得周围环境的点云图；

S2.利用定位设备的位姿信息和点云图构建稠密图；

S3.结合水下潜航器的运动学特性、水流对水下潜航器消耗能量的影响、水下潜航器的运行速度和水下潜航器的运行时间，利用Hybrid A*算法对水下潜航器进行路径规划；步骤S3具体包括：

01.预测水下潜航器下一个时刻到达节点：

利用代价函数f_total＝λ₁J(T)+λ₂W_c+λ₃T+λ₄F_v选取水下潜航器设定邻域内代价值最小的节点，作为水下潜航器下一个时刻到达节点；

f_total表示总的代价函数的值，J(T)表示水下潜航器的运动学特性，W_c表示水下环境中水流对水下潜航器消耗能量的影响；λ₁、λ₂、λ₃、λ₄表示设定常数，T表示水下潜航器的运行时间，F_v表示对水下潜航器的速度限制；

02.碰撞检测：判断障碍物在△t时间内随水流移动到的位置与对应时间段预测的节点位置是否相同；若是，则判断发生碰撞，在剩下的节点中继续执行步骤01-02；若否，进入步骤S4；

S4.结合障碍物相对水下潜航器的方向和水流方向，利用Bspline算法对规划路径进行优化，得到最终的水下潜航器运行路径；其中，障碍物相对水下潜航器的方向根据稠密图得到；步骤S4具体为，利用如下代价函数选择使f_sum最小的控制点位置，实现对轨迹的优化；

f_sum＝λ₅f_w+λ₆f_s

f_sum表示总的代价函数，f_s表示曲线光滑度的代价函数，f_w表示水流对轨迹影响的代价函数，Q_i表示待优化的控制点位置，p_b表示样条曲线的次数，N+1表示控制点的个数，p_io表示障碍物相对控制点的方向向量，v_ic表示水流的速度向量，λ₅表示设定常数。

2.根据权利要求1所述的一种水下路径规划方法，其特征在于，步骤S1具体为，利用视觉传感器对水下潜航器进行定位并获得周围环境的点云图。

3.根据权利要求2所述的一种水下路径规划方法，其特征在于，步骤S2具体为，利用视觉传感器的位姿信息和点云图构建ESDF图。

4.根据权利要求1所述的一种水下路径规划方法，其特征在于，水下环境中水流对水下潜航器消耗能量的影响W_c表达式为：

v_w表示水流的速度，v_c表示水下潜航器的速度。

5.根据权利要求1所述的一种水下路径规划方法，其特征在于，水下潜航器的速度限制F_v表达式为：

F_v＝f_xv+f_yv+f_zv

v_μc表示水下潜航器速度v_c在x轴，y轴，z轴上的速度分量，v_max表示设定的水下潜航器速度最大值，f_μv表示水下潜航器在其中一个轴上速度的代价值。

6.一种水下路径规划系统，其特征在于，包括：计算机可读存储介质和处理器；

所述计算机可读存储介质用于存储可执行指令；

所述处理器用于读取所述计算机可读存储介质中存储的可执行指令，执行权利要求1至5任一项所述的水下路径规划方法。