CN113324547A - 基于迭代扩展rts平滑滤波算法的多auv协同定位方法 - Google Patents

基于迭代扩展rts平滑滤波算法的多auv协同定位方法 Download PDF

Info

Publication number
CN113324547A
CN113324547A CN202110569681.3A CN202110569681A CN113324547A CN 113324547 A CN113324547 A CN 113324547A CN 202110569681 A CN202110569681 A CN 202110569681A CN 113324547 A CN113324547 A CN 113324547A
Authority
CN
China
Prior art keywords
auv
equation
state
rts
algorithm
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Withdrawn
Application number
CN202110569681.3A
Other languages
English (en)
Inventor
周萌萌
张冰
赵强
潘梦婷
左思雨
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Jiangsu University of Science and Technology
Original Assignee
Jiangsu University of Science and Technology
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Jiangsu University of Science and Technology filed Critical Jiangsu University of Science and Technology
Priority to CN202110569681.3A priority Critical patent/CN113324547A/zh
Publication of CN113324547A publication Critical patent/CN113324547A/zh
Withdrawn legal-status Critical Current

Links

Images

Classifications

    • GPHYSICS
    • G01MEASURING; TESTING
    • G01CMEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20Instruments for performing navigational calculations
    • G01C21/203Specially adapted for sailing ships
    • GPHYSICS
    • G06COMPUTING; CALCULATING OR COUNTING
    • G06FELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10Complex mathematical operations
    • G06F17/11Complex mathematical operations for solving equations, e.g. nonlinear equations, general mathematical optimization problems
    • GPHYSICS
    • G06COMPUTING; CALCULATING OR COUNTING
    • G06FELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10Complex mathematical operations
    • G06F17/16Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Mathematical Physics (AREA)
  • Data Mining & Analysis (AREA)
  • Theoretical Computer Science (AREA)
  • Computational Mathematics (AREA)
  • Mathematical Analysis (AREA)
  • Mathematical Optimization (AREA)
  • Pure & Applied Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Software Systems (AREA)
  • Algebra (AREA)
  • Databases & Information Systems (AREA)
  • General Engineering & Computer Science (AREA)
  • Operations Research (AREA)
  • Automation & Control Theory (AREA)
  • Computing Systems (AREA)
  • Image Analysis (AREA)

Abstract

本发明公开了一种基于迭代扩展RTS平滑滤波算法的多AUV协同定位方法,包括如下步骤:对AUV运动学模型建模,得到其运动方程;根据运动方程,建立主从式多AUV协同导航定位的量测模型,得到量测方程;对非线性系统进行迭代EKF滤波得到状态和方差的最大后验估计,采用RTS最优平滑算法修正滤波结果,生成一种新的建议分布函数RTS‑IEKF;结合步骤S1的AUV的运动学方程与步骤S3的滤波算法,建立适用于协同导航系统的滤波算法,将RTS最优平滑算法与此滤波算法相结合,估算得到AUV的位置。本发明不仅具有简单易行的优点,而且通过协同定位算法将领航AUV的高精度导航定位信息融合后,跟随AUV的导航定位精度可以有效提高,定位系统具有良好的准确性和稳定性。

Description

基于迭代扩展RTS平滑滤波算法的多AUV协同定位方法
技术领域
本发明属于自主水下航行器领域,涉及水下主从式多AUV协同定位技术,具体涉及一种基于迭代扩展RTS平滑滤波算法的多AUV协同定位方法。
背景技术
自主水下航行器(Autonomous UnderwaterVehicles,AUV)在诸多军事和民事方面得到了广泛应用。单一AUV的使用已经不能满足当前的需要。因此,深入研究由多个AUV组成的协作系统有着重要的意义。在AUV协同系统中,配备低精度导航装置的航行器利用水声通信设备获取高精度航行器的位置信息以及二者间相对距离参考信息后,通过信息融合技术实现导航定位误差的协同校正。
目前对AUV位置进行估计所用的方法有基于状态空间模型的滤波技术以及基于间歇观测信息的卡尔曼滤波方法。卡尔曼滤波是一种常见的滤波算法,该滤波算法是建立在观测噪声为高斯噪声的基础上,但由于水下环境多变,往往观测噪声为非高斯分布,便会造成协同导航定位精度的降低。
发明内容
发明目的:为了克服现有技术中存在的不足,提供一种基于迭代扩展RTS平滑滤波算法的多AUV协同定位方法,其能够有效校正水下航行过程中的AUV位置误差,提高定位精度。
技术方案:为实现上述目的,本发明提供一种基于迭代扩展RTS平滑滤波算法的多AUV协同定位方法,包括如下步骤:
S1:对AUV运动学模型建模,得到其运动方程;
S2:根据运动方程,建立主从式多AUV协同导航定位的量测模型,得到量测方程;
S3:对非线性系统进行迭代EKF滤波得到状态和方差的最大后验估计,采用RTS最优平滑算法修正滤波结果,生成一种新的建议分布函数RTS-IEKF;
S4:结合步骤S1的AUV的运动学方程与步骤S3的滤波算法,建立适用于协同导航系统的滤波算法,将RTS最优平滑算法与此滤波算法相结合,估算得到AUV的位置。
进一步地,所述步骤S1中AUV的运动方程为:
Figure BDA0003082176040000011
式中,xk,yk,θk分别为AUV在二维投影平面中的横向坐标、纵向坐标和偏航角;Vk为前向合成速度;
Figure BDA00030821760400000211
为偏航角速度;T为采样周期;
设噪声为高斯白噪声,则在噪声影响下的系统输入为:
Figure BDA0003082176040000021
其中,Vmk
Figure BDA0003082176040000022
分别为AUV在tk时刻的速度和偏航角速度的量测值;wvk和wωk均为相互独立的零均值高斯白噪声。
进一步地,所述步骤S2中量测方程为:
Figure BDA0003082176040000023
其中,dxk,k+1和dyk,k+1分别为跟随AUV在采样周期时间内的运动失径;上标S为跟随AUV;上标M为领航AUV;Rk为k时刻主从AUV之间的距离。
进一步地,所述步骤S3具体包括如下过程:
A1:迭代EKF滤波:
A1-1)状态预测及迭代初始化:
Figure BDA0003082176040000024
Figure BDA0003082176040000025
其中,
Figure BDA0003082176040000026
为状态转移模型及过程噪声的Jacobian阵,
Figure BDA0003082176040000027
Pk∣k,0为状态向量及其协方差的迭代初始值;
A1-2)迭代更新状态及方差:
Figure BDA0003082176040000028
Figure BDA0003082176040000029
其中下标i对应当前迭代次数,Rk为观测噪声协方差阵,
Figure BDA00030821760400000210
Pk∣k,i分别为第i次迭代运算后更新的状态估计及其协方差;
A1-3)滤波结果存储:存储时间区间[0,T]内各时刻的滤波结果用于下一步的最优平滑;
A2:RTS区间平滑:
A2-1)初始化:用k=T时刻的滤波结果初始化平滑器的状态及方差
Figure BDA0003082176040000031
其中,上标f表示迭代EKF滤波,s表示RTS平滑;
A2-2)平滑修正:对于各时刻k=T-1,T-2,…,0,状态及方差的平滑修正递推公式为:
Figure BDA0003082176040000032
Figure BDA0003082176040000033
其中,
Figure BDA0003082176040000034
为k时刻的平滑增益,Fk+1|k为系统状态转移模型,
Figure BDA0003082176040000035
Figure BDA0003082176040000036
是迭代EKF滤波的状态一步预测值及其协方差,
Figure BDA0003082176040000037
Figure BDA0003082176040000038
是迭代EKF滤波后的状态估计值及其协方差,
Figure BDA0003082176040000039
Figure BDA00030821760400000310
分别为RTS平滑修正后k时刻的状态向量及其协方差;可知,平滑由k=T-1开始计算到k=0,由
Figure BDA00030821760400000311
逐步逆向递推,依次得到
Figure BDA00030821760400000312
A3:生成RTS-IEKF建议分布:
依据步骤A1、A2对各时刻的状态向量及协方差进行平滑修正,得到修正后的状态及方差序列为:
Figure BDA00030821760400000313
从而构造如下RTS-IEKF建议分布函数:
Figure BDA00030821760400000314
可知,平滑修正为滤波过程之后的处理,在实现中为缓解平滑滞后问题,对时间区间[0,T]实施分段平滑,取平滑步长Ts=T/3,从而边滤波边平滑渐进生成建议分布函数。
进一步地,所述步骤S4具体包括如下步骤:
B1:描述AUV运动的状态方程为:
Figure BDA00030821760400000315
由上式可得,AUV导航系统的状态向量Xk由AUV的位置信息(xk,yk),航向角信息θk表示:Xk=[xk yk θk ωk]T
B2:将AUV运动的状态方程与IEKF算法结合得到系统的状态转移方程为:
Figure BDA00030821760400000316
该方程的雅克比矩阵为:
Figure BDA0003082176040000041
B3:IEKF算法的观测方程由主从AUV系统的量测距离决定,假设主AUV的位置为(xki,yki),从AUV的位置为(xkm,ykm),系统的观测状态方程为:
Figure BDA0003082176040000042
对观测方程进行Taylor级数展开,得到该方程的雅克比矩阵为:
Figure BDA0003082176040000043
其中,
Figure BDA0003082176040000044
代表主从AUV之间的距离;
基于IEKF的多AUV协同导航算法的更新阶段用递推公式如下:
对k+1时刻的系统状态估计
Figure BDA0003082176040000045
和对应的协方差阵
Figure BDA0003082176040000046
有:
Figure BDA0003082176040000047
其中,
Figure BDA0003082176040000048
Figure BDA0003082176040000049
由如下递归方程可得:
Figure BDA00030821760400000410
Figure BDA00030821760400000411
Figure BDA00030821760400000412
Figure BDA00030821760400000413
由于IEKF采用的状态估计的多步迭代,通过使量测模型在状态更新值附近的多次线性化,在一定程度上修正了量测模型的线性化误差,即一定程度上修正了由泰勒级数近似引起的截断误差,从而使算法的稳定性增强。
进一步地,所述步骤S4中将RTS最优平滑算法与导航滤波算法相结合,并用此方法来估算AUV位置的具体方式如下:
C1:描述AUV运动的状态方程为:
Figure BDA0003082176040000051
由上式可得,AUV导航系统的状态向量Xk由AUV的位置信息(xk,yk),航向角信息θk表示:Xk=[xk yk θk ωk]T
C2:将AUV运动的状态方程与RTS平滑算法结合得到系统的状态转移方程为:
Figure BDA0003082176040000052
该方程的雅克比矩阵为:
Figure BDA0003082176040000053
输入噪声的雅可比矩阵为:
Figure BDA0003082176040000054
C3:设k时刻的系统状态估计为
Figure BDA0003082176040000055
对应的协方差阵为
Figure BDA0003082176040000056
基于RTS区间平滑的多AUV协同导航算法的更新阶段用递推公式如下:
Figure BDA0003082176040000057
Figure BDA0003082176040000058
本发明解决水下通信频率低,传输速率慢等问题所造成的协同定位系统的更新频率降低的问题
有益效果:本发明与现有技术相比,引入迭代扩展RTS平滑滤波算法,RTS平滑能较好的抑制观测噪声,而且当观测噪声较大时,RTS平衡能稳定滤波结果,提高估算精度,解决了水下通信频率低,传输速率慢等原因所造成的协同定位系统的更新频率降低的问题,本发明方法不仅具有简单易行的优点,而且通过协同定位算法将领航AUV的高精度导航定位信息融合后,跟随AUV的导航定位精度可以有效提高,定位系统具有良好的准确性和稳定性。
附图说明
图1为本发明方法的结构框图;
图2为主从式AUV协同导航系统;
图3为仿真实验中AUV实际运动轨迹图;
图4为仿真实验中跟随AUV定位误差比较图;
图5为仿真实验中RTS协同误差比较图;
图6为仿真实验中跟随AUVekf平滑滤波与ekf滤波比较误差图。
具体实施方式
下面结合附图和具体实施例,进一步阐明本发明,应理解这些实施例仅用于说明本发明而不用于限制本发明的范围,在阅读了本发明之后,本领域技术人员对本发明的各种等价形式的修改均落于本申请所附权利要求所限定的范围。
本发明提供一种基于迭代扩展RTS平滑滤波算法的多AUV协同定位方法,参照图1,其包括如下步骤:
S1:对AUV运动学模型建模,得到其运动方程:
Figure BDA0003082176040000061
式中,xk,yk,θk分别为AUV在二维投影平面中的横向坐标、纵向坐标和偏航角;Vk为前向合成速度;
Figure BDA0003082176040000062
为偏航角速度;T为采样周期;
设噪声为高斯白噪声,则在噪声影响下的系统输入为:
Figure BDA0003082176040000063
其中,Vmk
Figure BDA0003082176040000064
分别为AUV在tk时刻的速度和偏航角速度的量测值;wvk和wωk均为相互独立的零均值高斯白噪声。
S2:根据运动方程,参照图2,建立主从式多AUV协同导航定位的量测模型,得到量测方程:
Figure BDA0003082176040000065
其中,dxk,k+1和dyk,k+1分别为跟随AUV在采样周期时间内的运动失径;上标S为跟随AUV;上标M为领航AUV;Rk为k时刻主从AUV之间的距离。
S3:对非线性系统进行迭代EKF滤波得到状态和方差的最大后验估计,采用RTS最优平滑算法修正滤波结果,生成一种新的建议分布函数RTS-IEKF;
本实施例中具体包括如下过程:
A1:迭代EKF滤波:
A1-1)状态预测及迭代初始化:
Figure BDA0003082176040000071
Figure BDA0003082176040000072
其中,
Figure BDA0003082176040000073
为状态转移模型及过程噪声的Jacobian阵,
Figure BDA0003082176040000074
Pk∣k,0为状态向量及其协方差的迭代初始值;
A1-2)迭代更新状态及方差:
Figure BDA0003082176040000075
Figure BDA0003082176040000076
其中下标i对应当前迭代次数,Rk为观测噪声协方差阵,
Figure BDA0003082176040000077
Pk∣k,i分别为第i次迭代运算后更新的状态估计及其协方差;
A1-3)滤波结果存储:存储时间区间[0,T]内各时刻的滤波结果用于下一步的最优平滑;
A2:RTS区间平滑:
A2-1)初始化:用k=T时刻的滤波结果初始化平滑器的状态及方差
Figure BDA0003082176040000078
其中,上标f表示迭代EKF滤波,s表示RTS平滑;
A2-2)平滑修正:对于各时刻k=T-1,T-2,…,0,状态及方差的平滑修正递推公式为:
Figure BDA0003082176040000079
Figure BDA00030821760400000710
其中,
Figure BDA0003082176040000081
为k时刻的平滑增益,Fk+1|k为系统状态转移模型,
Figure BDA0003082176040000082
Figure BDA0003082176040000083
是迭代EKF滤波的状态一步预测值及其协方差,
Figure BDA0003082176040000084
Figure BDA0003082176040000085
是迭代EKF滤波后的状态估计值及其协方差,
Figure BDA0003082176040000086
Figure BDA0003082176040000087
分别为RTS平滑修正后k时刻的状态向量及其协方差;可知,平滑由k=T-1开始计算到k=0,由
Figure BDA0003082176040000088
逐步逆向递推,依次得到
Figure BDA0003082176040000089
A3:生成RTS-IEKF建议分布:
依据步骤A1、A2对各时刻的状态向量及协方差进行平滑修正,得到修正后的状态及方差序列为:
Figure BDA00030821760400000810
从而构造如下RTS-IEKF建议分布函数:
Figure BDA00030821760400000811
可知,平滑修正为滤波过程之后的处理,在实现中为缓解平滑滞后问题,对时间区间[0,T]实施分段平滑,取平滑步长Ts=T/3,从而边滤波边平滑渐进生成建议分布函数。
S4:结合步骤S1的AUV的运动学方程与步骤S3的滤波算法,建立适用于协同导航系统的滤波算法,将RTS最优平滑算法与此滤波算法相结合,估算得到AUV的位置。
本实施例中步骤S4具体包括如下步骤:
B1:描述AUV运动的状态方程为:
Figure BDA00030821760400000812
由上式可得,AUV导航系统的状态向量Xk由AUV的位置信息(xk,yk),航向角信息θk表示:Xk=[xk yk θk ωk]T
B2:将AUV运动的状态方程与IEKF算法结合得到系统的状态转移方程为:
Figure BDA00030821760400000813
该方程的雅克比矩阵为:
Figure BDA00030821760400000814
B3:IEKF算法的观测方程由主从AUV系统的量测距离决定,假设主AUV的位置为(xki,yki),从AUV的位置为(xkm,ykm),系统的观测状态方程为:
Figure BDA00030821760400000815
对观测方程进行Taylor级数展开,得到该方程的雅克比矩阵为:
Figure BDA0003082176040000091
其中,
Figure BDA0003082176040000092
代表主从AUV之间的距离;
基于IEKF的多AUV协同导航算法的更新阶段用递推公式如下:
对k+1时刻的系统状态估计
Figure BDA0003082176040000093
和对应的协方差阵
Figure BDA0003082176040000094
有:
Figure BDA0003082176040000095
其中,
Figure BDA0003082176040000096
Figure BDA0003082176040000097
由如下递归方程可得:
Figure BDA0003082176040000098
Figure BDA0003082176040000099
Figure BDA00030821760400000910
Figure BDA00030821760400000911
由于IEKF采用的状态估计的多步迭代,通过使量测模型在状态更新值附近的多次线性化,在一定程度上修正了量测模型的线性化误差,即一定程度上修正了由泰勒级数近似引起的截断误差,从而使算法的稳定性增强。
本实施例步骤S4中将RTS最优平滑算法与导航滤波算法相结合,并用此方法来估算AUV位置的具体方式如下:
C1:描述AUV运动的状态方程为:
Figure BDA00030821760400000912
由上式可得,AUV导航系统的状态向量Xk由AUV的位置信息(xk,yk),航向角信息θk表示:Xk=[xk yk θk ωk]T
C2:将AUV运动的状态方程与RTS平滑算法结合得到系统的状态转移方程为:
Figure BDA0003082176040000101
该方程的雅克比矩阵为:
Figure BDA0003082176040000102
输入噪声的雅可比矩阵为:
Figure BDA0003082176040000103
C3:设k时刻的系统状态估计为
Figure BDA0003082176040000104
对应的协方差阵为
Figure BDA0003082176040000105
基于RTS区间平滑的多AUV协同导航算法的更新阶段用递推公式如下:
Figure BDA0003082176040000106
Figure BDA0003082176040000107
本实施例还提供一种基于迭代扩展RTS平滑滤波算法的多AUV协同定位系统,该系统包括网络接口、存储器和处理器;其中,网络接口,用于在与其他外部网元之间进行收发信息过程中,实现信号的接收和发送;存储器,用于存储能够在所述处理器上运行的计算机程序指令;处理器,用于在运行计算机程序指令时,执行上述共识方法的步骤。
本实施例还提供一种计算机存储介质,该计算机存储介质存储有计算机程序,在处理器执行所述计算机程序时可实现以上所描述的方法。所述计算机可读介质可以被认为是有形的且非暂时性的。非暂时性有形计算机可读介质的非限制性示例包括非易失性存储器电路(例如闪存电路、可擦除可编程只读存储器电路或掩膜只读存储器电路)、易失性存储器电路(例如静态随机存取存储器电路或动态随机存取存储器电路)、磁存储介质(例如模拟或数字磁带或硬盘驱动器)和光存储介质(例如CD、DVD或蓝光光盘)等。计算机程序包括存储在至少一个非暂时性有形计算机可读介质上的处理器可执行指令。计算机程序还可以包括或依赖于存储的数据。计算机程序可以包括与专用计算机的硬件交互的基本输入/输出系统(BIOS)、与专用计算机的特定设备交互的设备驱动程序、一个或多个操作系统、用户应用程序、后台服务、后台应用程序等。
本领域内的技术人员应明白,本申请的实施例可提供为方法、系统、或计算机程序产品。因此,本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且,本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。
本申请是参照根据本申请实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器,使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中,使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品,该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上,使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理,从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
基于上述方案,为了验证本发明方法的效果,进行仿真实验,具体为:参照图2,通过一个主AUV和一个从AUV进行,运动过程中,以主AUV的运动位置信息作为从AUV的滤波轨迹信息。假设主从AUV的通信周期为5s,即主从AUV每隔5s进行一次信息交互,主AUV广播自身的位置信息,从AUV接收主AUV的位置信息进行卡尔曼滤波处理并更新自身的位置。假设主从AUV深度一致,即只在水平面进行二维模型的仿真分析。当AUV进行直线运动时,主AUV的起始位置信息为(0,0),运动过程中的航速为3m/s,航行时间为1000s,系统误差矩阵为Q=diag[0.052 0.052],量测噪声矩阵为R=diag[52 0.052 0.052]。
具体的仿真结果如图3至图6所示。
如图3所示:分别为主AUV的航行轨迹,从AUV的真实轨迹,从AUV的航位推算轨迹以及从AUV的滤波过后的轨迹。由图可知,随着时间的推移,从AUV的航推轨迹与真实轨迹偏差越来越大,而经过滤波之后的从AUV航行轨迹与真实轨迹相近,表明从AUV的滤波轨迹性能较好。
由图4可知,随着时间的增大,从AUV的航推误差越来越大,而经过滤波过后的误差变化波动很小,幅度平缓。
图5所示为协同误差和RTS平滑协同误差的对比,由图可知,RTS平滑误差变化比协同误差小。
图6所示为从AUVekf平滑滤波与ekf平滑滤波相比较,由图可知,两者之间误差精度较小。
实验证明,基于RTS-IEKF滤波的协同定位算法,能有效提高从AUV的导航定位精度,定位误差减小。

Claims (6)

1.一种基于迭代扩展RTS平滑滤波算法的多AUV协同定位方法,其特征在于,包括如下步骤:
S1:对AUV运动学模型建模,得到其运动方程;
S2:根据运动方程,建立主从式多AUV协同导航定位的量测模型,得到量测方程;
S3:对非线性系统进行迭代EKF滤波得到状态和方差的最大后验估计,采用RTS最优平滑算法修正滤波结果,生成一种新的建议分布函数RTS-IEKF;
S4:结合步骤S1的AUV的运动学方程与步骤S3的滤波算法,建立适用于协同导航系统的滤波算法,将RTS最优平滑算法与此滤波算法相结合,估算得到AUV的位置。
2.根据权利要求1所述的一种基于迭代扩展RTS平滑滤波算法的多AUV协同定位方法,其特征在于,所述步骤S1中AUV的运动方程为:
Figure FDA0003082176030000011
式中,xk,yk,θk分别为AUV在二维投影平面中的横向坐标、纵向坐标和偏航角;Vk为前向合成速度;
Figure FDA0003082176030000016
为偏航角速度;T为采样周期;
设噪声为高斯白噪声,则在噪声影响下的系统输入为:
Figure FDA0003082176030000012
其中,Vmk
Figure FDA0003082176030000013
分别为AUV在tk时刻的速度和偏航角速度的量测值;wvk和wωk均为相互独立的零均值高斯白噪声。
3.根据权利要求1所述的一种基于迭代扩展RTS平滑滤波算法的多AUV协同定位方法,其特征在于,所述步骤S2中量测方程为:
Figure FDA0003082176030000014
其中,dxk,k+1和dyk,k+1分别为跟随AUV在采样周期时间内的运动失径;上标S为跟随AUV;上标M为领航AUV;Rk为k时刻主从AUV之间的距离。
4.根据权利要求1所述的一种基于迭代扩展RTS平滑滤波算法的多AUV协同定位方法,其特征在于,所述步骤S3具体包括如下过程:
A1:迭代EKF滤波:
A1-1)状态预测及迭代初始化:
Figure FDA0003082176030000015
Figure FDA0003082176030000021
其中,
Figure FDA0003082176030000022
为状态转移模型及过程噪声的Jacobian阵,
Figure FDA0003082176030000023
为状态向量及其协方差的迭代初始值;
A1-2)迭代更新状态及方差:
Figure FDA0003082176030000024
Figure FDA0003082176030000025
其中下标i对应当前迭代次数,Rk为观测噪声协方差阵,
Figure FDA0003082176030000026
分别为第i次迭代运算后更新的状态估计及其协方差;
A1-3)滤波结果存储:存储时间区间[0,T]内各时刻的滤波结果用于下一步的最优平滑;
A2:RTS区间平滑:
A2-1)初始化:用k=T时刻的滤波结果初始化平滑器的状态及方差
Figure FDA0003082176030000027
其中,上标f表示迭代EKF滤波,s表示RTS平滑;
A2-2)平滑修正:对于各时刻k=T-1,T-2,…,0,状态及方差的平滑修正递推公式为:
Figure FDA0003082176030000028
Figure FDA0003082176030000029
其中,
Figure FDA00030821760300000210
为k时刻的平滑增益,Fk+1|k为系统状态转移模型,
Figure FDA00030821760300000211
Figure FDA00030821760300000212
是迭代EKF滤波的状态一步预测值及其协方差,
Figure FDA00030821760300000213
Figure FDA00030821760300000214
是迭代EKF滤波后的状态估计值及其协方差,
Figure FDA00030821760300000215
Figure FDA00030821760300000216
分别为RTS平滑修正后k时刻的状态向量及其协方差;可知,平滑由k=T-1开始计算到k=0,由
Figure FDA00030821760300000217
逐步逆向递推,依次得到
Figure FDA00030821760300000218
A3:生成RTS-IEKF建议分布:
依据步骤A1、A2对各时刻的状态向量及协方差进行平滑修正,得到修正后的状态及方差序列为:
Figure FDA00030821760300000219
从而构造如下RTS-IEKF建议分布函数:
Figure FDA0003082176030000031
5.根据权利要求1所述的一种基于迭代扩展RTS平滑滤波算法的多AUV协同定位方法,其特征在于,所述步骤S4具体包括如下步骤:
B1:描述AUV运动的状态方程为:
Figure FDA0003082176030000032
由上式可得,AUV导航系统的状态向量Xk由AUV的位置信息(xk,yk),航向角信息θk表示:Xk=[xk yk θk ωk]T
B2:将AUV运动的状态方程与IEKF算法结合得到系统的状态转移方程为:
Figure FDA0003082176030000033
该方程的雅克比矩阵为:
Figure FDA0003082176030000034
B3:IEKF算法的观测方程由主从AUV系统的量测距离决定,假设主AUV的位置为(xki,yki),从AUV的位置为(xkm,ykm),系统的观测状态方程为:
Figure FDA0003082176030000035
对观测方程进行Taylor级数展开,得到该方程的雅克比矩阵为:
Figure FDA0003082176030000036
其中,
Figure FDA0003082176030000037
代表主从AUV之间的距离;
基于IEKF的多AUV协同导航算法的更新阶段用递推公式如下:
对k+1时刻的系统状态估计
Figure FDA0003082176030000038
和对应的协方差阵
Figure FDA0003082176030000039
有:
Figure FDA00030821760300000310
其中,
Figure FDA0003082176030000041
Figure FDA0003082176030000042
由如下递归方程可得:
Figure FDA0003082176030000043
Figure FDA0003082176030000044
Figure FDA0003082176030000045
Figure FDA0003082176030000046
6.根据权利要求5所述的一种基于迭代扩展RTS平滑滤波算法的多AUV协同定位方法,其特征在于,所述步骤S4中将RTS最优平滑算法与导航滤波算法相结合,并用此方法来估算AUV位置的具体方式如下:
C1:描述AUV运动的状态方程为:
Figure FDA0003082176030000047
由上式可得,AUV导航系统的状态向量Xk由AUV的位置信息(xk,yk),航向角信息θk表示:Xk=[xk yk θk ωk]T
C2:将AUV运动的状态方程与RTS平滑算法结合得到系统的状态转移方程为:
Figure FDA0003082176030000048
该方程的雅克比矩阵为:
Figure FDA0003082176030000049
输入噪声的雅可比矩阵为:
Figure FDA00030821760300000410
C3:设k时刻的系统状态估计为
Figure FDA0003082176030000051
对应的协方差阵为
Figure FDA0003082176030000052
基于RTS区间平滑的多AUV协同导航算法的更新阶段用递推公式如下:
Figure FDA0003082176030000053
Figure FDA0003082176030000054
CN202110569681.3A 2021-05-25 2021-05-25 基于迭代扩展rts平滑滤波算法的多auv协同定位方法 Withdrawn CN113324547A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110569681.3A CN113324547A (zh) 2021-05-25 2021-05-25 基于迭代扩展rts平滑滤波算法的多auv协同定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110569681.3A CN113324547A (zh) 2021-05-25 2021-05-25 基于迭代扩展rts平滑滤波算法的多auv协同定位方法

Publications (1)

Publication Number Publication Date
CN113324547A true CN113324547A (zh) 2021-08-31

Family

ID=77416663

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110569681.3A Withdrawn CN113324547A (zh) 2021-05-25 2021-05-25 基于迭代扩展rts平滑滤波算法的多auv协同定位方法

Country Status (1)

Country Link
CN (1) CN113324547A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114370878A (zh) * 2022-01-04 2022-04-19 江苏科技大学 一种基于stackf的多auv协同定位方法

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103471595A (zh) * 2013-09-26 2013-12-25 东南大学 一种面向ins/wsn室内移动机器人紧组合导航的迭代扩展rts均值滤波方法
CN106525042A (zh) * 2016-09-27 2017-03-22 哈尔滨工程大学 一种基于蚁群与扩展卡尔曼滤波相结合的多auv协同定位方法
CN108195376A (zh) * 2017-12-13 2018-06-22 天津津航计算技术研究所 小型无人机自主导航定位方法
CN108681621A (zh) * 2018-04-09 2018-10-19 郑州轻工业学院 基于Chebyshev正交多项式扩展RTS Kalman平滑方法
CN109596128A (zh) * 2019-01-14 2019-04-09 哈尔滨工程大学 一种基于多水听器提高多auv协同定位性能的方法
CN109974706A (zh) * 2019-03-08 2019-07-05 哈尔滨工程大学 一种基于双运动模型的主从式多auv协同导航方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103471595A (zh) * 2013-09-26 2013-12-25 东南大学 一种面向ins/wsn室内移动机器人紧组合导航的迭代扩展rts均值滤波方法
CN106525042A (zh) * 2016-09-27 2017-03-22 哈尔滨工程大学 一种基于蚁群与扩展卡尔曼滤波相结合的多auv协同定位方法
CN108195376A (zh) * 2017-12-13 2018-06-22 天津津航计算技术研究所 小型无人机自主导航定位方法
CN108681621A (zh) * 2018-04-09 2018-10-19 郑州轻工业学院 基于Chebyshev正交多项式扩展RTS Kalman平滑方法
CN109596128A (zh) * 2019-01-14 2019-04-09 哈尔滨工程大学 一种基于多水听器提高多auv协同定位性能的方法
CN109974706A (zh) * 2019-03-08 2019-07-05 哈尔滨工程大学 一种基于双运动模型的主从式多auv协同导航方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
曹蓓: "粒子滤波改进算法及其应用研究", 《中国优秀硕士学位论文全文库》, pages 35 - 47 *
袁 华 润: "多 AUV 协 同 导航 定位 算法研 究 与 性能 分析", 《中国优秀硕士学位论文全文库》, pages 22 - 40 *

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114370878A (zh) * 2022-01-04 2022-04-19 江苏科技大学 一种基于stackf的多auv协同定位方法
CN114370878B (zh) * 2022-01-04 2023-10-27 江苏科技大学 一种基于stackf的多auv协同定位方法

Similar Documents

Publication Publication Date Title
CN108759833B (zh) 一种基于先验地图的智能车辆定位方法
CN114370878B (zh) 一种基于stackf的多auv协同定位方法
JP5901504B2 (ja) 宇宙船の動きを制御するシステム及び方法
CN111983926B (zh) 一种最大协熵扩展椭球集员滤波方法
Crouse A General Solution to Optimal Fixed-Gain ($\alpha $-$\beta $-$\gamma $ etc.) Filters
CN112671373B (zh) 一种基于误差控制的卡尔曼滤波自适应滤波算法
CN102999696A (zh) 噪声相关系统基于容积信息滤波的纯方位跟踪方法
CN113324547A (zh) 基于迭代扩展rts平滑滤波算法的多auv协同定位方法
CN112269174A (zh) 基于交互式多模型信息融合的助推滑翔飞行器目标估计方法及系统
Shivam et al. Tracking control by the newton-raphson flow: Applications to autonomous vehicles
JP3894424B2 (ja) 目標追尾装置
Jwo et al. GPS navigation processing using the quaternion-based divided difference filter
CN113763434B (zh) 一种基于卡尔曼滤波多运动模型切换的目标轨迹预测方法
CN108681621A (zh) 基于Chebyshev正交多项式扩展RTS Kalman平滑方法
US20230078005A1 (en) Navigation assistance method for a mobile carrier
CN114323007A (zh) 一种载体运动状态估计方法及装置
Gruyer et al. Experimental comparison of Bayesian positioning methods based on multi-sensor data fusion
WO2018014449A1 (zh) 改善相对位置传感器性能的方法、装置及计算机存储介质
CN114139109A (zh) 一种目标跟踪方法、系统、设备、介质及数据处理终端
CN110712768B (zh) 一种控制力矩陀螺群初始框架位置确定方法
CN109571464B (zh) 一种基于惯性和二维码导航的机器人初始对准方法
CN113008239A (zh) 一种多auv协同定位鲁棒延迟滤波方法
Chen et al. On SINS/GPS integrated navigation filtering method aided by radial basis function neural network
CN111796271B (zh) 一种比例导引目的地约束下的目标跟踪方法及装置
CN111879323B (zh) 一种基于前端融合的航向角计算方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
WW01 Invention patent application withdrawn after publication

Application publication date: 20210831

WW01 Invention patent application withdrawn after publication