A three-dimensional path planning and control method for underwater robot

Abstract

The present invention discloses a three-dimensional path planning and control method for an underwater robot, comprising: evaluating the path quality through a fitness function and constructing an AUV path; planning the constructed AUV path through an RB-FWH algorithm, and obtaining the planned AUV path based on the coding method of the design solution; the AUV starts to navigate according to the planned AUV path based on the real-time geographical location of the AUV measured by the GPS inertial navigation and the Doppler log; when an obstacle is detected, the obstacle photographed is compared with the obstacle features in the underwater environment elevation model, and after the comparison is successful, the data of the GPS inertial navigation and the Doppler log are corrected to determine the actual position of the AUV, and if it deviates from the preset path, the AUV is controlled to return to the preset route and continue to navigate according to the preset path. Aiming at the shortcomings of the traditional FWH algorithm, the present invention adopts the pivot method for improvement, mainly optimizes the step of updating the candidate solution of the FWH algorithm, and effectively improves the accuracy and speed of path planning.

Description

Technical Field

[0001] The invention belongs to the field of underwater robots, relates to path planning and control technology, and specifically relates to a three-dimensional path planning and control method for an underwater robot that integrates a rotation axis method and a distribution estimation algorithm. Background Art

[0002] Path planning technology is one of the key technologies of autonomous underwater robots. It refers to the process of pre-calculating, setting, and optimizing the navigation direction and route of the planned equipment in order to reach a certain target or complete a certain task. Path planning technology, to a certain extent, indicates the degree of intelligence of underwater robots. With the rapid development of computer control technology, artificial intelligence technology, and advanced control theory, scholars at home and abroad have applied a variety of intelligent algorithms to the path planning of AUV (underwater robot), such as artificial potential field method, fast stepping method, A* algorithm, particle swarm optimization algorithm, ant colony optimization algorithm, genetic algorithm, EDA algorithm and other path planning methods. However, traditional path planning algorithms have various shortcomings in practical applications. In view of these shortcomings, researchers at home and abroad have optimized the algorithms in various ways. For example, Sebag and Ducoulombier used Gaussian probability model in EDA to deal with optimization problems in continuous search intervals. Due to the characteristics of univariate Gaussian probability model, EDA using this model can handle some unimodal optimization problems well. Liu Rundong combined a histogram distribution estimation algorithm with the adaptive shrinkage method of search interval and successfully applied it to AUV path planning. Xia Guimei et al. combined a MIMIC distribution estimation algorithm with the pivot method to make up for the problem of insufficient local solution ability of the MIMIC distribution estimation algorithm. Liu Jianjun et al. combined the FWH-based distribution estimation algorithm with the pattern search method to further improve the optimal solution accuracy and convergence speed of the algorithm.

[0003] The best existing method is a master's thesis of South China University of Technology in 2020, a study on self-service underwater vehicle path planning based on distribution estimation algorithm. The study mainly focuses on the path planning algorithms in static ocean environments and dynamic ocean environments, and proposes the FWH algorithm to deal with the AUV path planning problem. Based on the equal-width histogram distribution estimation algorithm, this method takes into account the problem of low algorithm accuracy and introduces a method of adaptive contraction of the search interval to improve the algorithm accuracy and accelerate the convergence speed. However, the problem with this method is that the disadvantage of the FWH algorithm is that it has a large amount of calculation and a slow algorithm convergence speed. One way to solve this problem is to reasonably set the number of divided intervals, but it is inevitable that the accuracy of the algorithm will be affected. At present, most AUV navigation devices use GPS inertial navigation, and its disadvantage is that the error will accumulate more and more, resulting in the AUV being unable to accurately reach the target area or target point according to the preset planned path. Summary of the invention

[0004] Purpose of the invention: In order to overcome the deficiencies in the prior art, a three-dimensional path planning and control method for an underwater robot that integrates a rotation axis method and a distribution estimation algorithm is provided. Aiming at the shortcomings of the traditional FWH algorithm, the rotation axis method (Rosenbrock method) is used to improve it, mainly by optimizing the step of updating candidate solutions of the FWH algorithm, thereby improving the accuracy and speed of path planning.

[0005] Technical solution: To achieve the above purpose, the present invention provides a three-dimensional path planning and control method for an underwater robot, comprising the following steps:

[0006] S1: Evaluate the path quality through the fitness function and construct the AUV path;

[0007] S2: Plan the constructed AUV path through the RB-FWH algorithm, and obtain the planned AUV path based on the encoding method of the design solution;

[0008] S3: The AUV starts to navigate along the planned AUV path based on the real-time geographic location of the AUV measured by the GPS inertial navigation and the Doppler log;

[0009] S4: When an obstacle is detected, the obstacle is photographed and compared with the obstacle features in the underwater environment elevation model. After the comparison is successful, the data of the GPS inertial navigation and Doppler log are corrected to determine the actual position of the AUV. If it deviates from the preset path, the AUV is controlled to return to the preset route and continue to navigate along the preset path.

[0010] Furthermore, the fitness function in step S1 is:

[0011] F fitness =L length +S smooth +O security

[0012] Among them, L length is the path length, L length The smaller the value, the shorter the path and the less energy consumed;

[0013] S smooth Indicates the smoothness of the path, S smooth The smaller the value, the smoother the path, which is more conducive to the AUV to achieve the predetermined path navigation;

[0014] O security Indicates path security, a value of zero indicates a safe path.

[0015] Furthermore, in the fitness function of step S1, L length , S smooth and O securityThe expressions are as follows:

[0016]

[0017] Among them, Dis PsPt Represents the Euclidean distance between the starting point and the end point, L path Represents the actual length of the entire path;

[0018]

[0019] Among them, N uneven Indicates the number of path segments in a path whose curvature exceeds the maximum turning radius of the AUV. If the curvature of a path exceeds the maximum turning radius of the AUV, the path is considered an infeasible path; N seg Indicates the total number of path segments in a path;

[0020]

[0021] Among them, N obt represents the number of waypoints that fall into the unnavigable area, N profile Indicates the number of three-dimensional planing surfaces.

[0022] Furthermore, in step S1, the straight line segment method is used to construct the AUV path, specifically: each path includes a series of waypoints, and these waypoints form a path segment every two, and then these path segments constitute the final path.

[0023] Find a certain number of waypoints on the map, and then use straight line segments to connect adjacent waypoints to form the final path. The straight line segment path construction is relatively easy to implement, and the number of path segments in the path can be determined by controlling the number of waypoints, which is convenient for finding a feasible path. If the obstacles in the environmental space are distributed relatively evenly and sparsely, the number of waypoints can be selected to be smaller. If the obstacles in the environmental space are distributed relatively densely, the number of waypoints can be selected to be larger, which is convenient for the algorithm to find a feasible path. These path points are respectively located in the planing surface of the environment. Due to the actual situation, the AUV has a steering angle, limited maneuverability, limited endurance time and other factors. If the path is not smooth enough, it is not suitable for AUV navigation. The present invention uses a fitness function to evaluate the path quality and determine whether the path is suitable for AUV navigation.

[0024] Furthermore, the RB-FWH algorithm in step S2 is obtained by optimizing the population update step of the FWH algorithm through the Rosenbrock algorithm. The specific optimization process is:

[0025] A1: Initialize the population;

[0026] A2: Build a probability model;

[0027] A3: Sampling probability model, generating candidate parameter solutions of size M;

[0028] A4: Based on the candidate parameter solutions, determining new excellent candidate solutions;

[0029] A5: Updating the current optimal solution through the Rosenbrock algorithm;

[0030] A6: Updating the probability vector, obtaining the probabilities of excellent solutions in each interval, and generating new more advantageous candidate solutions according to the magnitudes of the probabilities of excellent solutions in each interval;

[0031] A7: The algorithm stops and gives the result.

[0032] Furthermore, the specific step A1 is: giving initial values to the size M of the candidate parameter solutions, the equal division number N of the intervals of each variable, the size S (S < M) of the excellent candidate solutions, the learning probability α, the number of iterations H, etc.;

[0033] The specific step A2 is: equally dividing the continuous space [a, b] of the variable into N parts, and the length of each interval is At the beginning, the value-taking probabilities of each interval are the same, all being

[0034] The specific step A3 is: taking values for each dimension variable of each individual, and using the roulette wheel random sampling method to generate candidate parameter solutions of size M;

[0035] The specific step A4 is: based on the fitness function, calculating the fitness values of the candidate parameter solutions, sorting the candidate solutions according to their fitness values, and selecting the top S candidate solutions with high fitness as excellent candidate solutions;

[0036] The specific step A5 is: randomly selecting T individuals from the current S populations as the initial points for Rosenbrock search, and taking the new individuals obtained as part of the new generation population to increase the population diversity;

[0037] The specific step A6 is: for the continuous space of the value-taking of a certain dimension of the variable, counting the probabilities of excellent solutions in its N divided small intervals, and the probability is P i :

[0038]

[0039] The specific step A7 is: when the algorithm reaches the number of iterations or the given precision, the algorithm stops and gives the result.

[0040] Furthermore, the AUV path planning method in step S2 is: one individual represents one path, and each dimensional variable in the individual represents a waypoint. Each waypoint is composed of X / Y / Z three-dimensional coordinate information, where the Y axis coordinate is known, and the X / Z axis coordinate information is the value to be solved by the algorithm, and the X / Z axis coordinate information is solved based on the encoding method of the solution;

[0041] The solution is encoded as:

[0042]

[0043] Furthermore, the method for establishing the three-dimensional planing surface diagram in step S1 is:

[0044] Environmental information includes navigable positions and non-navigable positions, which are distinguished by two different colors. The X-axis, Y-axis, and Z-axis represent the lengths of the environmental areas, respectively. The constructed three-dimensional environmental model is cut into parallel sections along the Y-axis to form a three-dimensional environmental surface map. Each surface map is composed of uniform squares. The number of squares should be set reasonably. If the number of squares is small, the environmental information recognition is low and the algorithm accuracy is reduced. If the number of squares is too large, the environmental information recognition is high and the algorithm accuracy is high, but the algorithm speed is slowed down. The surface map is digitized, the navigable area is replaced by 0, and the non-navigable area is replaced by 1, and then all the surface map data are stored in a three-dimensional array.

[0045] Furthermore, if the preset path is deviated in step S4, the main thrust, side thrust and vertical thrust propellers are controlled to make the AUV return to the preset route.

[0046] Beneficial effects: Compared with the prior art, the present invention has the following advantages:

[0047] 1. The path planning method provided by the present invention is based on an equal-width histogram distribution estimation algorithm, and uses the Rosenbrock algorithm to improve the FWH algorithm, optimizes the population update step of the FWH algorithm, reduces the number of iterations of the equal-width histogram distribution estimation algorithm, avoids the problem of slow convergence speed caused by too many iterations, and greatly improves the FWH algorithm in terms of optimal solution accuracy and convergence speed, so that the path found by the algorithm in AUV path planning applications is more accurate and reasonable, and the speed of finding the optimal path is faster.

[0048] 2. The path control method provided by the present invention uses sensors such as underwater cameras, sonars, and Doppler speed meters, combined with path planning algorithms, to increase the actual navigation accuracy of the AUV under preset path planning. BRIEF DESCRIPTION OF THE DRAWINGS

[0049] Figure 1 Schematic diagram of the optimization process of the RB-FWH algorithm in the present invention;

[0050] Figure 2 It is a control schematic diagram of the AUV path in the present invention;

[0051] Figure 3 This is a simulation diagram of the AUV path provided in this embodiment. DETAILED DESCRIPTION

[0052] The present invention is further explained below in conjunction with the accompanying drawings and specific embodiments. It should be understood that these embodiments are only used to illustrate the present invention and are not used to limit the scope of the present invention. After reading the present invention, various equivalent forms of modifications to the present invention by those skilled in the art all fall within the scope defined by the claims attached to this application.

[0053] The present invention provides a three-dimensional path planning and control method for an underwater robot, comprising the following steps:

[0054] S1: Evaluate the path quality through the fitness function and construct the AUV path;

[0055] The fitness function is:

[0056] F fitness =L length +S smooth +O security

[0057] Among them, L length is the path length, L length The smaller the value, the shorter the path and the less energy consumed;

[0058] S smooth Indicates the smoothness of the path, S smooth The smaller the value, the smoother the path, which is more conducive to the AUV to achieve the predetermined path navigation;

[0059] O security Indicates path security, a value of zero indicates a safe path.

[0060] L length , S smooth and O security The expressions are as follows:

[0061]

[0062] Among them, Dis PsPt Represents the Euclidean distance between the starting point and the end point, L path Represents the actual length of the entire path;

[0063]

[0064] Among them, N unevenIndicates the number of path segments in a path whose bending degree exceeds the maximum turning radius of the AUV. If the bending degree of a path exceeds the maximum turning radius of the AUV, then this path is regarded as an infeasible path; N seg Indicates the total number of path segments in a path;

[0065]

[0066] Among them, N obt Indicates the number of path points falling into the non-navigable area, N profile Indicates the number of three-dimensional cross-sections.

[0067] The method for establishing a three-dimensional cross-section diagram is as follows:

[0068] The environmental information includes navigable positions and non-navigable positions, which are distinguished by two different colors. The X-axis, Y-axis, and Z-axis respectively represent the lengths of the environmental area. The constructed three-dimensional environmental model is cut along the Y-axis into parallel cross-sections to form a three-dimensional environmental cross-section diagram. Each cross-section diagram is composed of uniform grids, and the number of grids should be set reasonably. If the number of grids is small, the recognition degree of environmental information is low and the algorithm accuracy decreases. If the number of grids is too large, the recognition degree of environmental information is high and the algorithm accuracy is high, but the algorithm speed slows down; The cross-section diagram is digitized, the navigable area is replaced by 0, the non-navigable area is replaced by 1, and then all cross-section diagram data is stored in a three-dimensional array.

[0069] S2: Plan the constructed AUV path through the RB-FWH algorithm, and based on the coding method of the design solution, obtain the planned AUV path:

[0070] In this embodiment, the RB-FWH algorithm is obtained by optimizing the population update step of the FWH algorithm through the Rosenbrock algorithm, referring to Figure 1 and its specific optimization process is as follows:

[0071] A1: Initialize the population: Give initial values to the parameter candidate solution scale M, the number of equal divisions N of the interval of each variable, the excellent candidate solution scale S (S < M), the learning probability α, the number of iterations H, etc.;

[0072] A2: Construct a probability model: Divide the continuous space [a, b] of the variable into N equal parts, and the length of each interval is At the beginning, the value probability of each interval is the same, which is all

[0073] A3: Sample the probability model: Take values for each dimension variable of each individual, and use the roulette wheel random sampling method to generate a parameter candidate solution with a scale of M;

[0074] A4: Determine new excellent candidate solutions based on parameter candidate solutions: Calculate the fitness values ​​of the parameter candidate solutions based on the fitness function, sort the candidate solutions according to their fitness values, and select the first S solutions with high fitness as excellent candidate solutions;

[0075] A5: Update the current optimal solution through the Rosenbrock algorithm: randomly select T individuals from the current S population as the initial point for Rosenbrock search, and use the obtained new individuals as part of the new generation population to increase population diversity;

[0076] A6: Update the probability vector to obtain the probability of an excellent solution in each interval: For a continuous space of a variable with a certain dimension, count the probability of an excellent solution in each interval in its N divided small intervals, and the probability is P i :

[0077]

[0078] A7: When the algorithm reaches the number of iterations or the given accuracy, the algorithm stops and gives the result.

[0079] In this embodiment, the straight line segment method is used to construct the AUV path. Specifically, each path includes a series of waypoints, and these waypoints form a path segment every two, and then these path segments form the final path.

[0080] Find a certain number of waypoints on the map, and then use straight line segments to connect adjacent waypoints to form the final path. The straight line segment path construction is relatively easy to implement, and the number of path segments in the path can be determined by controlling the number of waypoints, which is convenient for finding a feasible path. If the obstacles in the environment space are distributed relatively evenly and sparsely, the number of waypoints can be selected to be smaller. If the obstacles in the environment space are distributed relatively densely, the number of waypoints can be selected to be larger, which is convenient for the algorithm to find a feasible path. These path points are respectively located in the planing surface of the environment. Due to the actual situation, the AUV has a steering angle, limited maneuverability, limited endurance time and other factors. If the path is not smooth enough, it is not suitable for AUV navigation. In this embodiment, the fitness function of step S1 is used to evaluate the path quality to determine whether the path is suitable for AUV navigation.

[0081] The AUV path planning method is: one individual represents one path, and each dimensional variable in the individual represents a waypoint. Each waypoint consists of X / Y / Z three-dimensional coordinate information, where the Y-axis coordinate is known, and the X / Z-axis coordinate information is the value to be solved by the algorithm. Based on the encoding method of the solution, the X / Z-axis coordinate information is solved;

[0082] The solution is encoded as:

[0083]

[0084] S3: Reference Figure 2 , the AUV begins to navigate along the planned AUV path based on the real-time geographic location of the AUV measured by GPS inertial navigation and Doppler log;

