Path planning and tracking control method for unmanned vehicle

Abstract

The invention discloses a path planning and tracking control method, device and equipment for an unmanned vehicle, and belongs to the field of underwater vehicle path control, and the method comprises the steps: obtaining the initial state of the unmanned vehicle and the global information of a marine environment; iteratively searching an optimal energy-saving path of the unmanned vehicle by using a preset energy-saving path planning algorithm based on the initial state and the known ocean information, and constructing the energy-saving path planning algorithm by using an improved particle swarm optimization based on quantum behaviors; when the unmanned vehicle navigates, a preset path tracking control algorithm is adopted to control the unmanned vehicle to track the optimal energy-saving path; the unmanned vehicle is controlled based on the energy-saving path planning algorithm and the path tracking control algorithm, and the purpose of prolonging the operation time of the unmanned vehicle is achieved.

Description

Technical Field

[0001] This application relates to the technical field of underwater vehicle path control, and particularly to a path planning and tracking control method, device and equipment for an unmanned underwater vehicle. Background Art

[0002] An unmanned underwater vehicle (UUV) is an underwater vehicle that can achieve various underwater tasks through autonomous control or remote control. As a new type of intelligent unmanned platform on the sea, UUV plays an important role in the development and monitoring of marine resources. Currently, UUV is developing towards intelligence and can perform autonomous navigation and tasks through issued commands. However, when performing long-term duty tasks, UUV must consider the key factor of energy consumption. Therefore, it is particularly important to extend the operation time when UUV performs autonomous navigation and tasks. Summary of the Invention

[0003] The main purpose of this application is to provide a path planning and tracking control method, device and equipment for an unmanned underwater vehicle, aiming to solve the technical problem of the short operation time when UUV performs autonomous navigation and tasks.

[0004] To achieve the above purpose, this application provides a path planning and tracking control method for an unmanned underwater vehicle, including: obtaining the initial state of the unmanned underwater vehicle and the global information of the marine environment; based on the initial state and the global information of the marine environment, using a preset energy-saving path planning algorithm to iteratively search for the optimal energy-saving path of the unmanned underwater vehicle; when the unmanned underwater vehicle is sailing, using a preset path tracking control algorithm to control the unmanned underwater vehicle to track the optimal energy-saving path, and the path tracking control algorithm is calculated based on the sliding mode energy-saving control algorithm; wherein, the process of constructing the energy-saving path planning algorithm includes: obtaining a new position update expression and a new fitness function, the new position update expression is obtained according to the position update expression of the particle swarm algorithm based on quantum behavior, and the new fitness function is obtained according to a pre-constructed static power consumption expression, a pre-constructed dynamic power consumption expression and a pre-constructed danger function, and the static power consumption expression is determined based on the initial state and the global information of the marine environment; randomly generating particles after population initialization based on the Latin hypercube sampling method; constructing the energy-saving path planning algorithm according to the new position update expression, the new fitness function and the particles after population initialization.

[0005] Optionally, the new position update expression is obtained according to the position update expression of the particle swarm algorithm based on quantum behavior, including: obtaining the first expression of the attraction factor of the particle swarm algorithm based on quantum behavior; introducing a dynamic weight factor, a random number factor, and a sine-cosine factor into the first expression of the attraction factor to obtain the second expression of the attraction factor, and the second expression of the attraction factor is:

[0006] where, is a random number of is a random number of is a random number of is the dynamic weight factor, is the second expression of the attraction factor, and are respectively the individual optimal position and the global optimal position of the particle in the -th iteration in the -dimensional space; introducing the Levy flight algorithm and a step size control factor into the first expression of the attraction factor to obtain the third expression of the attraction factor, and the third expression of the attraction factor is:

[0007] where, is the third expression of the attraction factor, is the step size control factor, is the random path, represents the dot product operation, and are respectively the individual optimal position and the global optimal position of the particle i in the -th iteration in the -dimensional space; using the second expression of the attraction factor and the third expression of the attraction factor to replace the first expression of the attraction factor in the position update expression in the particle swarm algorithm based on quantum behavior respectively to obtain the new position update expression, where the new position update expression is:

[0008] where, is the average optimal extreme value, defined as the average value of the individual optimal positions of all particles in the particle swarm, is the contraction-expansion factor, u is a random number, when it is greater than 0.5, the sign takes the + sign, otherwise takes the - sign, is the particlei The current fitness value is the minimum fitness value of the particle up to now.

[0009] Optionally, the iterative search for the optimal energy-saving path of the unmanned vehicle using a preset energy-saving path planning algorithm includes: obtaining the kinematic model and dynamic model of the unmanned vehicle; calculating the current individual optimal value and current global optimal value of each particle based on the new position update expression; based on the particles after population initialization, using the energy-saving path planning algorithm to iteratively search for the energy-saving path of the unmanned vehicle, and obtaining the optimal energy-saving path after reaching the preset iterative termination condition; wherein, in each iteration, according to the new position update expression, update the current position of each particle to obtain each particle after position update, calculate the new fitness value of each particle after position update according to the new fitness function, and compare each new fitness value with the individual optimal value in the previous iteration to obtain the current individual optimal value and the corresponding particle, use the current individual optimal value as the current global optimal value, assign the current individual optimal value to each particle, and control each particle to move in the direction of the particle corresponding to the current individual optimal value to obtain a planned path.

[0010] Optionally, the new fitness function is obtained according to a pre-constructed static power consumption expression, a pre-constructed dynamic power consumption expression, and a pre-constructed hazard function, and includes: obtaining the corrected kinematic model and corrected dynamic model of the unmanned vehicle; based on the corrected kinematic model and corrected dynamic model, obtaining the actuator thrust and speed of the unmanned vehicle; obtaining the dynamic power consumption expression of the unmanned vehicle based on the actuator thrust and the speed; obtaining the static power consumption expression and the hazard function; based on the static power consumption expression, the hazard function, and the dynamic power consumption expression obtained by summation processing, obtaining a new fitness function.

