TWI742635B - Method of triggering and counteracting for teaching position and posture - Google Patents

Abstract

A method of triggering and counteracting for teaching posture and position is provided. In posture maintenance mode, an automation mechanical system computes and execute the new posture maintenance command every cycle for make a machine apparatus stay in a first posture, determines that the machine apparatus receives an external force and a direction of the external force, and switches to a force maintenance mode when there is torque difference between the new posture maintenance commands and a reference command of the first posture. In the force maintenance mode, the automation mechanical system determines and executes a power maintenance command based on a counteraction force and the current power of each motor for reducing a resistance of adjusting the machine apparatus manually toward the direction of the external force to be a second posture.

Description

Trigger and compensation method of teaching position and posture

The present invention relates to the posture teaching of related mechanical equipment, and particularly relates to the triggering and compensation method of teaching position and posture.

In the prior art, when teaching the position and posture of a mechanical device (such as a multi-axis mechanism or a robotic arm), the user usually operates a teaching box with multiple operation buttons, and the aforementioned multiple operation buttons are used to control the The multiple motors of the mechanical equipment rotate to make the mechanical equipment reach the position and posture desired by the user.

In the above teaching technology, the user must convert the desired position and posture from the space coordinate system (that is, the XYZ axis coordinate system) to the motion coordinate system of the mechanical equipment (a multi-dimensional coordinate system composed of multiple motor positions) to operate, and then smoothly Make the mechanical equipment reach the user's desired position and posture. The above teaching techniques are not only time-consuming but also not intuitive.

In order to solve the above problems, another position and posture teaching technology has been proposed. The aforementioned position and posture teaching technology is that the user first operates the mechanical equipment to enter the torque mode, and then manually adjusts the posture of the mechanical equipment, so as to intuitively make the mechanical equipment reach The desired position and posture.

However, every time the user wants to teach the position and posture, he must first manually operate the mechanical equipment to switch to the torque mode, which is quite inconvenient.

In addition, in the torque mode, each motor of the mechanical equipment outputs a fixed torque to maintain a fixed posture without additional external force. When the user adjusts the posture of the mechanical equipment manually, the friction and friction of the mechanical equipment must be overcome. The aforementioned torque can change the posture of the mechanical equipment, which is quite laborious.

Therefore, the existing posture teaching technology has the above-mentioned problems, and a more effective solution is urgently needed.

The main purpose of the present invention is to provide a method for triggering and compensating the teaching position and posture, which can automatically detect whether the user starts to perform posture teaching.

To achieve the above objective, the present invention provides a method for triggering and compensating teaching position and posture, which is used in an automated mechanical system including a controller, a motor driver, and mechanical equipment. The mechanical equipment includes a plurality of motors for changing positions and postures. The method for triggering and compensating the teaching position and posture includes the following steps: In the position and posture maintenance mode, the controller calculates and executes a new position and posture maintenance command every other cycle to maintain the mechanical device in the first position and posture, wherein The position and attitude maintenance commands include the motor control commands of each motor; when there is a difference between the output torque of the new attitude maintenance command and the output torque of a reference attitude command corresponding to the first attitude, it is determined that the mechanical device accepts a The external force determines the direction of the external force and switches to a power maintenance mode; in the power maintenance mode, determines the current power of each motor of the mechanical equipment; determines the compensation force according to the direction of the external force; and, according to the compensation force and the current power of each motor Calculate the power maintenance command and execute the power maintenance command via the motor driver Adjusting the mechanical equipment in the direction of the external force to reduce the resistance of the second posture, wherein the power maintenance command includes the motor control command of each motor.

The invention allows users to teach the posture of mechanical equipment in a convenient and labor-saving way.

1: Mechanical equipment

11: Motor

12: Transmission mechanism

2: Motor driver

3: Controller

31: Mechanism gravity compensation module

32: Friction compensation module

33: Security Module

40: Teaching status

400: Attitude maintenance mode

401: Power Maintenance Mode

41: working status

410: Attitude Maintenance Mode

D1: Turn to information

F1, F2, F2’, F3: Force

M1, M2: Moment

P1: Location information

V1: Speed information

S10-S18: The first trigger and compensation step

S20-S22: Posture maintenance steps

S30-S32: Compensation steps

S400-S410: Friction compensation steps

S500-S515: Second trigger and compensation step

FIG. 1 is a structural diagram of an automated mechanical system according to an embodiment of the present invention.

Fig. 2 is a schematic diagram of the teaching state and the working state of an embodiment of the present invention.

FIG. 3 is a schematic diagram of calculating compensation force according to an embodiment of the present invention.

4A is a first schematic diagram of posture teaching of an embodiment of the present invention.

4B is a second schematic diagram of posture teaching of an embodiment of the present invention.

4C is a third schematic diagram of posture teaching of an embodiment of the present invention.

Fig. 5 is a flowchart of a method for triggering and compensating teaching position and posture according to the first embodiment of the present invention.

Fig. 6 is a partial flowchart of a method for triggering and compensating teaching position and posture according to the second embodiment of the present invention.

FIG. 7 is a partial flowchart of a method for triggering and compensating teaching position and posture according to the third embodiment of the present invention.

Fig. 8 is a partial flow chart of the triggering and compensation method of teaching position and posture according to the fourth embodiment of the present invention.

9 is a first part of a flowchart of the triggering and compensation method of teaching position and posture according to the fifth embodiment of the present invention.

FIG. 10 is the second part of the flowchart of the triggering and compensation method of teaching position and posture according to the fifth embodiment of the present invention.

With regard to a preferred embodiment of the present invention, the detailed description is given below in conjunction with the drawings.

First, please refer to FIG. 1, which is an architecture diagram of an automated mechanical system of an embodiment of the present invention. The present invention discloses a method for triggering and compensating teaching position and posture (hereinafter referred to as the method for short), and the method is mainly applied to the automated mechanical system as shown in FIG. 1 and FIG. 2.

As shown in Figure 1, the automated mechanical system includes a mechanical device 1, a motor driver 2, and a controller 3.

The mechanical device 1 (such as a robotic arm or other multi-axis moving device) includes a plurality of motors 11 for changing positions and attitudes, and a plurality of transmission mechanisms 12 (such as mechanical limbs) connected to the plurality of motors 11 respectively. Each motor 11 is respectively connected to the motor driver 2 to receive the control of the motor driver 2 to rotate, and drive the corresponding transmission mechanism 12 to move.

The motor driver 2 is used to control each motor 11 and can obtain position information of each axis motor 11 according to the state of each motor 11 and feed it back to the controller 3.

The controller 3 is used to calculate the motor control commands of each motor, and execute the motor control commands via the motor driver 2.

In one embodiment, the controller 3 determines the rotation direction and rotation speed of each motor 11 based on the position information of each motor 11, thereby calculating the torque required to move or maintain the posture (or read from the torque sensor installed in each motor 11). Take the torque value) and output to the motor driver 2. The motor driver 2 controls each motor 11 corresponding to the torque value to move the mechanical equipment 1 or maintain the posture.

In one embodiment, when the mechanical device 1 receives the user's posture instruction, the controller 3 can control the motors 11 through the motor driver 2 to provide compensating force in time, so as to reduce the resistance of the user to pull the mechanical device 1.

Specifically, the controller 3 includes a gravity compensation module 31 for compensating gravity, a friction compensation module 32 for compensating friction, and a safety module 33 for avoiding excessive compensation. The detailed operation mode will be described later. Detailed.

Please refer to FIG. 2 together. FIG. 2 is a schematic diagram of the teaching state and the working state of an embodiment of the present invention. The automated mechanical system of the present invention can operate between two states, namely the teaching state 40 and the working state 41.

When the automated mechanical system is in the teaching state 40, the user can adjust the posture of the mechanical device 1, and the automated mechanical system can record the adjusted posture. The teaching state 40 includes two modes, the posture maintenance mode 400 and the power maintenance mode 401.

Specifically, when the automated mechanical system is in the posture maintenance mode 400, it continuously controls the mechanical device 1 to assume a specified posture, that is, the motors 11 will continue to maintain the same position or the same moving speed, and when the mechanical device 1 suffers When an external force is applied, each motor is controlled to increase or decrease the output torque, so that the mechanical device 1 can maintain a specified posture or maintain the same moving speed.

When the automated mechanical system is in the power maintenance mode 401, it continuously controls the motors 11 to output the same power (such as the same torque), that is, when no external force is encountered, the mechanical device 1 can maintain the corresponding posture; suffer enough to overcome the resistance (such as friction) Or output power), the mechanical device 1 will move in the direction of the external force to assume a new posture. In this way, the user can adjust the posture of the mechanical device 1.

Furthermore, the automated mechanical system of the present invention is set to switch directly to the posture maintenance mode 400 after entering the teaching state 40 to wait for the user to start posture teaching. When an external force is detected (for example, the user intends to move the mechanical device 1), it automatically switches to the power maintenance mode 401 so that the user can adjust the posture of the mechanical device 1.

Furthermore, in the power maintaining mode 401, the automated mechanical system can switch back to the posture maintaining mode 400 to maintain the specified posture (such as adjusting Posture) or the specified movement speed.

When the automated mechanical system is in working state 41 (such as through automatic detection or manual operation by the user), the automated mechanical system can enter the posture maintenance mode 400 and control the mechanical equipment 1 to assume a specified posture according to the control command or schedule set by the user , Continuously assume different postures (such as the posture learned in the teaching state) or change the position and/or posture at a specified speed to achieve automatic control (such as automatic assembly, automatic detection or automatic picking, etc.).

The posture maintenance mode 400 of the teaching state 40 and the posture maintenance mode 410 of the working state 41 may be the same or similar operation modes, but are not limited.

In one embodiment, the speed of the automated mechanical system moving the mechanical device 1 in the posture maintenance mode 400 of the teaching state 40 (ie, the posture transformation speed) is less than the speed of moving the mechanical device 1 in the posture maintenance mode 400 of the working state 41.

Specifically, the teaching state 40 is usually assisted by the user. When performing a posture change, in order to avoid damage caused by accidentally colliding with the user or surrounding objects at an excessively fast posture change speed, the posture in the posture maintenance mode 400 is deliberately reduced. Change speed. The working state 41 is usually when the mechanical device 1 operates independently, and there are no users or irrelevant objects around, the attitude maintaining mode 410 can be operated at a faster attitude change speed to improve the efficiency of automated processing.

In one embodiment, the controller 3 may include a storage module (not shown in the figure), and the aforementioned storage module may include a non-transitory computer-readable medium and store a computer program (such as a control program for the mechanical device 1). The computer program is recorded with a computer readable program code. The controller 3 can execute a computer program to control the motors 11 via the motor driver 2 to implement the steps of the method of the embodiments of the present invention.

Please also refer to FIG. 5, which is a flowchart of the triggering and compensation method of teaching position and posture according to the first embodiment of the present invention. The method mainly includes the following steps.

Step S10: the controller 3 enters the posture maintenance mode 400.

Step S11: In the attitude maintenance mode 400, the controller 3 calculates a new attitude maintenance command every other period (such as 0.1 millisecond, 0.5 millisecond, 1 second, or 5 seconds), and executes the new attitude maintenance command.

In an embodiment, the posture maintenance command includes a motor control command of each motor 11. The controller 3 obtains the current posture (which may include the current position) every other cycle, and compares whether the current posture is with the specified posture (the first posture). The position of the system or the position of the robot coordinate system) match. If the postures match, the controller 3 can continue to output posture maintenance commands with the same parameters, even if each motor 11 maintains the current power output (such as issuing the same motor control commands); if the postures (or positions) do not match, the controller 3 can Calculate the difference between the current posture and the specified posture and/or the difference between the current position and the specified position, and calculate a new posture maintenance command based on this difference (such as recalculating the moving direction, moving distance and/or output of each motor 11) The torque is used as a new motor control command, so that the mechanical device 1 can assume a specified posture after each motor 11 moves) and/or move to a specified position, and send the new posture maintenance command to the motor driver 2 to respond to each motor 11 The motors 11 are controlled and adjusted one by one, so that the current posture of the mechanical equipment is corrected to the designated posture. In this way, the mechanical device 1 can adapt to the applied external force and continuously maintain the designated posture (first posture).

In other words, the aforementioned new posture maintenance command is used to control each motor 11 to change the rotation mode (such as changing the output torque) to offset the external force received by the mechanical device 1.

