CN113510746A - Rapid self-collision detection method with cooperation of multiple mechanical arms - Google Patents
Rapid self-collision detection method with cooperation of multiple mechanical arms Download PDFInfo
- Publication number
- CN113510746A CN113510746A CN202110458532.XA CN202110458532A CN113510746A CN 113510746 A CN113510746 A CN 113510746A CN 202110458532 A CN202110458532 A CN 202110458532A CN 113510746 A CN113510746 A CN 113510746A
- Authority
- CN
- China
- Prior art keywords
- cuboid
- coordinate system
- joint
- coordinates
- axis
- 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
- 238000001514 detection method Methods 0.000 title claims abstract description 31
- 238000000034 method Methods 0.000 claims abstract description 27
- 230000009466 transformation Effects 0.000 claims description 37
- 239000011159 matrix material Substances 0.000 claims description 35
- 239000013598 vector Substances 0.000 claims description 10
- 239000013307 optical fiber Substances 0.000 claims description 3
- 230000001131 transforming effect Effects 0.000 claims description 3
- 230000003287 optical effect Effects 0.000 claims description 2
- 239000007787 solid Substances 0.000 description 3
- 238000010586 diagram Methods 0.000 description 2
- 241000764238 Isis Species 0.000 description 1
- 230000003321 amplification Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000006870 function Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000003199 nucleic acid amplification method Methods 0.000 description 1
- 230000008569 process Effects 0.000 description 1
- 239000011435 rock Substances 0.000 description 1
Images
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J19/00—Accessories fitted to manipulators, e.g. for monitoring, for viewing; Safety devices combined with or specially adapted for use in connection with manipulators
- B25J19/0095—Means or methods for testing manipulators
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Manipulator (AREA)
- Numerical Control (AREA)
Abstract
The invention provides a quick self-collision detection method with cooperation of multiple mechanical arms, which comprises the following steps: amplifying each moving joint of the plurality of mechanical arms and a structural part fixedly connected with the moving joints to be equivalent to a cuboid with maximum envelope, and respectively establishing a joint coordinate system and a cuboid coordinate system; calculating the pose of each cuboid according to the positions of the motion joints of the mechanical arm and the relations among the joint coordinate systems and between the joint coordinate systems and the cuboid coordinate systems; determining the vertex of the cuboid in another cuboid coordinate system according to the pose of the cuboid, and judging whether two cuboids which are not directly connected collide according to whether the vertex coordinates and projections of the two cuboids in different directions are overlapped; repeating until two cuboids collide, otherwise, the multiple mechanical arms do not collide. The invention avoids additionally adding a sensor, has no any limit on the working condition, has low cost, simple calculation and high execution efficiency; the method is suitable for active detection and collision early warning.
Description
Technical Field
The invention relates to the technical field of collision detection, in particular to a rapid self-collision detection method with multiple mechanical arms in cooperation.
Background
With the development of the robot technology, the mechanical arm becomes more and more flexible, and in order to ensure the self-safety of the multi-degree-of-freedom mechanical arm, the mechanical arm is required to have a function of rapid self-collision detection. Many construction machines are equipped with multiple mechanical arms, such as multiple-arm rock drills, arch-mounted trolleys, which require consideration of both collision detection between joints inside a single arm, self-collision detection between multiple working arms, and collision detection between a working arm and a basket arm.
Collision detection based on a force sensor or a current sensor is a common collision detection method for industrial robot arms, but the method needs to install a high-precision sensor, and the cost of the robot arm is increased. The Chinese patent with application number 202010323632.7 discloses a rapid self-collision detection method for a space manipulator, wherein the space manipulator carries out model envelope on a sphere bounding box by adopting a sphere bounding box method, so that the self-collision can be effectively detected in real time aiming at a space robot embedded system under limited resources; and a two-layer collision detection scheme is adopted, so that the effectiveness of the rapid self-collision detection method is ensured. However, most of the members of the mechanical arm are strip-shaped, one moving member needs to be enveloped by a plurality of spheres, and a large overlapping part exists between two adjacent spheres, so that the number of collision bodies is multiplied, and the memory cost and the detection times are increased.
Disclosure of Invention
Aiming at the technical problems that the cost is increased due to the fact that additional sensors are added according to the self-collision detection requirement among multiple mechanical arms in the prior art, the invention provides a rapid self-collision detection method with cooperation of multiple mechanical arms.
In order to achieve the purpose, the technical scheme of the invention is realized as follows: a quick self-collision detection method with multiple mechanical arms in cooperation comprises the following steps:
the method comprises the following steps: amplifying each moving joint of the plurality of mechanical arms and a structural part fixedly connected with the moving joints to be equivalent to a cuboid with maximum envelope, and respectively establishing a joint coordinate system and a cuboid coordinate system;
step two: calculating the pose of each cuboid according to the positions of the motion joints of the mechanical arm and the relations among the joint coordinate systems and between the joint coordinate systems and the cuboid coordinate systems;
step three: determining the vertex of the cuboid in another cuboid coordinate system according to the pose of the cuboid in the step two, and judging whether two cuboids which are not directly connected collide according to whether the vertex coordinates and the projections of the two cuboids in different directions are overlapped;
step four: and repeating the third step until a certain two cuboids collide, otherwise, the multiple mechanical arms do not collide.
Preferably, the joint coordinate system is a joint coordinate system (i, j) of a j-th joint of the ith manipulator; the rectangular parallelepiped coordinate system corresponding to the joint coordinate system (i, j) is a rectangular parallelepiped coordinate system (i, j') which is a coordinate system on the central point of the rectangular parallelepiped of the maximum envelope of the jth joint of the ith manipulator. And the relation coordinate system and the rectangular coordinate system are respectively expressed to realize the relation between the relation coordinate system and the rectangular coordinate system.
Preferably, the coordinate axis direction of the rectangular coordinate system (i, j') is parallel to the edge line of the rectangular solid of the maximum envelope. The determination of coordinates is facilitated.
Preferably, the pose of the rectangular coordinate system (i, j') in the joint coordinate system (i, j) is transformed by a homogeneous transformation matrixAs shown, the length of the cuboid with the maximum envelope along the three axis directions of the cuboid coordinate system (i, j') is expressed asi,j′Lx、i,j′LyAndi,j′Lzwhereini,j′Lxfor the length in the direction of the x-axis,i,j′Lyfor the length in the direction of the y-axis,i,j′Lzis the length along the z-axis. The relationship of the two coordinate systems is conveniently determined by utilizing the homogeneous transformation matrix.
Preferably, the method for calculating the pose of each cuboid in the second step is as follows: based on the current position of each joint on the ith mechanical arm, calculating a joint coordinate system (i, j) of the jth joint of the ith mechanical arm as a homogeneous transformation matrix in the last joint coordinate system (i, j-1)Let the homogeneous transformation matrix of the base of the ith manipulator in the world coordinate system w beThe homogeneous transformation matrix of the cuboid coordinate system of the cuboid with the maximum envelope of the jth joint of the ith mechanical arm in the world coordinate system w isThe pose of the cuboid is determined by utilizing the homogeneous transformation matrix between the coordinate system and the world coordinate system, so that the coordinate of the vertex of the cuboid can be conveniently determined.
Preferably, the method for judging whether two cuboids which are not directly connected collide according to the vertex coordinates comprises the following steps:
step S3.1: setting a coordinate on the central point of the cuboid m as a cuboid coordinate system a, and setting a coordinate on the central point of the cuboid n as a cuboid coordinate system b; calculating to obtain first coordinates of 8 vertexes of the cuboid m in the cuboid coordinate system a according to the lengths of the cuboid m in the three axial directions;
step S3.2: calculating second coordinates of 8 vertexes of the cuboid m in the cuboid coordinate system b according to the homogeneous transformation matrix and the first coordinates between the cuboid coordinate system a and the cuboid coordinate system b;
step S3.3: if any maximum value of the three coordinates in the second coordinates is smaller than the corresponding first coordinates or any minimum value of the three coordinates of the second coordinates is larger than the corresponding first coordinates, the cuboid m and the cuboid n do not collide;
step S3.4: and exchanging the cuboid m and the cuboid n, and repeatedly executing the steps S3.31-S3.3. Whether the cuboids collide is judged by calculating the coordinates of the cuboids, and the calculation is simple.
Preferably, the method for determining the vertex of the rectangular solid in another rectangular solid coordinate system in the third step, i.e. calculating the second coordinate in step S3.2, is as follows:wherein,bPkis the second coordinate of the kth vertex of the cuboid m in the coordinate system b,aPkis a first coordinate of the kth vertex of the cuboid m in the cuboid coordinate system a, k is 1, 2.. 8;is a homogeneous transformation matrix of the rectangular coordinate system a in the rectangular coordinate system b, andwherein,is a homogeneous transformation matrix of a rectangular coordinate system b in a world coordinate system wFor transforming the matrix to homogeneityThe inverse of (a) is,is a homogeneous transformation matrix of a rectangular coordinate system a in a world coordinate system w, r11-r33For elements related to the rotation transformation, px、py、pzAre elements related to the translation transformation.
Preferably, the method for determining that the cuboid m and the cuboid n do not collide in step S3.3 is: either of the following conditions is satisfied:
wherein x is three-coordinatemax、ymax、zmaxRespectively the second coordinatebPkMaximum of x-axis, y-axis, z-axis coordinates of (a), xmin、ymin、zminRespectively the second coordinatebPkThe minimum value of the x-axis, y-axis and z-axis coordinates of the optical fiber;are respectively the first coordinateaPkMinimum of x-axis, y-axis, z-axis coordinates ofThe value of the one or more of the one,are respectively the first coordinateaPkMaximum of x-axis, y-axis, z-axis coordinates of (a). If the vertex coordinates of one cuboid are not in the range of the vertex coordinates of the other cuboid, the two cuboids do not want to collide.
Preferably, the method for judging whether two cuboids which are not directly connected collide according to whether projections of the two cuboids in different directions overlap or not is as follows: selecting a homogeneous transformation matrix between two cuboid coordinatesAnd judging that the projections of the two cuboids on the vectors in the 9 groups of directions are not overlapped, so that the two cuboids do not collide, and the other two cuboids do not collide.
Preferably, the vectors of the 9 sets of directions are respectively: [0 r ]31 -r21]T、[0 r32 -r22]T、[0 r33 -r23]T、[-r31 0 r11]T、[-r32 0 r12]T、[-r33 0 r13]T、[r21 -r11 0]T、[r22 -r12 0]T、[r23 -r13 0]TWhere T is the transpose of the vector.
Compared with the prior art, the invention has the beneficial effects that: the method comprises the steps of enabling each moving component of a plurality of mechanical arms to be equivalent to a cuboid with the largest envelope, calculating the pose of each cuboid in the world coordinate system, which is represented by a homogeneous transformation matrix, according to the variable values of each joint obtained in real time, and then sequentially judging whether two nonadjacent cuboids collide, wherein the number of the cuboids is the same as that of the moving components, and the number of collision bodies does not need to be additionally increased; the collision of the motion members in the two mechanical arms is judged in three steps, firstly 8 vertexes of a cuboid m are judged in a coordinate system b, then whether projections of the two cuboids in 9 groups of directions are overlapped is judged, finally 8 vertexes of a cuboid n are judged in a coordinate system a, the coordinates of the vertexes are directly used for comparing and judging with the edge length of the other cuboid, and the calculation complexity in 6 directions is reduced. The invention is based on the joint position, avoids additionally adding a sensor, has no any limitation on the working condition, and has the advantages of low cost, simple calculation and high execution efficiency. The method is used for the self-collision detection of the cooperation of multiple mechanical arms, and is suitable for active detection and collision early warning.
Drawings
In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings used in the description of the embodiments or the prior art will be briefly described below, it is obvious that the drawings in the following description are only some embodiments of the present invention, and for those skilled in the art, other drawings can be obtained according to the drawings without creative efforts.
FIG. 1 is a schematic flow chart of the present invention.
FIG. 2 is a simplified model schematic of a rectangular parallelepiped envelope of the present invention.
FIG. 3 is a simplified model diagram of a plurality of robotic arm collision volumes in accordance with the present invention.
FIG. 4 is a schematic diagram of a collision detection model between two cuboids according to the present invention.
Detailed Description
The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the drawings in the embodiments of the present invention, and it is obvious that the described embodiments are only a part of the embodiments of the present invention, and not all of the embodiments. All other embodiments, which can be obtained by a person skilled in the art without inventive effort based on the embodiments of the present invention, are within the scope of the present invention.
As shown in fig. 1, a method for detecting a fast self-collision with multiple mechanical arms in cooperation includes the following steps:
the method comprises the following steps: amplifying each moving joint of the plurality of mechanical arms and a structural member fixedly connected with the moving joints to be equivalent to a cuboid with maximum envelope, and respectively establishing a joint coordinate system and a cuboid coordinate system.
The method comprises the steps of properly amplifying each moving joint of a plurality of mechanical arms and a structural member fixedly connected with the moving joints to be equivalent to a maximum enveloping cuboid, obtaining size information of the maximum enveloping cuboid according to a structural drawing of the mechanical arms, and determining the amplification degree according to joint positioning accuracy and safety level of the mechanical arms.
The joint coordinate system is a joint coordinate system (i, j) of the jth joint of the ith mechanical arm, and the origin of the joint coordinate system is positioned on the central line of the joint rotating/moving axis; the rectangular parallelepiped coordinate system corresponding to the joint coordinate system (i, j) is a rectangular parallelepiped coordinate system (i, j') whose coordinate axis direction is parallel to the edge line of the rectangular parallelepiped having the largest envelope and whose origin is the center point of the rectangular parallelepiped, as shown in fig. 2, and can reduce the amount of calculation of coordinate transformation.
The pose of the rectangular coordinate system (i, j') in the joint coordinate system (i, j) is transformed by a homogeneous transformation matrixAs shown, the length of the cuboid with the maximum envelope along the three axis directions of the cuboid coordinate system (i, j') is expressed asi,j′Lx、i,j′LyAndi,j′Lzwhereini,j′Lxfor the length in the direction of the x-axis,i,j′Lyfor the length in the direction of the y-axis,i,j′Lzthe length along the z-axis direction is equal to the edge length of the cuboid in the corresponding direction,related to the structural size of the motion component and the establishment mode of the joint coordinate system (i, j) and the rectangular coordinate system (i, j'), during the motion process of the mechanical armNo change occurs.
In the embodiment shown in fig. 3, there are three arms, each having 7 joints. The joint coordinate system on the first mechanical arm comprises a No. 1 arm base coordinate system (1,0) and 7 joint coordinate systems represented by a coordinate system (1,1) -coordinate system (1,7), the joint coordinate system on the second mechanical arm comprises a No. 2 arm base coordinate system (2,0) and 7 joint coordinate systems represented by a coordinate system (2,1) -coordinate system (2,7), and the joint coordinate system on the third mechanical arm comprises a No. 3 arm coordinate system (3,0) and 7 joint coordinate systems represented by a coordinate system (3,1) -coordinate system (7, 7).
Step two: and calculating the pose of each cuboid according to the positions of the motion joints of the mechanical arm and the relations among the joint coordinate systems and between the joint coordinate systems and the cuboid coordinate systems.
The second step of calculating the pose of each cuboid comprises the following steps: based on the current position of each joint on the ith mechanical arm, calculating a joint coordinate system (i, j) of the jth joint of the ith mechanical arm as a homogeneous transformation matrix in the last joint coordinate system (i, j-1)Let the homogeneous transformation matrix of the base (fixed) of the ith manipulator in the world coordinate system w beThe homogeneous transformation matrix of the cuboid coordinate system of the cuboid with the maximum envelope of the jth joint of the ith mechanical arm in the world coordinate system w isIs a homogeneous transformation matrix of the joint coordinate system (i,1) in the joint coordinate system (i,0) of the base.
Step three: and determining the vertex of the cuboid in another cuboid coordinate system according to the pose of the cuboid in the step two, and judging whether two cuboids which are not directly connected collide according to whether the vertex coordinates and the projections of the two cuboids in different directions are overlapped.
The method for judging whether two cuboids which are not directly connected collide or not according to the vertex coordinates comprises the following steps:
step S3.1: setting a coordinate on the central point of the cuboid m as a cuboid coordinate system a, and setting a coordinate on the central point of the cuboid n as a cuboid coordinate system b; and calculating to obtain the first coordinates of 8 vertexes of the cuboid m in the cuboid coordinate system a according to the lengths of the cuboid m along the three axial directions.
The dimension of the cuboid m along the three axial directions of the coordinate system a is expressed asaLx、aLyAndaLzthe dimension of the cuboid n along the three axial directions of the coordinate system b is expressed asbLx、bLyAndbLz(ii) a Then the first coordinates of the 8 vertexes of the cuboid m in the cuboid coordinate system a are calculated as:
step S3.2: second coordinates of 8 vertexes of the rectangular parallelepiped m in the rectangular coordinate system b are calculated from the first coordinates and the homogeneous transformation matrix between the rectangular coordinate system a and the rectangular coordinate system b.
In the third step, the method for determining the vertex of the cuboid in another cuboid coordinate system, namely calculating the second coordinate in the step S3.2, comprises the following steps:wherein,bPkis the second coordinate of the kth vertex of the cuboid m in the coordinate system b,aPkis a first coordinate of the kth vertex of the cuboid m in the cuboid coordinate system a, k is 1, 2.. 8;is a homogeneous transformation matrix of the rectangular coordinate system a in the rectangular coordinate system b, andwherein,is a homogeneous transformation matrix of a rectangular coordinate system b in a world coordinate system wFor transforming the matrix to homogeneityThe inverse of (a) is,is a homogeneous transformation matrix of a rectangular coordinate system a in a world coordinate system w, r11-r33For 9 elements related to rotation, px、py、pzIs 3 elements associated with translation.
Step S3.3: if any maximum value of the three coordinates in the second coordinates is smaller than the corresponding first coordinates or any minimum value of the three coordinates of the second coordinates is larger than the corresponding first coordinates, the cuboid m and the cuboid n do not collide.
Calculate 8 verticesbPkMaximum value x of three coordinatesmax、ymax、zmaxAnd the minimum value xmin、ymin、zmin。
[1] The method for judging whether the cuboid m and the cuboid n collide in the step S3.3 comprises the following steps: either of the following conditions is satisfied:
case six:wherein x is three-coordinatemax、ymax、zmaxRespectively the second coordinatebPkMaximum of x-axis, y-axis, z-axis coordinates of (a), xmin、ymin、zminRespectively the second coordinatebPkThe minimum value of the x-axis, y-axis and z-axis coordinates of the optical fiber; are respectively the first coordinateaPkThe minimum of the x-axis, y-axis, and z-axis coordinates of the optical system,are respectively the first coordinateaPkMaximum of x-axis, y-axis, z-axis coordinates of (a).
Step S3.4: and exchanging the numbers of the cuboid m and the cuboid n, and repeatedly executing the steps S3.31-S3.3 to judge whether the cuboid m and the cuboid n do not collide.
The method for judging whether two cuboids which are not directly connected collide according to whether projections of the two cuboids in different directions are overlapped or not comprises the following steps: selecting a homogeneous transformation matrix between two cuboid coordinatesVector of middle 9 sets of directions, two longThe projections of the cubes on the vectors in the 9 groups of directions are not overlapped, so that the two cuboids do not collide, and the other two cuboids do not collide.
The vectors of the 9 sets of directions are respectively: [0 r ]31 -r21]T、[0 r32 -r22]T、[0 r33 -r23]T、[-r31 0 r11]T、[-r32 0 r12]T、[-r33 0 r13]T、[r21 -r11 0]T、[r22 -r12 0]T、[r23 -r13 0]TWhere T is the transpose of vectors that can be directly derived from the homogeneous transformation matrix calculated in step S3.2Without requiring additional computations.
Step four: and repeating the third step until a certain two cuboids collide, otherwise, the multiple mechanical arms do not collide.
Repeating the third step until two cuboids collide, which indicates that the multiple mechanical arms collide; if all the cuboid pairs are checked and no collision is found, the multiple mechanical arms do not collide with each other.
The above description is only for the purpose of illustrating the preferred embodiments of the present invention and is not to be construed as limiting the invention, and any modifications, equivalents, improvements and the like that fall within the spirit and principle of the present invention are intended to be included therein.
Claims (10)
1. A quick self-collision detection method with cooperation of multiple mechanical arms is characterized by comprising the following steps:
the method comprises the following steps: amplifying each moving joint of the plurality of mechanical arms and a structural part fixedly connected with the moving joints to be equivalent to a cuboid with maximum envelope, and respectively establishing a joint coordinate system and a cuboid coordinate system;
step two: calculating the pose of each cuboid according to the positions of the motion joints of the mechanical arm and the relations among the joint coordinate systems and between the joint coordinate systems and the cuboid coordinate systems;
step three: determining the vertex of the cuboid in another cuboid coordinate system according to the pose of the cuboid in the step two, and judging whether two cuboids which are not directly connected collide according to whether the vertex coordinates and the projections of the two cuboids in different directions are overlapped;
step four: and repeating the third step until a certain two cuboids collide, otherwise, the multiple mechanical arms do not collide.
2. The multi-robot-arm-coordinated rapid self-collision detection method according to claim 1, wherein the joint coordinate system is a joint coordinate system (i, j) of a j-th joint of an ith robot arm; the rectangular parallelepiped coordinate system corresponding to the joint coordinate system (i, j) is a rectangular parallelepiped coordinate system (i, j') which is a coordinate system on the central point of the rectangular parallelepiped of the maximum envelope of the jth joint of the ith manipulator.
3. The multi-robot-arm-coordinated rapid self-collision detection method according to claim 2, wherein the coordinate axis direction of the rectangular coordinate system (i, j') is parallel to the ridge line of the rectangular parallelepiped with the largest envelope.
4. The multi-robot-arm-coordinated rapid self-collision detection method according to claim 3, wherein the pose of the rectangular coordinate system (i, j') in the joint coordinate system (i, j) is transformed by a homogeneous transformation matrixAs shown, the length of the cuboid with the maximum envelope along the three axis directions of the cuboid coordinate system (i, j') is expressed asi,j′Lx、i,j′LyAndi,j′Lzwhereini,j′Lxfor the length in the direction of the x-axis,i,j′Lyfor the length in the direction of the y-axis,i,j′Lzis the length along the z-axis.
5. The multi-mechanical-arm-cooperated rapid self-collision detection method according to claim 4, wherein the method for calculating the pose of each cuboid in the second step is as follows: based on the current position of each joint on the ith mechanical arm, calculating a joint coordinate system (i, j) of the jth joint of the ith mechanical arm as a homogeneous transformation matrix in the last joint coordinate system (i, j-1)Let the homogeneous transformation matrix of the base of the ith manipulator in the world coordinate system w beThe homogeneous transformation matrix of the cuboid coordinate system of the cuboid with the maximum envelope of the jth joint of the ith mechanical arm in the world coordinate system w is
6. The multi-mechanical-arm-coordinated rapid self-collision detection method according to claim 1 or 5, wherein the method for judging whether two cuboids which are not directly connected collide or not according to the vertex coordinates comprises the following steps:
step S3.1: setting a coordinate on the central point of the cuboid m as a cuboid coordinate system a, and setting a coordinate on the central point of the cuboid n as a cuboid coordinate system b; calculating to obtain first coordinates of 8 vertexes of the cuboid m in the cuboid coordinate system a according to the lengths of the cuboid m in the three axial directions;
step S3.2: calculating second coordinates of 8 vertexes of the cuboid m in the cuboid coordinate system b according to the homogeneous transformation matrix and the first coordinates between the cuboid coordinate system a and the cuboid coordinate system b;
step S3.3: if any maximum value of the three coordinates in the second coordinates is smaller than the corresponding first coordinates or any minimum value of the three coordinates of the second coordinates is larger than the corresponding first coordinates, the cuboid m and the cuboid n do not collide;
step S3.4: and exchanging the cuboid m and the cuboid n, and repeatedly executing the steps S3.31-S3.3.
7. The multi-robot-arm-coordinated rapid self-collision detection method according to claim 6, wherein the method for determining the vertex of the rectangular parallelepiped in another rectangular parallelepiped coordinate system in the third step, i.e. calculating the second coordinate in step S3.2, comprises:wherein,bPkis the second coordinate of the kth vertex of the cuboid m in the coordinate system b,aPkis a first coordinate of the kth vertex of the cuboid m in the cuboid coordinate system a, k is 1, 2.. 8;is a homogeneous transformation matrix of the rectangular coordinate system a in the rectangular coordinate system b, andwherein,is a homogeneous transformation matrix of a rectangular coordinate system b in a world coordinate system wFor transforming the matrix to homogeneityThe inverse of (a) is,is a homogeneous transformation matrix of a rectangular coordinate system a in a world coordinate system w, r11-r33For elements related to the rotation transformation, px、py、pzAre elements related to the translation transformation.
8. The multi-manipulator-coordinated rapid self-collision detection method according to claim 7, wherein the method for determining that the cuboid m and the cuboid n do not collide in step S3.3 comprises: either of the following conditions is satisfied:wherein x is three-coordinatemax、ymax、zmaxRespectively the second coordinatebPkMaximum of x-axis, y-axis, z-axis coordinates of (a), xmin、ymin、zminRespectively the second coordinatebPkThe minimum value of the x-axis, y-axis and z-axis coordinates of the optical fiber;are respectively the first coordinateaPkThe minimum of the x-axis, y-axis, and z-axis coordinates of the optical system,are respectively the first coordinateaPkMaximum of x-axis, y-axis, z-axis coordinates of (a).
9. The multi-manipulator-coordinated rapid self-collision detection method according to claim 7, wherein the method for judging whether two cuboids which are not directly connected collide according to whether the projections of the two cuboids in different directions are overlapped is as follows: selecting a homogeneous transformation matrix between two cuboid coordinatesAnd judging that the projections of the two cuboids on the vectors in the 9 groups of directions are not overlapped, so that the two cuboids do not collide, and the other two cuboids do not collide.
10. The method of claim 9The method for detecting the rapid self-collision of the cooperation of multiple mechanical arms is characterized in that vectors in 9 groups of directions are respectively as follows: [0 r ]31 -r21]T、[0 r32 -r22]T、[0 r33 -r23]T、[-r31 0 r11]T、[-r32 0 r12]T、[-r33 0 r13]T、[r21 -r11 0]T、[r22 -r12 0]T、[r23 -r13 0]TWhere T is the transpose of the vector.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110458532.XA CN113510746B (en) | 2021-04-27 | 2021-04-27 | Rapid self-collision detection method with cooperation of multiple mechanical arms |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110458532.XA CN113510746B (en) | 2021-04-27 | 2021-04-27 | Rapid self-collision detection method with cooperation of multiple mechanical arms |
Publications (2)
Publication Number | Publication Date |
---|---|
CN113510746A true CN113510746A (en) | 2021-10-19 |
CN113510746B CN113510746B (en) | 2022-03-29 |
Family
ID=78063471
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202110458532.XA Active CN113510746B (en) | 2021-04-27 | 2021-04-27 | Rapid self-collision detection method with cooperation of multiple mechanical arms |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN113510746B (en) |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPS6040835A (en) * | 1983-08-11 | 1985-03-04 | Mitsubishi Electric Corp | Stopper device |
CN107116540A (en) * | 2016-02-24 | 2017-09-01 | 中国科学院沈阳计算技术研究所有限公司 | A kind of robot collision checking method that structure is surrounded based on SCS |
EP3386686A1 (en) * | 2015-12-08 | 2018-10-17 | KUKA Deutschland GmbH | Method for detecting a collision of a robotic arm having an object, and robot having a robotic arm |
CN111360824A (en) * | 2020-02-27 | 2020-07-03 | 中科新松有限公司 | Double-arm self-collision detection method and computer-readable storage medium |
CN111546377A (en) * | 2020-04-22 | 2020-08-18 | 哈尔滨工业大学 | Rapid self-collision detection method for space manipulator |
-
2021
- 2021-04-27 CN CN202110458532.XA patent/CN113510746B/en active Active
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPS6040835A (en) * | 1983-08-11 | 1985-03-04 | Mitsubishi Electric Corp | Stopper device |
EP3386686A1 (en) * | 2015-12-08 | 2018-10-17 | KUKA Deutschland GmbH | Method for detecting a collision of a robotic arm having an object, and robot having a robotic arm |
CN107116540A (en) * | 2016-02-24 | 2017-09-01 | 中国科学院沈阳计算技术研究所有限公司 | A kind of robot collision checking method that structure is surrounded based on SCS |
CN111360824A (en) * | 2020-02-27 | 2020-07-03 | 中科新松有限公司 | Double-arm self-collision detection method and computer-readable storage medium |
CN111546377A (en) * | 2020-04-22 | 2020-08-18 | 哈尔滨工业大学 | Rapid self-collision detection method for space manipulator |
Also Published As
Publication number | Publication date |
---|---|
CN113510746B (en) | 2022-03-29 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN108705532B (en) | Mechanical arm obstacle avoidance path planning method and device and storage device | |
CN111360824B (en) | Double-arm self-collision detection method and computer-readable storage medium | |
Horsch et al. | Motion planning with many degrees of freedom-random reflections at c-space obstacles | |
WO2023024279A1 (en) | Dynamic obstacle avoidance method and apparatus for robot, and robot | |
CN113618742B (en) | Robot obstacle avoidance method and device and robot | |
CN112318506A (en) | Automatic calibration method, device, equipment, mechanical arm and medium for mechanical arm | |
CN109702751A (en) | A kind of location class of seven freedom series connection mechanical arm is against solution method | |
CN112264991B (en) | Layered rapid on-orbit collision detection method suitable for space manipulator | |
CN111546376A (en) | Rapid self-collision detection method of space combination mechanical arm | |
US20220105627A1 (en) | Singularity-free kinematic parameterization of soft robot manipulators | |
CN111546377A (en) | Rapid self-collision detection method for space manipulator | |
CN113510746B (en) | Rapid self-collision detection method with cooperation of multiple mechanical arms | |
Loloei et al. | Wrench feasible workspace analysis of cable-driven parallel manipulators using LMI approach | |
JP2017131990A (en) | Interference avoidance method | |
Sanders | Real‐time geometric modeling using models in an actuator space and cartesian space | |
Wang et al. | Optimal sensor placement for obstacle detection of manipulator based on relative entropy | |
CN113442144A (en) | Optimal pose determining method and device under constraint, storage medium and mechanical arm | |
US11959743B1 (en) | Dimensionless kinematics calibration method based on virtual point and related apparatus thereof | |
Afaghani et al. | On-line collision avoidance of two command-based industrial robotic arms using advanced collision map | |
Kwon et al. | Adaptive collision checking for continuous robot motions within motion constraints | |
CN114378860B (en) | Generating design technique for soft robot manipulator | |
CN116652972B (en) | Series robot tail end track planning method based on bidirectional greedy search algorithm | |
Qu et al. | Dynamic Viewpoint-Based Obstacle Avoidance of Robotic Arms in Complex Environments | |
CN116038765B (en) | Collision detection method and device for mechanical arm and computer equipment | |
CN117207200B (en) | Method and device for generating working space of mechanical arm and computer equipment |
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 |