CN114370878A - 一种基于stackf的多auv协同定位方法 - Google Patents

Abstract

本发明公开了一种基于STACKF的多AUV协同定位方法，包括：对AUV运动学模型建模，得到AUV运动学模型的运动方程；建立主从式多AUV协同导航定位的量测模型，得到量测模型的量测方程；采用容积卡尔曼滤波来求解多AUV协同导航中的状态估计值；采用基于渐消记忆指数加权法的噪声估值器算法生成系统噪声和量测噪声的协方差矩阵；将强跟踪算法中的渐消因子引入到容积卡尔曼滤波的预测和更新方程中，再结合AUV运动学模型的运动方程，估算AUV位置。本发明不仅具有简单易行的优点，而且所提算法通过点估计的方法，避免了对非线性系统模型的线性化近似处理，提升了精度；同时将强跟踪滤波以及自适应估计方法与容积卡尔曼滤波相结合，具有良好的自适应性和鲁棒性。

Description

一种基于STACKF的多AUV协同定位方法

技术领域

本发明属于AUV定位领域，涉及水下主从式多AUV协同定位技术，具体涉及一种基于STACKF的多AUV协同定位方法。

背景技术

自主水下航行器(Autonomous Underwater Vehicles,AUV)在诸多军事和民事方面得到了广泛应用。单一AUV的使用已经不能满足当前的需要。因此，深入研究由多个AUV组成的协作系统有着重要的意义。在AUV协同系统中，配备低精度导航装置的航行器利用水声通信设备获取高精度航行器的位置信息以及二者间相对距离参考信息后，通过信息融合技术实现导航定位误差的协同校正。

目前对AUV位置进行估计所用的方法有基于状态空间模型的滤波技术以及基于间歇观测信息的卡尔曼滤波方法。卡尔曼滤波(CKF)是一种常见的滤波算法，该滤波算法是建立在观测噪声为高斯噪声的基础上，但由于水下环境多变，往往观测噪声为非高斯分布，便会造成协同导航定位精度的降低。

发明内容

发明目的：为了克服现有技术中存在的水下航行过程中协同导航定位精度低的问题，提供一种基于STACKF的多AUV协同定位方法，建立适用于导航系统的滤波算法，其能够校正水下航行过程中的AUV位置误差，提高多AUV协同定位精度。

技术方案：为实现上述目的，本发明提供一种基于STACKF的多AUV协同定位方法，包括如下步骤：

S1：对AUV运动学模型建模，得到AUV运动学模型的运动方程；

S2：基于运动方程，建立主从式多AUV协同导航定位的量测模型，得到量测模型的量测方程；

S3：采用容积卡尔曼滤波(CKF)来求解多AUV协同导航中的状态估计值；

S4：基于状态估计值和量测方程，采用基于渐消记忆指数加权法的噪声估值器算法生成系统噪声和量测噪声的协方差矩阵Q_k和R_k；

S5：将强跟踪算法中的渐消因子引入到步骤S3的容积卡尔曼滤波(CKF)的预测和更新方程中，修正多AUV协同导航中的状态估计值和系统噪声和量测噪声的协方差矩阵，有效避免模型不准确造成的滤波下降，再结合AUV运动学模型的运动方程，估算AUV位置，实现多AUV协同定位。

进一步地，所述步骤S1中运动学模型的运动方程为：

式中，x_k，y_k，θ_k分别为AUV在二维投影平面中的横向坐标、纵向坐标和偏航角；V_k为前向合成速度；

为偏航角速度；T为采样周期；设噪声为高斯白噪声，则在噪声影响下的系统输入为：

其中，V_mk和

分别为AUV在t_k时刻的速度和偏航角速度的量测值；w_vk和w_ωk均为相互独立的零均值高斯白噪声。

进一步地，所述步骤S2中量测模型的量测方程为：

其中，dx_k,k+1和dy_k,k+1分别为跟随AUV在采样周期时间内的运动失径；上标S为跟随AUV；上标M为领航AUV；A_k为k时刻主从AUV之间的距离。

进一步地，所述步骤S3具体包括如下步骤：

A1：时间更新

假设后验概率密度函数

已知，对估计均方差做Cholesky分解，可得

计算容积点，其中i＝1,2,3...,m；m＝2n，有

将容积点通过状态方程进行传递后，可得新的采样点

k时刻的一步状态预测值

k时刻的一步预测估计均方差

A2：量测更新

对一步预测均方差进行Cholesky分解

计算容积点，其中i＝1,2,3...,m；m＝2n

将容积点通过量测方程进行传递后，可得新的采样点

Z_i,k/k-1＝h(X_i,k/k-1)

k时刻的量测预测值为

自相关估计均方差为

互相关方差矩阵估计值为

系统滤波增益矩阵为

k时刻的状态估计值

k时刻的均方差估计值为

进一步地，所述步骤S4具体包括如下步骤：

B1：确定遗忘因子，生成加权系数

d_k＝(1-b)/(1-b^k),0＜b＜1

式中，b为遗忘因子，一般取0.95；d_k为生成k+1时刻噪声协方差矩阵的加权系数；

B2：计算系统噪声的协方差矩阵

将上式改写为

式中，ε_k为残差值；Q'_k为k时刻生成的系统噪声协方差矩阵；G_k＝(U^TU)^-1U^T,U为系统噪声驱动阵；

B3：计算量测噪声的协方差矩阵

式中，R'_k为k时刻生成的量测噪声协方差矩阵。

进一步地，所述步骤S5中强跟踪算法中的渐消因子的计算方式为：

M_k+1＝P_zz,k+1/k-V_k+1+N_k+1

进一步地，所述步骤S5中渐消因子的引入方式为：

在步骤A1中的一步预测估计均方差P_k∣k-1中引入渐消因子，如下

进一步地，所述步骤S5中结合AUV运动学模型的运动方程，估算AUV位置的方法为：

描述AUV运动的状态方程为：

由上式可得，AUV导航系统的状态向量X_k由AUV的位置信息(x_k,y_k)，航向角信息θ_k表示：

X_k＝[x_k y_k θ_k]^T

量测方程为

信息协方差矩阵为

M_k+1＝P_zz,k+1/k-V_k+1+N_k+1

渐消因子为

修正k时刻的一步预测估计均方差

噪声协方差矩阵

基于上述方案，采用步骤A2的量测更新方式，获取到AUV位置的估算数据。

本发明中CKF通过选取一组满足求容积准则的、具有相同权重的采样点，经过非线性方程的转换，产生新的点集来给出下一时刻系统状态预测的概率密度函数。通过点估计的方法，CKF避免了对非线性系统模型的线性化近似处理，从而可以达到三阶泰勒展开的精度。

在多AUV协同定位中，本发明针对协同定位精度问题，对强跟踪自适应容积卡尔曼滤波算法(STACKF)进行改进。本发明已知目标模型，得到协同导航滤波过程；通过水声通信设备得到主AUV位置信息以及主从AUV二者信息距离后，即可利用此信息实现自身位置误差的协同校正；STACKF算法得到滤波的信息、预测的误差和滤波增益，从而计算出任意时刻的系统状态估计值。本发明所提算法通过点估计的方法，避免了对非线性系统模型的线性化近似处理，提升了精度；同时将强跟踪滤波以及自适应估计方法与容积卡尔曼滤波相结合，具有良好的自适应性。

有益效果：本发明与现有技术相比，不仅具有简单易行的优点，而且所提算法通过点估计的方法，避免了对非线性系统模型的线性化近似处理，提升了精度，通过协同定位算法将领航AUV的高精度导航定位信息融合后，跟随AUV的导航定位精度可以有效提高；同时将强跟踪滤波以及自适应估计方法与容积卡尔曼滤波相结合，具有良好的自适应性和鲁棒性。

附图说明

图1为双模型领航式AUV导航图；

图2为算法流程图；

图3仿真实验中AUV实际运动轨迹图；

图4仿真实验中跟随AUV运动轨迹图；

图5仿真实验中跟随AUV定位误差比较图。

具体实施方式

下面结合附图和具体实施例，进一步阐明本发明，应理解这些实施例仅用于说明本发明而不用于限制本发明的范围，在阅读了本发明之后，本领域技术人员对本发明的各种等价形式的修改均落于本申请所附权利要求所限定的范围。

如图1所示为双模型领航式AUV导航示意图，本发明提供一种基于STACKF的多AUV协同定位方法，用于实现AUV的位置估算，参照图2，其具体包括如下步骤：

S1：对AUV运动学模型建模，得到AUV运动学模型的运动方程：

式中，x_k，y_k，θ_k分别为AUV在二维投影平面中的横向坐标、纵向坐标和偏航角；V_k为前向合成速度；

为偏航角速度；T为采样周期；设噪声为高斯白噪声，则在噪声影响下的系统输入为：

其中，V_mk和

分别为AUV在t_k时刻的速度和偏航角速度的量测值；w_vk和w_ωk均为相互独立的零均值高斯白噪声。

S2：基于运动方程，建立主从式多AUV协同导航定位的量测模型，得到量测模型的量测方程：

其中，dx_k,k+1和dy_k,k+1分别为跟随AUV在采样周期时间内的运动失径；上标S为跟随AUV；上标M为领航AUV；A_k为k时刻主从AUV之间的距离。

S3：采用容积卡尔曼滤波(CKF)来求解多AUV协同导航中的状态估计值：

具体包括如下步骤：

A1：时间更新

假设后验概率密度函数

已知，对估计均方差做Cholesky分解，可得

计算容积点，其中i＝1,2,3...,m；m＝2n，有

将容积点通过状态方程进行传递后，可得新的采样点

k时刻的一步状态预测值

k时刻的一步预测估计均方差

A2：量测更新

对一步预测均方差进行Cholesky分解

计算容积点，其中i＝1,2,3...,m；m＝2n

将容积点通过量测方程进行传递后，可得新的采样点

Z_i,k/k-1＝h(X_i,k/k-1)

k时刻的量测预测值为

自相关估计均方差为

互相关方差矩阵估计值为

系统滤波增益矩阵为

k时刻的状态估计值

k时刻的均方差估计值为

S4：基于状态估计值和量测方程，采用基于渐消记忆指数加权法的噪声估值器算法生成系统噪声和量测噪声的协方差矩阵Q_k和R_k：

具体包括如下步骤：

B1：确定遗忘因子，生成加权系数

d_k＝(1-b)/(1-b^k),0＜b＜1

式中，b为遗忘因子，本实施例取0.95；d_k为生成k+1时刻噪声协方差矩阵的加权系数；

B2：计算系统噪声的协方差矩阵

将上式改写为

式中，ε_k为残差值；Q'_k为k时刻生成的系统噪声协方差矩阵；G_k＝(U^TU)^-1U^T,U为系统噪声驱动阵；

B3：计算量测噪声的协方差矩阵

式中，R'_k为k时刻生成的量测噪声协方差矩阵。

S5：将强跟踪算法中的渐消因子引入到步骤S3的容积卡尔曼滤波(CKF)的预测和更新方程中，修正多AUV协同导航中的状态估计值和系统噪声和量测噪声的协方差矩阵，有效避免模型不准确造成的滤波下降，再结合AUV运动学模型的运动方程，估算AUV位置，实现多AUV协同定位：

强跟踪算法中的渐消因子的计算方式为：

M_k+1＝P_zz,k+1/k-V_k+1+N_k+1

渐消因子的引入方式为：

在步骤A1中的一步预测估计均方差P_k∣k-1中引入渐消因子，如下

结合AUV运动学模型的运动方程，估算AUV位置的方法为：

描述AUV运动的状态方程为：

由上式可得，AUV导航系统的状态向量X_k由AUV的位置信息(x_k,y_k)，航向角信息θ_k表示：

X_k＝[x_k y_k θ_k]^T

量测方程为

信息协方差矩阵为

M_k+1＝P_zz,k+1/k-V_k+1+N_k+1

渐消因子为

修正k时刻的一步预测估计均方差

噪声协方差矩阵

基于上述方案，采用步骤A2的量测更新方式，获取到AUV位置的估算数据。

本实施例还提供一种基于改进STACKF的多AUV协同定位系统，该系统包括网络接口、存储器和处理器；其中，网络接口，用于在与其他外部网元之间进行收发信息过程中，实现信号的接收和发送；存储器，用于存储能够在所述处理器上运行的计算机程序指令；处理器，用于在运行计算机程序指令时，执行上述共识方法的步骤。

本实施例还提供一种计算机存储介质，该计算机存储介质存储有计算机程序，在处理器执行所述计算机程序时可实现以上所描述的方法。所述计算机可读介质可以被认为是有形的且非暂时性的。非暂时性有形计算机可读介质的非限制性示例包括非易失性存储器电路(例如闪存电路、可擦除可编程只读存储器电路或掩膜只读存储器电路)、易失性存储器电路(例如静态随机存取存储器电路或动态随机存取存储器电路)、磁存储介质(例如模拟或数字磁带或硬盘驱动器)和光存储介质(例如CD、DVD或蓝光光盘)等。计算机程序包括存储在至少一个非暂时性有形计算机可读介质上的处理器可执行指令。计算机程序还可以包括或依赖于存储的数据。计算机程序可以包括与专用计算机的硬件交互的基本输入/输出系统(BIOS)、与专用计算机的特定设备交互的设备驱动程序、一个或多个操作系统、用户应用程序、后台服务、后台应用程序等。

本领域内的技术人员应明白，本申请的实施例可提供为方法、系统、或计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。

本申请是参照根据本申请实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。

这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。

基于上述提供的方案，为了验证上述方案的有效性和实际效果，本实施例进行仿真实验来进行验证，实验仿真通过两个主AUV和一个从AUV进行，运动过程中，以主AUV的运动位置信息作为从AUV的滤波轨迹信息。假设主从AUV深度一致，即只在水平面进行二维模型的仿真分析。采样周期t为1s，仿真总时间为1500s，定位误差为0.1m，距离量测误差为0.1m。跟随AUV速度误差为0.1，主AUV和从AUV真实航行速度为1m/s。遗忘因子b取值为0.95。主AUV1起点(-300，0)，主AUV2起点(300，0)，均以初始航向角60°沿直线航行，跟随AUV起点(0，0)，以初始航向角60°做S形运动。仿真图具体如图3至图5所示。

根据图3至图5可见，本发明提供的基于改进STACKF滤波的协同定位算法，能有效提高从AUV的导航定位精度，定位误差减小。

Claims (8)

1.一种基于STACKF的多AUV协同定位方法，其特征在于，包括如下步骤：

S1：对AUV运动学模型建模，得到AUV运动学模型的运动方程；

S2：基于运动方程，建立主从式多AUV协同导航定位的量测模型，得到量测模型的量测方程；

S3：采用容积卡尔曼滤波来求解多AUV协同导航中的状态估计值；

S4：基于状态估计值和量测方程，采用基于渐消记忆指数加权法的噪声估值器算法生成系统噪声和量测噪声的协方差矩阵Q_k和R_k；

S5：将强跟踪算法中的渐消因子引入到步骤S3的容积卡尔曼滤波的预测和更新方程中，修正多AUV协同导航中的状态估计值和系统噪声和量测噪声的协方差矩阵，再结合AUV运动学模型的运动方程，估算AUV位置，实现多AUV协同定位。

2.根据权利要求1所述的一种基于STACKF的多AUV协同定位方法，其特征在于，所述步骤S1中运动学模型的运动方程为：

式中，x_k，y_k，θ_k分别为AUV在二维投影平面中的横向坐标、纵向坐标和偏航角；V_k为前向合成速度；

为偏航角速度；T为采样周期；设噪声为高斯白噪声，则在噪声影响下的系统输入为：

其中，V_mk和

分别为AUV在t_k时刻的速度和偏航角速度的量测值；w_vk和w_ωk均为相互独立的零均值高斯白噪声。

3.根据权利要求1所述的一种基于STACKF的多AUV协同定位方法，其特征在于，所述步骤S2中量测模型的量测方程为：

其中，dx_k,k+1和dy_k,k+1分别为跟随AUV在采样周期时间内的运动失径；上标S为跟随AUV；上标M为领航AUV；A_k为k时刻主从AUV之间的距离。

4.根据权利要求1所述的一种基于STACKF的多AUV协同定位方法，其特征在于，所述步骤S3具体包括如下步骤：

A1：时间更新

假设后验概率密度函数

已知，对估计均方差做Cholesky分解，可得

计算容积点，其中i＝1,2,3...,m；m＝2n，有

将容积点通过状态方程进行传递后，可得新的采样点

k时刻的一步状态预测值

k时刻的一步预测估计均方差

A2：量测更新

对一步预测均方差进行Cholesky分解

计算容积点，其中i＝1,2,3...,m；m＝2n

将容积点通过量测方程进行传递后，可得新的采样点

Z_i,k/k-1＝h(X_i,k/k-1)

k时刻的量测预测值为

自相关估计均方差为

互相关方差矩阵估计值为

系统滤波增益矩阵为

k时刻的状态估计值

k时刻的均方差估计值为

5.根据权利要求4所述的一种基于STACKF的多AUV协同定位方法，其特征在于，所述步骤S4具体包括如下步骤：

B1：确定遗忘因子，生成加权系数

d_k＝(1-b)/(1-b^k),0＜b＜1

式中，b为遗忘因子；d_k为生成k+1时刻噪声协方差矩阵的加权系数；

B2：计算系统噪声的协方差矩阵

将上式改写为

式中，ε_k为残差值；Q'_k为k时刻生成的系统噪声协方差矩阵；G_k＝(U^TU)^-1U^T,U为系统噪声驱动阵；

B3：计算量测噪声的协方差矩阵

式中，R'_k为k时刻生成的量测噪声协方差矩阵。

6.根据权利要求4所述的一种基于STACKF的多AUV协同定位方法，其特征在于，所述步骤S5中强跟踪算法中的渐消因子的计算方式为：

M_k+1＝P_zz,k+1/k-V_k+1+N_k+1

7.根据权利要求6所述的一种基于STACKF的多AUV协同定位方法，其特征在于，所述步骤S5中渐消因子的引入方式为：

在步骤A1中的一步预测估计均方差P_k∣k-1中引入渐消因子，如下

8.根据权利要求6所述的一种基于STACKF的多AUV协同定位方法，其特征在于，所述步骤S5中结合AUV运动学模型的运动方程，估算AUV位置的方法为：

描述AUV运动的状态方程为：

由上式可得，AUV导航系统的状态向量X_k由AUV的位置信息(x_k,y_k)，航向角信息θ_k表示：

X_k＝[x_k y_k θ_k]^T

量测方程为

信息协方差矩阵为

M_k+1＝P_zz,k+1/k-V_k+1+N_k+1

渐消因子为

修正k时刻的一步预测估计均方差

噪声协方差矩阵

基于上述方案，采用步骤A2的量测更新方式，获取到AUV位置的估算数据。

Images