CN111947651B - 水下组合导航信息融合方法、***及自主式水下航行器 - Google Patents
水下组合导航信息融合方法、***及自主式水下航行器 Download PDFInfo
- Publication number
- CN111947651B CN111947651B CN202010691625.2A CN202010691625A CN111947651B CN 111947651 B CN111947651 B CN 111947651B CN 202010691625 A CN202010691625 A CN 202010691625A CN 111947651 B CN111947651 B CN 111947651B
- Authority
- CN
- China
- Prior art keywords
- information
- navigation
- ast
- speed
- 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.)
- Active
Links
- 238000007500 overflow downdraw method Methods 0.000 title claims abstract description 22
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 142
- 238000000034 method Methods 0.000 claims abstract description 73
- 238000005516 engineering process Methods 0.000 claims abstract description 43
- 238000001914 filtration Methods 0.000 claims abstract description 41
- 230000004927 fusion Effects 0.000 claims abstract description 41
- 238000005259 measurement Methods 0.000 claims description 46
- 239000011159 matrix material Substances 0.000 claims description 29
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 claims description 15
- 230000000694 effects Effects 0.000 claims description 6
- 238000000827 velocimetry Methods 0.000 claims description 6
- 230000009977 dual effect Effects 0.000 claims description 5
- 230000005856 abnormality Effects 0.000 claims description 4
- 230000005540 biological transmission Effects 0.000 claims description 3
- 238000007571 dilatometry Methods 0.000 claims description 3
- 238000003860 storage Methods 0.000 claims description 3
- 230000008961 swelling Effects 0.000 claims description 3
- 230000008569 process Effects 0.000 abstract description 23
- 238000012360 testing method Methods 0.000 abstract description 15
- 230000009471 action Effects 0.000 description 18
- 238000009826 distribution Methods 0.000 description 18
- 238000010586 diagram Methods 0.000 description 17
- 230000008901 benefit Effects 0.000 description 14
- 238000004088 simulation Methods 0.000 description 14
- 238000011161 development Methods 0.000 description 10
- 238000002474 experimental method Methods 0.000 description 10
- 238000004364 calculation method Methods 0.000 description 7
- 230000002829 reductive effect Effects 0.000 description 6
- 238000010187 selection method Methods 0.000 description 5
- 230000007774 longterm Effects 0.000 description 4
- 101100141522 Arabidopsis thaliana RKF1 gene Proteins 0.000 description 3
- 238000012937 correction Methods 0.000 description 3
- 238000004519 manufacturing process Methods 0.000 description 3
- 230000004044 response Effects 0.000 description 3
- 238000012795 verification Methods 0.000 description 3
- 206010063385 Intellectualisation Diseases 0.000 description 2
- 230000001133 acceleration Effects 0.000 description 2
- 238000009825 accumulation Methods 0.000 description 2
- 238000011109 contamination Methods 0.000 description 2
- 238000009795 derivation Methods 0.000 description 2
- 239000006185 dispersion Substances 0.000 description 2
- 238000007667 floating Methods 0.000 description 2
- 230000000670 limiting effect Effects 0.000 description 2
- 230000010355 oscillation Effects 0.000 description 2
- 239000013535 sea water Substances 0.000 description 2
- 238000010200 validation analysis Methods 0.000 description 2
- 240000006829 Ficus sundaica Species 0.000 description 1
- 230000003044 adaptive effect Effects 0.000 description 1
- 230000002411 adverse Effects 0.000 description 1
- 238000004458 analytical method Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000015572 biosynthetic process Effects 0.000 description 1
- 230000015556 catabolic process Effects 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 238000004590 computer program Methods 0.000 description 1
- 239000000470 constituent Substances 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 230000007123 defense Effects 0.000 description 1
- 238000006731 degradation reaction Methods 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 230000010339 dilation Effects 0.000 description 1
- 238000011156 evaluation Methods 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 238000012423 maintenance Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 239000013307 optical fiber Substances 0.000 description 1
- 230000036961 partial effect Effects 0.000 description 1
- 230000008092 positive effect Effects 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 230000001737 promoting effect Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000012827 research and development Methods 0.000 description 1
- 230000002441 reversible effect Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 230000001629 suppression Effects 0.000 description 1
- 230000009897 systematic effect Effects 0.000 description 1
- 238000012546 transfer Methods 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
- 230000007704 transition Effects 0.000 description 1
- 238000013519 translation Methods 0.000 description 1
- 239000013598 vector Substances 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/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
- 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
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/10—Simultaneous control of position or course in three dimensions
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Aviation & Aerospace Engineering (AREA)
- Measurement Of Velocity Or Position Using Acoustic Or Ultrasonic Waves (AREA)
- Navigation (AREA)
Abstract
本发明属于水下定位技术领域,公开了一种水下组合导航信息融合方法、***及自主式水下航行器,利用SINS导航技术获取载体的姿态、速度、位置相关信息;利用DVL导航技术获取外部速度信息;利用AST导航技术获取外部位置信息;利用滤波算法将获取到的载体的姿态、速度、位置相关信息、外部速度信息、外部位置信息进行融合,并输出融合后的导航信息。本发明能够实现长航时、长航程导航定位,同时能够校正和抑制定位过程中的误差。本发明可有效抑制观测野值对滤波结果的影响,具有更好的数据平稳性。同时试验结果初步表明:相比于TFKF算法,本发明提出的FRKF算法在非高斯环境中具有更高的组合导航精度和稳定性。
Description
技术领域
本发明属于水下定位技术领域,尤其涉及一种水下组合导航信息融合方法、***及自主式水下航行器。
背景技术
目前,高精度、高可靠性的水下导航定位技术在自主式水下航行器(AutonomousUnderwater Vehicles,AUV)的发展战略中具有重要地位。AUV具有活动范围大、机动性好、安全灵活、智能化、隐蔽性好等优点,目前是水下航行器的主要发展方向。AUV顺利完成任务,达到预定使命要求,离不开水下导航定位技术的有力支持,水下导航定位技术是促使AUV技术发展和应用的关键技术。与水面以上导航相比,水下导航辅助信息源少、环境十分复杂,因此水下导航定位难度相对更大。随着现代水下各方面需求的复杂多样化,尤其是水下作战样式逐渐向无人化、智能化、编队化等方向发展,对AUV的自主控制、远程续航及隐蔽长航时等方面的能力提出了更高的要求。未来AUV必须具有自主、精确的水下长航时、长航程的导航定位能力。
目前常用的水下导航定位手段有:惯性导航、水声定位及海洋物理场匹配导航等。
(1)惯性导航技术:
惯性导航技术是导航技术的重要分支,而INS是惯性技术的核心。INS利用加速度计和陀螺仪等惯性传感器测量运载体相对于惯性空间的线运动和角运动,根据牛顿第二定律在给定运动运动初始信息的条件下,通过计算机实时解算载体的三维姿态、速度、位置等导航信息。按照惯性测量装置安装在载体上的方式,INS可分为平台式惯导***(GINS)和捷联式惯导***(SINS)。它们的主要区别在于GINS利用环架将惯性敏感元件与载体的角速度隔离开来,而INS将陀螺仪和加速度计直接与载体固联。前者使用的是机械物理平台,而后者使用的是计算机建立的数学平台。
GINS存在体积庞大、质量大及制造成本高昂等诸多缺点。相比于GINS,SINS有着结构简单、重量轻、制造成本低、方便维护等优势,因此SINS将逐步取代GINS并成为现代惯性导航技术研究和发展的主流。SINS在导航过程中不向外界发射电磁波或其他信号,不易受外界干扰,具有高隐蔽性、抗干扰能力强的独特优势。SINS已经成为各种航行器自主式导航的核心装置。
无论是GINS还是SINS,由于初始姿态误差及IMU器件误差的存在,基于牛顿第二定律进行积分推算的惯导***在解算过程中不可避免的存在振荡型、常值型和积累型的***误差,且INS的导航定位误差随着时间不断积累,因此***的长期导航定位精度难以保证。
(2)水声定位技术
水下有界误差导航定位需要借助外部传感器***实现,由于海水对电磁波拒止屏蔽作用的影响,在水下无法有效使用无线电或卫星导航定位手段。与电磁波信号不同,声波信号在海水中传播衰减很小,它是目前水下通讯及导航定位最有效的信息传播载体。水声定位***一般由多个基元(接收器或应答器)组成,其可按基线(基元间的连线)的长度分为三种:(1)长基线(Long Baseline,LBL)***:基线长度一般为100m~6000m;(2)短基线(Short Baseline,SBL)***;基线长度一般为1m~50m;(3)超短基线(ultra-shortbaseline,USBL)***:基线长度一般小于1m。
较SBL和USBL定位***,LBL定位***可实现大范围、深海域较高精度的定位导航,但是LBL定位***的复杂性也决定了其布设、校准难度也较SBL、USBL定位***高。LBL导航定位主要依赖于声应答器(信标),在AUV执行任务之前,这些声应答器通常以系泊的方式部署在海底以上50-600米。LBL定位***可以被认为是由多个AST组成的距离辅助导航***,它通过同时测量来自多个AST的距离,实现对水下目标的导航定位。
AST是水下距离辅助导航定位的最小实现单元,可通过对AST进行距离测量来为AUV提供绝对位置参考信息。AST的工作模式可分为信标模式和应答模式,信标模式是指AST仅向外界发射声脉冲信号;应答模式是指AST在接收到潜器或水面运载体发射的询问信号时,同时发射应答信号。在实际应用中,可根据需求选择AST的工作模式。AST具有制造成本低、布设灵活方便、局部海域定位精度高等优点,基于AST的水下距离辅助导航定位逐渐受到国内外学者的关注。
(3)航位推算技术
航位推算通常指二维平面内或三维空间中的位置推算。在水下环境中,利用DVL速信息并同时利用磁罗经(magnetic compass pilot,MCP)测航向信息可递推出二维平面的位置信息。而对于三维空间的航位推算,则需要通过陀螺罗经输出的三维姿态信息及DVL的测速信息来递推获取三维位置信息。DVL本质上属于速度声纳设备,它以多普勒效应为基础,利用声脉冲发射信号与散射回波信号之间的多普勒频移信息,得到运载体相对海水层或海底的速度。DVL的测速误差随时间不会累积,短期内航位推算方式具有较高的定位精度。对于航位推算***来说,其无法直接消除导航传感器的误差。因此与INS一样,航位推算***也存在定位误差随时间不断积累的问题,其长期定位精度难以保证。
(4)水下组合导航技术
在水下环境中,SINS难以长时间独立工作,需要利用各种外部辅助手段为SINS提供参考信息,再利用信息融合技术,实现对SINS初始姿态误差的补偿及累积误差的校正和抑制。目前主要的方法有:
1)利用AUV上浮或者投布放浮标等方式获取位置辅助信息;
2)利用水下声学定位***(Acoustic positioning system,APS)提供位置辅助信息;
3)利用多普勒速度计程仪(Doppler velocity log,DVL)提供速度辅助信息;
4)通过海洋物理场匹配获取位置辅助信息等。
在以上的方式中,目前存在的问题:1)AUV通过上浮或者利用浮标获取位置信息的方式存在暴露目标的风险;
2)海洋物理场匹配现阶段仍然处于理论研究阶段,精确地建立地球物理数据库及检测地球物理参数等重大问题尚未得到有效解决,海洋物理长的建设还不能满足实际应用的需求;
3)水下DVL测速容易受到外部复杂环境的干扰,不能保证连续的高精度测速;
4)水下声学定位***的布设成本高,尤其是在远海、深海布设的难度更加大。
综上所述,现有技术存在的问题是:1)现有水下单一定位方式,持续时间短,且定位精度不高,误差大。
(2)现有水下组合定位方法存在暴露目标的风险,容易受到外部复杂环境的干扰,无法保证连续高精度测速;且成本高,难度大。
解决上述技术问题的难度:不同于陆地上或水面上,由于电磁波信号在水中衰减极快,导致水下SINS的动态启动以及长航时、长航程导航定位无法获取来自GPS的速度和位置信息的辅助。由于初始姿态误差及IMU器件误差的存在,基于牛顿第二定律进行积分推算的惯导***在解算过程中不可避免的存在振荡型、常值型和积累型的***误差,且INS的导航定位误差随着时间不断积累,因此***的长期导航定位精度难以保证。在水下无卫星的环境中,SINS难以长时间独立工作,获取的速度、位置信息将产生较大的误差,使导航工作变得困难。为解决这种问题,需要利用各种外部辅助手段为SINS提供参考信息。难度在于如何选择合适的组合导航方式及导航数据信息融合算法的实现。
解决上述技术问题的意义:随着科学技术的不断发展,占据着地球70%以上面积的海洋已经成为许多国家争相开发和利用的领域以及军事必争之地。未来战争将是由陆地向太空和海洋迅速延伸的立体战争,海洋空间成为各海洋大国以及濒海国家积极开发与利用的主要阵地,深海战场将成为未来冲突与战争的主要场所。水下导航定位技术成为海洋资源勘探、开发,海洋工程以及军事作战和精确打击等国民和国防应用领域中必不可少的关键技术。高精度、高可靠性的水下导航定位技术在AUV的发展战略中具有重要地位。随着水下导航定位技术的迅猛发展,单一的导航传感器由于其自身原因无法满足AUV对高精度和可靠性导航信息的需求,因此通过多种导航传感器输出的导航信息进行有效融合,解决上述问题,可以提高导航***的导航定位精度和容错能力,可实现水下AUV的长航时、长航程高精度低误差导航工作,可解决现有水下组合导航定位方法存在暴露目标的风险,实现AUV远程续航及隐蔽长航时工作。
发明内容
针对现有技术存在的问题,本发明提供了一种水下组合导航信息融合方法、***及自主式水下航行器。具体涉及一种SINS/DVL/AST水下组合导航信息融合方法及水下组合导航信息融合***。
本发明是这样实现的,一种SINS/DVL/AST水下组合导航信息融合方法,所述SINS/DVL/AST水下组合导航信息融合方法包括:
步骤一,利用SINS导航技术获取载体的姿态、速度、位置相关信息;利用DVL导航技术获取外部速度信息;利用AST导航技术获取外部位置信息;
步骤二,利用滤波算法将获取到的载体的姿态、速度、位置相关信息、外部速度信息、外部位置信息进行融合,并输出融合后的导航信息。
进一步,步骤一中,所述利用DVL获取外部速度信息的方法包括:
(1)单波束配置测速,
发射声波频率为f0,其速度为c,AUV沿水平方向航行,速度记为vx;
根据多普勒效应,在P点处接收到的频率为:
在O'点处接受到的频率为:
DVL接收频率减发射频率即多普勒频移:
fd=f2-f0;
vx在水中的传递速度远远小于c,α≈α',式简化为:
f0,α,c已知数,求出DVL的频移计算出相应的速度:
(2)双波束Janus配置测速:沿艏部方向波束的频率为:
艉部方向的频率为:
fr1和fr2是DVL艏部和艉部方向的接受频率,计算出多普勒频移为:
得到双波束配置情况下的航行器速度为:
(3)四波束Janus配置测速,在水下航行器的左右侧分别各添加一个发射器,构成四波束多普勒配置,航速为:
fd13,fd24为x和y方向的多普勒频移,计算航行器速度为:
根据多普勒的fd13和fd24计算出偏流角
进一步,步骤一中,所述AST导航技术获取外部位置信息的方法包括:
x=rcosθx,y=rcosθy,z=rcosθz;
式中,r=cs·ts/2表示USBL到测量目标的斜距;cs表示水中声速,ts为声信号在水中的双程传播时延;θx、θy由USBL两轴上的阵元进行方位估计得到;θx、θy和θz满足如下关系式
cos2θx+cos2θy+cos2θz=1;
当USBL的位置准确已知时,通过USBL与AST之间进行问答,并通过USBL对AST进行方位估计求解获取AST的位置;当AST的位置准确已知时,通过USBL与AST之间进行问答求解获取USBL的位置。
进一步,步骤二所述利用滤波算法将获取到的载体的姿态、速度、位置相关信息、外部速度信息、外部位置信息进行融合包括:
基于获取到的信息是否受到非高斯噪声污染从而设置子滤波器采用RKF算法或标准KF算法;
当速度观测信息和位置观测信息均受到非高斯噪声的污染时,则子滤波器1和子滤波器2均采用RFK算法;
当DVL输出的速度观测信息受到非高斯噪声污染,而AST输出的位置观测信息正常,则可设置子滤波器1为RKF算法,子滤波器2为标准的KF算法;
当DVL输出的速度观测信息正常,AST输出的位置观测信息受到非高斯噪声污染,则设置子滤波器1为标准的KF算法,子滤波器2为RKF算法。
进一步,步骤二,所述利用滤波算法将获取到的载体的姿态、速度、位置相关信息、外部速度信息、外部位置信息进行融合包括以下步骤:
(1)时间更新:
(2)量测更新:
2.2)若判定观测量zi,k正常,则令λi,k=1;若判定观测量zi,k异常,则根据下式计算λi,k;
2.6)根据Pk|k=(I-KkHk)Pk|k-1计算状态估计误差协方差矩阵Pi,k;
本发明的另一目的在于提供一种实施所述SINS/DVL/AST水下组合导航信息融合方法的SINS/DVL/AST水下组合导航信息融合***,所述SINS/DVL/AST水下组合导航信息融合***包括:
SINS导航模块,用于利用SINS导航技术获取载体的姿态、速度、位置相关信息;
DVL导航模块,用于利用DVL导航技术获取外部速度信息;
AST导航模块,用于利用AST导航技术获取外部位置信息;
融合模块,用于利用滤波算法将获取到的载体的姿态、速度、位置相关信息、外部速度信息、外部位置信息进行融合,并输出融合后的导航信息。
本发明的另一目的在于提供一种存储在计算机可读介质上的计算机程序产品,包括计算机可读程序,供于电子装置上执行时,提供用户输入接口以实施所述SINS/DVL/AST水下组合导航信息融合方法。
本发明的另一目的在于提供一种计算机可读存储介质,储存有指令,当所述指令在计算机上运行时,使得计算机执行所述SINS/DVL/AST水下组合导航信息融合方法。
本发明的另一目的在于提供一种实施所述水下组合导航信息融合方法的自主式水下航行器。
综上所述,本发明的优点及积极效果为:本发明能够实现长航时、长航程导航定位,同时能够校正和抑制定位过程中的误差。本发明提出的FRKF算法可有效抑制观测野值对滤波结果的影响,且相比于TFKF算法,FRKF算法具有更好的数据平稳性。同时试验结果初步表明:相比于TFKF算法,本发明提出的FRKF算法在非高斯环境中具有更高的组合导航精度和稳定性。
本发明由SINS、DVL和AST等导航传感器组成,基于各个导航传感器提供相应的导航信息,利用滤波算法将这些导航信息进行融合,进而得到更高精度、更加稳定的导航信息。
本发明设计FKF算法为无反馈模式,即主滤波只对子滤波器输出进行融合,且融合后的结果对子滤波器无反馈重置;不仅提高了滤波的容错性能,同时保证某个子滤波器发生故障时不会影响整个滤波器的滤波性能,从而确保水下组合导航***的容错性能。
附图说明
图1是本发明实施例提供的水下组合导航信息融合方法流程图。
图2是本发明实施例提供的捷联惯导***原理图。
图3是本发明实施例提供的DVL测速的配置中测速原理图。
图4是本发明实施例提供的AST水声导航定位原理示意图。
图5是本发明实施例提供的AST+USBL定位原理几何示意图。
图6是本发明实施例提供的水下组合导航信息融合***结构示意图。
图中:1、SINS导航模块;2、DVL导航模块;3、AST导航模块;4、融合模块。
图7是本发明实施例提供的SINS/DVL/AST水下组合导航信息融合***原理图。
图8是本发明实施例提供的SINS/AST组合导航原理示意图。
图9是本发明实施例提供的SINS/DVL/AST组合导航原理示意图。
图10是本发明实施例提供的FKF算法的结构图。
图11是本发明实施例提供的不同组合导航方法得到的位置误差示意图。
图12是本发明实施例提供的不同方法的组合导航姿态误差示意图。
图13是本发明实施例提供的不同方法的组合导航速度误差示意图。
图14是本发明实施例提供的不同方法的组合导航位置误差示意图。
图15是本发明实施例提供的不同方法的组合导航姿态误差图。
图16是本发明实施例提供的不同方法的组合导航速度误差图。
图17是本发明实施例提供的不同方法的组合导航位置误差图。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白,以下结合实施例,对本发明进行进一步详细说明。应当理解,此处所描述的具体实施例仅仅用以解释本发明,并不用于限定本发明。
现有水下单一定位方式,持续时间短,且定位精度不高,误差大。现有水下组合定位方法存在暴露目标的风险,容易受到外部复杂环境的干扰,无法保证连续高精度测速;且成本高,难度大。
针对现有技术存在的问题,本发明提供了一种水下组合导航信息融合方法、***及自主式水下航行器,下面结合附图对本发明作详细的描述。
如图1所示,本发明实施例提供的水下组合导航信息融合方法包括:
S101,利用SINS导航技术获取载体的姿态、速度、位置相关信息;利用DVL导航技术获取外部速度信息;利用AST导航技术获取外部位置信息。
S102,利用滤波算法将获取到的载体的姿态、速度、位置相关信息、外部速度信息、外部位置信息进行融合,并输出融合后的导航信息。
步骤S101中,利用SINS获取载体的姿态、速度、位置相关信息包括:
陀螺和加速度计直接固联在运载体上,所以这种惯性导航***称之为捷联式惯导***(SINS),简称捷联惯导。陀螺仪用来测量运载体的角速度信息。加速度计用来测量运载体的线运动信息,机载计算机根据陀螺仪输出的角速度信息,通过姿态矩阵解算可得到航向和姿态角参数。加速度计输出加速度信息经过坐标转换后变换成沿导航系的加速度信息,经过计算机两次积分可得到载***置参数。捷联惯导***原理图如图2。
步骤S101中,利用DVL获取外部速度信息包括:
多普勒计程仪(DVL)是声学仪器,利用载体发射的超声波至水底返回产生的多普勒频移进行工作。多普勒计程仪可以提供在载体坐标系中声波发射源的发射速度。当其在可测量的海底范围内时,可以提供前进方向以及侧向的基准速度,输出的速度信息准确度很高且较为稳定。
DVL测速的配置分为3种:单波束配置、双波束Janus配置和四波束Janus配置,其配置的测速原理如图3所示。具体包括:
(1)单波束配置测速
单向单波束配置的DVL如上图所示。假设其发射声波频率为f0,其速度为c,AUV沿水平方向航行,速度记为vx。
根据多普勒效应原理,在P点处接收到的频率为:
在O'点处接受到的频率为:
DVL接收频率减其发射频率即多普勒频移:
fd=f2-f0
可知,vx在水中的传递速度远远小于c,此时可以近似α≈α',式可以简化为:
f0,α,c都是已知数,只要求出DVL的频移就可以计算出相应的速度:
(2)双波束Janus配置测速:
双波束值的是在航行器的艏部和艉部方向各发射一个波束,相比于单波束的情况可以提高一定的测速精度。这是因为当水下航行器上下方向存在波动时,因双波束自身对称特性,两个波束的发射频率相同,其波束倾斜角也相同。如上图3所示,可以得出此时沿艏部方向波束的频率为:
由于向艉部发射的波束与水下航行器运动的方向是反方向,所以速度应该为负值,此时艉部方向的频率是:
在这其中,fr1和fr2是DVL艏部和艉部方向的接受频率,根据上述原理可以计算出多普勒频移为:
可得到双波束配置情况下的航行器速度为:
(3)四波束Janus配置测速:
在实际应用中,还需要测量水下航行器的横向移动速度,在水下航行器的左右侧分别各添加一个发射器,构成四波束多普勒配置,此时刻航速为:
fd13,fd24为x和y方向的多普勒频移,可计算航行器速度为:
根据多普勒的fd13和fd24可计算出偏流角
事实上,只需要三条波束就可以提供水下航行器的三个方向上的速度分量,但目前实际工程中大多采用四波束Janus配置。其原因是平面阵列天线产生四波束更为容易,其精度相比于三波束而言更高,还可以给***提供适当的测量冗余度,以提高***的可靠性。
步骤S101中,AST+USBL水下定位基本原理
AST作为LBL定位***的延伸,已在水下组合导航***中获得了成功应用。海底AST定位原理示意图如图4所示。
由上图4可知,在AST的位置事先得到校准的前提下,只需测得AUV与AST之间的相对位置XUVS,即可实现水下声学单应答器(或信标)辅助下的定位导航。AUV的绝对位置表达式如下式所示。
XUV=XS+XUVS
式中,XUV为AUV在n系下的坐标,XS为事先校准得到的AST在n系下的坐标,XUVS为n系下AUV相对于AST的坐标。AST的导航定位方式可分为距离方位法定位、纯距离定位方式和纯方位定位方式。各方式的原理如下:
距离方位法定位:UV上的USBL与AST进行单词问答,即可测定相对距离和方位,然后解算出UV的位置。此方法单次问答即可实现定位,但是***复杂性高。
纯距离定位:UV上的USBL或收发合置换能器与AST进行多点多次问答,即可测定相对坐标,然后解算出UV的位置。此方法***复杂性低,但需要多点测量,测量精度与测量点的位置密切相关。
纯方位定位:UV上的USBL与AST进行多点多次问答,可测定AST的方位信息,即可测定测量点的相对位置坐标,然后可解算出UV的位置。此方法可工作于被动方式,隐蔽性好,但***较复杂,需要多点测量。
AUV在水下航行的过程总,若注重定位信息的实时性,应选择距离方位法定位方法;若注重降低***的整体复杂性,应选择纯距离定位方式;若注重AUV的隐蔽性,应选择纯方位定位方式。
AST+USBL定位原理三维几何关系示意图如图5所示。USBL是一个4元平面阵,各阵元之间的几何关系满足十字垂直。以声学阵的中心为原点o,阵元1和阵元2在x轴(x轴指向船右舷),阵元3和阵元4在y轴(y轴指向船艏),根据右手螺旋定则确定z轴,oxyz即为基阵坐标系(记为s系)。
x=rcosθx,y=rcosθy,z=rcosθz
式中,r=cs·ts/2表示USBL到测量目标的斜距;cs表示水中声速,ts为声信号在水中的双程传播时延。θx、θy可由USBL两轴上的阵元进行方位估计得到。θx、θy和θz满足如下关系式
cos2θx+cos2θy+cos2θz=1
当USBL(即AUV)的位置准确已知时,可通过USBL与AST之间进行问答,并通过USBL对AST进行方位估计求解获取AST的位置。反之,当AST的位置准确已知时,亦可通过USBL与AST之间进行问答来求解获取USBL(即AUV)的位置。
步骤S102中,本发明实施例提供的利用滤波算法将获取到的载体的姿态、速度、位置相关信息、外部速度信息、外部位置信息进行融合包括:
基于获取到的信息是否受到非高斯噪声污染从而设置子滤波器采用RKF算法或标准KF算法。
当速度观测信息和位置观测信息均受到非高斯噪声的污染时,则子滤波器1和子滤波器2均采用RFK算法。
当DVL输出的速度观测信息受到非高斯噪声污染,而AST输出的位置观测信息正常,则可设置子滤波器1为RKF算法,子滤波器2为标准的KF算法。
当DVL输出的速度观测信息正常,AST输出的位置观测信息受到非高斯噪声污染,则设置子滤波器1为标准的KF算法,子滤波器2为RKF算法。
本发明实施例提供的利用滤波算法将获取到的载体的姿态、速度、位置相关信息、外部速度信息、外部位置信息进行融合包括以下步骤:
(1)时间更新:
(2)量测更新:
2.2)若判定观测量zi,k正常,则令λi,k=1;若判定观测量zi,k异常,则根据下式计算λi,k。
2.6)根据Pk|k=(I-KkHk)Pk|k-1计算状态估计误差协方差矩阵Pi,k。
如图6-图7所示,本发明实施例提供的水下组合导航信息融合***包括:
SINS导航模块1,用于利用SINS导航技术获取载体的姿态、速度、位置相关信息。
DVL导航模块2,用于利用DVL导航技术获取外部速度信息。
AST导航模块3,用于利用AST导航技术获取外部位置信息。
融合模块4,用于利用滤波算法将获取到的载体的姿态、速度、位置相关信息、外部速度信息、外部位置信息进行融合,并输出融合后的导航信息。
下面结合具体实施例对本发明的技术方案作进一步说明。
实施例1:
1、组合导航技术
1.1SINS/AST组合导航技术
SINS/AST组合导航***只需在AUV上装备一个水声测距设备(如USBL***或收发合置换能器),利用水下预先投(布)放水平位置和深度已知的单应答器(信标),通过获取单应答器(信标)与AUV之间的位置信息,再利用信息融合算法即可实现精确的导航定位。SINS/AST组合导航模式充分发挥了SINS和APS的优势,并且极大简化了使用条件和降低了使用成本。SINS/AST组合导航基本原理是当AUV运动到AST的作用区域内时,由AST为其提供精确的位置信息从而抑制SINS的误差发散,SINS/AST组合导航原理示意图如图8所示。
1.2SINS/DVL/AST组合导航技术
AST的作用范围是有限的,由图9可以看出,当AUV运动到AST作用区域时,SINS的位置误差发散会得到有效抑制。但是当AUV离开AST的作用区域时,SINS的位置误差将会以很快的速度持续发散,限制了AUV在水下航行的能力和范围。因此使用SINS/AST组合导航方式存在一定的局限性。DVL测得的速度误差不会随时间累积,AUV在大深度、广海域航行时,DVL可为SINS提供可靠的外速度辅助信息。当AUV在水下机动航行时,为了抑制SINS位置误差的持续发散可设计以下组合导航方案:当AUV进入AST的作用区域时,采用SINS/DVL/AST组合导航模式,此时SINS的导航定位误差保持稳定;当AUV离开AST的作用区域时,采用SINS/DVL组合导航模式,此时AUV在DVL速度辅助的条件下导航定位误差缓慢发散,在即将达到误差上限时,使得AUV能够再次进入AST作用范围。SINS/DVL/AST组合导航原理示意图如图9所示。
2、本发明方案
2.1多传感器组合导航***模型
本发明采用的水下组合导航***如图所示。组合导航***主要由SINS、DVL和AST等导航传感器组成。各个导航传感器提供相应的导航信息,利用滤波算法将这些导航信息进行融合,进而得到更高精度、更加稳定的导航信息。
2.2组合导航***状态方程
(1)SINS状态方程建立
SINS经过初始对准后,俯仰失准角、横滚失准角和航向失准角均满足线性条件。根据SINS误差方程的推导,选取SINS状态量为:
上式对应的状态方程如下所示。
式中,WSINS~N(0,QSINS)为***噪声,QSINS为***噪声协方差阵。
由式(2.2.2)建立的状态转移矩阵FSINS为:
式中,F为7×7矩阵,F中的非零元素如下:
F1,4=1/Re,
F2,1=(vE/Re)tanLsecL,
F2,3=secL/Re,
F3,3=(vN/Re)tanL,
F3,4=2ωie sinL+(vE/Re)tanL,
F3,6=-fU,
F3,7=fN,
F4,3=-[2ωiesinL+(vE/Re)tanL],
F4,5=fU,
F4,7=-fE
F5,4=-1/Re,
F5,6=ωiesinL+(vE/Re)tanL,
F5,7=-[ωiecosL+(vE/Re)],
F6,1=-ωiesinL,
F6,3=1/Re,
F6,5=-[ωiesinL+(vE/Re)tanL],
F6,7=-vE/Re,
F7,1=ωiecosL+(vE/Re)(secL)2
F7,3=tanL/Re,
F7,5=[ωiecosL+(vE/Re)],
F7,6=vN/Re (2.2.4)
(2)DVL状态方程建立
刻度系数误差δC、速度偏移误差δVd以及偏流角误差δφ等是影响DVL对速度的测量主要因素,其中,δC和δφ可表示为一阶Markov过程。因此,DVL的误差模型如(2.2.6)所示。
式中,βd、βφ分别为δVd和δφ的一阶Markov过程相关时间;wd、wφ分别为δVd和δφ的Gauss白噪声;δC为常数。
根据式,选取DVL状态量为:
XDVL=[δVd;δφ;δL] (2.2.7)
上式对应的状态方程为;
式中,FDVL=diag(-βd,-βφ,0),WDVL为Gauss白噪声。
(3)AST状态方程的建立
类似于LBL定位***,AST定位***的定位误差可表示为一阶Markov过程,如下式所示。
式中,δL、δλ、δh分别为纬度误差、经度误差和高度误差;分别为δL、δλ和δh对应的一阶Markov过程相关时间;wL、wλ和wh分别为δL、δλ和δh的Gauss白噪声。根据式(2.2.9),选取AST的状态量为:
XAST=[δL;δλ;δh] (2.2.10)
由式(2.2.9)和式(2.2.10)可得AST的状态方程为:
式中,FAST=diag(-τL,-τλ,-τh),WAST为Gauss白噪声。
2.2.2组合导航***量测方程
(1)SINS/DVL量测方程
(2)SINS/AST量测方程
AST可在作用范围内向AUV提供距离辅助信息XUVS,进而通过(2.1.1)转化为n系下的经度λAST、纬度LAST和高度hAST等位置辅助信息。选取纬度误差δL和精度误差δλ作为观测量,SINS/AST量测方程如式(2.2.15)所示。
式中,LAST、λAST可式(2.1.1)获取,量测矩阵Hp=[I2×2,02×11]。
2.3联邦鲁棒卡尔曼滤波(FRKF)算法
AUV在深水环境中航行时,其获取的外部辅助信息可分为DVL提供的速度信息和AST提供的位置信息。单一的集中式滤波器无法全面客服不同导航传感器***带来的不良影响,FKF利用其独特的结构优势可将多种导航传感器的信息进行组合,为运载体提供准确、可靠的导航信息。FKF的基本构成单元是KF,可以说FKF不仅具备KF的优点,而且还继承了KF的缺点。深海环境复杂,这就导致AUV获取的外部辅助信息易受非高斯噪声污染,此种情形下FKF也像KF一样易发散。针对此问题,本发明根据现有的鲁棒KF算法(Robust KalmanFilter,RKF),提出联邦鲁棒卡尔曼滤波(FRKF)算法。
2.3.1RKF算法
k时刻离散KF的时间更新方程和量测更新方程如下:
(a)时间更新方程
(b)量测更新方程
Pk|k=(I-KkHk)Pk|k-1 (2.3.6)
将式(2.3.8)带入(2.3.7)可得:
式(2.3.9)可转化为求解λk的非线性问题,如式(2.3.10)所示。
式中,λk可通过牛顿迭代法求解。因此λk(i+1)与λk(i)的关系可表示为:
式中,且λk(i)初始值为λk(0)=1。当评判指标满足时,迭代终止。在求解出λk后,通过式(2.3.8)对量测噪声阵Rk进行膨胀,得到新的量测噪声阵λkRk。用λkRk替换Rk进行标准KF滤波即可得到RKF算法。
2.3.2FKF算法
2.3.2.1FKF基本原理
FKF算法的基本结构如图所示。由图10可以看出,FKF采用两个局部滤波器和一个主滤波器组成的两级滤波结构。对于组合导航***来说,因SINS能够完备地给出姿态、速度和位置信息,因此组合导航***将SINS作为公共参考***。记SINS/DVL组合导航***对应的子滤波器为“子滤波器1”,其对应的标准KF算法为“KF1算法”,其对应的鲁棒KF算法为“RKF1算法”。SINS/AST组合导航***对应的子滤波器为“子滤波器2”,其对应的标准KF算法为“KF2算法”,其对应的鲁棒KF算法为“RKF2算法”。
DVL可为SINS提供速度观测zv,AST可为SINS提供位置观测zp。根据信息分配系数βi的分配策略,可将FKF分为四个模式:
(1)无反馈模式:在此模式下仅在滤波初始时刻对信息进行分配,主滤波器只发挥融合作用,融合结果不对子滤波器进行反馈重置,容错性能较高。
(2)零复位模式:在此种模式下,当子滤波器向主滤波器输出融合结果后,自动置零。
(3)融合反馈模式:在此种模式下,主滤波器融合子滤波器的输出后,将结果反馈给子滤波器以重置子滤波器,此种模式滤波精度较高,但容错性较差。
(4)变比例模式:在此种模式下,子滤波器与主滤波器对信息平均分配,此种模式容错性较差。在实际应用中可根据需求选择合适的工作模式。由于各个观测源相互独立,k时刻FKF的具体过程如下。
(a)信息分配
∑βi+βm=1 (2.3.13)
式中,βm为主滤波器的信息分配系数,下标m表示主滤波器。
(b)时间更新
(c)量测更新
(d)信息融合
本发明为了确保滤波的容错性能,设计FKF算法为无反馈模式,即主滤波只对子滤波器输出进行融合,且融合后的结果对子滤波器无反馈重置。这样设置的好处是在某个子滤波器发生故障时不会影响整个滤波器的滤波性能,从而确保水下组合导航***的容错性能。
2.3.2.2信息分配系数自适应选取方法
在SINS/DVL/AST组合导航***中,各个子滤波器公共状态向量相同,因此设计主滤波器状态量和协方差阵Pm不进行时间更新,即设计Pm,k=0。组合导航***选择信息分配系数是决定FKF滤波性能的重要因素,传统的信息分配系数选取方法多依赖于经验或大量实验,一般设置为各个子滤波器具有相同的分配系数。在此种分配方式下,一个子滤波器性能的下降可能会对FKF的全局滤波性能造成较大影响。因此根据各子滤波器滤波精度自适应地调整信息分配系数对FKF的整体性能具有重要意义。为了选取合理的信息分配系数,首先对分配系数βi与滤波精度之间的关系进行分析,具体如下。
FKF状态量估计误差协方差的定义如下式所示。
由上式可以看出,越大,Pi越小,表示Xi的估计精度越高;反之,越小,Pi越大,表示Xi的估计精度越低。由式(2.3.12)、式(2.3.14)和式(2.3.16)可知,第i个子滤波器在k时刻的先验估计为:
又卡尔曼滤波增益Ki,k可表示为:
将式(2.3.20)和(2.3.22)代入式(2.3.21)得:
K时刻第i个子滤波器状态估计误差协方差的后验估计Pi,k为:
将式(2.3.23)和式(2.3.24)分别代入式(2.3.16)得:
通过以上推导可知,子滤波器的估计精度越高,Pi,k越小,由(2.3.24)可知越大;反之,子滤波器的估计精度越低,Pi,k越大,由(2.3.24)可知越小。因此,选取信息分配系数βi时应满足:估计精度越高的子滤波器应分配更大的βi。又因状态估计误差协方差Pi,k是反应子滤波器估计精度的重要指标,因此βi的调整策略如下所示。
式中,表示矩阵的对角线元素组成的一个新的对角矩阵;||·||F表示Frobenius范数。由式(2.3.26)可以看出,当子滤波器的估计精度越高,即越大,从而βi越小,因此估计精度低的子滤波器应分配更小的信息分配系数。以上信息分配系数选取方法可使FKF始终保证精度高的子滤波器具备更大的信息分配系数,从而可在保证容错性能的前提下有效提高FKF的滤波精度。
由上式可以看出,若第i个子滤波器估计精度较高,即Pi,k越小,越大,越大,βi越大,从而第i个子滤波器分配较大的权值;反之,若第i个子滤波器估计精度较低,即Pi,k越大,越小,越小,βi越小,从而第i个子滤波器分配较小的权值。式(2.3.28)不仅能够使FKF算法具备较好的容错性能,而且可使FKF算法保持较高的滤波精度。
2.3.2.3融合参与信息时间不同步问题
式(2.3.12)-(2.3.16)对FKF算法进行了公式描述,其要求所有参与信息融合的子滤波器获取的导航传感器子***输出的观测信息是同步的,但是这在实际应用中尤其是水下组合导航过程中是无法保证的,如SINS输出频率一般为100Hz-1000Hz,而DVL的输出频率一般为1Hz-几Hz,AST的输出频率一般只有0.1Hz-1Hz,且AST与DVL的输出并不能保证完全同步。因此,必须在信息融合时对各子滤波器输出作同步处理。本发明只考虑AST和DVL数据更新周期为SINS数据更新周期的整数倍时的简单情况,具体分析处理过程如下。假设SINS数据更新频率和周期分别为fINS、TINS,DVL数据更新频率和周期分别为fDVL、TDVL,AST数据更新频率和周期分别为fAST、TAST。SINS、DVL和AST数据更新周期之间的关系如式(2.3.29)所示。
TDVL=N1·TINS,TAST=N2·TINS (2.3.29)
式中,N1、N2为正整数,实际应用中N1一般不等于N2。设计FKF融合策略,具体如下。
S1:若N1≠N2,且N1>N2,则在kk·N2·TINS时刻进行主滤波器信息融合,融合时的子滤波器1输出的状态量X1和状态估计误差协方差P1均是通过时间更新过程(预测)获得的,直至子滤波器1获取观测量时,融合时的X1和P1均是通过量测更新过程获得;
S2:若N1≠N2,且N1<N2,则在kk·N1·TINS时刻进行主滤波器信息融合,融合时的子滤波器2输出的状态量X2和状态估计误差协方差P2均是通过时间更新过程获得的,直至子滤波器2获取观测量时,融合时的X2和P2均是通过量测更新过程获得;
S3:若N1≠N2,且N1·TINS>1s,N2·TINS>1s,则主滤波器在T's时刻进行融合,T'为不为0的自然数,且T'<N1·TINS,T'<N2·TINS,N1·TINS和N2·TINS均是T'的整数倍。在kk·N1·TINS进行信息融合时的子滤波器1输出的状态量X1和状态估计误差协方差P1均是通过量测更新过程获得的,其余时刻子滤波器1输出的状态量X1和状态估计误差协方差P1均是通过时间更新过程获得的。同理,在kk·N2·TINS进行信息融合时的子滤波器2输出的状态量X2和状态估计误差协方差P2均是通过量测更新过程获得的,其余时刻子滤波器2输出的状态量X2和状态估计误差协方差P2均是通过时间更新过程获得的。这种融合策略在无观测信息更新时利用子滤波器的预测进行T's一次的融合,有利于充分利用子滤波器的预测信息,但也可能使滤波整体性能低于S1和S2策略。
2.3.3FRKF算法
2.3.1节通过引入膨胀因子λ实现对KF的鲁棒化,得到RKF算法。KF与RKF的主要区别在于量测更新过程:对于标准的KF算法来说,用λkRk替换Rk的按照式(2.3.3)-(2.3.6)进行量测更新即可得到RKF算法;对于RKF算法来说,当λk=1时,RKF算法将退化为标准的KF算法。因此,FRKF算法与FKF算法具有相同的时间更新过程,区别仅在于两者的量测更新过程。以下给出k时刻FRKF算法的量测更新过程如S1-S4所示。
S1:根据式(2.3.3)计算μi,k,根据式(2.3.7)判断观测量zi,k是否正常。若判定观测量zi,k正常,则令λi,k=1;若判定观测量zi,k异常,则根据式(2.3.9)-(2.3.11)计算λi,k。根据式(2.3.8)计算
S4:根据式(2.3.6)计算状态估计误差协方差矩阵Pi,k。
由以上过程推导出的FRKF算法,适用于速度观测信息和位置观测信息均受到非高斯噪声的污染的情形,在此情形下其子滤波器1和子滤波器2均为RKF算法。若DVL输出的速度观测信息受到非高斯噪声污染,而AST输出的位置观测信息正常,在FRKF算法中可设置子滤波器1为RKF算法,子滤波器2为标准的KF算法;反之,若DVL输出的速度观测信息正常,AST输出的位置观测信息受到非高斯噪声污染,在FRKF算法中可设置子滤波器1为标准的KF算法,子滤波器2为RKF算法。因此,可以认为本发明提出的FRKF算法是广义上的联邦鲁棒滤波算法,即可根据子滤波器获取的观测信息是否受到非高斯噪声污染来设置子滤波器为RKF算法或标准KF算法。
下面结合具体实验对本发明作进一步说明。
3实验:
3.1基于船载实测数据半物理仿真试验验证
3.1.1数据生成
选取两组3600s船载实测数据进行水下组合导航半物理仿真试验,仿真试验是基于matlab 2015a软件进行的。选取的两组3600s数据包含:陀螺仪和加速度计的原始数据,对应的姿态、速度和位置基准,对应的GPS输出,对应的DVL输出。这两组3600s数据对应的DVL输出、GPS输出满足:
(1)第一组DVL输出数据不受非高斯噪声污染;第二组DVL输出数据受数据野值污染。
(2)GPS输出不受非高斯噪声污染。
利用GPS输出的位置信息模拟水下AST输出的位置信息:AST的输出频率一般为0.1Hz-1Hz,由此对GPS输出的位置信息进行降频,使其10s输出一个位置信息(频率为0.1Hz)。AST+USBL导航定位误差最大不超过15m。因此,仿真试验在生成AST定位信息时,基于降频后的GPS位置信息认为地引入幅值为20m的随机定位误差。
3.1.2实验验证1
为了检验2.3.2节提出分配系数自适应选取方法的有效性,利用第一组数据进行导航仿真试验。在组合导航试验中,FKF设置为无反馈模式,即各个子滤波器进行独立滤波。
记按照式(2.3.26)确定信息分配系数的FKF为IFKF(Improved FKF);信息分配系数平均分配的FKF算法为TFKF(tradition FKF),TFKF算法即为经典的FKF算法。选择第一组3600s数据,结合AUV在水下组合导航的实际,即AUV会使出AST的作用范围的情况:设置从第1500s开始AUV使出AST的作用范围;从第2400s开始,设置AUV重新进入AST的作用范围。也就是说AST提供的位置信息丢失了900s,且在无观测信息期间子滤波器2只进行时间更新(预测),即在此期间子滤波器2的融合结果是通过预测得到的。设置AST在初始时刻对AUV的定位误差为20m。在以上仿真条件下,分别利用KF2算法(SINS/AST组合导航***)、IFKF算法、TFKF算法进行组合导航试验,组合导航采用闭环修正模式,即在主滤波器进行信息融合后,利用融合后的状态量对SINS输出的姿态、速度和位置进行修正,同时子滤波器中的姿态、速度和位置对应的状态量重新置0。组合导航得到的位置误差结果分别为图11(a)-(b)所示。图11(a)-(b)分别为纬度估计误差和经度估计误差,其中,黑色虚线为KF2算法(SINS/AST)得到的位置估计误差,蓝色虚线为TFKF算法得到的位置估计误差,红色实线为IFKF算法得到的位置估计误差。
由图11可以明显看出,当AUV驶出AST作用范围(1500s~2400s)时,KF2算法对位置的估计误差曲线呈发散趋势,TFKF算法和IFKF算法对位置的估计误差曲线虽有缓慢发散趋势,TFKF算法和IFKF算法的组合导航精度和稳定性明显优于KF2算法。在此期间,KF2算法得到的纬度/经度误差最大值为-242.90m/-86.37m,TFKF算法得到的经度/纬度误差最大值为-22.40m/-32.24m,IFKF算法得到的纬度/经度误差最大值为-20.29m/-31.31m。这是因为在AUV的航行过程中,IFKF算法主滤波器的输出是通过式(2.3.28)进行加权融合得到的,且IFKF算法采用闭环修正模式实时地修正公共参考***SINS的解算误差,这就使得AUV在驶出AST作用范围之前SINS/DVL组合导航子***与SINS/AST组合导航子***对位置的估计精度相当。AUV离开AST作用范围后,SINS/AST无观测信息,此期间可认为子滤波器2发生故障。式(2.3.28)使主滤波器输出中的子滤波器1的权值高于子滤波器2的权值,因此在AUV离开AST作用范围后,子滤波器1起主要作用,致使IFKF算法得到的位置估计误差曲线呈缓慢发散趋势。验证了本发明所提信息分配系数自适应选取方法的有效性。
3.1.3试验验证2
利用选取的第二组3600s数据验证非高斯条件下FRKF算法相比与TFKF算法的优势。基于上节模拟生成的AST数据,分别利用FRKF算法和TFKF算法进行组合导航试验。
DVL属于主动声纳设备,在特殊环境下频繁使用DVL发散声呐信号容易降低AUV的隐蔽性。为了进一步增强AUV的隐蔽性,模拟AUV在进入AST作用范围后降低DVL的使用频率的情形:对DVL的输出进行人为地阻隔,使AUV每隔30s获取一个速度观测信号。同时,由于水下环境的复杂性,AST输出易受非高斯噪声的污染,每隔150s人为地将幅值为500m的位置误差(野值)引入式(2.2.15)。图8(a)-(c)分别为利用不同方法进行组合导航后的俯仰角误差、横滚角误差和航向角误差。
图12中,黑色实现表示利用TFKF算法进行组合导航的姿态误差曲线,红色虚线表示利用FRKF算法进行组合导航的姿态误差曲线。由图可以看出,当SINS/AST子滤波器观测量受到野值污染时,TFKF算法的组合导航姿态误差曲线在野值出现的时刻发生了突变,而FRKF算法的组合导航姿态误差曲线平稳。当组合导航收敛后,FRKF算法得到的俯仰角误差从0.1°下降到0.010°以内,横滚角误差从0.1°下降到0.015°以内,航向角误差从0.5°下降到0.3°以内。
图13和图14分别为利用不同方法进行组合导航的速度误差和位置误差。图13(a)和图13(b)分别为东向速度误差和北向速度误差;图14(a)和图14(b)分别为纬度误差和精度误差。图13中,黑色实线为利用TFKF算法进行组合导航的速度误差曲线,红色虚线为利用FRKF算法进行组合导航的速度误差曲线;图14中,黑色实线为利用TFKF算法进行组合导航的位置误差曲线,红色虚线为利用FRKF算法进行组合导航的位置误差曲线。
通过试验可知,当组合导航收敛后,利用FRKF算法组合导航的东向速度误差收敛到0.2m/s以内,北向速度误差收敛到0.15m/s以内;纬度误差收敛到25m以内,经度误差收敛到15m以内。可以看出,当观测信息受到野值污染是,FRKF算法的组合导航精度和稳定性明显优于TFKF算法。通过试验及计算可知,利用TFKF算法和FRKF算法得到的速度误差标准差和位置误差标准差如下表所示。
表1不同方法得到的组合导航速度、位置误差标准差
TFKF算法 | FRKF算法 | |
东向速度误差标准差(m/s) | 0.2386 | 0.1332 |
北向速度误差标准差(m/s) | 0.3528 | 0.2734 |
纬度误差标准差(m) | 19.7700 | 8.6070 |
经度误差标准差(m) | 20.2200 | 6.9290 |
由图12-图13及表1可以看出,本发明提出的FRKF算法可有效抑制观测野值对滤波结果的影响,且相比于TFKF算法,FRKF算法具有更好的数据平稳性。试验结果初步表明:相比于TFKF算法,本发明提出的FRKF算法在非高斯环境中具有更高的组合导航精度和稳定性。
下面结合基于车载实测数据半物理仿真试验验证对本发明作进一步描述。
为了进一步验证本发明提出FRKF算法的有效性及相比于单一RKF算法的优势,设计了基于FOSN光纤捷联惯导***的车载实验。
车载实验在武汉市内进行,数据采集***的数据主要包含:UTC时间基准、陀螺仪和加速度计输出的原始数据、GPS输出的位置数据以及由SINS/GPS组合导航后生成的姿态、速度和位置基准数据。
利用SINS/GPS组合导航后的姿态和速度基准数据模拟生成深海DVL输出的速度数据,利用GPS输出的位置数据模拟生成AST输出的位置数据,模拟数据生成过程如下。
(1)深海DVL测速数据模拟
设置DVL漂移为航程的0.5%,DVL短时精度为0.05m/s,考虑到深海环境的复杂性,设置每间隔12s载体系测速受到幅值为50m/s的速度野值污染。实验中,将上述设置条件人为地引入上式。
(2)深海AST定位数据模拟
人为地对GPS数据进行降频,由1Hz降为0.1Hz。同时人为地引入幅值为50m的随机位置误差。由于深海环境的复杂性,AST输出极易受非高斯噪声的污染,每隔30s人为地将幅值为1nm的位置误差(野值)引入。
为了进一步验证本发明提出的FRKF算法的有效性和优势,选取3600s深海模拟数据进行水下组合导航仿真试验。考虑到提高AUV的隐蔽性,假设AUV进入AST作用范围以后,使AUV每隔30s受到一个DVL测速信号。同时,考虑到AUV会驶入AST的作用范围的情况:设置从第1600s开始AUV驶出AST的作用范围;从第2600s开始,设置AUV重新进入AST的作用范围。分别利用RKF1算法(SINS/DVL)、RKF2算法(SINS/AST)和FRKF算法进行组合导航试验,下图为不同方法组合导航后的姿态、速度和位置误差。图15不同方法的组合导航姿态误差。图16不同方法的组合导航速度误差。图17不同方法的组合导航位置误差。
图15、16、17为车载实测数据半物理仿真试验验证的误差曲线。具体地,图15不同方法的组合导航姿态误差。图15(a)~图15(c)分别为利用不同方法进行组合导航后的俯仰角误差、横滚角误差和航向角误差。
图16不同方法的组合导航速度误差。图16(a)、图16(b)分别为利用不同方法进行组合导航后的东向速度误差和北向速度误差。
图17不同方法的组合导航位置误差。图17(a)、图17(b)分别为利用不同方法进行组合导航后的纬度误差和经度误差。
由上图15-17可以看出,相比于RKF1和RKF2,本文提出的FRKF具有更高的容错性能,即当子滤波器2发生故障时,FRKF仍会使组合导航***整体的导航定位精度保持稳定。这是因为FRKF根据子滤波器精度对信息分配系数进行自适应调整,使滤波器的整体性能始终与性能较好的子滤波器保持一致。通过试验可知,组合导航收敛后,FRKF算法获取的纬度误差绝对值在48m以内,经度误差绝对值在56m以内。仿真试验结果进一步验证了本文提出的FRKF算法的有效性及相比于单一的RKF1算法和单一的RKF2算法在深海组合导航中的优势。
通过以上的实施方式的描述,本领域的技术人员可以清楚地了解到本发明可借助软件加必需的硬件平台的方式来实现,当然也可以全部通过硬件来实施。基于这样的理解,本发明的技术方案对背景技术做出贡献的全部或者部分可以以软件产品的形式体现出来,该计算机软件产品可以存储在存储介质中,如ROM/RAM、磁碟、光盘等,包括若干指令用以使得一台计算机设备(可以是个人计算机,服务器,或者网络设备等)执行本实用新型各个实施例或者实施例的某些部分所述的方法。
以上所述仅为本发明的较佳实施例而已,并不用以限制本发明,凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等,均应包含在本发明的保护范围之内。
Claims (7)
1.一种水下组合导航信息融合方法,其特征在于,所述水下组合导航信息融合方法包括:
步骤一,利用SINS导航技术获取载体的姿态、速度、位置相关信息,并利用DVL导航技术获取外部速度信息,同时利用AST导航技术获取外部位置信息;
步骤二,利用滤波算法将获取到的载体的姿态、速度、位置相关信息、外部速度信息、外部位置信息进行融合,并输出融合后的导航信息;
利用滤波算法将获取到的载体的姿态、速度、位置相关信息、外部速度信息、外部位置信息进行融合的方法包括:
基于获取到的信息是否受到非高斯噪声污染,并采用RKF算法或标准KF算法设置子滤波器;
速度观测信息和位置观测信息均受到非高斯噪声的污染时,则子滤波器1和子滤波器2均采用RKF算法;
当DVL输出的速度观测信息受到非高斯噪声污染,而AST输出的位置观测信息正常,则可设置子滤波器1为RKF算法,子滤波器2为标准的KF算法;
DVL输出的速度观测信息正常,AST输出的位置观测信息受到非高斯噪声污染,则设置子滤波器1为标准的KF算法,子滤波器2为RKF算法;
进行融合的方法进一步包括以下步骤:
(1)时间更新:
(2)量测更新:
2.2)若判定观测量zi,k正常,则令λi,k=1;若判定观测量zi,k异常,则根据下式计算λi,k;
2.6)根据Pk|k=(I-KkHk)Pk|k-1计算状态估计误差协方差矩阵Pi,k;
2.如权利要求1所述水下组合导航信息融合方法,其特征在于,步骤一中,所述利用DVL获取外部速度信息的方法包括:
(1)单波束配置测速,
发射声波频率为f0,其速度为c,AUV沿水平方向航行,速度记为vx;
根据多普勒效应,在P点处接收到的频率为:
在O'点处接受到的频率为:
DVL接收频率减发射频率即多普勒频移:
fd=f2-f0;
vx在水中的传递速度远远小于c,α≈α',式简化为:
f0,α,c已知数,求出DVL的频移计算出相应的速度:
(2)双波束Janus配置测速:沿艏部方向波束的频率为:
艉部方向的频率为:
fr1和fr2是DVL艏部和艉部方向的接受频率,计算出多普勒频移为:
得到双波束配置情况下的航行器速度为:
4.如权利要求1所述水下组合导航信息融合方法,其特征在于,步骤一中,所述AST导航技术获取外部位置信息的方法包括:
x=rcosθx,y=rcosθy,z=rcosθz;
式中,r=cs·ts/2表示USBL到测量目标的斜距;cs表示水中声速,ts为声信号在水中的双程传播时延;θx、θy由USBL两轴上的阵元进行方位估计得到;θx、θy和θz满足如下关系式
cos2θx+cos2θy+cos2θz=1;
当USBL的位置准确已知时,通过USBL与AST之间进行问答,并通过USBL对AST进行方位估计求解获取AST的位置;当AST的位置准确已知时,通过USBL与AST之间进行问答求解获取USBL的位置。
5.一种实施如权利要求1-4任一项所述水下组合导航信息融合方法的水下组合导航信息融合***,其特征在于,所述水下组合导航信息融合***包括:
SINS导航模块,用于利用SINS导航技术获取载体的姿态、速度、位置相关信息;
DVL导航模块,用于利用DVL导航技术获取外部速度信息;
AST导航模块,用于利用AST导航技术获取外部位置信息;
融合模块,用于利用滤波算法将获取到的载体的姿态、速度、位置相关信息、外部速度信息、外部位置信息进行融合,并输出融合后的导航信息。
6.一种计算机可读存储介质,储存有指令,当所述指令在计算机上运行时,使得计算机执行如权利要求1-4任一项所述水下组合导航信息融合方法。
7.一种实施权利要求1-4任一项所述水下组合导航信息融合方法的自主式水下航行器。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010691625.2A CN111947651B (zh) | 2020-07-17 | 2020-07-17 | 水下组合导航信息融合方法、***及自主式水下航行器 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010691625.2A CN111947651B (zh) | 2020-07-17 | 2020-07-17 | 水下组合导航信息融合方法、***及自主式水下航行器 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN111947651A CN111947651A (zh) | 2020-11-17 |
CN111947651B true CN111947651B (zh) | 2022-06-21 |
Family
ID=73341542
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202010691625.2A Active CN111947651B (zh) | 2020-07-17 | 2020-07-17 | 水下组合导航信息融合方法、***及自主式水下航行器 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN111947651B (zh) |
Families Citing this family (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN112880679B (zh) * | 2021-01-11 | 2022-08-16 | 西北工业大学 | 一种基于导航数据库信息的fls虚拟波束生成方法 |
CN112987054B (zh) * | 2021-02-24 | 2023-03-03 | 博雅工道(北京)机器人科技有限公司 | 标定sins/dvl组合导航***误差的方法和装置 |
CN113093092B (zh) * | 2021-04-01 | 2022-06-14 | 哈尔滨工程大学 | 一种水下鲁棒自适应单信标定位方法 |
CN113819906B (zh) * | 2021-09-23 | 2023-07-21 | 哈尔滨工程大学 | 一种基于统计相似度量的组合导航鲁棒滤波方法 |
CN114166203B (zh) * | 2021-11-16 | 2024-02-09 | 哈尔滨工程大学 | 一种基于改进的s-h自适应联邦滤波的智能水下机器人多源组合导航方法 |
CN115392117B (zh) * | 2022-08-15 | 2023-04-25 | 哈尔滨工程大学 | 一种水下高速机动平台高帧率无模糊声学导航方法 |
CN117826132A (zh) * | 2024-03-04 | 2024-04-05 | 河海大学 | 一种基于侧扫声呐图像的dvl速度野值检测方法 |
Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
KR20120096626A (ko) * | 2011-02-23 | 2012-08-31 | 대양전기공업 주식회사 | 수중운동체의 위치 추정 방법 및 수중운동체의 위치 추정 장치 |
CN110006433A (zh) * | 2019-04-22 | 2019-07-12 | 哈尔滨工程大学 | 海底油气管检测机器人的组合导航定位***及方法 |
CN110763253A (zh) * | 2019-10-09 | 2020-02-07 | 哈尔滨工程大学 | 一种基于svr的组合导航***故障诊断方法 |
Family Cites Families (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN108663051A (zh) * | 2018-04-28 | 2018-10-16 | 南京信息工程大学 | 一种水下无源组合导航***建模及信息融合方法 |
CN109765929B (zh) * | 2019-01-14 | 2022-04-05 | 哈尔滨工程大学 | 一种基于改进rnn的uuv实时避障规划方法 |
CN109737959A (zh) * | 2019-03-20 | 2019-05-10 | 哈尔滨工程大学 | 一种基于联邦滤波的极区多源信息融合导航方法 |
-
2020
- 2020-07-17 CN CN202010691625.2A patent/CN111947651B/zh active Active
Patent Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
KR20120096626A (ko) * | 2011-02-23 | 2012-08-31 | 대양전기공업 주식회사 | 수중운동체의 위치 추정 방법 및 수중운동체의 위치 추정 장치 |
CN110006433A (zh) * | 2019-04-22 | 2019-07-12 | 哈尔滨工程大学 | 海底油气管检测机器人的组合导航定位***及方法 |
CN110763253A (zh) * | 2019-10-09 | 2020-02-07 | 哈尔滨工程大学 | 一种基于svr的组合导航***故障诊断方法 |
Non-Patent Citations (5)
Title |
---|
A Method for Underwater Dynamic Gravimetry Combining Inertial Navigation System, Doppler Velocity Log, and Depth Gauge;Zhiming Xiong,et al.;《IEEE GEOSCIENCE AND REMOTE SENSING LETTERS》;20191025;第17卷(第8期);全文 * |
SINS/DVL/AST水下组合导航中的鲁棒信息融合方法;朱兵等;《国防科技大学学报》;20201031;第42卷(第5期);全文 * |
一种基于集中滤波的SINS/DVL/USBL水下组合导航算法;张亚文等;《导航定位与授时》;20170131;第4卷(第1期);第25-31页 * |
基于自适应滤波的SINS_DVL组合导航算法研究;范晓亮;《中国优秀博硕士学位论文全文数据库(硕士) 信息科技辑》;20190115(第01期);第24-27页 * |
张亚文等.一种基于集中滤波的SINS/DVL/USBL水下组合导航算法.《导航定位与授时》.2017,第4卷(第1期), * |
Also Published As
Publication number | Publication date |
---|---|
CN111947651A (zh) | 2020-11-17 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN111947651B (zh) | 水下组合导航信息融合方法、***及自主式水下航行器 | |
CN104316045B (zh) | 一种基于sins/lbl的auv水下交互辅助定位***及定位方法 | |
Zhang et al. | Autonomous underwater vehicle navigation: a review | |
Melo et al. | Survey on advances on terrain based navigation for autonomous underwater vehicles | |
Grenon et al. | Enhancement of the inertial navigation system for the Morpheus autonomous underwater vehicles | |
Eustice et al. | Experimental results in synchronous-clock one-way-travel-time acoustic navigation for autonomous underwater vehicles | |
CN111829512B (zh) | 一种基于多传感器数据融合的auv导航定位方法及*** | |
Miller et al. | Autonomous underwater vehicle navigation | |
CN103744098B (zh) | 基于sins/dvl/gps的auv组合导航*** | |
CN111221018B (zh) | 一种用于抑制海上多路径的gnss多源信息融合导航方法 | |
CN108444476B (zh) | 一种考虑水声通信延迟的多水下无人航行器极区协同导航方法 | |
CN111380518B (zh) | 一种引入径向速度的sins/usbl紧组合导航定位方法 | |
CN112556697A (zh) | 一种基于联邦结构的浅耦合数据融合导航方法 | |
CN110186461A (zh) | 一种基于重力梯度信息测距的协同导航方法 | |
Zhu et al. | A calibration method of USBL installation error based on attitude determination | |
CN112747748A (zh) | 一种基于逆向解算的领航auv导航数据后处理方法 | |
CN115096302A (zh) | 捷联惯性基导航***信息滤波鲁棒对准方法、***及终端 | |
CN104061930A (zh) | 基于捷联惯性制导和多普勒计程仪的导航方法 | |
Hartsfield | Single transponder range only navigation geometry (STRONG) applied to REMUS autonomous under water vehicles | |
CN112684453B (zh) | 一种基于无人潜航器双基地声纳***的定位误差修正方法 | |
Harris et al. | Cooperative acoustic navigation of underwater vehicles without a DVL utilizing a dynamic process model: Theory and field evaluation | |
CN116358544A (zh) | 基于声学特征匹配定位修正惯性导航误差的方法和*** | |
Zhou et al. | Hybrid tightly-coupled SINS/LBL for underwater navigation system | |
CN103697887B (zh) | 一种基于捷联惯导***和多普勒计程仪的优化导航方法 | |
CN115200571A (zh) | 一种基于多传感器组合的短期水下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 |