Self-collision prevention method for special-shaped five-joint robot

Abstract

This invention provides a self-collision prevention method for an irregularly shaped five-joint robot, relating to the field of robot motion control, and includes the following steps: Step S1: Calculate the target angles for the second to fourth joints to reach the required pose based on the spatial position of the target being processed; calculate the spatial position of the end-effector center point of the third joint based on the target angles of the second and third joints; Step S2: Determine whether self-collision will occur based on the target angle of the fourth joint, the spatial position of the end-effector center point of the third joint, and the tool. If so, proceed to Step S3; otherwise, do not process; Step S3: If the current running speed of each joint exceeds 50% of the normal maximum allowable speed, initiate an emergency stop; otherwise, pause. This invention can effectively improve the robot's motion safety and reduce maintenance costs.

Description

Technical Field

[0001] This invention relates to the field of robot motion control, and in particular to a method for preventing self-collision in a non-standard five-joint robot. Background Technology

[0002] Irregularly shaped five-jointed robots are commonly used in automation, manufacturing, and medical fields. They are flexible in design, powerful in function, and capable of performing a variety of complex tasks. With the rapid development of robotics technology, the use of irregularly shaped five-jointed robots is becoming increasingly convenient. However, with simplified control methods, especially in workpiece coordinate system programming motion, robot self-collision has become possible. Therefore, ensuring that the motion controller can effectively prevent collisions and accidental contact during task execution, and reduce maintenance costs, has become a key focus of technological research. Summary of the Invention

[0003] The main objective of this invention is to propose a self-collision prevention method for irregular five-joint robots, which can effectively improve the robot's motion safety and reduce maintenance costs.

[0004] This invention is achieved through the following technical solution:

[0005] A method for preventing self-collision in a five-joint alien robot, the five-joint alien robot comprising a first joint to a fourth joint connected sequentially from bottom to top, and a tool mounted at the end of the fourth joint, comprising the following steps:

[0006] Step S1: Based on the spatial position of the target to be processed, calculate the target angles from the second to the fourth joints to achieve the required pose for processing, and calculate the spatial position of the center point of the end of the third joint based on the target angles of the second and third joints.

[0007] Step S2: Based on the target angle of the fourth joint, the spatial position of the center point of the end of the third joint, and the tool, determine whether it will cause self-collision. If so, proceed to step S3; otherwise, do not process.

[0008] Step S3: If the current running speed of each joint exceeds 50% of the normal maximum allowable speed, then enter emergency stop; otherwise, enter pause.

[0009] Furthermore, the x-direction position of the irregular five-joint robot is determined by the horizontal slide of the first joint.

[0010] Furthermore, in step S1, the y and z components of the target's spatial position in the Cartesian coordinate system are represented as follows: The target angles corresponding to the second to fourth joints are obtained by solving the system of equations through inverse kinematics. , , And then according to the formula Calculate the y and z components of the spatial position of the center point at the distal end of the third joint, where, These are the lengths of the second, third, and fourth joints, respectively.

[0011] Furthermore, step S2 specifically includes the following steps:

[0012] Step S21: Determine the first minimum value based on whether the target angle of the fourth joint is greater than -90°. If the distance from the end of the third joint to the first joint is less than the first minimum value, proceed to step S22; otherwise, it is determined that no self-collision will occur.

[0013] Step S22: When the distance from the end of the third joint to the first joint is less than the y component of the spatial position of the center point of the end of the third joint, it is determined that no self-collision will occur; otherwise, proceed to step S23.

[0014] Step S23: If the target angle of the fourth joint is not greater than -90°, it is determined that a self-collision will occur, and proceed to step S3; if the target angle of the fourth joint is greater than -90°, proceed to step S24.

[0015] Step S24: Determine whether the minimum non-collision distance is less than the y-component in the spatial position of the center point of the end of the third joint. If so, it is determined that no self-collision will occur; otherwise, it is determined that a self-collision will occur, and proceed to step S3. If the fourth joint is not equipped with a tool, the minimum non-collision distance is calculated based on the width of the first joint and the width of the fourth joint. If the fourth joint is equipped with a tool, the minimum non-collision distance is calculated based on the width of the first joint and the length of the tool. If the end of the fourth joint is a rotatable flange and a tool is installed, the minimum non-collision distance is calculated based on the width of the first joint, the length of the tool, and the angle of the flange.

[0016] Furthermore, in step S21, the first minimum value dis min Represented as ,in, Let be the radius of the collision detection sphere for the third joint, with the end of the third joint as its center. r1 is the target angle corresponding to the fourth joint, r3 is the width of the first joint, r4 is the width of the third joint, and l4 is the length of the fourth joint.

[0017] Furthermore, in step S24, if the fourth joint is not equipped with a tool, the minimum collision-free distance is... If a tool is added to the fourth joint, the minimum non-collision distance is [value missing]. If the end of the fourth joint is a rotatable flange and tools are installed, the minimum non-collision distance is [value missing]. Where r4 is the width of the third joint, r tFor the length of the tool, For flange angle.

[0018] Furthermore, in step S3, when it is determined that a self-collision will occur, the register point R100.0 in the PLC is triggered. The PLC reads the R100.0 signal to trigger an emergency stop or pause and issue an alarm. The emergency stop is achieved by engaging the brakes of all joint motors.

[0019] As can be seen from the above description of the present invention, compared with the prior art, the present invention has the following beneficial effects:

[0020] This invention first calculates the target angles for the second to fourth joints to achieve the required pose based on the spatial position of the target being processed. Then, it calculates the spatial position of the center point of the end effector of the third joint based on the target angles of the second and third joints. Next, based on the target angle of the fourth joint, the spatial position of the center point of the end effector of the third joint, and the tool, it determines whether a self-collision will occur. If a collision is determined to occur, it performs an emergency stop or pause based on the current movement speed of each joint. Through a simple control method, this invention effectively reduces the probability of self-damage to the robot within the effective range of joint soft limits, improves the robot's movement safety, and reduces maintenance costs. Attached Figure Description

[0021] The present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments.

[0022] Figure 1 This is a flowchart of the present invention.

[0023] Figure 2 This is a structural schematic diagram of the irregular five-joint robot of the present invention.

[0024] Figure 3 This is a simplified diagram of the five-joint robot of the present invention when each joint is in the zero position.

[0025] Figure 4 This is a PLC program diagram for collision protection in this invention.

[0026] Among them, 1. First joint; 2. Second joint; 3. Third joint; 4. Fourth joint; 5. Tool. Detailed Implementation

[0027] The present invention will be further described below through specific embodiments.

[0028] like Figure 1 As shown, the self-collision prevention method for the heterogeneous five-joint robot includes the following steps:

[0029] Step S1: Based on the spatial position of the target to be processed, calculate the target angles from the second joint 2 to the fourth joint 4 to achieve the required pose for processing, and calculate the spatial position of the center point of the end of the third joint 3 based on the target angles of the second joint 2 and the third joint 3.

[0030] like Figure 2 As shown, the irregular five-joint robot includes a first joint 1, a second joint 2, a third joint 3, a fourth joint 4 connected sequentially from bottom to top, and a tool 5 installed at the end of the fourth joint 4. There is a soft limit to the motor movement between the first joint 1 and the second joint 2, so the second joint 2 and the first joint 1 will not collide. Therefore, the third joint 3 is used as the starting joint for collision analysis. The specific setting of the soft limit is in the prior art.

[0031] The lengths of the second joint 2, the third joint 3, and the fourth joint 4 of the alien five-joint robot are as follows: The widths of the first joint 1, the second joint 2, the third joint 3, and the fourth joint 4 are respectively Given that the spatial position (x, y, z) of the target being processed in the Cartesian coordinate system is represented by the y and z components as follows: By solving the system of equations through inverse kinematics, the target angles corresponding to the second joint 2 to the fourth joint 4 can be obtained. , , And then according to the formula Calculate the spatial position of the center point at the end of the third joint (3). y and z components and Among them, the process of solving the equations through inverse kinematics is the existing technology.

[0032] Step S2: Based on the target angle of the fourth joint 4, the spatial position of the center point of the end of the third joint 3, and the tool 5, determine whether self-collision will occur. If so, proceed to step S3; otherwise, do not process.

[0033] Specifically, the steps include the following:

[0034] Step S21: Determine the first minimum value based on whether the target angle of the fourth joint 4 is greater than -90°. If the distance from the end of the third joint 3 to the first joint 1 is less than the first minimum value, proceed to step S22; otherwise, it is determined that no self-collision will occur.

[0035] like Figure 3 As shown, each joint is in the zero position, with Determine the orientation of the fourth joint 4 for the dividing line, that is At that time, the fourth joint (4) is facing upwards. At that time, the fourth joint 4 faces downwards, thus determining the radius of the collision detection sphere of the third joint 3. ,Right now Therefore, the first minimum value is determined. When the distance from the end of the third joint 3 to the first joint 1 The system is determined to be non-collision-prone, with the collision detection sphere centered at the end of the third joint. Figure 3 The yellow circle in the middle represents this.

[0036] Step S22: When the distance from the end of the third joint 3 to the first joint 1 is less than the y-component of the spatial position of the center point of the end of the third joint 3. If the condition is not met, it is determined that no self-collision will occur; otherwise, proceed to step S23.

[0037] The x-direction position of the irregular five-joint robot is determined by the horizontal slide of the first joint 1. Therefore, when considering self-collision, only the yz plane needs to be considered. Since the z-plane is the height plane, only the distance in the y direction needs to be considered, that is, the y component of the spatial position of the center point of the end of the third joint 3. .when If it does not cause a self-collision, it is determined that it will not cause a self-collision. Then, further judgment must be made based on the target angle of the fourth joint 4.

[0038] Step S23: If the target angle of the fourth joint 4 is not greater than -90°, it is determined that a self-collision will occur, and proceed to step S3; if the target angle of the fourth joint 4 is greater than -90°, proceed to step S24.

[0039] Step S24: Determine whether the minimum non-collision distance is less than the y-component in the spatial position of the center point at the end of the third joint 3. If so, it is determined that no self-collision will occur; otherwise, it is determined that a self-collision will occur, and proceed to step S3. If the fourth joint 4 is not equipped with tool 5, the minimum non-collision distance is calculated based on the width of the first joint 1 and the width of the fourth joint 4. If the fourth joint 4 is equipped with tool 5, the minimum non-collision distance is calculated based on the width of the first joint 1 and the length of tool 5. If the end of the fourth joint 4 is a rotatable flange and tool 5 is installed, the minimum non-collision distance is calculated based on the width of the first joint 1, the length of tool 5, and the angle of the flange.

[0040] Specifically, the calculation of the minimum no-collision distance includes: if the fourth joint 4 is not equipped with tool 5, then the minimum no-collision distance is... If tool 5 is installed on the fourth joint 4, the minimum non-collision distance is [value missing]. If the end of the fourth joint 4 is a rotatable flange and tool 5 is installed, then the minimum non-collision distance is [value missing]. Where r4 is the width of the third joint 3, r t The length of tool 5 For flange angle.

[0041] Step S3: If the current running speed of each joint exceeds 50% of the normal maximum allowable speed, then enter emergency stop; otherwise, enter pause.

[0042] When the operating speed exceeds 50% of the normal maximum allowable speed, it indicates that the joint is operating too fast, and the robot needs to be stopped immediately. The normal maximum allowable speed is a threshold set according to the specific robot.

[0043] like Figure 4 As shown, when it is determined that the robot will collide with itself, a collision signal is sent by triggering register point R100.0 in the PLC. The PLC reads the R100.0 signal to trigger an emergency stop or pause and issue an alarm, so as to preserve the fault scene to the greatest extent possible, so that the user can check the position and formulate process adjustment strategies. The emergency stop is achieved by braking all joint motors.

[0044] In this invention, the terms "first," "second," and "third," etc., are used only to distinguish similar objects and are not necessarily used to describe a specific order or sequence, nor should they be construed as indicating or implying relative importance. The use of terms such as "upper," "lower," "left," "right," "front," and "rear" to indicate orientation or positional relationships is based on the orientation or positional relationships shown in the accompanying drawings and is only for the convenience of describing the invention, not to indicate or imply that the device referred to must have a specific orientation, or be constructed and operated in a specific orientation. Therefore, it should not be construed as a limitation on the scope of protection of this invention. Those skilled in the art can understand the specific meaning of the above terms in this application according to the specific circumstances.

