CN113640780B - 基于改进的联邦滤波的水下auv传感器时间配准方法 - Google Patents

Abstract

本发明公开一种基于改进的联邦滤波的水下AUV传感器时间配准方法，包括：S1、构建测距声纳的测距模型，获得声纳数据；S2、获取配准周期内姿态传感器数据，作为第一姿态数据，对所述第一姿态数据进行预处理，获得处理后的配准周期内姿态传感器数据，作为第二姿态数据，对所述第二姿态数据进行滤波处理，同时对所述第二姿态数据和所述声纳数据进行融合修正处理，获得i个子滤波器数据；S3、对所述i个子滤波器数据进行最优融合估计处理获得子滤波的误差协方差矩阵，对所述误差协方差矩阵进行更新，完成水下AUV传感器时间配准。本申请降低了噪声干扰，提高了准确度。

Description

基于改进的联邦滤波的水下AUV传感器时间配准方法

技术领域

本发明涉及建筑结构裂缝检测领域，特别是涉及一种基于改进的联邦滤波的水下AUV传感器时间配准方法。

背景技术

近几十年来，随着海洋石油开发工程、水库大坝工程、港口码头以及桥梁工程的不断深入和发展，形成了一系列水下开发结构物的新模式。混凝土作为目前用量最大的一种建筑材料，现已被广泛应用在这些工程中。然而混凝土结构在长期服役过程中，都面临着耐久性不良的严重问题，尤其是处于水下的部分，由于外部环境(如风浪、腐蚀、水力冲刷以及温度应力等)的侵扰、材料性能的改变以及施工条件的限制等诸多因素的影响，不可避免的会产生不同类型的缺陷。其中，裂缝是影响结构耐久性最为严重和普遍的病害之一。传统对水面下坝体的检测只是依靠潜水员携带部分便携式检测仪器进行检测，不仅工作效率低，甚至威胁工作人员的生命安全。

随着水下机器人技术的快速发展，可利用水下机器人作为载体，搭载水下摄像机和超声纵波检测装置，对水下混凝土建筑结构裂缝进行检测。检测过程中要求水下机器人距离水下混泥土结构物壁面一定距离稳定运行，常用测距声纳实现这一功能，由于机器人的姿态信息对测距声纳的量测距离有很大的影响，需要结合姿态信息对声纳数据进行修正。由于声纳与姿态传感器的采样频率不一样，使用姿态数据对声纳数据进行修正前，势必要进行传感器的时间配准，目前常用的时间配准方法有泰勒展开修正法、最小二乘法、内插外插发、最大熵准则法等。其中泰勒展开法要求采样的间隔必须相等，且算法存在近似计算，配准精度不高；最小二乘法则对多传感器数据进行虚拟融合，但是采样点必须严格的对应，起始采样点必须相同且配准精度较低；运用最大熵准则和最小均方估计进行时间配准时对量测信号的统计特性和不同量测之间相关度有要求，并且精度较低。

本发明针对现有的时间配准算法存在的配准精度低、只能在限定条件下使用，且抗干扰能力弱等不足，提出一种基于改进的联邦滤波的水下AUV传感器时间配准方法。

发明内容

本发明的目的是提供一种基于改进的联邦滤波的水下AUV传感器时间配准方法，以解决上述现有技术存在的问题，使提高配准精度、增强抗干扰能力。

为实现上述目的，本发明提供了如下方案：本发明提供一种基于改进的联邦滤波的水下AUV传感器时间配准方法，具体包括步骤为，

S1、构建测距声纳的测距模型，获得声纳数据；

S2、获取配准周期内姿态传感器数据，作为第一姿态数据，对所述第一姿态数据进行预处理，获得处理后的配准周期内姿态传感器数据，作为第二姿态数据，对所述第二姿态数据进行滤波处理，同时对所述第二姿态数据和所述声纳数据进行融合修正处理，获得i个子滤波器数据；

S3、对所述i个子滤波器数据进行最优融合估计处理获得子滤波的误差协方差矩阵，对所述误差协方差矩阵进行更新，完成水下AUV传感器时间配准。

可选地，构建测距声纳的测距模型过程为：

构建AUV运动方程；

式中，AUV前向运动速度为V，AUV沿着结构物壁面运动，x_k、y_k为AUV在k时刻中心点的位置信息，α为AUV的艏向角度与结构物壁面的夹角，θ为AUV的横摇角度；

将所述AUV运动方程代入所述声呐距离量测模型得到k时刻前左和后左测距声呐距离量测模型。

可选地，所述S2中第一姿态数据预处理的过程为：首先将所述第一姿态数据进行i等分，再进行平均值计算，进而获得第二姿态数据，预处理过程结束。

可选地，所述S2中滤波处理为采用改进的联邦卡尔曼滤波对第二姿态数据进行处理。

可选地，所述S2中所述融合修正处理的过程包括建立所述i个子滤波器的状态方程和量测方程。

可选地，建立所述子滤波器的状态方程包括：

确定i个子滤波器的状态量为：

其中：

式中d₁、d₂分别为前左测距声纳和后左测距声纳的量测距离，d为AUV距离结构物壁面的实际距离，α为AUV的艏向角度与结构物壁面的夹角，θ为AUV的横摇角度，R₁、R₂分别为AUV在声呐位置处的半径和浮心位置处的半径；

i个子滤波器状态方程为：

其中ω_k＝[ω_1，k-1 ω_2，k-1 ω_3，k-1 ω_4，k-1 ω_5，k-1]^T是系统的过程噪声，为高斯白噪声，其方差为Q；即子滤波器状态方程简写为：

X_k＝f(X_k-1，ω_k-1)＝A_k-1X_k-1+ω_k-1

其中A_k-1为k-1时刻，运动模型中状态预测函数f关于状态的Jacobian矩阵：

则第i个子滤波器的状态方程为：

Xⁱ _k＝f(Xⁱ _k-1，ω_k+1)＝Aⁱ _k-1Xⁱ _k-1+ωⁱ _k-1。

可选地，所述建立i个子滤波器的量测方程包括：

将前左和后左测距声呐的量测距离，以及AUV艏向角度与结构物壁面夹角和横摇角度作为观测量Z，得到观测方程Z_k：

Z_k＝H_kX_k+V_k，

其中，观测矩阵H为：

式中，X_k为k时刻的状态向量，V_k为量测噪声；

则第i个子滤波器的量测方程为：

Zⁱ _k＝H_kXⁱ _k+Vⁱ _k，

式中，H_k为量测矩阵。

可选地，所述S3的最优融合估计处理过程为：首先对所述i个子滤波器数据进行最优估计计算，其次采用改进的联邦卡尔曼滤波对所述最优估计计算的结果进行融合处理，最后获得一个最优的距离估计值，并更新下一时刻i个子滤波器的误差协方差矩阵。

本发明公开了以下技术效果：本发明在采用姿态数据对声纳数据修正的过程中，采用先分段修正，再融合各个修正值的方法，不仅能充分有效的利用姿态数据对声纳测距进行修正，得到最优的修正值，同时提高了滤波器的稳定性和抗干扰能力，进而降低了噪声干扰，提高了配准精度，本发明比现有技术相比具有更广泛的适用性。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1为本实施例的整体技术方案的流程图；

图2为本实施例中的数据等分取平均值进行融合获得声纳数据的过程的流程图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。

如图1-2所示，本发明提供一种基于改进的联邦滤波的水下AUV传感器时间配准方法，具体包括步骤为：

S10、构建测距声纳的测距模型，获得声纳数据；

S20、获取配准周期内姿态传感器数据，作为第一姿态数据，对所述第一姿态数据进行预处理，获得处理后的配准周期内姿态传感器数据，作为第二姿态数据，对所述第二姿态数据进行滤波处理，同时对所述第二姿态数据和所述声纳数据进行融合修正处理，获得i个子滤波器数据；

S30、对所述i个子滤波器数据进行最优融合估计处理获得子滤波的误差协方差矩阵，对所述误差协方差矩阵进行更新，完成水下AUV传感器时间配准。

可选地，S101构建测距声纳的测距模型过程为：

构建AUV运动方程；

式中，AUV前向运动速度为V，AUV沿着结构物壁面运动，x_k、y_k为AUV在k时刻中心点的位置信息，α为AUV的艏向角度与结构物壁面的夹角，θ为AUV的横摇角度；

将所述AUV运动方程代入所述声呐距离量测模型得到k时刻前左和后左测距声呐距离量测模型。

可选地，S201第一姿态数据预处理的过程为：首先将所述第一姿态数据进行i等分，再进行平均值计算，进而获得第二姿态数据，预处理过程结束。

可选地，S202滤波处理为采用改进的联邦卡尔曼滤波对第二姿态数据进行处理。

可选地，S203融合修正处理的过程包括建立所述i个子滤波器的状态方程和量测方程。

可选地，S204建立所述子滤波器的状态方程包括

确定i个子滤波器的状态量为：

其中：

式中d₁、d₂分别为前左测距声纳和后左测距声纳的量测距离，d为AUV距离结构物壁面的实际距离，α为AUV的艏向角度与结构物壁面的夹角，θ为AUV的横摇角度，R₁、R₂分别为AUV在声呐位置处的半径和浮心位置处的半径；

i个子滤波器状态方程为：

其中ω_k＝[ω_1，k-1 ω_2，k-1 ω_3，k-1 ω_4，k-1 ω_5，k-1]^T是系统的过程噪声，为高斯白噪声，其方差为Q；即子滤波器状态方程简写为：

X_k＝f(X_k-1，ω_k-1)＝A_k-1X_k-1+ω_k-1

其中A_k-1为k-1时刻，运动模型中状态预测函数f关于状态的Jacobian矩阵：

则第i个子滤波器的状态方程为：

Xⁱ _k＝f(Xⁱ _k-1，ω_k-1)＝Aⁱ _k-1Xⁱ _k-1+ωⁱ _k-1。

可选地，S205建立i个子滤波器的量测方程包括：

将前左和后左测距声呐的量测距离，以及AUV艏向角度与结构物壁面夹角和横摇角度作为观测量Z，得到观测方程Z_k：

Z_k＝H_kX_k+V_k，

其中，观测矩阵H为：

式中，X_k为k时刻的状态向量，V_k为量测噪声；

则第i个子滤波器的量测方程为：

Zⁱ _k＝H_kXⁱ _k+Vⁱ _k，

式中，H_k为量测矩阵。

可选地，S301最优融合估计处理过程为：首先对所述i个子滤波器数据进行最优估计计算，其次采用改进的联邦卡尔曼滤波对所述最优估计计算的结果进行融合处理，最后获得一个最优的距离估计值，并更新下一时刻i个子滤波器的误差协方差矩阵。

S302采用改进的联邦卡尔曼滤波对步骤二中的子滤波器估计结果进行融合，得到一个最优的距离估计值，并更新下一时刻各个子滤波器的误差协方差矩阵。具体包括：

设计一个联邦滤波器，本专利中涉及i个局部滤波器，主滤波器的下标为m，全局滤波器的下标为g，每个局部滤波器和主滤波器的状态方程和量测方程如下：

其中Xⁱ _k是局部滤波器或主滤波器的状态向量，Zⁱ _k是量测向量，Aⁱ _k-1为局部滤波器在k-1时刻的状态转移矩阵，H_k为量测矩阵，ωⁱ _k-1和Vⁱ _k分为局部滤波器的状态噪声矩阵和测量噪声矩阵，它们都是高斯白噪声矩阵，方差分别为Qⁱ _k-1和Rⁱ _k。

假设在时间k-1获得局部最优估计以及相应的协方差矩阵Pⁱ _k-1|k-1，根据最优融合估计算法在全局滤波器中融合这些局部最优估计，以获得全局最优估计/>以及方差P_g，k-1。局部滤波器和全局滤波器的状态噪声协方差矩阵分别为Qⁱ _k-1和Q_g，k-1，其中，主滤波器初始时刻的状态向量/>以及状态协方差矩阵P_g，0以及初始的状态噪声协方差矩阵Q_g，0是已知的。子滤波器的状态估计值以及估计的协方差矩阵和状态噪声的协方差矩阵由主滤波器经过信息分布银子β_i反馈给局部滤波器进行更新，即得到k时刻的参数值：

其中信息分布因子β_i满足：

且信息分布因子β_i是通过协方差矩阵Pⁱ _k-1|k-1的迹来求的：

同时主滤波器k时刻的信息量也被更新：

最终主滤波器融合各个子滤波器的结果，得到最优的状态估计值：

通过上述方程，联邦滤波器将各个局部滤波器与主滤波器联系起来，通过信息分布实现融合的过程。但是由于实际情况中状态噪声往往是变化的，难以确定，而传统的联邦联邦器不具备消除偏差影响的能力，在给定状态噪声的初始值Q_g，0之后，每个时刻只是简单的根据信息分布因子去更新子滤波器的状态噪声，当初始值存在较大的偏差或状态噪声存在变化时，联邦滤波精度会大大降低。

而S-H自适应滤波算法对于状态和量测噪声的统计特性位置的情况下使用最大后验估计的原理来获得估计值以提高滤波精度，采用遗忘因子的方法对统计特性未知的噪声进行实时更新，其更新过程如下所示：

其中为k时刻系统系统误差的数学期望的估计值，d_k为迭代因子，

因此为了增强系统的稳定性，对于局部滤波器，将联邦更新的原理与自适应更新的原理相结合，使用结合的联邦自适应原理来更新状态噪声的协方差，即对于每个局部滤波器，状态噪声的更新方式有两种：

其中为子滤波器联邦更新原则下的状态噪声更新，/>为子滤波器自适应原则下的更新过程。

改进的联邦卡尔曼滤波器则会对两个更新过程进行加权：

根据状态噪声的变化热性，噪声变大，α应该减小，为满足这一特性，采用变换后的指数函数作为权重的变化函数：

本发明在采用姿态数据对声纳数据修正的过程中，采用先分段修正，再融合各个修正值的方法，不仅能充分有效的利用姿态数据对声纳测距进行修正，得到最优的修正值，同时提高了滤波器的稳定性和抗干扰能力，进而降低了噪声干扰，提高了配准精度，本发明比现有技术相比具有更广泛的适用性。

在本发明的描述中，需要理解的是，术语“纵向”、“横向”、“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。

以上所述的实施例仅是对本发明的优选方式进行描述，并非对本发明的范围进行限定，在不脱离本发明设计精神的前提下，本领域普通技术人员对本发明的技术方案做出的各种变形和改进，均应落入本发明权利要求书确定的保护范围内。

Claims (3)

1.一种基于改进的联邦滤波的水下AUV传感器时间配准方法，其特征在于：具体包括步骤为，

S1、构建测距声呐的测距模型，获得声呐数据；构建测距声呐的测距模型过程为：构建AUV运动方程；

式中，AUV前向运动速度为V，AUV沿着结构物壁面运动，x_k、y_k为AUV在k时刻中心点的位置信息，α为AUV的艏向角度与结构物壁面的夹角，θ为AUV的横摇角度；将所述AUV运动方程代入所述测距声呐的测距模型得到k时刻前左和后左测距声呐距离量测模型；

S2、获取配准周期内姿态传感器数据，作为第一姿态数据，对所述第一姿态数据进行预处理，获得处理后的配准周期内姿态传感器数据，作为第二姿态数据，对所述第二姿态数据进行滤波处理，同时对所述第二姿态数据和所述声呐数据进行融合修正处理，获得i个子滤波器数据；所述S2中第一姿态数据预处理的过程为：首先将所述第一姿态数据进行i等分，再进行平均值计算，进而获得第二姿态数据，预处理过程结束；所述S2中所述融合修正处理的过程包括建立所述i个子滤波器的状态方程和量测方程；建立所述子滤波器的状态方程包括：确定i个子滤波器的状态量为：

其中：

式中d₁、d₂分别为前左测距声呐和后左测距声呐的量测距离，d为AUV距离结构物壁面的实际距离，α为AUV的艏向角度与结构物壁面的夹角，θ为AUV的横摇角度，R₁、R₂分别为AUV在声呐位置处的半径和浮心位置处的半径；

i个子滤波器状态方程为：

其中ω_k＝[ω_1，k-1 ω_2，k-1 ω_3，k-1 ω_4，k-1 ω_5，k-1]^T是系统的过程噪声，为高斯白噪声，其方差为Q；即子滤波器状态方程简写为：

X_k＝f(X_k-1，ω_k-1)＝A_k-1X_k-1+ω_k-1

其中A_k-1为k-1时刻，运动模型中状态预测函数f关于状态的Jacobian矩阵：

则第i个子滤波器的状态方程为：

Xⁱ _k＝f(Xⁱ _k-1，ω_k-1)＝Aⁱ _k-1Xⁱ _k-1+ωⁱ _k-1；

建立i个子滤波器的量测方程包括：

将前左和后左测距声呐的量测距离，以及AUV艏向角度与结构物壁面夹角和横摇角度作为观测量Z，得到观测方程Z_k：

Z_k＝H_kX_K+V_k，

其中，观测矩阵H为：

式中，X_k为k时刻的状态向量，V_k为量测噪声；

则第i个子滤波器的量测方程为：

Zⁱ _k＝H_kXⁱ _k+Vⁱ _k，

式中，H_k为量测矩阵；S3、对所述i个子滤波器数据进行最优融合估计处理获得子滤波的误差协方差矩阵，对所述误差协方差矩阵进行更新，完成水下AUV传感器时间配准。

2.根据权利要求1所述的基于改进的联邦滤波的水下AUV传感器时间配准方法，其特征在于：所述S2中滤波处理为采用改进的联邦卡尔曼滤波对第二姿态数据进行处理。

3.根据权利要求1所述的基于改进的联邦滤波的水下AUV传感器时间配准方法，其特征在于：所述S3的最优融合估计处理过程为：首先对所述i个子滤波器数据进行最优估计计算，其次采用改进的联邦卡尔曼滤波对所述最优估计计算的结果进行融合处理，最后获得一个最优的距离估计值，并更新下一时刻i个子滤波器的误差协方差矩阵。