CN111650941A - Medical mobile robot path planning method, device, medium and equipment based on RRT-Connect improvement - Google Patents
Medical mobile robot path planning method, device, medium and equipment based on RRT-Connect improvement Download PDFInfo
- Publication number
- CN111650941A CN111650941A CN202010525927.2A CN202010525927A CN111650941A CN 111650941 A CN111650941 A CN 111650941A CN 202010525927 A CN202010525927 A CN 202010525927A CN 111650941 A CN111650941 A CN 111650941A
- Authority
- CN
- China
- Prior art keywords
- random tree
- node
- random
- target point
- starting point
- 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.)
- Granted
Links
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- G05D1/0088—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot 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 or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- 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 an RRT-Connect-based improved medical mobile robot path planning method, a system, a medium and equipment, wherein the method judges the distance between a starting point and a target point according to a distance cost function, and simultaneously samples a second starting point and a second target point with equal distances between the starting point and the target point through a middle node sampling function, so that an improved algorithm can simultaneously grow six random trees from the starting point, the target point, the second starting point and the second target point, and each random tree is rapidly expanded towards the respective target direction. Meanwhile, a self-adaptive obstacle avoidance resampling principle is added into the algorithm, when the sampling point of the middle node sampling function collides with the obstacle or is in the obstacle, the sampling of the node is abandoned, and then resampling is carried out on the central position of the node and the adjacent node according to the sampling position of the middle node. The problem that the medical mobile robot is difficult to find a target point in a short time due to the fact that the path planning time cost is high in a complex environment is solved.
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 science and technology and the continuous improvement of the living standard of people, the medical mobile robot is deeply and widely researched and applied in the medical field. The medical mobile robot is a mobile robot used for medical treatment or auxiliary medical treatment in hospitals and clinics. The medical mobile robot has a complex working environment and a dense population, and the path planning problem is always concerned. How to ensure that a time cost function and a distance cost function of path planning of the medical mobile robot are optimal in a complex environment in the medical field. Meanwhile, in a known medical environment map or an unknown medical environment map, the medical mobile robot can complete path planning from an initial position to a target position only by solving the obstacle factor existing in the path planning process, so that the medical mobile robot can safely and accurately reach the target point.
Nowadays, path planning algorithms are applied in many fields, and different path planning algorithms are proposed for different environment maps. Common path planning algorithms include Dijkstra algorithm, Astar algorithm, simulated annealing algorithm, artificial potential field algorithm, and the like. For a complex map of a medical environment, although a traditional path planning algorithm can successfully plan a path from a starting position to a target position, the cost of the traditional path planning algorithm is huge, and an ideal effect is difficult to achieve. Meanwhile, when the path planning problem of high-dimensional space and complex constraint is solved, the optimal solution is difficult to solve, and the real-time performance and the high efficiency of the algorithm are influenced.
RRT-Connect is an algorithm for fast search of random trees with a double-tree structure, and was proposed in 2000 by the combination of professor LaValle and professor Kuffner of university of Tokyo, Japan. The method is optimized on the basis of an RRT (Rapid-expanding 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 can grow two fast expansion random trees from the initial position and the target position simultaneously to search a state space until the two trees are connected together. Although the RRT-Connect algorithm with the double-tree structure can greatly improve the search speed and reduce the search time, the route planning time cost of the RRT-Connect algorithm is high for a complex medical environment, and a target point is difficult to find in a short time.
Disclosure of Invention
The invention provides an RRT-Connect-based improved medical mobile robot path planning method, device, medium and equipment, which aims to sample a second starting point and a second target point which are equal in distance at the same time through a middle node sampling function, then grow six random trees in 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 an optimal path from the starting point to the target point from all the random trees to complete path planning.
The technical scheme provided by the invention is as follows:
a 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 path planning of the mobile robot 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 carrying out equidistant sampling in a known environment map based on the middle node sampling function to obtain a second starting point and a second target point;
and step 3: respectively taking an original starting point and an original target point as root nodes of a first random tree and a second random tree, taking the second starting point as starting points of a third random tree and a fourth random tree, taking a second target point as starting points of a fifth random tree and a sixth random tree, and performing initial node expansion 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 mutual expansion targets, the fourth random tree and the fifth random tree are mutual expansion targets, and the sixth random tree and the second random tree are mutual expansion targets; namely, the random trees expanding towards each other are adjacent random trees;
and 4, step 4: re-expanding the first random tree to the sixth random tree according to the steps 5 to 6 to obtain a planned path;
and 5: for each random tree, randomly selecting a sampling node Q from the environment maprandAnd calculating the distances between the other nodes in the same random tree and the selected node to find out the nearest node Q to the selected nodenear；
Step 6: nearest node Q from the random treenearAnd sampling node QrandAccording to the step length distance D of the node, obtaining a new node Q closest to the nearest nodenewJudging whether the acquired new node and the path between the new node and the nearest node collide with the barrier, if so, deleting the node from the random tree, and returning to the step 5, 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 length distance D, finishing the path planning of the random tree, and entering the 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;
and 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, a second starting point and a second target point which are equal in distance are simultaneously sampled through the middle node sampling function between the starting point and the target point, so that six random trees can be simultaneously grown from the starting point, the target point, the second starting point and the second target point after improvement, and each random tree is rapidly expanded towards the respective target direction. Meanwhile, a self-adaptive obstacle avoidance resampling principle is added, when the sampling point of the middle node sampling function collides with the obstacle or is in the obstacle, the sampling of the node is abandoned, and then resampling is carried out on the central position of the node and the adjacent node according to the sampling position of the middle node.
Further, the middle node sampling function is calculated according to the following formula:
wherein Q isinitIs an original starting point, QgoalIs an original target point, Qinit2Is the second starting point, Qgoal2Is the second target point.
Further, the distance between the nodes in the step 6 is a euclidean distance.
Further, the corresponding shortest path in the six random trees is used as the optimal path from the original starting point to the original target point.
In another aspect, a RRT-Connect based improved medical mobile robot path planning system includes:
an initialization unit: the system comprises a mobile robot path planning system, a mobile robot path planning system and a mobile robot path planning system, wherein the mobile robot path planning system is used for setting an original starting point and an original target point of the mobile robot path planning system in a known environment map;
a second start point and target point setting unit: the system comprises a distance cost function module, a middle node sampling function module and a first starting point and a second target point, wherein the distance cost function module is used for obtaining the distance between the starting point and the target point and carrying out equidistant sampling in a known environment map on the basis of the middle node sampling function module to obtain a second starting point and a second target point;
a random tree initialization unit: the system comprises a first random tree, a second random tree, a third random tree, a fourth random tree, a fifth random tree and a sixth random tree, wherein the first random tree and the sixth random tree are used for carrying out initial node expansion in a known environment map through a random sampling function;
a random tree expansion unit: re-expanding the first random tree to the sixth random tree according to the following modules to obtain a planned path;
the first random tree and the third random tree are mutual expansion targets, the fourth random tree and the fifth random tree are mutual expansion targets, and the sixth random tree and the second random tree are mutual expansion targets; 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 maprandAnd calculating the distances between the other nodes in the same random tree and the selected node to find out the nearest node Q to the selected nodenear；
A new node acquisition module: nearest node Q from the random treenearAnd sampling node QrandAccording to the step length distance D of the node, obtaining a new node Q closest to the nearest nodenewJudging whether the acquired new node and the path between the new node and the nearest node collide with the barrier, if so, deleting the node from the random tree and recalling the 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 length distance D, completing 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 is calculated according to the following formula:
wherein Q isinitIs an original starting point, QgoalIs an original target point, Qinit2Is the second starting point, Qgoal2Is the second target point.
In one aspect, a computer storage medium includes a computer program, which when executed by a processor implements a RRT-Connect based improved medical mobile robot path planning method.
In still another aspect, a medical mobile robot path planning apparatus based on RRT-Connect improvement includes a memory, a processor, and a computer program stored in the memory and executable on the processor, where the processor executes the program to make the medical mobile robot path planning apparatus based on RRT-Connect improvement realize the medical mobile robot path planning method based on RRT-Connect improvement.
Advantageous effects
Compared with the prior art, the invention mainly has the following advantages:
(1) according to the method, the middle-node sampling function is adopted to generate the fast-expanding random tree with the six-tree structure, so that the time cost of path planning of the algorithm can be greatly reduced, and the searching efficiency of path planning is improved;
(2) compared with the RRT-Connect algorithm with a double-tree structure, the improved algorithm has good effects on the iteration times and the path planning time, and has excellent path planning performance. Meanwhile, the algorithm can rapidly and efficiently plan a path to smoothly pass through the space of a complex environment in a short time.
(3) Besides, the invention can also be applied to path planning in the fields of port transportation, logistics transportation, unmanned automatic driving, aerospace, military rescue, epidemic prevention and disinfection and the like.
Drawings
FIG. 1 is a schematic block flow diagram of a process according to an embodiment of the present invention;
FIG. 2 is an experimental map of the complex environment in an example of the present invention;
FIG. 3 is a schematic diagram illustrating the arrangement of the starting points and the target points of the six random trees according to the embodiment of the present invention;
FIG. 4 is a diagram illustrating a development of a random tree according to an embodiment of the present invention;
fig. 5 is a diagram illustrating a result of path planning using the method according to the embodiment of the present invention.
Detailed Description
The invention will be further described with reference to the following figures and examples.
Preparation work 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 Qinit(270, 81) target point is Qgoal= (270,819). The black squares in the figure are obstacles and the white areas are feasible areas. The starting point in the map is located on the left side in the map; the target point is located on the right side of the map. When a reliable optimal path is planned, the mobile robot starts from the starting point and safely reaches the target point without collision along the planned path.
Referring to fig. 1, the present invention provides an improved medical mobile robot path planning method based on RRT-Connect, including the following steps:
step one, setting a starting point Q for path planning of the mobile robot in a known environment mapinitAnd target point QgoalSimultaneously acquiring position information of obstacles in the environment map;
step two, setting the step length distance of the nodes of the random tree in the growing process as D, and simultaneously setting the maximum iteration times of the algorithm;
specifically, the step length distance D of the expanded nodes of the random tree is 5, and the maximum iteration number of the algorithm is set to 50000.
Step three, judging a starting point Q according to a distance cost functioninitAnd target point QgoalAt a starting point QinitAnd target point QgoalSimultaneously sampling by a middle node sampling functionTo a second starting point Q with equal distanceinit2And a second target point Qgoal2So that the improved algorithm can simultaneously start from the starting point QinitTarget point QgoalA second starting point Qinit2A second target point Qgoal2Six random trees were grown. Second starting point Qinit2And a second target point Qgoal2The calculation formula of (a) is as follows:
specifically, the second starting point Q with equal distance is simultaneously sampled by the middle node sampling functioninit2And a second target point Qgoal2Expressed as a starting point QinitTo a second starting point Qinit2A second starting point Qinit2To a second target point Qgoal2A second target point Qgoal2To the target point QgoalAre all equal.
Step four, starting point QinitAs a random tree T1Root node of, target point QgoalAs a random tree T2Root node of, random tree T1、T2Expanding through a random sampling function, as shown in fig. 3 and 4;
step five, setting the second starting point Qinit2As a random tree T3、T4Root node of, the second target point Qgoal2As a random tree T5、T6Root node of, random tree T3、T4、T5、T6Also carrying out expansion through a random sampling function, as shown in FIGS. 3 and 4;
random tree T1The development goal of is a random tree T3Random tree T3The development goal of is a random tree T1The expansion angle is 180 degrees, and when they are connected together, the random tree T is stopped1And a random tree T3The expansion of (1). Random tree T4The development goal of is a random tree T5Random tree T5The development goal of is a random tree T4The expansion angle is 180 degrees, and when they are connected together, the random tree T is stopped4And a random tree T5The expansion of (1). Random tree T2The development goal of is a random tree T6Random tree T6The development goal of is a random tree T2The expansion angle is 180 degrees, and when they are connected together, the random tree T is stopped2And a random tree T6The expansion of (1).
Step six, in the environment map, a random tree T1Selecting a random sampling point Q by a random sampling functionrandRandom tree T1All nodes K ini(i ═ 1,2, …) and random sampling points QrandComparing by Euclidean distance function to find random distance sampling point QrandNearest node Q in (1)nearNearest node QnearThe calculation formula of (a) is as follows;
step seven, in the random tree T1Nearest node Q ofnearAnd random sampling point QrandGet the new node Q with the step distance Dnew；
Step eight, judging a new node QnewWhether collision occurs with an obstacle of the environment map, and if collision occurs, discarding the new node QnewAfter the step six and the step seven are carried out, a new node Q is obtained againnewAnd if no collision occurs, the step goes to the step nine.
Ninthly, judging a new node QnewAnd nearest node QnearWhether the connected path collides with an obstacle, and if so, the new node Q is discardednewAfter the step six, the step seven and the step eight are carried out, new nodes Q are obtained againnewIf no collision occurs, the new node Q is connectednewAdd to the path of the random tree.
Step tenWill random tree T2、T3、T4、T5、T6Replacing the random tree T in the sixth and seventh steps1Repeating the steps six, seven, eight and nine to obtain the random tree T1、T2、T3、T4、T5、T6In the process of expansion, a new node Q of the current random tree needs to be calculated in real timenewThe Euclidean distance from all nodes of the adjacent random tree, when the Euclidean distance is less than the step length D, the random tree is connected with the adjacent random tree, and similarly, when all the random trees T are connected1、T2、T3、T4、T5、T6When the distance is less than the step length D, all the adjacent random trees are connected, and the path planning is finished;
please refer to FIG. 5, step eleven, synthesize the random tree T1、T2、T3、T4、T5、T6Planning all path points to obtain the path points from the starting point QinitTo the target point QgoalIs optimized path S1(Qinit)，S2，…，Si(Qinit2)，…，Sj(Qgoal2)，…，Sn(Qgoal)。
Based on the method, the embodiment of the invention also provides a medical mobile robot path planning system improved based on RRT-Connect, which comprises:
an initialization unit: the system comprises a mobile robot path planning system, a mobile robot path planning system and a mobile robot path planning system, wherein the mobile robot path planning system is used for setting an original starting point and an original target point of the mobile robot path planning system in a known environment map;
a second start point and target point setting unit: the system comprises a distance cost function module, a middle node sampling function module and a first starting point and a second target point, wherein the distance cost function module is used for obtaining the distance between the starting point and the target point and carrying out equidistant sampling in a known environment map on the basis of the middle node sampling function module to obtain a second starting point and a second target point;
a random tree initialization unit: the system comprises a first random tree, a second random tree, a third random tree, a fourth random tree, a fifth random tree and a sixth random tree, wherein the first random tree and the sixth random tree are used for carrying out initial node expansion in a known environment map through a random sampling function;
a random tree expansion unit: re-expanding the first random tree to the sixth random tree 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 maprandAnd calculating the distances between the other nodes in the same random tree and the selected node to find out the nearest node Q to the selected nodenear；
A new node acquisition module: nearest node Q from the random treenearAnd sampling node QrandAccording to the step length distance D of the node, obtaining a new node Q closest to the nearest nodenewJudging whether the acquired new node and the path between the new node and the nearest node collide with the barrier, if so, deleting the node from the random tree and recalling the 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 length distance D, completing 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 the content of the first and second substances,Qinitis an original starting point, QgoalIs an original target point, Qinit2Is the second starting point, Qgoal2Is the second target point.
It should be understood that the functional unit modules in the embodiments of the present invention may be integrated into one processing unit, or each unit module may exist alone physically, or two or more unit modules are integrated into one unit module, and may be implemented in the form of hardware or software.
The embodiment of the present invention further provides a computer storage medium, which includes a computer program, and when the program is executed by a processor, the method for planning a route of a medical mobile robot based on RRT-Connect improvement is implemented.
The embodiment of the invention also provides medical mobile robot path planning equipment based on RRT-Connect improvement, which comprises a memory, a processor and a computer program which is stored on the memory and can run on the processor, and is characterized in that when the processor executes the program, the medical mobile robot path planning equipment based on RRT-Connect improvement realizes the medical mobile robot path planning method based on RRT-Connect improvement.
As will be appreciated by one skilled in the art, 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 flow diagrams and/or block diagrams, and combinations of flows and/or blocks in the flow diagrams 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: 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 implementations of the present invention, and the details of the embodiments are not to be construed as limiting the scope of the present invention, and any obvious changes, such as equivalent alterations, simple substitutions, etc., based on the technical solutions of the present invention may be made without departing from the spirit and scope of the present invention.
Claims (8)
1. A 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 path planning of the mobile robot 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 carrying out equidistant sampling in a known environment map based on the middle node sampling function to obtain a second starting point and a second target point;
and step 3: respectively taking an original starting point and an original target point as root nodes of a first random tree and a second random tree, taking the second starting point as starting points of a third random tree and a fourth random tree, taking a second target point as starting points of a fifth random tree and a sixth random tree, and performing initial node expansion 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 mutual expansion targets, the fourth random tree and the fifth random tree are mutual expansion targets, and the sixth random tree and the second random tree are mutual expansion targets;
and 4, step 4: re-expanding the first random tree to the sixth random tree according to the steps 5 to 6 to obtain a planned path;
and 5: for each random tree, randomly selecting a sampling node Q from the environment maprandAnd calculating the distances between the other nodes in the same random tree and the selected node to find out the nearest node Q to the selected nodenear；
Step 6: nearest node Q from the random treenearAnd sampling node QrandAccording to the step length distance D of the node, obtaining a new node Q closest to the nearest nodenewJudging whether the acquired new node and the path between the new node and the nearest node collide with the barrier, if so, deleting the node from the random tree, and returning to the step 5, 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 length distance D, finishing the path planning of the random tree, and entering the step 7;
and 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.
2. The method of claim 1, wherein the mid-node sampling function is calculated according to the following formula:
wherein Q isinitIs an original starting point, QgoalIs an original target point, Qinit2Is the second starting point, Qgoal2Is the second target point.
3. The method of claim 1, wherein the distance between nodes in step 6 is a Euclidean distance.
4. The method according to claim 1, wherein the corresponding shortest path in the six random trees is used as the optimal path from the original starting point to the original target point.
5. A medical mobile robot path planning system based on RRT-Connect improvement is characterized by comprising:
an initialization unit: the system comprises a mobile robot path planning system, a mobile robot path planning system and a mobile robot path planning system, wherein the mobile robot path planning system is used for setting an original starting point and an original target point of the mobile robot path planning system in a known environment map;
a second start point and target point setting unit: the system comprises a distance cost function module, a middle node sampling function module and a first starting point and a second target point, wherein the distance cost function module is used for obtaining the distance between the starting point and the target point and carrying out equidistant sampling in a known environment map on the basis of the middle node sampling function module to obtain a second starting point and a second target point;
a random tree initialization unit: the system comprises a first random tree, a second random tree, a third random tree, a fourth random tree, a fifth random tree and a sixth random tree, wherein the first random tree and the sixth random tree are used for carrying out initial node expansion in a known environment map through a random sampling function;
the first random tree and the third random tree are mutual expansion targets, the fourth random tree and the fifth random tree are mutual expansion targets, and the sixth random tree and the second random tree are mutual expansion targets;
a random tree expansion unit: re-expanding the first random tree to the sixth random tree 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 maprandAnd calculating the distances between the other nodes in the same random tree and the selected node to find out the nearest node Q to the selected nodenear；
A new node acquisition module: nearest node Q from the random treenearAnd sampling node QrandAccording to the step length distance D of the node, obtaining a new node Q closest to the nearest nodenewJudging whether the acquired new node and the path between the new node and the nearest node collide with the barrier, if so, deleting the node from the random tree and recalling the 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 length distance D, completing path planning of the random tree, and entering an optimal path acquisition unit;
an optimal path acquisition unit: and (4) from the six re-expanded random trees, synthesizing the first to six planned path points of the random trees to obtain an optimal path from the original starting point to the original target point.
6. The system of claim 5, wherein the mid-node sampling function module is calculated according to the following formula:
wherein Q isinitIs an original starting point, QgoalIs an original target point, Qinit2Is the second starting point, Qgoal2Is the second target point.
7. A computer storage medium comprising a computer program, wherein the computer program when executed by a processor implements the RRT-Connect based improved medical mobile robot path planning method of any one of claims 1-4.
8. An RRT-Connect based improved medical mobile robot path planning apparatus, comprising a memory, a processor, and a computer program stored on the memory and executable on the processor, wherein the processor executes the program to cause the RRT-Connect based improved medical mobile robot path planning apparatus to implement an RRT-Connect based improved medical mobile robot path planning method as claimed in any one of claims 1 to 4.
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 true CN111650941A (en) | 2020-09-11 |
| CN111650941B 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) |
Cited By (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112393739A (en) * | 2020-10-29 | 2021-02-23 | 国网安徽省电力有限公司检修分公司 | Improved rapid search random tree path planning method in large-area environment |
| CN112711256A (en) * | 2020-12-23 | 2021-04-27 | 杭州电子科技大学 | Automatic target search observation point generation method for mobile robot |
| CN113064426A (en) * | 2021-03-17 | 2021-07-02 | 安徽工程大学 | Intelligent vehicle path planning method for improving bidirectional fast search random tree algorithm |
| CN113204238A (en) * | 2021-04-22 | 2021-08-03 | 武汉理工大学 | Path planning method and device for mobile robot and storage medium |
| CN113341984A (en) * | 2021-06-15 | 2021-09-03 | 桂林电子科技大学 | Robot path planning method and device based on improved RRT algorithm |
| CN113671969A (en) * | 2021-08-25 | 2021-11-19 | 齐鲁工业大学 | Path planning method, system, medium and storage device in two-dimensional static environment |
| CN113804196A (en) * | 2020-09-17 | 2021-12-17 | 北京京东乾石科技有限公司 | Unmanned vehicle path planning method and related equipment |
| CN114153210A (en) * | 2021-12-01 | 2022-03-08 | 苏州盈科电子有限公司 | Movement control method and system for robot |
| CN114700937A (en) * | 2022-01-13 | 2022-07-05 | 深圳市越疆科技有限公司 | Mechanical arm, movement 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 |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20110035051A1 (en) * | 2009-08-10 | 2011-02-10 | Samsung Electronics Co., Ltd | Path planning apparatus and method for robot |
| US20110035087A1 (en) * | 2009-08-10 | 2011-02-10 | Samsung Electronics Co., Ltd. | Method and apparatus to plan motion path of robot |
| WO2015167220A1 (en) * | 2014-05-02 | 2015-11-05 | 한화테크윈 주식회사 | Device for planning path of mobile robot and method for planning path of mobile robot |
| CN108681787A (en) * | 2018-04-28 | 2018-10-19 | 南京航空航天大学 | Based on the unmanned plane method for optimizing route for improving the two-way random tree algorithm of Quick Extended |
| CN110497403A (en) * | 2019-08-05 | 2019-11-26 | 上海大学 | A kind of manipulator motion planning method improving two-way RRT algorithm |
| CN111141304A (en) * | 2019-12-30 | 2020-05-12 | 福州大学 | Path planning method based on concentric circle sampling and RRT guiding algorithm |
-
2020
- 2020-06-10 CN CN202010525927.2A patent/CN111650941B/en active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20110035051A1 (en) * | 2009-08-10 | 2011-02-10 | Samsung Electronics Co., Ltd | Path planning apparatus and method for robot |
| US20110035087A1 (en) * | 2009-08-10 | 2011-02-10 | Samsung Electronics Co., Ltd. | Method and apparatus to plan motion path of robot |
| WO2015167220A1 (en) * | 2014-05-02 | 2015-11-05 | 한화테크윈 주식회사 | Device for planning path of mobile robot and method for planning path of mobile robot |
| CN108681787A (en) * | 2018-04-28 | 2018-10-19 | 南京航空航天大学 | Based on the unmanned plane method for optimizing route for improving the two-way random tree algorithm of Quick Extended |
| CN110497403A (en) * | 2019-08-05 | 2019-11-26 | 上海大学 | A kind of manipulator motion planning method improving two-way RRT algorithm |
| CN111141304A (en) * | 2019-12-30 | 2020-05-12 | 福州大学 | Path planning method based on concentric circle sampling and RRT guiding algorithm |
Non-Patent Citations (2)
| Title |
|---|
| 康亮;赵春霞;郭剑辉;: "未知环境下改进的基于RRT算法的移动机器人路径规划" * |
| 莫栋成;刘国栋;: "改进的快速探索随机树双足机器人路径规划算法" * |
Cited By (15)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113804196A (en) * | 2020-09-17 | 2021-12-17 | 北京京东乾石科技有限公司 | Unmanned vehicle path planning method and related equipment |
| CN113804196B (en) * | 2020-09-17 | 2024-04-12 | 北京京东乾石科技有限公司 | Unmanned vehicle path planning method and related equipment |
| CN112393739A (en) * | 2020-10-29 | 2021-02-23 | 国网安徽省电力有限公司检修分公司 | Improved rapid search random tree path planning method in large-area environment |
| CN112711256A (en) * | 2020-12-23 | 2021-04-27 | 杭州电子科技大学 | Automatic target search observation point generation method for mobile robot |
| CN113064426A (en) * | 2021-03-17 | 2021-07-02 | 安徽工程大学 | Intelligent vehicle path planning method for improving bidirectional fast search random tree algorithm |
| CN113204238A (en) * | 2021-04-22 | 2021-08-03 | 武汉理工大学 | Path planning method and device for mobile robot and storage medium |
| 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 |
| CN113671969A (en) * | 2021-08-25 | 2021-11-19 | 齐鲁工业大学 | Path planning method, system, medium and storage device in two-dimensional static environment |
| CN113671969B (en) * | 2021-08-25 | 2024-04-05 | 齐鲁工业大学 | Path planning method, system, medium and storage device in two-dimensional static environment |
| CN114153210A (en) * | 2021-12-01 | 2022-03-08 | 苏州盈科电子有限公司 | Movement control method and system for robot |
| CN114153210B (en) * | 2021-12-01 | 2024-03-19 | 苏州盈科电子有限公司 | Movement control method and system of robot |
| CN114700937A (en) * | 2022-01-13 | 2022-07-05 | 深圳市越疆科技有限公司 | Mechanical arm, movement path planning method thereof, control system, medium and 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 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN111650941B (en) | 2023-05-02 |
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 | |
| Jaillet et al. | Path planning under kinematic constraints by rapidly exploring manifolds | |
| CN113219998B (en) | Improved bidirectional-RRT-based vehicle path planning method | |
| CN108444489A (en) | A kind of paths planning method improving RRT algorithms | |
| WO2018176596A1 (en) | Unmanned bicycle path planning method based on weight-improved particle swarm optimization algorithm | |
| CN107883961A (en) | A kind of underwater robot method for optimizing route based on Smooth RRT algorithms | |
| US20080306628A1 (en) | Multi-Modal Push Planner for Humanoid Robots | |
| US20120158176A1 (en) | Swarm robot and sweeping method using swarm robot | |
| CN108235725A (en) | Track based on high in the clouds ground drawing generating method, device, equipment and application program | |
| Nieuwenhuisen et al. | An effective framework for path planning amidst movable obstacles | |
| CN112987799B (en) | Unmanned aerial vehicle path planning method based on improved RRT algorithm | |
| CN108413963B (en) | Self-learning ant colony algorithm-based strip robot path planning method | |
| CN113296496B (en) | Gravity self-adaptive step length bidirectional RRT path planning method based on multiple sampling points | |
| CN112549016A (en) | Mechanical arm motion planning method | |
| CN112828889A (en) | Six-axis cooperative mechanical arm path planning method and system | |
| CN114161416B (en) | Robot path planning method based on potential function | |
| CN108871344B (en) | Football robot GGRRT path planning method | |
| Yan et al. | 3D PRM based real-time path planning for UAV in complex environment | |
| Mi et al. | A multi-heuristic A* algorithm based on stagnation detection for path planning of manipulators in cluttered environments | |
| CN114705196B (en) | Self-adaptive heuristic global path planning method and system for robot | |
| CN115870990A (en) | Obstacle avoidance path planning method for mechanical arm | |
| Sudhakara et al. | Probabilistic roadmaps-spline based trajectory planning for wheeled mobile robot | |
| 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 |
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 |