CN105785974B - 一种面向欠驱动自主水下航行器的航向容错控制系统 - Google Patents

Abstract

本发明公开了一种面向欠驱动自主水下航行器的航向容错控制系统。使用本发明能够实时的判断垂直舵是否正常工作，一旦垂直舵出现问题，立马切换控制模式，即：通过对安装在欠驱动自主水下航行器上的两个推进器施加不同的力，实现欠驱动自主水下航行器的自主行驶。

Description

一种面向欠驱动自主水下航行器的航向容错控制系统

技术领域

本发明涉及航向容错控制领域，具体涉及一种面向欠驱动自主水下航行器的航向容错控制系统。

背景技术

欠驱动自主水下航行器(Autonomous Underwater Vehicle，AUV)作为探寻海洋的一个重要设备，它的各方面得到广泛应用，它可以深入海底去获取信息，AUV还可以对水下设备进行维护和修理，在开发海底资源过程中替代人工运送所需要的设备，测量和进行现场监视等任务。它不仅在补给运输、勘探上发挥重要作用，还可以有效的完成布雷、扫雷、探测、水下捞救和潜艇救生等任务。但是由于缺少有效的监测和通讯手段，对其自主性和安全性提出了更高的要求。

垂直舵是控制欠驱动AUV航行方向的重要机构，因此对垂直舵的故障诊断也就成为AUV检测自身安全的重要方面。然而，在AUV执行任务过程中不能对垂直舵发生的故障进行有效及时的处理，AUV将无法正常进行航向控制，迫使AUV停止工作，进而导致AUV会因功能丧失而无法完成指定任务，甚至造成AUV丢失，带来难以估计的经济损失。目前尚未有一个有效的方法对AUV发生的故障进行及时有效的处理，

发明内容

有鉴于此，本发明提供了一种面向欠驱动自主水下航行器的航向容错控制系统，能够通过对推进器的控制，解决AUV因垂直舵故障而导致无法工作的问题。

一种面向欠驱动自主水下航行器的航向容错控制系统，作用在欠驱动自主水下航行器系统上，包括依次相连的第一减法器、航向控制选择模块、航向控制器、第二减法器、舵角控制器和舵机；所述航向控制选择模块的输出端还通过推进器差动补偿航向控制模块与欠驱动自主水下航行器系统的输入端连接；航向控制器的输出端和陀螺仪航向检测单元的输出端均与垂直舵故障诊断模块的输入端连接，垂直舵故障诊断模块的输出端与航向控制选择模块的输入端连接；舵机的输出端通过舵角检测单元与第二减法器的输入端连接；欠驱动自主水下航行器系统的输出端通过陀螺仪航向检测单元与第一减法器的输入端连接；陀螺仪航向检测单元还与垂直舵故障诊断模块的输入端连接；

所述第一减法器，用于将设定的期望航向角Ψ*与陀螺仪航向检测单元采集到的航向角Ψ进行差值运算，获得航向角差值ΔΨ，并作为控制变量输入至航向控制选择模块；

所述航向控制选择模块，用于根据垂直舵故障诊断模块的切换指令，实现不同的控制模式：在没有收到切换指令的初始情况下，航向控制选择模块用于将获得的航向角差值ΔΨ输出至航向控制器；航向控制选择模块一旦接收到由垂直舵故障诊断模块发送的切换指令，航向控制选择模块控制航向控制器停止工作，航向控制选择模块将获得航向角差值ΔΨ，作为控制变量输入至推进器差动补偿航向控制模块；

所述航向控制器，用于将航向角差值ΔΨ转换成舵机能够识别的舵角β*，并将舵角β*分别发送至垂直舵故障诊断模块和第二减法器的输入端；

所述第二减法器，用于将接收的舵角β*和舵角检测单元采集到的舵面偏转角度β进行差值计算，获得舵角差值Δβ并作为控制变量输入至舵角控制器；

所述舵角控制器，用于根据获得的舵角差值Δβ控制舵机产生舵面偏转；

所述舵角检测单元，用于实时采集舵机产生的舵面偏转角度β，并实时发送至第二减法器的输入端；

所述欠驱动自主水下航行器系统，在舵面偏转角度产生的水动力作用下，产生航向偏转；

所述陀螺仪航向检测单元，用于实时采集欠驱动自主水下航行器系统的航向角Ψ，并发送至第一减法器的输入端和垂直舵故障诊断模块；

所述垂直舵故障诊断模块，用于在接收到舵角β*后，将其转换成相应的航向角Ψ'，并与航向角Ψ进行差值运算，获得残差，并将获得的残差与设定的阈值范围进行比较，当残差处于设定的阈值范围，判定垂直舵正常，垂直舵故障诊断模块不发送切换指令至航向控制选择模块；否则，判定垂直舵异常，垂直舵故障诊断模块发送切换指令至航向控制选择模块；

所述推进器差动补偿航向控制模块，用于根据获得的航向角差值ΔΨ，获得作用在欠驱动自主水下航行器系统中所有推进器上的总推力变化量后，进而获得分别作用在每个推进器上的推力。

特别地，所述推进器差动补偿航向控制模块包括与航向控制选择模块依次相连的主推控制器和推力分配器；

所述主推控制器，用于将获得的航向角差值ΔΨ转换成欠驱动自主水下航行器系统中所有推进器上的总推力变化量；

所述推力分配器，用于对总推力变化量ΔF进行分配，相应的获得每个推进器的推力。

有益效果：

本发明利用航向控制选择模块，实时的判断垂直舵是否正常工作，一旦垂直舵出现问题，立马切换控制模式，即：通过对安装在欠驱动自主水下航行器上的两个推进器施加不同的力，实现欠驱动自主水下航行器的自主行驶。

附图说明

图1为欠驱动自主水下航行器的航向容错控制系统示意图。

图2为自主水下航行器垂直舵故障检测模块示意图。

图3为推进器差动补偿航向控制模块方框图。

具体实施方式

下面结合附图并举实施例，对本发明进行详细描述。

本发明提供了一种面向欠驱动自主水下航行器的航向容错控制系统，作用在欠驱动自主水下航行器系统上：本发明的主要思想在于：实时判别垂直舵是否正常工作，一旦垂直舵出现问题，立马切换控制模式，即：通过对推进器的控制，实现欠驱动自主水下航行器的航行。如图1所示，包括依次相连的第一减法器、航向控制选择模块、航向控制器、第二减法器、舵角控制器和舵机；所述航向控制选择模块的输出端还通过推进器差动补偿航向控制模块与欠驱动自主水下航行器系统的输入端连接；航向控制器的输出端和陀螺仪航向检测单元的输出端均与垂直舵故障诊断模块的输入端连接，垂直舵故障诊断模块的输出端与航向控制选择模块的输入端连接；舵机的输出端通过舵角检测单元与第二减法器的输入端连接；欠驱动自主水下航行器系统的输出端通过陀螺仪航向检测单元与第一减法器的输入端连接；陀螺仪航向检测单元还与垂直舵故障诊断模块的输入端连接；

所述第一减法器，用于将设定的期望航向角Ψ*与陀螺仪航向检测单元采集到的航向角Ψ进行差值运算，获得航向角差值ΔΨ，并作为控制变量输入至航向控制选择模块；

所述航向控制选择模块，用于根据垂直舵故障诊断模块的切换指令，实现不同的控制模式；航向控制选择模块包括两种模式：模式一：通过控制舵机，实现航向调整；模式二：通过控制推进器推力，实现航向调整。通过增加航向控制选择模块，欠驱动自主水下航行器具有了两种工作模式，实现了双重保障。

在没有收到切换指令的初始情况下，航向控制选择模块默认选择模式一；航向控制选择模块用于将获得的航向角差值ΔΨ输出至航向控制器；航向控制选择模块一旦接收到由垂直舵故障诊断模块发送的切换指令，航向控制选择模块则启动推进器差动补偿航向控制模块工作，航向控制器停止工作，航向控制选择模块将获得航向角差值ΔΨ，作为控制变量输入至推进器差动补偿航向控制模块；

所述航向控制器，根据公式(1)，用于将航向角差值ΔΨ转换成舵机能够识别的舵角β*，并将舵角β*分别发送至垂直舵故障诊断模块和第二减法器的输入端；

式中：k_I为比例系数，k_D为微分系数，k_I为积分系数，ψ^*为期望的航向角，ψ为陀螺仪航向角检测单元测量得到的实际航向角，Δψ_max为控制器设定输入的最大指令航向，Δβ^* _max为控制器设定的最大指令舵角，β^*(k)为k时刻的期望舵角，Δψ(k)为k时刻的航向角偏差，Δψ(k-1)为k-1时刻的航向角偏差，Δψ(k-2)为k-2时刻的航向角偏差。

所述第二减法器，用于将接收的舵角β*和舵角检测单元采集到的舵面偏转角度β进行差值计算，获得舵角差值Δβ并作为控制变量输入至舵角控制器；

所述舵角控制器，用于根据获得的舵角差值Δβ控制舵机产生舵面偏转；

所述舵角检测单元，用于实时采集舵机产生的舵面偏转角度β，并实时发送至第二减法器的输入端；

所述欠驱动自主水下航行器系统，在舵面偏转角度产生的水动力作用下，产生航向偏转；

所述陀螺仪航向检测单元，用于实时采集欠驱动自主水下航行器系统的航向角Ψ，并发送至第一减法器的输入端和垂直舵故障诊断模块；

所述垂直舵故障诊断模块，如图2所示，用于在接收到舵角β*后，将其转换成相应的航向角Ψ'，并与航向角Ψ进行差值运算，获得残差e，共连续采集n次；若每次获得的残差值均处于设定的阈值范围内，当残差处于设定的阈值范围，判定垂直舵正常，垂直舵故障诊断模块不发送切换指令至航向控制选择模块；否则，判定垂直舵异常，垂直舵故障诊断模块发送切换指令至航向控制选择模块；通常情况下，所述阈值范围设为根据不同的舵机而设定。一般设为航向角Ψ”的50％至80％。

所述推进器差动补偿航向控制模块，用于根据获得的航向角差值ΔΨ，获得作用在欠驱动自主水下航行器系统中所有推进器上的总推力变化量后，进而获得分别作用在每个推进器上的推力变化量。

所述推进器差动补偿航向控制模块包括与航向控制选择模块依次相连的主推控制器和推力分配器；

所述主推控制器，用于根据公式(2)，将获得的航向角差值ΔΨ转换成欠驱动自主水下航行器系统中所有推进器上的总推力变化量；

将航向角差值ΔΨ转换成总推力变化量ΔF，其中，P₁为比例系数、P₂为缩放系数，Δψ(k)为k时刻的航向角偏差，ΔF(k)为k时刻控制器输出的总推力变化量，ΔF_max为控制器设定的最大指令分配推力。

所述推力分配器，根据公式(3)和(4)，用于对总推力变化量ΔF进行分配，相应的获得每个推进器的推力。

当：Δψ(k)＞0时：

F₁(k)＝F₁(k-1)+ΔF(k)

F₂(k)＝F₂(k-1)-ΔF(k) (3)

当Δψ(k)＜0时：

F₁(k)＝F₁(k-1)-ΔF(k)

F₂(k)＝F₂(k-1)+ΔF(k) (4)

其中，F₁(k)为k时刻左推进器的推力，F₁(k-1)为k-1时刻左推进器的推力，F₂(k)为k时刻右推进器的推力，F₂(k-1)为k-1时刻右推进器的推力。

获得每个推进器的相应推力，并分别作用于欠驱动自主水下航行器的推进器，使欠驱动自主水下航行器正常行驶。

综上所述，以上仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (2)

1.一种面向欠驱动自主水下航行器的航向容错控制系统，作用在欠驱动自主水下航行器系统上，其特征在于，包括依次相连的第一减法器、航向控制选择模块、航向控制器、第二减法器、舵角控制器和舵机；所述航向控制选择模块的输出端还通过推进器差动补偿航向控制模块与欠驱动自主水下航行器系统的输入端连接；航向控制器的输出端和陀螺仪航向检测单元的输出端均与垂直舵故障诊断模块的输入端连接，垂直舵故障诊断模块的输出端与航向控制选择模块的输入端连接；舵机的输出端通过舵角检测单元与第二减法器的输入端连接；欠驱动自主水下航行器系统的输出端通过陀螺仪航向检测单元与第一减法器的输入端连接；陀螺仪航向检测单元还与垂直舵故障诊断模块的输入端连接；

所述第一减法器，用于将设定的期望航向角Ψ^*与陀螺仪航向检测单元采集到的航向角Ψ进行差值运算，获得航向角差值ΔΨ，并作为控制变量输入至航向控制选择模块；

所述航向控制选择模块，用于根据垂直舵故障诊断模块的切换指令，实现不同的控制模式：在没有收到切换指令的初始情况下，航向控制选择模块用于将获得的航向角差值ΔΨ输出至航向控制器；航向控制选择模块一旦接收到由垂直舵故障诊断模块发送的切换指令，航向控制选择模块控制航向控制器停止工作，航向控制选择模块将获得航向角差值ΔΨ，作为控制变量输入至推进器差动补偿航向控制模块；

所述航向控制器，用于将航向角差值ΔΨ转换成舵机能够识别的舵角β^*，并将舵角β^*分别发送至垂直舵故障诊断模块和第二减法器的输入端；

所述第二减法器，用于将接收的舵角β^*和舵角检测单元采集到的舵面偏转角度β进行差值计算，获得舵角差值Δβ并作为控制变量输入至舵角控制器；

所述舵角控制器，用于根据获得的舵角差值Δβ控制舵机产生舵面偏转；

所述舵角检测单元，用于实时采集舵机产生的舵面偏转角度β，并实时发送至第二减法器的输入端；

所述欠驱动自主水下航行器系统，在舵面偏转产生的水动力作用下，产生航向偏转；

所述陀螺仪航向检测单元，用于实时采集欠驱动自主水下航行器系统的航向角Ψ，并发送至第一减法器的输入端和垂直舵故障诊断模块；

所述垂直舵故障诊断模块，用于在接收到舵角β^*后，将其转换成相应的航向角Ψ'，并与航向角Ψ进行差值运算，获得残差，并将获得的残差与设定的阈值范围进行比较，当残差处于设定的阈值范围，判定垂直舵正常，垂直舵故障诊断模块不发送切换指令至航向控制选择模块；否则，判定垂直舵异常，垂直舵故障诊断模块发送切换指令至航向控制选择模块；

所述推进器差动补偿航向控制模块，用于根据获得的航向角差值ΔΨ，获得作用在欠驱动自主水下航行器系统中所有推进器上的总推力变化量后，进而获得分别作用在每个推进器上的推力。

2.如权利要求1所述的系统，其特征在于：所述推进器差动补偿航向控制模块包括与航向控制选择模块依次相连的主推控制器和推力分配器；

所述主推控制器，用于将获得的航向角差值ΔΨ转换成欠驱动自主水下航行器系统中所有推进器上的总推力变化量；

所述推力分配器，用于对总推力变化量ΔF进行分配，相应的获得每个推进器的推力。