CN111678520A - 一种水面无人艇导航系统和导航方法 - Google Patents

一种水面无人艇导航系统和导航方法 Download PDF

Info

Publication number
CN111678520A
CN111678520A CN202010526549.XA CN202010526549A CN111678520A CN 111678520 A CN111678520 A CN 111678520A CN 202010526549 A CN202010526549 A CN 202010526549A CN 111678520 A CN111678520 A CN 111678520A
Authority
CN
China
Prior art keywords
filter
local
navigation
local filter
matrix
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.)
Pending
Application number
CN202010526549.XA
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.)
University of Shanghai for Science and Technology
Original Assignee
University of Shanghai for 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 University of Shanghai for Science and Technology filed Critical University of Shanghai for Science and Technology
Priority to CN202010526549.XA priority Critical patent/CN111678520A/zh
Publication of CN111678520A publication Critical patent/CN111678520A/zh
Pending legal-status Critical Current

Links

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
    • 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/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • 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

Abstract

本发明提出一种基于自适应数据共享因子联合滤波器的水面无人艇导航系统，包括：主导航和第一数量个子导航，全局滤波器，以及与每个所述子导航分别对应的局部滤波器，其中，每个所述子导航分别与相对应的所述局部滤波器相连，所述主导航与每个所述局部滤波器分别相连，所述主导航与所述全局滤波器相连。本发明提供的基于自适应数据共享因子联合滤波器的水面无人艇导航系统和导航方法，可以识别出精度最低的导航子系统，并且避免使用这一子系统的导航信息数据；同时，将精度更高的导航子系统作为参考源来修正整体导航系统的误差，可以大幅度降低传统导航系统的导航精误差。

Description

