CN113510746B - 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 PDF

Info

Publication number
CN113510746B
CN113510746B CN202110458532.XA CN202110458532A CN113510746B CN 113510746 B CN113510746 B CN 113510746B CN 202110458532 A CN202110458532 A CN 202110458532A CN 113510746 B CN113510746 B CN 113510746B
Authority
CN
China
Prior art keywords
cuboid
coordinate system
joint
coordinates
coordinate
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.)
Active
Application number
CN202110458532.XA
Other languages
Chinese (zh)
Other versions
CN113510746A (en
Inventor
牛孔肖
李鹏宇
荆留杰
孙森震
陈帅
武颖莹
郑赢豪
鞠翔宇
刘涛
贾正文
徐受天
康家安
陈强
赵严振
时洋
游宇嵩
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
China Railway Engineering Equipment Group Co Ltd CREG
Original Assignee
China Railway Engineering Equipment Group Co Ltd CREG
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by China Railway Engineering Equipment Group Co Ltd CREG filed Critical China Railway Engineering Equipment Group Co Ltd CREG
Priority to CN202110458532.XA priority Critical patent/CN113510746B/en
Publication of CN113510746A publication Critical patent/CN113510746A/en
Application granted granted Critical
Publication of CN113510746B publication Critical patent/CN113510746B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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)
  • Numerical Control (AREA)
  • Manipulator (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

Rapid self-collision detection method with cooperation of multiple mechanical arms
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 matrix
Figure BDA0003041472570000021
As 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′Lzwherein, in the step (A),i,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)
Figure BDA0003041472570000022
Let the homogeneous transformation matrix of the base of the ith manipulator in the world coordinate system w be
Figure BDA0003041472570000023
The 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
Figure BDA0003041472570000024
The 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:
Figure BDA0003041472570000025
wherein the content of the first and second substances,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;
Figure BDA0003041472570000026
is a homogeneous transformation matrix of the rectangular coordinate system a in the rectangular coordinate system b, and
Figure BDA0003041472570000031
wherein the content of the first and second substances,
Figure BDA0003041472570000032
is a homogeneous transformation matrix of a rectangular coordinate system b in a world coordinate system w
Figure BDA0003041472570000033
For transforming the matrix to homogeneity
Figure BDA0003041472570000034
The inverse of (a) is,
Figure BDA0003041472570000035
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:
Figure BDA0003041472570000036
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;
Figure BDA0003041472570000037
are respectively the first coordinateaPkThe minimum of the x-axis, y-axis, and z-axis coordinates of the optical system,
Figure BDA0003041472570000038
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 coordinates
Figure BDA0003041472570000039
And 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.
Preference is given toAnd the vectors of the 9 groups 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 rectangular coordinate system (i, j') is in the joint coordinate system (i, j)The pose of the user is transformed by a homogeneous transformation matrix
Figure BDA0003041472570000041
As 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′Lzwherein, in the step (A),i,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,
Figure BDA0003041472570000051
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 arm
Figure BDA0003041472570000052
No 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)
Figure BDA0003041472570000053
Let the homogeneous transformation matrix of the base (fixed) of the ith manipulator in the world coordinate system w be
Figure BDA0003041472570000054
The 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
Figure BDA0003041472570000055
Is 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:
Figure BDA0003041472570000056
Figure BDA0003041472570000057
Figure BDA0003041472570000061
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:
Figure BDA0003041472570000062
wherein the content of the first and second substances,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;
Figure BDA0003041472570000063
is a homogeneous transformation matrix of the rectangular coordinate system a in the rectangular coordinate system b, and
Figure BDA0003041472570000064
wherein the content of the first and second substances,
Figure BDA0003041472570000065
is a homogeneous transformation matrix of a rectangular coordinate system b in a world coordinate system w
Figure BDA0003041472570000066
For transforming the matrix to homogeneity
Figure BDA0003041472570000067
The inverse of (a) is,
Figure BDA0003041472570000068
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:
the first condition is as follows:
Figure BDA0003041472570000069
case two:
Figure BDA00030414725700000610
case three:
Figure BDA00030414725700000611
case four:
Figure BDA00030414725700000612
case five:
Figure BDA00030414725700000613
case six:
Figure BDA00030414725700000614
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;
Figure BDA00030414725700000615
Figure BDA0003041472570000071
are respectively the first coordinateaPkThe minimum of the x-axis, y-axis, and z-axis coordinates of the optical system,
Figure BDA0003041472570000072
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 coordinates
Figure BDA0003041472570000073
And 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.
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.2
Figure BDA0003041472570000074
Without 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 (9)

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;
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: exchanging the cuboid m and the cuboid n, and repeatedly executing the steps S3.31-S3.3;
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 matrix
Figure FDA0003505701070000011
As 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′Lzwherein, in the step (A),i,j′Lxfor the length in the direction of the x-axis,i,j′Lyis along the y-axisThe length of the direction,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)
Figure FDA0003505701070000021
Let the homogeneous transformation matrix of the base of the ith manipulator in the world coordinate system w be
Figure FDA0003505701070000022
The 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
Figure FDA0003505701070000023
6. The multi-robot-arm-coordinated rapid self-collision detection method according to claim 1 or 5, wherein 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, comprises:
Figure FDA0003505701070000024
wherein the content of the first and second substances,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;
Figure FDA0003505701070000025
is a homogeneous transformation matrix of the rectangular coordinate system a in the rectangular coordinate system b, and
Figure FDA0003505701070000026
wherein the content of the first and second substances,
Figure FDA0003505701070000027
is a homogeneous transformation matrix of a rectangular coordinate system b in a world coordinate system w
Figure FDA0003505701070000028
For transforming the matrix to homogeneity
Figure FDA0003505701070000029
The inverse of (a) is,
Figure FDA00035057010700000210
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.
7. The multi-mechanical-arm-cooperative rapid self-collision detection method according to claim 6, 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:
Figure FDA00035057010700000211
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;
Figure FDA00035057010700000212
are respectively the first coordinateaPkThe minimum of the x-axis, y-axis, and z-axis coordinates of the optical system,
Figure FDA00035057010700000213
are respectively the first coordinateaPkMaximum of x-axis, y-axis, z-axis coordinates of (a).
8. 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 coordinates
Figure FDA00035057010700000214
And 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.
9. The multi-robot-arm-coordinated rapid self-collision detection method according to claim 8, wherein 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.
CN202110458532.XA 2021-04-27 2021-04-27 Rapid self-collision detection method with cooperation of multiple mechanical arms Active CN113510746B (en)

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 CN113510746A (en) 2021-10-19
CN113510746B true 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)

* Cited by examiner, † Cited by third party
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

Patent Citations (5)

* Cited by examiner, † Cited by third party
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
CN113510746A (en) 2021-10-19

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
WO2023024279A1 (en) Dynamic obstacle avoidance method and apparatus for robot, and robot
CN113021358B (en) Method and device for calibrating origin of coordinate system of mechanical arm tool and electronic equipment
CN113618742B (en) Robot obstacle avoidance method and device and robot
CN112318506A (en) Automatic calibration method, device, equipment, mechanical arm and medium for mechanical arm
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
CN113269835A (en) Industrial part pose identification method and device based on contour features and electronic equipment
CN111203916B (en) Robot joint and geometric body collision detection method
CN111546377A (en) Rapid self-collision detection method for space manipulator
CN113510746B (en) Rapid self-collision detection method with cooperation of multiple mechanical arms
US20220105627A1 (en) Singularity-free kinematic parameterization of soft robot manipulators
CN114147720A (en) Universal inverse kinematics solving method and device for multi-degree-of-freedom mechanical arm
Lee et al. Dynamic simulation of interactive robotic environment
JP2017131990A (en) Interference avoidance method
Sanders Real‐time geometric modeling using models in an actuator space and cartesian space
CN113442144A (en) Optimal pose determining method and device under constraint, storage medium and mechanical arm
Wang et al. Optimal sensor placement for obstacle detection of manipulator based on relative entropy
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
CN116652972B (en) Series robot tail end track planning method based on bidirectional greedy search algorithm
CN114378860B (en) Generating design technique for soft robot manipulator
JPH06238581A (en) Modelling of objective, judgement of interference between objectives, and device therefor
JPH085028B2 (en) Collision determination method for mobile body and collision determination device

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