CN110579740B - 一种基于自适应联邦卡尔曼滤波的无人船组合导航方法 - Google Patents
一种基于自适应联邦卡尔曼滤波的无人船组合导航方法 Download PDFInfo
- Publication number
- CN110579740B CN110579740B CN201910875242.8A CN201910875242A CN110579740B CN 110579740 B CN110579740 B CN 110579740B CN 201910875242 A CN201910875242 A CN 201910875242A CN 110579740 B CN110579740 B CN 110579740B
- Authority
- CN
- China
- Prior art keywords
- filter
- sub
- sins
- information
- gps
- 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.)
- Active
Links
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C17/00—Compasses; Devices for ascertaining true or magnetic north for navigation or surveying purposes
- G01C17/02—Magnetic compasses
- G01C17/28—Electromagnetic compasses
- G01C17/32—Electron compasses
-
- 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
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/42—Determining position
- G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
- G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
- G01S5/02—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using radio waves
- G01S5/0294—Trajectory determination or predictive filtering, e.g. target tracking or Kalman filtering
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Electromagnetism (AREA)
- Computer Networks & Wireless Communication (AREA)
- Automation & Control Theory (AREA)
- Position Fixing By Use Of Radio Waves (AREA)
- Navigation (AREA)
Abstract
本发明公开了一种基于自适应联邦卡尔曼滤波的无人船组合导航方法,利用无人船组合导航系统进行导航,所述无人船组合导航系统包括SINS、GPS、Compass和嵌入式处理器。所述嵌入式处理器内采用自适应联邦卡尔曼滤波算法进行传感器信息融合,输出无人船的位置、速度和姿态信息。本发明应用了无人船SINS/GPS/Compass组合导航的误差模型和观测模型,减小各个子系统之间的故障干扰,提高无人船导航系统的可靠性与稳定性。本发明在联邦卡尔曼滤波的基础上,设计联邦卡尔曼滤波的子滤波器的信息分配因子,在保障系统的容错能力的前提下,能够有效抑制系统的异常扰动,减小分配原则对滤波精度的影响。
Description
技术领域
本发明属于传感器信息融合领域,尤其涉及一种基于自适应联邦卡尔曼滤波的无人船组合导航方法。
背景技术
由于在海上应用的灵活性和多功能性,无人船在军事生产等各个方面都有着广泛的应用。联邦卡尔曼滤波应用于无人船、无人机、无人车上,组合导航可以检测出导航子系统的故障,将正常的子系统的导航信息进行组合,提高系统的容错性与自适应性。卡尔曼滤波、粒子滤波、联邦滤波等方法广泛用于组合导航传感器的信息融合。针对多传感器的信息融合领域,联邦卡尔曼滤波效果最为显著,但是其滤波精度受信息分配原则影响,以及在载体发生异常扰动时滤波精度较低的问题。
发明内容
为解决现有技术的上述问题,本发明要设计一种能够提高滤波精度的基于自适应联邦卡尔曼滤波的无人船组合导航方法。
为实现上述目的,本发明的技术方案如下:
一种基于自适应联邦卡尔曼滤波的无人船组合导航方法,利用无人船组合导航系统进行导航,所述无人船组合导航系统包括捷联惯导系统传感器即SINS、全球定位系统传感器即GPS、三维电子罗盘传感器即Compass和嵌入式处理器。所述嵌入式处理器内采用自适应联邦卡尔曼滤波算法进行传感器信息融合,输出无人船的位置、速度和姿态信息。所述自适应联邦卡尔曼滤波算法基于自适应联邦卡尔曼滤波器实现。所述自适应联邦卡尔曼滤波器包括主滤波器、SINS/GPS子滤波器、SINS/Compass子滤波器和自适应信息分配因子计算模块;所述主滤波器分别与SINS/GPS子滤波器和SINS/Compass子滤波器双向连接。所述SINS/GPS子滤波器输出位置、速度信息的状态估计值和估计误差协方差矩阵,SINS/Compass子滤波器输出姿态信息的状态估计值和估计均方误差协方差矩阵。自适应信息分配因子计算模块通过两个子滤波器的预测状态矢量的估计均方误差协方差矩阵计算出自适应信息分配因子,将归一化后的自适应信息分配因子送入主滤波器。主滤波器内进行时间更新过程和信息融合过程,将两个子滤波器的状态估计值和状态估计均方误差协方差矩阵与主滤波器的状态估计值进行信息融合,得到全局最优状态估计值,并反馈到两个子滤波器,进行子滤波器的重置。
所述组合导航方法,包括以下步骤:
A、建立无人船组合导航系统的状态方程
选择东北天地理坐标系为导航坐标系,状态方程考虑捷联惯导系统、加速度计、陀螺仪的误差因素,则所述组合导航系统的状态方程为:
组合导航系统的状态变量X为:
式中,分别为无人船东、北、天方向的姿态误差角;δVE、δVN、δVU分别为无人船东、北、天方向的速度误差;δλ、δL、δh分别为无人船的纬度、经度、高度的误差;εbx、εby、εbz分别为载体系下陀螺仪东、北、天方向的常值漂移;εrx、εry、εrz分别为载体系下陀螺仪东、北、天方向的慢变漂移;▽x、▽y、▽z分别为载体系下加速度计东、北、天方向的常值随机误差。
组合导航系统的白噪声矩阵W为:
W=[ωgx,ωgy,ωgz,ωrx,ωry,ωrz,ωax,ωay,ωaz]T
其中,ωgx、ωgy、ωgz分别为陀螺仪东、北、天方向的白噪声;ωrx、ωry、ωrz分别为陀螺仪东、北、天方向的一阶马尔科夫过程驱动白噪声;ωax、ωay、ωaz为加速度计东、北、天方向的一阶马尔科夫过程驱动白噪声。
陀螺仪的误差模型为:
ε=εb+εr+ωg
式中,εb为随机常数;εr为一阶马尔科夫过程随机噪声,εr满足下式:
Tg为陀螺仪相关时间,ωr为陀螺仪的一阶马尔科夫过程驱动白噪声。
加速度计的误差模型为:
▽=▽a
组合导航系统的状态转移矩阵为:
式中,FN为9行9列的基本导航参数的系统阵,具体如下:
FN(2,7)=-ωie sinL
FN(4,2)=-fU
FN(5,1)=fU
FN(5,3)=-fE
FN(6,1)=-fN
FN(6,2)=-fE
FN(6,7)=-2VEωie sinL
FN(9,6)=1
其余元素为0。
其中ωie为地球自转角速率,fE、fU分别为东、天方向的地球偏扁率,RE为地球赤道半径,RM地球表面上的任一点处沿子午圈的主曲率半径;RN为地球表面上的任一点处沿卯酉圈的主曲率半径;L为经度。
FM=diag{0,0,0,-1/Trx,-1/Try,-1/Trz,-1/Tax,-1/Tay,-1/Taz}
式中:
组合导航系统的控制矩阵为:
B、设计SINS/GPS子滤波器
SINS/GPS子滤波器采用位置和速度组合模式,建立SINS/GPS子滤波器的位置、速度的量测方程为:
Z1=HX1+V1
Z1为SINS/GPS子滤波器的量测值,H1为SINS/GPS子滤波器的量测矩阵,V1为SINS/GPS子滤波器的量测噪声。
SINS的速度信息为:
GPS的速度信息为:
SINS的位置信息为:
GPS的位置信息为:
式中,vIE、vIN、vIU分别为SINS东、北、天方向的速度信息;vGE、vGN、vGU分别为GPS东、北、天方向的速度信息;λI、LI、hI分别为SINS东、北、天方向的位置信息;λG、LG、hG分别为GPS东、北、天方向的位置信息;vN、vE、vU分别为无人船东、北、天方向的速度真值;λt、Lt、ht分别为无人船纬度、经度、高度的位置真值;δvE、δvN、δvU分别为SINS东、北、天方向的速度误差;MN、ME、MU分别为GPS东、北、天方向的速度误差;δλ、δL、δh分别为SINS的纬度、经度、高度的误差,NN、NE、NU分别为GPS的纬度、经度、高度的误差。
取SINS和GPS的速度差值作为SINS/GPS子滤波器量测值,定义速度量测方程为:
式中:
ZV为SINS/GPS子滤波器速度信息的量测值,HV为SINS/GPS子滤波器速度信息的量测矩阵,VV为SINS/GPS子滤波器速度信息的量测噪声,O3×3为3行3列的零矩阵,O3×12为3行12列的零矩阵。
取SINS和GPS的位置差值作为SINS/GPS子滤波器量测值,定义位置量测方程为:
式中:
Vp=[NN,NE,NU]T
RM=Re(1-2f+3fsin2L)
RN=Re(1+fsin2L)
式中:ZP为SINS/GPS子滤波器速度信息的量测值,HP为SINS/GPS子滤波器速度信息的量测矩阵,VP为SINS/GPS子滤波器速度信息的量测噪声,VP当做白噪声处理;Re为地球赤道半径;取6378137m;f为地球扁率,取1/298.257;O3×6为3行6列的零矩阵;O3×9为3行9列的零矩阵。
采用速度与位置的组合方式,建立SINS/GPS子滤波器的量测方程为:
C、设计SINS/Compass子滤波器
建立无人船组合导航SINS/Compass子滤波器的姿态的量测方程为:
Z2=H2X+V2
式中,Z2为SINS/Compass子滤波器的量测值,H2为SINS/Compass子滤波器的量测矩阵,V2为SINS/Compass子滤波器的量测噪声。
SINS的姿态信息为:
Compass的姿态信息为:
式中,分别为SINS东、北、天方向的姿态信息;/>分别为Compass东、北、天的姿态信息;/>分别为无人船东、北、天方向的姿态真值;分别为SINS东、北、天方向的姿态误差;δαE、δαN、δαU分别为Compass的东、北、天方向的姿态误差。
将SINS和Compass输出的姿态信息的差值作为SINS/Compass子滤波器的测量值,则观测方程为:
式中,H2=[I3×3,03×15],I3×3为3行3列的单位矩阵,O3×15为3行15列单位矩阵,V2=[δαc,δβc,δγc]T是均值为零的观测白噪声。
D、实现自适应联邦卡尔曼滤波的过程
SINS/GPS子滤波器和SINS/Compass子滤波器并行运行,将SINS、GPS、Compass数据进行分散式处理,在主滤波器中进行信息融合。两个子滤波器的估计误差协方差阵及状态估计值被送入主滤波器,并与主滤波器的估计值进行信息融合得到全局最优估计。按照信息分配原则将全局最优估计反馈到子滤波器,完成一次滤波,并重置两个子滤波器的估计值。
所述自适应联邦卡尔曼滤波器的信息融合过程在主滤波器内进行,包括信息分配、时间更新、量测更新、信息融合四个步骤,具体步骤如下:
D1、信息分配
将全局的状态估计、误差方差阵、噪声信息分配给两个子滤波器和主滤波器,分配原则为:
式中,i=1、2分别代表SINS/GPS子滤波器和SINS/Compass子滤波器,Pi,k-1为子滤波器k-1时刻的估计均方误差协方差矩阵,Pg,k-1为k-1时刻的全局最优估计均方误差协方差矩阵,Qi,k-1为子滤波器噪声k-1时刻的系统噪声方差阵,Qg,k-1为主滤波器噪声k-1时刻的系统噪声方差阵,为子滤波器k-1时刻的状态估计,/>为主滤波器k-1时刻的状态估计。
主滤波器中无信息分配,执行状态估计和协方差阵的时间更新,即Pm,k=0。
D2、时间更新
时间更新在两个子滤波器间独立进行,分别获得两个子滤波器的状态预测矢量和一步预测均方误差协方差矩阵:
式中,i=1、2分别代表SINS/GPS子滤波器和SINS/Compass子滤波器,Pi,k|k-1为子滤波器的一步预测均方误差协方差矩阵,为子滤波器的状态预测矢量,Φi,k|k-1为子滤波器k-1时刻到k时刻的一步转移矩阵,Γi,k-1为噪声驱动阵。
D3、量测更新
两个子滤波器接收量测信息并独立进行量测更新过程,分别获得两个子滤波器的状态估计值、滤波增益和估计均方误差协方差矩阵,并传至主滤波器:
式中,i=1、2分别代表SINS/GPS子滤波器和SINS/Compass子滤波器,为子滤波器k时刻的状态估计,Ki,k为子滤波器k时刻的滤波增益,Pi,k为估计均方误差协方差矩阵,Zi,k为子滤波器k时刻的量测值,Hi,k子滤波器k时刻的量测矩阵,Ri,k子滤波器k时刻的量测噪声方差阵,I单位矩阵。
量测更新过程仅在子滤波器内进行,主滤波器内只进行时间更新过程。
D4、信息融合
把两个子滤波器的状态估计信息和主滤波器的状态估计进行融合,从而得到全局状态估计信息:
式中,Pg,k为k时刻的全局最优估计均方误差协方差矩阵,P1,k为SINS/GPS子滤波器k时刻的估计均方误差协方差矩阵,P2,k为SINS/Compass子滤波器k时刻的估计均方误差协方差矩阵,Pm,k为主滤波器k时刻的估计均方误差协方差矩阵,为k时刻的全局最优状态估计。
D5、计算自适应信息分配因子
最优自适应因子满足以下条件:
最优自适应因子表示如下:
几何状态矢量与预测状态矢量的偏差为:
式中,∑i,k为Zi,k的等效权矩阵。
当估计量基于当前时刻的预测状态矢量,预测状态矢量的估计协方差矩阵如下:
最优自适应因子αk的期望值小于1,表示为:
式中:
则最优自适应因子基于预测状态矢量的估计协方差矩阵表示为:
由上式表示的自适应因子类似于由模型的预测状态与量测的估计状态之间的差异构造的自适应因子。
自适应因子表示为预测状态矢量的形式为:
通过基于预测状态矢量的最优自适应因子的计算方法求得自适应联邦卡尔曼滤波器的自适应信息分配因子为:
其中,b为常数,取0.85~1.0。
为确保自适应信息分配因子满足信息守恒定律,对自适应信息分配因子进行归一化处理。自适应信息分配因子归一化为:
式中,β'i,k为归一化后的子滤波器在k时刻的自适应信息分配因子。
与现有技术相比,本发明具有以下有益效果:
1、本发明提出一种无人船的SINS/GPS/Compass组合导航系统,应用了无人船SINS/GPS/Compass组合导航的误差模型和观测模型,采用联邦卡尔曼滤波,减小各个子系统之间的故障干扰,提高无人船导航系统的可靠性与稳定性。
2、本发明在联邦卡尔曼滤波的基础上,根据预测状态矢量的估计均方误差协方差矩阵的最优自适应因子的计算方法,设计联邦卡尔曼滤波的子滤波器的信息分配因子,提出一种自适应联邦卡尔曼滤波算法,仿真实验证明了本发明有效补偿了系统的异常扰动和模型误差,相比于非自适应联邦卡尔曼滤波算法,在保障系统的容错能力的前提下,能够有效抑制系统的异常扰动,减小分配原则对滤波精度的影响。
3、本发明采用计算基于预测状态矢量的估计均方误差协方差矩阵的最优自适应因子的方法选取联邦滤波器的信息分配因子,相比于预测残差的最优自适应因子的计算方法,该方法具有更高的可靠性。
附图说明
图1是本发明的流程示意图。
图2是纬度误差的EKF、AEKF仿真波形图。
图3是经度误差的EKF、AEKF仿真波形图。
图4是高度误差的EKF、AEKF仿真波形图。
图5是东向速度误差的EKF、AEKF仿真波形图。
图6是北向速度误差的EKF、AEKF仿真波形图。
图7是天向速度误差的EKF、AEKF仿真波形图。
图8是东向姿态误差的EKF、AEKF仿真波形图。
图9是北向姿态误差的EKF、AEKF仿真波形图。
图10是天向姿态误差的EKF、AEKF仿真波形图。
具体实施方式
下面结合附图对本发明进行进一步地描述。
本发明的自适应联邦卡尔曼滤波器是基于现有联邦卡尔曼滤波器改进而来,因为联邦卡尔曼滤波器的子滤波器是对状态估计值的权矩阵调整,自适应联邦卡尔曼滤波器是对状态估计值的协方差阵调整,因此两者的修正参数具有等价性。
如果采用自适应联邦卡尔曼滤波的自适应因子来表示联邦卡尔曼滤波器的信息分配因子,可以提高联邦卡尔曼滤波器的自适应性,下面对两者之间的等价性进行证明。
设子滤波器的滤波估计值、主滤波器的滤波估计值和全局状态估计分别为Xi,k、Xm,k、Xg,k,相应的权矩阵分别为∑i,k、∑m,k、∑g,k,相应的协方差矩阵分别为Pi,k、Pm,k、Pg,k。
自适应联邦卡尔曼滤波通过自适应因子使滤波参数自适应调整,从而获得最优的滤波效果。
利用最小二乘原理,自适应联邦卡尔曼滤波的极值原则为:
根据预测状态矢量和观测矢量的误差方差推导出极值函数为:
其中λk为拉格朗日乘数。
则自适应联邦卡尔曼滤波解为:
所以:
将联邦卡尔曼滤波的增益矩阵写成下式:
通过以上证明可以得出,联邦卡尔曼滤波器的信息分配因子可以通过最优自适应因子的计算方法计算得到。因而提出一种自适应联邦卡尔曼滤波算法,能够增加子滤波器的自适应性,提高滤波精度。
为了说明本发明的有效性和可行性,在SINS的误差模型下进行Matlab仿真,对本发明提出的自适应联邦卡尔曼滤波算法进行验证。假设无人船的初始位置为东经121.4°,北纬39.0°。传感器的误差参数设置为:陀螺仪的常值漂移为pi/180°/h,陀螺仪的慢变漂移为pi/180°/h,加速度计的偏置为1g,三维电子罗盘的姿态误差为0.5°,GPS的测速误差为0.1m/s,位置误差为10m。
表1展示联邦卡尔曼滤波方法(EKF)与自适应联邦卡尔曼滤波方法(AEKF)的仿真误差参数范围对比,从图中可以看出自适应联邦卡尔曼滤波算法(AEKF)相比于联邦卡尔曼滤波算法(EKF)的误差均可以收敛于更小的范围,当EKF算法能将误差较大时,AEKF算法仍能降低误差,控制误差在较小的范围,EKF算法获得较小误差时,AEKF算法的误差也能控制在较小的范围。图1展示的是SINS/GPS/Compass联邦滤波器结构图,从图中可以看出自滤波器和主滤波器的数据处理过程。图2展示的是纬度、经度、高度误差的EKF、AEKF仿真波形图,从图中可以看出AEKF算法的纬度、经度、高度误差都可以控制在较小的范围内,相比于EKF算法效果更佳。图3展示的是东向、北向、天向速度误差的EKF、AEKF仿真波形图,从图中可以看出AEKF算法的东向、北向、天向速度误差都可以控制在较小的范围内,相比于EKF算法效果更佳。图4展示的是东向、北向、天向姿态误差的EKF、AEKF仿真波形图,从图中可以看出EKF算法的天向姿态误差的范围过大,AEKF算法较好的控制了误差,将误差控制在合理范围内。可以得出结论:以上实验验证了本发明所提出的算法方案的有效性和优越性。
表1 EKF和AEKF仿真误差参数比对
纬度误差 | 经度 | 高度 | |
EKF | -1.885~1.939 | -0.710~0.618 | -3.857~4.010 |
AEKF | -1.840~1.435 | -0.714~0.616 | -0.592~0.488 |
东向速度误差 | 北向速度误差 | 天向速度误差 | |
EKF | -0.693~1.201 | -3.693~14.896 | -0.505~0.745 |
AEKF | -0.693~1.201 | -0.559~1.226 | -0.377~0.586 |
东向姿态误差 | 北向姿态误差 | 天向姿态误差 | |
EKF | -0.209~0.266 | -0.210~0.268 | -3690.023~4708.490 |
AEKF | -0.224~0.246 | -0.210~0.267 | -51.710~100.228 |
本发明不局限于本实施例,任何在本发明披露的技术范围内的等同构思或者改变,均列为本发明的保护范围。
Claims (1)
1.一种基于自适应联邦卡尔曼滤波的无人船组合导航方法,其特征在于:利用无人船组合导航系统进行导航,所述无人船组合导航系统包括捷联惯导系统传感器即SINS、全球定位系统传感器即GPS、三维电子罗盘传感器即Compass和嵌入式处理器;所述嵌入式处理器内采用自适应联邦卡尔曼滤波算法进行传感器信息融合,输出无人船的位置、速度和姿态信息;所述自适应联邦卡尔曼滤波算法基于自适应联邦卡尔曼滤波器实现;所述自适应联邦卡尔曼滤波器包括主滤波器、SINS/GPS子滤波器、SINS/Compass子滤波器和自适应信息分配因子计算模块;所述主滤波器分别与SINS/GPS子滤波器和SINS/Compass子滤波器双向连接;所述SINS/GPS子滤波器输出位置、速度信息的状态估计值和估计误差协方差矩阵,SINS/Compass子滤波器输出姿态信息的状态估计值和估计均方误差协方差矩阵;自适应信息分配因子计算模块通过两个子滤波器的预测状态矢量的估计均方误差协方差矩阵计算出自适应信息分配因子,将归一化后的自适应信息分配因子送入主滤波器;主滤波器内进行时间更新过程和信息融合过程,将两个子滤波器的状态估计值和状态估计均方误差协方差矩阵与主滤波器的状态估计值进行信息融合,得到全局最优状态估计值,并反馈到两个子滤波器,进行子滤波器的重置;
所述组合导航方法,包括以下步骤:
A、建立无人船组合导航系统的状态方程
选择东北天地理坐标系为导航坐标系,状态方程考虑捷联惯导系统、加速度计、陀螺仪的误差因素,则所述组合导航系统的状态方程为:
组合导航系统的状态变量X为:
式中,分别为无人船东、北、天方向的姿态误差角;δVE、δVN、δVU分别为无人船东、北、天方向的速度误差;δλ、δL、δh分别为无人船的纬度、经度、高度的误差;εbx、εby、εbz分别为载体系下陀螺仪东、北、天方向的常值漂移;εrx、εry、εrz分别为载体系下陀螺仪东、北、天方向的慢变漂移;/> 分别为载体系下加速度计东、北、天方向的常值随机误差;
组合导航系统的白噪声矩阵W为:
W=[ωgx,ωgy,ωgz,ωrx,ωry,ωrz,ωax,ωay,ωaz]T
其中,ωgx、ωgy、ωgz分别为陀螺仪东、北、天方向的白噪声;ωrx、ωry、ωrz分别为陀螺仪东、北、天方向的一阶马尔科夫过程驱动白噪声;ωax、ωay、ωaz为加速度计东、北、天方向的一阶马尔科夫过程驱动白噪声;
陀螺仪的误差模型为:
ε=εb+εr+ωg
式中,εb为随机常数;εr为一阶马尔科夫过程随机噪声,εr满足下式:、
Tg为陀螺仪相关时间,ωr为陀螺仪的一阶马尔科夫过程驱动白噪声;
加速度计的误差模型为:
组合导航系统的状态转移矩阵为:
式中,FN为9行9列的基本导航参数的系统阵,具体如下:
FN(2,7)=-ωie sinL
FN(4,2)=-fU
FN(5,1)=fU
FN(5,3)=-fE
FN(6,1)=-fN
FN(6,2)=-fE
FN(6,7)=-2VEωie sinL
FN(9,6)=1
其余元素为0;
其中ωie为地球自转角速率,fE、fU分别为东、天方向的地球偏扁率,RE为地球赤道半径,RM地球表面上的任一点处沿子午圈的主曲率半径;RN为地球表面上的任一点处沿卯酉圈的主曲率半径;L为经度;
FM=diag{0,0,0,-1/Trx,-1/Try,-1/Trz,-1/Tax,-1/Tay,-1/Taz}
式中:
组合导航系统的控制矩阵为:
B、设计SINS/GPS子滤波器
SINS/GPS子滤波器采用位置和速度组合模式,建立SINS/GPS子滤波器的位置、速度的量测方程为:
Z1=HX1+V1
Z1为SINS/GPS子滤波器的量测值,H1为SINS/GPS子滤波器的量测矩阵,V1为SINS/GPS子滤波器的量测噪声;
SINS的速度信息为:
GPS的速度信息为:
SINS的位置信息为:
GPS的位置信息为:
式中,vIE、vIN、vIU分别为SINS东、北、天方向的速度信息;vGE、vGN、vGU分别为GPS东、北、天方向的速度信息;λI、LI、hI分别为SINS东、北、天方向的位置信息;λG、LG、hG分别为GPS东、北、天方向的位置信息;vN、vE、vU分别为无人船东、北、天方向的速度真值;λt、Lt、ht分别为无人船纬度、经度、高度的位置真值;δvE、δvN、δvU分别为SINS东、北、天方向的速度误差;MN、ME、MU分别为GPS东、北、天方向的速度误差;δλ、δL、δh分别为SINS的纬度、经度、高度的误差,NN、NE、NU分别为GPS的纬度、经度、高度的误差;
取SINS和GPS的速度差值作为SINS/GPS子滤波器量测值,定义速度量测方程为:
式中:
ZV为SINS/GPS子滤波器速度信息的量测值,HV为SINS/GPS子滤波器速度信息的量测矩阵,VV为SINS/GPS子滤波器速度信息的量测噪声,O3×3为3行3列的零矩阵,O3×12为3行12列的零矩阵;
取SINS和GPS的位置差值作为SINS/GPS子滤波器量测值,定义位置量测方程为:
Vp=[NN,NE,NU]T
RM=Re(1-2f+3fsin2L)
RN=Re(1+fsin2L)
式中:ZP为SINS/GPS子滤波器速度信息的量测值,HP为SINS/GPS子滤波器速度信息的量测矩阵,VP为SINS/GPS子滤波器速度信息的量测噪声,VP当做白噪声处理;Re为地球赤道半径;取6378137m;f为地球扁率,取1/298.257;O3×6为3行6列的零矩阵;O3×9为3行9列的零矩阵;
采用速度与位置的组合方式,建立SINS/GPS子滤波器的量测方程为:
C、设计SINS/Compass子滤波器
建立无人船组合导航SINS/Compass子滤波器的姿态的量测方程为:
Z2=H2X+V2
式中,Z2为SINS/Compass子滤波器的量测值,H2为SINS/Compass子滤波器的量测矩阵,V2为SINS/Compass子滤波器的量测噪声;
SINS的姿态信息为:
Compass的姿态信息为:
式中,分别为SINS东、北、天方向的姿态信息;/>分别为Compass东、北、天的姿态信息;/>分别为无人船东、北、天方向的姿态真值;/>分别为SINS东、北、天方向的姿态误差;δαE、δαN、δαU分别为Compass的东、北、天方向的姿态误差;
将SINS和Compass输出的姿态信息的差值作为SINS/Compass子滤波器的测量值,则观测方程为:
式中,H2=[I3×3,03×15],I3×3为3行3列的单位矩阵,O3×15为3行15列单位矩阵,V2=[δαc,δβc,δγc]T是均值为零的观测白噪声;
D、实现自适应联邦卡尔曼滤波的过程
SINS/GPS子滤波器和SINS/Compass子滤波器并行运行,将SINS、GPS、Compass数据进行分散式处理,在主滤波器中进行信息融合;两个子滤波器的估计误差协方差阵及状态估计值被送入主滤波器,并与主滤波器的估计值进行信息融合得到全局最优估计;按照信息分配原则将全局最优估计反馈到子滤波器,完成一次滤波,并重置两个子滤波器的估计值;
所述自适应联邦卡尔曼滤波器的信息融合过程在主滤波器内进行,包括信息分配、时间更新、量测更新、信息融合四个步骤,具体步骤如下:
D1、信息分配
将全局的状态估计、误差方差阵、噪声信息分配给两个子滤波器和主滤波器,分配原则为:
式中,i=1、2分别代表SINS/GPS子滤波器和SINS/Compass子滤波器,Pi,k-1为子滤波器k-1时刻的估计均方误差协方差矩阵,Pg,k-1为k-1时刻的全局最优估计均方误差协方差矩阵,Qi,k-1为子滤波器噪声k-1时刻的系统噪声方差阵,Qg,k-1为主滤波器噪声k-1时刻的系统噪声方差阵,为子滤波器k-1时刻的状态估计,/>为主滤波器k-1时刻的状态估计;
主滤波器中无信息分配,执行状态估计和协方差阵的时间更新,即Pm,k=0;
D2、时间更新
时间更新在两个子滤波器间独立进行,分别获得两个子滤波器的状态预测矢量和一步预测均方误差协方差矩阵:
式中,i=1、2分别代表SINS/GPS子滤波器和SINS/Compass子滤波器,Pi,k|k-1为子滤波器的一步预测均方误差协方差矩阵,为子滤波器的状态预测矢量,Φi,k|k-1为子滤波器k-1时刻到k时刻的一步转移矩阵,Γi,k-1为噪声驱动阵;/>
D3、量测更新
两个子滤波器接收量测信息并独立进行量测更新过程,分别获得两个子滤波器的状态估计值、滤波增益和估计均方误差协方差矩阵,并传至主滤波器:
式中,i=1、2分别代表SINS/GPS子滤波器和SINS/Compass子滤波器,为子滤波器k时刻的状态估计,Ki,k为子滤波器k时刻的滤波增益,Pi,k为估计均方误差协方差矩阵,Zi,k为子滤波器k时刻的量测值,Hi,k子滤波器k时刻的量测矩阵,Ri,k子滤波器k时刻的量测噪声方差阵,I单位矩阵;
量测更新过程仅在子滤波器内进行,主滤波器内只进行时间更新过程;
D4、信息融合
把两个子滤波器的状态估计信息和主滤波器的状态估计进行融合,从而得到全局状态估计信息:
式中,Pg,k为k时刻的全局最优估计均方误差协方差矩阵,P1,k为SINS/GPS子滤波器k时刻的估计均方误差协方差矩阵,P2,k为SINS/Compass子滤波器k时刻的估计均方误差协方差矩阵,Pm,k为主滤波器k时刻的估计均方误差协方差矩阵,为k时刻的全局最优状态估计;
D5、计算自适应信息分配因子
最优自适应因子满足以下条件:
最优自适应因子表示如下:
几何状态矢量与预测状态矢量的偏差为:
式中,∑i,k为Zi,k的等效权矩阵;
当估计量基于当前时刻的预测状态矢量,预测状态矢量的估计协方差矩阵如下:
最优自适应因子αk的期望值小于1,表示为:
式中:
则最优自适应因子基于预测状态矢量的估计协方差矩阵表示为:
由上式表示的自适应因子类似于由模型的预测状态与量测的估计状态之间的差异构造的自适应因子;
自适应因子表示为预测状态矢量的形式为:
通过基于预测状态矢量的最优自适应因子的计算方法求得自适应联邦卡尔曼滤波器的自适应信息分配因子为:
其中,b为常数,取0.85~1.0;
为确保自适应信息分配因子满足信息守恒定律,对自适应信息分配因子进行归一化处理;自适应信息分配因子归一化为:
式中,βi',k为归一化后的子滤波器在k时刻的自适应信息分配因子。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910875242.8A CN110579740B (zh) | 2019-09-17 | 2019-09-17 | 一种基于自适应联邦卡尔曼滤波的无人船组合导航方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910875242.8A CN110579740B (zh) | 2019-09-17 | 2019-09-17 | 一种基于自适应联邦卡尔曼滤波的无人船组合导航方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN110579740A CN110579740A (zh) | 2019-12-17 |
CN110579740B true CN110579740B (zh) | 2023-03-31 |
Family
ID=68811476
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201910875242.8A Active CN110579740B (zh) | 2019-09-17 | 2019-09-17 | 一种基于自适应联邦卡尔曼滤波的无人船组合导航方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN110579740B (zh) |
Families Citing this family (22)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111044051B (zh) * | 2019-12-30 | 2023-11-03 | 星际智航(太仓)航空科技有限公司 | 一种复合翼无人机容错组合导航方法 |
CN111189441B (zh) * | 2020-01-10 | 2023-05-12 | 山东大学 | 一种多源自适应容错联邦滤波组合导航系统及导航方法 |
CN111623779A (zh) * | 2020-05-20 | 2020-09-04 | 哈尔滨工程大学 | 一种适用于噪声特性未知的时变系统自适应级联滤波方法 |
CN111811512B (zh) * | 2020-06-02 | 2023-08-01 | 北京航空航天大学 | 基于联邦平滑的mpos离线组合估计方法和装置 |
CN111678520A (zh) * | 2020-06-09 | 2020-09-18 | 上海理工大学 | 一种水面无人艇导航系统和导航方法 |
CN111780755B (zh) * | 2020-06-30 | 2023-05-05 | 南京理工大学 | 一种基于因子图和可观测度分析的多源融合导航方法 |
CN111928846B (zh) * | 2020-07-31 | 2024-04-05 | 南京理工大学 | 一种基于联邦滤波的多源融合即插即用组合导航方法 |
CN111999747B (zh) * | 2020-08-28 | 2023-06-20 | 大连海事大学 | 一种惯导-卫星组合导航系统的鲁棒故障检测方法 |
CN112284384B (zh) * | 2020-10-26 | 2023-11-17 | 东南大学 | 考虑量测异常的集群式多深海潜航器的协同定位方法 |
CN112346479B (zh) * | 2020-11-18 | 2023-08-22 | 大连海事大学 | 一种基于集中式卡尔曼滤波的无人航行器状态估计方法 |
CN112711055B (zh) * | 2020-12-08 | 2024-03-19 | 重庆邮电大学 | 一种基于边缘计算的室内外无缝定位系统及方法 |
CN112525188B (zh) * | 2020-12-15 | 2022-08-05 | 上海交通大学 | 一种基于联邦滤波的组合导航方法 |
CN112697154B (zh) * | 2021-01-31 | 2024-04-12 | 南京理工大学 | 一种基于矢量分配的自适应多源融合导航方法 |
CN113063429B (zh) * | 2021-03-18 | 2023-10-24 | 苏州华米导航科技有限公司 | 一种自适应车载组合导航定位方法 |
CN113472318B (zh) * | 2021-07-14 | 2024-02-06 | 青岛杰瑞自动化有限公司 | 一种顾及观测模型误差的分级自适应滤波方法及系统 |
CN113640780B (zh) * | 2021-08-23 | 2023-08-08 | 哈尔滨工程大学 | 基于改进的联邦滤波的水下auv传感器时间配准方法 |
CN114252077B (zh) * | 2021-12-17 | 2024-06-18 | 南京理工大学 | 基于联邦滤波器的双gps/sins的组合导航方法及系统 |
CN114413907A (zh) * | 2022-01-20 | 2022-04-29 | 中国人民解放军61540部队 | 一种人工智能的潜艇导航系统 |
CN115900701B (zh) * | 2022-11-14 | 2023-10-31 | 中山大学 | 一种卫导拒止环境下的轨道车组合导航方法及装置 |
CN116255988B (zh) * | 2023-05-11 | 2023-07-21 | 北京航空航天大学 | 一种基于舰船动力学组合导航复合抗干扰自适应滤波方法 |
CN116661465B (zh) * | 2023-07-04 | 2023-10-31 | 无锡八英里电子科技有限公司 | 一种基于时序分析与多传感器融合的机器人自动行驶方法 |
CN117346794B (zh) * | 2023-12-05 | 2024-02-23 | 山东省科学院海洋仪器仪表研究所 | 一种用于浒苔跟踪的无人船组合导航系统及导航方法 |
Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102662187A (zh) * | 2012-05-16 | 2012-09-12 | 山东大学 | 一种多模组合导航防诱骗装置及其工作方法 |
CN109373999A (zh) * | 2018-10-23 | 2019-02-22 | 哈尔滨工程大学 | 基于故障容错卡尔曼滤波的组合导航方法 |
CN109556632A (zh) * | 2018-11-26 | 2019-04-02 | 北方工业大学 | 一种基于卡尔曼滤波的ins/gnss/偏振/地磁组合导航对准方法 |
Family Cites Families (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN106291645B (zh) * | 2016-07-19 | 2018-08-21 | 东南大学 | 适于高维gnss/ins深耦合的容积卡尔曼滤波方法 |
-
2019
- 2019-09-17 CN CN201910875242.8A patent/CN110579740B/zh active Active
Patent Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102662187A (zh) * | 2012-05-16 | 2012-09-12 | 山东大学 | 一种多模组合导航防诱骗装置及其工作方法 |
CN109373999A (zh) * | 2018-10-23 | 2019-02-22 | 哈尔滨工程大学 | 基于故障容错卡尔曼滤波的组合导航方法 |
CN109556632A (zh) * | 2018-11-26 | 2019-04-02 | 北方工业大学 | 一种基于卡尔曼滤波的ins/gnss/偏振/地磁组合导航对准方法 |
Non-Patent Citations (2)
Title |
---|
A Loosely Coupled MEMS-SINS/GNSS Integrated System for Land Vehicle Navigation in Urban Areas;Wang Meiling et al.;《2017 IEEE International Conference on Vehicular Electronics and Safety (ICVES)》;20170628;第103-108页 * |
SINS/北斗/GPS 组合导航系统研究;陈帅 等;《火力与指挥控制》;20090630;第34卷(第6期);第131-134页 * |
Also Published As
Publication number | Publication date |
---|---|
CN110579740A (zh) | 2019-12-17 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110579740B (zh) | 一种基于自适应联邦卡尔曼滤波的无人船组合导航方法 | |
CN111024064B (zh) | 一种改进Sage-Husa自适应滤波的SINS/DVL组合导航方法 | |
CN108226980B (zh) | 基于惯性测量单元的差分gnss与ins自适应紧耦合导航方法 | |
CN113819906B (zh) | 一种基于统计相似度量的组合导航鲁棒滤波方法 | |
CN102829777B (zh) | 自主式水下机器人组合导航系统及方法 | |
CN106679693A (zh) | 一种基于故障检测的矢量信息分配自适应联邦滤波方法 | |
CN109883426B (zh) | 基于因子图的动态分配与校正多源信息融合方法 | |
CN107014376B (zh) | 一种适用于农业机械精准作业的姿态倾角估计方法 | |
CN104697520B (zh) | 一体化无陀螺捷联惯导系统与gps系统组合导航方法 | |
CN110243377B (zh) | 一种基于分层式结构的集群飞行器协同导航方法 | |
CN108303079B (zh) | 一种水下usbl反向应用的数据平滑方法 | |
CN101900573B (zh) | 一种实现陆用惯性导航系统运动对准的方法 | |
CN110849360B (zh) | 面向多机协同编队飞行的分布式相对导航方法 | |
CN109443342A (zh) | 新型自适应卡尔曼无人机姿态解算方法 | |
CN110926466A (zh) | 一种面向无人船组合导航信息融合的多尺度数据分块算法 | |
CN105300404A (zh) | 一种舰船基准惯性导航系统标校方法 | |
CN103697894A (zh) | 基于滤波器方差阵修正的多源信息非等间隔联邦滤波方法 | |
CN111189442A (zh) | 基于cepf的无人机多源导航信息状态预测方法 | |
CN111220151B (zh) | 载体系下考虑温度模型的惯性和里程计组合导航方法 | |
CN115265532A (zh) | 一种用于船用组合导航中的辅助滤波方法 | |
CN113608534A (zh) | 一种无人艇跟踪控制方法及系统 | |
CN116222551A (zh) | 一种融合多种数据的水下导航方法及装置 | |
CN112697154B (zh) | 一种基于矢量分配的自适应多源融合导航方法 | |
CN110375773B (zh) | Mems惯导系统姿态初始化方法 | |
CN111829511A (zh) | 一种基于m估计的auv组合导航方法及系统 |
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 | ||
GR01 | Patent grant | ||
GR01 | Patent grant |