Trajectory tracking control method for underwater robot

Abstract

The invention discloses an underwater robot trajectory tracking control method. The method comprises the following steps: establishing an AUV motion model; establishing a nonlinear state space model; carrying out linearization and discretization on the nonlinear state space model; defining a prediction model to predict system behaviors, and performing dynamic optimization; designing an objective function based on model predictive control (MPC); an improved ant colony algorithm IACO is introduced, and a target function is solved by adopting a dynamic adjustment confidence factor and a transition probability constant. According to the underwater robot trajectory tracking control method provided by the invention, the trajectory tracking stability and precision of the AUV can be improved, and the task execution capability of the AUV in a complex underwater environment is improved.

Description

Technical Field

[0001] The invention relates to underwater robot control technology, in particular to an underwater robot trajectory tracking control method under complex water flow interference. Background Art

[0002] As an important comprehensive infrastructure, reservoir dams are widely used in power generation, flood control and irrigation. China currently has about 98,000 dams of various types, of which 665 are used for power generation. These dams not only play an important economic and social role, but also may trigger a chain reaction of downstream dam groups and serious secondary disasters in the event of a high dam reservoir breach. In addition, many hydropower stations are located in the southwest region where geological disasters are frequent, and some old dams are also facing the problem of disease and aging. Therefore, it is particularly important to carry out dam inspections to prevent failures, timely assess risks and formulate response measures.

[0003] At present, dam inspection mainly relies on manual operation, which has problems such as low efficiency, high safety risks and incomplete data acquisition. With the continuous advancement of technology, autonomous underwater vehicle (AUV) inspection technology has gradually attracted attention. However, due to the influence of various interference factors such as water flow, it is difficult for AUV to achieve stable and high-precision trajectory tracking when operating underwater, such as reference [1]: Jaya AS, Kartidjo M W.Thrust and efficiency enhancement scheme of the fin propulsion of the biomimeticAutonomous Underwater Vehicle model in low-speed flow regime[J].Oceanengineering,

[0004] 2022(243-Jan.1).DOI:10.1016 / j.oceaneng.2021.110090. Therefore, it is particularly important to design a trajectory tracking control strategy with anti-interference ability.

[0005] In terms of trajectory tracking control, a lot of research has been carried out at home and abroad. Common methods include proportional integral differential control, backstepping control, adaptive control, fuzzy control and sliding mode control. For example, reference [2]: Qin Guoyang, Chen Qi, Zhang Jin. Design of UUV trajectory tracking controller based on IGWO-MPC and NDO-ISMC cascade algorithm [J]. Ship Engineering, 2024, 46(02): 1-11. DOI: 10.13788 / j.cnki.cbgc.2024.02.01. Although these methods have improved the trajectory tracking accuracy to a certain extent, they still do not fully consider the physical model limitations faced by underwater robots in complex hydrological environments. For example, the constraints of system state and control input will significantly affect the control effect.

[0006] Model Predictive Control (MPC) is an advanced control method based on system model, which mainly includes three parts: prediction model, rolling optimization and feedback correction. As mentioned in the literature [3]: Qin Guoyang, Chen Qi, Zhang Jin. Design of UUV trajectory tracking controller based on IGWO-MPC and NDO-ISMC cascade algorithm [J]. Ship Engineering, 2024, 46(02): 1-11. DOI: 10.13788 / j.cnki.cbgc.2024.02.01. The significant advantage of MPC is that it can incorporate various constraints of the system into the control algorithm, thereby improving the control performance.

[0007] With the deepening of research, MPC algorithm has gradually become a hot field in trajectory tracking control. For example, the literature [4]: ​​Li Z, Wang M, Yan Y, et al. Tube Model Predictive Control Based Cyber-attack-resilient Optimal Voltage Control ~ Strategy in Wind Farms [J]. CSEE Journal of Power and Energy Systems, 2024, 10 (2): 530-538. DOI: 10.17775 / CSEEJPES.2021.09490.

[0008] In order to address the problem that underwater robots are difficult to achieve accurate trajectory tracking when they are disturbed, reference [5] developed an adaptive controller based on a dynamic multi-body model. The controller can identify and compensate for the interference caused by external factors such as the operator. Although adaptive control technology can automatically adjust control parameters according to environmental changes and has good robustness, it is still difficult to form a unified and standardized controller design process. For example, reference [5]: HUANG H, ZHANG GC, LI JY, et al. Model Based Adaptive Control and Disturbance Compensation for Underwater Vehicles [J]. Chinese Journal of Mechanical Engineering, 2018, 31 (1): 1-13.

[0009] In order to solve the trajectory tracking problem of underwater robots under multi-objective constraints, researchers have tried to combine MPC with other optimization algorithms to improve control performance and anti-interference ability. However, this article does not take into account the physical limits of AUV (such as maximum speed and acceleration limits). This may result in the AUV being unable to move completely according to the predicted trajectory during the actual control process, thereby affecting the accuracy of trajectory tracking. As recorded in the literature [6]: Zhang Y, Liu X, Luo M, et al. MPC-based 3-D trajectory tracking for an autonomous underwater vehicle with constraints in complex ocean environments [J]. Ocean Engineering, 2019, 189: 106309. Summary of the invention

[0010] The technical problem to be solved by the present invention is to provide an underwater robot trajectory tracking control method that integrates the improved ant colony algorithm and model predictive control. The method enhances the optimization performance of the ant colony algorithm by introducing dynamically adjusted confidence factors and transfer probability constants into the traditional ant colony algorithm and improving the initial distribution design of the ant colony. The improved IACO algorithm is applied to the rolling optimization process of the model predictive control MPC; the control method overcomes the limitations of existing research, improves the trajectory tracking stability and accuracy of the AUV, and enhances the task execution capability of the AUV in complex underwater environments.

[0011] In order to solve the above technical problems, the technical solution adopted by the present invention is:

[0012] A method for tracking and controlling an underwater robot trajectory comprises the following steps:

[0013] Step 1: Establish AUV motion model;

[0014] Step 2: Establish a nonlinear state space model;

[0015] Step 3: Linearize and discretize the nonlinear state space model;

[0016] Step 4: Define a prediction model to predict system behavior and perform dynamic optimization;

[0017] Step 5: Design the objective function based on model predictive control;

[0018] Step 6: Introduce the improved ant colony algorithm IACO and solve the objective function by dynamically adjusting the confidence factor and the transition probability constant;

[0019] Step 7: The control input is updated in each control cycle through the feedback mechanism to ensure that the robot completes accurate trajectory tracking.

[0020] In step 1, for the AUV motion model, the forward speed, drift speed, and bow speed of the underwater robot in the carrier coordinate system {b} are represented by u, v, and r, respectively, and the x-axis position, y-axis position, and bow direction of the underwater robot in the inertial coordinate system {n} are represented by x, y, and ψ, respectively;

[0021] The velocity vector of the carrier coordinate system is defined as v' = [u, v, r] T , T represents the transposed matrix, and the position and heading vector in the inertial coordinate system are defined as η = [x, y, ψ] T Based on this definition, the motion coordinate transformation equation of the underwater robot is established:

[0022]

[0023] in: is the derivative of η, T(ψ) is the transformation matrix from the velocity of the carrier coordinate system {b} to the velocity of the inertial coordinate system {n}; it is expressed as:

[0024]

[0025] In the step 2, a nonlinear state space model is established, which specifically includes:

[0026] Establish a hydrodynamic model for the front-drive AUV:

[0027]

[0028] Where: M RBis the rigid body inertia matrix; M A is the additional mass matrix; represents the acceleration of the AUV, represents the relative acceleration of the AUV and the water flow in the carrier coordinate system, C RB (v) is the centrifugal force matrix; C A (v r ) is the centrifugal force matrix generated by the rotation of the carrier coordinate system, v r represents the relative speed between the AUV and the water flow in the carrier coordinate system, D(v r ) is the damping matrix; τ is the propulsion force of the AUV; τ d Environmental interference;

[0029] f u ,t r They are the forward force and bow moment generated by the AUV propeller respectively;

[0030] f uw ,f vw ,f rw They are the forward force, drift force and bow rolling moment generated by environmental disturbances;

[0031] Assume that the system state variable is x = [x, y, ψ, u, v, r] T , the control input is u=τ=[f u ,t r ] T , the system input is y = [x, y] T , thus the nonlinear state space model of AUV motion can be expressed as follows:

[0032]

[0033] y=P2x

[0034] in, represents the rate of change of the system state x, and f(x,u) describes the dynamic characteristics of the system, which accepts the current system state x and the system control input u as parameters and outputs

[0035] In step 3, the nonlinear state space model is linearized and discretized, including the following steps:

[0036] Step 3.1: Using the approximate linearization method, assume that the reference system runs along the desired trajectory. By obtaining the state and control information at each time point on the desired trajectory, and then processing the deviation between the reference system and the current system, a model predictive controller that can track the desired trajectory is designed; the equation of the reference system is as follows:

[0037]

[0038] in, Represents the reference state variable x r The rate of change, f(x r ,u r ) indicates acceptance of the current reference state x r and the reference control input u r As a parameter and output

[0039] At any reference point (x r ,u r ) is used to perform a first-order Taylor expansion on function (5), and we obtain:

[0040]

[0041] Finally, the linearized state space model of the system is obtained by subtracting function (6) from function (5);

[0042]

[0043] in, represents the linearized state variable;

[0044] represents the control input after linearization;

[0045]

[0046] Step 3.2: Use the Euler method to approximate the discretization of the linearized state space model to obtain a discretized model:

[0047] A k =I+T'·A(8);

[0048] B k =T'·B(9);

[0049] In the formula, A k and B k are the discretized system matrices, I is the unit matrix, and T' is the discrete time step. Then, the state space model, the linearized state space model, and the discretized model are combined to obtain the nonlinear system at any reference point (x r ,u r ) after linearization:

[0050]

[0051] In the formula, Indicates the state at the next moment. represents the state variable at discrete time k, represents the control input vector at discrete time k; Represents the discrete system output.

[0052] In step 4, a prediction model is defined to predict future system behavior and perform dynamic optimization.

[0053] Define the new state space equation as:

[0054]

[0055] In the formula, ξ(k+1) represents the state variable in the state space at time k+1, ξ(k) represents the state variable in the state space at time k, and Δu(k) represents the predicted input at time k;

[0056] is the state matrix, I m represents the identity matrix of dimension m,

[0057] is the control matrix,

[0058] is the system control output matrix,

[0059] is the state variable in the state space; represents the control input at time k-1, t represents the time; n is the dimension of the state variable, and m is the dimension of the control variable;

[0060] In order to simplify the calculation, it is assumed that the prediction time domain N P middle:

[0061]

[0062] In the formula, A k+i , B k+i represents the system matrix at time k+i;

[0063] From equations (11) and (12), we get the predicted output expression of the control system:

[0064]

[0065] In formula (13), is the predicted input of the system, ΔU is the control time domain N C The input increment within

[0066] Represents the prediction time domain N P The predicted output within, Ψ kand θ k is the system matrix; ΔU(k) ​​represents the control time domain N C Predicted input within

[0067] Represents the predicted output at time k+1; Represents the predicted output at time k+2; represents k+N c The predicted output at the moment; represents k+N P The predicted output at the moment;

[0068] represents the expanded output matrix; represents the original state transfer matrix; represents the square of the original state transfer matrix; N represents the original state transfer matrix C Power; N represents the original state transfer matrix P Power;

[0069] Δu(k) is the input increment; Δu(k+1) is the input increment at time k+1; Δu(k+N C -1) means k+N C -1 time input increment; N C represents the control time domain;

[0070] represents the expanded input matrix; N represents the original state transfer matrix C -1 power; N represents the original state transfer matrix P -1 power; N represents the original state transfer matrix P -2 power; N represents the original state transfer matrix P -N C Power.

[0071] In step 5, the objective function is constructed based on the system state deviation, control input and control increment. During the design, both the tracking speed and the control energy consumption are considered; therefore, the objective function is defined as:

[0072]

[0073] Where J(k) represents the optimization objective function; N P represents the prediction time domain; represents the predicted output at time k+i; Δu(k+i) represents the input increment at time k+i; Q and R are weight matrices;

[0074] In order to facilitate the solution through the corresponding matrix operation, equation (14) is adjusted to the following standard quadratic objective function:

[0075]

[0076] Where J(ΔU(k)) represents the objective function of the secondary optimization of the control increment ΔU(k); H k is a Hessian matrix; Represents the coefficient of the first-order term in the objective function;

[0077] in:

[0078] In the formula, represents the transposed matrix of the system model matrix; Q e Represents the weight matrix of state error; R e represents the weight matrix of the control increment; Θ k represents the system model matrix; Ψ k represents the state prediction matrix; ξ(k) represents the current extended state vector; therefore, the optimization problem of the ship trajectory tracking problem based on linear MPC is expressed as:

[0079]

[0080] st

[0081]

[0082] ΔU min ≤ΔU(k)≤ΔU max ,

[0083] U min ≤MΔU(k)+U(k)≤U max ,

[0084]

[0085] Where: ΔU min and ΔU max They represent the minimum and maximum values ​​of the control increment in the control time domain respectively; U min and U max They represent the minimum and maximum values ​​of the control quantity in the control time domain respectively; Y min and Y max represents the minimum and maximum values ​​of the output constraints; M is the matrix that converts the control increment into the control input; Y R (k) represents the reference output.

[0086] In step 6, the improved ant colony algorithm IACO sets the initial distribution of ants in the ant colony to evenly divide the domain, and dynamically adjusts the pheromone update strategy to avoid the algorithm from converging to the local optimal solution too early. Specifically, the steps are as follows:

[0087] Step 6.1: First, determine the number of ants N in the ant colony according to the complexity of the problem to be solved; then, divide all the definition domains of the problem evenly into N small intervals, each small interval corresponds to an ant i; place each ant in the center of the corresponding small interval, so that each ant has an initial search position; each ant also has a movable search interval, the length of which is the same as the length of each small interval after the definition domain is divided into N equal parts; when all ants are in the center of their respective search intervals, there is only one ant in each interval; as the ant moves, its search interval will be adjusted according to the degree of overlap with the adjacent interval, affecting the estimation of the number of ants in the adjacent interval;

[0088] Step 6.2: According to the above definition, if the domain of the problem is [u start ,u end ], when the number of ant colonies is N, the length of the subintervals on each axis is:

[0089]

[0090] Where: D u represents the length of the moving sub-interval of the ant on one axis; u end Indicates the end point of the moving interval; u start Indicates the starting point of the moving interval; N indicates the number of subintervals;

[0091] From formula (17), we can see that the length of each ant's moving sub-interval on one axis is D u , and the initial coordinate distribution of the ant colony is:

[0092]

[0093] Where: u i represents the coordinates of the ant on the i-th subinterval of an axis;

[0094] The left and right boundaries of the subinterval i are:

[0095] u iL =u start +(i-1)D u (19);

[0096] u iR =u start +iD u (20);

[0097] In the above formula: u iL represents the lower boundary of the ith subinterval; u iR represents the upper boundary of the i-th subinterval;

[0098] Step 6.3: During the execution of the algorithm, the update of pheromones can dynamically adjust the search range, so that the algorithm can adjust the exploration and utilization of different ranges according to the current search situation; design the following pheromone update formula:

[0099]

[0100] Where: Δτ i (t) represents the pheromone change at point i; Q i F is the pheromone released by the intelligent ant at point i; max is the maximum value of all objective function values ​​at time t; F i is the objective function value at point i; ξ is the adjustment constant.

[0101] In step 6.3, a dynamically adjusted confidence factor is introduced to balance the credibility of pheromones, thereby improving the algorithm's solution performance; specifically, the following steps are included:

[0102] Step 6.3.1: A dynamically changing pheromone strategy is used to ensure that the algorithm can continuously explore better solutions and avoid premature locking on non-global optimal solutions. The expression is:

[0103] Q i =γQ (22);

[0104] Where: Q i represents the pheromone released by the intelligent ant at point i; γ is the confidence factor; Q represents the initial pheromone;

[0105] Step 6.3.2: Introduce a confidence factor γ, which determines the credibility of the pheromone left by the ants; use a piecewise function to dynamically adjust the value of γ to balance the credibility of the pheromone. This piecewise function is:

[0106]

[0107] Where: N represents the maximum number of iterations; n represents the current number of iterations;

[0108] In summary, the dynamic pheromone update formula is as follows:

[0109] τ i (t+1)=(1-ρ)τ i (t)+Δτ i (t) (24);

[0110] In the formula, τi (t+1) is the pheromone concentration at point i at time t+1; ρ is the pheromone volatility factor; τ i (t) is the pheromone concentration at point i at time t; Δτ i (t) represents the pheromone change at point i.

[0111] In step 6, local search and global search are performed according to the state transition probability to improve the optimization efficiency; the specific steps include the following:

[0112] The transfer probability is determined by the pheromone concentration, and the formula is as follows:

[0113]

[0114] Among them, max(τ(t)) represents the maximum value of the pheromone in the tth iteration, τ i (t) represents the pheromone at point i, P i (t) represents the state transition probability of point i;

[0115] When the state transition probability is less than the transition probability constant, a local search is performed, and the search formula is:

[0116]

[0117] Among them, u new is the position to be moved, u old is the current position of the ant, r1 is a random number between [-1,1], and step is the local search step length;

[0118] When the state transition probability is greater than the transition probability constant, a global search is performed, and the search formula is:

[0119] u new =u old +r2·range(27);

[0120] Among them: r2 is a random number between [-0.5,0.5], and range is the interval size of the independent variable.

[0121] The present invention provides a method for tracking and controlling an underwater robot trajectory, which has the following technical effects:

[0122] 1) Step 1 of the present invention has the following advantages:

[0123] 1. Coordinate system separation for easy analysis: By defining the speed and position in the carrier coordinate system and the inertial coordinate system respectively, the robot motion is decomposed into components such as forward speed, lateral drift speed and bow pitch speed, which makes it easier to understand and analyze the motion characteristics of the underwater robot.

[0124] 2. Clear coordinate transformation relationship: The transformation matrix from the carrier coordinate system to the inertial coordinate system is defined, so that data in different coordinate systems can be accurately converted. This is particularly critical for motion control in complex environments, because physical quantities in different coordinate systems often need to be converted to each other.

[0125] 3. Unified definition of velocity and position vectors: The velocity vector in the carrier coordinate system and the position vector in the inertial coordinate system are defined as matrices respectively, which facilitates matrix operations in the control algorithm. This standardized processing can simplify the subsequent establishment of motion equations and controller design.

[0126] 4. Provide a basis for control algorithm design: Clear coordinate transformation and speed and position representation provide a basis for control algorithms. This processing method enables controllers based on state feedback, model predictive controllers, etc. to better adapt to changing motion requirements.

[0127] 5. Enhance the versatility and expansibility of the system: By defining the motion model and coordinate transformation matrix in this way, it can be easily extended to other types of underwater robots or different motion modes. This definition method is universal and expansible.

[0128] 2) Step 2 of the present invention has the following advantages:

[0129] 1. Considering the influence of multiple forces on AUV movement: The model includes the influence of various forces such as thrust, interference force, hydrodynamic force and hydrostatic force provided by the power system, and fully reflects the complex force conditions of AUV underwater. This multi-force comprehensive modeling method makes the model closer to the actual situation.

[0130] 2. Simplified modeling for easy control: The propeller and rudder drive models and their coupling relationships were ignored during model construction, and force and torque were used as control inputs and position as control outputs. This simplified processing reduces the complexity of the model, making it easier to design and calculate, and facilitating the design of control algorithms.

[0131] 3. Adapt to nonlinear characteristics and improve trajectory tracking performance: AUVs are subject to complex forces when moving underwater, and trajectory tracking exhibits obvious nonlinear characteristics. Using a nonlinear state space model to describe the motion characteristics of AUVs can more accurately reflect their nonlinear behavior, which helps to improve the accuracy and stability of trajectory tracking.

[0132] 4. Ignore the influence of details and focus on the main dynamic characteristics: Ignoring the influence of propeller and rudder coupling, the model focuses more on the hydrodynamic characteristics of the AUV, simplifies the calculation burden of control design, and ensures the accurate description of the main dynamic characteristics. This processing method improves the calculation efficiency of the model while ensuring the accuracy of the model.

[0133] 5. Adapt to under-actuated characteristics: AUV is an under-actuated system, and traditional heading control methods are difficult to achieve accurate trajectory tracking. This model compensates for the under-actuated characteristics through hydrodynamic control, ensuring the trajectory tracking capability of AUV in complex environments.

[0134] 6. Provide a basis for real-time trajectory tracking: This model not only considers the relative motion of the AUV in the water flow, but also defines the system state variables, control inputs, system inputs, etc., providing a basis for the actual real-time trajectory tracking algorithm. This state-space-based modeling method enables the system to respond to changes in the target trajectory in real time.

[0135] 3) Step 3 of the present invention has the following advantages:

[0136] 1. Easy to apply the MPC framework: MPC usually performs calculations based on linear models to solve the optimal solution to predictive control problems. By linearizing the nonlinear model, the nonlinear system can be converted into a linear discrete system that can be processed by MPC, so that complex AUV control problems can be solved under the MPC framework.

[0137] 2. Reduce computational complexity: Nonlinear models usually bring complex optimization calculations, while linearization can simplify model expression and reduce the complexity of solving MPC optimization problems, thereby achieving faster real-time calculations. This is especially important for AUV trajectory tracking control, which has high real-time requirements.

[0138] 3. Discretization facilitates numerical realization: The discretized model can be directly numerically calculated on the computer, which is suitable for actual digital control systems. Underwater robot control systems usually rely on digital controllers. Discretization can better adapt to the sampling period of the controller, thereby improving control accuracy.

[0139] 4. Local linearization provides appropriate approximation: Although the motion model of the AUV is highly nonlinear, a relatively accurate local dynamic behavior can be obtained through approximate processing within the linearization range. Especially in short-term predictions, this approximation is usually sufficient and can effectively reflect the local dynamic characteristics of the AUV.

[0140] 5. Suitable for the design of incremental control input: The linearized model is convenient for the design of incremental control input, so that it can better cope with complex environmental changes and improve the adaptability of trajectory tracking. MPC has advantages in incremental control, and discretization and linearization make incremental control more convenient.

[0141] 6. Improve the robustness and stability of the system: By using linearized models in the MPC framework, the stability and robustness of the system can be better analyzed. The predictive ability of MPC combined with linearization processing can effectively deal with model uncertainties and external disturbances in a small range, thereby enhancing the control performance of the system.

[0142] 7. Easy to handle constraints: MPC can easily handle linear constraints. By linearizing the model, constraints (such as speed, acceleration, and control force constraints) can be introduced more accurately in the control optimization, ensuring that the control input and output of the AUV operate within the physical constraints.

[0143] 4) Step 4 of the present invention has the following advantages:

[0144] 1. Improve prediction accuracy: By defining a prediction model, MPC can predict the system behavior at multiple future moments based on the current state at each control moment. This forward-looking prediction enables the controller to accurately track the predetermined trajectory and make advance adjustments to impending deviations, thereby improving control accuracy.

[0145] 2. Achieve dynamic optimization: The prediction model supports dynamic optimization, which enables MPC to adjust control inputs in real time and optimize system performance indicators (such as energy consumption, speed, and path error). Dynamic optimization ensures that the AUV (autonomous underwater robot) can operate stably and efficiently in a rapidly changing environment.

[0146] 3. Handling system constraints: MPC’s prediction model can take into account the physical constraints of the AUV (such as speed, acceleration, control force, etc.) during the optimization process. This ensures that the control input and system output are always within a safe range, avoiding over-control and instability, and improving the control safety and reliability of the AUV.

[0147] 4. Simplified optimization calculation: The definition of the prediction model and the expansion of the state space equations make it possible to simplify complex dynamic equations into linear prediction expressions, which facilitates matrix operations and optimization calculations in the MPC framework. This simplification process greatly reduces the computational complexity, making MPC more suitable for real-time applications.

[0148] 5. Ensure system stability: Through predictive models and dynamic optimization, MPC can balance system stability and performance optimization during the control process. The forward-looking control design of the predictive model can reduce trajectory deviation and oscillation, thereby improving the overall stability of the system.

[0149] 6. Design of extended prediction time domain: The definition of the prediction model allows MPC to make multi-step predictions within the extended prediction time domain to optimize the control input. This design provides a longer-term control strategy for the control system, allowing the AUV to continuously and smoothly follow the target trajectory under the prediction input at multiple moments.

[0150] 7. Convenient multi-step rolling optimization: Through the prediction model, MPC can dynamically adjust the control input based on a rolling optimization strategy (i.e., re-optimization at each step). This rolling optimization method ensures the optimality of each control moment, allowing the AUV to always follow the target in the optimal path.

[0151] 5) Step 5 of the present invention has the following advantages:

[0152] 1. Improve trajectory tracking accuracy: By introducing system state deviation in the objective function, the AUV is ensured to accurately follow the predetermined trajectory. Minimizing the state deviation helps reduce the error of the AUV deviating from the target path, thereby achieving higher trajectory tracking accuracy.

[0153] 2. Optimize control energy consumption: The objective function includes control input and control increment terms, so that the efficiency of control energy can be taken into account during the optimization process. This can reduce the energy consumption of the AUV during trajectory tracking, thereby extending battery life and improving mission execution time, which is particularly suitable for long-term missions or deep-sea operations.

[0154] 3. Balance speed and stability: The design of the objective function takes tracking speed into account, so that the control system can respond to trajectory changes more quickly and ensure that the AUV can follow the predetermined trajectory at a higher speed. At the same time, by balancing speed and energy consumption, control instability caused by the pursuit of speed can be avoided, achieving fast and stable control effects.

[0155] 6) Step 6 of the present invention has the following advantages:

[0156] 1. Balance local and global search: The local and global search switching mechanism based on state transition probability can effectively prevent the algorithm from falling into the local optimum. Local search is triggered when the state transition probability is small, and it can explore carefully around the solution to ensure that potential high-quality solutions are fully developed; when the state transition probability is large, global search will explore a wider solution space to help jump out of the local optimum.

[0157] 2. Improve search efficiency: The pheromone concentration controls the state transition probability, thereby adaptively adjusting the frequency of local and global searches. Dynamic adjustment of pheromone concentration can make the search process more efficient, allowing the algorithm to converge to a high-quality solution with fewer iterations.

[0158] 3. Enhance the diversity of solutions: Global search can increase the diversity of solutions through a larger range of exploration, thereby increasing the possibility of finding the global optimal solution. Local search helps to find a more accurate solution near the solution. Such a design can effectively improve the global optimization ability of the algorithm.

[0159] 4. Strong adaptability: By setting the transition probability constant, the ratio of local and global search can be flexibly adjusted according to the specific characteristics of the problem. For optimization problems of different complexity, the transition probability constant can be adjusted to adapt to different search requirements, ensuring that the algorithm exhibits good optimization performance in a variety of scenarios.

[0160] 5. Control the search step size: The setting of local search step size and global search interval size makes the search process more controllable. The local search step size can ensure detailed local optimization, while the global search interval size ensures the breadth of the search. This step size control mechanism can effectively balance the accuracy and scope of the search and improve the optimization effect.

[0161] 7) Step 7 of the present invention has the following advantages:

[0162] 1. Optimize control accuracy: The optimal control increment obtained by IACO in each control cycle makes the control decision more accurate, reduces error accumulation, and ensures that the underwater robot can accurately run along the predetermined trajectory.

[0163] 2. Enhance the robustness of the system: MPC uses a predictive model to continuously update the optimal control increment in the control domain, allowing the system to respond quickly to trajectory deviations under disturbances. Through a rolling optimization strategy, the control algorithm can be dynamically adjusted to maintain a stable trajectory tracking effect even if the environment or system parameters change.

[0164] 3. Improve the stability of path tracking: By solving and applying the optimal control increment in each control cycle, the optimal trajectory is gradually approached, which reduces the drastic changes in control input, increases the smoothness and stability of control, and makes the robot smoother in the trajectory tracking process.

[0165] 4. Easy to deal with complex constraints: The IACO optimization algorithm can effectively handle multi-constraint optimization problems and more flexibly deal with complex physical constraints and environmental interference during the control process, thereby improving the reliability and accuracy of trajectory tracking.

[0166] 5. Applicable to multi-step control requirements: IACO can be adaptively adjusted according to the requirements of different control cycles, thus being applicable to multi-step control requirements, further improving the application breadth and adaptability of the algorithm. BRIEF DESCRIPTION OF THE DRAWINGS

[0167] The present invention will be further described below in conjunction with the accompanying drawings and embodiments:

[0168] Figure 1 It is a schematic diagram of the AUV motion model in the present invention.

[0169] Figure 2(a) shows the function f1 and its convergence curve.

[0170] Figure 2(b) shows the function f2 and its convergence curve.

[0171] Figure 2(c) shows the function f3 and its convergence curve.

[0172] Figure 2(d) shows the function f4 and its convergence curve.

[0173] Figure 3 It is the nonlinear interference force curve.

[0174] Figure 4(a) is the trajectory tracking result curve.

[0175] Figure 4(b) is the ship heading angle curve.

[0176] Figure 4(c) is error curve 1.

[0177] Figure 4(d) is the second error curve. DETAILED DESCRIPTION

[0178] A trajectory tracking control method for an underwater robot establishes a motion model of the underwater robot, defines a motion coordinate system and uses six degrees of freedom to describe its motion, including forward movement, lateral drift, bow pitch, vertical heave, roll and pitch. By establishing a nonlinear state space model, the effects of multiple forces on the underwater robot are comprehensively considered, and the influence of the drive model of the propeller and rudder is ignored.

[0179] This paper proposes a trajectory tracking method that integrates improved ant colony algorithm and model predictive control (Improved Ant Colony Optimizer-Based Model Predictive Control, IACO-MPC), which aims to overcome the limitations of existing research and improve the mission execution capability of AUV in complex underwater environments. By introducing dynamically adjusted confidence factors and transition probability constants into the traditional ant colony algorithm and improving the initial distribution design of the ant colony, the optimization performance of the ant colony algorithm is enhanced. This innovation applies the improved IACO algorithm to the rolling optimization process of MPC, thereby improving the trajectory tracking stability and accuracy of the AUV.

[0180] A method for tracking and controlling an underwater robot trajectory comprises the following steps:

[0181] Step 1: Establish AUV motion model;

[0182] Step 2: Establish a nonlinear state space model;

[0183] Step 3: Linearize and discretize the nonlinear state space model;

[0184] Step 4: Define a prediction model to predict system behavior and perform dynamic optimization;

[0185] Step 5: Design the objective function based on model predictive control (MPC);

[0186] Step 6: Introduce the improved ant colony algorithm IACO and solve the objective function by dynamically adjusting the confidence factor and the transition probability constant;

[0187] Step 7: The control input is updated in each control cycle through the feedback mechanism to ensure that the robot completes accurate trajectory tracking.

[0188] In step 1, the AUV motion model is established, such as Figure 1 As shown in the figure, for the AUV motion model, the forward speed, lateral drift speed and bow rolling speed of the underwater robot in the carrier coordinate system {b} can be represented by u, v and r respectively, and the x-axis position, y-axis position and bow direction of the underwater robot in the inertial coordinate system {n} are represented by x, y and ψ respectively;

[0189] The present invention defines the velocity vector of the carrier coordinate system as v'=[u,v,r] T , T represents the transposed matrix, and the position and heading vector in the inertial coordinate system are defined as η = [x, y, ψ] T Based on this definition, the motion coordinate transformation equation of the underwater robot can be established:

[0190]

[0191] in: is the derivative of η, T(ψ) is the transformation matrix from the velocity of the carrier coordinate system {b} to the velocity of the inertial coordinate system {n}; it can be expressed as:

[0192]

[0193] In the step 2, a nonlinear state space model is established, which specifically includes:

[0194] When AUV is navigating underwater, it is affected by multiple forces, including the thrust provided by the power system, interference force, hydrodynamic force, and hydrostatic force. The combined influence of these forces causes the trajectory tracking model of AUV to show obvious nonlinear characteristics.

[0195] AUV trajectory tracking is a technology that tracks the path of a moving target in real time. It has high requirements on the maneuverability of the underwater robot. Accurate trajectory tracking cannot be achieved by simply relying on heading control, so a hydrodynamic model is usually used for trajectory tracking control. To simplify the model design, the hydrodynamic model ignores the influence of the propeller and rudder drive model and their coupling relationship when constructing it. It only uses force and torque as control input and position as control output. In view of the under-driven characteristics of AUV, it is necessary to establish a hydrodynamic model for the front-drive AUV:

[0196]

[0197] Where: M RB is the rigid body inertia matrix; M A is the additional mass matrix; represents the acceleration of the AUV, represents the relative acceleration of the AUV and the water flow in the carrier coordinate system, C RB (v) is the centrifugal force matrix; C A (v r ) is the centrifugal force matrix generated by the rotation of the carrier coordinate system, v r represents the relative speed between the AUV and the water flow in the carrier coordinate system, D(v r ) is the damping matrix; τ is the propulsion force of the AUV; τ d Environmental interference;

[0198] f u ,t r They are the forward force and bow moment generated by the AUV propeller respectively;

[0199] f uw ,f vw ,f rw They are respectively the forward force, lateral drift force and bow rolling moment generated by environmental interference.

[0200] Assume that the system state variable is x = [x, y, ψ, u, v, r] T , the control input is u=τ=[f u ,t r ] T , the system input is y = [x, y] T , thus the nonlinear state space model of AUV motion can be expressed as follows:

[0201]

[0202] y=P2x

[0203] in, represents the rate of change of the system state x, and f(x,u) describes the dynamic characteristics of the system, which accepts the current system state x and the system control input u as parameters and outputs

[0204] In step 3, the nonlinear state space model is linearized and discretized so as to apply a model predictive control (MPC) framework; the following steps are included:

[0205] Step 3.1: There are many ways to transform a nonlinear system into a linear time-varying system, which can be divided into two categories: exact linearization and approximate linearization. Because the exact linearization method lacks universality, it usually requires customized analysis for a specific system. Although the approximate method lacks accuracy, it is easy to operate and has strong applicability. Therefore, in the field of model predictive control (MPC), the approximate linearization method is usually used. This assumes that the reference system is already running along the desired trajectory. By obtaining the state and control information at each time point on the desired trajectory, and then processing the deviation between the reference system and the current system, a model predictive controller that can track the desired trajectory is designed. The equation of the reference system is as follows:

[0206]

[0207] in, Represents the reference state variable x r The rate of change, f(x r ,u r ) indicates acceptance of the current reference state x r and the reference control input u r As a parameter and output

[0208] At any reference point (x r ,u r ) Performing a first-order Taylor expansion on function (5) yields:

[0209]

[0210] Finally, the linearized state space model of the system can be obtained by subtracting function (6) from function (5).

[0211]

[0212] in, represents the linearized state variable;

[0213] represents the control input after linearization;

[0214]

[0215] Step 3.2: The present invention uses the Euler method to perform approximate discretization processing on the linearized state space model, thereby obtaining a discretized model:

[0216] A k =I+T'·A(8);

[0217] B k =T'·B(9);

[0218] In the formula, A k and B k are the discretized system matrices, I is the unit matrix, and T' is the discrete time step. Then, by combining the state space model, the linearized state space model, and the discretized model, we can get the nonlinear system at any reference point (x r ,u r ) after linearization:

[0219]

[0220] In the formula, Indicates the state at the next moment. represents the state variable at discrete time k, represents the control input vector at discrete time k; Represents the discrete system output.

[0221] In step 4, a prediction model is defined to predict future system behavior, and dynamic optimization is performed to ensure that the underwater robot can quickly and stably follow the predetermined trajectory;

[0222] MPC relies on predictions of future system behavior to make optimal control decisions in the present. Predictive models enable MPC to take constraints into account, optimize system performance, ensure stability, and improve the robustness of the control strategy. It allows the controller to respond in real time and adapt to system dynamics while providing flexibility to adjust control objectives and constraints. Predictive models are key to MPC's efficient, forward-looking control. Therefore, the new state space equation is defined as:

[0223]

[0224] In the formula, ξ(k+1) represents the state variable in the state space at time k+1, ξ(k) represents the state variable in the state space at time k, and Δu(k) represents the predicted input at time k;

[0225] is the state matrix, I m represents the identity matrix of dimension m;

[0226] is the control matrix;

[0227] Output matrix for system control;

[0228] is the state variable in the state space; represents the control input at time k-1;

[0229] t represents the time; n is the dimension of the state variable, and m is the dimension of the control variable.

[0230] In order to simplify the calculation, it is assumed that the prediction time domain N P middle:

[0231]

[0232] In the formula, A k+i , B k+i represents the system matrix at time k+i;

[0233] From equations (11) and (12), we can get the predicted output expression of the control system:

[0234]

[0235] In formula (13), is the predicted input of the system, ΔU is the control time domain N C The input increment within

[0236] Represents the prediction time domain N P The predicted output within, Ψ k and θ k is the system matrix; ΔU(k) ​​represents the control time domain N C Predicted input within

[0237] Represents the predicted output at time k+1; Represents the predicted output at time k+2; represents k+N c The predicted output at the moment; represents k+N P The predicted output at the moment;

[0238] represents the expanded output matrix; represents the original state transfer matrix; represents the square of the original state transfer matrix; N represents the original state transfer matrix C Power; N represents the original state transfer matrix P Power;

[0239] Δu(k) is the input increment; Δu(k+1) is the input increment at time k+1; Δu(k+N C -1) means k+N C -1 time input increment; N C represents the control time domain;

[0240] represents the expanded input matrix; N represents the original state transfer matrix C -1 power; N represents the original state transfer matrix P -1 power; N represents the original state transfer matrix P -2 power; N represents the original state transfer matrix P -N C Power;

[0241] In step 5, in order to ensure that the AUV can quickly and stably follow the predetermined trajectory, the objective function is constructed based on the system state deviation, control input and control increment. During the design, the tracking speed and control energy consumption are considered at the same time. Therefore, the objective function can be defined as:

[0242]

[0243] Where J(k) represents the optimization objective function; N P represents the prediction time domain; represents the predicted output at time k+i; Δu(k+i) represents the input increment at time k+i;

[0244] Q and R are weight matrices, and their values ​​can be adjusted according to the specific requirements of the control task to change the weight of each control objective.

[0245] To facilitate the solution, after corresponding matrix operations, equation (14) can be adjusted to the following standard quadratic objective function:

[0246]

[0247] Where J(ΔU(k)) represents the objective function of the secondary optimization of the control increment ΔU(k); H k is a Hessian matrix; Represents the coefficient of the first-order term in the objective function;

[0248] in:

[0249] In the formula, represents the transposed matrix of the system model matrix; Q e Represents the weight matrix of state error; R e represents the weight matrix of the control increment; Θ k represents the system model matrix; Ψ k represents the state prediction matrix; ξ(k) represents the current extended state vector;

[0250] Therefore, the optimization problem of the ship trajectory tracking problem based on linear MPC can be expressed as:

[0251]

[0252] st

[0253]

[0254] ΔU min ≤ΔU(k)≤ΔU max ,

[0255] U min ≤MΔU(k)+U(k)≤U max ,

[0256]

[0257] Where: ΔU min and ΔU max They represent the minimum and maximum values ​​of the control increment in the control time domain respectively; U min and U max They represent the minimum and maximum values ​​of the control quantity in the control time domain respectively; Y min and Y max represents the minimum and maximum values ​​of the output constraints; M is the matrix that converts the control increment into the control input; Y R (k) represents the reference output.

[0258] In step 6, the improved ant colony algorithm IACO sets the initial distribution of ants in the ant colony to evenly divide the domain, and dynamically adjusts the pheromone update strategy to avoid the algorithm from converging to a local optimal solution too early, specifically including the following steps:

[0259] Step 6.1: First, according to the complexity of the problem to be solved by the present invention, determine the number N of ants in the ant colony. Then, divide all possible solutions (domain) of the problem evenly into N small intervals, each of which corresponds to an ant i. Place each ant in the center of the corresponding small interval so that each ant has an initial search position. Each ant also has a movable search interval, the length of which is the same as the length of each small interval after the domain is divided into N equal parts. When all ants are at the center of their respective search intervals, there is only one ant in each interval. As the ants move, their search intervals will be adjusted according to the degree of overlap with adjacent intervals, affecting the estimation of the number of ants in adjacent intervals.

[0260] Step 6.2: According to the above definition, if the domain of the problem is [u start ,u end ], when the number of ant colonies is N, the length of the subintervals on each axis is:

[0261]

[0262] Where: D u represents the length of the moving sub-interval of the ant on one axis; u end Indicates the end point of the moving interval; u start Indicates the starting point of the moving interval; N indicates the number of sub-intervals.

[0263] From the above, we can see that the length of each ant's moving sub-interval on one axis is D u , and the initial coordinate distribution of the ant colony is:

[0264]

[0265] Where: u i Represents the coordinates of the ant on the i-th subinterval of an axis.

[0266] The left and right boundaries of the subinterval i are:

[0267] u iL =u start +(i-1)D u (19);

[0268] u iR =u start +iD u (20);

[0269] In the above formula: u iL represents the lower boundary of the ith subinterval; u iR represents the upper boundary of the i-th subinterval;

[0270] Step 6.3: During the execution of the algorithm, the update of pheromones can dynamically adjust the search range, so that the algorithm can adjust the exploration and utilization of different ranges according to the current search situation. Design the following pheromone update formula:

[0271]

[0272] Where: Δτ i (t) represents the pheromone change at point i; Q i F is the pheromone released by the intelligent ant at point i; max is the maximum value of all objective function values ​​at time t; F i is the objective function value at point i; ξ is the adjustment constant.

[0273] In step 6.3, a dynamically adjusted confidence factor is introduced to balance the credibility of pheromones, thereby improving the algorithm's solution performance. The specific steps include the following:

[0274] Step 6.3.1: In order to ensure that the intelligent ants will not fall into the local optimum due to excessive reliance on pheromones in the early stage of training, the present invention adopts a dynamically changing pheromone strategy to ensure that the algorithm can continuously explore better solutions and avoid premature locking on non-global optimal solutions. The expression is:

[0275] Q i =γQ (22);

[0276] Where: Q i represents the pheromone released by the intelligent ant at point i; γ is the confidence factor; Q represents the initial pheromone.

[0277] Step 6.3.2: In this model, the present invention introduces a confidence factor γ, which determines the credibility of the pheromone left by the ants. Since the number of explorations by the ant colony is limited in the early stage of training, the concentration of pheromones may not accurately reflect the path of the optimal solution. Therefore, the present invention uses a piecewise function to dynamically adjust the value of γ to balance the credibility of the pheromone and ensure that the algorithm does not lock on the local optimal solution too early. This piecewise function is:

[0278]

[0279] Where: N represents the maximum number of iterations; n represents the current number of iterations.

[0280] In summary, the dynamic pheromone update formula can be obtained as follows:

[0281] τ i (t+1)=(1-ρ)τ i (t)+Δτ i (t) (24);

[0282] In the formula, τ i (t+1) is the pheromone concentration at point i at time t+1; ρ is the pheromone volatility factor; τ i (t) is the pheromone concentration at point i at time t; Δτ i (t) represents the pheromone change at point i.

[0283] In step 6, local search and global search are performed according to the state transition probability to improve the optimization efficiency. The specific steps include:

[0284] The transfer probability is determined by the pheromone concentration, and the formula is as follows:

[0285]

[0286] Among them, max(τ(t)) represents the maximum value of the pheromone in the tth iteration, τ i (t) represents the pheromone at point i, P i (t) represents the state transition probability of point i.

[0287] When the state transition probability is less than the transition probability constant, a local search is performed, and the search formula is:

[0288]

[0289] Among them, u new is the position to be moved, u old is the current position of the ant, r1 is a random number between [-1,1], and step is the local search step.

[0290] When the state transition probability is greater than the transition probability constant, a global search is performed, and the search formula is:

[0291] u new =u old +r2·range(27);

[0292] Among them: r2 is a random number between [-0.5,0.5], and range is the interval size of the independent variable.

[0293] The step 7 specifically includes:

[0294] In each control cycle, the improved ant colony algorithm IACO is used to solve equation (16) and obtain a series of control increments in the control time domain:

[0295]

[0296] in: represents the optimal control increment vector at time k; represents the optimal control increment at time k; represents the optimal control increment at time k+1; Indicates that at time k+N c The optimal control increment is -1.

[0297] According to the basic principle of MPC, the first element in the control sequence output by the optimization model is used as the actual control input increment in the system:

[0298]

[0299] The above process is repeated in each new control cycle until the robot completes trajectory tracking control.

[0300] Verification example:

[0301] In order to verify the effectiveness of the model and algorithm, the present invention carried out simulation experiments.

[0302] The specific steps are as follows:

[0303] Step a: In order to test the optimization capability of the improved ant colony algorithm proposed in the present invention, four test functions with different characteristics in Table 1 are used to perform optimization tests.

[0304] Table 1 Test functions

[0305]

[0306] Step b: In order to verify the optimization performance of the IACO algorithm of the present invention, the IACO algorithm, the ACO algorithm and the EIWOA algorithm proposed in reference

[22] are optimized and compared by experiments on different types of test functions. In the simulation test, the population size of each algorithm is set to 300, the maximum number of iterations is set to 100, the pheromone volatility factor is set to ρ = 0.9, the transition probability constant is P0 = 0.2, and the local search step is step = 0.05. Each algorithm is independently run 10 times on different test functions, and the average and standard deviation results are shown in Table 2.

[0307] Table 2 Algorithm optimization comparison results

[0308]

[0309] The test results in Table 2 show that the IACO algorithm proposed in the present invention shows the best optimization effect on the four test functions, successfully finds the theoretical global optimal value of zero for all functions, and the optimization success rate reaches 100%. In addition, the IACO algorithm obtains a lower standard deviation in the optimization than the other two algorithms, indicating that it has significant advantages in stability and robustness.

[0310] Step c: In order to more intuitively demonstrate the superior optimization capability of the IACO algorithm, the convergence curves of some test functions are shown in Figure 2. Figure 2(a) to Figure 2(d) As shown. Figure 2(a) to Figure 2(d) It can be seen that among the test functions selected by the present invention, f1 can evaluate the ability of the algorithm to find the optimal solution in a complex and narrow search space. f2 tests the ability of the algorithm to find the global optimal solution in the case of multiple local minima. f3 tests the ability of the algorithm to accurately locate the local and global optimal solutions in a function with multiple extreme points. f4 tests the robustness and global search ability of the algorithm when dealing with non-smooth and nonlinear characteristics.

[0311] from Figure 2(a) to Figure 2(d) It can be seen that compared with ACO and EIWOA, the fitness value convergence curve of IACO shows a faster convergence speed and fewer iterations are required to reach a stable state. In addition, the function fitness value obtained by IACO after stabilization is lower, indicating that it has higher convergence accuracy and faster convergence speed. According to the verification results of the above simulation experiments, the following conclusions can be drawn: By introducing dynamically adjustable confidence factors and transition probability constants, and adopting a new ant colony initial distribution partitioning strategy, the IACO algorithm significantly enhances the optimization performance of the ACO algorithm. Overall, the IACO algorithm can find high-quality initial solutions more quickly and performs well in optimization speed, accuracy, and stability.

[0312] Step 4: In order to better verify the advantages of the IACO-MPC algorithm proposed in the present invention in trajectory tracking control, it is compared with the ACO-MPC and EIWOA-MPC algorithms to evaluate the robustness of the algorithm under the influence of disturbance forces such as water flow. Figure 3 The interference force shown in the figure is realized by applying IACO-MPC, ACO-MPC and EIWOA-MPC algorithms to realize the path following control for the initial point at (2,2); the end point at (44.44); passing through (2,44);, (14,44);, (14,2);, (24,2);, (34,44);, (44,2);.

[0313] Table 3 Trajectory tracking results of different control algorithms

[0314]

[0315] Figure 4(a) shows the trajectory tracking effects of different control strategies. It can be seen from the simulation results of Figure 4(b), Figure 4(c), and Figure 4(d) that the IACO-MPC control algorithm proposed in the present invention is superior to other control algorithms in terms of tracking speed and stability. All kinds of control strategies can basically successfully track the given trajectory. It can be seen from Table 3 that the IACO-MPC algorithm proposed in the present invention reduces the tracking error in the x direction by 86.6592% and 70.9756% respectively compared with the ACO-MPC algorithm and the EIWOA algorithm, and the average tracking error in the y direction is reduced by 31.0924% and 60.1942% respectively, and the average tracking error is reduced by 84.8698% and 69.7725% respectively. Therefore, the simulation experiment results show that the IACO-MPC trajectory tracking algorithm proposed in the present invention performs better in tracking accuracy.

Claims

1. A trajectory tracking control method for an underwater robot, characterized in that: The following steps are involved: Step 1: Establish AUV motion model; Step 2: Establish a nonlinear state space model; Step 3: Linearize and discretize the nonlinear state space model; Step 4: Define a prediction model to predict system behavior and perform dynamic optimization; Step 5: Design the objective function based on model predictive control; Step 6: Introduce the improved ant colony algorithm IACO and solve the objective function by dynamically adjusting the confidence factor and the transition probability constant; Step 7: The control input is updated in each control cycle through the feedback mechanism to ensure that the robot completes accurate trajectory tracking.

2. The underwater robot trajectory tracking control method according to claim 1, characterized in that: In step 1, for the AUV motion model, the forward speed, lateral drift speed and bow rolling speed of the underwater robot in the carrier coordinate system {b} are represented by u, v and r respectively, and the x-axis position, y-axis position and bow direction of the underwater robot in the inertial coordinate system {n} are represented by x, y and ψ respectively; The velocity vector of the carrier coordinate system is defined as v' = [u, v, r] T , T represents the transposed matrix, and the position and heading vector in the inertial coordinate system are defined as η = [x, y, ψ] T Based on this definition, the motion coordinate transformation equation of the underwater robot is established: in: is the derivative of η, T(ψ) is the transformation matrix from the velocity of the carrier coordinate system {b} to the velocity of the inertial coordinate system {n}; it is expressed as:

3. The underwater robot trajectory tracking control method according to claim 1, characterized in that: In the step 2, a nonlinear state space model is established, which specifically includes: Establish a hydrodynamic model for the front-drive AUV: Where: M RB is the rigid body inertia matrix; M A is the additional mass matrix; represents the acceleration of the AUV, represents the relative acceleration of the AUV and the water flow in the carrier coordinate system, C RB (v) is the centrifugal force matrix; C A (v r ) is the centrifugal force matrix generated by the rotation of the carrier coordinate system, v r represents the relative speed between the AUV and the water flow in the carrier coordinate system, D(v r ) is the damping matrix; τ is the propulsion force of the AUV; τ d Environmental interference; f u ,t r They are the forward force and bow moment generated by the AUV propeller respectively; f uw ,f vw ,f rw They are the forward force, drift force and bow rolling moment generated by environmental disturbances; Assume that the system state variable is x = [x, y, ψ, u, v, r] T , the control input is u=τ=[f u ,t r ] T , the system input is y = [x, y] T , thus the nonlinear state space model of AUV motion can be expressed as follows: y=P2x in, represents the rate of change of the system state x, and f(x,u) describes the dynamic characteristics of the system, which accepts the current system state x and the system control input u as parameters and outputs 4. The underwater robot trajectory tracking control method according to claim 1, characterized in that: In step 3, the nonlinear state space model is linearized and discretized, including the following steps: Step 3.1: Using the approximate linearization method, assume that the reference system runs along the desired trajectory. By obtaining the state and control information at each time point on the desired trajectory, and then processing the deviation between the reference system and the current system, a model predictive controller that can track the desired trajectory is designed; the equation of the reference system is as follows: in, Represents the reference state variable x r The rate of change, f(x r ,u r ) indicates acceptance of the current reference state x r and the reference control input u r As a parameter and output At any reference point (x r ,u r ) is used to perform a first-order Taylor expansion on function (5), and we obtain: Finally, the linearized state space model of the system is obtained by subtracting function (6) from function (5); in, represents the linearized state variable; represents the control input after linearization; Step 3.2: Use the Euler method to approximate the discretization of the linearized state space model to obtain a discretized model: A k =I+T'·A(8); B k =T'·B(9); In the formula, A k and B k are the discretized system matrices, I is the unit matrix, and T' is the discrete time step. Then, the state space model, the linearized state space model, and the discretized model are combined to obtain the nonlinear system at any reference point (x r ,u r ) after linearization: In the formula, Indicates the state at the next moment. represents the state variable at discrete time k, represents the control input vector at discrete time k; Represents the discrete system output.

5. The underwater robot trajectory tracking control method according to claim 1, characterized in that: In step 4, a prediction model is defined to predict future system behavior and perform dynamic optimization. Define the new state space equation as: In the formula, ξ(k+1) represents the state variable in the state space at time k+1, ξ(k) represents the state variable in the state space at time k, and Δu(k) represents the predicted input at time k; is the state matrix, I m represents the identity matrix of dimension m, is the control matrix, is the system control output matrix, is the state variable in the state space; represents the control input at time k-1, t represents the time; n is the dimension of the state variable, and m is the dimension of the control variable; In order to simplify the calculation, it is assumed that the prediction time domain N P middle: In the formula, A k+i , B k+i represents the system matrix at time k+i; From equations (11) and (12), we get the predicted output expression of the control system: In formula (13), is the predicted input of the system, ΔU is the control time domain N C The input increment within Represents the prediction time domain N P The predicted output within, Ψ k and θ k is the system matrix; ΔU(k) ​​represents the control time domain N C Prediction input within; Represents the predicted output at time k+1; Represents the predicted output at time k+2; represents k+N c The predicted output at the moment; represents k+N P The predicted output at the moment; represents the expanded output matrix; represents the original state transfer matrix; represents the square of the original state transfer matrix; N represents the original state transfer matrix C Power; N represents the original state transfer matrix P Power; Δu(k) is the input increment; Δu(k+1) is the input increment at time k+1; Δu(k+N C -1) means k+N C -1 time input increment; N C represents the control time domain; represents the expanded input matrix; N represents the original state transfer matrix C -1 power; N represents the original state transfer matrix P -1 power; N represents the original state transfer matrix P -2 power; N represents the original state transfer matrix P -N C Power.

6. The underwater robot trajectory tracking control method according to claim 1, characterized in that: In step 5, the objective function is constructed based on the system state deviation, control input and control increment. During the design, the tracking speed and control energy consumption are considered at the same time; therefore, the objective function is defined as: Where J(k) represents the optimization objective function; N P represents the prediction time domain; Represents the predicted output at time k+i; Δu(k+i) represents the input increment at time k+i; Q and R are weight matrices; In order to facilitate the solution through the corresponding matrix operation, equation (14) is adjusted to the following standard quadratic objective function: Where J(ΔU(k)) represents the objective function of the secondary optimization of the control increment ΔU(k); H k is a Hessian matrix; Represents the coefficient of the first-order term in the objective function; in: In the formula, represents the transposed matrix of the system model matrix; Q e Represents the weight matrix of state error; R e represents the weight matrix of the control increment; Θ k represents the system model matrix; Ψ k represents the state prediction matrix; ξ(k) represents the current extended state vector; therefore, the optimization problem of the ship trajectory tracking problem based on linear MPC is expressed as: ΔU min ≤ΔU(k)≤ΔU max , IN min ≤MΔU(k)+U(k)≤U max , Where: ΔU min and ΔU max They represent the minimum and maximum values ​​of the control increment in the control time domain respectively; U min and U max They represent the minimum and maximum values ​​of the control quantity in the control time domain respectively; Y min and Y max represents the minimum and maximum values ​​of the output constraints; M is the matrix that converts the control increment into the control input; Y R (k) represents the reference output.

7. The underwater robot trajectory tracking control method according to claim 1, characterized in that: In step 6, the improved ant colony algorithm IACO sets the initial distribution of ants in the ant colony to evenly divide the domain, and dynamically adjusts the pheromone update strategy to avoid the algorithm from converging to a local optimal solution too early, specifically including the following steps: Step 6.1: First, determine the number of ants N in the ant colony according to the complexity of the problem to be solved; then, divide all the definition domains of the problem evenly into N small intervals, each small interval corresponds to an ant i; place each ant in the center of the corresponding small interval, so that each ant has an initial search position; each ant also has a movable search interval, the length of which is the same as the length of each small interval after the definition domain is divided into N equal parts; when all ants are in the center of their respective search intervals, there is only one ant in each interval; as the ant moves, its search interval will be adjusted according to the degree of overlap with the adjacent interval, affecting the estimation of the number of ants in the adjacent interval; Step 6.2: According to the above definition, if the domain of the problem is [u start ,u end ], when the number of ant colonies is N, the length of the subintervals on each axis is: Where: D u represents the length of the moving sub-interval of the ant on one axis; u end Indicates the end point of the moving interval; u start Indicates the starting point of the moving interval; N indicates the number of subintervals; From formula (17), we can see that the length of each ant's moving sub-interval on one axis is D u , and the initial coordinate distribution of the ant colony is: Where: u i represents the coordinates of the ant on the i-th subinterval of an axis; The left and right boundaries of the subinterval i are: u iL =u start +(i-1)D u (19); and iR =and start +iD u (20); In the above formula: u iL represents the lower boundary of the ith subinterval; u iR represents the upper boundary of the i-th subinterval; Step 6.3: During the execution of the algorithm, the update of pheromones can dynamically adjust the search range, so that the algorithm can adjust the exploration and utilization of different ranges according to the current search situation; design the following pheromone update formula: Where: Δτ i (t) represents the pheromone change at point i; Q i F is the pheromone released by the intelligent ant at point i; max is the maximum value of all objective function values ​​at time t; F i is the objective function value at point i; ξ is the adjustment constant.

8. The underwater robot trajectory tracking control method according to claim 7, characterized in that: In step 6.3, a dynamically adjusted confidence factor is introduced to balance the credibility of pheromones, thereby improving the algorithm's solution performance; specifically, the following steps are included: Step 6.3.1: A dynamically changing pheromone strategy is used to ensure that the algorithm can continuously explore better solutions and avoid premature locking on non-global optimal solutions. The expression is: Q i =γQ (22); Where: Q i represents the pheromone released by the intelligent ant at point i; γ is the confidence factor; Q represents the initial pheromone; Step 6.3.2: Introduce a confidence factor γ, which determines the credibility of the pheromone left by the ants; use a piecewise function to dynamically adjust the value of γ to balance the credibility of the pheromone. This piecewise function is: Where: N represents the maximum number of iterations; n represents the current number of iterations; In summary, the dynamic pheromone update formula is as follows: t i (t+1)=(1-ρ)τ i (t)+Δτ i (t) (24); In the formula, τ i (t+1) is the pheromone concentration at point i at time t+1; ρ is the pheromone volatility factor; τ i (t) is the pheromone concentration at point i at time t; Δτ i (t) represents the pheromone change at point i.

9. The underwater robot trajectory tracking control method according to claim 7, characterized in that: In step 6, local search and global search are performed according to the state transition probability to improve the optimization efficiency; the specific steps include the following: The transfer probability is determined by the pheromone concentration, and the formula is as follows: Among them, max(τ(t)) represents the maximum value of the pheromone at the tth iteration, τ i (t) represents the pheromone at point i, P i (t) represents the state transition probability of point i; When the state transition probability is less than the transition probability constant, a local search is performed, and the search formula is: Among them, u new is the position to be moved, u old is the current position of the ant, r1 is a random number between [-1,1], and step is the local search step length; When the state transition probability is greater than the transition probability constant, a global search is performed, and the search formula is: u new =u old +r2·range(27); Among them: r2 is a random number between [-0.5,0.5], and range is the interval size of the independent variable.

10. The underwater robot trajectory tracking control method according to claim 1, characterized in that: The step 7 specifically includes: In each control cycle, the improved ant colony algorithm IACO is used to solve equation (16) and obtain a series of control increments in the control time domain: in: represents the optimal control increment vector at time k; represents the optimal control increment at time k; represents the optimal control increment at time k+1; Indicates that at time k+N c The optimal control increment is -1; According to the basic principle of MPC, the first element in the control sequence output by the optimization model is used as the actual control input increment in the system: The above process is repeated in each new control cycle until the robot completes trajectory tracking control.

Images