CN111546331B - Safety protection system and safety protection method for man-machine cooperative robot - Google Patents

Safety protection system and safety protection method for man-machine cooperative robot Download PDF

Info

Publication number
CN111546331B
CN111546331B CN202010306315.4A CN202010306315A CN111546331B CN 111546331 B CN111546331 B CN 111546331B CN 202010306315 A CN202010306315 A CN 202010306315A CN 111546331 B CN111546331 B CN 111546331B
Authority
CN
China
Prior art keywords
motion capture
robot
optical motion
coordinate
optical
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
CN202010306315.4A
Other languages
Chinese (zh)
Other versions
CN111546331A (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.)
Shanghai University of Engineering Science
Original Assignee
Shanghai University of Engineering Science
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 Shanghai University of Engineering Science filed Critical Shanghai University of Engineering Science
Priority to CN202010306315.4A priority Critical patent/CN111546331B/en
Publication of CN111546331A publication Critical patent/CN111546331A/en
Application granted granted Critical
Publication of CN111546331B publication Critical patent/CN111546331B/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
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1674—Programme controls characterised by safety, monitoring, diagnostic
    • B25J9/1676—Avoiding collision or forbidden zones
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1628—Programme controls characterised by the control loop
    • B25J9/1651—Programme controls characterised by the control loop acceleration, rate control
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01B—MEASURING LENGTH, THICKNESS OR SIMILAR LINEAR DIMENSIONS; MEASURING ANGLES; MEASURING AREAS; MEASURING IRREGULARITIES OF SURFACES OR CONTOURS
    • G01B11/00—Measuring arrangements characterised by the use of optical techniques
    • G01B11/002—Measuring arrangements characterised by the use of optical techniques for measuring two or more coordinates
    • G01B11/005—Measuring arrangements characterised by the use of optical techniques for measuring two or more coordinates coordinate measuring machines
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B2219/00—Program-control systems
    • G05B2219/30—Nc systems
    • G05B2219/39—Robotics, robotics to robotics hand
    • G05B2219/39091—Avoid collision with moving obstacles
    • 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

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Manipulator (AREA)

Abstract

The invention provides a safety protection system and a safety protection method for a man-machine cooperation robot, which comprises an optical motion capture camera, a main controller and a robot, wherein the optical motion capture camera acquires the position of the robot and the positions of surrounding obstacles and sends the positions to an optical motion capture camera coordinate system module; the optical motion capture camera coordinate system module calculates three-dimensional coordinates of the robot under the optical motion capture camera coordinate system according to the position of the robot to form first coordinates, and the optical motion capture camera coordinate system module calculates three-dimensional coordinates of surrounding obstacles under the optical motion capture camera coordinate system according to the positions of the surrounding obstacles to form second coordinates; the optical motion capture camera coordinate system module sends the first coordinate and the second coordinate to the main controller; and the main controller calculates the shortest distance between the robot and surrounding obstacles at the current moment according to the first coordinate and the second coordinate, and controls the running speed of the robot according to the shortest distance.

Description

Safety protection system and safety protection method for man-machine cooperative robot
Technical Field
The invention relates to the technical field of intelligent manufacturing, in particular to a safety protection system and a safety protection method for a man-machine cooperation robot.
Background
With the continuous development of science and technology, the automation and intelligent production process of the manufacturing industry is continuously improved, and more factories use robots to assist production. In the production process, the processes of assembly, welding and the like are realized by man-machine cooperation in many times, and in the man-machine cooperation process, the situation that a mechanical arm collides with a person to hurt the person occurs sometimes. Therefore, the safety problem that the robot and the robot work in the same working space is significant. At present, common safety protection methods include adding fences around a mechanical arm, wearing protective devices by workers and the like, but the methods cannot fundamentally solve the safety problem of man-machine cooperation.
Disclosure of Invention
The invention aims to provide a safety protection system and a safety protection method for a human-computer cooperation robot, and aims to solve the problems that the existing human-computer cooperation is not intelligent and unreliable.
In order to solve the above technical problem, the present invention provides a safety protection system for a human-computer cooperation robot, the safety protection system for a human-computer cooperation robot comprises an optical motion capture camera, a main controller and a robot, wherein:
the optical motion capture camera comprises an optical motion capture camera coordinate system module, acquires the position of the robot and the positions of surrounding obstacles, and sends the positions to the optical motion capture camera coordinate system module;
the optical motion capture camera coordinate system module calculates three-dimensional coordinates of the robot in the optical motion capture camera coordinate system based on the position of the robot to form first coordinates, an
The optical motion capture camera coordinate system module calculates three-dimensional coordinates of the surrounding obstacles in the optical motion capture camera coordinate system according to the positions of the surrounding obstacles to form second coordinates;
the optical motion capture camera coordinate system module sending the first coordinates and the second coordinates to the master controller;
and the main controller calculates the shortest distance between the robot and the surrounding obstacles at the current moment according to the first coordinate and the second coordinate, and controls the running speed of the robot according to the shortest distance.
Optionally, in the safety protection system for a human-computer-collaborative robot, the optical motion capture camera includes an infrared camera, a frame rate of the infrared camera is greater than 100Hz, a resolution of the infrared camera is greater than 640 × 480, the infrared camera is provided with a USB interface, and the USB interface is used for outputting a preprocessed image and a grayscale image.
Optionally, in the safety protection system for a human-machine-collaborative robot, the number of the optical motion capture cameras is 6 to 24.
The invention also provides a human-computer cooperation robot safety protection method based on the human-computer cooperation robot safety protection system, which comprises the following steps:
fixing and placing the optical motion capture camera in a field where the robot and the surrounding obstacles are located;
calibrating the optical motion capture camera to obtain a coordinate transformation matrix between the coordinate system of the optical motion capture camera and a world coordinate system;
respectively arranging a plurality of optical mark points on the surfaces of the surrounding obstacles and the surface of the robot, and acquiring the first coordinate and the second coordinate according to the optical mark points;
and converting the first coordinate and the second coordinate into coordinates in a world coordinate system by using the coordinate transformation matrix.
Optionally, in the safety protection method for a human-computer cooperation robot, the safety protection method for a human-computer cooperation robot further includes:
arranging 6 optical motion capture cameras which are respectively fixed on 6 tripods, and uniformly arranging the 6 tripods on a circumference which takes a captured object as a circle center and has a radius of 5-10 meters; or
4 optical motion capture cameras are respectively fixed at the tops of 4 tripods, the other 4 optical motion capture cameras are respectively fixed at the middle parts of the other 4 tripods, and 8 tripods are uniformly arranged on a circumference which takes a captured object as a circle center and has a radius of 5-10 meters; or alternatively
Setting 6 optical motion capture cameras to be respectively fixed at the tops of 6 tripods, setting another 6 optical motion capture cameras to be respectively fixed at the middle parts of another 6 tripods, and uniformly arranging 12 tripods on a circumference which takes a captured object as a circle center and has a radius of 5-10 meters; or alternatively
And arranging a rectangular truss, wherein 4 optical motion capture cameras are respectively fixed at the top end of the rectangular truss, the other 4 optical motion capture cameras are respectively fixed at the middle part of the rectangular truss, and the other 4 optical motion capture cameras are respectively fixed at the bottom of the rectangular truss.
Optionally, in the method for safeguarding a human-machine cooperative robot, the calibrating the optical motion capture camera includes:
a plurality of optical marking balls A 1 ，A 2 ，A 3 ，...，A n Arranging in the venue to enable as many optical marker balls as possible to be displayed within a frame of each of the optical motion capture cameras;
recording a plurality of said optical marker balls A 1 ，A 2 ，A 3 ，...，A n Coordinates p in the optical motion capture camera 1 ，p 2 ，p 3 ，...，p n Wherein p is i ＝[u i ，v i ，1] T ；
A plurality of the optical marker balls A 1 ，A 2 ，A 3 ，...，A n The coordinates under the world coordinate system are:
P i ＝[X i ，Y i ，Z i ，1] T ，
the three-dimensional coordinates of the optical marker ball in the optical motion capture camera coordinate system and the coordinates in the world coordinate system are transformed in relation to:
p i ＝sA[R，t]P i ，
wherein s is a scale factor and [ R, t ] is a rotational translation matrix of the world coordinate system converted to the optical motion capture camera coordinate system;
Figure BDA0002455914050000031
capturing a camera internal coefficient matrix for the optical motion;
(u 0 ，v 0 ) Image reference points for the optical motion capture camera; (α, β) is a length-width scaling factor from the image plane coordinates to the pixel coordinates in the frame buffer; γ is a coefficient of radial distortion of the optical motion capture camera.
Optionally, in the method for safeguarding a robot in cooperation with a human-machine, the calibrating the optical motion capture camera includes: the value Zi is set to 0 and,
Figure BDA0002455914050000041
wherein: h = lambda [ r ] 1 r 2 t]，
Figure BDA0002455914050000042
A system of linear equations can be obtained:
Figure BDA0002455914050000043
solving the external parameters as follows:
r 1 ＝λA -1 h 1
r 2 ＝λa -1 h 2
r 3 ＝r 1 ×r 2
t＝λA -1 h 3
Figure BDA0002455914050000044
optionally, in the safety protection method for a human-computer cooperative robot, after calibration is completed, a plurality of optical mark points are respectively arranged on the surfaces of the surrounding obstacles and the surface of the robot, and three-dimensional coordinates of the optical mark points of the surrounding obstacles in the optical motion capture camera coordinate system are the second coordinates:
Figure BDA0002455914050000045
the three-dimensional coordinates of the optical marker point of the robot in the optical motion capture camera coordinate system are the first coordinates:
Figure BDA0002455914050000046
calculating the distance between the surrounding obstacle and the robot according to the first coordinate and the second coordinate:
Figure BDA0002455914050000051
the surrounding barriers are human bodies, safety vests or safety helmets are worn on the human bodies, a plurality of optical mark points are arranged on the front and the back of each safety vest, and a plurality of optical mark points are arranged on the tops of the safety helmets and the brim of each safety helmet.
Optionally, in the safety protection method for the human-computer cooperative robot, the distance between every two optical mark points in each optical mark point of the first coordinate and the second coordinate is calculated to form a distance matrix,
Figure BDA0002455914050000052
calculating the shortest distance:
L min ＝minD m，n ；
when the shortest distance is smaller than a first preset threshold value, controlling the running speed of the robot to be zero; when the shortest distance is larger than a second preset threshold value, controlling the robot to run at the maximum speed; when the shortest distance is between the first preset threshold and the second preset threshold, controlling the running speed of the robot to be as follows:
Figure BDA0002455914050000053
wherein d is the shortest distance, d 1 Is the first predetermined threshold value, d 2 Is said second predetermined threshold, d 2 ＞d 1 ，v max Is the maximum operating speed of the robot.
Optionally, in the safety protection method for a human-computer cooperative robot, each optical mark point in the first coordinate is set as a first mark, each optical mark point in the second coordinate is set as a second mark, the first coordinate and the second coordinate are combined into a third coordinate, and the third coordinate is rapidly sorted according to an abscissa value;
and dividing the elements in the third coordinate into two sets with equal element numbers according to the x coordinate values, wherein the two sets are respectively as follows:
S 1 = { p ∈ S | x (p) ≦ kx }, and
S 2 ＝{p∈S|x(p)＞kx}，
respectively calculate S 1 And S 2 Is d 1 And d 2 Let d = min { d { 1 ，d 2 }；
Let P 1 ＝{p∈S 1 Ix (p) > kx-d }, and
P 2 ＝{p∈S 2 |x(p)＜kx+d}；
combining the P1 and the P2 into a fourth coordinate, and dividing elements in the fourth coordinate into two sets with equal element numbers according to a y coordinate value, wherein the two sets are as follows:
Q 1 ＝{p∈P 1 ∪P 2 y (p) is less than or equal to ky }, and
Q 2 ＝{p∈P 1 ∪P 2 |y(p)＞ky}，
respectively calculate Q 1 And Q 2 Minimum distance of (2);
w is to be 1 ＝(S 1 ∪Q 1 )∩(S 2 ∪Q 2 ) And W 2 ＝(S 1 ∪Q 2 )∩(S 2 ∪Q 1 ) The elements in (1) are sorted according to the z coordinate value, and the minimum distance in W1 and W2 is calculated respectively.
In the safety protection system and the safety protection method for the human-computer cooperation robot, the three-dimensional coordinates of the robot under the optical motion capture camera coordinate system are calculated through the optical motion capture camera coordinate system module to form first coordinates, the three-dimensional coordinates of surrounding obstacles under the optical motion capture camera coordinate system are calculated through the optical motion capture camera coordinate system module to form second coordinates, the main controller calculates the shortest distance between the robot and the surrounding obstacles at the current moment according to the first coordinates and the second coordinates, and controls the running speed of the robot according to the shortest distance, so that the safety distance between the intelligent control robot and a human body is realized, and the human-computer cooperation can be realized more reliably.
Drawings
FIG. 1 is a schematic flow chart diagram illustrating a safety protection method for a human-computer cooperative robot according to an embodiment of the present invention;
fig. 2 is a schematic view illustrating a flow of a safety protection method for a human-machine cooperative robot according to another embodiment of the present invention.
Detailed Description
The following describes the safety protection system and the safety protection method of the human-machine cooperative robot in further detail with reference to the accompanying drawings and specific embodiments. Advantages and features of the present invention will become apparent from the following description and from the claims. It is to be noted that the drawings are in a very simplified form and are not to precise scale, which is merely for the purpose of facilitating and distinctly claiming the embodiments of the present invention.
The core idea of the invention is to provide a safety protection system and a safety protection method for a human-computer cooperation robot, so as to solve the problems of non-intelligent and unreliable human-computer cooperation in the prior art.
In order to achieve the above idea, the present invention provides a safety protection system and a safety protection method for a human-computer cooperative robot, where the safety protection system for a human-computer cooperative robot includes an optical motion capture camera, a main controller, and a robot, where: the optical motion capture camera comprises an optical motion capture camera coordinate system module, acquires the position of the robot and the positions of surrounding obstacles, and sends the positions to the optical motion capture camera coordinate system module; the optical motion capture camera coordinate system module calculates three-dimensional coordinates of the robot under the optical motion capture camera coordinate system according to the position of the robot to form first coordinates, and the optical motion capture camera coordinate system module calculates three-dimensional coordinates of the surrounding obstacles under the optical motion capture camera coordinate system according to the positions of the surrounding obstacles to form second coordinates; the optical motion capture camera coordinate system module sending the first coordinates and the second coordinates to the master controller; and the main controller calculates the shortest distance between the robot and the surrounding obstacles at the current moment according to the first coordinate and the second coordinate, and controls the running speed of the robot according to the shortest distance.
< example one >
The present embodiment provides a safety protection system for a human-machine-collaborative robot, as shown in fig. 1, the safety protection system for a human-machine-collaborative robot includes an optical motion capture camera, a main controller, and a robot, wherein: the optical motion capture camera comprises an optical motion capture camera coordinate system module, acquires the position of the robot and the positions of surrounding obstacles, and sends the positions to the optical motion capture camera coordinate system module; the optical motion capture camera coordinate system module calculates three-dimensional coordinates of the robot under the optical motion capture camera coordinate system according to the position of the robot to form first coordinates, and the optical motion capture camera coordinate system module calculates three-dimensional coordinates of the surrounding obstacles under the optical motion capture camera coordinate system according to the positions of the surrounding obstacles to form second coordinates; the optical motion capture camera coordinate system module sending the first coordinates and the second coordinates to the master controller; and the main controller calculates the shortest distance between the robot and the surrounding obstacles at the current moment according to the first coordinate and the second coordinate, and controls the running speed of the robot according to the shortest distance.
Specifically, in the safety protection system for a human-computer-collaborative robot, the optical motion capture camera includes an infrared camera, a frame rate of the infrared camera is greater than 100Hz, a resolution of the infrared camera is greater than 640 × 480, the infrared camera is provided with a USB interface, and the USB interface is used for outputting a preprocessed image and a grayscale image. In the safety protection system for the human-computer cooperation robot, the number of the optical motion capture cameras is 6-24.
The optical motion capture camera refers to a multifunctional infrared camera capable of capturing highly reflective objects in a scene, and mainstream products include an OptiTrack V100R2 motion capture camera, a Realis RTS4000 motion capture camera and an ART TRACK5 motion capture camera. The frame rate of the images can reach 100Hz, the resolution is generally larger than 640 × 480, a high-speed USB interface is provided, a preprocessed image, a gray image, a preprocessed target object and an image compressed by MJPEG can be output, and the precision can reach sub-millimeter.
Full body motion capture requires a minimum of 6 optical motion capture cameras to be configured. If the tracking effect is to be improved or the capture range is to be increased, it is recommended to use 8 or more motion capture cameras. Currently a maximum of 24 motion capture cameras can be configured. When using 12 motion capture cameras, it is recommended to use a truss for the fixation, rather than a tripod.
In the safety protection system for a human-computer cooperation robot provided by the embodiment, the three-dimensional coordinates of the robot in the optical motion capture camera coordinate system are calculated through the optical motion capture camera coordinate system module to form first coordinates, the three-dimensional coordinates of surrounding obstacles in the optical motion capture camera coordinate system are calculated through the optical motion capture camera coordinate system module to form second coordinates, the main controller calculates the shortest distance between the robot and the surrounding obstacles at the current moment according to the first coordinates and the second coordinates, and controls the running speed of the robot according to the shortest distance, so that the safety distance between the intelligent control robot and a human body is realized, and the human-computer cooperation can be realized more reliably.
In summary, the above embodiments have been described in detail with respect to different configurations of the human-machine cooperative robot safety protection system, and it is understood that the present invention includes, but is not limited to, the configurations listed in the above embodiments, and any modifications based on the configurations provided by the above embodiments are within the scope of the present invention. One skilled in the art can take the contents of the above embodiments to take a counter-measure.
< example two >
The embodiment provides a safety protection method for a human-computer cooperation robot based on the safety protection system for the human-computer cooperation robot described in the previous embodiment, and as shown in fig. 2, the safety protection method for the human-computer cooperation robot includes: fixing and placing the optical motion capture camera in a field where the robot and the surrounding obstacles are located; calibrating the optical motion capture camera to obtain a coordinate transformation matrix between the coordinate system of the optical motion capture camera and a world coordinate system; respectively arranging a plurality of optical mark points on the surfaces of the surrounding obstacles and the surface of the robot, and acquiring the first coordinate and the second coordinate according to the optical mark points; and converting the first coordinate and the second coordinate into coordinates in a world coordinate system by using the coordinate transformation matrix.
Specifically, in the safety protection method for a human-computer cooperation robot, the safety protection method for a human-computer cooperation robot further includes: in order to realize the detection of the working space, according to factors such as the field environment, the sensor resolution, the viewing angle and the like, the following possible layout modes of the motion capture camera are proposed, for example: arranging 6 optical motion capture cameras which are respectively fixed on 6 tripods, and uniformly arranging the 6 tripods on a circumference which takes a captured object as a circle center and has a radius of 5-10 meters; or 4 optical motion capture cameras are respectively fixed at the tops of 4 tripods, the other 4 optical motion capture cameras are respectively fixed at the middle parts of the other 4 tripods, and 8 tripods are uniformly distributed on a circumference which takes a captured object as a circle center and has the radius of 5-10 meters; or 6 optical motion capture cameras are respectively fixed at the tops of 6 tripods, the other 6 optical motion capture cameras are respectively fixed at the middle parts of the other 6 tripods, and 12 tripods are uniformly arranged on a circumference which takes a captured object as a circle center and has a radius of 5-10 meters; or a rectangular truss is arranged, 4 optical motion capture cameras are respectively fixed at the top end of the rectangular truss, the other 4 optical motion capture cameras are respectively fixed at the middle part of the rectangular truss, and the other 4 optical motion capture cameras are respectively fixed at the bottom of the rectangular truss.
Further, in the method for safeguarding a robot cooperating with a human machine, the calibrating the optical motion capture camera includes: a plurality of optical marking balls A 1 ，A 2 ，A 3 ，...，A n Arranging in the field to enable as many optical marker balls as possible to be displayed in the picture of each optical motion capture camera; recording a plurality of said optical marker balls A 1 ，A 2 ，A 3 ，...，A n Coordinates p in the optical motion capture camera 1 ，p 2 ，p 3 ，...，p n Wherein p is i ＝[u i ，v i ，1] T (ii) a A plurality of the optical marker balls A 1 ，A 2 ，A 3 ，...，A n The coordinates under the world coordinate system are:
P i ＝[X i ，Y i ，Z i ，1] T ，
the three-dimensional coordinates of the optical marker ball in the optical motion capture camera coordinate system and the coordinates in the world coordinate system are transformed in relation to:
p i ＝sA[R，t]P i ，
wherein s is a proportionality coefficient and [ R, t ] is a rotational translation matrix for converting the world coordinate system to the optical motion capture camera coordinate system;
Figure BDA0002455914050000101
capturing a camera internal coefficient matrix for the optical motion;
(u 0 ，v 0 ) Image reference points for the optical motion capture camera; (α, β) is a length-width scaling factor from the image plane coordinates to the pixel coordinates in the frame buffer; γ is the coefficient of radial distortion of the optical motion capture camera.
Specifically, in the safety protection method for a human-computer cooperative robot, the calibrating the optical motion capture camera includes: the value Zi is set to 0 and,
Figure BDA0002455914050000102
wherein: h = lambda [ r ] 1 r 2 t]，
Figure BDA0002455914050000103
A system of linear equations can be obtained:
Figure BDA0002455914050000104
solving external parameters as follows:
r 1 ＝λA -1 h 1
r 2 ＝λa -1 h 2
r 3 ＝r 1 ×r 2
t＝λA -1 h 3
Figure BDA0002455914050000105
further, after calibration is completed, adhering optical mark points as many as possible on key parts of a human body and the outside of the robot, recording three-dimensional coordinates of m mark points on the human body and n mark points on the robot, and calculating the distance between the human body and the robot according to the three-dimensional coordinates of the human body and the robot; the euclidean distance from the person to the robot can be calculated using a formula. The distance matrix can be obtained by repeating the above process for every two optical mark points on the human body and the robot.
In the safety protection method for the human-computer cooperative robot, the three-dimensional coordinates of the optical marker points of the surrounding obstacles in the optical motion capture camera coordinate system are the second coordinates:
Figure BDA0002455914050000111
the three-dimensional coordinates of the optical marker point of the robot in the optical motion capture camera coordinate system are the first coordinates:
Figure BDA0002455914050000112
calculating the distance between the surrounding obstacles and the robot according to the first coordinate and the second coordinate:
Figure BDA0002455914050000113
further, in the safety protection method for the human-computer cooperative robot, the distance between every two optical mark points in the optical mark points of the first coordinate and the second coordinate is calculated to form a distance matrix,
Figure BDA0002455914050000114
calculating the shortest distance:
L min ＝minD m，n 。
comparing distance matrix L one by adopting traversal method m×n The size of each two points in the process, the consumed operation time is O [ (mn) 2 ]The algorithm is long in time consumption and poor in real-time performance, and is not suitable for the field of man-machine cooperation safety protection. Therefore, the problem of solving the minimum value is solved by adopting a divide-and-conquer method.
In addition, in the safety protection method for the human-computer cooperation robot, each optical mark point in the first coordinate is set as a first mark, each optical mark point in the second coordinate is set as a second mark, the first coordinate and the second coordinate are combined into a third coordinate, and the third coordinate is rapidly sequenced according to an abscissa value; and dividing the elements in the third coordinate into two sets with equal element numbers according to the x coordinate values, wherein the two sets are respectively as follows:
S 1 = { p ∈ S | x (p) ≦ kx }, and
S 2 ＝{p∈S|x(p)＞kx}，
respectively calculate S 1 And S 2 Is d 1 And d 2 Let d = min { d } 1 ，d 2 }；
Make P be 1 ＝{p∈S 1 Ix (p) > kx-d }, and
P 2 ＝{p∈S 2 |x(p)＜kx+d}；
combining the P1 and the P2 into a fourth coordinate, and dividing elements in the fourth coordinate into two sets with equal element numbers according to the y coordinate value, wherein the two sets are respectively as follows:
Q 1 ＝{p∈P 1 ∪P 2 y (p) is less than or equal to ky }, and
Q 2 ＝{p∈P 1 ∪P 2 |y(p)＞ky}，
respectively calculate Q 1 And Q 2 Minimum distance of (2);
w is to be 1 ＝(S 1 ∪Q 1 )∩(S 2 ∪Q 2 ) And W 2 ＝(S 1 ∪Q 2 )∩(S 2 ∪Q 1 ) The elements in (b) are sorted by z-coordinate value, and the minimum distance in W1 and W2 is calculated respectively. At merge, only two points belong to different sets will be updated. The final required calculation time was T = O ((m + n) (log (m + n)) 2 ))。
Further, in the safety protection method for the human-computer cooperative robot, when the shortest distance is smaller than a first preset threshold value, the running speed of the robot is controlled to be zero; when the shortest distance is larger than a second preset threshold value, controlling the robot to run at the maximum speed; when the shortest distance is between the first preset threshold and the second preset threshold, controlling the running speed of the robot to be as follows:
Figure BDA0002455914050000121
wherein d is the shortest distance，d 1 Is the first predetermined threshold value, d 2 Is said second predetermined threshold, d 2 ＞d 1 ，v max Is the maximum operating speed of the robot. Setting two threshold values d according to the distance around the robot 1 And d 2 Where the former is the minimum distance a person can approach the robot. The robot can be determined according to the extending distance of the mechanical arm and the length of the mounted tool, and in the distance, the robot must stop moving, otherwise, the robot is likely to cause harm to people; the latter is a distance threshold for early warning the distance between the person and the robot. The magnitude of this threshold can be determined artificially, taking into account the uncertainty of the human motion and the speed of response of the visual safety system, and when within this distance the robot must decelerate the motion. The magnitude relation of the two thresholds is d 2 ＞d 1 The larger the difference between the two is, the higher the safety factor is.
Specifically, in the safety protection method for the man-machine cooperative robot, the surrounding obstacles are human bodies, a safety vest or a safety helmet is worn on the human bodies, a plurality of optical mark points are respectively arranged at the front and the rear of the safety vest, and a plurality of optical mark points are arranged at the top of the safety helmet and the brim of the safety helmet.
The embodiments in the present description are described in a progressive manner, each embodiment focuses on differences from other embodiments, and the same and similar parts among the embodiments are referred to each other. For the system disclosed by the embodiment, the description is relatively simple because the system corresponds to the method disclosed by the embodiment, and the relevant points can be referred to the method part for description.
The above description is only for the purpose of describing the preferred embodiment of the present invention, and is not intended to limit the scope of the present invention, and any variations and modifications made by those skilled in the art based on the above disclosure are intended to fall within the scope of the appended claims.

Claims (9)

1. A safety protection system of a human-machine cooperation robot, comprising an optical motion capture camera, a main controller and a robot, wherein:
the optical motion capture camera comprises an optical motion capture camera coordinate system module, acquires the position of the robot and the positions of surrounding obstacles, and sends the positions to the optical motion capture camera coordinate system module;
the optical motion capture camera coordinate system module calculates three-dimensional coordinates of the robot in the optical motion capture camera coordinate system based on the position of the robot to form first coordinates, an
The optical motion capture camera coordinate system module calculates three-dimensional coordinates of the surrounding obstacles in the optical motion capture camera coordinate system according to the positions of the surrounding obstacles to form second coordinates;
the optical motion capture camera coordinate system module sending the first coordinates and the second coordinates to the master controller;
the main controller calculates the shortest distance between the robot and the surrounding obstacles at the current moment according to the first coordinate and the second coordinate, and controls the running speed of the robot according to the shortest distance;
when the shortest distance is smaller than a first preset threshold value, controlling the running speed of the robot to be zero; when the shortest distance is larger than a second preset threshold value, controlling the robot to run at the maximum speed; when the shortest distance is between the first preset threshold and the second preset threshold, controlling the running speed of the robot to be as follows:
Figure FDA0003930488270000011
wherein d is the shortest distance, d 1 Is said first predetermined threshold value, d 2 Is said second predetermined threshold, d 2 ＞d 1 ，v max Is the maximum operating speed of the robot.
2. The human-machine-collaborative robot security system of claim 1, wherein the optical motion capture camera comprises an infrared camera, the infrared camera has a frame rate greater than 100Hz, the infrared camera has a resolution greater than 640 x 480, the infrared camera is provided with a USB interface, and the USB interface is configured to output a pre-processed image and a grayscale image.
3. The humanoid cooperative robot safeguard system of claim 1, wherein the number of optical motion capture cameras is 6-24.
4. A human-machine cooperative robot safety protection method based on the human-machine cooperative robot safety protection system of claim 1, wherein the human-machine cooperative robot safety protection method comprises:
fixing and placing the optical motion capture camera in a field where the robot and the surrounding obstacles are located;
calibrating the optical motion capture camera to obtain a coordinate transformation matrix between the coordinate system of the optical motion capture camera and a world coordinate system;
respectively arranging a plurality of optical mark points on the surfaces of the surrounding obstacles and the surface of the robot, and acquiring the first coordinate and the second coordinate according to the optical mark points;
converting the first coordinate and the second coordinate into coordinates in a world coordinate system by using the coordinate transformation matrix;
said calibrating said optical motion capture camera comprises:
a plurality of optical marking balls A 1 ，A 2 ，A 3 ，...，A n Arranging in the venue to enable as many optical marker balls as possible to be displayed within a frame of each of the optical motion capture cameras;
recording a plurality of said optical marker balls A 1 ，A 2 ，A 3 ，...，A n Coordinates p in the optical motion capture camera 1 ，p 2 ，p 3 ，...，p n Wherein p is i ＝[u i ，v i ，1] T ；
A plurality of the optical marker balls A 1 ，A 2 ，A 3 ，...，A n The coordinates under the world coordinate system are:
P i ＝[X i ，Y i ，Z i ，1] T ，
the three-dimensional coordinates of the optical marker ball in the optical motion capture camera coordinate system and the coordinates in the world coordinate system are transformed in relation to:
p i ＝sA[R，t]P i ，
wherein s is a proportionality coefficient and [ R, t ] is a rotational translation matrix for converting the world coordinate system to the optical motion capture camera coordinate system;
Figure FDA0003930488270000021
capturing a camera internal coefficient matrix for the optical motion;
(u 0 ，v 0 ) Image reference points for the optical motion capture camera; (α, β) is a length-width scaling factor from the image plane coordinates to the pixel coordinates in the frame buffer; γ is a coefficient of radial distortion of the optical motion capture camera.
5. The safety protection method for a human-machine cooperative robot according to claim 4, wherein the safety protection method for a human-machine cooperative robot further comprises:
arranging 6 optical motion capture cameras which are respectively fixed on 6 tripods, and uniformly arranging the 6 tripods on a circumference which takes a captured object as a circle center and has a radius of 5-10 meters; or
4 optical motion capture cameras are respectively fixed at the tops of 4 tripods, the other 4 optical motion capture cameras are respectively fixed at the middle parts of the other 4 tripods, and 8 tripods are uniformly arranged on a circumference which takes a captured object as a circle center and has a radius of 5-10 meters; or
Setting 6 optical motion capture cameras to be respectively fixed at the tops of 6 tripods, setting another 6 optical motion capture cameras to be respectively fixed at the middle parts of another 6 tripods, and uniformly arranging 12 tripods on a circumference which takes a captured object as a circle center and has a radius of 5-10 meters; or alternatively
And arranging a rectangular truss, wherein 4 optical motion capture cameras are respectively fixed at the top end of the rectangular truss, the other 4 optical motion capture cameras are respectively fixed at the middle part of the rectangular truss, and the other 4 optical motion capture cameras are respectively fixed at the bottom of the rectangular truss.
6. The method of claim 4, wherein the calibrating the optical motion capture camera comprises: the value Zi is set to 0 and,
Figure FDA0003930488270000031
wherein: h = lambda [ r ] 1 r 2 t]，
Figure FDA0003930488270000032
A system of linear equations can be obtained:
Figure FDA0003930488270000033
solving the external parameters as follows:
r 1 ＝λA -1 h 1
r 2 ＝λa -1 h 2
r 3 ＝r 1 ×r 2
t＝λA -l h 3
Figure FDA0003930488270000041
7. the human-machine cooperative robot safeguard method according to claim 4, wherein the three-dimensional coordinates of the optical marker points of the surrounding obstacles in the optical motion capture camera coordinate system are the second coordinates:
Figure FDA0003930488270000042
the three-dimensional coordinates of the optical marker point of the robot in the optical motion capture camera coordinate system are the first coordinates:
Figure FDA0003930488270000043
calculating the distance between the surrounding obstacle and the robot according to the first coordinate and the second coordinate:
Figure FDA0003930488270000044
the surrounding barriers are human bodies, safety vests or safety helmets are worn on the human bodies, a plurality of optical mark points are arranged on the front and the back of each safety vest, and a plurality of optical mark points are arranged on the tops of the safety helmets and the brim of each safety helmet.
8. The method for safeguarding a human-computer cooperative robot of claim 7, wherein the distance between every two optical mark points in each of the first coordinate and the second coordinate is calculated to form a distance matrix,
Figure FDA0003930488270000045
calculating the shortest distance:
L min ＝minD m，n ；
when the shortest distance is smaller than a first preset threshold value, controlling the running speed of the robot to be zero; when the shortest distance is larger than a second preset threshold value, controlling the robot to run at the maximum speed; when the shortest distance is between the first preset threshold and the second preset threshold, controlling the running speed of the robot to be as follows:
Figure FDA0003930488270000051
wherein d is the shortest distance, d 1 Is the first predetermined threshold value, d 2 Is said second predetermined threshold, d 2 ＞d 1 ，v max Is the maximum operating speed of the robot.
9. The method for safeguarding a human-computer cooperative robot according to claim 8, wherein each optical marker point in the first coordinate is set as a first marker, each optical marker point in the second coordinate is set as a second marker, the first coordinate and the second coordinate are merged into a third coordinate, and the third coordinate is rapidly sorted according to an abscissa value;
and dividing the elements in the third coordinate into two sets with equal element numbers according to the x coordinate values, wherein the two sets are respectively as follows:
S 1 = { p ∈ S | x (p) ≦ kx }, and
S 2 ＝{p∈S|x(p)≤kx}，
respectively calculate S 1 And S 2 Is d 1 And d 2 Let d = min { d { 1 ，d 2 }；
Let P 1 ＝{p∈S 1 L (p) > kx-d }, and
P 2 ＝{p∈S 2 |x(p)＜kx+d}；
combining the P1 and the P2 into a fourth coordinate, and dividing elements in the fourth coordinate into two sets with equal element numbers according to the y coordinate value, wherein the two sets are respectively as follows:
Q 1 ＝{p∈P 1 ∪P 2 y (p) is less than or equal to ky }, and
Q 2 ＝{p∈P 1 ∪P 2 |y(p)＞ky}，
respectively calculate Q 1 And Q 2 (ii) a minimum distance;
w is to be 1 ＝(S 1 ∪Q 1 )∩(S 2 ∪Q 2 ) And W 2 ＝(S 1 ∪Q 2 )∩(S 2 ∪Q 1 ) The elements in (1) are sorted according to the z coordinate value, and the minimum distance in W1 and W2 is calculated respectively.
CN202010306315.4A 2020-04-17 2020-04-17 Safety protection system and safety protection method for man-machine cooperative robot Active CN111546331B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010306315.4A CN111546331B (en) 2020-04-17 2020-04-17 Safety protection system and safety protection method for man-machine cooperative robot

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010306315.4A CN111546331B (en) 2020-04-17 2020-04-17 Safety protection system and safety protection method for man-machine cooperative robot

Publications (2)

Publication Number Publication Date
CN111546331A CN111546331A (en) 2020-08-18
CN111546331B true CN111546331B (en) 2023-03-28

Family

ID=71996308

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010306315.4A Active CN111546331B (en) 2020-04-17 2020-04-17 Safety protection system and safety protection method for man-machine cooperative robot

Country Status (1)

Country Link
CN (1) CN111546331B (en)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112637492B (en) * 2020-12-19 2022-04-15 中建浩运有限公司 Intelligent entity exhibition system
CN113276122A (en) * 2021-06-23 2021-08-20 浙江安防职业技术学院 Safety detection robot control method and system
CN114670206A (en) * 2022-05-07 2022-06-28 法奥意威(苏州)机器人***有限公司 Robot control method, device, cooperative robot and storage medium

Family Cites Families (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2005345161A (en) * 2004-05-31 2005-12-15 Rikogaku Shinkokai System and method of motion capture
JP2014211404A (en) * 2013-04-22 2014-11-13 株式会社ノビテック Motion capture method
CN103279186B (en) * 2013-05-07 2015-11-18 兰州交通大学 Merge the multiple goal motion capture system of optical alignment and inertia sensing
CN104700433B (en) * 2015-03-24 2016-04-27 中国人民解放军国防科学技术大学 A kind of real-time body's whole body body motion capture method of view-based access control model and system thereof
JP2018004610A (en) * 2016-06-28 2018-01-11 木村　岳 System for measuring spatial coordinate and posture of object by using mems motion sensor in combination with geometric position measuring device
CN106308810A (en) * 2016-09-27 2017-01-11 中国科学院深圳先进技术研究院 Human motion capture system
CN110253570B (en) * 2019-05-27 2020-10-27 浙江工业大学 Vision-based man-machine safety system of industrial mechanical arm

Also Published As

Publication number Publication date
CN111546331A (en) 2020-08-18

Similar Documents

Publication Publication Date Title
CN111546331B (en) Safety protection system and safety protection method for man-machine cooperative robot
KR102379245B1 (en) Wearable device-based mobile robot control system and control method
US8731276B2 (en) Motion space presentation device and motion space presentation method
CN111055281A (en) ROS-based autonomous mobile grabbing system and method
US9679385B2 (en) Three-dimensional measurement apparatus and robot system
CN109048926A (en) A kind of intelligent robot obstacle avoidance system and method based on stereoscopic vision
JP4167954B2 (en) Robot and robot moving method
US10712566B2 (en) Information displaying system provided with head-mounted type display
JP5736622B1 (en) Detection device and operation control of manipulator equipped with the device
CN103895042A (en) Industrial robot workpiece positioning grabbing method and system based on visual guidance
CN105059190B (en) The automobile door opening collision warning device and method of view-based access control model
JP6589604B2 (en) Teaching result display system
CN111360818A (en) Mechanical arm control system through visual positioning
CN108897342B (en) Positioning and tracking method and system for fast-moving civil multi-rotor unmanned aerial vehicle
CN109345587B (en) Hybrid vision positioning method based on panoramic vision and monocular vision
CN205485417U (en) Engineering machine tool remote control system
CN106113067A (en) A kind of Dual-Arm Mobile Robot system based on binocular vision
CN110170996A (en) A kind of quick teaching system of robot based on stereoscopic vision
Han et al. Grasping control method of manipulator based on binocular vision combining target detection and trajectory planning
CN111702787B (en) Man-machine cooperation control system and control method
JPH0421105A (en) Stereoscopic teaching device for manipulator
JP2021065971A (en) Robot teaching system, image forming method and program
CN107363831B (en) Teleoperation robot control system and method based on vision
JP2778376B2 (en) Camera viewpoint change method
JPH10291188A (en) Stereogrpahic image presenting 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