CN116203956A - Speed obstacle planning method and system integrating safe distance and global information - Google Patents
Speed obstacle planning method and system integrating safe distance and global information Download PDFInfo
- Publication number
- CN116203956A CN116203956A CN202310167360.XA CN202310167360A CN116203956A CN 116203956 A CN116203956 A CN 116203956A CN 202310167360 A CN202310167360 A CN 202310167360A CN 116203956 A CN116203956 A CN 116203956A
- Authority
- CN
- China
- Prior art keywords
- node
- unmanned
- obstacle
- path
- point
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 50
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 35
- 238000004364 calculation method Methods 0.000 claims abstract description 8
- PCHJSUWPFVWCPO-UHFFFAOYSA-N gold Chemical compound [Au] PCHJSUWPFVWCPO-UHFFFAOYSA-N 0.000 claims description 25
- 239000010931 gold Substances 0.000 claims description 25
- 229910052737 gold Inorganic materials 0.000 claims description 25
- 230000004888 barrier function Effects 0.000 claims description 18
- 230000004927 fusion Effects 0.000 claims description 16
- 238000010586 diagram Methods 0.000 claims description 12
- 230000008569 process Effects 0.000 claims description 6
- 238000009499 grossing Methods 0.000 claims description 5
- 239000011159 matrix material Substances 0.000 claims description 5
- 238000012163 sequencing technique Methods 0.000 claims description 5
- 238000006243 chemical reaction Methods 0.000 claims description 3
- 230000003068 static effect Effects 0.000 abstract description 3
- 238000011156 evaluation Methods 0.000 abstract description 2
- 230000006870 function Effects 0.000 description 38
- 238000002474 experimental method Methods 0.000 description 5
- 230000008859 change Effects 0.000 description 3
- 238000005457 optimization Methods 0.000 description 3
- 238000004088 simulation Methods 0.000 description 3
- 238000011161 development Methods 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 230000007613 environmental effect Effects 0.000 description 2
- 238000013459 approach Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000000052 comparative effect Effects 0.000 description 1
- 238000004590 computer program Methods 0.000 description 1
- 230000010485 coping Effects 0.000 description 1
- 230000007123 defense Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000002452 interceptive effect Effects 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012544 monitoring process Methods 0.000 description 1
- 230000035772 mutation Effects 0.000 description 1
- 239000002245 particle Substances 0.000 description 1
- 230000008447 perception Effects 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0214—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with safety or protection criteria, e.g. avoiding hazardous areas
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0223—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving speed control of the vehicle
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention provides a speed obstacle planning method and system integrating safe distance and global information, wherein the method comprises the following steps: firstly, improving a Theta algorithm, adding a safety distance function in an evaluation function, and ensuring that a planned global route is safe and reliable; and then, advancing a preset path marked by an improved safety Theta path calculation rule, if a dynamic obstacle is encountered, calculating an optimal offset angle for avoiding the obstacle according to a speed obstacle method fused with global information, and planning a local path to continuously avoid the obstacle until a target point is reached. Aiming at the problem of unmanned node obstacle avoidance under a complex environment with static obstacles and dynamic obstacles, the invention improves Theta algorithm, and fuses safe global information in the local planning of a speed obstacle method, thereby planning a safe and reliable route.
Description
Technical Field
The invention belongs to the technical field of path planning algorithms, and particularly relates to a speed obstacle planning method and system integrating safe distance and global information.
Background
With the development of science and technology, more and more unmanned agents are discovered and applied to the fields of exploration, search and rescue, logistics, monitoring, patrol and the like. The path planning of the unmanned intelligent agent is a process of comprehensively analyzing global information and real-time perception information, has important application value for industry and agriculture, can replace dangerous exploration work, and greatly expands the range of operation.
Common path planning algorithms can be divided into global path planning and local path planning, for global path planning, environment information needs to be acquired in advance, and the common global planning algorithms include an a-th algorithm, a configuration space method, a free space method, a grid method, a particle swarm optimization algorithm and the like, and the disadvantage of global path planning is poor instantaneity and difficulty in processing complex path information. The local path planning is to plan by using the environmental information in the acquired range in a global or partially unknown environment, and common local path planning algorithms include a dynamic window method, a manual potential field method, a speed barrier method and the like, wherein the manual potential field method is convenient for real-time control, but is easy to sink into local optimum; the dynamic window method has low calculation complexity and poor dynamic obstacle avoidance effect. Local path planning has good real-time performance, but the prospective performance is poor, global environment information is difficult to consider, and often the planned path is not optimal.
To enable the unmanned agent to safely and reliably complete the task, the obstacle avoidance problem in the road searching needs to be considered. The intelligent body may encounter static or dynamic obstacles, sense surrounding environmental information through radar, range finder and other devices, and then perform corresponding obstacle avoidance operation. A static obstacle refers to an obstacle whose state is stationary or negligible with respect to the speed of movement of the agent, and a dynamic obstacle refers to a dynamic thing.
As unmanned agents become more sophisticated in functionality and more diverse in demands and tasks, new approaches to path planning for agents are also needed to better accommodate these changes and challenges. The path planning and obstacle avoidance method of the unmanned intelligent agent node is researched, so that the method is in compliance with the development trend of the intelligent era, meets the requirements of multiple fields such as military national defense, production and transportation and the like, and has great scientific significance and application value.
Disclosure of Invention
The invention aims to solve the technical problems that: aiming at the problems of global planning and local dynamic obstacle avoidance of unmanned nodes in a complex path, the invention provides a speed obstacle planning method integrating safety distance and global information, and solves the problem that the local obstacle avoidance is difficult to consider the global information and how to keep the safety distance when the obstacle avoidance is performed.
The invention adopts the following technical scheme for solving the technical problems:
the invention provides a speed obstacle planning method integrating safe distance and global information, which comprises the following steps:
s1, converting the pre-acquired real environment map information into a binary image by using an image conversion function, and determining a starting coordinate point and a target coordinate point of the unmanned node.
S2, starting a safety distance Theta algorithm on the binary image, planning an optimal preset route in a global environment, and storing inflection point coordinate information of the lower route.
S3, detecting local obstacles on the map, when dynamic obstacles are found, sorting the distances of the obstacles, merging overlapping ranges of the obstacles, and determining a collision area.
And S4, using the nearest inflection point of the preset path as a sub-target coordinate point, and planning an optimal offset angle by using a fusion algorithm to dynamically avoid the obstacle.
S5, circulating the sub-target coordinate points in the step S4 until the number of the local barriers is 0 or the local barriers reach the end point, indicating that the road finding and obstacle avoiding task is finished, and outputting a final global optimal path.
Further, in step S1, the map with the obstacle is loaded into a binary map, and then converted into a matrix only including 0 and 1, the point with the obstacle is 1, the point without the obstacle is 0, and the start coordinate point start and the target coordinate point goal of the unmanned node are determined.
Further, in step S2, a Theta algorithm is started, and the specific steps for planning the optimal preset route are as follows:
s201, taking the start coordinate point start as a first node to be checked, adding the first node to an OPEN LIST LIST, wherein the OPEN LIST represents a LIST of nodes which are not checked.
S202, the unmanned node starts from a starting coordinate point, seeks paths to eight points around a coordinate system, places passable path nodes into an OPEN LIST, sets the starting coordinate point as parent nodes of the nodes, places obstacle points and the passed path nodes into a CLOSE LIST, and the CLOSE LIST represents the checked infeasible nodes.
S203, taking out the start coordinate point from the OPEN LIST LIST, and putting the start coordinate point into the CLOSE LIST LIST.
S204, calculating a node with the minimum cost in a current OPEN LIST LIST of the unmanned node, defining a valuation function of the unmanned node from a starting coordinate point to a target point through any node n midway as f (n), defining a real distance from the starting coordinate point to any node n as g (n), defining a predicted distance from the node n to a target coordinate point as h (n), defining a safe distance function of the current position as k (n), and adopting a specific calculation formula as follows:
f(n)＝g(n)+h(n)+k(n)
The addition of the safety distance function is to avoid that the searched path is too closely attached to the obstacle, and is a comprehensive consideration of the shortest path and the safety of the path, and the specific formula is as follows:
wherein mu 1 、μ 2 As a weight coefficient, l is the distance between the current node and the nearest barrier, and d is the distance between the current position and the terminal point; the safety distance function ensures that the closer the node is to the obstacle, the greater the cost is, and the closer the node is to the end point, the smaller the cost is.
S205, taking out the node x with the minimum f (n) value from the OPEN LIST LIST as a path node of the next step, and putting the path node x into the CLOSE LIST LIST.
S206, excluding all nodes in the CLOSE LIST, adding feasible nodes around the current node x into the OPEN LIST, calculating f (n), g (n), h (n) and k (n) values of the feasible nodes, and setting the node x as a father node; if the neighboring node y of the node x is already in the OPEN LIST set, calculating a g (n) value of a new path that the unmanned node reaches the node y from the start coordinate point start via the node x, and judging whether the unmanned node needs to update the node according to the g (n) value:
(1) If the g (n) value of the new path from the starting coordinate point start to the node y through the node x is smaller than the original g (n) value of the node y, modifying the parent node of the unmanned node as the node x, and recalculating the f (n) value, wherein h (n) is unchanged.
(2) If the g (n) value of the new path is greater than the original g (n) of node y, g (n) is unchanged.
S207, the steps S204, S205, S206 are repeated cyclically until the target coordinate point gold of the unmanned node appears on the OPEN LIST.
S208, when the number of coordinate points in the OPEN LIST LIST is 0, indicating that a proper path is not found; when the target coordinate point gold of the unmanned node appears in the OPEN LIST, it indicates that the shortest path from the start coordinate point to the target coordinate point gold of the unmanned node is found, each node moves to the start point of the unmanned node along the father node from the target coordinate point gold to form a path of the unmanned node, and the path of the unmanned node is returned to the track coordinate point set path of the unmanned node, namely the planned optimal preset path result.
Further, 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 sequencing from small to large.
S302, calculating the angle of the obstacle target relative to the unmanned node, wherein the specific formula is as follows:
wherein v is ax Is the transverse velocity component of the unmanned node, v ay Is the longitudinal velocity component of the unmanned node, v bx For the lateral velocity component of the obstacle target, v by Is the longitudinal velocity component of the obstacle target.
S303, performing expansion treatment on the barrier, reducing the risk of collision with the wall, and calculating the included angle of the collision area after expansion, wherein the specific formula is as follows:
wherein R is the expansion radius of the obstacle target, and D is the distance between the obstacle target and the unmanned node.
Then calculating the initial included angle of the collision area and the final included angle of the collision area, wherein the specific formula is as follows:
wherein θ e Forming a half angle theta of a collision area for the obstacle target and the unmanned node 1 For the initial angle of the collision zone, θ 2 Is the ending angle of the impact zone.
S304, sorting from small to large according to the initial included angle of the collision area of the obstacle.
S305, merging collision areas of the obstacles; setting the collision area pinch angle of two adjacent obstacle targets in order from small to large to be theta a1 、θ a2 And theta b1 、θ b2 If theta a1 ≤θ b1 ≤θ a2 The collision areas of the two adjacent obstacles are overlapped, the combination is needed, and the initial included angle and the termination included angle of the combined new collision areas are respectively theta a1 And theta b2 The method comprises the steps of carrying out a first treatment on the surface of the Otherwise, no overlapping portion is indicated, and no merging process is required.
Further, the specific content of planning the optimal offset angle in step S4 is as follows:
in order to ensure the optimization of local obstacle avoidance, global preset route information is fused, and the fusion function is as follows:
G(θ)＝k(α·goal+β·path+γ·head(θ))
Wherein gold is a distance function of the unmanned node from the endpoint; path is a distance function from the tail end of the unmanned node track to the global path, and the shorter the distance is, the closer the distance is to the global optimal route; k is a smoothing coefficient; alpha, beta, gamma are weighting coefficients.
The specific formula of the distance function path of the global path is as follows:
wherein x is i 、y i Is the planned local path end point coordinates, x i 、y i The safe Theta algorithm plans the node coordinates of the global path.
The specific formula of the distance function gol of the unmanned node from the endpoint is as follows:
wherein x is i 、y i Is the planned local path end point coordinates, x goal 、y goal Is the endpoint coordinates of the unmanned node.
The specific formula of the error function head of the current steering and deflection end point angle in the local path planned by the unmanned node is as follows:
wherein θ n Is the new steering of unmanned nodes, theta is the offset, theta goal Is an angle biased toward the end point.
New steering θ of unmanned node n The specific formula of (2) is:
wherein θ m1 And theta m2 The initial included angle and the final included angle of the obstacle collision area closest to the unmanned node are respectively.
Solving the fusion function G (theta) to enable the angle with the smallest G (theta) function value to be the optimal offset angle, and obtaining an optimal route according to the optimal offset angle, wherein the coordinates of the unmanned node at the next moment are as follows:
Where newX and newY are the abscissas of the position after the unmanned node is updated, curX and curY are the abscissas of the position before the unmanned node changes steering, and v is the speed of the unmanned node.
The invention further provides a speed obstacle planning system integrating the safety distance and the global information, which comprises the following steps:
and the coordinate point module is used for converting the pre-acquired real environment map information into a binary image and determining a starting coordinate point and a target coordinate point of the unmanned node.
The safety distance Theta algorithm module is used for planning an optimal preset route in a global environment and storing inflection point coordinate information of the lower route.
And the collision area module is used for detecting local obstacles on the map, and when the dynamic obstacles are found, the distance ordering is carried out on the obstacles, the overlapping ranges of the obstacles are combined, and the collision area is determined.
And the offset angle module is used for planning an optimal offset angle by using a fusion algorithm and carrying out dynamic obstacle avoidance by taking the nearest inflection point of the preset path as a sub-target coordinate point.
And the optimal path module is used for circulating the sub-target coordinate points in the offset angle module until the number of the local barriers is 0 or the number of the local barriers reaches the end point, and outputting a final global optimal path.
Further, in the coordinate point module, the map with the obstacle is loaded into a binary map, and then converted into a matrix only comprising 0 and 1, the point with the obstacle is 1, the point without the obstacle is 0, and the starting coordinate point start and the target coordinate point gold of the unmanned node are determined.
Further, in the safe distance Theta algorithm module, the specific steps of planning the optimal preset route are as follows:
and step 1, taking the start coordinate point start as a first node to be checked, adding the first node to an OPEN LIST, wherein the OPEN LIST represents a LIST of nodes which are not checked.
And 2, starting from a starting point coordinate point, the unmanned node searches routes to eight surrounding points, putting passable path nodes into an OPEN LIST, setting the starting point coordinate point as a parent node of the nodes, putting obstacle points and the passed path nodes into a CLOSE LIST, and enabling the CLOSE LIST to represent the checked infeasible nodes.
And step 3, taking out the starting point coordinate point from the OPEN LIST, and putting the starting point coordinate point into the CLOSE LIST.
Step 4, calculating the node with the minimum cost in the current OPEN LIST of the unmanned node, defining a valuation function of the unmanned node reaching the target point from the initial coordinate point through any node n midway as f (n), defining a real distance from the initial coordinate point to any node n as g (n), defining a predicted distance from the node n to the target coordinate point as h (n), defining a safe distance function of the current position as k (n), and adopting the following specific calculation formula:
f(n)＝g(n)+h(n)+k(n)
Wherein mu 1 、μ 2 And l is the distance between the current node and the nearest barrier, and d is the distance between the current position and the terminal point.
And 5, taking the node x with the smallest f (n) value out of the OPEN LIST as a path node of the next step, and putting the path node x into the CLOSE LIST.
Step 6, excluding all nodes in the CLOSE LIST, adding feasible nodes around the current node x into the OPEN LIST, calculating f (n), g (n), h (n) and k (n) values of the feasible nodes, and setting the node x as a father node; if the neighboring node y of the node x is already in the OPEN LIST set, calculating a g (n) value of a new path that the unmanned node reaches the node y from the starting point coordinate point start via the node x, and judging whether the unmanned node needs to update the node according to the g (n) value:
(1) If the g (n) value of the new path from the starting point coordinate point to the node y through the node x is smaller than the original g (n) value of the node y, modifying the parent node of the unmanned node as the node x, and recalculating the f (n) value, wherein h (n) is unchanged.
(2) If the g (n) value of the new path is greater than the original g (n) of node y, g (n) is unchanged.
And 7, circularly repeating the steps 4, 5 and 6 until the target coordinate point gold of the unmanned node appears in the OPEN LIST.
Step 8, when the number of coordinate points in the OPEN LIST is 0, indicating that no suitable path is found; when the target coordinate point gold of the unmanned node appears in the OPEN LIST, it is indicated that the shortest path from the start point coordinate point to the target coordinate point gold of the unmanned node is found, each node moves to the start point of the unmanned node along the father node from the target coordinate point gold, a path of the unmanned node is formed, and the path of the unmanned node is returned to the track coordinate point set path of the unmanned node, namely, the planned optimal preset path result.
Further, in the collision zone module, the specific steps for determining the collision zone are as follows:
and step 1, detecting the distance between the obstacle target and the unmanned node and sequencing from small to large.
wherein v is ax Is the transverse velocity component of the unmanned node, v ay Is the longitudinal velocity component of the unmanned node, v bx For the lateral velocity component of the obstacle target, v by Is the longitudinal velocity component of the obstacle target.
Step 3, performing expansion treatment on the obstacle, and calculating an included angle of the collision area after expansion, wherein the specific formula is as follows:
wherein R is the expansion radius of the obstacle target, and D is the distance between the obstacle target and the unmanned node.
Then calculating the initial included angle of the collision area and the final included angle of the collision area, wherein the specific formula is as follows:
wherein θ e Forming a half angle theta of a collision area for the obstacle target and the unmanned node 1 For the initial angle of the collision zone, θ 2 Is the ending angle of the impact zone.
And 4, sorting from small to large according to the initial angle of the collision area of the obstacle.
Step 5, merging collision areas of the obstacles; setting the collision area pinch angle of two adjacent obstacle targets in order from small to large to be theta a1 、θ a2 And theta b1 、θ b2 If theta a1 ≤θ b1 ≤θ a2 The collision areas of the two adjacent obstacles are overlapped, the combination is needed, and the initial included angle and the termination included angle of the combined new collision areas are respectively theta a1 And theta b2 The method comprises the steps of carrying out a first treatment on the surface of the Otherwise, no overlapping portion is indicated, and no merging process is required.
Further, in the offset angle module, the specific content of the optimal offset angle is planned as follows:
the global preset route information is merged, and the merging function is as follows:
G(θ)＝k(α·goal+β·path+γ·head(θ))
where gold is the distance function of the unmanned node from the endpoint, path is the distance function of the unmanned node trajectory end to the global path, k is the smoothing coefficient, and α, β, γ are the weighting coefficients.
The specific formula of the distance function path of the global path is as follows:
Wherein x is i 、y i Is the planned local path end point coordinates, x i ' 、y i ' The safe Theta algorithm plans the node coordinates of the global path.
The specific formula of the distance function gol of the unmanned node from the endpoint is as follows:
wherein x is i 、y i Is the planned local path end point coordinates, x goal 、y goal Is the endpoint coordinates of the unmanned node.
The specific formula of the error function head of the current steering and deflection end point angle in the local path planned by the unmanned node is as follows:
wherein θ n Is the new steering of unmanned nodes, theta is the offset, theta goal Is an angle biased toward the end point.
New steering θ of unmanned node n The specific formula of (2) is:
wherein θ m1 And theta m2 The initial included angle and the final included angle of the obstacle collision area closest to the unmanned node are respectively.
Solving the fusion function G (theta) to enable the angle with the smallest G (theta) function value to be the optimal offset angle, and obtaining an optimal route according to the optimal offset angle, wherein the coordinates of the next moment are as follows:
where newX and newY are the abscissas of the position after the unmanned node is updated, curX and curY are the abscissas of the position before the unmanned node changes steering, and v is the speed of the unmanned node.
Compared with the prior art, the invention adopts the technical proposal and has the following remarkable technical effects:
Compared with the traditional Theta algorithm, the planned route has higher safety, the route and the obstacle can keep a certain safety distance, and the invention fuses local planning information, has high instantaneity and stronger capability of coping with sudden dynamic obstacles; compared with the traditional local planning method, the fusion evaluation function fully considers global information, calculates the optimal obstacle avoidance offset angle, and finds the global optimal solution, so that the safe dynamic obstacle avoidance can be performed on the optimal route. Experiments show that the unmanned node speed obstacle dynamic obstacle avoidance method integrating the safety distance and the global information has good real-time performance and higher safety of the planned route, and solves the problems that the global planning real-time performance is poor, the global information is difficult to consider by the local obstacle avoidance, and the safety distance is difficult to maintain.
Drawings
Fig. 1 is a flow chart of the present invention.
Fig. 2 is a graph of comparative experiments of different Theta algorithm output safe distance path diagrams.
Fig. 3 is a schematic diagram of a speed barrier of the present invention.
Fig. 4 is a schematic diagram of a fusion algorithm of the present invention.
FIG. 5 is a diagram of a dynamic obstacle avoidance experiment of a single obstacle of the present invention.
FIG. 6 is a diagram of a dynamic obstacle avoidance experiment of multiple obstacles of the present invention.
Fig. 7 is a graph of unmanned node-to-obstacle distances of the present invention.
Detailed Description
In order to enable the skilled person to better understand the method according to the present invention, the following describes in detail the technical solution according to the embodiment of the present invention with reference to the accompanying drawings in the examples of the present invention.
In order to achieve the above objective, the present invention provides a speed obstacle planning method integrating safe distance and global information, as shown in fig. 1, comprising the following steps:
s1, converting the pre-acquired real environment map information into a binary map by utilizing an image conversion function, determining a starting point (3, 5) and a target point (250 ) of an unmanned node, loading a map with obstacles into the binary map, and converting the map into a matrix only comprising 0 and 1, wherein the point with the obstacles is 1, and the point without the obstacles is 0.
S2, starting a safety distance Theta algorithm on the binary graph, planning an optimal preset route under the global environment, and storing inflection point coordinate information of a lower path, wherein (a) in the graph 2 is a path graph output by adopting an original Theta algorithm, and a black curve in the graph shows a track when unmanned nodes in the simulated sea area binary graph do path planning, and the track is obviously close to an obstacle and has great potential safety hazard. Fig. 2 (b) is a safe distance path diagram output by the method of the present invention, in which it can be seen that the path planned by the unmanned node always keeps a certain distance from the obstacle, and the two paths are compared. The result shows that the path planned by the safety distance Theta algorithm is safer and more reliable. The algorithm comprises the following specific steps:
S201, taking the start coordinate point start as a first node to be checked, adding the first node to an OPEN LIST LIST, wherein the OPEN LIST represents a LIST of nodes which are not checked.
S202, the unmanned node starts to seek paths to eight points around the coordinate system from a starting coordinate point start, places passable path nodes into an OPEN LIST, sets the starting coordinate point start as parent nodes of the nodes, places obstacle points with a value of 1 and the passed path nodes into a CLOSE LIST, and the CLOSE LIST represents the checked infeasible nodes.
S203, taking out the start coordinate point from the OPEN LIST LIST, and putting the start coordinate point into the CLOSE LIST LIST.
S204, calculating a node with the minimum cost in a current OPEN LIST LIST of the unmanned node, defining a valuation function of the unmanned node from a starting coordinate point to a target point through any node n midway as f (n), defining a real distance from the starting coordinate point to any node n as g (n), defining a predicted distance from the node n to a target coordinate point as h (n), defining a safe distance function of the current position as k (n), and adopting a specific calculation formula as follows:
f(n)＝g(n)+h(n)+k(n)
the addition of the safety distance function is to avoid that the searched path is too closely attached to the obstacle, and is a comprehensive consideration of the shortest path and the safety of the path, and the specific formula is as follows:
Wherein mu 1 、μ 2 For the weight coefficients, coefficient values of 0.427 and 0.573 are set, respectively. l is the distance between the current node and the nearest obstacle, and d is the distance between the current position and the terminal point. The safety distance function ensures that the closer the node is to the obstacle, the greater the cost is, and the closer the node is to the end point, the smaller the cost is.
S205, taking out the node x with the minimum f (n) value from the OPEN LIST LIST as a path node of the next step, and putting the node x into the CLOSE LIST LIST;
s206, excluding all nodes in the CLOSE LIST, adding feasible nodes around the current node x into the OPEN LIST, calculating f (n), g (n), h (n) and k (n) values of the feasible nodes, and setting the node x as a father node; if the neighboring node y of the node x is already in the OPEN LIST set, calculating a g (n) value of a new path that the unmanned node reaches the node y from the start coordinate point start via the node x, and judging whether the unmanned node needs to update the node according to the g (n) value:
(1) If the g (n) value of the new path from the starting coordinate point start to the node y through the node x is smaller than the original g (n) value of the node y, modifying the parent node of the unmanned node as the node x, and recalculating the f (n) value, wherein h (n) is unchanged.
(2) If the g (n) value of the new path is greater than the original g (n) of node y, g (n) is unchanged.
S207, the steps S204, S205, S206 are repeated cyclically until the target coordinate point gold of the unmanned node appears on the OPEN LIST.
S208, when the number of coordinate points in the OPEN LIST LIST is 0, indicating that a proper path is not found; when the target coordinate point gold of the unmanned node appears in the OPEN LIST, it indicates that the shortest path from the start coordinate point to the target coordinate point gold of the unmanned node has been found, and each node moves from the target coordinate point gold to the start point of the unmanned node along the father node to form a path of the unmanned node, and returns to the coordinate point set path, namely the planned optimal preset path result.
S3, as shown in FIG. 3, is a schematic diagram of the speed obstacle. The intelligent agent detects local obstacles on the map, if the dynamic obstacles are found, when the dynamic obstacles are found, the distance ordering is carried out on the obstacles, the overlapping ranges of the obstacles are combined, and a collision area is determined, wherein the specific steps are as follows:
s301, detecting the distance between the obstacle target and the unmanned node and sequencing from small to large.
S302, calculating the angle of the obstacle target relative to the unmanned node, wherein the specific formula is as follows:
wherein v is ax Is the transverse velocity component of the unmanned node, v ay A longitudinal velocity component that is an unmanned node; v bx For the lateral velocity component of the obstacle target, v by Is the longitudinal velocity component of the obstacle target.
S303, performing expansion treatment on the barrier, reducing the risk of collision with the wall, and calculating the included angle of the collision area after expansion, wherein the specific formula is as follows:
wherein R is the expansion radius of the obstacle target; d is the distance between the obstacle target and the unmanned node.
Then calculating the initial included angle of the collision area and the final included angle of the collision area, wherein the specific formula is as follows:
wherein θ e Forming a half of an included angle of a collision area for the obstacle target and the unmanned node; θ 1 For the initial angle of the collision zone, θ 2 Is the ending angle of the impact zone.
S304, sorting from small to large according to the initial angle of the collision area of the obstacle.
S305, merging collision areas of the obstacles. Assume that the collision zone pinch angle of two adjacent obstacle targets ordered from small to large is θ a1 、θ a2 And theta b1 、θ b2 The method comprises the steps of carrying out a first treatment on the surface of the If theta is a1 ≤θ b1 ≤θ a2 The collision areas of the two adjacent obstacles are overlapped, the combination is needed, and the initial included angle and the termination included angle of the combined new collision areas are respectively theta a1 And theta b2 The method comprises the steps of carrying out a first treatment on the surface of the Otherwise, no overlapping portion is indicated, and no merging process is required.
S4, as shown in FIG. 4, is a schematic diagram of algorithm fusion. The nearest inflection point of a preset path is used as a sub-target coordinate point, and the optimal offset angle is planned by using a fusion algorithm, so that dynamic obstacle avoidance is performed, and the specific steps are as follows:
in order to ensure the optimization of local obstacle avoidance, global preset route information is fused, and the fusion function is as follows:
G(θ)＝k(α·goal+β·path+γ·head(θ))
the gol is a distance function of the unmanned node from the end point, the path is a distance function of the unmanned node track end point to the global path, and the shorter the distance is, the closer the distance is to the global optimal route. k is a smoothing coefficient, set to 1.732, and α, β, γ are weighting coefficients, set to 0.241, 0.457, 0.302, respectively.
The specific formula of the distance function path of the global path is as follows:
wherein x is i 、y i Is the planned local path end point coordinates, x' i 、y′ i The safe Theta algorithm plans the node coordinates of the global path.
The specific formula of the distance function gol of the unmanned node from the endpoint is as follows:
wherein x is i 、y i Is the planned local path end point coordinates, x goal 、y goal Is the endpoint coordinates of the unmanned node.
The specific formula of the error function head of the current steering and deflection end point angle in the local path planned by the unmanned node is as follows:
wherein θ n Is the new steering of unmanned nodes, theta is the offset, theta goal Is an angle biased toward the end point.
New steering θ of unmanned node n The specific formula of (2) is:
wherein θ m1 And theta m2 The initial included angle and the final included angle of the obstacle collision area closest to the unmanned node are respectively.
Solving the fusion function G (theta) to enable the angle with the smallest G (theta) function value to be the optimal offset angle, and obtaining an optimal route according to the optimal offset angle, wherein the coordinates of the unmanned node at the next moment are as follows:
where newX and newY are the abscissas of the position after the unmanned node is updated, curX and curY are the abscissas of the position before the unmanned node changes steering, and v is the speed of the unmanned node.
S5, circulating the sub-target coordinate points searched in the step S4 until the number of the local barriers is 0 or the local barriers reach the end point, and indicating that the road searching and obstacle avoiding task is finished, so that a final global optimal path is obtained.
In order to verify the effectiveness and feasibility of the method of the invention, simulation experiments were performed. The experimental environment is as follows: WIN10, i5-7200U CPU; the compiling environment is as follows: MATLAB R2020a emulates a platform. The parameter information used in the experiments is shown in Table 1, and the experimental results are shown in FIGS. 5-7.
TABLE 1 parameter settings
As shown in FIG. 5, obstacle avoidance simulation is performed on a single dynamic obstacle, the starting point coordinates are set to be (3, 5), and the end point coordinates are set to be (250 ). In fig. 5, the (a) diagram is that the unmanned node advances according to the preset optimal route, and when the dynamic obstacle is detected, the optimal deflection angle is obtained to plan the local route through calculation of the fusion function, and obstacle avoidance is started. The graph (b) in fig. 5 shows that the unmanned node keeps going to the optimal preset route after avoiding the obstacle, and the simulation result shows that the method can accurately perform real-time obstacle avoidance.
As shown in fig. 6, to further simulate a complex obstacle avoidance scenario, three groups of obstacles are provided to respectively perform the forward route of the interfering unmanned node from different directions. The graph (a) in fig. 6 shows that the unmanned nodes avoid three groups of dynamic obstacles in sequence, and the local route is always close to the optimal preset route, so that the method meets the expected result, and the feasibility and effectiveness of the method in a complex scene are also verified. The angle change of the unmanned node during obstacle avoidance is shown in the (b) diagram in fig. 6, and as can be seen from the diagram, the unmanned node has three angle mutations, which respectively correspond to three obstacle avoidance scenes, and in the rest cases, the angle of the unmanned node has almost no change, which proves that the real-time performance of the method is very high under the condition of facing the sudden obstacle avoidance scenes. Fig. 7 shows the change in distance of the unmanned node from the obstacle from when the unmanned node encounters a first obstacle to when it leaves a second obstacle. As can be seen from the figure, the entire distance curve of the unmanned node from approaching to leaving the obstacle is from high to low and then from low to high, but a certain distance remains at the lowest point. This means that the unmanned node always keeps a certain safety distance when avoiding the obstacle. The method is further verified, and the safety is also considered while the real-time obstacle avoidance is ensured.
The embodiment of the invention also provides a speed obstacle planning system integrating the safety distance and the global information, which comprises 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 capable of running on a processor. It should be noted that each module in the above system corresponds to a specific step of the method provided by the embodiment of the present invention, and has a corresponding functional module and beneficial effect of executing the method. Technical details not described in detail in this embodiment may be found in the methods provided in the embodiments of the present invention.
The above embodiments are only for illustrating the technical idea of the present invention, and the protection scope of the present invention is not limited thereto, and any modification made on the basis of the technical scheme according to the technical idea of the present invention falls within the protection scope of the present invention.
Claims (10)
1. The speed obstacle planning method integrating the safe distance and the global information is characterized by comprising the following steps of:
s1, converting the pre-acquired real environment map information into a binary image by utilizing an image conversion function, and determining a starting coordinate point and a target coordinate point of an unmanned node;
S2, starting a safety distance Theta algorithm on the binary diagram, planning an optimal preset route in a global environment, and storing inflection point coordinate information of a lower route;
s3, detecting local obstacles on the map, when dynamic obstacles are found, sorting the distances of the obstacles, merging overlapping ranges of the obstacles, and determining a collision area;
s4, using the nearest inflection point of the preset path as a sub-target coordinate point, planning an optimal offset angle by using a fusion algorithm, and performing dynamic obstacle avoidance;
s5, circulating the sub-target coordinate points in the step S4 until the number of the local barriers is 0 or the local barriers reach the end point, indicating that the road finding and obstacle avoiding task is finished, and outputting a final global optimal path.
2. The speed obstacle planning method according to claim 1, wherein in step S1, a map with an obstacle is loaded into a binary map, and then converted into a matrix only including 0 and 1, the point with the obstacle is 1, the point without the obstacle is 0, and a start coordinate point start and a target coordinate point gold of the unmanned node are determined.
3. The speed obstacle planning method integrating safe distance and global information according to claim 1, wherein the step S2 of starting a safe distance Theta algorithm, the specific steps of planning an optimal preset route are as follows:
S201, taking a start coordinate point start as a first node to be checked, adding the first node to an OPEN LIST LIST, wherein the OPEN LIST represents a node LIST which is not checked;
s202, starting from a starting coordinate point start, an unmanned node searches routes to eight surrounding points, a passable path node is placed in an OPEN LIST, the starting coordinate point start is set as a father node of the nodes, an obstacle point and a passable path node are placed in a CLOSE LIST, and the CLOSE LIST represents an inspected infeasible node;
s203, taking out the start coordinate point from the OPEN LIST LIST, and putting the start coordinate point into the CLOSE LIST LIST;
s204, calculating a node with the minimum cost in a current OPEN LIST LIST of the unmanned node, defining a valuation function of the unmanned node from a starting coordinate point to a target point through any node n midway as f (n), defining a real distance from the starting coordinate point to any node n as g (n), defining a predicted distance from the node n to a target coordinate point as h (n), defining a safe distance function of the current position as k (n), and adopting a specific calculation formula as follows:
f(n)＝g(n)+h(n)+k(n)
wherein mu 1 、μ 2 As a weight coefficient, l is the distance between the current node and the nearest barrier, and d is the distance between the current position and the terminal point;
S205, taking out the node x with the minimum f (n) value from the OPEN LIST LIST as a path node of the next step, and putting the node x into the CLOSE LIST LIST;
s206, excluding all nodes in the CLOSE LIST, adding feasible nodes around the current node x into the OPEN LIST, calculating f (n), g (n), h (n) and k (n) values of the feasible nodes, and setting the node x as a father node; if the neighboring node y of the node x is already in the OPEN LIST set, calculating a g (n) value of a new path that the unmanned node reaches the node y from the start coordinate point start via the node x, and judging whether the unmanned node needs to update the node according to the g (n) value:
(1) If the g (n) value of a new path from the starting coordinate point start to the node y through the node x is smaller than the original g (n) value of the node y, modifying the father node of the unmanned node as the node x, and recalculating the f (n) value, wherein h (n) is unchanged;
(2) If the g (n) value of the new path is larger than the original g (n) of the node y, the g (n) is not changed;
s207, circularly repeating the steps S204, S205 and S206 until the target coordinate point gold of the unmanned node appears in the OPEN LIST LIST;
s208, when the number of coordinate points in the OPEN LIST LIST is 0, indicating that a proper path is not found; when the target coordinate point gold of the unmanned node appears in the OPEN LIST, it indicates that the shortest path from the start coordinate point to the target coordinate point gold of the unmanned node is found, each node moves to the start point of the unmanned node along the father node from the target coordinate point gold to form a path of the unmanned node, and the path of the unmanned node is returned to the track coordinate point set path of the unmanned node, namely the planned optimal preset path result.
4. The speed obstacle planning method integrating safe distance and global information as recited in claim 1, wherein the specific step of determining the collision area in step S3 is as follows:
s301, detecting the distance between an obstacle target and an unmanned node and sequencing from small to large;
s302, calculating the angle of the obstacle target relative to the unmanned node, wherein the specific formula is as follows:
wherein v is ax Is the transverse velocity component of the unmanned node, v ay Is the longitudinal velocity component of the unmanned node, v bx For the lateral velocity component of the obstacle target, v by A longitudinal velocity component that is an obstacle target;
s303, performing expansion treatment on the obstacle, and calculating an included angle of the collision area after expansion, wherein the specific formula is as follows:
wherein R is the expansion radius of the obstacle target, and D is the distance between the obstacle target and the unmanned node;
then calculating the initial included angle of the collision area and the final included angle of the collision area, wherein the specific formula is as follows:
wherein θ e Forming a half angle theta of a collision area for the obstacle target and the unmanned node 1 For the initial angle of the collision zone, θ 2 Is the termination included angle of the collision area;
s304, sorting from small to large according to the initial included angle of the obstacle collision area;
s305, merging collision areas of the obstacles; setting the collision area pinch angle of two adjacent obstacle targets in order from small to large to be theta a1 、θ a2 And theta b1 、θ b2 If theta a1 ≤θ b1 ≤θ a2 The collision areas of the two adjacent obstacles are overlapped, the combination is needed, and the initial included angle and the termination included angle of the combined new collision areas are respectively theta a1 And theta b2 The method comprises the steps of carrying out a first treatment on the surface of the Otherwise, no overlapping portion is indicated, and no merging process is required.
5. The speed obstacle planning method integrating safe distance and global information as set forth in claim 1, wherein the specific contents of planning the optimal offset angle in step S4 are as follows:
the global preset route information is merged, and the merging function is as follows:
G(θ)＝k(α·goal+β·path+γ·head(θ))
the gol is a distance function of the unmanned node from the end point, the path is a distance function of the unmanned node track end point to the global path, k is a smoothing coefficient, and alpha, beta and gamma are weighting coefficients;
the specific formula of the distance function path of the global path is as follows:
wherein x is i 、y i Is the planned local path end point coordinates, x' i 、y′ i The node coordinates of the global path are planned by a safety Theta algorithm;
the specific formula of the distance function gol of the unmanned node from the endpoint is as follows:
wherein x is i 、y i Is the planned local path end point coordinates, x goal 、y goal Is the endpoint coordinates of the unmanned node;
the specific formula of the error function head of the current steering and deflection end point angle in the local path planned by the unmanned node is as follows:
Wherein θ n Is the new steering of unmanned nodes, theta is the offset, theta goal Is the angle of deflection towards the end point;
new steering θ of unmanned node n The specific formula of (2) is:
wherein θ m1 And theta m2 The initial included angle and the final included angle which are closest to the unmanned node and the obstacle collision area respectively;
solving the fusion function G (theta) to enable the angle with the smallest G (theta) function value to be the optimal offset angle, and obtaining an optimal route according to the optimal offset angle, wherein the coordinates of the unmanned node at the next moment are as follows:
where newX and newY are the abscissas of the position after the unmanned node is updated, curX and curY are the abscissas of the position before the unmanned node changes steering, and v is the speed of the unmanned node.
6. A speed obstacle planning system incorporating security distance and global information, comprising:
the coordinate point module is used for converting the pre-acquired real environment map information into a binary image and determining a starting coordinate point and a target coordinate point of the unmanned node;
the safety distance Theta algorithm module is used for planning an optimal preset route in a global environment and storing inflection point coordinate information of the lower route;
the collision area module is used for detecting local obstacles on the map, sorting the distances of the obstacles when the dynamic obstacles are found, merging the overlapping ranges of the obstacles and determining a collision area;
The offset angle module is used for planning an optimal offset angle by using a fusion algorithm and carrying out dynamic obstacle avoidance by taking the nearest inflection point of a preset path as a sub-target coordinate point;
and the optimal path module is used for circulating the sub-target coordinate points in the offset angle module until the number of the local barriers is 0 or the number of the local barriers reaches the end point, and outputting a final global optimal path.
7. The speed obstacle planning system integrating safety distance and global information according to claim 6, wherein in the coordinate point module, a map with an obstacle is loaded into a binary map and converted into a matrix only comprising 0 and 1, the point with the obstacle is 1, the point without the obstacle is 0, and a start coordinate point start and a target coordinate point gold of an unmanned node are determined.
8. The speed obstacle planning system integrating safety distance and global information according to claim 6, wherein the specific steps of planning the optimal preset route in the safety distance Theta algorithm module are as follows:
step 1, taking a start coordinate point as a first node to be checked, adding the first node to an OPEN LIST, wherein the OPEN LIST represents a node LIST which is not checked;
step 2, the unmanned node starts from a starting point coordinate point, routes to eight surrounding points, places passable path nodes into an OPEN LIST, sets the starting point coordinate point as a father node of the nodes, places obstacle points and the passable path nodes into a CLOSE LIST, and the CLOSE LIST represents the checked infeasible nodes;
Step 3, taking out the starting point coordinate point from the OPEN LIST, and putting the starting point coordinate point into the CLOSE LIST;
step 4, calculating the node with the minimum cost in the current OPEN LIST of the unmanned node, defining a valuation function of the unmanned node reaching the target point from the initial coordinate point through any node n midway as f (n), defining a real distance from the initial coordinate point to any node n as g (n), defining a predicted distance from the node n to the target coordinate point as h (n), defining a safe distance function of the current position as k (n), and adopting the following specific calculation formula:
f(n)＝g(n)+h(n)+k(n)
wherein mu 1 、μ 2 As a weight coefficient, l is the distance between the current node and the nearest barrier, and d is the distance between the current position and the terminal point;
step 5, taking out the node x with the minimum f (n) value from the OPEN LIST as a path node of the next step, and putting the node x into the CLOSE LIST;
step 6, excluding all nodes in the CLOSE LIST, adding feasible nodes around the current node x into the OPEN LIST, calculating f (n), g (n), h (n) and k (n) values of the feasible nodes, and setting the node x as a father node; if the neighboring node y of the node x is already in the OPEN LIST set, calculating a g (n) value of a new path that the unmanned node reaches the node y from the starting point coordinate point start via the node x, and judging whether the unmanned node needs to update the node according to the g (n) value:
(1) If the g (n) value of a new path from the starting point coordinate point to the node y through the node x is smaller than the original g (n) value of the node y, modifying the father node of the unmanned node as the node x, and recalculating the f (n) value, wherein h (n) is unchanged;
(2) If the g (n) value of the new path is larger than the original g (n) of the node y, the g (n) is not changed;
step 7, circularly repeating the steps 4, 5 and 6 until the target coordinate point gold of the unmanned node appears in the OPEN LIST;
step 8, when the number of coordinate points in the OPEN LIST is 0, indicating that no suitable path is found; when the target coordinate point gold of the unmanned node appears in the OPEN LIST, it is indicated that the shortest path from the start point coordinate point to the target coordinate point gold of the unmanned node is found, each node moves to the start point of the unmanned node along the father node from the target coordinate point gold, a path of the unmanned node is formed, and the path of the unmanned node is returned to the track coordinate point set path of the unmanned node, namely, the planned optimal preset path result.
9. The speed obstacle planner system fusing safety distance and global information as set forth in claim 6, wherein the collision zone module determines the collision zone by:
Step 1, detecting the distance between an obstacle target and an unmanned node and sequencing from small to large;
step 2, calculating the angle of the obstacle target relative to the unmanned node, wherein the specific formula is as follows:
wherein v is ax Is the transverse velocity component of the unmanned node, v ay Is the longitudinal velocity component of the unmanned node, v bx For the lateral velocity component of the obstacle target, v by A longitudinal velocity component that is an obstacle target;
step 3, performing expansion treatment on the obstacle, and calculating an included angle of the collision area after expansion, wherein the specific formula is as follows:
wherein R is the expansion radius of the obstacle target, and D is the distance between the obstacle target and the unmanned node;
then calculating the initial included angle of the collision area and the final included angle of the collision area, wherein the specific formula is as follows:
wherein θ e Forming a half angle theta of a collision area for the obstacle target and the unmanned node 1 For the initial angle of the collision zone, θ 2 Is the termination included angle of the collision area;
step 4, sorting from small to large according to the initial angle of the collision area of the obstacle;
step 5, merging collision areas of the obstacles; setting the collision area pinch angle of two adjacent obstacle targets in order from small to large to be theta a1 、θ a2 And theta b1 、θ b2 If theta a1 ≤θ b1 ≤θ a2 The collision areas of the two adjacent obstacles are overlapped, the combination is needed, and the initial included angle and the termination included angle of the combined new collision areas are respectively theta a1 And theta b2 The method comprises the steps of carrying out a first treatment on the surface of the Otherwise, no overlapping portion is indicated, and no merging process is required.
10. The speed obstacle planning system integrating safe distance and global information according to claim 6, wherein the offset angle module is configured to plan the optimal offset angle as follows:
the global preset route information is merged, and the merging function is as follows:
G(θ)＝k(α·goal+β·path+γ·head(θ))
the gol is a distance function of the unmanned node from the end point, the path is a distance function of the unmanned node track end point to the global path, k is a smoothing coefficient, and alpha, beta and gamma are weighting coefficients;
the specific formula of the distance function path of the global path is as follows:
wherein x is i 、y i Is the planned local path end point coordinates, x' i 、y′ i The node coordinates of the global path are planned by a safety Theta algorithm;
the specific formula of the distance function gol of the unmanned node from the endpoint is as follows:
wherein x is i 、y i Is the planned local path end point coordinates, x goal 、y goal Is the endpoint coordinates of the unmanned node;
the specific formula of the error function head of the current steering and deflection end point angle in the local path planned by the unmanned node is as follows:
wherein θ n Is the new steering of unmanned nodes, theta is the offset, theta goal Is the angle of deflection towards the end point;
new steering θ of unmanned node n The specific formula of (2) is:
wherein θ m1 And theta m2 The initial included angle and the final included angle which are closest to the unmanned node and the obstacle collision area respectively;
solving the fusion function G (theta) to enable the angle with the smallest G (theta) function value to be the optimal offset angle, and obtaining an optimal route according to the optimal offset angle, wherein the coordinates of the next moment are as follows:
where newX and newY are the abscissas of the position after the unmanned node is updated, curX and curY are the abscissas of the position before the unmanned node changes steering, and v is the speed of the unmanned node.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310167360.XA CN116203956A (en) | 2023-02-27 | 2023-02-27 | Speed obstacle planning method and system integrating safe distance and global information |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310167360.XA CN116203956A (en) | 2023-02-27 | 2023-02-27 | Speed obstacle planning method and system integrating safe distance and global information |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN116203956A true CN116203956A (en) | 2023-06-02 |
Family
ID=86510818
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310167360.XA Pending CN116203956A (en) | 2023-02-27 | 2023-02-27 | Speed obstacle planning method and system integrating safe distance and global information |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116203956A (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117706942A (en) * | 2024-02-05 | 2024-03-15 | 四川大学 | Environment sensing and self-adaptive driving auxiliary electronic control method and system |
-
2023
- 2023-02-27 CN CN202310167360.XA patent/CN116203956A/en active Pending
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117706942A (en) * | 2024-02-05 | 2024-03-15 | 四川大学 | Environment sensing and self-adaptive driving auxiliary electronic control method and system |
| CN117706942B (en) * | 2024-02-05 | 2024-04-26 | 四川大学 | Environment sensing and self-adaptive driving auxiliary electronic control method and system |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US11970161B2 (en) | Apparatus, method and article to facilitate motion planning of an autonomous vehicle in an environment having dynamic objects | |
| Petrich et al. | Map-based long term motion prediction for vehicles in traffic environments | |
| CN107894773A (en) | A kind of air navigation aid of mobile robot, system and relevant apparatus | |
| KR102049962B1 (en) | Sampling based optimal tree planning method and recording medium storing program for executing the same, and computer program stored in recording medium for executing the same | |
| JP2022160538A (en) | Collision detection method, device, electronic apparatus, storage medium, automatic driving vehicle, and computer program | |
| CN111121812B (en) | Path optimization method, electronic device and storage medium | |
| Zhuge et al. | A novel dynamic obstacle avoidance algorithm based on collision time histogram | |
| Ben-Messaoud et al. | Smooth obstacle avoidance path planning for autonomous vehicles | |
| CN111998858B (en) | Unmanned aerial vehicle route planning method based on improved A-algorithm | |
| CN116203956A (en) | Speed obstacle planning method and system integrating safe distance and global information | |
| CN109341698B (en) | Path selection method and device for mobile robot | |
| CN117215317B (en) | Unmanned ship local path planning method, equipment and storage medium | |
| Macharet et al. | Data gathering tour optimization for Dubins' vehicles | |
| CN115562290A (en) | Robot path planning method based on A-star penalty control optimization algorithm | |
| Li et al. | Adaptive sampling-based motion planning with a non-conservatively defensive strategy for autonomous driving | |
| Guo et al. | Safe path planning with gaussian process regulated risk map | |
| Yang et al. | Path planning and collision avoidance methods for distributed multi-robot systems in complex dynamic environments | |
| Hallgarten et al. | Stay on track: A frenet wrapper to overcome off-road trajectories in vehicle motion prediction | |
| CN116448134B (en) | Vehicle path planning method and device based on risk field and uncertain analysis | |
| Gómez-Huélamo et al. | Improving multi-agent motion prediction with heuristic goals and motion refinement | |
| Dang | Autonomous mobile robot path planning based on enhanced A* algorithm integrating with time elastic band | |
| Xu et al. | Hybrid path planning method for USV using bidirectional A* and improved DWA considering the manoeuvrability and COLREGs | |
| Meng et al. | A navigation framework for mobile robots with 3D LiDAR and monocular camera | |
| Jafari et al. | Reactive path planning for emergency steering maneuvers on highway roads | |
| Wang et al. | Dynamic path planning algorithm for autonomous vehicles in cluttered environments |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination |