CN114115254A - RRT path planning method for multi-robot elastic formation - Google Patents
RRT path planning method for multi-robot elastic formation Download PDFInfo
- Publication number
- CN114115254A CN114115254A CN202111353398.3A CN202111353398A CN114115254A CN 114115254 A CN114115254 A CN 114115254A CN 202111353398 A CN202111353398 A CN 202111353398A CN 114115254 A CN114115254 A CN 114115254A
- Authority
- CN
- China
- Prior art keywords
- robot
- formation
- following
- obstacle
- virtual
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 230000015572 biosynthetic process Effects 0.000 title claims abstract description 85
- 238000000034 method Methods 0.000 title claims abstract description 29
- 230000004888 barrier function Effects 0.000 claims description 14
- 238000011084 recovery Methods 0.000 claims description 2
- 238000005070 sampling Methods 0.000 abstract description 5
- 230000010355 oscillation Effects 0.000 abstract description 4
- 230000002093 peripheral effect Effects 0.000 abstract description 3
- 101100134058 Caenorhabditis elegans nth-1 gene Proteins 0.000 description 3
- 238000010586 diagram Methods 0.000 description 3
- 238000013459 approach Methods 0.000 description 2
- 239000003814 drug Substances 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000012423 maintenance Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0287—Control of position or course in two dimensions specially adapted to land vehicles involving a plurality of land vehicles, e.g. fleet or convoy travelling
- G05D1/0291—Fleet control
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Manipulator (AREA)
Abstract
The invention relates to the technical field of robot path planning, in particular to an RRT path planning method for multi-robot elastic formation, aiming at the problems of randomness of an RRT algorithm and peripheral oscillation of obstacles, a target node expansion weight is introduced, so that the direction of a target node and the direction of a random sampling node jointly determine the direction of new nodes, and the path planning is optimized to a great extent; meanwhile, a concept of virtual springs is introduced for multi-robot formation, under the condition that the moving path of the following robot is influenced by the obstacle, the virtual springs are compressed by the pressure exerted by the obstacle, the formation is adjusted to a state capable of smoothly passing through, after the following robot smoothly passes through the obstacle, the pressure exerted by the obstacle disappears, and the formation is restored to an initial state, so that the multi-robot can adaptively adjust the formation by judging whether the obstacle exists or not.
Description
Technical Field
The invention relates to the technical field of robot path planning, in particular to an RRT path planning method for multi-robot elastic formation.
Background
The algorithm is a path planning algorithm based on random sampling, the algorithm takes a starting point where a robot is located as a root node of a random search tree, samples randomly in a state space to obtain new nodes, adds feasible new nodes into the search tree, and finally backtracks the nodes according to the parent-child relationship of the search tree to form a path from the initial node to a target node. However, in the process of forming a robot path by using the RRT algorithm, there are problems of strong randomness, oscillation around obstacles, and the like.
The multi-robot path planning is a further extension of single-robot path planning and is characterized in that a barrier-free feasible path is searched from a starting point to a target point in a built state space by referring to a certain characteristic index. Compared with a single robot, the multi-robot path planning can fully acquire environmental information and improve the task completion capability of the robot. Multi-robot path planning is widely used in the fields of industry, agriculture, medicine, search, etc.
The multi-robot also relates to formation control, and the formation control is widely researched at home and abroad, and the main control methods include a virtual navigation method, a behavior basic method and a navigator-follower method. The navigator-follower method is a popular research for multi-robot formation because of its advantages of simple implementation and good expansibility. The navigator-follower method is to plan a path of a navigator robot and then determine the position of a follower robot according to the position of the navigator robot and formation information. The invention introduces the concept of a virtual spring on the basis of a navigator-follower method, so that a plurality of robots can adaptively adjust formation by judging whether obstacles exist or not.
Disclosure of Invention
The invention aims to provide an RRT path planning method for multi-robot elastic formation, which is innovated on the basis of a rapid search random tree (RRT) algorithm, enhances the target guidance of the RRT path planning method, relieves the problem of high randomness, introduces a concept of a virtual spring in the aspect of robot formation, and enables multi-robots to perform formation adjustment according to a state space.
In order to achieve the above object, the present invention provides a method for planning RRT paths for multi-robot flexible formation, comprising the following steps:
step 4, when the formation path of the following robot is interfered by an obstacle, the following robot adjusts the formation to bypass the obstacle according to the virtual pressure;
step 5, after the robot smoothly passes through the barrier, the following robot returns to the original formation path and continues to move along with the piloting robot;
step 6, the formation recovery formation moves to the target node,
if the formation path following the robot is interfered by the obstacle again, the step 4 is repeatedly executed;
and if no obstacle interference exists, keeping the formation till the target node.
The piloting robot forms a planned path according to an improved fast search random tree algorithm and introduces a target node to expand the weight.
An elastic formation model is adopted between the pilot robot and the following robot, namely a virtual spring is set to exist between the pilot robot and the following robot, and the virtual pressure is an imaginary acting force which is applied by an obstacle and causes the virtual spring to deform.
The state of the virtual spring determines the position relation of the pilot robot and the following robot, and the initial formation distance of the pilot robot and the following robot can be determined through the free length of the virtual spring.
The following robot adjusts the formation to bypass the barrier according to the virtual pressure, the virtual pressure enables the virtual spring to compress, the following robot draws close to the moving track of the piloting robot to adjust, and the formation of the existing formation is kept to smoothly pass through the barrier.
After the robot smoothly passes through the barrier, the virtual pressure disappears, the virtual spring automatically recovers to the free length, and the following robot recovers to the initial formation distance between the following robot and the piloting robot.
Aiming at the problems of randomness of an RRT algorithm and peripheral oscillation of obstacles, the invention introduces a target node expansion weight to ensure that the direction of a target node and the direction of a random sampling node jointly determine the direction of new node generation, thereby optimizing the path planning to a great extent; meanwhile, a concept of virtual springs is introduced for multi-robot formation, under the condition that the moving path of the following robot is influenced by the obstacle, the virtual springs are compressed by the pressure exerted by the obstacle, the formation form is adjusted to a state capable of smoothly passing through, after the following robot smoothly passes through the obstacle, the pressure exerted by the obstacle disappears, and the formation form is restored to an initial state, so that the multi-robot can adaptively adjust the formation by judging whether the obstacle exists or not.
Drawings
In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings used in the description of the embodiments or the prior art will be briefly described below, it is obvious that the drawings in the following description are only some embodiments of the present invention, and for those skilled in the art, other drawings can be obtained according to these drawings without any creative effort.
Fig. 1 is a schematic flow chart of an RRT path planning method for multi-robot flexible formation according to the present invention.
Fig. 2 is a schematic diagram of a specific implementation process of the multi-robot elastic formation RRT path planning method of the present invention.
Fig. 3 is a schematic diagram of various state parameters when the robot elastic formation meets an obstacle.
Fig. 4 is a schematic diagram of various state parameters of the elastic formation of the robot after crossing an obstacle.
Detailed Description
Reference will now be made in detail to embodiments of the present invention, examples of which are illustrated in the accompanying drawings, wherein like reference numerals refer to the same or similar elements or elements having the same or similar function throughout. The embodiments described below with reference to the drawings are illustrative and are intended to be illustrative of the invention and should not be construed as limiting the invention.
Referring to fig. 1, the present invention provides a method for planning RRT paths for multi-robot flexible formation, comprising the following steps:
s1: forming a plurality of robots, and distinguishing a pilot robot and a following robot;
s2: the piloting robot plans a path and moves to a target node along the planned path;
s3: the following robot keeps a formation form and moves to a target node along with the pilot robot;
s4: when the formation path of the following robot is interfered by an obstacle, the following robot adjusts the formation to bypass the obstacle according to the virtual pressure;
s5: after the robot smoothly passes through the barrier, the following robot returns to the original formation path and continues to move along with the navigation robot;
s6: the formation resumes the formation moving towards the target node,
if the formation path following the robot is interfered by the obstacle again, repeatedly executing S4;
and if no obstacle interference exists, keeping the formation till the target node.
The piloting robot forms a planned path according to an improved fast search random tree algorithm and introduces a target node to expand weight.
An elastic formation model is adopted between the piloting robot and the following robot, namely a virtual spring is set to exist between the piloting robot and the following robot, and the virtual pressure is an imaginary acting force which is applied by an obstacle and causes the virtual spring to deform.
The state of the virtual spring determines the position relation of the pilot robot and the following robot, and the initial formation distance of the pilot robot and the following robot can be determined through the free length of the virtual spring.
In the process that the following robot adjusts the formation to bypass the barrier according to the virtual pressure, the virtual pressure causes the virtual spring to compress, the following robot draws close to the moving track of the piloting robot for adjustment, and the formation of the existing formation is kept until the obstacle smoothly passes through.
After the robot smoothly passes through the barrier, the virtual pressure disappears, the virtual spring automatically recovers to the free length, and the following robot recovers to the initial formation distance between the following robot and the piloting robot.
The main idea of the invention is to enable a multi-robot formation to move along a planned path, and simultaneously avoid obstacles to smoothly reach a final node by virtue of a virtual spring. Firstly, aiming at the problems of randomness of an RRT algorithm and peripheral oscillation of an obstacle, a target node expansion weight is introduced, so that the direction of a target node and the direction of a random sampling node jointly determine the direction of new node generation, and the path planning is optimized to a great extent; meanwhile, aiming at the concept of introducing virtual springs into multi-robot formation, under the condition that the moving path of the following robot is influenced by an obstacle, the virtual springs are compressed by the pressure exerted by the obstacle, and the formation shape is adjusted to a state of smooth passing; and after the following robot smoothly passes through the obstacle, the pressure applied by the obstacle disappears, and the formation is restored to the initial state, and the specific execution process refers to fig. 2.
The following is further illustrated from two specific aspects:
1. RRT path planning model of piloting robot
In order to enhance the target guidance of the new node expansion of the path, the expansion direction of the path node q (n +1) at the nth +1 moment of the piloting robot is assumed to be pointed to the target node q by the path node q (n) at the nth momentgoalDirection of (a), route node q (n) at time n points to random sampling node qrandCan be expressed as
q(n+1)＝q(n)+λ(ωgμ+(1-ωg)ν) (1)
Wherein λ is the node expansion step length, wgExpanding the weight for the target node, mu being q (n) pointing to qgoalThe unit vector of (a) is,
v is q (n) points to qrandThe unit vector of (a) is,
pbiasis the target bias probability, p is the random probability, if p < pbias，qrand＝qgoal(ii) a Otherwise, qrandAre randomly sampled points.
Assuming that the coordinate of a path node q (n +1) at the n +1 th moment of the piloting robot is (x)m(n+1),ym(n +1)), and the coordinate of q (n) is (x)m(n),ym(n))，qgoalHas the coordinates of
qrandHas the coordinates of
The formula (1) can be rewritten as
Wherein,
2. following robot elastic formation model
Supposing that a virtual spring exists between the piloting robot A and the following robots B and C respectively, and the free length of the virtual spring is leA stiffness coefficient of k, and a distance between A (n-1) and B (n-1) of
A (n-1) C (n-1) are at a distance of
The included angle between A (n-1) B (n-1) and A (n-1) A (n-2) is theta1The included angle between A (n-1) C (n-1) and A (n-1) A (n-2) is theta2Where A (n-2) is the coordinate point of the pilot robot A at the time n-2, and A (n-1), B (n-1) and C (n-1) respectively represent the coordinate points of the pilot robot A, the following robots B and C at the time n-1, as shown in FIG. 3. Based on the working principle of a spring, the elastic formation comprises two states of formation adjustment and formation maintenance. In the aspect of formation adjustment, if the following robot receives virtual pressure applied by an obstacle, the virtual spring is compressed, namely, the following robot approaches to B (n) along the moving track of the piloting robot, as shown in fig. 3. In terms of formation keeping, if the virtual pressure applied to the following robot disappears, the virtual spring will self-restore to the free length, i.e. the following robot will restore to B (n +1), as shown in fig. 4.
Suppose that the coordinates of the robot A, B and C at time n-1 are respectively
And
the n-th time coordinate is respectively
And
o is a set of following robots, Obstacle is a set of obstacles, Eage (B (n-1), B (n)) is a set of tracks from the nth-1 moment to the nth moment of the following robot B,
beta is the included angle between the moving track of the pilot robot and the horizontal line and alpha is the coordinate distance from the nth-1 moment to the nth moment of the robot B1An included angle is formed between a coordinate connecting line from the nth-1 moment to the nth moment and a horizontal line for following the robot B. During formation adjustment, the following robot B receives virtual pressure of the barrier between the n-1 th time and the n-th time
The virtual spring is compressed by the virtual spring, approaches to B (n) along the moving track of the pilot robot B, deviates from the original moving track in the process of moving to B (n) along the robot B, and forms a track deviation angle
According to FIG. 4, the virtual pressure is easily obtained
Is shown as
Wherein,
so as to obtain an included angle between a connecting line of the piloting robot and the following robot and a moving track of the piloting robot under the condition that the virtual pressure is applied to the barrier:
from this, the following robot coordinates can be found as:
after passing through the barrier, the pressure of the barrier on the following robot disappears, and the virtual spring is restored to the free length at the moment, namely the included angle between the connection line of the piloting robot and the following robot and the moving track of the piloting robot is restored to thetai。
While the invention has been described with reference to a preferred embodiment, it will be understood by those skilled in the art that various changes in form and detail may be made therein without departing from the spirit and scope of the invention.
Claims (6)
1. An RRT path planning method for multi-robot elastic formation is characterized by comprising the following steps:
step 1, forming a team by multiple robots, and distinguishing a pilot robot and a following robot;
step 2, the piloting robot plans a path and moves to a target node along the planned path;
step 3, the following robot keeps a formation form and moves to a target node along with the pilot robot;
step 4, when the formation path of the following robot is interfered by an obstacle, the following robot adjusts the formation to bypass the obstacle according to the virtual pressure;
step 5, after the robot smoothly passes through the barrier, the following robot returns to the original formation path and continues to move along with the piloting robot;
step 6, the formation recovery formation moves to the target node,
if the formation path following the robot is interfered by the obstacle again, the step 4 is repeatedly executed;
and if no obstacle interference exists, keeping the formation till the target node.
2. The multi-robot resilient formation RRT path planning method of claim 1,
the piloting robot forms a planned path according to an improved fast search random tree algorithm and introduces a target node to expand weight.
3. The multi-robot resilient formation RRT path planning method of claim 1,
an elastic formation model is adopted between the piloting robot and the following robot, namely a virtual spring is set to exist between the piloting robot and the following robot, and the virtual pressure is an imaginary acting force which is applied by an obstacle and causes the virtual spring to deform.
4. The multi-robot resilient formation RRT path planning method of claim 3,
the state of the virtual spring determines the position relation of the pilot robot and the following robot, and the initial formation distance of the pilot robot and the following robot can be determined through the free length of the virtual spring.
5. The multi-robot resilient formation RRT path planning method of claim 4,
in the process that the following robot adjusts the formation to bypass the barrier according to the virtual pressure, the virtual pressure enables the virtual spring to be compressed, the following robot draws close to the moving track of the piloting robot for adjustment, and the formation of the existing formation is kept until the obstacle smoothly passes through.
6. The multi-robot resilient formation RRT path planning method of claim 5,
after the robot smoothly passes through the barrier, the virtual pressure disappears, the virtual spring automatically recovers to the free length, and the following robot recovers to the initial formation distance between the following robot and the piloting robot.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111353398.3A CN114115254A (en) | 2021-11-16 | 2021-11-16 | RRT path planning method for multi-robot elastic formation |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111353398.3A CN114115254A (en) | 2021-11-16 | 2021-11-16 | RRT path planning method for multi-robot elastic formation |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN114115254A true CN114115254A (en) | 2022-03-01 |
Family
ID=80396660
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111353398.3A Pending CN114115254A (en) | 2021-11-16 | 2021-11-16 | RRT path planning method for multi-robot elastic formation |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114115254A (en) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115167388A (en) * | 2022-06-07 | 2022-10-11 | 哈尔滨理工大学 | RRT multi-robot formation path planning algorithm based on target guidance |
| CN115903814A (en) * | 2022-11-22 | 2023-04-04 | 哈尔滨工业大学（深圳） | Multi-robot optimal formation path planning based on convex polygon tree |
Citations (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106896817A (en) * | 2015-12-17 | 2017-06-27 | 中国科学院沈阳自动化研究所 | A kind of many AUV formation control methods based on viscous damping mode |
-
2021
- 2021-11-16 CN CN202111353398.3A patent/CN114115254A/en active Pending
Patent Citations (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106896817A (en) * | 2015-12-17 | 2017-06-27 | 中国科学院沈阳自动化研究所 | A kind of many AUV formation control methods based on viscous damping mode |
Non-Patent Citations (2)
| Title |
|---|
| 张伟民，付仕雄: "基于改进RRT*算法的移动机器人路径规划", 《华中科技大学学报（自然科学版）》, pages 31 - 36 * |
| 李 珣，何晶晶，赵征凡，严 冬，王晓华: "基于一致性改进模型的多机器人编队运动规划方法", 《西安工程大学学报》, pages 44 - 53 * |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115167388A (en) * | 2022-06-07 | 2022-10-11 | 哈尔滨理工大学 | RRT multi-robot formation path planning algorithm based on target guidance |
| CN115903814A (en) * | 2022-11-22 | 2023-04-04 | 哈尔滨工业大学（深圳） | Multi-robot optimal formation path planning based on convex polygon tree |
| CN115903814B (en) * | 2022-11-22 | 2023-08-18 | 哈尔滨工业大学（深圳） | Multi-robot optimal formation path planning method based on convex polygon tree |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109990787B (en) | Method for avoiding dynamic obstacle in complex scene by robot | |
| CN107234617B (en) | Obstacle avoidance path planning method guided by artificial potential field irrelevant to obstacle avoidance task | |
| CN108444489A (en) | A kind of paths planning method improving RRT algorithms | |
| CN109491389B (en) | Robot trajectory tracking method with speed constraint | |
| CN110231824B (en) | Intelligent agent path planning method based on straight line deviation method | |
| Panfeng et al. | Global minimum-jerk trajectory planning of space manipulator | |
| CN114115254A (en) | RRT path planning method for multi-robot elastic formation | |
| CN109866222B (en) | Mechanical arm motion planning method based on longicorn stigma optimization strategy | |
| CN113414761B (en) | Method for optimizing motion trail of redundant mechanical arm | |
| CN113858205A (en) | Seven-axis redundant mechanical arm obstacle avoidance algorithm based on improved RRT | |
| CN111633646A (en) | Robot motion planning method based on DMPs and modified obstacle avoidance algorithm | |
| CN114939872B (en) | MIRRT-Connect algorithm-based intelligent storage redundant mechanical arm dynamic obstacle avoidance motion planning method | |
| CN110530373B (en) | Robot path planning method, controller and system | |
| Gochev et al. | Motion planning for robotic manipulators with independent wrist joints | |
| CN114115362A (en) | Unmanned aerial vehicle flight path planning method based on bidirectional APF-RRT algorithm | |
| CN113650011B (en) | Method and device for planning splicing path of mechanical arm | |
| Liu et al. | Evasion of a team of dubins vehicles from a hidden pursuer | |
| An et al. | Obstacle avoidance path planning based on improved APF and RRT | |
| Cao et al. | Multi-AUV target searching under ocean current based on PPSO and velocity synthesis algorithm | |
| Lin et al. | An improved artificial potential field method for path planning of mobile robot with subgoal adaptive selection | |
| CN102427341A (en) | Kalman filtering-based transmission noise suppression method for remote iterative learning control system | |
| CN115533920A (en) | Collaborative planning method and system for solving inverse kinematics of rope-driven mechanical arm and computer storage medium | |
| Wang et al. | A multi-RRT based hierarchical path planning method | |
| CN115167466A (en) | Mobile robot standard control set local path planning method and system | |
| Helguera et al. | A local-based method for manipulators path planning in heavy cluttered 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 | ||
| RJ01 | Rejection of invention patent application after publication | ||
| RJ01 | Rejection of invention patent application after publication |
Application publication date: 20220301 |