一种水面无人艇导航系统和导航方法
技术领域
本发明具体涉及一种基于自适应数据共享因子联合卡尔曼滤波器的水面无人艇导航系统和导航方法。
背景技术
对水面无人艇(以下称USV)来说，常见的导航系统会集成微机电惯导系统(MEMS-INS)、全球卫星定位系统(GPS)和多普勒测速仪(DVL)三种传感器。其中MEMS-INS可以提供连续导航解决方案，但是由于加速度计和陀螺仪的漂移，其可靠性会随着时间的推移而降低。所以在USV导航系统中，GPS和DVL被用作参考系统，用于矫正MEMS-INS的误差。但是这仍然不能消除整个导航系统的误差，误差的来源是由于恶劣的天气和DVL噪声的影响，而这会影响整个导航系统的效率。当GPS的信号质量甚至中断时，通常会将多普勒测速仪DVL与微机电惯导系统(MEMS-INS)集成在一起以修正其误差。而INS/DVL集成方法的效率受到多个参数的影响，例如DVL本身的相关噪声以及INS和DVL之间的变换矩阵。实际上，存在与DVL测量相关的噪声，这是无法被法检测或消除的，从而降低了INS/DVL集成导航系统的效率。如何避免由于单个子导航的精度下降而对整体导航系统精度产生的不利影响是亟待解决的问题。
当前常用的逼近常数的数据共享因子联合卡尔曼滤波器(Data Sharing FactorCombined Filter,DSFCF)方法由两个与主滤波器相联的局部滤波器组成。每个局部滤波器都与GPS或DVL等子导航之一连接，以处理其位置信息。该方法的重要参数是常量DSF，它是每个局部过滤器的“贡献”，并且在两个局部滤波器之间平分该参数(例如，对于两个本地滤波器，DSF值等于0.5)。其缺点是常数DSF实际上不能根据精确的导航误差来调整其值，这最终会影响整个USV导航系统的效率。
发明内容
本发明是为了解决上述问题而进行的，提供了一种基于自适应数据共享因子联合卡尔曼滤波器的水面无人艇导航系统，不仅稳定可靠，还可以大幅度降低传统导航系统的导航精误差。
为实现上述目的，本发明提出一种基于自适应数据共享因子联合卡尔曼滤波器的水面无人艇导航系统，具有这样的特征，包括：主导航和第一数量个子导航，全局滤波器，以及与每个所述子导航分别对应的局部滤波器，其中，每个所述子导航分别与相对应的所述局部滤波器相连，所述主导航与每个所述局部滤波器分别相连，所述主导航与所述全局滤波器相连。
另外，本发明提供的基于自适应数据共享因子联合卡尔曼滤波器的水面无人艇导航系统，还具有这样的特征，包括：作为所述主导航的微机电惯导系统，作为所述子导航的全球卫星定位系统和多普勒测速仪，所述全局滤波器，以及与所述全球卫星定位系统和所述多普勒测速仪分别对应的第一局部滤波器和第二局部滤波器，其中，所述全球卫星定位系统和所述多普勒测速仪分别与所述第一局部滤波器和所述第二局部滤波器相连，所述微机电惯导系统与所述全局滤波器相连，
所述微机电惯导系统与所述第一局部滤波器和所述第二局部滤波器分别相连。
另外，本发明还提供了一种运行上述基于自适应数据共享因子联合滤波器的水面无人艇导航系统的导航方法，具有这样的特征，包括以下步骤：
自适应参数获取：根据各所述局部滤波器以及第一预设规则、第二预设规则、第三预设规则、第四预设规则分别得到各所述子导航的自适应数据共享因子联合滤波器值；
数据分散处理：基于当前时刻的局部滤波器测量方程以及下一时刻的局部滤波器状态变量更新方程，根据第五预设规则对各所述局部滤波器进行滤波处理，得到各局部滤波器在下一时刻的状态滤波值
Figure BDA0002531717330000032
和各局部滤波器在下一时刻的状态向量的协方差的滤波值Pi(k+1/k+1)，作为局部最优估计结果；
数据融合处理：将所述局部最优估计结果输入全局滤波器，根据第六预设规则进行滤波，得到全局滤波器最终状态向量Xm和全局滤波器状态向量的协方差矩阵的最终滤波值Pm；
信息反馈校正：根据所述全局滤波器最终状态向量Xm和所述全局滤波器状态向量的协方差矩阵的最终滤波值Pm，根据第七预设规则调整所述全局滤波器中各子导航自适应数据共享因子联合滤波器值Bii。
另外，本发明提供的导航方法，还具有这样的特征，所述自适应参数获取步骤中，还包括以下步骤：
根据第一预设规则得到各局部滤波器的更新协方差Cik；
根据第二预设规则得到增加各更新协方差
Figure BDA0002531717330000031
作为自适应滤波器；
根据第三预设规则得到各更新协方差Cik与增加更新协方差
Figure BDA0002531717330000041
之间的数学相关性；
根据各所述更新协方差Cik与增加的更新协方差
Figure BDA0002531717330000042
之间的关系以及第四预设规则得到所述各子导航的自适应数据共享因子联合滤波器值。
另外，本发明提供的导航方法，还具有这样的特征，所述第一预设规则为：
Figure BDA0002531717330000043
其中，Cik是局部滤波器i在采样时刻k的更新协方差，
Rik是所述局部滤波器i在采样时刻k的的度量协方差，
Hik是所述局部滤波器i在采样时刻k的的度量矩阵，
Figure BDA0002531717330000044
是度量矩阵Hik的转置，
Figure BDA0002531717330000045
是度量协方差Rik的逆矩阵。
另外，本发明提供的导航方法，还具有这样的特征，所述第二预设规则包括：
Figure BDA0002531717330000046
其中，
Figure BDA0002531717330000047
是在采样时刻k所述局部滤波器i的所述增加更新协方差，
M是当前参考子导航的采样频率和性能的窗口大小，
ηij是增加的所述局部滤波器i的更新矩阵，j为ηij矩阵列向量，
Figure BDA0002531717330000048
是ηij的转置，
K是卡尔曼增益。
另外，本发明提供的导航方法，还具有这样的特征，所述第三预设规则为：
Figure BDA0002531717330000051
其中，∝ik为Cik与
Figure BDA0002531717330000052
之间的数学相关性，
tr(·)是矩阵的迹。
另外，本发明提供的导航方法，还具有这样的特征，所述第四预设规则为：
Figure BDA0002531717330000053
其中，Bik为采样时刻k的所述局部滤波器i对应的所述子导航i的自适应数据共享因子联合滤波器值，
Figure BDA0002531717330000054
为所述Cik和
Figure BDA0002531717330000055
之间的数学相关性的逆矩阵。
另外，本发明提供的导航方法，还具有这样的特征，所述当前时刻的局部滤波器测量方程Zi(k)以及下一时刻的局部滤波器状态变量更新方程Xi(k+1)分别为：
Zi(k)＝Hi(k)Xi(k)+Vi(k)
Figure BDA0002531717330000056
其中，k+1为采样时刻k的下一时刻，
Figure BDA0002531717330000057
是采样时刻k的时变状态转移矩阵，
Γ(k)是采样时刻k的系统干扰矩阵，
W(k)是采样时刻k的模型噪声向量，
Hi(k)是采样时刻k所述局部滤波器i的观察矩阵，
Vi(k)是采样时刻k所述局部滤波器i的测量噪声向量，
Xi(k)是采样时刻k所述局部滤波器i的状态向量，
Figure BDA0002531717330000058
δPi为微机电惯导系统位置误差，δvi为微机电惯导系统速度误差，δφi为微机电惯导系统高度误差，
Figure BDA00025317173300000612
为加速度计偏置误差，δεi为陀螺仪偏置误差，δvDVLi为多普勒测速仪速度误差，
Figure BDA0002531717330000061
为微机电惯导系统位置误差的转置，
Figure BDA0002531717330000062
为微机电惯导系统速度误差的转置，
Figure BDA0002531717330000063
为微机电惯导系统高度误差的转置，
Figure BDA0002531717330000064
为加速度计偏置误差的转置，
Figure BDA0002531717330000065
为陀螺仪偏置误差的转置，
Figure BDA0002531717330000066
为多普勒测速仪速度误差的转置。
另外，本发明提供的导航方法，还具有这样的特征，第五预设规则为：
Figure BDA0002531717330000067
Figure BDA0002531717330000068
Ki(k+1)＝Pi(k+1/k)Hi(k+1)(Hi(k+1)Pi(k+1/k)Hi(k+1)T+R)-1
其中，
Figure BDA0002531717330000069
是k+1时刻相对于采样时刻k局部滤波器i的局部最优估计结果，
Figure BDA00025317173300000610
是采样时刻k局部滤波器i的状态滤波值，
Pi(k+1/k)是k+1时刻相对于采样时刻k局部滤波器i的状态向量协方差矩阵，
Pi(k/k)是采样时刻k局部滤波器i的状态向量协方差矩阵的滤波值，
Figure BDA00025317173300000611
是采样时刻k的时变状态转移矩阵的转置，
Γ(k)T是采样时刻k的系统干扰矩阵的转置，
Q(k)是过程误差的协方差，
R是Vi(k)的协方差，
Ki(k+1)是k+1时刻局部滤波器i的增益阵，
Hi(k+1)是k+1时刻局部滤波器i的观察矩阵，
Hi(k+1)T是k+1时刻局部滤波器i的观察矩阵的转置。
另外，本发明提供的导航方法，还具有这样的特征，所述各局部滤波器在下一时刻的状态滤波值
Figure BDA0002531717330000071
和各局部滤波器在下一时刻的状态向量的协方差的滤波值Pi(k+1/k+1)分别为：
Figure BDA0002531717330000072
Pi(k+1/k+1)＝(1-Ki(k+1)Hi(k+1))Pi(k+1/k)
其中，Zi(k+1)是k+1时刻局部滤波器i的测量方程。
另外，本发明提供的导航方法，还具有这样的特征，其特征在于，所述局部滤波器i的测量方程Zi包含所述第一局部滤波器的观测测量值Z1和所述第二局部滤波器的观测值测量值Z2，分别为
Figure BDA0002531717330000073
z2＝[VN(MEMS) -VN(DVL) VE(MEMS) -VE(DVL)] (9)
其中，
Figure BDA0002531717330000074
为GPS和微机电惯导系统之间的纬度差，
(λMEMS-λGPS)为GPS和微机电惯导系统之间的经度差，
(VN(MEMS)-VN(DVL))为DVL和微机电惯导系统之间的向北速度差，
(VE(MEMS)-VE(DVL))为DVL和微机电惯导系统之间的向东速度差。
另外，本发明提供的导航方法，还具有这样的特征，所述第六预设规则包括：所述全局滤波器最终状态向量Xm和所述全局滤波器状态向量的协方差矩阵的最终滤波值Pm分别为
Figure BDA0002531717330000075
Figure BDA0002531717330000081
其中，n为局部滤波器的总数量，
Figure BDA0002531717330000082
为Pi的逆矩阵。
另外，本发明提供的导航方法，还具有这样的特征，所述第七预设规则包括：
Figure BDA0002531717330000083
其中，Bi为所述局部滤波器i对应的所述子导航i的自适应数据共享因子联合滤波器值，
PiK为卡尔曼增益下局部滤波器i状态向量的协方差的滤波值，
PiK -1为PiK的逆矩阵，
Ki为局部滤波器i的增益阵，
Hi为所述局部滤波器i的观察矩阵。
发明作用和效果
本发明所涉及的基于自适应数据共享因子联合卡尔曼滤波器的水面无人艇导航系统，在全局滤波器的融合算法中，使用了一种新的自适应DSF的贡献参数Bi。Bi代表联合滤波器中第i个子导航的比例精度值，并可通过Bi增减来同步调节第i个子导航的比例精度值，从而识别出精度最低的子导航，并且避免使用这一子系统的导航信息数据；同时，将精度更高的子导航作为参考源来修正整体导航系统的误差。另外，本发明中，与逼近常数DSFCF方法相比，自适应DSFCF方法非常稳定且可靠。因此，采用自适应DSFCF方法的GPS/DVL/MEMS-INS是提供可靠的USV导航系统的最佳选择。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图示出的结构获得其他的附图。
图1是本发明的基于自适应数据共享因子联合卡尔曼滤波器的水面无人艇导航系统的导航方法一实施例的流程模块图。
图2是本发明的基于自适应数据共享因子联合卡尔曼滤波器的水面无人艇导航系统一实施例的结构框图。
图3是本发明的基于自适应数据共享因子联合卡尔曼滤波器的导航系统一实施例的导航流程图。
本发明目的的实现、功能特点及优点将结合实施例，参照附图做进一步说明。
具体实施方式
以下参照附图及实施例对本发明所涉及的一种基于自适应数据共享因子联合卡尔曼滤波器的水面无人艇导航系统作详细的描述。以下结合附图对本发明的原理和特征进行描述，所举实例只用于解释本发明，并非用于限定本发明的范围。
本发明提出一种基于自适应数据共享因子联合滤波器的水面无人艇导航系统，包括：主导航和第一数量个子导航，全局滤波器，以及与每个所述子导航分别对应的局部滤波器，其中，每个所述子导航分别与相对应的所述局部滤波器相连，所述主导航与每个所述局部滤波器分别相连，所述主导航与所述全局滤波器相连。
图2是本发明的基于自适应数据共享因子联合卡尔曼滤波器的水面无人艇导航系统一实施例的结构框图。
如图2所示，本方法提出的自适应数据共享因子联合卡尔曼滤波器(Data SharingFactor Combined Filter,DSFCF)是基于联合卡尔曼滤波器(CombinedFilter,CF)改进而来，由两阶段的处理数据技术组成。第一步包括局部卡尔曼滤波器，每个局部卡尔曼滤波器由一个子导航(可以是GPS或DVL)连接以处理其位置信息。第二步是全局滤波器，用于处理和组合局部卡尔曼滤波器输出的信息。相应的，数据处理分为两个主要步骤，第一步是对数据进行分散处理,第二步是分散处理过的数据再进行全局融合。
更具体而言，在本发明中，在由多种传感器分别组成的子导航中选择一个信息全面，可靠性有保证的子导航作为参考源，使之与第一数量个其他子导航两两联合，分别形成第一数量个对数据进行分散处理的局部滤波器。
在一些实施例中，如图2所示，本发明提出的基于自适应数据共享因子联合卡尔曼滤波器的水面无人艇导航系统，包括：作为所述主导航的微机电惯导(以下称MEMS-INS)，作为所述子导航的全球卫星定位(以下称GPS)和多普勒测速仪(以下称DVL)，全局滤波器和与所述GPS和DVL分别对应的第一局部滤波器和第二局部滤波器，其中，所述GPS和DVL分别与所述第一局部滤波器和所述第二局部滤波器相连，所述MEMS-INS与所述第一局部滤波器和所述第二局部滤波器分别相连，所述MEMS-INS与所述全局滤波器相连。所述的全球卫星定位(以下称GPS)系统和多普勒测速仪(以下称DVL)作为导航参考源来纠正主导航系统MEMS-INS系统的误差。
图1是本发明的基于自适应数据共享因子联合卡尔曼滤波器的水面无人艇导航系统的导航方法一实施例的流程模块图。
图3是本发明的基于自适应数据共享因子联合卡尔曼滤波器的导航系统一实施例的导航流程图。
如图1、3所示，本发明还提供了一种运行上述基于自适应数据共享因子联合滤波器的水面无人艇导航系统的导航方法，包括以下步骤：
自适应参数获取模块：根据各所述局部滤波器以及第一预设规则、第二预设规则、第三预设规则、第四预设规则分别得到各所述子导航的(GPS和DVL)自适应数据共享因子联合滤波器(以下称DSF)值Bik。
具体的，自适应DSF值Bik中，Bi作为子导航i自适应数据共享因子联合滤波器值，代表联合滤波器中第i个子导航的比例精度值，k为采样时间。例如B1和B2可以分别代表GPS子导航和DVL子导航的比例精度值。当Bi的值增加时，该第i个子导航的比例精度值在全局滤波器中也会增加，反之亦然。与此同时，在全局滤波器中，精度降低的子导航的Bi值将减小。因此通过自适应DSF局部滤波器，可以分离出精度最低的子导航，并且避免使用这一子系统的导航信息数据。同时，将精度最高的子导航作为最终参考源来修正整体导航系统的误差。
在一些实施例中，B1和B2分别代表GPS子导航和DVL子导航的比例精度值。由于GPS和DVL精度的变化是影响整个USV导航系统精度的主要问题。因此，在一些实施例中，通过自适应DSF(Bi)连续记录GPS和DVL的精度。
另外，本发明提供的基于自适应数据共享因子联合卡尔曼滤波器的水面无人艇导航系统，在所述自适应参数获取模块中，还包括以下步骤：
根据第一预设规则得到各局部滤波器KF i的更新协方差Cik。
更具体而言，所述第一预设规则为：
Figure BDA0002531717330000121
其中，Cik是局部滤波器i在采样时刻k的更新协方差，
Rik是所述局部滤波器i在采样时刻k的度量协方差，
Hik是所述局部滤波器i在采样时刻k的度量矩阵，
Figure BDA0002531717330000122
是度量矩阵Hik的转置，
Figure BDA0002531717330000123
是度量协方差Rik的逆矩阵。
由于联合滤波器参数的更新受到未被计算在内的误差影响，例如未计算的故障偏差和未知的动态条件，因此更新协方差Cik内包含了未计算误差的影响，因为它直接包含在更新的计算中。
具体而言，当确切的动态方程式已知时，更新协方差等于Cik。当不能精确地知道动态方程，由于未知数据的影响，更新误差协方差Cik可能会增加。同样，当没有精确的测量方程式时，更新误差协方差Cik将会随着度量矩阵Rik的增加而增加。
随后，根据第二预设规则得到增加各更新协方差
Figure BDA0002531717330000131
作为自适应滤波器。
更具体而言，所述第二预设规则包括：
Figure BDA0002531717330000132
其中，
Figure BDA0002531717330000133
是在采样时刻k所述局部滤波器i的所述增加更新协方差，M是与所述局部滤波器i对应的所述子导航的采样频率和性能的窗口大小，ηij是增加的所述局部滤波器i的更新矩阵，j为ηij矩阵列向量，
Figure BDA0002531717330000134
是ηij的转置，
K是卡尔曼增益。
随后，根据第三预设规则得到各更新协方差Cik与增加更新协方差
Figure BDA0002531717330000135
之间的数学相关性。
更具体而言，所述第三预设规则为：
Figure BDA0002531717330000136
其中，∝ik为Cik与
Figure BDA0002531717330000137
之间的数学相关性，tr(·)是矩阵的迹。
当前采样时刻k下确切的测量噪声向量Vi(k)已知时，更新协方差Cik值将与增加的更新协方差
Figure BDA0002531717330000138
值相反，从而使∝ik值变为零，说明当前时间估算结果稳定性良好。而当测量噪声突然变化并且当前子导航在采样时刻k时的精度下降时，时间点k处的更新协方差Cik将偏离
Figure BDA0002531717330000139
并且∝ik值将增加，说明采样时刻k时估算结果稳定性不佳。因此∝ik值直接受到测量噪声的影响，且成正相关关系。
最后，根据所述数学相关性以及第四预设规则得到所述各子导航的自适应数据共享因子联合滤波器值Bik。
更具体而言，所述第四预设规则为：
Figure BDA0002531717330000141
其中，Bik 为采样时刻k的各所述子导航i的自适应数据共享因子联合滤波器值，
Figure BDA0002531717330000142
为所述Cik和
Figure BDA0002531717330000143
之间的数学相关性的逆矩阵。
当自适应DSF的值设置Bik和测量噪声之间存在较大差异时，说明观察不良，且该局部滤波器KF i的比例精度Bik值在全局滤波器中将减小(贡献参数变小)。类似地，当自适应DSF的值设置Bik和测量噪声之间的差很小时，说明观察良好，并且该局部滤波器(第i个)KFi的比例精度Bik值将在全局滤波器中增大(贡献参数变大)。因此，自适应DSF(Bi)可以提高整个联合卡尔曼滤波器的稳定性和准确性，从而提升USV整体导航系统的精度。
数据分散处理模块：基于当前时刻的局部滤波器测量方程以及下一时刻的局部滤波器状态变量更新方程，根据第五预设规则对各所述局部滤波器进行滤波处理，得到各局部滤波器在下一时刻的状态滤波值
Figure BDA0002531717330000144
和各局部滤波器在下一时刻的状态向量的协方差的滤波值Pi(k+1/k+1)，作为局部最优估计结果。
更具体而言，本发明提供的基于自适应数据共享因子联合卡尔曼滤波器的水面无人艇导航系统，所述当前时刻的局部滤波器测量方程Zi(k)以及下一时刻的局部滤波器状态变量更新方程Xi(k+1)分别为：
Zi(k)＝Hi(k)Xi(k)+Vi(k)
Figure BDA0002531717330000151
其中，k为当前时刻，k+1为下一时刻，
Figure BDA0002531717330000152
是采样时刻k的时变状态转移矩阵，
Γ(k)是采样时刻k的系统干扰矩阵，
W(k)是采样时刻k的模型噪声向量，
Hi(k)是采样时刻k所述局部滤波器i的观察矩阵，
Vi(k)是采样时刻k所述局部滤波器i的测量噪声向量，
Xi(k)是采样时刻k所述局部滤波器i的状态向量，
Figure BDA0002531717330000153
δPi为微机电惯导系统位置误差，δvi为微机电惯导系统速度误差，δφi为微机电惯导系统高度误差，
Figure BDA00025317173300001513
为加速度计偏置误差，δεi为陀螺仪偏置误差，δvDVLi为多普勒测速仪速度误差，
Figure BDA0002531717330000154
为微机电惯导系统位置误差的转置，
Figure BDA0002531717330000155
为微机电惯导系统速度误差的转置，
Figure BDA0002531717330000156
为微机电惯导系统高度误差的转置，
Figure BDA0002531717330000157
为加速度计偏置误差的转置，
Figure BDA0002531717330000158
为陀螺仪偏置误差的转置，
Figure BDA0002531717330000159
为多普勒测速仪速度误差的转置。
另外，所述第五预设规则为：
Figure BDA00025317173300001510
Figure BDA00025317173300001511
Ki(k+1)＝Pi(k+1/k)Hi(k+1)(Hi(k+1)Pi(k+1/k)Hi(k+1)T+R)-1
其中，
Figure BDA00025317173300001512
是k+1时刻相对于采样时刻k局部滤波器i的局部最优估计结果，
Figure BDA0002531717330000161
是采样时刻k局部滤波器i的状态滤波值，
Pi(k+1/k)是k+1时刻相对于采样时刻k局部滤波器i的状态向量协方差矩阵，
Pi(k/k)是采样时刻k局部滤波器i的状态向量协方差矩阵的滤波值，
Figure BDA0002531717330000162
是采样时刻k的时变状态转移矩阵的转置，
Γ(k)T是采样时刻k的系统干扰矩阵的转置，
Q(k)是过程误差的协方差，
R是Vi(k)的协方差，
Ki(k+1)是k+1时刻局部滤波器i的增益阵，
Hi(k+1)是k+1时刻局部滤波器i的观察矩阵，
Hi(k+1)T是k+1时刻局部滤波器i的观察矩阵的转置。
而所述各局部滤波器在下一时刻的状态滤波值
Figure BDA0002531717330000163
和各局部滤波器在下一时刻的状态向量的协方差的滤波值Pi(k+1/k+1)分别为：
Figure BDA0002531717330000164
Pi(k+1/k+1)＝(1-Ki(k+1)Hi(k+1))Pi(k+1/k)
其中，Zi(k+1)是k+1时刻局部滤波器i的测量方程。
更具体而言，可求得各局部滤波器i的测量方程，并进一步代入各局部滤波器在下一时刻的状态滤波值
Figure BDA0002531717330000165
另外，本发明提供的基于自适应数据共享因子联合卡尔曼滤波器的水面无人艇导航系统中，在一些实施例中，所述局部滤波器i的测量方程Zi包含局部滤波器1的观测测量值Z1和局部滤波器2的观测值测量值(Z2)，分别为
Figure BDA0002531717330000171
z2＝[VN(MEMS) -VN(DVL) VE(MEMS) -VE(DVL)] (9)
其中，
Figure BDA0002531717330000172
为GPS和微机电惯导系统之间的纬度差，
(λMEMS-λGPS)为GPS和微机电惯导系统之间的经度差，
(VN(MEMS)-VN(DVL))为DVL和微机电惯导系统之间的向北速度差，
(VE(MEMS)-VE(DVL))为DVL和微机电惯导系统之间的向东速度差。
数据融合处理模块：将所述局部最优估计结果输入全局滤波器，根据第六预设规则进行滤波，得到全局滤波器最终状态向量Xm和全局滤波器状态向量的协方差矩阵的最终滤波值Pm。
在本发明中，各局部滤波器根据状态方程和测量方程进行滤波，输出建立在局部测量基础上的最优估计结果Xi,Pi,(i＝1,2,……n)。在一些实施例中，通过两个局部卡尔曼滤波器KF(本地KF 1和本地KF 2)的测量结果Z1、Z2，得到本地KF 1和本地KF 2在下一时刻的状态滤波值
Figure BDA0002531717330000173
Figure BDA0002531717330000174
作为全局滤波器的数据输入，以估计最终结果。局部最优估计再在全局滤波器内按照融合算法合成,从而获得全局估计Xm,Pm。
更具体而言，所述第六预设规则包括：所述全局滤波器最终状态向量Xm和所述全局滤波器状态向量的协方差矩阵的最终滤波值Pm分别为
Figure BDA0002531717330000175
Figure BDA0002531717330000176
其中，n为局部滤波器的总数量，
Figure BDA0002531717330000181
为Pi的逆矩阵。
也可写作如下形式：
Figure BDA0002531717330000182
Figure BDA0002531717330000183
信息反馈校正模块：根据所述全局滤波器最终状态向量Xm和所述全局滤波器状态向量的协方差矩阵的最终滤波值Pm，根据第七预设规则调整所述全局滤波器中各子导航自适应数据共享因子联合滤波器值Bi。
更具体而言，通过两个局部卡尔曼滤波器KF(本地KF 1和本地KF 2)的测量结果Z1、Z2，得到本地KF 1和本地KF 2在下一时刻的状态滤波值
Figure BDA0002531717330000184
作为全局滤波器的数据输入，以估计最终结果。所述最终估计结果将反馈到MEMS-INS系统以纠正其错误。同时，自适应DSFCF的设置值(Bi)由局部滤波器KF 1和局部滤波器KF 2的数据输入(P1和P2)更新。即，全局滤波器将全局结果Xm和按信息分配原则形成的协方差矩阵Pm向各局部滤波器进行信息反馈，来保证系统有较好的容错性。所述第七预设规则包括：
Figure BDA0002531717330000185
其中，Bi为所述局部滤波器i对应的所述子导航i的自适应数据共享因子联合滤波器值，
PiK为卡尔曼增益下局部滤波器i状态向量的协方差的滤波值，
PiK -1为PiK的逆矩阵，
Ki为局部滤波器i的增益阵，
Hi为所述局部滤波器i的观察矩阵。
也可写作如下形式：
Figure BDA0002531717330000191
如上所述，所述信息反馈校正步骤中：当测量噪声突然变化，或在当前时刻k处有子导航精度下降时，全局估计Xm产生波动，当前时刻k处的更新协方差Cik偏离
Figure BDA0002531717330000192
∝ik值增加，根据所述第七预设规则，导致当前所述子导航对应的Bik下降，使该子导航在全局导航系统中的贡献比率下降，避免其精度下降对全局导航系统精度产生不利影响。
更具体而言，当在时间点k，地球表面某些位置的GPS精度降低时，会导致第1个局部卡尔曼滤波器KF 1的度量协方差R1k值不准确。同时，反映局部滤波器KF 1估计精度的P1值将不正确地增加，并且使第1个局部卡尔曼滤波器KF 1在时间点k处的更新协方差C1k，与从时间点k-M到时间点k之间增加的更新协方差
Figure BDA0002531717330000193
相比，C1k值有所增加。从而导致全局滤波器中第1个局部卡尔曼滤波器KF 1的自适应DSF比例精度B1k值减小。减小的B1k值将反馈到局部滤波器KF 1以校正和减小P1值，从而减少因GPS子导航精度降低带来的误差影响。
类似地，当DVL的精度由于在时间点k的未知测量噪声而降低时，导致第2个局部卡尔曼滤波器KF2的度量协方差R2k值不准确。同时，反映局部滤波器KF 2的估计精度的P2将不正确地增加，并使第2个局部卡尔曼滤波器KF2在时间点k处的更新协方差的C2k值，与从时间点k-M到时间点k之间增加的更新协方差
Figure BDA0002531717330000194
相比，C2k值有所增加。从而导致全局滤波器中自适应DSF比例精度B2k值减小，最后将更新数据反馈到局部滤波器KF 2以校正和减小P2值，从而减少因DVL子导航精度降低带来的误差影响。
实施例的作用与效果
根据本实施例所涉及的所涉及的基于自适应数据共享因子联合卡尔曼滤波器的水面无人艇导航系统，在全局滤波器的融合算法中，使用了一种新的自适应DSF的贡献参数Bi。Bi代表联合滤波器中第i个子导航的比例精度值，并可通过Bi增减来同步调节第i个子导航的比例精度值。从而识别出精度最低的子导航，并且避免使用这一子系统的导航信息数据；同时，将精度更高的子导航作为参考源来修正整体导航系统的误差。另外，本实施例中，与逼近常数DSFCF方法相比，自适应DSFCF方法非常稳定且可靠。因此，采用自适应DSFCF方法的GPS/DVL/MEMS-INS是提供可靠的USV导航系统的最佳选择。
上述实施方式为本发明的优选案例，并不用来限制本发明的保护范围。
上述本发明实施例序号仅仅为了描述，不代表实施例的优劣。
以上仅为本发明的优选实施例，并非因此限制本发明的专利范围，凡是利用本发明说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其他相关的技术领域，均同理包括在本发明的专利保护范围内。

Claims (14)

1.一种基于自适应数据共享因子联合滤波器的水面无人艇导航系统，其特征在于，包括：
主导航和第一数量个子导航，
全局滤波器，以及与每个所述子导航分别对应的局部滤波器，
其中，每个所述子导航分别与相对应的所述局部滤波器相连，
所述主导航与每个所述局部滤波器分别相连，
所述主导航与所述全局滤波器相连。
2.如权利要求1所述的基于自适应数据共享因子联合滤波器的水面无人艇导航系统，其特征在于，包括：
作为所述主导航的微机电惯导系统，
作为所述子导航的全球卫星定位系统和多普勒测速仪，
所述全局滤波器，
以及与所述全球卫星定位系统和所述多普勒测速仪分别对应的第一局部滤波器和第二局部滤波器，
其中，所述全球卫星定位系统和所述多普勒测速仪分别与所述第一局部滤波器和所述第二局部滤波器相连，
所述微机电惯导系统与所述全局滤波器相连，
所述微机电惯导系统与所述第一局部滤波器和所述第二局部滤波器分别相连。
3.一种运行权利要求1-2所述的基于自适应数据共享因子联合滤波器的水面无人艇导航系统的导航方法，其特征在于，包括以下步骤：
自适应参数获取：根据各所述局部滤波器以及第一预设规则、第二预设规则、第三预设规则、第四预设规则分别得到各所述子导航的自适应数据共享因子联合滤波器值；
数据分散处理：基于当前采样时刻的局部滤波器测量方程以及下一时刻的局部滤波器状态变量更新方程，根据第五预设规则对各所述局部滤波器进行滤波处理，得到各局部滤波器在下一时刻的状态滤波值
Figure FDA0002531717320000021
和各局部滤波器在下一时刻的状态向量的协方差的滤波值Pi(k+1/k+1)，作为局部最优估计结果；
数据融合处理：将所述局部最优估计结果输入全局滤波器，根据第六预设规则进行滤波，得到全局滤波器最终状态向量Xm和全局滤波器状态向量的协方差矩阵的最终滤波值Pm；
信息反馈校正：根据所述全局滤波器最终状态向量Xm和所述全局滤波器状态向量的协方差矩阵的最终滤波值Pm，根据第七预设规则调整所述全局滤波器中各子导航自适应数据共享因子联合滤波器值Bi。
4.根据权利要求3所述的导航方法，其特征在于，所述自适应参数获取步骤中，还包括以下步骤：
根据第一预设规则得到各局部滤波器的更新协方差Cik；
根据第二预设规则得到增加各更新协方差
Figure FDA0002531717320000022
作为自适应滤波器；
根据第三预设规则得到各更新协方差Cik与增加更新协方差
Figure FDA0002531717320000023
之间的数学相关性；
根据所述数学相关性以及第四预设规则得到所述各子导航的自适应数据共享因子联合滤波器值。
5.根据权利要求4所述的导航方法，其特征在于，所述第一预设规则为：
Figure FDA0002531717320000031
其中，Cik是局部滤波器i在采样时刻k的更新协方差，
Rik是所述局部滤波器i在采样时刻k的度量协方差，
Hik是所述局部滤波器i在采样时刻k的度量矩阵，
Figure FDA0002531717320000032
是度量矩阵Hik的转置，
Figure FDA0002531717320000033
是度量协方差Rik的逆矩阵。
6.根据权利要求5所述的导航方法，其特征在于，所述第二预设规则包括：
Figure FDA0002531717320000034
其中，
Figure FDA0002531717320000035
是在采样时刻k所述局部滤波器i的所述增加更新协方差，
M是与所述局部滤波器i对应的所述子导航的采样频率和性能的窗口大小，
ηij是增加的所述局部滤波器i的更新矩阵，j为ηij矩阵列向量，
Figure FDA0002531717320000036
是ηij的转置，
K是卡尔曼增益。
7.根据权利要求6所述的导航方法，其特征在于，所述第三预设规则为：
Figure FDA0002531717320000037
其中，∝ik为Cik与
Figure FDA0002531717320000038
之间的数学相关性，
tr(·)是矩阵的迹。
8.根据权利要求7所述的导航方法，其特征在于，所述第四预设规则为：
Figure FDA0002531717320000041
其中，Bik为采样时刻k的所述局部滤波器i对应的所述子导航i的自适应数据共享因子联合滤波器值，
Figure FDA0002531717320000042
为所述Cik和
Figure FDA0002531717320000043
之间的数学相关性的逆矩阵。
9.根据权利要求8所述的导航方法，其特征在于，所述当前采样时刻的局部滤波器测量方程Zi(k)以及下一时刻的局部滤波器状态变量更新方程Xi(k+1)分别为：
Zi(k)＝Hi(k)Xi(k)+Vi(k)
Figure FDA0002531717320000044
其中，k+1为采样时刻k的下一时刻，
Figure FDA0002531717320000045
是采样时刻k的时变状态转移矩阵，
Γ(k)是采样时刻k的系统干扰矩阵，
W(k)是采样时刻k的模型噪声向量，
Hi(k)是采样时刻k所述局部滤波器i的观察矩阵，
Vi(k)是采样时刻k所述局部滤波器i的测量噪声向量，
Xi(k)是采样时刻k所述局部滤波器i的状态向量，
Figure FDA0002531717320000046
δPi为微机电惯导系统位置误差，δvi为微机电惯导系统速度误差，δφi为微机电惯导系统高度误差，
Figure FDA0002531717320000051
为加速度计偏置误差，δεi为陀螺仪偏置误差，δvDVLi为多普勒测速仪速度误差，
Figure FDA0002531717320000052
为微机电惯导系统位置误差的转置，
Figure FDA0002531717320000053
为微机电惯导系统速度误差的转置，
Figure FDA0002531717320000054
为微机电惯导系统高度误差的转置，
Figure FDA0002531717320000055
为加速度计偏置误差的转置，
Figure FDA0002531717320000056
为陀螺仪偏置误差的转置，
Figure FDA0002531717320000057
为多普勒测速仪速度误差的转置。
10.根据权利要求9所述的导航方法，其特征在于，第五预设规则为：
Figure FDA0002531717320000058
Figure FDA0002531717320000059
Ki(k+1)＝Pi(k+1/k)Hi(k+1)(Hi(k+1)Pi(k+1/k)Hi(k+1)T+R)-1
其中，
Figure FDA00025317173200000510
是k+1时刻相对于采样时刻k局部滤波器i的局部最优估计结果，
Figure FDA00025317173200000511
是采样时刻k局部滤波器i的状态滤波值，
Pi(k+1/k)是k+1时刻相对于采样时刻k局部滤波器i的状态向量协方差矩阵，
Pi(k/k)是采样时刻k局部滤波器i的状态向量协方差矩阵的滤波值，
Figure FDA00025317173200000512
是采样时刻k的时变状态转移矩阵的转置，
Γ(k)T是采样时刻k的系统干扰矩阵的转置，
Q(k)是过程误差的协方差，
R是Vi(k)的协方差，
Ki(k+1)是k+1时刻局部滤波器i的增益阵，
Hi(k+1)是k+1时刻局部滤波器i的观察矩阵，
Hi(k+1)T是k+1时刻局部滤波器i的观察矩阵的转置。
11.根据权利要求10所述的导航方法，其特征在于，所述各局部滤波器在下一时刻的状态滤波值
Figure FDA0002531717320000061
和各局部滤波器在下一时刻的状态向量的协方差的滤波值Pi(k+1/k+1)分别为：
Figure FDA0002531717320000062
Pi(k+1/k+1)＝(1-Ki(k+1)Hi(k+1))Pi(k+1/k)
其中，Zi(k+1)是k+1时刻局部滤波器i的测量方程。
12.根据权利要求11所述的导航方法，其特征在于，所述局部滤波器i的测量方程Zi包含所述第一局部滤波器的观测测量值Z1和所述第二局部滤波器的观测值测量值Z2，分别为
Figure FDA0002531717320000063
z2＝[VN(MEMS)-VN(DVL) VE(MEMS)-VE(DVL)]
其中，
Figure FDA0002531717320000064
为GPS和微机电惯导系统之间的纬度差，
(λMEMS-λGPS)为GPS和微机电惯导系统之间的经度差，
(VN(MEMS)-VN(DVL))为DVL和微机电惯导系统之间的向北速度差，
(VE(MEMS)-VE(DVL))为DVL和微机电惯导系统之间的向东速度差。
13.根据权利要求12所述的导航方法，其特征在于，所述第六预设规则包括：所述全局滤波器最终状态向量Xm和所述全局滤波器状态向量的协方差矩阵的最终滤波值Pm分别为
Figure FDA0002531717320000071
Figure FDA0002531717320000072
其中，n为局部滤波器的总数量，
Pi -1为Pi的逆矩阵。
14.根据权利要求13所述的导航方法，其特征在于，所述第七预设规则包括：
Figure FDA0002531717320000073
其中，Bi为所述局部滤波器i对应的所述子导航i的自适应数据共享因子联合滤波器值，
PiK为卡尔曼增益下局部滤波器i状态向量的协方差的滤波值，
PiK -1为PiK的逆矩阵，
Ki为局部滤波器i的增益阵，
Hi为所述局部滤波器i的观察矩阵。
CN202010526549.XA 2020-06-09 2020-06-09 一种水面无人艇导航系统和导航方法 Pending CN111678520A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010526549.XA CN111678520A (zh) 2020-06-09 2020-06-09 一种水面无人艇导航系统和导航方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010526549.XA CN111678520A (zh) 2020-06-09 2020-06-09 一种水面无人艇导航系统和导航方法

Publications (1)

Publication Number Publication Date
CN111678520A true CN111678520A (zh) 2020-09-18

Family

ID=72435309

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010526549.XA Pending CN111678520A (zh) 2020-06-09 2020-06-09 一种水面无人艇导航系统和导航方法

Country Status (1)

Country Link
CN (1) CN111678520A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112710304A (zh) * 2020-12-17 2021-04-27 西北工业大学 一种基于自适应滤波的水下自主航行器导航方法

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109737959A (zh) * 2019-03-20 2019-05-10 哈尔滨工程大学 一种基于联邦滤波的极区多源信息融合导航方法
CN110579740A (zh) * 2019-09-17 2019-12-17 大连海事大学 一种基于自适应联邦卡尔曼滤波的无人船组合导航方法
CN110823217A (zh) * 2019-11-21 2020-02-21 山东大学 一种基于自适应联邦强跟踪滤波的组合导航容错方法

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109737959A (zh) * 2019-03-20 2019-05-10 哈尔滨工程大学 一种基于联邦滤波的极区多源信息融合导航方法
CN110579740A (zh) * 2019-09-17 2019-12-17 大连海事大学 一种基于自适应联邦卡尔曼滤波的无人船组合导航方法
CN110823217A (zh) * 2019-11-21 2020-02-21 山东大学 一种基于自适应联邦强跟踪滤波的组合导航容错方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
MOHAMED Z.MOSTAFA ET AL.: "GPS/DVL/MEMS-INS smartphone sensors integrated method to enhance USV navigation system based on adaptive DSFCF", 《IET RADAR,SONAR & NAVIGATION》 *

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112710304A (zh) * 2020-12-17 2021-04-27 西北工业大学 一种基于自适应滤波的水下自主航行器导航方法

Similar Documents

Publication Publication Date Title
CN107525503B (zh) 基于双天线gps和mimu组合的自适应级联卡尔曼滤波方法
CN112097763B (zh) 一种基于mems imu/磁力计/dvl组合的水下运载体组合导航方法
US7307585B2 (en) Integrated aeroelasticity measurement system
CN111721289A (zh) 车辆定位方法、装置、设备、存储介质及车辆
CN112505737B (zh) 一种gnss/ins组合导航方法
CN111156987A (zh) 基于残差补偿多速率ckf的惯性/天文组合导航方法
CN109471146B (zh) 一种基于ls-svm的自适应容错gps/ins组合导航方法
CN111721288B (zh) 一种mems器件零偏修正方法、装置及存储介质
EP2095148A1 (en) Arrangement for and method of two dimensional and three dimensional precision location and orientation determination
CA2565143A1 (en) System and method for aligning multiple navigation components
CN102353378A (zh) 一种矢量形式信息分配系数的自适应联邦滤波方法
CN110006421B (zh) 一种基于mems和gps的车载导航方法及系统
CN112946681B (zh) 融合组合导航信息的激光雷达定位方法
Karaim et al. Low-cost IMU data denoising using Savitzky-Golay filters
CN116007620A (zh) 一种组合导航滤波方法、系统、电子设备及存储介质
CN114136315A (zh) 一种基于单目视觉辅助惯性组合导航方法及系统
CN111678520A (zh) 一种水面无人艇导航系统和导航方法
CN113009816B (zh) 时间同步误差的确定方法及装置、存储介质及电子装置
Akshaya et al. Multi-sensor data fusion for aerodynamically controlled vehicle based on FGPM
CN112033438B (zh) 一种基于速度拟合的晃动基座自对准方法
CN113175926A (zh) 一种基于运动状态监测的自适应水平姿态测量方法
CN115112119A (zh) 一种基于lstm神经网络辅助的车载导航方法
CN114018262B (zh) 一种改进的衍生容积卡尔曼滤波组合导航方法
CN111473786A (zh) 一种基于局部反馈的两层分布式多传感器组合导航滤波方法
CN113267183A (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
RJ01 Rejection of invention patent application after publication
RJ01 Rejection of invention patent application after publication

Application publication date: 20200918