CN109765523B - 基于自适应akf的单应答器斜距水声定位方法及系统 - Google Patents

Abstract

本发明属于水声通信导航领域，涉及一种基于自适应AKF的单应答器斜距水声定位方法及系统。该系统由应答器、声信号收发器、多普勒流速计、姿态传感器、深度传感器、自适应卡尔曼滤波控制器组成。基于自适应卡尔曼滤波(AKF)算法，整合水下航行器单一斜距信息、深度测量信息、速度信息、姿态信息，且考虑各个传感器的噪声、时间延迟、更新频率和多传感器信息融合，建立水下航行器的单一应答器单一斜距定位方法，计算出水下航行器的三维坐标，实现精确定位。本发明的方法可节省应答器布放与定位作业成本，能够克服受限于声信号在水下传播的速度与距离，斜距测量更新率较慢，与深度、姿态测量信息的更新不同步问题，从而提高定位效率和精度。

Description

基于自适应AKF的单应答器斜距水声定位方法及系统

技术领域

本发明属于水声通信导航领域，涉及一种基于自适应AKF的单应答器斜距水声定位方法及系统。

背景技术

水声定位系统中，常用的主要有长基线、短基线、超短基线等方式，虽然这三种方式能够提供可靠的水下航行器三维坐标，实现定位，但均需要三支以上的应答器，并且需要分别对每支应达器进行定位校正，需要耗费大量作业时间成本。

目前，国内学者进行了一些研究。例如：王熙赢等人提出了一种LBL/MINS组合导航系统及其导航信息融合方法(申请号 201510518511.7)，该系统包括长基线水下水声定位系统LBL和微惯性导航系统MINS，两系统之间通过扩展卡尔曼滤波器将系统的定位信息进行数据融合，输出最优的导航定位结果。但是长基线水下水声定位系统所需的应答器较多，至少3个以上，系统复杂操作繁琐。

孙晓颖(申请号201410406943.4)等人提出了一种基于近场源定位算法的多目标水声定位方法和系统，该系统将阵列信号处理领域的近场源定位算法应用于水声定位系统中，可以同时估计出角度和距离信息。但该方法距离信息很难提取，对各路接收信号有很严格的限制。

阳凡林(申请号：201510875555.5)等人提出了一种基于等效声速梯度的长基线水声定位方法，该方法不需要准确的声速剖面，有效消除了声速剖面代表性误差的影响。该方法同样属于长基线水下水声定位方法范畴，所需的应答器较多，至少3个以上，系统复杂操作繁琐。

杨明华(申请号：201520453130.0)提出了一种基于多种水声定位技术的水下目标定位系统，该系统包括定位装置、潜水装置和应答装置。该方法使得系统在长基线水声定位和超短基线水声定位两个定位模式中切换，同样需要布放多个海底应答器，系统复杂。

综上，目前公开的现有技术中所公布的技术方案无法降低应答器布放、校正、以及回收的作业成本。因此，亟需一种水声定位方法及系统能够实现降低应答器布放、校正、以及回收的作业成本，并同时达到精确水下航行器定位的目的。

发明内容

针对上述技术问题，本发明提供了一种基于自适应AKF的单应答器斜距水声定位方法及系统，通过多传感器信息融合和自适应卡尔曼滤波算法最优估计，只需一个应答器，降低应答器布放、校正、以及回收的作业成本，并同时达到精确水下航行器定位。

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

基于自适应AKF的单应答器斜距水声定位方法及系统，通过单一应答器、声信号收发器、多普勒流速计、姿态传感器、深度传感器获得水下航行器单一斜距信息、深度测量信息、速度信息、姿态信息，采用自适应卡尔曼滤波来融合测量信息；建立单一斜距定位模型，基于自适应卡尔曼滤波算法，通过迭代运算，根据传感器测量数据修正重新建立程序误差协方差矩阵Q_k与观测误差协方差矩阵R_k；采用自适应卡尔曼滤波算法，在没有任何测量数据时，只进行定位预测，直到有任何观测值更新立即进行定位修正。

进一步地，所述方法包括以下步骤：

(1)将水下航行器在地球坐标系统下的位置、速度以及姿态角设为系统状态参数，将单一应答器的测量数据和多普勒流速计、深度计、姿态传感器的测量数据融合；

k时刻系统状态x_k与观测值y_k表示为：

其中，[X,Y,Z]^T为水下航行器在地球坐标下的X,Y,Z方向的位置， [u,v,w]^T为水下航行器在地球坐标下的X,Y,Z方向的速度；[ψ,φ,θ]^T为水下航行器的艏向角、纵摇角、横摇角；

sr为水下航行器至海底单一应答器之间的斜距；[u_d,v_d,w_d]^T分别为水下航行器多普勒流速计所测量到的在参考坐标系S_d下的X_d,Y_d,Z_d三个方向的速度；

(2)构建单一斜距定位系统模型，系统状态x_k的方程表示为：

x_k＝f(x_k-1)+q_k-1

其中，f(x_k-1)为k-1时刻的系统状态函数，q_k-1为k-1时刻的系统状态误差；根据前一时刻，即k-1时刻的系统状态，计算k时刻系统状态x_k，表示为：

其中，Δt表示k-1时刻到k时刻的时间差；

根据坐标转换，k时刻观测值y_k表示为：

其中，(x_T y_T z_T)为海底单一应答器绝对位置；r_k为k时刻观测误差；T_n是由参考坐标系S_d转换到地球坐标系的旋转矩阵；

系统状态x_k的k-1时刻的过渡矩阵A_x(m_k-1)与观测值y_k的观测矩阵H_x(m_k)分别表示为：

式中：

0_m×n代表维度为m×n的零矩阵；I_n代表维度n×n的单位矩阵，L为

对[ψ,φ,θ]^T微分的矩阵；

(3)k-1时刻的系统状态误差q_k-1与观测误差r_k的协方差矩阵分别为Q_k-1、R_k；

将r_k中的参数设置为应答器、声信号收发器、多普勒流速计、姿态传感器和压力传感器中各个传感器的测量误差；

q_k中的参数对系统状态x_k的影响以及r_k中参数对观测值y_k的影响分别独立，将q_k的参数以及r_k的参数分别称为程序误差协方差矩阵 Q_k、观测误差协方差矩阵R_k；其中Q_k、R_k为对角矩阵；

(4)设定Q_k、R_k的初始猜测值，基于自适应卡尔曼滤波算法，通过迭代运算，根据传感器测量数据修正重新建立Q_k与R_k；

将系统状态x_k微分可得到：

假设动态噪声误差w_k的功率谱密度函数W_c为：

W_c＝diag([w₁,w₂,w₃,w₄,w₅,w₆])

diag(·)表示(·)的1×n或n×1向量转化为n×n矩阵且对角元素为(·) 的向量；

系统状态误差的协方差矩阵Q_k为：

(5)观测值非同步更新的定位计算：采用自适应卡尔曼滤波的最佳线性估测性能，在没有任何测量数据时，只进行定位预测，直到有任何观测值更新立即进行定位修正；

依据系统观测矩阵H_x(m_k)，单一应答器、深度传感器、多普勒流速计、姿态传感器相对应的观测矩阵分别表示如下：

1)斜距观测矩阵

2)深度观测矩阵

H_depth＝[0_1×2,1,0_1×6]

3)速度观测矩阵

H_Sd-velocity＝[0_3×3,T_n ^-1,L]

4)姿态角观测矩阵

H_angle＝[0_3×6 I₃]

假设在k时刻系统只收到斜距与深度数据，则将观测值表示为

获得观测矩阵

并且将观测误差改写为r_k＝[r_range,r_depth]_k，其中r_range,r_depth分别为斜距与深度测量误差。

进一步地，步骤4)中，采用自适应卡尔曼滤波算法，利用前一刻的观测值通过迭代运算，迭代修正程序误差协方差矩阵Q_k与观测误差协方差矩阵R_k，具体为：

(1)预估程序误差协方差矩阵Q_k

式中：Φ为系统状态的动态矩阵；Γ为动态噪声分布矩阵；Δt_k-1＝t_k-t_k-1为时间间隔；W_c为w_k-1的功率谱密度函数，表示各个参数随机干扰的程度；

(2)预估系统平均状态

式中：

为系统平均状态估计值；m_k-1为k-1时刻系统平均状态实际值；A_k-1为t_k-1至t_k的状态过渡矩阵；

(3)预估状态误差协方差矩阵

式中：

为系统状态x_k在t_k时刻下的误差协方差矩阵的估计值； P_k-1为系统状态x_k在t_k-1时刻下的误差协方差矩阵，P_k为系统状态x_k在 t_k时刻下的误差协方差矩阵的估计值；

为k-1时刻的系统状态过渡矩阵；

(4)ρ为预估自由度：ρ的范围为0＜ρ≤1

式中：v_k代表t_k时刻的自由度；n为正整数；

(5)预估尺度矩阵：

式中：

为预估的尺度矩阵；

(6)计算卡尔曼增益K_k

(7)计算测量参数的残差

式中：y_k为k时刻观测值；

(8)修正预估系统平均状态

其中，

代表系统状态观测值；

(9)更新状态误差的协方差矩阵P_k

其中，I代表矩阵纬度，

为更新观测矩阵；

(10)更新自由度v_k

(11)更新观测误差协方差矩阵R_k

式中：a_k＝v_k-n-1。

一种单应答器水声组合定位系统，采用所述基于自适应AKF的单应答器斜距水声定位方法及系统进行定位，所述定位系统包括：设置于海底的一个应答器、设置于水下航行器上的声信号收发器、多普勒流速计、姿态传感器、压力传感器、自适应卡尔曼滤波控制器；

所述声信号收发器向所述应答器发射声脉冲信号，所述应答器接收该声脉冲信号后随即回复一应答声脉冲信号，所述声信号收发器将接收到的应答声脉冲信号发送至所述自适应卡尔曼滤波控制器，获得水下航行器至海底单一应答器之间的斜距；并且所述多普勒流速计、所述姿态传感器以及所述压力传感器分别将测量获得的流速信息、水下航行器的姿态信息以及压力信息发送至所述自适应卡尔曼滤波控制器；

所述自适应卡尔曼滤波控制器基于自适应AKF的单应答器斜距水声定位方法及系统对水下航行器进行精确定位。

本发明的有益技术效果：

1)本发明所提供的基于自适应AKF的单应答器斜距水声定位方法及系统，整合水下航行器斜距信息、深度测量信息、速度信息、姿态信息，且考虑各个传感器的噪声、时间延迟、更新频率和多传感器信息融合，建立水下水下航行器的单一斜距定位方法，计算出水下航行器的三维坐标。

2)本发明所提供的单应答器单一斜距水声定位方法，只需一个应答器，降低应答器布放、校正、以及回收的作业成本，并同时达到精确水下航行器定位。

3)本发明所提供的单应答器单一斜距水声定位方法，所用的自适应卡尔曼滤波算法，可使程序误差共变异协方差矩阵Q_k与观测误差共变异矩阵R_k可以在算法中利用前一刻的测量信息迭代修正。

4)本发明所提供的单应答器单一斜距水声定位方法，不需要提供精确的误差协方差矩阵Q_k和R_k，而是通过迭代运算法更新模型，且计算方程会根据当前的测量数据而改变，进而提高单一斜距定位系统的定位更新效率。

5)本发明所提供的单应答器单一斜距水声定位方法，能够克服受限于声信号在水下传播的速度与距离，斜距测量的更新率较慢，与速度、深度、姿态测量信息的更新不同步问题，从而提高定位效率和精度。

附图说明

图1是本发明实施例中基于AKF的单应答器单一斜距水声定位方法流程图；

图2是本发明实施例中固定时间定位估算流程示意图；

图3是本发明实施例中单应答器斜距测量示意图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细描述。应当理解，此处所描述的具体实施例仅仅用于解释本发明，并不用于限定本发明。

相反，本发明涵盖任何由权利要求定义的在本发明的精髓和范围上做的替代、修改、等效方法以及方案。进一步，为了使公众对本发明有更好的了解，在下文对本发明的细节描述中，详尽描述了一些特定的细节部分。对本领域技术人员来说没有这些细节部分的描述也可以完全理解本发明。

受限于声音在水下传播的速度与传播距离，斜距测量的更新率不快，与DVL、深度计、姿态传感器测量数据的更新也不同步。为降低这些测量因素造成的定位误差，本发明实施例采用自适应卡尔曼滤波来融合测量信息。传统的扩展式卡尔曼滤波(EKF)融合单一斜距与传感器或动态模式等信息来进行水下水下航行器的定位，都必须提供精确的协方差矩阵Q_k、R_k，才能够达到良好的定位估算效果，但是实际应用过程中很难精确估算Q_k和R_k。基于此，本发明实施例利用自适性卡尔曼滤波(AKF)算法建立单一斜距定位系统，不需要提供精确的Q_k、R_k，而是通过迭代运算法更新系统，并且控制方程也会根据当前的测量信息进行修正，进而提升单一斜距定位系统的定位更新效率，并且能够节省应答器布放与定位作业成本、提高定位效率和精度。

本实施例提供一种基于自适应AKF的单应答器斜距水声定位方法及系统，通过单一应答器、声信号收发器、多普勒流速计、姿态传感器、深度传感器获得水下航行器单一斜距信息、深度测量信息、速度信息、姿态信息，采用自适应卡尔曼滤波来融合测量信息；建立单一斜距定位模型，基于自适应卡尔曼滤波算法，通过迭代运算，根据传感器测量数据修正重新建立程序误差协方差矩阵Q_k与观测误差协方差矩阵R_k；采用自适应卡尔曼滤波算法，在没有任何测量数据时，只进行定位预测，直到有任何观测值更新立即进行定位修正。

所述方法包括以下步骤：

(1)将水下航行器在地球坐标系统下的位置、速度以及姿态角设为系统状态参数，将单一应答器的测量数据和多普勒流速计、深度计、姿态传感器的测量数据融合；

k时刻系统状态x_k与观测值y_k表示为：

其中，[X,Y,Z]^T为水下航行器在地球坐标下的X,Y,Z方向的位置， [u,v,w]^T为水下航行器在地球坐标下的X,Y,Z方向的速度；[ψ,φ,θ]^T为水下航行器的艏向角、纵摇角、横摇角；

sr为水下航行器至海底单一应答器之间的斜距；[u_d,v_d,w_d]^T分别为水下航行器多普勒流速计所测量到的在参考坐标系S_d下的 X_d,Y_d,Z_d三个方向的速度；

(2)构建单一斜距定位系统模型，系统状态x_k的方程表示为：

x_k＝f(x_k-1)+q_k-1

其中，f(x_k-1)为k-1时刻的系统状态函数，q_k-1为k-1时刻的系统状态误差；根据前一时刻，即k-1时刻的系统状态，计算k时刻系统状态x_k，表示为：

其中，Δt表示k-1时刻到k时刻的时间差；

根据坐标转换，k时刻观测值y_k表示为：

其中，(x_T y_T z_T)为海底单一应答器绝对位置；r_k为k时刻观测误差；T_n是由参考坐标系S_d转换到地球坐标系的旋转矩阵；

系统状态x_k的k-1时刻的过渡矩阵A_x(m_k-1)与观测值y_k的观测矩阵H_x(m_k)分别表示为：

式中：

0_m×n代表维度为m×n的零矩阵；I_n代表维度n×n的单位矩阵，L为

对[ψ,φ,θ]^T微分的矩阵；

(3)为实现卡尔曼滤波，传统的方式必须提供精确的q_k-1与r_k的协方差矩阵Q_k-1、R_k。但绝大部分情况下难以准确估计其值，本实施例中做如下处理：

k-1时刻的系统状态误差q_k-1与观测误差r_k的协方差矩阵分别为 Q_k-1、R_k；

将r_k中的参数设置为应答器、声信号收发器、多普勒流速计、姿态传感器和压力传感器中各个传感器的测量误差；

q_k中的参数对系统状态x_k的影响以及r_k中参数对观测值y_k的影响分别独立，将q_k的参数以及r_k的参数分别称为程序误差协方差矩阵 Q_k、观测误差协方差矩阵R_k，Q_k、R_k为对角矩阵；

(4)由于自变量程序误差协方差矩阵Q_k、自变量观测误差协方差矩阵R_k对卡尔曼滤波最佳化的结果有重大影响，因此，本发明实施例所述方法使用自适性卡尔曼滤波建立单一斜距定位系统，设定Q_k、 R_k的初始猜测值，基于自适应卡尔曼滤波算法，通过迭代运算，根据传感器测量数据修正重新建立Q_k与R_k；

将系统状态x_k微分可得到：

假设动态噪声误差w_k的功率谱密度函数W_c为：

W_c＝diag([w₁,w₂,w₃,w₄,w₅,w₆])

diag(·)表示(·)的1×n或n×1向量转化为n×n矩阵且对角元素为(·) 的向量；

系统状态误差的的协方差矩阵Q_k为：

(5)观测值非同步更新的定位计算：卡尔曼滤波融合单一应答器与DVL、深度计、姿态传感器的测量信息，建立的单一斜距系统，各个传感器测量的信息更新频率不同步，收到测量信息的时间不一致，且存在数据中断的问题。单一斜距定位系统必须等到所有的观测值全部更新之后才进行定位估算，必然会使得单一斜距定位系统运算延迟，导致无法快速有效地修正水下航行器状态。

为了提升单一斜距定位系统的修正效率，本实施例所述方法采用自适应卡尔曼滤波的最佳线性估测性能，在没有任何测量数据时，只进行定位预测，直到有任何观测值更新立即进行定位修正；

依据系统观测矩阵H_x(m_k)，单一应答器、深度传感器、多普勒流速计、姿态传感器相对应的观测矩阵分别表示如下：

1)斜距观测矩阵

2)深度观测矩阵

H_depth＝[0_1×2,1,0_1×6]

3)速度观测矩阵

4)姿态角观测矩阵

H_angle＝[0_3×6 I₃]

假设在k时刻系统只收到斜距与深度数据，则将观测值表示为

获得观测矩阵

并且将观测误差改写为r_k＝[r_range,r_depth]_k，其中r_range,r_depth分别为斜距与深度测量误差。

因此单一斜距定位系统可以在任何给定的时间进行估算，只要取得观测值便可以进行定位修正。

其中，在本实施例，步骤4)中，采用自适应卡尔曼滤波算法，利用前一刻的观测值通过迭代运算，迭代修正程序误差协方差矩阵Q_k与观测误差协方差矩阵R_k，如图1所示，具体为：

(1)预估程序误差的协方差矩阵Q_k

式中：Φ为系统状态的动态矩阵；Γ为动态噪声分布矩阵；Δt_k-1＝t_k-t_k-1为时间间隔；W_c为w_k-1的功率谱密度函数，表示各个参数随机干扰的程度；

(2)预估系统平均状态

式中：

为系统平均状态估计值；m_k-1为k-1时刻系统平均状态实际值；A_k-1为t_k-1至t_k的状态过渡矩阵；

(3)预估状态误差协方差矩阵

式中：

为系统状态x_k在t_k时刻下的误差协方差矩阵的估计值； P_k-1为系统状态x_k在t_k-1时刻下的误差协方差矩阵，P_k为系统状态x_k在 t_k时刻下的误差协方差矩阵的估计值；

为k-1时刻的系统状态过渡矩阵；

(4)q_k预估自由度：ρ的范围为0＜ρ≤1

式中：v_k代表t_k时刻的自由度；n为正整数；

(5)预估尺度矩阵：

式中：

为预估的尺度矩阵；

(6)计算卡尔曼增益K_k

(7)计算测量参数的残差

式中：y_k为k时刻观测值；

(8)修正预估系统平均状态

其中，

代表系统状态观测值；

(9)更新状态误差的协方差矩阵P_k

其中，I代表矩阵纬度，

为更新观测矩阵；

(10)更新自由度v_k

(11)更新观测误差协方差矩阵R_k

式中：a_k＝v_k-n-1

由上述计算步骤可看出，自适性卡尔曼滤波算法中与离散卡尔曼滤波算法的不同在于自适性卡尔曼滤波的程序误差共变异协方差矩阵Q_k与观测误差共变异矩阵R_k可以在算法中利用前一刻的测量信息迭代修正。如图2所示，以每1秒进行定位估算为例，在第1秒内没有测量数据时，只进行定位估算，当在1.3s取得测量数据时，则通过时变的观测协方差矩阵即时的修正定位估算状态，到达第2秒时，数据已进行更新修正，提高定位精度。

本实施例所提供的方法由应答器、声信号收发器、多普勒流速计、姿态传感器、压力传感器、自适应卡尔曼滤波控制器组成。基于自适应卡尔曼滤波(AKF)算法，整合水下航行器单一斜距信息、深度测量信息、速度信息、姿态信息，且考虑各个传感器的噪声、时间延迟、更新频率和多传感器信息融合，建立水下水下航行器的单一斜距定位方法，计算出水下航行器的三维坐标，实现精确定位。

受限于声音在水下传播的速度与距离，斜距测量的更新率不快，与速度、深度、姿态测量信息的更新也不同步，为降低这些测量因素造成的定位误差，本实施例所述方法采用自适应卡尔曼滤波融合测量资料，利用迭代与递归程序达到最佳估测。

本实施例所提供的基于自适性卡尔曼滤波(AKF)所建立的单一斜距定位方法，不需要提供精确的误差协方差矩阵Q_k和R_k，而是通过迭代运算法更新模型，且测量方程会根据当前的测量数据而改变，进而提高单一斜距定位系统的定位更新效率。

本实施例还提供一种单应答器水声组合定位系统，采用上述基于自适应AKF的单应答器斜距水声定位方法及系统进行定位，所述定位系统包括：设置于海底的一个应答器、设置于水下航行器上的声信号收发器、多普勒流速计、姿态传感器、压力传感器、自适应卡尔曼滤波控制器；

所述声信号收发器向所述应答器发射声脉冲信号，所述应答器接收该声脉冲信号后随即回复一应答声脉冲信号，所述声信号收发器将接收到的应答声脉冲信号发送至所述自适应卡尔曼滤波控制器，获得水下航行器至海底单一应答器之间的斜距；并且所述多普勒流速计、所述姿态传感器以及所述压力传感器分别将测量获得的流速信息、水下航行器的姿态信息以及压力信息发送至所述自适应卡尔曼滤波控制器；

所述自适应卡尔曼滤波控制器基于自适应AKF的单应答器斜距水声定位方法及系统对水下航行器进行精确定位。

其中，单应答器斜距测量原理如图3所示，水下航行器发射机通过安置于底部的换能器向水下应答器(经高精度绝对校准且位置已知)发射声脉冲信号(即询问信号)，应答器接收该信号后随即回复一应答声脉冲信号，水下航行器内部接收机记录发射询问信号和接收应答信号的时间间隔的表达式为

式中：C为声波在海水中的速度；t为声信号在水下航行器与海底应答器之间的双程传播往返时间总和。

通过该表达式即可算出水下航行器至海底应答器之间的斜距sr；由于应答器的深度z已知，因此，水下航行器至应答器之间的水平距离d可表示为：

应当理解的是，对本领域普通技术人员来说，可以根据上述说明加以改进或变换，而所有这些改进和变换都应属于本发明所附权利要求的保护范围。

Claims (3)

1.基于自适应AKF的单应答器斜距水声定位方法，其特征在于，通过单一应答器、声信号收发器、多普勒流速计、姿态传感器、深度传感器获得水下航行器单一斜距信息、深度测量信息、速度信息、姿态信息，采用自适应卡尔曼滤波来融合测量信息；建立单一斜距定位模型，基于自适应卡尔曼滤波算法，通过迭代运算，根据传感器测量数据修正重新建立程序误差协方差矩阵Q_k与观测误差协方差矩阵R_k；采用自适应卡尔曼滤波算法，在没有任何测量数据时，只进行定位预测，直到有任何观测值更新时立即进行定位修正；

所述方法包括以下步骤：

(1)将水下航行器在地球坐标系统下的位置、速度以及姿态角设为系统状态参数，将单一应答器测量的斜距信息数据和多普勒流速计、深度计、姿态传感器的测量数据融合；

k时刻系统状态x_k与观测值y_k表示为：

其中，[X,Y,Z]^T为水下航行器在地球坐标下的X,Y,Z方向的位置，[u,v,w]^T为水下航行器在地球坐标下的X,Y,Z方向的速度；[ψ,φ,θ]^T为水下航行器的艏向角、纵摇角、横摇角；

sr为水下航行器至海底单一应答器之间的斜距；[u_d,v_d,w_d]^T分别为水下航行器多普勒流速计所测量到的在参考坐标系S_d下的X_d,Y_d,Z_d三个方向的速度；

(2)构建单一斜距定位系统模型，系统状态x_k的方程表示为：

x_k＝f(x_k-1)+q_k-1

其中，f(x_k-1)为k-1时刻的系统状态函数，q_k-1为k-1时刻的系统状态误差；根据前一时刻，即k-1时刻的系统状态，计算k时刻系统状态x_k，表示为：

其中，Δt表示k-1时刻到k时刻的时间差；

根据坐标转换，k时刻观测值y_k表示为：

其中，(x_T y_T z_T)为海底单一应答器绝对位置；r_k为k时刻观测误差；T_n是由参考坐标系S_d转换到地球坐标系的旋转矩阵；

系统状态x_k的k-1时刻的过渡矩阵A_x(m_k-1)与观测值y_k的观测矩阵H_x(m_k)分别表示为：

式中：

0_m×n代表维度为m×n的零矩阵；I_n代表维度n×n的单位矩阵，L为

对[ψ,φ,θ]^T微分的矩阵；

(3)k-1时刻的系统状态误差q_k-1与观测误差r_k的协方差矩阵分别为Q_k-1、R_k；

将r_k中的参数设置为应答器、声信号收发器、多普勒流速计、姿态传感器和压力传感器中各个传感器的测量误差；

q_k中的参数对系统状态x_k的影响以及r_k中参数对观测值y_k的影响分别独立，将q_k的参数以及r_k的参数分别称为程序误差协方差矩阵Q_k、观测误差协方差矩阵R_k；其中Q_k、R_k为对角矩阵；

(4)设定Q_k、R_k的初始猜测值，基于自适应卡尔曼滤波算法，通过迭代运算，根据传感器测量数据修正重新建立Q_k与R_k；

将系统状态x_k微分可得到：

假设动态噪声误差w_k的功率谱密度函数W_c为：

W_c＝diag([w₁,w₂,w₃,w₄,w₅,w₆])

diag(·)表示(·)的1×n或n×1向量转化为n×n矩阵且对角元素为(·) 的向量；

系统状态误差的协方差矩阵Q_k为：

(5)观测值非同步更新的定位计算：采用自适应卡尔曼滤波的最佳线性估测性能，在没有任何测量数据时，只进行定位预测，直到有任何观测值更新立即进行定位修正；

依据系统观测矩阵H_x(m_k)，单一应答器、深度传感器、多普勒流速计、姿态传感器相对应的观测矩阵分别表示如下：

1)斜距观测矩阵

2)深度观测矩阵

H_depth＝[0_1×2,1,0_1×6]

3)速度观测矩阵

4)姿态角观测矩阵

H_angle＝[0_3×6 I₃]

假设在k时刻系统只收到斜距与深度数据，则将观测值表示为

获得观测矩阵

并且将观测误差改写为r_k＝[r_range,r_depth]_k，其中r_range,r_depth分别为斜距与深度测量误差。

2.根据权利要求1所述基于自适应AKF的单应答器斜距水声定位方法，其特征在于，

步骤4)中，采用自适应卡尔曼滤波算法，利用前一刻的观测值通过迭代运算，迭代修正程序误差协方差矩阵Q_k与观测误差协方差矩阵R_k，具体为：

(1)预估程序误差协方差矩阵Q_k

式中：Φ为系统状态的动态矩阵；Γ为动态噪声分布矩阵；Δt_k-1＝t_k-t_k-1为时间间隔；W_c为w_k-1的功率谱密度函数，表示各个参数随机干扰的程度；

(2)预估系统平均状态

式中：

为系统平均状态估计值；m_k-1为k-1时刻系统平均状态实际值；A_k-1为t_k-1至t_k的状态过渡矩阵；

(3)预估状态误差协方差矩阵

式中：P_k-为系统状态x_k在t_k时刻下的误差协方差矩阵的估计值；P_k-1为系统状态x_k在t_k-1时刻下的误差协方差矩阵，P_k为系统状态x_k在t_k时刻下的误差协方差矩阵的估计值；

为k-1时刻的系统状态过渡矩阵；

(4)ρ为预估自由度：ρ的范围为0＜ρ≤1

式中：v_k代表t_k时刻的自由度；n为正整数；

(5)预估尺度矩阵：

式中：

为预估的尺度矩阵；

(6)计算卡尔曼增益K_k

(7)计算测量参数的残差

式中：y_k为k时刻观测值；

(8)修正预估系统平均状态

其中，

代表系统状态观测值；

(9)更新状态误差的协方差矩阵P_k

其中，I代表矩阵纬度，

为更新观测矩阵；

(10)更新自由度v_k

(11)更新观测误差协方差矩阵R_k

式中：a_k＝v_k-n-1。

3.一种基于自适应AKF的单应答器斜距水声定位系统，采用权利要求1-2任一项所述基于自适应AKF的单应答器斜距水声定位方法进行定位，其特征在于，所述定位系统包括：设置于海底的一个应答器、设置于水下航行器上的声信号收发器、多普勒流速计、姿态传感器、压力传感器、自适应卡尔曼滤波控制器；

所述声信号收发器向所述应答器发射声脉冲信号，所述应答器接收该声脉冲信号后随即回复一应答声脉冲信号，所述声信号收发器将接收到的应答声脉冲信号发送至所述自适应卡尔曼滤波控制器，获得水下航行器至海底单一应答器之间的斜距；并且所述多普勒流速计、所述姿态传感器以及所述压力传感器分别将测量获得的流速信息、水下航行器的姿态信息以及压力信息发送至所述自适应卡尔曼滤波控制器；

所述自适应卡尔曼滤波控制器基于自适应AKF的单应答器斜距水声定位方法对水下航行器进行精确定位。

Images