一种基于反馈融合结构的自主水下航行器控制行为融合方法技术

本发明专利技术提出一种基于反馈融合结构的自主水下航行器控制行为融合方法，将AUV视线导引行为、轨迹跟踪行为和避障行为的输出相互关联，其中视线导引行为是主要的导引方法，轨迹跟踪行为和避障行为对视线导引行为一个修正，从而融合得到总的输出结果

A fusion method for control behavior of autonomous underwater vehicles based on feedback fusion structure

A control behavior fusion method for Autonomous Underwater Vehicle Based on feedback fusion structure is proposed, which correlates the output of line-of-sight guidance behavior, trajectory tracking behavior and obstacle avoidance behavior of AUV. The line-of-sight guidance behavior is the main guidance method, and the trajectory tracking behavior and obstacle avoidance behavior modify the line-of-sight guidance behavior. So as to fuse the total output results.

【技术实现步骤摘要】
一种基于反馈融合结构的自主水下航行器控制行为融合方法
本专利技术涉及水下自动控制
，具体为一种基于反馈融合结构的自主水下航行器控制行为融合方法。
技术介绍
对于自主水下航行器(AUV)的单个行为来说，每一个基本行为是由控制系统和触发器两部分组成，其中控制系统能使被控对象按照特定方式运行，而触发器则决定了机器人应该合适进行相关的操作。为了能够同时实现多个目标，有时候被控对象需要执行很多的基本行为。当某个时刻只要仅有一个行为触发，系统都能够比较平稳地运行。然而当两个甚至更多的行为同时触发，并且每个行为都需要执行不同的操作时，控制系统将使用一定的仲裁方法，来对输出结果进行融合，最后产生一个综合的输出结果。建立AUV控制系统行为层的过程时，从AUV的性质和使命开始分析，可以确定控制系统行为层包含的具体行为，有视线导引行为、轨迹跟踪行为、避障行为、深度控制行为和速度控制行为。通过分析可以发现，轨迹跟踪行为和避障行为的作用是相互矛盾的，如果自主航行器航路任务上存在障碍，那么避障行为起作用的时候必然会造成AUV偏离任务航行轨迹，这个时候轨迹跟踪行为必然会起相反的作用将AUV拉回任务轨迹。而航行器需要的逻辑是，既能够保证航行器在距离障碍的安全距离之外，又能够偏离任务轨迹最少。在基于行为体系结构的基础上，根据多个行为产生动作的不同仲裁方法，研究人员又提出了更详细的分类方法。CarrerasM总结出了在AUV中应用的四种典型的基于行为的结构体系：包含结构，融合结构，动态选择结构和带反馈的融合结构。本专利技术就是一种带反馈的融合结构，是综合上述结构的优点选择的一种比较好的结构，带反馈的融合结构是根据环境反馈，修改融合各个行为输出结果加权和的每一个权值，得到一个可预测的期望结果，降低了不可预测的错误出现的几率，同时曲线的曲折也比较少，可以得到连续的曲线。
技术实现思路
行为层的行为有视线导引行为、轨迹跟踪行为、避障行为、深度控制行为和速度控制行为。其中深度控制行为和速度控制行为分别输出参考俯仰角和参考速度，它们的结果不需要进行融合，而另外三个行为的输出是相互关联的，它们之间需要融合得到总的输出结果ψ(参考航向角)：其中ψref是视线导引航向角，ψΔ是轨迹跟踪旋转角，是避障旋转角，f1，f2为调整系数。其中视线导引行为是主要的导引方法，轨迹跟踪行为和避障行为只是对视线导引行为做一个修正，因此直接设ψref的系数等于1，动态融合主要是调整f1和f2的关系。为实现上述目的，本专利技术的技术方案为：所述一种基于反馈融合结构的自主水下航行器控制行为融合方法，其特征在于：包括以下步骤：步骤1：通过AUV上的前视声呐测得障碍物，并获得障碍物在AUV载体系的坐标；步骤2：计算避障旋转角其中kb为避障比例系数，为AUV受到的斥力的合力，采用公式计算得到，其中kf为比例系数，I为声呐总波束数量，di为第i个声呐波束探测到的距离，d0为AUV的安全距离，dm为声呐的最大探测距离，ψi为探测波束在AUV载体系的视线角，j为虚数符号；与AUV载体系ox轴的夹角；步骤3：计算轨迹跟踪误差ε(t)＝S(t)j+1tan(δp(t))其中S(t)j+1为误差位置向量在轨迹向量上的投影长度，δp(t)为轨迹跟踪误差角；误差位置向量轨迹向量(Xj+1,Yj+1)和(Xj,Yj)分别为AUV当前目标点Wpj+1和上一个目标点Wpj的坐标，(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是视线导引航向角。有益效果本专利技术既能够保证航行器在距离障碍的安全距离之外，又能够偏离任务轨迹最少，融合结构刚好满足这个需求。本专利技术的附加方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本专利技术的实践了解到。附图说明本专利技术的上述和/或附加的方面和优点从结合下面附图对实施例的描述中将变得明显和容易理解，其中：图1：轨迹跟踪模型示意图；图2：行为融合仿真结果图。具体实施方式下面详细描述本专利技术的实施例，所述实施例是示例性的，旨在用于解释本专利技术，而不能理解为对本专利技术的限制。本实施例中的基于反馈融合结构的自主水下航行器控制行为融合方法包括以下步骤：步骤1：通过AUV上的前视声呐测得障碍物，并获得障碍物在AUV载体系的坐标。步骤2：通过以下过程计算避障旋转角用势场法计算AUV受到的斥力的合力：其中kf为比例系数，I为声呐总波束数量，di为第i个声呐波束探测到的距离，d0为AUV的安全距离，dm为声呐的最大探测距离，ψi为探测波束在AUV载体系的视线角，j为虚数符号；在AUV载体坐标系中，在oz轴上的投影为：在ox轴上的投影为：ψF为与AUV载体系ox轴的夹角；得到其中kb为避障比例系数。步骤3：计算轨迹跟踪误差。定义轨道跟踪误差(CTE：CrossTrackError)ε(t)为需要控制到最小的轨迹跟踪误差。ε(t)＝S(t)j+1tan(δp(t))其中S(t)j+1为误差位置向量在轨迹向量上的投影长度，δp(t)为轨迹跟踪误差角；误差位置向量轨迹向量(Xj+1,Yj+1)和(Xj,Yj)分别为AUV当前目标点Wpj+1和上一个目标点Wpj的坐标，(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：结合AUV导引弹道中的变提前角法，使用ε(t)计算轨迹跟踪旋转角ψΔ来修正ψref以达到消除ε(t)的目的。根据步骤3得到的轨迹跟踪误差，使用PI控制方法通过公式ψΔ＝kpe·ε(t)+kie∫ε(t)计算轨迹跟踪旋转角ψΔ，其中kpe和kie分别为比例系数和积分系数；步骤5：根据AUV当前障碍的中的最短障碍距离dmin，按照公式计算调整系数f1和f2，其中k1，k2，d1，d2，Rs为设定参数。在实际中将AUV靠近障碍的距离分为四个阶段：可见(Sight)，接近(Reach)，亲近(Close)，危险(Danger)，则有所以设定参数k1，k2，d1，d2，Rs满足但dmin≥100时，有f1＞f2，此时轨迹跟踪行为起主要作用；当dmin＜100之后，有f1＜f2，避障行为开始起作用；当dmin≤80之后，f1远小于f2，避障行为起主要作用，AUV将绕开障碍，直到前视声呐探测到没有障碍为止。步骤6：根据公式计算融合得到最终的参考航向角ψ，其中ψref是视线导引航向角。下面以TH50I-AUV预编本文档来自技高网...

【技术保护点】
1.一种基于反馈融合结构的自主水下航行器控制行为融合方法，其特征在于：包括以下步骤：步骤1：通过AUV上的前视声呐测得障碍物，并获得障碍物在AUV载体系的坐标；步骤2：计算避障旋转角

【技术特征摘要】
1.一种基于反馈融合结构的自主水下航行器控制行为融合方法，其特征在于：包括以下步骤：步骤1：通过AUV上的前视声呐测得障碍物，并获得障碍物在AUV载体系的坐标；步骤2：计算避障旋转角其中kb为避障比例系数，为AUV受到的斥力的合力，采用公式计算得到，其中kf为比例系数，I为声呐总波束数量，di为第i个声呐波束探测到的距离，d0为AUV的安全距离，dm为声呐的最大探测距离，ψi为探测波束在AUV载体系的视线角，j为虚数符号；ψF为与AUV载体系ox轴的夹角；步骤3：计算轨迹跟踪误差ε(t)＝S(t)j+1tan(δp(t))其中S(t)j+1为误差位置向量在轨迹向量上的投影长度，δp(t)为轨迹跟踪误差角；误差位置向量轨迹向量(Xj+1,Yj+1)和(Xj,Yj)分别为AUV当前目标点Wpj+1...

【专利技术属性】
技术研发人员：高剑，李步方，严卫生，张福斌，王鹏，崔荣鑫，
申请(专利权)人：西北工业大学，
类型：发明
国别省市：陕西,61