TWI766807B - Safe reset method of robotic arm - Google Patents

Abstract

A safe reset method of a robotic arm, which is applied to a motor-driven robotic arm, the end of which can be driven to move along a first path, wherein there is a position point on the first path, and the end of the robotic arm can be moved has an offset point in the range that is not on the first path. The reset method includes: defining a second path between the offset point and the position point; calculating the return speed of the end of the robot arm moving from the offset point to the position point along the second path, and calculating the return speed according to the return speed Torque; determine whether the return torque is greater than a torque limit, if so, make the end of the robot arm return to the position point at the speed limit corresponding to the torque limit; otherwise, make the end of the robot arm return to the position point at the return speed.

Description

Safe reset method of robotic arm

The present invention is related to robots, in particular to a method for resetting a mechanical arm after being collided.

Figure 6 illustrates a traditional closed-loop control system for robotic arm control. Conventionally, when the closed-loop control system receives a position command and needs to move the servo motor 91 to a target position, the closed-loop control system will, according to the position command and a current actual position fed back from the servo motor 91, pass the position command. After the calculation and position compensation of the control loop 92, the required speed command is generated to the speed control loop 93 of the drive. Then the speed control loop 93 generates a torque command according to the speed command, the current actual speed fed back from the servo motor 91 and the required speed compensation, and the driver controls the current of the motor according to the torque command, so that the servo motor 91 can The corresponding output torque is output, so that the robotic arm can quickly move to the target position.

However, when the robot arm detects an abnormal situation, for example, the robot arm collides with a foreign object during movement, for safety, the servo motor 91 will stop feeding back values such as the current actual position, and the staff may move the robot arm away to eliminate the foreign body. At this time, the position of the robot arm is quite far away from the original motion path. When the abnormal state is eliminated and the servo motor 91 starts to feed back the current actual position and other values to the drive, it may position) is too far away from the original motion path, so that the driver needs to output a large motor current so that the servo motor 91 can quickly rotate to the target position. Excessive motor current may trigger the safety mechanism of the servo motor 91 and cause the system to shut down, or even damage the servo motor 91 . Therefore, in the case of abnormality of the robot arm, a better safety recovery mechanism of the robot arm is quite required.

The main purpose of the present invention is to improve the lack of the control mechanism of the existing robot arm, and further propose a new safety recovery mechanism of the robot arm, which can also be used when the robot arm is moved to a relatively remote position due to abnormal conditions. Can be reset safely.

In order to achieve the aforementioned purpose, the present invention provides a method for safe resetting of a robotic arm, which is applied to a robotic arm, the robotic arm has a motor, the motor has a driver, and the end of the robotic arm is at the lower edge of the drive of the motor. A first path movement is defined as having a position point on the first path, and an offset point not located on the first path is defined in the range within which the distal end of the robotic arm can move; the reset method includes: define a second path between the offset point and the position point; calculating a return speed of the end of the robotic arm moving from the offset point to the position point along the second path, and calculating a return torque according to the return speed; Judging whether the return torque is greater than a torque limit, if the judgment result is yes, make the end of the robot arm return to the position point along the second path at a limited speed corresponding to the torque limit; if the judgment result is no, then The end of the robot arm is made to return to the position point along the second path at the return speed.

Preferably, the second path is a straight path.

Preferably, if the return torque is greater than the torque limit, after the end of the robotic arm moves to a new offset point along the second path at the limit speed, the following steps are performed: calculating a new return speed of the end of the robotic arm moving from the new offset point to the position point along the second path, and calculating a new return torque according to the new return speed; Judging whether the new return torque is greater than the torque limit, if the judgment result is yes, make the end of the robot arm return to the position point along the second path at the limit speed corresponding to the torque limit; if the judgment result is no , then the end of the robotic arm is made to return to the position point along the second path at the new return speed.

Preferably, if the return torque is less than the torque limit, the motor drives the distal end of the robotic arm with its rated maximum output torque to return to the position along the second path at the return speed.

Preferably, the torque limit is 20% of the rated maximum output torque of the motor.

The above-mentioned objects and advantages of the present invention can be easily understood from the detailed description and accompanying drawings of the following selected embodiments.

A method for safe resetting of a robotic arm provided by the present invention is applied to a robotic arm as shown in Figures 2 and 3. In this embodiment, a multi-axis robotic arm 1 is used as an example, and each axis 11 of the robotic arm is used as an example. A servo motor 12 is provided, and each servo motor 12 has a driver 2 and can be driven to rotate, thereby allowing the robot arm 1 to change its posture. Each driver 2 is electrically connected to a control computer 3 , and the control computer 3 can preset a series of position commands to make the driver 2 drive the servo motor 12 to rotate, so that the end 13 of the robotic arm 1 is driven at the lower edge of the servo motor 12 . A first path L1 moves (see Fig. 5).

The firmware structure of the driver 2 of the servo motor 12 is shown in Figure 4. It can be seen that the driver 2 adopts a closed-loop control system. The closed-loop control system includes a position control loop 41, a speed control loop 42, and a torque switch 43. and a torque control loop 44 . When the control computer 3 generates a position command to move the end 13 of the robotic arm 1 to a target position, the position control loop 41 generates a speed command according to the target position and a current actual position fed back by the servo motor 12 . The speed control loop 42 generates a torque command according to the speed command and a current actual speed fed back by the servo motor 12. The torque switch 43 is used to limit the output torque. The operation of the torque switch 43 will be described in more detail in the following paragraphs. 's introduction.

Referring to FIG. 5, the situation shown is for the end point 13 of the robotic arm 1 to move along a first path L1 from a point A to an end point C to perform the action of drawing a line, wherein the first path L1 is also defined with a position at the starting point. The position point B between A and the end point C. Suppose that when the end 13 of the robot arm 1 moves to the position point B, one of the joints touches the foreign object and the robot arm 1 stops moving. The position point B is moved to an offset point B', wherein the offset point B' is located in the range within which the distal end 13 of the robot arm 1 can move, and the offset point B' is not on the first path L1. A second path L2 is defined between the offset point B' and the position point B; in this example, the second path L2 is the shortest path in the space from the offset point B' to the position point B, but the present invention does not Not limited to this.

The safe reset method of the mechanical arm of the present invention includes the following steps:

Step S1: The control computer 3 calculates a return speed of the end 13 of the robotic arm 1 moving from the offset point B' to the position point B along the second path L2, and calculates a return speed in accordance with the distance between the end 13 and the force arm of the servo motor 12. torque. It should be noted that, in this embodiment, in the process of moving the end 13 of the robot arm 1 from the offset point B' to the position point B, it may be necessary to drive a plurality of servo motors 12 at the same time. Calculate the corresponding return torque. In the following paragraphs, only one of the servo motors 12 is described, and the rest of the servo motors 12 can be deduced by analogy.

Step S2: The control computer 3 judges whether the return torque is greater than a torque limit. If the judgment result is yes, then go to step S3, and then return to step S1 from step S3; if the judgment result is no, go to step S4. Under normal circumstances, if the distance between the offset point B' and the position point B is too far, the required rotation angle of the servo motor 12 within a predetermined time (for example, 1 millisecond) is too large, which may cause the servo motor 12 to rotate. The return speed and return torque are too large, so in this embodiment, the driver 2 of the servo motor 12 will reduce the output current of the driver 2 to a safe value by controlling the torque switch 43, so that the servo motor 12 can operate at a lower The output torque (for example, 20% of the rated maximum output torque of the servo motor 12) is rotated.

Step S3: The driver 2 drives the servo motor 12, so that the distal end 13 of the robotic arm 1 moves along the second path L2 to another offset point in the second path L2 at a limit speed corresponding to the above torque limit within a predetermined time. B", the control computer 3 will iterate this offset point B" as the offset point B' in step S1. After returning to step S1, the control computer 3 will calculate a new return speed from the new offset point B" to the position point B, and obtain a corresponding new return torque. When returning to step S2, the original step S2 The "return torque" in is iterated as "new return torque", and it is judged whether the "new return torque" is greater than the torque limit. If the judgment result is yes, then repeat step S3 and step S1; Step S4.

Step S4: Make the end 13 of the robot arm 1 return to the position point B along the second path L2 at the return speed or the new return speed. It should be noted that, at this time, the torque limit of the torque switch 43 is set at the rated maximum output torque of the servo motor 12, and the servo motor 12 drives the end 13 of the robot arm 1 to return to the position B at the return speed, unless the robot arm 1 is at the return speed. When an abnormal collision occurs again during the movement, the torque limit of the torque switch 43 is set to 20% of the rated maximum output torque of the servo motor 12.

Step S5: When the end 13 of the robotic arm 1 returns to the position point B, according to the original preset position command, the end 13 is moved along the first path L1 from the position point B to the end point C. The torque limit is also the rated maximum output torque of the servo motor 12 .

Through the safe reset method of the robotic arm of the present invention, when the robotic arm 1 is moved away from the first path L1 due to an accidental stop, the torque limit of the torque switch 43 can be controlled on the firmware, so that the robotic arm 1 can be slowly controlled. Returning to the first path L1 along the second path L2 prevents the servo motor 12 from returning with too much return torque and return speed, thereby effectively reducing the possibility of the servo motor 12 triggering a safety mechanism or even being damaged.

However, the disclosure of the above embodiments is only used to illustrate the present invention, not to limit the present invention, and the replacement of equivalent elements should still belong to the scope of the present invention.

To sum up, those skilled in the art can understand that the present invention can achieve the above-mentioned purpose, and it complies with the provisions of the Patent Law.

1: Robotic arm

11: Shaft

12: Servo motor

13: End

2: Drive

3: Control the computer

41: Position control loop

42: Speed control loop

43: Torque switch

44: Torque control loop

A: starting point

B: location point

B', B": offset point

C: end point

L1: first path

L2: Second path

91: Servo motor

92: Position control loop

93: Speed control loop

Figure 1 is a flow chart of the method of the present invention; Figure 2 is a schematic diagram of a robotic arm in the present invention; Figure 3 is a schematic block diagram of the robotic arm and its control computer and driver in the present invention; Fig. 4 is a block schematic diagram of the robot arm driver in the present invention; Fig. 5 is a schematic diagram of the movement path of the robotic arm in the present invention; FIG. 6 is a block diagram of a conventional manipulator driver.

Claims (4)

A safe reset method of a robotic arm, which is applied to a robotic arm, the robotic arm has a motor, the motor has a driver, and the end of the robotic arm moves along a first path under the drive of the motor, defining the first path. There is a position point on a path, and an offset point not located on the first path is defined in the range within which the end of the robotic arm can move; the reset method includes: between the offset point and the position point Define a second path; Step S1: Calculate a return speed of the end of the robotic arm moving from the offset point to the position along the second path, and calculate a return torque according to the return speed; Step S2: Determine whether the return torque is greater than a torque limit; Step S3: If the determination result of Step S2 is yes, then make the end of the robot arm move to a new offset along the second path at a speed limit corresponding to the torque limit After the point, calculate a new return speed for the end of the robot arm to move from the new offset point to the position point along the second path, and calculate a new return torque according to the new return speed; determine the new return torque Whether it is greater than the torque limit, if the judgment result is yes, make the end of the robot arm return to the position point along the second path at the limit speed corresponding to the torque limit; if the judgment result is no, make the mechanical arm return to the position The end of the arm returns to the position along the second path at the new return speed; Step S4: if the judgment result of step S2 is negative, the end of the robot arm returns to the position along the second path at the return speed point. The method for safely resetting a robotic arm according to claim 1, wherein the second path is a straight path. The method for safely resetting a robotic arm according to claim 1, wherein if the return torque is less than the torque limit, the motor drives the end of the robotic arm with its rated maximum output torque to return along the second path at the return speed the location point. The method for safely resetting a robotic arm as claimed in claim 1, wherein the torque is limited to 20% of the rated maximum output torque of the motor.

Images