Safety control methods and systems for robots

Abstract

This invention provides a safety control method and system for a robot. The method includes: acquiring joint angles of each joint of the robot through a first sensing unit; calculating a first end-effector pose based on the joint angles, determining whether the first end-effector pose exceeds a first preset limit value, and if so, triggering a safety mechanism; acquiring base data of the robot base and end-effector data through a second sensing unit, the base data including base acceleration and base magnetic field position, and the end-effector data including end-effector acceleration, end-effector magnetic field position, and end-effector angular velocity; calculating a second end-effector pose based on the base data and end-effector data, determining whether the second end-effector pose exceeds the first preset limit value, and if so, triggering a safety mechanism. This invention provides two independent detection methods to observe the robot's end-effector motion parameters, providing good safety redundancy and increasing the system's reliability.

Description

Technical Field

[0001] This invention relates primarily to the field of robotics, and more particularly to a method and system for the safety control of robots. Background Technology

[0002] Coexisting-Cooperative-Cognitive Robots (Tri-Co Robots) are robots capable of interacting naturally with their work environment, humans, and other robots, autonomously adapting to complex and dynamic environments, and working collaboratively. A crucial aspect of coexisting robots is the safety of human-robot interaction. Statistics show that most robot-related safety accidents involve human injuries from being pinched or impacted. Therefore, robots need collision detection capabilities.

[0003] Current collision detection solutions often involve installing joint torque sensors and dual encoders at the robot's joints to estimate external torque. This collision detection method is relatively simple and lacks safety redundancy; if the joint torque sensor or dual encoder fails, the collision detection will fail. Furthermore, the joint torque sensor and dual encoder required for this solution are relatively expensive, limiting its application.

[0004] Therefore, there is an urgent need for a reliable and widely applicable robot safety control method. Summary of the Invention

[0005] The technical problem to be solved by the present invention is to provide a safety control method and system for robots, which solves the problem of lack of safety redundancy in existing collision detection schemes.

[0006] To address the aforementioned technical problems, this invention provides a safety control method for a robot, comprising: acquiring joint angles of each joint of the robot via a first sensing unit; calculating a first end-effector pose of the robot's end effector based on the joint angles, determining whether the first end-effector pose exceeds a first preset limit value, and if so, triggering a safety mechanism; acquiring base data of the robot's base and end-effector data of the robot's end effector via a second sensing unit, wherein the base data includes base acceleration and base magnetic field position, and the end-effector data includes end-effector acceleration, end-effector magnetic field position, and end-effector angular velocity; calculating a second end-effector pose of the robot's end effector based on the base data and the end-effector data, determining whether the second end-effector pose exceeds the first preset limit value, and if so, triggering a safety mechanism.

[0007] Optionally, the first sensing unit includes multiple encoders, each located near a joint.

[0008] Optionally, the second sensing unit includes a first inertial sensor and a first geomagnetic sensor mounted on the robot base, and a second inertial sensor, a second geomagnetic sensor, and a MEMS gyroscope mounted on the robot end effector.

[0009] Optionally, collecting base data of the robot base and end effector data of the robot end effector through the second sensing unit includes: collecting the base acceleration through the first inertial sensor and the base magnetic field position through the first geomagnetic sensor; collecting the end effector acceleration through the second inertial sensor and the end effector magnetic field position through the second geomagnetic sensor; and collecting the end effector angular rate through the MEMS gyroscope.

[0010] Optionally, calculating the first end-effector pose based on the joint angles includes: performing forward homing calculations based on the joint angles to obtain the first end-effector pose.

[0011] Optionally, calculating the second end-effector pose of the robot end-effector based on the base data and the end-effector data includes: calculating a transformation matrix from the robot end-effector coordinate system to the robot base coordinate system based on the base data and the end-effector data; calculating the second end-effector acceleration of the end-effector acceleration in the robot base coordinate system based on the transformation matrix; integrating the second end-effector acceleration to obtain a second end-effector velocity; and integrating the second end-effector velocity to obtain the second end-effector pose.

[0012] Optionally, the safety mechanism includes warning, deceleration, braking, and stopping.

[0013] Optionally, the method further includes: calculating the difference between the first end pose and the second end pose, determining whether the difference exceeds a first threshold, and if so, triggering a security mechanism.

[0014] Optionally, determining whether the difference exceeds the first threshold includes: determining whether the difference between any component of the first end pose and the second end pose exceeds the first threshold; if so, triggering a security mechanism.

[0015] Optionally, the method further includes: calculating a first end-effector velocity of the robot end based on the joint angle, determining whether the first end-effector velocity exceeds a second preset limit value, and if so, triggering a safety mechanism; calculating a second end-effector velocity of the robot end based on the base data and the end-effector data, determining whether the second end-effector velocity exceeds the second preset limit value, and if so, triggering a safety mechanism.

[0016] Optionally, calculating the first end effector velocity of the robot end effector based on the joint angles includes: performing differential processing on the joint angles to obtain the joint velocities of each joint; and obtaining the first end effector velocity based on the joint velocities and the Jacobian matrix.

[0017] Optionally, calculating the second end-effector velocity of the robot end-effector based on the base data and the end-effector data includes: calculating the transformation matrix from the robot end-effector coordinate system to the robot base coordinate system based on the base data and the end-effector data; calculating the second end-effector acceleration of the end-effector acceleration in the robot base coordinate system based on the transformation matrix; and integrating the second end-effector acceleration to obtain the second end-effector velocity.

[0018] Optionally, the method further includes: calculating the speed difference between the first end velocity and the second end velocity, determining whether the speed difference exceeds a second threshold, and if so, triggering a safety mechanism.

[0019] To address the aforementioned technical problems, this invention provides a robot safety control system, comprising: a first detection unit, including: a first sensing unit comprising multiple encoders, each encoder located near a joint, the first sensing unit being configured to collect joint angles of each joint of the robot; a first control unit configured to calculate a first end effector pose of the robot end effector based on the joint angles, and determine whether the first end effector pose exceeds a first preset limit value; if so, triggering a safety mechanism; and a second detection unit, including: a second sensing unit comprising a first inertial sensor and a first geomagnetic sensor mounted on the robot base, and a second inertial sensor, a second geomagnetic sensor, and a MEMS gyroscope mounted on the robot end effector; the second sensing unit being configured to collect base data of the robot base and end effector data of the robot end effector, the base data including base acceleration and base magnetic field position, and the end effector data including end effector acceleration, end effector magnetic field position, and end effector angular rate; and a second control unit configured to calculate a second end effector pose of the robot end effector based on the base data and the end effector data, and determine whether the second end effector pose exceeds the first preset limit value; if so, triggering a safety mechanism.

[0020] Optionally, the second control unit is further configured to: calculate a transformation matrix from the robot end-effector coordinate system to the robot base coordinate system based on the base data and the end-effector data; calculate a second end-effector acceleration of the end-effector acceleration in the robot base coordinate system based on the transformation matrix; integrate the second end-effector acceleration to obtain a second end-effector velocity; and integrate the second end-effector velocity to obtain a second end-effector pose.

[0021] Optionally, the first control unit is further configured to calculate a first end-effector velocity of the robot end based on the joint angle, determine whether the first end-effector velocity exceeds a second preset limit value, and if so, trigger a safety mechanism; the second control unit is further configured to calculate a second end-effector velocity of the robot end based on the base data and the end-effector data, determine whether the second end-effector velocity exceeds the second preset limit value, and if so, trigger a safety mechanism.

[0022] Optionally, the first control unit is further configured to perform differential processing on the joint angles to obtain the joint velocities of each joint; and to obtain the first end-effector velocity based on the joint velocities and the Jacobian matrix.

[0023] Optionally, the system further includes: a third control unit, configured to obtain the first end-effector pose from the first control unit, obtain the second end-effector pose from the second control unit, calculate the difference between the first end-effector pose and the second end-effector pose, determine whether the difference exceeds a first threshold, and if so, trigger a safety mechanism.

[0024] Compared with the prior art, the present invention has the following advantages:

[0025] The robot safety control method and system of the present invention provide two independent detection methods to observe the robot end motion parameter information, which has good safety redundancy and increases the reliability and stability of the robot system. The present invention uses inertial sensors, geomagnetic sensors and MEMS gyroscopes to calculate the second end pose, which has lower cost and wider applicability. Attached Figure Description

[0026] The accompanying drawings are included to provide a further understanding of this application; they are incorporated into and constitute a part of this application. The drawings illustrate embodiments of this application and, together with this specification, serve to explain the principles of the invention. In the drawings:

[0027] Figure 1 This is a schematic diagram of a robot system according to an embodiment of the present invention;

[0028] Figure 2 This is a schematic diagram of the structure of a robot joint according to an embodiment of the present invention;

[0029] Figure 3 yes Figure 1 System block diagram of the safety control system for the robot;

[0030] Figure 4 This is a flowchart of a robot safety control method according to an embodiment of the present invention;

[0031] Figure 5 yes Figure 4The flowchart for step S44. Detailed Implementation

[0032] To more clearly illustrate the technical solutions of the embodiments of this application, the accompanying drawings used in the description of the embodiments will be briefly introduced below. Obviously, the drawings described below are merely some examples or embodiments of this application. For those skilled in the art, these drawings can be applied to other similar scenarios without creative effort. Unless obvious from the context or otherwise specified, the same reference numerals in the drawings represent the same structures or operations.

[0033] As indicated in this application and claims, unless the context clearly indicates otherwise, the words "a," "an," "an," and / or "the" are not specifically singular and may include plural forms. Generally speaking, the terms "comprising" and "including" only indicate the inclusion of explicitly identified steps and elements, which do not constitute an exclusive list, and the method or apparatus may also include other steps or elements.

[0034] Unless otherwise specifically stated, the relative arrangement, numerical expressions, and values ​​of the components and steps described in these embodiments do not limit the scope of this application. It should also be understood that, for ease of description, the dimensions of the various parts shown in the drawings are not drawn to actual scale. Techniques, methods, and devices known to those skilled in the art may not be discussed in detail, but where appropriate, such techniques, methods, and devices should be considered part of the specification. In all examples shown and discussed herein, any specific values ​​should be interpreted as merely exemplary and not as limitations. Therefore, other examples of exemplary embodiments may have different values. It should be noted that similar reference numerals and letters in the following drawings denote similar items; therefore, once an item is defined in one drawing, it need not be further discussed in subsequent drawings.

[0035] Furthermore, it should be noted that the use of terms such as "first" and "second" to define components is merely for the purpose of distinguishing the corresponding components. Unless otherwise stated, these terms have no special meaning and therefore should not be construed as limiting the scope of protection of this application. In addition, although the terminology used in this application is selected from commonly known and used terms, some terms mentioned in this application's specification may have been chosen by the applicant according to his or her judgment, and their detailed meanings are explained in the relevant sections of this description. Moreover, this application should be understood not only through the actual terms used, but also through the meaning implied by each term.

[0036] Flowcharts are used in this application to illustrate the operations performed by the system according to embodiments of this application. It should be understood that the preceding or following operations are not necessarily performed in exact order. Instead, various steps can be processed in reverse order or simultaneously. Furthermore, other operations may be added to these processes, or one or more steps may be removed from these processes.

[0037] Figure 1 This is a schematic diagram of a robot system according to an embodiment of the present invention. Figure 1 As shown, the robot system 100 includes a robot body 10 and a robot safety control system. The robot body 10 includes a robot base 111, multiple joints 112, multiple robotic arms 113, and a robot end effector 114. The multiple joints 112 include joints between the base and the robotic arms, and joints between the robotic arms. The joints mainly serve to connect, transmit, and drive the robotic arms to move along a desired trajectory. Figure 2 This is a schematic diagram of the structure of a robot joint according to an embodiment of the present invention. Figure 2 As shown, joint 200 includes a drive motor 211 and a reducer 212. The drive motor 211 is the power source for the robot. The reducer 212 is used to convert the torque and speed output by the drive motor 211 into the torque and speed required by the robotic arm B.

[0038] Figure 3 yes Figure 1 A system block diagram of the safety control system for a robot. (Example) Figure 3 As shown, the robot's safety control system 300 includes a first detection unit 31 and a second detection unit 32. The first detection unit 31 includes a first sensing unit 311 and a first control unit 312. The second detection unit 32 includes a second sensing unit 321 and a second control unit 322. The first sensing unit 311 includes multiple encoders, each located near a joint, and is configured to collect the joint angles of each joint of the robot. Figure 2 As shown, in this embodiment, the encoder 213 is placed on the output side of the reducer 212 to detect the joint angle. In some embodiments, the encoder 213 may also be placed on the input side of the reducer 212, and this application does not limit this.

[0039] like Figure 1As shown, the second sensing unit 321 may include a first inertial sensor 121 and a first geomagnetic sensor 122 mounted on the robot base, and a second inertial sensor 123, a second geomagnetic sensor 124, and a MEMS gyroscope 125 mounted on the robot end effector. The first inertial sensor 121 is used to acquire the base acceleration of the robot base 111. The first geomagnetic sensor 122 is used to acquire the base magnetic field position of the robot base 111. The second inertial sensor 123 is used to acquire the end effector acceleration of the robot end effector 114. The second geomagnetic sensor 124 is used to acquire the end magnetic field position of the robot end effector 114. The MEMS gyroscope 125 is used to acquire the end effector angular rate of the robot end effector 114. The first inertial sensor 121 and the second inertial sensor 123 can be triaxial accelerometers, capable of detecting the acceleration components of the robot base and the robot end effector in three directions in Cartesian space, respectively. The robot's safety control system is used to control the robot body to move within a preset limit range, avoiding abnormal contact or collision between the robot body and the human.

[0040] like Figure 3 As shown, the first control unit 312 is configured to calculate the first end-effector pose of the robot based on the joint angles acquired by the first sensing unit 311, and determine whether the first end-effector pose exceeds a first preset limit value. If so, a safety mechanism is triggered. Assume that the joint angles of each joint of the robot acquired by the first sensing unit 311 are (q1, q2, ..., q...). n ), where q n Let be the joint angle of the nth joint. Preferably, the first control unit is configured to determine the joint angle (q1, q2, ..., q...). nThe robot performs forward kinematics to obtain the first end-effector pose. The first end-effector pose can be represented in the form xyzabc, where xyzabc represents [X-axis distance, Y-axis distance, Z-axis distance, yaw angle, pitch angle, and yaw angle]. The first end-effector pose is represented as (x1, y1, z1, A1, B1, C1). To ensure the safety of people interacting with the robot, a first preset limit is set for the robot. If the robot's end-effector exceeds this first preset limit, it may cause harm to people. The first preset limit can be a limiting plane. Determining whether the first end-effector pose exceeds the first preset limit requires calculating the distance from (x1, y1, z1) to the limiting plane. If the distance is positive, it indicates that the first preset limit has not been exceeded. If the distance is negative, it indicates that the first preset limit has been exceeded, triggering a safety mechanism. The safety mechanism includes warning, deceleration, braking, and stopping. In some embodiments, the distance exceeding the first preset limit value can be classified into four intervals, for example, the distance exceeding the limit can be divided into four intervals. When the distance exceeding the limit is in the first interval, the first control unit controls the voice device or display device to issue a warning; when the distance exceeding the limit is in the second interval, the first control unit controls the robot end effector to decelerate; when the distance exceeding the limit is in the third interval, the first control unit controls the robot end effector to brake; when the distance exceeding the limit is in the fourth interval, the first control unit controls the robot end effector to stop moving.

[0041] The second control unit 322 is configured to calculate the second end-effector pose of the robot's end effector based on the base data and end-effector data acquired by the second sensing unit 321. The base data includes base acceleration and base magnetic field position, and the end-effector data includes end-effector acceleration, end-effector magnetic field position, and end-effector angular velocity. It determines whether the second end-effector pose exceeds a first preset limit value; if so, a safety mechanism is triggered. When the robot is stationary, i.e., when the joint angles of each joint acquired by the first sensing unit 311 remain unchanged, it is assumed that the base acceleration acquired by the first inertial sensor is (g... x1 g y1 g z1 The location of the base magnetic field collected by the first geomagnetic sensor is (m). x1 m y1 m z1 The second inertial sensor collected the end-effector acceleration (g). x2 g y2 g z2 The location of the terminal magnetic field collected by the second geomagnetic sensor is (m). x2 m y2 m z2 The second control unit 322 calculates the second end-effector pose of the robot's end effector based on the base data and the end-effector data, including the following steps:

[0042] 1) Calculate the transformation matrix from the robot's end-effector coordinate system to the robot's base coordinate system based on the base data and end-effector data.

[0043] The robot coordinate system involved in this invention includes a robot base coordinate system, a robot end effector coordinate system, and a world coordinate system. The robot base coordinate system is a Cartesian coordinate system used to describe the robot's body motion, with its xy plane parallel to the robot base and its z-axis pointing upwards. If the robot is installed upside down, the z-axis points downwards. The world coordinate system is a Cartesian coordinate system with the Earth as a reference. The robot end effector coordinate system is a coordinate system established with the tool's center point as the origin, and its default tool center point is the center position of the robot's end effector.

[0044] First, based on the base acceleration (g) collected by the first inertial sensor x1 g y1 g z1 ) and the base magnetic field position (m) collected by the first geomagnetic sensor x1 m y1 m z1 Calculate the rotation matrix of the robot base relative to the world coordinate system. The rotation matrix R of the robot base relative to the world coordinate system is... 1 It can be represented as:

[0045]

[0046] Where ψ1, θ1, and γ1 are the attitude transformation angles from the robot base to the world coordinate system. In some embodiments, θ1, γ1, and ψ1 can be derived by the following formula:

[0047]

[0048]

[0049]

[0050] Where g is the acceleration due to gravity.

[0051] Then, based on the end-effector acceleration (g) collected by the second inertial sensor... x2 g y2 g z2 The end magnetic field position (m) collected by the second geomagnetic sensor x2 m y2 m z2 Calculate the rotation matrix R of the robot's end effector relative to the world coordinate system. 2 It can be represented as:

[0052]

[0053] Where ψ2, θ2, and γ2 are the attitude transformation angles from the robot's end effector to the world coordinate system. In some embodiments, θ2, γ2, and ψ2 can be derived by the following formula:

[0054]

[0055]

[0056]

[0057] Where g is the acceleration due to gravity.

[0058] Based on the rotation matrix R of the robot base relative to the world coordinate system 1 The rotation matrix R of the robot's end effector relative to the world coordinate system 2 Calculate the transformation matrix R from the robot's end-effector coordinate system to the robot's base coordinate system. b :

[0059] R b =R 2′ R 1

[0060] Where R 2′ R represents 2 The transpose of .

[0061] As shown above, when the robot is stationary, the initial pose matrix R of the robot's end effector relative to the robot's base coordinate system is... b 0, set the initial attitude matrix R b By converting 0, we can obtain the robot end effector's attitude angles (A0, B0, C0) in the robot's base coordinate system.

[0062] When the robot moves, the angular rate (ω) output by the MEMS gyroscope installed at the robot's end effector can be collected. x ω y ω z Then, the attitude angle is obtained by integrating the angular rate. The attitude angle can be obtained by integrating the angular rate using the Euler angle method and the quaternion method.

[0063] The following section uses the Euler angle method as an example to illustrate the steps for deriving attitude angles using the Euler angle method:

[0064]

[0065] B k =B k-1 +cosA k-1 ω yk T s -sinA k-1 ω zkT s

[0066]

[0067] Where k represents the k-th operation, T s The sampling period is (ω) xk ω yk ω zk (A) represents the k-th acquisition result from the MEMS gyroscope. k B k C k The result is the k-th attitude estimation. From this, the rotation matrix R during the motion process can be obtained from the attitude angles. b k .

[0068] 2) Calculate the second end-effector acceleration in the robot's base coordinates based on the transformation matrix.

[0069] The second terminal acceleration can be calculated using the following formula:

[0070]

[0071] in R is the second terminal acceleration. b k The transformation matrix is... The end-effector acceleration is collected by the second inertial sensor.

[0072] 3) Integrate the second terminal acceleration to obtain the second terminal velocity, and integrate the second terminal velocity to obtain the second terminal pose.

[0073] Specifically, regarding the second terminal acceleration By integrating, we can obtain the velocity (v) of the robot's end effector relative to the robot's base coordinate system. x2 ,v y2 v z2 For velocity (v) x2 ,v y2 v z2 After further integration, the positional change of the robot's end effector (dx2, dy2, dz2) can be obtained. When the robot is stationary, the joint angles of each joint can be collected by the first sensor, and the initial position of the robot's end effector (x0, y0, z0) can be obtained by performing forward kinematics on the joint angles. Thus, the position of the robot's end effector in the robot's base coordinates after the robot moves can be represented as follows:

[0074] x2=x0+dx2

[0075] y2=y0+dy2

[0076] z2=z0+dz2

[0077] The robot's posture can be determined by the rotation transformation matrix R. b k The transformation yields (A2, B2, C2). Thus, the second end pose (x2, y2, z2, A2, B2, C2) can be obtained.

[0078] The system determines whether the second end-effector pose (x2, y2, z2, A2, B2, C2) exceeds a first preset limit. If so, a safety mechanism is triggered. The first preset limit can be a limiting plane. Determining whether the second end-effector pose exceeds the first preset limit requires calculating the distance from (x2, y2, z2) to the limiting plane. If the distance is positive, it indicates that the first preset limit has not been exceeded. If the distance is negative, it indicates that the first preset limit has been exceeded, and a safety mechanism will be triggered. The safety mechanism includes warning, deceleration, braking, and stopping. This invention uses inertial sensors, geomagnetic sensors, and MEMS gyroscopes to calculate the second end-effector pose, resulting in lower cost and wider applicability.

[0079] In some embodiments, the first control unit 312 is further configured to calculate a first end-effector velocity based on the joint angles, determine whether the first end-effector velocity exceeds a second preset limit value, and if so, trigger a safety mechanism. Preferably, the first control unit is further configured to perform differential processing on the joint angles to obtain the joint velocities of each joint; and obtain the first end-effector velocity based on the joint velocities and the Jacobian matrix. Specifically, the first control unit 312 processes the joint angles (q1, q2, ..., q...) collected by the first sensing unit 311. n By performing differential processing, the joint velocities of each joint of the robot can be obtained. Then, the terminal velocity is calculated using the following formula:

[0080]

[0081] Where v is the terminal velocity vector, is the joint velocity vector, and J is the Jacobian matrix. Thus, we can obtain the first end effector velocity (v) of the robot's end effector in the robot's base coordinate system. x1 v y1 v z1 ).

[0082] Determine the first terminal velocity (v) x1 v y1 v z1 If the first terminal velocity (v) exceeds the second preset limit value, a safety mechanism is triggered. The second preset limit value can be preset as needed; this application does not impose any restrictions on its value. The first terminal velocity (v) is then determined. x1 v y1 vz1 Whether the second preset limit value is exceeded can be determined by selecting either a strict or lenient judgment method as needed. For example, in the strict judgment method, the second preset limit value can be set to (T). x T y T z Strict judgment methods include judging v x1 Is it less than T? x , and v y1 Is it less than T? y v z1 Is it less than T? z If the result of any component judgment is negative, then the first end velocity is determined to exceed the second preset limit value, triggering the safety mechanism. In the lenient judgment method, the second preset limit value can be set to T2. The lenient judgment method includes determining the first end velocity (v...) x1 v y1 v z1 Calculate the first end and velocity v1; determine whether the first end and velocity v1 exceed the second preset limit value T2. If so, determine that the first end velocity exceeds the second preset limit value and trigger the safety mechanism.

[0083] Preferably, the first terminal velocity can be calculated using the following formula:

[0084]

[0085] Where v1 represents the first terminal velocity, (v x1 v y1 v z1 ) represents the first terminal velocity.

[0086] In some embodiments, the second control unit 322 is further configured to calculate the second end-effector velocity of the robot end based on the base data and end-effector data acquired by the second sensing unit 321, determine whether the second end-effector velocity exceeds a second preset limit value, and if so, trigger a safety mechanism. As described above, when calculating the second end-effector pose, the end-effector acceleration (g) can be calculated based on the transformation matrix. x2 g y2 g z2 Second end-effector acceleration in robot base coordinates Integrating the second end-effector acceleration, we can obtain the second end-effector velocity (v) of the robot's end-effector relative to the robot's base coordinate system. x2 v y2 v z2 Similarly, determine the second final velocity (v). x2 v y2 v z2Whether the second preset limit value is exceeded can be determined by selecting either a strict or lenient judgment method as needed. For example, in the strict judgment method, the second preset limit value can be set to (T). x T y T z Strict judgment methods include judging v x2 Is it less than T? x , and v y2 Is it less than T? y v z2 Is it less than T? z If the result of any component judgment is negative, then the second terminal velocity is determined to exceed the second preset limit value, triggering the safety mechanism. In the lenient judgment method, the second preset limit value can be set to T2. The lenient judgment method includes determining the second terminal velocity (v...) x2 v y2 v z2 Calculate the second end and velocity v2; determine whether the second end and velocity v2 exceed the second preset limit value T2. If so, determine that the second end velocity exceeds the second preset limit value and trigger the safety mechanism.

[0087] Preferably, the second terminal velocity can be calculated using the following formula:

[0088]

[0089] Where v2 represents the second terminal velocity, (v x2 v y2 v z2 ) represents the second terminal velocity.

[0090] The second preset limit value can be set to (T) x T y T z Determine the second terminal velocity (v) x2 v y2 v z2 Whether it exceeds the second preset limit value includes judging v x2 Is it less than T? x , and v y2 Is it less than T? y v z2 Is it less than T? z If the result of any component is negative, the second end speed is determined to exceed the second preset limit value, triggering the safety mechanism.

[0091] In some embodiments, the robot's safety control system further includes a third control unit (not shown). The third control unit is configured to acquire a first end-effector pose (x1, y1, z1, A1, B1, C1) from a first control unit, acquire a second end-effector pose (x2, y2, z2, A2, B2, C2) from a second control unit, calculate the difference between the first and second end-effector poses, determine whether the difference exceeds a first threshold, and if so, trigger a safety mechanism. Calculating the difference between the first and second end-effector poses includes calculating the differences corresponding to each component of the pose, for example, calculating the difference between x2 and x1, the difference between y2 and y1, ..., the difference between C2 and C1. If the difference of any component exceeds the first threshold, the safety mechanism is triggered. The value of the first threshold can be set as needed, and this application does not limit it.

[0092] In some embodiments, the third control unit is further configured to obtain a first end velocity (v) from the first control unit. x1 v y1 v z1 The second terminal velocity (v) is obtained from the second control unit. x2 v y2 v z2 The system calculates the difference between the first and second end velocities, determines whether the difference exceeds a second threshold, and if so, triggers a safety mechanism. Preferably, calculating the difference between the first and second end velocities includes calculating the difference between a component of the first end velocity and the corresponding component of the second end velocity; for example, calculating v... x2 and v x1 The difference, v y1 and v y2 The difference, v z1 and v z2 The difference between the two thresholds is considered, and if any difference exceeds the second threshold, a security mechanism is triggered. The value of the second threshold can be set as needed. This application does not impose any restrictions on this.

[0093] The robot safety control system of the present invention provides two independent detection units to observe the robot's end effector motion parameters, which has good safety redundancy, increases the reliability and stability of the robot system, and has wider applicability.

[0094] Figure 4 This is a flowchart of a robot safety control method according to an embodiment of the present invention. Figure 4 As shown, the robot safety control method 400 includes the following steps:

[0095] Step S41: Collect the joint angles of each joint of the robot through the first sensing unit.

[0096] Preferably, the first sensing unit includes multiple encoders, each located near a joint. The encoders can be placed on the output side or the input side of the reducer of the joint. The encoders acquire the joint angles of each joint of the robot. The acquired joint angles of each robot joint can be (q1, q2, ..., q...). n ), where q n Let be the joint angle of the nth joint.

[0097] Step S42: Calculate the first end-effector pose of the robot end based on the joint angles, and determine whether the first end-effector pose exceeds the first preset limit value. If so, trigger the safety mechanism.

[0098] Preferably, based on the joint angles (q1, q2, ..., q... n The robot performs forward kinematics to obtain the first end-effector pose. The first end-effector pose can be represented in the form xyzabc, where xyzabc represents [X-axis distance, Y-axis distance, Z-axis distance, yaw angle, pitch angle, and yaw angle]. The first end-effector pose is represented as (x1, y1, z1, A1, B1, C1). To ensure the safety of people interacting with the robot, a first preset limit is set for the robot. If the robot's end-effector exceeds this first preset limit, it may cause harm to people. The first preset limit can be a limiting plane. Determining whether the first end-effector pose exceeds the first preset limit requires calculating the distance from (x1, y1, z1) to the limiting plane. If the distance is positive, it indicates that the first preset limit has not been exceeded. If the distance is negative, it indicates that the first preset limit has been exceeded, triggering a safety mechanism. The safety mechanism includes warning, deceleration, braking, and stopping.

[0099] Step S43: Collect base data of the robot base and end data of the robot end via the second sensing unit. The base data includes base acceleration and base magnetic field position, and the end data includes end acceleration, end magnetic field position, and end angular rate.

[0100] Preferably, the second sensing unit includes a first inertial sensor and a first geomagnetic sensor mounted on the robot base, and a second inertial sensor, a second geomagnetic sensor, and a MEMS gyroscope mounted on the robot end effector. The acquisition of base data and end effector data via the second sensing unit includes: acquiring base acceleration via the first inertial sensor and base magnetic field position via the first geomagnetic sensor; acquiring end effector acceleration via the second inertial sensor and end effector magnetic field position via the second geomagnetic sensor; and acquiring end effector angular rate via the MEMS gyroscope.

[0101] Step S44: Calculate the second end pose of the robot end based on the base data and the end-effector data, and determine whether the second end pose exceeds the first preset limit value. If so, trigger the safety mechanism.

[0102] Figure 5 yes Figure 4 The flowchart for step S44. (See attached flowchart.) Figure 5 As shown, calculating the second end-effector pose of the robot based on the base data and end-effector data includes:

[0103] Step S441: Calculate the transformation matrix from the robot end-effector coordinate system to the robot base coordinate system based on the base data and end-effector data.

[0104] When the robot is stationary, that is, when the joint angles of each joint remain constant, assume that the collected base acceleration is (g x1 g y1 g z1 The location of the base magnetic field is (m) x1 m y1 m z1 The terminal acceleration is (g) x2 g y2 g z2 The position of the end magnetic field is (m) x2 m y2 m z2 First, based on the base acceleration (g) x1 g y1 g z1 ) and the position of the base magnetic field (m) x1 m y1 m z1 Calculate the rotation matrix R of the robot base relative to the world coordinate system. 1 Then, based on the terminal acceleration (g) x2 g y2 g z2 ) and the position of the terminal magnetic field (m x2 m y2 m z2 Calculate the rotation matrix R of the robot's end effector relative to the world coordinate system. 2 Based on the rotation matrix R1 of the robot base relative to the world coordinate system and the rotation matrix R of the robot end effector relative to the world coordinate system... 2 Calculate the transformation matrix R from the robot's end-effector coordinate system to the robot's base coordinate system. b :

[0105] R b =R 2′ R 1

[0106] Where R2′ R represents 2 The transpose of .

[0107] When the robot moves, the angular velocity (ω) is collected. x ω y ω z Then, the attitude angles are obtained by integrating the angular rate. The attitude angles can be obtained by integrating the angular rate using the Euler angles method. From the attitude angles, the rotation matrix R during the motion can be obtained. b k .

[0108] Step S442: Calculate the second end-effector acceleration in the robot base coordinates based on the transformation matrix.

[0109] The second terminal acceleration can be calculated using the following formula:

[0110]

[0111] in R is the second terminal acceleration. b k The transformation matrix is... This refers to the terminal acceleration.

[0112] Step S443: Integrate the second terminal acceleration to obtain the second terminal velocity, and integrate the second terminal velocity to obtain the second terminal pose.

[0113] Specifically, regarding the second terminal acceleration By integrating, we can obtain the velocity (v) of the robot's end effector relative to the robot's base coordinate system. x2 v y2 v z2 For velocity (v) x2 v y2 v z2 After further integration, the positional changes of the robot's end effector (dx2, dy2, dz2) can be obtained. When the robot is stationary, the joint angles of each joint can be collected by the first sensor, and the initial position of the robot's end effector (x0, y0, z0) can be obtained by forward solving the joint angles. Thus, the position of the robot's end effector in the robot's base coordinate system after the robot moves can be represented as follows:

[0114] x2=x0+dx2

[0115] y2=y0+dy2

[0116] z2=z0+dz2

[0117] The robot's posture can be determined by the rotation transformation matrix R. b kThe transformation yields (A2, B2, C2). Thus, the second end pose (x2, y2, z2, A2, B2, C2) can be obtained.

[0118] For a more detailed explanation of steps S441 to S443, please refer to the above. Figure 3 The configuration of the second control unit 322 will not be repeated here.

[0119] The system determines whether the second end-effector pose (x2, y2, z2, A2, B2, C2) exceeds a first preset limit. If so, a safety mechanism is triggered. The first preset limit can be a limiting plane. To determine whether the second end-effector pose exceeds the first preset limit, the distance from (x2, y2, z2) to the limiting plane needs to be calculated. If the distance is positive, it indicates that the first preset limit has not been exceeded. If the distance is negative, it indicates that the first preset limit has been exceeded, and a safety mechanism will be triggered. The safety mechanism includes warning, deceleration, braking, and stopping.

[0120] In some embodiments, the method further includes the steps of: calculating a first end-effector velocity of the robot end effector based on joint angles, determining whether the first end-effector velocity exceeds a second preset limit value, and if so, triggering a safety mechanism. Calculating the first end-effector velocity based on joint angles includes adjusting the joint angles (q1, q2, ..., q...). n By performing differential processing, the joint velocities of each joint of the robot can be obtained. Then, the terminal velocity is calculated using the following formula:

[0121]

[0122] Where v is the terminal velocity vector, is the joint velocity vector, and J is the Jacobian matrix. Thus, we can obtain the first end effector velocity (v) of the robot's end effector in the robot's base coordinate system. x1 v y1 v z1 Determine the first terminal velocity (v) x1 v y1 v z1 If the first terminal velocity (v) exceeds the second preset limit value, a safety mechanism is triggered. The second preset limit value can be preset as needed; this application does not impose any restrictions on its value. The first terminal velocity (v) is then determined. x1 v y1 v z1 Whether the second preset limit value is exceeded can be determined by selecting either a strict or lenient judgment method as needed. For example, in the strict judgment method, the second preset limit value can be set to (T). x T y T zStrict judgment methods include judging v x1 Is it less than T? x , and v y1 Is it less than T? y v z1 Is it less than T? z If the result of any component judgment is negative, then the first end velocity is determined to exceed the second preset limit value, triggering the safety mechanism. In the lenient judgment method, the second preset limit value can be set to T2. The lenient judgment method includes determining the first end velocity (v...) x1 v y1 v z1 Calculate the first end and velocity v1; determine whether the first end and velocity v1 exceed the second preset limit value T2. If so, determine that the first end velocity exceeds the second preset limit value and trigger the safety mechanism.

[0123] Preferably, the first terminal velocity can be calculated using the following formula:

[0124]

[0125] Where v1 represents the first terminal velocity, (v x1 v y1 v z1 ) represents the first terminal velocity.

[0126] In some embodiments, the method further includes the steps of: calculating a second end-effector velocity of the robot end-effector based on base data and end-effector data; determining whether the second end-effector velocity exceeds a second preset limit value; and if so, triggering a safety mechanism. Calculating the second end-effector velocity based on base data and end-effector data includes: calculating a transformation matrix from the robot end-effector coordinate system to the robot base coordinate system based on the base data and end-effector data; calculating the second end-effector acceleration in the robot base coordinate system based on the transformation matrix; and integrating the second end-effector acceleration to obtain the second end-effector velocity. When calculating the second end-effector pose, the end-effector acceleration (g) can be calculated based on the transformation matrix. x2 g y2 g z2 Second end-effector acceleration in robot base coordinates Integrating the second end-effector acceleration, we can obtain the second end-effector velocity (v) of the robot's end-effector relative to the robot's base coordinate system. x2 v y2 v z2 The second preset limit value can be set in advance as needed; this application does not impose any restriction on the value of the second preset limit value. Similarly, the second terminal velocity (v) is determined. x2 v y2 v z2Whether the second preset limit value is exceeded can be determined by selecting either a strict or lenient judgment method as needed. For example, in the strict judgment method, the second preset limit value can be set to (T). x T y T z Strict judgment methods include judging v x2 Is it less than T? x , and v y2 Is it less than T? y v z2 Is it less than T? z If the result of any component judgment is negative, then the second terminal velocity is determined to exceed the second preset limit value, triggering the safety mechanism. In the lenient judgment method, the second preset limit value can be set to T2. The lenient judgment method includes determining the second terminal velocity (v...) x2 v y2 v z2 Calculate the second end and velocity v2; determine whether the second end and velocity v2 exceed the second preset limit value T2. If so, determine that the second end velocity exceeds the second preset limit value and trigger the safety mechanism.

[0127] Preferably, the second terminal velocity can be calculated using the following formula:

[0128]

[0129] Where v2 represents the second terminal velocity, (v x2 v y2 v z2 ) represents the second terminal velocity.

[0130] In some embodiments, the method further includes the steps of: calculating the difference between the first end-effector pose and the second end-effector pose, determining whether the difference exceeds a first threshold, and if so, triggering a security mechanism. Determining whether the difference exceeds the first threshold includes: determining whether the difference between any component of the first end-effector pose and the second end-effector pose exceeds the first threshold; if so, triggering a security mechanism. Calculating the difference between the first end-effector pose (x1, y1, z1, A1, B1, C1) and the second end-effector pose (x2, y2, z2, A2, B2, C2) includes calculating the differences corresponding to each component in the pose, for example, calculating the difference between x2 and x1, the difference between y2 and y1, ..., the difference between C2 and C1. If the difference of any component exceeds the first threshold, the security mechanism is triggered. The value of the first threshold can be set as needed, and this application does not limit it.

[0131] In some embodiments, the method further includes the steps of: calculating the difference between a first end velocity and a second end velocity, determining whether the difference exceeds a second threshold, and if so, triggering a safety mechanism. Preferably, the first end velocity (v) is calculated...x1 v y1 v z1 ) and the second terminal velocity (v x2 v y2 v z2 The difference includes calculating the difference between the component of the first terminal velocity and the corresponding component of the second terminal velocity. For example, calculating v... x2 and v x1 The difference, v y1 and v y2 The difference, v z1 and v z2 The difference between the two thresholds is considered, and if any difference exceeds the second threshold, a security mechanism is triggered. The value of the second threshold can be set as needed. This application does not impose any restrictions on this.

[0132] The basic concepts have been described above. Obviously, for those skilled in the art, the above disclosure is merely illustrative and does not constitute a limitation of this application. Although not explicitly stated herein, those skilled in the art may make various modifications, improvements, and corrections to this application. Such modifications, improvements, and corrections are suggested in this application, and therefore remain within the spirit and scope of the exemplary embodiments of this application.

[0133] Furthermore, this application uses specific terms to describe embodiments of the application. For example, "an embodiment," "one embodiment," and / or "some embodiments" refer to a particular feature, structure, or characteristic related to at least one embodiment of the application. Therefore, it should be emphasized and noted that "an embodiment," "one embodiment," or "an alternative embodiment" mentioned twice or more in different locations in this specification do not necessarily refer to the same embodiment. In addition, certain features, structures, or characteristics in one or more embodiments of the application can be appropriately combined.

[0134] Some aspects of this application can be executed entirely by hardware, entirely by software (including firmware, resident software, microcode, etc.), or by a combination of hardware and software. The aforementioned hardware or software may be referred to as a "data block," "module," "engine," "unit," "component," or "system." The processor may be one or more application-specific integrated circuits (ASICs), digital signal processors (DSPs), digital signal processing devices (DAPDs), programmable logic devices (PLDs), field-programmable gate arrays (FPGAs), processors, controllers, microcontrollers, microprocessors, or combinations thereof. Furthermore, aspects of this application may manifest as computer products residing in one or more computer-readable media, including computer-readable program code. For example, computer-readable media may include, but are not limited to, magnetic storage devices (e.g., hard disks, floppy disks, magnetic tapes, etc.), optical discs (e.g., compressed CDs, digital multifunction DVDs, etc.), smart cards, and flash memory devices (e.g., cards, sticks, key drives, etc.).

[0135] A computer-readable medium may contain a propagated data signal containing computer program code, for example, on baseband or as part of a carrier wave. This propagated signal may take various forms, including electromagnetic, optical, and so on, or suitable combinations thereof. A computer-readable medium can be any computer-readable medium other than a computer-readable storage medium, which can be connected to an instruction execution system, apparatus, or device to enable communication, propagation, or transmission of a program for use. The program code located on the computer-readable medium can be propagated through any suitable medium, including radio, cable, fiber optic cable, radio frequency signals, or similar media, or any combination of the above media.

[0136] Similarly, it should be noted that, in order to simplify the description of the present application and thus aid in the understanding of one or more embodiments of the invention, the foregoing description of the embodiments of the present application sometimes combines multiple features into a single embodiment, drawing, or description thereof. However, this disclosure method does not imply that the subject matter of the application requires more features than those mentioned in the claims. In fact, the embodiments contain fewer features than all the features of the single embodiments disclosed above.

[0137] Although this application has been described with reference to specific embodiments, those skilled in the art should recognize that the above embodiments are only used to illustrate this application, and various equivalent changes or substitutions can be made without departing from the spirit of this application. Therefore, any changes or modifications to the above embodiments within the essential spirit of this application will fall within the scope of the claims of this application.

Claims

1. A safety control method for a robot, characterized in that, include: The joint angles of each joint of the robot are collected by the first sensing unit; The robot end-effector pose is calculated based on the joint angles. It is then determined whether the first end-effector pose exceeds a first preset limit value. If so, a safety mechanism is triggered. The second sensing unit collects base data of the robot base and end data of the robot end effector. The base data includes base acceleration and base magnetic field position, and the end effector data includes end effector acceleration, end effector magnetic field position, and end effector angular velocity. The second end pose of the robot end is calculated based on the base data and the end-effector data. It is then determined whether the second end pose exceeds the first preset limit value. If so, a safety mechanism is triggered. The calculation of the second end-effector pose based on the base data and the end-effector data includes: calculating the rotation matrix of the robot base relative to the world coordinate system based on the base acceleration and the base magnetic field position; calculating the rotation matrix of the robot end-effector relative to the world coordinate system based on the end-effector acceleration and the end-effector magnetic field position; calculating the transformation matrix from the robot end-effector coordinate system to the robot base coordinate system based on the rotation matrix of the robot base relative to the world coordinate system and the rotation matrix of the robot end-effector relative to the world coordinate system; obtaining the attitude angle by integrating the end-effector angular rate, and obtaining the rotation matrix during the motion process from the attitude angle and the transformation matrix; calculating the second end-effector acceleration in the robot base coordinate system based on the rotation matrix during the motion, integrating the second end-effector acceleration to obtain the second end-effector velocity, and integrating the second end-effector velocity to obtain the second end-effector pose.

2. The method as described in claim 1, characterized in that, The first sensing unit includes multiple encoders, each located near a joint.

3. The method as described in claim 1, characterized in that, The second sensing unit includes a first inertial sensor and a first geomagnetic sensor mounted on the robot base, and a second inertial sensor, a second geomagnetic sensor, and a MEMS gyroscope mounted on the robot end effector. The first inertial sensor acquires the base acceleration, the first geomagnetic sensor acquires the base magnetic field position, the second inertial sensor acquires the end effector acceleration, the second geomagnetic sensor acquires the end effector magnetic field position, and the MEMS gyroscope acquires the end effector angular rate.

4. The method as described in claim 1, characterized in that, Calculating the first end-effector pose of the robot based on the joint angles includes: performing forward homing calculations on the joint angles to obtain the first end-effector pose.

5. The method as described in claim 1, characterized in that, Calculating the second end-effector pose of the robot end based on the base data and the end-effector data includes: Calculate the transformation matrix from the robot's end-effector coordinate system to the robot's base coordinate system based on the base data and the end-effector data; Calculate the second end-effector acceleration in the robot's base coordinates based on the transformation matrix; Integrating the second terminal acceleration yields the second terminal velocity, and integrating the second terminal velocity yields the second terminal pose.

6. The method as described in claim 1, characterized in that, The safety mechanisms include warning, deceleration, braking, and stopping.

7. The method as described in claim 1, characterized in that, Also includes: Calculate the difference between the first end pose and the second end pose, and determine whether the difference exceeds a first threshold. If so, trigger a security mechanism.

8. The method as described in claim 7, characterized in that, Determining whether the difference exceeds the first threshold includes: determining whether the difference between any component of the first end pose and the second end pose exceeds the first threshold; if so, triggering a security mechanism.

9. The method as described in claim 1, characterized in that, Also includes: The first end-effector velocity of the robot is calculated based on the joint angle. It is then determined whether the first end-effector velocity exceeds a second preset limit value. If so, a safety mechanism is triggered. The second end-effector speed of the robot is calculated based on the base data and the end-effector data. It is then determined whether the second end-effector speed exceeds the second preset limit value. If so, a safety mechanism is triggered.

10. The method as described in claim 9, characterized in that, The calculation of the first end-effector velocity of the robot's end effector based on the joint angle includes: The joint angles are differentially processed to obtain the joint velocities of each joint; The first end velocity is obtained based on the joint velocity and the Jacobian matrix.

11. The method as described in claim 1, characterized in that, Also includes: Calculate the speed difference between the first end velocity and the second end velocity, and determine whether the speed difference exceeds a second threshold. If it does, trigger a safety mechanism.

12. A safety control system for a robot, characterized in that, include: The first detection unit includes: The first sensing unit includes multiple encoders, each encoder being located near a joint. The first sensing unit is configured to collect the joint angles of each joint of the robot. The first control unit is configured to calculate the first end pose of the robot end based on the joint angle, determine whether the first end pose exceeds a first preset limit value, and if so, trigger a safety mechanism. The second detection unit includes: The second sensing unit includes a first inertial sensor and a first geomagnetic sensor mounted on the robot base, and a second inertial sensor, a second geomagnetic sensor, and a MEMS gyroscope mounted on the robot end effector. The second sensing unit is configured to collect base data of the robot base and end effector data of the robot end effector. The base data includes base acceleration and base magnetic field position, and the end effector data includes end effector acceleration, end effector magnetic field position, and end effector angular rate. The second control unit is configured to calculate the second end pose of the robot end based on the base data and the end data, determine whether the second end pose exceeds the first preset limit value, and if so, trigger a safety mechanism. The calculation of the second end-effector pose based on the base data and the end-effector data includes: calculating the rotation matrix of the robot base relative to the world coordinate system based on the base acceleration and the base magnetic field position; calculating the rotation matrix of the robot end-effector relative to the world coordinate system based on the end-effector acceleration and the end-effector magnetic field position; calculating the transformation matrix from the robot end-effector coordinate system to the robot base coordinate system based on the rotation matrix of the robot base relative to the world coordinate system and the rotation matrix of the robot end-effector relative to the world coordinate system; obtaining the attitude angle by integrating the end-effector angular rate, and obtaining the rotation matrix during the motion process from the attitude angle and the transformation matrix; calculating the second end-effector acceleration in the robot base coordinate system based on the rotation matrix during the motion, integrating the second end-effector acceleration to obtain the second end-effector velocity, and integrating the second end-effector velocity to obtain the second end-effector pose.

13. The system as described in claim 12, characterized in that, include: The first control unit is further configured to calculate the first end-effector velocity of the robot end based on the joint angle, determine whether the first end-effector velocity exceeds a second preset limit value, and if so, trigger a safety mechanism. The second control unit is further configured to calculate the second end speed of the robot end based on the base data and the end data, determine whether the second end speed exceeds the second preset limit value, and if so, trigger a safety mechanism.

14. The system as described in claim 13, characterized in that, The first control unit is further configured to perform differential processing on the joint angles to obtain the joint velocities of each joint; and to obtain the first end-effector velocity based on the joint velocities and the Jacobian matrix.

15. The system as described in claim 12, characterized in that, Also includes: The third control unit is configured to obtain the first end-effector pose from the first control unit, obtain the second end-effector pose from the second control unit, calculate the difference between the first end-effector pose and the second end-effector pose, determine whether the difference exceeds a first threshold, and if so, trigger a security mechanism.

Images