CN110487279B - Path planning method based on improved A-Algorithm - Google Patents
Path planning method based on improved A-Algorithm Download PDFInfo
- Publication number
- CN110487279B CN110487279B CN201910794684.XA CN201910794684A CN110487279B CN 110487279 B CN110487279 B CN 110487279B CN 201910794684 A CN201910794684 A CN 201910794684A CN 110487279 B CN110487279 B CN 110487279B
- Authority
- CN
- China
- Prior art keywords
- path
- nodes
- point
- points
- obstacle
- 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.)
- Active
Links
Images
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/20—Instruments for performing navigational calculations
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Numerical Control (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention discloses a path planning method based on an improved A-x algorithm. Aiming at the problem that the path obtained by the A-algorithm has more turning points and is not optimal, the key turning points in the path are merged on the basis of the A-algorithm, so that the turning points in the path are reduced, the number of cubic spline interpolation points is also reduced, the interpolation efficiency is improved, then the purpose of smoothing the path is achieved by utilizing cubic spline interpolation according to the path nodes after the turning points are merged, the path length is shorter and the whole body is smoother through the improved algorithm, and the motion of the incomplete robot is better met.
Description
Technical Field
The invention belongs to the field of intelligent robots, and particularly relates to a path planning method based on an improved A-star algorithm.
Background
With the continuous development of robot technology, more and more robots are used to provide various services, even replacing human work. The autonomous navigation is the key for realizing intellectualization and playing roles of the robot, and the path planning is an important component of the automatic navigation capability of the robot. The problem of path planning has always been a hot research content in the field of intelligent mobile robots. The path planning means that in the prior map, the mobile robot can automatically plan a collision-free path from a starting point to an end point by utilizing a sensor to acquire information according to the surrounding environment.
There are many classifications of routing algorithms. According to whether the external environment information is known or not, a global path planning algorithm and a local path planning algorithm can be divided; and the search method can be divided into a blind search algorithm and a heuristic search algorithm according to the algorithm. The blind search focuses on a search process rather than a search target, which often involves a huge search space, and thus, a large amount of memory resources are consumed and efficiency is low, specifically, an breadth-first algorithm, a depth-first algorithm, a Dijkstra algorithm, and the like. In the process of searching, the heuristic search develops the search towards a favorable direction according to heuristic information related to the problem, so that a plurality of meaningless search paths can be avoided, the search range is greatly reduced, the complexity of the problem is reduced, and a greedy algorithm and an A-star algorithm are common. The A-star algorithm is used as a widely used heuristic search algorithm, simultaneously takes the advantages of the Dijkstra algorithm and the greedy algorithm into consideration, can ensure that an optimal path is found, and can also enable the search direction to be more definite, so that the search space is smaller and the search speed is higher.
Disclosure of Invention
The purpose of the invention is as follows: when the traditional a-algorithm expands the neighboring nodes in the search process, only one layer of nodes is expanded outward with the current node as the center, that is, 8 nodes adjacent to the current node, at this time, the angle of the motion direction of the robot is limited to be an integral multiple of 45 degrees, and the motion direction is limited, and if the traditional a-algorithm expands the 8 neighboring nodes, the final path obtained by the robot path planning in the actual environment may not be optimal. Aiming at the problems, the invention provides a path planning method based on an improved A-star algorithm, the path planned by the improved algorithm is shorter in length, the path is smoother, and the path searching efficiency is higher.
The technical scheme is as follows: in order to realize the purpose of the invention, the technical scheme adopted by the invention is as follows: a path planning method based on an improved A-algorithm comprises the following steps:
step 1: representing the environment of the robot as a grid map, and searching an initial path from a starting point to a target point in the grid map by using an A-x algorithm;
step 2: extracting all nodes in the initial path, and sequentially judging whether three adjacent nodes are collinear or not by using an area method from a starting point so as to find all turning points, namely turning points, on the path;
and 3, step 3: sequentially connecting inflection points to a target point by using a straight line from a starting point to obtain an updated path; taking each straight line as L (i, j), wherein i, j respectively represents the starting point and the end point of the straight line segment, i =0,1, 2.. Multidot.n-1, j =1, 2.. Multidot.n, and n is the number of inflection points;
and 4, step 4: judging whether the straight line segment in the updated path passes through the obstacle or not in the step 3, eliminating redundant inflection points, and updating the path;
and 5: and 4, smoothing the path after the redundant inflection point is removed in the step 4 by adopting cubic spline interpolation to obtain the path which is finally planned.
Further, in step 4, since the environment where the robot is located is represented as a grid map, when whether an obstacle exists between any two nodes in the path is searched for in a fixed step length in a fixed direction (in a transverse direction or a longitudinal direction), a situation that the obstacle is missed may occur; the method judges whether the straight line segment passes through the barrier or not by dynamically selecting the search direction, and removes the redundant inflection point between i and j when the straight line segment L (i, j) does not pass through the barrier.
Further, whether the straight line segment passes through the barrier or not is judged by dynamically selecting the search direction, and the redundant inflection point is removed, and the specific process is as follows:
4-1, selecting any two nodes in the path updated in the step 3 as a search starting point A and a target point B, wherein the coordinates of the two points AB are the centers of the grid map grids where the two points AB are located and are respectively marked as (x) 1 ,y 1 ) And (x) 2 ,y 2 );
4-2, calculating a linear equation y = kx + b of the AB, wherein k is the slope of the straight line, and b is the ordinate of the intersection point of the straight line and the longitudinal axis;
4-3, determination of ABs (y) at two points AB 2 -y 1 ) And abs (x) 2 -x 1 ) Size, if abs (y) 2 -y 1 )>abs(x 2 -x 1 ) Performing step 4-4 by adopting horizontal search; otherwise, adopting longitudinal search to execute step 4-5;
4-4, searching whether an obstacle exists between the two nodes AB in a fixed step length in the transverse direction; if an obstacle exists, the inflection point between the two nodes AB cannot be eliminated, and the corresponding path cannot be updated; if no obstacle exists, the inflection point between the two nodes AB is removed, and the corresponding path is updated;
4-5, searching whether an obstacle exists between the two nodes AB in a fixed step length in the longitudinal direction; if an obstacle exists, the inflection point between the two nodes AB cannot be eliminated, and the corresponding path cannot be updated; if no obstacle exists, the inflection point between the two nodes AB is removed, and the corresponding path is updated;
4-6, according to all nodes in the traversal path in the steps 4-1-4-5, eliminating redundant inflection points and updating the path.
Further, the step 4-4 searches whether there is an obstacle between the two nodes AB in the transverse direction by a fixed step size, which is as follows:
4-4-a, recording all intersection points of the longitudinal grid lines and the line segments AB on the grid map as a (1), a (2), a, m and the number of the intersection points; calculating the vertical coordinates of all intersection points by using the linear equation of AB, and analyzing the vertical coordinates to obtain whether barriers exist around the intersection points;
4-4-b, determining the number of grids that will have the intersection points a (i), i =1,2, \8230;, m as common intersection points; if the intersection point a (i) is a common intersection point of the four grids, entering the step 4-4-c; if the intersection point a (i) is a common intersection point of the two grids, entering the step 4-4-d;
4-4-c, judging the positive and negative of the slope k of the line segment AB, and if the slope is positive, checking whether the adjacent three grids above the line segment AB have obstacles; if the slope is negative, checking whether three adjacent grids below the line segment AB have obstacles or not;
if an obstacle exists, the inflection point between the two nodes AB cannot be eliminated, the corresponding path cannot be updated, and the obstacle search between the two nodes AB is finished; if no obstacle exists, i = i +1, turning to the step 4-4-b, and continuing to judge the next intersection point;
4-4-d, judging whether the left grid and the right grid adjacent to the intersection point are obstacles or not; if the obstacle exists, the search for the obstacle between the two nodes AB is finished; if the intersection point is not the obstacle, i = i +1, turning to step 4-4-b, and continuing to judge the next intersection point;
4-4-e, after all the intersection points are traversed, the search for the obstacles between the two nodes AB is finished.
Further, the step 4-5 searches whether an obstacle exists between the two nodes AB in a fixed step length in the longitudinal direction, which is specifically as follows:
4-5-a, recording all intersection points of the transverse grid lines and the line segments AB on the grid map as b (1), b (2), a, b (m), wherein m is the number of the intersection points; calculating the horizontal coordinates of all intersection points by using the linear equation of AB, and analyzing to obtain whether barriers exist around the intersection points or not through the horizontal coordinates;
4-5-b, determining the grid number with the intersection points b (j), j =1,2, \8230, m as the common intersection points; if the intersection point b (j) is a common intersection point of the four grids, entering the step 4-5-c; if the intersection point b (j) is a common intersection point of the two grids, entering the step 4-5-d;
4-5-c, judging the positive and negative of the slope k of the line segment AB, and if the slope is positive, checking whether the adjacent three grids above the line segment AB have obstacles; if the slope is negative, checking whether three grids adjacent to the lower part of the line segment AB have obstacles or not;
if an obstacle exists, the inflection point between the two nodes AB cannot be eliminated, the corresponding path cannot be updated, and the obstacle search between the two nodes AB is finished; if no obstacle exists, i = i +1, going to step 4-5-b, and continuing to judge the next intersection point;
4-5-d, judging whether the left grid and the right grid adjacent to the intersection point are obstacles or not, and if the left grid and the right grid are obstacles, finishing searching the obstacles between the two nodes AB; if the obstacle is not the obstacle, i = i +1, the step 4-5-b is carried out, and the next intersection point is continuously judged;
4-5-e, after all the intersection points are traversed, the search of the obstacle between the two nodes AB is finished.
Further, in step 5, a cubic spline interpolation is used to smooth the path from which the redundant inflection point is removed, and the specific process is as follows:
5-1, setting a path with n +1 data nodes with the node coordinates of (x) 0 ,y 0 ),(x 1 ,y 1 ),(x 2 ,y 2 ),...,(x n ,y n ) (ii) a In each subinterval x i ≤x≤x i+1 Creating a spline difference equation:
g i (x)=a i +b i (x-x i )+c i (x-x i ) 2 +d i (x-x i ) 3
wherein, a i ,b i ,c i ,d i Coefficients representing spline curves;
5-2, calculating step length h i =x i+1 -x i ,i=0,1,...,n-1;
5-3, data node and end point condition M 0 =0,M n Substituting =0 into the following matrix equation yields:
wherein M is i I =0, 1.. And n denotes a spline difference equation quadratic differential value;
5-4, solving the matrix equation to obtain a spline difference equation secondary differential value M i ,i=0,1,...,n;
5-5, calculating the coefficient a of the spline curve i ,b i ,c i ,d i The formula is as follows:
a i =y i
wherein i =0,1,. Cndot.n-1;
and 5-6, solving the coefficient in the spline difference equation of each section to obtain a specific expression of each section of curve.
Has the advantages that: compared with the prior art, the technical scheme of the invention has the following beneficial technical effects:
compared with the traditional cubic spline interpolation smooth path algorithm, the smooth track planning algorithm fusing the improved A-star algorithm and the cubic spline interpolation provided by the invention reduces the number of interpolation points while reducing the number of path turning points, and also reduces the total path length. The introduction of cubic spline interpolation enables the whole path to be smoother, thereby avoiding the situations of rapid acceleration and rapid deceleration at the turning of the robot, enabling the motion form to be more continuous and more conforming to the dynamics control of the incomplete robot.
Drawings
FIG. 1 is an overall flow diagram of the method of the present invention;
fig. 2 is a schematic diagram of three points of an a-algorithm planned route segment according to an embodiment of the present invention;
FIG. 3 is a diagram illustrating a merge critical corner process according to an embodiment of the present invention;
FIG. 4 is a schematic diagram of a lateral traversal of an embodiment of the present invention;
FIG. 5 is a schematic diagram of a vertical traversal of an embodiment of the invention;
FIG. 6 is a simulation diagram of a conventional A-algorithm planned path;
fig. 7 is a simulation diagram of an improved a-algorithm planned path according to an embodiment of the present invention.
Detailed Description
The technical scheme of the invention is further explained by combining the drawings and the embodiment.
The invention relates to a path planning method based on an improved A-algorithm, the whole flow of which is shown in figure 1, and the method comprises the following steps:
step 1: and representing the environment where the robot is located into a grid map, and searching an initial path from a starting point to a target point in the grid map by using an A-x algorithm.
Step 2: extracting all nodes in the initial path, and sequentially judging whether three adjacent nodes are collinear or not by using an area method from a starting point so as to find all turning points, namely inflection points, on the path; the method comprises the following specific steps:
as shown in fig. 2, a, B and C represent three adjacent nodes in the path marked by a x algorithm, and the area S of the triangle ABC is calculated ABC When S is ABC When the distance is not 0, the three points A, B and C are not collinear, and the point B is an inflection point in the path;
triangle ABC area S ABC The calculation formula of (c) is as follows:
wherein the coordinate of the point A is (x) A ,y A ) And the coordinate of the point B is (x) B ,y B ) And the coordinate of the point C is (x) C ,y C )。
And step 3: sequentially connecting inflection points to a target point by using a straight line from a starting point to obtain an updated path; taking each straight line as L (i, j), wherein i, j respectively represents the starting point and the end point of the straight line segment, i =0,1, 2.. Multidot.n-1, j =1, 2.. Multidot.n, and n is the number of inflection points;
and 4, step 4: judging whether the straight line segment in the updated path passes through the obstacle or not in the step 3, eliminating redundant inflection points, and updating the path;
the method judges whether the straight line segment passes through the barrier or not by dynamically selecting the searching direction, and removes the redundant inflection point between i and j when the straight line segment L (i, j) does not pass through the barrier. The specific process is as follows:
4-1, selecting any two nodes in the path updated in the step 3 as a search starting point A and a target point B, wherein the coordinates of the two points AB are the centers of the grid map grids where the two points AB are located and are respectively marked as (x) 1 ,y 1 ) And (x) 2 ,y 2 )。
4-2, calculating the linear equation y = kx + b for AB, k being the slope of the line and b being the ordinate of the intersection of the line with the longitudinal axis.
4-3, determination of ABs (y) at two points AB 2 -y 1 ) And abs (x) 2 -x 1 ) Size, dabs (y) 2 -y 1 )>abs(x 2 -x 1 ) Adopting horizontal search to execute step 4-4; otherwise, adopting longitudinal search to execute step 4-5.
4-4, searching whether an obstacle exists between the two nodes AB in a fixed step length in the transverse direction; as shown in fig. 4, a black grid represents an obstacle, a white grid represents a passable region, a straight line represents a line segment arbitrarily connecting two inflection points, and a black point represents an intersection of the straight line and the grid;
if an obstacle exists, the inflection point between the two nodes AB cannot be eliminated, and the corresponding path cannot be updated; if no obstacle exists, the inflection point between the two nodes AB is removed, and the corresponding path is updated;
and searching whether an obstacle exists between two nodes AB in a fixed step length in the transverse direction by the following method:
4-4-a, recording all intersection points of the longitudinal grid lines and the line segments AB on the grid map as a (1), a (2), a (m) and m as the number of the intersection points; calculating the vertical coordinates of all the intersection points by using the linear equation of the AB, and analyzing the vertical coordinates to obtain whether barriers exist around the intersection points;
4-4-b, determining the number of grids that will have the intersection points a (i), i =1,2, \8230;, m as common intersection points; if the intersection point a (i) is a common intersection point of the four grids, entering the step 4-4-c; if the intersection point a (i) is a common intersection point of the two grids, entering the step 4-4-d;
4-4-c, judging the positive and negative of the slope k of the line segment AB, and if the slope is positive, checking whether the adjacent three grids above the line segment AB have obstacles; if the slope is negative, checking whether three grids adjacent to the lower part of the line segment AB have obstacles or not;
if the obstacle exists, the inflection point between the two nodes AB cannot be eliminated, the corresponding path cannot be updated, and the obstacle search between the two nodes AB is finished; if no obstacle exists, i = i +1, going to step 4-4-b, and continuing to judge the next intersection point;
4-4-d, judging whether the left grid and the right grid adjacent to the intersection point are obstacles or not, and if the left grid and the right grid are obstacles, finishing searching the obstacles between the two nodes AB; if the intersection point is not the obstacle, i = i +1, turning to step 4-4-b, and continuing to judge the next intersection point;
4-4-e, after all the intersection points are traversed, the search of the obstacle between the two nodes AB is finished.
4-5, searching whether an obstacle exists between two nodes AB in a fixed step length in the longitudinal direction, as shown in FIG. 5; if an obstacle exists, the inflection point between the two nodes AB cannot be eliminated, and the corresponding path cannot be updated; if no obstacle exists, the inflection point between the two nodes AB is removed, and the corresponding path is updated;
and searching whether an obstacle exists between two nodes AB in a fixed step length in the longitudinal direction by the following method:
4-5-a, recording all intersection points of the transverse grid lines and the line segments AB on the grid map as b (1), b (2), a, b (m), wherein m is the number of the intersection points; calculating the horizontal coordinates of all intersection points by using the linear equation of AB, and analyzing to obtain whether barriers exist around the intersection points or not through the horizontal coordinates;
4-5-b, determining the grid number with the intersection points b (j), j =1,2, \8230, m as the common intersection points; if the intersection point b (j) is a common intersection point of the four grids, entering the step 4-5-c; if the intersection point b (j) is a common intersection point of the two grids, entering the step 4-5-d;
4-5-c, judging the positive and negative of the slope k of the line segment AB, and if the slope is positive, checking whether the adjacent three grids above the line segment AB have obstacles; if the slope is negative, checking whether three grids adjacent to the lower part of the line segment AB have obstacles or not;
if an obstacle exists, the inflection point between the two nodes AB cannot be eliminated, the corresponding path cannot be updated, and the obstacle search between the two nodes AB is finished; if no obstacle exists, i = i +1, going to step 4-5-b, and continuing to judge the next intersection point;
4-5-d, judging whether the left grid and the right grid adjacent to the intersection point are obstacles, and if the left grid and the right grid are obstacles, finishing searching the obstacles between the two nodes AB; if the intersection point is not the obstacle, i = i +1, turning to the step 4-5-b, and continuing to judge the next intersection point;
4-5-e, after all the intersection points are traversed, the search for the obstacles between the two nodes AB is finished.
4-6, according to all nodes in the traversal path in the steps 4-1-4-5, eliminating redundant inflection points and updating the path.
As shown in fig. 3, where a, B, C, D, E represent extracted adjacent path inflection points. Starting from the point A, connecting the AC, and eliminating the obstacle when the AD does not appear, so that two redundant inflection points B and C can be eliminated, and the updating path is A-D-E. When AE is connected, an obstacle appears in the middle, so that point D cannot be removed, and the path cannot be updated. And then, taking the point D as a starting point, and sequentially connecting the point D with the following inflection points to judge whether the path can be updated or not until the end point is reached.
And 5: and (5) smoothing the path after the redundant inflection point is removed in the step (4) by adopting cubic spline interpolation to obtain the path which is finally planned. The specific process is as follows:
5-1, setting a path to have n +1 data nodes, and the coordinates of the nodes are respectively (x 0 ,y 0 ),(x 1 ,y 1 ),(x 2 ,y 2 ),...,(x n ,y n ) (ii) a In each sub-interval x i ≤x≤x i+1 Creating a spline difference equation:
g i (x)=a i +b i (x-x i )+c i (x-x i ) 2 +d i (x-x i ) 3
wherein, a i ,b i ,c i ,d i Coefficients representing spline curves;
5-2, calculating step length h i =x i+1 -x i ,i=0,1,...,n-1;
5-3, data node and end point condition M 0 =0,M n Insertion of =0 into the matrix equation yields:
wherein M is i I =0, 1.. N, n denotes a spline difference equation quadratic differential value;
5-4, solving the matrix equation to obtain a spline difference equation secondary differential value M i ,i=0,1,...,n;
5-5, calculating the coefficient a of the spline curve i ,b i ,c i ,d i The formula is as follows:
a i =y i
wherein i =0, 1.., n-1;
and 5-6, solving the coefficient in the spline difference equation of each section to obtain a specific expression of each section of curve.
Fig. 6 is a planned path diagram of a conventional a-algorithm, fig. 7 is a planned path diagram of an embodiment of the invention after a-algorithm improvement and smoothing, S denotes a starting point, and E denotes a target point. As can be seen from fig. 7, after merging the critical inflection points, the number of inflection points in the path is significantly reduced, and in addition to the redundant inflection points of the conventional a-star algorithm, the total length of the path is also reduced, and the path is gradually smooth. But a plurality of spikes exist at the turning position of the path, and cubic spline interpolation is introduced to ensure that the path is smooth enough, so that the speed and the acceleration of the robot at the turning position are kept continuous.
Claims (4)
1. A path planning method based on an improved A-algorithm is characterized in that: the method comprises the following steps:
step 1: representing the environment of the robot into a grid map, and searching an initial path from a starting point to a target point in the grid map by using an A-star algorithm;
step 2: extracting all nodes in the initial path, and sequentially judging whether three adjacent nodes are collinear or not by using an area method from a starting point so as to find all turning points, namely inflection points, on the path;
and step 3: sequentially connecting inflection points to a target point by using a straight line from a starting point to obtain an updated path; taking each straight line as L (i, j), wherein i, j respectively represents the starting point and the end point of the straight line segment, i =0,1, 2.. Multidot.n-1, j =1, 2.. Multidot.n, and n is the number of inflection points;
and 4, step 4: judging whether the straight line segment in the updated path passes through the obstacle or not in the step 3, eliminating redundant inflection points, and updating the path;
and 5: performing smoothing processing on the path with the redundant inflection point removed in the step 4 by adopting cubic spline interpolation to obtain a path which is finally planned;
in the step 4, whether the straight line segment passes through the barrier or not is judged by a method of dynamically selecting a search direction, and when the straight line segment L (i, j) does not pass through the barrier, a redundant inflection point between i and j is removed; the specific process is as follows:
4-1, selecting any two nodes in the path updated in the step 3 as a search starting point A and a target point B, wherein the coordinates of the two points AB are the centers of the grid map grids where the two points AB are located and are respectively marked as (x) 1 ,y 1 ) And (x) 2 ,y 2 );
4-2, calculating a linear equation y = kx + b of the AB, wherein k is the slope of the straight line, and b is the ordinate of the intersection point of the straight line and the longitudinal axis;
4-3, determination of ABs (y) at two points AB 2 -y 1 ) And abs (x) 2 -x 1 ) Size, dabs (y) 2 -y 1 )>abs(x 2 -x 1 ) Performing step 4-4 by adopting horizontal search; otherwise, adopting longitudinal search and executing the step 4-5;
4-4, searching whether an obstacle exists between the two nodes AB in a fixed step length in the transverse direction; if an obstacle exists, the inflection point between the two nodes AB is not removed, and the corresponding path is not updated; if no obstacle exists, the inflection point between the two nodes AB is removed, and the corresponding path is updated;
4-5, searching whether an obstacle exists between the two nodes AB in a fixed step length in the longitudinal direction; if an obstacle exists, the inflection point between the two nodes AB is not removed, and the corresponding path is not updated; if no obstacle exists, the inflection point between the two nodes AB is removed, and the corresponding path is updated;
4-6, according to all nodes in the traversal path in the steps 4-1-4-5, eliminating redundant inflection points and updating the path.
2. A path planning method based on the modified a-algorithm according to claim 1, characterized in that: step 4-4 searches whether there is an obstacle between two nodes AB with a fixed step size in the transverse direction, which is as follows:
4-4-a, recording all intersection points of the longitudinal grid lines and the line segments AB on the grid map as a (1), a (2), 8230, wherein a (m) and m are the number of the intersection points; calculating the vertical coordinates of all the intersection points by using the linear equation of the AB, and analyzing the vertical coordinates to obtain whether barriers exist around the intersection points or not;
4-4-b, determining the number of grids with the intersection points a (i), i =1,2, \8230;, m as common intersection points; if the intersection point a (i) is a common intersection point of the four grids, entering the step 4-4-c; if the intersection point a (i) is a common intersection point of the two grids, entering the step 4-4-d;
4-4-c, judging the positive and negative of the slope k of the line segment AB, and if the slope is positive, checking whether the adjacent three grids above the line segment AB have obstacles; if the slope is negative, checking whether three grids adjacent to the lower part of the line segment AB have obstacles or not;
if the obstacle exists, the inflection point between the two nodes AB is not removed, the corresponding path is not updated, and the obstacle search between the two nodes AB is finished; if no obstacle exists, i = i +1, going to step 4-4-b, and continuing to judge the next intersection point;
4-4-d, judging whether the left grid and the right grid adjacent to the intersection point are obstacles or not; if the node B is the obstacle, the obstacle search between the two nodes AB is finished; if the intersection point is not the obstacle, i = i +1, turning to step 4-4-b, and continuing to judge the next intersection point;
4-4-e, after all the intersection points are traversed, the search of the obstacle between the two nodes AB is finished.
3. A path planning method based on the modified a-algorithm according to claim 1, characterized in that: step 4-5 searches whether there is an obstacle between two nodes AB in a fixed step length in the longitudinal direction, specifically as follows:
4-5-a, recording all intersection points of the transverse grid lines and the line segments AB on the grid map as b (1), b (2), \ 8230, wherein b (m) and m are the number of the intersection points; calculating the horizontal coordinates of all intersection points by using a linear equation of AB, and analyzing the horizontal coordinates to obtain whether barriers exist around the intersection points;
4-5-b, determining the number of grids that will have the intersection points b (j), j =1,2, \8230;, m as common intersection points; if the intersection point b (j) is a common intersection point of the four grids, entering the step 4-5-c; if the intersection point b (j) is a common intersection point of the two grids, entering the step 4-5-d;
4-5-c, judging the positive and negative of the slope k of the line segment AB, and if the slope is positive, checking whether the adjacent three grids above the line segment AB have obstacles; if the slope is negative, checking whether three grids adjacent to the lower part of the line segment AB have obstacles or not;
if the obstacle exists, the inflection point between the two nodes AB is not removed, the corresponding path is not updated, and the obstacle search between the two nodes AB is finished; if no obstacle exists, i = i +1, going to step 4-5-b, and continuing to judge the next intersection point;
4-5-d, judging whether the left grid and the right grid adjacent to the intersection point are obstacles, and if the left grid and the right grid are obstacles, finishing searching the obstacles between the two nodes AB; if the intersection point is not the obstacle, i = i +1, turning to the step 4-5-b, and continuing to judge the next intersection point;
4-5-e, after all the intersection points are traversed, the search of the obstacle between the two nodes AB is finished.
4. A path planning method based on the modified a algorithm according to any one of claims 1 to 3, characterized in that: in the step 5, the path after the redundant inflection point is removed is smoothed by cubic spline interpolation, and the specific process is as follows:
5-1, setting the path to have n +1 data nodes, and the coordinates of the nodes are (x) 0 ,y 0 ),(x 1 ,y 1 ),(x 2 ,y 2 ),...,(x n ,y n ) (ii) a In each sub-interval x i ≤x≤x i+1 Creating a spline difference equation:
g i (x)=a i +b i (x-x i )+c i (x-x i ) 2 +d i (x-x i ) 3
wherein, a i ,b i ,c i ,d i Coefficients representing spline curves;
5-2, calculating step length h i =x i+1 -x i ,i=0,1,…,n-1;
5-3, data node and end point condition M 0 =0,M n Substituting =0 into the following matrix equation yields:
wherein, M i I =0,1, \ 8230, n denotes the quadratic differential value of the spline difference equation;
5-4, solving the matrix equation to obtain a spline difference equation secondary differential value M i ,i=0,1,…,n;
5-5, calculating the coefficient a of the spline curve i ,b i ,c i ,d i The formula is as follows:
a i =y i
wherein, i =0,1, \8230;, n-1;
5-6, solving the coefficient in each section of spline difference equation to obtain a specific expression of each section of curve.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910794684.XA CN110487279B (en) | 2019-08-27 | 2019-08-27 | Path planning method based on improved A-Algorithm |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910794684.XA CN110487279B (en) | 2019-08-27 | 2019-08-27 | Path planning method based on improved A-Algorithm |
Publications (2)
Publication Number | Publication Date |
---|---|
CN110487279A CN110487279A (en) | 2019-11-22 |
CN110487279B true CN110487279B (en) | 2022-12-13 |
Family
ID=68554357
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201910794684.XA Active CN110487279B (en) | 2019-08-27 | 2019-08-27 | Path planning method based on improved A-Algorithm |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN110487279B (en) |
Families Citing this family (15)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113066148B (en) * | 2020-01-02 | 2023-07-18 | 沈阳美行科技股份有限公司 | Map data processing method, device, equipment and storage medium |
CN111060109B (en) * | 2020-01-03 | 2021-08-27 | 东南大学 | Unmanned ship global path planning method based on improved A-star algorithm |
CN113539050B (en) * | 2020-04-20 | 2022-09-23 | 华为技术有限公司 | Data processing method, device and equipment |
CN111552288B (en) * | 2020-04-28 | 2021-03-16 | 西南交通大学 | Mobile robot path smoothing method |
CN111652436B (en) * | 2020-06-03 | 2023-04-18 | 中铁二院工程集团有限责任公司 | Contour line-based automatic construction pavement line selection method |
CN111906779B (en) * | 2020-06-30 | 2022-05-10 | 珠海一微半导体股份有限公司 | Obstacle crossing end judgment method, obstacle crossing control method, chip and robot |
CN111880534A (en) * | 2020-07-17 | 2020-11-03 | 桂林电子科技大学 | Secondary path planning method based on grid map |
CN112099498A (en) * | 2020-09-08 | 2020-12-18 | 合肥学院 | Path planning method and system based on parameterized Thiele continuous fractional interpolation |
CN112327856B (en) * | 2020-11-13 | 2022-12-06 | 云南电网有限责任公司保山供电局 | Robot path planning method based on improved A-star algorithm |
CN113359721B (en) * | 2021-05-31 | 2022-10-25 | 西安交通大学 | Improved A-based AGV path planning method combined with motion control |
CN113570682B (en) * | 2021-08-02 | 2024-05-07 | 北京经纬恒润科技股份有限公司 | Right-angle routing method and device |
CN113577772B (en) * | 2021-09-27 | 2022-01-11 | 深圳易帆互动科技有限公司 | Tile map-based unit moving method and device and readable storage medium |
CN114489040A (en) * | 2021-12-13 | 2022-05-13 | 中煤科工集团信息技术有限公司 | Hybrid path planning method based on improved A-star algorithm and artificial potential field algorithm |
CN114593726A (en) * | 2022-02-22 | 2022-06-07 | 深圳鹏行智能研究有限公司 | Path smoothing method and device |
CN115164914B (en) * | 2022-07-11 | 2023-10-03 | 北京中航世科电子技术有限公司 | Navigation method, system, electronic equipment and medium for individual combat |
Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN108549388A (en) * | 2018-05-24 | 2018-09-18 | 苏州智伟达机器人科技有限公司 | A kind of method for planning path for mobile robot based on improvement A star strategies |
WO2019042295A1 (en) * | 2017-08-31 | 2019-03-07 | 广州小鹏汽车科技有限公司 | Path planning method, system, and device for autonomous driving |
CN109798909A (en) * | 2019-02-01 | 2019-05-24 | 安徽达特智能科技有限公司 | A kind of method of global path planning |
CN109945873A (en) * | 2019-04-04 | 2019-06-28 | 东南大学 | A kind of mixed path planing method for indoor mobile robot motion control |
Family Cites Families (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN105043396B (en) * | 2015-08-14 | 2018-02-02 | 北京进化者机器人科技有限公司 | The method and system of self-built map in a kind of mobile robot room |
-
2019
- 2019-08-27 CN CN201910794684.XA patent/CN110487279B/en active Active
Patent Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2019042295A1 (en) * | 2017-08-31 | 2019-03-07 | 广州小鹏汽车科技有限公司 | Path planning method, system, and device for autonomous driving |
CN108549388A (en) * | 2018-05-24 | 2018-09-18 | 苏州智伟达机器人科技有限公司 | A kind of method for planning path for mobile robot based on improvement A star strategies |
CN109798909A (en) * | 2019-02-01 | 2019-05-24 | 安徽达特智能科技有限公司 | A kind of method of global path planning |
CN109945873A (en) * | 2019-04-04 | 2019-06-28 | 东南大学 | A kind of mixed path planing method for indoor mobile robot motion control |
Also Published As
Publication number | Publication date |
---|---|
CN110487279A (en) | 2019-11-22 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110487279B (en) | Path planning method based on improved A-Algorithm | |
CN109764886A (en) | A kind of paths planning method | |
CN110954122B (en) | Automatic driving track generation method under high-speed scene | |
CN112327856B (en) | Robot path planning method based on improved A-star algorithm | |
CN112526988B (en) | Autonomous mobile robot and path navigation and path planning method and system thereof | |
CN110319837A (en) | Indoor complex condition path planning method for service robot | |
CN112306067B (en) | Global path planning method and system | |
CN112965485B (en) | Robot full-coverage path planning method based on secondary area division | |
CN112486178A (en) | Dynamic path planning method based on directed D (delta) algorithm | |
CN114161416A (en) | Robot path planning method based on potential function | |
CN108827311A (en) | A kind of manufacturing shop unmanned handling system paths planning method | |
CN113189988B (en) | Autonomous path planning method based on Harris algorithm and RRT algorithm composition | |
CN114705196A (en) | Self-adaptive heuristic global path planning method and system for robot | |
CN113721608A (en) | Robot local path planning method and system and readable storage medium | |
CN115167398A (en) | Unmanned ship path planning method based on improved A star algorithm | |
CN117740020A (en) | Smooth path improvement method based on A-star algorithm and cubic B spline curve fusion | |
CN113721969A (en) | Multi-scale space vector data cascade updating method | |
Wang et al. | Application of A* algorithm in intelligent vehicle path planning | |
CN116734877A (en) | Robot dynamic obstacle avoidance method based on improved A-algorithm and dynamic window method | |
CN112509132B (en) | Ship application-oriented three-dimensional path calculation method | |
Bigaj et al. | A memetic algorithm based procedure for a global path planning of a movement constrained mobile robot | |
CN111210628B (en) | Boundary road section selection method and device in urban reachable area calculation | |
CN114995391A (en) | 4-order B spline curve path planning method for improving A-star algorithm | |
CN113467480A (en) | Global path planning algorithm for unmanned equation | |
CN114353814A (en) | Improved JPS path optimization method based on Angle-Propagation Theta 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 | ||
GR01 | Patent grant | ||
GR01 | Patent grant |