CN114260877A - Method and device for dragging teaching speed limit of mechanical arm joint, electronic equipment and medium - Google Patents
Method and device for dragging teaching speed limit of mechanical arm joint, electronic equipment and medium Download PDFInfo
- Publication number
- CN114260877A CN114260877A CN202111567730.6A CN202111567730A CN114260877A CN 114260877 A CN114260877 A CN 114260877A CN 202111567730 A CN202111567730 A CN 202111567730A CN 114260877 A CN114260877 A CN 114260877A
- Authority
- CN
- China
- Prior art keywords
- joint motor
- joint
- torque
- speed
- output torque
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
Images
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/0081—Programme-controlled manipulators with master teach-in means
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J17/00—Joints
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J18/00—Arms
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1602—Programme controls characterised by the control system, structure, architecture
- B25J9/161—Hardware, e.g. neural networks, fuzzy logic, interfaces, processor
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
- B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
Abstract
The invention discloses a dragging teaching speed-limiting method for a mechanical arm joint, which comprises the following steps: acquiring the actual dragging speed of the joint; when the actual dragging speed exceeds the preset safe dragging speed; calculating the adjusting moment of a joint motor through a dynamic model, wherein the direction of the adjusting moment is opposite to the current movement direction of the joint; and converting the adjusting torque into a motion control command of the joint motor and sending the motion control command to the joint motor. According to the method for speed limiting of the dragging teaching of the mechanical arm joint, when the mechanical arm is subjected to dragging teaching, if the actual dragging speed of the joint exceeds the safe dragging speed, a joint motor generates resistance opposite to the dragging direction, so that the dragging speed of the mechanical arm joint is reduced, the dragging speed of the mechanical arm joint is prevented from being too high, and the safety risk problem caused by too high speed is avoided. In addition, the invention also discloses a dragging teaching speed limiting device of the mechanical arm joint, electronic equipment and a medium.
Description
The proposal is a divisional application, the parent application number of which is CN202110759410.4, the application date of which is 2021-07-06, and the name of the case is 'method, device, electronic equipment and medium for demonstrating speed limit by dragging of mechanical arm joint'.
Technical Field
The invention relates to the field of mechanical arms, in particular to a method and a device for dragging, teaching and speed limiting of a mechanical arm joint, electronic equipment and a medium.
Background
The dragging teaching is also called manual teaching, teaching programming work of the mechanical arm is completed by an operator directly in a manual dragging mode, and then the mechanical arm can trigger an instruction for repeatedly executing a teaching track according to an external signal.
When the mechanical arm is dragged and taught, a teaching button needs to be clicked first, so that the mechanical arm enters a dragging teaching mode. After the mechanical arm enters the dragging teaching mode, the mechanical arm is dragged to move to an expected position by an operator, and in the process that the mechanical arm is dragged, the mechanical arm can record and store the motion pose or motion track of the mechanical arm in real time, so that the dragging teaching process of the mechanical arm is completed.
In the prior art, a mechanical arm is in a torque loop control mode, the motion state of a mechanical arm joint is determined by a motor output torque and an external dragging torque in a dragging teaching mode, and when the external dragging torque is too large, the mechanical arm joint has safety risk due to too high speed. In addition, when the mechanical arm joint moves rapidly due to being dragged, even if the external dragging torque stops being applied, the dynamic model of the mechanical arm joint can generate force for maintaining the current movement, so that the mechanical arm joint can still continue to move for a certain distance at the current speed, and safety risks also exist in the process.
Disclosure of Invention
The invention mainly aims to provide a method for speed limitation of drag teaching of a mechanical arm joint, and aims to solve the problem of safety risk caused by too high speed of the conventional mechanical arm joint during drag teaching.
In order to achieve the purpose, the invention provides a method for demonstrating speed limit by dragging of a mechanical arm joint, which comprises the following steps:
acquiring the actual dragging speed of the joint;
when the actual dragging speed exceeds the preset safe dragging speed;
calculating the adjusting moment of a joint motor through a dynamic model, wherein the direction of the adjusting moment is opposite to the current movement direction of the joint;
and converting the adjusting torque into a motion control command of the joint motor and sending the motion control command to the joint motor.
The method for teaching speed limit of the mechanical arm joint by dragging further comprises the following steps:
setting a speed threshold value of a joint when the mechanical arm carries out dragging teaching;
and carrying out speed synthesis on the set speed threshold and the current movement direction of the joint to obtain the safe dragging speed of the joint.
When the actual dragging speed is less than or equal to the safe dragging speed, the mechanical arm joint dragging teaching speed-limiting method further comprises the following steps:
calculating to obtain theoretical moment of the joint based on the dynamic model and the actual dragging speed of the joint;
determining the moment constraint range of the joint according to the theoretical moment of the joint and the current speed direction of the joint;
and performing saturation adjustment on the output torque according to the torque constraint range.
Wherein, according to the theoretical moment of the joint and the current speed direction thereof, determining the moment constraint range of the joint comprises the following steps:
if the directions of the theoretical moment and the actual dragging speed are the same, determining that the moment constraint range of the joint is [0, T ]1]；
If the theoretical moment and the actual dragging speed are in the same negative direction, the moment constraint range of the joint is determined to be [ -T ]1，0]；
If the direction of the theoretical moment is positive and the direction of the actual dragging speed is negative, determining the moment constraint range of the joint as T1，T2]；
If the direction of the theoretical moment is negative and the direction of the actual dragging speed is positive, the moment constraint range of the joint is determined to be [ -T ]2，-T1]；
T1Is the theoretical moment of the joint, T2For rated torque, T, of joint motors1Less than T2And T1And T2Both positive values, the positive and negative directions of the moment and the speed are referenced to the zero point of the joint.
Wherein, carry out saturation adjustment to output torque according to moment restraint scope includes:
when the torque constraint range of the joint is [0, T ]1]And the direction of the output torque is positive: if the output torque is greater than T1Then the output torque is adjusted to T1(ii) a If the output torque is less than or equal to T1The output torque does not need to be adjusted;
when the torque constraint range of the joint is [ -T ]1，0]And the direction of the output torque is negative: if the output torque is greater than-T1The output torque does not need to be adjusted; if the output torque is less than or equal to-T1Then the output torque is adjusted to-T1；
When the torque constraint range of the joint is [ T ]1，T2]And the direction of the output torque is positive: if the output torque is greater than T1And is less than T2The output torque does not need to be adjusted; if the output torque is less than T1Then the output torque is adjusted to T1(ii) a If the output torque is greater than T2Then the output torque is adjusted to T2；
When the torque constraint range of the joint is [ -T ]2，-T1]And the direction of the output torque is negative: if the output torque is greater than-T2And is less than-T1The output torque does not need to be adjusted; if the output torque is less than-T2Then the output torque is adjusted to-T2(ii) a If the output torque is greater than-T1Then the output torque is adjusted to-T1。
The invention also provides a dragging teaching speed limiting device of the mechanical arm joint, which comprises:
the speed acquisition module is used for acquiring the actual dragging speed of the joint;
the adjusting torque module is used for calculating the adjusting torque of the joint motor through the dynamic model when the actual dragging speed exceeds the preset safe dragging speed, and the direction of the adjusting torque is opposite to the current movement direction of the joint;
and the command generation module is used for converting the adjusting torque into a motion control command of the joint motor and sending the motion control command to the joint motor.
Wherein, the arm joint drag teaching speed limiting device still includes:
the speed setting module is used for setting a speed threshold of the joint when the mechanical arm carries out dragging teaching;
and the speed synthesis module is used for carrying out speed synthesis on the set speed threshold and the current movement direction of the joint to obtain the safe dragging speed of the joint.
Wherein, the arm joint drag teaching speed limiting device still includes:
the theoretical moment module is used for calculating and obtaining the theoretical moment of the joint based on the dynamic model and the actual dragging speed of the joint;
the moment constraint module is used for determining the moment constraint range of the joint according to the theoretical moment of the joint and the current speed direction of the joint;
and the torque adjusting module is used for performing saturation adjustment on the output torque according to the torque constraint range.
The invention also proposes an electronic device comprising:
a memory for storing a computer program;
a processor for implementing the above-described drag teaching speed-limiting method for a robot joint when executing a computer program, the drag teaching speed-limiting method for a robot joint at least including the steps of:
acquiring the actual dragging speed of the joint;
when the actual dragging speed exceeds the preset safe dragging speed;
calculating the adjusting moment of a joint motor through a dynamic model, wherein the direction of the adjusting moment is opposite to the current movement direction of the joint;
and converting the adjusting torque into a motion control command of the joint motor and sending the motion control command to the joint motor.
The present invention also provides a medium storing a computer program which, when executed by a processor, implements the aforementioned drag teaching speed-limiting method for a robot joint, the drag teaching speed-limiting method for a robot joint including at least the steps of:
acquiring the actual dragging speed of the joint;
when the actual dragging speed exceeds the preset safe dragging speed;
calculating the adjusting moment of a joint motor through a dynamic model, wherein the direction of the adjusting moment is opposite to the current movement direction of the joint;
and converting the adjusting torque into a motion control command of the joint motor and sending the motion control command to the joint motor.
Compared with the prior art, the embodiment of the invention has the beneficial technical effects that:
when the mechanical arm is dragged and taught, whether the actual dragging speed of each joint of the mechanical arm exceeds the preset safe dragging speed or not is judged, wherein each mechanical arm joint corresponds to one preset safe dragging speed. If the dragging speed does not exceed the safe dragging speed, the speed of the mechanical arm joint is not limited, if the dragging speed exceeds the safe dragging speed, the adjusting torque of the joint motor of the mechanical arm joint is calculated through a dynamic model, the direction of the adjusting torque is opposite to the current movement direction (dragging direction) of the joint, and the reverse adjusting torque enables the joint motor to generate resistance opposite to the dragging direction, so that the dragging speed of the mechanical arm joint is reduced, the dragging speed of the mechanical arm joint is prevented from being too high, and the safety risk problem caused by too high speed is avoided.
Drawings
Fig. 1 is a schematic flow chart illustrating a method for teaching speed limit by dragging a robot joint according to an embodiment of the present invention;
FIG. 2 is a schematic illustration of joint speed limiting in an embodiment of the present invention;
FIG. 3 is a schematic flow chart illustrating a method for teaching speed limit by dragging a robot joint according to an embodiment of the present invention;
FIG. 4 is a schematic structural diagram of a robot joint drag teaching speed limiting device according to an embodiment of the present invention;
fig. 5 is a schematic structural view of a robot joint drag teaching speed limiting device according to an embodiment of the present invention.
Detailed Description
Reference will now be made in detail to embodiments of the present invention, examples of which are illustrated in the accompanying drawings, wherein like or similar reference numerals refer to the same or similar elements or elements having the same or similar function throughout. The embodiments described below with reference to the drawings are exemplary and intended to be illustrative of the present invention and should not be construed as limiting the present invention, and all other embodiments that can be obtained by one skilled in the art based on the embodiments of the present invention without inventive efforts shall fall within the scope of protection of the present invention.
The dragging teaching speed-limiting method for the mechanical arm joint is suitable for multi-joint robots, such as three-axis joint robots, four-axis joint robots, five-axis joint robots, six-axis joint robots, seven-axis joint robots and the like. The multi-joint robot comprises a main controller and a plurality of joints which are in communication connection with the main controller respectively, wherein each joint is internally provided with a related joint controller, a joint motor, an encoder and the like, the encoder comprises a code disc and a reading head, the code disc is sleeved on a motor shaft of the joint motor and rotates along with the motor shaft, the reading head is used for reading data on the code disc and sending the data to the joint controller or the main controller, the joint controller or the main controller calculates the rotating speed and the position of the joint according to the received data, and the rotating speed of the joint can be detected in real time through the encoder.
Therefore, the invention provides a speed limiting method for drag teaching of a mechanical arm joint, which aims to limit the drag speed of the joint by generating an adjusting torque opposite to the drag direction of a joint motor of the joint when the actual drag speed of each joint exceeds the safe drag speed, so as to prevent the drag speed of the joint from being too high, namely: corresponding to applying a resistance to the overdrive joint.
Referring to fig. 1, fig. 1 is a schematic flow chart of a method for demonstrating speed limitation by dragging a robot joint according to an embodiment of the present invention, including the following steps:
s10, acquiring the actual dragging speed of the joint;
in this embodiment, the dragging speed of the joint is the speed of the joint in the joint space, which is substantially the rotation speed of the joint motor, and the speed direction of the joint motor is converted from the positive direction to the negative direction, for example, the positive direction of the joint motor is the positive direction, and the negative direction of the joint motor is the negative direction.
The actual dragging speed of the joints refers to the rotating speed of the joint motor during rotation, wherein the actual dragging speed of each joint of the mechanical arm is different, and a six-axis mechanical arm is taken as an example. Assuming that the actual drag speed of the first joint of the six-axis robot arm is 30 °/s, the actual drag speed of the second joint may be 35 °/s, the actual drag speed of the third joint may be 40 °/s, the actual drag speed of the fourth joint may be 45 °/s, the actual drag speed of the fifth joint may be 50 °/s, and the actual drag speed of the sixth joint may be 60 °/s.
When the dragging teaching is carried out, the encoder of each joint detects the rotation data of the joint motor, then the joint controller of the corresponding joint processes the rotation data to calculate the rotation speed of the joint motor, namely the actual dragging speed of the joint, and then the calculated actual dragging speed of the joint is sent to the main controller of the mechanical arm. In addition, the rotation data detected by the encoder can also be directly sent to the main controller, so that the main controller calculates the actual dragging speed of the joint according to the rotation data.
S20, when the actual dragging speed exceeds the preset safe dragging speed, calculating the adjusting moment of the joint motor through the dynamic model, wherein the direction of the adjusting moment is opposite to the current movement direction of the joint;
in some embodiments, after the actual dragging speed of the joint is obtained, whether the speed of the joint needs to be limited is determined based on the actual dragging speed and the corresponding safe dragging speed. Specifically, if the actual dragging speed exceeds the preset safe dragging speed, the joint needs to be limited in speed, and if the actual dragging speed does not exceed the preset safe dragging speed, the joint does not need to be limited in speed.
Because the actual dragging speed of each joint of the mechanical arm is different, when the mechanical arm is dragged and taught, the speed of each joint needs to be limited respectively, so that the actual dragging speed of each joint is smaller than the respective safe dragging speed, and if the actual dragging speed of any joint exceeds the preset safe dragging speed, the mechanical arm still has the aforementioned safety risk problem.
Further, the safe drag speed of each joint is also different, and still taking the six-axis robot arm as an example, assuming that the safe drag speed threshold of the first joint is 40 °/s, the safe drag speed threshold of the second joint may be 45 °/s, the safe drag speed threshold of the third joint is 50 °/s, the safe drag speed threshold of the fourth joint is 55 °/s, the safe drag speed threshold of the fifth joint is 60 °/s, and the safe drag speed threshold of the sixth joint is 70 °/s.
Referring to fig. 2, fig. 2 is a schematic diagram illustrating a speed limit of a robot arm joint according to an embodiment of the present invention.
In this embodiment, when the actual dragging speed exceeds the safe dragging speed, the adjustment torque of the joint motor is calculated through the dynamic model, where the direction of the adjustment torque is opposite to the current motion direction (dragging direction) of the joint, and the resultant force shown in fig. 2 is the resultant force of the dragging torque and the adjustment torque.
And S30, converting the adjusting torque into a motion control command of the joint motor, and sending the motion control command to the joint motor.
The adjusting torque is converted into a motion control instruction of the joint motor, the joint motor generates resistance opposite to the joint dragging direction according to the control instruction, the actual dragging speed of the joint is reduced through the resistance, the purpose of speed limitation is achieved, and the safety of the mechanical arm during dragging teaching is improved.
The specific application is as follows: the dragging mechanical arm is used for dragging and teaching the mechanical arm, in the dragging process, the encoder of each joint detects the rotation data of the corresponding joint motor in real time, and then the rotation data is processed by the joint controller or the main controller so as to calculate the rotation speed of the joint motor, namely the actual dragging speed of the joint. Assuming that the actual dragging speed of a certain joint is +45 °/s, the corresponding safe dragging speed is +40 °/s, + represents the joint motor of the joint to rotate forward (assuming that the forward rotation is the positive direction), i.e., the rotation direction of the joint motor, and 45 °/s represents the rotation speed of the joint motor, i.e., the joint motor rotates 45 ° per second. At the moment, the actual dragging speed of the joint is +45 degrees/s, and is greater than the safe dragging speed of the joint +40 degrees/s, then the adjusting moment of a joint motor is obtained through calculation of a dynamic model, the direction of the adjusting moment is opposite to the rotating direction of the joint motor, and the adjusting moment obtained through calculation is assumed to be-60N. After the adjusting torque is obtained through calculation, the adjusting torque is converted into a motion control instruction of a joint motor, the joint motor controls the joint motor to rotate according to the motion control instruction, so that the speed of the joint motor is reduced under the action of the adjusting torque, and the rotating speed of the joint motor can be reduced to be lower than +40 degrees/s, so that the dragging speed of the joint is prevented from being too high, the safety risk is avoided, and the safety of the mechanical arm during dragging teaching is improved.
Furthermore, before limiting the speed of each joint of the mechanical arm, the safe dragging speed of each joint needs to be set, the speed of the safe dragging speed is set according to the actual situation, and the speed direction of the safe dragging speed is the same as the current movement direction (dragging direction) of the joint.
Specifically, referring to fig. 3, fig. 3 is a schematic flow chart of a method for speed limiting by dragging teaching of a robot arm joint according to an embodiment of the present invention, including the following steps:
s1, setting a speed threshold of the joint when the mechanical arm carries out drag teaching;
and S2, synthesizing the speed of the set speed threshold value and the current movement direction of the joint to obtain the safe dragging speed of the joint during dragging teaching.
Each joint is correspondingly set with a speed threshold, namely the maximum dragging speed of the joint, after the speed threshold is set, the current movement direction (dragging direction) of the joint is obtained through an encoder, and then the speed threshold and the movement direction of the joint are synthesized to obtain the safe dragging speed of the joint. The movement direction of the joint takes the zero point of the joint as a reference, and if the positive rotation (clockwise rotation) of the joint motor is taken as the positive direction, the reverse rotation (counterclockwise rotation) of the joint motor is taken as the negative direction, and the zero point is equivalent to the origin of the two-dimensional rectangular coordinate system.
Example (c): assuming that the speed threshold of a certain joint of the mechanical arm is set to 50 °/s, if the encoder detects that the current motion direction of the joint is a positive direction, the synthesized safe dragging speed is +50 °/s, and if the encoder detects that the current motion direction of the joint is a negative direction, the synthesized safe dragging speed is-50 °/s.
Further, the motion of the mechanical arm is generated under the combined action of the external dragging torque and the output torque of the joint motor, and when each joint of the mechanical arm moves, the dynamic model can calculate to obtain a theoretical torque for maintaining the current motion of each joint, namely: the external drag torque + the output torque is the theoretical torque. In order to ensure the controllability of the mechanical arm, the output torque of the joint motor cannot exceed the theoretical torque calculated by the dynamic model, if the output torque of the joint motor exceeds the theoretical torque, when the dragging is stopped (the external dragging torque is zero), because the output torque of the joint motor is greater than the theoretical torque, the joint still moves under the action of the resultant force of the two, the joint cannot be stopped when the hand is loosened, and the safety risk problem is easy to occur.
Therefore, the output torque of the joint motor of each joint is restrained, so that the output torque of the joint motor is smaller than the theoretical torque calculated by the dynamic model.
Specifically, referring to fig. 3, fig. 3 is a flowchart of a robot arm dragging teaching speed limiting method according to an embodiment of the present invention, including the following steps:
s40, calculating to obtain theoretical moment of the joint based on the dynamic model and the actual dragging speed of the joint;
and when the actual dragging speed of the joint is less than or equal to the safe dragging speed, calculating to obtain the theoretical moment of the joint based on the dynamic model and the actual dragging speed.
S50, determining the moment constraint range of the joint according to the theoretical moment of the joint and the current speed direction of the joint;
after the theoretical moment of the joint is obtained through calculation, the moment constraint range of the joint is determined according to the theoretical moment of the joint and the current speed direction, and the theoretical moment and the speed direction are referred to the zero calibration direction of the joint.
And S60, performing saturation adjustment on the output torque according to the torque constraint range.
After the moment constraint range is determined, the output moment is subjected to saturation adjustment according to the moment constraint range, so that the output moment of the joint motor is always smaller than the theoretical moment, and the joints of the mechanical arm can stop moving after hands are loosened.
Example (c): assuming that the torque constraint range determined after steps S40 and S50 is [0, 100], if the output torque calculated next time is 80N, the output torque does not need to be adjusted, and the output torque can be directly converted into the motion control command of the joint motor, whereas if the output torque calculated next time is 110N, the output torque needs to be adjusted to 100N, and then the adjusted output torque is converted into the motion control command of the joint.
Further, the specific determination method of the torque constraint range provided by the invention is as follows:
if the directions of the theoretical moment and the actual dragging speed are the same, determining that the moment constraint range of the joint is [0, T ]1]；
If the theoretical moment and the actual dragging speed are in the same negative direction, the moment constraint range of the joint is determined to be [ -T ]1，0]；
If the direction of the theoretical moment is positive and the direction of the actual dragging speed is negative, determining the moment constraint range of the joint as T1，T2]；
If the direction of the theoretical moment is negative and the direction of the actual dragging speed is positive, the moment constraint range of the joint is determined to be [ -T ]2，-T1]；
Wherein, T1Is the theoretical moment of the joint, T2Is the rated torque, T, of the servo motor1Less than T2And T1And T2Both positive values, the positive and negative directions of the moment and the speed are referenced to the zero point of the joint.
Example (c): let T be1Is 100N, T2The actual dragging speed is 150N, the actual dragging speed is 50 degrees/s, the forward rotation (clockwise rotation) of the joint motor is the forward direction, the reverse rotation (anticlockwise rotation) of the joint motor is the reverse direction, the directions of the theoretical moment and the actual dragging speed are firstly judged, and then the moment constraint range of the joint is determined according to the directions of the theoretical moment and the actual dragging speed.
Specifically, if the theoretical moment is +100N and the actual dragging speed is +50 °/s, determining the moment constraint range of the joint as [0, 100 ]; if the theoretical moment is-100N and the actual dragging speed is-50 degrees/s, determining the moment constraint range of the joint as-100, 0; if the theoretical moment is +100N and the actual dragging speed is-50 degrees/s, determining the moment constraint range of the joint as [100, 150 ]; if the theoretical torque is-100N and the actual drag speed is + 50/s, the torque constraint range for the joint is determined to be-150, -100.
Further, the specific adjusting method of the output torque provided by the invention comprises the following steps:
when the torque constraint range of the joint is [0, T ]1]And the output torque is positive: if the output torque is greater than T1Then the output torque is adjusted to T1(ii) a If the output torque is less than or equal to T1The output torque does not need to be adjusted;
when the torque constraint range of the joint is [ -T ]1，0]And when the output torque is negative: if the output torque is greater than-T1The output torque does not need to be adjusted; if the output torque is less than or equal to-T1Then the output torque is adjusted to-T1；
When the torque constraint range of the joint is [ T ]1，T2]And the output torque is positive: if the output torque is greater than T1And is less than T2The output torque does not need to be adjusted; if the output torque is less than T1Then the output torque is adjusted to T1(ii) a If the output torque is greater than T2Then the output torque is adjusted to T2；
When the torque constraint range of the joint is [ -T ]2，-T1]And when the output torque is negative: if the output torque is greater than-T2And is less than-T1The output torque does not need to be adjusted; if the output torque is less than-T2Then the output torque is adjusted to-T2(ii) a If the output torque is greater than-T1Then the output torque is adjusted to-T1。
Example (c): let T be1Is 100N, T2150N, the actual dragging speed is 50 degrees/s, the forward rotation (clockwise rotation) of the joint motor is the forward direction, the reverse rotation (anticlockwise rotation) of the joint motor is the reverse direction, the directions of the theoretical moment and the actual dragging speed are judged firstly, then the moment constraint range of the joint is determined according to the directions of the theoretical moment and the actual dragging speed, and then the moment constraint range is calculated for the next time to obtain the torque constraint rangeThe output torque of (a) is adjusted, specifically:
when the torque constraint range of the joint is [0, 100] and the direction of the output torque is positive:
if the output torque obtained by the next calculation is +120N, the output torque is adjusted to be +100N, and if the output torque obtained by the next calculation is +90N, the output torque does not need to be adjusted;
when the torque constraint range of the joint is [ -100, 0] and the direction of the output torque is negative:
if the output torque obtained by the next calculation is-90N, the output torque does not need to be adjusted, and if the output torque obtained by the next calculation is-120N, the output torque is adjusted to-100;
when the torque constraint range of the joint is [100, 150] and the direction of the output torque is positive:
if the output torque calculated next time is +120N, it is not necessary to adjust the output torque, if the output torque calculated next time is 90N, the output torque is adjusted to 100N, and if the output torque calculated next time is 160N, the output torque is adjusted to 150N.
When the torque constraint range of the joint is [ -150, -100] and the direction of the output torque is negative:
if the next calculated output torque is-120N, the output torque does not need to be adjusted, if the next calculated output torque is-90N, the output torque is-100N, and if the next calculated output torque is-160N, the output torque is-150N.
Referring to fig. 4, fig. 4 is a schematic structural diagram of a robot joint drag teaching speed limiting device according to an embodiment of the present invention, including:
the speed acquisition module 10 is used for acquiring the actual dragging speed of the joint;
the adjusting torque module 20 is used for calculating the adjusting torque of the joint motor through the dynamic model when the actual dragging speed exceeds the preset safe dragging speed, and the direction of the adjusting torque is opposite to the current movement direction of the joint;
and the instruction generating module 30 is configured to convert the adjusting torque into a motion control instruction of the joint motor, and send the motion control instruction to the joint motor.
Further, the device for teaching speed limit by dragging the mechanical arm joint provided by the invention further comprises:
the speed setting module is used for setting a speed threshold of the joint when the mechanical arm carries out dragging teaching;
and the speed synthesis module is used for carrying out speed synthesis on the set speed threshold and the current motion direction of the joint to obtain the safe dragging speed of the joint during dragging teaching.
Referring to fig. 5, fig. 5 is a schematic structural diagram of a robot arm dragging teaching speed limiting device according to an embodiment of the present invention, further including:
the theoretical moment module 40 is used for calculating and obtaining the theoretical moment of the joint based on the dynamic model and the actual dragging speed of the joint;
the moment constraint module 50 is used for determining the moment constraint range of the joint according to the theoretical moment of the joint and the current speed direction of the joint;
and the torque adjusting module 60 is used for performing saturation adjustment on the output torque according to the torque constraint range.
Based on the aforementioned method for teaching speed limitation by dragging the mechanical arm joint, the invention further provides an electronic device, which includes:
a memory for storing a computer program;
a processor configured to implement the method for speed-limiting teaching for robot joint according to each of the embodiments described above when executing a computer program, wherein the method for speed-limiting teaching for robot joint drag includes at least the following steps:
step 1, acquiring the actual dragging speed of a joint;
step 2, when the actual dragging speed exceeds the preset safe dragging speed, calculating the adjusting moment of the joint motor through a dynamic model, wherein the direction of the adjusting moment is opposite to the current movement direction of the joint;
and 3, converting the adjusting torque into a motion control command of the joint motor, and sending the motion control command to the joint motor.
Based on the proposed method for speed-limiting for teaching the robot joint by dragging, the present invention further provides a medium storing a computer program, wherein the computer program, when executed by a processor, implements the method for speed-limiting for teaching the robot joint by dragging described in the foregoing embodiments, wherein the method for speed-limiting for teaching the robot joint by dragging at least comprises the following steps:
step 1, acquiring the actual dragging speed of a joint;
step 2, when the actual dragging speed exceeds the preset safe dragging speed, calculating the adjusting moment of the joint motor through a dynamic model, wherein the direction of the adjusting moment is opposite to the current movement direction of the joint;
and 3, converting the adjusting torque into a motion control command of the joint motor, and sending the motion control command to the joint motor.
In the several embodiments provided in the present application, it should be understood that the disclosed method and apparatus may be implemented in other ways. For example, the above-described apparatus embodiments are merely illustrative, and for example, the division of the modules is only one logical division, and other divisions may be realized in practice, for example, a plurality of modules or components may be combined or integrated into another apparatus, or some features may be omitted, or not executed. In addition, the shown or discussed mutual coupling or direct coupling or communication connection may be an indirect coupling or communication connection through some interfaces, devices or modules, and may be in an electrical, mechanical or other form.
The modules described as separate parts may or may not be physically separate, and parts displayed as modules may or may not be physical modules, may be located in one place, or may be distributed on a plurality of network modules. Some or all of the modules may be selected according to actual needs to achieve the purpose of the solution of the present embodiment.
In addition, functional modules in the embodiments of the present invention may be integrated into one processing module, or each of the modules may exist alone physically, or two or more modules are integrated into one module. The integrated module can be realized in a hardware mode, and can also be realized in a software functional module mode.
The integrated module, if implemented in the form of a software functional module and sold or used as a stand-alone product, may be stored in a computer readable storage medium. Based on such understanding, the technical solution of the present invention may be embodied in the form of a software product, which is stored in a storage medium and includes instructions for causing a computer device (which may be a personal computer, a server, or a network device) to execute all or part of the steps of the method according to the embodiments of the present invention. And the aforementioned storage medium includes: a U-disk, a removable hard disk, a Read-Only Memory (ROM), a Random Access Memory (RAM), a magnetic disk or an optical disk, and other various media capable of storing program codes.
The above description is only a part of or preferred embodiments of the present invention, and neither the text nor the drawings should be construed as limiting the scope of the present invention, and all equivalent structural changes, which are made by using the contents of the present specification and the drawings, or any other related technical fields, are included in the scope of the present invention.
Claims (11)
1. A drag teaching control method for a mechanical arm joint is characterized by comprising the following steps:
acquiring the rotating speed of a joint motor;
when the rotating speed of the joint motor is less than or equal to a preset safe dragging speed, calculating to obtain a theoretical moment of the joint motor based on a dynamic model and the rotating speed of the joint motor;
determining a torque constraint range of the joint motor according to the theoretical torque of the joint motor and the current speed direction of the joint motor;
judging that the output torque of the joint motor exceeds the torque constraint range of the joint motor;
and adjusting the output torque of the joint motor according to the torque constraint range of the joint motor.
2. The drag teaching control method of a robot arm joint according to claim 1, wherein the determining the torque constraint range of the joint motor according to the theoretical torque of the joint motor and the current speed direction of the joint motor comprises:
if the theoretical moment of the joint motor and the speed direction of the joint motor are the same positive, determining that the moment constraint range of the joint motor is [0, T ]1]；
The adjusting the output torque of the joint motor according to the torque constraint range of the joint motor comprises:
when the torque constraint range of the joint motor is [0, T ]1]And the direction of the output torque of the joint motor is positive: if the output torque of the joint motor is greater than T1Adjusting the output torque of the joint motor to T1；
Wherein, T is1The theoretical moment of the joint motor is the theoretical moment of the joint motor, and the positive and negative directions of the theoretical moment and the speed of the joint motor take the zero point of the joint of the mechanical arm as reference.
3. The drag teaching control method of a robot arm joint according to claim 1, wherein the determining the torque constraint range of the joint motor according to the theoretical torque of the joint motor and the current speed direction of the joint motor comprises:
if the theoretical moment of the joint motor and the speed direction of the joint motor are negative, determining that the moment constraint range of the joint motor is [ -T ]1，0]；
The adjusting the output torque of the joint motor according to the torque constraint range of the joint motor comprises:
when the torque constraint range of the joint motor is [ -T ]1，0]And when the direction of the output torque of the joint motor is negative: if the output torque of the joint motor is less than or equal to-T1Then adjusting the output torque of the joint motor to-T1；
Wherein, T is1The theoretical moment of the joint motor is the theoretical moment of the joint motor, and the positive and negative directions of the theoretical moment and the speed of the joint motor take the zero point of the joint of the mechanical arm as reference.
4. The drag teaching control method of a robot arm joint according to claim 1, wherein the determining the torque constraint range of the joint motor according to the theoretical torque of the joint motor and the current speed direction of the joint motor comprises:
if the direction of the theoretical moment of the joint motor is positive and the speed direction of the joint motor is negative, determining that the moment constraint range of the joint motor is [ T ]1，T2]；
The adjusting the output torque of the joint motor according to the torque constraint range of the joint motor comprises:
when the torque constraint range of the joint motor is [ T ]1，T2]And the direction of the output torque of the joint motor is positive: if the output torque of the joint motor is less than T1Adjusting the output torque of the joint motor to T1(ii) a If the output torque of the joint motor is greater than T2Adjusting the output torque of the joint motor to T2；
Wherein, T is1Is the theoretical moment of the joint motor, T2For rated torque of the joint motor, T1Less than T2And said T is1And T2The theoretical moment, the rated moment and the positive and negative directions of the speed of the joint motor are all positive values, and the zero point of the mechanical arm joint is used as reference.
5. The drag teaching control method of a robot arm joint according to claim 1, wherein the determining the torque constraint range of the joint motor according to the theoretical torque of the joint motor and the current speed direction of the joint motor comprises:
if the direction of the theoretical moment of the joint motor is negative and the speed direction of the joint motor is positive, determining that the moment constraint range of the joint motor is [ -T ]2，-T1]；
The adjusting the output torque of the joint motor according to the torque constraint range of the joint motor comprises:
when the torque constraint range of the joint motor is [ -T ]2，-T1]And when the direction of the output torque is negative: if the output torque of the joint motor is less than-T2Then adjusting the output torque of the joint motor to-T2(ii) a If the output torque of the joint motor is more than-T1Then adjusting the output torque of the joint motor to-T1；
Wherein, T is1Is the theoretical moment of the joint motor, T2For rated torque of the joint motor, T1Less than T2And said T is1And T2The theoretical moment, the rated moment and the positive and negative directions of the speed of the joint motor are all positive values, and the zero point of the mechanical arm joint is used as reference.
6. The utility model provides a mechanical arm joint's dragging teaching controlgear which characterized in that includes:
a memory for storing executable program instructions;
a processor executing the executable program instructions to perform the steps of:
acquiring the rotating speed of a joint motor;
when the rotating speed of the joint motor is less than or equal to a preset safe dragging speed, calculating to obtain a theoretical moment of the joint motor based on a dynamic model and the rotating speed of the joint motor;
determining a torque constraint range of the joint motor according to the theoretical torque of the joint motor and the current speed direction of the joint motor;
judging that the output torque of the joint motor exceeds the torque constraint range of the joint motor;
and adjusting the output torque of the joint motor according to the torque constraint range of the joint motor.
7. The robot joint drag teaching control apparatus of claim 6, wherein said processor when executing said executable program instructions further performs the steps of:
if the theoretical moment of the joint motor and the speed direction of the joint motor are the same positive, determining that the moment constraint range of the joint motor is [0, T ]1]；
When the torque constraint range of the joint motor is [0, T ]1]And the direction of the output torque of the joint motor is positive: if the output torque of the joint motor is greater than T1Adjusting the output torque of the joint motor to T1；
Wherein, T is1The theoretical moment of the joint motor is the theoretical moment of the joint motor, and the positive and negative directions of the theoretical moment and the speed of the joint motor take the zero point of the joint of the mechanical arm as reference.
8. The robot joint drag teaching control apparatus of claim 6, wherein said processor when executing said executable program instructions further performs the steps of:
if the theoretical moment of the joint motor and the speed direction of the joint motor are negative, determining that the moment constraint range of the joint motor is [ -T ]1，0]；
When the torque constraint range of the joint motor is [ -T ]1，0]And when the direction of the output torque of the joint motor is negative: if the output torque of the joint motor is less than or equal to-T1Then adjusting the output torque of the joint motor to-T1；
Wherein, T is1The theoretical moment of the joint motor is the theoretical moment of the joint motor, and the positive and negative directions of the theoretical moment and the speed of the joint motor take the zero point of the joint of the mechanical arm as reference.
9. The robot joint drag teaching control apparatus of claim 6, wherein said processor when executing said executable program instructions further performs the steps of:
if the direction of the theoretical moment of the joint motor is positive and the speed direction of the joint motor is negative, determining that the moment constraint range of the joint motor is [ T ]1，T2]；
When the torque constraint range of the joint motor is [ T ]1，T2]And the direction of the output torque of the joint motor is positive: if the output torque of the joint motor is less than T1Adjusting the output torque of the joint motor to T1(ii) a If the output torque of the joint motor is greater than T2Adjusting the output torque of the joint motor to T2；
Wherein, T is1Is the theoretical moment of the joint motor, T2For rated torque of the joint motor, T1Less than T2And said T is1And T2The theoretical moment, the rated moment and the positive and negative directions of the speed of the joint motor are all positive values, and the zero point of the mechanical arm joint is used as reference.
10. The robot joint drag teaching control apparatus of claim 6, wherein said processor when executing said executable program instructions further performs the steps of:
if the direction of the theoretical moment of the joint motor is negative and the speed direction of the joint motor is positive, determining that the moment constraint range of the joint motor is [ -T ]2，-T1]；
When the torque constraint range of the joint motor is [ -T ]2，-T1]And when the direction of the output torque is negative: if the output torque of the joint motor is less than-T2Then adjusting the output torque of the joint motor to-T2(ii) a If the output torque of the joint motor is more than-T1Then adjusting the output torque of the joint motor to-T1；
Wherein, T is1Is the theoretical moment of the joint motor, T2For rated torque of the joint motor, T1Less than T2And said T is1And T2The theoretical moment, the rated moment and the positive and negative directions of the speed of the joint motor are all positive values, and the zero point of the mechanical arm joint is used as reference.
11. A readable storage medium storing executable program instructions which, when executed by a processor, implement the drag teaching control method for a robot arm joint according to any one of claims 1 to 5.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111567730.6A CN114260877A (en) | 2021-07-06 | 2021-07-06 | Method and device for dragging teaching speed limit of mechanical arm joint, electronic equipment and medium |
Applications Claiming Priority (2)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111567730.6A CN114260877A (en) | 2021-07-06 | 2021-07-06 | Method and device for dragging teaching speed limit of mechanical arm joint, electronic equipment and medium |
| CN202110759410.4A CN113246107B (en) | 2021-07-06 | 2021-07-06 | Method and device for dragging teaching speed limit of mechanical arm joint, electronic equipment and medium |
Related Parent Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110759410.4A Division CN113246107B (en) | 2021-07-06 | 2021-07-06 | Method and device for dragging teaching speed limit of mechanical arm joint, electronic equipment and medium |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN114260877A true CN114260877A (en) | 2022-04-01 |
Family
ID=77190684
Family Applications (3)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110759410.4A Active CN113246107B (en) | 2021-07-06 | 2021-07-06 | Method and device for dragging teaching speed limit of mechanical arm joint, electronic equipment and medium |
| CN202111567730.6A Pending CN114260877A (en) | 2021-07-06 | 2021-07-06 | Method and device for dragging teaching speed limit of mechanical arm joint, electronic equipment and medium |
| CN202111568383.9A Pending CN114260878A (en) | 2021-07-06 | 2021-07-06 | Method and device for dragging teaching speed limit of mechanical arm joint, electronic equipment and medium |
Family Applications Before (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110759410.4A Active CN113246107B (en) | 2021-07-06 | 2021-07-06 | Method and device for dragging teaching speed limit of mechanical arm joint, electronic equipment and medium |
Family Applications After (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111568383.9A Pending CN114260878A (en) | 2021-07-06 | 2021-07-06 | Method and device for dragging teaching speed limit of mechanical arm joint, electronic equipment and medium |
Country Status (1)
| Country | Link |
|---|---|
| CN (3) | CN113246107B (en) |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN1200691A (en) * | 1995-09-14 | 1998-12-02 | 株式会社安川电机 | Teaching unit for robots |
| US20140012419A1 (en) * | 2012-07-05 | 2014-01-09 | Canon Kabushiki Kaisha | Robot control apparatus and robot control method |
| CN108582069A (en) * | 2018-04-17 | 2018-09-28 | 上海达野智能科技有限公司 | Robot drags teaching system and method, storage medium, operating system |
| CN109397265A (en) * | 2018-11-13 | 2019-03-01 | 华中科技大学 | A kind of joint type industrial robot dragging teaching method based on kinetic model |
| CN111152225A (en) * | 2020-01-15 | 2020-05-15 | 北京科技大学 | Uncertain mechanical arm fixed time trajectory tracking control method with input saturation |
| CN112571420A (en) * | 2020-12-21 | 2021-03-30 | 西北工业大学 | Dual-function model prediction control method under unknown parameters |
| CN112894821A (en) * | 2021-01-30 | 2021-06-04 | 同济大学 | Current method based collaborative robot dragging teaching control method, device and equipment |
Family Cites Families (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN100488734C (en) * | 2003-10-10 | 2009-05-20 | 三菱电机株式会社 | Robot controlling device |
| JP2009262306A (en) * | 2008-04-28 | 2009-11-12 | Kobe Steel Ltd | Method of teaching robot |
| KR101214685B1 (en) * | 2011-03-23 | 2013-01-09 | 고려대학교 산학협력단 | Method for controlling robot manipulator device based on frequency analysys of impact on the device |
| JP6514273B2 (en) * | 2017-06-19 | 2019-05-15 | ファナック株式会社 | Robot system that displays speed |
| CN107972036B (en) * | 2017-12-25 | 2021-04-27 | 厦门大学嘉庚学院 | Industrial robot dynamics control system and method based on TensorFlow |
| CN108656112B (en) * | 2018-05-15 | 2022-02-25 | 清华大学深圳研究生院 | Mechanical arm zero-force control experiment system for direct teaching |
| CN108748147A (en) * | 2018-06-01 | 2018-11-06 | 清华大学深圳研究生院 | A kind of control system and method for ectoskeleton mechanical arm |
| CN109531565B (en) * | 2018-08-27 | 2023-11-03 | 苏州博众智能机器人有限公司 | Control method and device of mechanical arm, service robot and storage medium |
| CN111478628B (en) * | 2019-01-07 | 2021-08-31 | 东元电机股份有限公司 | Torque mode motor driving device |
-
2021
- 2021-07-06 CN CN202110759410.4A patent/CN113246107B/en active Active
- 2021-07-06 CN CN202111567730.6A patent/CN114260877A/en active Pending
- 2021-07-06 CN CN202111568383.9A patent/CN114260878A/en active Pending
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN1200691A (en) * | 1995-09-14 | 1998-12-02 | 株式会社安川电机 | Teaching unit for robots |
| US20140012419A1 (en) * | 2012-07-05 | 2014-01-09 | Canon Kabushiki Kaisha | Robot control apparatus and robot control method |
| CN108582069A (en) * | 2018-04-17 | 2018-09-28 | 上海达野智能科技有限公司 | Robot drags teaching system and method, storage medium, operating system |
| CN109397265A (en) * | 2018-11-13 | 2019-03-01 | 华中科技大学 | A kind of joint type industrial robot dragging teaching method based on kinetic model |
| CN111152225A (en) * | 2020-01-15 | 2020-05-15 | 北京科技大学 | Uncertain mechanical arm fixed time trajectory tracking control method with input saturation |
| CN112571420A (en) * | 2020-12-21 | 2021-03-30 | 西北工业大学 | Dual-function model prediction control method under unknown parameters |
| CN112894821A (en) * | 2021-01-30 | 2021-06-04 | 同济大学 | Current method based collaborative robot dragging teaching control method, device and equipment |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113246107A (en) | 2021-08-13 |
| CN113246107B (en) | 2022-01-11 |
| CN114260878A (en) | 2022-04-01 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Safeea et al. | Kuka sunrise toolbox: Interfacing collaborative robots with matlab | |
| US9764462B2 (en) | Robot apparatus and robot controlling method | |
| JP5327722B2 (en) | Robot load estimation apparatus and load estimation method | |
| CN106945043B (en) | Multi-arm cooperative control system of master-slave teleoperation surgical robot | |
| US10864632B2 (en) | Direct teaching method of robot | |
| KR20180059888A (en) | Robot teaching method and robot arm control device | |
| CN109746913B (en) | Method and system for teaching robot posture keeping dragging | |
| CN110394801B (en) | Joint control system of robot | |
| JP2006212741A (en) | Task skill generating device | |
| JPH0720941A (en) | Flexible servo control method capable of specifying softness on work coordinates | |
| CN113001069B (en) | Welding seam tracking method of six-joint robot | |
| CN113246107B (en) | Method and device for dragging teaching speed limit of mechanical arm joint, electronic equipment and medium | |
| CN112847373B (en) | Robot track synchronous control method and computer readable storage medium | |
| JP3698770B2 (en) | Load weight estimation method | |
| TWI594858B (en) | Robotic arm teaching system | |
| Campbell et al. | Superpositioning of behaviors learned through teleoperation | |
| CN109483551B (en) | Method, device and system for controlling multi-axis motion of robot | |
| CN111152231A (en) | Robot teaching method based on force control | |
| JPH08190433A (en) | Load weight estimating method | |
| JPS618236A (en) | Moving body control unit | |
| JP2002331479A (en) | Method for controlling industrial robot | |
| JP2019214105A (en) | Robot control apparatus and robot control method | |
| US20230302643A1 (en) | Systems and Hybrid Position Force Control Processes of an Industrial Robot | |
| US20230302637A1 (en) | Systems and Hybrid Position Force Control Processes of an Industrial Robot | |
| JP2020069616A (en) | Motor driving device, robot driving system and motor driving program |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination |