UUV dock returning method and system based on RS path planning

Abstract

The invention provides a UUV back-to-dock method and system based on RS path planning, and the method comprises the steps: planning a UUV return route through employing a Hybrid A * path planning method based on an RS curve, and navigating the UUV to a first set distance right in front of a dock entrance; and the UUV is visually guided back to the dock at a short distance. The method has the advantages that on the basis of calculation of the RS curve, the UUV backing path is considered, the posture can be conveniently adjusted in time in the docking process, the path is saved, and the efficiency of the path is improved to a great extent; under the condition that an obstacle exists, a path which can only advance is stipulated not to exist, and a path which allows to retreat is stipulated to exist; in combination with a visual positioning guide technology, the short-distance docking precision is high, and the defect that the civil Beidou GPS positioning precision does not meet the short-distance docking positioning requirement is overcome; the method meets the under-actuated system path planning requirement, and the UUV does not need to be modified.

Description

Technical Field

[0001] This application belongs to the technical field of underwater unmanned vehicle docking, and particularly relates to a UUV docking method and system based on RS path planning. Background Technique

[0002] Unmanned Underwater Vehicle (UUV) provides important support for ocean exploration and is widely used in fields such as resource exploration and military reconnaissance. The autonomous docking of UUV is an important part of its intelligent operation. Limited by volume and mass, the energy carried by UUV during mission execution is very limited. To achieve long-term underwater operation, it is necessary to return to the dock for energy replenishment and equipment maintenance. In addition, when UUV performs complex tasks, it needs to exchange information with the surface platform to upload mission information and download missions. Therefore, the UUV docking task is expected to autonomously complete path planning, safely return to port, smoothly enter the dock, and finally achieve a series of recovery actions such as docking control under the premise of obstacle avoidance ability.

[0003] Due to problems such as severe acoustic signal delay and light signal scattering in the underwater environment, communication and positioning are difficult. Therefore, surface docking has become a widely used docking method at present. To improve the automation level of UUV docking, UUV is equipped with a positioning system and a path planning algorithm. According to obstacles such as docks existing on the sea surface, coast, and dock station, combined with its own motion characteristics, navigation conditions, and energy reserves, it calculates the optimal path from the starting point to the docking point to achieve a series of recovery actions such as returning, approaching, and docking. Usually, it is required that the obtained path is the shortest in distance or the lowest in energy consumption under the condition of no collision.

[0004] The problem that the UUV docking path planning expects to solve is: under the condition of satisfying the motion constraints of the underactuated UUV, autonomously plan an optimal collision-free smooth path to guide the UUV to dock smoothly.

[0005] Early docking path planning required technicians to perform manual operations and plan the route based on environmental information. Due to limited information acquisition and the predominance of human factors, it often failed to meet the actual obstacle avoidance and recovery task requirements. With the gradual development of computer technology, the intelligence and autonomy of UUV have been continuously improved, and the route planning has changed from manual participation to autonomous planning. Since UUV belongs to an underactuated system, the application of path planning methods on UUV is greatly restricted.

[0006] In robotics, path planning can adopt methods of graph traversal, optimal curve, and the fusion of both:

[0007] The A* algorithm is a commonly used path - finding and graph - traversing algorithm. This algorithm grids the map, represents the map with a two - dimensional array, and marks the obstacles. During the planning process, it searches for and records the cost values of surrounding points (the sum of the distance from the current node to this node and the distance from this node to the end point, where the distance calculation uses Euclidean distance or Manhattan distance). It selects the node with the minimum cost value as the new starting point until the end point is found. This method does not consider the actual motion constraints, and the obtained path is not a smooth path. It needs subsequent processing to obtain a smooth path and cannot be directly used in UUV path planning.

[0008] Dubins curve method: For an object moving at a constant speed in a plane under non - holonomic constraints, the shortest path from the starting point to the target point consists of two arcs with a radius equal to the minimum turning radius and a straight - line segment connecting these two arcs. This smooth path is called the Dubins curve, which meets the maneuverability constraints of UUVs and has been applied to UUV path - planning problems. However, the Dubins curve stipulates that it can only move forward and cannot move backward, and it does not consider the obstacle - avoidance problem.

[0009] The Reeds - Shepp curve (abbreviated as RS curve) is an optimal smooth curve that connects given start and end positions without obstacles. It is composed of several arcs with a fixed radius and a straight - line segment spliced together, where the radius of the arc is set as the minimum turning radius of the UUV. The path generated based on the RS curve includes forward and backward left - turn (L + ,L - ), right - turn (R + ,R - ), and straight - line (S + ,S - ) navigation states. It has been proven that the shortest path connecting the start and end points can be found among 46 state combinations, and each combination has no more than 5 navigation states. The total path length is the sum of the arc lengths of all left / right - turn path arcs and the length of the straight - line path. The RS curve has an analytical form and conforms to UUV motion constraints.

[0010] The HybridA* algorithm improves the defect that the ordinary A* ignores the object motion constraints. Currently, it is mainly used to solve path - planning problems in parallel parking and reverse - parking in the field of autonomous driving. This method is a combination of the A* algorithm and vehicle kinematics. During the search, it uses the Dubins or RS curve to optimize the path generated by the A* algorithm. The planning process starts from the starting point, restricted by the minimum turning radius, and only searches for the positions it may reach in the next path search.

[0011] When the UUV returns to the docking station on the water surface, it can be positioned and guided by GPS. However, when the distance from the docking station is reduced to less than 15m, the accuracy of GPS cannot meet the requirements of the docking task, which may cause the UUV to miss the docking station or collide with it. Usually, visual navigation is an important means to solve the problem of high-precision navigation at close range and is widely used in fields such as autonomous driving.

[0012] Currently, there are the following methods for visual-guided autonomous docking:

[0013] Point light source positioning:

[0014] The point light source positioning method captures and extracts the position of the point light source on the docking interface of the docking station through a camera installed at the head of the UUV, and compares it with the actual position of the point light source to obtain the relative pose of the UUV and the docking station, and guides the UUV accordingly. However, due to the interference of direct sunlight, water surface reflection, etc., this positioning method for the underwater dark environment cannot be directly used for the visual docking of the UUV on the sea surface.

[0015] Object detection and object tracking algorithms:

[0016] A camera installed above the docking station is used to detect and track the position of the UUV during the docking process. Common object detection algorithms include the YOLO series, SSD, RCNN, EfficientDet, etc. Among them, EfficientDet applies a weighted bi-directional feature pyramid network (BiFPN) and uses a compound scaling method to uniformly scale the resolution, depth, and width of all backbone networks, feature networks, and bounding box / class prediction networks at the same time, which can achieve simple and fast multi-scale feature fusion and accurately detect objects of different sizes. The implementation of object tracking algorithms can be divided into traditional tracking algorithms based on filtering and object tracking algorithms based on deep learning. The object tracking algorithm based on filtering uses a filter to match the candidate region and the template image, which has the advantage of fast speed, but it is difficult to solve problems such as occlusion and scale change; the object tracking algorithm based on deep learning covers two methods: DBT and JDT. The former includes Deep SORT, etc., and the latter includes algorithms such as Tracktor++ and FairMOT. Among them, Deep SORT has a high accuracy in the re-identification task of occluded objects and disappeared objects.

[0017] From the perspective of control theory, a system with fewer input variables than the variables to be controlled is called an underactuated system, that is, a class of nonlinear systems in which the number of independent control variables is less than the number of degrees of freedom of the system. Most UUVs in practical applications have six degrees of freedom, namely forward and backward, left and right, up and down, roll, pitch, and yaw. The roll angular velocity, yaw angular velocity, and pitch angular velocity can be used for direct control. The number of degrees of freedom is more than the dimension of the control input, which is a typical underactuated system. This means that there are fewer possible actions than the degrees of freedom, and it is required that the target trajectory curve during the path tracking process be sufficiently smooth. Most of the above-mentioned common search and planning algorithms are based on the assumption of full drive and omnidirectional movement of the system, which does not conform to the navigation trajectory performance of UUVs in reality. Therefore, further smoothing processing is required, which increases the computational complexity and system complexity.

[0018] In addition, in the current UUV path planning, only forward movement is considered and backward movement is not involved. When the UUV misses the docking station or the initial position due to disturbances such as ocean currents and sea surface wind waves, and it is difficult to plan a forward path due to the heading angle, since only forward movement is specified, a longer path needs to be used to re-adjust the position and attitude. Summary of the Invention

[0019] The purpose of this application is to overcome the defect that the prior art only considers forward movement and does not involve backward movement.

[0020] To achieve the above purpose, this application proposes a UUV docking method based on RS path planning, including:

[0021] Step 1: Use the HybridA* path planning method based on the RS curve to plan the UUV's return route and navigate the UUV to a first set distance directly in front of the docking station entrance;

[0022] Step 2: Guide the UUV to dock through close-range vision.

[0023] As an improvement of the above method, the Step 1 includes:

[0024] Step 1-1: Initialize the open list OpenList for storing the nodes considered on the optimal path during the search and the closed list CloseList for storing the nodes that are no longer considered; set the starting point as the current node and add it to OpenList; initialize and rasterize the obstacle map, and set the cost value of the obstacle position to infinity;

[0025] Step 1-2: Take the minimum turning radius of the UUV and the current position as inputs, and obtain the left turn L + forward and left, left turn L - backward and left, right turn R + forward and right, right turn R - backward and right, straight forward S+ 、 Straight backward S - The positions that can be reached by 6 operations, i.e., the expanded nodes;

[0026] Steps 1-3: First, remove the current node from the Open List and add it to the Close List. Determine whether each expanded node is in the Close List. If it is, discard it. If not, continue to determine whether it is already in the Open List. If not, calculate the total cost value F of the node, use the parent node of the current node as the current node, and add it to the Open List; if the current node is in the Open List, calculate the new G value. If the G value decreases, update the G value and the total cost value F of the node;

[0027] Among them, the total cost value F = G + H; H is the heuristic term, that is, the cost value from the expanded node to the target point; the calculation of H is divided into two items. The first item is the non-omnidirectional obstacle-free heuristic cost, that is, in the planning, obstacles are not considered, only the kinematic constraints of the UUV are considered, and the optimal curve RS is used to calculate the path length from the node to the target point as H1; the second item is the omnidirectional obstacle heuristic cost, that is, in the planning, only the obstacle information of the map is considered and the motion constraint conditions of the UUV are not considered, and the A* algorithm is used to obtain the collision-free shortest distance from this node to the target point as H2; finally, H takes the value of H = max{H1, H2};

[0028] Step 1-4: Search for the point with the smallest total cost value F and update it as the current node;

[0029] Step 1-5: After each N times of executing Steps 1-2 to 1-4 in a loop, query whether there is a collision-free RS curve from the current node to the target point. If there is, stop expanding nodes, use the current node as the final node, and directly add the collision-free RS curve path with the smallest cost value queried to the final path sequence; if not, return to Step 1-2 and continue to expand child nodes;

[0030] Step 1-6: Determine whether the target point is in the Open List. If it is, end the path planning. The final Close List contains all the parent nodes from the starting point to the target point and the corresponding section of the path, and splice all the paths as the output path; otherwise, return to Step 1-2.

[0031] As an improvement of the above method, the N is 1 / 4 of the Euclidean distance from the current node position to the target position.

[0032] As an improvement of the above method, Step 1-6 further includes:

[0033] When the search reaches within the second set distance from the target point, use the RS curve to connect the current node to the target point. If it is detected that the UUV has no collision with the obstacle, retain the curve and directly generate the final path. If there is a collision, return to step 1-2.

[0034] As an improvement to the above method, the second set distance is 10m.

[0035] As an improvement to the above method, step 2 includes: performing vision-based target detection and tracking through a camera installed on the dock, and generating a return-to-dock control command to send to the UUV;

[0036] Among them, the EfficientDet network is used for target detection, and the Deep SORT algorithm is used for target tracking.

[0037] As an improvement to the above method, the first set distance is 15m.

[0038] This application also provides a UUV return-to-dock system based on RS path planning, which is implemented based on the above method. The system includes:

[0039] The first planning module is used to plan the UUV's return route using the HybridA* path planning method based on the RS curve, and navigate the UUV to a position 15m directly in front of the dock entrance;

[0040] The second planning module is used to guide the UUV to return to the dock through close-range vision.

[0041] Compared with the prior art, the advantages of this application are as follows:

[0042] 1. Based on the calculation of the RS curve, considering the UUV's backward path, it is convenient to adjust the attitude in a timely manner during the return-to-dock process, save the path, and greatly improve the efficiency of the path; and in the case of obstacles, a path that only allows forward movement may not exist, while a path that allows backward movement must exist;

[0043] 2. Combining vision positioning and guidance technology, the close-range docking accuracy is high, overcoming the defect that the positioning accuracy of civilian Beidou GPS does not meet the requirements of close-range return-to-dock docking positioning;

[0044] 3. The method of the present invention meets the path planning requirements of underactuated systems, and the UUV does not need to be modified. Description of the Drawings

[0045] Figure 1 Shown is the block diagram of the HybridA* algorithm;

[0046] Figure 2 Shown is the block diagram of the RS curve implementation;

[0047] Figure 3 Shown is a schematic diagram of the extended node for UUV docking path planning and the HybridA* search process;

[0048] As shown in Fig. 4(a), the A* search path based on the HybridA* search process for UUV docking path planning is shown;

[0049] As shown in Fig. 4(b), the Hybrid A* search path based on RS in the HybridA* search process for UUV docking path planning is shown;

[0050] Figure 5 Shown is the effect diagram of close-range vision positioning target detection. Figure 6 Shown is the flow chart of the UUV docking method based on RS path planning. Specific implementation manner

[0051] The technical solution of the present application will be described in detail below with reference to the accompanying drawings.

[0052] As Figure 6 shown, the present invention proposes a UUV docking method based on RS path planning, and the method includes:

[0053] Step 1: Use the HybridA* path planning method based on the RS curve to plan the UUV return route. The block diagram of the Hybrid A* algorithm is as Figure 1 shown.

[0054] As shown in Fig. 4(a), in the A* path search, the space is divided into small grids, the grid centers are used as the nodes for A* path planning, and a path avoiding obstacles is found among the nodes. Only connectivity is guaranteed when solving the path, and the actual feasibility of the UUV is not guaranteed. As shown in Fig. 4(b), HybridA* considers both the spatial connectivity and the heading of the UUV. By considering the kinematic constraints of the UUV, the path nodes searched by HybridA* can be any point in the two-dimensional grid. During the expansion process of the nodes, first, the father node, according to the current UUV state and obstacle information, within a certain move_step with the given steer_list and direction_list (angle sequence and direction sequence), solves a collision-free path, and takes the last point of this path as the position of the next child node. Therefore, the position of the child node in the map grid depends on the position where the end point of a path falls in the grid under the motion constraints. It can be seen from Fig. 4(b) that the HybridA* based on RS considers the self-kinematic limitations of the UUV during path search and can obtain a feasible and relatively smooth path.

[0055] The specific implementation steps are as follows:

[0056] 1) Initialization. Set the docking preparation point about 15 m directly in front of the center of the docking station entrance. When reaching the preparation point, the heading needs to be adjusted to face the docking station directly. Set the coordinates of this preparation point as the origin and establish a coordinate system accordingly. The vehicle moves according to the following model:

[0057]

[0058] where x and y are the position coordinates of the UUV, represents the differential of x, represents the differential of y; θ is the heading angle, and w is the heading angular velocity. Then the coordinates of the preparation point (x, y, θ) = (0, 0, 0).

[0059] Initialize the open list Open List (i.e., the nodes considered on the optimal path during the search) and the closed list Close List (i.e., the nodes no longer considered), and set the given starting point (x s , y s , θ s ) as the current node and add it to OpenList.

[0060] Initialize and rasterize the obstacle map, and set the cost value of the obstacle position to infinity.

[0061] 2) Node expansion. Take the minimum turning radius and the current position of the UUV as inputs, and obtain the positions that can be reached through 6 operations, namely turning left L + forward, turning left L - backward, turning right R + forward, turning right R - backward, going straight forward S + and going straight backward S - under the heading angle limit, that is, expand the nodes;

[0062] 3) Update of Open List and Close List. First, remove the current node from the Open List and add it to the Close List. Check whether each expanded node is in the Close List. If it is, discard it. If not, continue to check whether it is already in the Open List. If not, calculate the F value of this node, set its parent node as the current node, and add it to the Open List. If it is, calculate the new G value (the cost of moving from the current node to this expanded node) and check the current G value of the node. If it decreases, update the G value and the total cost F value of this node, and set the parent node as the current node. Among them, the total cost F = G + H. The heuristic term, that is, the cost H of moving from this expanded node to the target point, is calculated in two parts. The first part is the non-holonomic without obstacles heuristic. In the planning, obstacles are not considered, only the kinematic constraints of the UUV are considered, and the optimal curve RS is used to calculate the path length from the node to the target point as H1. To improve the algorithm efficiency, the RS distances from each point in the map to the target point are calculated offline and directly called in the application. The second part is the holonomic with obstacles heuristic. In the planning, only the obstacle information of the map is considered and the motion constraints of the UUV are not considered. The A* algorithm is used to obtain the collision-free shortest distance from this node to the target point as H2. The final output of the heuristic function is H = max{H1, H2}.

[0063] 4) Search for the point with the minimum total cost F value and update it as the current node.

[0064] 5) Query the collision-free RS curve. After each N loops in step 2), query whether there is a collision-free RS curve from the current node (x, y, θ) to the target point (where N is set to 1 / 4 of the Euclidean distance from the current node position to the target position). If there is, stop expanding nodes, take the current node as the final node, and directly add the collision-free RS curve path with the minimum cost found to the final path sequence. If not, return to step 2) and continue to expand child nodes.

[0065] 6) Judgment of the end of the planning. Check whether the target point is in the Open List. If it is, the path has been found. To improve the search efficiency, when the search reaches near the target point (within 10m), use the RS curve to connect the current node and the target point. If there is no collision between the UUV and the obstacles, retain this curve and directly generate the final path. If there is a collision, return and continue to expand nodes. The final Close List contains all the parent nodes from the starting point to the target point and the corresponding section of the path. Concatenate all the paths as the path output by the Hybrid A* algorithm. The implementation block diagram of the RS curve is as Figure 2 shown.

[0066] Step 2: Close-range visual guidance for docking

[0067] The UUV is equipped with a GPS positioning system and a wireless communication antenna. When returning from a long distance, it autonomously plans the path according to the GPS positioning using the HybridA* algorithm based on the RS curve. When the distance between the UUV and the docking station is less than 15m, the docking station performs vision-based target detection and tracking through a camera installed on the docking station, confirms the position of the UUV, performs data fusion processing by wirelessly collecting information such as the longitude, latitude, attitude, and speed of the vehicle, calculates the path to generate a docking control command, and sends the information to the UUV wirelessly to ensure that the UUV smoothly docks along the center line of the docking station.

[0068] The specific implementation steps are as follows:

[0069] 1) Use the EfficientDet network for target detection. During training, first use the publicly available COCO dataset for pre-training, and then use the UUV image data for fine-tuning.

[0070] 2) Use the Deep SORT algorithm for target tracking.

[0071] The effect diagram of close-range visual guidance for docking is as Figure 5 shown.

[0072] This application also provides a UUV docking system based on RS path planning, which is implemented based on the above method. The system includes:

[0073] The first planning module is used to plan the return route of the UUV using the HybridA* path planning method based on the RS curve and navigate the UUV to a first set distance directly in front of the docking station entrance.

[0074] The second planning module is used to guide the UUV to dock in close-range vision.

[0075] This application can also provide a computer device, including: at least one processor, a memory, at least one network interface, and a user interface. Each component in the device is coupled together through a bus system. It can be understood that the bus system is used to realize the connection and communication between these components. In addition to the data bus, the bus system also includes a power bus, a control bus, and a status signal bus.

[0076] Among them, the user interface can include a display, a keyboard, or a pointing device. For example, a mouse, a trackball, a touchpad, or a touch screen, etc.

[0077] It can be understood that the memory in the disclosed embodiments of the present application can be a volatile memory or a non-volatile memory, or can include both volatile and non-volatile memories. Among them, the non-volatile memory can be a read-only memory (ROM), a programmable ROM (PROM), an erasable programmable ROM (EPROM), an electrically erasable programmable ROM (EEPROM), or a flash memory. The volatile memory can be a random access memory (RAM), which is used as an external cache. By way of example but not limitation, many forms of RAM are available, such as static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), double data rate SDRAM (DDR SDRAM), enhanced SDRAM (ESDRAM), synchlink DRAM (SLDRAM), and direct rambus RAM (DRRAM). The memories described herein are intended to include but not be limited to these and any other suitable types of memories.

[0078] In some embodiments, the memory stores the following elements, executable modules, or data structures, or subsets or supersets thereof: an operating system and application programs.

[0079] Among them, the operating system includes various system programs, such as a framework layer, a core library layer, a driver layer, etc., and is used to implement various basic services and process hardware-based tasks. The application programs include various application programs, such as a media player and a browser, etc., and are used to implement various application services. The program for implementing the method of the disclosed embodiments of the present application can be included in the application programs.

[0080] In the above embodiments, by calling the programs or instructions stored in the memory, specifically, the programs or instructions stored in the application programs, the processor is configured to:

[0081] Execute the steps of the above method.

[0082] The above method can be applied to a processor or implemented by a processor. The processor may be an integrated circuit chip with the ability to process signals. During implementation, the steps of the above method can be completed by the integrated logic circuit of the hardware in the processor or instructions in the form of software. The above-mentioned processor may be a general-purpose processor, a digital signal processor (DSP), an application specific integrated circuit (ASIC), a field programmable gate array (FPGA), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components. It can implement or execute the various methods, steps, and logic block diagrams disclosed above. The general-purpose processor may be a microprocessor or the processor may also be any conventional processor, etc. Combining the steps of the above-disclosed method can be directly embodied as being executed by a hardware decoding processor, or completed by a combination of the hardware and software modules in the decoding processor. The software module may be located in a mature storage medium in the art such as a random access memory, flash memory, read-only memory, programmable read-only memory, or electrically erasable programmable memory, register, etc. This storage medium is located in the memory, and the processor reads the information in the memory and combines its hardware to complete the steps of the above method.

[0083] It can be understood that these embodiments described in the present application can be implemented using hardware, software, firmware, middleware, microcode, or a combination thereof. For hardware implementation, the processing unit can be implemented in one or more application specific integrated circuits (ASICs), digital signal processors (DSPs), digital signal processing devices (DSPDs), programmable logic devices (PLDs), field-programmable gate arrays (FPGAs), general-purpose processors, controllers, microcontrollers, microprocessors, other electronic units for performing the functions described in the present application, or a combination thereof.

[0084] For software implementation, the techniques of the present application can be implemented by executing the functional modules of the present application (such as procedures, functions, etc.). The software code can be stored in the memory and executed by the processor. The memory can be implemented inside or outside the processor.

[0085] The present application may also provide a non-volatile storage medium for storing a computer program. When the computer program is executed by a processor, each step in the above method embodiments can be implemented.

[0086] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present application and not to limit them. Although the present application has been described in detail with reference to the embodiments, those of ordinary skill in the art should understand that any modification or equivalent replacement of the technical solutions of the present application does not depart from the spirit and scope of the technical solutions of the present application, and they should all be covered within the scope of the claims of the present application.

Claims

1. A UUV docking method based on RS path planning, comprising: Step 1: Plan the return route of the UUV using the HybridA* path planning method based on the RS curve, and navigate the UUV to the first set distance directly in front of the docking station; Step 2: Close-range vision guides the UUV back to dock.

2. The UUV docking method based on RS path planning according to claim 1, characterized in that: The step 1 comprises: Step 1-1: Initialize the OpenList, which is used to store the nodes considered on the optimal path in the search, and the Close List, which is used to store the nodes no longer considered; set the starting point as the current node and add it to the OpenList; initialize and rasterize the obstacle map, and set the obstacle position cost value to infinity; Step 1-2: Take the minimum turning radius and current position of the UUV as input and obtain the K value of the forward left turn under the heading angle limit. + , then turn left - , turn right R + , then turn right R - , Straight ahead S + , Straight back S - The locations that 6 operations may reach, i.e., the expansion nodes; Step 1-3: First, remove the current node from the Open List and add it to the Close List. Determine whether each extended node is in the Close List. If so, discard it. If not, continue to determine whether it is already in the Open List. If not, calculate the total cost value F of the node, take the parent node of the current node as the current node, and add it to the Open List. If the current node is in the Open List, calculate the new G value. If the G value decreases, update the G value and total cost value F of the node. Among them, the total cost value F = G + H; H is the heuristic term, that is, the cost value of moving from the extended node to the target point; the calculation of H is divided into two items, the first item is the non-omnidirectional barrier-free heuristic cost, that is, obstacles are not considered in the planning, only the kinematic constraints of the UUV are considered, and the path length from the node to the target point is calculated using the optimal curve RS as H1; the second item is the omnidirectional obstacle heuristic cost, that is, only the obstacle information of the map is considered in the planning without considering the motion constraints of the UUV, and the A* algorithm is used to obtain the non-collision-free shortest distance from the node to the target point as H2; the final value of H is H = max{H1,H2}; Step 1-4: Search for the point with the smallest total cost F value and update it as the current node; Step 1-5: After executing steps 1-2 to 1-4 N times in each loop, check whether there is a collision-free RS curve from the current node to the target point. If so, do not expand the node, take the current node as the final node, and directly add the collision-free RS curve path with the smallest cost value to the final path sequence; if not, return to step 1-2 and continue to expand the child node; Step 1-6: Determine whether the target point is in the Open List. If so, end the path planning. The final Close List contains all parent nodes from the starting point to the target point and the corresponding path. All paths are concatenated as the output path; otherwise, return to step 1-2.

3. The UUV docking method based on RS path planning according to claim 2, characterized in that: The N is 1 / 4 of the Euclidean distance from the current node position to the target position.

4. The UUV docking method based on RS path planning according to claim 2, characterized in that: The steps 1-6 also include: When the search reaches the second set distance of the target point, the RS curve is used to connect the current node and the target point. If there is no collision between the UUV and the obstacle, the curve is retained and the final path is directly generated. If there is a collision, return to step 1-2.

5. The UUV docking method based on RS path planning according to claim 4, characterized in that: The second set distance is 10m.

6. The UUV docking method based on RS path planning according to claim 1, characterized in that: The step 2 comprises: The camera installed on the docking station performs vision-based target detection and tracking, and generates docking control instructions to send to the UUV; Among them, the EfficientDet network is used for target detection, and the Deep SORT algorithm is used for target tracking.

7. The UUV docking method based on RS path planning according to claim 1, characterized in that: The first set distance is 15m.

8. A UUV docking system based on RS path planning, implemented based on any method described in claims 1-7, characterized in that: The system comprises: A first planning module is used to plan a return route for the UUV using a HybridA* path planning method based on an RS curve, and navigate the UUV to a first set distance directly in front of the docking station; and The second planning module is used to guide the UUV back to dock using close-range vision.

Images