Unmanned underwater vehicle anti-interference tracking control method based on model prediction

Abstract

The invention discloses an unmanned underwater vehicle anti-interference tracking control method based on model prediction, and aims to solve the technical bottlenecks that a traditional linearization model ignores nonlinear dynamic characteristics, an existing anti-interference method lacks an execution mechanism constraint processing mechanism, and MPC is difficult to collaboratively optimize tracking precision, energy consumption and constraint conditions under three-degree-of-freedom coupling. Multi-source uncertain interference in a marine environment is considered in design of a controller, physical constraints of an execution mechanism are converted into hard boundary conditions of a convex optimization problem, and an optimal control sequence with anti-interference performance and constraint satisfaction is solved by adopting a rolling time domain optimization strategy. According to the method, the trajectory tracking robustness of the AUV under the complex ocean working condition is remarkably improved, and meanwhile, the dynamic constraint compliance of the system within the full working condition range is ensured.

Description

Technical Field

[0001] The present invention belongs to the technical field of underwater vehicles, and in particular relates to an anti-disturbance tracking control method for an unmanned underwater vehicle based on model prediction. Background Art

[0002] As critical equipment for marine resource exploration and environmental monitoring, AUVs' trajectory tracking capabilities directly impact mission performance. However, the time-varying currents, turbulent disturbances, and obstacle distribution in complex underwater environments lead to highly nonlinear and multi-degree-of-freedom coupled dynamics in AUVs. Traditional control strategies based on kinematic simplification struggle to meet the interference rejection requirements in highly dynamic scenarios.

[0003] In terms of control technology development, existing AUV trajectory tracking methods often use PID, sliding mode control, or backstepping methods. These designs often rely on linearized models or static disturbance assumptions, making it difficult to effectively reconcile the conflicts between model uncertainty, environmental disturbances, and the physical constraints of the actuators. Especially under strong disturbance conditions, problems such as control variable saturation and dynamic response lag frequently occur, severely restricting the system's robustness and tracking accuracy.

[0004] In recent years, MPC technology has been gradually applied to the control of underwater vehicles (AUVs), leveraging its rolling optimization, multi-objective constraint handling, and feedforward-feedback coordination mechanisms. Existing research indicates that MPC can effectively address the three-degree-of-freedom motion coupling characteristics of AUVs by solving optimization problems online within a finite time domain. However, technical bottlenecks remain in nonlinear dynamic modeling, real-time compensation for unknown disturbances, and ensuring optimal feasibility under sudden interference.

[0005] Existing technical solutions:

[0006] In response to the challenges of strong nonlinearity, time-varying interference and physical constraints of AUVs, different control technologies have been proposed and applied to improve the anti-disturbance tracking performance.

[0007] Active Disturbance Rejection Control (ADRC) technology uses a linear extended state observer to estimate internal and external disturbances in real time and directly compensate for them in the control variable, reducing model dependence. For example, it uses a fuzzy neural network to adaptively adjust controller parameters to improve robustness to time-varying disturbances such as ocean currents and turbulence.

[0008] Sliding Mode Control (SMC) technology uses nonlinear functions to suppress chattering and dynamically switches control strategies based on disturbance thresholds, balancing response speed and stability. In high-disturbance scenarios, SMC uses a nonlinear disturbance observer to estimate the total uncertainty of water circulation disturbances, achieving finite-time convergence of the heading tracking error.

[0009] MPC uses a rolling-horizon optimization strategy to coordinate multi-objective optimization problems such as trajectory tracking accuracy, energy consumption, and physical constraints, with outstanding performance in three-degree-of-freedom coupled motion. Furthermore, MPC is combined with an artificial potential field method to handle dynamic obstacle avoidance, and an extended Kalman filter is used to predict trajectory oscillations, achieving integrated navigation, tracking, and obstacle avoidance control.

[0010] Although ADRC can achieve real-time estimation and compensation of disturbances through an extended state observer, its performance is highly dependent on the tuning of the observer parameters. In particular, in the dynamic scenarios of AUVs with strong nonlinearity and multi-degree-of-freedom coupling, the matching relationship between the observer bandwidth and the system's dynamic characteristics is difficult to globally optimize. For example, when the AUV performs large-angle maneuvers or encounters high-frequency turbulence disturbances, the extended state observer's estimation of the nonlinear hydrodynamic damping term has a significant lag, resulting in incomplete compensation and even a decrease in phase margin, exacerbating the steady-state error of trajectory tracking. In addition, traditional ADRC lacks an explicit processing mechanism for the physical constraints of the actuator, requiring the additional introduction of a constraint softening strategy, which may sacrifice the dynamic response performance of the control system.

[0011] Although SMC is known for its strong robustness, its design nature, which relies on high-frequency switching functions, leads to inherent jitter in the control signal. The response hysteresis and input saturation characteristics of the AUV actuator further amplify the jitter effect, causing increased thruster wear and even system oscillation instability. In addition, the conservative assumption of the upper bound of the disturbance made by traditional SMC difficult to adapt to the time-varying disturbances in complex ocean environments. If an adaptive law is used to dynamically adjust the switching gain, although the jitter can be further alleviated, it will introduce additional parameter identification errors and reduce the transient performance of disturbance suppression. In particular, in the multi-degree-of-freedom collaborative control of AUVs, the coupled design of multiple sliding surfaces can easily lead to a surge in the complexity of the control law and lack systematic support for multi-objective optimization, limiting its applicability in long-duration missions. Summary of the Invention

[0012] In order to overcome the shortcomings of the existing technology, the present invention provides an anti-disturbance tracking control method for unmanned underwater vehicles based on model prediction. In view of the technical bottlenecks that traditional linearized models ignore nonlinear dynamic characteristics, existing anti-disturbance methods lack an actuator constraint processing mechanism, and MPC is difficult to coordinately optimize tracking accuracy, energy consumption and constraints under three-degree-of-freedom coupling, the multi-source uncertain interference in the marine environment is taken into account in the design of the controller, and the physical constraints of the actuator are converted into hard boundary conditions of the convex optimization problem. The rolling time domain optimization strategy is used to solve the optimal control sequence with anti-disturbance performance and constraint satisfaction. This method significantly improves the trajectory tracking robustness of the AUV under complex marine conditions, while ensuring the dynamic constraint compliance of the system under the full range of working conditions.

[0013] The technical solutions adopted by the present invention to solve the technical problems are as follows:

[0014] Step 1: Construct AUV kinematic model and dynamic model;

[0015] Step 2: Define the constraint set that controls the input;

[0016] Step 3: Disturbance rejection and tracking control.

[0017] Preferably, the step 1 is specifically:

[0018] Step 1-1: Establish inertial coordinate system O E -x E y E z E and body coordinate system O B -x B y B z B The dual reference system characterizes the AUV planar motion characteristics;

[0019] The inertial coordinate system is based on a reference point O selected on the surface of the Earth. E is the origin, x E Point in the appropriate direction in the horizontal plane, y E Located on the horizontal plane x E O E z E The body coordinate system is fixed to the AUV body and its origin is set at the buoyancy center position O. B , x B Extending along the AUV vertical axis and pointing forward, z B Located in the longitudinal symmetry plane of the AUV and pointing downward, y B Perpendicular to Ox B z B flat and pointing to the right;

[0020] The dual reference system covers the three degrees of freedom parameters of the AUV in planar motion, including displacement and yaw along the x and y axes; the pitch and roll motion of the AUV in planar motion are ignored. The conversion between the inertial coordinate system and the body coordinate system is achieved through the rotation matrix, which is defined as:

[0021]

[0022] Where, ψ is the heading angle of the AUV;

[0023] Therefore, the kinematic model of AUV planar motion is expressed as:

[0024]

[0025] in, Indicates the position and attitude of the AUV's planar motion, represents the speed of the AUV; u, v, and r represent the forward velocity, lateral velocity, and angular velocity of the AUV's horizontal motion in the global coordinate system, respectively;

[0026] Step 1-2: Consider the actuators of the fully driven AUV for the dynamic model, including the two lateral auxiliary thrusters in the plane, the main thruster that provides forward thrust, and the vertical rudder; the main thruster moves along the AUV baseline, that is, x B Axis arrangement, providing thrust input for longitudinal advance and retreat freedom Surge; auxiliary thrusters are symmetrically distributed in pairs on the horizontal plane x B y B The plane independently controls the lateral translational freedom Sway; the cross rudder is installed on the tail guide surface, which generates yaw moment by deflecting the rudder angle to control the heading freedom Yaw;

[0027] Considering the comprehensive interference terms such as fluid parameter uncertainty, unmodeled hydrodynamic effects and environmental disturbances The AUV dynamic model is established as:

[0028]

[0029] in,

[0030]

[0031]

[0032] D(v)=-diag{X u +X |u|u |u|Y v +Y |v|v |v|N r +N |r|r |r|}

[0033] M is the inertia matrix of the system, C(v) is the Coriolis centripetal force matrix, and D(v) is the damping matrix; The force and torque vectors of the AUV motion are defined, and the constraint set U of the control input is defined, then τ∈U; d=[d X d Y d N ] T is an unknown interference vector, there exists a positive satisfy m represents the mass of the AUV; represents the longitudinal additional mass coefficient caused by the longitudinal acceleration, Indicates the lateral additional mass coefficient caused by lateral acceleration, I z Represents winding z BThe moment of inertia generated by the shaft, Indicates the additional moment of inertia coefficient caused by the heading acceleration, X u represents the longitudinal linear water damping coefficient caused by the longitudinal velocity, X |u|u represents the second-order longitudinal nonlinear water damping coefficient generated by the longitudinal velocity, Y v represents the lateral linear water damping coefficient caused by the lateral velocity, Y |v|v represents the second-order lateral nonlinear water damping coefficient caused by the lateral velocity, N r Indicates the linear water damping coefficient caused by the angular velocity, N |r|r It represents the second-order heading nonlinear water damping coefficient caused by the heading angular velocity;

[0034] In summary, the planar motion model of the fully driven AUV is expressed as:

[0035]

[0036] Preferably, the step 2 is specifically as follows:

[0037] Step 2-1: Define the thrust of the auxiliary thrusters as T1 and T2, the thrust provided by the main thruster as T3; the rudder angle of the vertical rudder is δ R express;

[0038] The resultant force and moment acting on the AUV are expressed as:

[0039] τ=[F X F Y N] T =τ T +τ R (5)

[0040] Among them, τ T and τ R are the forces and moments generated by the propeller and rudder, respectively, expressed as:

[0041]

[0042] Each actuator's control input will generate corresponding forces and torques:

[0043]

[0044]

[0045] in, is the hydrodynamic coefficient of the steering gear force, is the hydrodynamic coefficient of the moment; L1 and L2 are the distances between the two auxiliary thrusters and the AUV's center of mass;

[0046]

[0047] Therefore, the mapping relationship between the actuator input vector and the resultant force and torque acting on the AUV can be described as follows:

[0048] τ=B τ u (10)

[0049] in, is the actuator control input vector; B τ is the control allocation matrix, which is derived as:

[0050]

[0051] Step 2-2: During the dynamic response of the thrusters, subject to the limitations of motor power, mechanical structure strength, and fluid dynamic characteristics, the effective thrust of each thruster must meet the following requirements:

[0052] T i,min ≤T i ≤T i,max i=1,2,3 (12)

[0053] Among them, T i represents the thrust of the i-th thruster, T i,min and T i,max are the minimum and maximum thrust values ​​of the i-th propeller, which are the minimum / maximum thrust thresholds of the propeller, respectively. Their values ​​are determined by the propeller geometry parameters, the rated power of the propulsion motor, and the discharge characteristics of the power battery;

[0054] Step 2-3: The real-time deflection angle of a single control surface must meet the mechanical limit and hydrodynamic stability requirements:

[0055] δ R,min ≤δ R ≤δ R,max (13)

[0056] Among them, δ R,min and δ R,max They are the minimum and maximum allowable deflection angles of the rudder, respectively, which are determined by the maximum angle of the steering gear servo mechanism, the clearance limit between the rudder and the hull, and the critical angle of fluid stall;

[0057] Define the above equations (12) and (13) together as the constraint set U of the control input, then u∈U.

[0058] Preferably, the step 3 is specifically as follows:

[0059] Step 3-1: The plane motion model in equation (4) is derived as follows:

[0060]

[0061] Equation (14) is discretized, and the discrete-time dynamic model with a sampling period of T is derived using the Newton-Euler method as follows:

[0062]

[0063] Among them, A(v k )=I-TM -1 (C(v k )+D(v k )), B(v k )=TM -1 B τ (v k );w k =TM -1 d k is the interference value at the kth moment, which is limited to the interference set W, that is, w k ∈W;v k+1 and v k Denote the velocity vector of AUV at time k+1 and k, respectively, d k represents the interference term at the kth moment, Θ k represents the actuator action vector at the kth moment, B τ (v k ) represents the transformation matrix between the actuator action and the control force and torque at the kth moment;

[0064] Step 3-2: Without considering the disturbance, the nominal model of AUV dynamics is established as:

[0065]

[0066] in, and denote the nominal speed of the AUV at time k+1 and k, respectively;

[0067] Let v d is the reference velocity vector, then the velocity tracking error is defined as is the nominal control force, u k is the action vector at the kth moment, and the nominal action vector satisfies

[0068] Step 3-3: Define the cost function J of the MPC controller as:

[0069]

[0070] Among them, Q, R, and P are the weight matrices of AUV state, control force, and terminal error respectively; N is the prediction time domain; and is the state error and control input predicted at step i at time k; represents the state error of the Nth step at the kth moment;

[0071] The controller constraints are defined as:

[0072]

[0073] in, is the state error set, Ω(ε) is the terminal constraint set, represents the set of control input constraints for the nominal system;

[0074] Step 3-4: In summary, the MPC anti-disturbance controller of the unmanned underwater navigation system is:

[0075]

[0076] The constraints are:

[0077]

[0078] A computer program enables a computer to execute the above-mentioned anti-disturbance tracking control method for an unmanned underwater vehicle.

[0079] An electronic device comprises: a processor and a memory; the memory is used to store a computer program, and the processor is used to execute the computer program stored in the memory, so that the electronic device performs the above-mentioned unmanned underwater vehicle anti-disturbance tracking control method.

[0080] A computer-readable storage medium stores a computer program, which, when executed by a processor, implements the above-mentioned unmanned underwater vehicle anti-disturbance tracking control method.

[0081] A chip includes: a processor for calling and running a computer program from a memory, so that a device equipped with the chip executes the above-mentioned unmanned underwater vehicle anti-disturbance tracking control method.

[0082] A computer program product, comprising a computer storage medium storing a computer program, wherein the computer program comprises instructions executable by at least one processor, and wherein the instructions, when executed by the at least one processor, implement the above-mentioned unmanned underwater vehicle anti-disturbance tracking control method.

[0083] The beneficial effects of the present invention are as follows:

[0084] 1. This paper establishes a motion model for an unmanned underwater navigation system, taking into account marine environmental disturbances and the physical properties of actuators. Actuator modeling describes the relationship between actuator control inputs and the forces and torques applied to the navigation system. This model accounts for actuator control input saturation and provides a predictive basis for the design of a disturbance rejection tracking controller.

[0085] 2. Based on model predictive control, this paper designs a disturbance-rejecting tracking controller for an unmanned underwater vehicle. Through rolling horizon optimization, this controller achieves disturbance-rejecting trajectory tracking in complex ocean environments. The designed controller solves a multi-objective cost function online, including state error, input, and disturbance rejection. While simultaneously considering the physical constraints of the actuators and a bounded set of disturbances, it significantly improves the robustness and adaptability of the AUV in dynamic disturbance environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0086] Figure 1 Schematic diagram of the global coordinate system and the carrier coordinate system;

[0087] Figure 2 It is the flow chart of anti-disturbance control;

[0088] Figure 3 Schematic diagram of control simulation results: (a) motion trajectory; (b) heading angle change;

[0089] Figure 4 Schematic diagram of speed change: (a) linear speed change; (b) heading angular speed change;

[0090] Figure 5 Error change diagram;

[0091] Figure 6 Schematic diagram of control input changes. DETAILED DESCRIPTION

[0092] The present invention will be further described below with reference to the accompanying drawings and examples.

[0093] This paper addresses the multi-source uncertain interference problem in the trajectory tracking control of autonomous underwater vehicles (AUVs) and designs a model-prediction-based anti-disturbance tracking control method for AUVs. Traditional control methods use linearized models that ignore nonlinear and time-varying parameters, leading to model mismatch and reduced system robustness and tracking accuracy. Existing anti-disturbance architectures lack an actuator constraint processing mechanism, which can easily lead to control overruns and instability. Traditional model predictive control (MPC) struggles to balance the synergistic relationship between tracking accuracy, energy optimization, and dynamic constraints under three-degree-of-freedom coupling, and is prone to infeasible prediction domain optimization issues in the event of sudden disturbances.

[0094] To address these shortcomings, this paper proposes a model-prediction-based anti-disturbance tracking control method for unmanned underwater vehicles. This method incorporates multi-source uncertain interference in the marine environment into the controller design. The physical constraints of the actuators are transformed into hard boundary conditions for a convex optimization problem, and a rolling-horizon optimization strategy is employed to solve the optimal control sequence that achieves both anti-disturbance performance and constraint satisfaction. This method significantly improves the robustness of the AUV's trajectory tracking in complex marine conditions while ensuring compliance with dynamic constraints across the entire operating range.

[0095] The present invention aims to solve the problems of model mismatch and control input saturation caused by multi-source uncertain interference in AUV trajectory tracking control. In view of the technical bottlenecks that traditional linearized models ignore nonlinear dynamic characteristics, existing anti-interference methods lack actuator constraint processing mechanisms, and MPC is difficult to coordinately optimize tracking accuracy, energy consumption and constraints under three-degree-of-freedom coupling, a model-prediction-based anti-interference tracking control method is proposed. By incorporating marine environmental interference into the controller design and converting the physical constraints of the actuator into hard boundary conditions of the convex optimization problem, the rolling time domain optimization strategy is combined to generate an optimal control sequence with both anti-interference performance and constraint satisfaction, which significantly improves the trajectory tracking robustness of AUVs in complex marine environments.

[0096] 1. AUV kinematic model and dynamic model;

[0097] The underwater environment is complex and time-varying. Ocean currents, water temperature fluctuations, and obstacle distribution all significantly impact AUV movement. In the design and development of AUVs, establishing accurate kinematic and dynamic models is the core foundation for achieving autonomous navigation, path planning, and control optimization.

