CN111650941B - RRT-Connect-based improved medical mobile robot path planning method - Google Patents
RRT-Connect-based improved medical mobile robot path planning method Download PDFInfo
- Publication number
- CN111650941B CN111650941B CN202010525927.2A CN202010525927A CN111650941B CN 111650941 B CN111650941 B CN 111650941B CN 202010525927 A CN202010525927 A CN 202010525927A CN 111650941 B CN111650941 B CN 111650941B
- Authority
- CN
- China
- Prior art keywords
- random tree
- node
- random
- starting point
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 42
- 238000005070 sampling Methods 0.000 claims abstract description 61
- 230000006872 improvement Effects 0.000 claims abstract description 14
- 230000006870 function Effects 0.000 claims description 41
- 238000004590 computer program Methods 0.000 claims description 14
- 238000003860 storage Methods 0.000 claims description 6
- 238000004364 calculation method Methods 0.000 claims description 3
- 230000002194 synthesizing effect Effects 0.000 claims 1
- 238000004422 calculation algorithm Methods 0.000 abstract description 23
- 238000012952 Resampling Methods 0.000 abstract description 4
- 230000003044 adaptive effect Effects 0.000 abstract description 2
- 238000010586 diagram Methods 0.000 description 10
- 230000008569 process Effects 0.000 description 5
- 230000000694 effects Effects 0.000 description 3
- 230000009286 beneficial effect Effects 0.000 description 2
- 230000002457 bidirectional effect Effects 0.000 description 1
- 230000009977 dual effect Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000002360 preparation method Methods 0.000 description 1
- 230000002265 prevention Effects 0.000 description 1
- 238000002922 simulated annealing Methods 0.000 description 1
- 238000004659 sterilization and disinfection Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 230000009466 transformation Effects 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/0088—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots characterized by the autonomous decision making process, e.g. artificial intelligence, predefined behaviours
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Aviation & Aerospace Engineering (AREA)
- Business, Economics & Management (AREA)
- Health & Medical Sciences (AREA)
- Artificial Intelligence (AREA)
- Evolutionary Computation (AREA)
- Game Theory and Decision Science (AREA)
- Medical Informatics (AREA)
- Feedback Control In General (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Manipulator (AREA)
Abstract
The invention discloses a medical mobile robot path planning method, a system, a medium and equipment based on RRT-Connect improvement, wherein the method judges the distance between a starting point and a target point according to a distance cost function, and a second starting point and a second target point with equal distance are sampled simultaneously through a middle node sampling function between the starting point and the target point, so that an improved algorithm can grow six random trees from the starting point, the target point, the second starting point and the second target point simultaneously, and each random tree is rapidly expanded towards respective target directions. Meanwhile, an adaptive obstacle avoidance resampling principle is added into the algorithm, when the sampling point of the middle node sampling function collides with an obstacle or is inside the obstacle, the sampling of the node is abandoned, and then resampling is carried out on the central positions of the node and the adjacent nodes according to the sampling position of the middle node. The method solves the problems that the cost of path planning time of the medical mobile robot is high and the target point is difficult to find in a short time in a complex environment.
Description
Technical Field
The invention belongs to the field of robot path planning, and particularly relates to a medical mobile robot path planning method, device, medium and equipment based on RRT-Connect improvement.
Background
With the continuous progress of modern technology, the living standard of people is continuously improved, and medical mobile robots are deeply and widely researched and applied in the medical field. Medical mobile robots refer to mobile robots used for medical treatment or assisted medical treatment in hospitals, clinics. The working environment of the medical mobile robot is complex, the population is dense, and the path planning problem is always focused. How to ensure that the time cost function and the distance cost function of the path planning of the medical mobile robot in the complex environment of the medical field are optimal. Meanwhile, in the known medical environment map or the unknown medical environment map, the obstacle factors existing in the path planning process need to be solved, and the medical mobile robot can complete the path planning from the initial position to the target position, so that the target point can be reached safely and accurately.
Today, path planning algorithms are applied in many fields, and different path planning algorithms are proposed for different environment maps. The more common path planning algorithms are Dijkstra algorithm, astar algorithm, simulated annealing algorithm, artificial potential field algorithm and the like. For complex map of medical environment, conventional path planning algorithm can successfully plan a path from a starting position to a target position, but the cost is huge, and ideal effect is difficult to achieve. Meanwhile, when solving the path planning problems of a high-dimensional space and complex constraint, the optimal solution is difficult to solve, and the instantaneity and the high efficiency of the algorithm are also affected.
RRT-Connect is an algorithm for fast searching random trees in a dual tree structure, proposed in 2000 by the combination of LaValle professor and Kuffner professor at the university of tokyo, japan. The method is optimized based on an RRT (Rapid-Exploring Random Tree) algorithm, has the characteristic of good quick search, and can effectively improve the search speed and reduce the cost function of the search time. Meanwhile, the algorithm has the characteristic of bidirectional expansion, and two fast expansion random trees can be simultaneously grown from an initial position and a target position to search a state space until the two trees are connected together. The RRT-Connect algorithm with the double tree structure can greatly improve the search speed and reduce the search time, but has great cost for planning the path of a complex medical environment, and the target point is difficult to find in a short time.
Disclosure of Invention
The invention provides a medical mobile robot path planning method, a device, a medium and equipment based on RRT-Connect improvement, and aims to simultaneously sample a second starting point and a second target point with equal distance through a middle node sampling function, then grow six random trees at the starting point, the target point, the second starting point and the second target point to expand forwards until all adjacent random trees are connected together, and then select the optimal path from the starting point to the target point from all the random trees to finish path planning.
The technical scheme provided by the invention is as follows:
the medical mobile robot path planning method based on RRT-Connect improvement comprises the following steps:
step 1: setting an original starting point and an original target point of mobile robot path planning in a known environment map;
step 2: obtaining the distance between the starting point and the target point according to the distance cost function, and performing equidistant sampling in a known environment map based on the intermediate node sampling function to obtain a second starting point and a second target point;
step 3: the method comprises the steps that an original starting point and an original target point are respectively used as root nodes of a first random tree and a second random tree, the second starting point is used as a starting point of a third random tree and a fourth random tree, the second target point is used as a starting point of a fifth random tree and a sixth random tree, and node initial expansion is carried out on the first random tree to the sixth random tree in a known environment map through a random sampling function;
the first random tree and the third random tree are expansion targets of each other, the fourth random tree and the fifth random tree are expansion targets of each other, and the sixth random tree and the second random tree are expansion targets of each other; namely, the random trees expanding towards each other are adjacent random trees;
step 4: re-expanding the first to sixth random trees according to the steps 5-6 to obtain a planned path;
step 5: for each random tree, randomly selecting a sampling node Q from the environment map rand And calculates the distance between the rest nodes and the selected node in the same random tree, and finds out the nearest node Q with the selected node near ；
Step 6: from the nearest node Q of the random tree near And sampling node Q rand According to the step length distance D of the nodes, acquiring a new node Q nearest to the nearest node new Judging whether the obtained new node, the paths between the new node and the nearest node collide with the obstacle, if so, deleting the node from the random tree, returning to the step 5, otherwise, putting the new nodeAdding the path into the path of the random tree until the distance between the new node of the current random tree and all nodes of the adjacent random tree is smaller than the step length distance D of the nodes, completing the path planning of the random tree, and entering step 7;
the first random tree and the third random tree, the fourth random tree and the fifth random tree, and the sixth random tree and the second random tree are adjacent random trees;
step 7: and selecting an optimal path from the original starting point to the original target point from the path points in the six re-expanded random trees.
The distance between the starting point and the target point is judged according to the distance cost function, and a second starting point and a second target point with the same distance are sampled simultaneously through the middle node sampling function between the starting point and the target point, so that six improved random trees can grow from the starting point, the target point, the second starting point and the second target point simultaneously, and each random tree expands rapidly towards the respective target direction simultaneously. Meanwhile, an adaptive obstacle avoidance resampling principle is added, when the sampling point of the middle node sampling function collides with an obstacle or is inside the obstacle, the sampling of the node is abandoned, and then resampling is carried out at the central positions of the node and the adjacent nodes according to the sampling position of the middle node.
Further, the middle node sampling function is calculated according to the following formula:
wherein Q is init For the original starting point, Q goal For the original target point, Q init2 For the second starting point, Q goal2 Is the second target point.
Further, the distance between the nodes in the step 6 is a euclidean distance.
Further, the shortest path corresponding to the six random trees is taken as the optimal path from the original starting point to the original target point.
In another aspect, a medical mobile robot path planning system based on RRT-Connect improvement, comprises:
an initializing unit: the method comprises the steps of setting an original starting point and an original target point of mobile robot path planning in a known environment map;
a second start point and target point setting unit: the method comprises the steps of obtaining the distance between a starting point and a target point according to a distance cost function, and carrying out equidistant sampling in a known environment map based on a middle node sampling function module to obtain a second starting point and a second target point;
random tree initializing unit: the method comprises the steps that an original starting point and an original target point are used as root nodes of a first random tree and a second random tree respectively, the second starting point is used as a starting point of a third random tree and a fourth random tree, the second target point is used as a starting point of a fifth random tree and a sixth random tree, and node initial expansion is carried out on the first random tree to the sixth random tree in a known environment map through a random sampling function;
random tree expansion unit: re-expanding the first to sixth random trees according to the following modules to obtain a planned path;
the first random tree and the third random tree are expansion targets of each other, the fourth random tree and the fifth random tree are expansion targets of each other, and the sixth random tree and the second random tree are expansion targets of each other; namely, the random trees expanding towards each other are adjacent random trees;
a random node sampling module: for each random tree, randomly selecting a sampling node Q from the environment map rand And calculates the distance between the rest nodes and the selected node in the same random tree, and finds out the nearest node Q with the selected node near ；
The new node acquisition module: from the nearest node Q of the random tree near And sampling node Q rand According to the step length distance D of the nodes, acquiring a new node Q nearest to the nearest node new And judging the obtainedIf the new node is collided with the obstacle, deleting the node from the random tree, and recalling a random node sampling module, otherwise, adding the new node into the path of the random tree until the distance between the new node of the current random tree and all nodes of the adjacent random tree is smaller than the node step distance D, completing the path planning of the random tree, and entering an optimal path acquisition unit;
the first random tree and the third random tree, the fourth random tree and the fifth random tree, and the sixth random tree and the second random tree are adjacent random trees;
an optimal path acquisition unit: and selecting an optimal path from the original starting point to the original target point from the six re-expanded random trees.
Further, the middle node sampling function module calculates according to the following formula:
wherein Q is init For the original starting point, Q goal For the original target point, Q init2 For the second starting point, Q goal2 Is the second target point.
In one aspect, a computer storage medium includes a computer program that when executed by a processor implements the improved medical mobile robot path planning method based on RRT-Connect.
In yet another aspect, a medical mobile robot path planning device based on RRT-Connect improvement includes a memory, a processor, and a computer program stored on the memory and executable on the processor, wherein the processor, when executing the program, causes the medical mobile robot path planning device based on RRT-Connect to implement the medical mobile robot path planning method based on RRT-Connect improvement.
Advantageous effects
Compared with the prior art, the invention has the following advantages:
(1) The invention adopts the middle node sampling function to generate the fast expanding random tree with the six-tree structure, which can greatly reduce the time cost of path planning of the algorithm and improve the searching efficiency of path planning;
(2) Compared with the RRT-Connect algorithm with the double-tree structure, the improved algorithm iteration times and path planning time can achieve good effects, and the improved RRT-Connect algorithm has excellent path planning performance. Meanwhile, the algorithm can rapidly and efficiently plan a space for a path to smoothly pass through a complex environment in a short time.
(3) In addition, the method can be also applied to path planning in the fields of port carrying, logistics transportation, unmanned automatic driving, aerospace, military rescue, epidemic prevention and disinfection and the like.
Drawings
FIG. 1 is a schematic block diagram of a process according to an example of the invention;
FIG. 2 is an experimental map of the complex environment described in the examples of the present invention;
FIG. 3 is a schematic diagram of the setting of the starting points and target points of the six random trees in the example of the present invention;
FIG. 4 is a schematic diagram of a random tree expansion in an example of the invention;
FIG. 5 is a schematic diagram of the result of path planning using the method of the present invention.
Detailed Description
The invention will be further described with reference to the drawings and examples.
Preparation: as shown in fig. 2, the range of the environment map is 500×900, and the starting point of the mobile robot is set to Q init = (270, 81), target point Q goal = (270,819). Black squares in the figure are obstacles, and white areas are feasible areas. The starting point in the figure is positioned on the left side of the map; the target point is located on the right side of the map. When planningWhen a reliable optimal path is good, the mobile robot starts from the starting point, and reaches the target point along the planned path safely without collision.
Referring to fig. 1, the invention provides a medical mobile robot path planning method based on RRT-Connect improvement, comprising the following steps:
step one, setting a starting point Q of mobile robot path planning in a known environment map init And target point Q goal Simultaneously acquiring the position information of the obstacle in the environment map;
step two, setting the step length distance of the node of the random tree in the growing process as D, and setting the maximum iteration times of an algorithm;
specifically, the node step distance d=5 expanded by the random tree, and the maximum iteration number of the algorithm is set to 50000.
Step three, judging a starting point Q according to the distance cost function init And target point Q goal At the start point Q init And target point Q goal Simultaneously sampling second starting points Q with equal distances through a middle node sampling function init2 And a second target point Q goal2 So that the improved algorithm can simultaneously start from the starting point Q init Target point Q goal A second starting point Q init2 Second target point Q goal2 Six random trees were grown. Second starting point Q init2 And a second target point Q goal2 The calculation formula of (2) is as follows:
specifically, the second starting points Q with equal distances are sampled simultaneously by the middle node sampling function init2 And a second target point Q goal2 Which is denoted as starting point Q init To the second startPoint Q init2 Is a distance of the second start point Q init2 To the second target point Q goal2 Is the distance of the second target point Q goal2 To target point Q goal Are equal in distance.
Step four, the starting point Q init As a random tree T 1 Is the root node of target point Q goal As a random tree T 2 Is the root node of the random tree T 1 、T 2 Expansion is performed through a random sampling function, as shown in figures 3 and 4;
step five, the second starting point Q init2 As a random tree T 3 、T 4 Root node of (2), second target point Q goal2 As a random tree T 5 、T 6 Is the root node of the random tree T 3 、T 4 、T 5 、T 6 Expansion is also performed through random sampling functions, as shown in fig. 3 and 4;
random tree T 1 Is extended to random tree T 3 Random tree T 3 Is extended to random tree T 1 The expansion angle is 180 degrees, when they are connected together, the random tree T is stopped 1 Random tree T 3 Is an extension of (3). Random tree T 4 Is extended to random tree T 5 Random tree T 5 Is extended to random tree T 4 The expansion angle is 180 degrees, when they are connected together, the random tree T is stopped 4 Random tree T 5 Is an extension of (3). Random tree T 2 Is extended to random tree T 6 Random tree T 6 Is extended to random tree T 2 The expansion angle is 180 degrees, when they are connected together, the random tree T is stopped 2 Random tree T 6 Is an extension of (3).
Step six, in the environment map, the random tree T 1 Selecting a random sampling point Q by a random sampling function rand Random tree T 1 All nodes K in (1) i (i=1, 2, …) and random sample point Q rand By comparing Euclidean distance functions, a distance random sampling point Q is found rand The nearest node Q in (1) near Nearest node Q near Is calculated by the formula of (2)As follows;
step seven, in the random tree T 1 Is the nearest node Q of (2) near And random sampling point Q rand Between, obtain new node Q with step distance D new ；
Step eight, judging the new node Q new Whether collision with an obstacle of the environment map occurs, and if collision occurs, discarding the new node Q new After going to the sixth and seventh steps, the new node Q is obtained again new If no collision occurs, go to step nine.
Step nine, judging the new node Q new With the nearest node Q near Whether the connected path collides with an obstacle, and if so, the new node Q is discarded new After going to the steps six, seven and eight, the new node Q is acquired again new If no collision occurs, the new node Q new To the path of the random tree.
Step ten, the random tree T 2 、T 3 、T 4 、T 5 、T 6 Replacing the random tree T in the step six and seven 1 Repeating the steps of six, seven, eight and nine and randomly setting a tree T 1 、T 2 、T 3 、T 4 、T 5 、T 6 In the expanding process, a new node Q of the current random tree needs to be calculated in real time new The Euclidean distance between the random tree and all nodes of the adjacent random tree is smaller than the step length D, the random tree is connected with the adjacent random tree, and similarly, when all the random trees T 1 、T 2 、T 3 、T 4 、T 5 、T 6 When the distance of the path is smaller than the step length D, all adjacent random trees are connected, and path planning is completed;
referring to FIG. 5, step eleven, comprehensive random tree T 1 、T 2 、T 3 、T 4 、T 5 、T 6 Planning all path points to obtain the following path pointsStarting point Q init To target point Q goal Is the optimal path S of (1) 1 (Q init )，S 2 ，…，S i (Q init2 )，…，S j (Q goal2 )，…，S n (Q goal )。
Based on the above method, the embodiment of the invention also provides a medical mobile robot path planning system based on RRT-Connect improvement, comprising:
an initializing unit: the method comprises the steps of setting an original starting point and an original target point of mobile robot path planning in a known environment map;
a second start point and target point setting unit: the method comprises the steps of obtaining the distance between a starting point and a target point according to a distance cost function, and carrying out equidistant sampling in a known environment map based on a middle node sampling function module to obtain a second starting point and a second target point;
random tree initializing unit: the method comprises the steps that an original starting point and an original target point are used as root nodes of a first random tree and a second random tree respectively, the second starting point is used as a starting point of a third random tree and a fourth random tree, the second target point is used as a starting point of a fifth random tree and a sixth random tree, and node initial expansion is carried out on the first random tree to the sixth random tree in a known environment map through a random sampling function;
random tree expansion unit: re-expanding the first to sixth random trees according to the following modules to obtain a planned path;
a random node sampling module: for each random tree, randomly selecting a sampling node Q from the environment map rand And calculates the distance between the rest nodes and the selected node in the same random tree, and finds out the nearest node Q with the selected node near ；
The new node acquisition module: from the nearest node Q of the random tree near And sampling node Q rand According to the step length distance D of the nodes, acquiring a new node Q nearest to the nearest node new And judges whether the acquired new node, the path between the new node and the nearest node collides with the obstacle, if so,deleting the node from the random tree and recalling a random node sampling module, otherwise, adding a new node into the path of the random tree until the distance between the new node of the current random tree and all nodes of the adjacent random tree is smaller than the node step distance D, completing the path planning of the random tree, and entering an optimal path acquisition unit;
the first random tree and the third random tree, the fourth random tree and the fifth random tree, and the sixth random tree and the second random tree are adjacent random trees;
an optimal path acquisition unit: and selecting an optimal path from the original starting point to the original target point from the six re-expanded random trees.
The middle node sampling function module is calculated according to the following formula:
wherein Q is init For the original starting point, Q goal For the original target point, Q init2 For the second starting point, Q goal2 Is the second target point.
It should be understood that the functional unit modules in the embodiments of the present invention may be centralized in one processing unit, or each unit module may exist alone physically, or two or more unit modules may be integrated into one unit module, and may be implemented in hardware or software.
The embodiment of the invention also provides a computer storage medium, which comprises a computer program, wherein the program realizes the medical mobile robot path planning method based on RRT-Connect improvement when being executed by a processor, and the beneficial effects of the method are referred to as beneficial effects of the method part and are not repeated herein.
The embodiment of the invention also provides medical mobile robot path planning equipment based on the RRT-Connect, which comprises a memory, a processor and a computer program stored on the memory and capable of running on the processor, and is characterized in that when the processor executes the program, the medical mobile robot path planning equipment based on the RRT-Connect realizes the medical mobile robot path planning method based on the RRT-Connect.
It will be appreciated by those skilled in the art that embodiments of the present application may be provided as a method, system, or computer program product. Accordingly, the present application may take the form of an entirely hardware embodiment, an entirely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, the present application may take the form of a computer program product embodied on one or more computer-usable storage media (including, but not limited to, disk storage, CD-ROM, optical storage, and the like) having computer-usable program code embodied therein.
The present application is described with reference to flowchart illustrations and/or block diagrams of methods, apparatus (systems) and computer program products according to embodiments of the application. It will be understood that each flow and/or block of the flowchart illustrations and/or block diagrams, and combinations of flows and/or blocks in the flowchart illustrations and/or block diagrams, can be implemented by computer program instructions. These computer program instructions may be provided to a processor of a general purpose computer, special purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, create means for implementing the functions specified in the flowchart flow or flows and/or block diagram block or blocks.
These computer program instructions may also be stored in a computer-readable memory that can direct a computer or other programmable data processing apparatus to function in a particular manner, such that the instructions stored in the computer-readable memory produce an article of manufacture including instruction means which implement the function specified in the flowchart flow or flows and/or block diagram block or blocks.
These computer program instructions may also be loaded onto a computer or other programmable data processing apparatus to cause a series of operational steps to be performed on the computer or other programmable apparatus to produce a computer implemented process such that the instructions which execute on the computer or other programmable apparatus provide steps for implementing the functions specified in the flowchart flow or flows and/or block diagram block or blocks.
Finally, it should be noted that: the above embodiments are only for illustrating the technical solution of the present invention and not for limiting the same, and although the present invention has been described in detail with reference to the above embodiments, it should be understood by those skilled in the art that the above embodiments are merely illustrative of the exemplary implementation of the present invention, and the details in the embodiments do not limit the scope of the present invention, any obvious changes based on the equivalent transformation, simple substitution, etc. of the technical solution of the present invention fall within the scope of the present invention without departing from the spirit and scope of the present invention.
Claims (6)
1. The medical mobile robot path planning method based on RRT-Connect improvement is characterized by comprising the following steps:
step 1: setting an original starting point and an original target point of mobile robot path planning in a known environment map;
step 2: obtaining the distance between the starting point and the target point according to the distance cost function, and performing equidistant sampling in a known environment map based on the intermediate node sampling function to obtain a second starting point and a second target point;
step 3: the method comprises the steps that an original starting point and an original target point are respectively used as root nodes of a first random tree and a second random tree, the second starting point is used as a starting point of a third random tree and a fourth random tree, the second target point is used as a starting point of a fifth random tree and a sixth random tree, and node initial expansion is carried out on the first random tree to the sixth random tree in a known environment map through a random sampling function;
the first random tree and the third random tree are expansion targets of each other, the fourth random tree and the fifth random tree are expansion targets of each other, and the sixth random tree and the second random tree are expansion targets of each other;
step 4: re-expanding the first to sixth random trees according to the steps 5-6 to obtain a planned path;
step 5: for each random tree, randomly selecting a sampling node Q from the environment map rand And calculates the distance between the rest nodes and the selected node in the same random tree, and finds out the nearest node Q with the selected node near ；
Step 6: from the nearest node Q of the random tree near And sampling node Q rand According to the step length distance D of the nodes, acquiring a new node Q nearest to the nearest node new Judging whether the obtained new node, the paths between the new node and the nearest node collide with the obstacle or not, deleting the node from the random tree if the collision happens, returning to the step 5, otherwise, adding the new node into the path of the random tree until the distance between the new node of the current random tree and all the nodes of the adjacent random tree is smaller than the node step distance D, completing the path planning of the random tree, and entering the step 7;
step 7: selecting an optimal path from an original starting point to an original target point from path points in the six re-expanded random trees;
the middle node sampling function is calculated according to the following formula:
wherein Q is init For the original starting point, Q goal For the original target point, Q init2 For the second starting point, Q goal2 Is a second target point;
nearest node Q near The calculation formula of (2) is shown as follows；
Wherein Q is rand Representing a random tree T 1 Selecting a random sampling point by a random sampling function, K i Representing a random tree T 1 In (a), n represents a random tree T 1 Number of intermediate nodes.
2. The method according to claim 1, wherein the distance between the nodes in step 6 is a euclidean distance.
3. The method according to claim 1, wherein the optimal path from the original starting point to the original target point is defined by the shortest path corresponding to the six random trees.
4. An RRT-Connect based improved medical mobile robot path planning system, comprising:
an initializing unit: the method comprises the steps of setting an original starting point and an original target point of mobile robot path planning in a known environment map;
a second start point and target point setting unit: the method comprises the steps of obtaining the distance between a starting point and a target point according to a distance cost function, and carrying out equidistant sampling in a known environment map based on a middle node sampling function module to obtain a second starting point and a second target point;
random tree initializing unit: the method comprises the steps that an original starting point and an original target point are used as root nodes of a first random tree and a second random tree respectively, the second starting point is used as a starting point of a third random tree and a fourth random tree, the second target point is used as a starting point of a fifth random tree and a sixth random tree, and node initial expansion is carried out on the first random tree to the sixth random tree in a known environment map through a random sampling function;
the first random tree and the third random tree are expansion targets of each other, the fourth random tree and the fifth random tree are expansion targets of each other, and the sixth random tree and the second random tree are expansion targets of each other;
random tree expansion unit: re-expanding the first to sixth random trees according to the following modules to obtain a planned path;
a random node sampling module: for each random tree, randomly selecting a sampling node Q from the environment map rand And calculates the distance between the rest nodes and the selected node in the same random tree, and finds out the nearest node Q with the selected node near ；
The new node acquisition module: from the nearest node Q of the random tree near And sampling node Q rand According to the step length distance D of the nodes, acquiring a new node Q nearest to the nearest node new Judging whether the obtained new node, the paths between the new node and the nearest node collide with an obstacle, if so, deleting the node from the random tree, and recalling a random node sampling module, otherwise, adding the new node into the path of the random tree until the distances between the new node of the current random tree and all the nodes of the adjacent random tree are smaller than the node step distance D, completing path planning of the random tree, and entering an optimal path acquisition unit;
an optimal path acquisition unit: synthesizing the first to six planned path points of the random tree from the six re-expanded random trees to obtain an optimal path from the original starting point to the original target point;
the middle node sampling function module is calculated according to the following formula:
wherein Q is init For the original starting point, Q goal For the original target point, Q init2 For the second starting point, Q goal2 Is a second target point;
nearest node Q near The calculation formula of (2) is shown below;
wherein Q is rand Representing a random tree T 1 Selecting a random sampling point by a random sampling function, K i Representing a random tree T 1 In (a), n represents a random tree T 1 Number of intermediate nodes.
5. A computer storage medium comprising a computer program, characterized in that the computer program, when executed by a processor, implements a medical mobile robot path planning method based on RRT-Connect improvement as claimed in any one of claims 1-3.
6. A medical mobile robot path planning apparatus based on RRT-Connect improvement, comprising a memory, a processor and a computer program stored on the memory and executable on the processor, wherein the processor, when executing the program, causes the medical mobile robot path planning apparatus based on RRT-Connect to implement a medical mobile robot path planning method based on RRT-Connect improvement as claimed in any one of claims 1-3.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010525927.2A CN111650941B (en) | 2020-06-10 | 2020-06-10 | RRT-Connect-based improved medical mobile robot path planning method |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010525927.2A CN111650941B (en) | 2020-06-10 | 2020-06-10 | RRT-Connect-based improved medical mobile robot path planning method |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN111650941A CN111650941A (en) | 2020-09-11 |
| CN111650941B true CN111650941B (en) | 2023-05-02 |
Family
ID=72347556
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202010525927.2A Active CN111650941B (en) | 2020-06-10 | 2020-06-10 | RRT-Connect-based improved medical mobile robot path planning method |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN111650941B (en) |
Families Citing this family (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113804196B (en) * | 2020-09-17 | 2024-04-12 | 北京京东乾石科技有限公司 | Unmanned vehicle path planning method and related equipment |
| CN112393739B (en) * | 2020-10-29 | 2022-07-29 | 国网安徽省电力有限公司检修分公司 | Improved rapid search random tree path planning method in large-area environment |
| CN112711256B (en) * | 2020-12-23 | 2022-05-20 | 杭州电子科技大学 | Automatic generation method for target search observation points of mobile robot |
| CN113064426B (en) * | 2021-03-17 | 2022-03-15 | 安徽工程大学 | Intelligent vehicle path planning method for improving bidirectional fast search random tree algorithm |
| CN113204238B (en) * | 2021-04-22 | 2023-09-12 | 武汉理工大学 | Path planning method, equipment and storage medium for mobile robot |
| CN113341984A (en) * | 2021-06-15 | 2021-09-03 | 桂林电子科技大学 | Robot path planning method and device based on improved RRT algorithm |
| CN113671969B (en) * | 2021-08-25 | 2024-04-05 | 齐鲁工业大学 | Path planning method, system, medium and storage device in two-dimensional static environment |
| CN114153210B (en) * | 2021-12-01 | 2024-03-19 | 苏州盈科电子有限公司 | Movement control method and system of robot |
| CN114700937B (en) * | 2022-01-13 | 2024-02-13 | 深圳市越疆科技有限公司 | Mechanical arm, motion path planning method thereof, control system, medium and robot |
| CN115031739A (en) * | 2022-08-12 | 2022-09-09 | 中国科学院自动化研究所 | Continuum robot path planning method and device, electronic equipment and storage medium |
Family Cites Families (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR101667030B1 (en) * | 2009-08-10 | 2016-10-17 | 삼성전자 주식회사 | Path planning apparatus of robot and method thereof |
| KR101667029B1 (en) * | 2009-08-10 | 2016-10-17 | 삼성전자 주식회사 | Method and apparatus of path planing for a robot |
| KR102165437B1 (en) * | 2014-05-02 | 2020-10-14 | 한화디펜스 주식회사 | Path planning apparatus of mobile robot |
| CN108681787B (en) * | 2018-04-28 | 2021-11-16 | 南京航空航天大学 | Unmanned aerial vehicle path optimization method based on improved bidirectional fast expansion random tree algorithm |
| CN110497403A (en) * | 2019-08-05 | 2019-11-26 | 上海大学 | A kind of manipulator motion planning method improving two-way RRT algorithm |
| CN111141304B (en) * | 2019-12-30 | 2021-11-02 | 福州大学 | Path planning method based on concentric circle sampling and RRT guiding algorithm |
-
2020
- 2020-06-10 CN CN202010525927.2A patent/CN111650941B/en active Active
Also Published As
| Publication number | Publication date |
|---|---|
| CN111650941A (en) | 2020-09-11 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111650941B (en) | RRT-Connect-based improved medical mobile robot path planning method | |
| CN110228069B (en) | Online obstacle avoidance motion planning method for mechanical arm | |
| CN113219998B (en) | Improved bidirectional-RRT-based vehicle path planning method | |
| CN111546347B (en) | Mechanical arm path planning method suitable for dynamic environment | |
| CN108444489A (en) | A kind of paths planning method improving RRT algorithms | |
| US8892253B2 (en) | Swarm robot and sweeping method using swarm robot | |
| CN113296496B (en) | Gravity self-adaptive step length bidirectional RRT path planning method based on multiple sampling points | |
| US20080306628A1 (en) | Multi-Modal Push Planner for Humanoid Robots | |
| CN111761582B (en) | Mobile mechanical arm obstacle avoidance planning method based on random sampling | |
| CN112987799B (en) | Unmanned aerial vehicle path planning method based on improved RRT algorithm | |
| CN112549016A (en) | Mechanical arm motion planning method | |
| CN111610786A (en) | Mobile robot path planning method based on improved RRT algorithm | |
| CN113359746A (en) | Path planning method and device based on improved bidirectional RRT and Dijkstra fusion algorithm | |
| Raja et al. | Path planning for mobile robots in dynamic environments using particle swarm optimization | |
| CN112828889A (en) | Six-axis cooperative mechanical arm path planning method and system | |
| CN110705803B (en) | Route planning method based on triangle inner center guide RRT algorithm | |
| Dirik et al. | Rrt-dijkstra: An improved path planning algorithm for mobile robots | |
| Wang et al. | Learning of long-horizon sparse-reward robotic manipulator tasks with base controllers | |
| Lai et al. | Path planning and obstacle avoidance approaches for robot arm | |
| Mahadev et al. | Collecting a swarm in a grid environment using shared, global inputs | |
| Zang et al. | Path planning based on Bi-RRT algorithm for redundant manipulator | |
| Chi et al. | Risk-Informed-RRT*: A sampling-based human-friendly motion planning algorithm for mobile service robots in indoor environments | |
| Nasir | Adaptive rapidly-exploring-random-tree-star (RRT*)-smart: algorithm characteristics and behavior analysis in complex environments | |
| Jiang et al. | Learning humanoid reaching tasks in dynamic environments | |
| CN116476058A (en) | Remote puncture robot path planning method 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 | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |