WO2022028286A1 - 一种运动约束辅助的基于改进Sage-Husa自适应滤波的水下组合导航方法 - Google Patents
一种运动约束辅助的基于改进Sage-Husa自适应滤波的水下组合导航方法 Download PDFInfo
- Publication number
- WO2022028286A1 WO2022028286A1 PCT/CN2021/108862 CN2021108862W WO2022028286A1 WO 2022028286 A1 WO2022028286 A1 WO 2022028286A1 CN 2021108862 W CN2021108862 W CN 2021108862W WO 2022028286 A1 WO2022028286 A1 WO 2022028286A1
- Authority
- WO
- WIPO (PCT)
- Prior art keywords
- error
- underwater
- measurement
- filtering
- integrated navigation
- Prior art date
Links
- 238000001914 filtration Methods 0.000 title claims abstract description 70
- 230000033001 locomotion Effects 0.000 title claims abstract description 49
- 230000003044 adaptive effect Effects 0.000 title claims abstract description 38
- 238000000034 method Methods 0.000 title claims abstract description 38
- 238000005259 measurement Methods 0.000 claims abstract description 58
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 27
- 230000001133 acceleration Effects 0.000 claims abstract description 17
- 239000011159 matrix material Substances 0.000 claims description 42
- 230000007704 transition Effects 0.000 claims description 14
- 230000008569 process Effects 0.000 claims description 9
- 230000009466 transformation Effects 0.000 claims description 7
- 230000005284 excitation Effects 0.000 claims description 6
- 241000251468 Actinopterygii Species 0.000 claims description 5
- 239000013535 sea water Substances 0.000 claims description 3
- 238000006243 chemical reaction Methods 0.000 claims 1
- 238000004364 calculation method Methods 0.000 abstract description 2
- 238000010586 diagram Methods 0.000 description 4
- 238000013459 approach Methods 0.000 description 3
- 230000006872 improvement Effects 0.000 description 3
- 238000005516 engineering process Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000004088 simulation Methods 0.000 description 2
- 238000004458 analytical method Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000000354 decomposition reaction Methods 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 238000005562 fading Methods 0.000 description 1
- 238000005295 random walk Methods 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 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/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/20—Instruments for performing navigational calculations
-
- 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
-
- 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
Definitions
- the underwater vehicle As the underwater vehicle technology becomes more and more mature, the underwater vehicle as a reliable detection tool has received extensive attention at home and abroad, and how to improve the accuracy of underwater navigation has also become the focus of scholar in various fields.
- the inertial device is independent and does not rely on the characteristics of external information, so it still has high reliability in the underwater environment, but the error of the inertial device will diverge over time, thus affecting the positioning accuracy.
- the Doppler log has high accuracy, and the error does not diverge with time, so the speed information of the strapdown inertial navigation can be well corrected and the positioning accuracy is improved.
- the present invention introduces an improved Sage-Husa adaptive algorithm based on centripetal acceleration constraints to assist the inertial/Doppler combined navigation, and proposes a motion constraint-assisted underwater based on improved Sage-Husa adaptive filtering.
- the integrated navigation method introduces centripetal acceleration to constrain the speed of the underwater vehicle when the underwater vehicle turns due to submarine ditches, fish schools and strong maneuverability resulting in errors in the Doppler measurement information, and the Sage- The Husa adaptive filtering algorithm is improved to reduce the positioning error and improve the positioning accuracy.
- a motion constraint-assisted underwater integrated navigation method based on improved Sage-Husa adaptive filtering comprising the following steps:
- Step 2 On the basis of the inertial navigation error equation, the velocity error, drift angle error and proportional coefficient error in the Doppler error model are introduced as the state quantities of the underwater integrated navigation to construct the state of the underwater integrated navigation system based on Kalman filtering. equation;
- Step 3 establish a carrier coordinate system, decompose the motion of the underwater vehicle into a plane perpendicular to the Z axis and a plane perpendicular to the X axis, and establish constraints according to the relationship between the centripetal acceleration and the forward speed of the underwater vehicle, and construct Complete motion constraint model;
- Step 5 Discretize the state equation and the measurement equation, establish a filter equation in combination with steps 2 and 4, and use the standard Kalman filtering algorithm to solve the problem when the underwater glider is running normally.
- the measurement noise changes , using the improved Sage-Husa adaptive filtering algorithm for time update, measurement update and filter update.
- Two pairs of transducers are installed at the bottom of the underwater submersible to transmit beams in four directions.
- the Doppler frequency shift is obtained by measuring the frequency of the transmitted beam and the frequency of the reflected beam, so as to obtain the three directions of the carrier coordinate system.
- the speed of the underwater vehicle is as follows:
- the Doppler log error model is:
- v d is the forward speed of the underwater vehicle
- c is the speed of light
- v x , vy , and v z are the speeds in the three directions of the coordinate system of the vehicle carrier
- f 0 is the frequency of the transmitted wave
- f d1 , f d2 , f d3 , f d4 represent the Doppler frequency shift
- ⁇ is the inclination of the transmit beam
- ⁇ v dU , ⁇ v dE , ⁇ v dN are the velocity errors of the submersible in the northeast sky coordinate system in three directions
- ⁇ v d is the Doppler Velocity measurement error
- ⁇ is the pitch angle of the submarine
- ⁇ C is the scale factor error
- K d , ⁇ and ⁇ are the track direction, azimuth misalignment angle and yaw angle error of the submarine considering the yaw angle, respectively.
- a k,k-1 represents the state transition matrix of the system from time k-1 to time k;
- ⁇ k,k-1 represents the noise driving matrix of the system, and
- W k-1 is the system noise excitation sequence;
- the corresponding state vector is:
- ⁇ v d , ⁇ , ⁇ C are represented by the following formulas:
- ⁇ L, ⁇ , ⁇ h represent the longitude, latitude and height errors of the carrier
- ⁇ v E , ⁇ v N , ⁇ v U are the velocity errors of the carrier in the three directions of east, north and sky
- ⁇ , ⁇ , ⁇ are the attitude angles of the carrier Error
- ⁇ x , ⁇ y , ⁇ y are gyroscope biases
- ⁇ d -1 , ⁇ ⁇ -1 are the correlation time of velocity offset error and the correlation time of drift angle error
- ⁇ d , ⁇ ⁇ are excitation white noise.
- step 3 specifically includes the following process:
- a rx and a rz are the centripetal acceleration values in the x and z directions in the submersible carrier coordinate system; are the sensitive angular velocities of the x-axis and z-axis of the inertial device; is the added value of the x, z axis ratio; are the angular velocity of the earth's rotation and the angular velocity caused by the motion of the carrier, respectively.
- the second measurement equation is:
- the quantity measurement Z 1 is the difference between the speed calculated by the inertial navigation solution and the speed measured by the Doppler log, and the quantity measurement Z 2 is the speed constraint of the submersible in two directions and the acceleration constraint when the carrier moves.
- the state space model in step 5 includes:
- Z k is the observation vector of the sensor at time k
- H is the transformation matrix of the system from the state space to the observation space
- V k is the measurement noise sequence
- the state transition matrix A of the system is:
- H 1 [0 3 ⁇ 3 I 3 ⁇ 3 S 1 0 3 ⁇ 6 S 2 ]
- H 2 [0 3 ⁇ 4 M 1 M 2 M 3 M 4 0 3 ⁇ 4 ]
- X k X k,k-1 +K k (Z k -H k X k,k-1 )
- X k represents the state variable of the carrier at time k
- A represents the state transition matrix of the system from time k to time k+1;
- Z k is the observation vector of the sensor at time k; H is the transformation matrix of the system from state space to observation space; K k is the Kalman filter gain at time k; Q is the system noise covariance matrix; R is the observation covariance matrix; P is the error covariance matrix.
- the optimal ⁇ k will be calculated in real time to prevent the filtering from divergent; judge whether the filtering is divergent according to the following formula:
- the measurement noise changes includes at least one of the following situations: the underwater glider encounters an obstacle or turns with strong maneuverability.
- the obstacles include underwater ditches and schools of fish.
- the present invention has the following advantages and beneficial effects:
- the method of the invention introduces the centripetal acceleration error generated when the underwater submersible turns or performs strong maneuvering actions as a measure. Compared with the traditional motion constraint, the method of the present invention is more complete, and can effectively restrain the forward speed of the carrier and avoid the carrier due to strong maneuvering. The measurement information caused by the movement has a large error, which effectively improves the navigation accuracy of the system.
- the improved Sage-Husa algorithm proposed by the present invention increases the fault judgment of the measurement information and the optimal estimation of the scale factor, which can not only reduce the error when the measurement information is correct
- the filtering calculation amount can also reduce the divergence of the system filtering when there is an error in the measurement information, and has better robustness and reliability, thereby improving the navigation accuracy of the system.
- the method of the invention can effectively reduce the error when the Doppler measurement information is wrong, and can be used to improve the underwater integrated navigation accuracy, positioning accuracy and fault tolerance of the navigation system, so as to realize the more accurate operation of the underwater submersible.
- Figure 1 is a schematic diagram of the working principle of the Doppler log.
- Figure 2 is a schematic diagram of the Doppler log error model.
- Figure 3 is a schematic diagram of the decomposition of the motion of the underwater submersible.
- Fig. 4 is the flow chart of the adaptive filtering algorithm based on improved Sage-Husa aided by motion constraints.
- Fig. 5 is the position and velocity error diagram of the navigation system under the standard Kalman filter algorithm.
- Figure 6 is the position and velocity error map of the navigation system based on the improved Sage-Husa adaptive filtering algorithm assisted by motion constraints.
- Figure 7 shows the position and velocity errors of the navigation system under the motion constraint-assisted Sage-Husa adaptive algorithm when the measurement noise is disturbed.
- Figure 8 shows the position and velocity errors of the navigation system under the improved Sage-Husa adaptive algorithm assisted by motion constraints when the measurement noise is disturbed.
- the present invention proposes an underwater integrated navigation method based on improved Sage-Husa adaptive filtering assisted by motion constraints.
- the implementation principle and method flow are shown in Figures 1 to 4, and the specific steps are as follows:
- Step 1 according to the working principle of the Doppler log, establish a Doppler log error model:
- the principle of the Doppler log is relatively simple. As shown in Figure 1, two pairs of transducers are installed at the bottom of the underwater submersible to transmit beams in four directions. By measuring the frequency of the transmitted beam and the reflected beam The frequency is obtained by Doppler frequency shift, so as to obtain the speed of the underwater vehicle in the three directions of the carrier coordinate system as follows:
- v d is the forward speed of the underwater vehicle
- c is the speed of light
- f 0 is the frequency of the transmitted wave
- f d1 , f d2 , f d3 , and f d4 represent the Doppler frequency shift
- ⁇ is the inclination of the transmitted beam
- ⁇ v dU , ⁇ v dE , ⁇ v dN is the velocity error of the submersible in three directions in the northeast sky coordinate system
- ⁇ v d is the Doppler velocity measurement error
- ⁇ is the pitch angle of the submersible
- ⁇ C is the scale factor error
- K d , ⁇ and ⁇ are the submersible, respectively Consider the track direction, azimuth misalignment and yaw angle error of the yaw angle.
- Step 2 On the basis of the inertial navigation error equation, the velocity error ⁇ v d , the drift angle error ⁇ and the proportional coefficient error ⁇ C in the Doppler error model are introduced as the state quantities of the underwater integrated navigation, as shown in Figure 2, to construct The state equation of the underwater integrated navigation system based on Kalman filter:
- a k,k-1 represents the state transition matrix of the system from time k-1 to time k; ⁇ k,k-1 represents the noise driving matrix of the system, and W k-1 is the system noise excitation sequence.
- the corresponding state vector is:
- ⁇ v d , ⁇ , ⁇ C can be expressed by the following formulas:
- ⁇ L, ⁇ , ⁇ h represent the longitude, latitude and height errors of the carrier
- ⁇ v E , ⁇ v N , ⁇ v U are the velocity errors of the carrier in the three directions of east, north and sky
- ⁇ , ⁇ , ⁇ are the attitude angles of the carrier Error
- ⁇ x , ⁇ y , ⁇ y are gyroscope biases
- ⁇ d -1 , ⁇ ⁇ -1 are the correlation time of velocity offset error and the correlation time of drift angle error
- ⁇ d , ⁇ ⁇ are excitation white noise.
- Step 3 any movement of the underwater vehicle can be decomposed into two planes that are perpendicular to each other, establish a carrier coordinate system, and decompose the movement of the underwater vehicle into a plane perpendicular to the Z axis and a plane perpendicular to the X axis, as shown in the figure. 3, and establish constraints according to the relationship between the centripetal acceleration and forward velocity of the underwater vehicle, and build a complete motion constraint model:
- v n is the speed of the submersible in the navigation coordinate system
- g n is the gravitational acceleration in the navigation coordinate system
- a rx and a rz are the centripetal acceleration values in the x and z directions in the submersible carrier coordinate system
- step 4 the measurement equation (1) is established according to the navigation information of the inertial device and the Doppler log, and the measurement equation (2) is established according to the complete motion constraint model:
- v E , v N , and v U are the speeds in the northeast sky direction of the submarine calculated by the inertial navigation solution (the northeast sky coordinate system is selected as the navigation coordinate system in the present invention).
- v dE , v dN , v dU are the velocities in the three directions of the northeast sky after coordinate transformation measured by the Doppler log.
- the measurement Z 1 is the difference between the speed calculated by the inertial navigation solution and the speed measured by the Doppler log
- the measurement Z 2 is the speed constraint of the submersible in two directions and the acceleration constraint when the carrier is moving.
- the value of Quantity Measurement Z2 should be 0 or white noise.
- Step 5 Discretize the state equation and the measurement equation, establish a filter equation in combination with steps 2 and 4, and use the standard Kalman filtering algorithm to solve the problem when the underwater glider is running normally.
- the improved Sage-Husa adaptive filtering algorithm is used for time update, measurement update and filter update. The specific process is shown in Figure 4. :
- the state space model of the system includes state equations and measurement equations, as follows:
- Z k is the observation vector of the sensor at time k
- H is the transformation matrix of the system from the state space to the observation space
- V k is the measurement noise sequence
- the state transition matrix A of the system is:
- the measurement matrix of the system is:
- H 1 [0 3 ⁇ 3 I 3 ⁇ 3 S 1 0 3 ⁇ 6 S 2 ]
- H 2 [0 3 ⁇ 4 M 1 M 2 M 3 M 4 0 3 ⁇ 4 ]
- the system state noise is usually relatively stable, so the present invention only adapts the measurement noise.
- the measurement prediction error formula is:
- the prediction residual method it is possible to manually judge whether the filtering is divergent. If the filtering is normal, the general filtering is performed. If filtering divergence is detected, the optimal ⁇ k is calculated in real time to prevent the filtering from divergent.
- X k X k,k-1 +K k (Z k -H k X k,k-1 )
- X k represents the state variable of the carrier at time k
- A represents the state transition matrix of the system from time k to time k+1;
- Z k is the observation vector of the sensor at time k; H is the transformation matrix of the system from state space to observation space; K k is the Kalman filter gain at time k; Q is the system noise covariance matrix; R is the observation covariance matrix; P is the error covariance matrix.
- the standard Kalman filter algorithm is used for comparison with the motion constraint-assisted underwater integrated navigation method based on improved Sage-Husa adaptive filtering provided by the present invention.
- Figure 5 shows the position error and velocity error of the combined system under the standard Kalman filtering algorithm. It can be seen that the standard Kalman algorithm lacks the understanding of the statistical noise characteristics of the system in the early stage of the experimental solution, and the filtering error is large.
- the improved Sage-Husa adaptive filtering algorithm with additional motion constraints constrains the speed of the carrier in three directions and adapts to the measurement noise, which can well suppress the divergence of the combined system error and greatly improve the navigation accuracy of the system. As shown in Figure 6.
- Figures 7 and 8 show the navigation errors of the system before and after the improvement of the Sage-Husa adaptive filtering algorithm.
- the improved Sage-Husa adaptive filtering algorithm can well suppress the filtering divergence and improve the accuracy and robustness of the navigation system. .
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)
Abstract
一种运动约束辅助的基于改进Sage-Husa自适应滤波的水下组合导航方法,包括:建立多普勒计程仪误差模型;构建基于卡尔曼滤波的水下组合导航***的状态方程;根据水下潜航器向心加速度和前向速度间的关系建立约束条件,构建完整运动约束模型;建立两个量测方程;建立滤波方程,在水下滑翔器正常行驶时采用标准卡尔曼滤波算法进行解算,当量测噪声发生变化时,采用改进的Sage-Husa自适应滤波算法进行时间更新、量测更新和滤波更新。该方法能够提高水下组合导航***的滤波精度,较好地抑制量测干扰时的滤波发散,具有较好的鲁棒性和可靠性。
Description
本发明属于导航制导和控制领域,涉及水下导航方法,具体涉及一种运动约束辅助的基于改进Sage-Husa自适应滤波的水下组合导航方法。
随着水下潜器技术愈渐成熟,水下潜航器作为可靠探测工具受到了国内外广泛关注,如何提高水下导航精度也成为各领域学者关注的焦点。目前应用最为成熟也最为广泛的是惯性/多普勒组合导航技术。惯性器件独立自主,不依靠外界信息的特性使其在水下环境依然具有较高可靠性,但惯性器件的误差会随着时间发散,从而影响定位的精度。而多普勒计程仪精度较高,误差不随着时间发散,因此可以很好的修正捷联惯导的速度信息,提高定位的精度。
但水下环境较为复杂,水下潜航器通常在海底以锯齿波滑翔,在滑翔过程中,若遇到海底沟渠、聚集性鱼群以及强机动性转弯时,多普勒计程仪提供的量测信息则会发生错误,并且预先设置的量测噪声矩阵将无法适应受到扰动的模型,从而影响定位的精度,而当水下潜航器以锯齿波滑翔或进行强机动性转弯时,所产生的向心加速度将会对水下潜航器的速度产生约束。
发明内容
为解决上述问题,本发明引入基于向心加速度约束以及改进的 Sage-Husa自适应算法来辅助惯性/多普勒组合导航,提出一种运动约束辅助的基于改进Sage-Husa自适应滤波的水下组合导航方法,在水下潜航器因为海底沟渠、鱼群以及强机动性转弯时导致多普勒量测信息出现错误时,引入向心加速度对水下潜航器的速度进行约束,并且对Sage-Husa自适应滤波算法进行改进,从而减小定位误差,提高定位精度。
为了达到上述目的,本发明提供如下技术方案:
一种运动约束辅助的基于改进Sage-Husa自适应滤波的水下组合导航方法,包括如下步骤:
步骤1,根据多普勒计程仪的工作原理,建立多普勒计程仪误差模型;
步骤2,在惯性导航误差方程的基础上,引入多普勒误差模型中速度误差、偏流角误差以及比例系数误差作为水下组合导航的状态量构建基于卡尔曼滤波的水下组合导航***的状态方程;
步骤3,建立载体坐标系,将水下潜航器的运动分解到垂直于Z轴平面和垂直于X轴平面,并根据水下潜航器向心加速度和前向速度间的关系建立约束条件,构建完整运动约束模型;
步骤4,根据惯性器件以及多普勒计程仪的导航信息建立第一量测方程,并根据步骤2中完整运动约束模型建立第二量测方程;
步骤5,对状态方程以及量测方程进行离散化处理,结合步骤2和步骤4建立滤波方程,在水下滑翔器正常行驶时采用标准卡尔曼滤波算法进行解算,当量测噪声发生变化时,采用改进的Sage-Husa自 适应滤波算法进行时间更新、量测更新和滤波更新。
进一步的,所述步骤1中多普勒计程仪的工作原理为:
水下潜航器底端安装前后左右各两对换能器,向四个方向发射波束,通过测量发射波束的频率以及反射后波束频率得到多普勒频移,从而得到载体坐标系三个方向上水下潜航器的速度如下:
f
d13=f
d1-f
d3,f
d24=f
d2-f
d4
所述多普勒计程仪误差模型为:
其中,v
d为水下潜航器的前向速度,c为光速,v
x、v
y、v
z为潜航器载体坐标系三个方向的速度;f
0为发射波频率,f
d1、f
d2、f
d3、f
d4表示的是多普勒频移;α为发射波束倾角;δv
dU、δv
dE、δv
dN为东北天坐标系下潜航器三个方向的速度误差;δv
d为多普勒测速误差;β为潜航器俯仰角;δC为比例因子误差;K
d、γ以及δ△分别为潜航器考虑偏流角的航迹向、方位失准角以及偏流角误差。
进一步的,所述步骤2中基于卡尔曼滤波的水下组合导航***的状态方程为:
X
k=A
k,k-1X
k-1+Γ
k,k-1W
k-1
其中,A
k,k-1表示***从k-1时刻到k时刻的状态转移矩阵;Γ
k,k-1表示***的噪声驱动矩阵,W
k-1为***噪声激励序列;
相应的状态向量为:
其中,δv
d、δ△、δC由如下公式表示:
式中δL、δλ、δh表示载体经度、纬度以及高度误差;δv
E、δv
N、δv
U为载体在东、北、天三个方向上的速度误差;α、β、γ为载体的姿态角误差;ε
x、ε
y、ε
y为陀螺仪零偏;
表示加计零偏;β
d
-1、β
△
-1为速度偏移误差的相关时间和偏流角误差的相关时间;ω
d、ω
△均为激励白噪声。
进一步的,所述步骤3具体包括如下过程:
假设潜航器垂直于前向速度的两个方向速度只与海水流速有关且假设流速为0,可得约束条件:
潜航器的任意运动都分解到垂直于z轴平面与垂直于x轴平面,根据运动学公式,可得:
根据惯性器件可得:
由此可得完整的运动约束模型为:
从而得到完整运动约束的误差模型为:
其中,
为潜航器在载体坐标系下的速度;a
rx、a
rz为潜航器载体坐标系中x和z方向的向心加速度值;
为惯性器件x轴和z轴敏感角速度;
为加计x、z轴比力值;
分别为地球自转角速度和由载体运动引起的角速度。
进一步的,所述步骤4中第一量测方程为:
第二量测方程为:
其中量测量Z
1为惯导解算出的速度与多普勒计程仪测速之差,量 测量Z
2为潜航器两个方向的速度约束以及载体运动时的加速度约束。
进一步的,所述步骤5中状态空间模型包括:
X
k=A
k,k-1X
k-1+Γ
k,k-1W
k-1
Z
k=H
kX
k+V
k
其中,Z
k为k时刻传感器的观测向量;H为***从状态空间到观测空间的转换矩阵,V
k为量测噪声序列;
***的状态转移矩阵A为:
***的量测矩阵为:
H
1=[0
3×3 I
3×3 S
1 0
3×6 S
2]
其中:
H
2=[0
3×4 M
1 M
2 M
3 M
4 0
3×4]
假设:
则:
其中:
进一步的,所述步骤5中改进的Sage-Husa自适应滤波算法过程如下:
X
k=X
k,k-1+K
k(Z
k-H
kX
k,k-1)
P
k=(I-K
kH
k)P
k,k-1
其中,X
k表示载体k时刻的状态变量,A表示***从k时刻到k+1时刻的状态转移矩阵;
Z
k为k时刻传感器的观测向量;H为***从状态空间到观测空间的转换矩阵;K
k为k时刻的卡尔曼滤波增益;Q为***噪声协方差矩阵;R为观测协方差矩阵;P为误差协方差矩阵。
进一步的,所述β
k通过如下过程推导:
在卡尔曼滤波中,量测预测误差公式为:
两边同时求方差可得:
利用指数减小记忆加权平均方法可得:
判断滤波是否发散,滤波正常,则按照一般滤波进行,若检测到滤波发散,则实时计算出最优β
k,阻止滤波的发散;根据下式判断滤波是否发散:
当上式成立时,则代表滤波发散,其中γ为储备系数,当γ=1时是最严格的收敛判据,采用最严格的收敛判据可得:
进一步的,所述步骤5中,当量测噪声发生变化的情况包括以下 情形中的至少一种:水下滑翔器遇到障碍物或强机动性转弯。
进一步的,所述障碍物包括水下沟渠、鱼群。
与现有技术相比,本发明具有如下优点和有益效果:
本发明方法引入水下潜航器拐弯或进行强机动动作时产生的向心加速度误差为量测量,相比于传统的运动约束更为完整,可以有效约束载体的前向速度,避免载体因为强机动运动导致的量测信息出现较大误差,有效提高了***的导航精度。本发明提出的改进的Sage-Husa算法,在传统的Sage-Husa自适应滤波基础上,增加了对量测信息的故障判断以及比例因子的最优估计,不仅可以减小量测信息正确时的滤波计算量,还可以在量测信息出现误差时减少***滤波的发散,具有较好的鲁棒性和可靠性,从而提高***的导航精度。本发明方法可以在多普勒量测信息发生错误时有效减小误差,可用于提高水下组合导航精度、定位精度以及导航***容错性,实现水下潜航器更精确的作业。
图1为多普勒计程仪的工作原理图。
图2为多普勒计程仪误差模型示意图。
图3为水下潜航器运动分解示意图。
图4为运动约束辅助的基于改进Sage-Husa自适应滤波算法流程图。
图5为标准卡尔曼滤波算法下导航***的位置及速度误差图。
图6为运动约束辅助的基于改进Sage-Husa自适应滤波算法下导 航***的位置及速度误差图。
图7为量测噪声受到干扰时,运动约束辅助的Sage-Husa自适应算法下导航***的位置及速度误差图。
图8为量测噪声受到干扰时,运动约束辅助的改进Sage-Husa自适应算法下导航***的位置及速度误差图。
以下将结合具体实施例对本发明提供的技术方案进行详细说明,应理解下述具体实施方式仅用于说明本发明而不用于限制本发明的范围。
本发明提出一种运动约束辅助的基于改进Sage-Husa自适应滤波的水下组合导航方法,实现原理及方法流程如图1-图4所示,具体步骤如下:
步骤1,根据多普勒计程仪的工作原理,建立多普勒计程仪误差模型:
多普勒计程仪的原理较为简单,如图1所示,水下潜航器底端安装前后左右各两对换能器,向四个方向发射波束,通过测量发射波束的频率以及反射后波束频率得到多普勒频移,从而得到载体坐标系三个方向上水下潜航器的速度如下:
f
d13=f
d1-f
d3,f
d24=f
d2-f
d4
相应的,得到多普勒计程仪的误差模型为:
其中,v
d为水下潜航器的前向速度,c为光速,
为潜航器在载体坐标系下的速度;f
0为发射波频率,f
d1、f
d2、f
d3、f
d4表示的是多普勒频移;α为发射波束倾角;δv
dU、δv
dE、δv
dN为东北天坐标系下潜航器三个方向的速度误差;δv
d为多普勒测速误差;β为潜航器俯仰角;δC为比例因子误差;K
d、γ以及δ△分别为潜航器考虑偏流角的航迹向、方位失准角以及偏流角误差。
步骤2,在惯性导航误差方程的基础上,引入多普勒误差模型中速度误差δv
d、偏流角误差δ△以及比例系数误差δC作为水下组合导航的状态量,如图2所示,构建基于卡尔曼滤波的水下组合导航***的状态方程:
其中,运动约束辅助的基于改进Sage-Husa自适应滤波算法的水下组合导航***的状态方程为:
X
k=A
k,k-1X
k-1+Γ
k,k-1W
k-1
其中,A
k,k-1表示***从k-1时刻到k时刻的状态转移矩阵;Γ
k,k-1表示***的噪声驱动矩阵,W
k-1为***噪声激励序列。相应的状态向量为:
其中,δv
d、δ△、δC可由如下公式表示:
式中δL、δλ、δh表示载体经度、纬度以及高度误差;δv
E、δv
N、δv
U为载体在东、北、天三个方向上的速度误差;α、β、γ为载体的姿态角误差;ε
x、ε
y、ε
y为陀螺仪零偏;
表示加计零偏;β
d
-1、β
△
-1为速度偏移误差的相关时间和偏流角误差的相关时间;ω
d、ω
△均为激励白噪声。
步骤3,水下潜航器的任意运动皆可以分解到相互垂直的两个平面上,建立载体坐标系,将水下潜航器的运动分解到垂直于Z轴平面和垂直于X轴平面,如图3所示,并根据水下潜航器向心加速度和前向速度间的关系建立约束条件,构建完整运动约束模型:
由于海底一定深度的水流速较为稳定,因此假设潜航器垂直于前向速度的两个方向速度只与海水流速有关且假设流速为0,可得约束条件:
该约束条件只能约束潜航器两个方向的速度,而潜航器在拐弯或进行强机动运动时会产生向心加速度,根据运动学公式,可得:
根据惯性器件可得:
由此可得完整的运动约束模型为:
从而得到完整运动约束的误差模型为:
其中,
为从导航坐标系到载体坐标系的转移矩阵,
表示导航坐标系下的姿态角。v
n为导航坐标系下潜航器的速度,g
n为导航坐标系下的重力加速度,
为载体坐标系下陀螺仪x轴和z轴的零偏,a
rx、a
rz为潜航器载体坐标系中x和z方向的向心加速度值;
为惯性器件x轴和z轴敏感角速度;
为加速度计x、z轴比力值;
分别为地球自转角速度和由载体运动引起的角速度。
步骤4,根据惯性器件以及多普勒计程仪的导航信息建立量测方程(1),并根据完整运动约束模型建立量测方程(2):
式中,v
E,v
N、v
U为惯导解算出的潜航器的东北天方向(本发明中选用东北天坐标系为导航坐标系)的速度。v
dE、v
dN、v
dU为多普勒计程仪所测得经过坐标转换后东北天三个方向的速度。量测量Z
1为惯导解算出的速度与多普勒计程仪测速之差,量测量Z
2为潜航器两个方向的速度约束以及载体运动时的加速度约束,在潜航器正常工作时,量测量Z
2的值应为0或为白噪声。
步骤5,对状态方程以及量测方程进行离散化处理,结合步骤2和步骤4建立滤波方程,在水下滑翔器正常行驶时采用标准卡尔曼滤波算法进行解算,当水下滑翔器遇到水下沟渠、鱼群或强机动性转弯等情况时,量测噪声发生变化时,采用改进的Sage-Husa自适应滤波算法进行时间更新、量测更新和滤波更新,具体流程如图4所示:
结合步骤1-步骤4,***的状态空间模型包括状态方程和量测方程,具体如下:
X
k=A
k,k-1X
k-1+Γ
k,k-1W
k-1
Z
k=H
kX
k+V
k
其中,Z
k为k时刻传感器的观测向量;H为***从状态空间到观测空间的转换矩阵,V
k为量测噪声序列;
***的状态转移矩阵A为:
***的量测矩阵为:
H
1=[0
3×3 I
3×3 S
1 0
3×6 S
2]
其中:
H
2=[0
3×4 M
1 M
2 M
3 M
4 0
3×4]
假设:
则:
其中:
在水下组合导航***中,通常***状态噪声是较为稳定的,因此本发明只对量测噪声进行自适应。在卡尔曼滤波中,量测预测误差公式为:
两边同时求方差可得:
利用指数减小记忆加权平均方法可得:
传统Sage-Husa滤波中认为:
其中b为渐消因子,但由于随着滤波次数k的增大,b
k会趋近0,自适应滤波的权重会趋近于1-b,并将保持不变;同时,初始值
对
的分配权值逐渐衰减,并逐渐接近于常值0。上述原因使得噪声估计器的自适应程度降低,滤波的精度便会随之下降。
根据预报残差法,可以人为判断滤波是否发散,滤波正常,则按照一般滤波进行,若检测到滤波发散,则实时计算出最优β
k,阻止滤波的发散。
滤波发散判据:
当上式成立时,则代表滤波发散,其中γ为储备系数,当γ=1时是最严格的收敛判据,采用最严格的收敛判据可得:
解得:
综上:
由此可得步骤5中基于改进Sage-Husa自适应滤波算法过程如下:
X
k=X
k,k-1+K
k(Z
k-H
kX
k,k-1)
P
k=(I-K
kH
k)P
k,k-1
其中,X
k表示载体k时刻的状态变量,A表示***从k时刻到k+1时刻的状态转移矩阵;
Z
k为k时刻传感器的观测向量;H为***从状态空间到观测空间的转换矩阵;K
k为k时刻的卡尔曼滤波增益;Q为***噪声协方差矩阵;R为观测协方差矩阵;P为误差协方差矩阵。
具体实施例
为了验证提出算法的正确性,这里提供了一个基于Matlab平台的仿真测试,仿真参数设置如下:
1.惯性元件指标及导航初始参数设置:
陀螺零偏稳定性:eb=0.2°/h
加计零偏稳定性:db=100ug
2.误差分析
结合上述参数,分别采用标准卡尔曼滤波算法和本发明提供的运动约束辅助的基于改进Sage-Husa自适应滤波的水下组合导航方法进行对比。图5给出了标准卡尔曼滤波算法下组合***的位置误差及速度误差,可以看出,标准卡尔曼算法在实验解算初期缺乏对***统计噪声特性的了解,滤波误差较大。附加运动约束的改进Sage-Husa自适应滤波算法对载体的三个方向速度进行约束,对量测噪声进行自适应,可以很好的抑制组合***误差的发散,也大大提高了***的导航精度,如图6所示。
为了验证滤波发散情况下改进自适应滤波的性能,在量测量600秒到610秒处加了十倍的量测噪声。图7和图8给出了Sage-Husa自适应滤波算法改进前后***的导航误差情况,经过改进的Sage-Husa自适应滤波算法可以很好的抑制滤波发散,提高导航***的精度及鲁棒性。
本发明方案所公开的技术手段不仅限于上述实施方式所公开的技术手段,还包括由以上技术特征任意组合所组成的技术方案。应当指出,对于本技术领域的普通技术人员来说,在不脱离本发明原理的前提下,还可以做出若干改进和润饰,这些改进和润饰也视为本发明的保护范围。
Claims (10)
- 一种运动约束辅助的基于改进Sage-Husa自适应滤波的水下组合导航方法,其特征在于,包括如下步骤:步骤1,根据多普勒计程仪的工作原理,建立多普勒计程仪误差模型;步骤2,在惯性导航误差方程的基础上,引入多普勒误差模型中速度误差、偏流角误差以及比例系数误差作为水下组合导航的状态量构建基于卡尔曼滤波的水下组合导航***的状态方程;步骤3,建立载体坐标系,将水下潜航器的运动分解到垂直于Z轴平面和垂直于X轴平面,并根据水下潜航器向心加速度和前向速度间的关系建立约束条件,构建完整运动约束模型;步骤4,根据惯性器件以及多普勒计程仪的导航信息建立第一量测方程,并根据步骤2中完整运动约束模型建立第二量测方程;步骤5,对状态方程以及量测方程进行离散化处理,结合步骤2和步骤4建立滤波方程,在水下滑翔器正常行驶时采用标准卡尔曼滤波算法进行解算,当量测噪声发生变化时,采用改进的Sage-Husa自适应滤波算法进行时间更新、量测更新和滤波更新。
- 根据权利要求1所述的运动约束辅助的基于改进Sage-Husa自适应滤波的水下组合导航方法,其特征在于:所述步骤1中多普勒计程仪的工作原理为:水下潜航器底端安装前后左右各两对换能器,向四个方向发射波束,通过测量发射波束的频率以及反射后波束频率得到多普勒频移,从而得到载体坐标系三个方向上水下潜航器的速度如下:f d13=f d1-f d3,f d24=f d2-f d4所述多普勒计程仪误差模型为:
- 根据权利要求1所述的运动约束辅助的基于改进Sage-Husa自适应滤波的水下组合导航方法,其特征在于,所述步骤2中基于卡尔曼滤波的水下组合导航***的状态方程为:X k=A k,k-1X k-1+Γ k,k-1W k-1其中,A k,k-1表示***从k-1时刻到k时刻的状态转移矩阵;Γ k,k-1表示***的噪声驱动矩阵,W k-1为***噪声激励序列;相应的状态向量为:其中,δv d、δ△、δC由如下公式表示:
- 根据权利要求1所述的运动约束辅助的基于改进Sage-Husa自适应滤波的水下组合导航方法,其特征在于,所述步骤3具体包括如下过程:假设潜航器垂直于前向速度的两个方向速度只与海水流速有关且假设流速为0,可得约束条件:潜航器的任意运动都分解到垂直于z轴平面与垂直于x轴平面,根据运动学公式,可得:根据惯性器件可得:由此可得完整的运动约束模型为:从而得到完整运动约束的误差模型为:
- 根据权利要求1所述的运动约束辅助的基于改进Sage-Husa自适应滤波的水下组合导航方法,其特征在于,所述步骤5中状态空间模型包括:X k=A k,k-1X k-1+Γ k,k-1W k-1Z k=H kX k+V k其中,Z k为k时刻传感器的观测向量;H为***从状态空间到观测空间的转换矩阵,V k为量测噪声序列;***的状态转移矩阵A为:***的量测矩阵为:H 1=[0 3×3 I 3×3 S 1 0 3×6 S 2]其中:H 2=[0 3×4 M 1 M 2 M 3 M 4 0 3×4]假设:则:其中:
- 根据权利要求1所述的运动约束辅助的基于改进Sage-Husa自适应滤波的水下组合导航方法,其特征在于,所述步骤5中,当量测噪声发生变化的情况包括以下情形中的至少一种:水下滑翔器遇到障碍物或强机动性转弯。
- 根据权利要求1所述的运动约束辅助的基于改进Sage-Husa自适应滤波的水下组合导航方法,其特征在于,所述障碍物包括水下沟渠、鱼群
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
US17/779,167 US11754400B2 (en) | 2020-08-04 | 2021-07-28 | Motion constraint-aided underwater integrated navigation method employing improved Sage-Husa adaptive filtering |
Applications Claiming Priority (2)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010772026.3A CN112254718B (zh) | 2020-08-04 | 2020-08-04 | 一种运动约束辅助的基于改进Sage-Husa自适应滤波的水下组合导航方法 |
CN202010772026.3 | 2020-08-04 |
Publications (1)
Publication Number | Publication Date |
---|---|
WO2022028286A1 true WO2022028286A1 (zh) | 2022-02-10 |
Family
ID=74223881
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
PCT/CN2021/108862 WO2022028286A1 (zh) | 2020-08-04 | 2021-07-28 | 一种运动约束辅助的基于改进Sage-Husa自适应滤波的水下组合导航方法 |
Country Status (3)
Country | Link |
---|---|
US (1) | US11754400B2 (zh) |
CN (1) | CN112254718B (zh) |
WO (1) | WO2022028286A1 (zh) |
Cited By (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114567401A (zh) * | 2022-04-14 | 2022-05-31 | 中国人民解放***箭军工程大学 | 一种基于感知通信一体化的无人机蜂群状态联合估计方法 |
CN114623822A (zh) * | 2022-05-12 | 2022-06-14 | 东南大学 | 基于惯性约束的多波束水下地形组合匹配方法 |
CN114993302A (zh) * | 2022-05-27 | 2022-09-02 | 中国人民解放军海军工程大学 | 基于多柔性节点的水下智能定位***和方法 |
CN115079227A (zh) * | 2022-07-26 | 2022-09-20 | 武汉优米捷光电子制造有限责任公司 | 基于改进无迹卡尔曼滤波的自旋弹组合导航方法 |
CN115112154A (zh) * | 2022-08-30 | 2022-09-27 | 南开大学 | 一种水下自主导航定位***的标定方法 |
CN115574816A (zh) * | 2022-11-24 | 2023-01-06 | 东南大学 | 仿生视觉多源信息智能感知无人平台 |
CN115855049A (zh) * | 2023-02-07 | 2023-03-28 | 河海大学 | 基于粒子群优化鲁棒滤波的sins/dvl导航方法 |
CN117553787A (zh) * | 2024-01-09 | 2024-02-13 | 湖南大学无锡智能控制研究院 | 水下无人航行器的协同导航方法、装置及*** |
CN117606491A (zh) * | 2024-01-24 | 2024-02-27 | 中国海洋大学 | 一种自主式水下航行器的组合定位导航方法及装置 |
Families Citing this family (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN112254718B (zh) * | 2020-08-04 | 2024-04-09 | 东南大学 | 一种运动约束辅助的基于改进Sage-Husa自适应滤波的水下组合导航方法 |
CN113984054A (zh) * | 2021-09-17 | 2022-01-28 | 兰州交通大学 | 基于信息异常检测的改进Sage-Husa自适应融合滤波方法及多源信息融合设备 |
US20220326016A1 (en) * | 2021-09-23 | 2022-10-13 | Harbin Engineering University | Robust Filtering Method for Integrated Navigation Based on Statistical Similarity Measure |
CN114166203B (zh) * | 2021-11-16 | 2024-02-09 | 哈尔滨工程大学 | 一种基于改进的s-h自适应联邦滤波的智能水下机器人多源组合导航方法 |
CN114637036B (zh) * | 2022-05-18 | 2022-08-19 | 深圳华大北斗科技股份有限公司 | 非完整性约束自适应量测噪声方法 |
CN116067370B (zh) * | 2023-04-03 | 2023-06-27 | 广东智能无人***研究院(南沙) | 一种imu姿态解算方法及设备、存储介质 |
CN116222578B (zh) * | 2023-05-04 | 2023-08-29 | 山东大学 | 基于自适应滤波和最优平滑的水下组合导航方法及*** |
CN117367410B (zh) * | 2023-12-04 | 2024-03-19 | 中南大学 | 状态估计方法、无人水下航行器及计算机可读存储介质 |
Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20090287414A1 (en) * | 2007-05-14 | 2009-11-19 | Zupt, Llc | System and process for the precise positioning of subsea units |
CN109141436A (zh) * | 2018-09-30 | 2019-01-04 | 东南大学 | 改进的无迹卡尔曼滤波算法在水下组合导航中的应用方法 |
CN110146076A (zh) * | 2019-06-06 | 2019-08-20 | 哈尔滨工业大学(威海) | 一种无逆矩阵自适应滤波的sins/dvl组合定位方法 |
CN110940340A (zh) * | 2019-12-23 | 2020-03-31 | 中科探海(苏州)海洋科技有限责任公司 | 基于小型uuv平台的多传感器信息融合方法 |
CN111024064A (zh) * | 2019-11-25 | 2020-04-17 | 东南大学 | 一种改进Sage-Husa自适应滤波的SINS/DVL组合导航方法 |
CN112254718A (zh) * | 2020-08-04 | 2021-01-22 | 东南大学 | 一种运动约束辅助的基于改进Sage-Husa自适应滤波的水下组合导航方法 |
Family Cites Families (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
NO336097B1 (no) * | 2013-05-16 | 2015-05-11 | Høgskolen I Ålesund | Undervannsfarkost |
CN103616026B (zh) * | 2013-12-17 | 2016-05-04 | 哈尔滨工程大学 | 一种基于h∞滤波的auv操纵模型辅助捷联惯导组合导航方法 |
CN109253726B (zh) * | 2018-06-22 | 2020-05-05 | 东南大学 | 一种水下滑翔器导航定位***及上浮精度校正方法 |
CN109032178B (zh) * | 2018-08-06 | 2021-08-24 | 江苏科技大学 | 全驱动auv回收控制***及自主回收方法 |
CN109443379B (zh) * | 2018-09-28 | 2020-07-21 | 东南大学 | 一种深海潜航器的sins/dvl水下抗晃动对准方法 |
CN109579850B (zh) * | 2019-01-16 | 2022-04-29 | 中国海洋大学 | 基于对水速度辅助惯导的深水智能导航方法 |
US11726200B2 (en) * | 2020-01-21 | 2023-08-15 | The Regents Of The University Of Colorado, A Body Corporate | Adaptive hybrid tracking algorithms for radio signal parameters estimations |
-
2020
- 2020-08-04 CN CN202010772026.3A patent/CN112254718B/zh active Active
-
2021
- 2021-07-28 WO PCT/CN2021/108862 patent/WO2022028286A1/zh active Application Filing
- 2021-07-28 US US17/779,167 patent/US11754400B2/en active Active
Patent Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20090287414A1 (en) * | 2007-05-14 | 2009-11-19 | Zupt, Llc | System and process for the precise positioning of subsea units |
CN109141436A (zh) * | 2018-09-30 | 2019-01-04 | 东南大学 | 改进的无迹卡尔曼滤波算法在水下组合导航中的应用方法 |
CN110146076A (zh) * | 2019-06-06 | 2019-08-20 | 哈尔滨工业大学(威海) | 一种无逆矩阵自适应滤波的sins/dvl组合定位方法 |
CN111024064A (zh) * | 2019-11-25 | 2020-04-17 | 东南大学 | 一种改进Sage-Husa自适应滤波的SINS/DVL组合导航方法 |
CN110940340A (zh) * | 2019-12-23 | 2020-03-31 | 中科探海(苏州)海洋科技有限责任公司 | 基于小型uuv平台的多传感器信息融合方法 |
CN112254718A (zh) * | 2020-08-04 | 2021-01-22 | 东南大学 | 一种运动约束辅助的基于改进Sage-Husa自适应滤波的水下组合导航方法 |
Non-Patent Citations (6)
Title |
---|
FAN XIAOLIANG: "Research on Integrated Navigation Algorithm of SINS/DVL Based on Adaptive Filtering", MASTER THESIS, TIANJIN POLYTECHNIC UNIVERSITY, CN, no. 1, 15 January 2019 (2019-01-15), CN , XP055892715, ISSN: 1674-0246 * |
WANG CHONG, ZENG QING-JUN: "Design of AUV integrated navigation system based on Sage-husa adaptive filtering algorithm", ELECTRONIC DESIGN ENGINEERING, SHIJIE ZHISHI CHUBANSHE, CN, vol. 21, no. 15, 5 August 2013 (2013-08-05), CN , XP055892713, ISSN: 1674-6236, DOI: 10.14022/j.cnki.dzsjgc.2013.15.030 * |
WANG YANGYANG: "Research on the Kinematics Model of AUV and Its Application in the Integrated Navigation", MASTER THESIS, TIANJIN POLYTECHNIC UNIVERSITY, CN, no. 3, 25 May 2019 (2019-05-25), CN , XP055892723, ISSN: 1674-0246 * |
WEI YANHUI, LIU JING;HAO SHENG-GONG: "SINS/DVL Integrated Navigation System Based on Improved Adaptive Filtering Algorithm", AUTOMATION & INSTRUMENTATION, CN, vol. 34, no. 5, 25 May 2019 (2019-05-25), CN , pages 95 - 100, XP055892710, ISSN: 1001-9944, DOI: 10.19557/j.cnki.1001-9944.2019.05.023 * |
XU XIAOSU, PAN YONGFEI; ZOU HAIJUN: "SINS/DVL Integrated Navigation System Based on Adaptive Filtering", HUAZHONG KEJI DAXUE XUEBAO (ZIRAN KEXUE BAN)/JOURNAL OF HUAZHONG UNIVERSITY OF SCIENCE AND TECHNOLOGY (NATURAL SCIENCE EDITION), HUAZHONG KEJI DAXUE, CN, vol. 43, no. 3, 11 March 2015 (2015-03-11), CN , XP055892719, ISSN: 1671-4512, DOI: 10.13245/j.hust.150319 * |
YANG YIPENG; YAN XIAOZHEN; LUO QINGHUA: "A SINS/DVL Integrated Navigation Positioning Method Based on Improved Adaptive Filtering Technology", 2019 IEEE INTERNATIONAL CONFERENCE ON SMART INTERNET OF THINGS (SMARTIOT), IEEE, 9 August 2019 (2019-08-09), pages 262 - 268, XP033658050, DOI: 10.1109/SmartIoT.2019.00047 * |
Cited By (14)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114567401B (zh) * | 2022-04-14 | 2023-02-14 | 中国人民解放***箭军工程大学 | 一种基于感知通信一体化的无人机蜂群状态联合估计方法 |
CN114567401A (zh) * | 2022-04-14 | 2022-05-31 | 中国人民解放***箭军工程大学 | 一种基于感知通信一体化的无人机蜂群状态联合估计方法 |
CN114623822A (zh) * | 2022-05-12 | 2022-06-14 | 东南大学 | 基于惯性约束的多波束水下地形组合匹配方法 |
CN114993302A (zh) * | 2022-05-27 | 2022-09-02 | 中国人民解放军海军工程大学 | 基于多柔性节点的水下智能定位***和方法 |
CN114993302B (zh) * | 2022-05-27 | 2024-05-28 | 中国人民解放军海军工程大学 | 基于多柔性节点的水下智能定位***和方法 |
CN115079227A (zh) * | 2022-07-26 | 2022-09-20 | 武汉优米捷光电子制造有限责任公司 | 基于改进无迹卡尔曼滤波的自旋弹组合导航方法 |
CN115112154A (zh) * | 2022-08-30 | 2022-09-27 | 南开大学 | 一种水下自主导航定位***的标定方法 |
CN115574816A (zh) * | 2022-11-24 | 2023-01-06 | 东南大学 | 仿生视觉多源信息智能感知无人平台 |
CN115574816B (zh) * | 2022-11-24 | 2023-03-14 | 东南大学 | 仿生视觉多源信息智能感知无人平台 |
CN115855049A (zh) * | 2023-02-07 | 2023-03-28 | 河海大学 | 基于粒子群优化鲁棒滤波的sins/dvl导航方法 |
CN117553787A (zh) * | 2024-01-09 | 2024-02-13 | 湖南大学无锡智能控制研究院 | 水下无人航行器的协同导航方法、装置及*** |
CN117553787B (zh) * | 2024-01-09 | 2024-03-26 | 湖南大学无锡智能控制研究院 | 水下无人航行器的协同导航方法、装置及*** |
CN117606491A (zh) * | 2024-01-24 | 2024-02-27 | 中国海洋大学 | 一种自主式水下航行器的组合定位导航方法及装置 |
CN117606491B (zh) * | 2024-01-24 | 2024-04-26 | 中国海洋大学 | 一种自主式水下航行器的组合定位导航方法及装置 |
Also Published As
Publication number | Publication date |
---|---|
US11754400B2 (en) | 2023-09-12 |
US20220404152A1 (en) | 2022-12-22 |
CN112254718B (zh) | 2024-04-09 |
CN112254718A (zh) | 2021-01-22 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
WO2022028286A1 (zh) | 一种运动约束辅助的基于改进Sage-Husa自适应滤波的水下组合导航方法 | |
WO2020062791A1 (zh) | 一种深海潜航器的sins/dvl水下抗晃动对准方法 | |
CN109724599B (zh) | 一种抗野值的鲁棒卡尔曼滤波sins/dvl组合导航方法 | |
CN103743395B (zh) | 一种惯性重力匹配组合导航***中时间延迟的补偿方法 | |
CN107797125B (zh) | 一种减小深海探测型auv导航定位误差的方法 | |
CN105891863B (zh) | 一种基于高度约束的扩展卡尔曼滤波定位方法 | |
CN109029454A (zh) | 一种基于卡尔曼滤波的横坐标系捷联惯导***阻尼算法 | |
CN109596144B (zh) | Gnss位置辅助sins行进间初始对准方法 | |
CN104316045A (zh) | 一种基于sins/lbl的auv水下交互辅助定位***及定位方法 | |
CN110794409B (zh) | 一种可估计未知有效声速的水下单信标定位方法 | |
CN108931791A (zh) | 卫惯紧组合钟差修正***和方法 | |
CN106840211A (zh) | 一种基于kf和stupf组合滤波的sins大方位失准角初始对准方法 | |
CN110940340A (zh) | 基于小型uuv平台的多传感器信息融合方法 | |
CN112747748A (zh) | 一种基于逆向解算的领航auv导航数据后处理方法 | |
CN111982105B (zh) | 一种基于sins/lbl紧组合的水下导航定位方法及*** | |
CN112798016A (zh) | 一种基于sins与dvl组合的auv行进间快速初始对准方法 | |
CN104061930A (zh) | 基于捷联惯性制导和多普勒计程仪的导航方法 | |
Allotta et al. | Localization algorithm for a fleet of three AUVs by INS, DVL and range measurements | |
CN110873813B (zh) | 一种水流速度估算方法、组合导航方法及装置 | |
CN116222578B (zh) | 基于自适应滤波和最优平滑的水下组合导航方法及*** | |
CN114459476B (zh) | 基于虚拟速度量测的水下无人潜航器测流dvl/sins组合导航方法 | |
CN103616026A (zh) | 一种基于h∞滤波的auv操纵模型辅助捷联惯导组合导航方法 | |
CN113670303B (zh) | 一种基于rbf神经网络的sins/dvl组合导航流速补偿方法 | |
Jørgensen et al. | IMU Calibration and Validation in a Factory, Remote on Land and at Sea | |
CN103697887A (zh) | 基于捷联惯性制导和多普勒计程仪的优化导航方法 |
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: 21853556 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: 21853556 Country of ref document: EP Kind code of ref document: A1 |
|
122 | Ep: pct application non-entry in european phase |
Ref document number: 21853556 Country of ref document: EP Kind code of ref document: A1 |