Step S12: The controller 3 compares the new posture maintenance command (such as the latest posture maintenance command executed in the latest cycle) with the reference posture command used to assume the first posture (such as the output torque provided by the posture maintenance command and the reference The output torque provided by the posture command is compared) to detect whether the posture maintenance command has changed (such as whether the output torque changes), that is, to determine whether the mechanical device 1 is subjected to external force (that is, the user applies force to the mechanical device 1 for posture teaching ), so that the controller 3 generates a new posture maintenance command to offset the external force in order to maintain the mechanical device 1 in the specified posture.

The aforementioned reference posture command is used to control the rotation mode of each motor 11 so that the mechanical device 1 assumes a specified posture (such as the first posture) without external force.

Step S13: The controller 3 judges whether there is a difference between the new posture maintaining command and the reference posture command (for example, there is a difference between the output torque provided by the new posture maintaining command and the output torque provided by the reference posture command).

If the controller 3 determines that there is a difference between the new posture maintaining command and the reference posture command, it can determine the direction of the external force and execute step S14. If the controller 3 determines that there is no difference between the new posture maintenance command and the reference posture command, it means that the mechanical device 1 is not subjected to external force, and skips to step S11 for execution.

In one embodiment, each posture maintaining command is used to provide a set of output torques. The controller 3 judges the difference between the new output torque corresponding to the new attitude maintenance command and the output torque of the reference attitude command when the torque difference between the new attitude maintenance command and the reference attitude command meets the preset torque threshold. There are differences, and the direction of the external force is determined according to the torque change.

In this way, the present invention can effectively detect whether the user intends to perform gesture teaching through the change of the gesture command, and no additional sensing device is required.

Step S14: When it is determined that the user intends to perform posture teaching, the controller 3 switches to the power maintenance mode.

Step S15: The controller 3 determines the current power of each motor of the mechanical device 1, such as the current torque, rotation speed and/or rotation direction.

Step S16: The controller 3 determines the compensation force according to the direction of the external force. In one embodiment, the direction of the compensation force is the same as the external force, that is, the compensation force is used to offset the resistance of the mobile mechanical device 1, but it is not limited to this.

Step S17: The controller 3 calculates a group of power maintenance commands according to the compensation force and the current power of each motor, and executes the calculated power maintenance commands via the motor driver 2. The aforementioned power maintenance order Including the motor control commands of each motor 11, when each motor 11 operates according to the corresponding motor control command, the resistance to adjust the mechanical device to another posture (the second posture, that is, the posture desired by the user) in the direction of the external force can be reduced.

For example, the magnitude of the compensation force may be equal to (or close to) the magnitude of the friction force (such as the maximum static friction force or the dynamic friction force) of the mechanical device 1, but the direction is opposite (that is, the same direction as the external force). Since the frictional force is compensated, the user only needs to apply a small amount of external force (for example, an external force smaller than the frictional force) to push the mechanical device 1 in a desired direction.

In another example, the magnitude of the compensation force is slightly larger than the magnitude of the frictional force of the mechanical device 1 and is in the same direction as the external force, so that the mechanical device 1 will slowly move in the direction of the previously applied external force when no external force is applied. . In this way, the user can more easily move the mechanical device 1 in a desired direction. It is worth mentioning that, in this situation, when the mechanical device 1 moves to the position desired by the user (ie assumes the posture desired by the user), the user only needs to apply a little resistance to stop the mechanical device 1 from moving and maintain The user's desired posture.

Step S18: The mechanical device 1 accepts the user's posture instruction operation, and assumes the posture desired by the user. Then, the controller 3 records the final posture (second posture), (for example, records the position of each motor 11).

The present invention allows users to trigger posture teaching in a convenient manner, and teach the posture of mechanical equipment in a labor-saving manner, and no additional sensors are required.

Please refer to FIGS. 4A to 4C together. FIG. 4A is the first schematic diagram of the posture teaching of an embodiment of the present invention, FIG. 4B is the second schematic diagram of the posture teaching of an embodiment of the present invention, and FIG. 4C is the present invention A third schematic diagram of posture teaching in an implementation aspect. 4A to 4C are used to illustrate how the present invention triggers the posture teaching and how to perform force compensation during the posture teaching process.

As shown in FIG. 4A, the automated mechanical system switches to the posture maintenance mode 400 after entering the teaching state 40, and maintains the first posture (such as executing the posture reference command of the first posture) and is in a static state. When the user wants to perform posture teaching, the external force F1 can be directly applied to the transmission mechanism 12 of the mechanical device 1 to change the first posture of the mechanical device 1. After receiving the external force, the controller 3 calculates a new attitude maintaining command based on the external force F1, and controls multiple motors 11 through the motor driver 2 to implement the new attitude maintaining command (that is, the torque M1 is added to offset the external force F1) to maintain the first One gesture.

Then, as shown in FIG. 4B, when the controller 3 detects that the attitude reference command of the first attitude is different from the new attitude maintenance command (that is, the moment M1 is added), it determines that the user has the intention of attitude teaching and switches to power The mode 401 is maintained, and the direction of the external force F1 is calculated based on the moment M1.

In the power maintenance mode 401, the controller 3 determines the compensation torque M2 (for example, the maximum static friction force) according to the direction of the external force F1 and generates a corresponding power maintenance command. After the power maintenance command is executed through the motor driver 2, since the motor 11 will provide the compensation torque M2 in the direction of the external force F1, the user can move the transmission mechanism 12 with less effort to make the mechanical device 1 assume the desired second posture (as shown in FIG. 4C). Show).

In this way, the present invention allows the user to trigger the posture teaching conveniently and perform posture teaching with less effort.

Please refer to FIG. 5 and FIG. 6 together. FIG. 6 is a partial flowchart of the triggering and compensation method of teaching position and posture according to the second embodiment of the present invention. In this embodiment, each posture is recorded based on the current position of each motor 11 when the mechanical device 11 assumes this posture, that is, the aforementioned current posture includes the current position of each motor 11, and the aforementioned first posture includes the current position of each motor 11 The first position and the aforementioned second posture include the second position of each motor 11.

Compared with the method shown in FIG. 5, step S11 of the method of this embodiment further includes the following steps.

Step S20: In the posture maintaining mode 400, the controller 3 obtains the current posture of the mechanical device every other period.

Step S21: The controller 3 calculates a new posture maintenance command according to the difference between the current posture and the first posture.

In one embodiment, the controller 3 can calculate the difference between the current position of each motor 11 and the first position, and generate each motor 11 to move back from the current position to the first position based on the calculated position difference. The motor control command is used as a new posture maintenance command.

Step S22: The controller 3 controls the rotation of each motor 11 via the motor driver 2 according to each motor control command of the new posture maintaining command so that the mechanical device becomes the first posture.

In one embodiment, each of the aforementioned motor control commands includes a rotation speed command and a rotation direction command.

Thereby, by repeating steps S20-S22 continuously, the present invention can adapt the external force to maintain the designated first posture by the mechanical device 1 in the posture maintaining mode. In addition, the output of the posture maintenance command changed by adapting to the external force can be used to detect whether the user intends (that is, to detect whether the user starts posture teaching).

Please refer to FIG. 5 and FIG. 7 together. FIG. 7 is a partial flowchart of the triggering and compensation method of teaching position and posture according to the third embodiment of the present invention. Compared with the method of FIG. 5, step S16 of the method of this embodiment includes the following steps.

Step S30: The controller 3 calculates the friction force compensation force. The aforementioned friction force compensation force is used to offset or reduce the friction force generated by the mechanical device 1 teaching movement in the direction of the external force.

In one embodiment, if the mechanical equipment 1 is in a moving state, the magnitude of the friction force compensation force can be close to or equal to the magnitude of the dynamic friction force, but in the opposite direction; if the mechanical equipment 1 is stationary, the magnitude of the friction force compensation force The value can be close to or equal to the magnitude of the maximum static friction, but in the opposite direction.

Step S31: The controller 3 calculates the gravity compensation force according to the current power of each motor, and the aforementioned gravity compensation force is used to offset or reduce the gravity of the mechanical equipment.

Specifically, since the transmission mechanism 12 has a weight, it will collapse or change the posture due to gravity during the posture teaching process, and the user must overcome the gravity of the transmission mechanism 12 to move the mechanical device 1. Therefore, the present invention compensates for the gravity of the mechanical device 1, which can further save the user's required force for posture teaching.

Step S32: The controller 3 determines the final compensation force according to the aforementioned friction compensation force and the aforementioned gravity compensation force.

In an embodiment, the aforementioned frictional force compensation force and the aforementioned gravity compensation force are moments, and the controller 3 calculates the resultant moment of the aforementioned frictional force compensation force and the aforementioned gravity compensation force as the final compensation force.

Please also refer to FIG. 3, which is a schematic diagram of calculating the compensation force according to an embodiment of the present invention. Figure 3 is used to illustrate how the invention calculates the compensation force.

As shown in the figure, the motor driver 2 obtains the torque F1 of the external force received in each communication cycle, and feeds the torque F1 back to the mechanism gravity compensation module 31 and the friction compensation module 32.

Specifically, the controller 3 can calculate the torque F1 from the position information P1, the steering information D1, and/or the rotational speed information V1 of each motor 11, feed back the position information P1 to the mechanism gravity compensation module 31, and combine the steering information D1 with the rotational speed The information V1 is fed back to the friction torque compensation module 32.

The mechanism gravity compensation module 31 can calculate the gravity compensation torque F3 required by each motor 11 to bear the gravity of the mechanical device 1 in the current posture according to the position information P1 of each communication cycle. The friction torque compensation module 32 can calculate the friction compensation torque F2 required by each motor 11 to eliminate the aforementioned friction force according to the steering information D1 and the rotational speed information V1 in each communication cycle.

In addition, in order to avoid excessive friction force compensation torque F2 causing the mechanical device 1 to move too fast, the friction force compensation torque F2 will be fed back to the safety module 33 for adjustment and generate adjusted friction force compensation torque F2'.

Then, the controller 3 integrates the gravity compensation torque F1 and the friction compensation torque F2', calculates and outputs the final control torque C1 to the motor driver 2, and the motor driver 2 controls each motor 11 according to the control torque C1. Since the control torque C1 includes the gravity compensation torque F3, when each motor 11 rotates according to the control torque C1, sufficient supporting force can be provided to keep the mechanical device 1 stable in the power maintenance mode of the motor driver 2 without It collapsed because of insufficient support. Furthermore, the control torque C1 also includes the adjusted friction compensation torque F2’. When the user directly applies force to the mechanical device 1 When each motor 11 rotates according to the control torque C1, each motor 11 can eliminate the friction force generated by the adjusted friction force compensation torque F2', so that the user can easily and safely push and pull each transmission mechanism 12 , And then use manpower to directly teach the position.

Please refer to FIG. 5, FIG. 7 and FIG. 8 together. FIG. 8 is a partial flowchart of the triggering and compensation method of teaching position and posture according to the fourth embodiment of the present invention. Compared with the method shown in FIG. 5 and FIG. 7, the embodiment of FIG. 8 illustrates how to calculate the friction force compensation force (ie, step S30) in more detail. The method of this embodiment uses the following steps to calculate the friction force compensation force.

Step S400: The controller 3 determines whether each motor 11 is about to rotate due to an external force.

If any motor 11 does not receive external force in the posture maintenance mode (for example, the output torque of the motor 11 has not changed), the controller 3 executes step S401: it is determined that the user has not directly applied force to the transmission mechanism 12 (that is, has not started to perform Posture teaching) or although the external force is applied, but the external force does not act on the motor 11, the motor 11 does not need to perform friction compensation, and the friction compensation force of the motor is set to zero.

If any motor 11 receives an external force, the controller 3 switches to the power maintenance mode and executes step S402: determining whether the moving speed of each transmission mechanism 12 is zero (that is, the transmission mechanism 12 has overcome the maximum static friction force and starts to move).

If the moving speed of any transmission mechanism 12 is zero, the controller 3 determines that the motor 11 is not rotating, or although it is rotating, but the power provided does not exceed the maximum static friction force of the transmission mechanism 12, so that the transmission mechanism 12 remains stationary and executes Step S403: Set the friction compensation force of the motor used to move the transmission mechanism 12 as the initial friction force (for example, close to or equal to the maximum static friction force).

If the moving speed of any transmission mechanism 12 is greater than zero, the controller 3 determines that the power provided by the motor 11 has exceeded the maximum static friction force of the transmission mechanism 12, so that the transmission mechanism 12 starts to move, and executes step S404: setting to move this The friction compensation force of the motor of the transmission mechanism 12 is dynamic friction. The aforementioned dynamic friction is less than the aforementioned initial friction

In one embodiment, the controller 3 can calculate the aforementioned dynamic friction force according to the dynamic friction coefficient and the fretting friction force corresponding to the current rotation speed of each motor 11. Taking the torque as an example, it can be calculated as the following formula (1): dynamic friction torque = fretting friction torque + dynamic friction coefficient * current rotation speed... formula (1).

Next, the controller 3 executes step S405: judging whether the moving speed of any transmission mechanism 12 is greater than the safe speed.

If the moving speed of the transmission mechanism 12 is not greater than the safe speed, the controller 3 does not need to perform the safety function, that is, execute step S406: directly output the calculated friction compensation force.

If the moving speed of any transmission mechanism 12 is greater than the safe speed, the controller 3 executes the safety function, that is, step S407: the controller 3 reduces the friction compensation force to reduce the moving speed of the transmission mechanism 12.

In this way, the present invention can adjust the friction compensation force according to the current movement state of the mechanical device 1 so as to improve the user experience when the user performs posture teaching.

Please refer to FIGS. 9 and 10 together. FIG. 9 is a flowchart of the first part of the triggering and compensation method of teaching position and posture according to the fifth embodiment of the present invention, and FIG. 10 is the teaching position of the fifth embodiment of the present invention. The second part of the flow chart of the trigger and compensation method with attitude. The method of this embodiment includes the following steps.

Step S500: the automated mechanical system accepts an operation (for example, accepts an operation to enter the teaching state via a human-machine interface), and is controlled by the controller 3 to switch to the teaching state 40.

Then, the controller 3 executes steps S501-S508 to detect whether the user starts to perform the posture teaching in the posture maintenance mode, and automatically switches to the power maintenance mode when the user’s starting intention is detected. Formula to provide compensation and accept posture teaching. Steps S501-S508 in FIG. 9 are the same as or similar to steps S10-S17 in FIG. 5, and the details of their execution will not be repeated.

Then, the controller 3 executes step S509: in the power maintenance mode, it is detected whether the mechanical device 1 stops changing position and posture, for example, whether the current position of each motor 11 continues to change to detect whether the user has completed the position and posture teaching.

If the mechanical device 1 does not stop changing the position and posture, the controller 3 executes step S509 again to continue the detection.

If the mechanical device 1 stops changing the position and the posture, the controller 3 executes step S510: recording the current posture (second posture), such as recording the current position of each motor 11 as the position of the second posture.

Then, the controller 3 executes step S511: the controller 3 determines whether to leave the teaching state 40, such as the user operating the human-machine interface to control the automated mechanical system to leave the teaching state 40, or the automated mechanical system automatically leaves the teaching state 40 after completing the posture teaching .

If it does not leave the teaching state 40, the controller 3 controls the automated mechanical system to go to step S501, which is to switch back to the posture maintenance mode 400 to detect the new teaching intention.

If leaving the teaching state 40, the controller 3 executes step S512: controlling the automated mechanical system to switch to the working state, and enter the posture maintenance mode 410.

In one embodiment, the user can operate the human-machine interface to control the automated mechanical system to enter the working state 41, or the automated mechanical system automatically enters the working state 41 after completing the posture teaching.

Next, the controller 3 executes step S513: the controller 3 determines whether any work control command is received.

In an embodiment, the aforementioned work control command may include a motor control command of each motor 11 (such as a position command, a steering command and/or a rotation speed command).

Then, the controller 3 executes step S514: execute the work control command to control each motor 11 to rotate to the position specified by the work control command, and assume the posture corresponding to the work control command.

In an embodiment, one of the work control commands may be a command that requires the mechanical device 1 to assume the second posture (that is, the posture learned through the aforementioned posture teaching). In step S514, the controller 3 controls the motors. 11 Rotate to the position of the second posture recorded in the teaching state 40.

Step S515: The controller 3 determines whether to end the work. For example, the user can operate the human-machine interface to control the automated mechanical system to leave the working state 41, or the automated mechanical system automatically leaves the working state 41 after completing the work.

If it is determined to end the work, the controller 3 ends the execution of the method. If it is determined that the work is not finished, the controller 3 executes step S513 again.

In this way, the present invention can use the posture learned in the teaching state 40 for the working state 41.

The above are only preferred specific examples of the present invention, and are not limited to the scope of the patent of the present invention. Therefore, all equivalent changes made by using the content of the present invention are included in the scope of the present invention in the same way. bright.

S10-S18: The first trigger and compensation step

Claims (9)

A method for triggering and compensating teaching position and posture is used in an automated mechanical system including a controller, a motor driver, and a mechanical device. The mechanical device includes a plurality of motors for changing positions and postures. The teaching position and The posture triggering and compensation method includes the following steps: a) In a posture maintenance mode, the controller calculates and executes a new posture maintenance command every other period to maintain the mechanical device in a first posture, wherein the posture maintains The command includes a motor control command for each of the motors, wherein the plurality of attitude maintenance commands are respectively used to provide a plurality of output torques; b) calculating the output torque provided by the new attitude maintenance command and the corresponding first attitude A torque difference between the output torque provided by a reference attitude command. When the torque difference is greater than a torque critical value, it is determined that the mechanical device receives an external force, and the direction of the external force is determined according to the torque difference and switched to a Power maintenance mode; c) In the power maintenance mode, determine the current power of each motor of the mechanical equipment; d) Determine a compensation force according to the direction of the external force; and e) According to the compensation force and the motor’s The current power calculates a power maintenance command and executes the power maintenance command via the motor driver to reduce the resistance of adjusting the mechanical device to a second posture in the direction of the external force, wherein the power maintenance command includes the motor control command of each motor. The method for triggering and compensating the teaching position and posture according to claim 1, wherein the step a) includes the following steps: a1) In the posture maintenance mode, the controller obtains a current posture of the mechanical device every this period A2) Calculate the new posture maintenance command based on a difference between the current posture and the first posture; and a3) Each of the motor control commands of the new posture maintaining command controls the rotation of each motor so that the mechanical device is in the first posture, wherein each of the motor control commands includes a rotation speed command and a rotation direction command. The method for triggering and compensating teaching position and posture according to claim 2, wherein the current posture includes the current position of each motor, and the first posture includes a first position of each motor; the step a2) is to calculate each motor The difference between the current position of the motor and the first position, and each of the motor control commands for moving each of the motors from the current position to the first position is generated according to the difference as the new Posture maintenance command. The method for triggering and compensating the teaching position and posture according to claim 1, wherein in the posture maintaining mode, the new posture maintaining command is used to control the motors to change the rotation mode to offset the external force on the mechanical equipment The reference posture command is used to control the rotation mode of each motor so that the mechanical device assumes the first posture without external force. The method for triggering and compensating the teaching position and posture according to claim 1, wherein the step d) includes a step d1) calculating the friction force required to offset or reduce the mechanical device's teaching movement in the direction of the external force A friction compensation force. The method for triggering and compensating the teaching position and posture according to claim 6, wherein the multiple motors are used to drive multiple transmission mechanisms to move to change the posture of the mechanical equipment, and the step c) is to obtain the information of each transmission mechanism. A moving speed; the step d1) includes the following steps: d11) when the motor is not subjected to external force, setting the friction compensation force of the motor to zero; d12) when the moving speed of any of the transmission mechanisms is zero, Set the friction compensation force of the corresponding motor as an initial friction force; d13) when the moving speed of any of the transmission mechanisms is greater than zero, set the friction compensation force of the corresponding motor as a dynamic friction force; and d14) When the moving speed of any of the transmission mechanisms is greater than a safe speed, reduce the friction compensation force. The method for triggering and compensating the teaching position and posture according to claim 5, wherein the step d) includes the following steps: d2) calculating a required amount of gravity for offsetting or reducing the gravity of the mechanical device based on the current power of each motor Gravity compensation force; and d3) determine the final compensation force based on the friction compensation force and the gravity compensation force. The method for triggering and compensating the teaching position and posture as described in claim 1, further includes the following steps: f) When detecting that the mechanical equipment stops changing the position and posture in the power maintenance mode, record a current of each motor The position is used as the position of the second posture; and g) switching to the posture maintenance mode. The method for triggering and compensating the teaching position and posture as described in claim 8, further includes the following steps: h) when the automated mechanical system is operated to switch to a teaching state, repeat the steps a) to g) ; And i) when the automated mechanical system is operated to switch to a working state and a working control command that requires the mechanical equipment to assume the second posture is received, control each of the motors to rotate to the recorded state in the teaching state The position of the second posture.

Images