CN113324547A - 基于迭代扩展rts平滑滤波算法的多auv协同定位方法 - Google Patents
基于迭代扩展rts平滑滤波算法的多auv协同定位方法 Download PDFInfo
- 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
Links
- 238000009499 grossing Methods 0.000 title claims abstract description 56
- 238000000034 method Methods 0.000 title claims abstract description 39
- 230000005540 biological transmission Effects 0.000 title description 4
- 238000001914 filtration Methods 0.000 claims abstract description 52
- 238000005259 measurement Methods 0.000 claims abstract description 17
- 238000005315 distribution function Methods 0.000 claims abstract description 9
- 239000011159 matrix material Substances 0.000 claims description 23
- 238000012937 correction Methods 0.000 claims description 12
- 230000008569 process Effects 0.000 claims description 12
- 239000000126 substance Substances 0.000 claims description 12
- 230000007704 transition Effects 0.000 claims description 12
- 238000005070 sampling Methods 0.000 claims description 6
- 238000009826 distribution Methods 0.000 claims description 4
- 230000015572 biosynthetic process Effects 0.000 claims description 3
- 238000003786 synthesis reaction Methods 0.000 claims description 3
- 238000004590 computer program Methods 0.000 description 13
- 238000010586 diagram Methods 0.000 description 12
- 238000004088 simulation Methods 0.000 description 7
- 238000003860 storage Methods 0.000 description 6
- 238000012545 processing Methods 0.000 description 5
- 238000004891 communication Methods 0.000 description 4
- 230000006870 function Effects 0.000 description 4
- 238000005516 engineering process Methods 0.000 description 2
- 230000003287 optical effect Effects 0.000 description 2
- 238000004458 analytical method Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 230000003993 interaction Effects 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
- G01C21/203—Specially adapted for sailing ships
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/11—Complex mathematical operations for solving equations, e.g. nonlinear equations, general mathematical optimization problems
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/16—Matrix 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
技术领域
本发明属于自主水下航行器领域，涉及水下主从式多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的运动方程为：
式中，xk，yk，θk分别为AUV在二维投影平面中的横向坐标、纵向坐标和偏航角；Vk为前向合成速度；
为偏航角速度；T为采样周期；
设噪声为高斯白噪声，则在噪声影响下的系统输入为：
其中，Vmk和
分别为AUV在tk时刻的速度和偏航角速度的量测值；wvk和wωk均为相互独立的零均值高斯白噪声。
进一步地，所述步骤S2中量测方程为：
其中，dxk,k+1和dyk,k+1分别为跟随AUV在采样周期时间内的运动失径；上标S为跟随AUV；上标M为领航AUV；Rk为k时刻主从AUV之间的距离。
进一步地，所述步骤S3具体包括如下过程：
A1：迭代EKF滤波：
A1-1)状态预测及迭代初始化：
其中，
为状态转移模型及过程噪声的Jacobian阵，
Pk∣k,0为状态向量及其协方差的迭代初始值；
A1-2)迭代更新状态及方差：
其中下标i对应当前迭代次数，Rk为观测噪声协方差阵，
Pk∣k,i分别为第i次迭代运算后更新的状态估计及其协方差；
A1-3)滤波结果存储：存储时间区间[0,T]内各时刻的滤波结果用于下一步的最优平滑；
A2：RTS区间平滑：
A2-1)初始化：用k＝T时刻的滤波结果初始化平滑器的状态及方差
其中，上标f表示迭代EKF滤波，s表示RTS平滑；
A2-2)平滑修正：对于各时刻k＝T-1,T-2,…,0，状态及方差的平滑修正递推公式为:
其中，
为k时刻的平滑增益，Fk+1|k为系统状态转移模型，
和
是迭代EKF滤波的状态一步预测值及其协方差，
和
是迭代EKF滤波后的状态估计值及其协方差，
和
分别为RTS平滑修正后k时刻的状态向量及其协方差；可知，平滑由k＝T-1开始计算到k＝0，由
逐步逆向递推，依次得到
A3：生成RTS-IEKF建议分布：
依据步骤A1、A2对各时刻的状态向量及协方差进行平滑修正，得到修正后的状态及方差序列为：
从而构造如下RTS-IEKF建议分布函数：
可知，平滑修正为滤波过程之后的处理，在实现中为缓解平滑滞后问题，对时间区间[0,T]实施分段平滑，取平滑步长Ts＝T/3,从而边滤波边平滑渐进生成建议分布函数。
进一步地，所述步骤S4具体包括如下步骤：
B1：描述AUV运动的状态方程为：
由上式可得，AUV导航系统的状态向量Xk由AUV的位置信息(xk,yk)，航向角信息θk表示：Xk＝[xk yk θk ωk]T；
B2：将AUV运动的状态方程与IEKF算法结合得到系统的状态转移方程为：
该方程的雅克比矩阵为：
B3：IEKF算法的观测方程由主从AUV系统的量测距离决定，假设主AUV的位置为(xki,yki)，从AUV的位置为(xkm,ykm)，系统的观测状态方程为：
对观测方程进行Taylor级数展开，得到该方程的雅克比矩阵为：
其中，
代表主从AUV之间的距离；
基于IEKF的多AUV协同导航算法的更新阶段用递推公式如下：
对k+1时刻的系统状态估计
和对应的协方差阵
有：
其中，
和
由如下递归方程可得：
由于IEKF采用的状态估计的多步迭代，通过使量测模型在状态更新值附近的多次线性化，在一定程度上修正了量测模型的线性化误差，即一定程度上修正了由泰勒级数近似引起的截断误差，从而使算法的稳定性增强。
进一步地，所述步骤S4中将RTS最优平滑算法与导航滤波算法相结合，并用此方法来估算AUV位置的具体方式如下：
C1：描述AUV运动的状态方程为：
由上式可得，AUV导航系统的状态向量Xk由AUV的位置信息(xk,yk)，航向角信息θk表示：Xk＝[xk yk θk ωk]T；
C2：将AUV运动的状态方程与RTS平滑算法结合得到系统的状态转移方程为：
该方程的雅克比矩阵为：
输入噪声的雅可比矩阵为：
C3：设k时刻的系统状态估计为
对应的协方差阵为
基于RTS区间平滑的多AUV协同导航算法的更新阶段用递推公式如下：
本发明解决水下通信频率低，传输速率慢等问题所造成的协同定位系统的更新频率降低的问题
有益效果：本发明与现有技术相比，引入迭代扩展RTS平滑滤波算法，RTS平滑能较好的抑制观测噪声，而且当观测噪声较大时，RTS平衡能稳定滤波结果，提高估算精度，解决了水下通信频率低，传输速率慢等原因所造成的协同定位系统的更新频率降低的问题，本发明方法不仅具有简单易行的优点，而且通过协同定位算法将领航AUV的高精度导航定位信息融合后，跟随AUV的导航定位精度可以有效提高，定位系统具有良好的准确性和稳定性。
附图说明
图1为本发明方法的结构框图；
图2为主从式AUV协同导航系统；
图3为仿真实验中AUV实际运动轨迹图；
图4为仿真实验中跟随AUV定位误差比较图；
图5为仿真实验中RTS协同误差比较图；
图6为仿真实验中跟随AUVekf平滑滤波与ekf滤波比较误差图。
具体实施方式
下面结合附图和具体实施例，进一步阐明本发明，应理解这些实施例仅用于说明本发明而不用于限制本发明的范围，在阅读了本发明之后，本领域技术人员对本发明的各种等价形式的修改均落于本申请所附权利要求所限定的范围。
本发明提供一种基于迭代扩展RTS平滑滤波算法的多AUV协同定位方法，参照图1，其包括如下步骤：
S1：对AUV运动学模型建模，得到其运动方程：
式中，xk，yk，θk分别为AUV在二维投影平面中的横向坐标、纵向坐标和偏航角；Vk为前向合成速度；
为偏航角速度；T为采样周期；
设噪声为高斯白噪声，则在噪声影响下的系统输入为：
其中，Vmk和
分别为AUV在tk时刻的速度和偏航角速度的量测值；wvk和wωk均为相互独立的零均值高斯白噪声。
S2：根据运动方程，参照图2，建立主从式多AUV协同导航定位的量测模型，得到量测方程：
其中，dxk,k+1和dyk,k+1分别为跟随AUV在采样周期时间内的运动失径；上标S为跟随AUV；上标M为领航AUV；Rk为k时刻主从AUV之间的距离。
S3：对非线性系统进行迭代EKF滤波得到状态和方差的最大后验估计，采用RTS最优平滑算法修正滤波结果，生成一种新的建议分布函数RTS-IEKF；
本实施例中具体包括如下过程：
A1：迭代EKF滤波：
A1-1)状态预测及迭代初始化：
其中，
为状态转移模型及过程噪声的Jacobian阵，
Pk∣k,0为状态向量及其协方差的迭代初始值；
A1-2)迭代更新状态及方差：
其中下标i对应当前迭代次数，Rk为观测噪声协方差阵，
Pk∣k,i分别为第i次迭代运算后更新的状态估计及其协方差；
A1-3)滤波结果存储：存储时间区间[0,T]内各时刻的滤波结果用于下一步的最优平滑；
A2：RTS区间平滑：
A2-1)初始化：用k＝T时刻的滤波结果初始化平滑器的状态及方差
其中，上标f表示迭代EKF滤波，s表示RTS平滑；
A2-2)平滑修正：对于各时刻k＝T-1,T-2,…,0，状态及方差的平滑修正递推公式为:
其中，
为k时刻的平滑增益，Fk+1|k为系统状态转移模型，
和
是迭代EKF滤波的状态一步预测值及其协方差，
和
是迭代EKF滤波后的状态估计值及其协方差，
和
分别为RTS平滑修正后k时刻的状态向量及其协方差；可知，平滑由k＝T-1开始计算到k＝0，由
逐步逆向递推，依次得到
A3：生成RTS-IEKF建议分布：
依据步骤A1、A2对各时刻的状态向量及协方差进行平滑修正，得到修正后的状态及方差序列为：
从而构造如下RTS-IEKF建议分布函数：
可知，平滑修正为滤波过程之后的处理，在实现中为缓解平滑滞后问题，对时间区间[0,T]实施分段平滑，取平滑步长Ts＝T/3,从而边滤波边平滑渐进生成建议分布函数。
S4：结合步骤S1的AUV的运动学方程与步骤S3的滤波算法，建立适用于协同导航系统的滤波算法，将RTS最优平滑算法与此滤波算法相结合，估算得到AUV的位置。
本实施例中步骤S4具体包括如下步骤：
B1：描述AUV运动的状态方程为：
由上式可得，AUV导航系统的状态向量Xk由AUV的位置信息(xk,yk)，航向角信息θk表示：Xk＝[xk yk θk ωk]T；
B2：将AUV运动的状态方程与IEKF算法结合得到系统的状态转移方程为：
该方程的雅克比矩阵为：
B3：IEKF算法的观测方程由主从AUV系统的量测距离决定，假设主AUV的位置为(xki,yki)，从AUV的位置为(xkm,ykm)，系统的观测状态方程为：
对观测方程进行Taylor级数展开，得到该方程的雅克比矩阵为：
其中，
代表主从AUV之间的距离；
基于IEKF的多AUV协同导航算法的更新阶段用递推公式如下：
对k+1时刻的系统状态估计
和对应的协方差阵
有：
其中，
和
由如下递归方程可得：
由于IEKF采用的状态估计的多步迭代，通过使量测模型在状态更新值附近的多次线性化，在一定程度上修正了量测模型的线性化误差，即一定程度上修正了由泰勒级数近似引起的截断误差，从而使算法的稳定性增强。
本实施例步骤S4中将RTS最优平滑算法与导航滤波算法相结合，并用此方法来估算AUV位置的具体方式如下：
C1：描述AUV运动的状态方程为：
由上式可得，AUV导航系统的状态向量Xk由AUV的位置信息(xk,yk)，航向角信息θk表示：Xk＝[xk yk θk ωk]T；
C2：将AUV运动的状态方程与RTS平滑算法结合得到系统的状态转移方程为：
该方程的雅克比矩阵为：
输入噪声的雅可比矩阵为：
C3：设k时刻的系统状态估计为
对应的协方差阵为
基于RTS区间平滑的多AUV协同导航算法的更新阶段用递推公式如下：
本实施例还提供一种基于迭代扩展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的运动方程为：
式中，xk，yk，θk分别为AUV在二维投影平面中的横向坐标、纵向坐标和偏航角；Vk为前向合成速度；
为偏航角速度；T为采样周期；
设噪声为高斯白噪声，则在噪声影响下的系统输入为：
其中，Vmk和
分别为AUV在tk时刻的速度和偏航角速度的量测值；wvk和wωk均为相互独立的零均值高斯白噪声。
3.根据权利要求1所述的一种基于迭代扩展RTS平滑滤波算法的多AUV协同定位方法，其特征在于，所述步骤S2中量测方程为：
其中，dxk,k+1和dyk,k+1分别为跟随AUV在采样周期时间内的运动失径；上标S为跟随AUV；上标M为领航AUV；Rk为k时刻主从AUV之间的距离。
4.根据权利要求1所述的一种基于迭代扩展RTS平滑滤波算法的多AUV协同定位方法，其特征在于，所述步骤S3具体包括如下过程：
A1：迭代EKF滤波：
A1-1)状态预测及迭代初始化：
其中，
为状态转移模型及过程噪声的Jacobian阵，
为状态向量及其协方差的迭代初始值；
A1-2)迭代更新状态及方差：
其中下标i对应当前迭代次数，Rk为观测噪声协方差阵，
分别为第i次迭代运算后更新的状态估计及其协方差；
A1-3)滤波结果存储：存储时间区间[0,T]内各时刻的滤波结果用于下一步的最优平滑；
A2：RTS区间平滑：
A2-1)初始化：用k＝T时刻的滤波结果初始化平滑器的状态及方差
其中,上标f表示迭代EKF滤波，s表示RTS平滑；
A2-2)平滑修正：对于各时刻k＝T-1,T-2,…,0，状态及方差的平滑修正递推公式为:
其中，
为k时刻的平滑增益，Fk+1|k为系统状态转移模型，
和
是迭代EKF滤波的状态一步预测值及其协方差，
和
是迭代EKF滤波后的状态估计值及其协方差，
和
分别为RTS平滑修正后k时刻的状态向量及其协方差；可知，平滑由k＝T-1开始计算到k＝0，由
逐步逆向递推，依次得到
A3：生成RTS-IEKF建议分布：
依据步骤A1、A2对各时刻的状态向量及协方差进行平滑修正，得到修正后的状态及方差序列为：
从而构造如下RTS-IEKF建议分布函数：
5.根据权利要求1所述的一种基于迭代扩展RTS平滑滤波算法的多AUV协同定位方法，其特征在于，所述步骤S4具体包括如下步骤：
B1：描述AUV运动的状态方程为：
由上式可得，AUV导航系统的状态向量Xk由AUV的位置信息(xk,yk)，航向角信息θk表示：Xk＝[xk yk θk ωk]T；
B2：将AUV运动的状态方程与IEKF算法结合得到系统的状态转移方程为：
该方程的雅克比矩阵为：
B3：IEKF算法的观测方程由主从AUV系统的量测距离决定，假设主AUV的位置为(xki,yki)，从AUV的位置为(xkm,ykm)，系统的观测状态方程为：
对观测方程进行Taylor级数展开，得到该方程的雅克比矩阵为：
其中，
代表主从AUV之间的距离；
基于IEKF的多AUV协同导航算法的更新阶段用递推公式如下：
对k+1时刻的系统状态估计
和对应的协方差阵
有：
其中，
和
由如下递归方程可得：
6.根据权利要求5所述的一种基于迭代扩展RTS平滑滤波算法的多AUV协同定位方法，其特征在于，所述步骤S4中将RTS最优平滑算法与导航滤波算法相结合，并用此方法来估算AUV位置的具体方式如下：
C1：描述AUV运动的状态方程为：
由上式可得，AUV导航系统的状态向量Xk由AUV的位置信息(xk,yk)，航向角信息θk表示：Xk＝[xk yk θk ωk]T；
C2：将AUV运动的状态方程与RTS平滑算法结合得到系统的状态转移方程为：
该方程的雅克比矩阵为：
输入噪声的雅可比矩阵为：
C3：设k时刻的系统状态估计为
对应的协方差阵为
基于RTS区间平滑的多AUV协同导航算法的更新阶段用递推公式如下：
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)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114370878A (zh) * | 2022-01-04 | 2022-04-19 | 江苏科技大学 | 一种基于stackf的多auv协同定位方法 |
Citations (6)
| 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协同导航方法 |
-
2021
- 2021-05-25 CN CN202110569681.3A patent/CN113324547A/zh not_active Withdrawn
Patent Citations (6)
| 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)
| Title |
|---|
| 曹蓓: "粒子滤波改进算法及其应用研究", 《中国优秀硕士学位论文全文库》, pages 35 - 47 * |
| 袁 华 润: "多 ＡＵＶ 协 同 导航 定位 算法研 究 与 性能 分析", 《中国优秀硕士学位论文全文库》, pages 22 - 40 * |
Cited By (2)
| 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 |