[0011] Optionally, the control of the unmanned vehicle to track the optimal energy-saving path using a preset path tracking control algorithm includes: obtaining the pre-constructed tracking error of the unmanned vehicle, wherein the tracking error is constructed based on the speed error and the energy error; constructing a sliding mode surface based on the tracking error, and calculating the maximum gradient value and the sliding mode control force according to a first objective function, wherein the first objective function is constructed according to the sliding mode surface; calculating the control force of the actuator of the unmanned vehicle based on the maximum gradient value and the sliding mode control force, and controlling the actuator to track the optimal energy-saving path based on the control force.

[0012] Optionally, calculating the control force of the actuator of the unmanned vehicle based on the maximum gradient value and the sliding mode control force includes: obtaining the tracking error expression, where the tracking error expression is:

[0013] where is the velocity tracking error, is the position tracking error, is the six-degree-of-freedom velocity vector in the ocean current environment, is the desired velocity, is the six-degree-of-freedom position vector under the ocean current model, is the desired position; obtaining the expression of the sliding mode surface, where the expression of the sliding mode surface is:

[0014] where , is the preset three-dimensional control parameter; obtaining the derivative expression of the sliding mode surface according to the expression of the sliding mode surface, where the derivative expression of the sliding mode surface is:

[0015] where is the inertia matrix, is the control force of the actuator of the unmanned vehicle, is the environmental disturbance force, is the Coriolis centripetal force matrix, is the fluid damping matrix, is the restoring force and moment vector generated by gravity and buoyancy, is the coordinate transformation matrix, is the derivative of the desired velocity, is the derivative of the desired position, is the six-degree-of-freedom velocity vector in the ocean current environment, is the water flow velocity in the ground coordinate system; calculating the sliding mode control force according to the derivative expression of the sliding mode surface; constructing a first objective function based on the tracking error expression, the sliding mode control force and the adjustment term; obtaining the Hamiltonian equation of the first objective function based on the first objective function, and obtaining the maximum gradient value according to the Hamiltonian equation of the first objective function; obtaining the control force of the actuator of the unmanned vehicle based on the sum of the maximum gradient value and the sliding mode control force.

[0016] Optionally, obtaining the corrected kinematic model and the corrected dynamic model of the unmanned vehicle includes: obtaining the kinematic model and the dynamic model of the unmanned vehicle in a still water environment, where the expression of the kinematic model is:

[0017] Among them, , , , , are the position coordinates of the unmanned vehicle in the ground coordinate system, is the attitude angle of the unmanned vehicle, , , are the linear velocities of the unmanned vehicle in the static water environment, , , are the angular velocities of the unmanned vehicle in the static water environment; the expression of the dynamic model is:

[0018] Among them, is the six-degree-of-freedom velocity vector in the static water environment, is the inertia matrix, is the Coriolis centripetal force matrix, is the fluid damping matrix, is the system control force, is the ocean environmental force, including three parts: sea wind, sea wave and ocean current; replacing the velocity parameters in the kinematic model and dynamic model in the static water environment with the velocity of the unmanned vehicle relative to the ocean current, the modified kinematic model and the modified dynamic model are obtained, where the modified kinematic model is:

[0019] Among them, , is the velocity of the unmanned vehicle relative to the ocean current, is the velocity vector of the water flow in the body coordinate system, is the ground velocity of the unmanned vehicle in the body coordinate system, the water flow velocity in the ground coordinate system, J is the conversion from the body coordinate system to the ground coordinate system; the modified dynamic model is:

[0020] Among them, is the environmental disturbance force, is the control force of the actuator of the unmanned vehicle.

[0021] Optionally, obtaining the Hamiltonian equation of the first objective function based on the first objective function, and obtaining the maximum gradient value according to the Hamiltonian equation of the first objective function, includes: obtaining the expression of the first objective function, where the expression of the first objective function is:

[0022] Where, is the tracking start time, is the tracking end time, Q1 and Q2 are two preset adjustment values, and are two specific gravity adjustment factors, is the inertia matrix, is the control force of the actuator of the unmanned vehicle, is the speed of the unmanned vehicle relative to the ocean current, is the water flow velocity in the ground coordinate system; rewriting the expression of the first objective function based on the Hamiltonian equation expression to obtain the Hamiltonian equation of the first objective function, where the Hamiltonian equation expression of the first objective function is:

[0023] Where, is the tracking error, is the sliding mode control force, and are the weight adjustment matrices, The function uses the hyperbolic tangent smoothing function to eliminate discontinuities and singularities in the calculation, is the weight adjustment value, is the inertia matrix, is the speed of the unmanned vehicle relative to the ocean current, is the water flow velocity in the ground coordinate system, is the control force of the actuator of the unmanned vehicle; taking the derivative of the Hamiltonian equation expression of the first objective function to obtain the maximum gradient value.

[0024] To achieve the above object, the present application further provides a path planning and tracking control device for an unmanned vehicle, including: an acquisition module, configured to acquire the initial state of the unmanned vehicle and the global information of the marine environment; a path planning module, configured to iteratively search for the optimal energy-saving path of the unmanned vehicle by using a preset energy-saving path planning algorithm based on the initial state and the global information of the marine environment; a path tracking module, configured to control the unmanned vehicle to track the optimal energy-saving path by using a preset path tracking control algorithm when the unmanned vehicle is navigating, and the path tracking control algorithm is calculated based on the sliding mode energy-saving control algorithm; wherein, the process of constructing the energy-saving path planning algorithm includes: acquiring a new position update expression and a new fitness function, the new position update expression is obtained according to the position update expression of the particle swarm algorithm based on quantum behavior, and the new fitness function is obtained according to a pre-constructed static power consumption expression, a pre-constructed dynamic power consumption expression, and a pre-constructed danger function, and the static power consumption expression is determined based on the initial state and the global information of the marine environment; randomly generating particles after population initialization based on the Latin hypercube sampling method; constructing the energy-saving path planning algorithm according to the new position update expression, the new fitness function, and the particles after population initialization.

[0025] To achieve the above object, the present application further provides an electronic device, including: at least one processor; and a memory communicatively connected to the at least one processor; wherein, the memory stores instructions executable by the at least one processor, and the instructions are executed by the at least one processor to enable the at least one processor to execute the above path planning and tracking control method for an unmanned vehicle.

[0026] A path planning and tracking control method, device, and equipment for an unmanned vehicle provided by an embodiment of the present application obtain the initial state of the unmanned vehicle and the global information of the marine environment; based on the initial state, the global information of the marine environment, and the known environmental information, iteratively search for the optimal energy-saving path of the unmanned vehicle by using a preset energy-saving path planning algorithm, wherein, an energy-saving path planning algorithm is constructed by using an improved particle swarm algorithm based on quantum behavior; when the unmanned vehicle is navigating, control the unmanned vehicle to track the optimal energy-saving path by using a preset path tracking control algorithm; the present application realizes the technical effect of extending the operation time of the unmanned vehicle based on the energy-saving path planning algorithm and the path tracking control algorithm. Description of the Drawings

[0027] Figure 1 It is a schematic flowchart provided for an embodiment of the path planning and tracking control method for an unmanned vehicle of the present application; FIG. 2(a) is a schematic diagram of different coordinate systems during the path planning process provided by an embodiment of the path planning and tracking control method for the unmanned vehicle of the present application; FIG. 2(b) is a schematic diagram for comparing energy-saving paths of different algorithms during the path planning process provided by an embodiment of the path planning and tracking control method for the unmanned vehicle of the present application; FIG. 2(c) is a schematic diagram for comparing the convergence situations of different algorithms during the path planning process provided by an embodiment of the path planning and tracking control method for the unmanned vehicle of the present application; FIG. 2(d) is a schematic diagram of the shortest path under the DSCL-QPSO algorithm during the path planning process provided by an embodiment of the path planning and tracking control method for the unmanned vehicle of the present application; FIG. 2(e) is a schematic diagram of the energy-saving path under the DSCL-QPSO algorithm during the path planning process provided by an embodiment of the path planning and tracking control method for the unmanned vehicle of the present application; Figure 3 FIG. is a schematic diagram for comparing sampling results provided by an embodiment of the path planning and tracking control device for the unmanned vehicle of the present application; FIG. 4(a) is a schematic diagram of path tracking provided by an embodiment of the path planning and tracking control method for the unmanned vehicle of the present application; FIG. 4(b) is a schematic diagram of pose and related errors provided by an embodiment of the path planning and tracking control method for the unmanned vehicle of the present application; Figure 5 FIG. is a structural block diagram provided by an embodiment of the path planning and tracking control device for the unmanned vehicle of the present application.

[0028] The realization, functional features and advantages of the object of the present application will be further described with reference to the embodiments and the accompanying drawings. Detailed Embodiments

[0029] It should be understood that the specific embodiments described herein are only used to explain the present application and are not used to limit the present application.

[0030] Based on the purpose of saving the energy consumption of UUVs, the present invention proposes a complex sea area energy-saving path planning and path tracking control method based on a particle swarm optimization algorithm.

[0031] First, the present application establishes the kinematic and dynamic models of the UUV and the mathematical model of the ocean environment. Secondly, the present application extracts the electronic chart information and environmental information, converts them into the data required for path planning, and then models the path planning data. Thirdly, for the static obstacles in the navigation area, combined with the ocean environmental conditions, the present application adopts an improved quantum-behavior-based particle swarm optimization algorithm, and obtains an energy-saving path through appropriate objective functions and constraint conditions. Finally, the present application uses the sliding-mode energy-saving control method with optimal control parameters for the generated energy-saving path to perform path tracking.

[0032] Referring to Figure 1 , Figure 1 FIG. is a flowchart of a path planning and tracking control method for an unmanned vehicle provided by an embodiment of the present application. This method can be executed by a processor in the unmanned vehicle. The path planning and tracking control method of the unmanned vehicle may include: S10. Obtain the initial state of the unmanned vehicle and the global information of the ocean environment; Specifically, before using this method to perform path planning and tracking control on the unmanned vehicle, the processor first needs to construct a mathematical model.

[0033] Among them, the mathematical model of the initial state of the unmanned underwater vehicle (hereinafter referred to as UUV) can be modeled as the UUV kinematic model:

[0034]

[0035]

[0036] Among them, the position coordinates in the ground coordinate system are defined as , , , the three Euler angles formed by the vehicle coordinate system and the ground coordinate system , the Euler angle is defined as the attitude angle of the UUV. The attitude angle is used to describe the navigation attitude of the UUV. The linear velocity and angular velocity of the UUV are represented by , , , p , , .

[0037] The dynamic model of the UUV is as follows:

[0038] Among them, is the six-degree-of-freedom velocity vector, is the inertia matrix, is the Coriolis centripetal force matrix, is the fluid damping matrix, is the system control force, is the ocean environmental force.

[0039] The above UUV kinematic model is established in a static water environment. However, in actual situations, the influence of dynamic water flow on the UUV's motion needs to be considered. In this application, the UUV's kinematic model under the influence of ocean currents is described by using the UUV's velocity relative to the ocean current to replace the velocity parameter in the above UUV kinematic model.

[0040] The processor configures the water flow velocity in the ground coordinate system as:

[0041] where, are the velocity components in three directions respectively.

[0042] The UUV's velocity relative to the ocean current is configured as:

[0043] where, is the UUV's velocity relative to the ground in the vehicle coordinate system; is the water flow velocity in the vehicle coordinate system, and .

[0044] Thus, the UUV kinematic model in a dynamic water flow environment can be reconfigured as:

[0045] where, is the UUV's velocity relative to the ocean current, is the water flow velocity in the ground coordinate system, J is the transformation from the vehicle coordinate system to the ground coordinate system; In the above UUV dynamic model, it is assumed that the water flow around the UUV is stationary. In actual situations, the movement of the fluid around the UUV often needs to be considered. At this time, the UUV dynamic model in a dynamic water flow environment should be expressed as:

[0046] where, v is the six-degree-of-freedom velocity vector, is the six-degree-of-freedom relative velocity vector, and represent the rigid body and the added mass matrix respectively, is the total inertia matrix, and represent the rigid body and the additional Coriolis centripetal force matrix respectively, is the total Coriolis centripetal force matrix, is the fluid damping matrix, is the restoring force and moment vector generated by gravity and buoyancy, is the control force matrix of the vehicle, is the environmental disturbance force matrix.

[0047] Assume that the change in ocean current velocity is relatively slow and the change in ocean current velocity is a small quantity relative to the UUV velocity. Then, the dynamic model of the UUV in the dynamic water flow environment can be simplified as:

[0048] where, is the generalized velocity of the UUV relative to the fluid in the vehicle coordinate system, that is , is the velocity of the water flow in the vehicle coordinate system.

[0049] Next, the processor constructs a collision avoidance model as needed. When establishing the collision avoidance model, the processor only considers static obstacles. If it is necessary to expand the static obstacle area, a circular obstacle area is formed with the center of the obstacle as the center and the distance from the center to the longest edge as the radius.

[0050] Referring to Fig. 2(a), the processor establishes a UUV coordinate system with the position of the UUV as the origin. Similarly, an obstacle coordinate system is established with the center of the obstacle as the origin. The corresponding coordinates of the UUV in the UUV coordinate system are , and the corresponding coordinates in the obstacle coordinate system are . The coordinates of the obstacle relative to the UUV are , and the orientation angle of the UUV pointing to the obstacle is .

[0051] The processor translates the origin of the obstacle coordinate system to the origin of the UUV coordinate system to make them coincide. The above translation process can be expressed as:

[0052]

[0053] where, is the transformation matrix. Considering the safety and actual fault tolerance of the UUV in this application, the actual range of the UUV is expanded to a circle.

[0054] Next, the ocean current model is established. The ocean current field information in the ocean current model can be obtained through satellite observations and high-frequency radars, or through mooring experiments for on-site measurement. In this application, multiple viscous Lamb vortices are superimposed to simulate the flow field in the ocean, so as to simulate the influence of ocean currents on the movement of UUVs. The viscous Lamb vortices simulate the flow field in the ocean in a superimposed manner, which can not only achieve a coherent fluid structure but also be easy to analyze and process. Therefore, it is widely used in the path planning of UUVs. The velocity vector of the viscous Lamb vortices adopted in this application is:

[0055] where, 、 are the velocity components of the ocean current in the horizontal and vertical directions respectively.

[0056] In the ocean environment, since the seawater flow in the vertical direction is much smaller than that in the horizontal direction, the influence of the seawater flow in the vertical direction is generally not considered. Assuming that the UUV operates on a two-dimensional plane, then:

[0057]

[0058] where, is the vorticity of the vortex, is the fluid viscosity, 、 are the Laplace operator and the gradient respectively, and then we get:

[0059]

[0060]

[0061] where, is the current position vector, is the vortex center point vector, is the vortex radius, is the vortex intensity coefficient, is the vortex intensity, x , y is the corresponding position coordinate, x 0 ,y 0 is the corresponding center coordinate.

[0062] S20. Based on the initial state and the global information of the ocean environment, use the preset energy-saving path planning algorithm to iteratively search for the optimal energy-saving path of the unmanned vehicle, where an energy-saving path planning algorithm is constructed by using the improved particle swarm optimization algorithm based on quantum behavior; In an embodiment of the present invention, step S20 may specifically include the following execution process: S201. Construct a new position update expression and a new fitness function according to the quantum behavior-based particle swarm optimization algorithm to obtain an improved quantum behavior-based particle swarm optimization algorithm; S202. Construct an energy-saving path planning algorithm according to the new position update expression and the new fitness function.

[0063] It should be noted that the quantum behavior-based particle swarm optimization algorithm (Quantum Behavior Particle Swarm Optimization, QPSO) is an improved swarm intelligence algorithm, which is improved from the classical PSO algorithm. In this application, the quantum theory is applied to the solution of combinatorial optimization problems. Specifically, the interaction between population individuals is controlled by introducing quantum bits, so that it can effectively jump out of the local optimal solution.

[0064]

[0065]

[0066]

[0067]

[0068] Among them, is the number of iterations ( ); is the number of particles; the dimension of the particle; represents the particle i at the th iteration, the velocity vector and position vector of the particle i at the th dimension space; and represent at the th iteration, the individual optimal position and global optimal position of the particle i at the th dimension space, is the average optimal extreme value, defined as the average value of the individual optimal positions of all particles in the particle swarm; and are both random variables, and their values are between 0 and 1. u is a random number between 0 and 1. When it is greater than 0.5, the sign takes the + sign, otherwise it takes the - sign. is the contraction and expansion factor. is the attraction factor, which is a key parameter for adjusting the convergence behavior of QPSO.

[0069] It should be noted that the particles in the population after initialization are randomly generated based on Latin Hypercube Sampling.

[0070] It should be noted that Latin Hypercube Sampling (LHS) originated in the field of statistics and is a stratified random sampling method that can effectively sample from the distribution range of variables. Latin Hypercube Sampling divides each vector in the D-dimensional matrix variable into uniform layers within a specific region, and randomly selects and generates random numbers in each layer to form a sample. The main implementation steps of Latin Hypercube Sampling LHS are as follows: Step 1: Determine the population size , the dimension , and the upper and lower limits of the search space.

[0071] Step 2: Divide the upper and lower limits of the search space into equal intervals, and divide the original search space into small cubic spaces.

[0072] Step 3: Generate a matrix , where each column of the matrix is a complete permutation of equal intervals.

[0073] Step 4: Select a small cube in each row of the matrix , and randomly generate an individual in each small cube, so that individuals can be extracted.

[0074] It should be noted that in most existing meta-heuristic algorithms, the initialization of the population uses the method of random sampling, that is, randomly generating a population distributed between the upper and lower limits of the search space. This method may lead to uneven distribution of individuals in the space and clustering of some individuals, thus reducing the diversity of the initial population. This application uses LHS for population initialization, which can generate individuals that are relatively evenly dispersed in a specific region. Compared with random sampling, LHS can avoid clustering of individuals in the initial population, thereby improving the efficiency of the optimization process. In Figure 3 , the positions of individuals using traditional random sampling and the positions of individuals after population initialization based on LHS are shown. By comparison, it can be clearly seen that the sample distribution obtained by LHS is more uniform. Therefore, the quality of the population generated by this application using LHS for population initialization is higher, and the population has diversity.

[0075] After initializing the population, step S201 may further include the following execution process: S2011. Calculate the current individual optimal value and the current global optimal value of each particle based on the new position update expression; In the embodiment of the present application, step S2011 may specifically include the following execution process: S20111. Obtain the first expression of the attraction factor of the particle swarm algorithm based on quantum behavior and; S20112. Introduce a dynamic weight factor, a random number factor, and a sine-cosine factor into the first expression of the attraction factor to obtain the second expression of the attraction factor. The expression of the second expression of the attraction factor is:

[0076] where, is a random number of is a random number of is a random number of is the dynamic weight factor, is the starting coefficient, T is the iteration termination number, is the second expression of the attraction factor, and respectively represent the individual optimal position and the global optimal position of the particle in the th iteration in the th dimensional space; S20113. Introduce the Levy flight algorithm and the step size control factor into the first expression of the attraction factor to obtain the third expression of the attraction factor. The third expression of the attraction factor is:

[0077] where, is the third expression of the attraction factor, is the step size control factor, is the random path, and respectively represent the individual optimal position and the global optimal position of particle i in the th iteration in the th dimensional space, represents the dot product operation; S20114. Introduce the second expression of the attraction factor and the third expression of the attraction factor into the position update expression in the particle swarm algorithm based on quantum behavior to obtain a new position update expression. Among them, the new position update expression is:

[0078] where, is the average optimal extreme value, defined as the average of the individual optimal positions of all particles in the particle swarm. is the contraction and expansion factor. u is a random number. When it is greater than 0.5, the sign takes the plus sign, otherwise it takes the minus sign. is the current fitness value of the particle. is the minimum fitness value of the particle up to now. S20115. Obtain a new fitness function according to the sum of the pre-built static power consumption expression, the pre-built dynamic power consumption expression, and the pre-built hazard function.

[0079] Specifically, before step S20115, the method may further include the following execution steps: Obtain the corrected kinematic model and the corrected dynamic model of the unmanned vehicle. Based on the corrected kinematic model and the corrected dynamic model, obtain the actuator thrust and speed of the unmanned vehicle. Based on the actuator thrust and speed, obtain the dynamic power consumption of the unmanned vehicle.

[0080] Specifically, obtain the predefined actual total navigation energy consumption expression, and the actual total navigation energy consumption is defined as:

[0081] Static power consumption is the fixed energy required for the computing, sensing, and navigation devices to maintain the normal operation of the UUV, and the expression can be as follows:

[0082] where represents the static total power. represents the total navigation time.

[0083] The calculation of the thruster power consumption function needs to consider the specific ocean current information to ensure that the thrust can be accurately evaluated and calculated in different ocean environments. This application uses the method of piecewise integration for calculation. represents the dynamic power consumption expression, and the dynamic power consumption expression is as follows:

[0084] where represents the actuator thrust. represents the corresponding waypoint. represents the six-degree-of-freedom speed of the UUV in the ocean current environment.

[0085] In addition, the processor also needs to set the obstacle hazard function. Denote the coordinates of a discrete point on the path. To ensure the safety of the UUV, a danger function needs to be set. If the obstacle area is represented as , then the danger function can be expressed as

[0086] Thus, the processor configures the fitness function as:

[0087] where and are weight coefficients, which determine the proportion of each cost in the total cost function.

[0088] S2012. Based on the particles after population initialization, use the energy-saving path planning algorithm to iteratively search for the energy-saving path of the unmanned vehicle. After reaching the preset iteration termination condition, obtain the optimal energy-saving path. Among them, in each iteration, update the current position of each particle according to the new position update expression to obtain each particle after position update; calculate the new fitness value of each particle after position update according to the new fitness function, and compare each new fitness value with the individual optimal value in the previous iteration to obtain the current individual optimal value and the corresponding particle. Take the current individual optimal value as the current global optimal value, assign the current individual optimal value to each particle, and control each particle to move in the direction of the particle corresponding to the current individual optimal value to obtain a planned path. Continue to iterate until the preset iteration termination condition is reached to obtain the optimal energy-saving path.

[0089] Based on the above improved particle swarm algorithm and fitness function, path planning can be completed.

[0090] S30. When the unmanned vehicle is navigating, use a preset path tracking control algorithm to control the unmanned vehicle to track the optimal energy-saving path. Among them, a path tracking control algorithm is constructed based on the sliding mode energy-saving control algorithm.

[0091] In the embodiment of the present invention, step S30 may specifically include the following execution process: S301. Obtain the tracking error of the pre-constructed unmanned vehicle, where the tracking error is constructed based on the speed error and the energy error; S302. Construct a sliding mode surface based on the tracking error, and calculate the maximum gradient value of the first objective function and the sliding mode control force, where the first objective function is constructed according to the sliding mode surface; S303. Calculate the control force of the actuator of the unmanned vehicle based on the maximum gradient value and the sliding mode control force, and control the actuator to track the optimal energy-saving path based on the control force.

[0092] In the embodiment of the present invention, step S303 may specifically include the following execution process: S3031. Obtain the tracking error expression, and the tracking error expression is:

[0093] where is the velocity tracking error, is the position tracking error, is the six-degree-of-freedom velocity vector in the ocean current environment; is the desired velocity; is the six-degree-of-freedom position vector under the ocean current model; is the desired position.

[0094] S3032. Obtain the expression of the sliding mode surface, and the expression of the sliding mode surface is:

[0095] where , represents the preset three-dimensional control parameter, is the objective function; The control objective of this application is:

[0096] where is the first derivative of the objective function; The processor can obtain the Lyapunov function:

[0097] According to the Lyapunov stability theory, to ensure global asymptotic stability, the derivative needs to be negative definite, that is, the following conditions need to be satisfied:

[0098] To meet this condition, only , , can ensure that the derivative is negative definite, where the degree of freedom i = 1, 2, 3.

[0099] This function is optimized by a smoothing function to obtain:

[0100] S3033. Obtain the derivative expression of the sliding mode surface according to the expression of the sliding mode surface, and the derivative expression of the sliding mode surface is:

[0101] Expanding the above formula, we can get:

[0102]

[0103] Among them, represents the ocean environmental force. According to the above formula, can be obtained, which is the control force of the actuator of the unmanned vehicle. is the inertia matrix, is the Coriolis centripetal force matrix, is the fluid damping matrix, and S1 and S2 are preset adjustment matrices.

[0104] S3034. Calculate the sliding mode control force according to the derivative expression of the sliding mode surface; S3035. Construct the first objective function based on the tracking error expression, the sliding mode control force, and the adjustment term; S3036. Obtain the Hamiltonian equation of the first objective function based on the first objective function, and obtain the maximum gradient value according to the Hamiltonian equation of the first objective function; Specifically, step S3036 may include the following execution process: S30361. Obtain the first objective function expression, and the expression of the first objective function is:

[0105] Among them, represents the start time of tracking, represents the end time of tracking, Q1 and Q2 are two preset adjustment values, and are two specific gravity adjustment factors, is the inertia matrix, is the control force of the actuator of the unmanned vehicle; The first objective function is also the cost function. After rewriting it, we get:

[0106] Let , , .

[0107] Write the above formula in the form of a Hamiltonian system of equations, that is, execute step S30362.

[0108] S30362. Rewrite the first objective function expression based on the Hamiltonian equation expression to obtain the Hamiltonian equation of the first objective function. Among them, the Hamiltonian equation expression of the first objective function is:

[0109] Among them, represents the tracking error, represents the sliding mode control force, is the weight adjustment value, and represents the weight adjustment matrix, The function uses the hyperbolic tangent smoothing function to eliminate discontinuities and singularities in the calculation.

[0110] S30363. Take the derivative of the Hamiltonian equation expression of the first objective function to obtain the maximum gradient value.

[0111]

[0112] Among them, is defined as the nominal trajectory derivative, is defined as the nominal control force, is defined as the nominal weight adjustment value, is defined as the nominal trajectory.

[0113] Furthermore, define the following equation:

[0114] Among them, is defined as the nominal weight adjustment value.

[0115] Furthermore, solve the maximum gradient of the sliding mode control force as:

[0116] Define the constraint condition as:

[0117] S30364. Based on the sum value of the maximum gradient value and the sliding mode control force, obtain the control force of the actuator of the unmanned vehicle.

[0118] The final energy-saving control law, that is, the control force of the actuator is:

[0119] Among them, is the sliding mode control force, is the energy-saving sliding mode control force, that is, the control force of the actuator, is the gradient of the energy-saving sliding mode control force, is the control adjustment parameter. See Figure 2(a) - Figure 2(e) , Figure 2(a) is a schematic diagram of different coordinate systems, Figure 2(b) is a comparison diagram of energy-saving paths of different algorithms, Figure 2(c) is a comparison of the convergence of different algorithms, Figure 2(d) is the shortest path under the algorithm of this application, and Figure 2(e) is the energy-saving path under the algorithm of this application. See Figure 4(a) - Figure 4(b), Fig. 4(a) is a schematic diagram of path tracking, and Fig. 4(b) is a schematic diagram of pose and related error simulation results.

[0120] Reference Figure 5 , based on the above method embodiments, the present application further provides a path planning and tracking control device for an unmanned vehicle, which is designed to control the unmanned vehicle to travel under an energy-saving path and an energy-saving path tracking method, so as to extend the operation time. The path planning and tracking control device 10 may include an acquisition module 101, a path planning module 102, and a path tracking module 103. Among them, the acquisition module 101 is used to acquire the initial state of the unmanned vehicle and the global information of the marine environment; the path planning module 102 is used to iteratively search for the optimal energy-saving path of the unmanned vehicle based on the initial state and the global information of the marine environment by using a preset energy-saving path planning algorithm; the path tracking module 103 is used to control the unmanned vehicle to track the optimal energy-saving path by using a preset path tracking control algorithm during the navigation of the unmanned vehicle, and the path tracking control algorithm is calculated based on a sliding mode energy-saving control algorithm; among them, the process of constructing the energy-saving path planning algorithm includes: obtaining a new position update expression and a new fitness function. The new position update expression is obtained according to the position update expression of the particle swarm algorithm based on quantum behavior, and the new fitness function is obtained according to a pre-constructed static power consumption expression, a pre-constructed dynamic power consumption expression, and a pre-constructed danger function. The static power consumption expression is determined based on the initial state and the global information of the marine environment; randomly generating particles after population initialization based on the Latin hypercube sampling method; constructing an energy-saving path planning algorithm according to the new position update expression, the new fitness function, and the particles after population initialization.

[0121] It is not difficult to find that this embodiment is a device embodiment corresponding to the above method embodiment, and this embodiment can be implemented in cooperation with the above method embodiment. The relevant technical details and technical effects mentioned in the above embodiments are still valid in this embodiment. To avoid repetition, they will not be elaborated here. Correspondingly, the relevant technical details mentioned in this embodiment can also be applied to the above embodiments.

[0122] It is worth mentioning that each module involved in this embodiment is a logical module. In practical applications, a logical unit can be a physical unit, a part of a physical unit, or a combination of multiple physical units. In addition, to highlight the innovative part of the present application, units that are not closely related to solving the technical problems proposed by the present application are not introduced in this embodiment, but this does not mean that there are no other units in this embodiment.

[0123] To achieve the above object, the present application further provides an electronic device, including: at least one processor; and a memory communicatively connected to the at least one processor; wherein, the memory stores instructions executable by the at least one processor, and the instructions are executed by the at least one processor to enable the at least one processor to execute the path planning and tracking control method of the unmanned vehicle provided in any one of the above embodiments.

[0124] Wherein, the memory and the processor are connected by a bus. The bus may include any number of interconnected buses and bridges, and the bus connects various circuits of one or more processors and the memory together. The bus may also connect various other circuits such as peripheral devices, voltage regulators, and power management circuits, which are well known in the art, so the present application will not further describe them.

[0125] The processor is responsible for managing the bus and general processing, and can also provide various functions, including timing, peripheral interface, voltage regulation, power management, and other control functions. The magnetic memory can be used to store data used by the processor when executing operations.

