CN117367410A - State estimation method, unmanned underwater vehicle and computer readable storage medium - Google Patents
State estimation method, unmanned underwater vehicle and computer readable storage medium Download PDFInfo
- Publication number
- CN117367410A CN117367410A CN202311645357.0A CN202311645357A CN117367410A CN 117367410 A CN117367410 A CN 117367410A CN 202311645357 A CN202311645357 A CN 202311645357A CN 117367410 A CN117367410 A CN 117367410A
- Authority
- CN
- China
- Prior art keywords
- underwater vehicle
- unmanned underwater
- data
- course angle
- state estimation
- 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.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 71
- 238000005259 measurement Methods 0.000 claims abstract description 42
- 238000012216 screening Methods 0.000 claims abstract description 33
- 238000001914 filtration Methods 0.000 claims abstract description 18
- 230000003044 adaptive effect Effects 0.000 claims abstract description 12
- 239000011159 matrix material Substances 0.000 claims description 45
- 230000000694 effects Effects 0.000 abstract description 8
- 230000008569 process Effects 0.000 abstract description 6
- 230000007704 transition Effects 0.000 description 6
- 238000009825 accumulation Methods 0.000 description 3
- 238000004364 calculation method Methods 0.000 description 3
- 230000001960 triggered effect Effects 0.000 description 3
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 description 3
- 230000001133 acceleration Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 2
- 230000008859 change Effects 0.000 description 2
- 238000006243 chemical reaction Methods 0.000 description 2
- 238000012937 correction Methods 0.000 description 2
- 230000006870 function Effects 0.000 description 2
- 230000010354 integration Effects 0.000 description 2
- 230000007774 longterm Effects 0.000 description 2
- 239000002245 particle Substances 0.000 description 2
- 230000006978 adaptation Effects 0.000 description 1
- 238000004458 analytical method Methods 0.000 description 1
- 238000013473 artificial intelligence Methods 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 238000004590 computer program Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000000593 degrading effect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 238000006073 displacement reaction Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000011835 investigation Methods 0.000 description 1
- 238000012423 maintenance Methods 0.000 description 1
- 238000013178 mathematical model Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 230000008447 perception Effects 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 238000005070 sampling Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 238000012546 transfer Methods 0.000 description 1
- 230000001131 transforming effect Effects 0.000 description 1
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/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
- G01C21/203—Specially adapted for sailing ships
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention discloses a state estimation method, an unmanned underwater vehicle and a computer readable storage medium, wherein the method comprises the following steps: acquiring measurement data of an unmanned underwater vehicle; acquiring first course angle data of an unmanned underwater vehicle; setting a self-adaptive event triggering condition, and screening the measurement data according to the self-adaptive event triggering condition to obtain a screening coefficient; and carrying out state estimation on the unmanned underwater vehicle according to the measurement data, the first course angle data, the screening coefficient and the extended Kalman filtering method. By filtering the measurement data by combining the adaptive event with the extended Kalman filtering method, the sensor can quickly process the measurement data when receiving interference, a filtering effect with a good effect is obtained, and meanwhile, the state estimation precision of the unmanned underwater vehicle is improved.
Description
Technical Field
The invention relates to the technical field of autonomous aircrafts, in particular to a state estimation method, an unmanned underwater vehicle and a computer-readable storage medium.
Background
At present, unmanned underwater vehicles play an important role in civil and military marine operations such as underwater structure maintenance, marine science investigation and the like. With the rapid development of unmanned platforms and artificial intelligence, there is an increasing interest in using unmanned underwater vehicles for more complex and more durable marine services, so that unmanned underwater vehicles need to have long-term stable and efficient autonomous capability when performing tasks, and have accurate perception of their own states. Currently, underwater ultra-short baseline acoustic positioning systems (Ultra Short Base Line, USBL), doppler velocimeters (Doppler Velocity Log, DVL) and inertial navigation systems (Inertial Navigation System, INS) are common important components in unmanned underwater vehicle integrated navigation positioning systems. Due to the complexity of the underwater environment and the characteristics of the sensors, there are great challenges to the state estimation of unmanned underwater vehicles.
The inertial measurement units (Inertial Measurement Unit, IMU) in INS can drift under long-term operation, heading measurement can have accumulated errors, and the observed data of each sensor can have large noise and errors under certain emergency conditions, so that the data needs to be filtered. Conventional filters such as mean filter, median filter, kalman Filter (KF), extended Kalman Filter (EKF) and Particle Filter (PF) have respective limitations and cannot solve the problems of the prior art.
Disclosure of Invention
In order to overcome the defects in the prior art, the invention provides a state estimation method, an unmanned underwater vehicle and a computer readable storage medium.
The technical scheme adopted for solving the technical problems is as follows:
in a first aspect, an embodiment of the present invention provides a state estimation method for an unmanned underwater vehicle, the method including:
acquiring measurement data of the unmanned underwater vehicle;
acquiring first course angle data of the unmanned underwater vehicle;
setting a self-adaptive event triggering condition, and screening the measurement data according to the self-adaptive event triggering condition to obtain a screening coefficient;
performing state estimation on the unmanned underwater vehicle according to the measurement data, the first course angle data, the screening coefficient and an extended Kalman filtering method;
the performing state estimation on the unmanned underwater vehicle according to the measurement data, the first course angle data, the screening coefficient and an extended kalman filtering method includes:
constructing a state vector of the unmanned underwater vehicle according to the position information, the speed information and the course angle information of the unmanned underwater vehicle;
establishing a state prediction equation and a covariance prediction equation of the unmanned underwater vehicle according to the state vector;
constructing an observation equation according to the measurement data and the first course angle data, and obtaining an observation actual value and an observation predicted value;
calculating an extended Kalman gain according to the state prediction equation, the covariance prediction equation, the observation equation and the screening coefficient;
updating the state prediction equation and the covariance prediction equation according to the extended Kalman gain to obtain a state estimation vector of the unmanned underwater vehicle;
the extended Kalman gain is calculated by the formulaCalculated, said->For the extended kalman gain factor at time k, and (2)>The prediction covariance matrix at the moment k is shown as a prediction covariance matrix, H is shown as an observation matrix, R is shown as an observation noise matrix, I is shown as an identity matrix, and the number of the observation noise matrix is +.>For the screening of coefficients->Approaching ≡.
In some embodiments, the unmanned underwater vehicle comprises a USBL transducer, a DVL, and the acquiring the measurement data of the unmanned underwater vehicle comprises:
acquiring position data of the unmanned underwater vehicle through the USBL transducer;
and acquiring speed data of the unmanned underwater vehicle through the DVL.
In some embodiments, the unmanned underwater vehicle further comprises an IMU, the acquiring first heading angle data of the unmanned underwater vehicle comprises:
acquiring second course angle data of the unmanned underwater vehicle through the IMU;
calculating to obtain third course angle data through the position data and the speed data;
and comparing the second course angle data with the third course angle data to correct the second course angle data so as to obtain the first course angle data.
In some embodiments, the method further comprises:
before calculating the extended Kalman gain, a Jacobian matrix of the state prediction equation is calculated.
In some embodiments, the state estimation vector is represented by the formulaCalculated, said->For the state estimation vector at time k +.>For the state vector at time k, which is presumed from the state vector at time k-1,/for the state vector at time k>For the extended kalman gain factor at time k, and (2)>For observing the actual value +.>To observe the predicted value.
In some embodiments, the setting the adaptive event triggering condition includes:
setting a threshold value of the self-adaptive event;
and determining a screening coefficient based on the threshold value, the residual error of the observed predicted value and the observed actual value.
In some embodiments, the threshold value is calculated by the formulaCalculated, said->Is maximum a priori threshold->For observing the actual value +.>For observing predictive value, ++>Is a threshold value.
In some embodiments, the filter coefficients pass the formulaCalculated, said->For the screening of coefficients->For observing the actual value +.>For observing predictive value, ++>Is a threshold value.
In a second aspect, an embodiment of the present invention provides an unmanned underwater vehicle, the unmanned underwater vehicle including a fuselage, sensors including a USBL transducer, a DVL, and an IMU, and a controller including:
at least one processor; the method comprises the steps of,
a memory communicatively coupled to the at least one processor; wherein,
the memory stores instructions executable by the at least one processor to enable the at least one processor to perform the method as described in any one of the embodiments above.
In a third aspect, embodiments of the present invention also provide a non-transitory computer-readable storage medium storing computer-executable instructions that, when executed by an unmanned underwater vehicle, cause the unmanned underwater vehicle to perform a method as described in any of the embodiments above.
Compared with the prior art, the invention has at least the following beneficial effects:
the invention provides a state estimation method based on an unmanned underwater vehicle, which is used for obtaining third course angle data through data estimation by combining a USBL transducer and a DVL sensor, comparing the third course angle data with second course angle data measured by an IMU sensor, correcting the second course angle data, and obtaining first course angle data, thereby improving the drift problem of course measurement. The state estimation is carried out on the unmanned underwater vehicle by combining the self-adaptive event, the extended Kalman filtering method and the corrected first course angle data, so that the accurate estimation on the state of the unmanned underwater vehicle can be realized.
Drawings
The invention will be further described with reference to the drawings and examples.
FIG. 1 is a flow chart of one embodiment of a state estimation method of the present invention;
FIG. 2 is a graph of second heading angle data in one embodiment of a state estimation method of the present invention;
FIG. 3 is a graph of first heading angle data in one embodiment of a state estimation method of the present invention;
FIG. 4 is a graph of longitudinal position data in one embodiment of a state estimation method of the present invention;
FIG. 5 is a graph of lateral position data in one embodiment of the state estimation method of the present invention;
FIG. 6 is a graph of longitudinal position data estimated based on a state estimation method in one embodiment of the state estimation method of the present invention;
FIG. 7 is a graph of lateral position data estimated based on a state estimation method in one embodiment of the state estimation method of the present invention;
FIG. 8 is a graph of longitudinal velocity data estimated based on a state estimation method in one embodiment of the state estimation method of the present invention;
FIG. 9 is a graph of lateral velocity data estimated based on a state estimation method in one embodiment of the state estimation method of the present invention.
Detailed Description
The conception, specific structure, and technical effects produced by the present invention will be clearly and completely described below with reference to the embodiments and the drawings to fully understand the objects, features, and effects of the present invention. It is apparent that the described embodiments are only some embodiments of the present invention, but not all embodiments, and that other embodiments obtained by those skilled in the art without inventive effort are within the scope of the present invention based on the embodiments of the present invention. It should be further noted that, if not conflicting, the various features of the embodiments of the present application may be combined with each other, which is within the protection scope of the present application.
In addition, while functional block division is performed in a device diagram and logical order is shown in a flowchart, in some cases, the steps shown or described may be performed in a different order than the block division in the device, or in the flowchart. Moreover, the words "first," "second," "third," and the like as used herein do not limit the data and order of execution, but merely distinguish between identical or similar items that have substantially the same function and effect.
Currently, underwater ultra-short baseline acoustic positioning systems (Ultra Short Base Line, USBL), doppler velocimeters (Doppler Velocity Log, DVL) and inertial navigation systems (Inertial Navigation System, INS) are common important components in unmanned underwater vehicle (Unmanned Underwater Vehicle, UUV) integrated navigational positioning systems. The inertial measurement unit (Inertial Measurement Unit, IMU) is arranged in the INS, the IMU is a sensor for measuring the three-axis attitude angle and acceleration of an object, the IMU can generate drift problem under long-time work, and accumulated errors can exist in course measurement. In addition, the observed data of each sensor has large noise and error in some emergency situations, and filtering processing is needed to be carried out on the data. Conventional filters such as mean filter, median filter, kalman Filter (KF), extended Kalman Filter (EKF) and Particle Filter (PF) have respective limitations and cannot solve the problems of the prior art. Therefore, there is a certain deviation in the estimation of the state of the unmanned underwater vehicle.
In order to improve the state estimation precision of the unmanned underwater vehicle, the invention provides a state estimation method which is used for the unmanned underwater vehicle, wherein data estimation of a USBL transducer and a DVL sensor are combined to obtain third course angle data, the third course angle data is compared with second course angle data measured by an IMU sensor, the second course angle data is corrected, and first course angle data is obtained, so that the drift problem of course measurement is improved. And carrying out state estimation on the unmanned underwater vehicle by combining the self-adaptive event triggering condition, the extended Kalman filtering method and the corrected first course angle data, thereby being beneficial to improving the robustness of the state estimation of the unmanned underwater vehicle.
Referring to fig. 1, fig. 1 is a flow chart of a state estimation method according to an embodiment of the invention.
As shown in fig. 1, the state estimation method includes:
step S101: and acquiring measurement data of the unmanned underwater vehicle.
It will be appreciated that unmanned underwater vehicles are particularly devices capable of autonomously or controllably performing underwater tasks under water, which are typically unmanned, have autonomous navigation and communication functions, and are capable of performing various tasks under water, such as reconnaissance, surveillance, subsea exploration, rescue, and the like. Since unmanned underwater vehicles are unmanned, various sensors are required to measure their states for unmanned underwater vehicles.
In the embodiment of the invention, the measurement data is obtained by measuring the state of the unmanned underwater vehicle through the sensor on the unmanned underwater vehicle. The measurement data includes position data and speed data required in making unmanned underwater vehicle state estimates.
Specifically, in this embodiment, the unmanned underwater vehicle includes a USBL transducer and a DVL. The step S101 includes:
step S1011: position data of the unmanned underwater vehicle are acquired through the USBL transducer.
The USBL is a high-precision underwater sonar positioning technology, and can accurately position an underwater target. The USBL comprises a transponder arranged in the deep sea and a USBL transducer arranged on the unmanned underwater vehicle, the USBL transducer arranged on the unmanned underwater vehicle sends out a series of sound wave signals, the transponder returns to the receiving matrix after receiving the sound wave signals sent by the USBL transducer, the position data of the unmanned underwater vehicle are calculated, and the position data of the unmanned underwater vehicle can be obtained through the USBL transducer. The position data comprises longitudinal position data of the unmanned underwater vehicle under the navigation coordinate axisAnd lateral position data of the unmanned underwater vehicle in the navigation coordinate axis +.>。
Step S1012: and acquiring speed data of the unmanned underwater vehicle through the DVL.
Among these, DVL is a sonar device for measuring velocity relative to the water bottom, which uses the doppler effect of acoustic signals to measure velocity. In principle, the measurement of the DVL is its own speed, and in general, the DVL is fixedly installed at the bottom of the unmanned underwater vehicle, and three axes of the D-system (DVL coordinate system, rightward, forward and upward) are parallel to three axes of the carrier coordinate system when installed, so that the measurement of the DVL is the speed of the unmanned underwater vehicle, and speed data of the unmanned underwater vehicle can be acquired through the DVL. The speed data comprises longitudinal speed data of the unmanned underwater vehicle under the carrier coordinate axisAnd transverse speed data of the unmanned underwater vehicle in the carrier coordinate axis +.>。
Step S102: acquiring first course angle data of an unmanned underwater vehicle;
the first course angle data is the course angle data closest to the actual course angle of the unmanned underwater vehicle. In this embodiment, the unmanned underwater vehicle further includes an IMU, which is a sensor for measuring the three-axis attitude angle and acceleration of an object, and generally includes a gyroscope and an accelerometer, where the gyroscope outputs a rotation change rate, so that in the conversion process, time integration of the output of the gyroscope is required to obtain orientation information, and errors are continuously accumulated as time is accumulated in the integration process, so that finally, the orientation is deviated. Therefore, the IMU can generate drift problems under long-time operation, and heading measurement can have accumulated errors.
In this embodiment, the heading angle data acquired through the IMU is referred to as second heading angle data, and as time is accumulated, error accumulation occurs in the second heading angle data, which cannot correctly reflect the state of the unmanned underwater vehicle about the heading angle. Referring to fig. 2, fig. 2 is a second heading angle data graph. From this graph, it can be observed that, as time goes by, the error between the heading angle data (i.e., the second heading angle data) acquired by the IMU and the real heading angle data becomes larger and larger, and a drift phenomenon occurs. The first course angle data is obtained through comparing and correcting the measured data with the second course angle, and the state of the unmanned underwater vehicle about the course angle can be accurately reflected to a certain extent.
Specifically, step S102 includes:
step S1021: and acquiring second course angle data of the unmanned underwater vehicle through the IMU.
The second course angle data isAnd is measured by a sensor IMU.
Step S1022: and calculating to obtain third course angle data through the position data and the speed data.
The displacement direction and the speed direction are consistent in a short time, so that the speed data of the unmanned underwater vehicle under the navigation coordinate axis can be calculated by using the position data of two similar sampling moments acquired by USBL, and then the following steps are:
wherein,for the transverse speed data of the unmanned underwater vehicle at the moment k in the navigation coordinate axis,/for the unmanned underwater vehicle>The longitudinal speed data of the unmanned underwater vehicle at the moment k under the navigation coordinate axis are obtained; />For the transverse position data of the unmanned underwater vehicle at the moment k in the navigation coordinate axis,/>The longitudinal position data of the unmanned underwater vehicle at the moment k under the navigation coordinate axis are obtained;for the transverse position data of the unmanned underwater vehicle at the moment k-1 in the navigation coordinate axis, +.>The longitudinal position data of the unmanned underwater vehicle at the moment k-1 in the navigation coordinate axis are obtained; />The time variation from time k-1 to time k. Position data +.>、/>、/>And->Determination of event changes by USBL acquisitionQuantity of transformation->The speed data in the navigation coordinate axis can be +.>、/>And calculating to obtain speed data of the unmanned underwater vehicle at the moment k under the navigation coordinate axis.
The conversion relation between the navigation coordinate axis and the carrier coordinate axis according to the speed can be obtained:
from this calculation it is possible to:。
wherein,is the third heading angle data. And calculating the third course angle data by using the speed data of the unmanned underwater vehicle in the navigation coordinate axis obtained by calculating the position data obtained by the USBL and the speed data of the unmanned underwater vehicle in the carrier coordinate axis obtained by the DVL.
Step S1023: and comparing the second course angle data with the third course angle data to correct the second course angle data so as to obtain the first course angle data.
Comparing the second course angle data with the third course angle data, and continuously adopting the second course angle data when the error between the second course angle data and the third course angle data is in a certain range; and correcting the second course angle when the error between the second course angle data and the third course angle exceeds a certain range. The comparison correction formula is as follows:
wherein,for the first heading angle data, < >>For the second heading angle data, +.>And M is a threshold value for the third course angle data. The threshold value M is set according to the actual error range requirement, and when the second course angle data is +>And third heading angle data->The square of the deviation is smaller than or equal to a threshold value M, and the second course angle data is continuously used as data reflecting the course angle state of the unmanned underwater vehicle; when the second course angle data->And third heading angle data->And if the square of the deviation is greater than or equal to the threshold value M, using the third course angle data as data reflecting the course angle state of the unmanned underwater vehicle. Referring to fig. 3, fig. 3 is a graph of first heading angle data. When the deviation between the second course angle data and the third course angle data is smaller than a certain value, continuing to use the second course angle data as data reflecting the state of the course angle of the unmanned underwater vehicle; when the deviation between the second course angle data and the third course angle data exceeds a certain value, correcting the second course angle data, and taking the corrected data as course angle data of the unmanned underwater vehicle at the current moment. Therefore, the error between the first course angle data and the real course angle data is always within the allowable error range, so as to solve the problem that the accumulated error exists in course measurementThe questions are given.
Step S103: setting a self-adaptive event triggering condition, and screening the measurement data according to the self-adaptive event triggering condition to obtain a screening coefficient.
The measurement data are obtained by measuring the unmanned underwater vehicle through the sensor, but in an emergency, the measurement data have large noise and errors. An adaptive event is an event that is automatically triggered according to a specific scenario and requirement, and can automatically identify and analyze measurement data, and respond and process according to analysis results. The measurement data are data which are required to be used when the unmanned underwater vehicle performs state estimation, in order to improve the precision of the unmanned underwater vehicle state estimation, adaptive event triggering conditions are set for noise and errors of the measurement data under emergency conditions, and the measurement data are screened according to the adaptive event triggering conditions, so that screening data are obtained. The noise and error of the measured data are larger than the screening data obtained when the noise and error of the measured data are smaller.
Step S104: and carrying out state estimation on the unmanned underwater vehicle according to the measurement data, the first course angle data, the screening coefficient and the extended Kalman filtering method.
In this embodiment, step S104 includes:
step S1041: and constructing a state vector of the unmanned underwater vehicle according to the position information, the speed information and the course angle information of the unmanned underwater vehicle.
Constructing a state vector of the unmanned underwater vehicle by using information required for estimating the state of the unmanned underwater vehicle, wherein the state vector is expressed in the following form:
wherein x is a state vector of the unmanned underwater vehicle,for the longitudinal position information of the unmanned underwater vehicle in the navigation coordinate axis,/for the unmanned underwater vehicle>For the lateral position information of the unmanned underwater vehicle in the navigation coordinate axis,/for the unmanned underwater vehicle>For longitudinal speed information of unmanned underwater vehicle under carrier coordinate axis,/->For the lateral speed information of the unmanned underwater vehicle under the carrier coordinate axis,/-for the unmanned underwater vehicle>Is course angle information of the unmanned underwater vehicle.
Step S1042: and establishing a state prediction equation and a covariance prediction equation of the unmanned underwater vehicle according to the state vector.
It will be appreciated that the state prediction equation of an unmanned underwater vehicle is an important mathematical model describing how the unmanned underwater vehicle changes its position, velocity and attitude over time under external forces. In the unmanned underwater vehicle control system, a state prediction equation may be used to predict the future state of the unmanned underwater vehicle.
Before a state prediction equation is established, a motion model of the unmanned underwater vehicle is established. In this embodiment, the motion model of the unmanned underwater vehicle is:
the motion model is derived by analyzing the kinematics and dynamics model of the unmanned underwater vehicle, and represents that the state at the next moment is obtained by adding the variable quantity and the state noise to the current state. Wherein,for the state vector of the unmanned underwater vehicle at time k-1>For the state vector of the unmanned underwater vehicle at time k +.>For a rotation matrix from the carrier coordinate system to the navigation coordinate system,/a>For the speed matrix of the unmanned underwater vehicle at time k-1>Is at->Quality of degree of freedom->For the moment k-1 at +.>Thrust of degree of freedom->For the moment k-1 at +.>Coefficient of resistance of degree of freedom->For the time variation from moment k-1 to moment k,/>Is the state noise at time k-1.
The state prediction equation of the unmanned underwater vehicle can be written according to the motion model of the unmanned underwater vehicle:
wherein,representing rootPredicted state vector of unmanned underwater vehicle at time k based on state estimation at time k-1, +.>The state transition matrix is used for representing the transition relation from the state at the last moment to the state at the current moment; b is a control matrix>Is a control vector. According to the state prediction equation, the predicted state vector of the unmanned underwater vehicle at the current moment consists of the transition quantity of the state at the last moment and the change quantity of the state at the last moment.
However, the state at each instant has an uncertainty, and this uncertainty is typically represented by a covariance matrix P. In order for this uncertainty to pass between each instant, a covariance prediction equation needs to be established. In this embodiment, the covariance prediction equation is:
wherein,representing a predicted covariance matrix of the unmanned underwater vehicle at time k estimated from the covariance matrix at time k-1,/for the unmanned underwater vehicle at time k>Is a state transition matrix; q is a state noise matrix, which is noise brought by the covariance prediction model itself.
Since the motion of unmanned underwater vehicles is not generally linearly variable, the state transition matrix in the state prediction equation and the covariance prediction equation needs to be calculated before the extended Kalman gain is calculatedIs a jacobian matrix of (c).
In this embodiment, the state transition matrixThe jacobian matrix of (c) is:
step S1043: and constructing an observation equation according to the measurement data and the first course angle data and obtaining an observation value.
It will be appreciated that the observation equations for an unmanned underwater vehicle describe how the sensors measure the state of the unmanned underwater vehicle, including position, speed, heading angle, etc. The sensor measurement result obtained through the observation equation can be compared with the solution of the state prediction equation, so that the state estimation is carried out on the unmanned underwater vehicle.
The first course angle data are course angle data subjected to comparison and correction, and the state of the unmanned underwater vehicle about the course angle can be accurately reflected to a certain extent.
The observation equation is:
wherein,for observing the actual value +.>For observing matrix +.>For observing noise +.>To observe the predicted value. In this embodiment, H is an identity matrix.
The observed values were:
wherein,for longitudinal position data of unmanned underwater vehicle in navigation coordinate axis,/for the navigation coordinate axis>For the lateral position data of the unmanned underwater vehicle in the navigation coordinate axis,/for the unmanned underwater vehicle>For longitudinal speed data of unmanned underwater vehicle in carrier coordinate axis,/for unmanned underwater vehicle>For the lateral speed data of an unmanned underwater vehicle in the carrier coordinate axis,/>Is the first heading angle data. Referring to fig. 4, fig. 4 is a graph of longitudinal position data of an unmanned underwater vehicle in a navigation coordinate axis, and in some special cases, the data may have large noise and errors. Referring to fig. 5, fig. 5 is a graph of lateral position data of an unmanned underwater vehicle under a navigation coordinate axis, and in some special cases, the data may have larger noise and errors. When a similar large noise occurs, it is necessary to filter the unmanned underwater vehicle in order to ensure the accuracy of the state estimation thereof. In the invention, the adaptive event is used for filtering in combination with an extended Kalman filtering method.
Step S1044: and calculating the extended Kalman gain according to the state prediction equation, the covariance prediction equation, the observation equation and the screening coefficient.
And calculating the extended Kalman gain according to a screening coefficient obtained according to the self-adaptive event triggering condition, a state prediction equation, a covariance prediction equation and an observation equation in the extended Kalman filtering method.
In this embodiment, the extended kalman gain is:
wherein,for extending the Kalman gain coefficient, the coefficient is used to weigh the prediction covariance matrix +.>And covariance of observed quantity->To determine the weight between the predictive model and the observation model. />A predicted covariance matrix representing the unmanned underwater vehicle at time k estimated from the covariance at time k-1,/for the unmanned underwater vehicle at time k>For the observation matrix, R is the covariance matrix of the observation quantity, I is the identity matrix,/is the +.>For the screening of coefficients->Approaching ≡.
Wherein the screening coefficientObtained from adaptive events, which have the possibility of only two values of 0 and 1, when the filter coefficients +.>When 0, the extended Kalman gain factor +.>Then->Approaching infinity +.>Tending to infinity to obtain the extended Kalman gain factor +.>And the method tends to be infinitesimal, namely the noise variance is infinitesimal, and the actual observation value is unreliable. When screening coefficient->At 1, the extended Kalman gain factorAnd degrading into a traditional extended Kalman gain coefficient calculation equation.
Step S1045: and updating the state prediction equation and the covariance prediction equation according to the extended Kalman gain to obtain a state estimation value of the unmanned underwater vehicle.
Updating a state prediction equation according to the extended Kalman gain, wherein the state update formula is as follows:
wherein,state estimation vector representing unmanned underwater vehicle at the current moment,/->A predicted state vector representing the unmanned underwater vehicle at the present time obtained from the state estimation at the previous time,/->In order to extend the kalman gain factor,for observing the actual value +.>To be seen asAnd measuring a matrix. />Representing the residual between the actual observed actual value and the predicted observed predicted value. Extended Kalman gain factor->For transforming the representation of the residual from the observation domain to the state domain and determining the weights between the prediction model and the observation model, the residual being less weighted if the prediction model is believed to be more; if the observation model is believed to be more, the weight of the residual error is larger; if the noise is too large, the actual observation value is not credible, and the weight of the residual error is infinitely small. Referring to fig. 6 to 9, there are a longitudinal position data graph estimated based on a state estimation method, a lateral position data graph estimated based on a state estimation method, a longitudinal speed data graph estimated based on a state estimation method, and a lateral speed data graph estimated based on a state estimation method, respectively. The error between the estimated data and the real data is smaller as can be seen from fig. 6 to 9, it can be seen that the measured data is filtered by combining the adaptive event with the extended kalman filtering method, so that the measured data can be rapidly processed when the sensor receives the interference, a filtering effect with a good effect is obtained, and meanwhile, the state estimation precision of the unmanned underwater vehicle is improved.
Updating a covariance prediction equation according to the extended Kalman gain, wherein a state updating formula is as follows:
wherein,is the current covariance matrix, which represents the noise distribution of the best estimate, left for use in the next iteration; />For expanding the Kalman gain factor, +.>For observing matrix +.>And (3) representing a predicted covariance matrix of the unmanned underwater vehicle at the time k obtained by covariance estimation at the time k-1. In this iteration, the uncertainty of the noise is reduced due to the covariance matrix, but in the next iteration, the uncertainty of the noise is increased due to the introduction of the transfer noise. It is therefore necessary to update the covariance prediction equation by extending the kalman gain coefficient.
In the present embodiment, step S103: setting a self-adaptive event triggering condition, and screening the measurement data according to the self-adaptive event triggering condition to obtain a screening coefficient. It comprises the following steps:
step S1031: a threshold value of the adaptive event is set.
In this embodiment, the threshold setting formula of the adaptive event is:
wherein,is maximum a priori threshold->For observations->For predictive value +.>Is a threshold value. Maximum a priori threshold->The size of (2) is set according to the actual situation and the requirement, when the actual value is observed +.>And observe predictive value->The difference between them is larger, threshold value +.>Then decrease; when observing the actual value +.>And observe predictive value->The difference between them is small, threshold value +.>Is close to the maximum a priori threshold +.>. Namely, when observing the actual value +.>The less noise of (2), the threshold value +.>The higher; when observing the actual value +.>The greater the noise of (2), the threshold value +.>The lower the easier it is to trigger an adaptation event.
Step S1032: and determining a screening coefficient based on the threshold value, the residual error of the observed actual value and the observed predicted value.
In this embodiment, the adaptive event triggering conditions are:
wherein,for the screening of coefficients->Is threshold value (S)>For observing the actual value +.>To observe the predicted value. Due to threshold +.>Is a one-dimensional value, while the actual value is observed +>And observe predictive value->Is a matrix by observing the actual value +.>And observe predictive value->Multiplying the subtracted matrix by the transposed matrix of the matrix to obtain a matrix capable of representing the observed actual value +.>And observe predictive value->A one-dimensional value of the difference between the two values, and a threshold value +.>For comparison. When the value is smaller than the threshold value +.>Indicating that the actual value is observed +.>And observe predictive value->The difference between them is small, the actual value is observed +.>There is no large noise, the coefficients are filtered +.>Setting as 1; when the value is greater than or equal to the threshold value +.>Indicating that the actual value is observed +.>And observe predictive value->The difference between them is large, the actual value is observed +.>If there is a large noise, i.e. there is a large noise and error in the observed data of the sensor, the coefficients will be filtered>Set to 0.
In summary, in the prior art, the position data and the speed data acquired by the unmanned underwater vehicle through the USBL and the DVL may generate larger noise and error under an emergency, and the heading angle data acquired by the IMU may generate a drift phenomenon along with accumulation of time, so that the state estimation accuracy of the unmanned underwater vehicle is affected to a certain extent.
According to the state estimation method provided by the embodiment of the invention, the position data and the speed data of the unmanned underwater vehicle are obtained through USBL and DVL, the third course angle data is obtained through IMU, the second course angle data of the unmanned underwater vehicle is obtained through IMU, the third course angle data and the second course angle data are compared and corrected, the first course angle data are obtained, and the first course angle data are used as the current course angle data of the unmanned underwater vehicle, so that the problem that drift phenomenon is generated due to accumulation of the course angle data obtained through IMU along with time is solved.
And establishing a self-adaptive event triggering condition, a state prediction equation and an observation equation, forming an observation actual value by using position data and speed data of the unmanned underwater vehicle and first course angle data obtained through USBL and DVL, obtaining an observation predicted value through the state prediction equation and the observation equation, and obtaining a predicted state vector through the state prediction equation. When the residual error between the observed actual value and the observed predicted value is larger, the observed actual value has larger noise, and the self-adaptive event is triggered to screen the coefficientSetting 1, so that the extended Kalman gain coefficient tends to be infinitesimal, and the state estimation vector of the unmanned underwater vehicle at the current moment is consistent with the predicted state vector; when the residual error between the observed actual value and the observed predicted value is smaller, the observed actual value is normal, and the self-adaptive event is triggered to screen the coefficient +.>And setting the state estimation vector to be 0, and calculating the extended Kalman gain coefficient according to a traditional extended Kalman gain coefficient calculation equation, wherein the state estimation vector of the unmanned underwater vehicle at the current moment is the optimal state estimation vector. When the observed actual value has larger noise, the state estimation vector and the predicted state vector of the unmanned underwater vehicle at the current moment are kept consistent; when the observed actual value is normal, the unmanned underwater vehicle state estimation is carried out on the current moment according to the extended Kalman filtering method, so that the robustness of the unmanned underwater vehicle state estimation is improved.
The embodiment of the invention provides an unmanned underwater vehicle, which comprises a body, a sensor and a controller, wherein the sensor comprises a USBL transducer, a DVL and an IMU, and the controller comprises: at least one processor; and a memory communicatively coupled to the at least one processor. Wherein the memory stores instructions executable by the at least one processor to enable the at least one processor to perform the state estimation method of any of the embodiments described above.
The embodiment of the invention also provides a non-volatile computer readable storage medium, wherein the computer readable storage medium stores computer executable instructions, which when executed by the unmanned underwater vehicle, enable the unmanned underwater vehicle to execute the state estimation method in any of the above embodiments.
The apparatus embodiments described above are merely illustrative, wherein the elements illustrated as separate elements may or may not be physically separate, and the elements shown as elements may or may not be physical elements, may be located in one place, or may be distributed over a plurality of network elements. Some or all of the modules may be selected according to actual needs to achieve the purpose of the solution of this embodiment.
From the above description of embodiments, it will be apparent to those skilled in the art that the embodiments may be implemented by means of software plus a general purpose hardware platform, but may also be implemented by means of hardware. Those skilled in the art will appreciate that all or part of the processes implementing the methods of the above embodiments may be implemented by a computer program for instructing relevant hardware, where the program may be stored in a computer readable storage medium, and where the program may include processes implementing the embodiments of the methods described above. The storage medium may be a magnetic disk, an optical disk, a Read Only Memory (ROM), a random access Memory (Random Access Memory, RAM), or the like.
Finally, it should be noted that: the above embodiments are only for illustrating the technical solution of the present invention, and are not limiting; the technical features of the above embodiments or in the different embodiments may also be combined within the idea of the invention, the steps may be implemented in any order, and there are many other variations of the different aspects of the invention as described above, which are not provided in detail for the sake of brevity; although the invention has been described in detail with reference to the foregoing embodiments, it will be understood by those of ordinary skill in the art that: the technical scheme described in the foregoing embodiments can be modified or some technical features thereof can be replaced by equivalents; such modifications and substitutions do not depart from the spirit of the invention.
Claims (10)
1. A method of state estimation for an unmanned underwater vehicle, the method comprising:
acquiring measurement data of the unmanned underwater vehicle;
acquiring first course angle data of the unmanned underwater vehicle;
setting a self-adaptive event triggering condition, and screening the measurement data according to the self-adaptive event triggering condition to obtain a screening coefficient;
performing state estimation on the unmanned underwater vehicle according to the measurement data, the first course angle data, the screening coefficient and an extended Kalman filtering method;
the performing state estimation on the unmanned underwater vehicle according to the measurement data, the first course angle data, the screening coefficient and an extended kalman filtering method includes:
constructing a state vector of the unmanned underwater vehicle according to the position information, the speed information and the course angle information of the unmanned underwater vehicle;
establishing a state prediction equation and a covariance prediction equation of the unmanned underwater vehicle according to the state vector;
constructing an observation equation according to the measurement data and the first course angle data, and obtaining an observation actual value and an observation predicted value;
calculating an extended Kalman gain according to the state prediction equation, the covariance prediction equation, the observation equation and the screening coefficient;
updating the state prediction equation and the covariance prediction equation according to the extended Kalman gain to obtain a state estimation vector of the unmanned underwater vehicle;
the extended Kalman gain is calculated by the formulaCalculated, said->For the extended kalman gain factor at time k, and (2)>The prediction covariance matrix at the moment k is shown as a prediction covariance matrix, H is shown as an observation matrix, R is shown as an observation noise matrix, I is shown as an identity matrix, and the number of the observation noise matrix is +.>For the screening of coefficients->Approaching ≡.
2. The method of claim 1, wherein the unmanned underwater vehicle comprises a USBL transducer, a DVL, and wherein the acquiring the measurement data of the unmanned underwater vehicle comprises:
acquiring position data of the unmanned underwater vehicle through the USBL transducer;
and acquiring speed data of the unmanned underwater vehicle through the DVL.
3. The method of claim 2, wherein the unmanned underwater vehicle further comprises an IMU, and wherein the acquiring the first heading angle data of the unmanned underwater vehicle comprises:
acquiring second course angle data of the unmanned underwater vehicle through the IMU;
calculating to obtain third course angle data through the position data and the speed data;
and comparing the second course angle data with the third course angle data to correct the second course angle data so as to obtain the first course angle data.
4. The state estimation method according to claim 1, characterized in that the method further comprises:
before calculating the extended Kalman gain, a Jacobian matrix of the state prediction equation is calculated.
5. The state estimation method of claim 1, wherein the state estimation vector is represented by the formulaCalculated, said->For the state estimation vector at time k +.>For the state vector at time k, which is presumed from the state vector at time k-1,/for the state vector at time k>For the extended kalman gain factor at time k, and (2)>For observing the actual value +.>To observe the predicted value.
6. The state estimation method according to claim 1, wherein the setting an adaptive event trigger condition includes:
setting a threshold value of the self-adaptive event;
and determining a screening coefficient based on the threshold value, the residual error of the observed predicted value and the observed actual value.
7. The state estimation method of claim 6, wherein the threshold value is calculated by the formulaCalculated, said->Is maximum a priori threshold->For observing the actual value +.>For observing predictive value, ++>Is a threshold value.
8. The state estimation method of claim 6, wherein the filter coefficients pass the formulaCalculated, said->For the screening of coefficients->For observing the actual value +.>For observing predictive value, ++>Is a threshold value.
9. An unmanned underwater vehicle, the unmanned underwater vehicle comprising a fuselage, sensors and a controller, the sensors comprising a USBL transducer, a DVL and an IMU, the controller comprising:
at least one processor; the method comprises the steps of,
a memory communicatively coupled to the at least one processor; wherein,
the memory stores instructions executable by the at least one processor to enable the at least one processor to perform the method of any one of claims 1-8.
10. A non-transitory computer readable storage medium storing computer executable instructions which, when executed by an unmanned underwater vehicle, cause the unmanned underwater vehicle to perform the method of any of claims 1-8.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311645357.0A CN117367410B (en) | 2023-12-04 | 2023-12-04 | State estimation method, unmanned underwater vehicle and computer readable storage medium |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311645357.0A CN117367410B (en) | 2023-12-04 | 2023-12-04 | State estimation method, unmanned underwater vehicle and computer readable storage medium |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN117367410A true CN117367410A (en) | 2024-01-09 |
| CN117367410B CN117367410B (en) | 2024-03-19 |
Family
ID=89402671
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202311645357.0A Active CN117367410B (en) | 2023-12-04 | 2023-12-04 | State estimation method, unmanned underwater vehicle and computer readable storage medium |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117367410B (en) |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111750865A (en) * | 2020-07-04 | 2020-10-09 | 东南大学 | Self-adaptive filtering navigation method for dual-function deep sea unmanned submersible vehicle navigation system |
| CN112710304A (en) * | 2020-12-17 | 2021-04-27 | 西北工业大学 | Underwater autonomous vehicle navigation method based on adaptive filtering |
| US20220404152A1 (en) * | 2020-08-04 | 2022-12-22 | Southeast University | Motion constraint-aided underwater integrated navigation method employing improved sage-husa adaptive filtering |
| WO2023045357A1 (en) * | 2021-09-23 | 2023-03-30 | 哈尔滨工程大学 | Integrated navigation robust filtering method based on statistical similarity measurement |
| CN116222578A (en) * | 2023-05-04 | 2023-06-06 | 山东大学 | Underwater integrated navigation method and system based on self-adaptive filtering and optimal smoothing |
| CN117146830A (en) * | 2023-10-31 | 2023-12-01 | 山东科技大学 | Self-adaptive multi-beacon dead reckoning and long-baseline tightly-combined navigation method |
-
2023
- 2023-12-04 CN CN202311645357.0A patent/CN117367410B/en active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111750865A (en) * | 2020-07-04 | 2020-10-09 | 东南大学 | Self-adaptive filtering navigation method for dual-function deep sea unmanned submersible vehicle navigation system |
| US20220404152A1 (en) * | 2020-08-04 | 2022-12-22 | Southeast University | Motion constraint-aided underwater integrated navigation method employing improved sage-husa adaptive filtering |
| CN112710304A (en) * | 2020-12-17 | 2021-04-27 | 西北工业大学 | Underwater autonomous vehicle navigation method based on adaptive filtering |
| WO2023045357A1 (en) * | 2021-09-23 | 2023-03-30 | 哈尔滨工程大学 | Integrated navigation robust filtering method based on statistical similarity measurement |
| CN116222578A (en) * | 2023-05-04 | 2023-06-06 | 山东大学 | Underwater integrated navigation method and system based on self-adaptive filtering and optimal smoothing |
| CN117146830A (en) * | 2023-10-31 | 2023-12-01 | 山东科技大学 | Self-adaptive multi-beacon dead reckoning and long-baseline tightly-combined navigation method |
Non-Patent Citations (3)
| Title |
|---|
| XU BO ET AL.: "Cooperative Localisation of AUVs based on Huber-based Robust Algorithm and Adaptive Noise Estimation", 《JOURNAL OF NAVIGATION》, vol. 72, no. 4, pages 875 - 893 * |
| 吴子敬: "基于自适应滤波长基线组合AUV定位算法", 《齐齐哈尔大学学报(自然科学版)》, vol. 39, no. 6, pages 27 - 33 * |
| 王其;徐晓苏;张涛;李佩娟;: "模糊自适应滤波在水下航行器组合导航系统中的应用", 中国惯性技术学报, no. 03, pages 72 - 77 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN117367410B (en) | 2024-03-19 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US9223007B2 (en) | Kalman filtering with indirect noise measurements | |
| Shakhtarin et al. | Modification of the nonlinear kalman filter in a correction scheme of aircraft navigation systems | |
| CN110412546B (en) | Positioning method and system for underwater target | |
| US20190187297A1 (en) | System and method for locating a moving object | |
| CN111578928B (en) | Positioning method and device based on multi-source fusion positioning system | |
| EP3333546A1 (en) | Apparatus and method for data-based referenced navigation | |
| CN112710304B (en) | Underwater autonomous vehicle navigation method based on adaptive filtering | |
| KR20210013526A (en) | Apparatus and method for terrain aided navigation using inertial position | |
| CN113960622A (en) | Real-time positioning method and device fusing laser radar and IMU sensor information | |
| Jørgensen et al. | Underwater position and attitude estimation using acoustic, inertial, and depth measurements | |
| CN110703205A (en) | Ultrashort baseline positioning method based on adaptive unscented Kalman filtering | |
| Fagogenis et al. | Improving underwater vehicle navigation state estimation using locally weighted projection regression | |
| CN109471192B (en) | High-precision dynamic data processing method for full-automatic gravity tester | |
| WO2017141469A1 (en) | Position estimation device | |
| Krishnamurthy et al. | A self-aligning underwater navigation system based on fusion of multiple sensors including DVL and IMU | |
| CN117367410B (en) | State estimation method, unmanned underwater vehicle and computer readable storage medium | |
| Liu et al. | Navigation algorithm based on PSO-BP UKF of autonomous underwater vehicle | |
| CN115218927B (en) | Unmanned aerial vehicle IMU sensor fault detection method based on secondary Kalman filtering | |
| Fernandes et al. | Gnss/mems-ins integration for drone navigation using ekf on lie groups | |
| Wang et al. | Novel cooperative navigation method for multi-AUVs based on optimal weight distribution method | |
| CN114061592B (en) | Adaptive robust AUV navigation method based on multiple models | |
| CN114741659B (en) | Adaptive model on-line reconstruction robust filtering method, device and system | |
| CN114018250B (en) | Inertial navigation method, electronic device, storage medium and computer program product | |
| KR102501275B1 (en) | Precise trajectory prediction method and apparatus of a highly maneuvering target through flight dynamics estimation and multipath suppression | |
| RU2625603C2 (en) | Method of unmanned aircraft motion parameters minimax filtration with correction from satellite navigation system |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |