CN104679008B - 一种auv自主寻底控制方法 - Google Patents

Abstract

本发明公开一种AUV自主寻底控制方法，该发明适用于水下无人无缆潜器（AUV）在下潜过程中的寻底控制。该发明将载体下潜过程分解为无动力下潜和动力下潜两个阶段，并通过多个传感器数据的融合，实现载体安全到达在预定的高度并顺利进入探测作业。本发明操作简单，安全实用，可有效实现AUV在下潜过程中的寻底任务，提高了AUV***实际应用的可靠性。

Description

一种AUV自主寻底控制方法

技术领域

本发明涉及水下机器人技术领域，尤其涉及一种AUV自主寻底控制方法。

背景技术

AUV在水下执行任务时一般要经历下潜、海底航行作业、抛载上浮等三个主要工作阶段，本发明主要针对AUV的下潜工作过程。通常情况下，AUV从海面以45°到50°的倾角无动力下潜，当到达一定深度以后，依靠测距声纳获取的参数来判定是否到达预定高度，如果到达便抛载下潜压铁，并以定高模式航行作业。由于深度计在水下的误差随深度的增加而加大，预定抛载深度与实际抛载深度存在一定偏差，同时测距声纳在实际工作过程中作用距离有限且容易产生虚警，这有可能造成载体近海底抛载或者远离海底抛载，前者对载体而言是相当危险的，后者则造成载体寻底任务失败。因此，传统的寻底方法其安全性和可靠性均得不到有效地保障。

发明内容

为解决现有技术中存在的问题，本发明要解决的问题是提供一种AUV自主寻底控制方法，使得AUV能够安全、稳定、可靠地完成下潜寻底过程。

本发明为实现上述目的所采用的技术方案是：一种AUV自主寻底控制方法，包括以下步骤：

AUV无动力下潜至预定深度，并抛载下潜压铁；

AUV在预定深度悬停，根据姿态传感器的信息调整AUV姿态；

AUV从预定深度开始进行垂直寻底操作；

AUV到达预定高度后，开始定高航行。

所述AUV无动力下潜过程中，根据深度传感器判断是否到达预定深度。

所述AUV在预定深度根据姿态反馈信息确定悬停时间。

所述AUV的垂向速度由深度偏差微分及垂向加速度信息积分来推算。

所述垂直寻底操作采用自适应滤波算法，具体为：

离散***的状态方程为：

s_k＝As_k+Bu_k+w_k （2）

z_k＝Hs_k+v_k

初始化：

P_0|0＝10^-6*I

预测：

P_k|k-1＝AP_k-1|k-1A^T+BQ_k-1B^T

更新：

K_k＝P_k|k-1H^T(HP_k|k-1H^T+R_k)^-1

P_k|k＝(I-K_k)HP_k|k-1

其中，s_k＝h_k，表示k时刻载体所在的对底高度，其初始化由深度信息结合实地海深估算得到；u_k＝V_kΔt，V_k表示k时刻载体的垂向速度，由加速度积分得到，Δt表示速度间隔时间；Q_k＝diag(w_k ²)，R_k＝diag(v_k ²)，I是单位矩阵；z_k是k时刻多普勒的测底高度值；w_k是过程驱动噪声，v_k为观测噪声，Q_k、R_k分别为过程噪声和观测噪声的协方差矩阵。

在AUV自主寻底过程中，各个传感器对AUV的状态进行监控，其报警过程包括状态监测机制，即

其中v₀是载体动力下潜时的垂向速度，σ_k是载体离底高度的估计量偏差，h_k是k时刻多普勒的测底距离值，h_k-1是k-1时刻多普勒的测底距离值，是k时刻测底距离值的估计值，Δt是两次测底之间的时间间隔，如果（1）式成立，则传感器产生的报警有效，AUV抛载上浮；否则，视为虚警。

本发明具有以下优点及有益效果：

1.过程简洁、便于执行。本发明中AUV先实现定深然后再自主寻底，将原来AUV下潜的过程进行分段控制，过程简单、逻辑清晰便于工程应用。

2.安全稳定。本发明在AUV自主寻底过程中，从离海底较远处开始以悬停模式垂直寻底，同时实时监测、判别底跟踪高度信息的有效性，从而保证了AUV寻底过程的安全稳定。

3.可靠性高。本发明规避了测距声纳使用过程中虚警问题的影响，提高了AUV***实际应用的可靠性。

附图说明

图1是本发明的寻底过程示意图；

图2是本发明中载体的寻底传感器组成示意图；

图3是本发明的自主寻底流程图。

具体实施方式

下面结合附图及实施例对本发明做进一步的详细说明。

本发明主要针对AUV从无动力下潜、动力下潜到定高航行作业这一阶段进行自主寻底控制，如附图1所示。AUV寻底过程中所用到的控制计算机和传感器如附图2所示，主要由自动驾驶计算机、姿态传感器(TCM5)、多普勒传感器(DVL)、加速度传感器、深度传感器以及抛载装置组成。

AUV安装一台自动驾驶计算机，用于实时监测AUV***姿态、垂向速度、深度、高度等状态信息，推算AUV自身位置，控制与调度AUV下潜工作过程；AUV安装有姿态传感器（TCM5），用于周期性测量***姿态数据；AUV安装有深度计、加速度计、多普勒和抛载机构，用于周期性采集***各种状态信息。自动驾驶计算机依据多传感器数据融合结果，控制抛载机构执行抛载动作。

