CN114459414A - 一种半潜式航行体的深度检测方法 - Google Patents

Abstract

本发明公开了一种半潜式航行体的深度检测方法，两个深度传感器分别采集艇艏深度数据和艇艉深度数据；巴特沃兹低通滤波器分别对艇艏深度数据和艇艉深度数据进行低通巴特沃兹滤波，消除艇艏深度数据和艇艉深度数据中的高频信号，得到艇艏低频深度测量值和艇艉低频深度测量值；深度融合滤波器对艇艏低频深度测量值和艇艉低频深度测量值进行融合，消除深度传感器安装位置所带来的误差信号，得到航行体深度估计值；一阶互补滤波器对航行体内置惯性导航设备采集的航行体垂向速度和航行体深度估计值进行融合，消除航行体运动带来的误差信号，得到航行体深度值；本发明能够消除航行体本身的测量误差，该方法简单可靠。

Description

一种半潜式航行体的深度检测方法

技术领域

本发明属于海洋机器人技术领域，具体涉及一种半潜式航行体的深度检测方法。

背景技术

半潜式航行体是一种可运行在水下一定深度的近水面机器人。与常规的水下机器人不同，该类型机器人下潜深度较浅，容易受到外部环境的干扰。机器人顶端配置有通讯桅杆，在航行时会对机器人的航行性能和流体特性产生影响，且由于顶端通讯桅杆的存在，机器人的下潜深度有很大的限制，对深度控制产生了很高的要求。高精度的深度控制需要特殊的深度采集方式，同时也要求采集的深度具有稳定、平滑的特点。在高海况的情况下，由于半潜式航行体的惯性较小，海浪波动会对深度传感器的测量造成较大的干扰，进而影响深度控制的效果。同时，由于航行体在近水面航行时表面受流作用不同，压力式的深度传感器在航行体表面的不同测点所测得的深度也会有所不同。

目前，国内外关于海浪滤波器的研究，多以单滤波器的研究为主。一部分针对海浪本身的频幅特点，采用低通滤波器、陷阱滤波器、卡尔曼滤波器等针对海浪的一阶高频特性进行滤波；另一个部分从机器人本身出发，通过分析机器人的动力学特征建立海浪扰动观测器，对海浪中与机器人波频相关的特征进行滤波。由于半潜式航行体是一个复杂的机器人系统且所处环境复杂多变，所以在在实际的工程实践中不可能采用参数复杂繁多、且计算量过于庞大的滤波器。

发明内容

有鉴于此，本发明提供了一种半潜式航行体的深度检测方法，是针对高海况条件下海洋机器人执行作业任务的一种简单可靠的深度检测方法，通过设置低通滤波器，并采取多深度传感器融合和结合垂向速度滤波的方法消除航行体本身的测量误差，该方法简单可靠。

本发明是通过下述技术方案实现的：

一种半潜式航行体的深度检测方法，具体步骤如下：

步骤一，将两个深度传感器分别安装在航行体的艇艏和艇艉，两个深度传感器分别采集艇艏深度数据和艇艉深度数据；

步骤二，构建两个相同的巴特沃兹低通滤波器，两个巴特沃兹低通滤波器分别对艇艏深度数据和艇艉深度数据进行低通巴特沃兹滤波，消除艇艏深度数据和艇艉深度数据中的高频信号，得到艇艏低频深度测量值和艇艉低频深度测量值；

步骤三，构建深度融合滤波器，深度融合滤波器采用极大似然估计的方法对艇艏低频深度测量值和艇艉低频深度测量值进行融合，消除深度传感器安装位置所带来的误差信号，得到航行体深度估计值；

步骤四，建一阶互补滤波器，一阶互补滤波器采用一阶互补滤波的方法对航行体内置惯性导航设备采集的航行体垂向速度和航行体深度估计值进行融合，消除航行体运动带来的误差信号，得到航行体深度值。

进一步的，在步骤二中，所述巴特沃兹低通滤波器的构建方法如下：

通过风速仪采集当前风速，根据当前风速情况下的当前波浪的一阶扰动频率fs确定巴特沃兹低通滤波器的阶数N和截止频率Ω_c，根据阶数N和截止频率Ω_c建立巴特沃兹低通滤波器的频域特征方程|H_a(Ω)|²，表达式如下：

式中，Ω为当前频率，为变量。

进一步的，在步骤三中，所述深度融合滤波器的时域特征的表达式如下：

式中，h_c为航行体深度估计值；h_f为艇艏低频深度测量值；h_b为艇艉低频深度测量值；

为艇艏低频深度测量值的方差；

为艇艉低频深度测量值的方差。

进一步的，在步骤四中，所述一阶互补滤波器的时域特征的表达式如下：

h_p(t)＝k(h_p(t-1)+dt*v_c(t))+(1-k)h_c(t) 公式(3)

式中，h_p为经一阶互补滤波器滤波后的航行体深度值；h_p(t)为t时刻航行体深度值；h_p(t-1)为t-1时刻航行体深度值；dt为深度传感器的深度测量间隔；v_c(t)为t时刻惯性导航设备采集的航行体垂向速度值；h_c(t)为t时刻的航行体深度估计值；k为一阶互补滤波器的调节参数。

进一步的，在步骤二中，根据当前风速情况下的当前波浪的一阶扰动频率fs确定巴特沃兹低通滤波器的阶数N和截止频率Ω_c的方法如下：

步骤2-1，通过航行体上安装的风速测量仪测量得到当前风速，根据当前风速确定当前海浪的峰值频率w，进而确定航行体的当前遭遇频率w_e，w_e的表达式如下：

式中，v为航行体的当前航速，γ为海浪的遭遇角，g为重力加速度；

步骤2-2，根据航行体的当前遭遇频率w_e，确定当前波浪的一阶扰动频率f_s，

步骤2-3，根据当前波浪的一阶扰动频率f_s，确定巴特沃兹低通滤波器的通带衰减a^p、阻带衰减a_S、通带频率Ω_p及通带频率Ω_s；

步骤2-4，根据通带频率Ω_p和通带频率Ω_s计算选择性因子k，

根据通带衰减a_p和阻带衰减a_S计算判别因子d，

步骤2-5，根据选择性因子k和判别因子d计算阶数N和截止频率Ω_c，计算公式如下：

N≥logd/logk 公式(5)

最终，根据公式(5)和公式(6)选择满足条件的巴特沃兹低通滤波器的阶数N和截止频率Ω_c。

有益效果：

(1)本发明首先通过安装在半潜式航行体艇艏和艇艉两端的压力式深度传感器测量半潜式航行体的艏艉的深度、通过安装在半潜式航行体内部的惯性导航设备测量航行体垂向速度，然后通过巴特沃兹低通滤波器滤除航行体艏艉测量深度中的高频信号得到低频的艏艉深度数据，接着通过极大似然估计的方法使得深度融合滤波器将艏艉深度数据进行融合处理，获取离散程度较小的航行体深度估计值，最后通过一阶互补滤波器联合航行体垂向速度对航行体深度估计值进行滤波，获取最终的航行体深度值，因此，本发明是一种简易、快速、通用的半潜式航行体深度检测方法，能够在计算力有限的情况下对深度进行简单快速的滤波，且设计参数简单、易于调节，便于实际工程使用，还能够有效地消除波浪高频影响以及航行体深度传感器的测量误差，使得检测得到的深度具有离散度低、平滑度高的特点，适用于高海况条件下的近水面的深度控制，即适用于采用多深度计采集深度，且配备惯性导航设备的海洋机器人，该方法也可以推广到其他具有多深度测量仪的海洋机器人系统中。

(2)本发明通过设置巴特沃兹低通滤波器消除海浪中的一阶高频波浪力对于测量深度值的影响，通过深度融合滤波器的信息融合和一阶互补滤波器的互补滤波消除深度传感器安装位置和半潜式航行体运动状态的影响，提高了高海况下半潜式航行体测量深度的抗干扰能力和精确度。

(3)本发明根据海浪中的一阶高频波浪干扰，根据不同风级下的海浪频率，确定巴特沃兹低通滤波器的阶数和截止频率，然后通过阶数和截止频率确定巴特沃兹低通滤波器，具有通带内最平坦的幅度特征，而且随着频率升高呈现单调递减，可依据风速动态调节巴特沃兹低通滤波器参数，提高巴特沃兹低通滤波器的滤波能力和效率。

(4)本发明由于高海况下半潜式航行体近水面航行条件下不同安装位置的深度传感器测量值有所不同，通过构建基于极大似然估计的深度融合滤波器，融合不同位置的深度测量值，消除安装位置对于测量精度的影响，且深度融合滤波器无需预先的参数设置，可根据艇艏深度传感器和艇艉深度传感器采集的深度数据的样本方差，将艇艏深度数据和艇艉深度数据的采集值进行信息互补，估计得出离散程度小且去除冗余信息的航行体深度估计值。

(5)本发明对于实际航行过程中航行体运动对于深度的影响，通过设置一阶互补滤波器，一阶互补滤波器可利用不同类型传感器的噪声特性，结合两种类型传感器的噪声特性进行结合滤波；对于惯导的垂向速度来说，由于其低频噪声的特性，在长时间积分情况下得到的垂向高度偏移会产生较大的积分偏移；而对于深度传感器来说，采集的深度值会具有很大的瞬时响应性，带来一定的高频特性，因此，本发明的一阶互补滤波器融合航行体深度估计值和航行体垂向速度，消除航行体运动状态对于测量深度的影响，最终得到稳定的航行体深度值。

附图说明

图1是本发明的流程图；

图2是5级风下的海浪波高图；

图3是本发明中艇艏深度传感器采集的艇艏深度数据图；

图4是本发明中艇艉深度传感器采集的艇艉深度数据图；

图5是本发明中巴特沃兹低通滤波器的幅频相频特征；

图6是本发明中艇艏巴特沃兹低通滤波器的输入深度及其频域图像；

图7是本发明中艇艏巴特沃兹低通滤波器的输出深度及其频域图像；

图8是本发明中艇艉巴特沃兹低通滤波器的输入深度及其频域图像；

图9是本发明中艇艉巴特沃兹低通滤波器的输出深度及其频域图像；

图10是本发明中通过深度融合滤波器融合后的深度图像；

图11是本发明中通过一阶互补滤波器后的深度图像。

具体实施方式

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

本实施例提供了一种半潜式航行体的深度检测方法，参见附图1，具体步骤如下：

步骤一，将两个深度传感器分别安装在航行体的艇艏和艇艉，两个深度传感器分别为艇艏深度传感器和艇艉深度传感器，艇艏深度传感器采集航行体的艇艏深度数据，艇艉深度传感器采集航行体的艇艉深度数据，如附图3-4所示；

步骤二，构建海浪低通滤波器，即构建两个相同的巴特沃兹低通滤波器，两个巴特沃兹低通滤波器分别为艇艏巴特沃兹低通滤波器和艇艉巴特沃兹低通滤波器，艇艏巴特沃兹低通滤波器对艇艏深度数据进行低通巴特沃兹滤波，消除艇艏深度数据中的高频信号，得到艇艏低频深度测量值，艇艉巴特沃兹低通滤波器对艇艉深度数据进行低通巴特沃兹滤波，消除艇艉深度数据中的高频信号，得到艇艉低频深度测量值，如附图6-9所示；

其中，所述巴特沃兹低通滤波器的构建方法如下：

通过风速仪采集当前风速，根据当前风速情况下的当前波浪的一阶扰动频率fs确定巴特沃兹低通滤波器的阶数N和截止频率Ω_c，根据阶数N和截止频率Ω_c建立巴特沃兹低通滤波器的频域特征方程|H_a(Ω)|²，表达式如下：

式中，Ω为当前频率，为变量；

根据所述频域特征方程构建巴特沃兹低通滤波器，完成巴特沃兹低通滤波器的构建，如附图5所示；

步骤三，构建深度融合滤波器，深度融合滤波器采用极大似然估计的方法对艇艏低频深度测量值和艇艉低频深度测量值进行融合，消除深度传感器安装位置所带来的误差信号，得到航行体深度估计值，以消除两个深度传感器安装位置的测量误差，如附图10所示；

其中，所述深度融合滤波器的时域特征方程的表达式如下：

式中，h_c为航行体深度估计值，单位为m；h_f为艇艏低频深度测量值，单位为m；h_b为艇艉低频深度测量值，单位为m；

为艇艏低频深度测量值的方差；

为艇艉低频深度测量值的方差；

该公式(2)是根据艇艏深度传感器和艇艉深度传感器采集的深度数据的样本方差，将艇艏深度数据和艇艉深度数据的采集值进行信息互补，估计得出离散程度小且去除冗余信息的航行体深度估计值。

步骤四，构建一阶互补滤波器，一阶互补滤波器采用一阶互补滤波的方法对航行体内置惯性导航设备采集的航行体垂向速度和航行体深度估计值进行融合，消除航行体运动带来的误差信号，得到航行体深度值，以对航行体深度估计值进行进一步的滤波，消除航行体本身运动对深度测量的干扰，如附图11所示；

其中，所述一阶互补滤波器的时域特征方程的表达式如下：

h_p(t)＝k(h_p(t-1)+dt*v_c(t))+(1-k)h_c(t) 公式(3)

式中，h_p为经一阶互补滤波器滤波后的航行体深度值，单位为m；h_p(t)为t时刻航行体深度值；h_p(t-1)为t-1时刻航行体深度值；dt为深度传感器的深度测量间隔，单位为s；v_c(t)为t时刻惯性导航设备采集的航行体垂向速度值(坐标系北东地)，单位为m/s；h_c(t)为t时刻的航行体深度估计值，单位为m；k为一阶互补滤波器的调节参数，用于调节航行体垂向速度和融合深度数据的比例关系，k越大，航行体垂向速度所占的比例越大，k越小，融合深度数据的深度值所占的比例越大，调节参数k可通过具体实验进行确定。

其中，在步骤二中，根据当前风速情况下的当前波浪的一阶扰动频率fs确定巴特沃兹低通滤波器的阶数N和截止频率Ω_c的方法如下：

步骤2-1，通过航行体上安装的风速测量仪测量得到当前风速，根据当前风速确定当前海浪的峰值频率w，进而确定航行体的当前遭遇频率w_e，w_e的表达式如下：

式中，v为航行体的当前航速，γ为海浪的遭遇角，g为重力加速度；

步骤2-2，根据航行体的当前遭遇频率w_e，确定当前波浪的一阶扰动频率f_s，

步骤2-3，根据当前波浪的一阶扰动频率f_s，确定巴特沃兹低通滤波器的通带衰减a_p、阻带衰减a_S、通带频率Ω_p及通带频率Ω_s；

步骤2-4，根据通带频率Ω_p和通带频率Ω_s计算选择性因子k，

根据通带衰减a_p和阻带衰减a_S计算判别因子d，

步骤2-5，根据选择性因子k和判别因子d计算阶数N和截止频率Ω_c，计算公式如下：

N≥logd/logk 公式(5)

最终，根据公式(5)和公式(6)选择满足条件的巴特沃兹低通滤波器的阶数N和截止频率Ω_c。

采取仿真实验的方式来验证本实施例中所提出的方法的正确性，通过PM谱的方法对波浪进行建模，设定半潜式航行体在海平面(海拔)以下3m的位置固定进行深度的采集，此时当前风速为5级，5级风下的海浪波高图参见附图2，附图3-4为5级风况下两个深度传感器采集的深度数据，附图5是巴特沃兹低通滤波器的幅频相频特征；附图6-9为经过巴特沃兹低通滤波器滤波后的低频深度测量值，附图10为经过深度融合滤波器融合后的航行体深度估计值，附图11为经过一阶互补滤波后输出的最终深度图像，由结果可知，本实施例中所提出方法处理的深度具有采集深度离散度低、平滑度高的特点，适用于高海况下的深度采集和处理。

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

Claims (5)

1.一种半潜式航行体的深度检测方法，其特征在于，具体步骤如下：

步骤一，将两个深度传感器分别安装在航行体的艇艏和艇艉，两个深度传感器分别采集艇艏深度数据和艇艉深度数据；

步骤二，构建两个相同的巴特沃兹低通滤波器，两个巴特沃兹低通滤波器分别对艇艏深度数据和艇艉深度数据进行低通巴特沃兹滤波，消除艇艏深度数据和艇艉深度数据中的高频信号，得到艇艏低频深度测量值和艇艉低频深度测量值；

步骤三，构建深度融合滤波器，深度融合滤波器采用极大似然估计的方法对艇艏低频深度测量值和艇艉低频深度测量值进行融合，消除深度传感器安装位置所带来的误差信号，得到航行体深度估计值；

步骤四，建一阶互补滤波器，一阶互补滤波器采用一阶互补滤波的方法对航行体内置惯性导航设备采集的航行体垂向速度和航行体深度估计值进行融合，消除航行体运动带来的误差信号，得到航行体深度值。

2.如权利要求1所述的一种半潜式航行体的深度检测方法，其特征在于，在步骤二中，所述巴特沃兹低通滤波器的构建方法如下：

通过风速仪采集当前风速，根据当前风速情况下的当前波浪的一阶扰动频率fs确定巴特沃兹低通滤波器的阶数N和截止频率Ω_c，根据阶数N和截止频率Ω_c建立巴特沃兹低通滤波器的频域特征方程|H_a(Ω)|²，表达式如下：

式中，Ω为当前频率，为变量。

3.如权利要求1或2所述的一种半潜式航行体的深度检测方法，其特征在于，在步骤三中，所述深度融合滤波器的时域特征的表达式如下：

式中，h_c为航行体深度估计值；h_f为艇艏低频深度测量值；h_b为艇艉低频深度测量值；

为艇艏低频深度测量值的方差；

为艇艉低频深度测量值的方差。

4.如权利要求3所述的一种半潜式航行体的深度检测方法，其特征在于，在步骤四中，所述一阶互补滤波器的时域特征的表达式如下：

h_p(t)＝k(h_p(t-1)+dt*v_c(t))+(1-k)h_c(t) 公式(3)

式中，h_p为经一阶互补滤波器滤波后的航行体深度值；h_p(t)为t时刻航行体深度值；h_p(t-1)为t-1时刻航行体深度值；dt为深度传感器的深度测量间隔；v_c(t)为t时刻惯性导航设备采集的航行体垂向速度值；h_c(t)为t时刻的航行体深度估计值；k为一阶互补滤波器的调节参数。

5.如权利要求2所述的一种半潜式航行体的深度检测方法，其特征在于，在步骤二中，根据当前风速情况下的当前波浪的一阶扰动频率fs确定巴特沃兹低通滤波器的阶数N和截止频率Ω_c的方法如下：

步骤2-1，通过航行体上安装的风速测量仪测量得到当前风速，根据当前风速确定当前海浪的峰值频率w，进而确定航行体的当前遭遇频率w_e，w_e的表达式如下：

式中，v为航行体的当前航速，γ为海浪的遭遇角，g为重力加速度；

步骤2-2，根据航行体的当前遭遇频率w_e，确定当前波浪的一阶扰动频率f_s，

步骤2-3，根据当前波浪的一阶扰动频率f_s，确定巴特沃兹低通滤波器的通带衰减a_p、阻带衰减a_S、通带频率Ω_p及通带频率Ω_s；

步骤2-4，根据通带频率Ω_p和通带频率Ω_s计算选择性因子k，

根据通带衰减a_p和阻带衰减a_S计算判别因子d，

步骤2-5，根据选择性因子k和判别因子d计算阶数N和截止频率Ω_c，计算公式如下：

N≥logd/logk 公式(5)

最终，根据公式(5)和公式(6)选择满足条件的巴特沃兹低通滤波器的阶数N和截止频率Ω_c。

Images