CN105783940B - 基于信息预评判及补偿修正的sins/dvl/es组合导航方法 - Google Patents

基于信息预评判及补偿修正的sins/dvl/es组合导航方法 Download PDF

Info

Publication number
CN105783940B
CN105783940B CN201610010208.0A CN201610010208A CN105783940B CN 105783940 B CN105783940 B CN 105783940B CN 201610010208 A CN201610010208 A CN 201610010208A CN 105783940 B CN105783940 B CN 105783940B
Authority
CN
China
Prior art keywords
dvl
sins
speed
navigation
kalman filter
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
Application number
CN201610010208.0A
Other languages
English (en)
Other versions
CN105783940A (zh
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.)
Southeast University
Original Assignee
Southeast University
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 Southeast University filed Critical Southeast University
Priority to CN201610010208.0A priority Critical patent/CN105783940B/zh
Publication of CN105783940A publication Critical patent/CN105783940A/zh
Application granted granted Critical
Publication of CN105783940B publication Critical patent/CN105783940B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices
    • 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
    • G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
    • G01S13/02—Systems using reflection of radio waves, e.g. primary radar systems; Analogous systems
    • G01S13/50—Systems of measurement based on relative movement of target
    • G01S13/58—Velocity or trajectory determination systems; Sense-of-movement determination systems
    • 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
    • G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
    • G01S13/86—Combinations of radar systems with non-radar systems, e.g. sonar, direction finder
    • 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
    • G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
    • G01S13/88—Radar or analogous systems specially adapted for specific applications
    • G01S13/881—Radar or analogous systems specially adapted for specific applications for robotics
    • 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
    • G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
    • G01S7/02—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S13/00
    • G01S7/40—Means for monitoring or calibrating

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Automation & Control Theory (AREA)
  • Robotics (AREA)
  • Electromagnetism (AREA)
  • Manufacturing & Machinery (AREA)
  • Navigation (AREA)

Abstract

本发明提供了一种基于信息预评判及导航结果补偿修正的SINS/DVL/ES组合导航方法。本发明在SINS/DVL组合前对DVL的数据有效性进行判断，当DVL数据信息无效时，DVL导航信息不参与组合导航；同时，本发明将残差x2检测法引入，对组合导航结果是否平稳进行评估，并设计了组合导航结果振荡时的补偿修正方法对Kalman输出结果进行实时评判和修正补偿。本发明既能有效抑制SINS/DVL组合导航姿态、速度的发散速度，提高组合系统姿态、速度精度，在一定程度上提高位置精度；又能解决在DVL数据信息质量下降所带来姿态误差的振荡问题。

Description

基于信息预评判及补偿修正的SINS/DVL/ES组合导航方法
技术领域
本发明涉及一种SINS/DVL/ES信息预评判及导航结果补偿修正方法，特别适用于舰船、水下自主航行器AUV等跟踪定位。
背景技术
AUV(Autonomous Underwater Vehicle)，自主式水下航行器)是一种可以自主地完成水下探测、攻击、运载、打捞等多种特定作业任务的水下航器，要求其在水下具有长期自主的定位导航和返航能力，并具有隐蔽性。为了保证其顺利解决水下作业，对AUV 提出了更高的导航精度要求。SINS(Strapdown Inertial Navigation Systems)，捷联式惯性导航系统)具有自主性、隐蔽性、抗干扰性，数据更新频率高，且在短时间内具有较高的精度的特点，特别适合水下导航。然而，惯性导航系统的最大的弱点在于定位误差随着时间累积增大，难以进行长航时的高精确定位，必须利用外部观测量对系统进行校正。 DVL(Dopplervelocity log)多普勒速度计程仪)是一种利用反射回波的多普勒频移能够测量水面或者是水下航行器相对于海底的绝对速度或者是某一水层的相对速度，具有一定的实时性和自主性。
由于电磁波在水中严重衰减的特性决定了AUV水下导航定位不能采用SINS/GNSS的组合导航。目前，应用于AUV水下导航定位的技术主要采用了基于SINS/DVL的组合导航系统。利用多普勒速度计程仪的速度误差不随时间积累的特性，与惯性导航系统构成组合导航系统，是提高惯性导航系统精度的有效途径。这种组合方式的AUV不需要接收卫星信号，所以也不需要采用“潜航-水面校正-潜航”的导航模式，提高了AUV 的水下隐蔽性，这对于水下或水面航行体不能接收卫星信号的情况下进行组合导航，具有十分重要的意义。
由于水下环境复杂，目前利用SINS/DVL组合导航系统的AUV由于DVL的探测范围有时无法到达海底，DVL受作用深度限制，超过数百米时，只能利用水层中的水团质点作反射层，变成对水的测速。所以特别是在深海作业区就不可避免地产生测速误差。如果这时候DVL的速度信息参与Kalman滤波，就会影响SINS的导航定位精度。因此对DVL的速度的有效性进行判断显得尤为重要。
发明内容
为了克服现有AUV水下导航技术的缺陷，本发明提供一种基于信息预评判及补偿修正的SINS/DVL/ES组合导航方法，所述基于信息预评判及导航结果补偿修正的 SINS/DVL/ES组合导航方法为基于SINS/DVL/ES组合导航系统的导航方法，该系统包括多普勒速度计程仪、测深仪、SINS、SINS/DVL/ES信息预评判模块、Kalman滤波处理模块、组合导航补偿修正评判模块和补偿修正处理模块；
所述的导航方法包括以下步骤：
1)SINS/DVL/ES信息预评判模块接收来自测深仪的深度信号DES，来自DVL的速度信号VDVL和来自捷联SINS的姿态信号当SINS/DVL/ES信息预评判模块接收到 DVL的速度信号VDVL数据时，进行以下两个判断：a.DES是否小于阈值D(th)；b.纵摇角增量Δθ是否小于纵摇角增量阀值Δθ(th)；
2)Kalman滤波模块接收来自SINS模块的速度Vins，姿态信号，如果根据步骤1)中a、b条件同时满足时则Kalman滤波模块接收DVL的速度信号VDVL，即DVL的速度信号VDVL参与Kalman滤波，若a、b条件不同时满足则断开DVL与Kalman滤波模块，即VDVL信息不参与Kalman滤波；
3)组合导航补偿修正模块判定是否对Kalman滤波处理模块的输出结果进行修正处理；并将其输出结果送至SINS；最后由SINS修正补偿输出经过修正补偿后的速度Vins′，位置Pins′，姿态等导航信息，具体为：
3.1)对Kalman滤波器的残差rk作如下二元假设：H0表示无故障，H1表示有故障；
无故障时：
其中Zk∈R6为观测量，Hk为观测矩阵，为预报值，φk/k-1为从k-1时刻到k时刻的一步状态转移矩阵，Ak为方差；
有故障时：
E{rk}＝μ，E{(rk-μ)(rk-μ)T}＝Ak；
故障检测函数为其中，γk是服从自由度为m的x2分布，即γk～χ2(m)，m为观测量Zk的维数；
3.2)在获取检测函数后，根据如下判定准则判断系统是否需要进行修正处理：若γk≤TD，判定不需要修正；其中TD由Pf＝P[γk>TD/H0]＝α代入预设的误检率Pf解算得到；
若γk>TD，判定需要修正；此时需要对SINS/DVL融合后的位置、速度分以下三种情况进行修正：
(a)若Pout(k)-Pout(k-1)≥△PD时，其中△PD为相邻滤波修正时刻位置变化率，Pout(k) 为k时刻的位置信息输出值，Pout(k-1)为k-1时刻的位置信息输出值，则Pout(k)＝Pout(k-1) +ΔP·Tn，其取值依据为△P小于载体实际机动的位置变化率,Tn为滤波周期；
同理当Vout(k)-Vout(k-1)≥ΔVD时，有Vout(k)＝Vout(k-1)+ΔV·Tn，其中ΔVD为相邻滤波周期的速度变化率。Tn为滤波周期；Pout(k)为k时刻的位置信息输出值，Pout(k-1)为k-1 时刻的位置信息输出值；
(b)若Pout(k)-Pout(k-1)≤-ΔPD，Vout(k)-Vout(k-1)≤-ΔVD时，
则Pout(k)＝Pout(k-1)-ΔP·Tn，Vout(k)＝Vout(k-1)-ΔV·Tn；
(c)若|Pout(k)-Pout(k-1)|＜ΔPD，|Vout(k)-Vout(k-1)|＜ΔVD时，则直接使用Kalman滤波的误差估计值进行补偿修正，公式为
其中为k时刻的位置修正值，Xp(k)为k时刻Kalman滤波位置估计值，为k时刻的速度修正值，XV(k)为k时刻Kalman滤波速度估计值， 即对SINS解算位置、速度的修正值等于Kalman滤波位置、速度误差估计值；
当γk<TD时，发生在DVL信号有效的情况时，
令Pout(k)、Vout(k)初始值为0；
此阶段不需要对Kalman滤波处理模块的输出结果进行修正，
其中Pins(k)、Vins(k)分别为SINS解算位置、速度值；即对SINS解算位置、速度的修正值等于Kalman滤波位置、速度误差估计值。
进一步地，所述步骤1)SINS/DVL/ES信息预评判模块接收来自测深仪的深度信号，来自DVL的速度信号VDVL和来自捷联SINS的姿态信号；当SINS/DVL/ES信息预评判模块接收到DVL的速度信号VDVL数据时，进行以下两个判断：a.DES是否小于阈值D(th)；b.纵摇角增量Δθ是否小于纵摇角增量阀值Δθ(th)，具体包括以下步骤：
1.1)SINS/DVL/ES信息预评判模块接收来自测深仪发送的深度信号DES进行判断，如果DES小于阈值D(th)，其中D(th)的值为所述SINS/DVL/ES组合导航系统的DVL产品的D(th)值，说明当前DVL所提供的速度数据VDVL能真实反应载体相对于海底的运动速度，DVL将VDVL送至Kalman滤波处理模块，即允许VDVL数据参与信息融合，运行1.2)，否则说明当前DVL的速度数据VDVL无效,即由于水深度值太大，VDVL测得的数据不能真实反应载体相对于海底的运动速度，而是相对于水流的速度，此时立即停止DVL的速度数据VDVL参与信息融合，程序重新运行1.1)进行下一次阈值比较，进入下一轮评判；
1.2)SINS/DVL/ES信息预评判模块根据捷联惯性导航系统SINS所提供的纵摇角增量Δθ进行判断，当纵摇角增量Δθ大于纵摇角增量阀值Δθ(th)时，所述Δθ(th)由具体的DVL和系统设计要求决定，AUV的瞬间纵摇上界为8度；说明AUV的瞬间纵摇变化将影响DVL的测速精度，如果AUV的纵摇角增量小于上界值时DVL将VDVL送至Kalman滤波处理模块，即允许VDVL数据参与信息融合，否则VDVL数据不参与Kalman滤波。
有益效果
与现有技术相比，本发明通过对DVL的数据在进行在SINS/DVL进行组合导航之前进行数据有效性判断，并对组合导航系统的输出结果引入了残差x2检测法对DVL信号进行实时评断和修正。本发明既能够有效抑制SINS/DVL组合导航的姿态和速度发散速度，提高组合系统速度、姿态精度，在一定程度上提高位置精度。又能解决了在DVL 数据信息质量下降造成的情况下带来姿态误差的振荡问题。
传统的SINS/DVL组合导航系统在DVL信号不可用的情况下姿态误差一直处于震荡状态，纵摇最大姿态误差为3.584°，横摇最大姿态误差为3.609°，航向最大误差为 9.81°；其速度误差存在较多毛刺，东向速度VE误差最大为5.148m/s，北向速度VN误差最大为5.58m/s，天向速度VU误差最大为9.215m/s。本发明在SINS/DVL组合导航前对 DVL的数据是否可用进行了判断，并对组合导航的结果进行修正，处理后的组合导航水平姿态误差在0.1°以内，系统运行8000s时航向姿态误差为-0.163°；速度误差在1m/s；如说明书附图4、5所示，本发明经过评判和补偿修正处理后的组合导航的姿态、速度误差中大部分较大尖锐毛刺可以消除，能够较大幅度提高组合导航姿态、速度精度，并能在一定程度上能够提高组合导航位置精度。
本发明在SINS/DVL松组合系统前对DVL的数据有效性进行判断，针对组合导航后的结果引入了残差x2检测法对DVL信号进行实时评判和修正补偿。实验表明，当AUV 工作深度距海底过高引起的DVL测速误差过大或其它干扰使DVL速度数据信息质量下降时或DVL信号重新恢复后Kalman滤波器的振荡的情况下都能够引起残差x2检测法的检测函数值发生变化。通过评判结果再结合SINS/DVL组合前后两次的输出导航结果与载体的实际机动性能，按照本发明提出的导航结果输出修正方法能够较大程度提高 SINS/DVL松组合系统的姿态、速度精度，在一定程度上提高SINS/DVL松组合系统的位置精度。本发明提出的一种基于SINS/DVL松组合的水下AUV组合导航系统及方法方案简单、易于实现，具有强较的工程实用性。
附图说明
图1为一种SINS/DVL/ES信息预评判及导航结果补偿修正方法的原理示意图；
图2为SINS/DVL组合评判模块原理示意图；
图3为补偿修正处理模块(速度参数)原理示意图；
图4为姿态误差曲线；
图5为速度误差曲线。
具体实施方式
下面对本发明技术方案进行详细说明，但是本发明的保护范围不局限于所述实施例。
实施例1：
如上所述，尽管参照特定的优选实施例已经表示和表述了本发明，但其不得解释为对本发明自身的限制。在不脱离所附权利要求定义的本发明的精神和范围前提下，可对其在形式上和细节上作出各种变化。
如说明书附图1所示，本发明由以下7个部分组成：
本发明由安装在水下AUV(Autonomous Underwater Vehicle，自主式水下航行器，简称AUV)上的SINS(Strapdown Inertial Navigation Systems，捷联式惯性导航系统，简称SINS)，DVL(Doppler velocity log，多普勒速度计程仪，简称DVL)， ES(Echo-Sounder，测深仪，简称ES)，SINS/DVL/ES信息预评判模块，Kalman滤波处理模块，组合导航补偿修正评判模块及补偿修正处理模块组成。
捷联惯性导航系统SINS模块包括惯性测量单元IMU元件及IMU处理单元，其中 IMU元件用于得到惯导，IMU处理单元对由IMU得到的数据进行捷联解算，得到包括速度Vins，位置Pins，姿态等导航信息。多普勒速度计程仪DVL模块主要利用发射的声波和接收的水底反射波之间的多普勒频移测量AUV相对于水底的航速VDVL，该 DVL输出AUV的航速VDVL。测深仪ES输出深度信息DES。SINS/DVL/ES信息预评判模块主要根据DVL所提供AUV的航速VDVL、测深仪ES输出深度信息DES和SINS 的速度Vins进行数据评判决策，根据融合评判结果判断DVL是否参与组合导航。Kalman滤波处理模块是使用Kalman滤波器进行DVL和SINS的信息融合处理。组合导航补偿修正判断模块是对用于判断DVL输出信号与SINS进行组合后的导航结果是否需要补偿修正。补偿修正处理模块是对组合导航的结果进行补偿修正处理。
一种SINS/DVL/ES信息预评判及导航结果补偿修正方法首先读取SINS的输出报文中SINS速度Vins、姿态信息和DVL的输出报文中速度VDVL、ES输出的深度DES信息。SINS/DVL/ES信息预评判模块是按说明书附图2所示的方法进行评判的：
第一步，该系统读取到由这四个参数后首先SINS/DVL/ES信息预评判模块对ES所提供的深度DES进行判断，如果DES小于阈值D(th)(D(th)由具体的DVL产品决定)说明当前DVL所提供的数据报文是可用的。否则，说明当前DVL的速度信息误差很大，应立刻停止信息融合；这主要是因为DVL受工作深度限制，超过数百米时，只能利用水层中的水团质点作反射层，变成对水的测速仪，而不是AUV的航速。
第二步，如果第一步DES小于阈值D(th)，则SINS/DVL/ES信息预评判模块根据捷联惯性导航系统SINS所提供的纵摇角增量Δθ进行判断，如果在第一步中DVL的速度信息可用的情况下，AUV的纵摇角增量Δθ大于Δθ(th)时(Δθ(th)由具体的DVL和系统设计要求决定，一般AUV的瞬间纵摇不大于8度，)，这是因为AUV瞬间的纵摇变化将极大地增大DVL的所对应的相对误差值，说明AUV的瞬间纵摇变化将影响DVL 的测速精度，此刻立即停止VDVL参与信息融合。
如果前面二步都评判通过，则将VDVL送入Kalman滤波处理模块，否则，不将VDVL送入Kalman滤波处理模块。
本发明中组合导航补偿修正判断模块是如下定义的：定义Kalman滤波处理模块中Kalman滤波器的残差为：
式中 为观测量 H为观测矩阵，为预报值，其中φk/k-1为从k-1时刻到k时刻的一步状态转移矩阵。
当DVL信号正常时，滤波器残差rk是零均值白噪声，其方差为：
其中，Rk为系统观测噪声方差阵。
Pk/k-1可通过
计算得到。
组合导航修正判断模块是按以下方法和步骤进行判断的：首先，对rk作如下二元假设：H0表示无故障，H1表示有故障。
无故障：E{rk}＝0，
有故障：E{rk}＝μ，E{(rk-μ)(rk-μ)T}＝Ak。
故障检测函数为其中，γk是服从自由度为m的χ2分布，即γk～χ2(m)。m为观测量Zk的维数。
在获取检测函数后，根据如下判定准则判断系统是否需要进行修正处理。
若γk>TD，判定需要修正处理，
若γk≤TD，判定不需要修正处理。
在上面的步骤中预先设置的门限TD，其决定了检测的性能。由奈曼-皮尔逊准则可知，当限定误检率Pf＝α时，则由Pf＝P[γk>TD/H0]＝α解出门限TD可使漏检率 Pf＝P[γk≤TD/H1]达到最小，因而TD可由误检率Pf决定。
本发明中补偿修正处理模块如说明书附图3补偿修正处理模块(位置参数)原理示意图所示按以下方法和步骤进行导航结果Pout(k)、Vout(k)补偿修正处理的：
首先将Pout(k)、Vout(k)置初始值0。
第二步，计算得到Kalman滤波器的残差
第三步，判断γk<TD是否成立？当DVL信号较好、滤波器进入稳定工作阶段，辅助信息估计模块会得出检测函数的值小于阈值，即γk<TD，此时认为DVL信号正常， DVL提供的参数信息可直接使用。此阶段
其中XP(k)＝[δL,δλ,δh]、XV(k)＝[δVE,δVN,δVU]为Kalman滤波位置、速度误差估计值。即对SINS解算位置、速度的修正值等于Kalman滤波位置、速度误差估计值。
第四步，若第三步中，信息评估模块检测出检测函数γk>TD时，这可能是滤波器处在初始阶段或DVL输出信息时出现了丢失、振荡等情况。此时需要对SINS/DVL融合后的位置、速度分以下三种情况进行补偿修正。
(a)当Pout(k)-Pout(k-1)≥ΔPD时，其中ΔPD为相邻滤波修正时刻位置变化率，则Pout(k)＝Pout(k-1)+ΔP·Tn，其取值依据为ΔP小于载体实际机动的位置变化率。Tn为滤波周期。
同理当Vout(k)-Vout(k-1)≥ΔVD时，有Vout(k)＝Vout(k-1)+ΔV·Tn，其中ΔVD为相邻滤波周期的位置变化率，一般取值较小。Tn为滤波周期。
(b)当Pout(k)-Pout(k-1)≤ΔPD时则Pout(k)＝Pout(k-1)-ΔP·Tn；
同理若Vout(k)-Vout(k-1)≤-ΔVD时则Vout(k)＝Vout(k-1)-ΔV·Tn。
(c)当|Pout(k)-Pout(k-1)|＜ΔPD，|Vout(k)-Vout(k-1)|＜ΔVD时,则直接使用Kalman滤波的误差估计值进行补偿修正，即
其中即对S I N S解算位置、速度的修正值等于Kalman滤波位置、速度误差估计值。

Claims (2)

1.一种基于信息预评判及补偿修正的SINS/DVL/ES组合导航方法，其特征在于，所述基于信息预评判及补偿修正的SINS/DVL/ES组合导航方法为基于SINS/DVL/ES组合导航系统的导航方法，该系统系统包括多普勒速度计程仪、测深仪、SINS、SINS/DVL/ES信息预评判模块、Kalman滤波处理模块、组合导航补偿修正评判模块和补偿修正处理模块；
所述的导航方法包括以下步骤：
1)SINS/DVL/ES信息预评判模块接收来自测深仪的深度信号DES，来自DVL的速度信号VDVL和来自捷联SINS的姿态信号当SINS/DVL/ES信息预评判模块接收到DVL的速度信号VDVL数据时，进行以下两个判断：a.DES是否小于阈值D(th)；b.纵摇角增量Δθ是否小于纵摇角增量阀值Δθ(th)；
2)Kalman滤波模块接收来自SINS模块的速度Vins，姿态信号，如果根据步骤1)中a、b条件同时满足时则Kalman滤波模块接收DVL的速度信号VDVL，若a、b条件不同时满足则断开DVL与Kalman滤波模块；
3)组合导航补偿修正模块判定是否对Kalman滤波处理模块的输出结果进行修正处理；并将其输出结果送至SINS；最后由SINS修正补偿输出经过修正补偿后的速度Vins′，位置Pins′，姿态导航信息，具体为：
3.1)对Kalman滤波器的残差rk作如下二元假设：H0表示无故障，H1表示有故障；
无故障时：
E{rk}＝0，
其中Zk∈R6为观测量，Hk为观测矩阵，为预报值，φk/k-1为从k-1时刻到k时刻的一步状态转移矩阵，Ak为方差；
有故障时：
E{rk}＝μ，E{(rk-μ)(rk-μ)T}＝Ak；
故障检测函数为其中，γk是服从自由度为m的x2分布，即γk～χ2(m)，m为观测量Zk的维数；
3.2)在获取检测函数后，根据如下判定准则判断系统是否需要进行修正处理：若γk≤TD，判定不需要修正；其中TD由Pf＝P[γk>TD/H0]＝α代入预设的误检率Pf解算得到；
若γk>TD，判定需要修正；此时需要对SINS/DVL融合后的位置、速度分以下三种情况进行修正：
(a)若Pout(k)-Pout(k-1)≥△PD时，其中△PD为相邻滤波修正时刻位置变化率，Pout(k)为k时刻的位置信息输出值，Pout(k-1)为k-1时刻的位置信息输出值，则Pout(k)＝Pout(k-1)+ΔP·Tn，其取值依据为△P小于载体实际机动的位置变化率,Tn为滤波周期；
同理当Vout(k)-Vout(k-1)≥ΔVD时，有Vout(k)＝Vout(k-1)+ΔV·Tn，其中ΔVD为相邻滤波周期的速度变化率，Tn为滤波周期；Pout(k)为k时刻的位置信息输出值，Pout(k-1)为k-1时刻的位置信息输出值；
(b)若Pout(k)-Pout(k-1)≤-ΔPD，Vout(k)-Vout(k-1)≤-ΔVD时，
则Pout(k)＝Pout(k-1)-ΔP·Tn，Vout(k)＝Vout(k-1)-ΔV·Tn；
(c)若|Pout(k)-Pout(k-1)|＜ΔPD，|Vout(k)-Vout(k-1)|＜ΔVD时，则直接使用Kalman滤波的误差估计值进行补偿修正，公式为
其中为k时刻的位置修正值，Xp(k)为k时刻Kalman滤波位置估计值，为k时刻的速度修正值，XV(k)为k时刻Kalman滤波速度估计值，
当γk<TD时，发生在DVL信号有效的情况时，
令Pout(k)、Vout(k)初始值为0；
此阶段不需要对Kalman滤波处理模块的输出结果进行修正，
其中Pins(k)、Vins(k)分别为SINS解算位置、速度值；
2.根据权利要求1所述的SINS/DVL/ES组合导航方法，其特征在于，所述步骤1)SINS/DVL/ES信息预评判模块接收来自测深仪的深度信号，来自DVL的速度信号VDVL和来自捷联SINS的姿态信号；当SINS/DVL/ES信息预评判模块接收到DVL的速度信号VDVL数据时，进行以下两个判断：a.DES是否小于阈值D(th)；b.纵摇角增量Δθ是否小于纵摇角增量阀值Δθ(th)，具体包括以下步骤：
1.1)SINS/DVL/ES信息预评判模块接收来自测深仪发送的深度信号DES进行判断，如果DES小于阈值D(th)，其中D(th)的值为所述SINS/DVL/ES组合导航系统的DVL产品的D(th)值，说明当前DVL所提供的速度数据VDVL能真实反应载体相对于海底的运动速度，DVL将VDVL送至Kalman滤波处理模块，运行1.2)，否则说明当前DVL的速度数据VDVL无效，VDVL测得的数据不能真实反应载体相对于海底的运动速度，而是相对于水流的速度，此时立即停止DVL的速度数据VDVL参与信息融合，程序重新运行1.1)进行下一次阈值比较，进入下一轮评判；
1.2)SINS/DVL/ES信息预评判模块根据捷联惯性导航系统SINS所提供的纵摇角增量Δθ进行判断，当纵摇角增量Δθ大于纵摇角增量阀值Δθ(th)时，所述Δθ(th)由具体的DVL和系统设计要求决定，AUV的瞬间纵摇上界为8度；说明AUV的瞬间纵摇变化将影响DVL的测速精度，如果AUV的纵摇角增量小于上界值时DVL将VDVL送至Kalman滤波处理模块，否则VDVL数据不参与Kalman滤波。
CN201610010208.0A 2016-01-07 2016-01-07 基于信息预评判及补偿修正的sins/dvl/es组合导航方法 Active CN105783940B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201610010208.0A CN105783940B (zh) 2016-01-07 2016-01-07 基于信息预评判及补偿修正的sins/dvl/es组合导航方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201610010208.0A CN105783940B (zh) 2016-01-07 2016-01-07 基于信息预评判及补偿修正的sins/dvl/es组合导航方法

Publications (2)

Publication Number Publication Date
CN105783940A CN105783940A (zh) 2016-07-20
CN105783940B true CN105783940B (zh) 2018-06-19

Family

ID=56390097

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201610010208.0A Active CN105783940B (zh) 2016-01-07 2016-01-07 基于信息预评判及补偿修正的sins/dvl/es组合导航方法

Country Status (1)

Country Link
CN (1) CN105783940B (zh)

Families Citing this family (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106840150B (zh) * 2017-01-25 2019-10-15 东南大学 一种针对组合导航中dvl失效的混合处理方法
CN107797125B (zh) * 2017-10-23 2018-10-26 西北工业大学 一种减小深海探测型auv导航定位误差的方法
CN110031882B (zh) * 2018-08-02 2023-05-30 哈尔滨工程大学 一种基于sins/dvl组合导航系统的外量测信息补偿方法
CN109990789A (zh) * 2019-03-27 2019-07-09 广东工业大学 一种飞行导航方法、装置及相关设备
CN110196068B (zh) * 2019-05-27 2022-11-18 哈尔滨工程大学 一种极区集中滤波组合导航系统残差向量故障检测与隔离方法
CN111649762B (zh) * 2019-12-28 2022-05-10 中国船舶重工集团公司第七一七研究所 一种惯性多普勒全参量高精度标定方法及装置
CN112729291B (zh) * 2020-12-29 2022-03-04 东南大学 一种深潜长航潜水器sins/dvl洋流速度估计方法
CN112648993B (zh) * 2021-01-06 2022-07-15 北京自动化控制设备研究所 一种多源信息融合组合导航系统及方法
CN114370870B (zh) * 2022-01-05 2024-04-12 中国兵器工业计算机应用技术研究所 适用于位姿测量卡尔曼滤波的滤波器更新信息筛选方法

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102004447A (zh) * 2010-11-11 2011-04-06 西北工业大学 水下航行器组合导航与控制半实物仿真试验系统
CN102809375A (zh) * 2012-08-07 2012-12-05 河海大学 水下导航与水质参数经纬度分布的感测与计算系统及方法
CN103090861A (zh) * 2013-01-14 2013-05-08 哈尔滨工程大学 水下机器人的多线地形匹配导航方法

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102004447A (zh) * 2010-11-11 2011-04-06 西北工业大学 水下航行器组合导航与控制半实物仿真试验系统
CN102809375A (zh) * 2012-08-07 2012-12-05 河海大学 水下导航与水质参数经纬度分布的感测与计算系统及方法
CN103090861A (zh) * 2013-01-14 2013-05-08 哈尔滨工程大学 水下机器人的多线地形匹配导航方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
A Variable Proportion Adaptive Federal Kalman Filter for INS/ESGM/GPS/DVL Integrated Navigation System;Gannan Yuan等;《2011 Four International Joint Conference on Computational Sciences and Optimization》;20111231;978-981 *
Position error correction for an autonomous underwater vehicle inertial navigation system (INS) using a particle filter;Glenn T,Donovan;《IEEE JOURNAL OF OCENIC ENGINEERING》;20120731;第37卷(第3期);431-445 *
基于SINS/DVL与LBL交互辅助的AUV水下定位系统;张涛等;《中国惯性技术学报》;20151231;第23卷(第6期);669-774 *

Also Published As

Publication number Publication date
CN105783940A (zh) 2016-07-20

Similar Documents

Publication Publication Date Title
CN105783940B (zh) 基于信息预评判及补偿修正的sins/dvl/es组合导航方法
US9097534B2 (en) Dynamic positioning architecture
CN100534859C (zh) 用于测试海上船舶的控制系统的方法和系统
Fallon et al. Cooperative AUV navigation using a single surface craft
CN104316045A (zh) 一种基于sins/lbl的auv水下交互辅助定位系统及定位方法
US20090287414A1 (en) System and process for the precise positioning of subsea units
Vasilijevic et al. Underwater vehicle localization with complementary filter: Performance analysis in the shallow water environment
Hegrenaes et al. Doppler water-track aided inertial navigation for autonomous underwater vehicle
CN208012618U (zh) 一种海管调查系统
CN108759842A (zh) 基于卡尔曼滤波的旋转式惯性导航系统外水平阻尼方法
McPhail et al. Range-only positioning of a deep-diving autonomous underwater vehicle from a surface ship
CN115560759A (zh) 一种基于海底油气管道检测的水下多源导航定位方法
CN104614554A (zh) 船基风速风向传感器基准误差自修正方法
CN111982105A (zh) 一种基于sins/lbl紧组合的水下导航定位方法及系统
CN110333369A (zh) 基于水面gps校正的uuv的dvl测速系统及自适应去噪方法
JP2023034807A (ja) 水中航走体の音響測位処理方法、音響測位処理プログラム、及び音響測位処理システム
CN106840150B (zh) 一种针对组合导航中dvl失效的混合处理方法
Zhao et al. The experimental study on GPS/INS/DVL integration for AUV
Rogne et al. Fault detection in lever-arm-compensated position reference systems based on nonlinear attitude observers and inertial measurements in dynamic positioning
CN111846144B (zh) 一种虚实融合的无人水面艇避碰测试方法
KR20220127401A (ko) 3d 세굴지도 제작 방법
CN107561544A (zh) 一种水下机器人的检测方法
CN118089793B (zh) 一种深潜器定位精度评估方法、装置、介质及设备
CN113280812B (zh) 一种基于sins/lbl紧组合的高程约束水下自适应导航方法及装置
Bryne et al. Inertial sensors for risk-based redundancy in dynamic positioning

Legal Events

Date Code Title Description
C06 Publication
PB01 Publication
C10 Entry into substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant