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

摘要

水下航行器路径规划算法，用于规划水下航行器的航行路径。该方法将水下障碍均近似等效为几何凸面体，建立以水下航行器航行目标点为中心的汇流场，构建自由环境下航行目标点的初始流场并引入水下障碍物的影响对初始流场进行校正，获得修正流场，并基于修正力促行获得水下航行器的路径点，从而获得航行路径。本发明规划的路径质量较高，水下航行器总能安全避开障碍物并最终到达目标点。为水下航行器自主控制系统搭载该路径规划算法，可进一步提高水下自主航行器的自主感知和决策控制能力，提高自主航行能力。

主权项

水下航行器路径规划算法，用于规划水下航行器的航行路径，其特征在于，包括以下步骤：S1：将水下障碍均近似等效为几何凸面体，采用统一的凸面体方程进行描述：其中，a,b,c,p,q,r为决定几何凸面体大小和形状的常数，(x0,y0,z0)表示几何凸面体的中心，Γ(P)＝1为障碍物表面方程；S2：建立以水下航行器航行目标点为中心的汇流场：定义该汇流场的速度为其中V0为汇流场的速率，d(P,Pd)表示水下航行器当前位置与航行目标位置的欧式距离；S3：构建自由环境下航行目标点的初始流场：假设水流流场为恒速流场，水流流速为uc(P)，将水流流速与汇流场流速按权重叠加，u(P)＝λsus(P)+λcuc(P)，其中u(P)为初始流场的流速，λs和λc分别表示汇流场和水流流场权重系数；S4：定义水下障碍物的修正矩阵，以定义水下障碍物对航行目标点初始流场的扰动影响：其中，K为障碍物的总数，ρk、σk分别表示第k个障碍物的排斥反应系数和切向反应系数，nk(P)表示第k个障碍物的径向法向量，tk(P)表示第k个障碍物的水平切向量：其中，nk(P)=∂Γk(P)∂x∂Γk(P)∂y∂Γk(P)∂zT,tk(P)=∂Γk(P)∂y-∂Γk(P)∂x0T;]]>S5：采用修正矩阵对初始流场进行修正，得到静态障碍环境下的修正流场：u‾(P)=M(P)u(P);]]>S6：获得水下航行器的下一路径点：其中ΔT为时间步长；S7：将各路径点依此连接，获得水下航行器的规划路径。