CN115562290A - Robot path planning method based on A-star penalty control optimization algorithm - Google Patents
Robot path planning method based on A-star penalty control optimization algorithm Download PDFInfo
- Publication number
- CN115562290A CN115562290A CN202211285517.0A CN202211285517A CN115562290A CN 115562290 A CN115562290 A CN 115562290A CN 202211285517 A CN202211285517 A CN 202211285517A CN 115562290 A CN115562290 A CN 115562290A
- Authority
- CN
- China
- Prior art keywords
- node
- current node
- parent
- path
- neighborhood
- 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 38
- 238000004422 calculation algorithm Methods 0.000 title claims abstract description 33
- 238000005457 optimization Methods 0.000 title claims abstract description 12
- 239000013598 vector Substances 0.000 claims description 12
- 230000004888 barrier function Effects 0.000 claims description 4
- 238000005259 measurement Methods 0.000 claims description 3
- 230000006870 function Effects 0.000 description 25
- 238000004590 computer program Methods 0.000 description 5
- 238000003860 storage Methods 0.000 description 4
- 238000004364 calculation method Methods 0.000 description 3
- 238000005516 engineering process Methods 0.000 description 3
- 238000009499 grossing Methods 0.000 description 3
- 238000000342 Monte Carlo simulation Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 2
- 230000007547 defect Effects 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 230000008447 perception Effects 0.000 description 2
- 239000000523 sample Substances 0.000 description 2
- 238000004088 simulation Methods 0.000 description 2
- 238000013473 artificial intelligence Methods 0.000 description 1
- 238000013528 artificial neural network Methods 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000007781 pre-processing Methods 0.000 description 1
- 238000010845 search algorithm Methods 0.000 description 1
- 238000004904 shortening 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/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
- G05D1/0246—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means
-
- 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/0257—Control of position or course in two dimensions specially adapted to land vehicles using a radar
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- Aviation & Aerospace Engineering (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Multimedia (AREA)
- Electromagnetism (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention provides a robot path planning method based on an A-star punishment control optimization algorithm. In addition, the penalty algorithm is introduced to optimize the cost function, so that the cost estimation of the cost function from the current node to the target node is more accurate, the inspiration is more purposeful, the number of the search nodes is reduced, and the path smoothness is improved.
Description
Technical Field
The invention relates to the technical field of robot path planning, in particular to a robot path planning method based on an A star penalty control optimization algorithm.
Background
With the development of modern manufacturing technology, the artificial intelligence technology is more widely applied to robots, and the robots become a complex integrating environment perception, path planning and motion control. The path planning is used as a key technology in the field of mobile robots, aims to find out a collision-free optimal path connecting a starting point and a terminal point, and is a key point for realizing autonomous navigation of an intelligent robot in a complex environment. Representative solutions to the robot path planning problem include an a-star algorithm, a D-star algorithm, a fast-expansion random tree method, an artificial potential field method, a neural network method, and the like.
The A star algorithm is used as a path planning method which is most widely applied at present, and has the advantages of high calculation efficiency, short planned path length and the like. The A star algorithm is a heuristic search algorithm, namely, a target end point is heuristically found, and the control cost is minimum, and the most appropriate shortest path to the target point is found on the basis. However, when the traditional a-star algorithm faces a relatively complex obstacle, a planned path has many inflection points, smoothness of the path is greatly reduced, and many useless nodes are searched, so that operation efficiency is reduced.
Many scholars at home and abroad improve the traditional A star algorithm, for example, the Chinese patent application No. 202010182339.3 with the patent name of 'a way-finding method based on A star optimization algorithm' carries out optimization of the algorithm by preprocessing barriers, reduces the searching time of nodes and the calculation memory, but does not deeply explore the problem of unsmooth path in complex environment; the Chinese patent application number is 201711374451.1, and the patent name is 'a mobile robot path planning method based on an improved A star algorithm', the curve is smoother by performing secondary smoothing processing on a planning result, but a large amount of search is still required to be performed around an obstacle.
Disclosure of Invention
The invention aims to: the invention aims to solve the technical problem of providing a robot path planning method based on an A star punishment control optimization algorithm aiming at the defects of the prior art, and aims to overcome the defects of the traditional A star algorithm so as to reduce the number of inflection points of a planned path, improve the smoothness of the path and reduce the number of exploration nodes, thereby shortening the search time.
The method comprises the following steps:
step 1, collecting map information of surrounding environment by using a sensor to establish a map of the environment where the robot is located;
step 2, rasterizing the map, determining the state of each grid as occupied or idle, and determining the position coordinates of the obstacles;
step 3, determining the coordinates (x) of the starting point of the path start ,y start ,z start ) And target point coordinates (x) goal ,y goal ,z goal )；x start 、y start 、z start Coordinate values in the directions of the x, y and z axes which are respectively the starting points; x is the number of goal 、y goal 、z goal Coordinate values in the directions of the x, y and z axes of the target point;
step 4, initializing an open list and a close list, and enabling a path starting point coordinate (x) start ,y start ,z start ) Putting the open list into the close list, and enabling the close list to be empty;
step 6, ignoring the barrier node, if the close list is not empty, also ignoring the node in the close list, and expanding the current node to obtain a neighborhood node of the current node;
step 7, judging whether the current node neighborhood node is in the open list:
if the current node neighborhood node is not in the open list, expanding the neighborhood nodes of the current node, adding the neighborhood nodes into the open list, taking the current node as a father node of the neighborhood nodes, and calculating a cost function of the neighborhood nodes of the current node, namely executing the step 8;
if the current node neighborhood node is in the open list, if the current node is taken as a father node and g (n) of the neighborhood node of the current node is lower than the original g (n), taking the current node as the father node and recalculating the cost function of the neighborhood node of the current node, namely executing the step 8, otherwise executing the step 9;
step 8, calculating a cost function f (n) of the neighborhood nodes, wherein n represents the nth node;
step 9, selecting a point with the minimum value of the cost function f (n) in the open list as a current node to be expanded, deleting the current node from the open list, and adding the current node to the close list close;
step 10, judging whether the target point is added into the open list:
if the target point is added into the open list, path planning is completed, and the target point is reversely solved to the starting point, namely the final path obtained by the path planning of the robot at this time;
if the destination point is not in the open list, step 5 is executed.
The sensor in the step 1 comprises a millimeter wave radar, a laser radar, a camera, an inertial measurement unit IMU and a global positioning system GPS.
In step 6, the types of the robots adopt different expansion methods to expand the neighborhood of the current node, and the method specifically comprises the following steps:
step 6-1, when the type of the robot is an unmanned automobile, firstly calculating the minimum turning radius R under the current speed min Then pass through
Calculating the steering angle delta corresponding to the minimum turning radius under the current vehicle speed, and aligning the steering angle delta to the delta and the delta in the current driving direction]The angle interval is evenly divided into i delta intervals,
delta is usually one degree, and the current node neighborhood is uniformly expanded;
6-2, when the type of the robot is an AGV trolley, uniformly performing neighborhood expansion on the current node in a two-dimensional plane 360-degree direction;
and 6-3, when the type of the robot is an unmanned aerial vehicle, performing neighborhood expansion on the current node in a three-dimensional space.
The calculating step of calculating the cost function f (n) in step 8 includes:
step 8-1, calculating an actual cost function g (n), wherein the actual cost function g (n) represents the actual moving cost value from a path starting point to the current node and is generated along the path from the path starting point to the current node;
g(n)＝g(n-1)+g'(n)
g (n-1) is the actual cost from the starting node to the parent node of the current node, and g' (n) is the cost from the parent node of the previous node to the current node.
Step 8-2, calculating a heuristic function h (n), and using Euclidean distance to represent the estimated cost value from the current node to the path target point, (x) n ,y n ,z n ) For the coordinates of the current node on the grid map, the euclidean distance represents the straight-line distance between two points, and the formula is as follows:
step 8-3, a penalty function ξ (n) is calculated.
Step 8-3 comprises:
step 8-3-1, two vectors n are introduced 1 ,n 2 ：
n 1 ＝(x n-1 -x parent ,y n-1 -y parent ,z n-1 -z parent ),n 2 ＝(x parent -x n ,y parent -y n ,z parent -z n )
Wherein (x) n ,y n ,z n ) (x) coordinates of the current node on the grid map parent ,y parent ,z parent ) (x) coordinates of parent node of current node in grid map n-1 ,y n-1 ,z n-1 ) In a grid map for a parent node of a parent nodeThe coordinates of (a); n is 1 Representing a vector pointing from the parent node of the parent node to the parent node of the current node, n 2 Representing a vector pointing from a parent node to a current node;
step 8-3-2, calculating two vectors n 1 ,n 2 Cosine value of angle θ therebetween:
and 8-3-3, calculating the value of a penalty function xi (n) according to the cosine value result:
a 1 、a 2 adjusting a threshold value, debugging according to different grid maps, and selecting a value suitable for the grid map according to the size and the precision of the grid map;
when cos theta =1, the current node, the father node and the father node of the father node are on the same straight line, the planned path is not turned, no punishment is carried out, and xi (n) =0;
when cos θ ≠ 1, and x n ＝x goal ,y n ＝y goal ,z n ＝z goal The method includes the steps that a current node is a target node, a path from a parent node of the current node to the target node turns relative to a planned path, namely an inflection point turns towards an end point, and then a penalty value is xi (n) = a 1 ；
When-1 is not more than cos theta<1, the current node is not the target point, which indicates that a turn occurs in the path planned by the target point, namely a common inflection point occurs, and the penalty value is ξ (n) = a 2 Wherein a is 2 >a 1 。
The invention has the beneficial effects that:
the robot path planning method based on the A-star penalty control optimization algorithm improves the problems that a path planned by a traditional A-star algorithm faces complex obstacles has a plurality of inflection points, low path smoothness and a large number of useless nodes, improves the smoothness of the planned path, and effectively reduces the number of search nodes and search time.
The current node neighborhood expansion method adopted by the invention considers the difference among different kinds of robots, and differentially expands the current node neighborhood according to the kinematics and dynamics characteristics of the different kinds of robots, thereby reducing the search of useless nodes and improving the universality of the algorithm. In addition, the penalty algorithm is introduced to optimize the cost function, so that the cost estimation of the cost function from the current node to the target node is more accurate, the inspiration is more purposeful, the number of the search nodes is reduced, and the path smoothness is improved.
In conclusion, the method provided by the invention has strong practicability and is beneficial to the development of the fields of robot motion planning, automatic driving and the like.
Drawings
The foregoing and/or other advantages of the invention will become more apparent from the following detailed description of the invention when taken in conjunction with the accompanying drawings.
FIG. 1 is a flow chart of the method of the present invention.
FIG. 2 is a flow chart of a penalty control algorithm used in the method of the present invention.
Fig. 3 is a diagram of a simulation operation result of an Rviz software in an Ubuntu environment by the robot path planning method of the a-star penalty control optimization algorithm provided by the present invention, wherein a white square pointed at 1 is an obstacle, 2 is a path planning starting point generated by using a monte carlo method, 3 is a coordinate system, 4 is a path planning end point generated by using the monte carlo method, 5 is a path planned by a conventional a-star algorithm, and 6 is a path planned by the a-star algorithm of the present invention.
Fig. 4 is a data comparison table of time, path length, and number of probe nodes required to reach the same target point planning path when a penalty function is introduced and when no penalty function is introduced.
Detailed Description
The method introduces penalty function control on the basis of the traditional A star algorithm, judges whether a certain node turns or not when calculating the cost of the node, and gives a certain penalty value if the certain node turns, thereby reducing the number of the turning points of a planned path, achieving the purposes of smoothing the path and reducing the number of probe nodes, and mainly comprising the following steps as shown in figure 1:
step 1, collecting information required by path planning by using sensors such as a millimeter wave radar, a laser radar, a camera, an IMU (Inertial Measurement Unit), a GPS (Global Positioning System) and the like, and drawing a map of the environment where the robot is located;
step 2, rasterizing the drawn map, determining the state of each grid as occupied or idle, and determining the position coordinates of corresponding obstacles;
step 3, determining the coordinates (x) of the starting point of the path start ,y start ,z start ) And target point coordinates (x) goal ,y goal ,z goal )；
Step 4, initializing an open list and a close list, and setting the coordinates (x) of the starting point of the path start ,y start ,z start ) Putting the open list into the close list, and enabling the close list to be empty;
step 6, ignoring nodes in the barrier node and the close list, and expanding the current node to obtain a current node neighborhood node;
step 7, judging whether the current node neighborhood node is in the open list:
if the current node neighborhood node is not in the open list, expanding the neighborhood node of the current node, adding the current node neighborhood node into the open list, taking the current node as a father node of the neighborhood node, calculating a cost function of the neighborhood node of the current node, and executing the step 7;
if the current node neighborhood node is in the open list, if the current node is taken as a father node and g (n) of the neighborhood node of the current node is lower than the original g (n), taking the current node as the father node, recalculating the cost function of the neighborhood node of the current node, and executing the step 7, otherwise, executing the step 8;
step 8, calculating a cost value of a cost function f (n) of the neighborhood node, wherein n represents the nth node;
step 9, selecting the point with the minimum value of the cost function f (n) in the open list (open list) as the current node (x) to be expanded n ,y n ,z n ) Deleting the current node from the open list and adding the current node to the close list;
step 10, judging whether the target point is added into the open list:
if the target point is added into the open list, path planning is finished, and the target point is reversely solved to the starting point, namely the final path obtained by the path planning of the robot at this time;
if the target point is not in the open list, executing step 5;
further, the robot in the step 1 can be an unmanned vehicle, an AGV cart, an unmanned aerial vehicle, an unmanned boat and various robots with perception decision-making capability;
further, the current node neighborhood nodes expanded in the step 6 need to be expanded according to the types of the robots in the step 1, the expansion of the current nodes by a common star-a algorithm adopts eight neighborhood nodes expansion, and due to the fact that the kinematics and dynamics of different types of robots are different, the current node neighborhood is expanded by adopting different expansion methods:
step 6-1, when the type of the robot is an unmanned automobile, firstly calculating the minimum turning radius R under the current speed min Then pass through
Calculating the steering angle delta corresponding to the minimum turning radius at the current vehicle speed, and comparing the steering angle delta with the steering angle delta-delta and the steering angle delta in the current driving direction]The angle interval is evenly divided into i delta intervals, the current node neighborhood is evenly expanded,
it should be noted that the minimum turning radius R min The turning radius of a household car is small, and the turning radius of commercial vehicles such as a passenger car and a freight car is large;
step 6-2, when the robot is an AGV trolley, the AGV trolley generally supports omnidirectional movement, and neighborhood expansion can be uniformly performed on the current node in the 360-degree direction of the two-dimensional plane in the scene;
6-3, when the type of the robot is an unmanned aerial vehicle, the unmanned aerial vehicle can move in a three-dimensional space, and neighborhood expansion can be performed on the current node in the three-dimensional space;
further, the calculating step of calculating the cost function f (n) in step 8 includes:
step 8-1, calculating an actual cost function g (n) to represent the actual cost value from the starting point of the path to the current node;
step 8-2, calculating a heuristic function h (n), and representing the estimated cost value from the current node to the path target point by using the Euclidean distance;
step 8-3, calculating a penalty function ξ (n) as shown in FIG. 2, which represents a penalty value from the current node to the path target point;
wherein (x) n ,y n ,z n ) (x) coordinates of the current node on the grid map parent ,y parent ,z parent ) (x) coordinates of parent node of current node in the grid map n-1 ,y n-1 ,z n-1 ) Coordinates of a parent node in the grid map for the parent node;
further, the specific calculation step of the penalty value of the calculated penalty function in step 8-3 includes:
step 8-3-1, two vectors n are introduced 1 ,n 2 ：
n 1 ＝(x n-1 -x parent ,y n-1 -y parent ,z n-1 -z parent ),n 2 ＝(x parent -x n ,y parent -y n ,z parent -z n )
Wherein n is 1 Representing a vector, n, pointing from the parent node of the parent node to the parent node of the current node 2 Representing a vector pointing from a parent node to a current node;
step 8-3-2, calculating the cosine value of the included angle between the two vectors in the step 8-3-1:
step 8-3-3, calculating a penalty value of a penalty function according to the cosine value result:
when cos theta =1, the current node, the father node and the father node of the father node are on the same straight line, the planned path is not turned, no punishment is carried out, and xi (n) =0;
when cos θ ≠ 1, and x n ＝x goal ,y n ＝y goal ,z n ＝z goal When the current node is a target node, a path from a parent node of the current node to the target node turns relative to a planned path, namely an inflection point turns towards an end point, and the penalty value is ξ (n) = a 1 ；
When-1 is not more than cos theta<1, the current node is not the target point, which indicates that a turn occurs in the path planned by the target point, namely a common inflection point occurs, and the penalty value is ξ (n) = a 2 It should be noted that in this method a 2 >a 1 。
In the result diagram of the simulation operation of the Rviz software in the Ubuntu environment, the grid map is a 1 、a 2 Selecting different values to debug, and finally determining a 1 ＝0.2，a 2 =1, the result shows that under the condition that the end point, the starting point and the position of the obstacle are the same, the inflection points of the path 6 planned by the A star algorithm are obviously reduced compared with the inflection points of the path 5 planned by the traditional A star algorithm, and the path is more convenient to carry outAnd (4) smoothing.
In order to verify the effectiveness of the method, the embodiment selects different target points to respectively perform path planning by using the A-star algorithm and the traditional A-star algorithm of the invention, the time required by the planning, the length of the planned path and the number of nodes explored in the planning are compared, and the data result is shown in fig. 4.
In a specific implementation, the present application provides a computer storage medium and a corresponding data processing unit, where the computer storage medium is capable of storing a computer program, and the computer program, when executed by the data processing unit, may run the inventive content of the robot path planning method based on the a-star penalty control optimization algorithm provided by the present invention and some or all of the steps in each embodiment. The storage medium may be a magnetic disk, an optical disk, a read-only memory (ROM), a Random Access Memory (RAM), or the like. It is clear to those skilled in the art that the technical solutions in the embodiments of the present invention can be implemented by means of a computer program and its corresponding general-purpose hardware platform. Based on such understanding, the technical solutions in the embodiments of the present invention may be essentially or partially implemented in the form of a computer program or a software product, where the computer program or the software product may be stored in a storage medium and include instructions for enabling a device (which may be a personal computer, a server, a single chip microcomputer, an MUU, or a network device) including a data processing unit to execute the method according to the embodiments or some parts of the embodiments of the present invention.
The present invention provides a robot path planning method based on a star penalty control optimization algorithm, and a number of methods and ways for implementing the technical solution are provided, and the above description is only a preferred embodiment of the present invention, and it should be noted that, for those skilled in the art, a number of improvements and embellishments can be made without departing from the principle of the present invention, and these improvements and embellishments should also be regarded as the protection scope of the present invention. All the components not specified in the present embodiment can be realized by the prior art.
Claims (7)
1. A robot path planning method based on A star punishment control optimization algorithm is characterized by comprising the following steps:
step 1, collecting map information of surrounding environment by using a sensor to establish a map of the environment where the robot is located;
step 2, rasterizing the map, determining the state of each grid as occupied or idle, and determining the position coordinates of the obstacles;
step 3, determining the coordinates (x) of the starting point of the path start ,y start ,z start ) And target point coordinates (x) goal ,y goal ,z goal )；x start 、y start 、z start Coordinate values in the directions of the x, y and z axes which are respectively the starting points; x is the number of goal 、y goal 、z goal Coordinate values in the x, y and z axis directions of the target point;
step 4, initializing an open list and a close list, and enabling a path starting point coordinate (x) start ,y start ,z start ) Putting the open list in the closed list, wherein the closed list is empty;
step 5, checking whether the open list is empty, if so, indicating that the path planning fails, and if not, executing step 6;
step 6, ignoring the barrier node, if the close list is not empty, also ignoring the node in the close list, and expanding the current node to obtain a neighborhood node of the current node;
step 7, judging whether the current node neighborhood node is in the open list:
if the current node neighborhood node is not in the open list, expanding the neighborhood nodes of the current node, adding the neighborhood nodes into the open list, taking the current node as a father node of the neighborhood nodes, and calculating a cost function of the neighborhood nodes of the current node, namely executing the step 8;
if the current node neighborhood node is in the open list, if the current node is taken as a father node and g (n) of the neighborhood node of the current node is lower than the original g (n), taking the current node as the father node and recalculating the cost function of the neighborhood node of the current node, namely executing the step 8, otherwise executing the step 9;
step 8, calculating a cost function f (n) of the neighborhood nodes, wherein n represents the nth node;
step 9, selecting a point with the minimum value of the cost function f (n) in the open list as a current node to be expanded, deleting the current node from the open list, and adding the current node to the close list close;
step 10, judging whether the target point is added into the open list:
if the target point is added into the open list, path planning is finished, and the target point is reversely solved to the starting point, namely the final path obtained by the path planning of the robot at this time;
if the destination point is not in the open list, step 5 is executed.
2. The method of claim 1, wherein the sensors in step 1 comprise millimeter wave radar, lidar, a camera, an Inertial Measurement Unit (IMU), and a Global Positioning System (GPS).
3. The method according to claim 2, wherein the type of the robot in step 6 is to expand the current node neighborhood by using different expansion methods, which specifically includes:
step 6-1, when the type of the robot is an unmanned automobile, firstly calculating the minimum turning radius R under the current speed min Then pass through
Calculating the steering angle delta corresponding to the minimum turning radius under the current vehicle speed, and aligning the steering angle delta to the delta and the delta in the current driving direction]The angle interval is evenly divided into i delta intervals, the neighborhood of the current node is evenly expanded,
6-2, when the type of the robot is an AGV trolley, uniformly performing neighborhood expansion on the current node in a two-dimensional plane 360-degree direction;
and 6-3, when the type of the robot is an unmanned aerial vehicle, performing neighborhood expansion on the current node in a three-dimensional space.
4. The method of claim 3, wherein the step of calculating the cost function f (n) in step 8 comprises:
step 8-1, calculating an actual cost function g (n), wherein the actual cost function g (n) represents the actual moving cost value from a path starting point to the current node and is generated along the path from the path starting point to the current node;
step 8-2, calculating a heuristic function h (n);
and 8-3, calculating a penalty function xi (n).
5. The method of claim 4, wherein step 8-2 comprises: representing the estimated cost value from the current node to the path target point by using Euclidean distance (x) n ,y n ,z n ) For the coordinates of the current node on the grid map, the euclidean distance represents the straight-line distance between two points, and the formula is as follows:
6. the method of claim 5, wherein step 8-3 comprises:
step 8-3-1, two vectors n are introduced 1 ,n 2 ：
n 1 ＝(x n-1 -x parent ,y n-1 -y parent ,z n-1 -z parent ),n 2 ＝(x parent -x n ,y parent -y n ,z parent -z n )
Wherein (x) n ,y n ,z n ) (x) coordinates of the current node on the grid map parent ,y parent ,z parent ) (x) coordinates of parent node of current node in grid map n-1 ,y n-1 ,z n-1 ) Coordinates of a parent node which is the parent node in the grid map; n is 1 Representing a vector pointing from the parent node of the parent node to the parent node of the current node, n 2 A vector representing a point from a parent node to a current node;
step 8-3-2, calculating two vectors n 1 ,n 2 Cosine of the angle θ therebetween:
and 8-3-3, calculating the value of a penalty function xi (n) according to the cosine value result:
a 1 、a 2 is to adjust the threshold.
7. The method according to claim 6, wherein in step 8-3-3, when cos θ =1, the current node, the parent node, and the parent node of the parent node are on the same straight line, which means that the planned path is not turned and no penalty is made, ξ (n) =0;
when cos θ ≠ 1, and x n ＝x goal ,y n ＝y goal ,z n ＝z goal The method includes the steps that a current node is a target node, a path from a parent node of the current node to the target node turns relative to a planned path, namely an inflection point turns towards an end point, and then a penalty value is xi (n) = a 1 ；
When-1 is not more than cos theta<1, and the current node is not the target point, indicating that a turn occurs in the planned path to the target point, namely thatNormal inflection point, where the penalty is ξ (n) = a 2 Wherein a is 2 >a 1 。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211285517.0A CN115562290A (en) | 2022-10-20 | 2022-10-20 | Robot path planning method based on A-star penalty control optimization algorithm |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211285517.0A CN115562290A (en) | 2022-10-20 | 2022-10-20 | Robot path planning method based on A-star penalty control optimization algorithm |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN115562290A true CN115562290A (en) | 2023-01-03 |
Family
ID=84767650
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202211285517.0A Pending CN115562290A (en) | 2022-10-20 | 2022-10-20 | Robot path planning method based on A-star penalty control optimization algorithm |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115562290A (en) |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117516549A (en) * | 2024-01-03 | 2024-02-06 | 江苏领创星通卫星通信科技有限公司 | Path planning algorithm based on inertial navigation and satellite |
| CN117723079A (en) * | 2023-12-04 | 2024-03-19 | 青岛蚂蚁机器人有限责任公司 | AGV global path planning method based on improved A star algorithm |
| CN118031975A (en) * | 2024-04-15 | 2024-05-14 | 山东省科霖检测有限公司 | Large-scale environmental humidity monitoring method and system |
-
2022
- 2022-10-20 CN CN202211285517.0A patent/CN115562290A/en active Pending
Cited By (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117723079A (en) * | 2023-12-04 | 2024-03-19 | 青岛蚂蚁机器人有限责任公司 | AGV global path planning method based on improved A star algorithm |
| CN117723079B (en) * | 2023-12-04 | 2024-05-28 | 青岛蚂蚁机器人有限责任公司 | AGV global path planning method based on improved A star algorithm |
| CN117516549A (en) * | 2024-01-03 | 2024-02-06 | 江苏领创星通卫星通信科技有限公司 | Path planning algorithm based on inertial navigation and satellite |
| CN117516549B (en) * | 2024-01-03 | 2024-03-29 | 江苏领创星通卫星通信科技有限公司 | Path planning algorithm based on inertial navigation and satellite |
| CN118031975A (en) * | 2024-04-15 | 2024-05-14 | 山东省科霖检测有限公司 | Large-scale environmental humidity monitoring method and system |
| CN118031975B (en) * | 2024-04-15 | 2024-06-11 | 山东省科霖检测有限公司 | Large-scale environmental humidity monitoring method and system |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN108445879B (en) | Unmanned ship obstacle avoidance method based on collision danger prediction area | |
| CN109764886B (en) | Path planning method | |
| CN115562290A (en) | Robot path planning method based on A-star penalty control optimization algorithm | |
| CN110703762A (en) | Hybrid path planning method for unmanned surface vehicle in complex environment | |
| CN115079705A (en) | Routing planning method for inspection robot based on improved A star fusion DWA optimization algorithm | |
| CN106371445A (en) | Unmanned vehicle planning control method based on topology map | |
| Ab Wahab et al. | Path planning for mobile robot navigation in unknown indoor environments using hybrid PSOFS algorithm | |
| CN113805597B (en) | Obstacle self-protection artificial potential field method local path planning method based on particle swarm optimization | |
| CN114428499A (en) | Astar and DWA algorithm fused mobile trolley path planning method | |
| Chen et al. | Path Planning for Autonomous Vehicle Based on a Two‐Layered Planning Model in Complex Environment | |
| CN113467476A (en) | Non-collision detection rapid stochastic tree global path planning method considering corner constraint | |
| Vilca et al. | Optimal multi-criteria waypoint selection for autonomous vehicle navigation in structured environment | |
| CN112731929A (en) | Ackerman model-based mobile robot obstacle avoidance path planning method | |
| CN115167398A (en) | Unmanned ship path planning method based on improved A star algorithm | |
| Wang et al. | Research on AGV task path planning based on improved A* algorithm | |
| Meng et al. | Improved hybrid A-star algorithm for path planning in autonomous parking system based on multi-stage dynamic optimization | |
| CN116184993A (en) | Path planning method based on Hybrid Astar and space corridor | |
| CN114184203B (en) | Path planning method, electronic equipment and mobile device | |
| Lin et al. | Path planning for autonomous car parking | |
| CN114564008A (en) | Mobile robot path planning method based on improved A-Star algorithm | |
| CN117308964A (en) | Path planning method and device for intelligent pleasure boat, unmanned boat and medium | |
| Yin et al. | A safety navigation method for integrating global path planning and local obstacle avoidance for self-driving cars in a dynamic environment | |
| CN114815899A (en) | Unmanned aerial vehicle three-dimensional space path planning method based on 3D laser radar sensor | |
| Lin et al. | Realization of Ackermann robot obstacle avoidance navigation based on Multi-sensor fusion SLAM | |
| Shi et al. | Local path planning of unmanned vehicles based on improved RRT algorithm |
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 |