AUV trajectory tracking method and system based on adaptive time-varying optimization control
By adopting adaptive time-varying optimization control method in AUV, configuring a time-varying linear prediction model and combining model prediction control and rolling optimization technology, the problem that traditional methods are difficult to cope with dynamic disturbances and uncertainties in complex underwater environments is solved, and high-precision trajectory tracking and task execution of AUV is achieved.
Patent Information
- Application Number
- CN202510275544.7
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2025-03-10
- Publication Date
- 2025-06-10
AI Technical Summary
Traditional AUV trajectory tracking control methods are difficult to effectively deal with dynamic disturbances and uncertainties in complex underwater environments, resulting in a contradiction between dynamic disturbance immunity and tracking accuracy.
Adaptive time-varying optimization control is adopted to configure the time-varying linear prediction model, define multi-constraint conditions and objective functions, and use model prediction control and rolling optimization techniques to optimize the control input of AUV to achieve high-precision trajectory tracking.
It realizes high-precision trajectory tracking and task execution of AUV in complex environments, improves the real-time, robustness and computing efficiency of the system, avoids control input mutations, and ensures system stability.
Smart Images

Figure CN120122446A_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the motion control of an autonomous underwater vehicle (AUV), and particularly to an AUV trajectory tracking method and system based on adaptive time-varying optimal control. Background Art
[0002] As a core device for performing tasks such as submarine cable laying, resource exploration, and deep-sea detection, the trajectory tracking accuracy of an AUV directly determines the reliability of task execution. However, the complex marine environment and the strong non-linear dynamics characteristics of the AUV itself lead to a significant contradiction between the dynamic disturbance rejection ability and the tracking accuracy of traditional control methods. Although model predictive control shows advantages in dynamic systems through rolling optimization and multi-constraint processing, its limitations in AUV applications are still prominent: on the one hand, non-linear model predictive control is difficult to meet the real-time requirement due to high computational complexity; on the other hand, model predictive control with a fixed linearization point causes the accumulation of tracking errors due to model mismatch, especially prone to control failure under strong non-linear working conditions.
[0003] Existing improvement schemes mostly focus on simplifying the prediction model or introducing a disturbance compensation mechanism. For example, using Taylor expansion for local linearization can reduce the computational burden, but the linearization error increases sharply as the state deviates from the equilibrium point; the state observer based on the extended Kalman filter can suppress some disturbances, but it is difficult to cope with actuator saturation and sudden ocean current disturbances. In addition, traditional model predictive control lacks flexibility in dynamic adjustment of constraint conditions and multi-objective collaborative optimization, resulting in sudden changes in control input when the AUV switches between trajectory tracking and obstacle avoidance tasks, affecting the system stability. Summary of the Invention
[0004] Object of the Invention: The first object of the present invention is to provide an AUV trajectory tracking method based on adaptive time-varying optimal control that takes into account real-time performance, robustness, and high precision to address the problem that traditional trajectory tracking control methods are difficult to effectively cope with dynamic disturbances and uncertainties in complex underwater environments, ensuring that the AUV can maintain high-precision trajectory tracking and task execution in complex environments; the second object of the present invention is to provide an AUV trajectory tracking system based on adaptive time-varying optimal control.
[0005] Technical Solution: An AUV trajectory tracking method based on adaptive time-varying optimal control of the present invention includes:
[0006] (1) Configure a time-varying linear prediction model;
[0007] Assume that the system state equation applies a constant control quantity and stabilizes at a certain equilibrium point, and then obtain a linearized time-varying system through the Taylor expansion method;
[0008] (2) Define multi-constraint conditions and an objective function;
[0009] (3) Optimize the control input of the AUV through the model predictive control method to minimize the path tracking error and reduce the variation of the control input; through rolling optimization, solve the optimal control input that satisfies multiple constraint conditions within each motion cycle, and these optimization results are used to guide the AUV to move along the reference trajectory;
[0010] (4) Apply the optimal control input to the motion model of the AUV and predict the trajectories at multiple future moments; obtain the actual trajectory of the AUV, compare it with the predicted trajectory, and calculate the error; according to the error, the feedback correction mechanism corrects the trajectory information;
[0011] (5) In each control cycle, return to step (3), recalculate the optimal control input using the feedback-corrected trajectory information, and continuously improve the control accuracy through rolling optimization until the tracking task is completed.
[0012] Furthermore, step (1) includes:
[0013] (1.1) Within the control time domain, assume that a constant control quantity is applied to the system state equation and the system state quantity stabilizes at At this time, the system state equation can be described as:
[0014]
[0015] The equilibrium point (ξ 0 , u 0 ) satisfies:
[0016] f(ξ 0 , u 0 ) = 0 (2)
[0017] That is, in the case of ξ 0 and u 0 , the system reaches a steady state;
[0018] Perform Taylor expansion on the system state equation at any point , retain the first-order term, and ignore the high-order terms; simplify the Taylor expansion of the system state equation to the incremental form:
[0019]
[0020] where, Δξ(t) represents the system state increment; represents the derivative of Δξ(t); A t,0 represents the system gain matrix; B t,0represents the system input matrix; Δu(t) represents the increment of the control signal; Equation (3) is a linear time-varying system used to describe the relationship between the state variables of the nonlinear system and the reference quantity, and at the same time reflects the linearization process of the nonlinear system near the reference trajectory;
[0021] Equation (4) is a linearized original state-space equation and explicitly considers external disturbances or system errors:
[0022]
[0023] where ξ(t) represents the system state; represents the derivative of ξ(t); u(t) represents the control signal; d t,0 (t) represents the deviation term;
[0024] Then, the state increment d t,0 (t) is defined in Equation (5), which represents the deviation between the current state of the system and the reference state, and is expressed as:
[0025]
[0026] where, ξ 0 (t) represents the initial value of the system state; represents ξ 0 (t)'s derivative;
[0027] Through Equations (4) and (5), the dynamic changes of the system state near the reference point (ξ 0 , u 0 ) can be described in detail, and the linearization error is regarded as a perturbation term for compensation.
[0028] Further, step (1) also includes:
[0029] (1.2) Using the Euler method to discretize the system to obtain a linear time-varying model suitable for discrete-time control:
[0030] ξ(k + 1) = A k,t ξ(k) + B k,t u(k) + d k,t (6)
[0031] where ξ(k) is the state variable at time k; u(k) is the control variable at time k; A k,t , B k,t , d k,t are the coefficient matrices at time k;
[0032] In order to accurately represent the dynamic characteristics of the system, the state variables and control variables are combined into an extended state variable
[0033] An additional component related to cumulative error or disturbance compensation is added to the original state to form a new extended state:
[0034]
[0035] where disturbance vector C k,t and D k,t are coefficient matrices at time k, 0 m×n is an m×n dimensional zero matrix, I m is an m dimensional identity matrix, 0 m is an m dimensional zero matrix; η(k|t) is the system output
[0036] To reduce the complexity of the prediction model, assume that the state matrix and the control matrix are piecewise constant in the time domain, i.e.:
[0037] If the system control time domain is N c , and the prediction time domain is N p , and the control increment Δu(t) of the state quantity ξ(t) at time t is known, then the state quantity and output quantity within the prediction time domain are:
[0038]
[0039] Set the control increment Δu(t+i|t) to 0 outside the control time domain N c to reduce the accumulation of prediction deviation and make the system behavior closer to the actual; the control increment Δu(t+i|t) is:
[0040]
[0041] The output quantity is:
[0042]
[0043] where η tr , η c , η sc represent the control system output quantity, the hard constraint output quantity, and the soft constraint output quantity in sequence;
[0044] To describe the future behavior of the above output quantity within the prediction time domain of length N p , define the following prediction vector Y tr (t):
[0045] Y tr (t) = [η tr (t + 1|t), ηtr (t + 2|t),..., η tr (t + N p |t)] T (12)
[0046] wherein,
[0047] It is composed of several unit matrix blocks I, zero matrix blocks, and scalar matrix p; in order to accurately describe the dynamic behavior of the system and predict future outputs, an output equation based on the state space model is adopted; Formula (14) provides a comprehensive framework for predicting future outputs:
[0048] Y tr (t) = Ψ trt ξ(t) + Θ trt ΔU(t) + Γ trt Φ(t) + Λ tr (t) (14)
[0049] wherein, Y tr (t) represents the predicted system output; ξ(t) represents the state variables of the system; ΔU(t) represents the control input increment; Φ(t) is the dynamic constraint of the system; Λ tr (t) represents the influence of other additional terms or external disturbances of the system; Ψ trt , Θ trt , Γ trt are matrices related to the state, control input, and output.
[0050] Furthermore, in step (2), the multiple constraint conditions include:
[0051] The thrust constraint condition of the AUV is defined as:
[0052] τ min (t) ≤ τ(t) ≤ τ max (t) (15)
[0053] wherein, τ min (t) and τ max (t) represent the minimum and maximum values of the thrust;
[0054] The constraint condition of the control input is:
[0055] U min ≤ U(k) ≤ U max (16)
[0056] wherein, U min , U max are the lower and upper limits of the control input;
[0057] The positioning and attitude constraint conditions of the AUV are defined as:
[0058]
[0059] where η min and η max are the upper and lower limits of the AUV torque; v min and v max are the lower and upper limits of the sailing speed; σ is the compensation amount, whose value can be adjusted according to the detection distance of the sensor. The longer the detection distance, the larger the speed upper limit, which is defined as:
[0060] σ = f(v obs , d obs ) (18)
[0061] where v obs and d obs are the relative speed and distance of the obstacle in sequence;
[0062] Introduce the path curvature into the speed constraint to correct the dynamic change of the speed:
[0063] v max (t) = v base (1 - ακ(t)) (19)
[0064] Formula (19) indicates that the maximum speed v max (t) changes with time and is adjusted by the reference speed v base and the time-dependent correction factor κ(t); the correction amount is controlled by the coefficient α;
[0065] Express the constraints of formulas (15)-(19) by state variables, that is:
[0066] x min ≤ x(k) ≤ x max (20)
[0067] where x min = [η min v min + σ] T , x max = [η max v max - σ] T are the upper and lower limits of the state vector.
[0068] Furthermore, in step (2), the design objective function J(k) is as follows:
[0069]
[0070] where η r(t+i|t) is the reference output at time t+1; Q and R are weight matrices respectively, used to balance the stability of the system and the control performance; ρ is the weight coefficient; ε is the relaxation factor; Δu(t+j|t) is the control increment at time t+j predicted based on time t, and the control increment is defined as follows:
[0071] Δu(t+j|t) = u(t+j|t) - u(t+j-1|t) (22).
[0072] Furthermore, step (3) includes: combining the objective function with multiple constraints, designing a corresponding quadratic programming model and solving the optimization problem.
[0073] Furthermore, the quadratic programming calculation process is as follows:
[0074] Formula (23) is used to define the relationship between the output deviation of the system, the control increment and the actual input. It compares the reference output sequence Y ref (t) with the predicted output to form the error vector E(t); the predicted output is obtained by linearly transforming the reference state ξ t , Γ t , A 1 (t) for the reference state ξ ref (t|t) or known information; meanwhile, formula (23) also gives that the input vector U(t) within the current prediction time domain is composed of the input U(t-1) at the previous moment plus the incremental input ΔU(t);
[0075]
[0076] Substituting formula (23) into formula (21) gives:
[0077]
[0078] Formula (24) describes a quadratic objective function G, comprehensively considering the error penalty, that is, the weighted square term of E(t) and the reference deviation, the weighted square term of the control increment penalty ΔU(t), and the soft constraint penalty term brought by the relaxation variable ε;
[0079] Assume the matrix is the Kronecker product, I m is the m-order identity matrix;
[0080] P t = E(t) T Q e E(t) + ρε 2
[0081]
[0082] Under the condition of satisfying the constraint conditions, the objective function in each sampling period can obtain the following quadratic programming problem:
[0083]
[0084] Formula (25) is used to measure the comprehensive cost of the tracking error, control increment, and soft constraint relaxation amount ε during the prediction period; η ref (t + i|t) is the reference output; ρ is the weighting term of the slack variable; in the model predictive control algorithm, the optimal control increment ΔU is obtained by minimizing G * ;
[0085] After the model predictive control algorithm solves the above steps in each control period, the increment in the control input sequence quantity is calculated; the first control increment in the control sequence is used as the actual input.
[0086] The present invention reduces the dimension and computational amount of the optimization problem by introducing the quadratic programming (QP) method and Kronecker product optimization. Compared with the prior art, the present invention enables the AUV to achieve higher-frequency real-time control on an embedded processor through an efficient hierarchical solution algorithm, significantly improving the computational efficiency and system response speed.
[0087] Furthermore, the control optimization process is periodically repeated to implement the entire step of rolling optimization;
[0088]
[0089] According to the rolling optimization strategy of model predictive control, the first control quantity of the feasible solution plus the future output quantity of the previous section can be used to obtain the input vector at the current moment, that is:
[0090] u(t) = u(t - 1) + Δu t (27)
[0091] The relationship between u(k) and the input τ is:
[0092] u(k) = τ - M -1 C(v(k))v(k) + M -1 D(v(k))v(k) + M -1 g(η(k))) (28)
[0093] Equation (28) shows that the actual physical thrust-torque vector τ is converted into the discrete model input u(k), and the difference part is exactly the necessary correction term generated by the dynamic effects such as the mass-inertia matrix M, the Coriolis and centrifugal force matrix C(v(k)), the drag D(v(k)), and the buoyancy g(ξ(k)) during the movement of the AUV; substituting the input vector u(k) into Equation (27) to obtain the thrust and torque τ, and the propulsion system of the AUV executes this input until the next moment.
[0094] Further, in step (3), the reference trajectory is determined by the target position initially given by the mother ship and the initial position of the AUV.
[0095] An AUV trajectory tracking system based on adaptive time-varying optimal control of the present invention includes:
[0096] An environment perception module for obtaining environment perception data and a reference trajectory from the mother ship;
[0097] A state detection sensor for detecting the state of the AUV in real time, including position and speed, to obtain the actual trajectory of the AUV;
[0098] A model predictive controller configured with a time-varying linear prediction model, multiple constraint conditions, and an objective function for rolling optimization, solving the optimal control input that satisfies multiple constraint conditions within each motion cycle, applying the optimal control input to the motion model of the AUV to guide the AUV to move along the reference trajectory, and predicting the trajectories at multiple future moments;
[0099] A feedback correction module for comparing the predicted trajectory of the AUV with the actual trajectory, calculating the error, and then correcting the trajectory information through a feedback correction mechanism. The trajectory information after feedback correction is input to the model predictive controller to recalculate the optimal control input.
[0100] Beneficial effects: Compared with the prior art, the present invention has the following remarkable advantages:
[0101] (1) The core of the present invention is based on the time-varying linear model of the AUV. The local linearization of its nonlinear dynamics model is carried out by the Taylor expansion method to construct a time-varying linear prediction model. This model can significantly reduce the computational complexity and improve the real-time performance of the controller. On this basis, combined with the method of model predictive control, by optimizing the control input within the future time window, the accurate tracking of the AUV trajectory is realized; by updating the linearization point in real time within each control cycle, the problem of error accumulation caused by the fixed linearization point is avoided.
[0102] (2) The present invention combines a rolling optimization strategy and multiple constraint conditions, and can dynamically adjust the control strategy by calculating the optimal control sequence in real time within each control cycle to cope with environmental changes and dynamic changes in task requirements. This design not only ensures high precision of the AUV in trajectory tracking, but also effectively addresses issues such as environmental disturbances, dynamic obstacles, and actuator saturation, enhancing the robustness of the control system. Especially in the task of submarine cable laying, the AUV can quickly adjust its trajectory and maintain high-precision trajectory tracking ability when facing water flow disturbances and model uncertainties.
[0103] In summary, the present invention combines time-varying linear model predictive control and rolling optimization technology, and can adjust the control strategy in real time in a dynamic and complex underwater environment, ensuring the efficiency and stability of the AUV when performing tasks such as submarine cable laying. Brief Description of the Drawings
[0104] Figure 1 is a flowchart of the AUV trajectory tracking method based on adaptive time-varying optimization control provided by an embodiment of the present invention;
[0105] Figure 2 is a logic block diagram of trajectory tracking control in an embodiment of the present invention;
[0106] Figure 3 is a schematic diagram of AUV submarine cable laying in an embodiment of the present invention. Detailed Embodiment
[0107] The following further describes the present invention with reference to the drawings.
[0108] An embodiment of the present invention is exemplarily introduced with AUV submarine cable laying. As Figure 3 shown, the cable is carried on a replaceable turntable in the middle of the AUV, and the AUV maintains a certain height from the seabed and lays the cable along a preset path. The AUV can be equipped with a high-pressure water gun, a cable burial plow, a robotic arm, a nail gun, etc. for laying / repair operations. On the premise of ensuring the safety of the AUV itself, to ensure the best cable laying quality, during the whole process, the AUV not only needs to track the preset path, but also needs to maintain the best suspension height from the seabed plane.
[0109] Under the working condition of submarine cable laying, the following assumptions are made for the AUV:
[0110] 1) The AUV is a rigid body with invariant mass and uniform distribution;
[0111] 2) The shape of the AUV is completely symmetric about each coordinate axis;
[0112] 3) The AUV has limited environmental perception ability, that is, without the support of an external mother ship, it cannot conduct a full-coverage scan of the seabed terrain;
[0113] 4) The AUV sensor is along the bow direction.
[0114] When the AUV lays cables, it is necessary to convert the cable-laying path given by the host computer into its own control instructions, including the AUV's own position, the preset path, and the target position.
[0115] Such as Figure 1 As shown, the embodiment of the present invention provides an AUV trajectory tracking method based on adaptive time-varying optimal control, including the following steps:
[0116] (1) Configure a time-varying linear prediction model;
[0117] To ensure that the AUV can stably execute tasks in a dynamic environment, it is first necessary to consider its kinematic and dynamic characteristics in the control system, which requires designing a model that can predict and optimize the control input in real time. Assume that the system state equation applies a constant control amount and stabilizes at a certain equilibrium point, and then obtain a linearized time-varying system through the Taylor expansion method. This processing method linearizes the dynamic behavior of the nonlinear system near the reference point and considers the linearization error as a perturbation term for compensation, laying a theoretical foundation for the subsequent predictive control design.
[0118] (1.1) Due to the nonlinear characteristics of the AUV system, directly solving the control problem has a high computational complexity. Therefore, a linearization method is used to simplify the system state equation. In the control time domain, assume that the system state equation applies a constant control amount And the system state quantity stabilizes at At this time, the system state equation can be described as:
[0119]
[0120] The equilibrium point (ξ 0 , u 0 ) satisfies:
[0121] f(ξ 0 , u 0 ) = 0 (2)
[0122] That is, in the case of ξ 0 and u 0 , the system reaches a steady state;
[0123] Perform a Taylor expansion of the system state equation at any point , retain the first-order term, and ignore the higher-order terms; simplify the Taylor expansion of the system state equation to an incremental form:
[0124]
[0125] Among them, Δξ(t) represents the system state increment; Denotes the derivative of Δξ(t); A t,0 Denotes the gain matrix of the system; B t,0 Denotes the system input matrix; Δu(t) denotes the increment of the control signal. Equation (3) is a linear time-varying system, which is used to describe the relationship between the state variables of the nonlinear system and the reference quantity, and at the same time reflects the linearization process of the nonlinear system near the reference trajectory;
[0126] Equation (4) is a linearized original state-space equation, and explicitly considers external disturbances or system errors:
[0127]
[0128] Among them, ξ(t) denotes the system state; Denotes the derivative of ξ(t); u(t) denotes the control signal; d t,0 (t) denotes the deviation term.
[0129] Next, the deviation term d t,0 (t) is defined in Equation (5), which represents the deviation between the current state of the system and the reference state, and is expressed as:
[0130]
[0131] Among them, ξ 0 (t) denotes the initial value of the system state; Denotes ξ 0 (t) derivative.
[0132] Through Equations (4) and (5), the dynamic changes of the system state near the reference point (ξ 0 , u 0 ) can be described in detail, and the linearization error is regarded as a perturbation term for compensation.
[0133] (1.2) The nonlinear system is transformed into a linear time-varying system by the linearization method, and a suitable predictive control model is designed according to this linear model. On this basis, the Euler method is used to discretize the system to obtain a linear time-varying model suitable for discrete-time control:
[0134] ξ(k + 1) = A k,t ξ(k) + B k,t u(k) + d k,t (6)
[0135] Among them, ξ(k) is the state variable at time k; u9k) is the control variable at time k; A k,t 、B k,t 、d k,t are the coefficient matrices at time k.
[0136] To accurately represent the dynamic characteristics of the system, the state variables and control variables are combined into an extended state variable
[0137] Based on the original state, additional components related to cumulative error or disturbance compensation, etc. are added to form a new extended state:
[0138]
[0139] Among them, disturbance vector C k,t 、D k,t are coefficient matrices at time k, 0 m×n is an m×n dimensional zero matrix, I m is an m-dimensional identity matrix, 0 m is an m-dimensional zero matrix; η(k|t) is the system output quantity.
[0140] To reduce the complexity of the prediction model, assume that the state matrix and the control matrix are piecewise constant in the time domain, that is:
[0141] If the system control time domain is N c , and the prediction time domain is N p , and the control increment Δu(t) of the state quantity ξ(t) at time t is known, then the state quantity and output quantity within the prediction time domain are:
[0142]
[0143] Set the control increment Δu(t+i|t) to 0 outside the control time domain N c to reduce the accumulation of prediction deviation and make the system behavior closer to the actual; the control increment Δu(t+i|t) is:
[0144]
[0145] The output quantity is:
[0146]
[0147] Among them, η tr 、η c 、η sc represent the control system output quantity, hard constraint output quantity, and soft constraint output quantity in sequence;
[0148] To describe the future behavior of the above output quantity within the prediction time domain of length N p , define the following prediction vector Y tr (t):
[0149] Y tr (t) = [η tr (t + 1|t), η tr (t + 2|t),..., η tr (t + N p |t)] T (12)
[0150] wherein,
[0151] is composed of several unit matrix blocks I, zero matrix blocks, and scalar matrix p; in order to accurately describe the dynamic behavior of the system and predict future outputs, an output equation based on the state - space model is adopted; formula (14) provides a comprehensive framework for predicting future outputs:
[0152] Y tr (t) = Ψ trt ξ(t) + Θ trt ΔU(t) + Γ trt Φ(t) + Λ tr (t) (14)
[0153] wherein, Y tr (t) represents the predicted system output; ξ(t) represents the state variable of the system; ΔU(t) represents the control - input increment; Φ(t) is the dynamic constraint of the system; Λ tr (t) represents the influence of other additional terms or external disturbances of the system; Ψ trt , Θ trt , Γ trt are matrices related to the state, control input, and output;
[0154] (2) Define multiple constraints and the objective function;
[0155] (2.1) In the actual submarine - cable laying task, the driving force and torque of the AUV are not only affected by the performance of the thrusters but also disturbed by the submarine - cable laying load and environmental dynamic factors. To cope with these influences, the improved input constraints will consider these dynamic changes to ensure the stability and safety of the system. The constraint conditions are defined as follows:
[0156] τ min (t) ≤ τ(t) ≤ τ max (t) (15)
[0157] wherein, τ min (t) and τ max (t) represent the minimum and maximum values of the thrust;
[0158] This constraint can ensure that the actuator operates within the safe working range and avoid hardware damage due to overload. However, considering only the thrust and torque constraints is not enough to fully constrain the motion behavior of the AUV.
[0159] During the path tracking and task execution process, the control input (including dynamic variables such as speed and acceleration) needs to be further constrained. The constraints of the control input are:
[0160] U min ≤U(k)≤U max (16)
[0161] Among them, U min , U max To control the lower and upper limits of the input; Due to the limited detection range and maximum driving force of the AUV sensor, its maximum driving speed must be limited. In addition, the positioning and posture of the AUV also need to be constrained accordingly, namely:
[0162]
[0163] Among them, η min , η max is the upper and lower limits of the AUV torque; v min 、v max are the lower and upper limits of the speed; σ is the compensation value, whose value can be adjusted with the sensor detection distance. The longer the detection distance, the greater the speed limit, which is defined as:
[0164] σ=f(v obs ,d obs ) (18)
[0165] Among them, v obs d obs They are the relative speed and distance of the obstacle respectively. To make up for the shortcomings in the existing speed constraint design, the path curvature is introduced into the speed constraint to correct the dynamic change of speed:
[0166] v max (t) = v base (1-ακ(t)) (19)
[0167] Formula (19) expresses the maximum speed v max (t) changes with time, through the reference speed v base The correction is controlled by the coefficient α, which reflects the effect of external factors (such as distance or environmental changes) on the speed.
[0168] The constraints of formulas (15)-(19) are expressed by state variables, that is:
[0169] x min≤ x(k) ≤ x max (20)
[0170] where x min = [η min v min + σ] T , x max = [η max v max - σ] T are the upper and lower limits of the state vector.
[0171] (2.2) Based on the above-mentioned constraints and state changes, further consider how to design the objective function to optimize the control effect and meet all constraint requirements. The specific objective function J(k) is as follows:
[0172]
[0173] where η r (t + i|t) is the reference output at time t + 1; Q and R are weight matrices used to balance the stability and control performance of the system; ρ is the weight coefficient; ε is the relaxation factor; Δu(t + j|t) is the control input increment at time t + j predicted based on time t, and the control increment is defined as follows:
[0174] Δu(t + j|t) = u(t + j|t) - u(t + j - 1|t) (22)
[0175] (3) Through the model predictive control method (MPC), optimize the control input of the AUV to minimize the path tracking error and reduce the change of the control input; through rolling optimization, solve the optimal control input that satisfies multiple constraint conditions within each motion cycle of the AUV, and these optimization results are used to guide the AUV to move along the reference trajectory to achieve high-precision path tracking;
[0176] As Figure 2 shown in the trajectory tracking control framework, in this framework, the AUV minimizes the path tracking error by optimizing the control input while considering environmental disturbances. Through task allocation, state estimation, optimizing the control input, and feedback correction, ensure the stable operation of the AUV in the dynamic underwater environment.
[0177] After defining the objective function, the next step is to transform this optimization problem into a solvable form. Since the quadratic programming (QP) method has high computational performance in dealing with optimization problems with constraints, combine the objective function with the constraint conditions and design the corresponding quadratic programming model to achieve efficient path tracking and control of the AUV. The specific quadratic programming calculation process is as follows:
[0178] Equation (23) is used to define the output deviation of the system and the relationship between the control increment and the actual input. Specifically, it compares the reference output sequence Y ref (t) with the "predicted output" here (obtained by linearly transforming the reference state ξ t , Γ t , A 1 (t), etc. with respect to the reference state ξ ref (t|t) or known information), thereby forming the error vector E(t). At the same time, Equation (23) also gives that the input vector U(t) within the current prediction horizon is composed of the previous moment's input U(t - 1) plus the incremental input ΔU(t).
[0179]
[0180] Substituting Equation (23) into Equation (21) gives:
[0181]
[0182] Equation (24) describes a quadratic objective function G, which comprehensively considers the error penalty, i.e., the weighted square term of E(t) and the reference deviation, the weighted square term of the control increment penalty ΔU(t), and the soft constraint penalty term brought by the slack variable ε. That is to say, by minimizing G, the tracking of the error and the limitation of the control input change can be achieved within the prediction horizon, while allowing the constraint to be moderately "relaxed" through the slack variable when necessary to ensure the feasibility of the overall optimization problem and take into account the control performance. Assume that the matrix is the Kronecker product, I m is the m - order identity matrix;
[0183] P t = E(t) T Q e E(t)+ ρε 2
[0184]
[0185] Under the condition of satisfying the constraint conditions, the objective function in each sampling period can obtain the following quadratic programming problem:
[0186]
[0187] Equation (25) is used to measure the comprehensive cost of the tracking error, the control increment, and the soft constraint relaxation amount ε within the prediction period. η ref (t + i|t) is the reference output; ρ is the weighted term of the slack variable; in the model predictive control algorithm, the optimal control increment ΔU * is obtained by minimizing G.
[0188] After the above steps are solved in each control cycle of the model predictive control algorithm, the increment in the control input sequence quantity is calculated; the first control increment in the control sequence is used as the actual input. The control optimization process is periodically repeated to implement the entire step of rolling optimization;
[0189]
[0190] According to the rolling optimization strategy of model predictive control, the input vector at the current moment can be obtained by taking the first control quantity of the feasible solution plus the future output quantity of the previous section, that is:
[0191] u(t) = u(t - 1) + Δu t (27)
[0192] The relationship between u(k) and the input τ is:
[0193] u(k) = τ - (M -1 C(v(k))v(k) + M -1 D(v(k))v(k) + M -1 g(η(k))) (28)
[0194] Formula (28) shows that the actual physical thrust-torque vector τ is converted into the discrete model input u(k), and the difference part is exactly the necessary correction term generated by the dynamic effects such as the mass-inertia matrix M, the Coriolis and centrifugal force matrix C(v(k)), the drag D(v(k)), and the buoyancy g(η(k)) during the movement of the AUV. Substitute the input vector u(k) into formula (27) to calculate the thrust and torque τ, and the propulsion system of the AUV executes this input until the next moment.
[0195] The reference trajectory is determined by the target position initially given by the mother ship and the initial position of the AUV.
[0196] (4) Apply the optimal control input to the motion model of the AUV and predict the trajectories at multiple future moments; obtain the actual trajectory of the AUV through the mother ship, compare it with the predicted trajectory, and calculate the error; according to the error, the feedback correction mechanism corrects the trajectory information and adjusts the current position and state.
[0197] (5) In each control cycle, return to step (3), recalculate the optimal control input using the feedback-corrected trajectory information, and adjust the actions according to the new control input. Continuously improve the control accuracy through rolling optimization until the tracking task is completed.
[0198] The embodiment of the present invention also provides an AUV trajectory tracking system based on adaptive time-varying optimization control, including:
[0199] An environmental perception module, which is used to obtain environmental perception data (such as obstacle and target position information, etc.) and a reference trajectory from the mother ship;
[0200] A state detection sensor, which is used to detect the state of the AUV in real time (such as the current position, speed, etc.) and obtain the actual trajectory of the AUV;
[0201] A model predictive controller, configured with a time-varying linear prediction model, multiple constraint conditions and an objective function, which is used for rolling optimization, solving the optimal control input that satisfies multiple constraint conditions within each motion cycle, applying the optimal control input to the motion model of the AUV to guide the AUV to move forward along the reference trajectory, and predicting the trajectories at multiple future moments;
[0202] A feedback correction module, which is used to compare the predicted trajectory of the AUV with the actual trajectory, calculate the error, and then correct the trajectory information through a feedback correction mechanism. The trajectory information after feedback correction is input into the model predictive controller to recalculate the optimal control input.
Claims
1. A method for AUV trajectory tracking based on adaptive time-varying optimization control, characterized in that: include: (1) Configure a time-varying linear prediction model; Assuming that the system state equation applies a constant control variable and stabilizes at a certain equilibrium point, the linearized time-varying system is obtained through the Taylor expansion method; (2) Define multiple constraints and objective functions; (3) Optimize the control input of the AUV through model predictive control methods to minimize the path tracking error and reduce the variation of the control input; Through rolling optimization, the optimal control input that satisfies multiple constraints in each motion cycle is solved. These optimization results are used to guide the AUV along the reference trajectory. (4) Apply the optimal control input to the AUV's motion model and predict the AUV's trajectory at multiple moments in the future; obtain the AUV's actual trajectory, compare it with the predicted trajectory, and calculate the error; based on the error, the feedback correction mechanism corrects the trajectory information; (5) In each control cycle, return to step (3) and use the feedback-corrected trajectory information to recalculate the optimal control input. The control accuracy is continuously improved through rolling optimization until the tracking task is completed.
2. The AUV trajectory tracking method based on adaptive time-varying optimization control according to claim 1 is characterized in that: Step (1) comprises: (1.1) In the control time domain, it is assumed that the system state equation applies a constant control variable And the system state quantity is stable at At this point, the system state equation can be described as: The equilibrium point (ξ0,u0) satisfies: f(ξ0,u0)=0 (2) That is, in the case of ξ0 and u0, the system reaches a steady state; The system state equation at any point Perform Taylor expansion at , retain the first-order terms, and ignore the higher-order terms; simplify the Taylor expansion of the system state equation into an incremental form: Among them, Δξ(t) represents the system state increment; represents the derivative of Δξ(t); A t,0 represents the gain matrix of the system; B t,0 represents the system input matrix; Δu(t) represents the increment of the control signal; Formula (3) is a linear time-varying system, which is used to describe the relationship between the state variables of the nonlinear system and the reference quantity, and also reflects the linearization process of the nonlinear system near the reference trajectory; Formula (4) is a linearized original state space equation that explicitly takes into account external disturbances or system errors: Among them, ξ(t) represents the system state; represents the derivative of ξ(t); u(t) represents the control signal; d t,0 (t) represents the deviation term; Next, the state increment d is defined in formula (5) t,0 (t), represents the deviation between the current state of the system and the reference state, which is expressed as: Among them, ξ0(t) represents the initial value of the system state; represents the derivative of ξ0(t); Formulas (4) and (5) can be used to describe in detail the dynamic changes of the system state near the reference point (ξ0,u0), and the linearization error can be regarded as a disturbance term for compensation.
3. The AUV trajectory tracking method based on adaptive time-varying optimization control according to claim 2 is characterized in that: Step (1) further includes: (1.2) The system is discretized using the Euler method to obtain a linear time-varying model suitable for discrete-time control: ξ(k+1)=A k,t ξ(k)+B k,t u(k)+d k,t (6) Among them, ξ(k) is the state variable at time k; u(k) is the control variable at time k; A k,t , B k,t d k,t is the coefficient matrix at time k; In order to accurately describe the dynamic characteristics of the system, the state variables and control variables are combined into extended state variables. The additional components related to the accumulated error or disturbance compensation are added to the original state to form a new extended state: in, Perturbation Vector C k,t , D k,t is the coefficient matrix at time k, 0 m×n is an m×n dimensional zero matrix, I m is the m-dimensional identity matrix, 0 m is an m-dimensional zero matrix; η(k|t) is the system output In order to reduce the complexity of the prediction model, it is assumed that the state matrix and control matrix It is piecewise constant in the time domain, that is: If the system control time domain is N c , the prediction time domain is N p , and the control increment Δu(t) of the state quantity ξ(t) at time t is known, then the state quantity and output quantity in the prediction time domain are: The control increment Δu(t+i|t) is calculated in the control time domain N c The control increment Δu(t+i|t) is set to 0 to reduce the accumulation of prediction deviation and make the system behavior closer to reality. The output is: Among them, η tr , η c , η sc represents the control system output, hard constraint output and soft constraint output respectively; In order to p Describe the future behavior of the above output in the prediction time domain and define the following prediction vector Y tr (t): Y tr (t)=[η tr (t+1|t),η tr (t+2|t),...,η tr (t+N p |t)] T (12) in, It consists of several unit matrix blocks I, zero matrix blocks and scalar matrix p. In order to accurately describe the dynamic behavior of the system and predict future outputs, the output equation based on the state space model is adopted. Formula (14) provides a comprehensive framework for predicting future outputs: Y tr (t)=Ψ trt ξ(t)+Θ trt ΔU(t)+Γ trt Φ(t)+Λ tr (t) (14) Among them, Y tr (t) represents the predicted system output; ξ(t) represents the state variable of the system; ΔU(t) represents the control input increment; Φ(t) is the dynamic constraint of the system; Λ tr (t) represents the influence of other additional items or external disturbances on the system; Ψ trt , Θ trt , Γ trt is the matrix relating states, control inputs and outputs.
4. The AUV trajectory tracking method based on adaptive time-varying optimization control according to claim 3 is characterized in that: In step (2), the multiple constraints include: The thrust constraint of AUV is defined as: τ min (t)≤τ(t)≤τ max (t) (15) Among them, τ min (t) and τ max (t) represents the minimum and maximum values of thrust; The constraints for the control input are: IN min ≤U(k)≤U max (16) Among them, U min , U max is the lower and upper limits of the control input; The positioning and attitude constraints of the AUV are defined as: Among them, η min , η max is the upper and lower limits of the AUV torque; v min 、v max are the lower and upper limits of the speed; σ is the compensation value, whose value can be adjusted with the sensor detection distance. The longer the detection distance, the greater the speed limit, which is defined as: σ=f(v obs ,d obs ) (18) Among them, v obs d obs They are the relative speed and distance of the obstacle, respectively; Introduce path curvature into the velocity constraint to correct the dynamic change of velocity: v max (t)=v base (1-αk(t)) (19) Formula (19) expresses the maximum speed v max (t) changes with time, through the reference speed v base and the time-dependent correction factor k(t); the correction amount is controlled by the coefficient α; The constraints of formulas (15)-(19) are expressed by state variables, that is: x min ≤x(k)≤x max (20) Among them, x min =[η min v min +σ] T , x max =[η max v max -σ] T are the upper and lower limits of the state vector.
5. The AUV trajectory tracking method based on adaptive time-varying optimization control according to claim 4 is characterized in that: In step (2), the design objective function J(k) is as follows: Among them, η r (t+i|t) is the reference output at time t+1; Q and R are weight matrices, which are used to balance the stability and control performance of the system; ρ is the weight coefficient; ε is the relaxation factor; Δu(t+j|t) is the control increment at time t+j based on the prediction at time t, and the control increment is defined as follows: Δu(t+j|t)=u(t+j|t)-u(t+j-1|t)(22).
6. The AUV trajectory tracking method based on adaptive time-varying optimization control according to claim 5 is characterized in that: Step (3) includes: combining the objective function with multiple constraints, designing a corresponding quadratic programming model and solving the optimization problem.
7. The AUV trajectory tracking method based on adaptive time-varying optimization control according to claim 6 is characterized in that: The quadratic programming calculation process is as follows: Formula (23) is used to define the output deviation of the system and the relationship between the control increment and the actual input, which will refer to the output sequence Y ref (t) is compared with the predicted output to form an error vector E(t); the predicted output is represented by Ψ t , Γ t , A1(t) to the reference state ξ ref (t|t) or known information is linearly transformed; at the same time, formula (23) also shows that the input vector U(t) in the current prediction time domain is composed of the input U(t-1) at the previous moment plus the incremental input ΔU(t); Substituting formula (23) into formula (21) yields: Formula (24) describes a quadratic objective function G, which comprehensively considers the error penalty, that is, the weighted square term of the deviation between E(t) and the reference, the weighted square term of the control increment penalty ΔU(t), and the soft constraint penalty term brought by the slack variable ε; Assumption Matrix is the Kronecker product, I m is the identity matrix of order m; When the constraints are met, the objective function in each sampling period can be obtained as the following quadratic programming problem: Formula (25) is used to measure the comprehensive cost of tracking error, control increment and soft constraint relaxation ε during the forecast period; η ref (t+i|t) is the reference output; ρ is the weighted term of the slack variable; in the model predictive control algorithm, the optimal control increment ΔU is obtained by minimizing G * ; After solving the above steps in each control cycle, the model predictive control algorithm calculates the increment in the control input sequence; the first control increment in the control sequence is taken as the actual input.
8. The AUV trajectory tracking method based on adaptive time-varying optimization control according to claim 7 is characterized in that: Repeat the control optimization process periodically to achieve the entire steps of rolling optimization; According to the rolling optimization strategy of model predictive control, the input vector at the current moment can be obtained by taking the first control quantity of the feasible solution and adding the future output quantity of one section, that is: u(t)=u(t-1)+Δu t (27) The relationship between u(k) and input τ is: u(k)=τ-(M -1 C(v(k))v(k)+M -1 D(v(k))v(k)+M -1 g(η(k))) (28) Formula (28) shows that the actual physical thrust-torque vector τ is converted into the discrete model input u(k). The difference part is the inevitable correction term generated by the dynamic effects of the mass-inertia matrix M, Coriolis and centrifugal force matrix C(v(k)), resistance D(v(k)) and buoyancy g(η(k)) during the movement of the AUV. Substituting the input vector u(k) into formula (27) to obtain the thrust and torque τ, the AUV's propulsion system executes the input until the next moment.
9. The AUV trajectory tracking method based on adaptive time-varying optimization control according to claim 8 is characterized in that: In step (3), the reference trajectory is determined by the target position initially given by the mothership and the initial position of the AUV.
10. An AUV trajectory tracking system based on adaptive time-varying optimization control, characterized in that: include: An environmental perception module, used to obtain environmental perception data and reference trajectories from the mothership; State detection sensor, used to detect the state of the AUV in real time, including position and speed, and obtain the actual trajectory of the AUV; Model predictive controller, configured with a time-varying linear prediction model, multiple constraints and objective functions, for rolling optimization, solving the optimal control input that satisfies multiple constraints in each motion cycle, applying the optimal control input to the AUV's motion model to guide the AUV along the reference trajectory, and predicting the trajectory at multiple moments in the future; The feedback correction module is used to compare the predicted trajectory of the AUV with the actual trajectory, calculate the error, and then correct the trajectory information through the feedback correction mechanism. The feedback-corrected trajectory information is input into the model predictive controller to recalculate the optimal control input.
Citation Information
Cited By
Processing center control method and system based on artificial intelligence
CN120802750A
CNN ocean current prediction-based long-voyage AUV energy-saving control method, program, equipment and storage medium
CN121613728A