[0098] The research object of this paper is a fully driven AUV. In order to achieve accurate motion planning and controller design, the first task is to build a planar kinematic and dynamic model of a three-degree-of-freedom cross-rudder AUV.

[0099] Coordinate system description for AUV planar motion, Figure 1 By establishing an inertial coordinate system (O E -x E y E z E ) and body coordinate system (O B -x B y B z B ) is used to characterize its motion characteristics. The inertial coordinate system is based on the reference point O selected on the surface of the earth. E is the origin, x E Point in the appropriate direction in the horizontal plane, y E Located on the horizontal plane x E O E zE The body coordinate system is fixed to the AUV body and its origin is set at the buoyancy center position O. B , x B Extending along the AUV vertical axis and pointing forward, z B Located in the longitudinal symmetry plane of the AUV and pointing downward, y B Perpendicular to Ox B z B Flat and pointing to the right.

[0100] The dual reference system covers the three degrees of freedom parameters of the AUV in planar motion, including displacement (forward, backward, and lateral movement) and yaw along the x and y axes; the present invention ignores the pitch and roll motion of the AUV in planar motion; the conversion between the two coordinate systems is achieved through the rotation matrix, which can be defined as:

[0101]

[0102] Where ψ is the heading angle of the AUV.

[0103] Therefore, the kinematic model of AUV planar motion can be expressed as:

[0104]

[0105] in, Indicates the position and attitude of the AUV's planar motion, represents the speed of the AUV. u, v, and r represent the forward velocity, lateral velocity, and angular velocity of the AUV's horizontal motion in the global coordinate system.

[0106] For the dynamic model of AUV, the actuator of the fully driven AUV is considered, including the two lateral auxiliary thrusters in the plane, the main thruster that provides forward thrust, and the vertical rudder. B Auxiliary thrusters are arranged in pairs and symmetrically on the horizontal plane (x B y B plane), independently controlling the lateral translational freedom (Sway); the cross rudder is installed on the tail guide surface, generating a yaw moment by deflecting the rudder angle to regulate the heading freedom (Yaw).

[0107] Considering comprehensive interference terms such as fluid parameter uncertainty, unmodeled hydrodynamic effects and environmental disturbances The AUV dynamic model is established as:

[0108]

[0109] in,

[0110]

[0111]

[0112] D(v)=-diag{X u +X |u|u |u|Y v +Y |v|v |v|N r +N |r|r |r|}

[0113] M is the inertia matrix of the system, C(v) is the Coriolis centripetal force matrix, and D(v) is the damping matrix. is the force and torque vector of AUV motion. Define the control force constraint set U, then τ∈U. d=[d X d Y d N ] T is an unknown interference vector, there exists a positive satisfy

[0114] In summary, the planar motion model of the fully driven AUV can be expressed as:

[0115]

[0116] 2. Define the set of constraints that control the input;

[0117] The thrust of the auxiliary thrusters is defined as T1 and T2, and the thrust provided by the main thruster is defined as T3. The rudder angle of the vertical rudder is δ R Since the research object is a fully driven AUV, it is necessary to consider the thrust distribution, so as to construct the mapping relationship between the actuator input vector and the resultant force and torque acting on the AUV.

[0118] The resultant force and moment acting on the AUV can be expressed as:

[0119] τ=[F X F Y N] T =τ T +τ R (25)

[0120] Among them, τ T and τ R are the forces and moments generated by the propeller and rudder, respectively, which can be expressed as:

[0121]

[0122] Each actuator's control input will generate corresponding forces and torques:

[0123]

[0124] in, are the hydrodynamic coefficients of the steering gear force and torque, respectively. The geometric configuration matrix is ​​related to the spatial layout and installation position of the thrusters and rudders. L1 and L2 are the distances between the two auxiliary thrusters and the center of mass of the AUV, respectively.

[0125]

[0126] Therefore, the mapping relationship between the actuator input vector and the resultant force and torque acting on the AUV can be described as:

[0127] τ=B τ u (30)

[0128] in, B is the actuator control input vector. τ The control allocation matrix can be derived as:

[0129]

[0130] In view of the physical constraints of the AUV propulsion system actuator, saturation constraints need to be imposed on the thrust of the thrusters to ensure system controllability. During the dynamic response of the thrusters, subject to the constraints of motor power limitations, mechanical structure strength, and fluid dynamic characteristics, the effective thrust of each thruster must meet the following requirements:

[0131] T i,min ≤T i ≤T i,max (i=1,2,3) (32)

[0132] Among them, T i represents the thrust of the i-th thruster, T i,min and T i,max are the minimum and maximum thrust values ​​of the i-th propeller. are the minimum / maximum thrust thresholds of the propeller, respectively, and their values ​​are determined by the propeller geometry parameters, the rated power of the propulsion motor, and the discharge characteristics of the power battery.

[0133] The real-time deflection angle of a single rudder surface must meet the mechanical limit and hydrodynamic stability requirements:

[0134] δ R,min ≤δ R ≤δ R,max (33)

[0135] Among them, δ R,min and δ R,maxare the minimum and maximum allowable deflection angles of the rudder, respectively, which are determined by the maximum angle of the steering servo mechanism, the clearance limit between the rudder and the hull, and the critical angle of fluid stall. Equations (12) and (13) above are defined together as the constraint set U of the control input, then u∈U.

[0136] 3. Anti-disturbance tracking control method;

[0137] This study is conducted on a fully driven AUV equipped with a three-thruster drive system, which has the ability to control the three degrees of freedom of advance (Surge), lateral movement (Sway) and yaw (Yaw) in the horizontal plane. In the face of complex interference factors such as ocean current disturbances, unmodeled hydrodynamic effects and sensor noise in the marine environment, this paper constructs a robust tracking architecture based on model predictive control. The controller uses a rolling time domain optimization strategy to solve the thrust distribution of the thrusters and the rudder compensation instructions in real time, achieving dynamic compensation for interference and accurate tracking of the motion trajectory. The design process of the controller is as follows: Figure 2 shown.

[0138] For the dynamic model of AUV, in order to facilitate the design of the controller, it needs to be further processed and discretized into the following state equation. The dynamic model in formula (4) can be derived as follows:

[0139]

[0140] The above equation is discretized, and the discrete-time dynamic model with a sampling period of T can be derived using the Newton-Euler method as follows:

[0141]

[0142] Among them, A(v k )=I-TM -1 (C(v k )+D(v k )), B(v k )=TM -1 B τ (v k ). k =TM -1 d k is the interference value at the kth moment, which is limited to the interference set W, that is, w k ∈W.

[0143] In order to meet the real-time requirements and avoid computational delays caused by solving complex robust optimization problems, thereby reducing the dimension of the MPC online optimization problem, the nominal model of the AUV without considering interference is established as

[0144]

[0145] Let vd is the reference velocity vector, then the velocity tracking error is defined as is the nominal control force. k is the action vector at the kth moment, and the nominal action vector satisfies

[0146] In view of the motion control requirements of AUV under unknown ocean disturbances, combined with the requirements of state tracking error suppression, input energy optimization and disturbance robustness, the cost function J of MPC can be designed as:

[0147]

[0148] Where Q, R, and P are the weight matrices of AUV state, control force, and terminal error, respectively. N is the prediction time domain; and is the state error and control input predicted at step i at time k. The matrix P can ensure closed-loop stability by solving the Lyapunov equation.

[0149] The controller constraints are defined as:

[0150]

[0151] in, is the state error set, and Ω(ε) is the terminal constraint set.

[0152] In summary, the MPC anti-disturbance controller of the unmanned underwater navigation system is:

[0153]

[0154] The constraints are:

[0155]

[0156] Example:

[0157] This example uses the CasADi framework to conduct simulation analysis to verify the effectiveness of the proposed disturbance-rejection model predictive control method in an AUV trajectory tracking task. A Z-shaped trajectory tracking scenario is used to test the MPC's tracking capability for complex paths and the stability of its heading control. The simulations utilize publicly available AUV model parameters.

[0158] Set the initial state of AUV to η0 = [00π / 2] T and v0 =

[000] T , sampling time T = 0.01s; the prediction step size of the MPC controller is N = 20; to simulate the actual ocean environment, the fluid dynamic disturbance is:

[0159]

[0160] The simulation results of the anti-disturbance tracking control of the unmanned underwater vehicle based on model prediction are as follows: Figure 3 — Figure 6 shown.

[0161] like Figure 3 (a), the MPC trajectory shows a smooth transition characteristic as a whole. Near the turning points of the reference trajectory, such as x = 20m and x = 40m, the heading is adjusted in advance through predictive optimization to avoid drastic changes in the heading angle. The tracking error of the trajectory in the straight line segment is small, with a deviation in the y direction of about ±1 meter, and the maximum lateral deviation in the turning stage is about 3m. In the straight line tracking segment from x = 40m to x = 60m, the longitudinal error between the MPC trajectory and the reference trajectory gradually converges to within 0.5 meters, reflecting the strengthening effect of the terminal penalty term in the objective function on the steady-state accuracy. Figure 3 (b) The AUV's heading angle undergoes two significant adjustments between 0 and 60 seconds. Around the 10th second, the heading angle rapidly increases from its initial value to approximately 100°. Around the 30th second, the heading angle rapidly decreases to -50°. Around the 50th second, it rises back to a positive heading angle. This verifies the controller's ability to respond to sudden changes in heading.

[0162] like Figure 4 As shown, the linear velocity exhibits periodic fluctuations (amplitude of approximately ±30°) between 0 and 60 seconds. The main thruster is frequently adjusted, with a particularly steep drop around 30 seconds. This is likely related to differential thrust control during sudden heading changes, reflecting the dynamic adjustments made by the MPC to balance trajectory tracking with energy optimization. The angular velocity fluctuates dramatically, peaking during the turning phase. The controller implements heading corrections through rapid rudder deflections in response to unknown disturbances.

[0163] Figure 5 and Figure 6 The changes in position error, heading angle error, and control input during the AUV's anti-disturbance tracking control process are demonstrated. The position error fluctuates between -2 and 10 meters within 60 seconds, showing an overall stable trend, indicating that MPC has basic robustness in controlling the AUV's displacement. However, the maximum error indicates the presence of uncompensated ocean current disturbances or model mismatch issues. Sudden changes of ±145° occur at 20 and 40 seconds, synchronized with the step change in heading command; the error converges to within ±3° in the steady-state phase, verifying the effectiveness of the yaw damping term. From 0 to 60 seconds, the thrust of the auxiliary thruster frequently switches between positive and negative, reflecting the need for lateral attitude adjustment, especially the violent fluctuations around 30 seconds, which are presumably used to compensate for the lateral drift caused by the sudden change in heading, verifying the differential thrust control strategy.

Claims

1. A disturbance rejection tracking control method for an unmanned underwater vehicle based on model prediction, characterized in that: The steps include: Step 1: Construct AUV kinematic model and dynamic model; Step 2: Define the constraint set that controls the input; Step 3: Disturbance rejection and tracking control.

2. The anti-disturbance tracking control method for an unmanned underwater vehicle based on model prediction according to claim 1, characterized in that: The step 1 is specifically as follows: Step 1-1: Establish inertial coordinate system O E -x E y E z E and body coordinate system O B -x B y B z B The dual reference system characterizes the AUV planar motion characteristics; The inertial coordinate system is based on a reference point O selected on the surface of the Earth. E is the origin, x E Point in the appropriate direction in the horizontal plane, y E Located on the horizontal plane x E O E z E The body coordinate system is fixed to the AUV body and its origin is set at the buoyancy center position O. B , x B Extending along the AUV vertical axis and pointing forward, z B Located in the longitudinal symmetry plane of the AUV and pointing downward, y B Perpendicular to Ox B z B Flat and pointing to the right; The dual reference system covers the three degrees of freedom parameters of the AUV in planar motion, including displacement and yaw along the x and y axes; the pitch and roll motion of the AUV in planar motion are ignored. The conversion between the inertial coordinate system and the body coordinate system is achieved through the rotation matrix, which is defined as: Where, ψ is the heading angle of the AUV; Therefore, the kinematic model of AUV planar motion is expressed as: in, Indicates the position and attitude of the AUV's planar motion, represents the speed of the AUV; u, v, and r represent the forward velocity, lateral velocity, and angular velocity of the AUV's horizontal motion in the global coordinate system, respectively; Step 1-2: Consider the actuators of the fully driven AUV for the dynamic model, including the two lateral auxiliary thrusters in the plane, the main thruster that provides forward thrust, and the vertical rudder; the main thruster moves along the AUV baseline, that is, x B Axis arrangement, providing thrust input for longitudinal advance and retreat freedom Surge; auxiliary thrusters are symmetrically distributed in pairs on the horizontal plane x B y B The plane independently controls the lateral translational freedom Sway; the cross rudder is installed on the tail guide surface, which generates yaw moment by deflecting the rudder angle to control the heading freedom Yaw; Considering the comprehensive interference terms such as fluid parameter uncertainty, unmodeled hydrodynamic effects and environmental disturbances The AUV dynamic model is established as: in, D(v)=-diag{X u +X |u|u |u|Y v +Y |v|v |v|N r +N |r|r |r|} M is the inertia matrix of the system, C(v) is the Coriolis centripetal force matrix, and D(v) is the damping matrix; The force and torque vectors of the AUV motion are defined, and the constraint set U of the control input is defined, then τ∈U; d=[d X d Y d N ] T is an unknown interference vector, there exists a positive satisfy m represents the mass of the AUV; represents the longitudinal additional mass coefficient caused by the longitudinal acceleration, Indicates the lateral additional mass coefficient caused by lateral acceleration, I z Represents winding z B The moment of inertia generated by the shaft, Indicates the additional moment of inertia coefficient caused by the heading acceleration, X u represents the longitudinal linear water damping coefficient caused by the longitudinal velocity, X |u|u represents the second-order longitudinal nonlinear water damping coefficient generated by the longitudinal velocity, Y v represents the lateral linear water damping coefficient caused by the lateral velocity, Y |v|v represents the second-order lateral nonlinear water damping coefficient caused by the lateral velocity, N r Indicates the linear water damping coefficient caused by the angular velocity, N |r|r It represents the second-order heading nonlinear water damping coefficient caused by the heading angular velocity; In summary, the planar motion model of the fully driven AUV is expressed as:

3. The anti-disturbance tracking control method for an unmanned underwater vehicle based on model prediction according to claim 2, characterized in that: The step 2 is specifically as follows: Step 2-1: Define the thrust of the auxiliary thrusters as T1 and T2, the thrust provided by the main thruster as T3; the rudder angle of the vertical rudder is δ R express; The resultant force and moment acting on the AUV are expressed as: τ=[F X F Y [N] T =t T +t R (5) Among them, τ T and τ R are the forces and moments generated by the propeller and rudder, respectively, expressed as: Each actuator's control input will generate corresponding forces and torques: in, is the hydrodynamic coefficient of the steering gear force, is the hydrodynamic coefficient of the moment; L1 and L2 are the distances between the two auxiliary thrusters and the AUV's center of mass; Therefore, the mapping relationship between the actuator input vector and the resultant force and torque acting on the AUV can be described as follows: τ=B τ u (10) in, is the actuator control input vector; B τ is the control allocation matrix, which is derived as: Step 2-2: During the dynamic response of the thrusters, subject to the limitations of motor power, mechanical structure strength, and fluid dynamic characteristics, the effective thrust of each thruster must meet the following requirements: T i,min ≤T i ≤T i,max i=1,2,3 (12) Among them, T i represents the thrust of the i-th thruster, T i,min and T i,max are the minimum and maximum thrust values ​​of the i-th propeller, which are the minimum / maximum thrust thresholds of the propeller, respectively. Their values ​​are determined by the propeller geometry parameters, the rated power of the propulsion motor, and the discharge characteristics of the power battery; Step 2-3: The real-time deflection angle of a single control surface must meet the mechanical limit and hydrodynamic stability requirements: d R,min ≤δ R ≤δ R,max (13) Among them, δ R,min and δ R,max They are the minimum and maximum allowable deflection angles of the rudder, respectively, which are determined by the maximum angle of the steering gear servo mechanism, the clearance limit between the rudder and the hull, and the critical angle of fluid stall; Define the above equations (12) and (13) together as the constraint set U of the control input, then u∈U.

4. The anti-disturbance tracking control method for an unmanned underwater vehicle based on model prediction according to claim 3, characterized in that: The step 3 is specifically as follows: Step 3-1: The plane motion model in equation (4) is derived as follows: Equation (14) is discretized, and the discrete-time dynamic model with a sampling period of T is derived using the Newton-Euler method as follows: Among them, A(v k )=I-TM -1 (C(v k )+D(v k )), B(v k )=TM -1 B τ (v k );w k =TM -1 d k is the interference value at the kth moment, which is limited to the interference set W, that is, w k ∈W;v k+1 and v k Denote the velocity vector of AUV at time k+1 and k, respectively, d k represents the interference term at the kth moment, Θ k represents the actuator action vector at the kth moment, B τ (v k ) represents the transformation matrix between the actuator action and the control force and torque at the kth moment; Step 3-2: Without considering the disturbance, the nominal model of AUV dynamics is established as: in, and denote the nominal speed of the AUV at time k+1 and k, respectively; Let v d is the reference velocity vector, then the velocity tracking error is defined as is the nominal control force, u k is the action vector at the kth moment, and the nominal action vector satisfies Step 3-3: Define the cost function J of the MPC controller as: Among them, Q, R, and P are the weight matrices of AUV state, control force, and terminal error respectively; N is the prediction time domain; and is the state error and control input predicted at step i at time k; represents the state error of the Nth step at the kth moment; The controller constraints are defined as: in, is the state error set, Ω(ε) is the terminal constraint set, represents the set of control input constraints for the nominal system; Step 3-4: In summary, the MPC anti-disturbance controller of the unmanned underwater navigation system is: The constraints are:

5. A computer program, characterized in that The computer program enables a computer to execute the method according to any one of claims 1 to 4.

6. An electronic device, characterized in that: include: processor and memory; The memory is used to store a computer program, and the processor is used to execute the computer program stored in the memory, so that the electronic device performs the method according to any one of claims 1 to 4.

7. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the method according to any one of claims 1 to 4 is implemented.

8. A chip, characterized in that: include: A processor, configured to call and run a computer program from a memory, so that a device equipped with the chip executes the method according to any one of claims 1 to 4.

9. A computer program product, characterized in that The computer program product comprises a computer storage medium storing a computer program, wherein the computer program comprises instructions executable by at least one processor, and when the instructions are executed by the at least one processor, the method according to any one of claims 1 to 4 is implemented.

Images