CN110220528A - A kind of two-way dynamic path planning method of automatic Pilot unmanned vehicle based on A star algorithm - Google Patents
A kind of two-way dynamic path planning method of automatic Pilot unmanned vehicle based on A star algorithm Download PDFInfo
- Publication number
- CN110220528A CN110220528A CN201910494421.7A CN201910494421A CN110220528A CN 110220528 A CN110220528 A CN 110220528A CN 201910494421 A CN201910494421 A CN 201910494421A CN 110220528 A CN110220528 A CN 110220528A
- Authority
- CN
- China
- Prior art keywords
- unmanned vehicle
- point
- starting point
- star algorithm
- target
- 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
Classifications
-
- 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/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
- G01C21/34—Route searching; Route guidance
- G01C21/3446—Details of route searching algorithms, e.g. Dijkstra, A*, arc-flags, using precalculated routes
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The present invention relates to a kind of two-way dynamic path planning method of automatic Pilot unmanned vehicle based on A star algorithm, includes the following steps: to initialize, and is loaded into grating map, sets the moving step length of starting point and aiming spot and unmanned vehicle；Two open lists and two close lists are created, be respectively used to search for from starting point to target point direction using A star algorithm and are searched for from target point to starting point direction using A star algorithm；When there is same node point in two open lists, expression has found the optimal path under current map environment, new starting point is set for new position by mobile unmanned truck position again later, by judging whether new position is that aiming spot terminates unmanned vehicle movement.When unmanned vehicle does not reach target position, cartographic information is updated, repeats pathfinding and moving process until unmanned vehicle reaches target position.The present invention accelerates pathfinding speed, may also adapt in the environment of variation by using A star algorithm respectively to starting point and target point.
Description
Technical field
The present invention relates to pathfinding algorithmic technique field, especially a kind of automatic Pilot unmanned vehicle based on A star algorithm is two-way
Dynamic path planning method.
Background technique
Path Planning Technique be automatic Pilot unmanned vehicle research a key areas, be realize unmanned vehicle autonomous positioning with
One of key technology of navigation, main task be allow unmanned vehicle be capable of quick and stable by there is the environment of barrier, together
When with optimal path reach setting target point.Path planning can be divided into according to whether unmanned vehicle ambient enviroment changes
Static path planning and active path planning.
In the path planning algorithm for unmanned vehicle, A star algorithm is a classical path planning algorithm, by opening
Cartographic information is given when the beginning, traverses surroundings nodes information to look for shortest path, algorithm has good stability, certainly
Dynamic to drive in the actual running environment of unmanned vehicle, path planning body of a map or chart in need of consideration is big, meanwhile, automatic Pilot nobody
In the environment of vehicle, have barrier it is often the case that.But since A star algorithm only starts given cartographic information and pathfinding
When need to traverse all nodes around, this makes it need to be traversed for a large amount of node in big map, cause calculate plan time
Longer, real-time is bad and cannot cope with and newly the problems such as barrier occurs.
Summary of the invention
In view of this, the purpose of the present invention is to propose to a kind of two-way dynamic roads of the automatic Pilot unmanned vehicle based on A star algorithm
Diameter planing method, to overcome traditional A star algorithm that cannot quickly plan outbound path in large scale map and be only used for static state
Deficiency in path planning.
The present invention is realized using following scheme: a kind of two-way dynamic route of automatic Pilot unmanned vehicle based on A star algorithm is advised
The method of drawing, comprising the following steps:
Step S1: initialization: map is imported in the computer of unmanned vehicle, and by map rasterizing；The grating map is M*M, M
Indicate the number of every row or each column grid；
Step S2: starting point, target point and unmanned vehicle moving step length are set on the grating map；The starting point, target
Point occupies a grid respectively；The mobile step-length of the unmanned vehicle is set as a grid number；
Step S3: it is gradually searched using A star algorithm to from starting point to target point and from target point to starting point both direction respectively
Rope；
Step S4: creation open1 list and close1 list are used from starting point to aiming spot to store in step S3
The node of A star search algorithm progress search one by one generation；Open2 list and close2 list are created, for storing in step S3
The node of search one by one generation is carried out using A star search algorithm from target point to initial point position；
Step S5: after each search one by one of step S4, sentenced by the nodal information in traversal open1 list and open2 list
Same node point is shown to be if coordinate value is identical with the presence or absence of the identical node of coordinate value in disconnected two lists.If there is identical
Optimal path has been found in node, expression, carries out step S6；Same node point if it does not exist then shows not find optimal path also,
It then again returns to step S3 and carries out search one by one next time；
Step S6: unmanned vehicle is moved to ground zero, judges whether ground zero is target point；If so, terminating；Otherwise, update ground
Figure information, return step S3.
Further, the particular content of the step S3 are as follows:
It is f=g+ that A star algorithm, which calculates the calculation formula estimated from original state, that is, starting point to dbjective state, that is, target point cost,
h；
Wherein, f indicates the cost estimation from original state to dbjective state；G indicates the cost from original state to NextState；
H indicate NextState to dbjective state shortest path cost；
It is described from starting point to target point direction use A star algorithm when, set the starting point in grating map at this time in A star algorithm
Starting point, the target point in map is set as the target point of A star algorithm at this time；A star algorithm is used from target point to starting point direction
When, set the target point in map at this time to the starting point of A star algorithm, the starting point in map is set as the mesh of A star algorithm at this time
Then punctuate carries out search one by one.
Further, the particular content of the step S6 are as follows: after finding optimal path, unmanned vehicle is according to the mobile step of setting
It is long to start to move along optimal path, ground zero is set by unmanned vehicle current location after the completion of movement, then current by comparison
Point coordinate and whether coordinate of ground point value identical judges whether current location is target position, then indicate if target position without
People's vehicle is successfully moved to target position；Otherwise, cartographic information is updated, is recycled back to this in step 4, is repeated, Zhi Daowu
People's vehicle is moved to target position.
Compared with prior art, the invention has the following beneficial effects:
The present invention is searched for target point direction, by the way that A star algorithm is respectively adopted in starting point and target point from target from the off
Point starts to search for starting point direction, indicates to find shortest path when there is same node point in two open lists.It in this way can be with
Optimal path is found within the shorter time.In addition, by the position of mobile unmanned vehicle, being updated after finding optimal path
Point and map reuse the above method, the method can be made to carry out Dynamic Programming in the environment of variation, and than general
A star algorithm find optimal path speed faster.
Detailed description of the invention
Fig. 1 is the flow chart of the embodiment of the present invention.
Fig. 2 is the bidirectional research schematic diagram of the embodiment of the present invention.
Fig. 3 is the optimal path figure of the embodiment of the present invention.
Specific embodiment
The present invention will be further described with reference to the accompanying drawings and embodiments.
As shown in Figure 1, present embodiments providing a kind of two-way dynamic route rule of the automatic Pilot unmanned vehicle based on A star algorithm
The method of drawing, comprising the following steps:
Step S1: initialization: map is imported in the computer of unmanned vehicle, and by map rasterizing；The grating map is M*M, M
Indicate the number of every row or each column grid；
Step S2: starting point, target point and unmanned vehicle moving step length are set on the grating map；The starting point, target
Point occupies a grid respectively；The mobile step-length of the unmanned vehicle is set as a grid number；
Step S3: it is gradually searched using A star algorithm to from starting point to target point and from target point to starting point both direction respectively
Rope；
Step S4: creation open1 list and close1 list are used from starting point to aiming spot to store in step S3
The node of A star search algorithm progress search one by one generation；Open2 list and close2 list are created, for storing in step S3
The node of search one by one generation is carried out using A star search algorithm from target point to initial point position；The creation list refers to
The tabular array created in program operation process.
Step S5: after each search one by one of step S4, believed by the node in traversal open1 list and open2 list
Breath judges to be shown to be same node point if coordinate value is identical with the presence or absence of the identical node of coordinate value in two lists.If there is
Optimal path has been found in same node point, expression, carries out step S6；Same node point if it does not exist then shows not finding also best
Path then again returns to step S3 and carries out search one by one next time；
Step S6: unmanned vehicle is moved to ground zero, judges whether ground zero is target point；If so, terminating；Otherwise, update ground
Figure information, return step S3.
In the present embodiment, the particular content of the step S3 are as follows:
It is f=g+ that A star algorithm, which calculates the calculation formula estimated from original state, that is, starting point to dbjective state, that is, target point cost,
h；
Wherein, f indicates the cost estimation from original state to dbjective state；G indicates the cost from original state to NextState；
H indicate NextState to dbjective state shortest path cost；
It is described from starting point to target point direction use A star algorithm when, set the starting point in grating map at this time in A star algorithm
Starting point, the target point in map is set as the target point of A star algorithm at this time；A star algorithm is used from target point to starting point direction
When, set the target point in map at this time to the starting point of A star algorithm, the starting point in map is set as the mesh of A star algorithm at this time
Then punctuate carries out search one by one.
In the present embodiment, the particular content of the step S6 are as follows: after finding optimal path, unmanned vehicle is according to the shifting of setting
Dynamic step-length starts to move along optimal path, sets ground zero for unmanned vehicle current location after the completion of movement, then pass through comparison
Current point coordinate and whether coordinate of ground point value is identical judges whether current location is target position, if target position then table
Show that unmanned vehicle is successfully moved to target position；Otherwise, cartographic information is updated, back to this circulation in step 4, is repeated, directly
Target position is moved to unmanned vehicle.
Preferably, in the present embodiment, in step s 5, when finding same node point in open1 and open2 list,
Expression has been successfully found optimal path, can carry out in next step；If not finding same node point in open1 and open2 list,
Then show not find optimal path also, it at this time should return step S3 progress next step search.
As shown in Fig. 2, for the bidirectional research exemplary diagram in the present embodiment.Region of search is evenly dividing as 8* by the present embodiment
8 grid spaces, each barrier occupy a grid, and starting point and target point also occupy a grid respectively.From the off
A star algorithm is used to target point direction, as shown by arrows in figure, in the first step search of search one by one, algorithm finds s1 and is
Best Point uses A star algorithm from target point to starting point direction, as shown by arrows in figure, the first of search one by one at the same time
It is Best Point that algorithm, which finds g1, in step search.Path as shown by arrows in figure is found after repeat search step, is eventually found figure
It is last to be connected expression by open1 the and open2 same node point in showing by s1, s2, open1 and open2 same node point, g2, g1
The optimal path found.
As shown in figure 3, for the optimal path figure found in the case of Fig. 2 starting point and target point and current map.
The foregoing is merely presently preferred embodiments of the present invention, all equivalent changes done according to scope of the present invention patent with
Modification, is all covered by the present invention.
Claims (5)
1. a kind of two-way dynamic path planning method of automatic Pilot unmanned vehicle based on A star algorithm, it is characterised in that: including following
Step:
Step S1: initialization: map is imported in the computer of unmanned vehicle, and by map rasterizing；The grating map is M*M, M
Indicate the number of every row or each column grid；
Step S2: starting point, target point and unmanned vehicle moving step length are set on the grating map；The starting point, target
Point occupies a grid respectively；The mobile step-length of the unmanned vehicle is set as a grid number；
Step S3: it is gradually searched using A star algorithm to from starting point to target point and from target point to starting point both direction respectively
Rope；
Step S4: creation open1 list and close1 list are used from starting point to aiming spot to store in step S3
The node of A star search algorithm progress search one by one generation；Open2 list and close2 list are created, for storing in step S3
The node of search one by one generation is carried out using A star search algorithm from target point to initial point position；
Step S5: after each search one by one of step S4, sentenced by the nodal information in traversal open1 list and open2 list
Same node point is shown to be if coordinate value is identical with the presence or absence of the identical node of coordinate value in disconnected two lists；If there is identical
Optimal path has been found in node, expression, carries out step S6；Same node point if it does not exist then shows not find optimal path also,
It then again returns to step S3 and carries out search one by one next time；
Step S6: unmanned vehicle is moved to ground zero, judges whether ground zero is target point；If so, terminating；Otherwise, update ground
Figure information, return step S3.
2. a kind of two-way dynamic path planning method of automatic Pilot unmanned vehicle based on A star algorithm according to claim 1,
It is characterized by: grating map described in step S1 is M*M, M indicates the number of every row or each column grid.
3. a kind of two-way dynamic path planning method of automatic Pilot unmanned vehicle based on A star algorithm according to claim 1,
It is characterized by: the step-length of the movement of unmanned vehicle described in step S2 is set as a grid number.
4. a kind of two-way dynamic path planning method of automatic Pilot unmanned vehicle based on A star algorithm according to claim 1,
It is characterized by: the particular content of the step S3 are as follows:
It is f=g+ that A star algorithm, which calculates the calculation formula estimated from original state, that is, starting point to dbjective state, that is, target point cost,
h；
Wherein, f indicates the cost estimation from original state to dbjective state；G indicates the cost from original state to NextState；
H indicate NextState to dbjective state shortest path cost；
It is described from starting point to target point direction use A star algorithm when, set the starting point in grating map at this time in A star algorithm
Starting point, the target point in map is set as the target point of A star algorithm at this time；A star algorithm is used from target point to starting point direction
When, set the target point in map at this time to the starting point of A star algorithm, the starting point in map is set as the mesh of A star algorithm at this time
Then punctuate carries out search one by one.
5. a kind of two-way dynamic path planning method of automatic Pilot unmanned vehicle based on A star algorithm according to claim 1,
It is characterized by: the particular content of the step S6 are as follows: after finding optimal path, unmanned vehicle starts according to the moving step length of setting
It is moved along optimal path, sets ground zero for unmanned vehicle current location after the completion of movement, then pass through comparison current point coordinate
With whether coordinate of ground point value identical judges whether current location is target position, if target position then indicated unmanned vehicle
Through being successfully moved to target position；Otherwise, cartographic information is updated, back to this circulation in step 4, is repeated, until unmanned vehicle moves
It moves to target position.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910494421.7A CN110220528A (en) | 2019-06-10 | 2019-06-10 | A kind of two-way dynamic path planning method of automatic Pilot unmanned vehicle based on A star algorithm |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910494421.7A CN110220528A (en) | 2019-06-10 | 2019-06-10 | A kind of two-way dynamic path planning method of automatic Pilot unmanned vehicle based on A star algorithm |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN110220528A true CN110220528A (en) | 2019-09-10 |
Family
ID=67815939
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201910494421.7A Pending CN110220528A (en) | 2019-06-10 | 2019-06-10 | A kind of two-way dynamic path planning method of automatic Pilot unmanned vehicle based on A star algorithm |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN110220528A (en) |
Cited By (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110549339A (en) * | 2019-09-11 | 2019-12-10 | 上海软中信息***咨询有限公司 | navigation method, navigation device, navigation robot and storage medium |
| CN110717003A (en) * | 2019-09-27 | 2020-01-21 | 四川长虹电器股份有限公司 | Intelligent shopping cart autonomous navigation and automatic following method based on path planning |
| CN111176286A (en) * | 2020-01-06 | 2020-05-19 | 重庆邮电大学 | Mobile robot path planning method and system based on improved D-lite algorithm |
| CN111243315A (en) * | 2020-01-16 | 2020-06-05 | 河北科技大学 | Vehicle positioning control system and control method |
| CN111597210A (en) * | 2020-05-07 | 2020-08-28 | 江苏海平面数据科技有限公司 | Road network track bidirectional query method based on dynamic pre-storage |
| CN111897341A (en) * | 2020-08-05 | 2020-11-06 | 三一专用汽车有限责任公司 | Parking path planning method, parking path planning device and computer-readable storage medium |
| CN112013866A (en) * | 2020-08-31 | 2020-12-01 | 电子科技大学 | Path planning method based on intelligent navigation system |
| CN112918487A (en) * | 2021-02-24 | 2021-06-08 | 京东鲲鹏(江苏)科技有限公司 | Unmanned vehicle starting method and device, electronic equipment and computer readable medium |
| CN114199270A (en) * | 2021-12-14 | 2022-03-18 | 安徽理工大学 | Robot path planning method integrating bidirectional search mechanism and improved A-algorithm |
| WO2023155371A1 (en) * | 2022-02-21 | 2023-08-24 | 上海机器人产业技术研究院有限公司 | Stable movement global path planning method for indoor mobile robot |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101971181A (en) * | 2008-06-24 | 2011-02-09 | 电子地图北美公司 | Methods and systems for dynamically adaptive road network hierarchy and routing |
| CN107687859A (en) * | 2017-09-06 | 2018-02-13 | 电子科技大学 | Most short method for searching based on A star algorithms |
| CN107702716A (en) * | 2017-08-31 | 2018-02-16 | 广州小鹏汽车科技有限公司 | A kind of unmanned paths planning method, system and device |
| CN108444488A (en) * | 2018-02-05 | 2018-08-24 | 天津大学 | Based on etc. steps sample A* algorithms unmanned local paths planning method |
| CN109618401A (en) * | 2019-01-29 | 2019-04-12 | 吉林工程技术师范学院 | Body area network Energy-aware adaptive optimization method based on two-way simultaneous A star algorithm |
-
2019
- 2019-06-10 CN CN201910494421.7A patent/CN110220528A/en active Pending
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101971181A (en) * | 2008-06-24 | 2011-02-09 | 电子地图北美公司 | Methods and systems for dynamically adaptive road network hierarchy and routing |
| CN107702716A (en) * | 2017-08-31 | 2018-02-16 | 广州小鹏汽车科技有限公司 | A kind of unmanned paths planning method, system and device |
| CN107687859A (en) * | 2017-09-06 | 2018-02-13 | 电子科技大学 | Most short method for searching based on A star algorithms |
| CN108444488A (en) * | 2018-02-05 | 2018-08-24 | 天津大学 | Based on etc. steps sample A* algorithms unmanned local paths planning method |
| CN109618401A (en) * | 2019-01-29 | 2019-04-12 | 吉林工程技术师范学院 | Body area network Energy-aware adaptive optimization method based on two-way simultaneous A star algorithm |
Non-Patent Citations (2)
| Title |
|---|
| JZ_TERRY: "双向A*算法浅析", 《HTTPS://BLOG.CSDN.NET/JZ_TERRY/ARTICLE/DETAILS/77487905》 * |
| 林娜 等: "基于双向A*算法的城市无人机航路规划", 《沈阳航空航天大学学报》 * |
Cited By (12)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110549339A (en) * | 2019-09-11 | 2019-12-10 | 上海软中信息***咨询有限公司 | navigation method, navigation device, navigation robot and storage medium |
| CN110717003A (en) * | 2019-09-27 | 2020-01-21 | 四川长虹电器股份有限公司 | Intelligent shopping cart autonomous navigation and automatic following method based on path planning |
| CN111176286A (en) * | 2020-01-06 | 2020-05-19 | 重庆邮电大学 | Mobile robot path planning method and system based on improved D-lite algorithm |
| CN111243315A (en) * | 2020-01-16 | 2020-06-05 | 河北科技大学 | Vehicle positioning control system and control method |
| CN111597210A (en) * | 2020-05-07 | 2020-08-28 | 江苏海平面数据科技有限公司 | Road network track bidirectional query method based on dynamic pre-storage |
| CN111897341A (en) * | 2020-08-05 | 2020-11-06 | 三一专用汽车有限责任公司 | Parking path planning method, parking path planning device and computer-readable storage medium |
| CN112013866A (en) * | 2020-08-31 | 2020-12-01 | 电子科技大学 | Path planning method based on intelligent navigation system |
| CN112918487A (en) * | 2021-02-24 | 2021-06-08 | 京东鲲鹏(江苏)科技有限公司 | Unmanned vehicle starting method and device, electronic equipment and computer readable medium |
| CN112918487B (en) * | 2021-02-24 | 2022-04-12 | 京东鲲鹏(江苏)科技有限公司 | Unmanned vehicle starting method and device, electronic equipment and computer readable medium |
| WO2022179328A1 (en) * | 2021-02-24 | 2022-09-01 | 京东鲲鹏(江苏)科技有限公司 | Method and apparatus for starting unmanned vehicle, electronic device, and computer-readable medium |
| CN114199270A (en) * | 2021-12-14 | 2022-03-18 | 安徽理工大学 | Robot path planning method integrating bidirectional search mechanism and improved A-algorithm |
| WO2023155371A1 (en) * | 2022-02-21 | 2023-08-24 | 上海机器人产业技术研究院有限公司 | Stable movement global path planning method for indoor mobile robot |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110220528A (en) | A kind of two-way dynamic path planning method of automatic Pilot unmanned vehicle based on A star algorithm | |
| WO2023155371A1 (en) | Stable movement global path planning method for indoor mobile robot | |
| CN111811514B (en) | Path planning method based on regular hexagon grid jump point search algorithm | |
| CN109059924B (en) | Accompanying robot incremental path planning method and system based on A-x algorithm | |
| Van Den Berg et al. | Anytime path planning and replanning in dynamic environments | |
| CN110006430B (en) | Optimization method of track planning algorithm | |
| CN112985408B (en) | Path planning optimization method and system | |
| US11747826B2 (en) | Method for route optimization based on dynamic window and redundant node filtering | |
| CN110231824B (en) | Intelligent agent path planning method based on straight line deviation method | |
| CN106774347A (en) | Robot path planning method, device and robot under indoor dynamic environment | |
| CN108444490B (en) | Robot path planning method based on depth fusion of visible view and A-x algorithm | |
| CN110006429A (en) | A kind of unmanned boat path planning method based on depth optimization | |
| CN106774425B (en) | A kind of method and system of unmanned plane during flying navigation | |
| CN109828579B (en) | Mobile robot path planning method for target incremental movement | |
| CN114510057A (en) | ROS-based mobile robot autonomous navigation method in indoor environment | |
| CN112612266B (en) | Unstructured road global path planning method and system | |
| CN115167398A (en) | Unmanned ship path planning method based on improved A star algorithm | |
| CN115167474A (en) | Mobile robot path planning optimization method | |
| Seder et al. | Hierarchical path planning of mobile robots in complex indoor environments | |
| CN116414139B (en) | Mobile robot complex path planning method based on A-Star algorithm | |
| CN117249842A (en) | Unmanned vehicle mixed track planning method based on track smooth optimization | |
| CN116465425A (en) | Heuristic path planning method for local optimization and bidirectional calculation | |
| CN114859892A (en) | Memory obstacle storage method based on cost map | |
| CN106767838A (en) | A kind of service robot air navigation aid and system based on virtual networks | |
| CN114564048A (en) | Improved method for planning flight path of agricultural four-rotor unmanned aerial vehicle |
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 | ||
| RJ01 | Rejection of invention patent application after publication | ||
| RJ01 | Rejection of invention patent application after publication |
Application publication date: 20190910 |