CN113984045A - Underwater robot mobile docking target motion state estimation method and system - Google Patents

Underwater robot mobile docking target motion state estimation method and system Download PDF

Info

Publication number
CN113984045A
CN113984045A CN202111192316.1A CN202111192316A CN113984045A CN 113984045 A CN113984045 A CN 113984045A CN 202111192316 A CN202111192316 A CN 202111192316A CN 113984045 A CN113984045 A CN 113984045A
Authority
CN
China
Prior art keywords
auv
homing
motion
state
moment
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
Application number
CN202111192316.1A
Other languages
Chinese (zh)
Other versions
CN113984045B (en
Inventor
李柯垚
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Hunan Aerospace Institute of Mechanical and Electrical Equipment and Special Materials
Original Assignee
Hunan Aerospace Institute of Mechanical and Electrical Equipment and Special Materials
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Hunan Aerospace Institute of Mechanical and Electrical Equipment and Special Materials filed Critical Hunan Aerospace Institute of Mechanical and Electrical Equipment and Special Materials
Priority to CN202111192316.1A priority Critical patent/CN113984045B/en
Publication of CN113984045A publication Critical patent/CN113984045A/en
Application granted granted Critical
Publication of CN113984045B publication Critical patent/CN113984045B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
    • G01S5/18—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using ultrasonic, sonic, or infrasonic waves
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
    • H—ELECTRICITY
    • H03—ELECTRONIC CIRCUITRY
    • H03H—IMPEDANCE NETWORKS, e.g. RESONANT CIRCUITS; RESONATORS
    • H03H17/00—Networks using digital techniques
    • H03H17/02—Frequency selective networks
    • H03H17/0248—Filters characterised by a particular frequency response or filtering method
    • H03H17/0255—Filters based on statistics
    • H03H17/0257—KALMAN filters

Abstract

The invention discloses a method and a system for estimating the motion state of a mobile docking target of an underwater robot, wherein an extended Kalman filter is used for filtering the motion state of a homing AUV measured by an inertial navigation system to obtain the motion state variation of the homing AUV; taking the motion state variable quantity of the homing AUV as a control vector, and filtering the relative motion state of the mother ship and the homing AUV measured by the ultra-short baseline positioning system by using a discrete Kalman filter to obtain the corrected relative motion state; and calculating the actual motion state of the mother sport boat according to the measured motion state of the homing AUV and the corrected relative motion state. When the discrete Kalman filter is used for filtering the relative motion state of the mother sports boat and the homing AUV, the current state variable quantity of the homing AUV is transmitted to the discrete Kalman filter as a control vector, so that the precision of the relative motion state of the mother sports boat and the homing AUV can be improved, and the calculation precision of the actual motion state of the mother sports boat is further improved.

Description

Underwater robot mobile docking target motion state estimation method and system
Technical Field
The invention relates to the technical field of movable docking of underwater robots, in particular to a method and a system for estimating the motion state of a movable docking target of an underwater robot.
Background
With the development of deep sea scientific research in China, more and more deep sea exploration tasks need to be participated in by an underwater robot (AUV for short), such as underwater investigation, large-range and long-range work network expansion and construction and the like. Due to the limited underwater communication capacity and the limited energy of the AUV, the AUV needs to be in butt joint with the docking station after an energy system warns and the operation data capacity is saturated or an early task is completed in the process of executing the task in the deep sea, so that the tasks of energy exchange, data uploading, task downloading and the like between the AUV and the docking station are completed. The underwater mobile docking technology is a key technology for completing docking between an AUV and a docking station, plays a crucial role in various deep sea detection tasks, and in the docking process of the AUV and the motion docking station, the homing AUV needs to estimate the motion state of a mother sports boat and guides the AUV to complete homing and docking by utilizing the estimated state information (including the position, the speed and the attitude of the mother sports boat) of the mother sports boat.
The existing underwater robot moving target state estimation technology regards an AUV and a mother boat as a linear motion relative motion system, namely, a uniform acceleration motion model in a relative motion model formula between a homing AUV and a moving mother boat. Therefore, the current mainstream method for estimating the state of the moving target of the underwater robot measures the relative position of the homing AUV and the mother boat in the period by using the ultra-short baseline positioning system, and filters the linear moving system through the discrete kalman filter to estimate the relative position difference of the homing AUV and the mother boat. However, the above method, because the real motion state of the system is simplified, loses a certain estimation accuracy, and cannot be applied to the mobile docking task, because the mobile docking task needs to know the relative position of the AUV and the mother boat and also needs to know the absolute position of the anemarrhena boat, and the posture of the mother boat is estimated by the absolute position.
Therefore, how to design a method for estimating the motion state of an underwater target, which can be applied to a mobile docking task, has become a technical problem to be solved urgently by those skilled in the art.
Disclosure of Invention
The invention provides a method and a system for estimating the motion state of a movable docking target of an underwater robot, which are used for solving the technical problem that the existing method for estimating the motion state of the underwater target cannot be applied to a movable docking task.
In order to solve the technical problems, the technical scheme provided by the invention is as follows:
a method for estimating the motion state of a mobile docking target of an underwater robot is applied to a homing AUV with an inertial navigation system and an ultra-short baseline positioning system, and comprises the following steps:
filtering the motion state of the homing AUV measured by the inertial navigation system by using an extended Kalman filter to obtain the motion state variation of the homing AUV;
taking the motion state variable quantity of the homing AUV as a control vector, and filtering the relative motion state of the mother sports boat and the homing AUV measured by the ultra-short baseline positioning system by using a discrete Kalman filter to obtain the corrected relative motion state of the mother sports boat and the homing AUV;
and calculating the actual motion state of the mother sports boat according to the motion state of the homing AUV measured by the inertial navigation system and the corrected relative motion state of the mother sports boat and the homing AUV.
Preferably, the relative motion state includes a position of the mother sports boat relative to the homing AUV and a speed of the mother sports boat relative to the homing AUV, and a motion variation of the homing AUV includes a speed variation and a position variation; the first state transition equation of the discrete kalman filter is:
Figure BDA0003301672210000021
wherein m represents the serial number of the ultra-short baseline system to the measurement time of the relative motion system, the relative motion system is composed of a mother ship and a homing AUV, DeltaL represents the position of the mother ship relative to the homing AUV, and DeltaL represents the position of the mother shipmIndicating the position of the mother boat in motion at the m-th moment relative to the homing AUV, DeltaLm-1Indicates the position of the mother boat in motion relative to the homing AUV at the m-1 th moment, Δ PauvAnd Δ VauvRespectively representing homing AUV self-positionAmount of change in setting and velocity, Δ Pauvm,m-1Represents the variation of the position of the homing AUV from the m-1 th moment to the m-th moment, delta Vauvm,m-1Representing the variation of the speed of the homing AUV from the m-1 th moment to the m-th moment, VrelativeIndicating the relative speed, V, of the mother boat in motion with respect to the homing AUVrelative,m-1Representing the speed, V, of the mother boat in motion relative to the homing AUV at time m-1auv,m-1The speed of the homing AUV at the m-1 th moment is represented, and Δ T represents the duration of the measurement period of the ultra-short baseline system.
Preferably, the filtering of the relative motion state of the mother sports boat and the homing AUV measured by the ultra-short baseline positioning system by using the discrete kalman filter includes the following steps:
initializing a state variable, a control vector, a state transition matrix, a control matrix and a covariance matrix of the first state transition equation;
predicting a one-step state variable of a first state transition equation at the m moment according to the predicted state variable at the m-1 moment, the control vector at the m-1 moment, the state transition matrixes from m-1 to m and the control matrix;
calculating a one-step covariance matrix from m-1 to m moments based on the predicted state transition matrix at m-1 moment, the control matrix at m-1 moment and the covariance matrix at m-1 moment;
obtaining the relative motion state of the mother sports boat and the homing AUV measured by the ultra-short baseline positioning system at the m moment and a corresponding measurement noise matrix, and calculating the gain at the m moment according to the predicted one-step state variable at the m moment, the one-step covariance matrix at the m-1 to m moments, the relative motion state of the mother sports boat and the homing AUV measured by the ultra-short baseline positioning system at the m moment and the corresponding measurement noise matrix; and updating the covariance matrix at the moment m based on the gain at the moment m, and correcting the one-step state variable at the moment m to obtain the relative motion state of the corrected mother boat at the moment m and the homing AUV.
Preferably, the state variables of the first state transition equation:
Figure BDA0003301672210000031
wherein r isx1The x-axis coordinate of the relative position of the sport mother boat and the homing AUV is obtained; r isy1The y-axis coordinate of the relative position of the sport mother boat and the homing AUV is obtained; r isz1Z-axis coordinate of relative position of mother boat in motion and homing AUV, -vauvxThe component of the relative speed of the mother boat in motion and the homing AUV in the direction of the x axis, -vauvyThe component of the relative speed of the mother boat in motion and the homing AUV in the y-axis direction, -vauvzThe component of the relative speed of the mother boat in motion and the homing AUV in the z-axis direction is shown;
predicting a one-step state variable of a first state transition equation at the m moment according to the predicted state variable at the m-1 moment, the control vector at the m-1 moment, the state transition matrixes from m-1 to m and the control matrix, and realizing the following formula:
Figure BDA0003301672210000032
wherein the content of the first and second substances,
Figure BDA0003301672210000033
representing a predicted one-step state variable of the first state transition equation at the moment m, wherein m is more than or equal to 1; a. them,m-1The state transition matrix is used for describing the propagation rule of the state variable from m-1 to m moments;
Figure BDA0003301672210000034
representing the state variable of the predicted and corrected first state transition equation m-1 moment; u shapem-1Which represents the control vector(s) of the control,
Figure BDA0003301672210000035
the value is the AUV position and speed variation of the current time and the last time, Bm,m-1Called control matrix, for describing the influence of control vector on state variable; wherein the content of the first and second substances,
Figure BDA0003301672210000036
Figure BDA0003301672210000037
calculating a one-step covariance matrix from m-1 to m moments based on the predicted state transition matrix at m-1 moment, the control matrix at m-1 moment and the covariance matrix at m-1 moment, and calculating by the following formula:
Figure BDA0003301672210000041
in the formula, Pm/m-1Is a one-step covariance matrix of the first state transition equation m-1 to m times, Pm-1Is the covariance matrix at the moment of the first state transition equation m-1.
Preferably, the gain at the m moment is calculated according to the predicted one-step state variable at the m moment, the one-step covariance matrix from the m-1 moment to the m moment, the relative motion state of the mother sports boat and the homing AUV at the m moment measured by the ultra-short baseline positioning system and the corresponding measured noise matrix; updating the covariance matrix at the moment m based on the gain at the moment m, correcting the state variable at the moment m to obtain the relative motion state of the corrected mother boat at the moment m and the homing AUV, and realizing the following calculation formula:
Figure BDA0003301672210000042
Figure BDA0003301672210000043
Figure BDA0003301672210000044
Pm＝(I-KmHm)Pm/m-1；
wherein the content of the first and second substances,
Figure BDA0003301672210000045
measuring the relative motion state of the mother sports boat and the homing AUV at the moment m for the ultra-short baseline positioning system; hmAn observation matrix for measuring the relative position of the mother boat and the homing AUV by an ultra-short baseline positioning system,
Figure BDA0003301672210000046
is a state variable of a relative motion system consisting of a mother sports boat and a homing AUV,
Figure BDA0003301672210000047
is composed of
Figure BDA0003301672210000048
A corresponding measurement noise matrix;
Figure BDA0003301672210000049
the state variable at the m moment is a corrected state variable, namely the relative motion state of the corrected mother boat at the m moment and the homing AUV; kmThe gain at time m; pmIs a covariance matrix at the m moment; and I is an identity matrix.
Preferably, the actual motion state of the mother boat comprises: any one or combination of several of the actual motion position, the actual speed and the heading angle of the mother boat in a preset fixed coordinate system; the method comprises the following steps that the motion state of a homing AUV measured by an inertial navigation system comprises the actual motion position of the homing AUV, the relative motion state of a corrected motion mother boat at the moment m and the homing AUV comprises the relative position of the corrected motion mother boat at the moment m and the homing AUV, and the actual motion state of the motion mother boat is calculated according to the motion state of the homing AUV measured by the inertial navigation system and the relative motion state of the corrected motion mother boat and the homing AUV, and comprises the following steps:
calculating the actual motion position of the motion carrier in a preset fixed coordinate system according to the actual motion position of the homing AUV measured by the inertial navigation system and the corrected relative position of the motion carrier and the homing AUV at the moment m;
based on the actual motion position of the moving mother ship, estimating the motion speed of the moving mother ship by adopting a difference method, wherein the calculation formula of the difference method is as follows:
Figure BDA0003301672210000051
wherein v ismsub,mRepresenting the m-th estimated actual speed of motion, P, of the mother boat in motionmusb,mRepresenting the m-th estimated actual movement position, P, of the mother boat in motionmsub,m-1Representing the actual motion position estimated at the moment m-1 of the mother boat in motion;
and estimating the heading of the mother ship by adopting least square normativity based on the actual motion position of the motion mother ship.
Preferably, the motion variation of the homing AUV includes a position variation rate, a speed variation rate and an attitude of the homing AUV in a fixed coordinate system; the method comprises the following steps of utilizing an extended Kalman filter to filter the motion state of a homing AUV measured by an inertial navigation system to obtain the motion state variation of the homing AUV, and comprising the following steps of:
constructing a state transfer equation of the self motion of the homing AUV according to the relationship between the motion state of the homing AUV in a preset carrier coordinate system and the motion state of the homing AUV in a preset fixed coordinate system;
the method comprises the steps of taking the linear speed of a homing AUV measured by a Doppler velocimeter under a carrier coordinate system and the attitude angle of the homing AUV measured by an inertial navigation system in a fixed coordinate system as observed quantities, taking the position change rate, the speed change rate and the attitude quantity of the homing AUV measured by the inertial navigation system in the fixed coordinate system as state variables, filtering a state transfer equation of the self-motion of the homing AUV through an extended Kalman filter, and predicting the position change rate, the speed change rate and the attitude quantity of the homing AUV in a measurement period of an ultra-short baseline positioning system in the fixed coordinate system.
Preferably, the state transition equation of homing AUV motion is:
Figure BDA0003301672210000052
the linear speed and angular speed parameters of the homing AUV in each coordinate axis direction in the carrier coordinate system are introduced as shown in table 1;
TABLE 1 AUV motion parameters and symbols in vector coordinate System
Figure BDA0003301672210000061
The position and attitude parameters of the homing AUV in each coordinate axis direction in the fixed coordinate system are introduced as shown in table 2;
TABLE 2 AUV pose parameters and symbols under fixed coordinate system
Figure BDA0003301672210000062
The subscript k in each parameter represents the measurement time serial number of the inertial navigation system, the unit is time, the superscript represents the derivative of the corresponding parameter, and delta represents the change rate of the corresponding parameter.
Preferably, the filtering of the state transition equation of the homing AUV motion by the extended kalman filter includes the following steps:
initializing a state variable, a control input quantity, a state transition matrix, a control matrix and a covariance matrix of a state transition equation of the homing AUV self motion;
and performing one-step state prediction on the state variable of the state transition equation of the homing AUV self motion and the covariance matrix thereof by the following formula:
Figure BDA0003301672210000063
Pk/k-1＝FkPk-1Fk T+Rk
wherein: x is the number ofkTo home the state variable of the AUV at time k,
Figure BDA0003301672210000064
delta xi, delta eta and delta zeta respectively represent the position change rate of the homing AUV in each coordinate axis direction in a fixed coordinate system,
Figure BDA0003301672210000065
respectively represents the directional velocity components of each coordinate axis of the homing AUV in a fixed coordinate system,
Figure BDA0003301672210000066
theta and psi respectively represent attitude angles of the homing AUV in all coordinate axis directions in a fixed coordinate system;
Figure BDA0003301672210000067
the predicted value of the state variable from the moment k-1 to the moment k; p is a predicted value of the covariance matrix; r is a motion noise covariance matrix; f is a jacobian matrix corresponding to F (x), and the expression at the time k is as follows:
Figure BDA0003301672210000071
acquiring the linear velocity of a homing AUV in a carrier coordinate system measured by a Doppler velocimeter and the attitude angle of the homing AUV in a solid coordinate system measured by an inertial navigation system as observed quantities, and correcting and updating the state variable and covariance of the homing AUV according to the following formula:
Figure BDA0003301672210000072
Pk＝(I-KkHk)Pk/k-1
wherein the content of the first and second substances,
Figure BDA0003301672210000073
is a modified estimate of the state variable at time k, PkA covariance matrix correction value at time k; z is a radical ofkTo observeThe amount of the compound (A) is,
Figure BDA0003301672210000074
Figure BDA0003301672210000075
nonlinear state transfer equation for homing AUV self-motion state, KkTo calculate the gain matrix, the expression is:
Kk＝Pk/k-1Hk T(HkPk/k-1Hk T+Qk)-1
Hkfor the jacobian matrix of the measurement equation, since the AUV directly uses the velocity information output by the doppler velocimeter and the attitude angle information of the inertial navigation system, the jacobian matrix of the measurement equation is:
Figure BDA0003301672210000076
accumulating the estimation results of the state variables of the AUV to obtain the total position change amount delta P of the homing AUV in the measurement period of the ultra-short baseline positioning systemauvAnd total amount of velocity change Δ Vauv。
A computer system comprising a memory, a processor and a computer program stored on the memory and executable on the processor, the steps of the method being performed when the computer program is executed by the processor.
The invention has the following beneficial effects:
1. the method and the system for estimating the motion state of the underwater robot movable docking target have the advantages that the motion state of the AUV is subjected to nonlinear observation, the self-inertial navigation system and the Doppler velocimeter on the homing AUV are used for measuring the self-motion state of the homing AUV, the state of the AUV is estimated by the extended Kalman filter, the self-motion state variable quantity of the AUV in the measurement period of the ultra-short baseline positioning system is obtained, when the ultra-short baseline positioning system measures the relative position of the AUV and the motion mother boat, the current state variable quantity of the AUV is transmitted to the Kalman discrete filter as a control vector, the estimation precision of the relative position is improved, the motion state of the motion mother boat with higher precision is estimated according to the relative position with higher precision, and the success rate of movable docking of the underwater robot is further improved.
2. In a preferred scheme, the motion state variation of the homing AUV is optimized by fusing motion state data of the homing AUV obtained by measurement in two different coordinate systems through expanding Kalman filtering, and the actual motion state of the mother sports boat is solved based on the optimized motion state variation of the homing AUV, so that the accuracy of motion state estimation of the mother sports boat can be further improved.
In addition to the objects, features and advantages described above, other objects, features and advantages of the present invention are also provided. The present invention will be described in further detail below with reference to the accompanying drawings.
Drawings
The accompanying drawings, which are incorporated in and constitute a part of this application, illustrate embodiments of the invention and, together with the description, serve to explain the invention and not to limit the invention. In the drawings:
fig. 1 is a schematic diagram of a method for estimating a motion state of a mobile docking target of an underwater robot in a preferred embodiment of the present invention;
fig. 2 is a flowchart of a method for estimating a motion state of a mobile docking target of an underwater robot in a preferred embodiment of the present invention;
fig. 3 is a schematic diagram of a coordinate system in a mobile docking of an underwater robot in a preferred embodiment of the present invention;
fig. 4 is a flow chart diagram of a method for estimating a motion state of a mobile docking target of an underwater robot in the present invention.
Detailed Description
The embodiments of the invention will be described in detail below with reference to the drawings, but the invention can be implemented in many different ways as defined and covered by the claims.
The first embodiment is as follows:
as shown in fig. 4, the present embodiment discloses a method for estimating a motion state of a mobile docking target of an underwater robot, which is applied to a homing AUV with an inertial navigation system and an ultra-short baseline positioning system, and includes the following steps:
filtering the motion state of the homing AUV measured by the inertial navigation system by using an extended Kalman filter to obtain the motion state variation of the homing AUV;
taking the motion state variable quantity of the homing AUV as a control vector, and filtering the relative motion state of the mother sports boat and the homing AUV measured by the ultra-short baseline positioning system by using a discrete Kalman filter to obtain the corrected relative motion state of the mother sports boat and the homing AUV;
and calculating the actual motion state of the mother sports boat according to the motion state of the homing AUV measured by the inertial navigation system and the corrected relative motion state of the mother sports boat and the homing AUV.
In addition, in the embodiment, a computer system is also disclosed, which includes a memory, a processor, and a computer program stored on the memory and executable on the processor, and when the processor executes the computer program, the steps of the method are implemented.
The method and the system for estimating the motion state of the underwater robot movable docking target have the advantages that the motion state of the AUV is subjected to nonlinear observation, the self-inertial navigation system and the Doppler velocimeter on the homing AUV are used for measuring the self-motion state of the homing AUV, the state of the AUV is estimated by the extended Kalman filter, the self-motion state variable quantity of the AUV in the measurement period of the ultra-short baseline positioning system is obtained, when the ultra-short baseline positioning system measures the relative position of the AUV and the motion mother boat, the current state variable quantity of the AUV is transmitted to the Kalman discrete filter as a control vector, the estimation precision of the relative position is improved, the motion state of the motion mother boat with higher precision is estimated according to the relative position with higher precision, and the success rate of movable docking of the underwater robot is further improved.
Example two:
the second embodiment is the preferred embodiment of the first embodiment, and the difference between the first embodiment and the second embodiment is that the specific steps of the underwater robot mobile docking target motion state estimation method are refined:
as shown in fig. 1-2, in the present embodiment, a method for estimating a motion state of a mobile docking target of an underwater robot is disclosed, which includes the following steps:
the method comprises the following steps: estimating the motion state of the homing AUV in the measurement period of the ultra-short baseline positioning system by using an extended Kalman filter to obtain the motion state variation of the homing AUV in the measurement period of the ultra-short baseline positioning system:
1.1, constructing a fixed coordinate system and a following coordinate system, and constructing a state transfer equation of the self motion of the homing AUV according to the relationship between the motion state of the homing AUV under a carrier coordinate system and the motion state of the homing AUV under the fixed coordinate system:
as shown in fig. 3, the origin E of the fixed coordinate system (E system) can be selected at a certain point on the sea surface, in the sea, or on the earth, and the E ξ axis points to the true north of the geography; the eta coordinate axis points to the true east of the geography; the E zeta coordinate axis points to the center of the earth, and the E zeta axis is selected as the main heading (the advancing direction of the AUV) of the AUV; selecting an E eta axis as the transverse direction of the AUV; the E zeta axis was chosen as the vertical direction of the AUV.
The origin O of the random coordinate system (O system) is selected at the gravity center G of the AUV. Placing the ox carrier in a longitudinal section, wherein the direction of the ox carrier is consistent with the direction of a bow and is parallel to the horizontal plane; the oy axis is perpendicular to the longitudinal section, and the direction of the oy axis is consistent with the starboard direction of the naval vessel and is parallel to the horizontal plane. The oz axis is in the longitudinal section and is oriented in the same direction below the bottom of the vessel and perpendicular to the horizontal plane.
The constructed state transition equation of the homing AUV self-motion is as follows:
Figure BDA0003301672210000101
wherein:
the linear and angular velocity parameters of the homing AUV in its carrier coordinate system for each axis are presented in table 1.
TABLE 1 AUV motion parameters and symbols in vector coordinate System
Figure BDA0003301672210000102
The position and attitude parameters of the homing AUV in each axis of the fixed coordinate system are presented in table 2.
TABLE 2 AUV pose parameters and symbols under fixed coordinate system
Figure BDA0003301672210000103
Subscript k in each parameter represents a measurement time serial number of the inertial navigation system, t is the duration of a measurement period of the inertial navigation system, superscript represents a derivative of the corresponding parameter, and delta represents a change rate of the parameter.
1.2, taking the linear velocity of the homing AUV under a carrier coordinate system and the attitude angle of the homing AUV in a fixed coordinate system as observed quantities, taking the position change rate, the speed change rate and the attitude quantity of the homing AUV in the fixed coordinate system as state variables, filtering a state transfer equation of the homing AUV self motion through an extended Kalman filter, and estimating the position change rate, the speed change rate and the attitude quantity of the homing AUV in the fixed coordinate system within a measurement period:
(1) setting state variables in an EKF filter (extended Kalman filter) as the position and the attitude of an AUV under a fixed coordinate system:
Figure BDA0003301672210000111
wherein, delta xi, delta eta and delta zeta respectively represent the position change of the homing AUV,
Figure BDA0003301672210000112
respectively represent AUV velocity components under a fixed coordinate system,
Figure BDA0003301672210000113
θ, ψ denote attitude angles of the AUV, respectively.
(2) And performing one-step state prediction on the state variable of the homing AUV and the covariance matrix thereof:
Figure BDA0003301672210000114
Pk/k-1＝FkPk-1Fk T+Rk
wherein:
Figure BDA0003301672210000115
the predicted value of the state variable from the moment k-1 to the moment k; p is a predicted value of the covariance matrix; r is a motion noise covariance matrix; f is a jacobian matrix corresponding to F (x), and the expression at the time k is as follows:
Figure BDA0003301672210000116
(3) taking the speed information of the homing AUV output by the Doppler velocimeter and the attitude angle of the homing AUV output by the inertial navigation system as measurement values, and expressing the measurement value of the homing AUV motion state at the moment k as follows:
Figure BDA0003301672210000117
wherein:
[uk,vk,wk]Tis the speed of the AUV in its coordinate system with the body,
Figure BDA0003301672210000118
is the attitude angle of the AUV.
Accordingly, the variance matrix of the measurement noise is represented as:
Figure BDA0003301672210000119
the distribution of the noise matrix obeys a gaussian white noise distribution.
(4) Based on the measured value, the state variable and covariance of the homing AUV are corrected and updated according to the following formula:
Figure BDA00033016722100001110
Pk＝(I-KkHk)Pk/k-1
wherein:
Figure BDA00033016722100001111
is an estimate of the state variable at time k, PkThe covariance matrix correction value at time k.
KkTo calculate the gain matrix, the expression is:
Kk＝Pk/k-1Hk T(HkPk/k-1Hk T+Qk)-1
Hkfor the jacobian matrix of the measurement equation, since the AUV directly uses the velocity information output by the doppler velocimeter and the attitude angle information of the inertial navigation system, the jacobian matrix of the measurement equation is:
Figure BDA0003301672210000121
(5) accumulating the estimation results of the AUV state variables to obtain delta PauvAnd Δ Vauv。
The angular velocity information under a carrier coordinate system and the linear velocity information output by a Doppler velocimeter, which are provided by an inertial navigation system carried by the homing AUV, are used as observed quantities, and an extended Kalman filter is used for carrying out filtering processing on the position, the position change rate and the attitude of the homing AUV in a fixed coordinate system, so that the motion state (including the position, the speed and the attitude) of the homing AUV in a measurement cycle of an ultra-short baseline positioning system is estimated, and the accuracy of the motion state of the homing AUV can be greatly improved.
Step two: determining the relative motion relation between the homing AUV and the mother boat, and establishing a state transfer equation of a relative motion system:
the motion description of the whole relative motion system is a mutual motion process of the under-actuated AUV and the moving target, wherein the under-actuated AUV is gradually close to the moving target, and the AUV is used as an observer to observe the motion state of the moving mother boat and estimate the motion state of the moving mother boat. The AUV establishes a nonlinear model close to real motion by using measurement data provided by the self inertial navigation system and the Doppler velocimeter, and when the USBL measures the relative position of the AUV and the mobile mother boat, the change of the self motion state of the AUV is used for estimating the state change of the system in one USBL measurement period. Thus, the relative motion model of the system, i.e., the state transition equation, can be expressed as follows:
Figure BDA0003301672210000122
wherein: Δ L denotes the position of the mother boat relative to the homing AUV, VrelativeRepresenting the relative speed, Δ P, which is the difference between the speed of the mother boat and the speed of the AUVauvAnd Δ VauvRespectively represent the position and speed variation of the homing AUV in one measurement period, and delta T represents the measurement period of the ultra-short baseline system.
Step three: and judging whether the AUV at the current moment measures the relative position of the homing AUV and the mother sports boat by using the ultra-short baseline positioning system. If not, repeating the operation in the step two, continuing to estimate the motion state of the homing AUV, and updating the delta PauvAnd Δ Vauv(ii) a If yes, entering the step four:
step four: taking the relative position of the homing AUV and the mother sports boat measured by the ultra-short baseline positioning system as an observed quantity, and performing filtering processing on the state transition equation of the relative sports system obtained in the step two by using a discrete Kalman filter to obtain the relative position estimation of the relative system, namely the relative position of the corrected mother sports boat and the homing AUV:
4.1 initialization of the state transition equation of the relative motion system:
at the initial moment when the homing AUV estimates the motion state of the mother boat, the relative position measured by the ultra-short baseline positioning system is taken as the initial relative position, namely:
ΔLk|k＝1＝[rx1,ry1,rz1]T
the initial relative velocity is:
-Vauv|k＝1＝[-vauvx,-vauvy,-vauvz]T
the state variables of the relative motion system are then:
Figure BDA0003301672210000131
4.2 the discrete Kalman filter uses a linear model to predict the state at time m and its covariance matrix from the state at time m-1:
Figure BDA0003301672210000132
wherein:
Am,m-1called state transition matrix, for describing the propagation law of state variables from m-1 to m time, Um-1Which represents the control vector(s) of the control,
Figure BDA0003301672210000133
the value is the AUV position and speed variation of the current time and the last time, Bm,m-1Called a control matrix, to describe the effect of the control vector on the state variables.
According to the state equation of the relative motion system:
Figure BDA0003301672210000134
it can be seen that the state transition matrix A in the relative motion systemm,m-1Can be expressed as:
Figure BDA0003301672210000135
control matrix Bm,m-1Can be expressed as:
Figure BDA0003301672210000141
accordingly, the one-step mean square error of the relative motion system is:
Figure BDA0003301672210000142
and 4.3, correcting the state variable of the relative motion system by using the measurement information of the ultra-short baseline positioning system to obtain an estimated value of the relative position of the homing AUV and the mother boat.
(1) Let the measured value of the relative motion system at time m be expressed as:
Figure BDA0003301672210000143
the variance matrix of the measurement noise is:
Figure BDA0003301672210000144
(2) and correcting and updating the state variable and covariance of the relative motion system based on the measured value of the relative position according to the following formula:
Figure BDA0003301672210000145
Figure BDA0003301672210000146
gain KmComprises the following steps:
Figure BDA0003301672210000147
the mean square error is estimated as:
Pm＝(I-KmHm)Pm/m-1
step five: and converting to obtain the absolute position of the mother sports boat in a fixed coordinate system by combining the position estimation value of the AUV and the relative position of the homing AUV estimated by the Kalman filter and the mother sports boat, and storing the state estimation result into a computer.
Step six: estimating the motion speed of the mobile mother boat by a difference method:
in the process of mobile docking, in order to ensure the success rate of docking, the mother sports boat is always in active uniform linear motion in the horizontal plane under the intervention of manual operation. Therefore, in the target estimation system, the ultra-short baseline positioning system is arranged to take out a mother boat position estimation point every ten times of measurement, and the speed of the mother boat in the time period can be estimated by mapping the coordinates of the estimation point to a horizontal plane and then differentiating the point. As the measured values become more and more accurate as the AUV gets closer to the moving parent boat, the new parent boat position measurement data is more accurate than the previous measurement data. Therefore, in the process of estimating the speed of the mother boat, the previous estimation result is not counted, and the new mother boat position estimation data and the initial position of the mother boat are taken as references.
The specific speed estimation method of the mother boat is as follows:
Figure BDA0003301672210000151
wherein, Pmusb,mDenotes the m-th estimate of the mother boat, Pmsub,m-1Representing a position estimate at a time on the mother boat.
Step seven: estimating the heading angle of the mother boat motion by a least square method:
the heading angle can utilize the position coordinates of the mother boat measured by the AUV to linearly estimate the heading of the mother boat by a least square method. Decoupling the mother boat absolute position coordinate resolved by the fusion positioning system combining the underwater ultra-short baseline positioning and the inertial navigation dead reckoning to a two-dimensional plane, then carrying out linear fitting on the data by adopting a least square parameter estimation method, and taking the obtained arctangent value of the slope as the heading angle estimation of the mother boat.
In this embodiment, each time a new estimation result of the spatial position of the mother sports boat is obtained, the difference method and the least square method introduced in the invention content are respectively used to estimate the speed of the mother sports boat, that is, the heading information, until the underwater mobile docking task is completed.
In summary, the underwater robot mobile docking target motion state estimation method provided by the invention utilizes a kalman filter (KF filter) and an extended kalman filter (EKF filter) to form an estimation system of the mother boat motion state. In the system, a Kalman filter is used for estimating relative position information of a relative motion system, and an extended Kalman filter is used for estimating motion state change of a homing AUV, so that the error caused by the fact that the homing AUV motion process is regarded as linear motion in the Kalman filter is made up, and meanwhile, the precision of the position, the speed and the posture of a mother boat is improved. The motion state estimation system can make full use of information of various sensors in the system, improves the state estimation precision of the homing AUV on the motion target when the underwater robot is in movable docking, provides more accurate guidance information, and is favorable for improving the success rate of movable docking recovery of the underwater robot.
The above is only a preferred embodiment of the present invention, and is not intended to limit the present invention, and various modifications and changes will occur to those skilled in the art. Any modification, equivalent replacement, or improvement made within the spirit and principle of the present invention should be included in the protection scope of the present invention.

Claims (10)

1. A motion state estimation method for a movable docking target of an underwater robot is applied to a homing AUV with an inertial navigation system and an ultra-short baseline positioning system, and is characterized by comprising the following steps:
filtering the motion state of the homing AUV measured by the inertial navigation system by using an extended Kalman filter to obtain the motion state variation of the homing AUV;
taking the motion state variable quantity of the homing AUV as a control vector, and filtering the relative motion state of the mother sports boat and the homing AUV measured by the ultra-short baseline positioning system by using a discrete Kalman filter to obtain the corrected relative motion state of the mother sports boat and the homing AUV;
and calculating the actual motion state of the mother sports boat according to the motion state of the homing AUV measured by the inertial navigation system and the corrected relative motion state of the mother sports boat and the homing AUV.
2. The underwater robot mobile docking target motion state estimation method according to claim 1, wherein the relative motion state includes a position of the mother sports boat relative to the homing AUV and a velocity of the mother sports boat relative to the homing AUV, and a motion variation of the homing AUV includes a velocity variation and a position variation; the first state transition equation of the discrete kalman filter is:
Figure FDA0003301672200000011
wherein m represents the serial number of the ultra-short baseline system to the measurement time of the relative motion system, the relative motion system is composed of a mother ship and a homing AUV, DeltaL represents the position of the mother ship relative to the homing AUV, and DeltaL represents the position of the mother shipmIndicating the position of the mother boat in motion at the m-th moment relative to the homing AUV, DeltaLm-1Indicates the position of the mother boat in motion relative to the homing AUV at the m-1 th moment, Δ PauvAnd Δ VauvRespectively represent the position and speed variation of the homing AUV, delta Pauv|m,m-1Representing the change in position of the homing AUV from the m-1 th moment to the m-th momentChemical quantity, Δ Vauv|m,m-1Representing the variation of the speed of the homing AUV from the m-1 th moment to the m-th moment, VrelativeIndicating the relative speed, V, of the mother boat in motion with respect to the homing AUVrelative,m-1Representing the speed, V, of the mother boat in motion relative to the homing AUV at time m-1auv，m-1The speed of the homing AUV at the m-1 th moment is represented, and Δ T represents the duration of the measurement period of the ultra-short baseline system.
3. The underwater robot mobile docking target motion state estimation method according to claim 2, wherein the discrete kalman filter is used to filter the relative motion states of the mother sports craft and the homing AUV measured by the ultra-short baseline positioning system, and the method comprises the following steps:
initializing a state variable, a control vector, a state transition matrix, a control matrix and a covariance matrix of the first state transition equation;
predicting a one-step state variable of a first state transition equation at the m moment according to the predicted state variable at the m-1 moment, the control vector at the m-1 moment, the state transition matrixes from m-1 to m and the control matrix;
calculating a one-step covariance matrix from m-1 to m moments based on the predicted state transition matrix at m-1 moment, the control matrix at m-1 moment and the covariance matrix at m-1 moment;
obtaining the relative motion state of the mother sports boat and the homing AUV measured by the ultra-short baseline positioning system at the m moment and a corresponding measurement noise matrix, and calculating the gain at the m moment according to the predicted one-step state variable at the m moment, the one-step covariance matrix at the m-1 to m moments, the relative motion state of the mother sports boat and the homing AUV measured by the ultra-short baseline positioning system at the m moment and the corresponding measurement noise matrix; and updating the covariance matrix at the moment m based on the gain at the moment m, and correcting the one-step state variable at the moment m to obtain the relative motion state of the corrected mother boat at the moment m and the homing AUV.
4. The underwater robot mobile docking target motion state estimation method according to claim 3, wherein the state variables of the first state transition equation:
Figure FDA0003301672200000021
wherein r isx1The x-axis coordinate of the relative position of the sport mother boat and the homing AUV is obtained; r isy1The y-axis coordinate of the relative position of the sport mother boat and the homing AUV is obtained; r isz1Z-axis coordinate of relative position of mother boat in motion and homing AUV, -vauvxThe component of the relative speed of the mother boat in motion and the homing AUV in the direction of the x axis, -vauvyThe component of the relative speed of the mother boat in motion and the homing AUV in the y-axis direction, -vauvzThe component of the relative speed of the mother boat in motion and the homing AUV in the z-axis direction is shown;
predicting a one-step state variable of a first state transition equation at the m moment according to the predicted state variable at the m-1 moment, the control vector at the m-1 moment, the state transition matrixes from m-1 to m and the control matrix, and realizing the following formula:
Figure FDA0003301672200000022
wherein the content of the first and second substances,
Figure FDA0003301672200000023
representing a predicted one-step state variable of the first state transition equation at the moment m, wherein m is more than or equal to 1; a. them，m-1The state transition matrix is used for describing the propagation rule of the state variable from m-1 to m moments;
Figure FDA0003301672200000024
representing the state variable of the predicted and corrected first state transition equation m-1 moment; u shapem-1Which represents the control vector(s) of the control,
Figure FDA0003301672200000025
the value is the AUV position and speed variation of the current time and the last time, Bm，m-1Is called asThe control matrix is used for describing the influence of the control vector on the state variable; wherein the content of the first and second substances,
Figure FDA0003301672200000026
Figure FDA0003301672200000031
calculating a one-step covariance matrix from m-1 to m moments based on the predicted state transition matrix at m-1 moment, the control matrix at m-1 moment and the covariance matrix at m-1 moment, and calculating by the following formula:
Figure FDA0003301672200000032
in the formula, Pm/m-1Is a one-step covariance matrix of the first state transition equation m-1 to m times, Pm-1Is the covariance matrix at the moment of the first state transition equation m-1.
5. The underwater robot mobile docking target motion state estimation method according to claim 4, wherein the gain at the time m is calculated according to a predicted state variable at the time m, a one-step covariance matrix at the time m-1 to m, a relative motion state of a mother sports boat and a homing AUV at the time m measured by an ultra-short baseline positioning system, and a corresponding measurement noise matrix; updating the covariance matrix at the moment m based on the gain at the moment m, correcting the state variable at the moment m to obtain the relative motion state of the corrected mother boat at the moment m and the homing AUV, and realizing the following calculation formula:
Figure FDA0003301672200000033
Figure FDA0003301672200000034
Figure FDA0003301672200000035
Pm＝(I-KmHm)Pm/m-1；
wherein the content of the first and second substances,
Figure FDA0003301672200000036
measuring the relative motion state of the mother sports boat and the homing AUV at the moment m for the ultra-short baseline positioning system; hmAn observation matrix for measuring the relative position of the mother boat and the homing AUV by an ultra-short baseline positioning system,
Figure FDA0003301672200000037
is a state variable of a relative motion system consisting of a mother sports boat and a homing AUV,
Figure FDA0003301672200000038
is composed of
Figure FDA0003301672200000039
A corresponding measurement noise matrix;
Figure FDA00033016722000000310
the state variable at the m moment is a corrected state variable, namely the relative motion state of the corrected mother boat at the m moment and the homing AUV; kmThe gain at time m; pmIs a covariance matrix at the m moment; and I is an identity matrix.
6. The underwater robot mobile docking target motion state estimation method as claimed in claim 1, wherein the actual motion state of the mother boat in motion comprises: any one or combination of several of the actual motion position, the actual speed and the heading angle of the mother boat in a preset fixed coordinate system; the method comprises the following steps that the motion state of a homing AUV measured by an inertial navigation system comprises the actual motion position of the homing AUV, the relative motion state of a corrected motion mother boat at the moment m and the homing AUV comprises the relative position of the corrected motion mother boat at the moment m and the homing AUV, and the actual motion state of the motion mother boat is calculated according to the motion state of the homing AUV measured by the inertial navigation system and the relative motion state of the corrected motion mother boat and the homing AUV, and comprises the following steps:
calculating the actual motion position of the motion carrier in a preset fixed coordinate system according to the actual motion position of the homing AUV measured by the inertial navigation system and the corrected relative position of the motion carrier and the homing AUV at the moment m;
based on the actual motion position of the moving mother ship, estimating the motion speed of the moving mother ship by adopting a difference method, wherein the calculation formula of the difference method is as follows:
Figure FDA0003301672200000041
wherein v ismsub,mRepresenting the m-th estimated actual speed of motion, P, of the mother boat in motionmusb,mRepresenting the m-th estimated actual movement position, P, of the mother boat in motionmsub,m-1Representing the actual motion position estimated at the moment m-1 of the mother boat in motion;
and estimating the heading of the mother ship by adopting least square normativity based on the actual motion position of the motion mother ship.
7. The underwater robot mobile docking target motion state estimation method according to claim 1, wherein the motion variation amount of the homing AUV includes a position variation rate, a speed variation rate, and an attitude amount of the homing AUV in a fixed coordinate system; the method comprises the following steps of utilizing an extended Kalman filter to filter the motion state of a homing AUV measured by an inertial navigation system to obtain the motion state variation of the homing AUV, and comprising the following steps of:
constructing a state transfer equation of the self motion of the homing AUV according to the relationship between the motion state of the homing AUV in a preset carrier coordinate system and the motion state of the homing AUV in a preset fixed coordinate system;
the method comprises the steps of taking the linear speed of a homing AUV measured by a Doppler velocimeter under a carrier coordinate system and the attitude angle of the homing AUV measured by an inertial navigation system in a fixed coordinate system as observed quantities, taking the position change rate, the speed change rate and the attitude quantity of the homing AUV measured by the inertial navigation system in the fixed coordinate system as state variables, filtering a state transfer equation of the self-motion of the homing AUV through an extended Kalman filter, and predicting the position change rate, the speed change rate and the attitude quantity of the homing AUV in a measurement period of an ultra-short baseline positioning system in the fixed coordinate system.
8. The underwater robot mobile docking target motion state estimation method according to claim 5, wherein a state transition equation of homing AUV motion is as follows:
Figure FDA0003301672200000051
the linear speed and angular speed parameters of the homing AUV in each coordinate axis direction in the carrier coordinate system are introduced as shown in table 1;
TABLE 1 AUV motion parameters and symbols in vector coordinate System
Figure FDA0003301672200000052
The position and attitude parameters of the homing AUV in each coordinate axis direction in the fixed coordinate system are introduced as shown in table 2;
TABLE 2 AUV pose parameters and symbols under fixed coordinate system
Figure FDA0003301672200000053
Subscript k in each parameter represents a measurement time serial number of the inertial navigation system, t is the duration of a measurement period of the inertial navigation system, superscript represents a derivative of the corresponding parameter, and delta represents a change rate of the parameter.
9. The underwater robot mobile docking target motion state estimation method according to claim 8, wherein a state transition equation of homing AUV self-motion is filtered through an extended Kalman filter, comprising the steps of:
initializing a state variable, a control input quantity, a state transition matrix, a control matrix and a covariance matrix of a state transition equation of the homing AUV self motion;
and performing one-step state prediction on the state variable of the state transition equation of the homing AUV self motion and the covariance matrix thereof by the following formula:
Figure FDA0003301672200000061
Pk/k-1＝FkPk-1Fk T+Rk
wherein: x is the number ofkTo home the state variable of the AUV at time k,
Figure FDA0003301672200000062
delta xi, delta eta and delta zeta respectively represent the position change rate of the homing AUV in each coordinate axis direction in a fixed coordinate system,
Figure FDA0003301672200000063
respectively represents the directional velocity components of each coordinate axis of the homing AUV in a fixed coordinate system,
Figure FDA0003301672200000064
theta and psi respectively represent attitude angles of the homing AUV in all coordinate axis directions in a fixed coordinate system;
Figure FDA0003301672200000065
the predicted value of the state variable from the moment k-1 to the moment k; p is a predicted value of the covariance matrix; r is a motion noise covariance matrix; f is a jacobian matrix corresponding to F (x), and the expression at the time k is as follows:
Figure FDA0003301672200000066
acquiring the linear velocity of a homing AUV in a carrier coordinate system measured by a Doppler velocimeter and the attitude angle of the homing AUV in a solid coordinate system measured by an inertial navigation system as observed quantities, and correcting and updating the state variable and covariance of the homing AUV according to the following formula:
Figure FDA0003301672200000067
Pk＝(I-KkHk)Pk/k-1
wherein the content of the first and second substances,
Figure FDA0003301672200000068
is a modified estimate of the state variable at time k, PkA covariance matrix correction value at time k; z is a radical ofkIn order to observe the quantity of the object,
Figure FDA0003301672200000069
Figure FDA00033016722000000610
nonlinear state transfer equation for homing AUV self-motion state, KkTo calculate the gain matrix, the expression is:
Kk＝Pk/k-1Hk T(HkPk/k-1Hk T+Qk)-1
Hkto measure the Jacobian matrix of equations, since the AUV directly uses the Doppler velocimeterThe output velocity information and the attitude angle information of the inertial navigation system, so that the Jacobian matrix of the measurement equation is:
Figure FDA00033016722000000611
accumulating the estimation results of the state variables of the AUV to obtain the total position change amount delta P of the homing AUV in the measurement period of the ultra-short baseline positioning systemauvAnd total amount of velocity change Δ Vauv。
10. A computer system comprising a memory, a processor and a computer program stored on the memory and executable on the processor, characterized in that the steps of the method of any of the preceding claims 1 to 9 are carried out by the processor when the computer program is executed by the processor.
CN202111192316.1A 2021-10-13 2021-10-13 Method and system for estimating motion state of movable butt-joint target of underwater robot Active CN113984045B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111192316.1A CN113984045B (en) 2021-10-13 2021-10-13 Method and system for estimating motion state of movable butt-joint target of underwater robot

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111192316.1A CN113984045B (en) 2021-10-13 2021-10-13 Method and system for estimating motion state of movable butt-joint target of underwater robot

Publications (2)

Publication Number Publication Date
CN113984045A true CN113984045A (en) 2022-01-28
CN113984045B CN113984045B (en) 2024-01-16

Family

ID=79738474

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111192316.1A Active CN113984045B (en) 2021-10-13 2021-10-13 Method and system for estimating motion state of movable butt-joint target of underwater robot

Country Status (1)

Country Link
CN (1) CN113984045B (en)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114610021A (en) * 2022-02-07 2022-06-10 哈尔滨工程大学 Autonomous underwater vehicle homing path planning method and device

Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107192983A (en) * 2017-07-04 2017-09-22 深圳潜行创新科技有限公司 A kind of device, method and system for observing underwater vehicle relative position
CN108151733A (en) * 2016-12-05 2018-06-12 中国科学院沈阳自动化研究所 Towards the INS/USBL combined navigation locating methods of AUV dockings
CN109141421A (en) * 2018-07-13 2019-01-04 哈尔滨工程大学 Expected path building method in the underwater dynamic target tracking of drive lacking AUV
US20190033887A1 (en) * 2016-03-31 2019-01-31 Fraunhofer-Gesellschaft Zur Foerderung Der Angewandten Forschung E.V. System for navigation of an autonomously navigating submersible body during entry into a docking station, method
CN109375646A (en) * 2018-11-14 2019-02-22 江苏科技大学 AUV docking recycling autonomous navigation method based on FMSRUPF algorithm
CN111208845A (en) * 2020-03-20 2020-05-29 哈尔滨工程大学 Underwater robot mobile docking recovery path planning method and device
WO2020130849A1 (en) * 2018-12-19 2020-06-25 Blue Logic As System for docking a submarine vessel to a docking port and a method for docking the submarine vessel on the docking port
CN111735455A (en) * 2020-06-02 2020-10-02 江苏帝一集团有限公司 Improved Gaussian distance iterative algorithm based butt joint recovery integrated navigation method
CN112925332A (en) * 2021-01-20 2021-06-08 上海交通大学 Cooperative intersection butt joint control method for unmanned ship and underwater unmanned submersible vehicle combined system

Patent Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20190033887A1 (en) * 2016-03-31 2019-01-31 Fraunhofer-Gesellschaft Zur Foerderung Der Angewandten Forschung E.V. System for navigation of an autonomously navigating submersible body during entry into a docking station, method
CN108151733A (en) * 2016-12-05 2018-06-12 中国科学院沈阳自动化研究所 Towards the INS/USBL combined navigation locating methods of AUV dockings
CN107192983A (en) * 2017-07-04 2017-09-22 深圳潜行创新科技有限公司 A kind of device, method and system for observing underwater vehicle relative position
CN109141421A (en) * 2018-07-13 2019-01-04 哈尔滨工程大学 Expected path building method in the underwater dynamic target tracking of drive lacking AUV
CN109375646A (en) * 2018-11-14 2019-02-22 江苏科技大学 AUV docking recycling autonomous navigation method based on FMSRUPF algorithm
WO2020130849A1 (en) * 2018-12-19 2020-06-25 Blue Logic As System for docking a submarine vessel to a docking port and a method for docking the submarine vessel on the docking port
CN111208845A (en) * 2020-03-20 2020-05-29 哈尔滨工程大学 Underwater robot mobile docking recovery path planning method and device
CN111735455A (en) * 2020-06-02 2020-10-02 江苏帝一集团有限公司 Improved Gaussian distance iterative algorithm based butt joint recovery integrated navigation method
CN112925332A (en) * 2021-01-20 2021-06-08 上海交通大学 Cooperative intersection butt joint control method for unmanned ship and underwater unmanned submersible vehicle combined system

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
RUAN LIANG ET AL.: "A Factor Graph Method for AUV Navigation in the Mobile Docking Progress", 《OCEAN ENGINEERING》, pages 1 - 12 *
刘俊杰;陈虹;王磊;: "基于主从同步的欠驱动AUV与移动平台水下对接控制", 舰船科学技术, no. 11, pages 95 - 99 *
时常鸣等: "面向AUV水下移动对接的实时路径规划方法", 《舰船科学技术》, vol. 42, no. 5, pages 148 - 152 *

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114610021A (en) * 2022-02-07 2022-06-10 哈尔滨工程大学 Autonomous underwater vehicle homing path planning method and device

Also Published As

Publication number Publication date
CN113984045B (en) 2024-01-16

Similar Documents

Publication Publication Date Title
Allotta et al. A comparison between EKF-based and UKF-based navigation algorithms for AUVs localization
Costanzi et al. UKF-based navigation system for AUVs: Online experimental validation
CN111596333B (en) Underwater positioning navigation method and system
CN109857124A (en) Unmanned boat exact path tracking and controlling method based on speed Yu the dual guidance in course
CN107656530B (en) Variable-parameter open-frame type ocean underwater robot trajectory tracking control method, device and system
CN106896824A (en) A kind of many AUV formation control methods based on object function
CN106896817A (en) A kind of many AUV formation control methods based on viscous damping mode
CN112015086B (en) Feedback control method for limited-time path tracking output of under-actuated surface ship
CN111618861A (en) Double-follow-up intelligent arm control method based on four-axis structure
Bj et al. Redesign and analysis of globally asymptotically stable bearing only SLAM
CN113984045B (en) Method and system for estimating motion state of movable butt-joint target of underwater robot
Allotta et al. Development and online validation of an UKF-based navigation algorithm for AUVs
Tan et al. Drag coefficient estimation model to simulate dynamic control of autonomous underwater vehicle (AUV) motion
Al-Sharman et al. State estimation for a small scale flybar-less helicopter
Mishra et al. Cooperative relative pose estimation to aid landing of an unmanned aerial vehicle on a moving platform
Kim et al. Path following for an autonomous underwater vehicle (AUV) by using a high-gain observer based on an AUV dynamic model
Tanaka et al. Underwater vehicle localization considering the effects of its oscillation
Belge et al. Sensor fusion based on integrated navigation data of sea surface vehicle with machine learning method
CN115031726A (en) Data fusion navigation positioning method
Kim et al. Panel-based bathymetric SLAM with a multibeam echosounder
Hajiyev et al. Adaptive Fading Kalman Filter with Q-adaptation for estimation of AUV dynamics
Zhang et al. An unscented Kalman filter-based visual pose estimation method for underwater vehicles
CN112013849A (en) Autonomous positioning method and system for surface ship
Kim et al. An approach towards online bathymetric slam
Zhang et al. Observability analysis for improving EKF-SLAM algorithm by using simulation

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