LU102942B1 - Path planning method based on improved a* algorithm in off-road environment - Google Patents
Path planning method based on improved a* algorithm in off-road environment Download PDFInfo
- Publication number
- LU102942B1 LU102942B1 LU102942A LU102942A LU102942B1 LU 102942 B1 LU102942 B1 LU 102942B1 LU 102942 A LU102942 A LU 102942A LU 102942 A LU102942 A LU 102942A LU 102942 B1 LU102942 B1 LU 102942B1
- Authority
- LU
- Luxembourg
- Prior art keywords
- child node
- road
- model
- threat
- path
- Prior art date
Links
- 238000004422 calculation algorithm Methods 0.000 title claims abstract description 60
- 238000000034 method Methods 0.000 title claims abstract description 33
- 230000007613 environmental effect Effects 0.000 claims abstract description 13
- 238000005457 optimization Methods 0.000 claims abstract description 9
- 239000011159 matrix material Substances 0.000 claims abstract description 6
- 238000011156 evaluation Methods 0.000 claims description 13
- 230000035945 sensitivity Effects 0.000 claims description 6
- 238000004364 calculation method Methods 0.000 claims description 5
- 230000003044 adaptive effect Effects 0.000 claims description 4
- 230000009286 beneficial effect Effects 0.000 description 8
- 238000010586 diagram Methods 0.000 description 6
- 230000002457 bidirectional effect Effects 0.000 description 3
- 238000004088 simulation Methods 0.000 description 3
- 238000005516 engineering process Methods 0.000 description 2
- 230000002068 genetic effect Effects 0.000 description 2
- 230000001066 destructive effect Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000035772 mutation Effects 0.000 description 1
- 239000002245 particle Substances 0.000 description 1
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/0217—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with energy consumption, time reduction or distance reduction criteria
-
- 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
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
-
- 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/0268—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means
- G05D1/0274—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means using mapping information stored in a memory device
-
- 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)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Aviation & Aerospace Engineering (AREA)
- Traffic Control Systems (AREA)
Abstract
The invention provides a path planning method based on an improved A* algorithm in 2off- road environment, including: dividing workspace of a vehicle into thé same grids, storing environmental information by using numerical matrix, and acquiring a final off-road environment model yi by building an obstacle model 5R0, a threat model 5RT and a road model yiR and fusing thèse models; analyzing a positional relationship between a child node and an obstacle, and building a sélection area of thé child node; introducing a direction change penalty rule in thé sélection area and building an évaluation fonction in thé off-road environment 5H by quantifying information of a local area; and achieving path optimization by setting an anti-collision safety distance D. The method quickly and effectively plans a safe, feasible and efficient path in off-road environment; thé number of inflection points is reduced by 4 tunes, thé efficiency is improved by 30%. Dradmg a workspace of a vehicle mto thé same grids Storing enviionmental information by using a numencal inatrix, and building an obstacle model, a threat model and a road model Acquiring a final off-road environment model by fusing thé obstacle model, thé threat model and thé road model Analyzing a positional relationshig between a child node and an obstacle, and building a sélection area of thé child node Introducing a direction chanse penalty rule in thé sélection area and building an évaluation fonction in thé off-road environment by quantifying information of a local area Achieving path optimization by setting an anti-collision safety distance
Description
C72PILU 2022.04.28 PATH PLANNING METHOD BASED ON IMPROVED A* ALGORITHM IN OFF-ROAD ENVIRONMENT oe
[0001] The present invention belongs to the field of path planning technologies, and in particular relates to a path planning method based on an improved A* algorithm in an off-road environment.
[0002] Path planning technologies can be roughly divided into an intelligent search method based on an optimal algorithm, a classic ant colony algorithm, a particle swarm optimization algorithm, a genetic algorithm, a probabilistic road map algorithm, a rapid-exploration random tree algorithm, etc. Geometric model-based path planning methods include the following classic planning algorithms: Dijkstra algorithm, A* algorithm, D* algorithm, Field D* algorithm, etc. The Hybrid A* algorithm has an excellent effect in path planning for an intelligent vehicle in an unstructured road environment. There are also algorithms for local obstacle avoidance, including an artificial potential field method and a dynamic window method. However, traditional path planning algorithms have unsatisfactory performance in complex scenes, e.g., in a complex off-road environment. Therefore, as for path planning in off-road scenes, there is a method for planning a randomly sampled path by collecting terrain information, but the planned path has too many inflection points. There is also a method for path planning in a continuous space by building, using an adaptive mutation genetic algorithm, a model for optimizing search speed and orientation, but this kind of algorithm may easily fall into the problem of local optimum. Another method proposes an improved A* algorithm, which improves the search efficiency by using a window moving method under the comprehensive impact of topographic slope and surface attributes. However, instead of improving the algorithm itself, this method only models the off-road environment, which makes the algorithm inefficient. Somebody else puts forward a probabilistic road map method based on a potential field model, which models the environment by the artificial potential field method and conducts path planning using the probability road map algorithm, and which has the disadvantage that the artificial potential field is not improved. Hence, this kind of method is easy to fall into the problem of local minimum.
[0003] Therefore, it is necessary to propose a path planning method based on an improved A* algorithm in an off-road environment to solve the above problems.
SUMMARY | 1 ee ——————— 1"
C72P1LU 2022.04.28
[0004] In view of this, the objective of the present invention is to provide a path planning method based on an improved A* algorithm in an off-road environment, so as to solve the problems in 4114102942 prior art that the traditional algorithms used for path planning have too many inflection points, may easily fall into the problem of local optimum, and are low in algorithm efficiency.
[0005] In order to fulfill the above objective, the present invention provides the following technical solutions.
[0006] The present invention provides a path planning method based on an improved A* algorithm in an off-road environment, including the following steps:
[0007] Al: dividing a workspace of an intelligent vehicle into grids of the same size, storing environmental information by using a numerical matrix, and acquiring a final off-road environment model R by building an obstacle model Ro, a threat model Rr and a road model Rp and fusing these models: R = Ro + Rr + Nr,
[0008] in which No represents the obstacle model, Ry represents the threat model and Rp represents the road model;
[0009] A2: analyzing a positional relationship between a child node and an obstacle, and building a selection area (i,j) of the child node;
[0010] A3: introducing a direction change penalty rule in a child node area and building an evaluation function in the off-road environment R by quantifying information of a local area: Ji) =R{g(Nparent) +n *Step)+Ro(h(n)), Ro = (1 — Qn) + et, Ry = 2— Qe, DP ifR(ij) <08 n= fence jf) +1 otherwise’
[0011] in which f(n) represents a global evaluation function, g(parent) represents a true cost value of a parent node of a node n, D_P represents direction change penalty, Step represents a moving cost, R(i,j) represents an off-road grid map value of node coordinates (7), h(n) represents an estimated cost of the node n, & represents environmental threat sensitivity, Ane represents a distance between a node and a target point, ds represents a distance between a starting point and the target point, R: and Ro represent adaptive adjustment coefficients, Q, and Q, represent a threat rate and a pass rate, and 7 is an adjustment coefficient; and
[0012] A4: achieving path optimization by setting an anti-collision safety distance D, and ensuring that the safety distance D and a distance L from a threat meet L > D: D = fe FRE) = 08 0 otherwise 2 —————————————
C72P1LU 2022.04.28
[0013] in which D represents the safety distance, Ri, j) represents the off-road grid map value of the node coordinates (i,), and cell, represents a length of a unit grid. LU102942
[0014] Further, the obstacle model in Al is expressed as: Ro = > M ij ie[o,R—1], ; je[o,c—1] 0 otherwise
[0015] in which Ro represents the obstacle model, O represents an obstacle area, (x; i Vi ;) represents coordinate points of an off-road grid model, R and C represent a length and a width of a set map respectively, and M;; represents a numerical value of each grid on the map;
[0016] the threat model Ry is expressed as: Re = > M if i€[0,R—1], je[o,c—1] My; = fr f(x, vu) ST 7 ={,Z-1,...,0}, 0 otherwise
[0017] in which Ry represents the threat model, T represents the threat, Z represents a threat level, r represents a radius of a threat range, R and C represent a length and a width of a set map respectively, and M;; represents a numerical value of each grid on the map; and
[0018] the road model Rp is expressed as: R rR — > M if ie[o,R—1], je[o,c—1] Mi = {x (ay vi) )H x € [0,0.8], | 0 otherwise
[0019] in which Rg represents the road model, H represents an off-road road, k represents a road traffic coefficient, R and C represent a length and a width of a set map respectively, and Mi; represents a numerical value of each grid on the map.
[0020] Further, the selection area of the child node in A2 needs to be built according to the following rules:
[0021] Rule 1: a child node 2, a child node 6, a child node 4, a child node 5 or a child node 13, a child node 9, a child node 14 and a child node 11 are not used as pre-selected points if the child node 4 or a child node 12 has a threat (the grid map value thereof in the off-road environment R>1); 3 - er ———
C72P1LU 2022.04.28
[0022] Rule 2: the child node 2, the child node 13, a child node 15, a child node 1 or the child node 6, the child node 9, a child node 10 and a child node 7 are not used as pre-selected points 102942 a child node 16 or a child node 8 has a threat; and
[0023] Rule 3: no treatment is made if there is no threat.
[0024] Further, designing the evaluation function in the off-road environment R in A3 includes the following steps:
[0025] C1: introducing a direction penalty rule: calculating a Direction 1 from a current node to its parent node and a Direction 2 from the current node to its child node, calculating a direction change D_Change=|Direction 1-Direction 2], setting the direction change penalty D_P*to infinity if D Change>4 , and selecting a corresponding direction change penalty coefficient if D_Change<4;
[0026] C2: determining whether there is an obstacle by quantifying information of the local area, searching for a better path according to the threat rate Q, if there is an obstacle, and narrowing a search range according to the pass rate Q, if there is no obstacle; and
[0027] C3: substituting the acquired direction. change penalty D_P and the threat rate Q, and the pass rate Q; of the local area into the evaluation function for calculation in the off-road environment À.
[0028] Further, ensuring that the safety distance D and fhe distance L from the threat meet L > D in A4 specifically includes the following steps:
[0029] D1: setting S as a starting point, coordinates of S being (xs, Ys), and conducting a forward Floyd algorithm by taking a next path point 1 according to a step length Æ from the starting point S, calculating and comparing the distance L and the safety distance D, and taking a next path point 2 if I > D until there is a path point n that does not meet L > D, then resetting a point n-1 as a starting point, and continuously taking points to repeat the above steps until an end point T is met and repetitions end;
[0030] D2: conducting a reverse Floyd algorithm by setting the end point T as a starting point and traversing path points in D1 in a direction T — S until the starting point S is met and repetitions end:
[0031] D3: if there is an intersection point between a path optimized by the forward Floyd algorithm and a path optimized by the reverse Floyd algorithm, taking the intersection point as an inflection point of the path; and if there is no intersection point, taking the path of which the sum of the number of inflection points and a path length is smaller.
[0032] Further, the threat rate Q. and the pass rate Q, in C2 are calculated by the following formulas: 4 —_—_—_—_——————————————————
C72P1LU 2022.04.28 0, = SD MEL (so A LU102942 0, = nytnp RDS EE L+D
[0033] in which Q, represents the threat rate, Q, represents the pass rate, L, represents the local area, M(i, j) represents a grid map value of the off-road environment, 6, represents environmental sensitivity, which is set to 0.5 in consideration that an intelligent off-road vehicle is capable of passing through a grassland and a dirt road easily, and is flexibly selectable according to a vehicle type and atask, ny, and np represent the number ofrows and the number of columns in which all values are smaller than &; in the off-road grid map respectively, and L and D represent a row and a column in the local area respectively.
[0034] As stated above, the path planning method based on the improved A* algorithm in the off- road environment according to the present invention has the following beneficial effects: a grid method is adopted to build and simulate a real off-road environment scene, and by hierarchical modeling of obstacles, threats and off-road roads in the off-road environment, multi-level maps can be fused to realize real simulation of the off-road environment, which facilitates path planning of an off-road vehicle in the real scene; the direction change penalty is introduced to control the smoothness of the planned path, such that the planned path can be made straighter; and the local area complexity penalty is designed to adaptively control the search space of the algorithm, such thatthe algorithm is made more intelligent and efficient. The improved A* algorithm can quickly plan a safe, straight and efficient optimized path under the comprehensive impact of many factors in the off-road environment and under the performance and task requirements of different vehicles.
[0035] Other advantages, objectives, and features of the present invention will be set forth in the following description, and will be apparent to those skilled in the art to some extent, or those skilled in the art can be taught from the practice of the present invention. The objectives and other advantages of the present invention can be realized and attained by the following description.
[0036] To make the objectives, technical solutions and beneficial effects of the present invention clearer, the following drawings are provided for illustration:
[0037] FIG. 1 is a flow chart according to the present invention; | [0038] FIG. 2 is a detailed diagram of child nodes according to the present invention;
[0039] FIG. 3 is a diagram of child nodes according to the present invention;
[0040] FIG. 4 is a schematic diagram of definition of direction change penalty according to the present invention;
C72P1LU 2022.04.28
[0041] FIG. 5 is a schematic diagram of definition of local area complexity according to the present invention; and LU102942
[0042] FIG. 6 is a schematic diagram of a bidirectional Floyd algorithm according to the present invention.
[0043] Referring to FIGS. 1-6, the present invention provides a path planning method based on an improved A* algorithm in an off-road environment, including the following steps: dividing a workspace of an intelligent vehicle into grids of the same size, storing environmental information by using a numerical matrix, and acquiring a final off-road environment model R by building an obstacle model Ro, à threat model Ry and a road model Ry and fusing these models: R = Ro + Rr + Rpg.
[0044] in which Mo represents the obstacle model, Rp represents the threat model and Rg represents the road model;
[0045] A2: analyzing a positional relationship between a child node and an obstacle, and building a selection area (i,j) of the child node;
[0046] A3: introducing a direction change penalty rule in a child node area and building an evaluation function in the off-road environment NR by quantifying information of a local area: Sri) =R4g(parent) +n *Step)+Ro(h(n)), R, = (1-Qp) +e, R,=2-Qt DP ifR(ij) <08 n= fence j) + 1 otherwise’
[0047] in which f(n) represents a global evaluation function, £(7parent) represents a true cost value of a parent node of a node n, D_P represents direction change penalty, Step represents a moving cost, R(i,j) represents an off-road grid map value of node coordinates (i,j), h(n) represents an estimated cost of the node n, & represents environmental threat sensitivity, dur represents a distance between a node and a target point, dg represents a distance between a starting point and the target point, Rf and Ro represent adaptive adjustment coefficients, Q, and Q, represent a threat rate and a pass rate, and 7 is an adjustment coefficient; and
[0048] A4: achieving path optimization by setting an anti-collision safety distance D, and ensuring that the safety distance D and a distance L from a threat meet L= D: D = pas RG, j) = 08 0 otherwise 6 ’ Em ——
C72P1LU 2022.04.28
[0049] in which D represents the safety distance, R(i, j) represents the off-road grid map value of the node coordinates (i, j), and cell, represents a length of a unit grid. LU102942
[0050] The working principle of the technical solution is described as below: first, an in-depth study is made on the real off-road environment, the workspace of the intelligent vehicle is divided into grids of the same size by a grid method, the environmental information is stored by using the numerical matrix, and the off-road scene is truly simulated by building the obstacle model, the threat model and the off-road model, and fusing the three hierarchical models; then, the positional relationship between the child node and the obstacle is analyzed, the selection area of the child node is built, and the global evaluation function of the A* algorithm is improved by introducing the direction change penalty and environmental feature penalty of the local area into the child node area; and finally, path optimization is achieved by setting the anti-collision safety distance D.
[0051] The technical solution has the following beneficial effects: the grid method is adopted to build and simulate the real off-road environment scene, and by hierarchical modeling of obstacles, threats and off-road roads in the off-road environment, multi-level maps can be fused to realize real simulation of the off-road environment, which facilitates path planning of an off-road vehicle in the real scene; the direction change penalty is introduced to control the smoothness of the planned path, such that the planned path can be made straighter; and the local area complexity penalty is designed to adaptively control the search space of the algorithm, such that the algorithm is made more intelligent and efficient.
[0052] In an embodiment of the present invention, the obstacle model described in A1 is expressed as: %= » My ' ie[0,R-1], je[o,c—1] M; = { if (xj, Yu) € 0 0 otherwise
[0053] in which Mo represents the obstacle model, O represents an obstacle area, (x; i» Vi j) represents coordinate points of an off-road grid model, R and C represent a length and a width of a set map respectively, and M;; represents a numerical value of each grid on the map;
[0054] the threat model Ry is expressed as: Re= ) My ie[o,R—1], je[o,c—1] | My = r yy) © Ty ={7,Z-1...,0}, 7 ——————————————————""—""""""""""—""""—"""————————
C72P1LU 2022.04.28
[0055] in which Rp represents the threat model, T represents the threat, Z represents a threat level, T represents a radius of a threat range, R and C represent a length and a width of a sètJ102942 map respectively, and M;; represents a numerical value of each grid on the map; and
[0056] the road model Ry is expressed as: Nr = > Mi; iefo,R—1], je[o,c—1] mue eo
[0057] in which Rg represents the road model, H represents an off-road road, k represents a road traffic coefficient, R and C represent a length and a width of a set map respectively, and Mi; represents a numerical value of each grid on the map.
. [0058] The working principle of the technical solution is described as below: the obstacle model is an area where the intelligent vehicle fails to pass in the off-road environment, such as buildings, forests and mountains; the threat model refers to existences that may cause destructive damages to the intelligent vehicle in the off-road environment, such as mines and enemy forces; and the road model is an area where the intelligent vehicle can run safely in the off-road environment, and in the off-road environment, the areas where the vehicle can run can be roughly divided into hard roads, dirt roads, grassland, sandy road, etc. According to the provision of Table 1 that the surface attributes and the road traffic coefficients are substituted into the road model Ry for calculation, the workspace of the intelligent vehicle is divided into the grids of the same size by the grid method, the environmental information is stored by using the numerical matrix, and the off-road scene is | truly simulated by building the obstacle model, the threat model and the off-road model and fusing the three models. It is worth noting that the priorities of the three models are: the threat model, the obstacle model and the road model. When the fused models are overlapping, the model with high priority and the grid with a high model data value are preferred. Table 1 Surface Attribute and Road Traffic Coefficient ET a ——— Coefficient k Wheeltype | Caterpillar | ma PTE Er jw] 8 dd
CT2P1LU 2022.04.28
[0059] The technical solution has the following beneficial effects: since the obstacles, threats and off-road roads in the off-road environment are modeled hierarchically, multi-level maps can HeJ102942 fused to realize real simulation of the off-road environment, which facilitates path planning of an off-road vehicle in the real scene.
[0060] In an embodiment of the present invention, the selection area of the child node in A2 needs to be built according to the following rules:
[0061] Rule 1: a child node 2, a child node 6, a child node 4, a child node 5 or a child node 13, a child node 9, a child node 14 and a child node 11 are not used as pre-selected points if the child node 4 or a child node 12 has a threat (the grid map value thereof in the off-road environment R>1);
[0062] Rule 2: the child node 2, the child node 13, a child node 15, a child node 1 or the child node 6, the child node 9, a child node 10 and a child node 7 are not used as pre-selected points if a child node 16 or a child node 8 has a threat; and
[0063] Rule 3: no treatment is made if there is no threat.
[0064] The working principle of the technical solution is described as below: in order to find a path from the starting point to the end point, it is necessary to define a way to select subsequent nodes. Considering that in the complex off-road environment, more free movements of the intelligent vehicle are expected to better avoid dangers, 16-adjacency (see FIG. 2) is selected. The traditional selection of a child node only draws the existence ofthe obstacle in the child node into consideration, but not the positional relationship between the child node and the obstacle, such that the planned path may have a grid vertex obliquely passing through the obstacle, which may lead to a collision. As shown in FIG. 3, which is a distribution diagram of 16 child nodes, the designed selection rule of the child node is described as below: Rulel: the child node 2, the child node 6, the child node 4, the child node 5 or the child node 13, the child node 9, the child node 14 and the child node 11 are not used as pre-selected points if the child node 4 or the child node 12 has a threat (the grid map value thereof in the off-road environment R>1); Rule2: the child node 2, the child node 13, the child node 15, the child node 1 or the child node 6, the child node 9, the child node 10 and the child node 7 are not used as pre-selected points if the child node 16 or the child node 8 has a threat; and Rule3: no treatment is made if there is no threat. I
[0065] The technical solution has the following beneficial effects: since the positional relationship between the child node and the obstacle in the complex off-road environment is considered, the planned path may not have a grid vertex obliquely passing through the obstacle, and thus, a collision can be avoided. The selection rule of the child node is designed to cause the intelligent vehicle to avoid dangers effectively, thereby fulfilling the objective of being safe and efficient. 9 -———————————
C72P1LU 2022.04.28
[0066] In an embodiment of the present invention, designing the evaluation function in the offU102942 road environment R in A3 includes the following steps:
[0067] C1: introducing a direction penalty rule: calculating a Direction 1 from a current node to its parent node and a Direction 2 from the current node to its child node, calculating a direction change D_Change=|Direction 1-Direction 2|, setting the direction change penalty D_P to infinity if D Change>4 , and selecting a corresponding direction change penalty coefficient if D_Change<4:;
[0068] C2: determining whether there is an obstacle by quantifying information of the local area, searching for a better path according to the threat rate Q, if there is an obstacle, and narrowing a search range according to the pass rate Q, if there is no obstacle; and
[0069] C3: substituting the acquired direction change penalty D_P and the threat rate Q, and the pass rate Q, of the local area into the evaluation function for calculation in the off-road environment X.
[0070] The working principle of the technical solution is described as below: the direction change penalty is introduced to reduce useless inflection points of the path. It is stipulated that the turning angle of the intelligent vehicle ranges from 0° to 90°. The rule of the direction change penalty is described as below: step 1, calculating a Direction 1 from a current node to its parent node, the directions being stipulated as in FIG. 4(a); step 2, calculating a Direction 2 from the current node to its child node; and
[0071] step 3, calculating the direction change D_Change=|Direction 1-Direction 2]; setting the direction change penalty D_P to infinity if D_Change>4; and selecting a corresponding direction change penalty coefficient with reference to Table 2 if D _Change<4.
Table 2 Reference Table of Direction Change Penalty Coefficient ;
ES EM qe TT) pe III
[0072] The technical solution has the following beneficial effects: the direction change penalty is introduced to control the smoothness of the planned path, such that the planned path can be made straighter and have less turns and smaller turning angles; and the local area complexity penalty is designed to adaptively control the search space of the algorithm, such that the algorithm is made more intelligent and efficient.
C72P1LU 2022.04.28
[0073] In an embodiment of the present invention, ensuring that the safety distance D and theU102942 distance L from the threat meet L > D in A4 specifically includes the following steps:
[0074] D1: setting S as a starting point, coordinates of S being (xs, Vs), and conducting a forward Floyd algorithm by taking a next path point 1 according to a step length k from the starting point S, calculating and comparing the distance L and the safety distance D, and taking a next path point 2 if L > D until there is a path point n that does not meet L > D, then resetting a point n-1 as a starting point, and continuously taking points to repeat the above steps until an end point T is met and repetitions end;
[0075] D2: conducting a reverse Floyd algorithm by setting the end point T as a starting point and traversing path points in D1 in a direction T > S until the starting point S is met and repetitions end; and
[0076] D3: if there is an intersection point between a path optimized by the forward Floyd algorithm and a path optimized by the reverse Floyd algorithm, taking the intersection point as an inflection point of the path; and if there is no intersection point, taking the path of which the sum of the number of inflection points and a path length is smaller.
[0077] The working principle of the technical solution is described as below: whether the optimized path is safe or not is determined by the relationship between the vertical distance L from a threat point to a connecting line and the set safety distance D. As shown in FIG. 6, if the coordinates of a point a are set to (xg, Ya); the coordinates of point S are set to (xs, Vs), and the coordinates of point n3 are set to (Xps, Yız), the distance L between the point a and a straight line S_n can be calculated; and the optimized path should meet the distance from the threat. The specific steps are as follows: step 1: starting from a starting point S, setting S as the starting point, taking the next path point 1 according to a step length k, calculating and comparing the distance L and the safety distance D, taking the next path point 2 if L = D till there is a path point n that does not meet L > D, then resetting a point n-1 as a starting point, and continuously taking points to repeat the above steps until an end point T is met and repetitions end; step 2: conducting a reverse Floyd algorithm by setting the end point T as the starting point and reversely traversing path points according to step 1 until the starting point S is met and repetitions end; and step 3: if there is an intersection point between a path optimized by a forward Floyd algorithm and a path optimized by the reverse Floyd algorithm, taking the intersection point as an inflection point of the path; and if there is no intersection point, taking the path of which the sum of the number of inflection points and a path length is smaller.
[0078] The technical solution has the following beneficial effects: in the improved Floyd algorithm, bidirectional optimization processing is designed to realize bidirectional smooth 11
C72P1LU 2022.04.28 optimization; the safety distance is designed to ensure that the path is optimized to avoid collision with the obstacle and the threat; and a more optimized path is acquired by ensuring that the distand&102942 from the threat meets L > D, such that the intelligent vehicle is safer in the running process.
[0079] In an embodiment of the present invention, the threat rate Q, and the pass rate Q, in C2 are calculated by the following formulas respectively: 0, = Lupe RENE AS Lier, RED 0, = ny tmp RE L+D
[0080] in which Q, represents the threat rate, Q; represents the pass rate, L, represents the local area, M(i, j) represents a grid map value of the off-road environment, 6, represents environmental sensitivity, which is set to 0.5 in consideration that an intelligent off-road vehicle is capable of passing through a grassland and a dirt road easily, and is flexibly selectable according to a vehicle type and a task, n, and np represent the number of rows and the number of columns in which all values are smaller than 6, in the off-road grid map respectively, and L and D represent a row and a column in the local area respectively.
[0081] The technical solution has the following working principle and beneficial effects: the threat rate Q, and the pass rate Q, are calculated by the formulas, and the search space of the node is adaptively adjusted according to the local area complexity, such that the time complexity of the algorithm is reduced. As shown in FIG. 5(a), if the current node is a node 1 and its parent node is a parent node 1 at this time, the current direction can be 3 by calculation. Referring to FIG. 5(b), the local area in this direction can be acquired. By observing the local area in FIG. 5(a), it is obvious that there are obstacles, threats and grasslands in this area. In this area, it is expected that the algorithm can expand the search scope and find a better path to avoid touching the obstacles and the threats. On the contrary, if there is no obstacle or threat in this area, it is expected that the algorithm can narrow the search range and improve the efficiency. |
[0082] In the end, it should be noted that the preferred embodiments are merely used to illustrate but not limit the technical solutions of the present invention. Although the present invention is described in detail through the preferred embodiments, it should be understood by those skilled in the art that various changes may be made in form and detail without departing from the scope of the present invention as defined by the appended claims.
12 ee ———————
Claims (6)
1. A path planning method based on an improved A* algorithm in an off-road environment, comprising the following steps: Al: dividing a workspace of an intelligent vehicle into grids of the same size, storing environmental information by using a numerical matrix, and acquiring a final off-road environment model R by building an obstacle model Ro, a threat model Ry and a road model Rr and fusing these models: R = Ro + Ry + Rpg, in which R, represents the obstacle model, Ry represents the threat model and Rp represents the road model; A2: analyzing a positional relationship between a child node and an obstacle, and building a selection area (i,j) of the child node; A3: introducing a direction change penalty rule in a child node area and building an evaluation function in the off-road environment R by quantifying information of a local area: Sn) =RAgWparent) +1 *Step)+Ro(h(n)), R, = (1— Qn) + es, Ry = 2— Qe, DP ifR(ij)<08 n= (RC) + 1 otherwise” in which f(n) represents a global evaluation function, g(#paren) represents a true cost value of a parent node of a node n, D_P represents direction change penalty, Step represents a moving cost, R(i, j) represents an off-road grid map value of node coordinates (i,/), h(n) represents an estimated cost of the node n, & represents environmental threat sensitivity, dy; represents a distance between a node and a target point, dg; represents a distance between a starting point and the target point, Rt and Ro represent adaptive adjustment coefficients, Q, and Q represent a threat rate and a pass rate, and 7 is an adjustment coefficient; and A4: achieving path optimization by setting an anti-collision safety distance D, and ensuring that the safety distance D and a distance L from a threat meet L = D: D= a RC) = 0.8 0 otherwise in which D represents the safety distance, RC, j) represents the off-road grid map value of the node coordinates (i, 7s and cell, represents a length of a unit grid. 13 _ =
C72P1LU 2022.04.28
2. The path planning method according to claim 1, wherein the obstacle model in A1 is expressed as: LU102942 Ro = > M ij ie[o,R—1], je[o,c—1] My = fr May v1) € 0 0 otherwise in which My represents the obstacle model, O represents an obstacle area, (x; i Vi j) represents coordinate points of an off-road grid model, R and C represent a length and a width of a set map respectively, and M;; represents a numerical value of each grid on the map; the threat model Ry is expressed as: Rr = > M if ie[o,R—1], je[o,c—1] M;; = fr (es, Vu) ET 7 =,z-1...0}, 0 otherwise in which Rr represents the threat model, T represents the threat, Z represents a threat level, r represents a radius of a threat range, R and C represent a length and a width of a set map respectively, and M;; represents a numerical value of each grid on the map; and the road model Rp is expressed as: R rR = > M ij iefo,r—1], je[o,c—1] M, = fF Bayi) cong) 0 otherwise in which Ry represents the road model, H represents an off-road road, k represents a road traffic coefficient, R and C represent a length and a width of a set map respectively, and M; j represents a numerical value of each grid on the map.
3. The path planning method according to claim 1, wherein the selection area of the child node in A2 needs to be built according to the following rules: rule 1: a child node 2, a child node 6, a child node 4, a child node 5 or a child node 13, a child node 9, a child node 14 and a child node 11 are not used as pre-selected points if the child node 4 or a child node 12 has a threat (the grid map value thereof in the off-road environment R>1); rule 2: the child node 2, the child node 13, a child node 15, a child node 1 or the child node 6, the child node 9, a child node 10 and a child node 7 are not used as pre-selected points if a child node 16 or a child node 8 has a threat; and 14
C72P1LU 2022.04.28 rule 3: no treatment is made if there is no threat.
LU102942
4. The path planning method according to claim 1, wherein designing the evaluation function in the off-road environment M in A3 comprises the following steps: CT: introducing a direction penalty rule: calculating a Direction 1 from a current node to its parent node and a Direction 2 from the current node to its child node, calculating a direction change D_Change=|Direction !-Direction 2[, setting the direction change penalty D P to infinity if D_Change>4, and selecting a corresponding direction change penalty coefficient if D Change<4; C2: determining whether there is an obstacle by quantifying information of the local area, searching for a better path according io the threat rate Q, if there is an obstacle, and narrowing a search range according to the pass rate @, if there is no obstacle; and C3: substituting the acquired direction change penalty D_P and the threat rate Q, and the pass rate Q, of the local area into the evaluation function for calculation in the off-road environment ‘A.
5. The path planning method according to claim 1, wherein ensuring that the safety distance D and the distance L from the threat meet L > D in A4 specifically comprises the following steps: D1: setting S as a starting point, coordinates of S being (xs, ys), and conducting a forward Floyd algorithm by taking a next path point 1 according to a step length Æ from the starting point S, calculating and comparing the distance L and the safety distance D, and taking a next path point 2if L = D until there is a path point n that does not meet L > D, then resetting a point n-1 as a starting point, and continuously taking points to repeat the above steps until an end point T is met and repetitions end; D2: conducting a reverse Floyd algorithm by setting the end point T as a starting point and traversing path points in D1 in a direction T — S until the starting point S is met and repetitions end; and D3: if there is an intersection point between a path optimized by the forward Floyd algorithm and à path optimized by the reverse Floyd algorithm, taking the intersection point as an inflection point of the path; and if there is no intersection point, taking the path of which the sum of the number of inflection points and a path length is smaller.
C72P1LU 2022.04.28
6. The path planning method according to claim 4, wherein the threat rate Q, and the pass 102942 rate Q, in C2 are calculated by the following formulas: 0, = (i, EL, RUN AGEJ)>Ee Eten, RAD 0, = nyo RSS L+D in which Q, represents the threat rate, Q; represents the pass rate, L, represents the local area, R(i,j) represents a grid map value of the off-road environment, 6; represents environmental sensitivity, which is set to 0.5 in consideration that an intelligent off-road vehicle is capable of passing through a grassland and a dirt road easily, and is flexibly selectable according to a vehicle type and a task, n; and np represent the number of rows and the number of columns in which all values are smaller than 6, in the off-road grid map respectively, and L and D represent a row and a column in the local area respectively. 16
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210377702.6A CN114706400B (en) | 2022-04-12 | 2022-04-12 | Path planning method based on improved A-x algorithm in off-road environment |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| LU102942B1 true LU102942B1 (en) | 2022-12-01 |
Family
ID=82172174
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| LU102942A LU102942B1 (en) | 2022-04-12 | 2022-05-06 | Path planning method based on improved a* algorithm in off-road environment |
Country Status (2)
| Country | Link |
|---|---|
| CN (1) | CN114706400B (en) |
| LU (1) | LU102942B1 (en) |
Families Citing this family (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115795697B (en) * | 2023-01-31 | 2023-04-21 | 中国电子科技集团公司第二十八研究所 | Method for generating field maneuvering channel under complex environment based on space grid |
| CN116147653B (en) * | 2023-04-14 | 2023-08-22 | 北京理工大学 | Three-dimensional reference path planning method for unmanned vehicle |
| CN116481557B (en) * | 2023-06-20 | 2023-09-08 | 北京斯年智驾科技有限公司 | Intersection passing path planning method and device, electronic equipment and storage medium |
Family Cites Families (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP5402057B2 (en) * | 2009-02-16 | 2014-01-29 | トヨタ自動車株式会社 | Mobile robot control system, route search method, route search program |
| WO2019139815A1 (en) * | 2018-01-12 | 2019-07-18 | Duke University | Apparatus, method and article to facilitate motion planning of an autonomous vehicle in an environment having dynamic objects |
| JP7479064B2 (en) * | 2019-06-03 | 2024-05-08 | リアルタイム ロボティクス， インコーポレーテッド | Apparatus, methods and articles for facilitating motion planning in environments with dynamic obstacles - Patents.com |
| CN110851948B (en) * | 2019-08-27 | 2020-07-31 | 清华大学 | Driving environment situation assessment method and device under unstructured road condition |
| CN111879328B (en) * | 2020-07-07 | 2021-02-23 | 清华大学 | Intelligent vehicle path planning method based on potential energy field probability map in cross-country environment |
| CN111780777B (en) * | 2020-07-13 | 2022-10-21 | 江苏中科智能制造研究院有限公司 | Unmanned vehicle route planning method based on improved A-star algorithm and deep reinforcement learning |
| CN112325892B (en) * | 2020-10-10 | 2023-08-25 | 南京理工大学 | Class three-dimensional path planning method based on improved A-algorithm |
| CN112947406A (en) * | 2021-01-14 | 2021-06-11 | 华南理工大学 | FLOYD and Astar-based hybrid path planning method |
| CN113126618B (en) * | 2021-03-17 | 2022-03-11 | 中国科学院合肥物质科学研究院 | Unmanned global path planning and re-planning method in cross-country environment |
| CN113156886A (en) * | 2021-04-30 | 2021-07-23 | 南京理工大学 | Intelligent logistics path planning method and system |
-
2022
- 2022-04-12 CN CN202210377702.6A patent/CN114706400B/en active Active
- 2022-05-06 LU LU102942A patent/LU102942B1/en active
Also Published As
| Publication number | Publication date |
|---|---|
| CN114706400B (en) | 2023-04-07 |
| CN114706400A (en) | 2022-07-05 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| LU102942B1 (en) | Path planning method based on improved a* algorithm in off-road environment | |
| CN107168305B (en) | Bezier and VFH-based unmanned vehicle track planning method under intersection scene | |
| CN111610786B (en) | Mobile robot path planning method based on improved RRT algorithm | |
| CN112033413B (en) | Path planning method based on improved A-algorithm combined with environment information | |
| CN111880561B (en) | Unmanned aerial vehicle three-dimensional path planning method based on improved whale algorithm in urban environment | |
| CN110487290B (en) | Unmanned vehicle local path planning method based on variable step size A star search | |
| CN112284393A (en) | Global path planning method and system for intelligent mobile robot | |
| CN113741453B (en) | Path planning method, device, equipment and medium for unstructured environment | |
| CN114877905A (en) | Inform-RRT path planning method for bidirectional dynamic growth | |
| CN115268448A (en) | Multi-robot path planning method based on conflict search and speed obstacle | |
| Qu et al. | A constrained VFH algorithm for motion planning of autonomous vehicles | |
| Van Koevering et al. | Provable probabilistic safety and feasibility-assured control for autonomous vehicles using exponential control barrier functions | |
| Yu et al. | Hierarchical framework integrating rapidly-exploring random tree with deep reinforcement learning for autonomous vehicle | |
| Yu et al. | SOF-RRT*: An improved path planning algorithm using spatial offset sampling | |
| CN116136417B (en) | Unmanned vehicle local path planning method facing off-road environment | |
| Seegmiller et al. | The Maverick planner: An efficient hierarchical planner for autonomous vehicles in unstructured environments | |
| CN115079693A (en) | Unmanned vehicle path planning method based on improved genetic algorithm and B spline fitting | |
| CN115016461A (en) | Robot path planning method based on IA-Star algorithm of dynamic end point strategy | |
| Li et al. | An interaction-aware predictive motion planner for unmanned ground vehicles in dynamic street scenarios | |
| CN110887503B (en) | Moving track simulation method, device, equipment and medium | |
| Qiao et al. | Lane change control of autonomous vehicle with real-time rerouting function | |
| Huang et al. | Robot path planning in narrow passages based on improved PRM method | |
| CN111580563A (en) | Unmanned aerial vehicle autonomous obstacle avoidance flight method based on seed search | |
| Liu et al. | Deep Reinforcement Learning-Based Driving Policy at Intersections Utilizing Lane Graph Networks | |
| Solomon | Development of a real-time hierarchical 3D path planning algorithm for unmanned aerial vehicles |