[0085] S4: When the forward-looking sonar detects an obstacle, the underwater lighting is turned on, and the underwater camera takes a picture of the obstacle and compares it with the obstacle features in the known underwater environment elevation model. Although the inertial navigation will accumulate errors over time, the data measured by the GPS inertial navigation and the Doppler log can still be used to narrow the comparison range of obstacles in the underwater environment model and speed up the search. After the comparison is successful, the data of the GPS inertial navigation and the Doppler log are corrected to determine the actual position of the AUV. If it deviates from the preset path, the main thrust, side thrust, and vertical thrust propellers are controlled to make the AUV return to the preset route and continue to navigate along the preset path.

[0086] This embodiment also provides a three-dimensional path planning and control system for an underwater robot, which includes a network interface, a memory and a processor; wherein the network interface is used to realize the reception and transmission of signals during the process of sending and receiving information between other external network elements; the memory is used to store computer program instructions that can be run on the processor; and the processor is used to execute the steps of the above-mentioned consensus method when running the computer program instructions.

[0087] The present embodiment also provides a computer storage medium, which stores a computer program, and the method described above can be implemented when the processor executes the computer program. The computer readable medium can be considered to be tangible and non-temporary. Non-limiting examples of non-temporary tangible computer-readable media include non-volatile memory circuits (such as flash memory circuits, erasable programmable read-only memory circuits or mask read-only memory circuits), volatile memory circuits (such as static random access memory circuits or dynamic random access memory circuits), magnetic storage media (such as analog or digital tapes or hard drives) and optical storage media (such as CDs, DVDs or Blu-ray discs), etc. The computer program includes processor executable instructions stored on at least one non-temporary tangible computer-readable medium. The computer program may also include or rely on stored data. The computer program may include a basic input / output system (BIOS) that interacts with the hardware of a special-purpose computer, a device driver that interacts with a specific device of a special-purpose computer, one or more operating systems, user applications, background services, background applications, etc.

[0088] Those skilled in the art will appreciate that the embodiments of the present application may be provided as methods, systems, or computer program products. Therefore, the present application may adopt the form of a complete hardware embodiment, a complete software embodiment, or an embodiment in combination with software and hardware. Moreover, the present application may adopt the form of a computer program product implemented in one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) that include computer-usable program code.

[0089] The present application is described with reference to the flowcharts and / or block diagrams of the methods, devices (systems), and computer program products according to the embodiments of the present application. It should be understood that each process and / or box in the flowchart and / or block diagram, as well as the combination of the processes and / or boxes in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing device to generate a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the processes in the flowchart and / or block diagram. Figure 1 A process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in a block or multiple blocks.

[0090] These computer program instructions may also be stored in a computer-readable memory capable of directing a computer or other programmable data processing device to operate in a specific manner, so that the instructions stored in the computer-readable memory produce an article of manufacture comprising an instruction device, which implements the process Figure 1 A process or multiple processes and / or boxes Figure 1 A function specified in one or more boxes.

[0091] These computer program instructions can also be loaded onto a computer or other programmable data processing device so that a series of operating steps are executed on the computer or other programmable device to produce a computer-implemented process, thereby providing instructions for implementing the process. Figure 1 A process or multiple processes and / or boxes Figure 1 The steps for the functions specified in one or more boxes.

[0092] Based on the above scheme, the present invention actually provides a path planning controller that integrates underwater camera, GPS inertial navigation, front sonar and Doppler odometer with RB-FWH algorithm. In order to verify the effect of the method of the present invention, the RB-FWH algorithm provided by the present invention and the traditional FWH algorithm are simultaneously applied to the AUV path planning in this embodiment, and the following results are obtained: Figure 3 As shown in the figure, the RB-FWH algorithm can obtain a better and more reasonable AUV path compared with the FWH algorithm.

Claims

1. A three-dimensional path planning and control method for an underwater robot, characterized in that: The steps include: S1: Evaluate the path quality through the fitness function and construct the AUV path; S2: Plan the constructed AUV path through the RB-FWH algorithm, and obtain the planned AUV path based on the encoding method of the design solution; S3: The AUV starts to navigate along the planned AUV path based on the real-time geographic location of the AUV measured by the GPS inertial navigation and the Doppler log; S4: When an obstacle is detected, the obstacle is compared with the obstacle features in the underwater environment elevation model. After the comparison is successful, the data of the GPS inertial navigation and Doppler log are corrected to determine the actual position of the AUV. If it deviates from the preset path, the AUV is controlled to return to the preset route and continue to navigate along the preset path; The RB-FWH algorithm in step S2 is obtained by optimizing the population update step of the FWH algorithm through the Rosenbrock algorithm. The specific optimization process is: A1: Initialize the population; A2: Build a probability model; A3: Sampling probability model, generating parameter candidate solutions of scale M; A4: Determine new excellent candidate solutions based on the parameter candidate solutions; A5: Update the current optimal solution through the Rosenbrock algorithm; A6: Update the probability vector to obtain the probability of an excellent solution in each interval; A7: The algorithm stops and gives the result; Step A1 specifically includes: giving initial values ​​to the parameter candidate solution scale M, the interval equalization number N of each variable, the excellent candidate solution scale S, the learning probability α, and the number of iterations H; Step A2 is as follows: divide the continuous space of variables [a, b] into N equal parts, with each interval length being At the beginning, the probability of taking values ​​in each interval is the same. Step A3 specifically includes: taking values ​​of each dimensional variable of each individual, and using roulette random sampling method to generate parameter candidate solutions of scale M; Step A4 specifically includes: calculating the fitness values ​​of the candidate parameter solutions based on the fitness function, sorting the candidate solutions according to their fitness values, and selecting the first S solutions with high fitness as excellent candidate solutions; Step A5 is specifically as follows: randomly select T individuals from the current S populations as the initial points for Rosenbrock search, and use the obtained new individuals as part of the new generation population; Step A6 is as follows: for a continuous space of a certain dimension of a variable, count the probability of an excellent solution in each of its N divided small intervals, and the probability is P i : Step A7 is specifically as follows: when the algorithm reaches the number of iterations or a given accuracy, the algorithm stops and gives a result.

2. A three-dimensional path planning and control method for an underwater robot according to claim 1, characterized in that: The fitness function in step S1 is: F fitness =L length +S smooth +O security Among them, L length is the path length, S smooth Indicates the smoothness of the path, O security Indicates path security, a value of zero indicates a safe path.

3. A three-dimensional path planning and control method for an underwater robot according to claim 2, characterized in that: In the fitness function of step S1, L length , S smooth and O security The expressions are as follows: Among them, Dis PsPt Represents the Euclidean distance between the starting point and the end point, L path Represents the actual length of the entire path; Among them, N umeven Indicates the number of path segments in a path whose curvature exceeds the maximum turning radius of the AUV. If the curvature of a path exceeds the maximum turning radius of the AUV, the path is considered an infeasible path; N seg Indicates the total number of path segments in a path; Among them, N obt represents the number of waypoints that fall into the unnavigable area, N profile Indicates the number of three-dimensional planing surfaces.

4. A three-dimensional path planning and control method for an underwater robot according to claim 1, characterized in that: In step S1, the AUV path is constructed by using a straight line segment method, specifically: each path includes a series of waypoints, and these waypoints form a path segment every two, and then these path segments form the final path.

5. The method for three-dimensional path planning and control of an underwater robot according to claim 1, characterized in that: The AUV path planning method in step S2 is as follows: one individual represents one path, and each dimensional variable in the individual represents a waypoint; each waypoint is composed of X / Y / Z three-dimensional coordinate information, wherein the Y-axis coordinate is known, and the X / Z-axis coordinate information is the value to be solved by the algorithm, and the X / Z-axis coordinate information is solved based on the encoding method of the solution; The solution is encoded as:

6. A three-dimensional path planning and control method for an underwater robot according to claim 3, characterized in that: The method for establishing the three-dimensional planing surface diagram in step S1 is: Environmental information includes navigable locations and non-navigable locations, which are distinguished by two different colors. The X-axis, Y-axis, and Z-axis represent the lengths of the environmental areas, respectively. The constructed three-dimensional environmental model is cut into parallel sections along the Y-axis to form a three-dimensional environmental surface map. Each surface map is composed of uniform squares. The surface map is digitized, with 0 replacing the navigable area and 1 replacing the non-navigable area. Then, a three-dimensional array is used to store all the surface map data.

7. A three-dimensional path planning and control method for an underwater robot according to claim 3, characterized in that: If the AUV deviates from the preset path in step S4, the main thrust, side thrust and vertical thrust propellers are controlled to make the AUV return to the preset route.

Images