[发明专利]一种基于反馈融合结构的自主水下航行器控制行为融合方法

权利要求书

1.一种基于反馈融合结构的自主水下航行器控制行为融合方法，其特征在于：包括以下步骤：

步骤1：通过AUV上的前视声呐测得障碍物，并获得障碍物在AUV载体系的坐标；

步骤2：计算避障旋转角

其中k_b为避障比例系数，

为AUV受到的斥力的合力，采用公式

计算得到，其中k_f为比例系数，I为声呐总波束数量，d_i为第i个声呐波束探测到的距离，d₀为AUV的安全距离，d_m为声呐的最大探测距离，ψ_i为探测波束在AUV载体系的视线角，j为虚数符号；ψ_F为与AUV载体系ox轴的夹角；

步骤3：计算轨迹跟踪误差

ε(t)＝S(t)_j+1tan(δ_p(t))

其中S(t)_j+1为误差位置向量在轨迹向量上的投影长度，δ_p(t)为轨迹跟踪误差角；

误差位置向量轨迹向量(X_j+1,Y_j+1)和(X_j,Y_j)分别为AUV当前目标点Wp_j+1和上一个目标点Wp_j的坐标，(X,Y)为AUV当前位置坐标；

轨迹跟踪误差角δ_p(t)＝ψ_trk(j+1)-ψ_(j+1)，而

ψ_trk(j+1)＝-arctan2(Y_j+1-Y_j,X_j+1-X_j)，ψ_(j+1)＝-arctan2(Y_j+1-Y,X_j+1-X)；

步骤4：根据步骤3得到的轨迹跟踪误差，通过公式

ψ_Δ＝kp_e·ε(t)+ki_e∫ε(t)

计算轨迹跟踪旋转角ψ_Δ，其中kp_e和ki_e分别为比例系数和积分系数；

步骤5：根据AUV当前障碍的中的最短障碍距离d_min，按照公式

调整系数f₁和f₂，其中k₁，k₂，d₁，d₂，R_s为设定参数，且设定参数满足当d_min≥100时，有f₁＞f₂，当d_min＜100之后，有f₁＜f₂；

步骤6：根据公式计算融合得到最终的参考航向角ψ，其中ψ_ref是视线导引航向角。