CN115933688A - Multi-robot cooperative work obstacle avoidance method, system, equipment and storage medium - Google Patents
Multi-robot cooperative work obstacle avoidance method, system, equipment and storage medium Download PDFInfo
- Publication number
- CN115933688A CN115933688A CN202211693044.8A CN202211693044A CN115933688A CN 115933688 A CN115933688 A CN 115933688A CN 202211693044 A CN202211693044 A CN 202211693044A CN 115933688 A CN115933688 A CN 115933688A
- Authority
- CN
- China
- Prior art keywords
- distance field
- robot
- value
- obstacle avoidance
- distance
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 35
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 9
- 239000013598 vector Substances 0.000 claims description 39
- 238000004364 calculation method Methods 0.000 claims description 34
- 101000617130 Homo sapiens Stromal cell-derived factor 1 Proteins 0.000 claims description 21
- 101000684994 Homo sapiens Stromal cell-derived factor 2 Proteins 0.000 claims description 21
- 102100021669 Stromal cell-derived factor 1 Human genes 0.000 claims description 21
- 102100023184 Stromal cell-derived factor 2 Human genes 0.000 claims description 21
- 238000004590 computer program Methods 0.000 claims description 20
- 239000002775 capsule Substances 0.000 claims description 19
- 238000012937 correction Methods 0.000 claims description 8
- 238000001914 filtration Methods 0.000 claims description 7
- ALYNCZNDIQEVRV-UHFFFAOYSA-N 4-aminobenzoic acid Chemical compound NC1=CC=C(C(O)=O)C=C1 ALYNCZNDIQEVRV-UHFFFAOYSA-N 0.000 claims description 6
- 230000015572 biosynthetic process Effects 0.000 claims description 3
- 238000003786 synthesis reaction Methods 0.000 claims description 3
- YZCKVEUIGOORGS-NJFSPNSNSA-N Tritium Chemical compound [3H] YZCKVEUIGOORGS-NJFSPNSNSA-N 0.000 claims 1
- 230000037237 body shape Effects 0.000 claims 1
- 238000001308 synthesis method Methods 0.000 claims 1
- 238000001514 detection method Methods 0.000 abstract description 2
- 238000010586 diagram Methods 0.000 description 11
- 230000006870 function Effects 0.000 description 4
- 238000012545 processing Methods 0.000 description 4
- 238000010146 3D printing Methods 0.000 description 3
- 238000007796 conventional method Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 230000002194 synthesizing effect Effects 0.000 description 2
- YZCKVEUIGOORGS-OUBTZVSYSA-N Deuterium Chemical compound [2H] YZCKVEUIGOORGS-OUBTZVSYSA-N 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
Images
Classifications
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02P—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN THE PRODUCTION OR PROCESSING OF GOODS
- Y02P90/00—Enabling technologies with a potential contribution to greenhouse gas [GHG] emissions mitigation
- Y02P90/02—Total factory control, e.g. smart factories, flexible manufacturing systems [FMS] or integrated manufacturing systems [IMS]
Landscapes
- Manipulator (AREA)
Abstract
The invention discloses a multi-robot cooperative work obstacle avoidance method system, equipment and a storage medium, and belongs to the technical field of robot trajectory planning. Setting bounding boxes for connecting rods and workpieces of the robots, dividing the operation area of the robots into squares, and calculating the coordinates of each vertex of each square; calculating the optimal robot inverse solution and the position of the bounding box of the robot connecting rod for the points on the operation track of each robot; for each bounding box, computing the distance field of the vertices of the square one by one and modifying; and comprehensively comparing the distance field values of all the joint points under all inverse solutions, and taking the posture with the maximum distance field value as the motion posture of the obstacle avoidance path point. According to the method, robot collision detection is carried out on the basis of the bounding box and the space distance field, the value of the distance field is corrected, the path after obstacle avoidance is calculated for the corrected distance field by using a Dijkstra algorithm, and the optimal obstacle avoidance posture of the robot at the obstacle avoidance path point is calculated, so that the obstacle avoidance of multi-robot tracks is realized, the realization cost is low, and the real-time performance is better.
Description
Technical Field
The invention relates to a robot obstacle avoidance method, and belongs to the technical field of robot track planning.
Background
The obstacle avoidance of the robot is an important research content of the robot track planning. The traditional robot obstacle avoidance method is mainly based on the 3d camera real-time point cloud for obstacle avoidance, and the method is high in hardware cost, high in software real-time requirement and complex in real-time realization.
Disclosure of Invention
The invention aims to overcome the defects of the prior art and provides a multi-robot cooperative work obstacle avoidance method, system, equipment and storage medium, which are low in cost and better in real-time performance.
In order to solve the technical problems, the technical scheme adopted by the invention is as follows:
a multi-robot cooperative work obstacle avoidance method comprises the following steps:
step 1: setting bounding boxes for connecting rods and workpieces of the robots, dividing a robot operation area into a plurality of three-dimensional squares, and calculating coordinates of vertexes of each square;
step 2: preliminarily planning the track path of each robot operation, and calculating the optimal robot inverse solution for points on the track;
and step 3: calculating the position of the bounding box of the robot connecting rod according to the obtained inverse solution of the robot;
and 4, step 4: for each bounding box, computing the distance field of the vertices of the square one by one; the distance field of each bounding box is synthesized into the distance field SDF1 for the entire work area,
and 5: setting a safety distance D safe For distance fields SDF1 the value is less than the safe distance D safe Is corrected as follows
d new ＝D safe -d
Obtaining a modified distance field SDF2 value d new (ii) a d is the calculated distance field value;
and 6: for the operation track points of the robot, inquiring the corresponding distance field from the distance field SDF1, and filtering out values smaller than the safe distance D safe The points divide the operation track into a plurality of sections, and the distance field values of all the points in some sections are smaller than the safe distance D safe If so, correcting the section of track;
and 7: for a track segment needing to be corrected, coordinates of a starting point and an end point of the segment are taken out, a corrected distance field value is inquired from a distance field SDF2, the value is used as a path weight, a shortest path is calculated by utilizing a Dijkstra algorithm, and the shortest path is used as an obstacle avoidance path;
and step 8: and calculating all inverse solutions of the robot for the points on the obstacle avoidance path obtained in the last step, acting each inverse solution on a joint axis of the robot, calculating coordinates of each joint point, inquiring distance field values from the distance field SDF2 by using the coordinates of the joint points, comprehensively comparing the distance field values of the joint points under all the inverse solutions, and taking the posture with the largest distance field value as the motion posture of the obstacle avoidance path point.
Further, the shape of the bounding box is a cuboid, a cylinder or a capsule body according to the shape of the robot connecting rod or the workpiece.
Further, in step 4, the distance field is synthesized by: the minimum value of the distance field in all bounding boxes is taken.
Further, in step 4, when the bounding box is a rectangular parallelepiped, the distance field calculating step is as follows:
inputting a vector b consisting of one point p in space, length, width and height, and outputting a distance field value d:
q＝abs(p)-b
d＝length(max(q,0))+min(max(q x ,max(q y ,q z )),0)
in the formula: p is the point coordinates at which the distance field needs to be computed; b is a vector consisting of the length, the width and the height of the cuboid; d is the calculated distance field value; q = (q) x ,q y ,q z ) T Is a calculated intermediate variable; abs represents the absolute value of each component of the calculated vector, length represents the length of the calculated vector, min represents the minimum value of each component of the calculated vector, and max represents the maximum value of each component of the calculated vector.
Further, in step 4, when the bounding box is cylindrical in shape, the distance field calculation step is as follows:
inputting a coordinate a of a point p in space and the centers of two bottom surfaces of a cylinder 2 、b 2 And outputting a distance field value d:
ba 2 ＝b 2 -a 2
pa 2 ＝p-a 2
baba＝dot(ba 2 ,ba 2 )
paba＝dot(pa 2 ,ba 2 )
x＝length(pa 2 *baba-ba 2 *paba-r 2 *baba
y＝abs(paba-baba*0.5-baba*0.5
x 2 ＝x*x
y 2 ＝y*y*baba
d＝sign9d 0 )*sqrt(abs(d 0 ))/baba
in the formula: p is the point coordinates at which the distance field needs to be computed; a is a 2 、b 2 The coordinates of the centers of the two bottom surfaces of the cylinder are shown; r is a radical of hydrogen 2 Is the radius of the cylinder; d is the calculated distance field value; ba 2 、pa 2 、baba、paba、x、y、x 2 、y 2 、temp 1 、temp 2 、d 0 Is a calculated intermediate variable;
abs represents the absolute value of each component of the calculated vector, length represents the length of the calculated vector, min represents the minimum value of each component of the calculated vector, max represents the maximum value of each component of the calculated vector, and dot represents the dot product of two calculated vectors.
Further, in step 4, when the bounding box is a capsule body, the distance field calculation step is as follows:
inputting a spatial point p, the coordinate a of the spherical center of two hemispheres of the capsule body 3 、b 3 Radius of capsule body r 3 And outputting a distance field value d:
ba 3 ＝b 3 -a 3
pa 3 ＝p-a 3
d＝length(pa 3 -ba 3 *h)-r 3
in the formula: p is the point coordinates at which the distance field needs to be computed; a is 3 、b 3 Coordinates of the spherical centers of two hemispheres of the capsule body are obtained; r is 3 Is the radius of the capsule body; d is the calculated distance field value; ba 3 、pa 3 H is the calculated intermediate variable.
abs represents the absolute value of each component of the calculation vector, length represents the length of the calculation vector, min represents the minimum value of each component of the calculation vector, max represents the maximum value of each component of the calculation vector, dot represents the dot product of the two vectors, and clamp (x, 0, 1) represents the bounding of the x value between 0 and 1.
A multi-robot cooperative work obstacle avoidance system comprises:
a grid division calculation module: setting bounding boxes for connecting rods and workpieces of the robots, dividing a robot operation area into a plurality of three-dimensional squares, and calculating coordinates of vertexes of each square;
a trajectory planning module: preliminarily planning the track path of each robot operation, and calculating the optimal robot inverse solution for points on the track;
bounding box position calculation module: calculating the position of the bounding box of the robot connecting rod according to the obtained inverse solution of the robot;
distance field synthesis module: for each bounding box, computing the distance field of the vertices of the square one by one; the distance field of each bounding box is synthesized into the distance field SDF1 for the entire work area,
distance field correction module: setting a safety distance D safe For distance fields SDF1 the value is less than the safe distance D safe Is corrected as follows
d new ＝D safe -d
Obtaining a modified distance field SDF2 value d new (ii) a d is the calculated distance field value;
a track correction module: to robotQuerying the corresponding distance field from the distance field SDF1, and filtering to obtain a value smaller than the safe distance D safe The points divide the operation track into a plurality of sections, and the distance field values of all the points in some sections are smaller than the safe distance D safe Then, the track section needs to be corrected;
obstacle avoidance path determination module: for a track segment needing to be corrected, coordinates of a starting point and an end point of the segment are taken out, a corrected distance field value is inquired from a distance field SDF2, the value is used as a path weight, a shortest path is calculated by utilizing a Dijkstra algorithm, and the shortest path is used as an obstacle avoidance path;
a motion pose determination module: and calculating all inverse solutions of the robot for the points on the obstacle avoidance path obtained in the last step, applying each inverse solution to a joint axis of the robot, calculating coordinates of each joint point, inquiring distance field values from the distance field SDF2 by using the coordinates of the joint points, comprehensively comparing the distance field values of the joint points under all the inverse solutions, and taking the posture with the largest distance field value as the motion posture of the obstacle avoidance path point.
A computer device comprising a processor and a memory, the memory having stored therein a computer program that is loaded and executed by the processor to implement the method of any preceding claim.
A computer-readable storage medium, in which a computer program is stored, which is loaded and executed by a processor to implement the method of any of the above.
A computer program product comprising computer instructions stored in a computer readable storage medium, from which a processor reads and executes the computer instructions to implement the method of any preceding claim.
The invention achieves the following beneficial effects:
according to the method, robot collision detection is carried out on the basis of the bounding box and the space distance field, the value of the distance field is corrected, the path after obstacle avoidance is calculated for the corrected distance field by using a Dijkstra algorithm, and the optimal obstacle avoidance posture of the robot at the obstacle avoidance path point is calculated, so that the obstacle avoidance of multi-robot tracks is realized, the realization cost is low, and the real-time performance is better.
Drawings
FIG. 1 is a schematic diagram of two robots cooperatively performing 3D printing;
FIG. 2 is a schematic diagram of a bounding box for the robot connecting rod, the workpiece and the positioner;
fig. 3 is a schematic diagram of the division of the robot working area into squares.
Detailed Description
The invention is further described below with reference to the accompanying drawings. The following examples are only for illustrating the technical solutions of the present invention more clearly, and the protection scope of the present invention is not limited thereby.
Example 1
The multi-robot cooperative work obstacle avoidance method in the embodiment specifically comprises the following steps:
step 1: the bounding boxes are arranged for connecting rods of the robots, workpieces and other objects, and cuboids, cylinders, capsule bodies and the like can be selected as the bounding boxes according to the shapes of the objects. Dividing a robot work area into n l ×n w ×n h And calculating the coordinates of each vertex of each square.
And 2, step: preliminarily planning the track path of each robot operation according to a conventional method, and calculating the optimal robot inverse solution for points on the track.
And step 3: and calculating the position of the bounding box of the robot connecting rod according to the obtained robot inverse solution.
And 4, step 4: for each bounding box, the distance field of the vertices of the box is computed one by one. Synthesizing the distance field of each bounding box into a distance field SDF1 of the entire work area, the synthesizing method: the distance field minimum in all bounding boxes is taken.
The distance field calculation steps and equations are as follows, respectively, for different bounding box shapes:
1) A cuboid (an input space point p, a vector b consisting of length, width and height, and an output distance field value d):
q＝abs(p)-b
d＝length(max(q,0))+min(max(q x ,max(q y ,q z )),0)
in the formula: p is the point coordinates at which the distance field needs to be computed; b is a vector consisting of the length, width and height of the cuboid; d is the calculated distance field value; q = (q) x ,q y ,q z ) T Is an intermediate variable of calculation, and has no practical meaning.
2) Cylinder (input space one point p, coordinate a of circle center of two bottom surfaces of cylinder 2 、b 2 Output distance field value d):
ba 2 ＝b 2 -a 2
pa 2 ＝p-a 2
baba＝dot(ba 2 ,ba 2 )
paba＝dot(pa 2 ,ba 2 )
x＝length(pa 2 *baba-ba 2 *paba-r 2 *baba
y＝abs(paba-baba*0.5-baba*0.5
x 2 ＝x*x
y 2 ＝y*y*baba
d＝sign(d 0 )*sqrt(abs(d 0 ))/baba
in the formula: p is the point coordinates at which the distance field needs to be computed; a is 2 、b 3 The coordinates of the centers of the two bottom surfaces of the cylinder are taken as the coordinates; r is 2 Is the radius of the cylinder; d is calculatedA distance field value; other letters are intermediate variables of calculation and have no actual meaning; sign () represents a symbol taking a certain number; sqrt () denotes taking the square root of an arithmetic number of a certain number.
3) Capsule body (input space one point p, coordinate a of two semi-sphere center of capsule body) 3 、b 3 Radius of capsule body r 3 Output distance field value d):
ba 3 ＝b 3 -a 3
pa 3 ＝p-a 3
d＝length(pa 3 -ba 3 *h)-r 3
in the formula: p is the point coordinates at which the distance field needs to be computed; a is 3 、b 3 Coordinates of the spherical centers of two hemispheres of the capsule body are obtained; r is 3 Is the radius of the capsule body; d is the calculated distance field value; other letters are intermediate variables of the calculation, with no actual meaning.
Abs in the above three formulas represents the absolute value of each component of the calculation vector, length represents the length of the calculation vector, min represents the minimum value of each component of the calculation vector, max represents the maximum value of each component of the calculation vector, dot represents the dot product of two vectors, and clamp (x, 0, 1) represents that the value of x is limited between 0 and 1;
and 5: setting a safety distance D safe For distance fields SDF1 the value is less than the safe distance D safe Is corrected as follows
d new ＝D safe -d
Obtaining a modified distance field SDF2 value d new 。
Step 6: for the operation track points of the robot, inquiring the corresponding distance field from the distance field SDF1, and filtering out values smaller than the safe distance D safe The points divide the operation track into a plurality of sections, and the distance field values of all the points in some sections are greater than the safety distance D safe The track section is a safe track without entering the safe track sectionCarrying out obstacle avoidance correction; the distance field values of all points in some segments are less than the safety distance D safe If the section of track is an unsafe track, it indicates that the robot collides with another object (another robot, a workpiece, or another object), and the section of track needs to be corrected.
And 7: and for the track segment needing to be corrected, extracting the coordinates of the starting point and the end point of the segment, inquiring the corrected distance field value from the distance field SDF2, using the value as a path weight, calculating the shortest path by using a Dijkstra algorithm, and using the shortest path as an obstacle avoidance path.
And 8: and calculating all inverse solutions of the robot for the points on the obstacle avoidance path obtained in the last step, applying each inverse solution to a joint axis of the robot, calculating coordinates of each joint point, inquiring a distance field value from the distance field SDF2 by using the coordinates of the joint points, comprehensively comparing the distance field values of the joint points under all the inverse solutions (comparing weighted average values, the weights of 1, 2 and 3 axes of the nodes are smaller, and the weights of 4, 5 and 6 axes are larger), and taking the posture with the largest distance field value as the motion posture of the obstacle avoidance path point.
And (1) avoiding the obstacle on the walking track of the robot in steps (1) to (7), and avoiding the obstacle on the corrected track in the posture of the robot in step (8). And (4) completing obstacle avoidance on one robot through the steps, repeating the steps from 2 to 8, and continuing to avoid obstacles on other robots.
The scheme divides the working area into squares, is limited by the accessibility of the robot, the working area of the robot is not particularly large, and the number of the squares of 100 multiplied by 100 is enough to meet the requirements of most projects. And secondly, enveloping the robot connecting rod, the workpiece and other objects by using bounding boxes, wherein the bounding boxes are all very regular objects (such as cuboids, cylinders and capsule bodies), and the calculation methods of the distance fields of different bounding boxes are provided, so that the distance field of the bounding box can be calculated very conveniently and rapidly. In the obstacle avoidance process, whether collision occurs or not is directly realized by comparing the values of the distance field, and an obstacle avoidance path and an obstacle avoidance posture are calculated by using the corrected values of the distance field on the basis. In the whole process, the required calculation storage space is small, the calculation efficiency is high, the programming is convenient to realize, and the method is suitable for scenes of static objects and dynamic objects (such as scenes of cooperative work of multiple robots).
Example 2
In the present embodiment, for example, two robots perform 3D printing simultaneously, as shown in fig. 1, the method includes two robots, a positioner, and a printed workpiece, and the obstacle avoidance method of the invention is described in detail.
Step 1: the bounding box is arranged for the robot connecting rod, the workpiece and the positioner, and the cuboid and the cylindrical bounding box are mainly adopted in the embodiment, as shown in figure 2.
As shown in fig. 3, the robot working area is divided into 100 × 100 × 100 squares (for clarity, 10 squares are shown as a unit in the figure), and the coordinates of the vertices of each square are calculated.
Step 2: preliminarily planning a track path for 3D printing of the robot according to a conventional method, and calculating an optimal robot inverse solution for points on the track.
And 3, step 3: and calculating the position of the bounding box of the robot connecting rod according to the obtained robot inverse solution.
And 4, step 4: for each bounding box, the distance field of the vertices of the square is computed one by one. The distance field of each bounding box is synthesized into the distance field SDF1 of the entire work area, taking the minimum value of the distance fields in all bounding boxes.
And 5: setting a safety distance D safe For distance fields SDF1 the median value is less than D safe Is corrected as follows
d new ＝D safe -d
Obtaining a modified distance field SDF2 value d new 。
And 6: for the operation track points of the robot, inquiring the corresponding distance field from the distance field SDF1, and filtering out values smaller than the safe distance D safe The points divide the operation track into a plurality of sections, and the distance field values of all the points in some sections are greater than the safety distance D safe The track is a safe track withoutObstacle avoidance correction is required to be carried out on the device; the distance field values of all points in some segments are less than the safety distance D safe If the section of track is an unsafe track, it indicates that the robot collides with another object (another robot, a workpiece, or another object), and the section of track needs to be corrected.
And 7: and for the track segment needing to be corrected, extracting the coordinates of the starting point and the end point of the segment, inquiring the corrected distance field value from the distance field SDF2, using the value as a path weight, and calculating the shortest path by utilizing a Dijkstra algorithm, wherein the shortest path is used as an obstacle avoidance path.
And step 8: and (3) calculating all inverse solutions of the robot for the points on the obstacle avoidance path obtained in the last step, applying each inverse solution to a joint shaft of the robot, calculating coordinates of each joint point, inquiring a distance field value from the distance field SDF2 by using the coordinates of the joint points, comprehensively comparing the distance field values of the joint points under all the inverse solutions (comparing a weighted average value, wherein the weight of the 1 and 2 shafts is 0.05, the weight of the 3 shaft is 0.1, the weight of the 4 shaft is 0.2, the weight of the 5 shaft is 0.3, and the weight of the 6 shaft is 0.3), and taking the posture with the largest distance field value as the motion posture of the obstacle avoidance path point.
Example 3
In this embodiment, a multi-robot cooperative work obstacle avoidance system is disclosed, including:
a grid division calculation module: setting bounding boxes for connecting rods and workpieces of the robots, dividing a robot operation area into a plurality of three-dimensional squares, and calculating coordinates of vertexes of each square;
a trajectory planning module: preliminarily planning the track path of each robot operation, and calculating the optimal robot inverse solution for points on the track;
bounding box position calculation module: calculating the position of the bounding box of the robot connecting rod according to the obtained inverse solution of the robot;
distance field synthesis module: for each bounding box, computing the distance field of the vertices of the square one by one; the distance field of each bounding box is synthesized to the distance field SDF1 for the entire region of operation,
distance field correction module: setting a safety distance D safe To, forValue less than a safe distance D in the distance field SDF1 safe Is corrected as follows
d new ＝D safe -d
Obtaining a modified distance field SDF2 value d new (ii) a d is the calculated distance field value;
a track correction module: for the operation track points of the robot, inquiring the corresponding distance field from the distance field SDF1, and filtering out values smaller than the safe distance D safe The points divide the operation track into a plurality of sections, and the distance field values of all the points in some sections are smaller than the safe distance D safe Then, the track section needs to be corrected;
obstacle avoidance path determination module: for a track segment needing to be corrected, coordinates of a starting point and an end point of the segment are taken out, a corrected distance field value is inquired from a distance field SDF2, the value is used as a path weight, a shortest path is calculated by utilizing a Dijkstra algorithm, and the shortest path is used as an obstacle avoidance path;
a motion pose determination module: and calculating all inverse solutions of the robot for the points on the obstacle avoidance path obtained in the last step, applying each inverse solution to a joint axis of the robot, calculating coordinates of each joint point, inquiring distance field values from the distance field SDF2 by using the coordinates of the joint points, comprehensively comparing the distance field values of the joint points under all the inverse solutions, and taking the posture with the largest distance field value as the motion posture of the obstacle avoidance path point.
Example 4
In this embodiment, a computer device is disclosed, where the computer device includes a processor and a memory, and a computer program is stored in the memory, and the computer program is loaded and executed by the processor to implement the multi-robot cooperative work obstacle avoidance method described in embodiment 1 or embodiment 2.
Example 5
In the embodiment, a computer-readable storage medium is disclosed, in which a computer program is stored, and the computer program is loaded and executed by a processor to implement the multi-robot cooperative work obstacle avoidance method described in embodiment 1 or embodiment 2.
Example 6
In this embodiment, a computer program product is disclosed, where the computer program product includes computer instructions, the computer instructions are stored in a computer-readable storage medium, and a processor reads and executes the computer instructions from the computer-readable storage medium, so as to implement the multi-robot cooperative work obstacle avoidance method described in embodiment 1 or embodiment 2.
As will be appreciated by one skilled in the art, embodiments of the present application may be provided as a method, system, or computer program product. Accordingly, the present application may take the form of an entirely hardware embodiment, an entirely software embodiment or an embodiment combining software and hardware aspects. Furthermore, the present application may take the form of a computer program product embodied on one or more computer-usable storage media (including, but not limited to, disk storage, CD-ROM, optical storage, and so forth) having computer-usable program code embodied therein.
The present application is described with reference to flowchart illustrations and/or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of the application. It will be understood that each flow and/or block of the flow diagrams and/or block diagrams, and combinations of flows and/or blocks in the flow diagrams and/or block diagrams, can be implemented by computer program instructions. These computer program instructions may be provided to a processor of a general purpose computer, special purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, create means for implementing the functions specified in the flowchart flow or flows and/or block diagram block or blocks.
These computer program instructions may also be stored in a computer-readable memory that can direct a computer or other programmable data processing apparatus to function in a particular manner, such that the instructions stored in the computer-readable memory produce an article of manufacture including instruction means which implement the function specified in the flowchart flow or flows and/or block diagram block or blocks.
These computer program instructions may also be loaded onto a computer or other programmable data processing apparatus to cause a series of operational steps to be performed on the computer or other programmable apparatus to produce a computer implemented process such that the instructions which execute on the computer or other programmable apparatus provide steps for implementing the functions specified in the flowchart flow or flows and/or block diagram block or blocks.
The above description is only a preferred embodiment of the present invention, and it should be noted that, for those skilled in the art, several modifications and variations can be made without departing from the technical principle of the present invention, and these modifications and variations should also be regarded as the protection scope of the present invention.
Claims (10)
1. A multi-robot cooperative work obstacle avoidance method is characterized by comprising the following steps:
step 1: setting bounding boxes for connecting rods and workpieces of the robots, dividing a robot operation area into a plurality of three-dimensional squares, and calculating coordinates of vertexes of each square;
step 2: preliminarily planning the track path of each robot operation, and calculating the optimal robot inverse solution for points on the track;
and step 3: calculating the position of the bounding box of the robot connecting rod according to the obtained inverse solution of the robot;
and 4, step 4: for each bounding box, computing the distance field of the vertices of the square one by one; the distance field of each bounding box is synthesized into the distance field SDF1 for the entire work area,
and 5: setting a safety distance D safe For distance fields SDF1 the value is less than the safe distance D safe Is corrected as follows
d new ＝D safe -d
Obtaining a modified distance field SDF2 value d new (ii) a d is the calculated distance field value;
step 6: for the operation track points of the robot, inquiring the corresponding distance field from the distance field SDF1, and filtering out values smaller than the safety distanceFrom D safe The points divide the operation track into a plurality of sections, and the distance field values of all the points in some sections are smaller than the safe distance D safe Then, the track section needs to be corrected;
and 7: for a track segment needing to be corrected, coordinates of a starting point and an end point of the segment are taken out, a corrected distance field value is inquired from a distance field SDF2, the value is used as a path weight, a shortest path is calculated by utilizing a Dijkstra algorithm, and the shortest path is used as an obstacle avoidance path;
and 8: and calculating all inverse solutions of the robot for the points on the obstacle avoidance path obtained in the last step, applying each inverse solution to a joint axis of the robot, calculating coordinates of each joint point, inquiring distance field values from the distance field SDF2 by using the coordinates of the joint points, comprehensively comparing the distance field values of the joint points under all the inverse solutions, and taking the posture with the largest distance field value as the motion posture of the obstacle avoidance path point.
2. The obstacle avoidance method through cooperation of multiple robots according to claim 1, wherein the bounding box is in a cuboid, cylinder or capsule shape according to the shape of a robot connecting rod or a workpiece.
3. The multi-robot cooperative work obstacle avoidance method according to claim 1, wherein in the step 4, the distance field synthesis method comprises: the minimum value of the distance field in all bounding boxes is taken.
4. The multi-robot cooperative work obstacle avoidance method according to claim 2, wherein in the step 4, when the bounding box is a cuboid, the distance field calculation step is as follows:
inputting a vector b consisting of one point p in space, length, width and height, and outputting a distance field value d:
q＝abs(p)-b
d＝length(max(q,0))+min(max(q x ,max(q y ,q z )),0)
in the formula: p is the point coordinates at which the distance field needs to be computed; b is the length, width and height of the cuboidA vector of (a); d is the calculated distance field value; q = (q) x ,q y ,q z ) T Is a calculated intermediate variable; abs represents the absolute value of each component of the calculation vector, length represents the length of the calculation vector, min represents the minimum value of each component of the calculation vector, and max represents the maximum value of each component of the calculation vector.
5. The multi-robot cooperative work obstacle avoidance method according to claim 2, wherein in the step 4, when the bounding box is a cylinder, the distance field calculation step is as follows:
inputting a coordinate a of a point p in space and the centers of two bottom surfaces of a cylinder 2 、b 2 And outputting a distance field value d:
ba 2 ＝b 2 -a 2
pa 2 ＝p-a 2
baba＝dot(ba 2 ,ba 2 )
paba＝dot(pa 2 ,ba 2 )
x＝length(pa 2 *baba-ba 2 *paba)-r 2 *baba
y＝abs(paba-baba*0.5)-baba*0.5
x 2 ＝x*x
y 2 ＝y*y*baba
d＝sign9d 0 )*sqrt(abs(d 0 ))/baba
in the formula: p being the distance field to be calculatedPoint coordinates; a is a 2 、b 2 The coordinates of the centers of the two bottom surfaces of the cylinder are shown; r is 2 Is the radius of the cylinder; d is the calculated distance field value; ba 2 、pa 2 、baba、paba、x、y、x 2 、y 2 、temp 1 、temp 2 、d 0 Is a calculated intermediate variable;
abs represents the absolute value of each component of the calculated vector, length represents the length of the calculated vector, min represents the minimum value of each component of the calculated vector, max represents the maximum value of each component of the calculated vector, and dot represents the dot product of two calculated vectors.
6. The multi-robot cooperative work obstacle avoidance method according to claim 2, wherein in step 4, when the bounding box is in a capsule body shape, the distance field calculation step is as follows:
inputting a point p in space, and coordinates a of the spherical centers of two hemispheres of the capsule body 3 、b 3 Radius of capsule body r 3 And outputting a distance field value d:
ba 3 ＝b 3 -a 3
pa 3 ＝p-a 3
d＝length(pa 3 -ba 3 *h)-r 3
in the formula: p is the point coordinates at which the distance field needs to be computed; a is 3 、b 3 Coordinates of the spherical centers of two hemispheres of the capsule body are obtained; r is a radical of hydrogen 3 Is the radius of the capsule body; d is the calculated distance field value; ba 3 、pa 3 H is the calculated intermediate variable.
abs represents the absolute value of each component of the calculation vector, length represents the length of the calculation vector, min represents the minimum value of each component of the calculation vector, max represents the maximum value of each component of the calculation vector, dot represents the dot product of the two vectors, and clamp (x, 0, 1) represents the bounding of the x value between 0 and 1.
7. A multi-robot cooperative work obstacle avoidance system is characterized by comprising:
a grid division calculation module: setting bounding boxes for connecting rods and workpieces of the robots, dividing a robot operation area into a plurality of three-dimensional squares, and calculating coordinates of vertexes of each square;
a trajectory planning module: preliminarily planning the track path of each robot operation, and calculating the optimal robot inverse solution for points on the track;
bounding box position calculation module: calculating the position of the bounding box of the robot connecting rod according to the obtained inverse solution of the robot;
distance field synthesis module: for each bounding box, computing the distance field of the vertices of the square one by one; the distance field of each bounding box is synthesized into the distance field SDF1 for the entire work area,
distance field correction module: setting a safety distance D safe For distance fields SDF1 the value is less than the safe distance D safe Is corrected as follows
d new ＝D safe -d
Obtaining a modified distance field SDF2 value d new (ii) a d is the calculated distance field value;
a track correction module: for the operation track points of the robot, inquiring the corresponding distance field from the distance field SDF1, and filtering out values smaller than the safe distance D safe The points divide the operation track into a plurality of sections, and the distance field values of all the points in some sections are smaller than the safe distance D safe Then, the track section needs to be corrected;
obstacle avoidance path determination module: for a track segment needing to be corrected, coordinates of a starting point and an end point of the segment are taken out, a corrected distance field value is inquired from a distance field SDF2, the value is used as a path weight, a shortest path is calculated by utilizing a Dijkstra algorithm, and the shortest path is used as an obstacle avoidance path;
a motion gesture determination module: and calculating all inverse solutions of the robot for the points on the obstacle avoidance path obtained in the last step, acting each inverse solution on a joint axis of the robot, calculating coordinates of each joint point, inquiring distance field values from the distance field SDF2 by using the coordinates of the joint points, comprehensively comparing the distance field values of the joint points under all the inverse solutions, and taking the posture with the largest distance field value as the motion posture of the obstacle avoidance path point.
8. A computer device, characterized in that the computer device comprises a processor and a memory, in which a computer program is stored, which computer program is loaded and executed by the processor to implement the method according to any of claims 1 to 6.
9. A computer-readable storage medium, in which a computer program is stored which is loaded and executed by a processor to implement the method according to any one of claims 1 to 6.
10. A computer program product comprising computer instructions stored in a computer readable storage medium, from which a processor reads and executes the computer instructions to implement the method of any one of claims 1 to 6.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211693044.8A CN115933688B (en) | 2022-12-28 | 2022-12-28 | Multi-robot cooperative work obstacle avoidance method, system, equipment and storage medium |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211693044.8A CN115933688B (en) | 2022-12-28 | 2022-12-28 | Multi-robot cooperative work obstacle avoidance method, system, equipment and storage medium |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN115933688A true CN115933688A (en) | 2023-04-07 |
| CN115933688B CN115933688B (en) | 2024-03-29 |
Family
ID=86552218
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202211693044.8A Active CN115933688B (en) | 2022-12-28 | 2022-12-28 | Multi-robot cooperative work obstacle avoidance method, system, equipment and storage medium |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115933688B (en) |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP6443905B1 (en) * | 2017-12-28 | 2018-12-26 | 深セン市優必選科技有限公司 | Robot motion path planning method, apparatus, storage medium, and terminal device |
| CN110216670A (en) * | 2019-04-30 | 2019-09-10 | 武汉理工大学 | A kind of industrial robot automatic obstacle-avoiding method and device based on loss field |
| CN110253570A (en) * | 2019-05-27 | 2019-09-20 | 浙江工业大学 | The industrial machinery arm man-machine safety system of view-based access control model |
| WO2020040979A1 (en) * | 2018-08-23 | 2020-02-27 | Realtime Robotics, Inc. | Collision detection useful in motion planning for robotics |
| CN113459109A (en) * | 2021-09-03 | 2021-10-01 | 季华实验室 | Mechanical arm path planning method and device, electronic equipment and storage medium |
| CN115416016A (en) * | 2022-08-15 | 2022-12-02 | 浙江工业大学 | Mechanical arm obstacle avoidance path planning method based on improved artificial potential field method |
| CN115502961A (en) * | 2022-08-02 | 2022-12-23 | 华南理工大学 | Human-machine online obstacle avoidance method and system based on human body arm motion information prediction |
-
2022
- 2022-12-28 CN CN202211693044.8A patent/CN115933688B/en active Active
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP6443905B1 (en) * | 2017-12-28 | 2018-12-26 | 深セン市優必選科技有限公司 | Robot motion path planning method, apparatus, storage medium, and terminal device |
| WO2020040979A1 (en) * | 2018-08-23 | 2020-02-27 | Realtime Robotics, Inc. | Collision detection useful in motion planning for robotics |
| CN110216670A (en) * | 2019-04-30 | 2019-09-10 | 武汉理工大学 | A kind of industrial robot automatic obstacle-avoiding method and device based on loss field |
| CN110253570A (en) * | 2019-05-27 | 2019-09-20 | 浙江工业大学 | The industrial machinery arm man-machine safety system of view-based access control model |
| CN113459109A (en) * | 2021-09-03 | 2021-10-01 | 季华实验室 | Mechanical arm path planning method and device, electronic equipment and storage medium |
| CN115502961A (en) * | 2022-08-02 | 2022-12-23 | 华南理工大学 | Human-machine online obstacle avoidance method and system based on human body arm motion information prediction |
| CN115416016A (en) * | 2022-08-15 | 2022-12-02 | 浙江工业大学 | Mechanical arm obstacle avoidance path planning method based on improved artificial potential field method |
Non-Patent Citations (1)
| Title |
|---|
| 安红瑞: "机械臂抓取过程中的碰撞检测与路径规划研究", 武汉理工大学硕士论文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN115933688B (en) | 2024-03-29 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US8089480B2 (en) | Method for meshing a curved surface | |
| CN111002315B (en) | Trajectory planning method and device and robot | |
| KR20110103221A (en) | Error compensation method for multi-axis controlled machines | |
| CN110722533B (en) | External parameter calibration-free visual servo tracking of wheeled mobile robot | |
| US10650586B2 (en) | Automated mesh generation | |
| CN104070523A (en) | Method for interpolating circular arcs in real time for industrial robots on basis of space coordinate transformation | |
| Hu et al. | Automatic surface roughing with 3D machine vision and cooperative robot control | |
| CN116330267A (en) | Control method based on industrial robot wrist singular point calculation | |
| CN115933688A (en) | Multi-robot cooperative work obstacle avoidance method, system, equipment and storage medium | |
| CN114372940A (en) | Real scene image synthesis method and system | |
| Copot et al. | Image-based and fractional-order control for mechatronic systems | |
| CN114089316A (en) | Combined calibration system, method and medium for laser radar-inertial navigation | |
| CN111283682A (en) | Geometric projection solution of forward kinematics of 4-UPU four-degree-of-freedom parallel robot | |
| Kivelä et al. | On-line path planning with collision avoidance for coordinate-controlled robotic manipulators | |
| CN114800491A (en) | Redundant mechanical arm zero-space obstacle avoidance planning method | |
| CN109773581B (en) | Method for applying robot to reappear machining | |
| Seifried et al. | Integrated design approaches for controlled flexible multibody systems | |
| CN113478495B (en) | Multi-dimensional mechanical arm smooth path planning method | |
| JP2015076026A (en) | Pattern matching device and pattern matching method | |
| CN115879332B (en) | Driving simulator motion platform control method and device, electronic equipment and storage medium | |
| Liu et al. | Hand-eye Calibration of Industrial Robots with 3D Cameras based on Dual Quaternions | |
| Yazicioglu et al. | Image based visual servoing using algebraic curves applied to shape alignment | |
| Liu et al. | Set space visual servoing of a 6-dof manipulator | |
| CN117798938B (en) | Non-singular evaluation control method and device for multi-joint robot | |
| US20230125071A1 (en) | Offline teaching device and motion-program generation method |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |