CN108490968A - 一种基于反馈融合结构的自主水下航行器控制行为融合方法 - Google Patents

Abstract

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

Description

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

技术领域

本发明涉及水下自动控制技术领域，具体为一种基于反馈融合结构的自主水下航行器控制行为融合方法。

背景技术

对于自主水下航行器(AUV)的单个行为来说，每一个基本行为是由控制系统和触发器两部分组成，其中控制系统能使被控对象按照特定方式运行，而触发器则决定了机器人应该合适进行相关的操作。为了能够同时实现多个目标，有时候被控对象需要执行很多的基本行为。当某个时刻只要仅有一个行为触发，系统都能够比较平稳地运行。然而当两个甚至更多的行为同时触发，并且每个行为都需要执行不同的操作时，控制系统将使用一定的仲裁方法，来对输出结果进行融合，最后产生一个综合的输出结果。

建立AUV控制系统行为层的过程时，从AUV的性质和使命开始分析，可以确定控制系统行为层包含的具体行为，有视线导引行为、轨迹跟踪行为、避障行为、深度控制行为和速度控制行为。

通过分析可以发现，轨迹跟踪行为和避障行为的作用是相互矛盾的，如果自主航行器航路任务上存在障碍，那么避障行为起作用的时候必然会造成AUV偏离任务航行轨迹，这个时候轨迹跟踪行为必然会起相反的作用将AUV拉回任务轨迹。而航行器需要的逻辑是，既能够保证航行器在距离障碍的安全距离之外，又能够偏离任务轨迹最少。

在基于行为体系结构的基础上，根据多个行为产生动作的不同仲裁方法，研究人员又提出了更详细的分类方法。Carreras M总结出了在AUV中应用的四种典型的基于行为的结构体系：包含结构，融合结构，动态选择结构和带反馈的融合结构。

本发明就是一种带反馈的融合结构，是综合上述结构的优点选择的一种比较好的结构，带反馈的融合结构是根据环境反馈，修改融合各个行为输出结果加权和的每一个权值，得到一个可预测的期望结果，降低了不可预测的错误出现的几率，同时曲线的曲折也比较少，可以得到连续的曲线。

发明内容

行为层的行为有视线导引行为、轨迹跟踪行为、避障行为、深度控制行为和速度控制行为。其中深度控制行为和速度控制行为分别输出参考俯仰角和参考速度，它们的结果不需要进行融合，而另外三个行为的输出是相互关联的，它们之间需要融合得到总的输出结果ψ(参考航向角)：

其中ψ_ref是视线导引航向角，ψ_Δ是轨迹跟踪旋转角，是避障旋转角，f₁，f₂为调整系数。其中视线导引行为是主要的导引方法，轨迹跟踪行为和避障行为只是对视线导引行为做一个修正，因此直接设ψ_ref的系数等于1，动态融合主要是调整f₁和f₂的关系。

为实现上述目的，本发明的技术方案为：

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

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

步骤2：计算避障旋转角

其中k_b为避障比例系数，

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

计算得到，其中k_f为比例系数，I为声呐总波束数量，d_i为第i个声呐波束探测到的距离，d₀为AUV的安全距离，d_m为声呐的最大探测距离，ψ_i为探测波束在AUV载体系的视线角，j为虚数符号；与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·ε(f)+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是视线导引航向角。

有益效果

本发明既能够保证航行器在距离障碍的安全距离之外，又能够偏离任务轨迹最少，融合结构刚好满足这个需求。

本发明的附加方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。

附图说明

本发明的上述和/或附加的方面和优点从结合下面附图对实施例的描述中将变得明显和容易理解，其中：

图1：轨迹跟踪模型示意图；

图2：行为融合仿真结果图。

具体实施方式

下面详细描述本发明的实施例，所述实施例是示例性的，旨在用于解释本发明，而不能理解为对本发明的限制。

本实施例中的基于反馈融合结构的自主水下航行器控制行为融合方法包括以下步骤：

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

步骤2：通过以下过程计算避障旋转角

用势场法计算AUV受到的斥力的合力：

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

在AUV载体坐标系中，在oz轴上的投影为：

在ox轴上的投影为：

ψ_F为与AUV载体系ox轴的夹角；得到

其中k_b为避障比例系数。

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

定义轨道跟踪误差(CTE：Cross Track Error)ε(t)为需要控制到最小的轨迹跟踪误差。

ε(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：结合AUV导引弹道中的变提前角法，使用ε(t)计算轨迹跟踪旋转角ψΔ来修正ψ_ref以达到消除ε(t)的目的。根据步骤3得到的轨迹跟踪误差，使用PI控制方法通过公式

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

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

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

计算调整系数f₁和f₂，其中k₁，k₂，d₁，d₂，R_s为设定参数。

在实际中将AUV靠近障碍的距离分为四个阶段：可见(Sight)，接近(Reach)，亲近(Close)，危险(Danger)，则有

所以设定参数k₁，k₂，d₁，d₂，R_s满足但d_min≥100时，有f₁＞f₂，此时轨迹跟踪行为起主要作用；当d_min＜100之后，有f₁＜f₂，避障行为开始起作用；当d_min≤80之后，f₁远小于f₂，避障行为起主要作用，AUV将绕开障碍，直到前视声呐探测到没有障碍为止。

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

下面以TH50I-AUV预编程型AUV为具体实施例描述本发明：

设定AUV初始点为wp₀(0,0,0)，初始位置为在水面的零点。

设定三个任务航路点：

wp₁(2,-5,600)，表示在到达目标点的路径上，要求航行器的深度为水下5米，目标点的坐标为(2,600)。

wp₂(800,-5,602)，要求航行器航行的深度为水下5米，目标点的坐标为(800,602)。

wp₃(790,-10,260)，要求航行器航行的深度为水下10米，目标点的坐标为(790,260)。

三个障碍的坐标分别为：

O₁(1,-5,280)，直径为25米，

O₂(300,-5,605)，直径为30米，

O₃(785,-10,450)，直径为20米。

得到一组航行结果。

对比仿真结果，在最基本的航路点导引上，航行器能够按照设定航路点完成任务，对轨迹的跟随方面也能够达到要求。对于分别处在航线正中间、偏左和偏右的三个障碍都能够做到绕开并继续航行。

尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在不脱离本发明的原理和宗旨的情况下在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。

Claims (1)

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·ε(f)+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是视线导引航向角。