A speed obstacle planning method and system integrating safety distance and global information

Abstract

The present invention proposes a speed obstacle planning method and system that integrates safety distance and global information. The method includes: first, improving the Theta* algorithm by adding a safety distance function to the evaluation function to ensure that the planned global route is safe and reliable; then, proceeding along the preset path planned by the improved safe Theta* path algorithm. If a dynamic obstacle is encountered, the optimal offset angle for avoiding the obstacle is calculated based on the speed obstacle method that integrates global information, and local path planning is performed to continuously avoid obstacles until the destination is reached. The present invention addresses the problem of unmanned node obstacle avoidance in complex environments with static and dynamic obstacles. It improves the Theta* algorithm and integrates safe global information into the local planning of the speed obstacle method to plan a safe and reliable route.

Description

Technical Field

[0001] The present invention belongs to the technical field of path planning algorithms, and in particular relates to a speed obstacle planning method and system that integrates safety distance and global information. Background Art

[0002] With the development of science and technology, more and more unmanned intelligent agents are being discovered, and they are being used in exploration, search and rescue, logistics, monitoring, patrol and other fields. Path planning for unmanned intelligent agents involves a comprehensive analysis of global and real-time perception information, which has important applications in industry and agriculture. Unmanned intelligent agents can replace dangerous exploration work and greatly expand the scope of operations.

[0003] Common path planning algorithms can be divided into global path planning and local path planning. Global path planning requires obtaining environmental information in advance. Common global planning algorithms include the A* algorithm, the configuration space method, the free space method, the grid method, and the particle swarm optimization algorithm. The disadvantages of global path planning are poor real-time performance and difficulty processing complex path information. Local path planning, on the other hand, refers to planning in a globally or partially unknown environment using environmental information within a certain range. Common local path planning algorithms include the dynamic window method, the artificial potential field method, and the speed barrier method. The artificial potential field method facilitates real-time control but is prone to falling into local optimality. The dynamic window method has low computational complexity but poor dynamic obstacle avoidance. Local path planning has good real-time performance, but its foresight is poor and it is difficult to consider global environmental information. The planned path is often suboptimal.

[0004] To ensure that unmanned agents can complete their tasks safely and reliably, obstacle avoidance during pathfinding must also be considered. The agent may encounter static or dynamic obstacles. It can use devices like radar and rangefinders to sense the surrounding environment and then take appropriate avoidance actions. Static obstacles are those that are stationary or negligible relative to the agent's movement speed. Dynamic obstacles are those that are in motion.

[0005] As the functions of unmanned intelligent agents become increasingly complex and their needs and tasks become more diverse, new methods for path planning are needed to better adapt to these changes and challenges. Research on path planning and obstacle avoidance for unmanned intelligent agent nodes is in line with the development trend of the intelligent era and meets the needs of various fields such as military defense, production and transportation. This has great scientific significance and application value. Summary of the Invention

[0006] The technical problem to be solved by the present invention is: In order to solve the problems of global planning and local dynamic obstacle avoidance of unmanned nodes on complex paths, the present invention proposes a speed obstacle planning method that integrates safety distance and global information, which solves the problem that it is difficult to consider global information in local obstacle avoidance and how to maintain a safe distance when avoiding obstacles.

[0007] The present invention adopts the following technical solutions to solve the above technical problems:

[0008] The present invention proposes a speed obstacle planning method that integrates safety distance and global information, comprising the following steps:

[0009] S1. The pre-collected real environment map information is converted into a binary image using an image conversion function, and the starting coordinate point and target coordinate point of the unmanned node are determined.

[0010] S2. On the binary graph, start the safe distance Theta* algorithm, plan the optimal preset route under the global environment, and store the turning point coordinate information of the next path.

[0011] S3. Perform local obstacle detection on the map. When dynamic obstacles are found, sort the obstacles by distance, merge the overlapping ranges of the obstacles, and determine the collision area.

[0012] S4. Use the nearest turning point of the preset path as the sub-target coordinate point, use the fusion algorithm to plan the optimal offset angle, and perform dynamic obstacle avoidance.

[0013] S5. Loop the sub-goal coordinate points in step S4 until the number of local obstacles is 0 or the end point is reached, indicating that the path-finding and obstacle avoidance task is completed, and output the final global optimal path.

[0014] Furthermore, in step S1, the map with obstacles is loaded as a binary image and then converted into a matrix containing only 0 and 1, where points with obstacles are 1 and points without obstacles are 0, and the starting coordinate point start and the target coordinate point goal of the unmanned node are determined.

[0015] Furthermore, in step S2, the Theta* algorithm is started, and the specific steps for planning the optimal preset route are as follows:

[0016] S201. The starting coordinate point start is taken as the first node to be checked and added to the OPEN LIST list. OPENLIST represents a list of nodes that have not been checked.

[0017] S202. The unmanned node starts from the starting coordinate point start and searches for a path to eight points around the coordinate system. It puts the traversable path nodes into the OPEN LIST list and sets the starting coordinate point start as the parent node of these nodes. It puts the obstacle points and the traversed path nodes into the CLOSE LIST list. The CLOSE LIST indicates the infeasible nodes that have been checked.

[0018] S203. Take the starting coordinate point start from the OPEN LIST list and put it into the CLOSE LIST list.

[0019] S204. Calculate the node with the lowest cost in the current OPEN LIST of the unmanned node. Define the cost function for the unmanned node to reach the target point from the starting coordinate point through any node n as f(n), the actual distance from the starting coordinate point to any node n as g(n), the estimated distance from node n to the target coordinate point as h(n), and the safety distance function of the current position as k(n). The specific calculation formula is as follows:

[0020] f(n)=g(n)+h(n)+k(n)

[0021] The safety distance function is added to prevent the search path from being too close to obstacles. It is a comprehensive consideration of the shortest path and path safety. The specific formula is:

[0022]

[0023] Among them, μ1 and μ2 are weight coefficients, l is the distance between the current node and the nearest obstacle, and d is the distance between the current position and the end point; the safety distance function ensures that the closer the node is to the obstacle, the greater the cost, and the closer to the end point, the smaller the cost.

[0024] S205 , take the node x with the smallest f(n) value from the OPEN LIST as the path node for the next step, and put it into the CLOSE LIST.

[0025] S206. Exclude all nodes in the CLOSE LIST, add feasible nodes around the current node x to the OPEN LIST, calculate their f(n), g(n), h(n), and k(n) values, and set node x as the parent node. If the adjacent node y of node x is already in the OPEN LIST, calculate the g(n) value of the new path from the starting coordinate point start via node x to node y. Based on the g(n) value, determine whether the unmanned node needs to update the node:

[0026] (1) If the g(n) value of the new path from the unmanned node starting from the starting coordinate point start through node x to node y is smaller than the original g(n) value of node y, then the parent node of the unmanned node is changed to node x, and the f(n) value is recalculated, while h(n) remains unchanged.

[0027] (2) If the g(n) value of the new path is greater than the original g(n) of node y, g(n) remains unchanged.

[0028] S207, repeat steps S204, S205, and S206 in a loop until the target coordinate point of the unmanned node appears in the OPEN LIST list.

[0029] S208. When the number of coordinate points in the OPEN LIST is 0, it indicates that no suitable path has been found. When the target coordinate point of the unmanned node, goal, appears in the OPEN LIST, it indicates that the shortest path from the starting coordinate point, start, to the target coordinate point, goal, has been found. Starting from the target coordinate point, goal, each node moves along the parent node to the starting point of the unmanned node, forming a path for the unmanned node. The set of trajectory coordinate points of the unmanned node, path, is returned, which is the planned optimal preset path result.

[0030] Furthermore, the specific steps of determining the collision area in step S3 are as follows:

[0031] S301: Detect the distance between the obstacle target and the unmanned node and sort them from small to large.

[0032] S302: Calculate the angle of the obstacle target relative to the unmanned node. The specific formula is:

[0033]

[0034] Among them, v ax is the lateral velocity component of the unmanned node, v ay is the longitudinal velocity component of the unmanned node, v bx is the lateral velocity component of the obstacle target, v by is the longitudinal velocity component of the obstacle target.

[0035] S303: Expand the obstacle to reduce the risk of hitting the wall, and calculate the angle of the collision area after expansion. The specific formula is:

[0036]

[0037] Among them, R is the expansion radius of the obstacle target, and D is the distance between the obstacle target and the unmanned node.

[0038] Then calculate the starting angle of the collision area and the ending angle of the collision area. The specific formula is:

[0039]

[0040] Among them, θ e It is half of the angle between the obstacle target and the unmanned node, θ1 is the starting angle of the collision area, and θ2 is the ending angle of the collision area.

[0041] S304 , sorting the obstacle collision areas from small to large according to their starting angles.

[0042] S305, merge the collision areas of the obstacles; set the collision area angle of two adjacent obstacle targets sorted from small to large to be θ a1 ,θ a2 and θ b1 ,θ b2 , if θ a1 ≤θ b1 ≤θ a2 , it means that the collision areas of the two adjacent obstacles have overlapping parts and need to be merged. The starting angle and ending angle of the new collision area after merging are θ a1 and θ b2 Otherwise, it means there is no overlap and no merging is required.

[0043] Furthermore, the specific contents of planning the optimal offset angle in step S4 are as follows:

[0044] In order to ensure the optimization of local obstacle avoidance, the global preset route information is integrated, and the fusion function is:

[0045] G(θ)=k(α·goal+β·path+γ·head(θ))

[0046] Among them, goal is the distance function between the unmanned node and the end point; path is the distance function between the end of the unmanned node trajectory and the global path. The shorter the distance, the closer it is to the global optimal route; k is the smoothing coefficient; α, β, and γ are weighting coefficients.

[0047] The specific formula of the distance function path of the global path is:

[0048]

[0049] Among them, x i 、y i is the coordinate of the end point of the planned local path, x i 、y i are the node coordinates for the global path planned by the secure Theta* algorithm.

[0050] The specific formula of the distance function goal between the unmanned node and the end point is:

[0051]

[0052] Among them, x i 、y i is the coordinate of the end point of the planned local path, x goal 、y goal are the endpoint coordinates of the unmanned node.

[0053] The specific formula of the error function head of the current turning and deviation angles toward the end point in the local path planned by the unmanned node is:

[0054]

[0055] Among them, θ n is the new direction of the unmanned node, θ is the offset, θ goal is the angle towards the end point.

[0056] Unmanned node new direction θ n The specific formula is:

[0057]

[0058] Among them, θ m1 and θ m2 They are the starting angle and ending angle of the obstacle collision area closest to the unmanned node.

[0059] Solve the fusion function G(θ) so that the angle with the minimum value of the G(θ) function is the optimal offset angle. Then, the optimal route can be obtained based on the optimal offset angle. At this time, the coordinates of the unmanned node at the next moment are:

[0060]

[0061] Among them, newX and newY are the horizontal and vertical coordinates of the unmanned node's updated position, curX and curY are the horizontal and vertical coordinates of the unmanned node's position before it changes direction, and v is the speed of the unmanned node.

[0062] Furthermore, the present invention also proposes a speed obstacle planning system that integrates safety distance and global information, including:

[0063] The coordinate point module is used to convert the pre-collected real environment map information into a binary image and determine the starting coordinate point and target coordinate point of the unmanned node.

[0064] The safety distance Theta* algorithm module is used to plan the optimal preset route in the global environment and store the turning point coordinate information of the next path.

[0065] The collision area module is used to detect local obstacles on the map. When dynamic obstacles are found, the obstacles are sorted by distance, the overlapping ranges of the obstacles are merged, and the collision area is determined.

[0066] The offset angle module is used to use the nearest turning point of the preset path as the sub-target coordinate point, and use the fusion algorithm to plan the optimal offset angle for dynamic obstacle avoidance.

[0067] The optimal path module is used to loop the sub-target coordinate points in the offset angle module until the number of local obstacles is 0 or the end point is reached, and output the final global optimal path.

[0068] Furthermore, in the coordinate point module, the map with obstacles is loaded as a binary image and then converted into a matrix containing only 0 and 1. The points with obstacles are 1, and the points without obstacles are 0. The starting coordinate point start and the target coordinate point goal of the unmanned node are determined.

[0069] Furthermore, in the safety distance Theta* algorithm module, the specific steps for planning the optimal preset route are as follows:

[0070] Step 1: Take the starting coordinate point start as the first node to be checked and add it to the OPEN LIST list. OPEN LIST represents a list of nodes that have not been checked.

[0071] Step 2: The unmanned node starts from the starting coordinate point start and searches for a path to the eight surrounding points. It puts the traversable path nodes into the OPEN LIST list and sets the starting coordinate point start as the parent node of these nodes. It puts the obstacle points and the traversed path nodes into the CLOSE LIST list. The CLOSE LIST represents the infeasible nodes that have been checked.

[0072] Step 3: Take the starting point coordinate point start from the OPEN LIST list and put it into the CLOSE LIST list.

[0073] Step 4. Calculate the node with the lowest cost in the unmanned node's current OPEN LIST. Define the cost function for the unmanned node to reach the target point from the starting coordinate point through any node n as f(n), the actual distance from the starting coordinate point to any node n as g(n), the estimated distance from node n to the target coordinate point as h(n), and the safety distance function of the current position as k(n). The specific calculation formula is as follows:

[0074] f(n)=g(n)+h(n)+k(n)

[0075]

[0076] Where μ1 and μ2 are weight coefficients, l is the distance between the current node and the nearest obstacle, and d is the distance between the current position and the end point.

[0077] Step 5: Take the node x with the smallest f(n) value from the OPEN LIST as the path node for the next step and put it into the CLOSE LIST.

[0078] Step 6: Exclude all nodes in the CLOSE LIST, add the feasible nodes around the current node x to the OPEN LIST, calculate their f(n), g(n), h(n), and k(n) values, and set node x as the parent node; if the adjacent node y of node x is already in the OPEN LIST set, calculate the g(n) value of the new path from the starting point start through node x to node y, and determine whether the unmanned node needs to update the node based on the g(n) value:

[0079] (1) If the g(n) value of the new path from the unmanned node starting from the starting coordinate point start through node x to node y is smaller than the original g(n) value of node y, then the parent node of the unmanned node is changed to node x, and the f(n) value is recalculated, while h(n) remains unchanged.

[0080] (2) If the g(n) value of the new path is greater than the original g(n) of node y, g(n) remains unchanged.

[0081] Step 7: Repeat steps 4, 5, and 6 until the target coordinate point of the unmanned node appears in the OPEN LIST list.

[0082] Step 8. When the number of coordinate points in the OPEN LIST is 0, it means that no suitable path has been found. When the target coordinate point of the unmanned node, goal, appears in the OPEN LIST, it means that the shortest path from the starting point, start, to the target coordinate point, goal, has been found. Starting from the target coordinate point, goal, each node moves along the parent node to the starting point of the unmanned node, forming a path for the unmanned node. The set of trajectory coordinate points of the unmanned node, path, is returned, which is the planned optimal preset path result.

[0083] Furthermore, in the collision area module, the specific steps for determining the collision area are as follows:

[0084] Step 1: Detect the distance between the obstacle target and the unmanned node and sort them from small to large.

[0085] Step 2: Calculate the angle of the obstacle target relative to the unmanned node. The specific formula is:

[0086]

[0087] Among them, v ax is the lateral velocity component of the unmanned node, v ay is the longitudinal velocity component of the unmanned node, v bx is the lateral velocity component of the obstacle target, v by is the longitudinal velocity component of the obstacle target.

[0088] Step 3: Expand the obstacle and calculate the collision angle after expansion. The specific formula is:

[0089]

[0090] Among them, R is the expansion radius of the obstacle target, and D is the distance between the obstacle target and the unmanned node.

[0091] Then calculate the starting angle of the collision area and the ending angle of the collision area. The specific formula is:

[0092]

[0093] Among them, θ e It is half of the angle between the obstacle target and the unmanned node, θ1 is the starting angle of the collision area, and θ2 is the ending angle of the collision area.

[0094] Step 4: Sort the obstacle collision areas from small to large according to their starting angles.

[0095] Step 5: Merge the collision areas of the obstacles; set the collision area angle between two adjacent obstacle targets sorted from small to large to be θ a1 ,θ a2 and θ b1 ,θ b2 , if θ a1 ≤θ b1 ≤θ a2 , it means that the collision areas of the two adjacent obstacles have overlapping parts and need to be merged. The starting angle and ending angle of the new collision area after merging are θ a1 and θ b2 Otherwise, it means there is no overlap and no merging is required.

[0096] Furthermore, in the offset angle module, the specific contents of planning the optimal offset angle are as follows:

[0097] Integrate the global preset route information, and its fusion function is:

[0098] G(θ)=k(α·goal+β·path+γ·head(θ))

[0099] Among them, goal is the distance function between the unmanned node and the end point, path is the distance function between the end of the unmanned node trajectory and the global path, k is the smoothing coefficient, and α, β, and γ are weighting coefficients.

[0100] The specific formula of the distance function path of the global path is:

[0101]

[0102] Among them, x i 、y i is the coordinate of the end point of the planned local path, are the node coordinates for the global path planned by the secure Theta* algorithm.

[0103] The specific formula of the distance function goal between the unmanned node and the end point is:

[0104]

[0105] Among them, x i 、y i is the coordinate of the end point of the planned local path, x goal 、y goal are the endpoint coordinates of the unmanned node.

[0106] The specific formula of the error function head of the current turning and deviation angles toward the end point in the local path planned by the unmanned node is:

[0107]

[0108] Among them, θ n is the new direction of the unmanned node, θ is the offset, θ goal is the angle towards the end point.

[0109] Unmanned node new direction θ n The specific formula is:

[0110]

[0111] Among them, θ m1 and θ m2 They are the starting angle and ending angle of the obstacle collision area closest to the unmanned node.

[0112] Solve the fusion function G(θ) so that the angle with the minimum value of the G(θ) function is the optimal offset angle. Then, the optimal route can be obtained based on the optimal offset angle. The coordinates at the next moment are:

[0113]

[0114] Among them, newX and newY are the horizontal and vertical coordinates of the unmanned node's updated position, curX and curY are the horizontal and vertical coordinates of the unmanned node's position before it changes direction, and v is the speed of the unmanned node.

[0115] The present invention adopts the above technical solution, and compared with the prior art, its significant technical effects are as follows:

[0116] This speed obstacle avoidance method, which integrates safety distance and global information, plans routes with greater safety than the traditional Theta* algorithm. The route maintains a certain safe distance from obstacles. Furthermore, the method integrates local planning information, resulting in high real-time performance and enhanced ability to cope with sudden dynamic obstacles. Compared to traditional local planning methods, the fused evaluation function fully considers global information, calculates the optimal obstacle avoidance offset angle, and finds the global optimal solution, enabling safe dynamic obstacle avoidance along the optimal route. Experiments have shown that the unmanned node speed obstacle dynamic avoidance method, which integrates safety distance and global information, plans routes with good real-time performance and increased safety, resolving the issues of poor real-time performance in global planning and the difficulty of local obstacle avoidance in considering global information and maintaining a safe distance. BRIEF DESCRIPTION OF THE DRAWINGS

[0117] Figure 1 It is a flow chart of the present invention.

[0118] Figure 2 This is a comparative experimental diagram of the safe distance path diagram output by different Theta* algorithms.

[0119] Figure 3 It is a schematic diagram of a speed barrier of the present invention.

[0120] Figure 4 Schematic diagram of the fusion algorithm of the present invention.

[0121] Figure 5 This is a diagram of a dynamic obstacle avoidance experiment of a single obstacle according to the present invention.

[0122] Figure 6 This is a diagram of a dynamic obstacle avoidance experiment with multiple obstacles according to the present invention.

[0123] Figure 7 It is the distance diagram between unmanned nodes and obstacles of the present invention. DETAILED DESCRIPTION

[0124] In order to enable technicians to better understand the method of the present invention, the technical solutions in the embodiments of the present invention will be described in detail below with reference to the accompanying drawings in the examples of the present invention.

[0125] To achieve the above objectives, the present invention provides a speed obstacle planning method that integrates safety distance and global information, such as Figure 1 As shown, the following steps are included:

[0126] S1. The pre-collected real environment map information is converted into a binary image using an image conversion function, and the starting point start(3,5) and target point goal(250,250) of the unmanned node are determined. The map with obstacles is loaded as a binary image and then converted into a matrix containing only 0 and 1, where points with obstacles are 1 and points without obstacles are 0.

[0127] S2. On the binary graph, start the safe distance Theta* algorithm to plan the optimal preset route under the global environment and store the inflection point coordinate information of the path, such as Figure 2 As shown, Figure 2 (a) is the path diagram output by the original Theta* algorithm. The black curve in the figure shows the trajectory of the unmanned node when performing path planning in the binary graph of the simulated sea area. This trajectory is obviously close to obstacles and poses a great safety hazard. Figure 2 Figure (b) shows the safe distance path diagram output by the proposed method. The figure shows that the trajectory of the unmanned node's path planning always maintains a certain distance from obstacles. Comparing the two, the results show that the path planned by the safe distance Theta* algorithm is safer and more reliable. The specific steps of the algorithm are as follows:

[0128] S201. The starting coordinate point start is taken as the first node to be checked and added to the OPEN LIST list. OPENLIST represents a list of nodes that have not been checked.

[0129] S202. The unmanned node starts from the starting coordinate point start and searches for a path to eight points around the coordinate system. It puts the traversable path nodes into the OPEN LIST list and sets the starting coordinate point start as the parent node of these nodes. It puts the obstacle points with a value of 1 and the traversed path nodes into the CLOSE LIST list. The CLOSE LIST indicates the infeasible nodes that have been checked.

[0130] S203. Take the starting coordinate point start from the OPEN LIST list and put it into the CLOSE LIST list.

[0131] S204. Calculate the node with the lowest cost in the current OPEN LIST of the unmanned node. Define the cost function for the unmanned node to reach the target point from the starting coordinate point through any node n as f(n), the actual distance from the starting coordinate point to any node n as g(n), the estimated distance from node n to the target coordinate point as h(n), and the safety distance function of the current position as k(n). The specific calculation formula is as follows:

[0132] f(n)=g(n)+h(n)+k(n)

[0133] The safety distance function is added to prevent the search path from being too close to obstacles. It is a comprehensive consideration of the shortest path and path safety. The specific formula is:

[0134]

[0135] Where μ1 and μ2 are weight coefficients, set to 0.427 and 0.573, respectively. l is the distance from the current node to the nearest obstacle, and d is the distance from the current location to the destination. The safety distance function ensures that the closer a node is to an obstacle, the greater its cost, while the closer it is to the destination, the smaller its cost.

[0136] S205. Take the node x with the smallest f(n) value from the OPEN LIST as the next path node and put it into the CLOSE LIST.

[0137] S206. Exclude all nodes in the CLOSE LIST, add feasible nodes around the current node x to the OPEN LIST, calculate their f(n), g(n), h(n), and k(n) values, and set node x as the parent node. If the adjacent node y of node x is already in the OPEN LIST, calculate the g(n) value of the new path from the starting coordinate point start via node x to node y. Based on the g(n) value, determine whether the unmanned node needs to update the node:

[0138] (1) If the g(n) value of the new path from the unmanned node starting from the starting coordinate point start through node x to node y is smaller than the original g(n) value of node y, then the parent node of the unmanned node is changed to node x, and the f(n) value is recalculated, while h(n) remains unchanged.

[0139] (2) If the g(n) value of the new path is greater than the original g(n) of node y, g(n) remains unchanged.

[0140] S207, repeat steps S204, S205, and S206 in a loop until the target coordinate point of the unmanned node appears in the OPEN LIST list.

[0141] S208. When the number of coordinate points in the OPEN LIST is 0, it indicates that no suitable path has been found. When the target coordinate point of the unmanned node, goal, appears in the OPEN LIST, it indicates that the shortest path from the starting coordinate point, start, to the target coordinate point, goal, has been found. Starting from the target coordinate point, goal, each node moves along the parent node to the starting point of the unmanned node, forming a path for the unmanned node. The coordinate point set, path, is returned, which is the planned optimal preset path result.

[0142] S3, such as Figure 3 The figure below shows a schematic diagram of speed obstacles. The agent performs local obstacle detection on the map. If a dynamic obstacle is found, it sorts the obstacles by distance, merges the overlapping ranges of the obstacles, and determines the collision area. The specific steps are as follows:

[0143] S301: Detect the distance between the obstacle target and the unmanned node and sort them from small to large.

[0144] S302: Calculate the angle of the obstacle target relative to the unmanned node. The specific formula is:

[0145]

[0146] Among them, v ax is the lateral velocity component of the unmanned node, v ay is the longitudinal velocity component of the unmanned node; v bx is the lateral velocity component of the obstacle target, v by is the longitudinal velocity component of the obstacle target.

[0147] S303: Expand the obstacle to reduce the risk of hitting the wall, and calculate the angle of the collision area after expansion. The specific formula is:

[0148]

[0149] Among them, R is the expansion radius of the obstacle target; D is the distance between the obstacle target and the unmanned node.

[0150] Then calculate the starting angle of the collision area and the ending angle of the collision area. The specific formula is:

[0151]

[0152] Among them, θ e It is half of the angle between the obstacle target and the unmanned node in the collision area; θ1 is the starting angle of the collision area, and θ2 is the ending angle of the collision area.

[0153] S304 , sorting the collision areas of the obstacles from small to large according to their starting angles.

[0154] S305: Merge the collision areas of the obstacles. Assume that the collision area angle between two adjacent obstacle targets sorted from small to large is θ a1 ,θ a2 and θ b1 ,θ b2 ; if θ a1 ≤θ b1 ≤θ a2, it means that the collision areas of the two adjacent obstacles have overlapping parts and need to be merged. The starting angle and ending angle of the new collision area after merging are θ a1 and θ b2 Otherwise, it means there is no overlap and no merging is required.

[0155] S4, such as Figure 4 The figure below is a schematic diagram of algorithm fusion. Taking the nearest turning point of the preset path as the sub-target coordinate point, the fusion algorithm is used to plan the optimal offset angle for dynamic obstacle avoidance. The specific steps are as follows:

[0156] In order to ensure the optimization of local obstacle avoidance, the global preset route information is integrated, and the fusion function is:

[0157] G(θ)=k(α·goal+β·path+γ·head(θ))

[0158] Here, goal is the distance function from the unmanned node to the endpoint, and path is the distance function from the end of the unmanned node's trajectory to the global path. Shorter distances indicate closer proximity to the global optimal path. k is the smoothing coefficient, set to 1.732. α, β, and γ are weighting coefficients, set to 0.241, 0.457, and 0.302, respectively.

[0159] The specific formula of the distance function path of the global path is:

[0160]

[0161] Among them, x i 、y i is the coordinate of the end point of the planned local path, x′ i , y′ i are the node coordinates for the global path planned by the secure Theta* algorithm.

[0162] The specific formula of the distance function goal between the unmanned node and the end point is:

[0163]

[0164] Among them, x i 、y i is the coordinate of the end point of the planned local path, x goal 、y goal are the endpoint coordinates of the unmanned node.

[0165] The specific formula of the error function head of the current turning and deviation angles toward the end point in the local path planned by the unmanned node is:

[0166]

[0167] Among them, θ n is the new direction of the unmanned node, θ is the offset, θ goal is the angle towards the end point.

[0168] Unmanned node new direction θ n The specific formula is:

[0169]

[0170] Among them, θ m1 and θ m2 They are the starting angle and ending angle of the obstacle collision area closest to the unmanned node.

[0171] Solve the fusion function G(θ) so that the angle with the minimum value of the G(θ) function is the optimal offset angle. Then, the optimal route can be obtained based on the optimal offset angle. At this time, the coordinates of the unmanned node at the next moment are:

[0172]

[0173] Among them, newX and newY are the horizontal and vertical coordinates of the unmanned node's updated position, curX and curY are the horizontal and vertical coordinates of the unmanned node's position before it changes direction, and v is the speed of the unmanned node.

[0174] S5. Loop through step S4 to find the sub-target coordinate points until the number of local obstacles is 0 or the end point is reached, indicating that the path-finding and obstacle avoidance task is completed and the final global optimal path is obtained.

[0175] In order to verify the effectiveness and feasibility of the method of the present invention, a simulation experiment was conducted. The experimental environment is: WIN10, i5-7200U CPU; the compilation environment is: MATLAB R2020a simulation platform. The parameter information used in the experiment is shown in Table 1, and the experimental results are shown in Figure 5-7 .

[0176] Table 1 Parameter settings

[0177]

[0178] like Figure 5 As shown in the figure, obstacle avoidance simulation is performed on a single dynamic obstacle, with the starting coordinates set to (3,5) and the end coordinates set to (250,250). Figure 5 Figure (a) shows the situation where the unmanned node moves along the preset optimal route. When a dynamic obstacle is detected, the fusion function is calculated to obtain the optimal deflection angle to plan the local route and start obstacle avoidance. Figure 5 Figure (b) shows an unmanned node that avoids obstacles and continues to move toward the optimal preset route. The simulation results show that this method can accurately avoid obstacles in real time.

[0179] like Figure 6 As shown in the figure, in order to further simulate the complex obstacle avoidance scenario, three groups of obstacles are set up to interfere with the forward route of the unmanned node from different directions. Figure 6 Figure (a) shows that the unmanned node avoids three groups of dynamic obstacles in turn, and the local route is always close to the optimal preset route, which is in line with the expected results and verifies the feasibility and effectiveness of this method in complex scenarios. Figure 6 Figure (b) shows the angle changes of the unmanned node during obstacle avoidance. As can be seen from the figure, the unmanned node has three sudden changes in angle, corresponding to three obstacle avoidance scenarios. In other cases, the angle of the unmanned node has hardly changed. This proves that this method has high real-time performance in the face of sudden obstacle avoidance scenarios. Figure 7 The figure shows the change in the distance between the unmanned node and the obstacle from the moment it encounters the first obstacle to the moment it leaves the second obstacle. As can be seen from the figure, the distance curve from approaching the obstacle to leaving it progresses from high to low and then back to high, but maintains a certain distance at the lowest point. This indicates that the unmanned node consistently maintains a safe distance when avoiding obstacles. This further verifies that this method ensures real-time obstacle avoidance while also balancing safety.

[0180] The present invention also proposes a speed obstacle planning system that integrates safety distance and global information, including a coordinate point module, a safety distance Theta* algorithm module, a collision area module, an offset angle module, an optimal path module, and a computer program executable on a processor. It should be noted that each module in the above system corresponds to the specific steps of the method provided in the present invention, and has the corresponding functional modules and beneficial effects of the execution method. For technical details not fully described in this embodiment, please refer to the method provided in the present invention.

[0181] The above embodiments are only for illustrating the technical idea of ​​the present invention and cannot be used to limit the protection scope of the present invention. Any changes made on the basis of the technical solution in accordance with the technical idea proposed by the present invention shall fall within the protection scope of the present invention.

Claims

1. A speed obstacle planning method integrating safety distance and global information, characterized in that: The following steps are involved: S1. Convert the pre-collected real environment map information into a binary image using an image conversion function, and determine the starting coordinate point and target coordinate point of the unmanned node; S2. On the binary graph, start the safe distance Theta* algorithm to plan the optimal preset route under the global environment and store the inflection point coordinate information of the path; S3. Perform local obstacle detection on the map. When dynamic obstacles are found, sort the obstacles by distance, merge the overlapping ranges of the obstacles, and determine the collision area. S4: Using the nearest turning point of the preset path as the sub-target coordinate point, the fusion algorithm is used to plan the optimal offset angle for dynamic obstacle avoidance. The specific content is as follows: Integrate the global preset route information, and its fusion function is: G(θ)=k(α·goal+β·path+γ·head(θ)) Among them, goal is the distance function between the unmanned node and the end point, path is the distance function between the end of the unmanned node trajectory and the global path, k is the smoothing coefficient, α, β, and γ are weighting coefficients; The specific formula of the distance function path of the global path is: Among them, x i 、y i is the coordinate of the end point of the planned local path, x′ i , y′ i are the node coordinates of the global path planned by the secure Theta* algorithm; The specific formula of the distance function goal between the unmanned node and the end point is: Among them, x i 、y i is the coordinate of the end point of the planned local path, x goal 、y goal is the end coordinate of the unmanned node; The specific formula of the error function head of the current turning and deviation angles toward the end point in the local path planned by the unmanned node is: Among them, θ n is the new direction of the unmanned node, θ is the offset, θ goal is the angle toward the end point; Unmanned node new direction θ n The specific formula is: Among them, θ m1 and θ m2 They are the starting angle and ending angle of the obstacle collision area closest to the unmanned node; Solve the fusion function G(θ) so that the angle with the minimum value of the G(θ) function is the optimal offset angle. Then, the optimal route can be obtained based on the optimal offset angle. At this time, the coordinates of the unmanned node at the next moment are: Among them, newX and newY are the horizontal and vertical coordinates of the unmanned node's updated position, curX and curY are the horizontal and vertical coordinates of the unmanned node's position before it changes direction, and v is the speed of the unmanned node; S5. Loop the sub-goal coordinate points in step S4 until the number of local obstacles is 0 or the end point is reached, indicating that the path-finding and obstacle avoidance task is completed, and output the final global optimal path.

