A dynamic target obstacle avoidance algorithm based on node adaptive iteration

Abstract

The application discloses a kind of dynamic target obstacle avoidance algorithm based on node self-adaptive iteration in unmanned ship technical field, comprising the following steps: step A) constructs each input class required by obstacle avoidance algorithm, including unmanned ship navigation state class, obstacle state matrix and obstacle avoidance node class;Step B) constructs obstacle avoidance iterative algorithm, including constructing obstacle avoidance node expansion method and setting node parameters, constructing optimal node acquisition mechanism and algorithm iteration method;Obstacle avoidance path is obtained and abnormal exit mechanism is set;The application solves the problems that the prior art is difficult to avoid in advance when facing dynamic obstacles, difficult to output smooth path and excessive consumption of computing resources of grid chart.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of unmanned surface vehicle, and particularly relates to a dynamic target obstacle avoidance algorithm for unmanned surface vehicle. BACKGROUND

[0002] With the rapid development of unmanned surface vehicle (USV) technology, safe and efficient autonomous obstacle avoidance function of USV becomes more and more important for autonomous navigation of USV. Traditional USV navigation algorithms mainly include path planning-based algorithms and reactive-based algorithms. However, these methods often face many challenges when the USV faces multiple obstacles or dynamic obstacles.

[0003] The path planning-based algorithm, such as A-star algorithm, is a heuristic search algorithm which combines the completeness of breadth-first search and the efficiency of depth-first search. Through reasonable setting of heuristic function, the algorithm can balance the calculation speed and the optimality of the result. The reactive-based algorithm, such as artificial potential field method and vector field histogram (VFH) method, the artificial potential field method guides the USV to plan the path by constructing the potential field, in which the target point generates attractive force and the obstacle generates repulsive force. The vector field histogram method is a real-time obstacle avoidance algorithm which describes the obstacle distribution around the USV by constructing histogram and calculates the safe path. The algorithm generates the environment grid map by using sensors, converts the obstacle information into polar histogram, plans the path by analyzing the groove (obstacle gap) of the histogram, and generates the final motion vector combined with the target direction. The algorithm can also consider kinematic constraints to generate smoother trajectories.

[0004] However, the current obstacle avoidance algorithm faces many problems:

[0005] Firstly, the description of USV navigation environment and obstacle information lacks dynamicity and efficiency. The obstacle avoidance algorithm usually uses grid map to construct the USV navigation environment and obstacle position, that is, the navigation sea area of the USV is divided into several blocks according to fixed length, and the target position, non-navigable area and obstacle position are marked in the map. This method will consume a lot of storage resources when the environment is large or the resolution is high, and will affect the response speed of the system in the process of calculation and grid updating; the obstacle information of the grid map is usually represented by binary or simple weighted value, which is difficult to express the dynamicity of the obstacle; the path planning algorithm based on grid is easy to produce zigzag path, which may cause the USV to deviate from the planned route in the actual navigation problem of the USV which needs smooth trajectory.

[0006] Secondly, obstacle avoidance algorithms using the potential field method and the vector field method suffer from the problem of finding local optima. The artificial potential field method and the obstacle avoidance vector method are obstacle avoidance algorithms based on the potential field method and the vector field method, respectively. The artificial potential field method treats targets and obstacles in the environment as "attractive forces" and "repulsive forces" of a virtual force field, respectively, and calculates the next direction and velocity of the unmanned surface vessel by synthesizing the attractive and repulsive forces. The vector field normal generates an obstacle distribution histogram in each direction, and the final direction of motion is calculated by analyzing the free directions in the histogram and combining them with the target direction. While these algorithms can avoid the influence of the grid map, they still easily get trapped in local optima.

[0007] Finally, due to dynamic constraints such as inertia, turning radius, acceleration, and maximum speed, unmanned surface vessels (USVs) cannot achieve instantaneous turning or stopping like land-based vehicles such as robots. Especially at high speeds, USVs require a certain amount of time and distance to complete directional adjustments. Under conditions of high inertia and low maneuverability, USVs must take obstacle avoidance maneuvers in advance when facing moving obstacles. For example, if an obstacle approaches the USV's path and the USV fails to turn in time, the risk of collision may increase. Existing algorithms suffer from insufficient real-time performance and global coverage, making it difficult to turn in time or in advance, leading to unreasonable path planning or obstacle avoidance failures. Summary of the Invention

[0008] This invention addresses the challenges of existing technologies, such as difficulty in early avoidance of dynamic obstacles, difficulty in smooth path output, and excessive computational resource consumption of grid maps. It provides a dynamic target obstacle avoidance algorithm based on node adaptive iteration. This algorithm can plan a route based on the actual turning capability of the unmanned surface vessel at a specific speed, and iterate the optimal route multiple times at fixed intervals. This enables early turning and avoidance when facing multiple dynamic obstacles, while avoiding the excessive computational resources occupied by grid maps.

[0009] The objective of this invention is achieved as follows: a dynamic target obstacle avoidance algorithm based on node adaptive iteration, comprising the following steps:

[0010] Step A) Construct the various input classes required for the obstacle avoidance algorithm, including the unmanned surface vessel navigation state class, the obstacle state matrix, and the obstacle avoidance node class;

[0011] Step B) Constructing an obstacle avoidance iterative algorithm, including constructing an obstacle avoidance node expansion method and setting node parameters, constructing an optimal node acquisition mechanism and an algorithm iteration method; obtaining the obstacle avoidance path and setting an abnormal exit mechanism.

[0012] Furthermore, step A) specifically includes:

[0013] Construct a navigation state class for the unmanned surface vessel (USV) based on its state at the start of the algorithm, including the USV's current longitude. and latitude ; current speed of the unmanned surface vehicle ; polar coordinate system is constructed with the north direction as the polar axis to obtain the sailing direction of the unmanned surface vehicle ; longitude of the target point in the current task and latitude ; sailing path , wherein the path is composed of a plurality of obstacle avoidance nodes; candidate list , which is composed of a plurality of obstacle avoidance nodes;

[0014] At the beginning of the algorithm, all obstacles in the sailing area obtained by the sensors of the unmanned surface vehicle are constructed into an obstacle state matrix, and each obstacle state vector includes: longitude and latitude ; polar coordinate system is constructed with the north direction as the polar axis to obtain the sailing direction of the obstacle ; current speed of the obstacle ; safety distance of the current obstacle ;

[0015] The obstacle avoidance node class is constructed in a fixed structure, which includes: longitude and latitude ; sailing direction of the obstacle avoidance node ; sailing time of the obstacle avoidance node ; current number of the obstacle avoidance node and its parent node number ; current cost of the obstacle avoidance node and expected cost ; number of child nodes expanded by the current node ; outermost node flag of the current node .

[0016] Further, the obstacle avoidance node expansion method and the setting of node parameters in step B) specifically include:

[0017] Each node expansion will take the optimal node selected in the iteration process of the algorithm or the initial state of the unmanned surface vehicle as the parent node, and each parent node will expand three child nodes, which are left turn node, straight node and right turn node; the sailing distance is a fixed set value, and the turning angle is self-adaptive based on the sailing speed and the dynamics model of the unmanned surface vehicle, which is defined as the maximum deflection angle of the unmanned surface vehicle within the sailing distance , and the obstacle avoidance nodes that cannot be used are deleted, and the remaining obstacle avoidance nodes will be added to the candidate list;

[0018] The node parameters are set, the cost value and the outermost node flag of the obstacle avoidance node are set, and the current cost of each obstacle avoidance node The current cost of its parent node Plus 0.8 times the sailing distance; the expected cost of each obstacle avoidance node The distance from the node to the target point plus the sum of the distances from the node to the expected sailing line; when expanding the obstacle avoidance node, set the outermost flag of all obstacle avoidance nodes to be added to the candidate list to true, and set the outermost flag of the parent node of these nodes to false.

[0019] Further, the deletion conditions of the obstacle avoidance node include two:

[0020] According to the time calculation of the obstacle avoidance node, the position of the obstacle at this time is calculated, and the obstacle avoidance node located within the safe range of the obstacle at this time is deleted;

[0021] Calculate the distance between the node and other obstacle avoidance nodes in the candidate list, and delete the nodes less than a certain distance.

[0022] Further, the optimal node acquisition mechanism and algorithm iteration method constructed in step B) specifically include:

[0023] The optimal node acquisition is performed after the obstacle avoidance node is added to the candidate list, considering the obstacle avoidance nodes whose outermost node flag in the candidate list is true, calculating the current cost And the expected cost The sum of the total value, the node with the minimum total value is selected as the optimal node and as the parent node in the next iteration;

[0024] The iteration method of the algorithm is as follows: based on the parent node, the obstacle avoidance node is expanded, the node is selected and its parameters are set, it is added to the candidate list, the optimal node is selected as the parent node, and the next iteration is performed.

[0025] Further, the exit mechanism in step B) includes three cases:

[0026] The first case is that the expanded node is near the target node, and the algorithm iteration is normally completed and exited;

[0027] The second case is that all obstacle avoidance nodes cannot expand more nodes, and the algorithm abnormally exits;

[0028] The third case is that the algorithm iteration exceeds a certain time, and the algorithm abnormally exits.

[0029] Further, when the algorithm normally exits according to the above first case, the algorithm starts to backtrack the complete obstacle avoidance path according to the final optimal node, and the specific method is to first add the node to the sailing path list Then the parent node of the node is added to the path list; the parent node of the node is continuously obtained and added to the path list until the unmanned ship position at the current time is traced back; at this time, the path formed by all the nodes in the path list is the complete obstacle avoidance path.

[0030] Further, when the algorithm appears the second or third case, it indicates that the current unmanned ship is located near the obstacle or the obstacle is near the target point, at this time, the algorithm abnormally exits, and issues a speed reduction or even parking instruction to ensure the safe navigation of the unmanned ship.

[0031] Compared with the prior art, the present application has the beneficial effects that:

[0032] In the present application, the deflection angle of the obstacle avoidance node can be adaptively adjusted according to the actual steering ability of the unmanned ship, the obstacle avoidance path reasoned by the algorithm is smoother, and more conforms to the motion law of the unmanned ship.

[0033] The obstacle avoidance path obtained by the present application can start the obstacle avoidance action in advance based on the motion state of the moving obstacle, has sufficient predictability and globality, and can automatically return to the preset route after the obstacle avoidance action is completed.

[0034] The method proposed in the present application does not need to generate an environment grid map, the algorithm consumes small calculation space, the iterative calculation speed is fast, and the number of obstacles and whether the obstacle moves are not limited. DETAILED DESCRIPTION

[0035] In order to more clearly illustrate the technical solutions in the embodiments of the present application or the prior art, the following will briefly introduce the drawings needed to be used in the embodiments or prior art description. Obviously, the drawings in the following description only are the embodiments of the present application, and for those skilled in the art, other drawings can be obtained without creative labor on the basis of the provided drawings.

[0036] Figure 1 The various structural composition diagrams required for the obstacle avoidance algorithm of the present application.

[0037] Figure 2 The obstacle avoidance node expansion diagram of the present application.

[0038] Figure 3 The obstacle avoidance node condition deletion diagram of the present application.

[0039] Figure 4 The obstacle avoidance path reasoning and abnormal exit condition diagram of the present application. DETAILED DESCRIPTION

[0040] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0041] The algorithm of this invention can be implemented through two technical solutions: constructing the input classes required for the obstacle avoidance algorithm and constructing an iterative obstacle avoidance algorithm.

[0042] In one aspect of the present invention, the various input classes required for the obstacle avoidance algorithm are constructed, including the following steps.

[0043] Step A1, construct the unmanned surface vessel navigation status class:

[0044] Based on the current moment, a navigation state class for the unmanned surface vessel (USV) is constructed. This class stores information such as the current navigation state of the USV, the target point position, and the obstacle avoidance path calculated by the algorithm iteration. Specific information includes: the current position information of the USV (longitude). and latitude Location information of the target point in the current task (longitude) and latitude The current speed of the unmanned surface vessel. The current direction of the unmanned surface vessel's (USV) navigation. The navigation path calculated by the algorithm The path consists of several obstacle avoidance nodes; this provides the algorithm with a list of candidate nodes. It consists of several obstacle avoidance nodes.

[0045] Step A2, construct the obstacle state matrix:

[0046] Using the current moment as a reference, obstacle information within a specific range is first acquired through sensors such as obstacle avoidance radar. Then, all obstacle information is used to construct an obstacle state matrix. The obstacle avoidance navigation path of the unmanned surface vessel (USV) is based on an obstacle matrix as the basis for iteration. The obstacle state matrix consists of multiple obstacle vectors, each containing five pieces of information: the current obstacle's position information (longitude, etc.). and latitude ); Current obstacle's direction of travel Current speed of the obstacle ; current safe distance from obstacles .

[0047] Step A3, construct the obstacle avoidance node class:

[0048] The iteration of the obstacle avoidance algorithm needs to use the obstacle avoidance nodes, and multiple obstacle avoidance nodes can construct the final navigation path. The information included in the obstacle avoidance node and the role thereof are as follows: position information (longitude and latitude ) of the obstacle avoidance node, which determines the position of the node; navigation direction of the obstacle avoidance node, which represents the navigation direction of the unmanned ship when iterating to the current position; navigation time of the obstacle avoidance node, which represents the time consumed when the unmanned ship iterates to the current position; number of the current obstacle avoidance node and number of the parent node , which are used to distinguish each iterated node and facilitate backtracking of each node to the previous parent node; current cost and expected cost of the obstacle avoidance node, wherein the current cost describes the distance cost consumed from the initial position of the unmanned ship to the current obstacle avoidance node, and the expected cost describes the sum of the distance cost from the current obstacle avoidance node to the navigation target point and the offset distance of the current obstacle avoidance node to the planned navigation line; number of the child nodes expanded by the current node , which describes the number of all the child nodes iterated by the current node; outermost node flag of the current node , which describes whether the current node is the latest node in the current path.

[0049] In another aspect of the present application, the obstacle avoidance iteration algorithm is constructed, and specifically includes the following steps:

[0050] Step B1, expanding the obstacle avoidance node:

[0051] The obstacle avoidance node is a key component of the safe navigation line in the obstacle avoidance algorithm. The role of each obstacle avoidance node in the algorithm is as follows: 1. The obstacle avoidance node can reflect the navigation state of the unmanned ship when navigating to the current position; 2. Each obstacle avoidance node will serve as the basis for expanding other obstacle avoidance nodes; 3. The safe navigation line finally obtained by the obstacle avoidance algorithm is composed of several obstacle avoidance nodes.

[0052] When the obstacle avoidance algorithm expands the obstacle avoidance node for the first time, the parent node is the current state of the unmanned ship; when the subsequent obstacle avoidance node expansion is performed, the parent node is the optimal obstacle avoidance node selected by the algorithm. The expansion of the obstacle avoidance node is based on the parent node, and the position, heading, time and other information of the parent node are used as the basis for node expansion.

[0053] The expansion mode of the obstacle avoidance node is as follows:

[0054] For each parent node, the algorithm expands into three obstacle avoidance nodes: a straight-ahead node, a left-turn node, and a right-turn node. The left-turn and right-turn nodes are obtained by deflecting the base node to the left or right by a fixed angle and traveling a fixed distance, respectively. The straight-ahead node is obtained by continuing to travel a fixed distance along the parent node's heading. Using the deflection angle, travel distance, and parent node information, the position, heading, and timing information of the expanded nodes can be calculated and set.

[0055] Step B2: Delete the extended obstacle avoidance nodes.

[0056] For each parent node, three new nodes can be expanded according to step B2. However, not all expanded nodes can be used. Specifically, some expanded nodes may collide with moving or stationary obstacles; in addition, some expanded nodes are located near other existing obstacle avoidance nodes. Therefore, after the parent node expands into three nodes, the unusable nodes need to be removed.

[0057] The expanded nodes need to remove nodes that may collide with the obstacle avoidance node. As the algorithm iterates, it takes time for the unmanned surface vessel to travel from the current moment to the obstacle avoidance node, during which time the dynamic obstacle has moved to a new position. The algorithm needs to determine whether, after a certain period of time, the expanded obstacle avoidance node is within the safe range of the obstacle. If the obstacle avoidance node is within the safe range of the obstacle, the algorithm needs to delete the obstacle avoidance node.

[0058] Expanding the node list requires removing nodes that are near other existing obstacle avoidance nodes. As the number of obstacle avoidance nodes increases, each location can be reached from the current position of the unmanned surface vessel via different paths. To avoid multiple obstacle avoidance nodes at the same location consuming excessive computing resources and time, the algorithm needs to remove nodes that are near other obstacle avoidance nodes. The algorithm iterates through the list of candidate nodes. If the list of candidate nodes If any obstacle avoidance node is located close to an extension node, the algorithm needs to delete that obstacle avoidance node.

[0059] Step B3, updating the candidate list and selecting the optimal node:

[0060] Through steps B1 and B2, the algorithm expands the list of obstacle avoidance nodes that can be added to the candidate list. Previously, it was necessary to consider the cost of obstacle avoidance nodes. and The outermost node label of the node Number of child nodes Node number and its parent node number Setting.

[0061] Current cost of the obstacle avoidance node Indicates the sailing cost from the current time of the unmanned ship to the current obstacle avoidance node, and the calculation method is the current cost of the parent node of the obstacle avoidance node Plus the value based on the fixed sailing distance; the expected cost of the obstacle avoidance node Indicates the distance cost from the current obstacle avoidance node to the target point and the offset cost of the current node to the expected sailing line, and the calculation method is the spherical distance from the obstacle avoidance node to the target point plus the distance from the obstacle avoidance node to the expected sailing line; The outermost node flag of the node is set to true, and the outermost node flag of its parent node is set to false, which indicates whether the node has an expanded node; The number of child nodes Indicates the number of child nodes that can be expanded by the node, and the number of child nodes of the obstacle avoidance node is 0, and the number of child nodes of its parent node is increased by 1; The parent node number of the obstacle avoidance node The number of the parent node; the new obstacle avoidance node number The maximum number in the candidate list plus one.

[0062] After the calculation and setting of the above parameters are completed, the obstacle avoidance node can be added to the candidate list After the update of the candidate list is completed, the algorithm needs to select an optimal node in the candidate list To perform the next algorithm iteration.

[0063] Selecting the optimal node can make the nodes calculated by the algorithm constantly approach the target point and avoid the obstacle area, and selecting the optimal node can improve the calculation speed of the algorithm and optimize the obstacle avoidance path. After expanding the candidate list each time, the algorithm will select the obstacle avoidance node with the minimum total cost value in the candidate list as the parent node of the next obstacle avoidance node expansion.

[0064] Step B4, set the exit mechanism and take the complete path:

[0065] The algorithm infers a complete obstacle avoidance path by repeatedly iterating steps B1 and B2. Each iteration brings the obstacle avoidance node closer to the target point or avoids the obstacle. The algorithm iterates from the current moment and has three exit mechanisms: 1. The algorithm can iterate and find an obstacle avoidance path; 2. The algorithm has no obstacle avoidance nodes to continue iterating; 3. The algorithm times out. These three exit mechanisms cover three possibilities during the unmanned surface vessel's (USV) navigation: 1. The USV can navigate to the target point via a specific route; 2. The USV is currently within the safe range of the obstacle and needs to stop; 3. The target point is already within the safe range of the obstacle. Except for mechanism 1, the algorithm cannot obtain a complete navigation path. In this case, the algorithm returns an error, and the USV decelerates to a stop.

[0066] When the obstacle avoidance node extended by the algorithm is within a certain distance of the target point, it proves that the algorithm has iterated and found a safe path to the target point. At this point, the algorithm will backtrack the complete path to the navigation path based on the obstacle avoidance node. Add this obstacle avoidance node as the first path node to the navigation path. And record the parent node number of the node in the candidate list. Find the node with that number and add it to the navigation path. Simultaneously, the parent node of each node is recorded, and so on, until the algorithm eventually backtracks to the current position of the unmanned surface vessel. The navigation path at this point is... All nodes will form a safe and complete navigation path to the target point.

[0067] Example 1

[0068] like Figure 1 As shown, to meet the iterative computation requirements of the obstacle avoidance algorithm, it is first necessary to digitize and structure the unmanned surface vessel (USV) state information, obstacle state information, and target point information contained in the environment. In addition, the algorithm also needs to construct temporary obstacle avoidance point information during the iteration process. Based on the above requirements, this invention provides various types of information storage methods, including the following steps:

[0069] In step A1, an unmanned surface vessel (USV) navigation status class was constructed, such as... Figure 1 As shown in the image, the current status of the unmanned surface vessel (USV) is indicated by the white vessel icon, which displays the USV's longitude information. for Latitude information for The target point in the current task is shown as a pentagram in the figure, where the longitude information of the target is included. for Latitude information for The current speed of the unmanned surface vessel. for The current direction of the unmanned surface vessel's (USV) navigation. for The navigation path calculated by the algorithm The path consists of several obstacle avoidance nodes, as shown in the figure, which is composed of all the solid black dots; this provides the algorithm with a candidate list of nodes to choose from. It consists of several obstacle avoidance nodes, as shown in the figure, which are composed of all solid black dots and all dashed white dots.

[0070] In step A2, the obstacle state matrix was constructed, as follows: Figure 1 As shown in the figure. The current obstacle state is represented by the gray ship icon. If multiple obstacles exist, their configurations are identical. Obstacle State Matrix ,in Indicates that there are in the current environment There are several obstacles, each represented by a five-dimensional vector, containing the following five pieces of information: the obstacle's longitude information. for Latitude information for Current direction of travel of obstacles for Current speed of the obstacle for ; current safe distance from obstacles for .

[0071] In step A3, an obstacle avoidance node class was constructed, such as... Figure 1 As shown in the figure. All obstacle avoidance nodes are indicated by the dot icons in the figure. The obstacle avoidance node information reflected in the figure is as follows: Longitude information of obstacle avoidance nodes. for Dimensional information of obstacle avoidance nodes for ; Navigation direction of obstacle avoidance nodes for The number of the obstacle avoidance node. The value is 5, corresponding to its parent node number. It is 2;

[0072] Steps B1 to B3 describe the specific calculation process of the algorithm, and their implementation is as follows:

[0073] In step B1, the algorithm expands the selected parent node, such as... Figure 2 As shown in the diagram, two node expansion scenarios are involved. Figure 2As shown in Fig. A, when the algorithm performs the first node expansion, the parent node uses the sailing state of the unmanned ship at the current time, uses the latitude and longitude position information, heading information and speed information of the unmanned ship at the current time; as shown in Fig. Figure 2 As shown in Fig. B, when the algorithm performs subsequent node expansion, the parent node uses the optimal node selected by the algorithm in the last iteration, i.e. numbered 1, uses the node position information, heading information, speed information and sailing time saved in the optimal node.

[0074] After the parent node for expanding the node is determined, the algorithm performs node expansion in a consistent manner. The distance sailed by the expanded node is shown in Fig. The distance set by the present application is fixed at 10 m; the expanded node sailing in the original heading is shown in Fig. Figure 2 A numbered 2 and Figure 2 B numbered 3. The angles of left deflection and rear deflection will be adaptively adjusted based on the sailing speed. With the change of the sailing speed, the deflection angle represents the angle that the unmanned ship can deflect in the distance of 10 m according to a specific speed and maximum steering capability. Based on the sailing distance of the expansion point and the sailing deflection direction, the position information and the heading information of the expanded node can be obtained, and the specific formula is as follows:

[0075]

[0076]

[0077]

[0078] wherein, and represent the latitude of the parent node and the expanded node respectively; and represent the longitude of the parent node and the expanded node respectively; is the fixed distance of sailing 10 m; is the radius of the earth; represents the sailing direction of the expanded node, and the calculation manner thereof is related to the steering condition of the expanded node; represents the sailing direction of the parent node.

[0079] The sailing time of the expanded node is the sailing time of the parent node plus the interval time , and the calculation manner of the interval time is as follows:

[0080]

[0081] wherein, is the fixed distance of sailing 10 m; is the sailing speed.

[0082] In step B2, the algorithm will verify the nodes expanded in step Bl and delete the obstacle avoidance nodes that are not usable, as shown in Figure 3 Figure 2. As shown in Figure 3 Figure 2A, the algorithm will delete the nodes that enter the safety range of the obstacle. First, the algorithm needs to calculate the position of all obstacles when the unmanned surface vehicle sails to the current batch of expanded nodes. According to the sailing time of the obstacle avoidance node , the algorithm calculates the position of all obstacles at that sailing time, as follows:

[0083]

[0084]

[0085]

[0086] wherein, and represent the latitude of the starting point of the obstacle and the latitude after a period of time, respectively; and represent the longitude of the starting point of the obstacle and the longitude after a period of time, respectively; is the sailing distance of the obstacle; is the radius of the earth; represents the sailing direction of the obstacle; is the sailing time of the obstacle avoidance node; is the sailing speed of the obstacle.

[0087] Next, the algorithm calculates the distance between the current obstacle avoidance node and the obstacle, as follows:

[0088]

[0089]

[0090]

[0091] wherein, the longitude of the current obstacle avoidance node is , and the latitude is ; the longitude of the obstacle is , and the latitude is ; is an intermediate variable, representing the square sum of the spherical arc difference between two points; is the spherical arc length between two points; is the distance between two points; represents the radius of the earth.

[0092] When the calculated distance is less than the safety range of the obstacle When an obstacle avoidance node enters the safe zone of an obstacle, the algorithm will delete that node.

[0093] like Figure 3 As shown in Figure B, the algorithm deletes obstacle avoidance nodes that are near other existing obstacle avoidance nodes. The distance between an obstacle avoidance node and other obstacle avoidance nodes is calculated in the same way as the distance between obstacles. When the distance between an extended obstacle avoidance node and other obstacle avoidance nodes is less than the node's safe distance, the extended node is deleted. The node's safe distance is calculated as follows:

[0094]

[0095] in, For node safety distance; This indicates a fixed distance of 10m for navigation; This indicates the iterative deflection angle of the unmanned surface vessel.

[0096] In step B3, the algorithm updates the candidate list and selects the optimal node. After steps B1 and B2, the algorithm obtains obstacle avoidance nodes that can be added to the candidate list. Step B3 needs to refine the obstacle avoidance node information and add it to the candidate list. The algorithm needs to set the cost of the obstacle avoidance node. and The outermost node label of the node Node number Number of child nodes Parent node number At the same time, the outermost node flag of the corresponding parent node needs to be set. Number of child nodes .

[0097] Since the travel distance from the parent node to the expanding node is fixed, the current cost of the obstacle avoidance node is... Add 0.8 times the travel distance to the current cost of the parent node, that is:

[0098]

[0099] in, The current cost for the obstacle avoidance node; The current cost of its parent node.

[0100] Expected cost of obstacle avoidance nodes The distance from the node to the target point is added to the sum of the distances from the node to the expected route. The calculation method for the distance from the node to the target point is the same as that described in step B2. The calculation method for the distance from the node to the expected route is as follows: [The starting point of the existing expected route is...] ,end and node latitude and longitude ; convert the three-point latitude and longitude coordinates into three-dimensional Cartesian coordinates, the conversion formula is as follows:

[0101]

[0102] Wherein is the radius of the earth, then convert point A and B into unit vectors and , according to the unit vector to calculate the great circle path direction:

[0103]

[0104] Then convert P to unit vector , calculate and the great circle angle, and finally get the node to the expected route of the spherical distance:

[0105]

[0106]

[0107] For the outermost node flag , the current obstacle avoidance node is set to , and the parent node is set to ; for the number of child nodes , the current obstacle avoidance node is set to 0, and the number of child nodes of the parent node is increased by one.

[0108] For the method of selecting the optimal node as follows: the algorithm will be in the list of candidates , first determine the outermost node flag of each obstacle avoidance node flag, when the flag is true, calculate the cost of each obstacle avoidance node, that is, the current cost plus the expected cost . Select the minimum cost of the obstacle avoidance node in the list as the optimal node as the parent node in the next algorithm iteration.

[0109] In step B4, the algorithm can infer the complete obstacle avoidance path through multiple iterations. When the algorithm expands a certain obstacle avoidance node to a distance less than a certain distance from the target point, it indicates that the path inference is complete. Based on this node, by backtracking the parent nodes of each node, all nodes on the path can be obtained.

[0110] As shown in A in Figure 4 , the algorithm will expand a large number of obstacle avoidance nodes, but the final backtracking obstacle avoidance path is unique, because the parent node of each obstacle avoidance node is unique.

[0111] The algorithm may exit abnormally under certain circumstances. These include situations where the unmanned surface vessel is currently within the obstacle's range, or where the obstacle is within the target point's range after a certain period of time. For example... Figure 4 As shown in Figure B, after multiple iterations, the obstacle moves to the vicinity of the target point. At this point, there is no path that can reach the vicinity of the target point. Figure 4 As shown in Figure C, at the start of the algorithm, the unmanned surface vessel (USV) is already near the obstacle. At this point, any obstacle avoidance node extended by the algorithm is within the safe range of the obstacle. When the algorithm faces either of the two scenarios mentioned above, it will return an abnormal value and control the USV to decelerate or even stop.

[0112] The above description of the embodiments is only for the purpose of helping to understand the method and core ideas of the present invention. It should be noted that those skilled in the art can make several improvements and modifications to the present invention without departing from the principles of the present invention, and these improvements and modifications also fall within the protection scope of the claims of the present invention.

Claims

1. A dynamic target obstacle avoidance algorithm based on node adaptive iteration, characterized in that, The method comprises the following steps: Step A) constructing various input sailing distance classes required by the obstacle avoidance algorithm, including an unmanned ship sailing state class, an obstacle state matrix, and an obstacle avoidance node class; Step B) constructing an obstacle avoidance iteration algorithm, including constructing an obstacle avoidance node expansion method and setting node parameters, constructing an optimal node acquisition mechanism and an algorithm iteration method; acquiring an obstacle avoidance path and setting an abnormal exit mechanism, constructing an obstacle avoidance node expansion method and setting node parameters specifically include: Each node expansion will take the optimal node or initial USV state selected in the algorithm iteration process as the parent node, and each parent node will expand three child nodes, which are left turn node, straight node and right turn node; the sailing distance Fixed set value, turning angle Adaptively changes based on the USV sailing speed and dynamics model, which is defined as the maximum deflection angle of the USV in the sailing distance Delete the unusable obstacle avoidance nodes, and the remaining obstacle avoidance nodes will be added to the candidate list. Set the node parameters, set the generation value and the outermost node flag of the obstacle avoidance node, the current cost of each obstacle avoidance node The current cost of its parent node Plus 0.8 times the navigation distance The expected cost of each obstacle avoidance node The distance from the node to the target point plus the sum of the distances from the node to the expected route; when expanding the obstacle avoidance node, set the outermost node flag of all obstacle avoidance nodes to be added to the candidate list to true, while setting the outermost node flag of the parent node of these nodes to false.

2. The dynamic target obstacle avoidance algorithm based on node adaptive iteration of claim 1, wherein, Step A) specifically includes: Construct a navigation state class for the unmanned surface vessel (USV) based on its state at the start of the algorithm, including the USV's current longitude. and latitude The current speed of the unmanned surface vessel. Construct a polar coordinate system with due north as the polar axis to obtain the unmanned surface vessel's (USV) navigation direction. Longitude of the target point in the current task and latitude ; navigation path The path consists of several obstacle avoidance nodes; candidate list It consists of several obstacle avoidance nodes; At the beginning of the algorithm, all the obstacles in the navigation area obtained by the unmanned vehicle sensors are constructed into an obstacle state matrix, each obstacle state vector including: the longitude and latitude of the current obstacle; a polar coordinate system is constructed with the north direction as the polar axis to obtain the navigation direction of the obstacle; the navigation speed of the current obstacle; and the safety distance of the current obstacle; Constructing the obstacle avoidance node class with fixed structure, including: the longitude of the obstacle avoidance node and the latitude ; the sailing direction of the obstacle avoidance node ; the sailing time of the obstacle avoidance node ; the number of the current obstacle avoidance node and the number of its parent node ; the current cost of the obstacle avoidance node and the expected cost ; the number of the child nodes expanded by the current node ; the outermost node flag of the current node .

3. The dynamic target obstacle avoidance algorithm based on node adaptive iteration of claim 2, wherein, The deletion conditions of the obstacle avoidance node include two kinds: According to the time calculation of the obstacle avoidance node, the position of the obstacle at this time is calculated, and the obstacle avoidance node located within the safety range of the obstacle at this time is deleted; Calculate the distance between the node and other obstacle avoidance nodes in the candidate list, and delete the nodes less than a certain distance.

4. The dynamic target obstacle avoidance algorithm based on node adaptive iteration of claim 3, wherein, Step B) constructing an optimal node acquisition mechanism and an algorithm iteration method specifically includes: The optimal node is obtained after adding the obstacle avoidance node to the candidate list, considering the outermost node in the candidate list as the true obstacle avoidance node, calculating its current cost and the expected cost , selecting the node with the minimum total cost value as the optimal node and as the parent node in the next iteration; The iteration method of the algorithm is as follows: based on the parent node, the obstacle avoidance node is expanded, the node is screened and its parameters are set, and it is added to the candidate list, the optimal node is selected as the parent node, and the next iteration is performed.

5. The dynamic target obstacle avoidance algorithm based on node adaptive iteration of claim 4, wherein, The exit mechanism in step B) includes three cases: The first case is that the expanded node is near the target node, and the algorithm iteration is normally completed and exited; The second case is that all obstacle avoidance nodes cannot expand more nodes, and the algorithm abnormally exits; The third case is that the algorithm iteration exceeds a certain time, and the algorithm abnormally exits.

6. The dynamic target obstacle avoidance algorithm based on node adaptive iteration of claim 5, wherein, When the algorithm exits normally according to the first case described above, the algorithm starts to backtrack the complete obstacle avoidance path according to the final optimal node, and the specific method is to first add the node to the sailing path list , then add its parent node to the path list; constantly obtain the parent node of the node and add it to the path list until it is backtracked to the current time of the unmanned ship position; at this time, the path composed of all nodes in the path list is the complete obstacle avoidance path.

7. The dynamic target obstacle avoidance algorithm based on node adaptive iteration of claim 5, wherein, When the algorithm appears in the above second or third case, it indicates that the current unmanned ship is near the obstacle or the obstacle is near the target point, at this time the algorithm abnormally exits, and a speed reduction or even a stop command is issued to ensure the safe sailing of the unmanned ship.

Images