CN109848995B - Industrial robot collision reaction method - Google Patents

Abstract

The invention proposes a collision reaction method for an industrial robot. After the robot collides, it firstly determines the direction of retreat according to the direction of the collision force, then estimates the stiffness and the retreat speed of the hit object according to the collision force and speed, and then calculates it in Cartesian space in combination with the initial coordinates. and the backward trajectory of the joint space, and finally the robot performs a backward motion. According to the different stiffness and collision position of the collided object, the present invention adopts different retreating speeds and retreating modes, so that the control system of the industrial robot can make timely and appropriate collision responses to the detected collisions, so as to avoid possible damages.

Description

A method of industrial robot collision response

technical field

The invention belongs to the technical field of industrial robots, in particular to a collision reaction method for industrial robots.

Background technique

Industrial robots are multi-degree-of-freedom manipulators or machine devices for industrial fields. Industrial robots are widely used in industrial production such as welding, grinding and polishing, spraying, handling, and assembly. In recent years, with the wider application of industrial robots, especially the application of multi-robot collaboration and human-robot collaboration, people have higher and higher requirements for robot safety.

When the robot detects a collision with the external environment, the easiest way is to stop immediately and keep the original position to prevent the robot from continuing to move and causing more serious damage. However, it is not enough to stop the robot. Since the robot generally has a large rigidity, when the robot collides with the workpiece, or the workpiece at the end of the robot collides with the workpiece, when the robot detects a collision and stops in time, it may cause one of the two to collide. A large contact force is maintained between them, which further damages the workpiece. In addition, when the robot collides with a person and stops in time, the person may be clamped and unable to move, and can only wait until the operator moves the robot away. During this period, it will inevitably cause further damage to the hit people. Therefore, when the robot detects a collision with the external environment, a simple shutdown is not enough, and a timely and appropriate collision response must be made. However, the working environment of the robot is very complex, and the types of collisions are also various. A feasible method is to establish a robot collision reaction action library, so that the user can choose the corresponding reaction action according to the actual working conditions.

SUMMARY OF THE INVENTION

The technical problem solved by the present invention is to provide a collision reaction method for an industrial robot, which adopts different retreating speeds and retreating modes according to different stiffnesses and collision positions of the objects to be collided with, so that the control system of the industrial robot can respond to the detected collisions. Prompt and appropriate collision response to avoid possible damage.

The technical solution that realizes the object of the present invention is:

A method for industrial robot collision reaction, comprising the following steps:

Step 1: Set the receding distance or receding angle displacement of the industrial robot after the collision in the host computer. When the industrial robot collides with an external object, collect and record the collision information of the industrial robot: record the position of the robot end at the collision position. Initial Cartesian space coordinates P(x ₀ , y ₀ , z ₀ ), initial joint space coordinates Q (q ₁ , q ₂ , q ₃ , q ₄ , q ₅ , q ₆ ) at , and industrial robots and external objects The maximum collision force _Fe between;

Step 2: Determine the backward direction of the industrial robot in the Cartesian space as the direction of the maximum collision force exerted by the collided object on the industrial robot when the collision occurs, namely:

Among them, r is the backward direction vector of the industrial robot, F _ex , F _yx , and F _zx are the components of the maximum collision force F _e in Cartesian space, and |F _e | is the size of the maximum collision force;

Step 3: Calculate the estimated stiffness σ ^* of the hit object according to the maximum collision force and the speed of the industrial robot;

Step 4: Calculate the backward speed v of the industrial robot after the collision according to the estimated stiffness σ ^* of the collision object;

Step 5: Calculate the backward trajectory ⁰ p of the industrial robot in the Cartesian space and the backward trajectory q in the joint space according to the initial Cartesian space coordinates, the backward direction and the backward speed of the industrial robot, and control the industrial robot to retreat according to the backward direction. The backward motion is performed along the backward trajectory ⁰ p in Cartesian space and the backward trajectory q in joint space;

Step 6: When the industrial robot retreats by the retreat distance or the retreat angle, it stops the action and waits for the next command from the upper computer.

Further, in the industrial robot collision response method of the present invention, in step 1, the magnitude and direction of the collision force between the industrial robot and the external environment are collected by a six-dimensional force sensor installed at the end of the industrial robot.

Further, in the industrial robot collision response method of the present invention, the calculation formula of the stiffness estimation value σ ^* in step 3 is:

Among them, |F _e | is the magnitude of the maximum collision force, v _r is the instantaneous speed of the collision position of the industrial robot when the collision occurs, and t ^* is the time it takes for the collision force to rise from zero to the maximum value.

Further, in the industrial robot collision reaction method of the present invention, the calculation formula of the retreat speed v in step 4 is:

where K ^* is an adjustment coefficient, and K ^* is a positive real number.

Further, in the industrial robot collision response method of the present invention, in step 5, the calculation formula of the backward trajectory ⁰ p of the industrial robot in the Cartesian space is:

The formula for calculating the backward trajectory q in the joint space is:

为发生碰撞的关节i坐标系至基坐标系的坐标变化矩阵；q∈R^6×1为工业机器人各关节的位置。in,

is the coordinate change matrix from the collided joint i coordinate system to the base coordinate system; q∈R ^6×1 is the position of each joint of the industrial robot.

Further, in the industrial robot collision response method of the present invention, the value range of K ^* is 1×10 ^-5 to 1×10 ^-4 .

Compared with the prior art, the present invention adopts the above technical scheme, and has the following technical effects:

1. The industrial robot collision response method of the present invention can automatically set different backward speeds and backward modes according to the different stiffness and collision position of the collision object, so that the industrial robot can make a timely and appropriate collision response to the collision, and avoid the possibility of causing the collision. damage;

2. The industrial robot collision response method of the present invention can customize the retreat distance, and realizes the adjustment of the retreat distance according to the actual working conditions;

3. The collision reaction method of the industrial robot of the present invention can make the robot avoid the occurrence of clamping the collided person after the collision with the person.

Description of drawings

Fig. 1 is the collision schematic diagram of the industrial robot colliding with the aluminum block according to Embodiment 1 of the present invention;

Fig. 2 is the collision schematic diagram of the industrial robot colliding with the wooden block according to the second embodiment of the present invention;

3 is a schematic diagram of the collision force and speed of the industrial robot colliding with the aluminum block according to Embodiment 1 of the present invention;

4 is a schematic diagram of the collision force and speed of the industrial robot colliding with a wooden block according to Embodiment 2 of the present invention;

FIG. 5 is a flow chart of the industrial robot collision response method of the present invention.

Detailed ways

Embodiments of the present invention are described in detail below, examples of which are illustrated in the accompanying drawings, wherein the same or similar reference numerals refer to the same or similar elements or elements having the same or similar functions throughout. The embodiments described below with reference to the accompanying drawings are exemplary and are only used to explain the present invention, but not to be construed as a limitation of the present invention.

A method for industrial robot collision response, as shown in Figure 5, includes the following steps:

Step 1: Set the receding distance or receding angle displacement of the industrial robot after the collision in the host computer. When the industrial robot collides with an external object, collect and record the collision information of the industrial robot: record the position of the robot end at the collision position. Initial Cartesian space coordinates P(x ₀ , y ₀ , z ₀ ) at , initial joint space coordinates Q (q ₁ , q ₂ , q ₃ , q ₄ , q ₅ , q ₆ ), and through the six-dimensional force sensor Collect the maximum collision force _Fe between the industrial robot and the external object;

Step 2: Determine the backward direction of the industrial robot in the Cartesian space as the direction of the maximum collision force exerted by the collided object on the industrial robot when the collision occurs, namely:

Among them, r is the backward direction vector of the industrial robot, F _ex , F _yx , and F _zx are the components of the maximum collision force F _e in Cartesian space, and |F _e | is the size of the maximum collision force;

Step 3: Calculate the estimated stiffness σ ^* of the hit object based on the maximum collision force F _e and the speed of the industrial robot:

Among them, |F _e | is the size of the maximum collision force, v _r is the instantaneous speed of the collision position of the industrial robot when the collision occurs, and t ^* is the time it takes for the collision force to rise from zero to the maximum value;

Step 4: Calculate the backward speed v of the industrial robot after the collision according to the estimated stiffness σ ^* of the collision object:

Among them, K ^* is the adjustment coefficient, and K ^* is a positive real number;

Step 5: Calculate the backward trajectory ⁰ p of the industrial robot in the Cartesian space and the backward trajectory q in the joint space according to the initial Cartesian space coordinates, backward direction and backward speed of the industrial robot:

为发生碰撞的关节i坐标系至基坐标系的坐标变化矩阵；q∈R^6×1为工业机器人各关节的位置；in,

is the coordinate change matrix from the collided joint i coordinate system to the base coordinate system; q∈R ^6×1 is the position of each joint of the industrial robot;

And control the industrial robot to move backward along the backward trajectory ⁰ p in the Cartesian space and the backward trajectory q in the joint space according to the backward direction and the backward speed;

Step 6: When the industrial robot retreats by the retreat distance, it stops and waits for the next command from the upper computer.

Example 1

This embodiment is described in detail by operating the ER30 industrial robot of Estun Robot Engineering Co., Ltd. to move in the negative direction of the z-axis in Cartesian space as an example. As shown in Figure 1, a Aluminum block, the industrial robot collides with the aluminum block during the movement, and the end of the industrial robot is equipped with a six-dimensional force sensor to detect the collision force information between the robot and the external environment.

The DH parameters of the ER30 industrial robot are shown in Table 1:

Table 1ER30 robot DH parameter table

connecting rod i α_i-1(°) a_i-1(mm) d_i(mm) θ_i(°) 1 0 0 412 0 2 90 200 0 90 3 0 800 0 0 4 90 165 899 0 5 -90 0 0 0 6 90 0 220 0

S1. Record the collision information of the industrial robot

When the ER30 robot collides with the aluminum block, record the Cartesian space coordinates P ₁ (825, 50, 623) where the robot end is at this time, and record the maximum collision force F _e1 =126N between the robot and the aluminum block when the collision occurs.

S2. Determine the backward direction of the industrial robot

The backward direction of the robot is set as: when the robot collides, the direction in which the collision object exerts the maximum collision force on the robot. Therefore, the backward direction of the robot after the collision is:

S3. Calculate the backward speed of the industrial robot

When the rigidity of the object hit by the robot is large, the robot is likely to maintain a large contact force with the object. At this time, the retreat speed v should be set to a large value, so that the contact force decreases rapidly; when the rigidity of the object hit by the robot is small, The contact force maintained by the robot and the object is small. At this time, the retreat speed v should be set to a small value to prevent other collisions from being too fast. The estimated stiffness σ ^* of the aluminum block in this example is calculated to be about 10 ³ N/m, so the estimated stiffness σ ^* of the aluminum block is multiplied by an adjustment factor K ^* to determine the robot's back-off speed, and the adjustment factor K ^* is set as 10 ^-4 , the backward speed v ₁ =0.1m/s of the robot colliding with the aluminum block is calculated.

S4, control the industrial robot to retreat

After calculating the backward speed and direction of the industrial robot, the joint space motion trajectory of the robot can be calculated according to the inverse solution matrix of the industrial robot, and the robot can be controlled to retreat along the direction of the maximum collision force.

以及机器人的运动速度v_z所绘制的曲线。在t₁时，控制系统控制机器人停止z轴逆方向的运动，转化为沿z轴正方向运动，即碰撞的相反方向，远离被撞物体，后退的速度约为0.1m/s。As shown in Figure 3, the collision force information F _z collected by the robot in the reaction experiment of colliding with the aluminum block,

And the curve drawn by the robot's movement speed v _z . At t ₁ , the control system controls the robot to stop the movement in the reverse direction of the z-axis, and convert it into a movement in the positive direction of the z-axis, that is, the opposite direction of the collision, away from the object to be hit, and the backward speed is about 0.1m/s.

The analysis shows that when the robot collides with the aluminum block, if the robot stops moving and remains in place, the collision part of the robot will maintain a contact force of about 130N with the aluminum block. However, the collision reaction action set by this method can cause the robot to retreat in the opposite direction of the collision, cancel the contact between the robot and the object to be hit, so that the contact force can be reduced to 0 in a short time (about 0.2s), and the robot retreat speed It can be automatically set to a reasonable value (0.1m/s in this example) according to the estimated stiffness of the collided object, which shows that the collision response of this method can timely and reasonably reduce the collision between the robot and the collided object. contact force.

Example 2

As shown in Figure 2, a wooden block is pre-placed on the motion path of the industrial robot, and the industrial robot collides with the wooden block during the movement.

S1. Record the collision information of the industrial robot

When the ER30 robot collides with the wooden block, record the Cartesian space coordinates P ₂ (825, 50, 635) where the robot end is at this time, and record the maximum collision force F _e2 =143N between the robot and the wooden block when the collision occurs.

S2. Determine the backward direction of the industrial robot

The backward direction of the robot is set as: when the robot collides, the direction in which the collision object exerts the maximum collision force on the robot. Therefore, the backward direction of the robot after the collision is:

S3. Calculate the backward speed of the industrial robot

The estimated stiffness σ ^* of the struck object is calculated to be 7×10 ² N/m. The adjustment coefficient K ^* is set to 10 ⁻⁴ , and V ₂ =0.07m/s is calculated.

S4, control the industrial robot to retreat

After calculating the backward speed and direction of the industrial robot, the joint space motion trajectory of the robot can be calculated according to the inverse solution matrix of the industrial robot, and the robot can be controlled to retreat along the direction of the maximum collision force.

以及机器人的运动速度v_z所绘制的曲线。在t₂时，控制系统控制机器人停止z轴逆方向的运动，转化为沿z轴正方向运动，即碰撞的相反方向，远离被撞物体，后退的速度约为0.07m/s。As shown in Figure 4, the collision force information F _z collected by the robot in the reaction experiment of colliding with the wooden block,

And the curve drawn by the robot's movement speed v _z . At t ₂ , the control system controls the robot to stop the movement in the reverse direction of the z-axis, and convert it into a movement in the positive direction of the z-axis, that is, in the opposite direction of the collision, away from the object to be hit, and the backward speed is about 0.07m/s.

The analysis shows that when the robot collides with the wooden block, if the robot stops moving, the collision part of the robot will maintain a contact force of about 150N with the wooden block. However, using the collision response action set by this method, the robot retreats in the opposite direction of the collision, so that the contact force can drop to 0 in about 0.2s, and the robot's retreat speed is automatically set to 0.07m/ s, which shows that the collision response of this method can timely and reasonably reduce the contact force between the robot and the collided object after the collision occurs.

The above are only some embodiments of the present invention. It should be pointed out that for those skilled in the art, some improvements can be made without departing from the principles of the present invention, and these improvements should be regarded as the present invention. scope of protection.

Claims (6)

1. An industrial robot collision reaction method is characterized by comprising the following steps:

step 1: set for the distance of retreating or the angle displacement of retreating of industrial robot after the collision in the host computer, when industrial robot and external object bump, gather and record industrial robot's collision information: recording the initial Cartesian space coordinate P (x) at which the robot tip is located at the collision location₀，y₀，z₀) Initial joint space coordinate Q (Q)₁，q₂，q₃，q₄，q₅，q₆) And the maximum collision force F between the industrial robot and the external object_e；

Step 2: determining the retreating direction of the industrial robot in the Cartesian space as the direction of the maximum collision force applied to the industrial robot by a collided object when a collision occurs, namely:

wherein r is a retreating direction vector of the industrial robot, F_ex，F_yx，F_zxIs the maximum collision force F_eComponent in Cartesian space, | F_eI is the maximum impact force;

and step 3: calculating the rigidity estimated value sigma of the collided object according to the maximum collision force and the instantaneous speed of the industrial robot^*；

And 4, step 4: rigidity estimation value sigma according to collided object^*Computing industryThe retreating speed v of the robot after collision;

and 5: calculating the retreating track of the industrial robot in the Cartesian space according to the initial Cartesian space coordinate, the retreating direction and the retreating speed of the industrial robot⁰p and a retreating track q in the joint space, and controlling the industrial robot to follow the retreating track in the Cartesian space at a retreating speed according to a retreating direction⁰p and a retreating track q in the joint space perform retreating action;

step 6: and when the industrial robot retreats by the retreating distance or the retreating angular displacement, stopping the action and waiting for the next command of the upper computer.

2. The collision reaction method for an industrial robot according to claim 1, characterized in that in step 1, the magnitude and direction of the collision force between the industrial robot and the external environment are collected by a six-dimensional force sensor installed at the end of the industrial robot.

3. An industrial robot collision reaction method according to claim 1, characterized in that the stiffness estimate σ in step 3^*The calculation formula of (2) is as follows:

wherein, | F_eL is the magnitude of the maximum impact force, v_rIs the instantaneous speed, t, of the collision position of the industrial robot when the collision occurs^*The time it takes for the collision force to rise from zero to a maximum.

4. A collision reaction method for an industrial robot according to claim 3, characterized in that the formula for the retreat velocity v in step 4 is:

wherein, K^*For adjustingPitch coefficient, and K^*Are positive real numbers.

5. Method for collision reaction of an industrial robot according to claim 4, characterised in that in step 5 the retreat trajectory of the industrial robot in Cartesian space⁰The formula for p is:

the formula for calculating the retreating track q in the joint space is:

wherein,

a coordinate change matrix from a joint i coordinate system to a base coordinate system, wherein the joint i coordinate system is in collision; q is an element of R^6×1The positions of the joints of the industrial robot.

6. An industrial robot collision reaction method according to claim 4, characterized in that K^*Is in the range of 1 × 10^-5～1×10^-4。

Images