CN112650256A - Improved bidirectional RRT robot path planning method - Google Patents

Improved bidirectional RRT robot path planning method Download PDF

Info

Publication number
CN112650256A
CN112650256A CN202011609932.8A CN202011609932A CN112650256A CN 112650256 A CN112650256 A CN 112650256A CN 202011609932 A CN202011609932 A CN 202011609932A CN 112650256 A CN112650256 A CN 112650256A
Authority
CN
China
Prior art keywords
path
rand
node
robot
tree
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
Application number
CN202011609932.8A
Other languages
Chinese (zh)
Inventor
李伟
任孟瀚
郑智宇
张延宇
杜海顺
周毅
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Henan University
Eagle Drive Technology Shenzhen Co Ltd
Original Assignee
Henan University
Eagle Drive Technology Shenzhen Co Ltd
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Henan University, Eagle Drive Technology Shenzhen Co Ltd filed Critical Henan University
Priority to CN202011609932.8A priority Critical patent/CN112650256A/en
Publication of CN112650256A publication Critical patent/CN112650256A/en
Pending legal-status Critical Current

Links

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

Landscapes

  • Engineering & Computer Science (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Numerical Control (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Manipulator (AREA)

Abstract

The invention aims to provide a path planning method based on an improved bidirectional RRT robot, and provides a central circle sampling strategy aiming at the defects of an RRT algorithm, so that the sampling randomness of the bidirectional RRT algorithm is reduced; an obstacle expansion strategy is integrated, so that a certain distance is kept between a planned path and an obstacle, and the planned path and the obstacle are more in line with the actual robot running path; a target deflection strategy is introduced, so that the searching efficiency of the algorithm is improved; finally, spline interpolation is carried out on the planned path, so that the planned path is more stable and smooth; finally, compared with other algorithms, the effectiveness of the algorithm provided by the text is verified, the CC _ BRRT algorithm provided by the text reduces the sampling blindness to a certain extent, shortens the path length, reduces the number of sampling nodes and improves the path smoothness.

Description

Improved bidirectional RRT robot path planning method
Technical Field
The invention belongs to the technical field of RRT algorithm path planning, and particularly relates to a path planning method based on an improved bidirectional RRT robot.
Background
The path planning is a key technology for safely reaching a target position from an initial position of a mobile robot under a multi-constraint condition, and is divided into local path planning and global path planning according to environment known information. The local path planning is that the robot utilizes a sensor to extract surrounding environment information under the environment that the environment is unknown, and a safe path from a current path node to a next path node is planned timely and effectively. The global path planning is to plan a collision-free path from a starting position to a target position before the robot moves on the premise that the environment of the robot is known.
In a complex environment, how to move a mobile robot from an initial position to a target position efficiently and in real time is always concerned by researchers, and through years of development, a path planning method is mature, and a plurality of methods are proposed and mainly divided into a method based on graph theory and a sampling method. The method based on the graph theory is represented by Dijkstra's algorithm and A-algorithm, the method needs discretization processing on a state space and then path searching, the method based on sampling mainly comprises PRM algorithm and RRT algorithm, a graph is randomly sampled and constructed in the state space to search a path, and compared with other algorithms, the RRT algorithm based on basic sampling has higher convergence speed and efficiently solves the path planning problem under unknown complex environment and high-dimensional environment; the RRT algorithm also has some defects, because of the randomness of the path planning of the algorithm, sampling needs to be carried out in the whole state space, the cost of searching invalid areas is high, the corners of the generated path are more, even some right angle changes in a small range can occur, the kinematics model of the robot cannot be satisfied, the traditional RRT algorithm for selecting the nearest point adopts the Euclidean distance, the result of path searching is influenced to a certain extent, the generated path is more tortuous, and the actual driving of the robot is not easy to realize.
Disclosure of Invention
The invention aims to provide a path planning method based on an improved bidirectional RRT robot, which is used for improving the path search time of a bidirectional RRT algorithm, reducing sampling points and enabling a generated path to smoothly conform to the motion of an actual robot.
As shown in fig. 1, the technical solution for solving the technical problem of the present invention is: a path planning method based on an improved bidirectional RRT robot comprises the following steps,
s1: expanding the obstacles by using an obstacle expansion technology, establishing a map model, and determining the initial position and the target position of the robot in the map;
the initial state of the obstacle is as shown in fig. 2, a circle in the figure is an original obstacle model, the expanded state is a square, wherein p is the distance from the center of the robot to the edge of the body, the arrangement is that (p is 1.5), and the initial state of the obstacle and the expanded space jointly form an impenetrable area. Through the barrier expansion technology, a certain safety distance is kept between the planned path and the barrier, the actual robot running state is better met, and the planned path is not attached to the barrier along with the traditional planned path and does not meet the actual motion condition.
S2: two fast exploration random trees Ta，TbRespectively from the starting position X of the robotstartAnd a target position XgoalThe sampling is carried out by utilizing a central circle sampling strategy in the state space expanded to obtain a random sampling point Xrand-a、Xrand-b; as shown in fig. 3, a random circle is generated by using a connection line between a start point and a target point as a center and a center point as a circle center, and a sampling point X is generated on the generated circlerand(for convenience of description, X is used hereinrandRefers to XrandA and Xrand-b), the specific calculation formula is as follows,
Figure BDA0002874370030000031
n∈(0,1)，
Figure BDA0002874370030000032
in the formula: n is a random number in the range of 0 to 1, m is a central circle coefficient, RwSampling radius, x, for a circumscribed circlestart,ystartThe abscissa and ordinate, x, of the starting pointgoal，ygoalThe abscissa and ordinate values, x, of the end pointcenter，ycenterIs the abscissa and ordinate values of the center point, xrand，yrandFor X produced on the centre circlerandThe horizontal and vertical coordinate values of (a).
S3: obtaining random sampling point XrandA later, will be in the tree TaMiddle distance point XrandA nearest child node as a nearest neighbor node
Figure BDA0002874370030000033
Then the
Figure BDA0002874370030000034
Towards XrandA direction generates a new node according to step S
Figure BDA0002874370030000035
New node
Figure BDA0002874370030000036
And the nearest neighbor node
Figure BDA0002874370030000037
Performing collision detection, checking whether the two nodes and the connecting line are in the obstacle, and if not, determining that the two nodes and the connecting line pass the collision detection; and will newly generate a node
Figure BDA0002874370030000038
Add to Tree TaForming an expanded tree Ta-E; by TaTo obtain Ta-E method for TaIs operated to obtain TbE, if extendedTree TbNearest neighbor node in E
Figure BDA0002874370030000039
Find TaNew node in E
Figure BDA00028743700300000310
And Tb-of
Figure BDA00028743700300000311
And Tbin-E
Figure BDA00028743700300000312
Distance is less than threshold for two spanning tree joins, Ta-E and Tb-E will make a connection and the planned path is found; wherein the tree TbExtension of (2) and TaThe expansion is performed in the state space in the same expansion manner until a path is found.
S4: and smoothing the obtained path by utilizing six-order spline interpolation to obtain a path which accords with the motion of the actual robot.
The expansion technique in step S1 is to expand the obstacle according to the distance p from the center of the robot to the edge of the robot, and the initial state of the obstacle and the expanded space together form an obstacle area.
The step S2 is specifically that two fast exploration random trees Ta，TbRespectively from starting point XstartTarget point XgoalExpanding the sampling point to a state space, and obtaining a sampling point X by using a central circle sampling strategyrand-a、XrandB is calculated in particular as:
Figure BDA0002874370030000041
n∈(0,1)，
Figure BDA0002874370030000042
in the formula: n isRandom number in the range of 0 to 1, m is the central circle coefficient, m is 1, RwRadius of a circle circumscribing 100 x 100 of the map, xstart,ystartRespectively as starting points XstartThe horizontal and vertical coordinate values of (x)goal，ygoalIs an end point XgoalThe horizontal and vertical coordinate values of (x)center，ycenterIs the abscissa and ordinate values of the center point, xrand，yrandAs a central circle sampling point XrandA or XrandThe abscissa and ordinate values of b.
The tree T in the step S3aMiddle distance point XrandA nearest child node as a nearest neighbor node
Figure BDA0002874370030000043
Is of the formula
Figure BDA0002874370030000044
Wherein the content of the first and second substances,
Figure BDA0002874370030000045
is a tree TaA child node in (c).
The selection of the nearest neighbor node in said step S3 uses a diagonal distance for selection,
diagonal distance
Figure BDA0002874370030000051
Wherein xaIs a tree TaX-coordinate value, y, of neutron nodeaIs a tree TaY-coordinate value, x, of neutron nodebIs equal to TaRandom sampling point X for neutron node comparisonrandThe x-coordinate value of a, ybIs equal to TaRandom sampling point X for neutron node comparisonrand-y coordinate value of a.
In the step S3
Figure BDA0002874370030000052
Towards XrandA direction generates a new node according to step S
Figure BDA0002874370030000053
The formula of (a) is specifically:
Figure BDA0002874370030000054
S＝min(s,dist)，
Figure BDA0002874370030000055
wherein, s is 3,
Figure BDA0002874370030000056
is composed of
Figure BDA0002874370030000057
The x-coordinate value of (a) is,
Figure BDA0002874370030000058
is composed of
Figure BDA0002874370030000059
The y-coordinate value of (a),
Figure BDA00028743700300000510
is composed of
Figure BDA00028743700300000511
The x-coordinate value of (a) is,
Figure BDA00028743700300000512
is composed of
Figure BDA00028743700300000513
Y coordinate value of (2), xrandIs XrandThe x-coordinate value of a, yrandIs XrandA y coordinate value of a, dist is
Figure BDA00028743700300000514
And Xrand-the euclidean distance of a.
The step S4 is to perform smoothing processing on the obtained path by using a six-order spline interpolation curve, give a path node and have a unique spline parameter set, so that the planned path passes through the path point to achieve the purpose of path smoothing,
the sixth-order spline interpolation curve h (x) aix6+bix5+cix4+…gi i＝0，1,2,…6，
Wherein a isi,bi,ci,…giThe undetermined coefficients are solved for the nodes that have traversed the path.
The invention has the beneficial effects that: aiming at the defects of the bidirectional RRT algorithm, the invention provides a central circle sampling strategy, and the randomness of bidirectional RRT sampling is reduced; an obstacle expansion strategy is integrated, so that a certain distance is kept between a planned path and an obstacle, and the planned path and the obstacle are more in line with an actual running path; and then, carrying out six-time spline interpolation smoothing processing on the planned path, so that the planned path is more stable and smooth.
Drawings
FIG. 1 is a flow chart of the present invention.
Fig. 2 is a diagram of a model of the barrier of the present invention after inflation.
Fig. 3 is a schematic diagram of the center circle sampling of the present invention.
FIG. 4 is a schematic diagram of a simulation environment for the simple environment of the present invention.
FIG. 5 is a schematic diagram of a simulation environment of a complex environment of the present invention.
Fig. 6 is a diagram of the extension process of GRRT random spanning tree in the simple environment of the present invention.
FIG. 7 is a diagram of the expansion process of GBRRT random spanning tree under the simple environment of the present invention.
FIG. 8 is a diagram of the expansion process of CC _ BRRT random spanning tree in the simple environment of the present invention.
Fig. 9 is a path diagram finally formed by the algorithm under the simple environment of the present invention.
FIG. 10 is a diagram of final path results of the present algorithm and other algorithms in a simple environment.
Fig. 11 is a diagram of an expansion process of a GRRT random spanning tree in a complex environment of the present invention.
FIG. 12 is a diagram of the expanding process of GBRRT random spanning tree under the complex environment of the present invention.
FIG. 13 is a diagram of the expansion process of CC _ BRRT random spanning tree in complex environment of the present invention.
FIG. 14 is a path diagram finally formed by the algorithm under the complex environment of the invention.
FIG. 15 is a diagram illustrating final path results of the present algorithm and other algorithms in a complex environment.
Fig. 16 is a diagram comparing the average number of nodes in different environments of CC _ BRRT, GRRT and GBRRT algorithms proposed by the present invention.
FIG. 17 is a diagram showing the comparison of the average length of CC _ BRRT, GRRT and GBRRT algorithms in different environments.
FIG. 18 is a graph showing the comparison of the average time of CC _ BRRT and GRRT and GBRRT algorithms in different environments.
Detailed Description
The technical solution in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.
As shown in fig. 1, the present invention includes the steps of,
s1: expanding the obstacles by using an obstacle expansion technology, establishing a map model, and determining the initial position and the target position of the robot in the map;
the expansion technique in step S1 is to expand the obstacle according to the distance p from the center of the robot to the edge of the robot, and the initial state of the obstacle and the expanded space together form an obstacle area.
S2: two fast exploration random trees Ta，TbRespectively from the starting position X of the robotstartAnd a target position XgoalThe sampling is carried out by utilizing a central circle sampling strategy in the state space expanded to obtain a random sampling point Xrand-a、Xrand-b；
The step S2 is specifically that two fast exploration random trees Ta，TbRespectively from starting point XstartTarget point XgoalExpanding the sampling point to a state space, and obtaining a sampling point X by using a central circle sampling strategyrand-a、XrandB is calculated in particular as:
Figure BDA0002874370030000071
n∈(0,1)，
Figure BDA0002874370030000081
in the formula: n is a random number in the range of 0 to 1, m is a central circle coefficient, m is 1, RwRadius of a circle circumscribing 100 x 100 of the map, xstart,ystartRespectively as starting points XstartThe horizontal and vertical coordinate values of (x)goal，ygoalIs an end point XgoalThe horizontal and vertical coordinate values of (x)center，ycenterIs the abscissa and ordinate values of the center point, xrand，yrandAs a central circle sampling point XrandA or XrandThe abscissa and ordinate values of b.
S3: obtaining random sampling point XrandA later, will be in the tree TaMiddle distance point XrandA nearest child node as a nearest neighbor node
Figure BDA0002874370030000082
Then the
Figure BDA0002874370030000083
Towards XrandA direction generates a new node according to step S
Figure BDA0002874370030000084
New node
Figure BDA0002874370030000085
And the nearest neighbor node
Figure BDA0002874370030000086
Performing collision detection, checking whether the two nodes and the connecting line are in the obstacle, and if not, determining that the two nodes and the connecting line pass the collision detection; if the collision detection is passed, the newly generated node is used
Figure BDA0002874370030000087
Add to Tree TaForming an expanded tree Ta-E; by TaTo obtain Ta-E method for TaIs operated to obtain TbE, if the tree T is expandedbNearest neighbor node in E
Figure BDA0002874370030000088
Find TaNew node in E
Figure BDA0002874370030000089
And Tb-of
Figure BDA00028743700300000810
And Tbin-E
Figure BDA00028743700300000811
Distance is less than threshold for two spanning tree joins, Ta-E and Tb-E will make a connection and the planned path is found; wherein the tree TbExtension of (2) and TaThe expansion is performed in the state space in the same expansion manner until a path is found.
The tree T in the step S3aMiddle distance point XrandA nearest child node as a nearest neighbor node
Figure BDA00028743700300000812
Is of the formula
Figure BDA00028743700300000813
Wherein the content of the first and second substances,
Figure BDA00028743700300000814
is a tree TaA child node in (c).
The selection of the nearest neighbor node in said step S3 uses a diagonal distance for selection,
diagonal distance
Figure BDA0002874370030000091
Wherein xaIs a tree TaX-coordinate value, y, of neutron nodeaIs a tree TaY-coordinate value, x, of neutron nodebIs equal to TaRandom sampling point X for neutron node comparisonrandThe x-coordinate value of a, ybIs equal to TaRandom sampling point X for neutron node comparisonrand-y coordinate value of a.
In the step S3
Figure BDA0002874370030000092
Towards XrandA direction generates a new node according to step S
Figure BDA0002874370030000093
The formula of (a) is specifically:
Figure BDA0002874370030000094
S＝min(s,dist)，
Figure BDA0002874370030000095
wherein, s is 3,
Figure BDA0002874370030000096
is composed of
Figure BDA0002874370030000097
The x-coordinate value of (a) is,
Figure BDA0002874370030000098
is composed of
Figure BDA0002874370030000099
The y-coordinate value of (a),
Figure BDA00028743700300000910
is composed of
Figure BDA00028743700300000911
The x-coordinate value of (a) is,
Figure BDA00028743700300000912
is composed of
Figure BDA00028743700300000913
Y coordinate value of (2), xrandIs XrandThe x-coordinate value of a, yrandIs XrandA y coordinate value of a, dist is
Figure BDA00028743700300000914
And Xrand-the euclidean distance of a.
S4: and smoothing the obtained path by utilizing six-order spline interpolation to obtain a path which accords with the motion of the actual robot.
The step S4 is to perform smoothing processing on the obtained path by using a six-order spline interpolation curve, give a path node and have a unique spline parameter set, so that the planned path passes through the path point to achieve the purpose of path smoothing,
the sixth-order spline interpolation curve h (x) aix6+bix5+cix4+…gi i＝0，1,2,…6，
Wherein a isi,bi,ci,…giThe undetermined coefficients are solved for the nodes that have traversed the path.
In order to verify the effectiveness of the algorithm in solving the robot path planning problem, the CC _ BRRT algorithm, namely the Center circle bidirectional RRT (CC _ BRRT) algorithm, is respectively compared with a target deviation double-tree GBRRT (GoalBiaRRT-Connect) algorithm and a target deviation GRRT (GoalBia-RRT) algorithm in an experiment, so that the effectiveness and the feasibility of the algorithm are comprehensively verified.
The algorithm and the comparison algorithm both adopt the same software and hardware platform, the operating environment is Windows10, the programming environment is Pycharm2019, the range of the experimental environment is 100m × 100m, the coordinates of the starting point are (0,0), and the coordinates of the target point are (100 ). The objective of the solution is to find a collision-free, shortest-distance and smooth effective path from the starting position to the target position, fig. 4 is a simple environment, fig. 5 is a map under a complex environment, and path planning is performed in the two environments.
The goal of planning a path is to plan a route for the robot from a starting position to a target position in a two-dimensional state space, such that the route satisfies the following conditions:
(1) the planned path keeps a certain safety distance with all the obstacles;
(2) the path is as short and smooth as possible.
In simple environment map 1, fig. 6 is an expansion process of GRRT, GBRRT, and herein algorithm CC _ BRRT random spanning tree.
As can be seen from fig. 6, in a simple environment, the GRRT algorithm has low efficiency in planning a path, the expansion trees are uniformly distributed in an obstacle-free area, the planned path has a large bend, and the path length is also long. As can be seen in fig. 7, there is significant meandering and ripple within multiple local areas of the GBRRT algorithm. As can be seen from fig. 8, the CC _ BRRT expansion tree is significantly reduced, a certain safety distance is maintained between the planned path and the obstacle, and fig. 9 is a path finally formed by fitting, which shows that the finally generated path is smoother and conforms to the actual running path.
Fig. 10 shows GRRT, CC _ BRRT, RRT, GRRT, and GRRT by short dashed line, dot-dash line, solid line, and dot-plus-sign line, respectively, and it can be seen from the figure that the path planned by the CC _ BRRT algorithm is relatively smooth and relatively short. Because the RRT algorithm has poor performance, GRRT is asymptotically optimal, and although the planned path is short and smooth, the cost of time spent and the cost of iteration times are large, the performance of the RRT algorithm is not compared with those of the two algorithms. The performance of the algorithm was compared with the GRRT and GBRRT algorithms, and the results of the three algorithms after 30 iterations of path planning are summarized in table 1.
TABLE 1 comparison of performance of three algorithms in simple environment
Figure BDA0002874370030000111
Fig. 11-14 are simulation results of CC _ BRRT and other algorithms in a complex environment, and it can be directly seen from the figures that the path in this document is relatively smooth and the path is relatively short. Fig. 11 shows that in a complex environment, GRRT algorithm sampling points cover the entire state space, so that the efficiency of planning a path is seriously reduced, the planned path is not smooth, and the path length is long; as can be seen from fig. 12, the number of GBRRTs is somewhat reduced relative to the number of GRRT algorithm nodes, but the planned path is comparatively tortuous in a large range and does not conform to the path of actual robot motion; as can be seen from fig. 13, the algorithm has relatively few sampling points, a certain distance is kept between the planned path and the obstacle, and the planned path is smoother after smoothing; fig. 14 shows the path finally generated, which corresponds to the actual robot travel path.
In fig. 15, the short-dashed line, the dot-dash line, the dotted line, the solid line, and the dot-plus-sign line respectively represent GRRT, CC _ BRRT, RRT, GRRT, and it can be seen from fig. 8 that the path planned by the CC _ BRRT algorithm is short and smooth. The results of the 30-time repeated path planning for each of CC _ BRRT and other algorithms are summarized in Table 2.
The results listed in table 2 are the average results of the node number, the path length, and the time obtained by independently running each algorithm for 30 times under the same environmental conditions for the present algorithm and the comparative algorithm, and it can be seen that under the complex environmental conditions, the path length planned by the improved algorithm herein, the time taken to plan the path, and the node number are superior to the other three algorithms, wherein under the complex environmental conditions, the path length planned by the improved algorithm herein is reduced by 72.6% in time compared to GRRT algorithm, and is reduced by about 11.7% in GBRRT time, and from the viewpoint of the path length, the path length of the algorithm herein is also reduced by a lot compared to other algorithms, and is reduced by 10.94% in path length planned by compared to GRRT algorithm, by about 4.8% in average path length compared to GBRRT, from the node number analysis, by 67.39% in node number compared to GRRT algorithm, and by about 9.73% in node number compared to GBRRT algorithm. The validity and reliability of the improved algorithm are further verified from a simple environment to a complex environment.
TABLE 2 comparison of performance of three algorithms in complex environments
Figure BDA0002874370030000121
In this document, the performance of the detection algorithm adopts the average node number, the average path length and the average time. In the experiment, the results were repeated 30 times to obtain an average value. Fig. 16-18 show, in bar graph form, the results of a comparison of the CC BRRT algorithm herein with the other two algorithms, and it can be directly seen that the algorithm herein is optimal, whether in time, node, or path length.
Aiming at the defects of the bidirectional RRT algorithm, the invention provides a Center circle bidirectional RRT (Center Circles BRRT: CC _ BRRT) algorithm, thereby reducing the randomness of bidirectional RRT sampling; an obstacle expansion strategy is integrated, so that a certain distance is kept between a planned path and an obstacle, and the planned path and the obstacle are more in line with an actual running path; and then, carrying out six-time spline interpolation smoothing processing on the planned path to enable the planned path to be more stable and smooth, so as to further improve the efficiency and the smoothness of the RRT algorithm planned path.

Claims (7)

1. A path planning method based on an improved bidirectional RRT robot is characterized in that: comprises the following steps of (a) carrying out,
s1: expanding the obstacles by using an obstacle expansion technology, establishing a map model, and determining the initial position and the target position of the robot in the map;
s2: two fast exploration random trees Ta，TbRespectively from the starting position X of the robotstartAnd a target position XgoalThe sampling is carried out by utilizing a central circle sampling strategy in the state space expanded to obtain a random sampling point Xrand-a、Xrand-b；
S3: obtaining random sampling point XrandAfter a, the tree TaMiddle distance point XrandA nearest child node as a nearest neighbor node
Figure FDA0002874370020000011
Then the
Figure FDA0002874370020000012
Towards XrandA direction generating a new node according to step S
Figure FDA0002874370020000013
New node
Figure FDA0002874370020000014
And the nearest neighbor node
Figure FDA0002874370020000015
Performing collision detection, checking whether the two nodes and the connecting line are in the obstacle, and if not, determining that the two nodes and the connecting line pass the collision detection; if the collision detection is passed, the newly generated node is detected
Figure FDA0002874370020000016
Add to Tree TaForming an expanded tree Ta-E; by TaTo obtain Ta-E method for TbIs operated to obtain TbE, if the tree T is expandedbNearest neighbor node in E
Figure FDA0002874370020000017
Find TaNew node in E
Figure FDA0002874370020000018
And Tb-of
Figure FDA0002874370020000019
And Tbin-E
Figure FDA00028743700200000110
Distance is less than threshold for two spanning tree joins, Ta-E and Tb-E will make a connection and the planned path is found; wherein the tree TbExtension of (2) and TaThe expansion is performed in the state space in the same expansion manner until a path is found.
S4: and smoothing the obtained path by utilizing six-order spline interpolation to obtain a path which accords with the motion of the actual robot.
2. The method for planning a robot path based on the improved bidirectional RRT of claim 1, wherein the method comprises the following steps: the expansion technique in step S1 is to expand the obstacle according to the distance p from the center of the robot to the edge of the robot, and the initial state of the obstacle and the expanded space together form an obstacle area.
3. The method for planning a robot path based on the improved bidirectional RRT of claim 2, wherein the method comprises the following steps: the step S2 is specifically that two fast exploration random trees Ta，TbRespectively from starting point XstartTarget point XgoalExpanding the sampling point to a state space, and obtaining a sampling point X by using a central circle sampling strategyrand-a、XrandB is calculated in particular as:
Figure FDA0002874370020000021
Figure FDA0002874370020000022
in the formula: n is a random number in the range of 0 to 1, m is a central circle coefficient, m is 1, RwRadius of a circle circumscribing 100 x 100 of the map, xstart,ystartRespectively as starting points XstartThe horizontal and vertical coordinate values of (x)goal，ygoalIs an end point XgoalThe horizontal and vertical coordinate values of (x)center，ycenterIs the abscissa and ordinate values of the center point, xrand，yrandAs a central circle sampling point XrandA or XrandThe abscissa and ordinate values of b.
4. The method for planning a robot path based on the improved bidirectional RRT of claim 3, wherein the method comprises the following steps: the tree T in the step S3aMiddle distance point XrandA nearest child node as a nearest neighbor node
Figure FDA0002874370020000023
Is of the formula
Figure FDA0002874370020000024
Wherein the content of the first and second substances,
Figure FDA0002874370020000025
is a tree TaA child node in (c).
5. The method for planning a robot path based on the improved bidirectional RRT of claim 4, wherein the method comprises the following steps: the selection of the nearest neighbor node in said step S3 uses a diagonal distance for selection,
diagonal distance
Figure FDA0002874370020000031
Wherein xaIs a tree TaX-coordinate value, y, of neutron nodeaIs a tree TaY-coordinate value, x, of neutron nodebIs equal to TaRandom sampling point X for neutron node comparisonrandThe x-coordinate value of a, ybIs equal to TaRandom sampling point X for neutron node comparisonrand-y coordinate value of a.
6. The method for planning a robot path based on the improved bidirectional RRT of claim 5, wherein the method comprises the following steps: in the step S3
Figure FDA0002874370020000032
Towards XrandA direction is generated according to step S, and new nodes are generated
Figure FDA0002874370020000033
The formula of (a) is specifically:
Figure FDA0002874370020000034
Figure FDA0002874370020000035
wherein, s is 3,
Figure FDA0002874370020000036
is composed of
Figure FDA0002874370020000037
The x-coordinate value of (a) is,
Figure FDA0002874370020000038
is composed of
Figure FDA0002874370020000039
The y-coordinate value of (a),
Figure FDA00028743700200000310
is composed of
Figure FDA00028743700200000311
The x-coordinate value of (a) is,
Figure FDA00028743700200000312
is composed of
Figure FDA00028743700200000313
Y coordinate value of (2), xrandIs XrandThe x-coordinate value of a, yrandIs XrandA y coordinate value of a, dist is
Figure FDA00028743700200000314
And Xrand-the euclidean distance of a.
7. The method for planning a robot path based on the improved bidirectional RRT of claim 6, wherein the method comprises the following steps: the step S4 is to perform smoothing processing on the obtained path by using a six-order spline interpolation curve, give a path node and have a unique spline parameter set, so that the planned path passes through the path point to achieve the purpose of path smoothing,
the sixth-order spline interpolation curve h (x) aix6+bix5+cix4+…gi i＝0，1,2,…6，
Wherein a isi,bi,ci,…giThe undetermined coefficients are solved for the nodes that have traversed the path.
CN202011609932.8A 2020-12-30 2020-12-30 Improved bidirectional RRT robot path planning method Pending CN112650256A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011609932.8A CN112650256A (en) 2020-12-30 2020-12-30 Improved bidirectional RRT robot path planning method

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011609932.8A CN112650256A (en) 2020-12-30 2020-12-30 Improved bidirectional RRT robot path planning method

Publications (1)

Publication Number Publication Date
CN112650256A true CN112650256A (en) 2021-04-13

Family

ID=75364113

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011609932.8A Pending CN112650256A (en) 2020-12-30 2020-12-30 Improved bidirectional RRT robot path planning method

Country Status (1)

Country Link
CN (1) CN112650256A (en)

Cited By (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113124891A (en) * 2021-04-20 2021-07-16 东软睿驰汽车技术（沈阳）有限公司 Driving path planning method and related device
CN113219998A (en) * 2021-06-15 2021-08-06 合肥工业大学 Improved bidirectional-RRT-based vehicle path planning method
CN113253729A (en) * 2021-05-18 2021-08-13 中国工商银行股份有限公司 Path planning method and device for cross-region inspection robot and storage medium
CN113375686A (en) * 2021-04-26 2021-09-10 北京旷视机器人技术有限公司 Path planning method and device and intelligent conveying system
CN113448336A (en) * 2021-08-30 2021-09-28 天津施格机器人科技有限公司 3D obstacle avoidance path planning method
CN113447029A (en) * 2021-08-31 2021-09-28 湖北第二师范学院 Safe path searching method based on large satellite map
CN113721622A (en) * 2021-08-31 2021-11-30 安徽工业大学 Robot path planning method
CN113934206A (en) * 2021-07-22 2022-01-14 浙江科技学院 Mobile robot path planning method and device, computer equipment and storage medium
CN114536328A (en) * 2022-01-26 2022-05-27 中国科学院合肥物质科学研究院 Mechanical arm motion planning method based on improved RRT algorithm
CN116784975A (en) * 2023-04-26 2023-09-22 安徽医科大学 Flexible puncture needle path planning method based on improved Bi-RRT algorithm

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR101339480B1 (en) * 2012-12-14 2013-12-10 고려대학교 산학협력단 Trajectory planning method for mobile robot using dual tree structure based on rrt
CN104516356A (en) * 2015-01-08 2015-04-15 西北工业大学 Dynamic obstacle evading algorithm based on RRT
CN108195383A (en) * 2018-03-13 2018-06-22 济南大学 A kind of unmanned scraper paths planning method in underground based on improvement RRT algorithms
CN111141304A (en) * 2019-12-30 2020-05-12 福州大学 Path planning method based on concentric circle sampling and RRT guiding algorithm
CN111752306A (en) * 2020-08-14 2020-10-09 西北工业大学 Unmanned aerial vehicle route planning method based on fast-expanding random tree

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR101339480B1 (en) * 2012-12-14 2013-12-10 고려대학교 산학협력단 Trajectory planning method for mobile robot using dual tree structure based on rrt
CN104516356A (en) * 2015-01-08 2015-04-15 西北工业大学 Dynamic obstacle evading algorithm based on RRT
CN108195383A (en) * 2018-03-13 2018-06-22 济南大学 A kind of unmanned scraper paths planning method in underground based on improvement RRT algorithms
CN111141304A (en) * 2019-12-30 2020-05-12 福州大学 Path planning method based on concentric circle sampling and RRT guiding algorithm
CN111752306A (en) * 2020-08-14 2020-10-09 西北工业大学 Unmanned aerial vehicle route planning method based on fast-expanding random tree

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
M.C. BERNARDES: "Robot-assisted automatic insertion of steerable needles with closed-loop imaging feedback and intraoperative trajectory replanning", 《MECHATRONICS》 *
向金林: "基于改进双向RRT的无人艇局部路径规划算法", 《中国造船》 *
顾岩: "狭小空间虚拟人手臂装配运动规划及智能寻优", 《计算机集成制造系统》 *

Cited By (15)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113124891A (en) * 2021-04-20 2021-07-16 东软睿驰汽车技术（沈阳）有限公司 Driving path planning method and related device
CN113375686A (en) * 2021-04-26 2021-09-10 北京旷视机器人技术有限公司 Path planning method and device and intelligent conveying system
CN113253729A (en) * 2021-05-18 2021-08-13 中国工商银行股份有限公司 Path planning method and device for cross-region inspection robot and storage medium
CN113219998A (en) * 2021-06-15 2021-08-06 合肥工业大学 Improved bidirectional-RRT-based vehicle path planning method
CN113219998B (en) * 2021-06-15 2022-07-05 合肥工业大学 Improved bidirectional-RRT-based vehicle path planning method
CN113934206A (en) * 2021-07-22 2022-01-14 浙江科技学院 Mobile robot path planning method and device, computer equipment and storage medium
CN113934206B (en) * 2021-07-22 2024-01-16 浙江科技学院 Mobile robot path planning method, mobile robot path planning device, computer equipment and storage medium
CN113448336A (en) * 2021-08-30 2021-09-28 天津施格机器人科技有限公司 3D obstacle avoidance path planning method
CN113447029B (en) * 2021-08-31 2021-11-16 湖北第二师范学院 Safe path searching method based on large satellite map
CN113721622A (en) * 2021-08-31 2021-11-30 安徽工业大学 Robot path planning method
CN113447029A (en) * 2021-08-31 2021-09-28 湖北第二师范学院 Safe path searching method based on large satellite map
CN113721622B (en) * 2021-08-31 2024-02-23 安徽工业大学 Robot path planning method
CN114536328A (en) * 2022-01-26 2022-05-27 中国科学院合肥物质科学研究院 Mechanical arm motion planning method based on improved RRT algorithm
CN114536328B (en) * 2022-01-26 2024-02-06 中国科学院合肥物质科学研究院 Mechanical arm motion planning method based on improved RRT algorithm
CN116784975A (en) * 2023-04-26 2023-09-22 安徽医科大学 Flexible puncture needle path planning method based on improved Bi-RRT algorithm

Similar Documents

Publication Publication Date Title
CN112650256A (en) Improved bidirectional RRT robot path planning method
CN111141304B (en) Path planning method based on concentric circle sampling and RRT guiding algorithm
CN113219998B (en) Improved bidirectional-RRT-based vehicle path planning method
CN110962130B (en) Heuristic RRT mechanical arm motion planning method based on target deviation optimization
CN110609547B (en) Mobile robot planning method based on visual map guidance
CN106371445A (en) Unmanned vehicle planning control method based on topology map
CN114161416B (en) Robot path planning method based on potential function
CN113359775B (en) Dynamic variable sampling area RRT unmanned vehicle path planning method
CN111610786A (en) Mobile robot path planning method based on improved RRT algorithm
CN115755908B (en) JPS guided Hybrid A-based mobile robot path planning method
CN112947480B (en) Mobile robot path planning method, storage medium and system
CN113467476A (en) Non-collision detection rapid stochastic tree global path planning method considering corner constraint
CN114545921B (en) Unmanned vehicle path planning algorithm based on improved RRT algorithm
CN114995431A (en) Mobile robot path planning method for improving A-RRT
CN114442628B (en) Mobile robot path planning method, device and system based on artificial potential field method
CN113124875A (en) Path navigation method
Xue et al. Hybrid bidirectional rapidly-exploring random trees algorithm with heuristic target graviton
CN114995384A (en) Improved HHO algorithm AGV path planning fusing neural network
CN114911233A (en) Football robot path planning method based on multi-optimization rapid expansion random tree
Zekui et al. Three-dimensional path planning for unmanned aerial vehicles based on the developed RRT algorithm
CN114046791A (en) Vehicle path planning method based on layered monitoring domain and self-adaptive artificial potential field method
CN114323047A (en) Full-coverage path planning algorithm based on polygon decomposition
Shi et al. Local path planning of unmanned vehicles based on improved RRT algorithm
Xu et al. Hybrid frontier detection strategy for autonomous exploration in multi-obstacles environment
CN115237139B (en) Unmanned ship path planning method considering virtual target point

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: 20210413