Track planning and tracking method suitable for small underwater robot

Abstract

The invention discloses a trajectory planning and tracking method suitable for a small underwater robot, and belongs to the technical field of automatic control, and the method comprises the steps: generating a three-dimensional underwater environment global path of the small underwater robot through employing an improved adaptive ant colony algorithm; a dynamic obstacle is considered, a dynamic window method is fused to carry out local optimization on the generated global path, and an optimized path is obtained; the ocean current field environment is considered, a quantum particle swarm algorithm is used for optimizing the generated global path, and an optimized path is obtained; and improving the MPC model by using an improved particle swarm optimization algorithm, and tracking the trajectory generated by the small underwater robot through the improved MPC model. By adopting the method, the problems of low trajectory planning efficiency, poor path smoothness, high energy consumption, insufficient trajectory tracking precision and the like in a dynamic environment in the prior art are solved, and the task execution capability and adaptability of the underwater robot in a complex environment are remarkably improved.

Description

Technical Field

[0001] The present invention relates to the field of automatic control technology, and in particular to a trajectory planning and tracking method suitable for a small underwater robot. Background Art

[0002] With the development of robotics, its applications have expanded into diverse fields, including 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 topic of research. When performing underwater tasks, small underwater robots often face complex environmental factors and dynamic changes. This is particularly true in complex three-dimensional environments, where effectively planning and avoiding dynamic obstacles presents a challenge. Furthermore, tracking the desired trajectory with minimal error is also an ongoing challenge.

[0003] Existing trajectory planning methods are mostly based on traditional global path planning algorithms and can be categorized as follows: those based on graph search, those based on random sampling, those based on machine learning, and those based on natural biological behavior. These algorithms can generate relatively effective paths in static environments, but struggle to avoid dynamic obstacles in real time in dynamic underwater environments. These algorithms often rely on global search, making them incapable of responding quickly to dynamic changes in the underwater environment, and some methods are not suitable for small underwater robotic devices.

[0004] Furthermore, factors like eddies in the marine environment significantly impact the performance of small underwater robots. Existing trajectory planning methods often fail to effectively account for the fluid dynamics of underwater environments, leading to a neglect of the energy consumption of small underwater robots due to eddies. Therefore, optimizing the trajectory and minimizing energy losses while maintaining safety remain major challenges in current technology.

[0005] Trajectory tracking involves a controlled object following a predetermined reference path using a specific strategy. Methods include those 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 offer limited tracking accuracy in static environments, they lack precision in dynamic underwater environments. This is particularly true when faced with dynamic obstacles and changing environmental conditions, resulting in a slow response and an inability for small underwater robots to adjust their paths in a timely manner to maintain accurate trajectory tracking. Summary of the Invention

[0006] The purpose of the present invention is to provide a trajectory planning and tracking method suitable for a small underwater robot. Taking the small underwater robot as the platform, the traditional ant colony algorithm is improved to solve the problems of easy falling into local optimality and slow convergence speed in planning problems. The solution of the fusion dynamic window method is used to solve the problem of moving obstacles in actual operations. The quantum particle swarm algorithm is used to optimize the trajectory of the small underwater robot based on the energy loss caused by ocean currents and reduce the influence of eddy currents. On this basis, the trajectory is smoothed. The IPSO is introduced to improve the traditional MPC strategy, improve the real-time performance and accuracy of trajectory tracking, and finally realize 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 a small underwater robot, comprising the following steps:

[0008] S1, using the improved adaptive ant colony algorithm to generate the global path of the small underwater robot in the three-dimensional underwater environment;

[0009] S2, considering dynamic obstacles, locally optimize the global path generated in step S1 by integrating the dynamic window method to obtain the optimized path;

[0010] S3, considering the ocean current environment, using the quantum particle swarm algorithm to optimize the global path generated in step S1 to obtain the optimized path;

[0011] S4. Use the improved particle swarm optimization algorithm to improve the MPC model, and track the trajectory generated by the small underwater robot through the improved MPC model.

[0012] Preferably, the improved adaptive ant colony algorithm includes:

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

[0014] S12. Consider the global path and improve the heuristic function, including:

[0015] The target point distance factor d and the adaptive adjustment factor ε driven by the number of iterations are introduced into the heuristic function. The formula is:

[0016]

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

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

[0019]

[0020] Where, τ ij (t) is the pheromone concentration; α and β are parameter constants; allowed k represents the set of feasible adjacent nodes for the kth ant; the denominator represents the set of feasible adjacent nodes for the kth ant at node i sum.

[0021] S13. Use the elite retention strategy to optimize the pheromone update of the ant colony algorithm, including:

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

[0023] Additional pheromone enhancement is given to accelerate the accumulation of pheromones on high-quality paths. The improved pheromone update rule is:

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

[0025]

[0026] Where ρ is the global pheromone volatility coefficient, Δτ ij (t) is the increment of pheromone on the current iterative path, Δτ ij * is the pheromone increment of elite ants; r is the number of elite ants; Q is the pheromone release constant, L * is the optimal solution path length;

[0027] The ant contribution is obtained by weighting the ant's position. Considering only the first 1 / 3 of the ants, the amount of pheromone obtained by the path is proportional to the ant's position. The pheromone increment update formula is:

[0028]

[0029] Where M is the total number of ants; μ is the ant rank; L av is the average path length; L μis the length of the path traversed by the μth ant.

[0030] Preferably, in step S2, locally optimizing the global path generated in step S1 by integrating the dynamic window method includes:

[0031] S21, introducing a path smoothing factor and a target point distance factor to improve the trajectory evaluation function, and adaptively adjusting each weight factor, and using the improved trajectory evaluation function to select the optimal local trajectory;

[0032] S22. Considering the motion state of the obstacle, for each moment, combined with the dynamic window method, the small underwater robot calculates a local motion window based on the current motion state and the obstacle position, 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 determines whether the local path is feasible based on the obstacle avoidance evaluation function.

[0033] Preferably, the improved trajectory evaluation function R(v,ω) formula is:

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

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

[0036] Where heading(v,ω) is the target direction angle evaluation subfunction, which measures the deviation between the current trajectory of the small underwater robot and the target direction; 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 speed 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 angular velocity change of the small underwater robot. The formula is:

[0037]

[0038] Where ω 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 is the maximum allowable angular velocity;

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

[0040]

[0041] Where length is the total length of the preset trajectory, a, b, and c are the weight coefficients of the target direction angle evaluation subfunction, obstacle distance factor, and speed 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] Where α1, α2, and α3 are the weights of obstacle avoidance safety (v, ω), path smoothness·Curvature (v, ω), and speed factor Speed ​​(v, ω), respectively.

[0046] Preferably, in step S3, optimizing the global path generated in step S1 using a quantum particle swarm algorithm includes:

[0047] Establish the cost function Fitness and transform the multi-objective function constraints into a single-objective problem. The goal 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] Where, L p is the distance of the small underwater robot from the starting point to the target point; E p is the total energy consumption caused by the interference of ocean currents, r(t) and w(t) are weight coefficients that change dynamically with the iteration process, satisfying r(t)+w(t)=1;

[0050]

[0051] Where r max is the initial value of the path length factor; r min is the end value of the path length factor; N c is the current iteration number; N max is the maximum number of iterations of the algorithm;

[0052] The cost function optimization model is:

[0053] P * =argminFitness;

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

[0055]

[0056] Where, P * To minimize the cost function, x s 、x g are the starting point and target point of the small underwater robot respectively; v min 、v max is the constraint of the propulsion speed of the small underwater robot, v b,i is the robot's advancing speed under node i.

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

[0058] Improve the computational complexity of the MPC model: Use the particle swarm optimization algorithm to optimize the control input u(k) and obtain the solution U * , the objective function of the global optimization phase of the particle swarm optimization algorithm is:

[0059]

[0060] Where N p is the prediction time domain, x ref is the expected trajectory, N u To control the time domain, k is the current discrete time point; i is the sequence number;

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

[0062]

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

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

[0065] Where R F is the weight matrix of thrust change rate, F t is the robot thrust at time t; F min The minimum allowable thrust; F max is the maximum allowable thrust; λ is the weight coefficient.

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

[0067] The prediction time domain is improved, and the particle swarm optimization algorithm is used to search for the optimal time domain combination. The particle swarm performs iterative search and adopts the error feedback adjustment rule to make the controller use the optimal time domain in different trajectory stages. The error feedback adjustment rule is:

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

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

[0070] Where, e t represents the trajectory error at time t, and κ is the adjustment coefficient.

[0071] Therefore, the present invention adopts the above-mentioned trajectory planning and tracking method suitable for a small underwater robot, which has the following beneficial effects:

[0072] (1) The present invention generates a global path based on an improved adaptive ant colony algorithm, which solves the problems of slow convergence speed and insufficient search accuracy of the traditional ant colony algorithm in a three-dimensional static obstacle environment. Secondly, the improved adaptive ant colony fusion dynamic window method is used to optimize the local path, so that the small underwater robot can not only effectively avoid local dynamic obstacles during movement, but also predict the future movement trajectory of the obstacles.

[0073] (2) The present invention takes the ocean current environment into consideration and optimizes the path by combining an improved adaptive ant colony algorithm with a quantum particle swarm algorithm, helping the underwater robot to select the optimal path with low energy consumption in a complex flow field environment, thereby extending the duration of the robot's mission and reducing energy waste;

[0074] (3) The present invention uses an improved particle swarm optimization algorithm 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 is further described in detail below through the accompanying drawings and embodiments. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0077] Figure 2 A schematic diagram of trajectory optimization considering dynamic obstacles according to an embodiment of the present invention;

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

[0079] Figure 4 Schematic diagram of MPC model trajectory tracking according to an embodiment of the present invention. DETAILED DESCRIPTION

[0080] In order to make the purpose, technical solutions and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, rather than all of the embodiments. The components of the embodiments of the present invention generally described and shown in the drawings herein can be arranged and designed in various different configurations. In the description of the present invention, it should be noted that the orientation or position relationship indicated by the terms "upper", "lower", "inside", "outside", etc. is based on the orientation or position relationship shown in the drawings, or is the orientation or position relationship in which the product of the invention is usually placed when in use. It is only for the convenience of describing the present invention and simplifying the description, and does not indicate or imply that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation, and therefore cannot be understood as a limitation on the present invention.

[0081] Example

[0082] like Figure 1 As shown, the present invention provides a trajectory planning and tracking method suitable for a small underwater robot, the steps comprising:

[0083] S1. Use the improved adaptive ant colony algorithm (IAACO) to generate a global path for a small underwater robot in a three-dimensional underwater environment. The improved adaptive ant colony algorithm includes:

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

[0085] Set multiple starting point sets and assign each ant to a starting point set for search, so that each ant is only responsible for searching its corresponding part of the area. Define the feasible point set G as follows:

[0086]

[0087] Where G ijkis the grid set with the least free area in each row, (i, j, k) is the three-dimensional coordinate of the center point of the grid, and non-obstacle means an obstacle-free grid. Ants will start from different starting points and search towards different target points. For example, the trajectory planning goal of a small underwater robot is to start from the starting point q S To the end point q G , the starting point and end point are set as target 1 and target 2 respectively. Ants start from different nodes in the starting point set and search along a path to different target points until they reach the target. This method can improve the efficiency and accuracy of 3D underwater trajectory planning and the global search capability of the algorithm.

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

[0089] The target point distance factor d and the adaptive adjustment factor ε driven by the number of iterations are introduced into the heuristic function to solve the problem of circuitous paths caused by the heuristic function only considering the distance between the current node and the next node in the traditional ant colony algorithm. The formula is:

[0090]

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

[0092] Then, in order to solve the problem of slow convergence and slow optimal solution, the transition probability is improved and the angle influence factor is introduced. and obstacle removal factor ξ io (t), while giving priority to the path, it can avoid obstacles and move to the end point with the minimum turning angle. The angle influence factor is

[0093]

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

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

[0096]

[0097] Where, d io is the distance from the current position to the obstacle; R s is the safety radius.

[0098] Improved transition probability The formula is:

[0099]

[0100] Where, τ ij (t) is the pheromone concentration; α and β are parameter constants; allowed k represents the set of feasible adjacent nodes for the kth ant; the denominator represents the set of feasible adjacent nodes for the kth ant at node i sum.

[0101] S13. Use the elite retention strategy to optimize the pheromone update of the ant colony algorithm. This includes:

[0102] After each iteration, r ants with good pathfinding performance are selected as elite ants, and these ants are given a priority order, that is, L1(t)≥L2(t)≥…≥L r (t).

[0103] Additional pheromone enhancement is given to accelerate the accumulation of pheromones on high-quality paths. The improved pheromone update rule is:

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

[0105]

[0106] Where ρ is the global pheromone volatility coefficient, Δτ ij (t) is the increment of pheromone on the current iterative path, Δτ ij * is the pheromone increment of elite ants; r is the number of elite ants; Q is the pheromone release constant, L * is the optimal solution path length;

[0107] The ant contribution is obtained by weighting the ant's position. Considering only the first 1 / 3 of the ants, the amount of pheromone obtained by the path is proportional to the ant's position. The pheromone increment update formula is:

[0108]

[0109] Where M is the total number of ants; μ is the ant rank; L av is the average path length; L μ is the length of the path traversed by the μth ant.

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

[0111] S21. The path smoothing factor and the target point distance factor are introduced to improve the trajectory evaluation function, and each weight factor is adaptively adjusted to enhance the smoothness of the path and improve the guidance effect of the target point. The improved trajectory evaluation function is used to select the optimal local trajectory. The improved trajectory evaluation function R(v,ω) formula is:

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

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

[0114] Where heading(v,ω) is the target direction angle evaluation subfunction, which measures the deviation between the current trajectory of the small underwater robot and the target direction; 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 speed 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 angular velocity change of the small underwater robot. The formula is:

[0115]

[0116] Where ω 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 is the maximum allowable angular velocity;

[0117] a', b', c' are adaptive weight factors. Compared with the traditional dynamic window method, the adjustment formula is:

[0118]

[0119] Where length is the total length of the preset trajectory, a, b, and c are the weight coefficients of the target direction angle evaluation subfunction, obstacle distance factor, and speed factor in the traditional dynamic window method, respectively.

[0120] S22. In a dynamic environment, the motion state of obstacles will continue to change. Therefore, the small underwater robot not only needs to avoid the current static or dynamic obstacles, but also needs to foresee the future trajectory of the obstacles. When executing the 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 will use the dynamic window method to predict the future trajectory at the current speed and angular velocity, and calculate the risk of collision with the obstacle. Assuming that the current speed of the small underwater robot is v and the angular velocity is ω, at each moment, combined with the dynamic window method, the small underwater robot calculates a local motion window DW=[v min ,v max ,ω min ,ω max ] and 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 whether the local path is feasible based on the obstacle avoidance evaluation function. The obstacle avoidance evaluation function J(v, ω) is:

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

[0122] Where α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 a dynamic environment.

[0123] like Figure 2As shown in the figure, a two-layer trajectory planning strategy consisting of global trajectory planning and local obstacle avoidance adjustment is constructed by integrating an improved adaptive ant colony algorithm with a dynamic window method. The static environment is first rasterized and a gridded map is constructed. In the initial stage of path planning, the robot uses the improved adaptive ant colony algorithm to perform a global trajectory search, generate an initial global optimal path, store pheromones along the path, and extract key points as local target points, providing global guidance for subsequent local optimization. During the real-time loop phase, the robot first initializes the current key node as the local target. Simultaneously, it continuously perceives the position and motion of obstacles in the surrounding environment. Combining its current linear and angular velocities, it dynamically calculates the velocity control range and searches for the optimal control input within this window. A trajectory is generated based on the motion model, and the optimal path is selected based on a trajectory evaluation function. The robot then moves along the optimal trajectory. The obstacle avoidance evaluation function is used to continuously check whether the current local target point has been reached. If not, the planning loop continues. If reached, the robot determines whether it has reached the global endpoint. If not, the robot updates to the next key node and reinitializes. If it has, the robot generates the complete path information and terminates the process.

[0124] S3. Considering the ocean current environment, the quantum particle swarm 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, constructing an ocean current field optimization model, using the quantum particle swarm algorithm to optimize the global path and obtain the optimal trajectory.

[0126] The ocean current field optimization model includes:

[0127] The energy required for a small underwater robot to travel from the starting node to the destination node needs to be evaluated by the resistance generated by the ocean current field of each part of the determined path. Consider the connecting path x i-1 and x i Node segment X i-1 X i , the speed of the small underwater robot at any point (x, y, z) on the road section can be described as:

[0128]

[0129] Where, The speed of a small underwater robot in an ocean current environment; is the propulsion speed of the small underwater robot; The ocean current velocity is selected according to the depth layer where the small underwater robot is located. If the current position of the small underwater robot is between two depth layers, the average value of the ocean current velocity of the two layers is taken for approximate calculation. is the ocean current velocity of the kth layer; therefore, the energy loss e of the i-th segment iIt can be described as:

[0130]

[0131] Where v is the resistance coefficient, which is determined by the structural design of the small underwater robot and the properties of the surrounding water; P b is the propulsion power of the small underwater robot, which is proportional to the cube of the propulsion speed of the small underwater robot when it navigates the subpath; J i is the length of the three-dimensional path. The total cost of the small underwater robot navigating along the path is the accumulation of the energy consumption of each sub-path, that is,

[0132]

[0133] Establish the cost function Fitness and transform the multi-objective function constraints into a single-objective problem. The goal 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] Where, L p is the distance of the small underwater robot from the starting point to the target point; E p is the total energy consumption caused by the interference of ocean currents, r(t) and w(t) are weight coefficients that change dynamically with the iteration process, satisfying r(t)+w(t)=1;

[0136]

[0137] Where r max is the initial value of the path length factor; r min is the end value of the path length factor; N c is the current iteration number; N max is the maximum number of iterations of the algorithm;

[0138] The cost function optimization model is:

[0139] P * =argminFitness;

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

[0141]

[0142] Where, P * To minimize the cost function, x s 、xg are the starting point and target point of the small underwater robot respectively; v min 、v max is the constraint of the propulsion speed of the small underwater robot, v b,i is the robot's advancing speed under node i.

[0143] like Figure 3 As shown in the figure, by combining the improved adaptive ant colony 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, the parameters are first initialized and a three-dimensional grid map is constructed through rasterization. The upper-level optimization models the path channel search as a combinatorial optimization task. Based on IAACO, a set of collision-free path channels x is quickly generated in the three-dimensional grid map. channel , focusing on solving the path feasibility and macro navigation structure; the lower-level optimization is based on the channel constraint, channel Internally call QPSO to further refine the path trajectory and obtain a continuous trajectory And calculate its corresponding cost The lower layer algorithm transmits the cost of the optimal path in the channel back to the upper layer, and the upper layer algorithm updates the pheromone according to the cost, and the cycle is repeated to integrate x channel Channel and The path forms a complete solution and determines whether it is the global optimal solution. If not, the adaptive weight adjustment is performed based on whether the loop ends. If the global optimal solution does not improve after continuous iteration or the loop ends, the optimal trajectory is output and the algorithm ends.

[0144] S4. Use the improved particle swarm optimization algorithm to improve the MPC model, and track the trajectory generated by the small underwater robot through the improved MPC model.

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

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

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

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

[0149] Where, ΔF t is the change in thrust of the propeller, ΔF min and ΔF max Represent the minimum and maximum thrust change rates respectively.

[0150] The underwater robot is subject to hydrodynamic constraints, and its acceleration a y Cannot exceed the maximum tolerable thrust F max The ratio of mass m is:

[0151]

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

[0153]

[0154] Where Q and R are the weight matrices of state error and control input respectively; x ref is the expected trajectory, N u is the control time domain; λ is the weight coefficient; ζ is the relaxation factor; R F Weight matrix of actual thrust change rate.

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

[0156]

[0157] Where, ΔU t is the rate of change of the controlled variable at time t, ΔU min With ΔU max are the minimum and maximum values ​​of the control variable change rate respectively; U t is the control value at time t; A u is a Nu×Nu lower triangular matrix with all elements set to 1; U min with U max are the minimum and maximum values ​​of the control quantity respectively; F min The minimum allowable thrust; F max Maximum allowable thrust; F t is the thrust at time t; V t is the robot speed at time t; V min With V max are the minimum and maximum speeds allowed by the robot respectively; the above optimization problem ensures the trajectory tracking accuracy of the underwater robot while improving the stability and energy efficiency of the system by controlling the thrust change and acceleration constraints 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 optimization, improvements are made to improve the computational efficiency and solution accuracy of MPC model optimization.

[0159] The particle swarm optimization algorithm (PSO) is used to globally search the control input sequence U={u0,u1,…,uN}'s approximate optimal initial solution U * Considering that the MPC model contains the disturbance compensation term ΔF(k) and the slack 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, that is, PSO only optimizes the control input u(k), while the disturbance compensation term ΔF(k) and the slack variable ζ are locally optimized by sequential quadratic programming. The objective function of the global optimization phase of the particle swarm optimization algorithm is:

[0160]

[0161] Where N p is the prediction time domain, x ref is the expected trajectory, N u To control the time domain, k is the current discrete time point; i is the sequence number;

[0162] Get the solution U * Finally, the disturbance compensation term ΔF(k) and the slack variable ζ in the MPC model are solved by 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] Where R F is the weight matrix of thrust change rate, F min The minimum allowable thrust; F max The maximum allowable thrust; λ is the weight coefficient;

[0167] Convert the above problem into standard quadratic programming form, the formula is:

[0168]

[0169] stAΔU≤b;

[0170] Where Δ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, and the prediction window is rolled to the next moment, and the above optimization process is re-executed to achieve closed-loop control.

[0172] Improve the prediction time domain: To solve the problem of fixed algorithm parameters, the PSO algorithm is used to search for the optimal time domain combination. The particle optimized by PSO is expressed as P = (N p ,N c ), the particle swarm optimization algorithm is used to search for the optimal time domain combination. The particle swarm improves the applicability through iterative search and adopts the error feedback adjustment rule, so that the controller adopts the optimal time domain in different trajectory stages. The error feedback adjustment rule is:

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

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

[0175] Where, e t represents the trajectory error at time t, and κ is the adjustment coefficient.

[0176] like Figure 4 As shown in the figure, the overall control process of the MPC model is as follows: the robot system first obtains a linear error model based on the robot model and state, the initial state of the environment, and the reference trajectory planned by S1-S3, i.e., the objective function of MPC. The control variable change rate, relaxation factor, and robot thrust change rate in the objective function all need to be optimized and solved using the IPSO algorithm. The system constraints are input into the IPSO algorithm. At this time, the improved particle swarm optimization algorithm first uses PSO to optimize the control variable change. The specific process is as follows: initializing the particle swarm includes confirming the initial data such as the number of particles and the number of iterations. The state of each particle consists of the current position and speed. The particle calculates the current fitness function based on the individual historical optimal position and the global optimal position for update. The particles are sorted according to this result and the optimal fitness is updated. At this time, the adaptive time domain optimization prediction control sequence performed by the traditional PSO algorithm is external. When the convergence condition (such as fitness stability or reaching the iteration limit) is met, the loop is jumped out; SQP is executed for local refinement search, that is, SQP is used to optimize the relaxation factor and the robot thrust change rate. After the above IPSO calculation is completed, three output values ​​will be generated, namely the control variable change rate, the relaxation factor and the robot thrust change rate. All three are input into the MPC optimization algorithm to distribute the robot's propulsion and speed control, and monitor the robot's actual feedback in real time for stroke closed-loop control.

[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 dynamic environments in the prior art, and significantly improves the task execution capability and adaptability of underwater robots in complex environments.

[0178] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention rather than to limit the same. Although the present invention has been described in detail with reference to the preferred embodiments, those skilled in the art should understand that they can still modify or replace the technical solutions of the present invention with equivalents, and these modifications or equivalent replacements 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 for a small underwater robot, characterized in that the steps include: S1, using the improved adaptive ant colony algorithm to generate the global path of the small underwater robot in the three-dimensional underwater environment; S2, considering dynamic obstacles, locally optimize the global path generated in step S1 by integrating the dynamic window method to obtain the optimized path; S3, considering the ocean current environment, using the quantum particle swarm algorithm to optimize the global path generated in step S1 to obtain the optimized path; S4. Use the improved particle swarm optimization algorithm to improve the MPC model, and track the trajectory generated by the small underwater robot through the improved MPC model.

2. A trajectory planning and tracking method for a small underwater robot according to claim 1, characterized in that: Improved adaptive ant colony algorithm includes: S11, setting multiple starting point sets and assigning each ant to a starting point set for searching, so that each ant is responsible for searching only its corresponding part of the area; S12. Consider the global path and improve the heuristic function, including: The target point distance factor d and the adaptive adjustment factor ε driven by the number of iterations are introduced into the heuristic function. The formula is: Where η ij (t) is the path heuristic information from grid i to grid j at time t; d jG is the distance from the next position to the target point; d ij is the distance from the current position to the next position; I c is the current iteration number; I max is the maximum number of iterations; Then, the transition probability is improved by introducing the angle influence factor and obstacle removal factor ξ io (t), improved transition probability The formula is: Where, τ ij (t) is the pheromone concentration; α and β are parameter constants; allowed k represents the set of feasible adjacent nodes for the kth ant; the denominator represents the set of feasible adjacent nodes for the kth ant at node i sum; S13. Use the elite retention strategy to optimize the pheromone update of the ant colony algorithm, including: After each iteration, select r ants with good pathfinding performance as elite ants and assign them a priority order. Additional pheromone enhancement is given to accelerate the accumulation of pheromones on high-quality paths. The improved pheromone update rule is: t ij (t+1)=(1-ρ)τ ij (t)+ρΔτ ij (t)+Δτ ij * ; Where ρ is the global pheromone volatility coefficient, Δτ ij (t) is the increment of pheromone on the current iterative path, Δτ ij * is the pheromone increment of elite ants; r is the number of elite ants; Q is the pheromone release constant, L * is the optimal solution path length; The ant contribution is obtained by weighting the ant's position. Considering only the first 1 / 3 of the ants, the amount of pheromone obtained by the path is proportional to the ant's position. The pheromone increment update formula is: Where M is the total number of ants; μ is the ant rank; L av is the average path length; L μ is the length of the path traversed by the μth ant.

3. A trajectory planning and tracking method for a small underwater robot according to claim 2, characterized in that: In step S2, the fusion dynamic window method is used to locally optimize the global path generated in step S1, including: S21, introducing a path smoothing factor and a target point distance factor to improve the trajectory evaluation function, and adaptively adjusting each weight factor, and using the improved trajectory evaluation function to select the optimal local trajectory; S22. Considering the motion state of the obstacle, for each moment, combined with the dynamic window method, the small underwater robot calculates a local motion window based on the current motion state and the obstacle position, 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 determines whether the local path is feasible based on the obstacle avoidance evaluation function.

4. A trajectory planning and tracking method for a small underwater robot according to claim 3, characterized in that: The improved trajectory evaluation function R(v,ω) formula is: R(v,ω)=ε(a'·heading(v,ω)+b'·dist obs (v,ω)+c'·vel(v,ω)+μ·f smooth (v,ω)+δ·dist goal (v,ω)); Where heading(v,ω) is the target direction angle evaluation subfunction, which measures the deviation between the current trajectory of the small underwater robot and the target direction; 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 speed 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 angular velocity change of the small underwater robot. The formula is: Where ω 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 is the maximum allowable angular velocity; a', b', c' are adaptive weight factors, and the adjustment formula is: Where length is the total length of the preset trajectory, a, b, and c are the weight coefficients of the target direction angle evaluation subfunction, obstacle distance factor, and speed factor in the traditional dynamic window method, respectively.

5. A trajectory planning and tracking method for a small underwater robot according to claim 4, characterized in that: The obstacle avoidance evaluation function J(v,ω) formula is: J(v,ω)=α1·Safety(v,ω)-α2·Curvature(v,ω)-α3·Speed(v,ω); Where α1, α2, and α3 are the weights of obstacle avoidance safety (v, ω), path smoothness Curvature (v, ω), and speed factor Speed ​​(v, ω), respectively.

6. A trajectory planning and tracking method for a small underwater robot according to claim 1, characterized in that: Step S3 uses the quantum particle swarm algorithm to optimize the global path generated in step S1, including: Establish the cost function Fitness and transform the multi-objective function constraints into a single-objective problem. The goal is to find the path with the minimum total cost. The cost function formula is: Fitness=r(t)L p +w(t)E p ; Where, L p is the distance of the small underwater robot from the starting point to the target point; E p is the total energy consumption caused by the interference of ocean currents, r(t) and w(t) are weight coefficients that change dynamically with the iteration process, satisfying r(t)+w(t)=1; Where r max is the initial value of the path length factor; r min is the end value of the path length factor; N c is the current iteration number; N max is the maximum number of iterations of the algorithm; The cost function optimization model is: P * =ang min Fitness; s.t. P(t s )=x s ,P(t g )=x g ; Where, P * To minimize the cost function, x s 、x g are the starting point and target point of the small underwater robot respectively; v min 、v max is the constraint of the propulsion speed of the small underwater robot, v b,i is the robot's advancing speed under node i.

7. The trajectory planning and tracking method for a small underwater robot according to claim 1, characterized in that: In step S4, the MPC model is improved by using the particle swarm optimization algorithm, including: Improve the computational complexity of the MPC model: Use the particle swarm optimization algorithm to optimize the control input u(k) and obtain the solution U * , the objective function of the global optimization phase of the particle swarm optimization algorithm is: Where N p is the prediction time domain, x ref is the expected trajectory, N u To control the time domain, k is the current discrete time point and i is the sequence number; The disturbance compensation term ΔF(k) and the slack variable ζ in the MPC model are solved by gradient-based sequential quadratic programming, and the formula is: st x(k+1)=f(x(k),u(k)); F min ≤F t ≤F max ; Where R F is the weight matrix of thrust change rate, F t is the propulsion force of the robot at time t; F min The minimum allowable thrust; F max The maximum allowable thrust; λ is the weight coefficient; The first control variable u0 in the optimized control sequence is selected and applied to the current control cycle, and the prediction window is rolled to the next moment, and the above optimization process is re-executed to achieve closed-loop control; The prediction time domain is improved, and the particle swarm optimization algorithm is used to search for the optimal time domain combination. The particle swarm performs iterative search and adopts the error feedback adjustment rule to make the controller use the optimal time domain in different trajectory stages. The error feedback adjustment rule is: N p (k+1)=N p (k)+κ(e t -is t-1 ): e t =||x(k)-x ref (k)||; Where, e t represents the trajectory error at time t, and κ is the adjustment coefficient.

Images