2. The speed obstacle planning method integrating safety distance and global information according to claim 1, characterized in that: In step S1, the map with obstacles is loaded as a binary image and then converted into a matrix containing only 0 and 1. The points with obstacles are 1 and the points without obstacles are 0. The starting coordinate point start and the target coordinate point goal of the unmanned node are determined.

3. The speed obstacle planning method integrating safety distance and global information according to claim 1, characterized in that: In step S2, the safe distance Theta* algorithm is started, and the specific steps for planning the optimal preset route are as follows: S201, take the starting coordinate point start as the first node to be checked and add it to the OPEN LIST list, where OPENLIST represents a list of nodes that have not been checked; S202: The unmanned node starts from the starting coordinate point start and searches for a path to the eight surrounding points. It puts the traversable path nodes into the OPEN LIST list and sets the starting coordinate point start as the parent node of these nodes. It puts the obstacle points and the traversed path nodes into the CLOSE LIST list. The CLOSE LIST represents the infeasible nodes that have been checked. S203, taking the starting coordinate point start from the OPEN LIST list and putting it into the CLOSE LIST list; S204. Calculate the node with the lowest cost in the current OPEN LIST of the unmanned node. Define the cost function for the unmanned node to reach the target point from the starting coordinate point through any node n as f(n), the actual distance from the starting coordinate point to any node n as g(n), the estimated distance from node n to the target coordinate point as h(n), and the safety distance function of the current position as k(n). The specific calculation formula is as follows: f(n)=g(n)+h(n)+k(n) Among them, μ1 and μ2 are weight coefficients, l is the distance between the current node and the nearest obstacle, and d is the distance between the current position and the end point; S205. Take the node x with the smallest f(n) value from the OPEN LIST as the next path node and put it into the CLOSE LIST. S206. Exclude all nodes in the CLOSE LIST, add feasible nodes around the current node x to the OPEN LIST, calculate their f(n), g(n), h(n), and k(n) values, and set node x as the parent node. If the adjacent node y of node x is already in the OPEN LIST, calculate the g(n) value of the new path from the starting coordinate point start via node x to node y. Based on the g(n) value, determine whether the unmanned node needs to update the node: (1) If the g(n) value of the new path from the unmanned node starting from the starting coordinate point start via node x to node y is smaller than the original g(n) value of node y, then the parent node of the unmanned node is changed to node x, and the f(n) value is recalculated, while h(n) remains unchanged; (2) If the g(n) value of the new path is greater than the original g(n) of node y, g(n) remains unchanged; S207, repeat steps S204, S205, and S206 in a loop until the target coordinate point of the unmanned node appears in the OPENLIST list; S208. When the number of coordinate points in the OPEN LIST is 0, it indicates that no suitable path has been found. When the target coordinate point of the unmanned node, goal, appears in the OPEN LIST, it indicates that the shortest path from the starting coordinate point, start, to the target coordinate point, goal, has been found. Starting from the target coordinate point, goal, each node moves along the parent node to the starting point of the unmanned node, forming a path for the unmanned node. The set of trajectory coordinate points of the unmanned node, path, is returned, which is the planned optimal preset path result.

4. The speed obstacle planning method integrating safety distance and global information according to claim 1, characterized in that: The specific steps for determining the collision area in step S3 are as follows: S301, detecting the distance between the obstacle target and the unmanned node and sorting them from small to large; S302: Calculate the angle of the obstacle target relative to the unmanned node. The specific formula is: Among them, v ax is the lateral velocity component of the unmanned node, v ay is the longitudinal velocity component of the unmanned node, v bx is the lateral velocity component of the obstacle target, v by is the longitudinal velocity component of the obstacle target; S303: Expand the obstacle and calculate the collision area angle after expansion. The specific formula is: Among them, R is the expansion radius of the obstacle target, and D is the distance between the obstacle target and the unmanned node; Then calculate the starting angle of the collision area and the ending angle of the collision area. The specific formula is: Among them, θ e It is half of the angle between the obstacle target and the unmanned node, θ1 is the starting angle of the collision area, and θ2 is the ending angle of the collision area; S304: Sort the obstacles in ascending order according to their starting angles in the collision area. S305, merge the collision areas of the obstacles; set the collision area angle of two adjacent obstacle targets sorted from small to large to be θ a1 ,θ a2 and θ b1 ,θ b2 , if θ a1 ≤θ b1 ≤θ a2 , it means that the collision areas of the two adjacent obstacles have overlapping parts and need to be merged. The starting angle and ending angle of the new collision area after merging are θ a1 and θ b2 Otherwise, it means there is no overlap and no merging is required.

5. A speed obstacle planning system integrating safety distance and global information, characterized in that: include: The coordinate point module is used to convert the pre-collected real environment map information into a binary image to determine the starting coordinate point and target coordinate point of the unmanned node; The safe distance Theta* algorithm module is used to plan the optimal preset route under the global environment and store the coordinate information of the turning points of the path; The collision area module is used to detect local obstacles on the map. When dynamic obstacles are found, the obstacles are sorted by distance, the overlapping ranges of the obstacles are merged, and the collision area is determined. The offset angle module is used to use the nearest turning point of the preset path as the sub-target coordinate point, and use the fusion algorithm to plan the optimal offset angle for dynamic obstacle avoidance. The specific contents are as follows: Integrate the global preset route information, and its fusion function is: G(θ)=k(α·goal+β·path+γ·head(θ)) Among them, goal is the distance function between the unmanned node and the end point, path is the distance function between the end of the unmanned node trajectory and the global path, k is the smoothing coefficient, α, β, and γ are weighting coefficients; The specific formula of the distance function path of the global path is: Among them, x i 、y i is the coordinate of the end point of the planned local path, x i ′、y i ′ is the node coordinate of the global path planned by the secure Theta* algorithm; The specific formula of the distance function goal between the unmanned node and the end point is: Among them, x i 、y i is the coordinate of the end point of the planned local path, x goal 、y goal is the end coordinate of the unmanned node; The specific formula of the error function head of the current turning and deviation angles toward the end point in the local path planned by the unmanned node is: Among them, θ n is the new direction of the unmanned node, θ is the offset, θ goal is the angle toward the end point; Unmanned node new direction θ n The specific formula is: Among them, θ m1 and θ m2 They are the starting angle and ending angle of the obstacle collision area closest to the unmanned node; Solve the fusion function G(θ) so that the angle with the minimum value of the G(θ) function is the optimal offset angle. Then, the optimal route can be obtained based on the optimal offset angle. The coordinates at the next moment are: Among them, newX and newY are the horizontal and vertical coordinates of the unmanned node's updated position, curX and curY are the horizontal and vertical coordinates of the unmanned node's position before it changes direction, and v is the speed of the unmanned node; The optimal path module is used to loop the sub-target coordinate points in the offset angle module until the number of local obstacles is 0 or the end point is reached, and output the final global optimal path.

6. The speed obstacle planning system integrating safety distance and global information according to claim 5, characterized in that: In the coordinate point module, the map with obstacles is loaded as a binary image and then converted into a matrix containing only 0 and 1. Points with obstacles are 1, and points without obstacles are 0. The starting coordinate point start and the target coordinate point goal of the unmanned node are determined.

7. The speed obstacle planning system integrating safety distance and global information according to claim 5, characterized in that: In the safety distance Theta* algorithm module, the specific steps for planning the optimal preset route are as follows: Step 1: Take the starting coordinate point start as the first node to be checked and add it to the OPEN LIST list. OPENLIST represents the list of nodes that have not been checked. Step 2: The unmanned node starts from the starting coordinate point start and searches for a path to the eight surrounding points. It puts the traversable path nodes into the OPEN LIST list and sets the starting coordinate point start as the parent node of these nodes. It puts the obstacle points and the traversed path nodes into the CLOSE LIST list. The CLOSE LIST represents the infeasible nodes that have been checked. Step 3. Take the starting point coordinate point start from the OPEN LIST list and put it into the CLOSE LIST list; Step 4. Calculate the node with the lowest cost in the unmanned node's current OPEN LIST. Define the cost function for the unmanned node to reach the target point from the starting coordinate point through any node n as f(n), the actual distance from the starting coordinate point to any node n as g(n), the estimated distance from node n to the target coordinate point as h(n), and the safety distance function of the current position as k(n). The specific calculation formula is as follows: f(n)=g(n)+h(n)+k(n) Where μ1 and μ2 are weight coefficients, l is the distance between the current node and the nearest obstacle, and d is the distance between the current position and the end point; Step 5: Take the node x with the smallest f(n) value from the OPEN LIST as the path node for the next step and put it into the CLOSE LIST. Step 6: Exclude all nodes in the CLOSE LIST list, add the feasible nodes around the current node x to the OPEN LIST list, calculate their f(n), g(n), h(n), and k(n) values, and set node x as the parent node; if the adjacent node y of node x is already in the OPEN LIST set, calculate the g(n) value of the new path from the starting point start through node x to node y, and determine whether the unmanned node needs to update the node based on the g(n) value: (1) If the g(n) value of the new path from the unmanned node starting from the starting coordinate point start via node x to node y is smaller than the original g(n) value of node y, then the parent node of the unmanned node is changed to node x, and the f(n) value is recalculated, while h(n) remains unchanged; (2) If the g(n) value of the new path is greater than the original g(n) of node y, g(n) remains unchanged; Step 7: Repeat steps 4, 5, and 6 until the target coordinate point of the unmanned node appears in the OPEN LIST list. Step 8. When the number of coordinate points in the OPEN LIST is 0, it means that no suitable path has been found; When the target coordinate point goal of the unmanned node appears in the OPEN LIST, it means that the shortest path from the starting coordinate point start to the target coordinate point goal of the unmanned node has been found. Starting from the target coordinate point goal, each node moves along the parent node to the starting point of the unmanned node, forming the path of the unmanned node. The set of trajectory coordinate points of the unmanned node path is returned, which is the planned optimal preset path result.

8. The speed obstacle planning system integrating safety distance and global information according to claim 5, characterized in that: In the collision area module, the specific steps for determining the collision area are as follows: Step 1: Detect the distance between the obstacle target and the unmanned node and sort them from small to large; Step 2: Calculate the angle of the obstacle target relative to the unmanned node. The specific formula is: Among them, v ax is the lateral velocity component of the unmanned node, v ay is the longitudinal velocity component of the unmanned node, v bx is the lateral velocity component of the obstacle target, v by is the longitudinal velocity component of the obstacle target; Step 3: Expand the obstacle and calculate the collision angle after expansion. The specific formula is: Among them, R is the expansion radius of the obstacle target, and D is the distance between the obstacle target and the unmanned node; Then calculate the starting angle of the collision area and the ending angle of the collision area. The specific formula is: Among them, θ e It is half of the angle between the obstacle target and the unmanned node, θ1 is the starting angle of the collision area, and θ2 is the ending angle of the collision area; Step 4: Sort the obstacles in ascending order according to their starting angles in the collision area. Step 5: Merge the collision areas of the obstacles; set the collision area angle between two adjacent obstacle targets sorted from small to large to be θ a1 ,θ a2 and θ b1 ,θ b2 , if θ a1 ≤θ b1 ≤θ a2 , it means that the collision areas of the two adjacent obstacles have overlapping parts and need to be merged. The starting angle and ending angle of the new collision area after merging are θ a1 and θ b2 Otherwise, it means there is no overlap and no merging is required.

Images