A Path Planning and Tracking Control Method for Unmanned Aerial Vehicles

Abstract

The present application discloses a path planning and tracking control method, device and equipment for an unmanned vehicle, belonging to the field of underwater vehicle path control. The method includes obtaining the initial state of the unmanned vehicle and the global information of the ocean environment; based on the initial state and the known ocean information, using a preset energy-saving path planning algorithm to iteratively search for the optimal energy-saving path of the unmanned vehicle, wherein an energy-saving path planning algorithm is constructed by using an improved particle swarm optimization algorithm based on quantum behavior; when the unmanned vehicle is navigating, using a preset path tracking control algorithm to control the unmanned vehicle to track the optimal energy-saving path; the present application controls the unmanned vehicle based on the energy-saving path planning algorithm and the path tracking control algorithm, achieving the purpose of extending the operation time of the unmanned vehicle.

Description

Technical Field

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

[0002] Unmanned underwater vehicle (UUV) refers to an underwater vehicle that achieves various underwater tasks through autonomous control or remote control. As a new type of intelligent unmanned platform at sea, UUV plays an important role in the development and monitoring of marine resources. At present, UUV is developing towards intelligence, and can perform autonomous navigation and missions by issuing commands. However, when UUV performs long-term duty tasks, energy consumption must be considered as a key factor. Therefore, it is particularly important to extend the operation time of UUV when performing autonomous navigation and missions. 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 aerial vehicle, aiming to solve the technical problem of short operation time when UUV is performing autonomous navigation and executing tasks.

[0004] To achieve the above-mentioned purpose, the present application provides a path planning and tracking control method for an unmanned aerial vehicle, comprising: obtaining the initial state of the unmanned aerial vehicle and the global information of the ocean environment; based on the initial state and the global information of the ocean environment, using a preset energy-saving path planning algorithm to iteratively search for the optimal energy-saving path of the unmanned aerial vehicle; when the unmanned aerial vehicle is navigating, using a preset path tracking control algorithm to control the unmanned aerial vehicle to track the optimal energy-saving path, 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 comprises: 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 a particle swarm algorithm based on quantum behavior, 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, the static power consumption expression is determined based on the initial state and the global information of the ocean environment; randomly generating particles after population initialization based on the Latin hypercube sampling method; and 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 a 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 a second expression of the attraction factor, where the second expression of the attraction factor is:

[0006]

[0007] 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 in the particle swarm algorithm based on quantum behavior to obtain a third expression of the attraction factor, where the third expression of the attraction factor is:

[0008]

[0009] 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 respectively replace the first expression of the attraction factor in the position update expression in the particle swarm algorithm based on quantum behavior to obtain the new position update expression, where the new position update expression is:

[0010]

[0011] 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-expansion factor, u is a random number, when it is greater than 0.5, If the sign is positive, take the + sign; otherwise, take the - sign. is the particle i 's current fitness value, is the minimum fitness value of the particle so far.

[0012] 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 after reaching the preset iterative termination condition, obtaining the optimal energy-saving path; wherein, 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.

[0013] 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 danger function, including: 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 danger function; based on the static power consumption expression, the danger function, and the dynamic power consumption expression obtained by summation processing, obtaining the new fitness function.

[0014] Optionally, controlling the unmanned vehicle to track the optimal energy-saving path using a preset path tracking control algorithm includes: obtaining the tracking error of the pre-constructed 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 the first objective function, wherein the first objective function is constructed according to the sliding mode surface; calculating the actuator thrust 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.

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

[0016]

[0017] 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:

[0018]

[0019] 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:

[0020]

[0021] where is the inertia matrix, is the actuator thrust, is the ocean environmental 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 actuator thrust based on the sum of the maximum gradient value and the sliding mode control force.

[0022] 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:

[0023]

[0024] Where , , , , 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 still water environment, , , are the angular velocities of the unmanned vehicle in the still water environment; the expression of the dynamic model is:

[0025]

[0026] Where is the six-degree-of-freedom velocity vector in the still water environment, is the inertia matrix, is the Coriolis centripetal force matrix, is the fluid damping matrix, is the actuator thrust, is the ocean environmental force, including three parts: sea breeze, sea wave and ocean current; replacing the velocity parameters in the kinematic model and the dynamic model in the still water environment with the velocity of the unmanned vehicle relative to the ocean current to obtain the corrected kinematic model and the corrected dynamic model, where the corrected kinematic model is:

[0027]

[0028] Where , 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 velocity of the unmanned vehicle relative to the ground in the body coordinate system, the water flow velocity in the ground coordinate system, J is the transformation from the body coordinate system to the ground coordinate system; the corrected dynamic model is:

[0029]

[0030] Where is the ocean environmental force, is the actuator thrust.

[0031] 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:

[0032]

[0033] 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 actuator thrust, is the speed of the unmanned vehicle relative to the ocean current, the water flow speed 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:

[0034]

[0035] where, is the tracking error, is the sliding mode control force, and are the weight adjustment matrices, The function eliminates the discontinuity and singularity in the calculation with the hyperbolic tangent smoothing function, is the weight adjustment value, is the inertia matrix, is the speed of the unmanned vehicle relative to the ocean current, the water flow speed in the ground coordinate system, is the actuator thrust; taking the derivative of the Hamiltonian equation expression of the first objective function to obtain the maximum gradient value.

[0036] 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 for acquiring the initial state of the unmanned vehicle and the global information of the marine environment; a path planning module for iteratively searching 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 for controlling the unmanned vehicle to track the optimal energy-saving path by using a preset path tracking control algorithm when the unmanned vehicle is sailing, 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.

[0037] 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.

[0038] 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 sailing, control the unmanned vehicle to track the optimal energy-saving path by using a preset path tracking control algorithm; based on the energy-saving path planning algorithm and the path tracking control algorithm, the technical effect of extending the operation time of the unmanned vehicle is achieved. Description of the Drawings

[0039] 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;

[0040] Figure 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;

[0041] Figure 2(b) is a schematic diagram comparing the 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;

[0042] Figure 2(c) is a schematic diagram 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;

[0043] Figure 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;

[0044] Figure 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;

[0045] Figure 3 It is a schematic diagram comparing the sampling results provided by an embodiment of the path planning and tracking control device for the unmanned vehicle of the present application;

[0046] Figure 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;

[0047] Figure 4(b) is a schematic diagram of the pose and related errors provided by an embodiment of the path planning and tracking control method for the unmanned vehicle of the present application;

[0048] Figure 5 It 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.

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

[0050] 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.

[0051] 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 the particle swarm optimization algorithm.

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

[0053] Refer 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:

[0054] S10. Obtain the initial state of the unmanned vehicle and the global information of the ocean environment;

[0055] Specifically, before using this method to perform path planning and tracking control on the unmanned vehicle, the processor first needs to construct a good mathematical model.

[0056] 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:

[0057]

[0058]

[0059]

[0060] 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 , , .

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

[0062]

[0063] Among them, is a six - degree - of - freedom velocity vector, is the inertia matrix, is the Coriolis centripetal force matrix, is the fluid damping matrix, is the actuator thrust, is the ocean environmental force.

[0064] 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 movement 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.

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

[0066]

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

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

[0069]

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

[0071] Thus, the UUV's kinematic model in a dynamic water flow environment can be re - configured as:

[0072]

[0073] where, is the UUV's velocity relative to the ocean current, the water flow velocity in the ground coordinate system, J is the transformation from the body coordinate system to the ground coordinate system;

[0074] 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:

[0075]

[0076] where, vis the six - degree - of - freedom velocity vector, is the six - degree - of - freedom relative velocity vector, and respectively represent the rigid body and the added mass matrix, is the total inertia matrix, and respectively represent the rigid body and the added Coriolis centripetal force matrix, 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 ocean environmental force matrix.

[0077] 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:

[0078]

[0079] 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.

[0080] 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.

[0081] 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 .

[0082] 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:

[0083]

[0084]

[0085] where, For the transformation matrix, considering the safety and actual fault tolerance of the UUV, the actual range of the UUV is extended to a circle in this application.

[0086] 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 can be obtained through mooring tests on-site. 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 the UUV. 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:

[0087]

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

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

[0090]

[0091]

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

[0093]

[0094]

[0095]

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

[0097] S20. Based on the initial state and the global information of the marine environment, use a preset energy-saving path planning algorithm to iteratively search for the optimal energy-saving path of the unmanned vehicle. Among them, an energy-saving path planning algorithm is constructed by using an improved quantum-behavior-based particle swarm optimization algorithm;

[0098] In the embodiment of the present invention, step S20 may specifically include the following execution process:

[0099] 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;

[0100] S202. Construct an energy-saving path planning algorithm according to the new position update expression and the new fitness function.

[0101] 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.

[0102]

[0103]

[0104]

[0105]

[0106] 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 in the th-dimensional space; and represent at the th iteration, the individual optimal position and global optimal position of the particle i in the th-dimensional 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 from 0 to 1. When it is greater than 0.5, the sign is "+", otherwise the sign is "-". is the contraction-expansion factor, is the attraction factor, which is a key parameter for adjusting the convergence behavior of QPSO.

[0107] It should be noted that the particles in the population after initialization are randomly generated based on Latin hypercube sampling.

[0108] 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 interval of variables. Latin hypercube sampling divides each vector in the d-dimensional matrix variable into uniform layers within a specific region, and randomly selects random numbers in each layer to form a sample. The main implementation steps of Latin hypercube sampling LHS are as follows:

[0109] Step 1: Determine the population size , the dimension , and the upper and lower limits of the search space.

[0110] 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.

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

[0112] 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.

[0113] It should be noted that in most existing meta-heuristic algorithms, random sampling is used for population initialization, that is, a population randomly generated between the upper and lower limits of the search space. This method may lead to uneven distribution of individuals in the population 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 individual clustering in the initial population, thereby improving the efficiency of the optimization process. In Figure 3Among them, 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 using LHS for population initialization in this application is higher, and the population has diversity.

[0114] After the population initialization, step S201 may further include the following execution process:

[0115] S2011. Calculate the current individual optimal value and the current global optimal value of each particle based on the new position update expression;

[0116] In the embodiment of this application, step S2011 may specifically include the following execution process:

[0117] S20111. Obtain the first expression of the attraction factor of the particle swarm algorithm based on quantum behavior and;

[0118] 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:

[0119]

[0120] Among them, 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 number of iterations to termination, 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;

[0121] S20113. Introduce 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. The third expression of the attraction factor is:

[0122]

[0123] Among them, 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 -dimensional space, represents the dot product operation;

[0124] S20114. Introduce the second expression of the attraction factor and the third expression of the attraction factor into the position update expression in the quantum-behaved particle swarm optimization algorithm to obtain a new position update expression, where the new position update expression is:

[0125]

[0126] 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 + sign, otherwise it takes the - sign, is the current fitness value of the particle, is the minimum fitness value of the particle up to now;

[0127] S20115. Obtain a new fitness function according to the sum value of the pre-constructed static power consumption expression, the pre-constructed dynamic power consumption expression, and the pre-constructed hazard function.

[0128] Specifically, before step S20115, the method may further include the following execution steps:

[0129] Obtain the corrected kinematic model and the corrected dynamic model of the unmanned vehicle;

[0130] Based on the corrected kinematic model and the corrected dynamic model, obtain the actuator thrust and speed of the unmanned vehicle;

[0131] Based on the actuator thrust and speed, obtain the dynamic power consumption of the unmanned vehicle.

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

[0133]

[0134] 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:

[0135]

[0136] where, represents the static total power, represents the total navigation time.

[0137] The calculation of the thruster power consumption function needs to consider specific ocean current information to ensure that the thrust can be accurately evaluated and calculated under different ocean environments. In this application, the calculation is carried out by means of piecewise integration. represents the dynamic power consumption expression, and the dynamic power consumption expression is as follows:

[0138]

[0139] Among them, represents the actuator thrust, represents the corresponding waypoint, represents the six-degree-of-freedom velocity of the UUV in the ocean current environment.

[0140] In addition, the processor also needs to set the obstacle danger function. represents 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

[0141]

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

[0143]

[0144] Among them, and are weight coefficients, which determine the proportion of each cost in the total cost function.

[0145] 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.

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

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

[0148] In an embodiment of the present invention, step S30 may specifically include the following execution process:

[0149] S301. Obtain the tracking error of the pre-constructed unmanned vehicle, wherein the tracking error is constructed based on the speed error and the energy error;

[0150] 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, wherein the first objective function is constructed according to the sliding mode surface;

[0151] S303. Calculate the actuator thrust 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.

[0152] In an embodiment of the present invention, step S303 may specifically include the following execution process:

[0153] S3031. Obtain the tracking error expression, and the tracking error expression is:

[0154]

[0155] Wherein, is the speed tracking error, is the position tracking error, is the six-degree-of-freedom velocity vector under 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.

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

[0157]

[0158] Wherein, , represents the preset three-dimensional control parameter, is the objective function;

[0159] The control objective of this application is:

[0160]

[0161] Wherein, is the first derivative of the objective function;

[0162] The processor can obtain the Lyapunov function:

[0163]

[0164] 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:

[0165]

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

[0167] This function is optimized with a smoothing function:

[0168]

[0169] S3033. Obtain the derivative expression of the sliding surface according to the expression of the sliding surface. The derivative expression of the sliding surface is:

[0170]

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

[0172]

[0173]

[0174] Among them, represents the ocean environmental force. According to the above formula, we can find , that is, the actuator thrust, is the inertia matrix, is the Coriolis centripetal force matrix, is the fluid damping matrix, and S1, S2 are preset adjustment matrices.

[0175] S3034. Calculate the sliding mode control force according to the derivative expression of the sliding surface;

[0176] S3035. Construct the first objective function based on the tracking error expression, the sliding mode control force, and the adjustment term;

[0177] 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;

[0178] Specifically, step S3036 may include the following execution process:

[0179] S30361. Obtain the first objective function expression. The expression of the first objective function is:

[0180]

[0181] where, represents the tracking start time, represents the tracking end time, Q1 and Q2 are two preset adjustment values, and are two proportion adjustment factors, is the inertia matrix, is the actuator thrust;

[0182] The first objective function is also the cost function. After rewriting it, we get:

[0183]

[0184] Let , , .

[0185] Write the above equation in the form of Hamiltonian equations, that is, execute step S30362.

[0186] S30362. Based on the Hamiltonian equation expression, rewrite the first objective function expression to obtain the Hamiltonian equation of the first objective function. Among them, the Hamiltonian equation expression of the first objective function is:

[0187]

[0188] where, represents the tracking error, represents the sliding mode control force, is the weight adjustment value, and represent the weight adjustment matrix, The function uses the hyperbolic tangent smoothing function to eliminate the discontinuity and singularity in the calculation.

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

[0190]

[0191] where, 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.

[0192] Furthermore, the following equation is defined:

[0193]

[0194] where is defined as the nominal weight adjustment value.

[0195] Furthermore, the maximum gradient of the sliding mode control force is solved as:

[0196]

[0197] The constraint condition is defined as:

[0198]

[0199] S30364. Based on the sum value of the maximum gradient value and the sliding mode control force, the actuator thrust is obtained.

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

[0201]

[0202] where 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. Refer to Figure 2(a) - Figure 2(e) , Fig. 2(a) is a schematic diagram of different coordinate systems, Fig. 2(b) is a comparison diagram of energy-saving paths of different algorithms, Fig. 2(c) is a comparison of the convergence situations of different algorithms, Fig. 2(d) is the shortest path under the algorithm of this application, and Fig. 2(e) is the energy-saving path under the algorithm of this application. Refer to 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.

[0203] Refer to 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, 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 an energy-saving path planning algorithm according to the new position update expression, the new fitness function, and the particles after population initialization.

[0204] 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.

[0205] 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.

[0206] 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 for an unmanned vehicle provided in any one of the above embodiments.

[0207] Among them, the memory and the processor are connected in a bus manner. 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 can 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.

[0208] 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 the data used by the processor when executing operations.

[0209] 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 content of 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 vehicle, characterized in that, Including: Obtain the initial state of the unmanned vehicle and the global information of the marine environment; Based on the initial state and the global information of the marine environment, use a preset energy-saving path planning algorithm to iteratively search for the optimal energy-saving path of the unmanned vehicle; When the unmanned vehicle is sailing, use a preset path tracking control algorithm to control the unmanned 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; Among them, the process of constructing the energy-saving path planning algorithm includes: Obtain 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. 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 generate particles after population initialization based on the Latin hypercube sampling method; Construct the energy-saving path planning algorithm according to the new position update expression, the new fitness function, and the particles after population initialization 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, including: 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 the speed, obtain the dynamic power consumption expression of the unmanned vehicle; Obtain the static power consumption expression and the danger function; Based on the static power consumption expression, the danger function, and the dynamic power consumption expression obtained by summation processing, obtain a new fitness function.

2. The path planning and tracking control method for the unmanned 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 the attraction factor of the particle swarm algorithm based on quantum behavior; Introduce a dynamic weight factor, a random number factor, and a sine-cosine factor into the first expression of the attraction factor to obtain a second expression of the attraction factor. The second expression of the attraction factor is: Among them, is a random number of is a random number of is a random number of is a dynamic weight factor, is a starting coefficient, represents the current iteration number, 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 at the i th iteration in the -dimensional space; Introduce the Levy flight algorithm and a step size control factor into the first expression of the attraction factor to obtain a third expression of the attraction factor. The third expression of the attraction factor is: Among them, is the third expression of the attraction factor, represents the current iteration number, is the step size control factor, is the random path, and are respectively at the th iteration, the individual optimal position and the global optimal position of the particle i in the -dimensional space; represents the dot product operation; In the position update expression of the particle swarm algorithm based on quantum behavior, use 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 respectively, to obtain the new position update expression. Among them, the new position update expression is: Among them, is the new position update expression, is the current particle position expression, is the average optimal extreme value, defined as the average of the individual optimal positions of all particles in the particle swarm, represents the current iteration number, is the contraction-expansion factor, u is a random number. When it is greater than 0.5, the sign takes the + sign, otherwise it takes the - sign, is the particle i 's current fitness value, is the minimum fitness value of the particle up to now.

3. The path planning and tracking control method of the unmanned vehicle according to claim 1, characterized in that The iterative search for the optimal energy-saving path of the unmanned vehicle using the preset energy-saving path planning algorithm includes: Obtain the kinematic model and the dynamic model of the unmanned 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 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 flying towards 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 vehicle as claimed in claim 1, wherein The method of using the preset path tracking control algorithm to control the unmanned vehicle to track the optimal energy-saving path includes: Obtain the tracking error of the pre-constructed unmanned vehicle, where the tracking error is constructed based on the velocity error and the energy error; Construct a sliding mode surface based on the tracking error, and calculate the maximum gradient value and the sliding mode control force according to the first objective function, where the first objective function is constructed based on the sliding mode surface; Calculate the actuator thrust 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.

5. The path planning and tracking control method of the unmanned vehicle according to claim 4, characterized in that, The calculation of the actuator thrust based on the maximum gradient value and the sliding mode control force includes: Obtain the tracking error expression, and the tracking error expression is: wherein, is the speed tracking error, is the position tracking error, is the six - degree - of - freedom velocity vector under 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; Obtain the expression of the sliding mode surface, and the expression of the sliding mode surface is: Among them, , is a preset three-dimensional control parameter; 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: Among them, is the inertia matrix, is the velocity vector considering the ocean current, 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 ocean environmental force, is the actuator thrust, is the derivative of the desired velocity, is the derivative of the desired position, and are both preselected control values, is the derivative of the sliding mode surface; Calculate the sliding mode control force according to the derivative expression of the sliding mode surface; Construct a first objective function based on the tracking error expression, the sliding mode control force and the adjustment term; 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; Obtain the actuator thrust based on the sum of the maximum gradient value and the sliding mode control force.

6. The path planning and tracking control method for an unmanned vehicle according to claim 1, characterized in that, The obtaining of the corrected kinematic model and the corrected dynamic model of the unmanned vehicle includes: Obtain the kinematic model and the dynamic model of the unmanned vehicle in the static water environment, where the expression of the kinematic model is: Among them, is the position vector, , , , are the position coordinates of the unmanned vehicle in the ground coordinate system, is the attitude angle of the unmanned vehicle, is the derivative of the position, is the velocity vector, , is the linear velocity 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: wherein, is the six-degree-of-freedom velocity vector in a static 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, is the restoring force and moment vector generated by gravity and buoyancy, is the actuator thrust, is the ocean environmental force, including three parts: sea wind, sea wave and sea current; Replace the velocity parameters in the kinematic model and the dynamic model in the static water environment with the velocity of the unmanned vehicle relative to the ocean current to obtain the corrected kinematic model and the corrected dynamic model, where the corrected kinematic model is: Among them, , is the speed of the unmanned vehicle relative to the ocean current, is the velocity vector of the water current in the vehicle coordinate system, is the ground velocity of the unmanned vehicle in the vehicle coordinate system, is the water current velocity in the ground coordinate system, J represents the transformation from the vehicle coordinate system to the ground coordinate system; The corrected dynamic model is: wherein, is the velocity of the unmanned vehicle relative to the ocean current, is the derivative of the velocity of the unmanned vehicle relative to the ocean current, is the inertia matrix, 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 ocean environmental force, is the actuator thrust.

7. The path planning and tracking control method for the unmanned vehicle according to claim 5, characterized in that The obtaining of the Hamiltonian equation of the first objective function based on the first objective function and the obtaining of the maximum gradient value according to the Hamiltonian equation of the first objective function include: Obtain the first objective function expression, where the expression of the first objective function is: Among them, represents the tracking start time, represents 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 sliding mode control force, is the actuator thrust, is the ocean environmental force, is the Coriolis centripetal force matrix, is the damping matrix corrected by the hyperbolic tangent smoothing function, is the restoring force and moment vector generated by gravity and buoyancy, is the velocity of the unmanned vehicle relative to the ocean current, is the water current velocity in the ground coordinate system; Rewrite the first objective function expression based on the Hamiltonian equation expression to obtain the Hamiltonian equation of the first objective function, where the expression of the Hamiltonian equation of the first objective function is: Among them, is the tracking error, is the sliding mode control force, and is the weight adjustment matrix, is the damping matrix corrected by the hyperbolic tangent smoothing function, is the weight adjustment value, is the inertia matrix, is the actuator thrust, is the ocean environmental force, is the Coriolis centripetal force matrix, is the restoring force and moment vector generated by gravity and buoyancy, is the velocity of the unmanned vehicle relative to the ocean current, is the water flow velocity in the ground coordinate system; Take the derivative of the Hamiltonian equation expression of the first objective function to obtain the maximum gradient value.

8. A path planning and tracking control device for an unmanned vehicle, characterized in that, Includes: An acquisition module for acquiring the initial state of the unmanned vehicle and the global information of the marine environment; A path planning module for iteratively searching 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; A path tracking module for controlling 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 a sliding mode energy-saving control algorithm; Wherein, the process of constructing the energy-saving path planning algorithm includes: Obtain 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 generate particles after population initialization based on the Latin hypercube sampling method; Construct the energy-saving path planning algorithm according to the new position update expression, the new fitness function, and the particles after population initialization; 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, including: 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 the speed, obtain the dynamic power consumption expression of the unmanned vehicle; Obtain the static power consumption expression and the danger function; Based on the static power consumption expression, the danger function, and the dynamic power consumption expression obtained by summation processing, obtain a new fitness function.

9. An electronic device, characterized in that, Includes: 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 so that the at least one processor can execute the path planning and tracking control method of the unmanned vehicle according to any one of claims 1 to 7.

Images