[0045] Furthermore, in the description of this application, unless otherwise stated, "multiple" means two or more. "And / or" describes the relationship between related objects, indicating that three relationships can exist. For example, A and / or B can represent: A alone, A and B simultaneously, or B alone. The character " / " generally indicates that the preceding and following related objects have an "or" relationship.

[0046] The above are merely specific embodiments of the present invention, but the design concept of the present invention is not limited thereto. Any non-substantial modifications made to the present invention using this concept shall be considered as infringing upon the protection scope of the present invention.

Claims

1. A method for preventing self-collision of a special-shaped five-joint robot, the special-shaped five-joint robot comprising first to fourth joints connected in sequence from bottom to top, and a tool mounted at the end of the fourth joint, characterized in that: Includes the following steps: Step S1: Based on the spatial position of the target to be processed, calculate the target angles from the second to the fourth joints to achieve the required pose for processing, and calculate the spatial position of the center point of the end of the third joint based on the target angles of the second and third joints. Step S2: Based on the target angle of the fourth joint, the spatial position of the center point of the end of the third joint, and the tool, determine whether it will cause self-collision. If so, proceed to step S3; otherwise, do not process. Step S3: If the current running speed of each joint exceeds 50% of the normal maximum allowable speed, then enter emergency stop; otherwise, enter pause. The x-direction position of the irregular five-joint robot is determined by the horizontal slide of the first joint; In step S1, the y and z components of the target's spatial position in the Cartesian coordinate system are represented as follows: The target angles corresponding to the second to fourth joints are obtained by solving the system of equations through inverse kinematics. , , And then according to the formula Calculate the y and z components of the spatial position of the center point at the distal end of the third joint, where, These are the lengths of the second, third, and fourth joints, respectively. Step S2 specifically includes the following steps: Step S21: Determine the first minimum value based on whether the target angle of the fourth joint is greater than -90°. If the distance from the end of the third joint to the first joint is less than the first minimum value, proceed to step S22; otherwise, it is determined that no self-collision will occur. Step S22: When the distance from the end of the third joint to the first joint is less than the y component of the spatial position of the center point of the end of the third joint, it is determined that no self-collision will occur; otherwise, proceed to step S23. Step S23: If the target angle of the fourth joint is not greater than -90°, it is determined that a self-collision will occur, and proceed to step S3; if the target angle of the fourth joint is greater than -90°, proceed to step S24. Step S24: Determine whether the minimum non-collision distance is less than the y-component in the spatial position of the center point of the end of the third joint. If so, it is determined that no self-collision will occur; otherwise, it is determined that a self-collision will occur, and proceed to step S3. If the fourth joint is not equipped with a tool, the minimum non-collision distance is calculated based on the width of the first joint and the width of the fourth joint. If the fourth joint is equipped with a tool, the minimum non-collision distance is calculated based on the width of the first joint and the length of the tool. If the end of the fourth joint is a rotatable flange and a tool is installed, the minimum non-collision distance is calculated based on the width of the first joint, the length of the tool, and the angle of the flange.

2. The self-collision avoidance method for a special-shaped five-joint robot according to claim 1, characterized in that: In step S21, the first minimum value dis min Represented as ,in, Let be the radius of the collision detection sphere for the third joint, with the end of the third joint as its center. r1 is the target angle corresponding to the fourth joint, r3 is the width of the first joint, r4 is the width of the third joint, and l4 is the length of the fourth joint.

3. The self-collision avoidance method for a special-shaped five-joint robot according to claim 2, characterized in that: In step S24, if the fourth joint is not equipped with a tool, the minimum collision-free distance is... If a tool is added to the fourth joint, the minimum non-collision distance is [value missing]. If the end of the fourth joint is a rotatable flange and tools are installed, the minimum non-collision distance is [value missing]. Where r4 is the width of the third joint, r t For the length of the tool, For flange angle.

4. The self-collision avoidance method for a special-shaped five-joint robot according to claim 1 or 2 or 3, characterized in that: In step S3, when it is determined that a self-collision will occur, the register point R100.0 in the PLC is triggered. The PLC reads the R100.0 signal to trigger an emergency stop or pause and issue an alarm. The emergency stop is achieved by engaging the brakes of all joint motors.

Images