本发明将载体下潜过程细分为无动力下潜和动力下潜两个阶段，无动力下潜阶段主要依据深度参数判定是否到达抛载深度，动力下潜阶段完成自主寻底工作，并在寻底下潜过程中，依据多传感器数据的融合，采用本发明状态监测及自适应滤波算法，使载体安全可靠地到达预定高度，顺利进入下一阶段探测作业。

采用本发明设计的状态监测机制作用于***整个下潜过程，对AUV下潜时DVL***的底跟踪信息以及***姿态信息进行综合判断，当报警信息有效时AUV抛载上浮，否则视为虚警。

采用本发明设计的自适应滤波算法作用于***下潜过程中的动力下潜阶段，控制AUV完成稳定高效的寻底过程。

所述的状态监测机制为：

其中v₀是载体动力下潜时的垂向速度，σ_k是载体离底高度的估计量偏差，如果(1)式成立，则该报警有效；否则，视为虚警。

所述的自适应滤波算法为：

设离散***的状态方程如下

s_k＝As_k+Bu_k+w_k （2）

z_k＝Hs_k+v_k

其中

s_k＝h_k，表示k时刻载体所在的对底高度；u_k＝V_kΔt，V_k表示k时刻载体的垂向速度，Δt表示速度采样时间；z_k是k时刻传感器的测底距离；w_k是过程驱动噪声，v_k为观测噪声。

设Q_k、R_k分别为过程噪声和观测噪声的协方差矩阵，采用如下滤波算法对AUV的高度值进行滤波：

1)初始化

P_0|0＝10^-6*I

2)预测

P_k|k-1＝AP_k-1|k-1A^T+BQ_k-1B^T

3)更新

K_k＝P_k|k-1H^T(HP_k|k-1H^T+R_k)^-1

P_k|k＝(I-K_k)HP_k|k-1

其中s_k的初始化由深度计信息结合实地海深推算得到，Q_k＝diag(w_k ²)，R_k＝diag(v_k ²)，I是单位矩阵。

本发明的使用操作步骤为：

1.首先预置载体在下潜至离地高度为200m时抛载下潜压铁，此过程以深度计输出信息为准；

2.当载体到达深度计显示的预定深度并抛载以后，便切换至定深悬停工作模式；

3.定深调整待载体姿态平稳以后，切换至自适应悬停工作模式开始寻底；

4.依靠DVL寻底探测功能，同时辅以加速度计和TCM5等输出信息监控载体下潜，通过自适应滤波算法对各信息进行融合并作出相应的控制决策；

5.当载体到达期望的离底高度（80米），便进入定高航行模式并开始探测作业。

Claims (5)

1.一种AUV自主寻底控制方法，其特征在于，包括以下步骤：

AUV无动力下潜至预定深度，并抛载下潜压铁；

AUV在预定深度悬停，根据姿态传感器的信息调整AUV姿态；

AUV从预定深度开始进行垂直寻底操作；

AUV到达预定高度后，开始定高航行；

所述垂直寻底操作采用自适应滤波算法，具体为：

离散***的状态方程为：

$\begin{array}{c}{s}_k={\mathrm{As}}_k+{\mathrm{Bu}}_k+w_k\\ z_k={\mathrm{Hs}}_k+v_k\end{array}---\left(2\right)$

初始化：

$s_{0|0}={\stackrel{\‾}{h}}_0$

P_0|0＝10^-6*I

预测：

${\hat{s}}_{k|k-1}=A{\hat{s}}_{k-1|k-1}+{\mathrm{Bu}}_k$

P_k|k-1＝AP_k-1|k-1A^T+BQ_k-1B^T

更新：

K_k＝P_k|k-1H^T(HP_k|k-1H^T+R_k)^-1

${\hat{s}}_{k|k}={\hat{s}}_{k|k-1}+K_k\[z_k-H{\hat{s}}_{k|k-1}\]$

P_k|k＝(I-K_k)HP_k|k-1

其中，s_k＝h_k，表示k时刻载体所在的对底高度，其初始化由深度信息结合实地海深估算得到；u_k＝V_kΔt，V_k表示k时刻载体的垂向速度，由加速度积分得到，Δt表示速度间隔时间；Q_k＝diag(w_k ²)，R_k＝diag(v_k ²)，I是单位矩阵；z_k是k时刻多普勒的测底高度值；w_k是过程驱动噪声，v_k为观测噪声，Q_k、R_k分别为过程噪声和观测噪声的协方差矩阵。

2.根据权利要求1所述的一种AUV自主寻底控制方法，其特征在于，所述AUV无动力下潜过程中，根据深度传感器判断是否到达预定深度。

3.根据权利要求1所述的一种AUV自主寻底控制方法，其特征在于，所述AUV在预定深度根据姿态反馈信息确定悬停时间。

4.根据权利要求1所述的一种AUV自主寻底控制方法，其特征在于，所述AUV的垂向速度由深度偏差微分及垂向加速度信息积分来推算。

5.根据权利要求1所述的一种AUV自主寻底控制方法，其特征在于，在AUV自主寻底过程中，各个传感器对AUV的状态进行监控，其报警过程包括状态监测机制，即

$\begin{array}{c}{\hat{h}}_k=h_{k-1}+v_0\mathrm{\&Delta;}t\\ |h_k-{\hat{h}}_k|<{\mathrm{\&sigma;}}_k\end{array}---\left(1\right)$

其中v₀是载体动力下潜时的垂向速度，σ_k是载体离底高度的估计量偏差，h_k是k时刻多普勒的测底距离值，h_k-1是k-1时刻多普勒的测底距离值，是k时刻测底距离值的估计值，Δt是两次测底之间的时间间隔，如果(1)式成立，则传感器产生的报警有效，AUV抛载上浮；否则，视为虚警。