A trajectory planning and tracking method suitable for small underwater robots

Abstract

The application discloses a trajectory planning and tracking method suitable for a small underwater robot, and belongs to the technical field of automatic control, which comprises the following steps: generating a three-dimensional underwater environment global path of the small underwater robot by using an improved adaptive ant colony algorithm; considering dynamic obstacles, fusing a dynamic window method to locally optimize the generated global path to obtain an optimized path; considering an ocean current field environment, using a quantum particle swarm optimization algorithm to optimize the generated global path to obtain an optimized path; using an improved particle swarm optimization algorithm to improve an MPC model, and tracking the trajectory of the small underwater robot by using the improved MPC model. The application adopts the above method, solves the problems of low trajectory planning efficiency, poor path smoothness, high energy consumption and insufficient trajectory tracking precision in the prior art under a dynamic environment, and significantly improves the task execution capability and adaptability of the underwater robot in a complex environment.

Description

Technical Field

[0001] This invention relates to the field of automation control technology, and in particular to a trajectory planning and tracking method suitable for small underwater robots. Background Technology

[0002] With the development of robotics technology, its applications are increasingly extending to multiple fields such as marine rescue, environmental monitoring, and resource exploration. Small underwater robots, with their autonomous navigation, real-time monitoring, and rapid response capabilities, have become a hot research topic for scholars. When performing tasks underwater, small underwater robots often face complex environmental factors and dynamic changes. Especially in complex three-dimensional environments, effectively planning and avoiding dynamic obstacles has become a challenge. Furthermore, tracking the desired trajectory with minimal error is another ongoing challenge.

[0003] Existing trajectory planning methods are mostly based on traditional global path planning algorithms, which can be divided into the following categories: graph search-based planning algorithms, random sampling-based planning algorithms, machine learning-based planning algorithms, and algorithms based on natural biological behavior. These algorithms can generate relatively effective paths in static environments, but in dynamic underwater environments, they struggle to avoid dynamic obstacles in real time. These algorithms largely rely on global search, making it difficult to respond quickly to dynamic changes in the underwater environment, and some methods are not suitable for small underwater robotic devices.

[0004] Furthermore, factors such as eddies in the marine environment have a significant impact on the performance of small underwater robots. Most existing trajectory planning methods fail to effectively consider the hydrodynamic factors in the underwater environment, resulting in path planning neglecting the energy consumption of small underwater robots by eddies. Therefore, optimizing the path and reducing energy loss while maintaining safety is a major challenge in current technology.

[0005] Trajectory tracking refers to a controlled object following a pre-given reference path using a certain strategy. Methods include path tracking strategies based on road geometry, classical control theory, and modern control theory. However, existing trajectory tracking control algorithms suffer from slow response speeds and fixed parameters. Furthermore, while traditional control methods can provide a certain level of tracking accuracy in static environments, their accuracy is insufficient in dynamic underwater environments, especially when facing dynamic obstacles and changing environmental conditions. This slow response speed prevents small underwater robots from adjusting their paths in time to maintain accurate trajectory tracking. Summary of the Invention

[0006] The purpose of this invention is to provide a trajectory planning and tracking method suitable for small underwater robots. Using a small underwater robot as a platform, this invention addresses the problems of getting trapped in local optima and slow convergence speed in planning problems by improving the traditional ant colony algorithm. It also solves the problem of moving obstacles in actual operation by using a fusion dynamic window method. Furthermore, it optimizes the trajectory by using quantum particle swarm optimization to reduce the energy loss of small underwater robots caused by ocean currents and reduce the influence of eddies. On this basis, trajectory smoothing is performed, and the real-time performance and accuracy of trajectory tracking are improved by introducing IPSO to improve the traditional MPC strategy. Finally, it realizes the collaborative operation of planning and tracking.

[0007] To achieve the above objectives, the present invention provides a trajectory planning and tracking method suitable for small underwater robots, comprising the following steps:

[0008] S1. Use the improved adaptive ant colony algorithm to generate a global path for the three-dimensional underwater environment of the small underwater robot;

[0009] S2. Considering dynamic obstacles, the global path generated in step S1 is locally optimized by incorporating the dynamic window method to obtain the optimized path.

[0010] S3. Considering the ocean current field environment, the global path generated in step S1 is optimized using the quantum particle swarm optimization algorithm to obtain the optimized path.

[0011] S4. Improve the MPC model using an improved particle swarm optimization algorithm, and use the improved MPC model to track the trajectory generated by the small underwater robot.

[0012] Preferred, improved adaptive ant colony algorithms include:

[0013] S11. Set multiple starting point sets and assign each ant to a starting point set for searching, so that each ant is only responsible for searching its corresponding part of the area.

[0014] S12. Considering global paths, improve the heuristic function, including:

[0015] Introducing the target point distance factor d and the iteration-driven adaptive adjustment factor ε into the heuristic function, the formula is as follows:

[0016]

[0017] In the formula, η ij (t) represents the path heuristic information from grid i to grid j at time t; d jG d represents the distance from the next position to the target point. ij The distance from the current position to the next position; I c I is the current iteration number; maxThis represents the maximum number of iterations.

[0018] Then, the transition probability is improved by introducing an angle influence factor. and obstacle removal factor ξ io (t), the improved transition probability The formula is:

[0019]

[0020] In the formula, τ ij (t) represents the pheromone concentration; α and β are parameter constants; allowed k The denominator represents the set of feasible neighboring nodes of the k-th ant at node i; the denominator represents the set of all feasible neighboring nodes of the k-th ant at node i. sum.

[0021] S13. Optimize pheromone updates in the ant colony algorithm using an elite preservation strategy, including:

[0022] After each iteration, select r ants with good pathfinding performance as elite ants and assign them a priority order.

[0023] To provide additional pheromone enhancement and accelerate pheromone accumulation on prime paths, the improved pheromone update rule is as follows:

[0024] τ ij (t+1)=(1-ρ)τ ij (t)+ρΔτ ij (t)+Δτ ij * ;

[0025]

[0026] In the formula, ρ is the global pheromone evaporation coefficient, and Δτ ij (t) represents the increment of pheromone on the current iteration path, Δτ ij * R is the pheromone increment of elite ants; r is the number of elite ants; Q is the pheromone release constant, L * The path length is the optimal solution path length;

[0027] The ant's contribution is weighted according to its position, considering only the first 1 / 3 of the ants. The amount of pheromone obtained along the path traversed is proportional to the ant's position. The pheromone increment update formula is:

[0028]

[0029] In the formula, M is the total number of ants; μ is the ant's position; L av L is the average path length. μLet be the path length traversed by the μth ant.

[0030] Preferably, step S2, which integrates the dynamic window method to perform local optimization of the global path generated in step S1, includes:

[0031] S21. The trajectory evaluation function is improved by introducing a path smoothing factor and a target point distance factor, and the weight factors are adaptively adjusted. The improved trajectory evaluation function is used to select the optimal local trajectory.

[0032] S22. Considering the motion state of the obstacle, for each moment, the small underwater robot calculates a local motion window based on the current motion state and the position of the obstacle using the dynamic window method, and searches for the optimal control input within the window. For each set of candidate control inputs (v, ω), the small underwater robot calculates the distance between the future trajectory and the obstacle, and judges whether the local path is feasible based on the obstacle avoidance evaluation function.

[0033] The preferred and improved trajectory evaluation function R(v,ω) is as follows:

[0034] R(v,ω)=ε(a'·heading(v,ω)+b'·dist obs (v,ω)+c'·vel(v,ω)+μ·

[0035] f smooth (v,ω)+δ·dist goal (v,ω));

[0036] In the formula, heading(v,ω) is the target orientation angle evaluation sub-function, which measures the deviation between the current trajectory of the small underwater robot and the target orientation; dist obs (v,ω) is the obstacle distance factor, which calculates the minimum distance between the trajectory of the small underwater robot and the nearest obstacle; vel(v,ω) is the velocity factor, which measures the speed of the small underwater robot on the preset trajectory; dist goal (v,ω) is the distance factor from the end of the current trajectory of the small underwater robot to the target point; f smooth (v,ω) is the path smoothing factor, which controls the change in the angular velocity of the small underwater robot. The formula is:

[0037]

[0038] In the formula, ω is the angular velocity within the current trajectory candidate window; ω prev ω is the angular velocity of the small underwater robot at the previous moment; max The maximum permissible angular velocity;

[0039] a', b', and c' are adaptive weighting factors, and the adjustment formula is as follows:

[0040]

[0041] In the formula, length is the preset total trajectory length, and a, b, and c are the weight coefficients of the target orientation angle evaluation sub-function, obstacle distance factor, and velocity factor in the traditional dynamic window method, respectively.

[0042] Preferably, the obstacle avoidance evaluation function formula is:

[0043] J(v,ω)=α1·Safety(v,ω)-α2·Curvature(v,ω)-α3·

[0044] Speed, ω);

[0045] In the formula, α1, α2, and α3 are the weights of obstacle avoidance safety (v,ω), path smoothness·Curvature (v,ω), and speed factor Speed ​​(v,ω), respectively.

[0046] Preferably, step S3, which uses the quantum particle swarm optimization algorithm to optimize the global path generated in step S1, includes:

[0047] Establish a cost function, Fitness, to transform the multi-objective constraint into a single-objective problem. The objective is to find the path with the minimum total cost. The cost function formula is:

[0048] Fitness=r(t)L p +w(t)E p ;

[0049] In the formula, L p E represents the distance a small underwater robot travels from its starting point to its destination. p The total energy consumption caused by ocean current interference is denoted as r(t) and w(t) are weighting coefficients that change dynamically with the iteration process, satisfying r(t) + w(t) = 1.

[0050]

[0051] In the formula, r max This is the initial value for the path length factor; r min The final value of the path length factor; N c N represents the current iteration number; max This represents the maximum number of iterations for the algorithm.

[0052] The cost function optimization model is as follows:

[0053] P * =argminFitness;

[0054] stP(ts )=x s ,P(t g )=x g ;

[0055]

[0056] In the formula, P * To minimize the cost function, x s x g These are the starting point and target point for the small underwater robot, respectively; v min v max For the constraint of propulsion speed of small underwater robots, v b,i This represents the robot's propulsion speed at node i.

[0057] Preferably, the improvement of the MPC model using the particle swarm optimization algorithm in step S4 includes:

[0058] Improve the computational complexity of the MPC model: Optimize the control input u(k) using the particle swarm optimization algorithm to obtain the solution U. * The objective function for the global optimization phase of the particle swarm optimization algorithm is:

[0059]

[0060] In the formula, N p To predict the time domain, x ref For the desired trajectory, N u To control the time domain, k is the current discrete time point; i is the sequence number;

[0061] The perturbation compensation term ΔF(k) and slack variable ζ in the MPC model are solved using gradient-based sequential quadratic programming, and the formula is as follows:

[0062]

[0063] stx(k+1) = f(x(k), u(k));

[0064] F min ≤F t ≤F max ;

[0065] In the formula, R F F is the weight matrix for the thrust rate of change. t Let F be the robot thrust at time t; min Minimum allowable thrust; F max λ represents the maximum allowable thrust; λ is the weighting coefficient.

[0066] The first control variable u0 in the optimized control sequence is selected and applied to the current control cycle. The prediction window is rolled to the next moment, and the above optimization process is re-executed to achieve closed-loop control.

[0067] To improve the prediction time domain, a particle swarm optimization algorithm is used to search for the optimal time domain combination. The particle swarm optimization algorithm iteratively searches and employs an error feedback adjustment rule to ensure the controller uses the optimal time domain at different trajectory stages. The error feedback adjustment rule is as follows:

[0068] N p (k+1)=N p (k)+κ(e t -e t-1 );

[0069] e t =||x(k)-x ref (k)||;

[0070] In the formula, e t Let t represent the trajectory error at time t, and κ be the adjustment coefficient.

[0071] Therefore, the present invention employs the above-described trajectory planning and tracking method suitable for small underwater robots, which has the following beneficial effects:

[0072] (1) In this invention, the global path is generated based on the improved adaptive ant colony algorithm, which solves the problems of slow convergence speed and insufficient search accuracy of the traditional ant colony algorithm in three-dimensional static obstacle environment; secondly, the local path is optimized by using the improved adaptive ant colony fusion dynamic window method, so that the small underwater robot can not only effectively avoid local dynamic obstacles during the movement, but also predict the future movement trajectory of the obstacles.

[0073] (2) In this invention, the ocean current field environment is taken into consideration. The path is optimized by combining the improved adaptive ant colony algorithm and the quantum particle swarm algorithm. This helps the underwater robot select the optimal path with low energy consumption in the complex flow field environment, thereby extending the duration of the robot's task and reducing energy waste.

[0074] (3) In this invention, an improved particle swarm optimization algorithm is used to optimize the MPC model, which solves the problems of high computational complexity and insufficient real-time performance of the existing MPC model in underwater trajectory tracking applications, and improves the computational efficiency of the MPC model.

[0075] The technical solution of the present invention will be further described in detail below with reference to the accompanying drawings and embodiments. Attached Figure Description

[0076] Figure 1 This is a flowchart of a method according to an embodiment of the present invention;

[0077] Figure 2 This is a schematic diagram of trajectory optimization considering dynamic obstacles in an embodiment of the present invention;

[0078] Figure 3 This is a schematic diagram of trajectory optimization considering ocean current field environment in an embodiment of the present invention;

[0079] Figure 4 This is a schematic diagram of MPC model trajectory tracking in an embodiment of the present invention. Detailed Implementation

[0080] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, not all embodiments. The components of the embodiments of the present invention described and shown in the accompanying drawings can generally be arranged and designed in various different configurations. In the description of the present invention, it should be noted that the terms "upper," "lower," "inner," "outer," etc., indicating orientation or positional relationships are based on the orientation or positional relationships shown in the accompanying drawings, or the orientation or positional relationships commonly used when the product of the invention is in use. They are only for the convenience of describing the present invention and simplifying the description, and are not intended to indicate or imply that the device or element referred to must have a specific orientation, or be constructed and operated in a specific orientation. Therefore, they should not be construed as limitations on the present invention.

[0081] Example

[0082] like Figure 1 As shown, this invention provides a trajectory planning and tracking method suitable for small underwater robots, the steps of which include:

[0083] S1. Generate a global path for the small underwater robot in a 3D underwater environment using the improved Adaptive Ant Colony Algorithm (IAACO). The improved Adaptive Ant Colony Algorithm includes:

[0084] S11. Use a two-way ant colony strategy.

[0085] We define multiple sets of starting points and assign each ant to one of these sets for searching, ensuring that each ant is only responsible for searching its corresponding region. The feasible point set G is defined as follows:

[0086]

[0087] In the formula, G ijkLet q be the set of grid cells with the minimum free area in each row, where (i,j,k) are the 3D coordinates of the grid center point, and non-obstacle represents an unobstructed grid. Ants will start from different sets of starting points and search towards different target points, such as the trajectory planning objective of a small underwater robot starting from the starting point q. S To the destination q G If the starting point and ending point are set as target 1 and target 2 respectively, the ants start from different nodes in the starting point set and search towards different target points along a certain path until they reach the target. In this way, the efficiency and accuracy of 3D underwater trajectory planning and the algorithm's global search capability can be improved.

[0088] S12. Consider global paths and improve the heuristic function.

[0089] Introducing a target point distance factor d and an iteration-driven adaptive adjustment factor ε into the heuristic function addresses the path detour problem caused by traditional ant colony algorithms that only consider the distance between the current node and the next node. The formula is as follows:

[0090]

[0091] In the formula, η ij (t) represents the path heuristic information from grid i to grid j at time t; d jG d represents the distance from the next position to the target point. ij The distance from the current position to the next position; I c I is the current iteration number; max This represents the maximum number of iterations.

[0092] Then, to address the issues of slow convergence speed and slow occurrence of the optimal solution, the transition probability is improved by introducing an angle influence factor. and obstacle removal factor ξ io (t) allows a path to be prioritized while avoiding obstacles with minimal turning angles to reach the destination. The angle influence factor is...

[0093]

[0094] In the formula, α is a fixed constant; θ ij (t) represents the angle between the candidate path and the target direction; τ is an adaptive parameter that dynamically adjusts with the number of iterations; d iG d represents the distance from the current location to the target point. ij This represents the distance from the current position to the next position.

[0095] Obstacle removal factor ξ io The expression for (t) is:

[0096]

[0097] In the formula, d io R is the distance from the current position to the obstacle. s For the safety radius.

[0098] Improved transition probability The formula is:

[0099]

[0100] In the formula, τ ij (t) represents the pheromone concentration; α and β are parameter constants; allowed k The denominator represents the set of feasible neighboring nodes of the k-th ant at node i; the denominator represents the set of all feasible neighboring nodes of the k-th ant at node i. sum.

[0101] S13. Optimize pheromone updates in the ant colony algorithm using an elite preservation strategy. This includes:

[0102] After each iteration, select r ants with good pathfinding performance as elite ants, and assign these ants a priority order, i.e., L1(t)≥L2(t)≥…≥L r (t).

[0103] To provide additional pheromone enhancement and accelerate pheromone accumulation on prime paths, the improved pheromone update rule is as follows:

[0104] τ ij (t+1)=(1-ρ)τ ij (t)+ρΔτ ij (t)+Δτ ij * ;

[0105]

[0106] In the formula, ρ is the global pheromone evaporation coefficient, and Δτ ij (t) represents the increment of pheromone on the current iteration path, Δτ ij * R is the pheromone increment of elite ants; r is the number of elite ants; Q is the pheromone release constant, L * The path length is the optimal solution path length;

[0107] The ant's contribution is weighted according to its position, considering only the first 1 / 3 of the ants. The amount of pheromone obtained along the path traversed is proportional to the ant's position. The pheromone increment update formula is:

[0108]

[0109] In the formula, M is the total number of ants; μ is the ant's position; L av L is the average path length. μ Let be the path length traversed by the μth ant.

[0110] S2. Considering dynamic obstacles, the global path generated in step S1 is locally optimized using the dynamic window method to obtain the optimized path. The optimization process includes:

[0111] S21. The trajectory evaluation function is improved by introducing a path smoothing factor and a target point distance factor, and the weight factors are adaptively adjusted to enhance the smoothness of the path and improve the guiding role of the target point. The improved trajectory evaluation function R(v,ω) is used to select the optimal local trajectory. The formula for the improved trajectory evaluation function R(v,ω) is:

[0112] R(v,ω)=ε(a'·heading(v,ω)+b'·dist obs (v,ω)+c'·vel(v,ω)+μ·

[0113] f smooth (v,ω)+δ·dist goal (v,ω));

[0114] In the formula, heading(v,ω) is the target orientation angle evaluation sub-function, which measures the deviation between the current trajectory of the small underwater robot and the target orientation; dist obs (v,ω) is the obstacle distance factor, which calculates the minimum distance between the trajectory of the small underwater robot and the nearest obstacle; vel(v,ω) is the velocity factor, which measures the speed of the small underwater robot on the preset trajectory; dist goal (v,ω) is the distance factor from the end of the current trajectory of the small underwater robot to the target point; f smooth (v,ω) is the path smoothing factor, which controls the change in the angular velocity of the small underwater robot. The formula is:

[0115]

[0116] In the formula, ω is the angular velocity within the current trajectory candidate window; ω prev ω is the angular velocity of the small underwater robot at the previous moment; max The maximum permissible angular velocity;

[0117] a', b', and c' are adaptive weighting factors. Compared to the traditional dynamic window method, the adjustment formula is as follows:

[0118]

[0119] In the formula, length is the preset total trajectory length, and a, b, and c are the weight coefficients of the target orientation angle evaluation sub-function, obstacle distance factor, and velocity factor in the traditional dynamic window method, respectively.

[0120] S22. In a dynamic environment, the motion state of obstacles is constantly changing. Therefore, small underwater robots not only need to avoid currently stationary or dynamic obstacles, but also need to predict the future trajectory of obstacles. When executing a global path, the small underwater robot monitors the position and motion state of obstacles in the surrounding environment in real time. For each moment, the small underwater robot uses a dynamic window method to predict its future trajectory at its current velocity and angular velocity, and calculates the collision risk with obstacles. Assuming the small underwater robot's current velocity is v and its angular velocity is ω, at each moment, combining the dynamic window method, the small underwater robot calculates a local motion window DW = [v... min ,v max ,ω min ,ω max The robot searches for the optimal control input within this window. For each set of candidate control inputs (v,ω), the small underwater robot calculates the distance between the future trajectory and the obstacle, and determines the feasibility of the local path based on the obstacle avoidance evaluation function J(v,ω). The obstacle avoidance evaluation function J(v,ω) is formulated as follows:

[0121] J(v,ω)=α1·Safety(v,ω)-α2·Curvature(v,ω)-α3·Speed(v,ω);

[0122] In the formula, α1, α2, and α3 are the weights of obstacle avoidance safety, path smoothness, and speed factors, respectively, which help small underwater robots select the optimal path in dynamic environments.

[0123] like Figure 2As shown, a two-layer trajectory planning strategy, combining global trajectory planning and local obstacle avoidance adjustment, is constructed by integrating an improved adaptive ant colony algorithm with a dynamic window method. First, the static environment is rasterized and a raster map is built. In the initial stage of path planning, the robot performs a global trajectory search based on the improved adaptive ant colony algorithm, generating an initial globally optimal path and storing pheromones along the path. Key points are extracted as local target points, providing global guidance for subsequent local optimization. During the execution of the real-time loop phase, the robot first initializes the current key node as a local target; simultaneously, it continuously perceives the position and motion state of obstacles in the surrounding environment, and dynamically calculates the speed control range based on its current linear and angular velocities. Within this window, it searches for the optimal control input, generates a trajectory based on the motion model, and selects the optimal path according to the trajectory evaluation function. At this point, the robot moves along the optimal trajectory. It continuously checks whether it has reached the current local target point based on the obstacle avoidance evaluation function—if not, it continues the loop planning; if it has reached the target point, it determines whether it is the global endpoint: if not, it updates to the next key node and re-initializes; if it is the endpoint, it generates complete path information and terminates the process.

[0124] S3. Considering the ocean current environment, the quantum particle swarm optimization algorithm is used to optimize the global path generated in step S1 to obtain the optimized path.

[0125] Specifically, considering the ocean current field environment, an ocean current field optimization model is constructed, and the quantum particle swarm optimization algorithm is used to optimize the global path and obtain the optimal trajectory.

[0126] Ocean current field optimization models include:

[0127] The energy required for a small underwater robot to travel from the starting point to the destination point needs to be evaluated by the resistance generated by the ocean currents that constitute each part of the defined path, considering the connecting path x. i-1 and x i The line segment X of the node i-1 X i The velocity of the small underwater robot at any point (x, y, z) on the road segment can be described as:

[0128]

[0129] In the formula, The speed of a small underwater robot in an ocean current environment; For the propulsion speed of small underwater robots; To determine the ocean current velocity, an appropriate current layer is selected based on the depth at which the small underwater robot is located. If the small underwater robot is currently located between two depth layers, the average current velocity of the two layers is used for approximate calculation. Let e ​​be the ocean current velocity at the k-th layer; therefore, the energy loss e in the i-th segment is... iIt can be described as:

[0130]

[0131] In the formula, v is the drag coefficient, which is determined by the structural design of the small underwater robot and the properties of the surrounding water; P b The propulsion power of a small underwater robot is proportional to the cube of its propulsion speed while navigating a sub-path; J i This represents the length of the three-dimensional path. The total cost of a small underwater robot navigating along the path is the sum of the energy consumption of each sub-path, i.e.

[0132]

[0133] Establish a cost function, Fitness, to transform the multi-objective constraint into a single-objective problem. The objective is to find the path with the minimum total cost. The cost function formula is:

[0134] Fitness=r(t)L p +w(t)E p ;

[0135] In the formula, L p E represents the distance a small underwater robot travels from its starting point to its destination. p The total energy consumption caused by ocean current interference is denoted as r(t) and w(t) are weighting coefficients that change dynamically with the iteration process, satisfying r(t) + w(t) = 1.

[0136]

[0137] In the formula, r max This is the initial value for the path length factor; r min The final value of the path length factor; N c N represents the current iteration number; max This represents the maximum number of iterations for the algorithm.

[0138] The cost function optimization model is as follows:

[0139] P * =argminFitness;

[0140] stP(t s )=x s ,P(t g )=x g ;

[0141]

[0142] In the formula, P * To minimize the cost function, x s xg These are the starting point and target point for the small underwater robot, respectively; v min v max For the constraint of propulsion speed of small underwater robots, v b,i This represents the robot's propulsion speed at node i.

[0143] like Figure 3 As shown, by combining the improved adaptive ant colony optimization algorithm and the quantum particle swarm optimization algorithm, the complementary advantages of the two in global search and local refinement are fully utilized to optimize the path in the ocean current field environment. In this strategy, parameters are first initialized and a three-dimensional raster map is constructed through rasterization. The upper-level optimization models the path channel search as a combinatorial optimization task, and a set of collision-free path channels x are quickly generated in the three-dimensional raster map based on IAACO. channel The focus is on resolving path feasibility and macro-level navigation structure; lower-level optimization uses channels as constraints, within x... channel Internally, QPSO is called to further refine the search of the path trajectory and obtain continuous trajectories. And calculate its corresponding cost. The lower-level algorithm transmits the cost of the optimal path within the channel back to the upper-level algorithm, which then updates the pheromone levels based on this cost. This process is repeated to integrate x. channel Channel and The path forms a complete solution, and it is determined whether it is the global optimum. If not, adaptive weight adjustments are made based on whether the loop has ended. If continuous iterations fail to improve the global optimum or the loop ends, the optimal trajectory is output, and the algorithm terminates.

[0144] S4. Improve the MPC model using an improved particle swarm optimization algorithm, and use the improved MPC model to track the trajectory generated by the small underwater robot.

[0145] Improvements to the MPC model using an improved particle swarm optimization algorithm include:

[0146] First, the problem that MPC solves is described as follows:

[0147] The actual rate of change of thrust of the robot's thrusters should be limited to prevent instability caused by violent manipulation, i.e.

[0148] ΔF min ≤ΔF t ≤ΔF max ;

[0149] In the formula, ΔF t Let ΔF be the change in thrust of the thruster. min and ΔF max These represent the minimum and maximum thrust change rates, respectively.

[0150] Underwater robots are constrained by hydrodynamics, and their acceleration a y The maximum withstandable thrust F cannot be exceeded. max The ratio of mass to mass m, i.e.:

[0151]

[0152] Based on the above constraints, the MPC optimization objective can be described as follows:

[0153]

[0154] In the formula, Q and R are the weight matrices for the state error and control input, respectively; x ref For the desired trajectory, N u For the control time domain; λ is the weighting coefficient; ζ is the relaxation factor; R F The weight matrix of the actual thrust change rate.

[0155] Therefore, the problem of coordinated control of robot trajectory tracking and stability can be transformed into an optimization problem under constraints, namely:

[0156]

[0157] In the formula, ΔU t Let ΔU be the rate of change of the control variable at time t. min With ΔU max These are the minimum and maximum values ​​of the rate of change of the control quantity, respectively; U t A is the control value at time t; u U is a Nu×Nu lower triangular matrix with all elements being 1; min with U max These are the minimum and maximum values ​​of the control quantity, respectively; F min Minimum allowable thrust; F max Maximum permissible thrust; F t V is the thrust at time t; t V represents the robot's velocity at time t; min With V max These represent the minimum and maximum allowable speeds for the robot, respectively. The above optimization problem, while ensuring the trajectory tracking accuracy of the underwater robot, improves the stability and energy efficiency of the system by controlling thrust variation and acceleration constraints, so as to adapt to the motion control requirements in complex hydrodynamic environments.

[0158] By combining the global search capability of PSO with the local convergence characteristics of gradient-based optimization, the computational efficiency and solution accuracy of MPC model optimization are improved.

[0159] The particle swarm optimization (PSO) algorithm is used to globally search the search space for the control input sequence U = {u0, u1, ..., u...}.N The approximate better initial solution U * Considering that the MPC model includes a perturbation compensation term ΔF(k) and a relaxation variable ζ, directly using PSO to optimize all variables may lead to an excessively high search space dimension and increased computational complexity. Therefore, a decomposition optimization strategy is adopted, i.e., PSO only optimizes the control input u(k), while the perturbation compensation term ΔF(k) and the relaxation variable ζ are locally optimized by sequential quadratic programming. The objective function of the global optimization stage of the particle swarm optimization algorithm is:

[0160]

[0161] In the formula, N p To predict the time domain, x ref For the desired trajectory, N u To control the time domain, k is the current discrete time point; i is the sequence number;

[0162] Obtain the solution U * Then, the perturbation compensation term ΔF(k) and slack variable ζ in the MPC model are solved using gradient-based sequential quadratic programming (SQP), and the formula is:

[0163]

[0164] stx(k+1) = f(x(k), u(k));

[0165] F min ≤F t ≤F max ;

[0166] In the formula, R F F is the weight matrix for the thrust rate of change. min Minimum allowable thrust; F max The maximum allowable thrust; λ is the weighting coefficient;

[0167] The above problem can be transformed into a standard quadratic programming form, with the following formula:

[0168]

[0169] stAΔU≤b;

[0170] In the formula, ΔU is the decision variable vector of the second-stage optimization problem; H is the Hessian matrix of the quadratic programming problem; g is the gradient vector; A and b represent the constraints of the optimization variables, respectively.

[0171] The first control variable u0 in the optimized control sequence is selected and applied to the current control cycle. The prediction window is then rolled to the next moment, and the above optimization process is repeated to achieve closed-loop control.

[0172] Improvements to the prediction time domain: To address the problem of fixed algorithm parameters, the PSO algorithm is used to search for the optimal time domain combination. The particle representation of PSO optimization is P = (N p N c The particle swarm optimization algorithm is used to search for the optimal time domain combination. Through iterative search and by employing an error feedback adjustment rule, the applicability of the particle swarm is improved, enabling the controller to use the optimal time domain at different trajectory stages. The error feedback adjustment rule is as follows:

[0173] N p (k+1)=N p (k)+κ(e t -e t-1 );

[0174] e t =||x(k)-x ref (k)||;

[0175] In the formula, e t Let t represent the trajectory error at time t, and κ be the adjustment coefficient.

[0176] like Figure 4 As shown, this is the overall control process of the MPC model. The robot system first obtains the linear error model, i.e. the objective function of MPC, based on the robot model and state, the initial state of the environment, and the reference trajectory planned by S1-S3. The rate of change of control variable, relaxation factor and rate of change of robot thrust in the objective function all need to be optimized and solved using the IPSO algorithm. The system constraints are input into the IPSO algorithm. The improved particle swarm optimization algorithm first uses PSO to optimize the change in control input. Specifically, the particle swarm is initialized, including determining the number of particles, iteration count, and other initial data. Each particle's state consists of its current position and velocity. The particle's fitness function is updated based on its historical best position and global best position. Based on this result, the particles are sorted, and the optimal fitness is updated. Then, an adaptive time-domain optimization predictive control sequence is generated using the traditional PSO algorithm. When the convergence condition is met (e.g., fitness stabilizes or the iteration limit is reached), the loop is exited. SQP is then executed for local fine-grained search, optimizing the relaxation factor and robot thrust rate of change. After the IPSO calculation, three output values ​​are generated: the control input rate of change, the relaxation factor, and the robot thrust rate of change. All three are input into the MPC optimization algorithm to allocate the robot's thrust and control its speed, while real-time monitoring of the robot's actual feedback and closed-loop control of the stroke.

[0177] Therefore, the present invention adopts the above-mentioned trajectory planning and tracking method suitable for small underwater robots, which solves the problems of low trajectory planning efficiency, poor path smoothness, high energy consumption and insufficient trajectory tracking accuracy in the existing technology under dynamic environment, and significantly improves the task execution capability and adaptability of underwater robots in complex environment.

[0178] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and not to limit them. Although the present invention has been described in detail with reference to preferred embodiments, those skilled in the art should understand that modifications or equivalent substitutions can still be made to the technical solutions of the present invention, and these modifications or equivalent substitutions cannot cause the modified technical solutions to deviate from the spirit and scope of the technical solutions of the present invention.

Claims

1. A trajectory planning and tracking method suitable for small underwater robots, characterized by the following steps: include: S1. Use the improved adaptive ant colony algorithm to generate a global path for the three-dimensional underwater environment of the small underwater robot; S2. Considering dynamic obstacles, the global path generated in step S1 is locally optimized by incorporating the dynamic window method to obtain the optimized path. S3. Considering the ocean current field environment, the global path generated in step S1 is optimized using the quantum particle swarm optimization algorithm to obtain the optimized path. S4. Improve the MPC model using an improved particle swarm optimization algorithm, and use the improved MPC model to track the trajectory generated by the small underwater robot. Among them, the improved adaptive ant colony algorithm includes: S11. Set multiple starting point sets and assign each ant to a starting point set for searching, so that each ant is only responsible for searching its corresponding part of the area. S12. Considering global paths, improve the heuristic function, including: Introducing the target point distance factor into the heuristic function Adaptive adjustment factor driven by iteration number The formula is: ； ； In the formula, for Time from the grid To grid Path heuristic information; This represents the distance from the next location to the target point. This represents the distance from the current position to the next position. This represents the current iteration number; This represents the maximum number of iterations. Then, the transition probability is improved by introducing an angle influence factor. and obstacle removal factors Improved transition probability The formula is: ； In the formula, The concentration of pheromones; These are parameter constants; The denominator represents the set of feasible adjacent grid cells for the k-th ant; the denominator represents the set of adjacent grid cells for the k-th ant. k Only ants in the grid i All feasible adjacent grid cells sum; S13. Optimize pheromone updates in the ant colony algorithm using an elite preservation strategy, including: After each iteration, select Ants with good pathfinding skills are selected as elite ants, and these ants are assigned a priority order. To provide additional pheromone enhancement and accelerate pheromone accumulation on prime paths, the improved pheromone update rule is as follows: ； ； In the formula, The global pheromone evaporation coefficient. This represents the increment of pheromones on the current iteration path. Increase the pheromone levels of elite ants; The number of elite ants; The path length is the optimal solution path length; The ant's contribution is weighted according to its position, considering only the first 1 / 3 of the ants. The amount of pheromone obtained along the path traversed is proportional to the ant's position. The pheromone increment update formula is: ； ； In the formula, M The total number of ants; The ant's position; This represents the average path length. For the first The length of the path traveled by an ant.

2. The trajectory planning and tracking method for small underwater robots according to claim 1, characterized in that, Step S2, which integrates the dynamic window method to perform local optimization of the global path generated in step S1, includes: S21. The trajectory evaluation function is improved by introducing a path smoothing factor and a target point distance factor, and the weight factors are adaptively adjusted. The improved trajectory evaluation function is used to select the optimal local trajectory. S22. Considering the obstacle's motion state, for each moment, using the dynamic window method, the small underwater robot calculates a local motion window based on the current motion state and obstacle position, and searches for the optimal control input within this window. For each set of candidate control inputs... The small underwater robot calculates the distance between its future trajectory and obstacles, and determines the feasibility of a local path based on the obstacle avoidance evaluation function.

3. The trajectory planning and tracking method for small underwater robots according to claim 2, characterized in that, Improved trajectory evaluation function The formula is: ； In the formula, The target orientation angle evaluation sub-function measures the deviation between the current trajectory of the small underwater robot and the target orientation; Using obstacle distance factor, calculate the minimum distance between the trajectory of a small underwater robot and the nearest obstacle; The velocity factor measures the speed of a small underwater robot on a preset trajectory. This is the distance factor from the end of the current trajectory of the small underwater robot to the target point. The path smoothing factor controls the angular velocity variation of the small underwater robot, and the formula is: ； In the formula, The angular velocity within the current trajectory candidate window; The angular velocity of the small underwater robot at the previous moment; The maximum permissible angular velocity; , , The adaptive weighting factor is adjusted using the following formula: ； In the formula, The preset total trajectory length, These are the weighting coefficients for the target orientation angle evaluation sub-function, obstacle distance factor, and velocity factor, respectively, in the traditional dynamic window method.

4. The trajectory planning and tracking method for small underwater robots according to claim 3, characterized in that, Obstacle avoidance evaluation function The formula is: ； In the formula, , , Obstacle avoidance safety Path smoothness and speed factor The weight.

5. The trajectory planning and tracking method for small underwater robots according to claim 1, characterized in that, Step S3 optimizes the global path generated in step S1 using the quantum particle swarm optimization algorithm, including: Establish cost function The multi-objective function constraint is transformed into a single-objective problem, with the objective of finding the path with the minimum total cost. The cost function formula is: ； In the formula, This refers to the distance a small underwater robot travels from its starting point to its destination. The total energy consumption resulting from ocean current disturbance. The weighting coefficients are dynamically changing as the iteration progresses, satisfying... ; ； In the formula, This is the initial value for the path length factor; This is the final value of the path length factor; This represents the current iteration number; This represents the maximum number of iterations for the algorithm. The cost function optimization model is as follows: ； ； ； In the formula, , These are the starting point and target point for the small underwater robot; Constraints on the propulsion speed of small underwater robots for The robot's propulsion speed under the grid.

6. The trajectory planning and tracking method for small underwater robots according to claim 1, characterized in that, Step S4 involves using the particle swarm optimization algorithm to improve the MPC model, including: Improve the computational complexity of the MPC model: Optimize the control input using the particle swarm optimization algorithm. To obtain the solution The objective function for the global optimization phase of the particle swarm optimization algorithm is: ； In the formula, To predict the time domain, For the desired trajectory, To control the time domain, For the current discrete time point, Number of sequences; The perturbation compensation term in the MPC model is calculated using gradient-based sequential quadratic programming. and slack variables Solve using the formula: ； ； ； In the formula, This is the weight matrix for the thrust rate of change. Let be the propulsion force of the robot at time t; Minimum allowable thrust; These are the weighting coefficients; Select the first control variable in the optimized control sequence Apply the control to the current control cycle, roll the prediction window to the next moment, and re-execute the above optimization process to achieve closed-loop control; To improve the prediction time domain, a particle swarm optimization algorithm is used to search for the optimal time domain combination. The particle swarm optimization algorithm iteratively searches and employs an error feedback adjustment rule to ensure the controller uses the optimal time domain at different trajectory stages. The error feedback adjustment rule is as follows: ； ； In the formula, express Trajectory error at any given time, This is for adjusting the coefficient.

Images