A model prediction-based accurate control method for underactuated underwater vehicles

Abstract

The application belongs to the technical field of underwater vehicle control, and provides a model prediction-based accurate control method for an underactuated underwater vehicle; the application introduces a sea current velocity term into nonlinear model prediction control and performs discretization, so that the prediction model can compensate for position drift caused by the sea current in advance, thereby improving the control accuracy and motion stability of the underwater vehicle in approaching a target point in a current environment; the control process of the vehicle is divided into three stages, and the state error weight matrix and the control input weight matrix are adaptively adjusted according to the current stage, and the velocity constraint boundary is dynamically scaled, so that the vehicle can quickly approach the target in the long-distance stage, and the velocity is automatically reduced and the control weight is increased in the approaching target stage to suppress overshoot and oscillation, thereby realizing the unity of efficiency and steady-state accuracy of the control process.

Description

Technical Field

[0001] This invention relates to the field of underwater vehicle control technology, and in particular to a model-predictive-based precise control method for underactuated underwater vehicles. Background Technology

[0002] Motion control of underwater vehicles in underwater environments typically involves multiple aspects, including position, attitude, velocity, and thruster output. Due to the complexity of the underwater environment, vehicles are susceptible to factors such as ocean current disturbances, uncertain model parameters, limited thruster output, and multi-degree-of-freedom coupling during motion. Therefore, the control system needs to ensure motion accuracy while also maintaining control stability and thruster output smoothness. Existing methods for fixed-point control and trajectory tracking control of underwater vehicles mainly fall into the following categories: PID control method: The PID control method is simple in structure, requires little computation, and is easy to implement in engineering. It is commonly used for heading control, depth control, and speed control of underwater vehicles. This method typically calculates the control input based on the error between the target value and the current state, and then achieves motion control through the thruster output.

[0003] However, traditional PID control methods typically rely on fixed control parameters, and their control performance tends to deteriorate when faced with ocean current disturbances, significant changes in target distance, or pronounced multi-degree-of-freedom coupling. For example, during the approach of a three-dimensional target point, the response speeds in the horizontal and depth directions may be inconsistent, resulting in situations where the horizontal position is close to the target while the depth is still changing slowly. Furthermore, when the error is large, the PID output may generate significant thrust fluctuations, leading to unstable thruster output.

[0004] Model predictive control (MRC) is a method that predicts the future motion state of an underwater vehicle within a certain prediction time domain based on its motion model, and obtains a better control input by optimizing the objective function. Compared with traditional PID control, MRC can handle problems such as velocity constraints and control input constraints, making it suitable for multivariable control tasks of underwater vehicles.

[0005] However, existing model predictive control methods still have certain shortcomings when applied to underwater vehicles. On the one hand, some methods do not fully consider the impact of ocean current disturbances on the predictive model, resulting in deviations between the predicted trajectory and the actual motion. On the other hand, control weights and velocity constraints usually use fixed parameters, making it difficult to dynamically adjust them according to different control requirements at long-distance approach, mid-range adjustment, and target approach stages. This can easily lead to overshoot, oscillation, or slow convergence when approaching the target.

[0006] Thrust Distribution Methods: Underwater vehicles typically generate the thrust required for motion through multiple thrusters. The desired motion command calculated by the controller needs to be further converted into the actual thrust output of each thruster. Existing methods often employ fixed-ratio distribution or empirical thrust distribution. These methods are simple to implement, but it is difficult to simultaneously consider the maximum thrust, minimum thrust, thrust change rate, and coordination relationship between multiple thrusters.

[0007] When a vehicle's attitude changes rapidly or its control error is large, a simple thrust distribution method may cause abrupt changes in thrust output, resulting in significant fluctuations in the thrust curve. This problem not only affects the vehicle's motion stability but also undermines the long-term reliable operation of practical underwater propulsion systems.

[0008] In summary, existing underwater vehicle control methods still suffer from problems such as insufficient compensation for ocean current disturbances, inconsistent response speeds in different directions, a single control strategy during the target approach phase, difficulty in adaptively adjusting control parameters, and large fluctuations in thruster output during the three-dimensional target approach and stationary control processes. Summary of the Invention

[0009] Therefore, one objective of this invention is to propose a model-predictive-based precise control method for underactuated underwater vehicles, which can achieve relatively stable three-dimensional motion control in turbulent ocean current environments, and make the horizontal position, depth and attitude responses more coordinated, while reducing thruster output abrupt changes and improving the control effect of the vehicle approaching the target point and maintaining stable stay.

[0010] To achieve the above objectives, the present invention provides a model-predictive-based precise control method for underactuated underwater vehicles, comprising: Obtain the current pose of the underactuated underwater vehicle Current speed Target state and ocean current velocity ; According to the current pose With the target state Calculate 3D distance And based on a preset long-distance threshold and close to the target threshold Determine the current control phase The control phase Including the long-distance approach phase Mid-range adjustment phase and approaching the target stage ; Based on the ocean current velocity A nonlinear model predictive control (NMPC) prediction model with ocean current compensation is constructed. The model is discretized, and based on the current control stage... Adaptive adjustment of the state weight matrix in the NMPC optimization problem and control input weight matrix and the lower limit of the control input. and upper limit ; Solve the NMPC optimization problem to obtain the desired velocity vector. ,in For the desired longitudinal velocity, For the desired vertical velocity, For the desired pitch angular velocity, The desired yaw rate; The desired velocity vector is then subjected to triaxial coordination correction to obtain the coordinated desired velocity vector. ; Based on the coordinated desired velocity vector With current speed The error is calculated by the PID controller to determine the generalized force. ; A thrust optimization allocation objective function is constructed, and the actual thrust output of each thruster is obtained by combining the upper and lower limits of thrust and the thrust rate of change constraints. It is used to drive the movement of underwater vehicles.

[0011] As a preferred option, define the basic state error weight matrix: ,in, These are the base values ​​for northward position, eastward position, and depth error weights, respectively. These are the base values ​​for the error weights of roll angle, pitch angle, and yaw angle, respectively. Define the basic control input weight matrix: ,in, These are the base values ​​for the control input weights of the desired longitudinal velocity and the desired vertical velocity, respectively. These are the base values ​​for the control input weights of the desired pitch rate and desired yaw rate, respectively. In the current control phase Below, the state weight matrix ,in These are the northward position error weights, the eastward position error weights, and the depth error weights, respectively. These are the roll angle error weights, pitch angle error weights, and yaw angle error weights, respectively. Control input weight matrix ,in, These are the weights for the expected longitudinal velocity and the expected vertical velocity, respectively. These are the weights for the expected pitch rate and the expected yaw rate, respectively. Based on the current control stage Adaptive adjustment of the state weight matrix in the NMPC optimization problem and control input weight matrix and the lower limit of the control input. and upper limit include: During the long-distance approach phase : ; During the mid-range adjustment phase : ; Approaching the goal According to the distance factor Dynamically increase the weights for northward position error, eastward position error, and depth error: ; ; ; and: ; ; The lower limit of the control input and upper limit Based on the stage scaling factor Scaling is performed.

[0012] Preferably, the kinematic equations of the nonlinear model predictive control (NMPC) prediction model with ocean current compensation are expressed as follows: ; ; ; ; ; ; in, This represents the velocity component of the ocean current in the northward direction. This represents the velocity component of the ocean current in the eastward direction. This represents the velocity component of the ocean current in the depth direction.

[0013] Preferably, the three-axis coordinated correction of the desired velocity vector specifically includes: Calculate the remaining horizontal distance and depth error When the remaining horizontal distance and depth error When all values ​​are greater than the set threshold, calculate the common speed scaling factor. ; Based on the aforementioned common speed ratio factor Correcting the desired longitudinal velocity and desired vertical velocity Preliminary correction values ​​were obtained. and ; Calculate heading error And based on the heading error Calculate the speed scaling factor Using speed scaling factor right A second correction is performed to obtain the final desired longitudinal velocity. .

[0014] Preferably, the speed scaling factor as follows: ; ; in, For heading error, This is the heading error threshold.

[0015] Preferably, the generalized force is calculated using a PID controller. During the process, anti-saturation limiting is applied to the integral term: ; in, Indicates the first Within the first control cycle, the first The integral term for each speed channel, Indicates the first Within the first control cycle, the first Speed ​​error of each speed channel Indicates the sampling period of the controller. This indicates the maximum allowable amplitude of the integral term. This indicates the minimum allowable amplitude of the integral term.

[0016] Preferably, the objective function for constructing thrust optimization allocation is as follows: ; in, For the generalized force tracking error weights, This represents the mapping matrix from thruster thrust to generalized force. This refers to the actual thrust output of the four thrusters of the underwater vehicle. Indicates the thrust of the left horizontal thruster. Indicates the thrust of the right horizontal thruster. Indicates the thrust of the forward vertical thruster. Indicates the thrust of the rear vertical thruster. Represents the expected generalized force vector. For thrust smoothing weights, This represents the thrust vector of the thruster in the previous control cycle.

[0017] Preferably, the solution yields the actual thrust output of each thruster. Solving using projection gradient iteration: ; in, Indicates the first The thrust vector of the thruster at the next iteration This represents the gradient descent step size. Describe the objective function exist relative thrust vector The gradient.

[0018] As a preferred option, it also includes: Real-time calculation of the current velocity amplitude of the underwater vehicle and three-dimensional distance ; When satisfied and and maintain continuously At the beginning of each control cycle, it is determined that the underwater vehicle has reached the target point and is stationary. At this time, the thrust output of each thruster is set to zero. in, To determine whether an underwater vehicle has reached its target point, the distance between the underwater vehicle and the target point is... To determine whether an underwater vehicle has reached its target point, the speed of the underwater vehicle... The number of consecutive control cycles required to determine stability.

[0019] Preferably, the determination of the current control stage Specifically: .

[0020] As a preferred option, the target heading angle is also included. Adaptive determination: When the current control phase For the long-distance approach phase Or mid-range adjustment phase When the target heading angle is calculated, it is calculated according to the following formula: ; ; ; in, , These represent the positions of the target point of the underactuated underwater vehicle in the north and east directions, respectively. , These are the current north and east positions of the underwater vehicle, respectively. When the current control phase To approach the target stage At that time, the target heading angle is equal to the current actual heading angle of the underwater vehicle: .

[0021] Compared with the prior art, the advantages and beneficial effects of the present invention are as follows: This invention provides a precise control method for underactuated underwater vehicles based on model prediction. By introducing and discretizing the ocean current velocity term into the nonlinear model predictive control, the prediction model can compensate for the position drift caused by the ocean current in advance. This solves the problem that existing model predictive control methods do not fully consider the deviation between the predicted trajectory and the actual motion caused by ocean current disturbances. As a result, it improves the control accuracy and motion stability of underwater vehicles approaching the target point in a current environment.

[0022] This invention divides the control process into three stages based on the three-dimensional distance between the vehicle and the target point: long-distance approach, mid-distance adjustment, and approaching the target. It adaptively adjusts the state error weight matrix and control input weight matrix according to the current stage, and dynamically scales the speed constraint boundary. This solves the problem in the prior art where the control weight and speed constraint use fixed parameters and are difficult to adapt to the needs of different control stages. It enables the vehicle to quickly approach the target in the long-distance stage, and automatically reduces the speed and increases the control weight to suppress overshoot and oscillation in the target approach stage, thus achieving a balance between high efficiency and steady-state accuracy in the control process.

[0023] This invention solves the problems of inconsistent response speeds in the horizontal and depth directions and easy horizontal detours when the heading is not aligned, by calculating the common speed scaling factor of the remaining horizontal distance and depth error and coordinating the expected speeds in the longitudinal and vertical directions. At the same time, it introduces a speed scaling factor based on the heading error. This invention enables the north, east and depth directions to reach the target point in a coordinated rhythm and improves the motion synchronization under multi-degree-of-freedom coupling.

[0024] This invention constructs an optimization objective function in the thrust distribution stage that includes generalized force tracking error and thrust change smoothing term, and strictly constrains the upper and lower limits of thrust and the thrust change rate of the thruster. It uses the projection gradient iteration method to solve the problem, which solves the defects of existing fixed ratio or empirical thrust distribution methods that are difficult to take into account the physical constraints of the thruster and are prone to sudden output changes. It reduces the fluctuation amplitude of the thrust curve, reduces thruster wear, and improves the reliability and engineering feasibility of the system in long-term operation.

[0025] This invention employs a phased switching strategy for calculating the target heading angle. During the approach phase, the target heading is locked to the current actual heading, avoiding sudden turns caused by a jump in the calculated target heading angle due to extremely small horizontal distance. This further improves the attitude stability and dwell stability of the vehicle during the terminal control phase.

[0026] Additional aspects and advantages of the invention will be set forth in part in the description which follows, and in part will be obvious from the description, or may be learned by practice of the invention. Attached Figure Description

[0027] The above and / or additional aspects and advantages of the present invention will become apparent and readily understood from the description of the embodiments taken in conjunction with the following drawings, in which: Figure 1 This is a flowchart illustrating the precise control method for underactuated underwater vehicles based on model prediction, according to an embodiment of the present invention. Figure 2 This is a schematic diagram of the three-dimensional motion trajectory of the underwater vehicle according to an embodiment of the present invention; Figure 3 This is a diagram showing the position response curves of the underwater vehicle in the north, east, and depth directions according to an embodiment of the present invention. Figure 4 This is a graph showing the attitude and velocity response of an underwater vehicle according to an embodiment of the present invention. Figure 5 The following are thrust output curves of four thrusters according to an embodiment of the present invention; Figure 6 This is a schematic diagram of the three-dimensional trajectory and the horizontal plane trajectory in an embodiment of the present invention; Figure 7 This is a schematic diagram of the maximum thrust change rate in a single cycle according to an embodiment of the present invention; Figure 8 This is a comparison chart of key indicators in embodiments of the present invention; Figure 9 This is a diagram showing the thrust output of the thruster in an embodiment of the present invention. Figure 10 This is a position response diagram in the north, east, and depth directions according to an embodiment of the present invention; Figure 11 This is a comparison diagram of distance arrival conditions and speed arrival conditions in an embodiment of the present invention; Figure 12 This is a comparison chart of three-dimensional target distance errors in an embodiment of the present invention. Detailed Implementation

[0028] Embodiments of the present invention are described in detail below, examples of which are illustrated in the accompanying drawings, wherein the same or similar reference numerals denote the same or similar elements or elements having the same or similar functions throughout. The embodiments described below with reference to the accompanying drawings are exemplary and intended to explain the present invention, and should not be construed as limiting the present invention.

[0029] like Figure 1 As shown in the figure, an embodiment of the present invention provides a precise control method for an underactuated underwater vehicle based on model prediction, specifically a current-compensated adaptive phased NMPC control and constrained thrust optimization allocation method for underwater vehicles. This method takes the underwater vehicle's current attitude, velocity, target state, and estimated current values ​​as inputs. It determines the current control stage through a phased control strategy, introduces a current compensation term into the nonlinear model predictive controller, and adaptively adjusts the state weights, control weights, and velocity constraints according to different control stages to solve for the desired velocity. Subsequently, a velocity coordination method is used to improve the inconsistency between horizontal position and depth direction responses. Then, a PID controller calculates the generalized force, and finally, a constrained thrust optimization allocator is used to obtain the actual thrust output of each thruster.

[0030] The method includes the following steps: S1: Obtain the current pose of the underactuated underwater vehicle. Current speed Target state and ocean current velocity .

[0031] Specifically, for the current pose The position and attitude of an underwater vehicle are defined as follows: ; in, This indicates the northward position of the underactuated underwater vehicle in the northeast coordinate system. This indicates the eastward position of the underactuated underwater vehicle in the northeast coordinate system. This indicates the depth of the underactuated underwater vehicle, with the downward direction being positive. Indicates the roll angle of an underactuated underwater vehicle. Indicates the pitch angle of an underactuated underwater vehicle. This indicates the heading angle of an underactuated underwater vehicle.

[0032] The current velocity vector of the aircraft is: ; in, The longitudinal velocity of the underactuated underwater vehicle. The vertical velocity of the underactuated underwater vehicle. The pitch rate of the underactuated underwater vehicle. The yaw rate is the angular velocity of the underactuated underwater vehicle.

[0033] Specifically, the target state Defined as: ; in, This represents the target state vector of an underactuated underwater vehicle. , , These represent the positions of the target point of the underactuated underwater vehicle in the north, east, and depth directions, respectively, with the depth direction being positive downwards; , , These represent the target roll angle, target pitch angle, and target heading angle of the underactuated underwater vehicle, respectively.

[0034] Let the ocean current speed be: ; in, This represents the velocity component of the ocean current in the northward direction. This represents the velocity component of the ocean current in the eastward direction. This represents the velocity component of the ocean current in the depth direction.

[0035] S2: Based on the current pose With the target state Calculate 3D distance And based on a preset long-distance threshold and close to the target threshold Determine the current control phase The control phase Including the long-distance approach phase Mid-range adjustment phase and approaching the target stage .

[0036] Furthermore, the error between the current position and the target point is: ; ; ; in, , , These represent the positions of the target point of the underactuated underwater vehicle in the north, east, and depth directions, respectively. , , These represent the current northward, eastward, and depth positions of the underwater vehicle.

[0037] Horizontal distance and three-dimensional distance They are respectively: ; ; set up For the long-distance stage threshold, To approach the target stage threshold, The control phase represents the three-dimensional distance between the current location of the underactuated underwater vehicle and the target point. Defined as: ; in, This indicates the long-range approach phase, the main objective of which is to enable the underactuated underwater vehicle to quickly approach the target area. This indicates the mid-range adjustment phase, where the main objective is to strike a balance between approach speed and motion stability. This indicates that the target phase is approaching, and the main objectives are to reduce speed, decrease overshoot, and achieve stable dwell.

[0038] With this phased strategy, the controller no longer uses a fixed set of parameters to complete the entire control process, but automatically switches the control focus based on the distance between the vehicle and the target point.

[0039] Furthermore, before the vehicle enters the target approach phase, its target heading angle is: ; When the vehicle approaches the target or the horizontal distance is small, to avoid a sudden turn due to a rapid change in the target's course, the following should be made: .

[0040] S3: Based on the aforementioned ocean current velocity A nonlinear model predictive control (NMPC) prediction model with ocean current compensation is constructed. The model is discretized, and based on the current control stage... Adaptive adjustment of the state weight matrix in the NMPC optimization problem and control input weight matrix and the lower limit of the control input. and upper limit .

[0041] Specifically, the ocean current speed is: ; in, This represents the velocity component of the ocean current in the northward direction. This represents the velocity component of the ocean current in the eastward direction. This represents the velocity component of the ocean current in the depth direction.

[0042] The nonlinear kinematic prediction model used in this invention is as follows: ; ; ; ; ; ; The standardized form is as follows: ; To improve prediction accuracy, this invention employs a fourth-order... The method discretizes the prediction model. Let... Let be the prediction step size for NMPC, then: ; ; ; ; ; By adding ocean current terms to the prediction model The controller can compensate for position drift caused by ocean currents in advance when predicting subsequent position changes of the underactuated underwater vehicle, thereby improving the control accuracy in turbulent environments.

[0043] Specifically, the optimization objective function for NMPC is: ; in, The prediction time domain length for NMPC, This is the state weight matrix under the current control phase. This is the control input weight matrix for the current control phase. State weights are used to adjust the importance of position and attitude errors in the objective function, while control input weights are used to constrain the magnitude of the desired velocity and angular velocity commands. Indicates the first The predicted state of the underactuated underwater vehicle at the predicted moment. Indicates the target state. Indicates the first The control input at the predicted time.

[0044] The optimization problem is represented as: ; The constraints are: ; ; ; in This represents the initial state at the 0th prediction time in the NMPC prediction time domain. This indicates the actual state of the underactuated underwater vehicle received by the controller in the current control cycle, therefore... Take as This serves as the starting point for subsequent state predictions. and These represent the lower and upper limits of the control input in the current control phase, respectively.

[0045] Define the basic state error weight matrix: ,in, These are the base values ​​for northward position, eastward position, and depth error weights, respectively. These are the base values ​​for the error weights of roll angle, pitch angle, and yaw angle, respectively. Define the basic control input weight matrix: ,in, These are the base values ​​for the control input weights of the desired longitudinal velocity and the desired vertical velocity, respectively. These are the base values ​​for the control input weights of the desired pitch rate and desired yaw rate, respectively. In the current control phase Below, the state weight matrix ,in These are the northward position error weights, the eastward position error weights, and the depth error weights, respectively. These are the roll angle error weights, pitch angle error weights, and yaw angle error weights, respectively. Control input weight matrix ,in, These are the weights for the expected longitudinal velocity and the expected vertical velocity, respectively. These are the weights for the expected pitch rate and the expected yaw rate, respectively.

[0046] Based on the current control stage Adaptive adjustment of the state weight matrix in the NMPC optimization problem and control input weight matrix and the lower limit of the control input. and upper limit include: During the long-distance approach phase : ; The scaling factor in the formula is used for weight scaling in the long-range phase, enabling the underactuated underwater vehicle to approach the target area at a faster speed.

[0047] During the mid-range adjustment phase Increase the weight of location tracking: ; In the mid-range phase, the weight parameters are still obtained by scaling the base weights proportionally; the position-related weights are increased to improve the position tracking capability during the target point approach process, while a certain amount of control input weights are retained to suppress sudden changes in velocity commands.

[0048] Approaching the goal To improve position and attitude accuracy and increase control input weights, a distance factor is defined: ; The distance factor describes the degree to which the underactuated underwater vehicle approaches the target point; the closer the vehicle is to the target point, the more significant the weighting effect of the distance factor becomes, thus improving the position and attitude control accuracy during the close-range phase. Therefore: ; ; ; ; ; During the target approach phase, by increasing the weight of the state error and increasing the penalty for the control input, the vehicle's motion becomes smoother as it approaches the target.

[0049] At the same time, speed constraints (lower limits of control input) and upper limit Scaling based on stage: ; ; ; Through the aforementioned adaptive weight and speed constraint adjustments, the vehicle can rapidly approach the target at long distances and automatically reduce its speed and enhance stability during the approach phase.

[0050] S4: Solve the NMPC optimization problem to obtain the desired velocity vector. ,in For the desired longitudinal velocity, For the desired vertical velocity, For the desired pitch angular velocity, The desired yaw rate.

[0051] The desired velocity for controller planning is defined as: ; in, This indicates the desired longitudinal velocity of the underactuated underwater vehicle obtained from the controller planning. This represents the desired vertical velocity obtained from controller planning. This represents the desired pitch rate obtained from controller planning. This represents the desired yaw rate obtained from controller planning.

[0052] S5: Perform three-axis coordination correction on the desired velocity vector to obtain the coordinated desired velocity vector. .

[0053] NMPC calculates the desired speed. Subsequently, to address the inconsistency in response speed between the horizontal and depth directions, this solution adjusts the desired longitudinal speed. and desired vertical velocity Coordinate and correct accordingly.

[0054] If the remaining horizontal distance and depth error If all values ​​are greater than the set threshold, then the common speed scaling factor is calculated. : ; horizontal distance This represents the distance between the current horizontal position of the underactuated underwater vehicle and the horizontal position of the target point. This indicates the error between the current depth and the target depth.

[0055] The horizontal velocity boundary is: ; The vertical velocity boundary is: ; The corrected longitudinal expected velocity is: ; The corrected vertical desired velocity is: ; Meanwhile, to reduce horizontal deviations due to course misalignment, the following course error is defined: ; when Greater than the preset heading error threshold When calculating the speed scaling factor In this embodiment, A value of 15° can be used, but it can also be adjusted according to the actual turning capability and control precision requirements of the aircraft. It is the minimum scaling factor that allows the longitudinal desired speed to be retained when the heading error is at its maximum (i.e., when the underwater vehicle's heading is completely opposite to the target point), i.e., the speed scaling factor. The lower limit value.

[0056] ; ; The final expected longitudinal velocity is: ; The desired speed is ultimately passed to the lower-level controller for coordination. for: .

[0057] S6: Based on the coordinated desired velocity vector With current speed The error is calculated by the PID controller to determine the generalized force. .

[0058] The current velocity vector of the aircraft is: ; The speed error is: ; For the One channel: ; in Indicates the first Within the first control cycle, the first Speed ​​error of each speed channel Indicates the first Within the first control cycle, the first The expected speed of each speed channel Indicates the first Within the first control cycle, the first The actual speed of each speed channel; ; in Indicates the first Within the first control cycle, the first The integral term for each speed channel, Indicates the sampling period of the controller. This indicates the maximum allowable amplitude of the integral term. Indicates the minimum allowable amplitude of the integral term; The differential term is: ; The PID output is: ; in Indicates the first Within the first control cycle, the first The speed channel is calculated by the generalized force output of the PID controller. Indicates the first The proportional gain of each speed channel Indicates the first Integral gain of each speed channel, Indicates the first Differential gain of each speed channel.

[0059] The combination of four channels yields a generalized force. : ; in This represents the desired thrust along the longitudinal direction of the aircraft, used to control longitudinal velocity. , This represents the desired thrust along the longitudinal direction of the aircraft, used to control vertical velocity. Or depth change, This represents the desired pitch moment about the lateral axis of the aircraft, used to control the pitch rate. , This represents the desired yaw moment about the vertical axis of the aircraft, used to control the yaw rate. .

[0060] S7: Construct the objective function for thrust optimization allocation, and combine it with the upper and lower limits of thrust and the thrust change rate constraint to obtain the actual thrust output of each thruster. It is used to drive the movement of underwater vehicles.

[0061] The outputs of the four thrusters of the underwater vehicle are: in, Indicates the thrust of the left horizontal thruster. Indicates the thrust of the right horizontal thruster. Indicates the thrust of the forward vertical thruster. This represents the thrust of the rear vertical thruster. The relationship between thruster output and generalized force is as follows: ; ; ; ; The matrix form is: ; in: ; In the formula, This represents the mapping matrix from thruster thrust to generalized force. Represents the expected generalized force vector. and These represent the equivalent arms of the vertical and horizontal thrusters relative to the vehicle's center of mass, respectively.

[0062] The thrust optimization objective function is: ; In the formula, For the generalized force tracking error weights, For thrust smoothing weights, This represents the thrust vector of the thruster in the previous control cycle; The constraints are: ; ; In the formula, and These represent the minimum and maximum thrust that a single thruster is allowed to output, respectively. Indicates the maximum allowable rate of change of thrust of the thruster. Indicates the first Within the first control cycle, the first The thrust of each thrust channel.

[0063] Solving using projection gradient iteration: ; In the formula Indicates the first The thrust vector of the thruster at the next iteration This represents the gradient descent step size, used to control the magnitude of the update in each iteration. Describe the objective function exist relative thrust vector The gradient indicates the direction in which the current thrust should be adjusted to reduce the objective function.

[0064] The current speed is: ; When the following conditions are met: ; and: ; and maintain continuously After one control cycle, it is considered that the underactuated underwater vehicle has reached the target and is stationary stably; among which To determine whether an underwater vehicle has reached its target point, the distance between the underwater vehicle and the target point is... To determine whether an underwater vehicle has reached its target point, the speed of the underwater vehicle... The number of consecutive control cycles required to determine stability.

[0065] At this point, the controller outputs zero thrust: ; The controller has stopped operating.

[0066] This invention incorporates ocean current velocity into the model predictive control process, allowing the prediction of the vehicle's subsequent motion to simultaneously consider positional shifts caused by ocean currents. Therefore, it can approach the target point more stably in turbulent environments. During control, the vehicle maintains a faster approach speed when far from the target, and automatically reduces velocity constraints and increases position error weights upon entering the target's vicinity, resulting in smoother motion near the target point and reduced overshoot and oscillations. Simultaneously, the controller coordinates the desired velocity based on the remaining horizontal distance and remaining depth error, ensuring that the north, east, and depth directions reach the target point at a more consistent pace, mitigating situations where the horizontal position is close to the target but the depth is still significantly lagging. Regarding thruster output, this scheme considers upper and lower limits of thrust and thrust rate of change during thrust distribution, reducing thrust abrupt changes and making the thrust output more consistent with the actual operating requirements of underwater thrusters.

[0067] To verify the control effect of the method of the present invention, simulation experiments and comparative experiments were conducted on the approach process of an underactuated underwater vehicle to a three-dimensional target point. The experimental results are as follows: Figure 2 As shown.

[0068] In one embodiment, the simulation experiment results are as follows: Figures 2-5 As shown.

[0069] Depend on Figure 2The three-dimensional and horizontal trajectories of the underwater vehicle shown demonstrate that after starting from its initial position, the vehicle moves along a continuous trajectory towards the target point and eventually approaches the target area. In the three-dimensional trajectory, the vehicle simultaneously completes northward and eastward position adjustments and depth descent, indicating that this scheme can uniformly control the three-dimensional attitude approach process of the underwater vehicle. In the horizontal trajectory, the overall movement path is relatively continuous, without obvious back-and-forth or divergent phenomena. As it approaches the target point, the trajectory changes tend to converge, indicating that the phased control strategy and near-target velocity constraints can reduce the intensity of movement near the target, which is beneficial to improving the underwater vehicle's stable approach capability near the target area.

[0070] Depend on Figure 3 The position response curves show that the vehicle's northward, eastward, and depth positions gradually approach the set target values ​​over time. The northward and eastward positions initially rise continuously, then the rate of change gradually decreases after approaching the target value; the depth direction gradually descends from the initial depth to near the target depth without significant reverse divergence. The three-dimensional distance error curves further show that the distance between the vehicle and the target point generally decreases, entering a smaller error range after approximately 40 seconds. These results demonstrate that this scheme can simultaneously coordinate horizontal and depth movements in three-dimensional space, enabling the underactuated underwater vehicle to possess a relatively stable target approach capability.

[0071] Depend on Figure 4 The attitude and velocity response curves shown demonstrate that the vehicle's position, attitude, and velocity all change accordingly during target approach. The roll angle and roll rate exhibit some oscillations in the mid-to-late stages, but the overall amplitude remains relatively small. The pitch angle changes with depth adjustment to facilitate the vehicle's descent. The heading angle is gradually adjusted in the early stages to enable the vehicle to move towards the target area. Regarding velocity response, the longitudinal velocity is higher at longer distances to improve approach efficiency and gradually decreases upon entering the target vicinity. The vertical velocity initially increases and then decreases during depth approach, indicating that the controller can adjust the vertical motion intensity based on the remaining depth error. Overall, this invention's solution can constrain and adjust the desired velocity based on the target distance and motion state, allowing the vehicle to maintain its target approach capability while gradually reducing its speed near the target, thus providing conditions for a stable arrival at the target area.

[0072] Depend on Figure 5The thrust output curves of the four thrusters shown demonstrate that, during target approach, each thruster can allocate thrust according to the generalized force requirement given by the upper-level controller. The horizontal thrusters primarily handle horizontal position adjustment and heading control, thus exhibiting some alternating amplitude variations in the mid-course phase. The vertical thrusters are mainly used for depth adjustment, maintaining a large positive thrust output during descent and gradually adjusting with changes in depth error. Overall, the thrust of each thruster remains within its set operating range, and no continuously unachievable thrust commands are observed. Upon approaching the target area, the overall thrust amplitude decreases, indicating that the constrained thrust distributor can limit the thruster output while meeting control requirements, allowing the generalized control force to be converted into a more reasonable actual thruster thrust, thereby improving the engineering feasibility of the control method.

[0073] In one embodiment, the comparative experimental results are as follows: Figures 6-12 As shown.

[0074] To further illustrate the control effect of this method, a traditional fixed-parameter NMPC method was set up as a comparative experiment. The comparative method did not employ current compensation, adaptive weight adjustment, staged control strategies, three-axis velocity coordination, or constrained thrust optimization allocation; it only used fixed predictive model parameters and fixed control weights for target point approach control. Both sets of experiments used the same underwater vehicle model, initial position, target point, current conditions, and thruster parameters, and recorded the vehicle's position, velocity, attitude, three-dimensional distance error, and thruster output. In the experiment, a three-dimensional distance between the vehicle and the target point of less than 0.8 m and a resultant velocity of less than 0.13 m / s, maintained continuously for 50 control cycles, were used as criteria for stable target point arrival. By comparing the stable arrival time, velocity at minimum distance, maximum deviation distance after entering the target neighborhood, and thruster output changes of the two methods, the improvement effect of this method in terms of stable approach to the target point and engineering feasibility was verified.

[0075] Depend on Figure 6The comparison between the 3D trajectory and the horizontal trajectory shows that both methods can drive the underwater vehicle from its initial position towards the target area, but their performance differs significantly upon approaching the target. The traditional method (fixed-parameter NMPC) exhibits noticeable back-and-forth oscillations and localized detours near the target, indicating that it maintains relatively fixed control weights and velocity constraints during the approach phase, making it prone to deviating again after high-speed passage through the target area. In contrast, the trajectory of this method (model predictive precise control) changes more smoothly after approaching the target area, without significant large-scale detours, and the overall 3D trajectory more closely resembles a continuous convergence process. These results demonstrate that this method, by introducing staged control, adaptive weight adjustment, and near-target velocity constraints, can maintain target approach capability at long distances and reduce motion amplitude at close range, thereby improving the stable approach performance of underactuated underwater vehicles near the target point.

[0076] Depend on Figure 7 The comparison of the single-cycle maximum thrust change rate shows that the traditional method (fixed-parameter NMPC) exhibits multiple sharp peaks in thrust change rate during the control process, with relatively large peak values, indicating a significant abrupt change in its thruster output. Particularly near the target region and during subsequent adjustments, the traditional method frequently generates large thrust changes, reflecting that fixed-parameter control is prone to control input jitter due to rapid changes in error direction near the target. In contrast, the thrust change rate of this method (model predictive precise control) is generally lower, showing only small changes near the initial adjustment and stage switching, with subsequent thrust changes being more gradual. These results demonstrate that the staged velocity constraints and constrained thrust allocation in this method can effectively reduce abrupt changes in thruster output and improve the smoothness of the thruster's operation. Depend on Figure 8The comparison of key indicators shows that our method (model predictive precise control) outperforms the traditional method (fixed parameter NMPC) in both stable arrival capability and control smoothness. Our method satisfies the stable arrival criterion at approximately 41.60 s, while the traditional method fails to meet this criterion by the end of the experiment. This indicates that while the traditional method can approach the target area briefly, it struggles to maintain low-speed stability near the target. Regarding the closest point velocity, our method achieves 0.123 m / s, significantly lower than the traditional method's 0.342 m / s, indicating that our method exhibits a gentler motion when approaching the target point and does not involve high-speed crossing. After entering the 0.8 m neighborhood, our method's maximum error is approximately 0.767 m, while the traditional method's error expands to approximately 9.93 m, indicating a significant deviation after crossing the target area. In terms of thruster output, our method shows a lower average thrust change rate than the traditional method, demonstrating that constrained thrust allocation and near-target velocity constraints reduce thrust abrupt changes and improve the smoothness of the control process. Overall, this method can stably approach the target area in a short time and reduce deviation and thrust jitter near the target.

[0077] Depend on Figure 9 The comparison of the thrust output of the four thrusters shows that the traditional method (fixed parameter NMPC) exhibits significant thrust oscillations in both the horizontal and vertical thrusters, especially as the vehicle approaches the target area, where the thrust continues to switch dramatically between positive and negative directions. This indicates that the control input is not stable enough near the target, easily leading to frequent thruster adjustments and energy waste. In contrast, the thruster output of the proposed method (model predictive precise control) changes more orderly overall. The thrust amplitude of the horizontal thruster gradually decreases, and the vertical thruster also significantly reduces its output intensity after depth adjustment, indicating that the controller can gradually reduce the control action according to the target distance and motion phase. This result demonstrates that the proposed method, by constraining thrust distribution and limiting speed near the target, can reduce large fluctuations in thruster output, allowing the generalized control force to be distributed more smoothly across the thrusters, thereby improving the engineering feasibility and actuator stability of the underactuated underwater vehicle control process.

[0078] Depend on Figure 10The comparison of position responses in the north, east, and depth directions shows that both methods can drive the underwater vehicle towards the target position in the early stages, but there are significant differences in the stability of the response near the target. The traditional method (fixed-parameter NMPC) approaches the target value earlier in the north and east directions, but then continues to fluctuate, especially deviating from the target value in the later stages, indicating a lack of effective deceleration and stability maintenance mechanisms near the target. In contrast, the proposed method (model predictive precise control) has a smoother position response in the north and east directions, without significant large oscillations after approaching the target. In the depth direction, the proposed method can dive to the vicinity of the target depth more continuously, while the traditional method, although also approaching the target depth, is still accompanied by some disturbances. Overall, the proposed method, through staged control and three-axis velocity coordination, makes the approach process in the horizontal and depth directions more coordinated, improving the problems of overshoot, fluctuation, and terminal deviation that are prone to occur near the target in the fixed-parameter NMPC.

[0079] Depend on Figure 11 The comparison of distance and velocity arrival conditions shows that both methods can gradually reduce the three-dimensional distance between the underwater vehicle and the target point in the early stages, but the stability near the target differs significantly. Our method (model-predictive precise control) gradually reduces the distance error to within the 0.8m threshold after approaching the target area, while the velocity also decreases to below the 0.13m / s threshold, indicating that the vehicle can not only approach the target point but also enter the target neighborhood at a lower speed. The traditional method (fixed-parameter NMPC), although it can also bring the distance error close to the target area in a short time, still exhibits significant fluctuations in its velocity curve near the target, repeatedly exceeding the velocity threshold, indicating that the vehicle is more likely to traverse the target area at high speed rather than arrive stably. These results demonstrate that our method, through velocity constraints and adaptive control weight adjustment during the approach phase, can simultaneously satisfy both distance and velocity arrival conditions, improving the stable approach capability of the underactuated underwater vehicle near the target point.

[0080] Depend on Figure 12The comparison of three-dimensional target distance errors shows that both methods can gradually reduce the distance error at long distances, indicating that both possess basic target approach capabilities. However, after entering the target's neighborhood, the performance of the two methods differs significantly. The distance error of this method (model prediction precise control) decreases to within the 0.8m threshold in about 40 seconds and maintains a small error range in the near-target phase, indicating that the vehicle can enter the target area relatively stably. Although the traditional method (fixed parameter NMPC) can also reduce the distance error in the short term, the error curve subsequently shows significant and repeated fluctuations, repeatedly increasing back above 1m, indicating that the vehicle experiences crossing, swaying, and deviation near the target, failing to achieve stable convergence. This result shows that the phased control and near-target velocity constraint of this method can improve the problem of oscillation in fixed parameter NMPC near the target, enabling the underwater vehicle to have better error maintenance capabilities after approaching the target.

[0081] In summary, the simulation results verify the effectiveness of this scheme in four aspects: three-dimensional trajectory convergence, position response coordination, attitude and velocity stability, and limited smoothing of thruster output. The method of this invention not only enables underactuated underwater vehicles to approach target points in three-dimensional space, but also improves the stability, coordination, and feasibility of the target approach process under the combined conditions of ocean current disturbances, velocity constraints, and thruster constraints.

[0082] Compared with the prior art, the present invention has the following advantages: In the nonlinear model predictive control of an underactuated underwater vehicle (UUV), ocean current compensation and a phased adaptive adjustment mechanism are introduced. By treating ocean current velocity as an external disturbance term in the prediction model, the controller can consider the impact of ocean currents on position changes when predicting the future motion of the UUV. Simultaneously, based on the three-dimensional distance between the UUV and the target point, the control weights and velocity constraints are adjusted in stages, allowing the controller to have different control emphases during long-distance approach and short-distance convergence, thereby improving the target point approach accuracy and stability of the UUV in turbulent environments.

[0083] A hierarchical control and constrained thrust allocation method for underactuated underwater vehicles is constructed. The upper-level controller calculates the desired velocity using current-compensated NMPC and coordinates the desired velocity based on the remaining horizontal distance and depth error to improve the asynchronous response between horizontal position and depth during three-dimensional motion. The lower-level controller converts the desired velocity into a generalized force and performs optimized thrust allocation under the constraints of upper and lower limits of thrust and thrust change rate, thereby reducing abrupt changes in thruster output and improving the smoothness and engineering feasibility of the vehicle's motion.

[0084] In the description of this specification, references to terms such as "one embodiment," "some embodiments," "example," "specific example," or "some examples," etc., indicate that a specific feature, structure, material, or characteristic described in connection with that embodiment or example is included in at least one embodiment or example of the invention. In this specification, the illustrative expressions of the above terms do not necessarily refer to the same embodiment or example. Furthermore, the specific features, structures, materials, or characteristics described may be combined in any suitable manner in one or more embodiments or examples.

[0085] It will be readily understood by those skilled in the art that this invention includes any combination of the inventive description and specific embodiments outlined in the foregoing specification, as well as the various parts shown in the accompanying drawings. Due to space limitations and for the sake of brevity, not all of these combinations have been described in detail. Any modifications, equivalent substitutions, or improvements made within the spirit and principles of this invention should be included within the scope of protection of this invention.

[0086] Although embodiments of the present invention have been shown and described above, it is to be understood that the above embodiments are exemplary and should not be construed as limiting the present invention. Those skilled in the art can make changes, modifications, substitutions, and variations to the above embodiments within the scope of the present invention without departing from the principles and spirit of the invention. The scope of the present invention is defined by the appended claims and their equivalents.

Claims

1. A precise control method for an underactuated underwater vehicle based on model prediction, characterized in that, include: Obtain the current pose of the underactuated underwater vehicle Current speed Target state and ocean current velocity ; According to the current pose With the target state Calculate 3D distance And based on a preset long-distance threshold and close to the target threshold Determine the current control phase The control phase Including the long-distance approach phase Mid-range adjustment phase and approaching the target stage ; Based on the ocean current velocity A nonlinear model predictive control (NMPC) prediction model with ocean current compensation is constructed. The model is discretized, and based on the current control stage... Adaptive adjustment of the state weight matrix in the NMPC optimization problem and control input weight matrix and the lower limit of the control input. and upper limit ; Solve the NMPC optimization problem to obtain the desired velocity vector. ,in For the desired longitudinal velocity, For the desired vertical velocity, For the desired pitch angular velocity, The desired yaw rate; The desired velocity vector is then subjected to triaxial coordination correction to obtain the coordinated desired velocity vector. ; Based on the coordinated desired velocity vector With current speed The error is calculated by the PID controller to determine the generalized force. ; A thrust optimization allocation objective function is constructed, and the actual thrust output of each thruster is obtained by combining the upper and lower limits of thrust and the thrust rate of change constraints. It is used to drive the movement of underwater vehicles.

2. The precise control method for underactuated underwater vehicles based on model prediction as described in claim 1, characterized in that, Define the basic state error weight matrix: ,in, These are the base values ​​for northward position, eastward position, and depth error weights, respectively. These are the base values ​​for the error weights of roll angle, pitch angle, and yaw angle, respectively. Define the basic control input weight matrix: ,in, These are the base values ​​for the control input weights of the desired longitudinal velocity and the desired vertical velocity, respectively. These are the base values ​​for the control input weights of the desired pitch rate and desired yaw rate, respectively. In the current control phase Below, the state weight matrix ,in These are the northward position error weights, the eastward position error weights, and the depth error weights, respectively. These are the roll angle error weights, pitch angle error weights, and yaw angle error weights, respectively. Control input weight matrix ,in, These are the weights for the expected longitudinal velocity and the expected vertical velocity, respectively. These are the weights for the expected pitch rate and the expected yaw rate, respectively. Based on the current control stage Adaptive adjustment of the state weight matrix in the NMPC optimization problem and control input weight matrix and the lower limit of the control input. and upper limit include: During the long-distance approach phase : ； During the mid-range adjustment phase : ； Approaching the goal According to the distance factor Dynamically increase the weights for northward position error, eastward position error, and depth error: ； ； ； and: ； ； The lower limit of the control input and upper limit Based on the stage scaling factor Scaling is performed.

3. The precise control method for underactuated underwater vehicles based on model prediction as described in claim 1, characterized in that, The kinematic equations of the nonlinear model predictive control (NMPC) prediction model with ocean current compensation are expressed as follows: ； ； ； ； ； ； in, This represents the velocity component of the ocean current in the northward direction. This represents the velocity component of the ocean current in the eastward direction. This represents the velocity component of the ocean current in the depth direction.

4. The precise control method for underactuated underwater vehicles based on model prediction as described in claim 1, characterized in that, The three-axis coordinated correction of the desired velocity vector specifically includes: Calculate the remaining horizontal distance and depth error When the remaining horizontal distance and depth error When all values ​​are greater than the set threshold, calculate the common speed scaling factor. ; Based on the aforementioned common speed ratio factor Correcting the desired longitudinal velocity and desired vertical velocity Preliminary correction values ​​were obtained. and ; Calculate heading error And based on the heading error Calculate the speed scaling factor Using speed scaling factor right A second correction is performed to obtain the final desired longitudinal velocity. .

5. The precise control method for underactuated underwater vehicles based on model prediction as described in claim 4, characterized in that, The speed scaling factor as follows: ； ； in, For heading error, The heading error threshold, This is the lower limit of the speed scaling factor.

6. The precise control method for underactuated underwater vehicles based on model prediction as described in claim 1, characterized in that, The generalized force is calculated using a PID controller. During the process, anti-saturation limiting is applied to the integral term: ； in, Indicates the first Within the first control cycle, the first The integral term for each speed channel, Indicates the first Within the first control cycle, the first Speed ​​error of each speed channel Indicates the sampling period of the controller. This indicates the maximum allowable amplitude of the integral term. This indicates the minimum allowable amplitude of the integral term.

7. The precise control method for underactuated underwater vehicles based on model prediction as described in claim 1, characterized in that, The objective function for constructing thrust optimization allocation is specifically as follows: ； in, For the generalized force tracking error weights, This represents the mapping matrix from thruster thrust to generalized force. This refers to the actual thrust output of the four thrusters of the underwater vehicle. Indicates the thrust of the left horizontal thruster. Indicates the thrust of the right horizontal thruster. Indicates the thrust of the forward vertical thruster. Indicates the thrust of the rear vertical thruster. Represents the expected generalized force vector. For thrust smoothing weights, This represents the thrust vector of the thruster in the previous control cycle.

8. The precise control method for underactuated underwater vehicles based on model prediction as described in claim 1, characterized in that, The solution yields the actual thrust output of each thruster. Solving using projection gradient iteration: ； in, Indicates the first The thrust vector of the thruster at the next iteration This represents the gradient descent step size. Describe the objective function exist relative thrust vector The gradient.

9. The precise control method for underactuated underwater vehicles based on model prediction as described in claim 1, characterized in that, Also includes: Real-time calculation of the current velocity amplitude of the underwater vehicle and three-dimensional distance ; When satisfied and and maintain continuously At the beginning of each control cycle, it is determined that the underwater vehicle has reached the target point and is stationary. At this time, the thrust output of each thruster is set to zero. in, To determine whether an underwater vehicle has reached its target point, the distance between the underwater vehicle and the target point is... To determine whether an underwater vehicle has reached its target point, the speed of the underwater vehicle... The number of consecutive control cycles required to determine stability.

10. The precise control method for underactuated underwater vehicles based on model prediction as described in any one of claims 1-9, characterized in that, Also includes the target heading angle Adaptive determination: When the current control phase For the long-distance approach phase Or mid-range adjustment phase When the target heading angle is calculated, it is calculated according to the following formula: ； ； ； in, , These represent the positions of the target point of the underactuated underwater vehicle in the north and east directions, respectively. , These are the current north and east positions of the underwater vehicle, respectively. When the current control phase To approach the target stage At that time, the target heading angle is equal to the current actual heading angle of the underwater vehicle: 。

Images