CN113246107A - 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
- CN113246107A CN113246107A CN202110759410.4A CN202110759410A CN113246107A CN 113246107 A CN113246107 A CN 113246107A CN 202110759410 A CN202110759410 A CN 202110759410A CN 113246107 A CN113246107 A CN 113246107A
- Authority
- CN
- China
- Prior art keywords
- joint
- speed
- output torque
- torque
- dragging
- 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.)
- Granted
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
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 the output torque of the joint based on the dynamic model and the actual dragging speed of the joint;
determining the torque constraint range of the joint according to the output torque 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 output torque of the joint and the current speed direction thereof, determining the torque restriction range of the joint comprises:
if the output torque and the actual dragging speed are in the same positive direction, determining the torque constraint range of the joint as 0, T1]；
If the output torque and the actual dragging speed are in the same negative direction, determining the torque constraint range of the joint as [ -T ]1，0]；
If the direction of the output torque is positive and the direction of the actual dragging speed is negative, determining the torque constraint range of the joint as [ T ]1，T2]；
If the direction of the output torque is negative and the direction of the actual dragging speed is positive, determining the torque constraint range of the joint as [ -T ]2，-T1]；
T1Is the output torque 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 output torque module is used for calculating to obtain the output torque 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 output 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: external drag torque + output torque = 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 the output torque of the joint based on the dynamic model and the actual dragging speed of the joint;
when the actual dragging speed of the joint is less than or equal to the safe dragging speed, the output torque of the joint can be calculated based on the dynamic model and the actual dragging speed, and the sum of the output torque and the external dragging torque is equal to the theoretical torque mentioned above.
S50, determining the torque restriction range of the joint according to the output torque of the joint and the current speed direction of the joint;
after the output torque of the joint is obtained through calculation, the torque constraint range of the joint is determined according to the output torque of the joint and the current speed direction, and the output torque 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 output torque and the actual dragging speed are in the same positive direction, determining the torque constraint range of the joint as 0, T1]；
If the output torque and the actual dragging speed are in the same negative direction, thenDetermining a torque constraint range of a joint as [ -T [ ]1，0]；
If the direction of the output torque is positive and the direction of the actual dragging speed is negative, determining the torque constraint range of the joint as [ T ]1，T2]；
If the direction of the output torque is negative and the direction of the actual dragging speed is positive, determining the torque constraint range of the joint as [ -T ]2，-T1]；
Wherein, T1Is the output torque 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 output torque and the actual dragging speed are firstly judged, and then the torque constraint range of the joint is determined according to the directions of the output torque and the actual dragging speed.
Specifically, if the output torque is +100N and the actual dragging speed is +50 °/s, determining the torque constraint range of the joint as [0, 100 ]; if the output torque is-100N and the actual dragging speed is-50 degrees/s, determining the torque constraint range of the joint as [ -100, 0 ]; if the output torque is +100N and the actual dragging speed is-50 degrees/s, determining the torque constraint range of the joint as [100, 150 ]; if the output 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, 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 output torque and the actual dragging speed are firstly judged, then the torque constraint range of the joint is determined according to the directions of the output torque and the actual dragging speed, and then the output torque obtained by the next calculation is adjusted according to the torque constraint range, 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 output torque module 40 is used for calculating and obtaining the output torque 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 output 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 (10)
1. A drag teaching speed limiting method for a mechanical arm joint is characterized by comprising the following steps:
acquiring the actual dragging speed of the joint;
when the actual dragging speed exceeds a preset safe dragging speed;
calculating an 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.
2. The method for speed limitation of robot arm joint according to claim 1, further comprising:
setting a velocity threshold for the joint;
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.
3. The robot arm joint drag teaching speed limit method according to claim 1, further comprising, when the actual drag speed is less than or equal to the safe drag speed:
calculating to obtain the output torque of the joint based on the dynamic model and the actual dragging speed;
determining the torque constraint range of the joint according to the output torque and the current speed direction of the joint;
and performing saturation adjustment on the output torque according to the torque constraint range.
4. The mechanical arm joint drag teaching speed limit method according to claim 3, wherein the determining the torque constraint range of the joint according to the output torque and the current speed direction of the joint comprises:
if the directions of the output torque and the actual dragging speed are the same positive, determining that the torque constraint range of the joint is [0, T ]1]；
If the directions of the output torque and the actual dragging speed are negative, determining that the torque constraint range of the joint is [ -T ]1，0]；
If the direction of the output torque is positive and the direction of the actual dragging speed is negative, determining that the torque constraint range of the joint is [ T ]1，T2]；
If the direction of the output torque is negative and the direction of the actual dragging speed is positive, determining that the torque constraint range of the joint is [ -T ]2，-T1]；
Wherein, T is1Is the output torque of the joint, T2For rated torque of the joint motor, T1Less than T2And said T is1And T2Both positive values, the positive and negative directions of the moment and the speed are referenced to the zero point of the joint.
5. The mechanical arm joint drag teaching speed limit method according to claim 4, wherein the performing saturation adjustment on the output torque according to the torque constraint range comprises:
when the joint is inThe torque constraint range is [0, T1]And the direction of the output torque is positive: if the output torque is larger than T1Adjusting the output torque 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 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-T1Adjusting the output torque 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 larger than T1And is less than T2The output torque does not need to be adjusted; if the output torque is less than T1Adjusting the output torque to T1(ii) a If the output torque is larger than T2Adjusting the output torque to T2；
When the torque constraint range of the joint is [ -T ]2，-T1]And when 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-T2Adjusting the output torque to-T2(ii) a If the output torque is greater than-T1Adjusting the output torque to-T1。
6. The utility model provides a mechanical arm joint's dragging teaching speed limiting device which characterized in that includes:
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 a joint motor through a 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.
7. The robot joint drag teaching speed limit device according to claim 6, further comprising:
the speed setting module is used for setting a speed threshold value 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.
8. The robot joint drag teaching speed limit device according to claim 6, further comprising:
the output torque module is used for calculating to obtain the output torque 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 output 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.
9. An electronic device, comprising:
a memory for storing a computer program;
a processor for implementing the drag teaching speed limit method for a robot joint according to any one of claims 1 to 5 when the computer program is executed.
10. A medium storing a computer program which, when executed by a processor, implements the drag teaching speed limit method for a robot joint according to any one of claims 1 to 5.
Priority Applications (3)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| 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 |
| 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 |
| CN202111568383.9A CN114260878A (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 (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| 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 Child Applications (2)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111568383.9A Division CN114260878A (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 Division CN114260877A (en) | 2021-07-06 | 2021-07-06 | Method and device for dragging teaching speed limit of mechanical arm joint, electronic equipment and medium |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113246107A true CN113246107A (en) | 2021-08-13 |
| CN113246107B CN113246107B (en) | 2022-01-11 |
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 After (2)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| 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 |
Country Status (1)
| Country | Link |
|---|---|
| CN (3) | CN113246107B (en) |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN1839019A (en) * | 2003-10-10 | 2006-09-27 | 三菱电机株式会社 | Robot controlling device |
| CN101570018A (en) * | 2008-04-28 | 2009-11-04 | 株式会社神户制钢所 | Robot teaching method |
| CN108582069A (en) * | 2018-04-17 | 2018-09-28 | 上海达野智能科技有限公司 | Robot drags teaching system and method, storage medium, operating system |
| US20180361591A1 (en) * | 2017-06-19 | 2018-12-20 | Fanuc Corporation | Robot system that displays speed |
| CN111152225A (en) * | 2020-01-15 | 2020-05-15 | 北京科技大学 | Uncertain mechanical arm fixed time trajectory tracking control method with input saturation |
| CN111478628A (en) * | 2019-01-07 | 2020-07-31 | 东元电机股份有限公司 | Torque mode motor driving device |
Family Cites Families (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| EP0850730B1 (en) * | 1995-09-14 | 2002-07-24 | Kabushiki Kaisha Yaskawa Denki | Teaching unit for robots |
| KR101214685B1 (en) * | 2011-03-23 | 2013-01-09 | 고려대학교 산학협력단 | Method for controlling robot manipulator device based on frequency analysys of impact on the device |
| JP6128767B2 (en) * | 2012-07-05 | 2017-05-17 | キヤノン株式会社 | Robot control apparatus and robot control method |
| 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 |
| CN109397265B (en) * | 2018-11-13 | 2020-10-16 | 华中科技大学 | Joint type industrial robot dragging teaching method based on dynamic model |
| CN112571420B (en) * | 2020-12-21 | 2022-08-23 | 西北工业大学 | Dual-function model prediction control method under unknown parameters |
| CN112894821B (en) * | 2021-01-30 | 2022-06-28 | 同济大学 | Current method based collaborative robot dragging teaching control method, device and equipment |
-
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 (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN1839019A (en) * | 2003-10-10 | 2006-09-27 | 三菱电机株式会社 | Robot controlling device |
| CN101570018A (en) * | 2008-04-28 | 2009-11-04 | 株式会社神户制钢所 | Robot teaching method |
| US20180361591A1 (en) * | 2017-06-19 | 2018-12-20 | Fanuc Corporation | Robot system that displays speed |
| CN108582069A (en) * | 2018-04-17 | 2018-09-28 | 上海达野智能科技有限公司 | Robot drags teaching system and method, storage medium, operating system |
| CN111478628A (en) * | 2019-01-07 | 2020-07-31 | 东元电机股份有限公司 | Torque mode motor driving device |
| CN111152225A (en) * | 2020-01-15 | 2020-05-15 | 北京科技大学 | Uncertain mechanical arm fixed time trajectory tracking control method with input saturation |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113246107B (en) | 2022-01-11 |
| CN114260877A (en) | 2022-04-01 |
| 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 | |
| Antonelli et al. | A new on-line algorithm for inverse kinematics of robot manipulators ensuring path tracking capability under joint limits | |
| JP3442941B2 (en) | Robot vibration suppression control device and control method thereof | |
| CN110605721A (en) | Mechanical arm dragging teaching method based on terminal six-dimensional force sensor | |
| CN102189550A (en) | Robot having learning control function | |
| CN109746913B (en) | Method and system for teaching robot posture keeping dragging | |
| CN110977990A (en) | Mechanical arm dragging teaching method based on terminal six-dimensional force sensor | |
| CN113246107B (en) | Method and device for dragging teaching speed limit of mechanical arm joint, electronic equipment and medium | |
| CN113001069B (en) | Welding seam tracking method of six-joint robot | |
| CN112847373B (en) | Robot track synchronous control method and computer readable storage medium | |
| JP3698770B2 (en) | Load weight estimation method | |
| JP2006281330A (en) | Robot simulation device | |
| TWI594858B (en) | Robotic arm teaching system | |
| Campbell et al. | Superpositioning of behaviors learned through teleoperation | |
| CN111152231A (en) | Robot teaching method based on force control | |
| JP4134369B2 (en) | Robot control device | |
| JP2645866B2 (en) | Manipulator control method and device | |
| JPS618236A (en) | Moving body control unit | |
| JPH08190433A (en) | Load weight estimating method | |
| JP2019214105A (en) | Robot control apparatus and robot control method | |
| JP2718687B2 (en) | Control device for position and force of multi-degree-of-freedom work machine | |
| US20230302643A1 (en) | Systems and Hybrid Position Force Control Processes of an Industrial Robot | |
| WO2022220217A1 (en) | Robot system, and control method and control program thereof |
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 | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |