[发明专利]水下航行器路径规划算法

权利要求书

1.水下航行器路径规划方法，用于规划水下航行器的航行路径，其特征在于，包括以下步骤：

S1：将水下障碍均近似等效为几何凸面体，采用统一的凸面体方程进行描述：

其中，a,b,c,p,q,r为决定几何凸面体大小和形状的常数，(x₀,y₀,z₀)表示几何凸面体的中心，Γ(P)＝1为障碍物表面方程；

S2：建立以水下航行器航行目标点为中心的汇流场：

定义该汇流场的速度为其中V₀为汇流场的速率，d(P,P_d)表示水下航行器当前位置与航行目标位置的欧式距离；其中，x_d表示：目标位置的x轴坐标，y_d表示：目标位置的y轴坐标，z_d表示：目标位置的z轴坐标；

S3：构建自由环境下航行目标点的初始流场：

假设水流流场为恒速流场，水流流速为u_c(P)，将水流流速与汇流场流速按权重叠加，u(P)＝λ_su_s(P)+λ_cu_c(P)，其中u(P)为初始流场的流速，λ_s和λ_c分别表示汇流场和水流流场权重系数；

S4：定义水下障碍物的修正矩阵，以定义水下障碍物对航行目标点初始流场的扰动影响：

其中，M_k(P)表示第k个障碍物的修正矩阵，K为障碍物的总数，ρ_k、σ_k分别表示第k个障碍物的排斥反应系数和切向反应系数，n_k(P)表示第k个障碍物的径向法向量，t_k(P)表示第k个障碍物的水平切向量，Γ_k(P)表示第k个障碍物的凸面体描述方程：其中，

S5：采用修正矩阵对初始流场进行修正，得到静态障碍环境下的修正流场：

其中，M(P)表示障碍物的修正矩阵；

S6：获得水下航行器的下一路径点：其中ΔT为时间步长；

S7：将各路径点依次连接，获得水下航行器的规划路径。

2.如权利要求1所述的水下航行器路径规划方法，其特征在于，所述路径规划算法进一步包括以下步骤：在步骤S4中，量化定义水下障碍物对初始流场的扰动影响，定义表示第k个障碍物的比例系数，取决于水下航行器与第k个障碍物表面之间的距离：

其中，K为障碍物的总数，Γ_i(P)为水下障碍物等效凸面体方程，Γ_k(P)表示第k个障碍物的凸面体描述方程；水下航行器距离水下障碍物表面的距离越大，越小，以使障碍物对初始流场的修正作用越小；对进行归一化处理：

其中其中：ω_k(P)表示第k个障碍物的归一化比例系数；

对修正矩阵进行量化处理，得到：

在步骤S5中，采用量化处理后的修正矩阵获得修正流场：

3.如权利要求1所述的水下航行器路径规划方法，其特征在于，在步骤S5中，若静态环境存在运动威胁时，引入运动威胁来修正流场：

计算威胁的相对水下航行器的参考运动速度其中为威胁的实际运动速度；其中：Γ_k(P)表示第k个障碍物的凸面体描述方程；

计算所有威胁的参考速度

构建流速为u(P)-v(P)的相对初始流场、以及流速为的相对修正流场；由于相对流场内的移动威胁可看作静止的，因此满足如下公式：

其中，M(P)表示修正矩阵；

构建威胁运动下的修正流场，其中为修正流场流速；

在步骤S6中，采用威胁运动下的修正流场构建得到静态障碍环境下的修正流场。

4.如权利要求1所述的水下航行器路径规划方法，其特征在于，对障碍物的排斥反应系数和切向反应系数、汇流流场权重系数、水流流场权重系数、进行迭代调整，以获得最优的水下航行器路径。