Auv dynamic docking method, device and medium based on flow field state estimation and nonlinear model predictive control

Abstract

The application discloses an AUV dynamic docking method and device based on flow field state estimation and nonlinear model prediction control and a medium, adopts local affine description of current position nearby flow velocity space variation, makes AUV historical motion residual error be able to simultaneously reverse local flow velocity and flow field gradient, in the prediction control stage, establishes follow-up coordinate system with the real-time attitude of dynamic docking platform, the axial distance between AUV and platform, lateral deviation, attitude deviation and relative velocity are included in multi-step nonlinear prediction, the flow field parameter posterior covariance is propagated to the position of predicted trajectory, the docking channel and the approach speed limit are dynamically tightened according to the most unfavorable flow field uncertainty, and switched to the safe waiting state when the flow field information is insufficient. The application not only compensates the motion deviation caused by the flow field, but also directly participates in the future state prediction and docking safety decision, and can make up for the deficiencies of the existing large-curvature path tracking method in moving target, local variable flow field and docking safety constraint.

Description

Technical Field

[0001] This invention belongs to the field of autonomous underwater vehicle navigation control and underwater dynamic docking technology, and particularly relates to an AUV dynamic docking method, equipment and medium based on flow field state estimation and nonlinear model predictive control (NMPC). Background Technology

[0002] Existing dynamic docking methods for autonomous underwater vehicles (AUVs) typically construct control laws based on the AUV's kinematic model and the relative attitude of the docking target, treating ocean currents as external disturbances or compensating for them with constant flow velocity. While these methods can achieve basic docking under conditions of slow flow field changes and a relatively stationary docking platform, the flow velocity varies with spatial position due to the combined effects of local eddies, platform-around-flow, and depth changes. A single flow velocity estimate cannot accurately describe the differences in current reception during the AUV's approach from a distance to near-end docking. Furthermore, existing methods often focus on predicting the next moment's position or adjusting speed based on the current state, without fully considering whether the flow field gradient is identifiable, how flow field estimation errors propagate along the predicted trajectory, and whether the AUV should still be allowed to enter narrow docking channels when the estimation reliability is insufficient. This can easily lead to situations where the nominal predicted trajectory meets constraints, but the actual navigation is affected by unestimated flow deviations.

[0003] Chinese invention patent CN121877009B discloses a path planning method and system for multiple autonomous underwater vehicles (AUVs). This method addresses global path planning for multiple AUVs in complex three-dimensional underwater environments. It pre-constructs models of seabed topography, obstacles, and ocean currents, combining path length, collision threat, smoothness, and ocean current energy consumption into a multi-objective fitness function. An improved Star-Raven optimization algorithm is then used to search for the optimal path point sequence. However, the ocean current model primarily serves as known environmental information for path energy consumption evaluation and planning result selection. The resulting path is essentially a route plan from the starting position to a fixed target position. It does not address the online identification of local flow field changes with spatial position during actual AUV navigation, nor does it consider estimation errors arising from inconsistencies between the flow field model and real-time observations. Therefore, it cannot determine whether to continue entering the docking area based on the uncertainty of the flow field estimation.

[0004] Chinese invention patent CN121722156B discloses an adaptive tracking control method for a large curvature path of an underactuated underwater vehicle (AUV). This method calculates the curvature adaptive update rate, forward sight distance, and desired yaw and pitch angles using an improved line-of-sight method. It predicts the next-moment position based on an AUV model, optimizes the target speed using an improved particle swarm optimization algorithm, and compensates for external disturbances and model parameter uncertainties using a width-learning-assisted adaptive sliding mode controller. While considering the impact of external disturbances such as ocean currents on path tracking, its approach primarily unifies hydrodynamic uncertainties and external effects into a single, estimated, and compensated disturbance term. It does not further separate the local velocity baseline and velocity spatial gradient, failing to explain the flow field effects experienced by the AUV at different spatial locations. Furthermore, its control objective remains tracking a predetermined static curved path, focusing on the next-moment position and longitudinal speed. It does not establish a multi-step relative motion model of the AUV relative to the moving docking platform, nor does it propagate disturbance estimation errors to subsequent safety constraints. For dynamic docking, even if the nominal position error has been reduced, if the local flow field estimation is unreliable, the AUV may still fail to dock due to lateral drift or excessive approach speed.

[0005] Therefore, under the conditions that the local flow field changes with spatial location, the dynamic docking platform is in continuous motion, and the flow field observation information is limited, how to perform joint estimation of AUV navigation status, local velocity, and flow field gradient with observable constraints, how to uniformly introduce the flow field mean, spatial gradient, and their estimation uncertainty into dynamic docking prediction and control, and how to promptly limit or stop the docking process when the flow field estimation reliability is insufficient, all require further research to improve the accuracy of state prediction, near-end alignment capability, and safety during the AUV dynamic docking process. Summary of the Invention

[0006] The problem to be solved by this invention is to provide a dynamic docking method for AUVs based on flow field state estimation and nonlinear model predictive control. By jointly estimating the local affine flow field and the AUV's navigation state, predicting the flow field gradient using a nonlinear model, and driving safety constraint contraction based on flow field uncertainty, a high-precision and high-safety autonomous docking of AUVs and dynamic docking platforms under complex flow fields can be achieved.

[0007] This invention adopts the following technical solution: a dynamic docking method for AUVs based on flow field state estimation and nonlinear model predictive control, comprising the following steps:

[0008] Step 1: Within the scrolling history window, using the actual ground displacement, relative water movement, attitude, control input, and relative position and attitude between the AUV and the dynamic docking platform, an affine model composed of the velocity baseline and the flow field gradient is used to characterize the local flow field near the current position, and a unified parameterized mapping from spatial position change to local flow field change is established.

[0009] Step 2: Construct a joint estimation model of AUV navigation status, current position velocity, and local flow field gradient; evaluate the sufficiency of historical motion observation information by using the rank and minimum eigenvalue of the flow field gradient information matrix; when the observation information is insufficient, maintain the stability of the estimation problem by using the prior constraint of the flow field gradient, and retain the uncertainty of the unidentifiable direction by using the posterior covariance; jointly estimate and output the AUV navigation status, current local velocity, local flow field gradient, and posterior covariance of flow field parameters.

[0010] Step 3: Establish a follow-up docking coordinate system based on the real-time attitude of the dynamic docking platform. Construct the docking error state using the axial distance, lateral position deviation, attitude deviation, and relative velocity of the AUV relative to the docking platform. Propagate the estimated current flow velocity and flow field gradient along the predicted trajectory to obtain the local flow velocity corresponding to each position in the predicted time domain. Combine the candidate control sequence with the nonlinear AUV motion model to recursively deduce the future state and then optimize the control sequence.

[0011] Step 4: Propagate the posterior covariance of the flow field parameters into the velocity covariance of each prediction step based on the future predicted position, extract the most unfavorable flow field uncertainty in the prediction time domain, and continuously shrink the lateral position, attitude and axial approach velocity constraints based on the uncertainty.

[0012] Step 5: When the flow field estimation uncertainty meets the safety threshold, the controller will continue to complete the approach and docking within the contracted feasible region. When the flow field estimation uncertainty does not meet the safety threshold, the control target will be switched to a safe waiting state outside the docking channel, so that the AUV reduces the approach speed and maintains a safe distance. After the flow field observability improves and the estimated covariance decreases to the safety threshold, docking will resume.

[0013] Furthermore, in step 1, the specific processing is as follows:

[0014] First, at discrete time The position, attitude, and speed of the AUV are unified into a navigation state. :

[0015] ;

[0016] At the same time, the position, attitude, and speed of the dynamic docking platform are uniformly recorded as... superscript This indicates the status of the dynamic docking platform.

[0017] The current observations are generated by the AUV's own navigation sensors, relative distance sensors, relative orientation sensors, and docking platform status information. Establish basic data mapping relationships:

[0018] ;

[0019] in, Indicates the length of the history window. Indicates that AUV is in the first Time-based control input, This indicates the local flow velocity at the current location of the AUV. This represents the current local flow field gradient; Indicates from time Up to the current moment AUV navigation status sequence, This represents the observation sequence obtained within the corresponding time period. Indicates from time At that time The applied control input sequence.

[0020] This state construction adopts the unified description method of position, attitude, longitudinal and lateral velocities, and angular velocities in AUV nonlinear kinematics and dynamics, providing a foundation for subsequent state prediction.

[0021] Then, based on the current AUV position To select any position within the history window The local flow velocity at that point is expressed as :

[0022] ;

[0023] Define intermediate variables , :

[0024] ;

[0025] The local velocity baseline and gradient parameters are combined into a single estimator. :

[0026] ;

[0027] The local flow velocity can then be further written as:

[0028] ;

[0029] in, For the first Local flow field parameter mapping matrix corresponding to each historical observation location; It is a third-order identity matrix. For Kronecker product.

[0030] This completes a unified parameterized mapping from spatial location changes to local flow field changes:

[0031] .

[0032] This relationship indicates that the positional changes experienced by the AUV within the historical window are not only used to update its own navigation trajectory, but also to explain the differences in motion residuals at different positions, and further to invert the spatial changes in the local flow field.

[0033] Furthermore, in step 2, the specific processing is as follows:

[0034] First, based on the status, control input, and observation data within the historical window, a joint estimation relationship between the AUV's navigation status and local flow field parameters is established:

[0035] ;

[0036] in, The prior state as the starting point of the historical window. Its prior error covariance; This represents theoretical observations calculated from the AUV's status and the dynamic docking platform's status. For observation error covariance; This is a discrete AUV nonlinear motion model; The model error covariance; Let be the prior covariance of the flow field gradient.

[0037] The above estimation relationship can be understood as follows: by applying prior state constraints, observation consistency constraints, dynamic consistency constraints, and prior flow field gradient constraints, a joint estimation result is obtained. ; where superscript This represents an estimated value. Estimate the current AUV status. For the current local velocity estimation, This is the current local flow field gradient estimate.

[0038] Then, the actual ground motion of the AUV and its motion relative to the water body satisfy the following:

[0039] ;

[0040] in, This represents the velocity residual, which is composed of changes in adjacent positions and the relative velocity of the AUV to the water body. The sampling period is This represents the result of converting the AUV's velocity relative to the water body to the inertial coordinate system.

[0041] Substituting the local flow field model, we get:

[0042] ;

[0043] in, This represents the position observation error, velocity observation error, and model discretization error.

[0044] To determine whether the flow field gradient can be sufficiently identified from historical motion data, a system is constructed. For the first The flow field gradient regression matrix corresponding to each historical observation location is expressed as:

[0045] ;

[0046] and flow field gradient information matrix :

[0047] ;

[0048] in, For the velocity residual covariance.

[0049] when At that time, the complete flow field gradient matrix It has local identifiability.

[0050] Furthermore, by

[0051] ;

[0052] Therefore, the condition for the identifiability of the flow field gradient is equivalent to:

[0053] ;

[0054] Therefore, the richer the displacement changes within the historical window, the more sufficient the information for estimating the flow field gradient; if the AUV only approaches the interface along a single straight line, it can only identify the changes in the flow field in that direction of motion, but cannot fully recover the entire three-dimensional gradient.

[0055] After the joint estimation is completed, the posterior covariance of the flow field parameters is obtained based on the historical location and velocity residuals. :

[0056] ;

[0057] This results in the complete output of the posterior estimation result:

[0058] ;

[0059] in, Used to initialize future state predictions Used to determine the current flow field baseline value Used to predict flow velocity at different spatial locations. Used to characterize the reliability of flow field estimation.

[0060] Then, using the dynamic docking platform as a reference, a mobile docking coordinate system is established, and... The rotation matrix representing the attitude of the docking platform is as follows:

[0061] ;

[0062] in, To connect to the platform at any time The corresponding attitude rotation matrix; For dynamic connection platform at all times The three-dimensional pose; This is a three-dimensional rotational transformation;

[0063] The positional error of the AUV relative to the docking platform is: :

[0064] ;

[0065] Attitude error is :

[0066] ;

[0067] The relative speed error is :

[0068] ;

[0069] This results in a three-dimensional dynamic docking error state. :

[0070] .

[0071] This is achieved through the above relationships:

[0072] ;

[0073] In this coordinate system, the relative position error is always decomposed around the dynamic interface axis, and will not lose its clear axial and lateral meaning due to the rotation of the docking platform in the inertial coordinate system.

[0074] Furthermore, in step 3, the specific processing is as follows:

[0075] In the prediction time domain Inside, order Indicates the current moment For the future Step the predicted value of the AUV state, and let the initial state estimate , the future No. Predict the flow velocity at the step location The velocity baseline and gradient output from step 2 are used to determine the following:

[0076] ;

[0077] make:

[0078] ;

[0079] Then we have:

[0080] ;

[0081] in, For the current moment The next chapter The local flow field parameter mapping matrix corresponding to each predicted location is derived from the predicted location. Compared to the present Location The position offset is determined and used to convert the uniform flow field parameter estimate. Mapped to the future Estimated flow velocity at each predicted location ; For the current moment The future number of AUVs is obtained by recursion based on AUV state estimation, candidate control inputs, and nonlinear motion models. Predict the location step by step.

[0082] Therefore, the propagation relationship of the flow field parameters along the predicted trajectory is as follows:

[0083] ;

[0084] Based on this, the future state of the AUV is recursively deduced according to a discrete nonlinear model:

[0085] ;

[0086] in, For the current discrete time Based on the current state of the AUV, the predicted flow field, and the dynamic motion state of the docking platform, the following is aimed at the future... The candidate control inputs calculated in each prediction step are used to recursively predict the state through a discrete nonlinear motion model. .

[0087] Substituting the flow field prediction relationship, it can be written as:

[0088] ;

[0089] This forms a continuously progressive chain of state calculations within the prediction time domain:

[0090] .

[0091] Existing technologies have predicted the attitude, velocity, and position of the next moment based on AUV motion models and established control objective functions accordingly. This invention further adds a local flow field term that varies with the predicted position to the prediction relationship of existing technologies.

[0092] At the same time, propagating the covariance of the flow field parameters along the predicted location yields:

[0093] ;

[0094] Therefore, the flow field estimation results output from step 2 further form two synchronous propagation paths:

[0095] ;

[0096] The first path is used to calculate the mean of the future state, and the second path is used to calculate the uncertainty of the future flow field.

[0097] After predicting the future state, flow velocity, and docking error, a nonlinear predictive control optimization problem is constructed, and the optimal predictive control sequence is obtained. :

[0098] ;

[0099] in, For predicting control sequences:

[0100] ;

[0101] To control the input increment:

[0102] ;

[0103] Used to weigh relative position, attitude, and velocity errors; Used to suppress drastic changes in thruster control parameters; Used to control the remaining docking error at the end of the predicted time domain; and To control the input boundaries.

[0104] Therefore, we can conclude that:

[0105] ;

[0106] After obtaining the optimal control sequence, only the first control variable is executed. Then, at the next sampling time, the estimation, prediction, and solution are repeated.

[0107] Furthermore, in step 4, the specific processing is as follows:

[0108] Based on the flow field covariance over the entire prediction time domain, the uncertainty of the most unfavorable flow field is extracted. :

[0109] ;

[0110] in, The largest eigenvalue of the covariance matrix is ​​represented by this index, which characterizes the velocity estimation error at the most unfavorable location and in the most unfavorable direction within the prediction time domain, thus establishing:

[0111] ;

[0112] In the coordinate system of the dynamic docking platform, let Indicates the axial distance from the AUV to the interface. This indicates the lateral positional error perpendicular to the docking axis. Let the relative velocity components along the docking axis be represented, then the docking feasible region driven by flow field uncertainty is expressed as:

[0113] ;

[0114] Its mathematical transitivity is as follows:

[0115] ;

[0116] when When the value decreases, it indicates that the reliability of the flow field estimation improves and the feasible docking region gradually recovers to the deterministic boundary.

[0117] when As the distance increases, the allowable lateral position error, attitude error, and approach speed decrease simultaneously, thus requiring a greater safety margin between the nominal predicted trajectory and the actual docking boundary.

[0118] Furthermore, in step 5, the specific processing is as follows:

[0119] To prevent the AUV from continuing to attempt plugging in when the flow field estimation completely fails, a failure protection threshold is set. :

[0120] ;

[0121] when At that time, the feasible docking domain is adopted. ;when When using a safe waiting feasible domain Therefore, the practically safe and feasible region is represented as :

[0122] ;

[0123] This leads to:

[0124] ;

[0125] Furthermore, based on the above process, the complete technical architecture of the present invention can be formally represented as follows:

[0126] ;

[0127] The system generates local flow field estimation results from historical observations and AUV motion residuals, and then generates future location-related velocity predictions from these local flow field estimation results. The system generates future dynamic docking errors from the predicted velocity and the AUV nonlinear model, and obtains the optimal predictive control sequence from the nonlinear model predictive control to acquire the current control input. Finally, the system generates a safe and feasible docking region from the posterior covariance of the flow field, and switches to a safe waiting state when flow field information is insufficient.

[0128] After the AUV executes control inputs, it generates new position, attitude, and velocity observations and returns to the flow field-state joint estimation process, re-entering the joint estimation. This completes the technical closed loop from state estimation to new observation feedback, enabling the flow field estimation results not only to compensate for current motion deviations but also to directly participate in future state prediction and docking safety decisions.

[0129] The present invention also provides: an electronic device, comprising:

[0130] One or more processors;

[0131] A storage device on which one or more programs are stored;

[0132] When the one or more programs are executed by the one or more processors, the one or more processors implement the aforementioned AUV dynamic docking method.

[0133] The present invention also provides a computer-readable storage medium storing a computer program thereon, which, when executed by a processor, implements the steps in the aforementioned AUV dynamic docking method.

[0134] Compared with the prior art, the present invention, employing the above technical solution, has the following technical effects:

[0135] 1. This invention, by jointly estimating local velocity and flow field gradient, can distinguish between the AUV's own propulsion motion, the dynamic docking platform's motion, and local flow field spatial changes, reducing estimation confusion caused by attributing all motion deviations to general external disturbances. By introducing explicit observability conditions and posterior covariance, the effectiveness of the flow field gradient can be jointly evaluated by historical motion data and observation quality, avoiding the forced use of unreliable gradient parameters when the displacement direction is insufficient. By implementing nonlinear model predictive control of flow field gradient prediction in the moving docking coordinate system, the propulsion, attitude, and approach speed can be adjusted in advance according to the dynamic platform's motion and different flow velocities at the predicted position, improving the accuracy of relative state prediction under complex flow field conditions. Through uncertainty-driven feasible region contraction and failure protection, when the flow field estimation is reliable, the AUV is allowed to gradually enter the docking channel; when the estimation is insufficient, the approach speed is actively reduced or the AUV remains in a safe waiting area, thus balancing dynamic docking accuracy, control continuity, constraint feasibility, and near-end insertion safety.

[0136] 2. This invention is an online closed-loop control method for the dynamic docking process of AUVs. It solves the problem of online estimation and control of whether dynamic docking can be safely completed under unknown local flow field conditions. It can not only compensate for motion deviations caused by the flow field, but also further determine whether the flow field estimation is sufficient to support the docking decision. It can make up for the shortcomings of existing large curvature path tracking methods in terms of moving targets, local variable flow fields and docking safety constraints. Attached Figure Description

[0137] Figure 1 This is a flowchart of the AUV dynamic docking method of the present invention;

[0138] Figure 2 This is a comparison of the actual values ​​of the spatially varying flow field, the traditional constant flow field estimation, and the local affine flow field estimation of this invention.

[0139] Figure 3 The graph shows the results of the flow field estimation uncertainty, the lateral feasible region, and the maximum approach velocity over time.

[0140] Figure 4 This is a comparison diagram of the dynamic docking trajectories of the present invention and the fixed constraint comparison method under a spatially varying flow field. Detailed Implementation

[0141] To make the objectives, technical solutions, and advantages of this invention clearer, the technical solutions of the application will be further described in detail below with reference to the accompanying drawings. The described embodiments are only a part of the embodiments involved in this invention. All non-innovative embodiments based on these embodiments by other researchers in the art are within the protection scope of this invention. Furthermore, the step numbers in the embodiments of this invention are only set for ease of explanation and do not limit the order of the steps. The execution order of each step in the embodiments can be adaptively adjusted according to the understanding of those skilled in the art.

[0142] This invention relates to a dynamic docking method for AUVs based on flow field state estimation and nonlinear model predictive control. The process is as follows: Figure 1 As shown, the local velocity and flow field gradient are first jointly estimated based on the historical motion state of the AUV, the motion state of the docking platform, and relative observation data. Then, the flow field estimation results are propagated along the future predicted trajectory to construct a nonlinear model predictive control problem in the dynamic docking coordinate system. Finally, the docking feasible region is adjusted based on the flow field estimation covariance, and the system switches to a safe waiting state when the flow field information is insufficient.

[0143] Specifically, this invention, through two technical points and a safety adjustment mechanism, forms a continuously progressive technical closed loop around the AUV's navigation state, local flow field parameters, dynamic docking error, predictive control sequence, and docking safety feasible domain. This enables the flow field estimation results not only to compensate for current motion deviations, but also to directly participate in future state prediction and docking safety decision-making.

[0144] The AUV is equipped with a navigation inertial sensor and a Doppler velocity sensor to collect its own attitude, position, and relative velocity in the water; it is also equipped with an optical / acoustic relative ranging and orientation sensor to collect the relative position and attitude between the AUV and the dynamic docking platform; the dynamic docking platform is equipped with a positioning and communication module to broadcast its own position, attitude, and velocity status data to the AUV in real time; all sensor sampling data are synchronously sent to the airborne embedded controller to complete joint estimation and model predictive control calculations.

[0145] (a) Technical point one: Joint estimation of local affine flow field and AUV navigation status under observability constraints.

[0146] Let the discrete time be The sampling period is The position, attitude, and velocity states of the AUV are as follows: and and to form a unified navigation status. :

[0147] ;

[0148] in, This indicates the three-dimensional position of the AUV in the inertial coordinate system; The attitude state of an AUV can be represented by roll angle, pitch angle, and yaw angle. It indicates the velocity state of the AUV, which can include longitudinal velocity, lateral velocity, vertical velocity and corresponding angular velocity in the body coordinate system; This represents the control input formed by the thruster and the control surfaces; This indicates the position, attitude, and speed of the dynamic docking platform at any given moment.

[0149] The above definition is consistent with the classic kinematic and dynamic modeling methods of AUVs. Existing underactuated underwater vehicle technology also constructs a nonlinear motion model based on position, attitude, longitudinal and lateral vertical velocities, as well as pitch and yaw angular velocities, and predicts the position at the next moment based on the model. This provides a direct model basis for establishing discrete state transition relationships.

[0150] During dynamic docking, if the flow field is represented only as a three-dimensional velocity vector that remains constant throughout the docking area, the model can only characterize the overall translational drift experienced by the AUV. As the AUV gradually approaches the docking interface from outside the docking channel, the ocean current may exhibit spatial differences due to local eddies, flow around the docking platform, depth changes, and boundary layer effects. In this case, the same constant flow velocity cannot simultaneously explain the motion residuals at the far and near ends.

[0151] Therefore, in length of Within the historical window, based on the current AUV position Use the local expansion center to expand any position within the window. The ocean current velocity at that location is expressed as:

[0152] ;

[0153] in, This indicates the local flow velocity at the current location. This represents the local flow field gradient. (Matrix) The Line 1 Column elements represent the first The velocity component relative to the first The local rate of change of each spatial coordinate. For example, The first line describes the variation of the component of the flow velocity in the first direction of the inertial coordinate system with the three-dimensional position.

[0154] This expression is essentially a first-order spatial expansion of the local flow field: when the prediction region is small and the flow field changes continuously within that region, the first-order term can preserve the main spatial variations of the local velocity; when the local spatial variations are negligible, let... The model naturally degenerates into a traditional constant velocity model. Therefore, this local affine form does not introduce independent assumptions that conflict with existing flow field models, but instead adds a spatial variation term that can be identified from motion data on the basis of constant velocity.

[0155] Unlike existing technologies that use location-dependent ocean current environment models to illustrate different flow velocities at different spatial locations, this invention simulates three-dimensional underwater ocean currents using a viscous eddy current model and seafloor boundary layer effects, further compressing this spatial variation into a first-order gradient form that can be estimated online within the docking local area.

[0156] To incorporate the baseline velocity and flow field gradient into the same estimation variable, let:

[0157] ;

[0158] in, Indicates to follow a fixed order The 9 elements are arranged into a column vector, therefore It simultaneously contains 3 local velocity components and 9 flow field gradient components. Using the vectorization relation of matrix multiplication, the local velocity can be rewritten as... Linear form:

[0159] ;

[0160] The use of vectorization here is not to add a new control structure, but to... Unified in linear parametric form: matrix Completely due to historical location differences It is confirmed that all the unknowns to be estimated are concentrated in Therefore, the flow field estimation problem can be directly connected with the standard forms of classical weighted least squares, Bayesian estimation, and moving time-domain estimation.

[0161] In length of Within the historical movement window, using AUV position, attitude, velocity, control input, and observations of the relative distance, relative orientation, and attitude between the AUV and the dynamic docking platform, the historical state of the AUV, the current velocity, and the local flow field gradient are jointly estimated:

[0162] (1)

[0163] Equation (1) constitutes the main formula of technical point one. It does not mechanically add up several error terms, but unifies the four requirements of "whether the window starting state is credible, whether the observation is correctly interpreted, whether the adjacent states conform to AUV dynamics, and whether the flow field gradient has a reasonable amplitude" into an estimation target.

[0164] First item These are the prior constraints of the window's starting state.

[0165] in, This represents the state estimate passed from the previous moving window to the starting point of the current window; This represents the error covariance of the prior state. Weighted norm notation is used. hour, The larger the value, the higher the prior confidence in the corresponding state component, and the greater the cost when the state deviates from the prior value; conversely, state components with higher prior uncertainty can be corrected more by current window observations. In the classical framework of moving-time estimation, this term is usually called the arrival cost, and its role is to retain effective information before the window in a finite history window, avoiding the complete loss of the influence of past observations after each window shift. Classical constrained moving-time estimation theory uses arrival cost, state transition error, and observation error to construct a finite window optimization problem to approximate the complete information estimation.

[0166] Second item It is an observation consistency term.

[0167] in, Indicates the first The combined observations actually obtained at any time can include the AUV's own position, attitude, speed, and distance, azimuth, pitch, or relative pose information with the dynamic docking platform; Indicates the candidate AUV state Status of the docking platform The theoretical observations are calculated. The difference between the two is the observation residual. This refers to the observation error covariance. When the measurement noise of a certain type of sensor is relatively low, its corresponding... The weights are relatively large; when a certain type of underwater acoustic distance or azimuth measurement is unstable, its weight decreases accordingly. Existing technologies have already used relative distance, horizontal azimuth, and pitch angle as basic observations for three-dimensional positioning and geometric constraints, and constructed evaluation functions using various error weights. Therefore, this invention uses the above-mentioned geometric quantities as joint estimation observations, which is consistent with existing underwater cooperative navigation methods.

[0168] Third item It is a dynamic consistency term.

[0169] in, The continuous kinematics and dynamics model of the AUV has been sampled over a period of time. The state transition function formed after discretization takes the current state as its input. Control input Local flow velocity corresponding to the current position The output is the theoretical state at the next time step. Actual state. The deviation from the theoretical state transition is no longer entirely and generally explained as random perturbation, but is instead addressed by adjusting... The local flow velocity and gradient within the window allow for a physically meaningful interpretation of the motion residuals. (Matrix) It represents the error covariance of the AUV model, which allows for the existence of hydrodynamic parameter errors, discretization errors, and unmodeled disturbances within a certain range, while preventing the estimator from generating unreasonable flow field parameters in order to perfectly fit the noise.

[0170] Last item It is a priori constraint term for the flow field gradient.

[0171] Among them, due to This term imposes a unified constraint on the nine gradient components. It does not presuppose that the actual flow field gradient is zero, but rather uses the premise that "the local flow field preferentially degenerates into a simpler constant velocity model when sufficient data is lacking" as an estimation prior. When the displacement and motion residuals within the history window provide sufficient information, the observation and dynamic terms can overcome this prior, causing the corresponding gradient components to deviate from zero. When the trajectory changes only in one direction and cannot support complete gradient identification, the unidentifiable gradient direction is preserved near zero, thus preventing arbitrary divergence in the estimation results. This form is consistent with the mathematical structure of classical Tikhonov regularization and Gaussian priors, ensuring the well-posedness of the estimation problem under limited data conditions and providing a unified basis for subsequent covariance calculations.

[0172] To further illustrate how equation (1) utilizes the flow field inversion from AUV motion, an approximation of the AUV's ground velocity can be constructed from observations at adjacent locations: ;

[0173] Then, the velocity of the AUV relative to the inertial frame of the water body is calculated from its attitude and volume coordinate system velocity. ;

[0174] The difference between the two constitutes the velocity residual. :

[0175] ;

[0176] in, This is the rotation matrix for transforming from the AUV body coordinate system to the inertial coordinate system. Speed ​​state The linear velocity component. Ignoring higher-order variations within the sampling period, the residual approximately satisfies:

[0177] ;

[0178] in, This comprehensively represents the position measurement error, velocity measurement error, attitude conversion error, and discretization error. Therefore, it is evident that the local flow field cannot be obtained by simply deploying an additional independent velocity sensor; rather, it can be inverted from the velocity difference between the actual ground motion and the model's relative motion to the water body. If the AUV is equipped with sensors capable of directly measuring relative water velocity or ground velocity, the corresponding observations can be further refined. This complements the aforementioned motion residuals.

[0179] If temporarily Assuming it has been determined by current observations of Earth velocity and relative water velocity, then regarding the gradient vector... The single-time observation relationship can be written as:

[0180] ;

[0181] By superimposing the relationships between different moments within a historical window in chronological order, we can obtain the standard weighted least squares structure and the corresponding flow field gradient information matrix. for:

[0182] (2)

[0183] in, For flow velocity residual The covariance of the matrix. Characterizing the amount of information provided by historical motion for the nine flow field gradient components: when a certain gradient direction does not cause a distinguishable change in motion residuals throughout the entire historical window, the quadratic form of the information matrix in the corresponding direction approaches zero; when the AUV is displaced in multiple independent spatial directions and the velocity changes in each direction can be observed in the residuals, the information matrix gradually becomes positive definite.

[0184] Complete Locally identifiable if and only if At that time, using the structure of the Kronecker product, we have:

[0185] ;

[0186] when When the matrix is ​​positive definite, its rank is 3. The rank of the Kronecker product is equal to the product of the ranks of the two matrices. Therefore:

[0187] ;

[0188] then, The condition for full rank is equivalent to the sum of the outer products of historical displacements being a positive definite matrix of order 3, and also equivalent to:

[0189] (3)

[0190] Equation (3) gives the explicit geometric conditions under which the flow field gradient can be fully identified: the displacement vectors within the history window cannot all be collinear, nor can they all be confined to the same plane, but should span three-dimensional space. If the AUV moves only along the docking axis within the window, it can only identify the change in velocity along that axial displacement, and cannot distinguish the gradient of the flow field to the other two spatial directions; if the AUV has two non-collinear motion directions in a horizontal plane, it can identify the flow field changes in that plane, but the gradient related to the depth direction still lacks sufficient information; only when the motion within the window simultaneously includes three linearly independent displacement directions can the complete three-dimensional gradient matrix have a basis for local identification.

[0191] Since dynamic docking missions typically require AUVs to gradually converge to the docking axis, the trajectory in the final stage may become increasingly close to a straight line. Therefore, this invention does not require the AUV to continuously perform large lateral maneuvers to estimate the flow field. The actual estimation uses a rolling history window: during the long-distance approach and channel adjustment phases, the AUV's original three-dimensional position changes accumulate flow field gradient information; when entering the near-end straight-line docking phase, the existing lateral and vertical displacements in the history window are retained for a period of time, and their posterior covariance gradually increases as new information becomes insufficient, and the feasible region is automatically tightened by a third mechanism. Thus, whether to continue docking does not depend on human judgment of "whether the current trajectory is rich enough," but is continuously reflected through the information matrix and posterior covariance.

[0192] when and When estimating unknowns, it is also necessary to distinguish between "changes in the base velocity" and "changes caused by the gradient multiplied by the position offset".

[0193] Now, construct the augmented position matrix:

[0194] ;

[0195] like Then there must be at least four affine independent points within the window, so that the constant velocity term and the first-order variation terms in the three spatial directions can be distinguished. Here, "affine independence" does not require the four points to have a fixed geometric shape, but rather that they cannot all fall in the same plane. This condition is consistent with equation (3): equation (3) focuses on the dimension spanned by the displacement with the current point as the expansion center, and the augmented matrix condition further expands the constant term. Included.

[0196] In order to provide more than just binary judgments of "distinguishable or undistinguishable", we define... :

[0197] ;

[0198] in, This is the smallest eigenvalue of the information matrix. If... This indicates that the information matrix is ​​a positive definite matrix; The larger the value, the more sufficient information is obtained even in the weakest observable direction; if If the value is close to zero, even if information from other directions is strong, there is still a gradient combination that is difficult to reliably distinguish. Evaluating the weakest observable direction with the minimum eigenvalue is a classic practice in observable Gramian analysis, which can avoid the problem of masking a very weak direction when only using trace or determinant.

[0199] The regularization term in equation (1) can be equivalently interpreted as a Gaussian prior of the flow field gradient:

[0200] ;

[0201] After local linearization and fixing the AUV state, regarding The weighted least squares subproblem can be written as:

[0202] ;

[0203] Regarding the objective function Taking the derivative and setting the gradient to zero, we get:

[0204] ;

[0205] Since the first item inside the parentheses is... ,so:

[0206] (4)

[0207] in, The information vector is formed by historical displacement and velocity residuals:

[0208] ;

[0209] Equation (4) clearly illustrates the relationship between the data terms and the prior terms. When a sufficient displacement excitation is obtained in a certain direction, The eigenvalues ​​in this direction are relatively large, and the estimation is mainly determined by the observed data; when there is a lack of excitation in a certain direction, the eigenvalues ​​of the information matrix in that direction are very small. To prevent matrix inversion from becoming unstable and to keep the estimated result near the prior zero, regularization can only guarantee the existence of stable solutions to numerical problems; it cannot transform an inherently unidentifiable direction into a direction that is truly identifiable by the data.

[0210] Will and Re-merged into Under the local linear Gaussian approximation, its posterior information matrix It can be written as:

[0211] ;

[0212] in, Let be the prior covariance of the flow field parameters, and the corresponding posterior covariance be... :

[0213] (5)

[0214] Equation (5) is not an independent estimation method from Equation (1), but rather an expression of uncertainty given by Equation (1) under a local quadratic approximation. The more effective observations in the historical window, the richer the location distribution, and the smaller the velocity residual noise, the larger the information matrix and the smaller the posterior covariance; conversely, when the AUV moves along a single direction for a long time or the observation noise increases, the covariance increases accordingly in the weakly observable direction.

[0215] As time goes by Advance to The earliest observation in the original window is removed, and the state, position, and motion residuals at the new time are added, and equation (1) is solved again on a rolling basis. The end-state estimate of the previous window is passed on as the arrival prior of the new window, and the flow field parameters and their covariance of the previous window can also be used as the parameter prior of the current window. Therefore, moving the window does not mean that the estimation starts from zero at each time, but rather forms the following information recursion:

[0216] .

[0217] The unified approach to handling nonlinear models, state constraints, and parameter priors in moving-time domain estimation is well-suited to the estimation problem of coupling AUV state with local flow field parameters in the method of this invention.

[0218] Ultimately, technical point one outputs three interrelated results: And simultaneously output the posterior covariance of the flow field parameters. .in, As the initial prediction state for technical point two; This serves as the baseline value for the flow field at the current location; Used to calculate the flow velocity at different locations on the predicted trajectory; Used to calculate the flow velocity uncertainty at each predicted location.

[0219] Thus, Equation (1) does not end after giving only one estimation result, but simultaneously transmits the flow field mean, spatial variation and estimation confidence to subsequent control and safety constraints.

[0220] (II) Technical Point Two: Nonlinear Model Predictive Control Based on Moving Docking Coordinate System for Predicting Flow Field Gradient

[0221] The fundamental difference between dynamic docking and fixed-path tracking lies in the fact that the docking platform itself may simultaneously undergo translation, rotation, and attitude changes. If calculations are only performed in the inertial coordinate system... While the positional difference can be obtained, it is impossible to directly distinguish the meaning of this difference in the docking axis, lateral direction, and vertical direction. When the attitude of the docking platform changes, the same positional error in the fixed inertial coordinate system may correspond to completely different docking risks. To address this, the present invention establishes a follower coordinate system with the dynamic docking platform as a reference, so that the relative position, relative attitude, and relative velocity of the AUV are all expressed in the local coordinate system of the docking platform.

[0222] Set up a dynamic docking platform at all times Position, attitude and velocity are respectively and The rotation matrix is ​​constructed based on the attitude of the docking platform. :

[0223] ;

[0224] matrix The vectors in the docking platform coordinate system are transformed to the inertial coordinate system, and therefore their transpose is... Used to transform vectors in the inertial coordinate system to the docking platform coordinate system.

[0225] The positional error of the AUV relative to the dynamic docking platform is defined as:

[0226] .

[0227] The first component of this error can be set as the distance along the docking interface axis, while the other two components represent the lateral and vertical deviations perpendicular to the docking axis. As the docking platform rotates, the rotation matrix is ​​updated synchronously, so the physical direction of the relative error always remains consistent with the current docking interface, rather than being fixed in the global coordinate direction.

[0228] Attitude error is denoted as :

[0229] ;

[0230] During the docking phase when attitude errors are relatively small, this difference can be directly used to evaluate the angular alignment between the AUV and the docking platform. If quaternions are used to describe attitude in engineering implementation, the quaternion error can also be converted into an equivalent three-dimensional small angular error, without changing the fundamental principle of this invention. The main line represents the relative attitude deviation.

[0231] The speed of an AUV relative to the water body is After converting it to an inertial coordinate system, it becomes The actual ground velocity of the AUV should also be superimposed with the local flow velocity estimated by technical point one. Therefore, the speed error relative to the dynamic docking platform is defined as :

[0232] ;

[0233] This relationship avoids the physical inconsistency that arises from directly subtracting the AUV's relative velocity to the water from the platform's velocity relative to the ground. If ocean currents propel the AUV and the docking platform with different ground motions, even if the AUV's propulsion speed remains constant, the relative approach speed will still change; therefore... This must be explicitly reflected in the relative velocity error. If the docking platform is also affected by ocean currents, its actual velocity relative to the ground is already included in the measured or estimated velocity. In this case, there is no need to repeat the layering process.

[0234] The relative position, relative attitude, and relative velocity are collectively considered as the docking error state:

[0235] .

[0236] This error state serves three purposes in subsequent NMPC: firstly, it acts as the primary controlled variable in the stage objective function; secondly, it is used to define... Whether it enters the docking channel and whether the attitude alignment conditions are met; thirdly, it is used to construct the safe and feasible region related to the flow field uncertainty in the third mechanism.

[0237] Let the prediction time domain length be uniformly adopted Indicates the current moment Predicted future Step AUV states, where: initial state .

[0238] here, The posterior state estimate from technique point one. Accordingly, Indicates from The predicted location extracted from it. Indicates the future number Step control input. The future state of the dynamic docking platform can be predicted based on its current speed, angular velocity, or reference motion plan obtained through communication, and used for step-by-step calculation. .

[0239] Traditional steady flow field prediction typically employs... This approach ignores the differences in the flow field that the AUV will be in after its position changes within the prediction time domain.

[0240] This invention utilizes the local flow field gradient output from the first technical point to predict the future flow field gradient. Predict the flow velocity at the step location:

[0241] (6)

[0242] Position in equation (6) The position of the AUV is determined by both the control input and the flow field, thus forming a coupled predictive relationship: the control input changes the future position of the AUV; the future position changes the flow field gradient term; and the change in predicted velocity further changes the position at the next moment. This coupling is precisely why a nonlinear model predictive control is used instead of a one-time linear compensation.

[0243] To ensure consistency of sign, all future states are represented using discrete-time state transition functions:

[0244] (7)

[0245] in, This is consistent with the discrete AUV motion model in Equation (1). Its inputs are the current predicted state, the current predicted control variable, and the predicted flow velocity at the current position; the output is the state at the next predicted time. The state derivative symbol is no longer used here to avoid confusing the continuous-time model. and discrete-time model Mixed use.

[0246] From the perspective of classical AUV kinematics, the position part in equation (7) can be understood as:

[0247] ;

[0248] The future position is determined by the AUV's propulsion relative to the water body and the predicted ocean current transport; the attitude and velocity components are updated based on AUV dynamics, control inputs, hydrodynamic damping, and restoring torque. Existing underactuated AUV technology already uses the current attitude, longitudinal and lateral velocities, and desired control variables to predict the velocity, attitude, and position at the next moment. The method of this invention further incorporates the spatially varying flow field into its prediction structure. After being used as input to a unified model, a location-related mapping matrix is ​​constructed to characterize the local flow field at different predicted locations. .

[0249] Then equation (6) can be uniformly written as:

[0250] ;

[0251] Here It is completely consistent with technical point one, without introducing any new flow field parameters. Because It is not without error constants, but has posterior covariance. According to the classical linear covariance propagation relationship, the first The velocity covariance at each predicted location is :

[0252] (8)

[0253] Equation (8) shows that the uncertainty of the future flow velocity depends not only on the accuracy of the current velocity estimate, but also on the distance between the future predicted location and the current unfolding center.

[0254] Will After dividing the equation into blocks according to the flow velocity base value and gradient parameters, equation (8) can be expanded as follows:

[0255] ;

[0256] in, The covariance of the current flow velocity baseline value, For the flow field gradient covariance, Let be the cross-covariance between the two. This expansion further illustrates that when the predicted location is close to the current expansion center, the velocity uncertainty is mainly due to... The decision is made as the predicted location moves further away from the current point, in relation to the gradient covariance. The relevant quadratic terms gradually increase. In other words, the controller will not mistakenly assume that the flow field estimation can maintain the same accuracy over arbitrarily distant prediction regions, but will naturally reflect the effective range of the local affine model.

[0257] Based on the above prediction model, a future control sequence is defined. :

[0258] ;

[0259] The following nonlinear model predictive control optimization problem is constructed to obtain the optimal predictive control sequence. :

[0260] (9)

[0261] st ;

[0262] Equation (9) is the main formula for technical point two. Its objective function contains... Used to evaluate the future Step relative position, attitude, and velocity errors.

[0263] matrix The weighted matrix can be configured as a block weight matrix according to the physical requirements of the docking process:

[0264]

[0265] in, Adjusting the relative position error is of paramount importance. The importance of adjusting attitude alignment error. Adjusting the relative velocity matching is important. At the far end of the docking channel, position convergence can be given appropriate attention, while when approaching the docking interface, more attention should be paid to lateral deviation, attitude deviation, and approach velocity. Specific changes can also be reflected through the docking reference trajectory and the constraint contraction in the third mechanism, without the need to add a new controller.

[0266] Control input increment is defined as :

[0267] ;

[0268] Cost item This is used to suppress abrupt changes in thruster commands between adjacent control moments. This does not directly require the control input itself to be as small as possible, but rather requires the control sequence to be continuous, thereby reducing frequent thruster acceleration / deceleration and rapid control surface switching.

[0269] At the same time, constraints Ensure that the propulsion force and control torque do not exceed the actuator's capacity. This further reflects the limitation of the thruster's response rate. Therefore, the trajectory obtained by the controller is not only feasible in the state space, but can also be continuously realized by the actual actuators.

[0270] Terminal Item Used to evaluate the remaining docking error at the end of the predicted time domain. If only minimizing the first... The stage error of each step may cause the controller to postpone larger errors to beyond the prediction time domain in order to reduce short-term control costs; with the addition of the terminal term, the controller needs to consider the impact of the current control action on the docking state in the more distant future. Matrix It is possible On this basis, the weights of position, attitude and velocity errors are appropriately increased to make the predicted end tend to a safe docking state.

[0271] Constraints of Equation (9) The uncertainty in flow field estimation and the docking safety geometry are directly incorporated into the optimization problem. Here... It is not a fixed channel, but is determined by a third mechanism based on the most unfavorable flow field uncertainty throughout the entire prediction time domain. Therefore, the controller cannot simply "softly encourage" the AUV to approach the channel center by using large weights in the objective function, but must ensure that the predicted state actually lies within the allowable set.

[0272] Find the optimal control sequence Then, the AUV executes only the first control variable. .

[0273] Arrival Time Subsequently, the AUV status, docking platform status, and relative geometry observations were reacquired, and the results were re-estimated using technical point one: .

[0274] The prediction time domain is then shifted forward and Equation (9) is solved again. This rolling process of "prediction-execution of the first step-re-estimation-prediction again" enables the controller to continuously correct the flow field prediction deviation and platform motion deviation, rather than performing an open-loop operation after calculating the complete docking trajectory once.

[0275] The core of this technique is not the sole application of NMPC, as model predictive control has already been used for underwater vehicle state prediction and docking control. Existing technologies have proposed using flow field state estimation results for MPC navigation; therefore, simply incorporating a constant flow velocity estimate into MPC does not provide sufficient technical differentiation.

[0276] The technical differences of this invention are mainly reflected in: first, the prediction error is defined in the dynamic docking platform's servo coordinate system; second, the local flow field gradient. Different flow velocities are gradually generated along the predicted trajectory; third, parameter covariance. The covariance of the predicted flow velocity is propagated through equation (8); fourth, this covariance continues into the third mechanism, rather than just as an additional output that is not utilized within the estimator.

[0277] (III) A mechanism: convex feasible region contraction and failure protection mechanism driven by flow field uncertainty.

[0278] Technical point one can provide the basic values ​​of the flow field, the flow field gradient, and their estimated covariance. Technical point two can predict the future movement of the AUV based on these results. However, prediction alone is insufficient to guarantee the safety of dynamic docking. This is because NMPC typically relies on estimated mean values. A state recursion is performed; even if the mean trajectory is located inside the docking channel, the actual flow velocity may still deviate from the mean due to estimation errors, causing the AUV to drift laterally, deflect at attitude, or approach speed limits. Therefore, instead of adding another optimization algorithm, the estimation uncertainty is transformed into the continuous contraction of the docking geometric boundary and velocity boundary.

[0279] According to equation (8), each predicted location has a corresponding velocity covariance. Since docking safety requires responsibility for the entire predicted trajectory, and cannot only consider the current position, the most unfavorable flow velocity uncertainty index is defined within the prediction time domain. :

[0280] (10)

[0281] in, It is the largest eigenvalue of the matrix.

[0282] For any unit direction vector The variance of the predicted flow velocity in this direction is According to the Rayleigh quotient property of symmetric positive definite matrices, we have:

[0283] ;

[0284] therefore, Indicates the first The largest standard deviation among all flow directions at each predicted location. Then, taking the maximum value over the entire prediction time domain, we obtain... It can uniformly cover the most unfavorable prediction location and the most unfavorable flow velocity direction. This definition does not introduce any new probability distribution assumptions, but directly uses the covariance output by the first technical point for conservative scale extraction.

[0285] Let the first coordinate axis of the docking platform coordinate system extend outward from the interface along the interface axis. The axial distance from the AUV to the interface is specified. ,Will The other two components form the lateral error vector.

[0286] Here, "lateral" does not only refer to the horizontal plane but also uniformly represents the two-dimensional error perpendicular to the docking axis. Let the component of the relative velocity error along the docking axis be... The constraint specifies that a negative value indicates that the AUV is approaching the interface. Because the constraint uses its absolute value, it can simultaneously limit both excessive approach speed and excessive reverse exit speed.

[0287] The feasible docking region driven by flow field uncertainty is defined as:

[0288] (11)

[0289] in, This represents the maximum axial distance within the docking control area. To approximate the basic lateral deviation allowed at the interface; This represents the slope by which the channel's allowable width increases with axial distance; This represents the maximum permissible attitude deviation when the flow field estimation is sufficiently reliable. The maximum permissible axial relative velocity when the flow field estimation is sufficiently reliable; and These represent the degree to which the flow field uncertainty affects the constraints on lateral position, attitude, and approach velocity, respectively.

[0290] When the AUV is far from the interface Larger allowable lateral deviation With a relatively wide width, the controller can initially utilize the larger space to complete directional adjustments; as... Reducing the lateral deviation allows it to gradually decrease, causing the predicted trajectory to converge in a funnel shape; when the flow field uncertainty... As the width increases, it is allowed to decrease further, requiring a greater margin between the nominal predicted trajectory and the channel boundary. This constraint can be understood as follows: the geometric channel itself remains unchanged, but in order to reserve a safety margin for unknown velocity deviations, the nominal trajectory region that NMPC can use is shrunk inward.

[0291] Attitude constraints They have the same meaning. When the flow field estimation is reliable, the attitude error is allowed to reach the deterministic boundary. When the uncertainty of the flow field increases, the controller needs to obtain better attitude alignment before entering the channel in order to reduce the risk of interface collision caused by the superposition of flow field disturbances and attitude errors.

[0292] Speed ​​constraints Then a continuously bounded fractional contraction form is adopted. When When, the upper bound equals ;along with Increasing this allows the speed to decrease monotonically, but it always remains non-negative. This is different from directly using... In contrast, the fractional form can avoid the upper bound of velocity suddenly becoming negative under finite uncertainty, while still achieving the physical requirement that the greater the uncertainty, the lower the approach velocity.

[0293] For fixed Axial distance constraint The linear interval constraint and the velocity constraint can be equivalently written as two linear inequalities:

[0294] ;

[0295] The lateral position constraint can be rewritten as:

[0296] ;

[0297] Since the L2 norm is a convex function, subtracting the L2 norm from the norm with respect to... The linear function is still a convex function, and its lower level set is a convex set.

[0298] Attitude constraints It is also a convex constraint of the second-order cone form. Therefore, when the right-hand boundary is non-negative, each subset defined by equation (11) is a closed convex set, and the intersection of a finite number of closed convex sets is also a closed convex set. In fixed It is a closed convex set.

[0299] The convexity conclusion needs to be accurately understood: it only indicates that the docking geometry and velocity constraints themselves do not introduce additional discrete choices or multiple disconnected regions, but does not mean that the entire NMPC problem becomes a convex optimization. Equation (9) still contains the nonlinear AUV state transition function. The coupling between the attitude rotation matrix, the flow field gradient, and the predicted position means that the overall problem remains a nonlinear constrained optimization problem. However, using a convex docking feasible region can avoid adding unnecessary geometric nonconvexity to the original nonlinear dynamics, which helps to maintain a clear solution structure.

[0300] When the flow field estimate is completely determined, i.e. When, equation (11) degenerates into:

[0301] ;

[0302] This involves a fixed funnel-shaped position channel, fixed attitude boundaries, and fixed approach velocity boundaries. Therefore, this mechanism is fully compatible with traditional deterministic docking constraints under ideal conditions where estimation is error-free, without introducing additional conservatism.

[0303] Further consider two different uncertainty levels ,because:

[0304] ;

[0305] and:

[0306] ;

[0307] Therefore, any state that satisfies the constraint of larger uncertainty must also satisfy the constraint of smaller uncertainty, and thus:

[0308] (12)

[0309] Equation (12) gives the monotonically shrinking property of the feasible region: the more uncertain the flow field estimate, the smaller the set of docking states that the AUV can occupy; as new observations increase and the posterior covariance decreases, the set gradually expands again. This change is caused by the same continuous variable. It is driven by the system and does not require the manual setting of multiple independent control stages.

[0310] However, when the flow field uncertainty is too large, the right end of the lateral or attitude constraint may become negative.

[0311] For example, when At that time, there is no nonnegative norm. Able to satisfy the corresponding constraints; when At this time, the attitude constraint also has no solution; This does not indicate an algorithm malfunction, but rather that the current estimate's reliability is insufficient to support secure plugging. If this situation is not handled separately, NMPC may report it as infeasible, or, when using soft constraints, continue approaching the interface by severely violating safety boundaries.

[0312] Therefore, a failure protection threshold for flow field uncertainty is set:

[0313] (13)

[0314] The threshold was chosen based on the following: when the AUV is closest to the mating interface and the axial distance approaches zero, the minimum deterministic width of the lateral position constraint is [value missing]. To ensure feasibility when at least the lateral and attitude errors are zero, the following conditions must be met: and Equation (13) is obtained by satisfying both equations simultaneously.

[0315] when At that time, the flow field uncertainty has not yet exceeded the minimum docking confidence boundary, and NMPC continues to use the docking feasible region of equation (11);

[0316] when At this time, the optimizer is no longer forced to use the interface as the terminal target, but instead the reference target is switched to a safe waiting position outside the docking channel.

[0317] Set the safe waiting position at least [distance] from the interface The region, and define the waiting feasible region. :

[0318] (14)

[0319] in, To provide a safe waiting distance outside the interface, The allowable lateral deviation near the waiting position, The allowable attitude deviation during the waiting phase, This refers to the permissible axial speed range when maintaining low speed. The waiting state does not require the AUV to be completely stationary, but rather to remain outside the docking channel at a low speed, observable, and capable of re-entering the docking process.

[0320] The terms of equation (14) are still linear constraints or L2 norm convex constraints, therefore It is also a closed convex set. The practically used safe set is defined as... :

[0321] (15)

[0322] when At this time, the NMPC's terminal reference still points to the dynamic interface, allowing the AUV to continue approaching within the contracted funnel-shaped channel;

[0323] when At this point, the terminal reference is changed to a safe waiting position, and the desired axial approach speed is set to zero or a small holding speed, so that the AUV no longer continues to penetrate into the narrow passage.

[0324] The AUV continues to acquire position, attitude, relative water velocity, and relative measurements of the docking platform while in the waiting area, with the moving window constantly updated; if subsequent observations... Reduce, and thus make Then the safe set is switched back to , Restore dynamic docking.

[0325] This failure protection is not a separate fourth control module, but rather a deterministic continuation of equation (11) when the uncertainty is large and the feasible region may be empty. Its input remains the output of technical point one. The constraints are still solved uniformly by NMPC in technical point two, so the overall structure of the two technical points and one mechanism of this invention will not be destroyed.

[0326] From the perspective of the entire process, the three parts form the following continuous calculation relationship:

[0327] ;

[0328] The first technical point involves obtaining the AUV's state, local velocity, flow field gradient, and covariance based on historical state, observations, and motion residuals. The second technical point uses the velocity baseline and gradient to predict ocean currents at different future locations, calculates docking errors using a nonlinear AUV model, and obtains the control sequence. The third mechanism uses the covariance propagation results to narrow the feasible domains of position, attitude, and velocity, and reverts the docking target to a safe waiting state when the uncertainty exceeds a threshold. After the AUV executes the first step of the optimal control sequence, new position changes and velocity residuals re-enter technical point one, thus forming a technical closed loop that combines historical motion with relative observations to execute control and generate new motion observations.

[0329] Furthermore, in one embodiment of the present invention, a numerical simulation experiment is constructed for the dynamic docking process of AUV under spatially varying flow field conditions to verify the effectiveness of local affine flow field estimation, flow field uncertainty propagation, confidence-driven feasible region shrinkage, and nonlinear model predictive control methods.

[0330] In this embodiment, the simulation object is a two-dimensional horizontal plane AUV dynamic docking model to intuitively demonstrate local flow field spatial changes, docking channel contraction, and trajectory boundary violations. The sampling period is set to 0.25s, the total simulation time is set to 55s, and a total of 220 discrete control moments are formed. The initial position of the AUV is set to (-17.50m, 2.80m) in the inertial coordinate system. The dynamic docking platform moves along the x-direction of the inertial coordinate system at a reference velocity of 0.05m / s, while a slow periodic motion with an amplitude of 0.30m and an angular frequency of 0.07rad / s is superimposed in the y-direction to simulate the dynamic displacement of the docking platform under the action of water flow and its own propulsion. The maximum speed of the AUV control input is set to 0.92m / s, and the maximum control increment between adjacent control moments is set to 0.28m / s to limit the abrupt changes in propulsion commands and reflect the actual response capability of the thruster.

[0331] This embodiment employs a rolling time-domain nonlinear model predictive control method to calculate the AUV control inputs. The prediction time domain length is set to 14 discrete time points, meaning each control solution covers a 3.5-second future motion process. At each control time point, 70 candidate control sequences are generated. State recursion, flow field prediction, error evaluation, and constraint violation calculations are performed on each candidate sequence. Finally, the control sequence with the minimum total cost is selected, and only the first control input is executed. The objective function simultaneously considers the axial position error, lateral position error, and changes in adjacent control inputs relative to the dynamic docking interface of the AUV. The weight of the lateral error is higher than that of the axial error to emphasize the lateral alignment accuracy requirements during the docking phase.

[0332] To construct a realistic underwater flow field with significant spatial variation characteristics, this embodiment superimposes background flow, spatial shear flow, local vortex flow, and slowly time-varying components. The local vortex center is located approximately 1.40 m behind the dynamic docking platform, with a vortex intensity parameter set to 8.00, and a smooth viscous vortex form is used to avoid center singularity. The spatial shear flow has a baseline component of 0.12 m / s along the x-direction, with an additional linear variation term added based on the relative position between the AUV and the dynamic docking platform. Simultaneously, slowly time-varying velocity components with amplitudes of 0.06 m / s and 0.08 m / s are superimposed in two coordinate directions. This configuration ensures that the flow field changes relatively smoothly far from the docking interface region, while the flow direction and velocity amplitude change significantly closer to the docking interface region, thus verifying the applicability of traditional constant flow field estimation in regions with significant local gradients.

[0333] In the flow field estimation process, this embodiment uses a sliding historical window with a length of 24 sampling times, that is, it uses the AUV position and velocity observations within the last 6 seconds to estimate the current velocity and local flow field gradient. The traditional comparison method uses a historical window with a length of 60 sampling times and only calculates a time-weighted constant velocity. The standard deviation of the velocity observation noise is set to 0.025 m / s, and the local gradient regularization coefficient is set to 0.35.

[0334] In the method of this invention, a first-order local affine model is used to represent the flow field near the current AUV position, that is, the flow velocity at any predicted position is calculated by combining the current position velocity estimate and the flow field gradient matrix; in contrast, the same constant velocity estimate is used repeatedly throughout the entire prediction region.

[0335] The comparison results of the actual value of the spatially varying flow field, the traditional constant flow field estimation, and the local affine flow field estimation of the method of this invention in this embodiment are as follows: Figure 2 As shown. Figure 2 The three sub-maps in the image use the same spatial range, the same velocity amplitude color scale, and the same AUV approach trajectory; among them, Figure 2 In the diagram, (a) represents the actual spatially varying flow field. Figure 2 (b) in the figure represents the result of traditional constant flow field estimation. Figure 2 In the figure, (c) represents the local affine flow field estimation result of the method of the present invention. The background color represents the velocity amplitude, streamlines and arrows represent the flow direction, the dotted line represents the actual approach trajectory of the AUV from far to near, the circular mark represents the position of the AUV at the selected evaluation time, the star mark represents the dynamic interface, and the white dashed line area represents the area where the local flow field gradient is more obvious near the interface.

[0336] Figure 2 The selected evaluation time corresponds to an axial distance of approximately 2.50 m between the AUV and the dynamic interface. At this point, the AUV has entered the near-end region where the local flow field changes significantly. Figure 2 As can be seen in (a), the streamlines near the interface are bent, and the velocity amplitude varies significantly at different spatial locations. Figure 2 The traditional constant flow field estimation in (b) can only give a flow field with an approximately consistent overall direction, and cannot represent the flow direction deflection and spatial variation of velocity amplitude in the near-interface region. Its root mean square error of regional velocity estimation is 0.528 m / s. Figure 2In (c), the local affine flow field estimation introduces a first-order spatial variation term based on the AUV's historical displacement and velocity residuals, resulting in a root mean square error (RMSE) of 0.564 m / s for the regional velocity estimation. While the overall RMS errors of both methods are similar within this evaluation region, the method of this invention can provide a velocity distribution that varies with spatial location in regions with significant local gradients near the interface. This allows subsequent nonlinear model predictive control to obtain location-dependent flow field inputs, rather than repeatedly using a single constant velocity across the entire prediction region.

[0337] Therefore, in the spatially varying flow field set in this embodiment, the method of the present invention not only reduces the error index of a certain static region, but also enables the flow field estimation results to be incorporated into the process of predicting the trajectory, flow field uncertainty, and docking feasible region shrinkage.

[0338] also, Figure 2 Furthermore, it is shown that the introduction of the flow field gradient matrix does not merely change the numerical value of the velocity estimation result, but enables different velocity inputs to be obtained at different locations within the NMPC prediction time domain. When the AUV is in a smooth flow field region far from the interface, the difference between the local affine model and the constant model is relatively small; as the AUV gradually enters the region near the interface, the traditional method still uses a single velocity vector to recursively predict the future position, while the method of this invention corrects the future velocity based on the offset between the predicted position and the current deployment position, and simultaneously calculates the uncertainty of the flow field estimation. Figure 2 The results verify the necessity of introducing local flow field gradients and uncertainty propagation in the dynamic docking end region of the present invention.

[0339] The results of the flow field estimation uncertainty, lateral feasible region, and maximum approach velocity over time in this embodiment are as follows: Figure 3 As shown.

[0340] Figure 3 The upper curve represents the flow field estimation uncertainty. The light-colored curve represents the original uncertainty calculated directly at each sampling time, while the dark-colored curve represents the uncertainty trend after sliding smoothing. The red dashed line represents the failure protection threshold. In this embodiment, the minimum lateral half-width parameter of the docking funnel is set to 0.24 m, and the influence coefficient of flow field uncertainty on the lateral safety margin is set to 1.00. Therefore, the failure protection threshold is set to 0.24. When the flow field uncertainty is higher than this threshold, it indicates that the current flow field estimation is insufficient to support the safe entry of the AUV into the final docking area. The controller prioritizes limiting the AUV's further approach and maintaining it within the safe area outside the docking channel.

[0341] Figure 3The middle section represents the boundary between the fixed geometric boundary and the lateral feasible region boundary after the flow field uncertainty has shrunk. The fixed geometric boundary is represented by a funnel-shaped function, where the base half-width near the interface is set to 0.24 m, and the coefficient for the channel width increasing with axial distance is set to 0.16. As the AUV gradually approaches the dynamic docking interface, its axial distance decreases, and the fixed geometric boundary itself gradually narrows. Furthermore, this invention subtracts the safety margin formed by the flow field uncertainty. When the flow field estimation uncertainty is large, the lateral region allowed by the nominal predicted trajectory shrinks significantly; as the uncertainty gradually decreases, the safety margin decreases accordingly, and the shrinking boundary gradually approaches the fixed geometric boundary. Therefore, the docking feasible region of this invention does not switch abruptly according to several artificially set discrete stages, but is continuously driven by the flow field estimation reliability.

[0342] Figure 3 The lower part indicates the maximum permissible axial approach velocity. In this embodiment, the maximum approach velocity when the flow field estimation is sufficiently reliable is set to 0.72 m / s, and the contraction factor of the flow field uncertainty with respect to the approach velocity is set to 2.00. It can be seen that in the early stages of the simulation, due to the small number of historical observation samples and the instability of local flow field parameters, the flow field uncertainty is high, and the maximum permissible approach velocity is significantly reduced. As AUV motion data gradually accumulates, the flow field estimation covariance decreases, and the maximum approach velocity continuously increases. In the middle and later stages, when the AUV enters a region with significant local flow field changes, the uncertainty fluctuates locally, and the permissible approach velocity automatically decreases accordingly. After the flow field estimation stabilizes again, the upper limit of the velocity recovers. This result demonstrates that the present invention can automatically adjust the AUV approach rhythm according to the flow field estimation state, rather than always approaching the dynamic docking interface at a fixed speed.

[0343] Figure 3 The three typical moments marked in the figure are set to 8s, 26s, and 48s, respectively. The first typical moment corresponds to the stage when the AUV completes the initial flow field observation and begins a stable approach. At this time, the flow field uncertainty has decreased significantly from its initial high value, but the feasible region still retains a certain safety margin. The second typical moment corresponds to the AUV entering the mid-range approach region. The flow field uncertainty remains at a low level, and the lateral feasible region and the upper limit of velocity are continuously adjusted according to local flow field changes. The third typical moment corresponds to the AUV approaching the end region of the interface. At this time, the channel geometry width itself has decreased significantly. The controller simultaneously retains a lateral safety margin based on the remaining flow field uncertainty and limits the final approach velocity. The results of the changes at these three moments verify the continuous coupling relationship between flow field uncertainty, lateral feasible region, and approach velocity.

[0344] Furthermore, the comparison results of the dynamic docking trajectories of the method of this invention and the fixed constraint control method under spatially varying flow fields are as follows: Figure 4As shown. The comparative method uses traditional constant flow field estimation and fixed-constraint NMPC, while the method of this invention uses local affine flow field estimation, flow field covariance propagation, and uncertainty-driven constraint contraction NMPC. Both methods use the same AUV initial position, the same dynamic docking platform trajectory, the same real-space changing flow field, and the same control input capability, thus ensuring that the comparison results mainly reflect the differences in flow field estimation methods and docking constraint processing methods.

[0345] Figure 4 The horizontal axis represents the axial position of the AUV relative to the dynamic docking interface, with the AUV gradually approaching the interface from left to right; the vertical axis represents the lateral error of the AUV relative to the docking axis. The light-colored filled area represents the actual physical docking safety channel, whose width gradually narrows as the axial distance between the AUV and the docking interface decreases; the star-shaped marker represents the dynamic docking interface, the circular marker represents the common initial position of both methods, and the green area represents the successful docking area. The red dashed line represents the control trajectory using a constant flow field and fixed constraints, the blue solid line represents the trajectory of the present invention, and the red cross-shaped marker indicates the location where the control method violated the lateral constraints.

[0346] It can be seen that, in the phase away from the docking interface, both methods enable the AUV to gradually approach the docking axis with relatively limited trajectory differences. However, as the AUV enters the region near the docking interface, the local spatial changes in the actual flow field gradually intensify. Traditional constant flow field estimation cannot accurately predict the lateral velocity at future locations, leading to a deviation between the nominal predicted trajectory and the actual executed trajectory of the control method. Because the control method consistently uses fixed geometric constraints and does not reserve additional safety margins based on flow field estimation errors, its trajectory exhibits multiple lateral constraint violations at the docking end and shows significant lateral deviation as it approaches the docking interface.

[0347] In contrast, the method of this invention utilizes local flow field gradients to correct the flow velocity at different locations within the prediction time domain and calculates the uncertainty based on the covariance of flow field parameters. When the local flow field change intensifies or the reliability of the flow field estimation decreases, the controller actively tightens the lateral nominal feasible region while reducing the maximum axial approach velocity, allowing the AUV to complete lateral error correction before entering the narrow docking region; as the flow field estimation gradually stabilizes, the feasible region and approach velocity are then continuously restored. Figure 4 As can be seen, the trajectory of the method of the present invention remains within the physical safety channel throughout the approach to the docking interface and eventually enters the docking success area, without any lateral constraint violation points visible in the figure.

[0348] also, Figure 4The inset in the upper right corner illustrates the actual motion trajectories of the dynamic docking platform and the AUV in an inertial coordinate system. Since the dynamic docking platform continuously moves forward in the x-direction and slowly oscillates in the y-direction, the absolute motion trajectory of the AUV is not oriented towards a fixed spatial point, but rather requires simultaneous compensation for both platform motion and the changing spatial flow field. The inset results show that both methods can follow the overall motion trend of the dynamic docking platform, but the control method exhibits a more pronounced lateral offset near the end, while the method of this invention maintains a smaller relative lateral error under continuous platform motion and achieves positional convergence with the dynamic docking interface at the simulation end.

[0349] comprehensive Figures 2 to 4 Simulation results show that the method of this invention does not rely solely on the root mean square error of the flow field in a single region to evaluate the control effect. Instead, it incorporates spatial position variations into the flow field prediction through a local affine flow field model, enabling the nonlinear model predictive control to obtain position-dependent velocity inputs within the prediction time domain. Specifically, Figure 2 The root mean square error (RMSE) of the regional flow field estimated by the traditional constant flow field method is 0.528 m / s, while the RMSE of the regional flow field estimated by the local affine flow field method of this invention is 0.564 m / s. The overall errors of the two methods within the evaluation region are similar. However, the traditional constant flow field estimation can only reuse a single velocity vector within the prediction region, making it difficult to express the flow direction curvature and spatial velocity variations in the near-interface region. In contrast, the method of this invention can form a locally spatially varying flow field based on the historical displacement of the AUV and the velocity residuals, and propagate the covariance of the flow field parameters to different locations in the predicted trajectory, further forming a flow field uncertainty index. Subsequently, the method of this invention uses this uncertainty to continuously shrink the lateral feasible region and adjust the maximum axial approach velocity, enabling the AUV to proactively retain a safety margin when the reliability of the flow field estimation is insufficient, rather than rashly entering the narrow docking region according to fixed constraints. Under the same spatially varying flow field and dynamic platform conditions, traditional constant flow field and fixed constraint methods exhibit lateral constraint violations at the docking end. However, the method of this invention can keep the trajectory within the physical docking channel and enter the successful docking region, thereby verifying the continuous technical interaction relationship between local flow field gradient estimation, flow field uncertainty propagation, and constraint contraction NMPC.

[0350] The above description is only a preferred embodiment of the present invention. It should be noted that for those skilled in the art, several improvements and modifications can be made without departing from the principle of the present invention, and these improvements and modifications should also be considered within the scope of protection of the present invention.

Claims

1. A dynamic docking method for AUVs based on flow field state estimation and nonlinear model predictive control, characterized in that, Includes the following steps: Step 1: Within the scrolling history window, using the actual ground displacement, relative water movement, attitude, control input, and relative position and attitude between the AUV and the dynamic docking platform, an affine model composed of the velocity baseline and the flow field gradient is used to characterize the local flow field near the current position, and a unified parameterized mapping from spatial position change to local flow field change is established. Step 2: Construct a joint estimation model of AUV navigation status, current position velocity, and local flow field gradient; evaluate the sufficiency of historical motion observation information by using the rank and minimum eigenvalue of the flow field gradient information matrix; when the observation information is insufficient, maintain the stability of the estimation problem by using the prior constraint of the flow field gradient, and retain the uncertainty of the unidentifiable direction by using the posterior covariance; jointly estimate and output the AUV navigation status, current local velocity, local flow field gradient, and posterior covariance of flow field parameters. Step 3: Establish a follow-up docking coordinate system based on the real-time attitude of the dynamic docking platform. Construct the docking error state using the axial distance, lateral position deviation, attitude deviation, and relative velocity of the AUV relative to the docking platform. Propagate the estimated current flow velocity and flow field gradient along the predicted trajectory to obtain the local flow velocity corresponding to each position in the predicted time domain. Combine the candidate control sequence with the nonlinear AUV motion model to recursively deduce the future state and then optimize the control sequence. Step 4: Propagate the posterior covariance of the flow field parameters into the velocity covariance of each prediction step based on the future predicted position, extract the most unfavorable flow field uncertainty in the prediction time domain, and continuously shrink the lateral position, attitude and axial approach velocity constraints based on the uncertainty. Step 5: When the flow field estimation uncertainty meets the safety threshold, the controller will continue to complete the approach and docking within the contracted feasible region. When the flow field estimation uncertainty does not meet the safety threshold, the control target will be switched to a safe waiting state outside the docking channel, so that the AUV reduces the approach speed and maintains a safe distance. After the flow field observability improves and the estimated covariance decreases to the safety threshold, docking will resume.

2. The AUV dynamic docking method according to claim 1, characterized in that, Step 1 establishes a unified parameterized mapping from spatial location changes to local flow field changes, specifically including: Step 1.1, at discrete time Next, position the AUV. ,attitude and speed Composition as navigation state The dynamic docking platform status is represented as The superscript d indicates the status of the docking platform; The current observation is formed by the AUV's own navigation sensors, relative distance sensors, relative orientation sensors, and docking platform status information. Establish basic data mapping relationships: ； in, Indicates the length of the history window. Indicates that AUV is in the first Time-based control input, This indicates the local flow velocity at the current AUV location. This represents the current local flow field gradient; Step 1.2: Based on the current AUV position An affine model combining velocity baselines and flow field gradient position offsets is used to calculate the position at any location within the historical window. Local flow velocity at the location : ； make The local velocity baseline and gradient parameters are combined into a single estimator variable. The local flow velocity is linearized as follows: ； in, For the first The local flow field parameter mapping matrix corresponding to each historical observation position is determined by the historical position difference; Step 1.3: Establish a unified parameterized mapping from spatial location changes to local flow field changes, expressed as: ； Make the current flow rate and flow field gradient At the same time, it becomes an estimable parameter.

3. The AUV dynamic docking method according to claim 2, characterized in that, In step 2, a joint estimation model is established for the AUV's navigation state, current position velocity, and local flow field gradient, expressed as: ； in, The prior state as the starting point of the historical window. Its prior error covariance; This represents theoretical observations calculated from the AUV's status and the dynamic docking platform's status. For observation error covariance; For discrete AUV nonlinear motion model; The model error covariance; The prior covariance of the flow field gradient; Discrete time Lower local flow field gradient matrix Vectorized representation of; Perform joint estimation and output posterior estimation results, including: current AUV state estimate. Current local velocity estimation Current local flow field gradient estimation Posterior covariance of flow field parameters .

4. The AUV dynamic docking method according to claim 3, characterized in that, In step 2, the sufficiency of historical motion observation information is evaluated by the rank and minimum eigenvalue of the flow field gradient information matrix, specifically as follows: Constructing the flow field gradient information matrix : ； in, For velocity residual covariance; For the first The flow field gradient regression matrix corresponds to each historical observation location; the superscript T indicates transpose. when At that time, the flow field gradient It possesses complete local identifiability; further calculation of the minimum eigenvalue of the flow field gradient information matrix. ,in The larger the value, the more complete the observation information in each gradient direction; When observational information is insufficient, estimation regularization is achieved through Gaussian prior constraints on the flow field gradient, and the regularization result is not directly used as reliable flow field information.

5. The AUV dynamic docking method according to claim 3, characterized in that, In step 3, the future state is recursively deduced using a nonlinear AUV motion model, specifically as follows: Step 3.1: Establish a follow-up docking coordinate system based on the real-time attitude of the dynamic docking platform: ； in, To connect to the platform at any time The corresponding attitude rotation matrix; For dynamic connection platform at all times The three-dimensional pose; This is a three-dimensional rotational transformation; Step 3.2: Calculate the three-dimensional position error of the AUV relative to the dynamic docking platform. 3D attitude error and relative velocity error : ； ； ； in, For dynamic connection platform at all times Location, For dynamic connection platform at all times speed; Step 3.3: Assemble the three-dimensional docking error state : ； Step 3.4: Propagate the current velocity along the predicted trajectory and calculate the future state: In the prediction time domain Inside, at the current moment State estimation Let the initial value be the initial state. Calculate the future number Predict the flow velocity at the step location : ； Future states are predicted recursively using a nonlinear AUV motion model. : ； Step 3.5: Form a continuously progressive state calculation chain within the prediction time domain: ； in, For the current moment Based on the state estimation results, candidate control sequences, and predicted flow field, the future flow field is... AUV position obtained at each predicted time; For at any time go through The predicted time-domain end AUV state obtained through step recursion is used as the terminal prediction state.

6. The AUV dynamic docking method according to claim 5, characterized in that, In step 4, the lateral position range, attitude allowable range, and axial approach speed range of the docking channel are continuously narrowed according to the uncertainty, specifically as follows: Step 4.1: Calculate the posterior covariance of the flow field parameters. Propagating along the predicted trajectory, we obtain the first... Flow velocity covariance at each predicted location : ； in, For the current moment Next time The local flow field parameter mapping matrix corresponding to each predicted location is used to map the unified flow field parameter estimates. Mapped to the future Estimated flow velocity at each predicted location ; Step 4.2: Extract the most unfavorable flow field uncertainty based on the flow field covariance over the entire prediction time domain. : ； in, This represents the largest eigenvalue of the covariance matrix; Step 4.3: Construct the docking feasible region driven by flow field uncertainty. : ； in, Indicates the axial distance from the AUV to the interface. This refers to the maximum permissible axial distance within the docking control area; This indicates the lateral positional error perpendicular to the docking axis; This is the upper limit of the basic lateral positional error allowed near the interface. The channel expansion factor that allows lateral error to vary with axial distance; Represents the relative velocity components along the docking axis; and These are the influence coefficients of flow field uncertainty on lateral position constraints, attitude constraints, and axial relative velocity constraints, respectively. This represents the maximum allowable attitude deviation when the flow field estimation is sufficiently reliable. The maximum permissible axial relative velocity when the flow field estimation is sufficiently reliable; Step 4.4, when Decreasing the value indicates improved reliability of the flow field estimation, and the feasible docking region gradually recovers to the deterministic boundary; when As the value increases, the permissible lateral position error, attitude error, and approach speed decrease simultaneously.

7. The AUV dynamic docking method according to claim 6, characterized in that, Step 5 involves switching the control target, specifically including: Set failure protection threshold : ； when At that time, the feasible docking domain is adopted. The controller completes the docking within the feasible domain; when When switching to the safe waiting feasible domain. Control the AUV to maintain a safe distance and reduce its approach speed, wait The docking was subsequently restored; The safe waiting feasible region , is represented as: ； in, To maintain a safe waiting distance, , , This represents the allowable deviation range during the waiting phase.

8. The AUV dynamic docking method according to claim 1, characterized in that, The closed-loop control process of the dynamic docking method includes: Local flow field estimation results are generated by historical observations and AUV motion residuals, and future location-related velocity predictions are generated from the local flow field estimation results. The future dynamic docking error is generated by the predicted flow velocity and the nonlinear AUV motion model. A safe and feasible docking region is constructed based on the posterior covariance of the flow field parameters. The safe and feasible docking region is used as a constraint for the nonlinear model predictive control to obtain the optimal predictive control sequence. The first control variable in the optimal predictive control sequence is executed. When the flow field information is insufficient, the system switches to a safe waiting state. After the AUV executes the control input, it generates new position, attitude, and velocity observations and returns to the flow field-state joint estimation process. It then re-enters the joint estimation, so that the flow field estimation results are not only used to compensate for the current motion deviation, but also directly participate in future state prediction and docking safety decision-making, forming a rolling closed-loop control.

9. An electronic device, characterized in that, include: One or more processors; A storage device on which one or more programs are stored; When the one or more programs are executed by the one or more processors, the one or more processors implement the AUV dynamic docking method as described in any one of claims 1 to 8.

10. A computer-readable storage medium, characterized in that, It stores a computer program that, when executed by a processor, implements the steps of the AUV dynamic docking method according to any one of claims 1 to 8.

Images