CN111708370A

CN111708370A - Multi-robot collaborative path planning method and system based on artificial potential field

Info

Publication number: CN111708370A
Application number: CN202010703148.7A
Authority: CN
Inventors: 赵涛; 李好东; 佃松宜
Original assignee: Sichuan University
Current assignee: Sichuan University
Priority date: 2020-07-21
Filing date: 2020-07-21
Publication date: 2020-09-25
Anticipated expiration: 2040-07-21
Also published as: CN111708370B

Abstract

The invention provides a multi-robot collaborative path planning method and a system based on an artificial potential field, which comprises the following steps: acquiring position coordinates and motion directions of a plurality of robots on a motion plane; judging whether the first robot and the second robot have collision risks or not based on the position coordinates and the motion direction; the first robot and the second robot are respectively different robots in the plurality of robots; and if so, respectively correcting the motion directions of the first robot and the second robot based on a preset artificial potential field model. The invention solves the technical problem that collision may occur between different robots when the robots work cooperatively in the prior art.

Description

Multi-robot collaborative path planning method and system based on artificial potential field

Technical Field

The invention relates to the technical field of automatic control, in particular to a multi-robot collaborative path planning method and system based on an artificial potential field.

Background

The autonomous mobile robot technology is an important branch of the robot technology, has a long history and a wide development prospect, and has important application in the fields of space exploration, factory automation, mining, risk elimination, military and the like. The mobile robot can be used for saving labor force and plays an irreplaceable role in some dangerous or unfit environment for people to work. The first condition for the mobile robot to complete the task is to find a safe collision-free path from the starting position to the target position, so the navigation problem is always one of the most concerned problems in the mobile robot technology. To date, many algorithms have been proposed to implement autonomous navigation of a mobile robot, such as neural networks, genetic algorithms, ant colony algorithms, a-algorithm, and PID control, among others.

However, these algorithms are only applicable to static path planning, and cannot adjust the path in real time according to the environmental information, so that a path planning algorithm capable of real-time control is proposed. For example, d.q.khan et al propose a mobile robot navigation method based on an improved remote control algorithm combining inertial sensors with visual information. S.h.park et al developed an extended guide circle method for mobile robot navigation. Meanwhile, many conventional methods for global (static) path planning are also extended to local (dynamic) path planning, such as artificial potential field method, road mapping method, and rolling window method. However, the above methods are only suitable for a single robot, and when the robot is faced with some complicated tasks, the single robot is unable to work, so that a multi-robot cooperation system is proposed. However, the multi-robot cooperation system in the prior art does not solve the fundamental problem of how to reliably realize safe collision avoidance among robots in multi-robot cooperation path planning, that is, the prior art has the technical problem that collision may occur among different robots when the multiple robots cooperate to work.

Disclosure of Invention

In view of the above, the present invention provides a method and a system for planning the same path of a multi-robot system based on an artificial potential field, so as to alleviate the technical problem in the prior art that collision may occur between different robots when the multiple robots cooperatively work.

In a first aspect, an embodiment of the present invention provides a multi-robot collaborative path planning method based on an artificial potential field, including: acquiring position coordinates and motion directions of a plurality of robots on a motion plane; judging whether the first robot and the second robot have collision risks or not based on the position coordinates and the motion direction; the first robot and the second robot are respectively different robots of the plurality of robots; and if so, respectively correcting the motion directions of the first robot and the second robot based on a preset artificial potential field model.

Further, if it is determined that the first robot and the second robot have a collision risk, the method further includes: acquiring the priority of the first robot and the priority of the second robot; and adjusting the movement speeds of the first robot and the second robot based on the priority, so that the movement speed of the robot with high priority is greater than the movement speed of the robot with low priority.

Further, based on the position coordinates and the moving direction, determining whether the first robot and the second robot have a collision risk includes: dividing the motion plane into four subareas by using a first straight line and a second straight line which pass through the central point by taking the position coordinate of the first robot as the central point; the first straight line is a straight line parallel to the movement direction of the first robot, and the second straight line is a straight line parallel to the movement direction of the second robot; determining a target distance between the first robot and the second robot based on the coordinate position of the first robot and the coordinate position of the second robot; judging whether the first robot and the second robot have collision risks or not based on the partition where the second robot is located, the target distance, the motion direction of the first robot and the target included angle; the target included angle is an included angle between the moving direction of the first robot and the moving direction of the second robot.

Further, based on the partition where the second robot is located, the target distance, the moving direction of the first robot, and the target included angle, determining whether the first robot and the second robot have a collision risk includes: determining at least one risk zone among the four zones based on the movement direction of the first robot and the target included angle; judging whether the partition where the second robot is located is the risk partition or not, and whether the target distance is smaller than a preset distance or not; and if so, judging that the first robot and the second robot have collision risks.

Further, based on a preset artificial potential field model, correcting the movement direction of the first robot, including: determining the attraction of the target point to the first robot based on a preset artificial potential field model; based on the preset artificial potential field model, taking the second robot as an obstacle, and determining repulsion force generated by the second robot on the first robot; summing the attraction force and the repulsion force to obtain a resultant force applied to the first robot; determining a steering angle of the first robot based on the resultant force and a movement speed of the first robot; correcting the moving direction of the first robot based on the steering angle.

Further, the mathematical expression of the preset artificial potential field model includes:

and:

wherein, F_att[x,y]A vector k representing components of the attraction of the target point to the robot in the x-axis and y-axis directions_attDenotes a gravitational gain coefficient, d_gRepresenting the distance of the robot to the target point, theta_gRepresenting the angle between the line connecting the robot to the target point and the x-axis, d₀Is a preset distance value; f_rep[x,y]A vector k representing components of the repulsive force generated by the obstacle to the robot in the directions of the x axis and the y axis_repDenotes a repulsive force gain coefficient, theta denotes a moving direction of the robot, k is a constant greater than 0, d_fl、d_fr、d_l、d_rAnd d_fRespectively represents the distance from the robot to the obstacle in front left, front right, left, right and front_flIs as followsA predetermined gain constant, k_frIs a second predetermined gain constant, and k_fl>1，k_fr>1。

Further, determining a steering angle of the first robot based on the resultant force and the movement speed of the first robot, comprises: determining a steering angle of the first robot by:

wherein S is_aIndicating steering angle, v_rRepresenting the speed of movement of the first robot, theta representing the direction of movement of the first robot, F_resxAnd F_resyRespectively representing the component of the resultant force in the x-axis direction and the component in the y-axis direction.

In a second aspect, an embodiment of the present invention further provides a system for planning a multi-robot collaborative path based on an artificial potential field, including: the system comprises an acquisition module, a judgment module and a correction module, wherein the acquisition module is used for acquiring the position coordinates and the motion directions of a plurality of robots on a motion plane; the judging module is used for judging whether the first robot and the second robot have collision risks or not based on the position coordinates and the motion direction; the first robot and the second robot are respectively different robots of the plurality of robots; and the correction module is used for correcting the motion directions of the first robot and the second robot respectively based on a preset artificial potential field model if the collision risk between the first robot and the second robot is judged.

Further, the system further comprises: the speed adjusting module is used for acquiring the priority of the first robot and the priority of the second robot if the first robot and the second robot are judged to have collision risks; and adjusting the movement speeds of the first robot and the second robot based on the priority, so that the movement speed of the robot with high priority is greater than the movement speed of the robot with low priority.

In a third aspect, an embodiment of the present invention further provides an electronic device, which includes a memory, a processor, and a computer program stored in the memory and executable on the processor, where the processor implements the steps of the method according to the first aspect when executing the computer program.

The invention provides a multi-robot collaborative path planning method and a system based on an artificial potential field, which comprises the following steps: acquiring position coordinates and motion directions of a plurality of robots on a motion plane; judging whether the first robot and the second robot have collision risks or not based on the position coordinates and the motion direction; the first robot and the second robot are respectively different robots in the plurality of robots; and if so, respectively correcting the motion directions of the first robot and the second robot based on a preset artificial potential field model. According to the invention, whether collision risks exist among robots is judged firstly, and then the movement direction of the robots with the collision risks is corrected based on the preset artificial potential field model, so that safe and reliable collision avoidance among the robots is realized, and the technical problem that collision possibly occurs among different robots when multiple robots cooperatively work in the prior art is solved.

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, and it is obvious that the drawings in the following description are some embodiments of the present invention, and other drawings can be obtained by those skilled in the art without creative efforts.

Fig. 1 is a flowchart of a multi-robot collaborative path planning method based on an artificial potential field according to an embodiment of the present invention;

fig. 2 is a schematic diagram of a first robot collision detection provided in the embodiment of the present invention;

fig. 3 is a schematic diagram of a second robot collision detection provided in the embodiment of the present invention;

fig. 4 is a schematic diagram of a third robot collision detection provided in the embodiment of the present invention;

fig. 5 is a schematic diagram of a fourth robot collision detection provided in the embodiment of the present invention;

fig. 6 is a schematic diagram of a fifth robot collision detection provided in the embodiment of the present invention;

fig. 7 is a schematic diagram of a sixth robot collision detection provided in the embodiment of the present invention;

fig. 8 is a schematic diagram of a collision detection of a seventh robot according to the embodiment of the present invention;

fig. 9 is a schematic diagram of a collision detection of an eighth robot according to an embodiment of the present invention;

fig. 10 is a schematic diagram of a collision detection of a ninth robot according to an embodiment of the present invention;

fig. 11 is a schematic diagram of collision detection of a tenth robot according to the embodiment of the present invention;

fig. 12 is a schematic diagram of collision detection of an eleventh robot according to an embodiment of the present invention;

fig. 13 is a schematic diagram illustrating a collision detection of a twelfth robot according to an embodiment of the present invention;

fig. 14 is a schematic diagram of collision detection of a thirteenth robot according to the embodiment of the present invention;

fig. 15 is a schematic diagram of collision detection of a fourteenth robot according to the embodiment of the present invention;

fig. 16 is a schematic diagram illustrating a collision detection of a fifteenth robot according to an embodiment of the present invention;

fig. 17 is a schematic diagram of a collision detection of a sixteenth robot according to the embodiment of the present invention;

FIG. 18 is a schematic diagram of a membership function according to an embodiment of the present invention;

fig. 19 is a schematic diagram of a collision avoidance process of three robots according to an embodiment of the present invention;

fig. 20 is a schematic view of a collision avoidance process between two robots according to an embodiment of the present invention;

fig. 21 is a schematic view of another collision avoidance process between two robots according to the embodiment of the present invention;

fig. 22 is a flowchart of a path planning method for any one robot in a multi-robot system according to an embodiment of the present invention;

fig. 23 is a schematic diagram of a collision avoidance process of two robots when there is no obstacle in an environment according to an embodiment of the present invention;

fig. 24 is a schematic diagram of a collision avoidance process of three robots when no obstacle exists in an environment according to an embodiment of the present invention;

FIG. 25 is a schematic diagram of two robots avoiding collision when there is a simple obstacle in an environment according to an embodiment of the present invention;

FIG. 26 is a schematic diagram of a collision avoidance process for three robots with a simple obstacle in an environment according to an embodiment of the present invention;

fig. 27 is a schematic diagram of a collision avoidance process of two robots when there is a complex obstacle in an environment according to an embodiment of the present invention;

fig. 28 is a schematic diagram of a collision avoidance process of three robots when there is a complex obstacle in an environment according to an embodiment of the present invention;

fig. 29 is a schematic view of an obstacle avoiding U-shaped obstacle according to an embodiment of the present invention;

fig. 30 is a schematic diagram of a multi-robot collaborative path planning system based on an artificial potential field according to an embodiment of the present invention.

Detailed Description

The technical solutions of the present invention will be described clearly and completely with reference to the accompanying drawings, and it should be understood that the described embodiments are some, but not all embodiments of the present invention. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.

The first embodiment is as follows:

fig. 1 is a flowchart of a multi-robot collaborative path planning method based on an artificial potential field according to an embodiment of the present invention. As shown in fig. 1, the method specifically includes the following steps:

step S102, acquiring position coordinates and motion directions of a plurality of robots on a motion plane.

Step S104, judging whether the first robot and the second robot have collision risks or not based on the position coordinates and the motion direction; the first robot and the second robot are respectively different robots among the plurality of robots. It should be noted that, in the embodiment of the present invention, for the collaborative path planning process of multiple robots, collision risk judgment is performed by using every two robots as a group.

And S106, if so, respectively correcting the motion directions of the first robot and the second robot based on a preset artificial potential field model.

The invention provides a multi-robot collaborative path planning method based on an artificial potential field, which comprises the steps of firstly judging whether collision risks exist among robots, and then correcting the motion direction of the robots with the collision risks based on a preset artificial potential field model, thereby realizing safe and reliable collision avoidance among the robots and relieving the technical problem that collision possibly occurs among different robots when the multiple robots work collaboratively in the prior art.

Optionally, step S104 further includes the steps of:

step S1041, dividing the motion plane into four subareas by using the position coordinate of the first robot as a central point and using a first straight line and a second straight line passing through the central point; the first straight line is a straight line parallel to the movement direction of the first robot, and the second straight line is a straight line parallel to the movement direction of the second robot.

Step S1042, determining a target distance between the first robot and the second robot based on the coordinate position of the first robot and the coordinate position of the second robot.

Step S1043, judging whether the first robot and the second robot have collision risks or not based on the partition where the second robot is located, the target distance, the motion direction of the first robot and the target included angle; the target included angle is an included angle between the moving direction of the first robot and the moving direction of the second robot.

Specifically, at least one risk zone is determined in the four zones based on the movement direction of the first robot and the target included angle; judging whether the partition where the second robot is located is a risk partition and whether the target distance is smaller than a preset distance; and if so, judging that the first robot and the second robot have collision risks.

When the distance is long, even if two robots are running along the same straight line in opposite directions, the two robots are still unlikely to collide in a short time, and their advancing directions may spontaneously become no longer colliding with each other with the passage of time, so that although the advancing directions of the two robots are colliding with each other at this time, the collision risk between the two robots can still be considered to be absent. Therefore, three dangerous distance thresholds (i.e., preset distances) D need to be set first when performing collision risk detection₁、D₂And D₃Wherein D is₁>D₂>D₃Their specific values are related to the size of the robot and the set maximum speed. At the same time, the first robot R is moved_iAnd a second robot R_jIs recorded as D_ijWhen the two robots are far enough apart, i.e. D_ij>D₁In the process, whether the advancing directions of the robot and the robot conflict or not can be considered to be completely safe, namely, the collision risk does not exist, at the moment, each robot moves according to the advancing direction and the speed which are planned by an artificial potential field method and a fuzzy inference system, and the robots are not influenced by each other. When the distance between the two robots is relatively close, i.e. D_ij≤D₁When the robot is moving, if there is a collision between their directions of travel, there is a risk of collision between the two robots.

Specifically, based on the position coordinates and the moving direction, the process of determining whether the first robot and the second robot have a collision risk is as follows:

first, the robot R is put_i(i.e. first robot) all possible directions of advance theta_i(i.e., the direction of motion) into four intervals: [0,90 ° ]]、[90°,180°]、[-90°,0]And [ -180 °, -90 ° ]]. Then, the robot R is moved_j(i.e., second robot) forward direction θ_jRelative to R_iDirection of advance theta_iIs recorded as theta_ij(i.e., the target included angle), then θ_ijCan be calculated from the following formula:

θ_ij＝θ_i-θ_j(1)

wherein theta is_ijThe value range of (1) is [ -180 DEG, 180 DEG ]]When the value calculated by the expression (1) exceeds this interval, θ can be made by adding ± 360 ° to_ijIs located within a prescribed range. And theta_iSame, theta_ijIt can also be divided into four intervals: [0,90 ° ]]、[90°,180°]、[-90°,0]And [ -180 °, -90 ° ]]. Fig. 2 is a schematic diagram of a first robot collision detection provided according to an embodiment of the present invention, where (a) in fig. 2 is a schematic diagram of area division, (b) is a schematic diagram of Rj located in a safety zone, (c) is a schematic diagram of Rj located in a risk zone, and (d) is a schematic diagram of Rj located in a risk zone. As shown in fig. 2 (a), black arrows represent the advancing direction of the robot, arrow tails represent the center coordinates of the robot, and then the robot R passes_iThe central point is made into two straight lines, one is made into R_iThe advancing direction is parallel to the line ①, and the other is parallel to R_jThe directions of advance are parallel and are marked as a straight line ②, then the straight line ① and the straight line ② divide the whole plane into four subareas, and then R needs to be judged_jIn which partition the central coordinate point of (b) is located. Due to theta_iAnd theta_ijAll may lie in any one of the four angular intervals, so there are sixteen possible combinations, as follows:

(1) when theta is_i∈[0,90°]，θ_ij∈[0,90°]As shown in fig. 2 (a), the following determination can be made according to the traveling directions of the two robots: if and only if R_jJust below line ① and to the right of line ②, there is a possibility of collision between the two robots_iThe numerical size of the distance D of the centers subdivides the area: note D>D₁Is in the region Z₀，D₁≥D>D₂Is in the region Z₁，D₂≥D>D₃Is in the region Z₂，D₃≥D>Region of 0 is Z₃As shown in fig. 2 (a). Wherein, the shaded part Z in the figure₁，Z₂And Z₃Collectively referred to as robot R_iRelative to R_jIn the danger zone, handle Z₀Referred to as robot R_iRelative to R_jWind ofDangerous area, other areas being called robots R_iRelative to R_jThe security zone of (a) is hereinafter referred to simply as the hazard zone, risk zone and security zone, respectively. Finally, can be based on R_jAnd particularly in which zone to determine the risk of collision between two robots. If R is_jIn the safety zone, as shown in the diagram (b) in fig. 2, since there is no collision between the two robots in the forward directions, there is no risk of collision even if the two robots are in close proximity. If R is_jLocated in the risk zone Z₀As shown in FIG. 2 (c), in this case, the robot R is assumed to be_iAnd R_jThe two advancing directions collide with each other, so that the possibility of collision exists, but due to the fact that the distance between the two advancing directions is long, even if a collision avoidance strategy between the robots is not adopted, the advancing directions of the robots may become not to collide with each other again as time goes on, and therefore the two advancing directions are still considered to have no collision risk. If R is_jLocated in the hazard zone Z₁、Z₂Or Z₃As shown in fig. 2 (d), this is because of the robot R_iAnd R_jNot only the collision exists in the advancing direction, but also the distance is relatively close, so that the collision risk among the robots is large. Wherein R is_jAt Z₃Maximum risk of collision, Z₂Second, Z₁The risk of collision is minimal.

(2) FIG. 3 is a schematic diagram of a second robot collision detection scheme according to an embodiment of the present invention, when θ is_i∈[0,90°]，θ_ij∈[90,180°]In the case of this, as shown in FIG. 3 (a), R is marked off in the same manner as in (1)_iRelative to R_jIf and only if R_jWhen the collision between two robots is possible under the line ① and over the line ②, the diagrams (b) and (c) in fig. 3 are R_jIn the safe area and the risk area, it is safe between the two robots. FIG. 3 (d) is a drawing R_jLocated in the hazard zone Z₁、Z₂Or Z₃The time, there is a greater risk of collision between the two robots at this time.

(3) FIG. 4 is a schematic diagram of a third robot collision detection scheme according to an embodiment of the present inventionθ_i∈[0,90°]，θ_ij∈[-90,0°]When, as shown in the diagram (a) in FIG. 4, if and only if R is_jAbove line ① and above line ②, there is a risk of collision between the two robots.

(4) FIG. 5 is a schematic diagram of a fourth robot collision detection according to an embodiment of the present invention, when θ is_i∈[0,90°]，θ_ij∈[-180,-90°]When, as shown in the diagram (a) in FIG. 5, if and only if R is_jWhile above line ① and to the right of line ②, there is a risk of collision between the two robots.

(5) FIG. 6 is a schematic diagram of a fifth robot collision detection according to an embodiment of the present invention, when θ is_i∈[90,180°]，θ_ij∈[0,90°]When, as shown in the diagram (a) in FIG. 6, if and only if R is_jAbove line ① and above line ②, there is a risk of collision between the two robots.

(6) FIG. 7 is a schematic diagram of a sixth robot collision detection scheme according to an embodiment of the present invention, when θ is_i∈[90,180°]，θ_ij∈[90,180°]When, as shown in the diagram (a) in FIG. 7, if and only if R is_jAbove line ① and to the left of line ②, there is a risk of collision between the two robots.

(7) FIG. 8 is a schematic diagram of a seventh robot collision detection according to the embodiment of the present invention, when θ is_i∈[90,180°]，θ_ij∈[-90,0°]When, as shown in the diagram (a) in FIG. 8, if and only if R is_jAnd below line ① and to the left of line ②, there is a risk of collision between the two robots.

(8) FIG. 9 is a schematic diagram of an eighth robot collision detection scheme according to an embodiment of the present invention, when θ is_i∈[90,180°]，θ_ij∈[-180,-90°]When, as shown in the diagram (a) in FIG. 9, if and only if R is_jBoth below line ① and above line ②, the risk of collision between the two robots is present.

(9) FIG. 10 is a schematic diagram of a ninth robot collision detection scheme according to an embodiment of the present invention, when θ is_i∈[-90,0°]，θ_ij∈[0,90°]When it is as shown in the figure10, (a) is as shown, if and only if R_jBelow line ① and below line ②, there is a risk of collision between the two robots.

(10) FIG. 11 is a schematic diagram of a tenth robot collision detection scheme according to an embodiment of the present invention, when θ is_i∈[-90,0°]，θ_ij∈[90,180°]When, as shown in the diagram (a) in FIG. 11, if and only if R is_jWhile below line ① and to the right of line ②, there is a risk of collision between the two robots.

(11) FIG. 12 is a schematic diagram of an eleventh robot collision detection according to an embodiment of the present invention, when θ is_i∈[-90,0°]，θ_ij∈[-90,0°]When, as shown in the diagram (a) in FIG. 12, if and only if R is_jWhile above line ① and to the right of line ②, there is a risk of collision between the two robots.

(12) FIG. 13 is a diagram illustrating a collision detection of a twelfth robot according to an embodiment of the present invention, when θ is_i∈[-90,0°]，θ_ij∈[-180,-90°]When, as shown in (a) of FIG. 13, if and only if R is_jAbove line ① and below line ②, the risk of collision between the two robots is present.

(13) FIG. 14 is a schematic diagram of a thirteenth robot collision detection scheme according to an embodiment of the present invention, when θ_i∈[-180,-90°]，θ_ij∈[0,90°]When, as shown in the diagram (a) in FIG. 14, if and only if R is_jAbove line ① and to the left of line ②, there is a risk of collision between the two robots.

(14) FIG. 15 is a schematic diagram of a fourteenth robot collision detection scheme according to an embodiment of the present invention, when θ is_i∈[-180,-90°]，θ_ij∈[90,180°]When, as shown in the diagram (a) in FIG. 15, if and only if R is_jAbove line ① and below line ②, the risk of collision between the two robots is present.

(15) FIG. 16 is a schematic diagram of a fifteenth robot collision detection scheme according to an embodiment of the present invention, when θ is_i∈[-180,-90°]，θ_ij∈[-90,0°]When, as shown in (a) of FIG. 16, when andonly when R is_jBelow line ① and below line ②, there is a risk of collision between the two robots.

(16) FIG. 17 is a schematic diagram of a sixteenth robot collision detection scheme according to an embodiment of the present invention, when θ is_i∈[-180,-90°]，θ_ij∈[-180,-90°]When, as shown in (a) of FIG. 17, if and only if R is_jAnd below line ① and to the left of line ②, there is a risk of collision between the two robots.

Above list R_iAnd R_jIn all cases when there is a risk of collision, from which the following conclusions can be drawn: when R is_jWhen the robot is located in the safe area, the two robots are not likely to collide with each other, and the collision risk is avoided. When R is_jLocated in the risk zone Z₀In the meantime, although there is a possibility of collision between the two robots, since the two robots are far apart from each other, it is still considered that there is no risk of collision between them. Finally, if and only if R_jAt R_iDanger zone Z of₁、Z₂Or Z₃Only between the two robots is there a risk of collision.

Optionally, in step S106, the correcting the moving direction of the first robot includes the following steps:

step S1061, determining the attraction force generated by the target point on the first robot based on the preset artificial potential field model.

And step S1062, based on the preset artificial potential field model, determining repulsion force generated by the second robot to the first robot by taking the second robot as an obstacle.

Step S1063, summing the attraction force and the repulsion force to obtain a resultant force applied to the first robot.

Step S1064, determining a steering angle of the first robot based on the resultant force and the movement speed of the first robot.

In step S1065, the movement direction of the first robot is corrected based on the steering angle.

When a traditional artificial potential field method is used for path planning, when a U-shaped obstacle is encountered, the robot enters a dead cycle due to a local minimum value and even collides with the obstacle, and aiming at the problem, the embodiment of the invention provides an improved artificial potential field algorithm which completely overcomes the problem of the local minimum value of the traditional artificial potential field method and enables the robot to directly bypass the shallow U-shaped obstacle. For deeper U-shaped obstacles, the robot can escape quickly without getting into a dead cycle in the form of a local minimum, although it will enter as well as the conventional artificial potential field method.

For the expression about the gravitational force in the conventional artificial potential field, the closer the robot is to the target point, the larger the gravitational force, and the farther the robot is. Thus, when the robot is far from the target point, the attraction force is very small, and the advancing direction of the robot is mainly determined by the repulsive force generated by the obstacle beside, so that the robot may be caused to move around the far path and even move away from the target point. In order to avoid the above situation, the mathematical expression of the preset artificial potential field model provided in the embodiment of the present invention includes:

wherein, F_att[x,y]A vector k representing components of the attraction of the target point to the robot in the x-axis and y-axis directions_attDenotes a gravitational gain coefficient, d_gRepresenting the distance of the robot to the target point, theta_gRepresenting the angle between the line connecting the robot to the target point and the x-axis, d₀Is a preset distance value. As can be seen from equation (2), in order to avoid the problem that the robot deviates from the target point seriously due to too small attraction force, when d_g>d₀When calculating the gravity, d is uniformly calculated_gValue d₀Therefore, the target point can be ensured to have enough attraction force to the robot all the time.

In order to solve the problem that when a robot encounters a U-shaped obstacle, the robot falls into a local minimum value and enters a dead cycle, two preset gain constants k are respectively introduced into repulsive forces generated by the obstacle in the front left and the front right of the robot in the embodiment of the invention_fl(first preset gain constant) and k_fr(first and second predetermined gain constants), bothGreater than 1, k to avoid the symmetry of the repulsion causing the robot to fall into a dead cycle_flAnd k_frThe values cannot be the same, and k is used to avoid the track from generating large oscillation_flAnd k_frThe values of (a) should not differ too much. At the moment, gains with different values are introduced into the repulsive force of the left front part and the repulsive force of the right front part, so that the robot can directly bypass a shallow U-shaped obstacle, and when the U-shaped obstacle is deep, the robot can quickly escape from the inside of the obstacle and smoothly reach a target point. In addition, the repulsive force is defined so that the robot can smoothly pass over an obstacle when the distance between the robot and the obstacle is sufficiently large (the distance threshold is set to d)₁) It is considered that the robot and the obstacle are safe, so the obstacle does not have to generate repulsion to the robot at this time, the repulsion should be zero, and when the robot directly moves toward the obstacle and is very close to the obstacle (the distance threshold is d)₂) The robot may not have enough time to adjust the advancing direction to collide with the obstacle, so the repulsive force should be set to be large in order to ensure that the robot does not collide with the obstacle. In conclusion, when d_fl、d_fr、d_l、d_rAnd d_fAre all greater than d₁When the repulsion force generated by the obstacle is equal to zero, on the contrary, a gain k is introduced_flAnd k_frThe robot can smoothly escape from the local minimum value. Thus, the improved repulsive force expression is as follows:

wherein, F_rep[x,y]A vector k representing components of the repulsive force generated by the obstacle to the robot in the directions of the x axis and the y axis_repDenotes a repulsive force gain coefficient, theta denotes a moving direction of the robot, k is a constant greater than 0, d_fl、d_fr、d_l、d_rAnd d_fRespectively represents the distance from the robot to the obstacle in front left, front right, left, right and front_flIs a first predetermined gain constant, k_frIs a second predetermined gain constant, and，k_fl>1，k_fr>1. when the robot is moving straight towards the obstacle and is very close, i.e. min (d)_fl,d_fr,d_l,d_r,d_f)≤d₁When k is_repAnd (2) formula medium gravitational gain k_attTogether ensuring that the defined attractive and repulsive forces are of the same order of magnitude. When min (d)_fl,d_fr,d_l,d_r,d_f)>d₁In order to make the moving direction of the robot irrelevant to the repulsive force generated by the obstacle, let k_rep＝0。

The resultant force expression experienced by the robot is as follows:

F_res[x,y]＝F_att[x,y]+F_rep[x,y](4)

finally, the steering angle of the first robot is determined by the following equation:

wherein S is_aIndicating steering angle, v_rRepresenting the speed of movement of the first robot, theta representing the direction of movement of the first robot, F_resxAnd F_resyRepresenting the component of the resultant force in the x-axis direction and the component in the y-axis direction, respectively.

Optionally, if it is determined that the first robot and the second robot have a collision risk, the method provided in the embodiment of the present invention further includes:

acquiring the priority of the first robot and the priority of the second robot;

and adjusting the movement speeds of the first robot and the second robot based on the priority, so that the movement speed of the robot with high priority is greater than the movement speed of the robot with low priority.

Optionally, the method provided in the embodiment of the present invention further includes a fuzzy control algorithm. Preferably, embodiments of the present invention employ motorni fuzzy control as the fuzzy control algorithm. Wherein, the input variables of the fuzzy control algorithm are three in total, and are respectively the distance (d) from the robot to the obstacle right ahead_f) Distance (d) from the robot to the target point_g) And the absolute value (as) of the steering angle. And finally outputting the motion speed (vr) of the robot through fuzzy reasoning and defuzzification according to the value of the input variable and the fuzzy reasoning rule.

Specifically, a variable d is input_fIs divided into { N, M, F }, (N-Near, M-Middle, F-Far), d_gIs divided into { N, F }, (N-Near, F-Far), a_sIs divided into (VS, S, M, B, VB), (V-Very, M-Middle, S-Small, B-Big), output variable V_rAre divided into { VS, MS, S, B, MB, VB }, (V-Very, M-Middle, S-Small, B-Big). The membership function for each input and output variable is shown in fig. 18. Fig. 18 (a) is a schematic diagram of a membership function of an input variable df, (b) is a schematic diagram of a membership function of an input variable dg, (c) is a schematic diagram of a membership function of an input variable as, and (d) is a schematic diagram of a membership function of an output variable vr.

There are 30 possible combinations of 3 × 2 × 5, which correspond to the 30 rules shown in table 1, respectively, depending on the values of the input variables. When the robot is far away from both the obstacle and the target point, the environment is safe, so in order to improve the efficiency of the robot, the moving speed of the robot (the output of the fuzzy control algorithm) may be relatively fast, as in rules 1 to 5 in table 1. When the distance to the obstacle and the target point is close, the environment is dangerous, and there is a risk of collision with the obstacle, so in order to ensure the safety of the robot, the moving speed of the robot should be relatively slow, so that there is more time to adjust the advancing direction to smoothly bypass the obstacle in the environment, as shown in rules 26 to 30 in table 1.

TABLE 1 fuzzy inference rules

Optionally, in the method provided by the embodiment of the present invention, the modifying of the moving directions of the multiple machines includes the following processes: the robot detection method includes the steps that firstly, whether collision risks with other robots exist or not is detected in real time in the moving process of the robots, when the risks are not detected and the distances among the robots are safe, the robots are not affected with each other, each robot moves according to the advancing direction planned by a preset artificial potential field model and the speed planned by a fuzzy control method, when the risks are not detected but the distances among the robots are close, repulsion force is generated among the robots through the preset artificial potential field model, the distance between the robots is guaranteed to be safe enough, and when the risks are detected, a collision avoidance strategy is executed, so that the robots can safely reach respective target positions.

Specifically, when the robot R_iDetecting the distance D₁Within range there are other robots R_jIn time, robot R_iThe position of the danger area can be updated in real time according to the position of the danger area and the advancing directions of the danger area and the danger area, and R is detected_jWhether it is located in a dangerous area, when the robot R is detected_jWhen the robot is located in the dangerous area, the robot needs to execute a collision avoidance strategy to ensure that each robot can safely reach the respective target position. When a collision avoidance strategy is formulated, the priority of each robot is set according to the working property or the number of the robot, and then the low-priority robot performs deceleration or stop operation to ensure that enough time is provided for adjusting the advancing direction and the distance. In addition, since stopping and restarting the robot requires the control system to perform complicated operations and adds unnecessary time loss, the robot is stopped only when a dangerous situation is encountered. Optionally, an anti-collision policy provided in an embodiment of the present invention includes:

(a) if R is detected_jAt R_iDanger zone Z of₁The low priority robot will slow down since R is now present_iAnd R_jIs relatively far away, the speed of the low priority robot need only be reduced to a little lower than the high priority robot to ensure that there is enough time between the two robots to adjust the heading and distance. Wherein, the adjustment of the advancing direction and the distance is realized by an artificial potential field method (namely the preset artificial potential field model), namely the robot R_iAnd R_jWhen the opposite side is regarded as a dynamic barrier, a repulsive force is generated between the two, so that the respective advancing directions are adjusted to safely bypass the opposite side. ByThe robots are still moving while adjusting the direction of advance, so that the distance between the robots is naturally adjusted.

(b) If R is detected_jAt R_iDanger zone Z of₂The low-priority robot also decelerates, except that the distance between the two robots is relatively short, so that a large collision risk exists, the remaining adjustment distance is short, and in order to ensure that enough time is available for adjusting the advancing direction and distance of the robot, the low-priority robot can decelerate greatly, so that the speed is far lower than that of the high-priority robot, but the robot cannot stop moving. Meanwhile, the dynamic robots are more dangerous than obstacles, and the distance between the robots is closer, so that the absolute value of the gain krep in the expression of the repulsion between the robots can be increased, the repulsion between the robots is increased, and the adjusting time is shortened.

(c) If R is detected_jAt R_iDanger zone Z of₃At the moment, because the distance between the robots is very close and a great collision risk exists, the low-priority robot can directly stop moving, meanwhile, the high-priority robot considers the low-priority robot as a static obstacle, and the high-priority robot adjusts the advancing direction through repulsive force generated by the low-priority robot until R is detected_jEscape hazard zone Z₃Thereafter, the low priority robot resumes motion.

(d) If R is detected_jAt R_iBut at a distance D from each other_ijVery recently, i.e. D_ij≤D₀Wherein D is₀<D₃Although there is no risk of collision between the two robots at this time, if the traveling directions of the two robots suddenly collide during the movement, since the distance between the robots is very close at this time, the robots are highly likely to collide with each other due to insufficient adjustment time. To avoid this, when R is detected_jAt R_iBut the distance between the two is less than a threshold value D₀When the robot moves, the repulsion force is still generated between the robots, but the moving speed of the robot and the repulsion force is not influenced, so that the robots can be ensured to moveWill not be too close.

Fig. 19 is a schematic diagram of a collision avoidance process of three robots according to an embodiment of the present invention, in which (a) in fig. 19 is a schematic diagram of initial positions of the three robots, (b) is a schematic diagram of collision avoidance performed between a first robot and a second robot, and (c) is a schematic diagram of collision avoidance performed between the first robot and a third robot.

Specifically, as shown in fig. 19, the robot R₁Dangerous distance D of (first robot)₁Two robots R exist in the range₂(second robot) and R₃(third robot), assume priority R of robot₁>R₂>R₃At this time, the robot R is low in priority₂、R₃Are all located at the high priority robot R₁Danger zone Z of₁Therefore v is₂(moving speed of second robot) and v₃(moving speed of the third robot) decreases while R is decreased₁And R₂R is₁And R₃A repulsive force is generated therebetween. As is clear from FIG. 19 (a), v is₂Less than v₁(speed of movement of first robot) after, R₁And R₂Will soon be greater than the danger distance D₁Reaches the position shown in (b) of FIG. 19, namely, R₂Successful escape from hazard zone Z₁From this it can be seen that R₁And R₂The collision avoidance is realized mainly by adjusting the distance. At the same time, as shown in fig. 19 (a), (b) and (c), R is the same as R₁And R₃The angles of the two robots can be adjusted towards the direction deviating from each other under the action of the repulsive force between the two robots, although the distance between the two robots is gradually reduced in the adjusting process, the advancing direction is effectively adjusted, and finally R is obtained₃Still successfully escape the danger zone Z₂From this it can be seen that R₁And R₃The collision avoidance is realized mainly by adjusting the advancing direction. When collision avoidance is successfully completed, the repulsive force between the robots immediately disappears, and the robots respectively continue to advance towards the target points of the robots.

FIG. 20 is a schematic diagram of a circuit board according to an embodiment of the present inventionThe collision avoidance process between the individual robots is schematically shown, and fig. 20 (a) shows an initial position diagram and (b) shows R₂Into R₁Z of (A)₂A schematic diagram of a zone, and (c) a schematic diagram of the completion of collision avoidance.

Specifically, as shown in (a) of fig. 20, a dot G₂Representative robot R₂In this case, although the robot R₁And R₂Distance D of₃<D_ij≤D₂However, since there is no conflict in the directions of travel, neither is in the danger zone of the other. Since the robot R₂Target point G₂The advancing directions of the two robots are in conflict after the advancing directions of the two robots cross a parallel critical point, and R is₂Will directly enter into R₁Z of (A)₂Zone, as shown in (b) of FIG. 20, so the system will go directly into Z₂The adjustment stage of the region, R, is finally shown in (c) of FIG. 20₂Free to escape R₁The danger zone of (2).

Fig. 21 is a schematic view of another collision avoidance process between two robots according to an embodiment of the present invention, in which (a) of fig. 21 shows an initial position schematic view, and (b) shows an R view₂Into R₁Z of (A)₃A schematic diagram of a zone, and (c) a schematic diagram of the completion of collision avoidance.

Specifically, as shown in (a) of fig. 21, the robot R₁And R₂Distance D of₀<D_ij≤D₃Also, since there is no conflict in the direction of advance, neither robot is in the risk zone of the other. At this time, the robot R₂Target point G₂The forward direction of the suction (2) gradually shifts to the left, and as shown in fig. 21 (b), when the forward directions of the suction (R) and the suction (R) cross a parallel critical point, the forward directions of the suction (R) and the suction (R) collide with each other₂Will directly enter into R₁Z of (A)₃Region, R, since there is a great risk of collision at this time₂Will stop moving directly, i.e. v ₂0 until R is detected₂Escape R₁Danger zone Z of₃After, R₂Just willThe movement is restarted as shown in (c) of fig. 21. Then passes through Z₂And adjusting the area, and finally successfully escaping from the dangerous area.

According to the adjustment process, the conflict of the advancing directions of the robots is not eliminated in the adjustment of the two current stages, or when the advancing directions of the two robots which are very close to each other suddenly conflict, the robots can be ensured not to collide through the two last stages, so that the safe and reliable collision avoidance between the robots is realized.

Optionally, fig. 22 is a flowchart of a path planning method for any one robot in a multi-robot system according to an embodiment of the present invention. As shown in fig. 22, if the robot R_iIs not in the hazard zone and R_iWith any other robot R_jIs not less than D₀And respectively planning the moving direction and the moving speed of the robot by an artificial potential field (namely a preset artificial potential field model) and a fuzzy reasoning system (namely a fuzzy control algorithm). If R is_iAnd R_jIs less than D₀Or R is_iIn the danger zone, a collision avoidance strategy is implemented to plan the moving direction and speed of the robot.

As can be seen from the above description, the multi-robot collaborative path planning method based on the artificial potential field according to the embodiment of the present invention includes: acquiring position coordinates and motion directions of a plurality of robots on a motion plane; judging whether the first robot and the second robot have collision risks or not based on the position coordinates and the motion direction; the first robot and the second robot are respectively different robots in the plurality of robots; and if so, respectively correcting the motion directions of the first robot and the second robot based on a preset artificial potential field model. According to the invention, whether collision risks exist among robots is judged firstly, and then the movement direction of the robots with the collision risks is corrected based on the preset artificial potential field model, so that safe and reliable collision avoidance among the robots is realized, and the technical problem that collision possibly occurs among different robots when multiple robots cooperatively work in the prior art is solved. Compared with other methods in the prior art, the collision avoidance strategy between the robots provided by the embodiment of the invention is simple, effective and easy to implement.

Example two:

in order to verify the effectiveness of the multi-robot collaborative path planning method based on the artificial potential field, the embodiment of the invention utilizes MATLAB to perform a series of simulations.

In the simulation experiment, different multi-robot system parameters, such as the number of robots, the number, position and shape of obstacles, the initial position target position of each robot, and the like, may be set. Fig. 23 is a schematic diagram of a collision process of two robots when there is no obstacle in the environment, fig. 24 is a schematic diagram of a collision process of three robots when there is no obstacle in the environment, fig. 25 is a schematic diagram of a collision process of two robots when there is a simple obstacle in the environment, fig. 26 is a schematic diagram of a collision process of three robots when there is a simple obstacle in the environment, fig. 27 is a schematic diagram of a collision process of two robots when there is a complex obstacle in the environment, and fig. 28 is a schematic diagram of a collision process of three robots when there is a complex obstacle in the environment. In the figure R_iRepresents the ith robot, S_iRepresents R_iStarting point of (1), G_iRepresents R_iThe target point of (1). Examples of this aspect₁Set as the highest priority robot, R₂Next, R₃Each robot is 10 × 10 cells in size, the motion plane is 500 × 500 cells in size, and all robots initially advance in directions toward respective target points.

The collision avoidance process between two robots when there is no obstacle is shown in fig. 23, where S is₁＝(150,350)，G₁＝(350,150)，S₂＝(350,150)，G₂The starting points of the two robots are the end points of the other robot respectively (150,350), so that the advancing directions of the two robots are in conflict with each other certainly during the operation process. In the position shown in fig. 23 (a), the robot R₁And R₂The other side is detected to enter the dangerous area of the other side, and then a collision avoidance strategy is executed. As shown in (b) of FIG. 23, since R₁Is located at the center point of R₂Left side, so R₁To R₂A repulsive force is generated to the right,make R₂Steering to the right, R₂Is also located at R₁Left side of, so R₂Also to R₁Generating a repulsive force to the right to make R₁And turning to the right. When the robot moves to the position shown in the diagram (c) in fig. 23, the advancing directions of the robot and the robot do not collide with each other, and the corresponding collision risk also disappears, but because the robot and the robot are close to each other, D_ij<D₀So that repulsive force still exists until the distance between the two is greater than the threshold value D₀. Fig. 23 (d) shows the complete movement trajectories of the two robots, and it can be seen from the trajectory diagrams that the two robots successfully achieve collision avoidance and safely reach their respective target points.

Fig. 24 is a schematic view of the collision avoidance process between three robots when there is no obstacle, and the start point and the target point of each robot are shown in (d) of fig. 24. In the position shown in fig. 24 (a), each robot enters the danger zone of the remaining two robots, so that a repulsive force is generated between the robots, and R is generated₃Distance R₁Recently, R₁Mainly by R₃Influence of repulsive force, so that the robot R₁It will turn to the right. In the same way, R₃Mainly by R₁Influence of repulsive force, so that the robot R₃But also to the right. Robot R₃The distance to the other two robots is far and the size is approximately the same, so R₃There is no significant change in direction. At the same time, since R₁Is highest, so R₂And R₃Will drop to less than R₁Is also seen from the trajectory of the robot movement in the figure, when R is present₂And R₃Has a relatively dense, i.e. relatively slow, R₁The trajectory of (a) is sparse, i.e. fast. At the position shown in the diagram (b) in FIG. 24, R₁And R₃The collision risk of has disappeared, collision avoidance is successfully completed, and R is now₁、R₂And R is₂And R₃There is still a risk of collision between, and R₁And R₃Are all at R₂So that robot R₂Will turn to the left, R₁And R₃Turn right until the robot movesWhen the line reaches the position shown in the diagram (c) in FIG. 24, R₁And R₂The risk of collision of (2) disappears, and R₂And R₃There is still a risk of collision between, due to R₂Has a higher priority than R₃Priority of (1), so R₃Will fall below R₂The two are then adjusted to the advancing direction by the repulsive force generated by the artificial potential field method until R₁And R₂When moved to the position shown in the graph (d) in FIG. 24, R₁And R₂The risk of collision of (D) disappears, and_ij≥D₀therefore, at this time, the collision avoidance process between the three robots is finished, then the repulsive force between the robots disappears, and the robots respectively continue to advance toward their own targets, and the diagram (d) in fig. 24 is a complete motion trajectory of the three robots, and it can be known from the trajectory diagram that collision avoidance can be smoothly achieved between the three robots, and the robots safely reach their respective target points.

FIG. 25 is a schematic view of collision avoidance process between two robots with an obstacle, wherein S₁＝(450,230)，G₁＝(160,230)，S₂＝(130,230)，G₂As shown in fig. 25 (d), 420,230. When two robots start to move, as the obstacles are detected at the front close positions of the two robots, the speeds planned by the fuzzy inference system are small, and meanwhile, the obstacles generate repulsion force on the robots to prevent the robots from colliding with the obstacles, wherein the robot R₁The front left is closer to the obstacle, so points at R₁Greater repulsion on the right, R₁Can turn to the right, and the robot R₂The right front is closer to the obstacle, so pointing to R₂Greater repulsion to the left, R₂It will turn to the left. When the two robots move to the positions shown in the diagram (a) in fig. 25, the robot R₁And R₂It is detected that the counterpart enters its own danger zone, and a collision avoidance strategy is performed to adjust the respective advancing directions using the repulsive force between the machines, as shown in (b) of fig. 25. When the two robots move to the positions shown in the diagram (c) in fig. 25, the robot R₁And R₂The collision risk disappears because the two are far away from each otherClose, so the repulsive force still exists until the distance between the two is greater than the threshold value D0. Fig. 25 (d) shows a complete path trajectory between two robots, and it can be seen from the trajectory diagram that the two robots not only safely avoid the obstacle, but also correctly detect the collision risk between the robots and implement the collision avoidance strategy, successfully avoid the other moving robot, and successfully reach their respective targets.

Fig. 26 is a schematic diagram of a collision avoidance process between three robots with an obstacle, and the starting point and the target point of each robot are shown in fig. 26 (d). Also, when the robots are just beginning to move, the speeds of all three robots are small because obstacles are detected in their close front. In the position shown in fig. 26 (a), the robot R₂And R₃All enter R₁Due to R in the danger zone₁Is highest, so R₂And R₃Will drop to less than R₁While adjusting the respective advancing directions by means of the repulsive force between the machines. When the robot moves to the position shown in fig. 26 (b), R₁And R₂The collision risk disappears, collision avoidance is smoothly completed, and R is at this time₁And R₃There is still a risk of collision between them, and then both execute the collision avoidance strategy until both travel to the position shown in (c) of fig. 26, R₁And R₃The collision risk disappears and the collision avoidance process between the three robots is finished at this time. But since then R₁And R₃The distance between the two robots is still relatively close, so that the repulsive force between the two robots does not disappear immediately until the distance between the two robots is greater than the threshold value D₀. The diagram (d) in fig. 26 shows the complete path planning of the three robots, and it can be seen that collision avoidance is successfully achieved among the three robots, and the three robots safely reach respective target points.

When the obstacles exist, the obstacles comprise U-shaped obstacles, annular obstacles, star-shaped obstacles and the like, and the positions of the obstacles are randomly distributed. The starting point and the target point of each robot are shown in (d) of fig. 27. In the position shown in fig. 27 (a), the two robots respectively detect that the other is located in their dangerous areasI.e., there is a collision risk between the robots, and thus the two generate a repulsive force, thereby adjusting the advancing direction, in fig. 27, (b) is a process of adjusting the direction, when R is₁And R₂The collision risk between the two robots is eliminated, but the repulsion force between the two robots is not eliminated immediately until the distance between the two robots is larger than the threshold value D because the distance between the two robots is still close₀As shown in fig. 27 (c). FIG. 27 (d) shows a robot R₁And R₂A complete motion trajectory.

The collision avoidance process between the three robots when there is an obstacle is shown in fig. 28, and the start point and the target point of each robot are shown in fig. 28 (c). FIG. 28 (a) shows a robot R₁And R₃Schematic diagram of the collision avoidance process, when R₁And R₃There is no collision between the two robots, i.e. the collision risk has disappeared, but since the distance between the two robots is still close, the repulsive force between the two robots does not disappear immediately until the distance between the two robots is greater than the threshold D₀. FIG. 28 (b) shows a robot R₂And R₃Schematic diagram of the collision avoidance process, at this time, R₂Is located at R₃Left side of, so R₂To R₃Generating a repulsive force to the right to make R₃Turning to the right, R₃Is also located at R₂Left side of, so R₃Also to R₂Generating a repulsive force to the right to make R₂Turn to the right until the risk of collision between the two disappears. FIG. 28 (c) is a view of the robot R₁、R₂And R₃Schematic diagram of the complete motion trajectory.

As can be seen from fig. 23 to 28, with the improved artificial potential field method and the fuzzy inference system, the robot can smoothly bypass obstacles in the environment, collision avoidance between robots can be well achieved with the collision avoidance strategy, and multi-robot path planning is well achieved by combining the two.

The multi-robot collaborative path planning method based on the artificial potential field further solves the problem that a U-shaped obstacle cannot be bypassed by a traditional artificial potential field method. As can be seen from fig. 29 (a) and (b), with the method provided by the embodiment of the present invention, the robot can directly bypass shallow U-shaped obstacles without entering them. As can be seen from fig. 29 (c) and (d), for a deeper U-shaped obstacle, the robot can quickly escape without falling into a local minimum and entering a dead cycle, although the robot can enter the inside of the obstacle.

Example three:

fig. 30 is a schematic diagram of a multi-robot collaborative path planning system based on an artificial potential field according to an embodiment of the present invention. As shown in fig. 30, the system includes: the device comprises an acquisition module 10, a judgment module 20 and a correction module 30.

Specifically, the acquiring module 10 is configured to acquire position coordinates and a moving direction of the plurality of robots in a moving plane.

The judging module 20 is configured to judge whether the first robot and the second robot have a collision risk based on the position coordinate and the moving direction; the first robot and the second robot are respectively different robots among the plurality of robots.

And a correcting module 30, configured to correct the motion directions of the first robot and the second robot based on a preset artificial potential field model if it is determined that there is a collision risk between the first robot and the second robot.

The multi-robot collaborative path planning system based on the artificial potential field provided by the embodiment of the invention firstly judges whether the robots have collision risks, and then corrects the motion direction of the robots with the collision risks based on the preset artificial potential field model, thereby realizing safe and reliable collision avoidance between the robots and relieving the technical problem that the different robots may collide when the multiple robots work collaboratively in the prior art.

Optionally, the system further comprises: the speed adjusting module 40 is configured to obtain priorities of the first robot and the second robot if it is determined that the first robot and the second robot have a collision risk; and adjusting the movement speeds of the first robot and the second robot based on the priority, so that the movement speed of the robot with high priority is greater than the movement speed of the robot with low priority.

Optionally, the determining module 20 is further configured to:

dividing a motion plane into four subareas by taking the position coordinate of the first robot as a central point and taking a first straight line and a second straight line which pass through the central point; the first straight line is a straight line parallel to the movement direction of the first robot, and the second straight line is a straight line parallel to the movement direction of the second robot; determining a target distance between the first robot and the second robot based on the coordinate position of the first robot and the coordinate position of the second robot; judging whether the first robot and the second robot have collision risks or not based on the partition where the second robot is located, the target distance, the motion direction of the first robot and the target included angle; the target included angle is an included angle between the moving direction of the first robot and the moving direction of the second robot.

Optionally, the modification module 30 is further configured to:

determining the attraction of the target point to the first robot based on a preset artificial potential field model; based on a preset artificial potential field model, taking the second robot as an obstacle, and determining repulsion force generated by the second robot to the first robot; summing the attraction force and the repulsion force to obtain a resultant force applied to the first robot; determining a steering angle of the first robot based on the resultant force and the movement speed of the first robot; the direction of movement of the first robot is corrected based on the steering angle.

Optionally, the mathematical expression of the preset artificial potential field model includes:

and:

wherein, F_att[x,y]A vector k representing components of the attraction of the target point to the robot in the x-axis and y-axis directions_attDenotes a gravitational gain coefficient, d_gRepresenting the distance of the robot to the target point, theta_gRepresenting the angle between the line connecting the robot to the target point and the x-axis, d₀Is a preset distance value; f_rep[x,y]A vector k representing components of the repulsive force generated by the obstacle to the robot in the directions of the x axis and the y axis_repDenotes a repulsive force gain coefficient, theta denotes a moving direction of the robot, k is a constant greater than 0, d_fl、d_fr、d_l、d_rAnd d_fRespectively represents the distance from the robot to the obstacle in front left, front right, left, right and front_flIs a first predetermined gain constant, k_frIs a second predetermined gain constant, and k_fl>1，k_fr>1。

Optionally, the steering angle of the first robot is determined by the following equation:

The embodiment of the present invention further provides an electronic device, which includes a memory, a processor, and a computer program stored in the memory and capable of running on the processor, and when the processor executes the computer program, the steps of the method in the first embodiment are implemented.

Finally, it should be noted that: the above embodiments are only used to illustrate the technical solution of the present invention, and not to limit the same; while the invention has been described in detail and with reference to the foregoing embodiments, it will be understood by those skilled in the art that: the technical solutions described in the foregoing embodiments may still be modified, or some or all of the technical features may be equivalently replaced; and the modifications or the substitutions do not make the essence of the corresponding technical solutions depart from the scope of the technical solutions of the embodiments of the present invention.

Claims

1. A multi-robot collaborative path planning method based on an artificial potential field is characterized by comprising the following steps:

acquiring position coordinates and motion directions of a plurality of robots on a motion plane;

judging whether the first robot and the second robot have collision risks or not based on the position coordinates and the motion direction; the first robot and the second robot are respectively different robots of the plurality of robots;

and if so, respectively correcting the motion directions of the first robot and the second robot based on a preset artificial potential field model.

2. The method of claim 1, wherein if it is determined that the first robot and the second robot are at risk of collision, the method further comprises:

acquiring the priority of the first robot and the priority of the second robot;

3. The method of claim 1, wherein determining whether the first robot and the second robot are at risk of collision based on the position coordinates and the direction of motion comprises:

dividing the motion plane into four subareas by using a first straight line and a second straight line which pass through the central point by taking the position coordinate of the first robot as the central point; the first straight line is a straight line parallel to the movement direction of the first robot, and the second straight line is a straight line parallel to the movement direction of the second robot;

determining a target distance between the first robot and the second robot based on the coordinate position of the first robot and the coordinate position of the second robot;

judging whether the first robot and the second robot have collision risks or not based on the partition where the second robot is located, the target distance, the motion direction of the first robot and the target included angle; the target included angle is an included angle between the moving direction of the first robot and the moving direction of the second robot.

4. The method of claim 3, wherein determining whether the first robot and the second robot have a collision risk based on the partition in which the second robot is located, the target distance, the moving direction of the first robot, and the target included angle comprises:

determining at least one risk zone among the four zones based on the movement direction of the first robot and the target included angle;

judging whether the partition where the second robot is located is the risk partition or not, and whether the target distance is smaller than a preset distance or not;

and if so, judging that the first robot and the second robot have collision risks.

5. The method of claim 1, wherein modifying the direction of motion of the first robot based on a preset artificial potential field model comprises:

determining the attraction of the target point to the first robot based on a preset artificial potential field model;

based on the preset artificial potential field model, taking the second robot as an obstacle, and determining repulsion force generated by the second robot on the first robot;

summing the attraction force and the repulsion force to obtain a resultant force applied to the first robot;

determining a steering angle of the first robot based on the resultant force and a movement speed of the first robot;

correcting the moving direction of the first robot based on the steering angle.

6. The method of claim 1 or 5, wherein the mathematical expression of the preset artificial potential field model comprises:

and:

7. The method of claim 5, wherein determining the steering angle of the first robot based on the resultant force and the velocity of motion of the first robot comprises:

determining a steering angle of the first robot by:

8. A multi-robot collaborative path planning system based on an artificial potential field is characterized by comprising: an acquisition module, a judgment module and a correction module, wherein,

the acquisition module is used for acquiring the position coordinates and the movement direction of the plurality of robots on the movement plane;

the judging module is used for judging whether the first robot and the second robot have collision risks or not based on the position coordinates and the motion direction; the first robot and the second robot are respectively different robots of the plurality of robots;

and the correction module is used for correcting the motion directions of the first robot and the second robot respectively based on a preset artificial potential field model if the collision risk between the first robot and the second robot is judged.

9. The system of claim 8, further comprising: the speed adjusting module is used for acquiring the priority of the first robot and the priority of the second robot if the first robot and the second robot are judged to have collision risks; and adjusting the movement speeds of the first robot and the second robot based on the priority, so that the movement speed of the robot with high priority is greater than the movement speed of the robot with low priority.

10. An electronic device comprising a memory, a processor and a computer program stored on the memory and executable on the processor, wherein the steps of the method of any of the preceding claims 1 to 7 are implemented when the computer program is executed by the processor.