[0126] The above are only the preferred embodiments of the present application, and do not limit the patent scope of the present application. Any equivalent structure or equivalent process transformation made by using the specification and drawings of the present application, or directly or indirectly applied in other related technical fields, shall be equally included in the patent protection scope of the present application.

Claims

1. A path planning and tracking control method for an unmanned aerial vehicle, characterized in that: include: Obtain the initial state of the unmanned aerial vehicle and global information of the ocean environment; Based on the initial state and the global information of the ocean environment, iteratively searching for the optimal energy-saving path of the unmanned aerial vehicle using a preset energy-saving path planning algorithm; When the unmanned aerial vehicle is navigating, a preset path tracking control algorithm is used to control the unmanned aerial vehicle to track the optimal energy-saving path, and the path tracking control algorithm is calculated based on a sliding mode energy-saving control algorithm; The process of constructing the energy-saving path planning algorithm includes: Obtaining a new position update expression and a new fitness function, wherein the new position update expression is obtained according to the position update expression of the particle swarm algorithm based on quantum behavior, and the new fitness function is obtained according to a pre-constructed static power consumption expression, a pre-constructed dynamic power consumption expression, and a pre-constructed hazard function, wherein the static power consumption expression is determined based on the initial state and the global information of the ocean environment; Randomly generate particles after population initialization based on Latin hypercube sampling method; The energy-saving path planning algorithm is constructed according to the new position update expression, the new fitness function and the particles after the population is initialized.

2. The path planning and tracking control method for an unmanned aerial vehicle according to claim 1, characterized in that: The new position update expression is obtained according to the position update expression of the particle swarm algorithm based on quantum behavior, including: Obtain the first expression of attraction factor of quantum-behavior-based particle swarm algorithm; The dynamic weight factor, the random number factor and the sine and cosine factors are introduced into the first expression of the attraction factor to obtain the second expression of the attraction factor. The second expression of the attraction factor is: in, for A random number, for A random number, for A random number, is the dynamic weight factor, is the starting coefficient, Indicates the current iteration number, T is the number of iteration terminations, is the second expression of the attraction factor, and In the The particle i In the The individual optimal position and the global optimal position in the dimensional space; The Levy flight algorithm and the step size control factor are introduced into the first expression of the attraction factor to obtain the third expression of the attraction factor, which is: in, is the third expression of the attraction factor, Indicates the current iteration number, is the step size control factor, is a random path, and In the The particle i In the The individual optimal position and global optimal position in dimensional space, Represents dot multiplication operation; The second attraction factor expression and the third attraction factor expression are used in the position update expression in the quantum behavior-based particle swarm algorithm to replace the first attraction factor expression in the position update expression, respectively, to obtain the new position update expression, wherein the new position update expression is: in, Update the expression for the new position, is the current particle position expression, is the average optimal extreme value, which is defined as the average value of the individual optimal positions of all particles in the particle swarm. Indicates the current iteration number, is the contraction-expansion factor, u is a random number. When it is greater than 0.5, The + sign is used for numbers, otherwise the - sign is used. For particles i The current fitness value of is the minimum fitness value of the particle so far.

3. The path planning and tracking control method for an unmanned aerial vehicle according to claim 1, characterized in that: The iterative search for the optimal energy-saving path of the unmanned aerial vehicle using a preset energy-saving path planning algorithm includes: Acquiring a kinematic model and a dynamic model of the unmanned aerial vehicle; Calculate the current individual optimal value and the current global optimal value of each particle based on the new position update expression; Based on the particles initialized from the population, the energy-saving path planning algorithm is used to iteratively search for an energy-saving path for the unmanned aerial vehicle, and after a preset iteration termination condition is reached, an optimal energy-saving path is obtained; Wherein, in each iteration, the current position of each particle is updated according to the new position update expression to obtain each particle after the position is updated, the new fitness value of each particle after the position is updated is calculated according to the new fitness function, and each new fitness value is compared with the individual optimal value in the previous iteration to obtain the current individual optimal value and the corresponding particle, the current individual optimal value is used as the current global optimal value, the current individual optimal value is assigned to each particle, and each particle is controlled to move in the direction of flight of the particle corresponding to the current individual optimal value to obtain a planned path.

4. The path planning and tracking control method for an unmanned aerial vehicle according to claim 1, characterized in that: The new fitness function is obtained according to a pre-constructed static power consumption expression, a pre-constructed dynamic power consumption expression and a pre-constructed hazard function, and includes: Obtaining a corrected kinematic model and a corrected dynamic model of the unmanned aerial vehicle; Based on the modified kinematic model and the modified dynamic model, obtaining the actuator thrust and speed of the unmanned aerial vehicle; Obtaining a dynamic power consumption expression of the unmanned aerial vehicle based on the actuator thrust and the speed; Get static power consumption expressions and hazard functions; Based on the static power consumption expression, the hazard function and the dynamic power consumption expression processed by addition, a new fitness function is obtained.

5. The path planning and tracking control method for an unmanned aerial vehicle according to claim 1, characterized in that: The adopting of a preset path tracking control algorithm to control the unmanned aerial vehicle to track the optimal energy-saving path includes: Acquire a pre-constructed tracking error of the unmanned aerial vehicle, wherein the tracking error is constructed based on a velocity error and an energy error; constructing a sliding mode surface based on the tracking error, and calculating a maximum gradient value and a sliding mode control force according to a first objective function, wherein the first objective function is constructed according to the sliding mode surface; The control force of the actuator of the unmanned aerial vehicle is calculated based on the maximum gradient value and the sliding mode control force, and the actuator is controlled to track the optimal energy-saving path based on the control force.

6. The path planning and tracking control method for an unmanned aerial vehicle according to claim 5, characterized in that: The calculating the control force of the actuator of the unmanned aerial vehicle based on the maximum gradient value and the sliding mode control force comprises: Obtain the tracking error expression, which is: in, is the speed tracking error, is the position tracking error, is the six-degree-of-freedom velocity vector in the ocean current environment, is the expected speed, is the six-degree-of-freedom position vector under the ocean current model, is the desired position; Get the expression of the sliding surface, the expression of the sliding surface is: in, , is the preset three-dimensional control parameter; The derivative expression of the sliding surface is obtained according to the expression of the sliding surface, and the derivative expression of the sliding surface is: in, is the inertia matrix, To consider the velocity vector in the case of ocean current, is the Coriolis centripetal force matrix, is the fluid damping matrix, are the restoring force and torque vectors due to gravity and buoyancy, is the coordinate transformation matrix, For the marine environment, is the actuator thrust, is the derivative of the desired velocity, is the derivative of the desired position, and These are pre-selected control values. is the derivative of the sliding surface; Calculating the sliding mode control force according to the derivative expression of the sliding mode surface; Constructing a first objective function based on the tracking error expression, the sliding mode control force and the adjustment term; Obtaining a Hamiltonian equation of the first objective function based on the first objective function, and obtaining a maximum gradient value according to the Hamiltonian equation of the first objective function; The control force of the actuator of the unmanned aerial vehicle is obtained based on the sum of the maximum gradient value and the sliding mode control force.

7. The path planning and tracking control method for an unmanned aerial vehicle according to claim 4, characterized in that: The obtaining of the corrected kinematic model and the corrected dynamic model of the unmanned aerial vehicle comprises: The kinematic model and dynamic model of the unmanned aerial vehicle in a still water environment are obtained, wherein the expression of the kinematic model is: in, is the position vector, , , , is the position coordinate of the unmanned aerial vehicle in the ground coordinate system, is the attitude angle of the unmanned aerial vehicle, is the derivative of position, is the velocity vector, , is the linear velocity of the unmanned vehicle in still water environment, , , is the angular velocity of the UAV in still water environment; The expression of the kinetic model is: in, is the six-degree-of-freedom velocity vector in a still water environment, The derivative of the six-degree-of-freedom velocity vector, is the inertia matrix, is the Coriolis centripetal force matrix, is the fluid damping matrix, are the restoring force and torque vectors due to gravity and buoyancy, For system control, It is the disturbance force of the ocean environment, including sea breeze, waves and currents; The velocity parameters in the kinematic model and the dynamic model in the still water environment are replaced by the velocity of the unmanned aerial vehicle relative to the ocean current to obtain a revised kinematic model and a revised dynamic model, wherein the revised kinematic model is: in, , is the speed of the UAV relative to the ocean current, is the velocity vector of the water flow in the carrier coordinate system, is the ground speed of the unmanned vehicle in the carrier coordinate system, is the water flow velocity in the ground coordinate system, J Represents the transformation from the carrier coordinate system to the ground coordinate system; The modified kinetic model is: in, is the speed of the UAV relative to the ocean current, is the derivative of the speed of the UAV relative to the ocean current, is the inertia matrix, is the Coriolis centripetal force matrix, is the fluid damping matrix, are the restoring force and torque vectors due to gravity and buoyancy, is the environmental interference force, is the control force of the actuator of the unmanned aerial vehicle.

8. The path planning and tracking control method for an unmanned aerial vehicle according to claim 6, characterized in that: The obtaining of the Hamiltonian equation of the first objective function based on the first objective function, and obtaining the maximum gradient value according to the Hamiltonian equation of the first objective function, comprises: Obtain the first objective function expression, wherein the expression of the first objective function is: in, Indicates the tracking start time. Indicates the tracking end time. Q1 and Q2 There are two preset adjustment values. and are two specific gravity adjustment factors, is the inertia matrix, is the sliding mode control force, is the control force of the actuator of the unmanned aerial vehicle, is the environmental interference force, is the Coriolis centripetal force matrix, is the damping matrix modified by the hyperbolic tangent smoothing function, are the restoring force and torque vectors due to gravity and buoyancy, is the speed of the UAV relative to the ocean current, is the water flow velocity in the ground coordinate system; The first objective function expression is rewritten based on the Hamiltonian equation expression to obtain the Hamiltonian equation of the first objective function, wherein the Hamiltonian equation expression of the first objective function is: in, is the tracking error, is the sliding mode control force, and is the weight adjustment matrix, is the damping matrix modified by the hyperbolic tangent smoothing function, is the weight adjustment value, is the inertia matrix, is the control force of the actuator of the unmanned aerial vehicle, is the environmental interference force, is the Coriolis centripetal force matrix, are the restoring force and torque vectors due to gravity and buoyancy, is the speed of the UAV relative to the ocean current, is the water flow velocity in the ground coordinate system; The Hamiltonian equation expression of the first objective function is differentiated to obtain the maximum gradient value.

9. A path planning and tracking control device for an unmanned aerial vehicle, characterized in that: include: An acquisition module is used to obtain the initial state of the unmanned aerial vehicle and the global information of the ocean environment; A path planning module, configured to iteratively search for an optimal energy-saving path for the unmanned aerial vehicle using a preset energy-saving path planning algorithm based on the initial state and global information of the ocean environment; A path tracking module, used to control the unmanned aerial vehicle to track the optimal energy-saving path using a preset path tracking control algorithm when the unmanned aerial vehicle is navigating, wherein the path tracking control algorithm is calculated based on a sliding mode energy-saving control algorithm; The process of constructing the energy-saving path planning algorithm includes: Obtaining a new position update expression and a new fitness function, wherein the new position update expression is obtained according to the position update expression of the particle swarm algorithm based on quantum behavior, and the new fitness function is obtained according to a pre-constructed static power consumption expression, a pre-constructed dynamic power consumption expression, and a pre-constructed hazard function, wherein the static power consumption expression is determined based on the initial state and global information of the ocean environment; Randomly generate particles after population initialization based on Latin hypercube sampling method; The energy-saving path planning algorithm is constructed according to the new position update expression, the new fitness function and the particles after the population is initialized.

10. An electronic device, characterized in that: include: at least one processor; and, a memory communicatively coupled to the at least one processor; Wherein, the memory stores instructions that can be executed by the at least one processor, and the instructions are executed by the at least one processor so that the at least one processor can execute the path planning and tracking control method of the unmanned aerial vehicle as described in any one of claims 1 to 8.

Images