Unmanned air cushion boat autonomous obstacle avoidance path planning method and system based on dynamic LOS, medium and program product

Abstract

The invention discloses an unmanned air cushion boat autonomous obstacle avoidance path planning method and system based on a dynamic LOS, a medium and a program product, and the method comprises the steps: firstly, building point cloud data and a grid map, and determining a starting point and a target point; and initializing and dynamically maintaining a waypoint set, and generating LOS guide trajectories including trajectories of additional and expected duration. Then collision detection is carried out on the generated track, if collision is found, the movement direction of an obstacle is determined, and an A * algorithm is used for planning a shortest obstacle avoidance path; and moving the track points along the obstacle avoidance direction, generating a new LOS guide track, and repeating the collision detection until no collision exists. A safe distance threshold value is set, the shortest distance between the unmanned air cushion boat and the obstacle boat is predicted through a CPA method, and if the distance is smaller than the threshold value, an obstacle avoidance waypoint and a direction movement track point determined according to the coastal event rule are inserted. And finally, constructing an LOS guide track with an expected duration according to the current waypoint, and running along the track. Compared with a single LOS guiding method, the method has the advantages that the calculation amount is less, the operation speed is higher, and the obstacle avoidance function can be realized.

Description

Technical Field

[0001] The present invention belongs to the field of ships, and particularly relates to an autonomous obstacle avoidance path planning method, system, medium and program product for an unmanned air-cushion vehicle based on dynamic LOS. Background Art

[0002] With the development needs of ocean exploration, the need for ships with higher speeds is more urgent in order to more efficiently complete tasks such as maritime transportation. Affected by their support methods, traditional ships are difficult to have higher speeds. An air-cushion vehicle (ACV) can lift off the navigation surface in a cushion state under the action of the air-cushion force generated by the lifting system at the bottom of the ship, so that its navigation resistance is much smaller than that of ordinary ships, thereby obtaining a higher speed and being able to transport personnel and materials faster. Nowadays, various unmanned equipment has developed in a blowout manner, and the research on unmanned air-cushion vehicles (UACVs) has also received extensive attention at home and abroad. When a UACV performs a task, it needs to move to the vicinity of the target point first. During this process, the UACV may encounter obstacles, which requires a path planning algorithm that can smoothly avoid obstacles. An excellent path planning method helps to improve the intelligent level of the UACV and enables the UACV to better complete tasks.

[0003] The existing path planning methods are mainly divided into three categories: graph search-based path planning algorithms, sampling-based path planning algorithms, and optimization-based path planning algorithms. Representative algorithms of graph search-based path planning algorithms include the A* algorithm, JPS algorithm, Dijkstra algorithm, etc. The planning speeds of these algorithms are relatively fast, but the generated paths are in the form of segmented straight lines. Except for omnidirectional mobile robots, robots with non-holonomic kinematic characteristics cannot execute such paths. Representative algorithms of sampling-based path planning algorithms mainly include the PRM algorithm and the RRT algorithm. These algorithms can converge to the global optimum as the sampling increases, but the search space of such algorithms is very large and the planning process is very time-consuming. The optimization-based path planning algorithm constructs a constrained optimal control problem and minimizes an objective function, and can consider multiple constraints at the same time, such as dynamic constraints, control quantity constraints, etc. This type of algorithm is relatively popular in the current field of autonomous driving. However, because the optimization problems in reality are generally non-linear, a good initial solution is required to further obtain a local optimal solution. Each of these methods has its own advantages and disadvantages. Therefore, in order to achieve better planning effects, the current planning algorithms are mainly hybrid methods. The path planning method for an unmanned air-cushion vehicle can also refer to this idea. The method proposed in the present invention is a hybrid method. Summary of the Invention

[0004] The object of the present invention is to provide a method, system, medium and program product for autonomous obstacle avoidance path planning of an unmanned hovercraft based on dynamic LOS.

[0005] The object of the present invention is achieved by the following technical solutions:

[0006] A method for autonomous obstacle avoidance path planning of an unmanned hovercraft based on dynamic LOS, the specific steps are as follows:

[0007] Step 1: Environmental modeling and initialization; use UACV perception equipment to obtain environmental obstacle information, establish point cloud data and grid map, determine the starting point and the target point; initialize and dynamically maintain the waypoint set, and generate a LOS guidance trajectory, including a trajectory with additional duration and expected duration;

[0008] Step 2: Perform collision detection on the generated trajectory; traverse the points in the additional duration trajectory point set and convert them into grid indexes. If the grid in the grid map is occupied, it means that the trajectory has a collision, and go to Step 3; otherwise, jump to Step 6;

[0009] Step 3: Determine the obstacle movement direction and generate an obstacle avoidance path; set the point penetrating the obstacle as the starting point and the point exiting the obstacle as the target point, use the A* algorithm to plan the shortest obstacle avoidance path from the starting point to the target point, and use the unit vector of the waypoint in the planned trajectory and the starting point to obtain the next obstacle avoidance movement direction; move the trajectory points along the obstacle avoidance direction in small steps, generate a new LOS guidance trajectory and repeat the collision detection until there is no collision;

[0010] Step 4: Determine whether there are dynamic obstacles; set a safety distance threshold, and use the CPA method to judge whether the closest distance point between the unmanned hovercraft and the obstacle ship will have a collision. If the closest distance is less than the safety distance threshold, it is determined that a collision has occurred, and go to Step 5; otherwise, jump to Step 6;

[0011] Step 5: Insert an obstacle avoidance waypoint; find the trajectory point of the unmanned hovercraft itself at the closest distance point between the unmanned hovercraft and the obstacle ship, calculate the vector from the position of the unmanned hovercraft to the position of the obstacle ship when the two are at the closest distance, then move the trajectory point once in the direction and length of the vector, and finally move the radius length of the obstacle along the normal direction of the line connecting the two points. The moved trajectory point is used as the inserted waypoint, and the side along which the trajectory point moves is determined according to maritime rules;

[0012] Step 6: Construct an expected duration trajectory of the LOS guidance trajectory with the current waypoint, and the unmanned hovercraft runs along the generated trajectory.

[0013] Further, the generation of the LOS guidance trajectory in Step 1 is specifically:

[0014] Through two waypoints and Construct a path - tangential coordinate system {p}, and the origin of {p} is the x p axis along the direction pointing to and the included angle between this direction and the x n axis of the coordinate system {n} is π p , and the calculation method of π p is as follows:

[0015]

[0016] Through the included angle π p calculate the longitudinal tracking error of the current position of the unmanned hovercraft on the path - tangential coordinate system {p} and the lateral tracking error The calculation process is as follows:

[0017]

[0018] Among them, is defined as follows:

[0019]

[0020] π p The relationship with the desired heading χ d is:

[0021]

[0022] Among them, K p is the proportional gain, which is calculated from the forward - looking distance Δ, that is:

[0023]

[0024] Furthermore, in step 3, a section of the LOS trajectory passing through the obstacle is directly moved out of the obstacle range along the obstacle - avoidance direction determined by the A* algorithm, and the two end - points of this section of the trajectory after moving out of the obstacle range are used as inserted waypoints and a LOS guidance trajectory is generated for collision detection. If there is a collision, the inserted waypoints will continue to be moved along the obstacle - avoidance direction in small steps, a LOS guidance trajectory is generated according to the inserted waypoints, and then collision detection is carried out until the generated guidance trajectory has no collision.

[0025] Furthermore, in step 3, starting from the point A where the obstacle is penetrated and ending at the point B where the obstacle is exited, search for the shortest distance from A to B, and use the cost function:

[0026] f(n)=g(n)+h(n)

[0027] Among them, g(n) is the distance from the starting point A to the ending point B, and h(n) is the heuristic estimated distance;

[0028] Based on the Euclidean distance as the estimated distance, construct an empty open list and a closed list. The open list is used to store the expanded neighbor nodes, and the closed list stores the nodes popped from the open list and the obstacle nodes; starting from the starting point, expand eight neighbor nodes and put them into the open list, pop the neighbor node with the minimum cost f(n) and put it into the closed list, continue to expand the neighbor nodes of the popped node and add them to the open list, and repeat the above process continuously until the target node is popped.

[0029] Furthermore, in step 4, predict the course and speed of the obstacle ship based on radar or visual information, and then perform traversal for a certain period of time.

[0030] Furthermore, in step 4, the time t CPA to the CPA and the distance d CPA are obtained, and the calculation method is as follows:

[0031]

[0032] d CPA =||p OS +v OS t CPA -(p TS +v TS t CPA )||

[0033] Among them, p and v are the position and velocity vectors of the own ship OS and the target ship TS, and the norm is the two-norm, which is defined as follows,

[0034]

[0035] Judge the collision by checking whether a collision may occur or may approach within a certain period in the future; judge by the following conditions:

[0036] 0 ≤ t CPA ≤ t risk ∩ d CPA ≤ d risk

[0037] Among them, t risk and d risk are user-defined parameters and are used as safety thresholds for judging collisions.

[0038] Furthermore, the maritime rules in step 5 include the rules for two ships meeting, specifically including the following three situations:

[0039] Overtaking: When a ship overtakes another ship, it shall avoid the ship being overtaken; A ship approaching another ship from a direction more than 22.5 degrees abaft the beam is the overtaking ship; Any change in the course between the two ships thereafter does not relieve the overtaking ship of the responsibility to keep a distance from the ship being overtaken until the overtaking ship finally overtakes and leaves;

[0040] Meeting head-on: When two power-driven vessels are meeting on reciprocal or nearly reciprocal courses so as to involve risk of collision, each shall alter her course to starboard so that each shall pass on the port side of the other;

[0041] Crossing: When two power-driven vessels are crossing so as to involve risk of collision, the vessel which has the other on her own starboard side shall keep out of the way and, if the circumstances of the case admit, avoid crossing ahead of the other power-driven vessel.

[0042] A computer device / equipment / system, comprising a memory, a processor, and a computer program stored on the memory, wherein the processor executes the computer program to implement the steps of a method for autonomous obstacle avoidance path planning of an unmanned hovercraft based on dynamic LOS.

[0043] A computer-readable storage medium, on which a computer program / instructions are stored, and when the computer program / instructions are executed by a processor, the steps of a method for autonomous obstacle avoidance path planning of an unmanned hovercraft based on dynamic LOS are implemented.

[0044] A computer program product, comprising a computer program / instructions, and when the computer program / instructions are executed by a processor, the steps of a method for autonomous obstacle avoidance path planning of an unmanned hovercraft based on dynamic LOS are implemented.

[0045] The beneficial effects of the present invention are as follows:

[0046] Compared with the traditional A* algorithm, the present invention has less computational complexity and faster running speed, and can achieve the obstacle avoidance function compared with a single LOS guidance method. At the same time, the present invention can be widely promoted in the field of path planning of unmanned hovercraft. Brief Description of the Drawings

[0047] Figure 1 It is a schematic diagram for finding the penetration and exit points.

[0048] Figure 2 It is a schematic diagram of inserting waypoints for static obstacle avoidance.

[0049] Figure 3 It is a schematic diagram of inserting waypoints for dynamic obstacle avoidance.

[0050] Figure 4 It is an effect diagram of static obstacle avoidance of an unmanned hovercraft.

[0051] Figure 5It is the dynamic obstacle avoidance effect diagram of an unmanned air-cushion vehicle.

[0052] Figure 6 This is the algorithm flowchart of the present invention.

[0053] Figure 7 It is the LOS guidance schematic diagram.

[0054] Figure 8 It is the schematic diagram of the A* algorithm for finding obstacle avoidance waypoints. Specific implementation manners

[0055] The present invention will be further described below with reference to the accompanying drawings.

[0056] Embodiment 1:

[0057] To solve the above technical problems, for the path planning problem of an unmanned air-cushion vehicle, the present invention provides a path planning method based on dynamic LOS planning. According to Figures 6 to 8 , the specific steps are as follows:

[0058] Step 1: Establish a corresponding task map according to actual task requirements. The environmental obstacle information is obtained through the UACV's own sensing devices such as radar and cameras. Establishing the task map includes determining the starting point and the target point, and updating the map in real time according to the situation.

[0059] Step 2: Obtain the environmental obstacle information through the UACV's own sensing devices. In the present invention, the obstacle information is determined by lidar detection, and then the obstacles are inflated according to the UACV size and the safety distance.

[0060] Step 3: Initialize a set of waypoints. Add the current position of the unmanned air-cushion vehicle and the target waypoint to the set. It is stipulated that the first waypoint in the set is the current position of the unmanned air-cushion vehicle, and the set of waypoints is dynamically maintained in the subsequent steps.

[0061] Through two waypoints and construct a path tangential coordinate system {p}. The origin of {p} is x p axis along the direction pointing to The angle between this direction and the x n axis of the coordinate system {n} is π p , and the calculation method of π p is as follows:

[0062]

[0063] Through the angle π p the longitudinal tracking error of the current position of the unmanned air-cushion vehicle on the path tangential coordinate system {p} and the lateral tracking error The calculation process is as follows:

[0064]

[0065] Among them, The definition of

[0066]

[0067] From the attached Figure 1 It can be known that π p And the relationship with the desired course χ d is:

[0068]

[0069] Among them, K p is the proportional gain, which is calculated from the forward viewing distance Δ, that is:

[0070]

[0071] Step 4: Generate the LOS guidance trajectory through the waypoints in the waypoint set. According to Figure 1 Among them, the black squares are obstacles, the gray boxes are the expansion ranges of the obstacles, the blue dotted lines are the LOS trajectories of the desired duration, the green dotted lines are the LOS trajectories of the additional duration, and the blue points are the waypoints that penetrate in and out of the obstacles. When generating the trajectory in the present invention, two trajectories are generated, that is, the trajectory of the desired duration and the trajectory of the additional duration are generated. The trajectory of the desired duration is at the back, and the trajectory of the additional duration is at the front. The trajectory of the desired duration is used to avoid collisions, and the trajectory of the additional duration is used to detect collisions.

[0072] Step 5: Perform collision detection on the generated trajectory. The trajectory of the additional duration is not subjected to collision detection. Therefore, when encountering an obstacle, it will directly pass through the obstacle. At this time, traverse the waypoints on the trajectory of the additional duration to find the waypoints that penetrate in and out of the obstacle. The method of collision detection is to traverse the points in the trajectory point set, convert the position of the point into a grid index, and check whether the grid in the grid map is occupied. If it is occupied, it means that the trajectory has a collision, and continue to the next step. Otherwise, the trajectory has no collision, and jump to step 10. After the detection of this step, if a collision occurs, find the two end points of the collided trajectory segment for the next step of processing.

[0073] Step 6: Determine the obstacle avoidance movement direction. In order to enable the trajectory of the unmanned hovercraft to avoid obstacles more quickly, the obstacle avoidance program can be understood as the problem of which side of the obstacle the unmanned hovercraft sails along with the shortest distance when avoiding obstacles. Therefore, the present invention proposes to refine the trajectory segment that encounters the obstacle and use the A* algorithm to determine which side to bypass the obstacle with a shorter distance.

[0074] Step 7: Determine the movement direction using the A* algorithm. Set the point that penetrates the obstacle in Step 5 as the starting point of the A* algorithm, and set the point that exits the obstacle as the target point of the A* algorithm. Use the A* algorithm to search for the shortest distance from the starting point to the target point, and then obtain the next obstacle avoidance movement direction using the unit vector between the waypoints in the trajectory planned by the A* algorithm and the starting point.

[0075] Taking the point A that penetrates the obstacle as the starting point and the point B that exits the obstacle as the end point, search for the shortest distance from A to B. First, determine the cost function used.

[0076] f(n) = g(n) + h(n)

[0077] where g(n) is the distance from the starting point A to the end point B, and h(n) is the heuristic estimated distance.

[0078] In the present invention, the Euclidean distance is taken as the estimated distance. Then the algorithm constructs an empty open list and a closed list. The open list is used to store the expanded neighbor nodes, and the closed list stores the nodes popped from the open list and the obstacle nodes. Starting from the starting point, expand the eight neighbor nodes and put them into the open list, pop the neighbor node with the minimum cost f(n) and put it into the closed list, continue to expand the neighbor nodes of the popped node and add them to the open list, and continuously repeat the above process until the target node is popped. In the present invention, A* is only used when encountering an obstacle, so the calculation time of the algorithm can be significantly reduced while still achieving the purpose of obstacle avoidance planning.

[0079] Step 8: Insert waypoints to generate an obstacle avoidance path. After determining the direction, it is necessary to continuously generate a LOS guidance trajectory according to the moved waypoints and perform collision detection. If the inserted waypoints are continuously determined through continuous movement and collision detection, a large amount of time is required for iterative calculation. To improve the running speed of the algorithm, the present invention proposes to directly move a section of the LOS trajectory passing through the obstacle out of the obstacle range along the obstacle avoidance direction determined by the A* algorithm. The two end points of this section of the trajectory are the waypoints used to generate the LOS guidance trajectory, that is, the waypoints to be inserted. After inserting these two waypoints, generate the LOS guidance trajectory and perform collision detection on the trajectory. If the generated trajectory has no collision, this section of the trajectory is the final required obstacle avoidance path, and jump to Step 10. If there is a collision, continue to the next step. According to Figure 2 The red points show the process of inserting waypoints, the yellow points are the finally inserted waypoints, and the blue line is the generated obstacle avoidance trajectory.

[0080] Step 9: Gradually move the trajectory described in Step 8 along the obstacle avoidance direction determined in Step 7, with a small step size each time. The two endpoints of this section of the trajectory are the waypoints used to generate the LOS guidance trajectory. Generate the LOS guidance trajectory using these two waypoints, and perform collision detection on the generated trajectory. Repeat Step 9 until there is no collision in the generated LOS trajectory. At this time, the LOS trajectory is the required obstacle avoidance path.

[0081] Step 10: After judging the static obstacle situation, it is necessary to judge whether there are dynamic obstacles. Set a safety distance threshold, predict the heading and speed of the obstacle ship based on radar or visual information. After traversing for a certain period of time, use CPA to judge whether the closest distance point between the unmanned hovercraft and the obstacle ship will collide. If the closest distance is less than the safety distance threshold, it can be determined that a collision will occur. If a collision will occur, proceed to the next step; otherwise, jump to Step 12.

[0082] During the collision avoidance of dynamic obstacles, risk assessment is required. The present invention uses the method of calculating the closest point of approach (CPA) to obtain the collision risk during the driving process. CPA is obtained by calculating the time t CPA to the CPA CPA and the distance d

[0083]

[0084] d CPA = ||p OS + v OS t CPA - (p TS + v TS t CPA )||

[0085] where p and v are the position and velocity vectors of the own ship (OS) and the target ship (TS), and the norm is the two-norm, defined as follows.

[0086]

[0087] Judge the collision by checking whether a collision may occur or may approach within a certain period in the future. It can be judged by the following conditions:

[0088] 0 ≤ t CPA ≤ t risk ∩ d CPA ≤ d risk

[0089] where t risk and d risk are user-defined parameters and serve as the safety thresholds for judging collisions.

[0090] Step 11: Insert obstacle avoidance waypoints. First, find the trajectory point of the unmanned hovercraft itself at the point with the shortest distance between the unmanned hovercraft and the obstacle ship, calculate the vector from the position of the unmanned hovercraft to the position of the obstacle ship when the distance between them is the shortest, then move the trajectory point once in the direction and length of the vector, and finally move the radius length of the obstacle along the normal direction of the line connecting the two points. The moved trajectory point is used as the inserted waypoint, and the side along which the trajectory point moves is determined according to maritime rules. According to Figure 3 In the figure, the red triangle is the obstacle ship, the blue triangle is the own ship, and the yellow line is the generated obstacle avoidance trajectory.

[0091] Step 12: Generate the expected duration trajectory of the LOS guidance trajectory with the inserted waypoints, and the unmanned hovercraft runs along the generated path.

[0092] Example 2:

[0093] According to Figure 4 The effect diagram of the obstacle avoidance of static obstacles by using the method proposed in the present invention in a simulation environment. The simulation environment uses an unmanned hovercraft, and the simulation verifies the feasibility of the static obstacle avoidance algorithm of the present invention.

[0094] According to Figure 5 The effect diagram of the obstacle avoidance of dynamic obstacles by using the method proposed in the present invention in a simulation environment. The simulation environment uses an unmanned hovercraft, and the simulation verifies the feasibility of the dynamic obstacle avoidance algorithm of the present invention.

[0095] The specific implementation verification of the present invention is carried out in a simulation environment. The simulation verification is carried out in the open-source vrx simulation environment using the unmanned hovercraft simulation model established by oneself, and the effect to be achieved by the present invention is achieved, verifying the feasibility of the design scheme of the present invention.

[0096] Example 3:

[0097] The functions of the autonomous obstacle avoidance path planning system for an unmanned hovercraft based on dynamic LOS can be illustrated by the aforementioned autonomous obstacle avoidance path planning method for an unmanned hovercraft based on dynamic LOS. The system includes a Central Processing Unit (CPU) 301, which can perform various appropriate actions and processes according to the program stored in the Read-Only Memory (ROM) 302 or the program loaded from the storage section 308 into the Random Access Memory (RAM) 303, such as executing the method described in the above embodiments. In the RAM 303, various programs and data required for the operation of the rescue response system are also stored. The CPU 301, ROM 302, and RAM 303 are connected to each other via a bus 304. An Input / Output (I / O) interface 305 is also connected to the bus 304.

[0098] The following components are connected to the I / O interface 305: an input section 306 including an audio input device, a button switch, etc.; an output section 307 including a Liquid Crystal Display (LCD), an audio output device, an indicator light, etc.; a storage section 308 including a hard disk, etc.; and a communication section 309 including a network interface card such as a LAN (Local Area Network) card, a modem, etc. The communication section 309 performs communication processing via a network such as the Internet. A driver 310 is also connected to the I / O interface 305 as needed. A removable medium 311, such as a magnetic disk, an optical disk, a magneto-optical disk, a semiconductor memory, etc., is installed on the driver 310 as needed so that the computer program read from it can be installed into the storage section 308 as needed.

[0099] Specifically, according to an embodiment of the present invention, the process described above with reference to the flowchart can be implemented as a computer software program. For example, an embodiment of the present invention includes a computer program product, which includes a computer program carried on a computer-readable medium, and the computer program includes a computer program for executing the method shown in the flowchart. In such an embodiment, the computer program can be downloaded and installed from the network via the communication section 309, and / or installed from the removable medium 311. When the computer program is executed by the Central Processing Unit (CPU) 301, various functions defined in the present invention are executed.

[0100] It should be noted that specific examples of computer-readable storage media may include, but are not limited to: electrical connections with one or more wires, portable computer disks, hard disks, random access memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM), flash memory, optical fibers, portable compact disc read-only memory (CD-ROM), optical storage devices, magnetic storage devices, or any suitable combination of the above. In the present invention, a computer-readable storage medium can be any tangible medium that contains or stores a program, and this program can be used by or in combination with an instruction execution system, apparatus, or device.

[0101] Specifically, the dynamic LOS-based autonomous obstacle avoidance path planning system for an unmanned hovercraft in this embodiment includes a processor and a memory. A computer program is stored on the memory. When the computer program is executed by the processor, it implements the dynamic LOS-based autonomous obstacle avoidance path planning method provided in the above embodiment.

[0102] On the other hand, the present invention also provides a computer-readable storage medium. This storage medium can be included in the dynamic LOS-based autonomous obstacle avoidance path planning system for an unmanned hovercraft described in the above embodiment; or it can exist alone and not be assembled into the dynamic LOS-based autonomous obstacle avoidance path planning system for an unmanned hovercraft. The above storage medium carries one or more computer programs. When the above one or more computer programs are executed by a processor of a dynamic LOS-based autonomous obstacle avoidance path planning system for an unmanned hovercraft, the dynamic LOS-based autonomous obstacle avoidance path planning system for an unmanned hovercraft is enabled to implement the dynamic LOS-based autonomous obstacle avoidance path planning method provided in the above embodiment.

[0103] As described above, the above embodiments are only used to illustrate the technical solutions of the present application, rather than to limit them; although the present application has been described in detail with reference to the foregoing embodiments, those of ordinary skill in the art should understand that they can still modify the technical solutions recorded in the foregoing embodiments, or perform equivalent replacements on some of the technical features; and these modifications or replacements do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present application.

Claims

1. An autonomous obstacle avoidance path planning method for unmanned hovercraft based on dynamic LOS, characterized by: The specific steps are as follows: Step 1: Environmental modeling and initialization; use UACV sensing equipment to obtain environmental obstacle information, establish point cloud data and grid maps, determine the starting point and target point; initialize and dynamically maintain the waypoint set, and generate the LOS guidance trajectory, including the trajectory of additional duration and expected duration; Step 2: Perform collision detection on the generated trajectory; traverse the points in the additional duration trajectory point set and convert them into grid indexes. If the grid in the grid map is occupied, it means that the trajectory has collided, and proceed to step 3; otherwise, jump to step 6; Step 3: Determine the obstacle movement direction and generate an obstacle avoidance path; set the point where the obstacle is penetrated as the starting point, and the point where the obstacle is penetrated as the target point, use the A* algorithm to plan the shortest obstacle avoidance path from the starting point to the target point, and use the unit vector of the waypoints in the planned trajectory and the starting point to obtain the next obstacle avoidance movement direction; move the trajectory point along the obstacle avoidance direction in small steps, generate a new LOS guidance trajectory and repeat the collision detection until there is no collision; Step 4: Determine whether there is a dynamic obstacle; set a safety distance threshold, and use the CPA method to determine whether the closest distance point between the unmanned hovercraft and the obstacle ship will collide. If the closest distance is less than the safety distance threshold, it is determined that a collision has occurred, and proceed to step 5; otherwise, jump to step 6; Step 5: Insert obstacle avoidance waypoints; find the unmanned hovercraft's own trajectory point at the point where the unmanned hovercraft and the obstacle ship are closest, calculate the vector from the position of the unmanned hovercraft to the position of the obstacle ship when the two are closest, and then move the trajectory point once according to the direction and length of the vector, and finally move it along the normal direction of the line connecting the two points by the length of the obstacle radius. The moved trajectory point is used as the inserted waypoint, and the direction of the trajectory point movement is determined according to maritime regulations. Step 6: Construct the expected duration trajectory of the LOS guidance trajectory based on the current waypoint, and the unmanned hovercraft moves along the generated trajectory.

2. The method for autonomous obstacle avoidance path planning of unmanned hovercraft based on dynamic LOS according to claim 1 is characterized in that: The LOS guidance trajectory generation in step 1 is specifically as follows: Passing two waypoints and Construct a path tangent coordinate system {p}, the origin of {p} is x p Axis points along The direction of the coordinate system {n} n The axis angle is π p , π p The calculation method is as follows: Through the angle π p Calculate the longitudinal tracking error of the current position of the unmanned hovercraft on the path tangent coordinate system {p} and lateral tracking error The calculation process is as follows: in, is defined as follows: π p and the expected course d The relationship is: Among them, K p is the proportional gain, which is calculated from the foresight distance Δ, that is:

3. The method for autonomous obstacle avoidance path planning of unmanned hovercraft based on dynamic LOS according to claim 1 is characterized in that: The step 3 moves a section of the LOS trajectory that passes through the obstacle directly out of the obstacle range along the obstacle avoidance direction determined by the A* algorithm, and uses the two end points of the trajectory after moving out of the obstacle range as inserted waypoints and generates a LOS guidance trajectory for collision detection. If there is a collision, the inserted waypoints will continue to be moved along the obstacle avoidance direction in small steps, and a LOS guidance trajectory will be generated based on the inserted waypoints, and then collision detection will be performed until the generated guidance trajectory is collision-free.

4. The method for autonomous obstacle avoidance path planning of unmanned hovercraft based on dynamic LOS according to claim 3 is characterized in that: In step 3, the entry point A is taken as the starting point and the exit point B is taken as the end point to search for the shortest distance from A to B, using the cost function: f(n)=g(n)+h(n) Among them, g(n) is the distance from the starting point A to the end point B, and h(n) is the heuristic estimated distance; Based on the Euclidean distance as the estimated distance, an empty open list and closed list are constructed. The open list is used to store the expanded neighbor nodes, and the closed list stores the nodes popped out from the open list and the obstacle nodes. Starting from the starting point, eight neighbor nodes are expanded and put into the open list, the neighbor node with the smallest cost f(n) is popped out and put into the closed list, the neighbor nodes of the popped-out node are continued to be expanded and added to the open list, and the above process is repeated until the target node is popped out.

5. The method for autonomous obstacle avoidance path planning of unmanned hovercraft based on dynamic LOS according to claim 1 is characterized in that: The step 4 predicts the course and speed of the obstacle ship based on radar or visual information, and then traverses for a certain period of time.

6. The method for autonomous obstacle avoidance path planning of unmanned hovercraft based on dynamic LOS according to claim 1, characterized in that: In step 4, the time t to CPA is calculated CPA and the distance d to CPA CPA The calculation method is as follows: d CPA =||p OS +v OS t CPA -(p TS +v TS t CPA )|| Among them, p and v are the position and velocity vectors of the ego ship OS and the target ship TS, and the norm is the two-norm, which is defined as follows: The collision is determined by checking whether a collision is likely to occur or is likely to occur within a certain period of time in the future; the following conditions are used to determine the collision: 0≤t CPA ≤t risk ∩d CPA ≤d risk Among them, t risk and d risk It is a custom parameter that serves as a safety threshold for determining collision.

7. The method for autonomous obstacle avoidance path planning of unmanned hovercraft based on dynamic LOS according to claim 1, characterized in that: The maritime rules in step 5 include the rules for the encounter of two ships, which specifically include the following three situations: Overtaking: When overtaking, a vessel shall keep clear of the vessel being overtaken; a vessel approaching another vessel at an angle of more than 22.5 degrees from the stern is the overtaking vessel; any subsequent change of course between the two vessels does not relieve the overtaking vessel of the responsibility to keep a distance from the vessel being overtaken until the latter vessel finally overtakes and leaves; Head-on encounter: When two powered vessels meet on opposite or nearly opposite routes and there is a risk of collision, each vessel shall change course to starboard and pass the other vessel on the port side; Crossing: When two powered vessels are crossing and there is a risk of collision, the vessel on the starboard side of the other powered vessel should give way and, if circumstances permit, avoid crossing in front of the other powered vessel.

8. A computer device / equipment / system comprising a memory, a processor and a computer program stored in the memory, characterized in that: The processor executes the computer program to implement the steps of the method according to any one of claims 1 to 7.

9. A computer-readable storage medium having a computer program / instruction stored thereon, characterized in that: When the computer program / instructions are executed by a processor, the steps of the method according to any one of claims 1 to 7 are implemented.

10. A computer program product comprising a computer program / instructions, characterized in that: When the computer program / instructions are executed by a processor, the steps of the method according to any one of claims 1 to 7 are implemented.

Images