WO2020062807A1 - 改进的无迹卡尔曼滤波算法在水下组合导航中的应用方法 - Google Patents
改进的无迹卡尔曼滤波算法在水下组合导航中的应用方法 Download PDFInfo
- Publication number
- WO2020062807A1 WO2020062807A1 PCT/CN2019/079136 CN2019079136W WO2020062807A1 WO 2020062807 A1 WO2020062807 A1 WO 2020062807A1 CN 2019079136 W CN2019079136 W CN 2019079136W WO 2020062807 A1 WO2020062807 A1 WO 2020062807A1
- Authority
- WO
- WIPO (PCT)
- Prior art keywords
- error
- equation
- measurement
- sins
- state
- Prior art date
Links
- 238000000034 method Methods 0.000 title claims abstract description 32
- 238000005259 measurement Methods 0.000 claims abstract description 87
- 238000012937 correction Methods 0.000 claims abstract description 30
- 238000001914 filtration Methods 0.000 claims abstract description 24
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 claims abstract description 10
- 239000011159 matrix material Substances 0.000 claims description 49
- 239000013598 vector Substances 0.000 claims description 46
- 238000005070 sampling Methods 0.000 claims description 15
- 230000003190 augmentative effect Effects 0.000 claims description 7
- 230000008569 process Effects 0.000 claims description 7
- 238000006243 chemical reaction Methods 0.000 claims description 6
- 230000005284 excitation Effects 0.000 claims description 6
- 230000005484 gravity Effects 0.000 claims description 6
- 230000009466 transformation Effects 0.000 claims description 6
- 230000001133 acceleration Effects 0.000 claims description 3
- 230000008859 change Effects 0.000 claims description 3
- 238000004364 calculation method Methods 0.000 abstract description 5
- 239000000654 additive Substances 0.000 description 5
- 230000000996 additive effect Effects 0.000 description 5
- 238000009825 accumulation Methods 0.000 description 3
- 238000010586 diagram Methods 0.000 description 3
- 238000012886 linear function Methods 0.000 description 3
- 230000003416 augmentation Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 2
- 238000004891 communication Methods 0.000 description 2
- 230000001186 cumulative effect Effects 0.000 description 2
- 238000011161 development Methods 0.000 description 2
- 230000018109 developmental process Effects 0.000 description 2
- 230000006378 damage Effects 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000007774 longterm Effects 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 238000011084 recovery Methods 0.000 description 1
- 238000000827 velocimetry Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
- G01C21/203—Specially adapted for sailing ships
-
- G—PHYSICS
- 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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
-
- 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/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/18—Stabilised platforms, e.g. by gyroscope
Definitions
- the invention belongs to the field of inertial navigation and relates to an application method of an improved unscented Kalman filter algorithm in underwater integrated navigation SINS / DVL / GPS.
- Autonomous Underwater Vehicles have a wide range of applications in civilian and military applications.
- Military aspects such as theater reconnaissance, detection and removal of mines, submarine countermeasures, maritime early warning blockades or ports, attacks on enemy ships or submarines, destruction of oil facilities and communication networks, underwater relay communications, etc .; civilian aspects such as marine resource survey and development, Marine rescue and salvage.
- navigation remains one of the main technical challenges facing AUVs.
- the navigation system must provide accurate attitude, heading, speed, and position information during long voyages, and accurate navigation capabilities are a key technology for the effective application and safe recovery of AUVs.
- due to its size, weight, power supply restrictions, and the particularity and concealment of the water medium it is a difficult task to achieve accurate navigation of the AUV.
- Inertial navigation is an autonomous navigation that does not require contact with the outside world and has good concealment. It is an ideal navigation method for AUVs.
- the inertial navigation system has the problem of error accumulation.
- the inertial navigation system can maintain good navigation accuracy in a short time, but the error accumulation effect is very obvious with the extension of time, and the navigation accuracy cannot be guaranteed. Therefore, other auxiliary navigation systems are usually used to correct the cumulative error of inertial navigation.
- Doppler velocimetry Roland C and other methods. Due to its high navigation accuracy and no error accumulation, GPS is the preferred method for high-precision navigation and positioning.
- the present invention discloses an application method of an improved unscented Kalman filtering algorithm in underwater integrated navigation, which can improve the filtering settlement efficiency and navigation accuracy of an underwater integrated navigation system, and ensure real-time performance and stability.
- the present invention provides the following technical solutions:
- the application method of the improved unscented Kalman filter algorithm in underwater integrated navigation includes the following steps:
- the coordinate system established in the step (1) specifically includes:
- inertial coordinate system does not rotate with the earth, the origin is located at the center of the earth, the z i axis points to the north pole, the x i axis points to the vernal equinox, and the y i axis and x i and z i form a right-handed coordinate system;
- Earth coordinate system fixedly connected to the earth, the origin is located at the center of the earth, the x e axis crosses the intersection of the prime meridian and the equator, the z e axis points to the north pole, and the y e axes x e , z e form a right-handed coordinate system;
- carrier coordinate system the origin is located at the center of the carrier, the z b axis is perpendicular to the carrier upwards, x b points to the front of the carrier, and y b and x b and z b form a right-handed coordinate system;
- n the navigation coordinate system that coincides with the east-north-sky geographic coordinate system.
- step (2) establishing a state equation of the SINS / DVL subsystem in the underwater submarine phase specifically includes the following steps:
- attitude error angle ( ⁇ E ⁇ N ⁇ U )
- gyro constant value drift ( ⁇ bx ⁇ by ⁇ bz )
- random constant error of the accelerometer As the state quantity of the SINS system, it is recorded as:
- the speed error equation represented by quaternion is derived from the speed differential equation of the ideal inertial specific gravity equation and the actual speed differential equation of the strapdown inertial navigation system:
- V n is the ideal speed of the carrier in the n series
- f b is the specific force in the b system
- Is the quaternion of the conversion from p to n Is the quaternion of the conversion from b to p, with Represent the transformation matrices of p-series to n-series and b-series to p-series
- ⁇ represents the actual measured value of the carrier
- ⁇ represents the error between the ideal value of the carrier and the actual measured value
- g n is the gravity acceleration under the n system. Is the accelerometer error vector under b system;
- R M and R N respectively represent the radius of curvature of the meridian circle and the radius of curvature of the circle of circle, L is the latitude of the carrier, ⁇ is the longitude of the carrier, and h is the height of the carrier;
- F N [ ⁇ ] is a non-linear continuous function
- ⁇ d represents the reciprocal of the correlation time of the speed offset error
- ⁇ d represents the excitation white noise
- the state vector of the integrated navigation system is The noise vector of the system is The equation of state of the system is expressed as:
- the state function F 1 [ ⁇ ] is a nonlinear continuous function
- ⁇ 1 (t) is the noise matrix of the subsystem.
- step (3) of establishing the measurement equation of the SINS / DVL subsystem in the underwater submarine phase specifically includes the following steps:
- the posture matrix used for the change is provided by SINS v EI , v NI , v UI respectively represent the three-axis velocity calculated by SINS, v ED , v ND , v UD respectively represent the three-axis velocity measured by DVL, v E , v N , v U represent the carrier in the navigation coordinate system
- the real speed under vx , v y , v z represents the real speed in the carrier coordinate system, ⁇ v DE , ⁇ v DN , ⁇ v DU is the error after the DVL speed measurement error is converted to the navigation coordinate system;
- H 1 [ ⁇ ] is a non-linear continuous function
- V 1 (t) is the measurement noise
- step (4) establishing a state equation of the SINS / GPS subsystem at the surface position correction stage specifically includes the following steps:
- the noise error of the system is:
- F 2 [ ⁇ ] is a nonlinear continuous function
- ⁇ 2 (t) is the noise matrix of the subsystem.
- step (5) of establishing the measurement equation of the SINS / GPS subsystem in the surface position correction stage specifically includes the following steps:
- L, ⁇ , V E and V N are the positions and velocities calculated by the SINS solution
- L G , ⁇ G , V GE and V GN are the positions, velocities of the GPS output
- ⁇ ⁇ represents the corresponding error
- H 2 [ ⁇ ] is a non-linear continuous function
- V 2 is a measurement noise
- the non-linear filtering equation of the underwater submarine stage established by integrating the steps (2) and (3) is:
- step (6) specifically includes the following steps:
- X k and Z k respectively, for the system state vector and the measurement vector, W k and V k t k are the time of noise and measurement noise matrix array subsystem stage submerged underwater, are zero mean and,
- the statistical characteristics are as follows:
- Q k and R k are the subsystem noise covariance matrix and the measurement noise covariance matrix respectively; the specific algorithm process is as follows:
- ⁇ i, k / k-1 f 1 ( ⁇ i, k-1 )
- ⁇ i, k / k-1 is the i-th sample point predicted at t k ;
- Z i, k / k-1 is the i-th measurement value
- x k and z k are respectively the state vector and the measurement vector of the system at time t k
- w k and v k are the noise matrix and the measurement noise matrix of the subsystem at the surface position correction stage, respectively, and the mean is zero.
- the statistical characteristics are as follows: q k and r k are the noise covariance matrix and the measured noise covariance matrix of the subsystem;
- ⁇ i, k / k-1 f 2 ( ⁇ i, k-1 )
- ⁇ i, k / k-1 is the i-th sample point predicted at t k ;
- Z i, k / k-1 is the i-th measurement value
- the position information of the filtering result every time the AUV rises to the surface is used as the new position information for the next submarine voyage, and the position is periodically corrected.
- the present invention has the following advantages and beneficial effects:
- the application method of the improved unscented Kalman filtering method proposed in the present invention in integrated navigation simplifies the UKF navigation algorithm based on the spherically distributed simplex sampling of complex additive noise, greatly reducing the dimension of the system state vector and the filtering algorithm.
- the complexity of the calculation has good real-time, stability and accuracy.
- FIG. 1 is a schematic diagram of the integrated system navigation for implementing the method of the present invention.
- Fig. 2 is a working flowchart of the integrated navigation system.
- Figure 3 is a schematic diagram of the UT transformation used in the improved unscented Kalman filtering method.
- Figure 4 is a block diagram of the solution flow of the improved unscented Kalman filtering method.
- Step 1 Define the coordinate system to be used
- inertial coordinate system does not rotate with the earth, the origin is located at the center of the earth, the z i axis points to the north pole, the x i axis points to the vernal equinox, and the y i axis and x i and z i form a right-handed coordinate system;
- Earth coordinate system fixedly connected to the earth, the origin is located at the center of the earth, the x e axis crosses the intersection of the prime meridian and the equator, the z e axis points to the north pole, and the y e axes x e , z e form a right-handed coordinate system;
- carrier coordinate system the origin is located at the center of the carrier, the z b axis is perpendicular to the carrier upwards, x b points to the front of the carrier, and y b and x b and z b form a right-handed coordinate system;
- n the navigation coordinate system that coincides with the east-north-sky geographic coordinate system.
- Step 2 Select state variables and measurements to establish a non-linear system model in the underwater submarine phase; take attitude error angle ( ⁇ E ⁇ N ⁇ U ), speed error ( ⁇ v E ⁇ v N ⁇ v U ), position error ( ⁇ L ⁇ ⁇ h) , Gyro constant drift ( ⁇ bx ⁇ by ⁇ bz ) and random constant error of accelerometer
- attitude error angle ⁇ E ⁇ N ⁇ U
- speed error ⁇ v E ⁇ v N ⁇ v U
- position error ⁇ L ⁇ ⁇ h
- Gyro constant drift ⁇ bx ⁇ by ⁇ bz
- random constant error of accelerometer As the state quantity of the SINS system, it is recorded as:
- the speed error equation represented by quaternion is derived from the speed differential equation of the ideal inertial specific gravity equation and the actual speed differential equation of the strapdown inertial navigation system:
- V n is the ideal speed of the carrier in the n series
- f b is the specific force in the b system
- Is the quaternion of the conversion from p to n Is the quaternion of the conversion from b to p, with Represent the transformation matrices of p-series to n-series and b-series to p-series, respectively.
- ⁇ represents the actual measured value of the carrier
- ⁇ represents the error between the ideal value of the carrier and the actual measured value
- g n is the gravity acceleration under the n system. Is the accelerometer error vector for the b system.
- R M and R N respectively indicate the radius of curvature of the meridian circle and the radius of curvature of the cymbal circle, and h represents the height at which the carrier is located.
- F N [ ⁇ ] is a non-linear continuous function.
- ⁇ d represents the reciprocal of the correlation time of the speed offset error
- ⁇ d represents the excitation white noise
- the state vector of the integrated navigation system is The noise vector of the system is The equation of state of the system can be expressed as:
- the state function F 1 [ ⁇ ] is a nonlinear continuous function
- ⁇ 1 (t) is the noise matrix of the subsystem.
- DVL measures the component of ground speed in the coordinate system of the carrier, in order to measure the speed of its output and the speed formed by SINS, the output speed of DVL must be transformed into the navigation coordinate system. among them,
- H 1 [ ⁇ ] is a non-linear continuous function
- V 1 (t) is the measurement noise
- Step 3 Select the state quantity and quantity measurement to establish a non-linear system model at the surface position correction stage, and take the state variables:
- the noise error of the system is:
- F 2 [ ⁇ ] is a nonlinear continuous function
- ⁇ 2 (t) is the noise matrix of the subsystem.
- H 2 [ ⁇ ] is a non-linear continuous function
- V 2 is a measurement noise
- Step 4 Discretize the equations established above and perform the unscented Kalman filter solution improved according to the algorithm shown in FIG. 4 to implement time update, measurement update, and filter update.
- the improved Kalman filter algorithm is introduced as follows:
- SSUT Spherical distribution simplex sampling transformation
- Unscented Kalman (UKF) inference algorithm it is generally necessary to perform state augmentation on system noise and measurement noise, but when the system noise and measurement noise are additive noise, it is not necessary to do augmentation processing, which is beneficial to further Reduce filtering calculations.
- the present invention studies a simplified SSUT sampling UKF algorithm based on complex additive noise.
- the nonlinear discrete-time system model of complex additive noise can be expressed as:
- the specific algorithm flow is as follows:
- ⁇ i, k / k-1 f ( ⁇ i, k-1 )
- the Kalman filter equation established in the underwater submarine phase is discretized.
- X k and Z k respectively, for the system state vector and the measurement vector, W k and V k t k are the time of noise and measurement noise matrix array subsystem stage submerged underwater, are zero mean and,
- the statistical characteristics are as follows:
- Q k and R k are the subsystem noise covariance matrix and the measurement noise covariance matrix, respectively.
- ⁇ i, k / k-1 f 1 ( ⁇ i, k-1 )
- x k and z k are respectively the state vector and the measurement vector of the system at time t k
- w k and v k are the noise matrix and the measurement noise matrix of the subsystem at the surface position correction stage, respectively, and the mean is zero.
- the statistical characteristics are as follows: q k and r k are the noise covariance matrix and the measured noise covariance matrix of the subsystem, respectively.
- the specific algorithm flow is as follows:
- ⁇ i, k / k-1 f 2 ( ⁇ i, k-1 )
- the position information of the filtering result when the AUV rises to the surface each time is used as the new position information of the next submarine, so as to realize the timing correction of the position and overcome the cumulative error of the inertial navigation.
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Navigation (AREA)
- Position Fixing By Use Of Radio Waves (AREA)
Abstract
Description
Claims (8)
- 改进的无迹卡尔曼滤波算法在水下组合导航中的应用方法,其特征在于,包括如下步骤:(1)对需要用到的坐标系进行定义:(2)建立水下潜航阶段SINS/DVL子***的状态方程:以SINS的3轴姿态误差,3轴速度误差,位置误差,3轴陀螺仪随机漂移、3轴加速度计零偏和DVL速度偏移误差、刻度系数误差建立21维状态向量,根据***误差方程建立该子***的状态方程;(3)建立水下潜航阶段SINS/DVL子***的量测方程:根据SINS解算的3维速度量和DVL测量的速度量之差作为量测量,并结合步骤(2)选取的误差状态向量建立该子***的量测方程;(4)建立水面位置修正阶段SINS/GPS子***的状态方程:不考虑天向运动速度及误差,以SINS的3轴姿态误差、速度误差、位置误差、3轴陀螺仪随机漂移、加速度计零偏建立12维状态向量,根据误差方程建立该子***的状态方程;(5)建立水面位置修正阶段SINS/GPS子***的量测方程:以SINS解算出来的位置、速度与GPS输出的位置、速度之差作为量测量,并结合步骤(4)选取的误差状态向量建立该子***的量测方程;(6)综合步骤(2)和步骤(3)建立水下潜航阶段的非线性滤波方程,在水下潜航一段时间以后,AUV浮出水面,结合步骤(4)和步骤(5)建立水面位置修正阶段非线性滤波方程,进行改进的无迹卡尔曼滤波解算,完成时间更新、量测更新和滤波更新,完成定时位置信息校正。
- 根据权利要求1所述的改进的无迹卡尔曼滤波算法在水下组合导航中的应用方法,其特征在于,所述步骤(1)建立的坐标系具体包括:i——惯性坐标系:不随地球旋转,原点位于地球中心,z i轴指向北极,x i轴指向春分点,y i轴与x i、z i构成右手坐标系;e——地球坐标系:与地球固联,原点位于地心,x e轴穿越本初子午线与赤道交点,z e轴指向北极,y e轴x e、z e构成右手坐标系;b——载体坐标系:原点位于运载体中心,z b轴垂直运载体向上,x b指向运载体前方,y b与x b、z b构成右手坐标系;p——实际计算得出的平台坐标系;n——与东-北-天地理坐标系重合的导航坐标系。
- 根据权利要求1所述的改进的无迹卡尔曼滤波算法在水下组合导航中的应用方法,其特征在于,所述步骤(2)中建立水下潜航阶段SINS/DVL子***的状态方程具体包括如下步骤:取姿态误差角(φ E φ N φ U)、速度误差(δv E δv N δv U)位置误差(δL δλ δh)、陀螺常值漂移(ε bx ε by ε bz)和加速度计随机常值误差 作为SINS***的状态量,记为:通过理想惯导比力方程的速度微分方程和捷联惯导***的实际速度微分方程,推导出四元数表示的速度误差方程:其中,Vn是载体在n系下的理想速度, 是n系下的地球自转角速度, 为n系相对于e系的角速度在n系下的投影,f b为b系下的比力;式中, 为p系到n系的转换四元数, 为b系到p系的转换四元数, 和 分别代表p系到n系以及b系到p系的转换矩阵;四元数姿态误差方程:位置误差方程:其中,R M和R N分别表示地球的子午圈曲率半径和卯酉圈曲率半径,L表示载体的纬度,λ表示载体的经度,h表示载体的高度;SINS***的噪声:W N(t)=[ω gx ω gy ω gz ω ax ω ay ω az] T则SINS的***误差方程可以表示为:取DVL速度偏移误差(δV dx δV dy δV dz)和刻度系数误差(Δk dx Δk dy Δk dz)作为DVL***状态变量,记为:X D(t)=[δV dx δV dy δV dz δk dx δk dy δk dz] TDVL的误差模型为:其中,β d表示速度偏移误差的相关时间倒数,ω d表示激励白噪声;相应的误差状态方程为:式中,状态函数F 1[·]为非线性连续函数,Γ 1(t)为该子***噪声阵。
- 根据权利要求1所述的改进的无迹卡尔曼滤波算法在水下组合导航中的应用方法,其特征在于,所述步骤(3)中建立水下潜航阶段SINS/DVL子***的量测方程具体包括如下步骤:由SINS和DVL形成的量测量:其中,由SINS提供变化所用的姿态矩阵 v EI,v NI,v UI分别表示SINS解算出的三轴速度,v ED,v ND,v UD分别表示DVL测出的三轴速度,v E,v N,v U表示载体在导航坐标系下的真实速度,v x,v y,v z表示在载体坐标系下的真实速度,δv DE,δv DN,δv DU为DVL测速误差换算到导航坐标系后的误差;Z 1=H 1[X 1(t),t]+V 1(t)式中:H 1[·]为非线性连续函数;V 1(t)为量测噪声。
- 根据权利要求1所述的改进的无迹卡尔曼滤波算法在水下组合导航中的应用方法,其特征在于,所述步骤(5)中建立水面位置修正阶段SINS/GPS子***的量测方程具体包括如下步骤:将SINS解算出来的位置、速度与GPS输出的位置位置、速度之差作为AUV水面位置修正阶段滤波解算的量测方程:式中L、λ、V E和V N分别为SINS解算出来的位置、速度,L G、λ G、V GE和V GN分别为GPS输出的位置位置、速度,δ·表示相应的误差;将上式展开,并结合之前选取的***误差状态X 2(t),得到量测方程为:Z 2=H 2[X 2(t),t]+V 2(t)式中:H 2[·]为非线性连续函数,V 2为量测噪声。
- 根据权利要求7所述的改进的无迹卡尔曼滤波算法在水下组合导航中的应用方法,其特征在于,所述步骤(6)中进行改进的无迹卡尔曼滤波解算的过程具体包括如下步骤:对水下潜航阶段的非线性滤波方程离散化有:其中,X k和Z k分别为***在t k时刻的状态向量以及量测向量,W k和V k分别为水下潜航阶段子***的噪声阵和量测噪声阵,且均值均为零,统计特性如下:<1>初始化增广状态向量及估计误差方差<2>计算sigma点χ i,k-1和相应的加权因子W iW i=(1-W 0)/22,其中,0≤W 0≤1χ i,k/k-1=f 1(χ i,k-1)χ i,k/k-1为t k时刻预测的第i个样本点;Z i,k/k-1=h 1(χ i,k/k-1)其中,Z i,k/k-1为第i个量测值;对水面位置修正阶段非线性滤波方程进行离散化有:其中,x k和z k分别为***在t k时刻的状态向量以及量测向量,w k和v k分别为水面位置修正阶段子***的噪声阵和量测噪声阵,且均值均为零,统计特性如下: q k和r k分别为该子***噪声协方差阵和量测噪声协方差阵;具体算法步骤如下:<1>初始化增广状态向量及估计误差方差<2>计算sigma点χ i,k-1和相应的加权因子W iW i=(1-W 0)/13,其中,0≤W 0≤1χ i,k/k-1=f 2(χ i,k-1)χ i,k/k-1为t k时刻预测的第i个样本点;Z i,k/k-1=h 2(χ i,k/k-1)其中,Z i,k/k-1为第i个量测值;将每次AUV浮出水面的时的滤波结果的位置信息作为下一次潜航的新的位置信息,定时修正位置。
Applications Claiming Priority (2)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201811159410.5 | 2018-09-30 | ||
CN201811159410.5A CN109141436A (zh) | 2018-09-30 | 2018-09-30 | 改进的无迹卡尔曼滤波算法在水下组合导航中的应用方法 |
Publications (1)
Publication Number | Publication Date |
---|---|
WO2020062807A1 true WO2020062807A1 (zh) | 2020-04-02 |
Family
ID=64814207
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
PCT/CN2019/079136 WO2020062807A1 (zh) | 2018-09-30 | 2019-03-21 | 改进的无迹卡尔曼滤波算法在水下组合导航中的应用方法 |
Country Status (2)
Country | Link |
---|---|
CN (1) | CN109141436A (zh) |
WO (1) | WO2020062807A1 (zh) |
Cited By (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111982117A (zh) * | 2020-08-17 | 2020-11-24 | 电子科技大学 | 一种基于深度学习的auv光学引导与测向方法 |
CN112393729A (zh) * | 2020-10-29 | 2021-02-23 | 国家深海基地管理中心 | 一种深海水面移动声学基准层联合导航定位***及方法 |
CN112432644A (zh) * | 2020-11-11 | 2021-03-02 | 杭州电子科技大学 | 基于鲁棒自适应无迹卡尔曼滤波的无人艇组合导航方法 |
CN113503891A (zh) * | 2021-04-22 | 2021-10-15 | 中国人民解放军海军工程大学 | 一种sinsdvl对准校正方法、***、介质及设备 |
Families Citing this family (17)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109141436A (zh) * | 2018-09-30 | 2019-01-04 | 东南大学 | 改进的无迹卡尔曼滤波算法在水下组合导航中的应用方法 |
CN109931936A (zh) * | 2019-03-18 | 2019-06-25 | 西北工业大学 | 一种基于移动中继站的弱连通auv协同导航方法 |
CN110926497B (zh) * | 2019-04-08 | 2020-10-16 | 青岛中海潮科技有限公司 | 水下运载器惯导误差预测、浮起校正自动规划方法、装置 |
CN110208843B (zh) * | 2019-05-21 | 2022-07-22 | 南京航空航天大学 | 一种基于增广伪距信息辅助的容错导航方法 |
CN110146076B (zh) * | 2019-06-06 | 2023-04-18 | 哈尔滨工业大学(威海) | 一种无逆矩阵自适应滤波的sins/dvl组合定位方法 |
CN110514203B (zh) * | 2019-08-30 | 2022-06-28 | 东南大学 | 一种基于isr-ukf的水下组合导航方法 |
CN112230296B (zh) | 2019-12-17 | 2021-07-23 | 东南大学 | 一种重力相关时间倒数确定方法 |
CN111366962A (zh) * | 2020-03-12 | 2020-07-03 | 国家深海基地管理中心 | 一种深远海低成本长航时协同导航定位*** |
CN111722295B (zh) * | 2020-07-04 | 2021-04-23 | 东南大学 | 一种水下捷联式重力测量数据处理方法 |
CN112254718B (zh) * | 2020-08-04 | 2024-04-09 | 东南大学 | 一种运动约束辅助的基于改进Sage-Husa自适应滤波的水下组合导航方法 |
CN111982126B (zh) * | 2020-08-31 | 2023-03-17 | 郑州轻工业大学 | 一种全源BeiDou/SINS弹性状态观测器模型设计方法 |
CN112684207B (zh) * | 2020-12-17 | 2022-03-11 | 东南大学 | 一种深潜载人潜水器adcp速度估计与修正算法 |
CN112729291B (zh) * | 2020-12-29 | 2022-03-04 | 东南大学 | 一种深潜长航潜水器sins/dvl洋流速度估计方法 |
CN113654559B (zh) * | 2021-08-20 | 2023-03-31 | 青岛澎湃海洋探索技术有限公司 | 基于多模型观测校正的auv导航方法 |
CN113959433B (zh) * | 2021-09-16 | 2023-12-08 | 南方电网数字平台科技(广东)有限公司 | 一种组合导航方法及装置 |
CN114777745A (zh) * | 2022-04-08 | 2022-07-22 | 南京信息工程大学 | 一种基于无迹卡尔曼滤波的倾斜取证建模方法 |
CN117606491B (zh) * | 2024-01-24 | 2024-04-26 | 中国海洋大学 | 一种自主式水下航行器的组合定位导航方法及装置 |
Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103278163A (zh) * | 2013-05-24 | 2013-09-04 | 哈尔滨工程大学 | 一种基于非线性模型的sins/dvl组合导航方法 |
CN103744098A (zh) * | 2014-01-23 | 2014-04-23 | 东南大学 | 基于sins/dvl/gps的auv组合导航*** |
CN109141436A (zh) * | 2018-09-30 | 2019-01-04 | 东南大学 | 改进的无迹卡尔曼滤波算法在水下组合导航中的应用方法 |
-
2018
- 2018-09-30 CN CN201811159410.5A patent/CN109141436A/zh active Pending
-
2019
- 2019-03-21 WO PCT/CN2019/079136 patent/WO2020062807A1/zh active Application Filing
Patent Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103278163A (zh) * | 2013-05-24 | 2013-09-04 | 哈尔滨工程大学 | 一种基于非线性模型的sins/dvl组合导航方法 |
CN103744098A (zh) * | 2014-01-23 | 2014-04-23 | 东南大学 | 基于sins/dvl/gps的auv组合导航*** |
CN109141436A (zh) * | 2018-09-30 | 2019-01-04 | 东南大学 | 改进的无迹卡尔曼滤波算法在水下组合导航中的应用方法 |
Non-Patent Citations (3)
Title |
---|
LIU, MINGYONG ET AL.: "Research on Integrated Navigation for Autonomous Underwater Vehicle based on an Improved Unscented Kalman Filter", ACTA ARMAMENTARII, vol. 32, no. 2, 28 February 2011 (2011-02-28), pages 252 - 255, XP055699666 * |
LV, ZHIGANG: "Research on SINS/DVL/GPS Integrated Navigation System Calibration Method and the Error Analysis of AUV", SHIP ELECTRONIC ENGINEERING, vol. 38, no. 6, 30 June 2018 (2018-06-30), pages 33 - 36 * |
ZHOU, BENCHUAN: "Federated filtering algorithm based on fuzzy adaptive UKF for marine SINS/GPS/DVL integrated system", 2010 CHINESE CONTROL AND DECISION CONFERENCE, 31 December 2010 (2010-12-31), XP031700090 * |
Cited By (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111982117A (zh) * | 2020-08-17 | 2020-11-24 | 电子科技大学 | 一种基于深度学习的auv光学引导与测向方法 |
CN111982117B (zh) * | 2020-08-17 | 2022-05-10 | 电子科技大学 | 一种基于深度学习的auv光学引导与测向方法 |
CN112393729A (zh) * | 2020-10-29 | 2021-02-23 | 国家深海基地管理中心 | 一种深海水面移动声学基准层联合导航定位***及方法 |
CN112393729B (zh) * | 2020-10-29 | 2021-07-16 | 国家深海基地管理中心 | 一种深海水面移动声学基准层联合导航定位***及方法 |
CN112432644A (zh) * | 2020-11-11 | 2021-03-02 | 杭州电子科技大学 | 基于鲁棒自适应无迹卡尔曼滤波的无人艇组合导航方法 |
CN112432644B (zh) * | 2020-11-11 | 2022-03-25 | 杭州电子科技大学 | 基于鲁棒自适应无迹卡尔曼滤波的无人艇组合导航方法 |
CN113503891A (zh) * | 2021-04-22 | 2021-10-15 | 中国人民解放军海军工程大学 | 一种sinsdvl对准校正方法、***、介质及设备 |
Also Published As
Publication number | Publication date |
---|---|
CN109141436A (zh) | 2019-01-04 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
WO2020062807A1 (zh) | 改进的无迹卡尔曼滤波算法在水下组合导航中的应用方法 | |
CN109443379B (zh) | 一种深海潜航器的sins/dvl水下抗晃动对准方法 | |
CN108827310B (zh) | 一种船用星敏感器辅助陀螺仪在线标定方法 | |
CN110514203B (zh) | 一种基于isr-ukf的水下组合导航方法 | |
CN103759742B (zh) | 基于模糊自适应控制技术的捷联惯导非线性对准方法 | |
CN104655131B (zh) | 基于istssrckf的惯性导航初始对准方法 | |
CN106123921A (zh) | 动态干扰条件下捷联惯导***的纬度未知自对准方法 | |
Arnold et al. | Robust model-aided inertial localization for autonomous underwater vehicles | |
CN101920762A (zh) | 一种基于噪声矩阵实时修正的船舶动力定位方法 | |
CN115200574A (zh) | 一种地球椭球模型下的极区横向组合导航方法 | |
Huang et al. | Weight self-adjustment Adams implicit filtering algorithm for attitude estimation applied to underwater gliders | |
CN112015086B (zh) | 一种欠驱动水面船有限时间路径跟踪输出反馈控制方法 | |
Ben et al. | A dual-state filter for a relative velocity aiding strapdown inertial navigation system | |
Xu et al. | USBL positioning system based adaptive kalman filter in AUV | |
CN115855049A (zh) | 基于粒子群优化鲁棒滤波的sins/dvl导航方法 | |
CN111964675A (zh) | 一种黑障区的飞行器智能导航方法 | |
CN104061930A (zh) | 基于捷联惯性制导和多普勒计程仪的导航方法 | |
CN110873577B (zh) | 一种水下快速动基座对准方法及装置 | |
Wang et al. | AUV navigation based on inertial navigation and acoustic positioning systems | |
CN103616026B (zh) | 一种基于h∞滤波的auv操纵模型辅助捷联惯导组合导航方法 | |
CN105300407A (zh) | 一种用于单轴调制激光陀螺惯导***的海上动态启动方法 | |
CN103697887B (zh) | 一种基于捷联惯导***和多普勒计程仪的优化导航方法 | |
Zhang | Autonomous underwater vehicle navigation using an adaptive Kalman filter for sensor fusion | |
Braga et al. | Navigation scheme for the LSTS SEACON vehicles: Theory and application | |
Bao et al. | AUV Docking Recovery Based on USBL Integrated Navigation Method |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
121 | Ep: the epo has been informed by wipo that ep was designated in this application |
Ref document number: 19865580 Country of ref document: EP Kind code of ref document: A1 |
|
NENP | Non-entry into the national phase |
Ref country code: DE |
|
122 | Ep: pct application non-entry in european phase |
Ref document number: 19865580 Country of ref document: EP Kind code of ref document: A1 |
|
122 | Ep: pct application non-entry in european phase |
Ref document number: 19865580 Country of ref document: EP Kind code of ref document: A1 |
|
32PN | Ep: public notification in the ep bulletin as address of the adressee cannot be established |
Free format text: NOTING OF LOSS OF RIGHTS PURSUANT TO RULE 112(1) EPC (EPO FORM 1205N DATED 26/05/2021) |
|
122 | Ep: pct application non-entry in european phase |
Ref document number: 19865580 Country of ref document: EP Kind code of ref document: A1 |