[发明专利]基于改进的联邦滤波的水下AUV传感器时间配准方法

权利要求书

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个子滤波器的误差协方差矩阵。