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

摘要

本发明提出一种基于反馈融合结构的自主水下航行器控制行为融合方法，将AUV视线导引行为、轨迹跟踪行为和避障行为的输出相互关联，其中视线导引行为是主要的导引方法，轨迹跟踪行为和避障行为对视线导引行为一个修正，从而融合得到总的输出结果通过融合既能够保证航行器在距离障碍的安全距离之外，又能够偏离任务轨迹最少，融合结构刚好满足这个需求。

主权项

1.一种基于反馈融合结构的自主水下航行器控制行为融合方法，其特征在于：包括以下步骤：步骤1：通过AUV上的前视声呐测得障碍物，并获得障碍物在AUV载体系的坐标；步骤2：计算避障旋转角其中kb为避障比例系数，为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(Yj+1‑Yj,Xj+1‑Xj)，ψ(j+1)＝‑arctan2(Yj+1‑Y,Xj+1‑X)；步骤4：根据步骤3得到的轨迹跟踪误差，通过公式ψΔ＝kpe·ε(f)+kie∫ε(t)计算轨迹跟踪旋转角ψΔ，其中kpe和kie分别为比例系数和积分系数；步骤5：根据AUV当前障碍的中的最短障碍距离dmin，按照公式调整系数f1和f2，其中k1，k2，d1，d2，Rs为设定参数，且设定参数满足但dmin≥100时，有f1＞f2，当dmin＜100之后，有f1＜f2；步骤6：根据公式计算融合得到最终的参考航向角ψ，其中ψ_ref是视线导引航向角。