AUV motion state estimation method based on virtual boat-mounted control and Kalman filtering

Abstract

The invention discloses an AUV motion state estimation method based on virtual boat-mounted control and Kalman filtering, and belongs to the technical field of autonomous underwater vehicle navigation. The method comprises the following steps: acquiring a heading angle, a heading angular speed, positions, a navigational speed and an observation distance of the AUV at the current moment, wherein the positions comprise a transverse position and a longitudinal position; the heading angle, the heading angular speed and the transverse position at the current moment are input into a virtual ship-mounted controller, the rudder angle of the AUV at the current moment is obtained, and the virtual ship-mounted controller comprises LOS guidance and a heading autopilot; the heading angle, the heading angular velocity, the position, the navigational speed, the observation distance and the rudder angle at the current moment are input into a Kalman filtering system, the state estimation value of the AUV at the next moment is obtained, the Kalman filtering system comprises an AUV prediction motion model, an observation model and a Kalman filter, and the state estimation value comprises the position, the heading angle, the navigational speed and the heading angular velocity; according to the method, the accuracy and reliability of AUV motion state estimation are improved.

Description

Technical Field

[0001] The present application belongs to the technical field of autonomous underwater vehicle navigation technology, and in particular relates to an AUV motion state estimation method based on virtual onboard control and Kalman filtering. Background Art

[0002] In recent years, with the continuous development of autonomous underwater vehicle (AUV) technology, its application in military, environmental monitoring, seabed resource exploration and other fields has received widespread attention. As an underwater device with autonomous navigation capabilities, AUV can complete tasks such as target detection, information collection and area patrol in complex underwater environments. However, the operating time and operating range of AUVs are limited by their limited energy reserves and underwater communication bandwidth, which requires them to return regularly for energy replenishment during long missions. Therefore, when returning to dock, AUVs must have accurate motion state information, such as position, attitude and speed, to ensure that they can successfully and safely enter the docking station.

[0003] Existing motion state estimation methods primarily rely on linearized filtering techniques, such as the extended Kalman filter, which fuses satellite navigation and positioning systems with Doppler velocimeter data. However, satellite navigation and positioning rely on electromagnetic wave signal transmission and are therefore unsuitable for underwater operations. Doppler velocimeters can only output high-precision velocity information when bottom tracking is established. Acoustic guidance methods such as USBL (Ultra Short Baseline) suffer from significant time delays and numerous outliers, posing significant challenges to trajectory tracking control during the autonomous and safe docking of AUVs. While outlier elimination strategies based on Kalman filtering can improve USBL data quality, their linear motion assumptions are inherently inconsistent with the three-dimensional nonlinear motion characteristics of the AUV during docking, resulting in prediction errors in dynamic behaviors such as velocity mutations and attitude angle oscillations that exceed practical engineering tolerances. Furthermore, existing studies generally employ a simplified three-degree-of-freedom horizontal motion model, failing to account for the time-varying hydrodynamic coefficients, the coupling of propeller dynamic delays, and external disturbances in shallow water environments. Consequently, existing techniques for estimating the AUV's motion state during docking suffer from low accuracy and poor stability. Summary of the Invention

[0004] The present application aims to solve at least one of the technical problems existing in the prior art. To this end, the present application proposes an AUV motion state estimation method based on virtual onboard control and Kalman filtering, which improves the accuracy and reliability of AUV motion state estimation.

[0005] In a first aspect, the present application provides an AUV motion state estimation method based on virtual onboard control and Kalman filtering, the method comprising:

[0006] Obtain the heading angle, heading angular velocity, position, speed and observation distance of the AUV at the current moment, where the position includes the lateral position and the longitudinal position;

[0007] Inputting the heading angle, heading angular velocity, and lateral position at the current moment into a virtual onboard controller to obtain the rudder angle of the AUV at the current moment, wherein the virtual onboard controller includes LOS guidance and heading autopilot;

[0008] The heading angle, heading angular velocity, position, speed, observation distance and rudder angle at the current moment are input into the Kalman filter system to obtain the state estimation value of the AUV at the next moment. The Kalman filter system includes an AUV predicted motion model, an observation model and a Kalman filter. The state estimation value includes position, heading angle, speed and heading angular velocity.

[0009] According to one embodiment of the present application, inputting the heading angle, heading angular velocity, and lateral position at the current moment into a virtual onboard controller to obtain the rudder angle of the AUV at the current moment includes:

[0010] Inputting the heading angle and lateral position of the AUV at the current moment into LOS guidance to obtain a desired heading angle and a desired heading angular velocity;

[0011] The desired heading angle, the desired heading angular velocity, the current heading angle and the current heading angular velocity are input into the heading autopilot to obtain the rudder angle of the AUV at the current moment.

[0012] According to one embodiment of the present application, inputting the rudder angle at the current moment into a Kalman filter system to obtain a state estimate value of the AUV at the next moment includes:

[0013] Inputting the heading angle, heading angular velocity, position, speed, observation distance and rudder angle at the current moment into an AUV prediction motion model to obtain an initial estimate of the state of the AUV at the next moment, the AUV prediction motion model including a prediction kinematics sub-model and a prediction dynamics sub-model;

[0014] establishing an observation model based on the observation distance;

[0015] The heading angle, heading angular velocity, position, speed, observation distance, rudder angle and initial estimated value of the state at the next moment at the current moment are input into the Kalman filter, and a state space equation is established in combination with the observation model. The initial estimated value of the state of the AUV at the next moment is corrected by the state correction formula to obtain the estimated value of the state of the AUV at the next moment.

[0016] According to one embodiment of the present application, the inputting of the heading angle, heading angular velocity, position, speed, observation distance and rudder angle at the current moment into the AUV prediction motion model to obtain an initial estimate of the state of the AUV at the next moment includes:

[0017] Inputting the current position, heading angle, heading angular velocity and speed into the prediction kinematics sub-model to obtain the initial position and initial heading angle of the AUV at the next moment;

[0018] The current speed, heading angular velocity and rudder angle are input into the prediction dynamics sub-model to obtain the initial speed and initial heading angular velocity of the AUV at the next moment.

[0019] According to one embodiment of the present application, the expression of the state space equation is as follows:

[0020] X(k+1)=f(X(k),τ(k))+G(k)w(k)

[0021] d(k)=h(X(k))+ε(k)

[0022] where f(·) is the nonlinear state transfer equation, X(k+1) is the state value at the next moment, X(k) is the state value at the current moment, τ(k) is the rudder angle, G(k) is the noise driving matrix, w(k) is the process noise matrix, ε(k) is the observation noise, d(k) is the observation distance, and h(·) is the nonlinear observation equation.

[0023] According to one embodiment of the present application, the state correction formula is as follows:

[0024]

[0025] in, is the estimated state value of AUV at the next moment, is the initial estimated value of the state of the AUV at the next moment, K(k+1) is the Kalman gain, d(k+1) is the observed distance of the AUV at the next moment, and h(·) is the nonlinear observation equation.

[0026] According to one embodiment of the present application, the Kalman filter includes one of an extended Kalman filter, an unscented Kalman filter and a Kalman filter, and the AUV prediction motion model includes one of a five-degree-of-freedom prediction motion model, a data-driven model, and a mechanism-data hybrid model.

[0027] In a second aspect, the present application provides an AUV motion state estimation device based on virtual onboard control and Kalman filtering, the device comprising:

[0028] An acquisition module is used to obtain the heading angle, heading angular velocity, position, speed and observation distance of the AUV at the current moment, where the position includes the lateral position and the longitudinal position;

[0029] a processing module, configured to input the heading angle, heading angular velocity, and lateral position at the current moment into a virtual onboard controller to obtain the rudder angle of the AUV at the current moment, wherein the virtual onboard controller includes a LOS guidance and a heading autopilot;

[0030] An estimation module is configured to input the heading angle, heading angular velocity, position, speed, observation distance, and rudder angle at the current moment into a Kalman filter system to obtain a state estimate of the AUV at the next moment. The Kalman filter system includes an AUV predicted motion model, an observation model, and a Kalman filter. The state estimate includes position, heading angle, speed, and heading angular velocity.

[0031] In a third aspect, the present application provides an electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, the AUV motion state estimation method based on virtual onboard control and Kalman filtering as described in the first aspect above is implemented.

[0032] In a fourth aspect, the present application provides a non-transitory computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the AUV motion state estimation method based on virtual onboard control and Kalman filtering as described in the first aspect above.

[0033] In a fifth aspect, the present application provides a chip, which includes a processor and a communication interface, wherein the communication interface is coupled to the processor, and the processor is used to run programs or instructions to implement the AUV motion state estimation method based on virtual onboard control and Kalman filtering as described in the first aspect.

[0034] In a sixth aspect, the present application provides a computer program product, comprising a computer program, which, when executed by a processor, implements the AUV motion state estimation method based on virtual onboard control and Kalman filtering as described in the first aspect above.

[0035] Additional aspects and advantages of the present application will be given in part in the description below, and in part will become obvious from the description below, or will be learned through practice of the present application.

[0036] The present invention provides an AUV motion state estimation method based on virtual onboard control and Kalman filtering, which has the following advantages over the prior art:

[0037] (1) The present invention obtains the heading angle, heading angular velocity, position, speed and observation distance of the AUV at the current moment and inputs them into the virtual onboard controller and Kalman filter system. It combines the LOS guidance and heading autopilot in the virtual onboard controller with the AUV prediction motion model and observation model in the Kalman filter system. The Kalman filter is used to correct the estimated AUV motion state error caused by the AUV prediction motion model parameter error and the virtual onboard control error, thereby realizing the estimation of the AUV motion state, improving the accuracy of the AUV motion state estimation and the efficiency of returning to the dock, and reducing potential operational risks.

[0038] (2) The present invention obtains the rudder angle command of the AUV through the LOS guidance and bow automatic rudder in the virtual onboard controller. Under the condition that the docking station perceives the AUV motion state information with only measurement information but no AUV self-feedback information, a virtual onboard controller is established to simulate the AUV rudder angle change, thereby realizing the estimation of the AUV motion state and the control of the AUV rudder angle, and improving the control accuracy of the AUV during navigation.

[0039] (3) The present invention inputs the current rudder angle, heading angle, heading angular velocity, position, speed and observation distance into the AUV prediction motion model to obtain the initial estimated value of the AUV state at the next moment, and establishes a state space equation in combination with the observation model. This information is input into the Kalman filter for state correction, which effectively improves the accuracy of AUV state estimation and enhances the navigation accuracy and stability of the AUV. By estimating and correcting the AUV state, the AUV can be better returned to the docking station smoothly, thereby improving the autonomy and safety of the AUV in the process of returning to the docking station and reducing the navigation risk caused by estimation errors. BRIEF DESCRIPTION OF THE DRAWINGS

[0040] The above and / or additional aspects and advantages of the present application will become apparent and easily understood from the description of the embodiments in conjunction with the following drawings, in which:

[0041] Figure 1 This is one of the flow charts of the AUV motion state estimation method based on virtual onboard control and Kalman filtering provided in an embodiment of the present application;

[0042] Figure 2 This is the second flow chart of the AUV motion state estimation method based on virtual onboard control and Kalman filtering provided in an embodiment of the present application;

[0043] Figure 3 Schematic diagram of the structure of an AUV motion state estimation device based on virtual onboard control and Kalman filtering provided in an embodiment of the present application;

[0044] Figure 4 It is a structural diagram of an electronic device provided in an embodiment of the present application. DETAILED DESCRIPTION

[0045] The following will be combined with the accompanying drawings in the embodiments of this application to clearly describe the technical solutions in the embodiments of this application. Obviously, the embodiments described are part of the embodiments of this application, not all of the embodiments. Based on the embodiments in this application, all other embodiments obtained by ordinary technicians in this field are within the scope of protection of this application.

[0046] The terms "first," "second," and the like in the specification and claims of this application are used to distinguish similar objects, and are not used to describe a specific order or precedence. It should be understood that the terms used in this manner are interchangeable where appropriate, so that the embodiments of this application can be implemented in an order other than that illustrated or described herein, and that the objects distinguished by "first," "second," and the like are generally of the same type, and do not limit the number of objects; for example, the first object can be one or more. In addition, the term "and / or" in the specification and claims refers to at least one of the connected objects, and the character " / " generally indicates that the objects connected are in an "or" relationship.

[0047] In the following, in combination with the accompanying drawings, the AUV motion state estimation method based on virtual onboard control and Kalman filtering, the AUV motion state estimation device based on virtual onboard control and Kalman filtering, the electronic device and the readable storage medium provided in the embodiments of the present application are described in detail through specific embodiments and their application scenarios.

[0048] Among them, the AUV motion state estimation method based on virtual onboard control and Kalman filtering can be applied to the terminal, and can be specifically executed by hardware or software in the terminal.

[0049] The terminal includes, but is not limited to, a portable communication device such as a mobile phone or tablet computer having a touch-sensitive surface (e.g., a touch screen display and / or a touch pad). It should also be understood that, in some embodiments, the terminal may not be a portable communication device, but a desktop computer having a touch-sensitive surface (e.g., a touch screen display and / or a touch pad).

[0050] In the following embodiments, a terminal including a display and a touch-sensitive surface is described. However, it should be understood that the terminal may include one or more other physical user interface devices such as a physical keyboard, a mouse, and a joystick.

[0051] The embodiment of the present application provides an AUV motion state estimation method based on virtual onboard control and Kalman filtering. The execution subject of the AUV motion state estimation method based on virtual onboard control and Kalman filtering can be an electronic device or a functional module or functional entity in the electronic device that can implement the AUV motion state estimation method based on virtual onboard control and Kalman filtering. The electronic devices mentioned in the embodiment of the present application include but are not limited to mobile phones, tablet computers, computers, cameras and wearable devices. The following describes the AUV motion state estimation method based on virtual onboard control and Kalman filtering provided in the embodiment of the present application using an electronic device as an example of the execution subject.

[0052] In recent years, autonomous underwater vehicles, as a new type of underwater unmanned exploration technology, have been widely used by various countries in military and civilian fields. Limited by the energy they carry and the underwater communication rate, AUVs find it difficult to work underwater for a long time. In order to extend the working time of AUVs, expand the operating range, and meet the needs of AUVs for wide-area seabed terrain exploration, information collection, and special operations, AUVs need to be regularly recovered for energy replenishment. If an AUV wants to successfully enter the docking station, it needs at least motion state information such as position, attitude, and speed. However, in shallow water environments, due to the measurement errors of the sensors themselves, the pollution of random noise, and the interference of underwater acoustic reverberation, the reliability of the sensor measurement data is reduced, and there are a large number of wild values in the observation data, which ultimately leads to deviations in the acquired AUV motion state and does not meet the actual use requirements.

[0053] An extended Kalman filter (ELF) filters data from a global navigation and positioning system (GNSS) and a Doppler velocimeter (DVP), enabling real-time estimation of the position, velocity, and heading of a navigating unmanned surface vehicle (USV). Experimental results demonstrate that this method effectively estimates the motion state of an UUV. Doppler velocimeters can only output high-precision velocity information when bottom tracking is established. Acoustic guidance methods such as USBL (USBL) present significant challenges in trajectory tracking and control during autonomous and safe AUV docking. AUVs often navigate underwater, where electromagnetic signals rapidly attenuate, preventing GNSS from effectively acquiring their position and attitude. Kalman filtering eliminates outliers in the USBL data, resulting in a more accurate AUV motion state. However, this method is only effective for AUVs that are stationary or in linear motion. AUVs often experience nonlinear motion during docking. Currently used motion models can only approximate this nonlinear motion and cannot accurately describe the AUV's motion during docking, thus failing to guarantee high-precision AUV motion state estimation.

[0054] Figure 1 This is one of the flow charts of the AUV motion state estimation method based on virtual onboard control and Kalman filtering provided in the embodiment of the present application, such as Figure 1 As shown, the AUV motion state estimation method based on virtual onboard control and Kalman filtering includes: step 110, step 120 and step 130.

[0055] Step 110: Obtain the heading angle, heading angular velocity, position, speed, and observation distance of the AUV at the current moment, where the position includes the lateral position and the longitudinal position;

[0056] It's easy to understand that USBL is an underwater positioning system used to locate autonomous underwater vehicles (AUVs). USBL uses signals transmitted between an underwater positioning dock and the AUV to determine the AUV's position. The USBL system transmits acoustic signals via a surface platform transceiver, which are received and replied by the AUV's onboard transponder. The USBL system calculates the azimuth angle using the phase difference between each hydrophone in the receiving array. It then uses the difference in round-trip signal propagation time and the speed of sound to infer the slant range, thereby determining the AUV's position and distance.

[0057] During the operation of the AUV, the electronic equipment obtains the heading angle, heading angular velocity, position, speed and observation distance of the AUV at the current moment. The position of the docking station is taken as the end point of the path, the opposite direction of the docking station opening is the heading angle, and the heading angular velocity is the changing rate of the AUV's heading angle.

[0058] The USBL system can calculate the three-dimensional position of the AUV through acoustic wave signals. The position of the AUV includes the lateral position and the longitudinal position. The lateral position is the horizontal coordinate of the AUV, and the longitudinal position is the vertical coordinate of the AUV.

[0059] The speed is the speed of the AUV in the water, and the speed of the AUV can be calculated by analyzing the relationship between the underwater position change of the AUV and time.

[0060] The observation distance is the distance between the dock and the AUV. The USBL system can obtain the observation distance of the AUV by measuring the signal propagation time between the AUV and the dock.

[0061] Step 120: Input the current heading angle, heading angular velocity, and lateral position into a virtual onboard controller to obtain the AUV's current rudder angle, wherein the virtual onboard controller includes LOS guidance and heading autopilot.

[0062] Furthermore, the electronic equipment inputs the AUV's current heading angle, heading angular velocity, and lateral position into a virtual onboard controller, which simulates the AUV's motion control system, including LOS (Line of Sight) guidance and heading autopilot. The virtual onboard controller generates a desired heading command using a LOS algorithm and implements heading tracking control using a heading autopilot (e.g., a PID controller or adaptive PD controller).

[0063] The heading autopilot calculates the rudder angle command of the AUV at the current moment according to the AUV heading angle, heading angular velocity, expected heading angle and expected heading angular velocity at the current moment.

[0064] Step 130: Input the heading angle, heading angular velocity, position, speed, observation distance, and rudder angle at the current moment into a Kalman filter system to obtain a state estimate of the AUV at the next moment. The Kalman filter system includes an AUV predicted motion model, an observation model, and a Kalman filter. The state estimate includes position, heading angle, speed, and heading angular velocity.

[0065] Finally, the heading angle, heading angular velocity, position, speed, observation distance and rudder angle at the current moment are input into the Kalman filter system. The Kalman filter system includes the AUV prediction motion model, observation model and Kalman filter. The AUV motion state is first estimated by the AUV prediction motion model, and then the estimated state is corrected by the Kalman filter to obtain the AUV state estimate at the next moment.

[0066] According to the AUV motion state estimation method based on virtual onboard control and Kalman filtering provided in an embodiment of the present application, the heading angle, heading angular velocity, position, speed and observation distance of the AUV at the current moment are obtained and input into the virtual onboard controller and Kalman filtering system. The LOS guidance and heading autopilot in the virtual onboard controller are combined with the AUV predicted motion model and observation model in the Kalman filtering system. The Kalman filter is used to correct the estimated AUV motion state error caused by the AUV predicted motion model parameter error and the virtual onboard control error, thereby realizing the estimation of the AUV motion state, improving the accuracy of the AUV motion state estimation and the efficiency of returning to the docking station, and reducing potential operational risks.

[0067] In some embodiments, inputting the current heading angle, heading angular velocity, and lateral position into a virtual onboard controller to obtain the rudder angle of the AUV at the current moment includes:

[0068] Inputting the heading angle and lateral position of the AUV at the current moment into LOS guidance to obtain a desired heading angle;

[0069] The desired heading angle, the desired heading angular velocity, the current heading angle and the current heading angular velocity are input into the heading autopilot to obtain the rudder angle of the AUV at the current moment.

[0070] It should be noted that the return of the AUV to the dock can be regarded as a path tracking problem. A desired path is planned with the dock position as the path end point and the opposite direction of the dock opening as the desired path angle. The AUV controls the rudder angle to track the desired path while maintaining a constant speed.

[0071] Considering the underactuated characteristics of the AUV, LOS guidance is used at the kinematic level to plan the desired heading angle, converting the path tracking problem into a heading angle tracking problem. In order to make the AUV arrive at the dock more smoothly, the calculation formula of the desired heading angle derived from the LOS system is as follows:

[0072] ψ d (k)=ψ dock (k)+atan(x(k) / Δ)

[0073] Among them, ψ dock (k) is the heading of the docking station, Δ is the sight angle, ψ d (k) is the desired heading angle of the AUV at the kth moment, and x(k) is the lateral position of the AUV at the kth moment.

[0074] The formula for calculating the desired heading angular velocity is as follows:

[0075]

[0076] Among them, r d (k) is the expected heading angular velocity of the AUV at the kth moment, and T is the sampling period.

[0077] At the dynamics layer, the heading autopilot is designed to manipulate the rudder to drive the AUV to track the desired heading angle derived from the LOS control. The calculation formula for the heading autopilot rudder angle designed by the virtual onboard dynamics control law is as follows:

[0078] τ(k)=k p (ψ d (k)-ψ(k))+k d (r d (k)-r(k))

[0079] Among them, k p and k d is the dynamic control parameter, τ(k) is the rudder angle, ψ(k) is the heading angle of the AUV at the kth moment, and r(k) is the heading angular velocity of the AUV at the kth moment.

[0080] It is worth noting that in order to ensure the navigation safety of the AUV, the input of the rudder angle should not be too large and should be limited to a safe range. The expression of the rudder angle input limit is as follows:

[0081]

[0082] Among them, τ max >0 means saturation limit of rudder angle.

[0083] It should be noted that the virtual onboard controller can be designed according to the actual control system of the AUV.

[0084] In this embodiment, the rudder angle command of the AUV is obtained through the LOS guidance and bow autopilot in the virtual onboard controller. Under the condition that the docking station perceives the AUV motion state information with only measurement information but no feedback information from the AUV itself, a virtual onboard controller is established to simulate the AUV rudder angle change, thereby realizing the estimation of the AUV motion state and the control of the AUV's rudder angle, and improving the control accuracy of the AUV during navigation.

[0085] In some embodiments, inputting the rudder angle at the current moment into a Kalman filter system to obtain a state estimate of the AUV at the next moment includes:

[0086] Inputting the heading angle, heading angular velocity, position, speed, observation distance and rudder angle at the current moment into an AUV prediction motion model to obtain an initial estimate of the state of the AUV at the next moment, the AUV prediction motion model including a prediction kinematics sub-model and a prediction dynamics sub-model;

[0087] establishing an observation model based on the observation distance;

[0088] The heading angle, heading angular velocity, position, speed, observation distance, rudder angle and initial estimated value of the state at the next moment at the current moment are input into the Kalman filter, and a state space equation is established in combination with the observation model. The initial estimated value of the state of the AUV at the next moment is corrected by the state correction formula to obtain the estimated value of the state of the AUV at the next moment.

[0089] It is easy to understand that the calculation formula of the observation model is as follows:

[0090]

[0091] Where ε(k) is the observation noise and d(k) is the observation distance.

[0092] In some embodiments, the state space equation is established with the AUV position, velocity and attitude as the state variables and the USBL distance as the observation variable. When the Kalman filter is an extended Kalman filter, since the state space equation is nonlinear, the Jacobian matrix F(k) and H(k) are required when using the extended Kalman filter. The calculation formula is as follows:

[0093]

[0094] Among them, X i (k), i=1,2,3,4,5 is the i-th state quantity of state quantity X(k), Tα1≥0 is the inverse of the maneuvering time constant, N ur ,I zz , N uτ is the hydrodynamic parameter.

[0095] Table 1 is a flow chart of the fusion of virtual onboard control and Kalman filter estimation of AUV state provided by an embodiment of the present application. As shown in Table 1, the Kalman filter is initialized each time the AUV starts to perform the return to dock mission before entering the water. The current USBL position, attitude and speed are used as the initial state of the Kalman filter, the opposite direction of the dock opening is used as the expected path angle, and the state covariance matrix is reset.

[0096] During the navigation process of the AUV, each time the AUV measurement distance data is obtained from the USBL, a control-prediction-correction iteration is performed to correct the initial estimated value of the AUV state at the next moment, and the estimated value of the AUV state at the next moment is obtained. Among them, Q(k) and R(k+1) are the variance matrices of the process noise w(k) and the observation noise ε(k), respectively. and is the estimated state and its covariance matrix, and is the estimated state and its covariance matrix. X(k)=[x(k),y(k),ψ(k),U(k),r(k)] T is the filtering state; G(k)=[0,0,0,T,T] T is the noise driving matrix; w(k)=[w1(k),w2(k)] T is the process noise matrix.

[0097] Table 1 Estimation of AUV nonlinear motion state by using extended Kalman filter integrated with virtual onboard control

[0098]

[0099]

[0100] In this embodiment, the rudder angle, heading angle, heading angular velocity, position, speed and observation distance at the current moment are input into the AUV prediction motion model to obtain the initial estimated value of the AUV state at the next moment, and the state space equation is established in combination with the observation model. This information is input into the Kalman filter for state correction, which effectively improves the accuracy of the AUV state estimation and enhances the navigation accuracy and stability of the AUV. By estimating and correcting the AUV state, the AUV can be better returned to the docking station smoothly, thereby improving the autonomy and safety of the AUV during the return to the docking station and reducing the navigation risk caused by estimation errors.

[0101] In some embodiments, inputting the heading angle, heading angular velocity, position, speed, observation distance, and rudder angle at the current moment into an AUV prediction motion model to obtain an initial estimate of the state of the AUV at the next moment includes:

[0102] Inputting the current position, heading angle, heading angular velocity and speed into the prediction kinematics sub-model to obtain the initial position and initial heading angle of the AUV at the next moment;

[0103] The current speed, heading angular velocity and rudder angle are input into the prediction dynamics sub-model to obtain the initial speed and initial heading angular velocity of the AUV at the next moment.

[0104] It is easy to understand that for AUV motion state estimation, in order to better describe the AUV motion state and facilitate the implementation of the Kalman filter algorithm, it is necessary to model the AUV motion. Common motion models include the uniform velocity model, uniform acceleration model, and Singer model, but these models can only approximately represent the AUV's motion within a certain period of time and cannot accurately describe the nonlinear motion of the AUV during the entire process of docking.

[0105] In some embodiments, the change of the bow angular velocity is relatively complex and difficult to express using common models. Therefore, based on the change law of the AUV bow angular velocity, an AUV bow dynamics model is established. The AUV bow dynamics model is integrated on the basis of the Singer model, and an AUV predictive motion model is established with the docking station coordinate system as the reference coordinate system. The AUV predictive motion model includes a predictive kinematics sub-model and a predictive dynamics sub-model.

[0106] The current position, heading angle, heading angular velocity and speed are input into the predictive kinematics sub-model to obtain the initial position and initial heading angle of the AUV at the next moment. The calculation formula of the predictive kinematics sub-model is as follows:

[0107] x(k+1)=x(k)+TU(k)cos(ψ(k))

[0108] y(k+1)=y(k)+TU(k)sin(ψ(k))

[0109] ψ(k+1)=ψ(k)+Tr(k)

[0110] Among them, x(k) is the lateral position of the AUV at the kth moment, y(k) is the longitudinal position of the AUV at the kth moment, and U(k) is the speed of the AUV at the kth moment.

[0111] The current speed, heading angular velocity, and rudder angle are input into the prediction dynamics sub-model to obtain the initial speed and initial heading angular velocity of the AUV at the next moment. The calculation formula of the prediction dynamics sub-model is as follows:

[0112] U(k+1)=(1-Tα1)U(k)+Tw1(k)

[0113]

[0114] Where w1(k) is the first process noise, w2(k) is the second process noise, Tα1≥0 is the inverse of the maneuvering time constant, N ur ,I zz , N uτ is the hydrodynamic parameter.

[0115] In some embodiments, w2(k) includes both the heading angular velocity estimation error and the dynamic modeling error. It is assumed that the two errors are a random process represented by Gaussian white noise.

[0116] In this embodiment, by establishing an AUV prediction motion model and comprehensively considering the factors of kinematics and dynamics, the motion state of the AUV can be predicted more accurately, providing a more accurate initial value for subsequent state correction, optimizing the navigation control strategy, enhancing the adaptability of the AUV in complex environments, helping the AUV to return to the dock smoothly, and reducing the risks caused by prediction errors.

[0117] In some embodiments, the state-space equation is expressed as follows:

[0118] X(k+1)=f(X(k),τ(k))+G(k)w(k)

[0119] d(k)=h(X(k))+ε(k)

[0120] where f(·) is the nonlinear state transfer equation, X(k+1) is the state value at the next moment, X(k) is the state value at the current moment, τ(k) is the rudder angle, G(k) is the noise driving matrix, w(k) is the process noise matrix, ε(k) is the observation noise, d(k) is the observation distance, and h(·) is the nonlinear observation equation.

[0121] In this embodiment, by establishing the state-space equation and combining the rudder angle, process noise matrix, noise driving matrix and observation noise, the estimation of the AUV motion state is achieved, which improves the accuracy of the AUV motion state estimation and the efficiency of returning to the docking station, and reduces potential operational risks.

[0122] In some embodiments, the state correction formula is as follows:

[0123]

[0124] in, is the estimated state value of AUV at the next moment, is the initial estimated value of the state of the AUV at the next moment, K(k+1) is the Kalman gain, d(k+1) is the observed distance of the AUV at the next moment, and h(·) is the nonlinear observation equation.

[0125] In some embodiments, Figure 2 This is a second flow chart of the AUV motion state estimation method based on virtual onboard control and Kalman filtering provided in an embodiment of the present application, such as Figure 2 As shown, LOS guidance is based on the docking station heading ψ dock (k) and the AUV lateral position x(k) to plan the desired heading angle ψ d (k) and the desired heading angular velocity r d (k). Then, the heading autopilot calculates the command rudder angle τ(k) as the control input of the extended Kalman filter based on the current AUV heading angle ψ(k) and heading angular velocity r(k). Then, the AUV motion state is estimated based on the predictive dynamic model and the predictive kinematic model. Finally, the observation model is established using the USBL observation distance d(k), and the estimated state is corrected to obtain the state estimate of the AUV at the next moment.

[0126] In this embodiment, the state correction formula is used to correct the AUV's motion state, which effectively improves the accuracy of the AUV's motion state estimation, enables the AUV to return to the docking station smoothly, improves the AUV's autonomy and safety during the return to the docking station, and reduces the navigation risk caused by estimation errors.

[0127] In some embodiments, the Kalman filter includes one of an extended Kalman filter, an unscented Kalman filter, and a Kalman filter, and the AUV prediction motion model includes one of a five-degree-of-freedom prediction motion model, a data-driven model, and a mechanism-data hybrid model.

[0128] It is worth noting that different Kalman filters can be selected according to whether the state and process of the AUV in different application scenarios are linear or nonlinear. If the state of the AUV is linear, the Kalman filter can be used. The Kalman filter estimates the state of the AUV by combining the predicted value and the measured value and minimizes the estimation error. If the state of the AUV is nonlinear, the extended Kalman filter or the unscented Kalman filter can be used.

[0129] It should be noted that the AUV prediction motion model includes one of the five-degree-of-freedom prediction motion model, the data-driven model, and the mechanism data hybrid model, which can be flexibly selected according to the actual application scenario.

[0130] In this embodiment, the AUV motion state is filtered and corrected by selecting a suitable Kalman filter and AUV prediction motion model according to the characteristics of the AUV motion state and the specific application scenario, thereby effectively improving the accuracy of the AUV motion state estimation, enabling more accurate state estimation in different environments, reducing the prediction error, and enabling the AUV to return to the docking station smoothly. This improves the autonomy and safety of the AUV during the return to the docking station and reduces the navigation risk caused by estimation errors.

[0131] The AUV motion state estimation method based on virtual onboard control and Kalman filtering provided in the embodiments of the present application can be executed by an AUV motion state estimation device based on virtual onboard control and Kalman filtering. In the embodiments of the present application, the AUV motion state estimation method based on virtual onboard control and Kalman filtering is executed by the AUV motion state estimation device based on virtual onboard control and Kalman filtering as an example to illustrate the AUV motion state estimation device based on virtual onboard control and Kalman filtering provided in the embodiments of the present application.

[0132] The embodiment of the present application also provides an AUV motion state estimation device based on virtual onboard control and Kalman filtering, such as Figure 3 As shown, the AUV motion state estimation device based on virtual onboard control and Kalman filtering includes: an acquisition module 310 , a processing module 320 and an estimation module 330 .

[0133] An acquisition module 310 is used to obtain the heading angle, heading angular velocity, position, speed and observation distance of the AUV at the current moment, where the position includes the lateral position and the longitudinal position;

[0134] The processing module 320 is configured to input the heading angle, heading angular velocity, and lateral position at the current moment into a virtual onboard controller to obtain the rudder angle of the AUV at the current moment, wherein the virtual onboard controller includes LOS guidance and heading autopilot;

[0135] The estimation module 330 is used to input the heading angle, heading angular velocity, position, speed, observation distance and rudder angle at the current moment into the Kalman filter system to obtain the state estimation value of the AUV at the next moment. The Kalman filter system includes an AUV predicted motion model, an observation model and a Kalman filter. The state estimation value includes position, heading angle, speed and heading angular velocity.

[0136] According to the AUV motion state estimation method based on virtual onboard control and Kalman filtering provided in an embodiment of the present application, the heading angle, heading angular velocity, position, speed and observation distance of the AUV at the current moment are obtained and input into the virtual onboard controller and Kalman filtering system. The LOS guidance and heading autopilot in the virtual onboard controller are combined with the AUV predicted motion model and observation model in the Kalman filtering system. The Kalman filter is used to correct the estimated AUV motion state error caused by the AUV predicted motion model parameter error and the virtual onboard control error, thereby realizing the estimation of the AUV motion state, improving the accuracy of the AUV motion state estimation and the efficiency of returning to the docking station, and reducing potential operational risks.

[0137] The AUV motion state estimation device based on virtual onboard control and Kalman filtering provided in the embodiment of the present application can achieve Figures 1 to 2 To avoid repetition, the various processes implemented in the embodiment of the AUV motion state estimation method based on virtual onboard control and Kalman filtering are not described here.

[0138] In some embodiments, as Figure 4 As shown, an embodiment of the present application further provides an electronic device 400, including a processor 401, a memory 402, and a computer program stored in the memory 402 and executable on the processor 401. When the program is executed by the processor 401, each process of the embodiment of the AUV motion state estimation method based on virtual onboard control and Kalman filtering is implemented, and the same technical effect can be achieved. To avoid repetition, it will not be described here.

[0139] It should be noted that the electronic devices in the embodiments of the present application include the mobile electronic devices and non-mobile electronic devices mentioned above.

[0140] An embodiment of the present application also provides a non-transitory computer-readable storage medium, on which a computer program is stored. When the computer program is executed by a processor, the various processes of the above-mentioned AUV motion state estimation method embodiment based on virtual onboard control and Kalman filtering are implemented, and the same technical effect can be achieved. To avoid repetition, it will not be repeated here.

[0141] The processor is the processor in the electronic device in the above embodiment. The readable storage medium includes a computer-readable storage medium, such as a computer read-only memory (ROM), a random access memory (RAM), a magnetic disk, or an optical disk.

[0142] An embodiment of the present application also provides a computer program product, including a computer program, which, when executed by a processor, implements the above-mentioned AUV motion state estimation method based on virtual onboard control and Kalman filtering.

[0143] The processor is the processor in the electronic device in the above embodiment. The readable storage medium includes a computer readable storage medium, such as a computer read-only memory ROM, a random access memory RAM, a magnetic disk or an optical disk.

[0144] An embodiment of the present application further provides a chip, which includes a processor and a communication interface, which is coupled to the processor. The processor is used to run programs or instructions to implement the various processes of the above-mentioned AUV motion state estimation method embodiment based on virtual onboard control and Kalman filtering, and can achieve the same technical effect. To avoid repetition, it will not be repeated here.

[0145] It should be understood that the chip mentioned in the embodiments of the present application can also be called a device-level chip, a device chip, a chip device, or an on-chip device chip, etc.

[0146] It should be noted that, in this article, the terms "comprise", "include" or any other variants thereof are intended to cover non-exclusive inclusion, so that a process, method, article or device comprising a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, article or device. In the absence of further restrictions, an element defined by the statement "comprises a ..." does not exclude the presence of other identical elements in the process, method, article or device comprising the element. In addition, it should be noted that the scope of the methods and devices in the embodiments of the present application is not limited to performing functions in the order shown or discussed, and may also include performing functions in a substantially simultaneous manner or in the opposite order according to the functions involved. For example, the described method may be performed in an order different from that described, and various steps may also be added, omitted, or combined. In addition, the features described with reference to certain examples may be combined in other examples.

[0147] Through the description of the above implementation methods, those skilled in the art can clearly understand that the above-mentioned embodiment methods can be implemented by means of software plus the necessary general hardware platform, and of course, by hardware, but in many cases the former is a better implementation method. Based on this understanding, the technical solution of the present application, or the part that contributes to the prior art, can be embodied in the form of a computer software product, which is stored in a storage medium (such as ROM / RAM, magnetic disk, optical disk), and includes a number of instructions for enabling a terminal (which can be a mobile phone, computer, server, or network device, etc.) to execute the AUV motion state estimation method based on virtual onboard control and Kalman filtering in each embodiment of the present application.

[0148] In the description of this application, "first feature" and "second feature" may include one or more such features.

[0149] In the description of this application, “plurality” means two or more.

[0150] The embodiments of the present application are described above in conjunction with the accompanying drawings, but the present application is not limited to the above-mentioned specific implementation methods. The above-mentioned specific implementation methods are merely illustrative and not restrictive. Under the guidance of this application, ordinary technicians in this field can also make many forms without departing from the purpose of this application and the scope of protection of the claims, all of which are within the protection of this application.

[0151] Throughout this specification, reference to terms such as "one embodiment," "some embodiments," "illustrative embodiments," "examples," "specific examples," or "some examples" means that the specific features, structures, materials, or characteristics described in conjunction with the embodiment or example are included in at least one embodiment or example of the present application. In this specification, the illustrative expressions of the above terms do not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials, or characteristics described may be combined in any suitable manner in any one or more embodiments or examples.

[0152] Although the embodiments of the present application have been shown and described, those skilled in the art will appreciate that various changes, modifications, substitutions, and variations may be made to the embodiments without departing from the principles and purpose of the present application, and that the scope of the present application is defined by the claims and their equivalents.

Claims

1. A method for estimating the motion state of an AUV based on virtual onboard control and Kalman filtering, characterized in that: The method comprises: Obtain the heading angle, heading angular velocity, position, speed and observation distance of the AUV at the current moment, where the position includes the lateral position and the longitudinal position; Inputting the heading angle, heading angular velocity, and lateral position at the current moment into a virtual onboard controller to obtain the rudder angle of the AUV at the current moment, wherein the virtual onboard controller includes LOS guidance and heading autopilot; The heading angle, heading angular velocity, position, speed, observation distance and rudder angle at the current moment are input into the Kalman filter system to obtain the state estimation value of the AUV at the next moment. The Kalman filter system includes an AUV predicted motion model, an observation model and a Kalman filter. The state estimation value includes position, heading angle, speed and heading angular velocity.

2. The AUV motion state estimation method based on virtual onboard control and Kalman filtering according to claim 1 is characterized in that: Inputting the heading angle, heading angular velocity, and lateral position at the current moment into the virtual onboard controller to obtain the rudder angle of the AUV at the current moment includes: Inputting the heading angle and lateral position of the AUV at the current moment into LOS guidance to obtain a desired heading angle and a desired heading angular velocity; The desired heading angle, the desired heading angular velocity, the current heading angle and the current heading angular velocity are input into the heading autopilot to obtain the rudder angle of the AUV at the current moment.

3. The AUV motion state estimation method based on virtual onboard control and Kalman filtering according to claim 1 is characterized in that: The inputting of the rudder angle at the current moment into the Kalman filter system to obtain the state estimation value of the AUV at the next moment includes: Inputting the heading angle, heading angular velocity, position, speed, observation distance and rudder angle at the current moment into an AUV prediction motion model to obtain an initial estimate of the state of the AUV at the next moment, the AUV prediction motion model including a prediction kinematics sub-model and a prediction dynamics sub-model; establishing an observation model based on the observation distance; The heading angle, heading angular velocity, position, speed, observation distance, rudder angle and initial estimated value of the state at the next moment at the current moment are input into the Kalman filter, and a state space equation is established in combination with the observation model. The initial estimated value of the state of the AUV at the next moment is corrected by the state correction formula to obtain the estimated value of the state of the AUV at the next moment.

4. The AUV motion state estimation method based on virtual onboard control and Kalman filtering according to claim 3 is characterized in that: The present moment heading angle, heading angular velocity, position, speed, observation distance and rudder angle are input into the AUV prediction motion model to obtain the initial estimated value of the AUV state at the next moment, including: Inputting the current position, heading angle, heading angular velocity and speed into the prediction kinematics sub-model to obtain the initial position and initial heading angle of the AUV at the next moment; The current speed, heading angular velocity and rudder angle are input into the prediction dynamics sub-model to obtain the initial speed and initial heading angular velocity of the AUV at the next moment.

5. The AUV motion state estimation method based on virtual onboard control and Kalman filtering according to claim 3 is characterized in that: The state space equation is expressed as follows: X(k+1)=f(X(k),τ(k))+G(k)w(k) d(k)=h(X(k))+ε(k) where f(·) is the nonlinear state transfer equation, X(k+1) is the state value at the next moment, X(k) is the state value at the current moment, τ(k) is the rudder angle, G(k) is the noise driving matrix, w(k) is the process noise matrix, ε(k) is the observation noise, d(k) is the observation distance, and h(·) is the nonlinear observation equation.

6. The AUV motion state estimation method based on virtual onboard control and Kalman filtering according to claim 3 is characterized in that: The state correction formula is as follows: in, is the estimated state value of AUV at the next moment, is the initial estimated value of the state of the AUV at the next moment, K(k+1) is the Kalman gain, d(k+1) is the observed distance of the AUV at the next moment, and h(·) is the nonlinear observation equation.

7. The AUV motion state estimation method based on virtual onboard control and Kalman filtering according to claim 1 is characterized in that: The Kalman filter includes one of an extended Kalman filter, an unscented Kalman filter and a Kalman filter, and the AUV prediction motion model includes one of a five-degree-of-freedom prediction motion model, a data-driven model and a mechanism-data hybrid model.

8. An AUV motion state estimation device based on virtual onboard control and Kalman filtering, implemented using the AUV motion state estimation method based on virtual onboard control and Kalman filtering according to any one of claims 1 to 7, characterized in that: The device comprises: An acquisition module is used to obtain the heading angle, heading angular velocity, position, speed and observation distance of the AUV at the current moment, where the position includes the lateral position and the longitudinal position; a processing module, configured to input the heading angle, heading angular velocity, and lateral position at the current moment into a virtual onboard controller to obtain the rudder angle of the AUV at the current moment, wherein the virtual onboard controller includes a LOS guidance and a heading autopilot; An estimation module is configured to input the heading angle, heading angular velocity, position, speed, observation distance, and rudder angle at the current moment into a Kalman filter system to obtain a state estimate of the AUV at the next moment. The Kalman filter system includes an AUV predicted motion model, an observation model, and a Kalman filter. The state estimate includes position, heading angle, speed, and heading angular velocity.

9. An electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein: When the processor executes the program, the AUV motion state estimation method based on virtual onboard control and Kalman filtering is implemented as described in any one of claims 1 to 7.

10. A non-transitory computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the AUV motion state estimation method based on virtual onboard control and Kalman filtering is implemented.

Images