CN113246107B - 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 PDF

Info

Publication number
CN113246107B
CN113246107B CN202110759410.4A CN202110759410A CN113246107B CN 113246107 B CN113246107 B CN 113246107B CN 202110759410 A CN202110759410 A CN 202110759410A CN 113246107 B CN113246107 B CN 113246107B
Authority
CN
China
Prior art keywords
joint
speed
output torque
dragging
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.)
Active
Application number
CN202110759410.4A
Other languages
Chinese (zh)
Other versions
CN113246107A (en
Inventor
姜宇
黄睿
郎需林
林炯辉
刘主福
刘培超
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Shenzhen Yuejiang Technology Co Ltd
Original Assignee
Shenzhen Yuejiang Technology Co Ltd
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Shenzhen Yuejiang Technology Co Ltd filed Critical Shenzhen Yuejiang Technology Co Ltd
Priority to CN202111567730.6A priority Critical patent/CN114260877A/en
Priority to CN202110759410.4A priority patent/CN113246107B/en
Priority to CN202111568383.9A priority patent/CN114260878A/en
Publication of CN113246107A publication Critical patent/CN113246107A/en
Application granted granted Critical
Publication of CN113246107B publication Critical patent/CN113246107B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • Artificial Intelligence (AREA)
  • Evolutionary Computation (AREA)
  • Fuzzy Systems (AREA)
  • Mathematical Physics (AREA)
  • Software Systems (AREA)
  • Manipulator (AREA)

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

Method and device for dragging teaching speed limit of mechanical arm joint, electronic equipment and medium
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, 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]；
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 theoretical moment of the joint based on the dynamic model and the actual dragging speed;
determining a moment constraint range of the joint according to the theoretical moment and the current speed direction of the joint;
and performing saturation adjustment on the output torque of the joint according to the torque constraint range.
4. The method for speed limitation of drag teaching of mechanical arm joint according to claim 3, wherein said determining the torque constraint range of said joint according to said theoretical torque and the current speed direction of said joint comprises:
if the directions of the theoretical moment and the actual dragging speed are the same positive, 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, determining that the moment constraint range of the joint is [ -T ]1，0]；
If the direction of the theoretical moment is positive and the direction of the actual dragging speed is negative, determining that the moment constraint range of the joint is [ T ]1，T2]；
If the direction of the theoretical moment is negative and the direction of the actual dragging speed is positive, determining that the moment constraint range of the joint is [ -T ]2，-T1]；
Wherein, T is1Is the theoretical moment of the joint, the 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 method for speed limitation of drag teaching of mechanical arm joint according to claim 4, wherein the saturation adjustment of the output torque of the joint according to the torque constraint range comprises:
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 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 theoretical 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 theoretical moment of the joint and the current speed direction of the joint;
and the moment adjusting module is used for performing saturation adjustment on the output moment of the joint according to the moment 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 storage 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.
CN202110759410.4A 2021-07-06 2021-07-06 Method and device for dragging teaching speed limit of mechanical arm joint, electronic equipment and medium Active CN113246107B (en)

Priority Applications (3)

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
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
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
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

Publications (2)

Publication Number Publication Date
CN113246107A CN113246107A (en) 2021-08-13
CN113246107B true CN113246107B (en) 2022-01-11

Family

ID=77190684

Family Applications (3)

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
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
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
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

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) CN114260877A (en)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114260877A (en) * 2021-07-06 2022-04-01 深圳市越疆科技有限公司 Method and device for dragging teaching speed limit of mechanical arm joint, electronic equipment and medium
CN116512225B (en) * 2023-06-16 2024-10-25 北京敏锐达致机器人科技有限责任公司 Robot dragging teaching method and device, electronic equipment and readable storage medium

Family Cites Families (18)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
DE69622572T2 (en) * 1995-09-14 2003-04-03 Kabushiki Kaisha Yaskawa Denki, Kitakyushu TEACHING DEVICE FOR ROBOTS
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
JP2011206886A (en) * 2010-03-30 2011-10-20 Yaskawa Electric Corp Device and method for controlling robot
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
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
CN108582069A (en) * 2018-04-17 2018-09-28 上海达野智能科技有限公司 Robot drags teaching system and method, storage medium, operating system
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
CN111478628B (en) * 2019-01-07 2021-08-31 东元电机股份有限公司 Torque mode motor driving device
CN111152225B (en) * 2020-01-15 2021-04-06 北京科技大学 Uncertain mechanical arm fixed time trajectory tracking control method with input saturation
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
CN114260877A (en) * 2021-07-06 2022-04-01 深圳市越疆科技有限公司 Method and device for dragging teaching speed limit of mechanical arm joint, electronic equipment and medium

Also Published As

Publication number Publication date
CN114260877A (en) 2022-04-01
CN113246107A (en) 2021-08-13
CN114260878A (en) 2022-04-01

Similar Documents

Publication Publication Date Title
Safeea et al. Kuka sunrise toolbox: Interfacing collaborative robots with matlab
CN113246107B (en) Method and device for dragging teaching speed limit of mechanical arm joint, electronic equipment and medium
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
CN105717869B (en) Industrial robot operating space path Mixed Circumscription method for solving
CN112847373B (en) Robot track synchronous control method and computer readable storage medium
CN114355771A (en) Cooperative robot force and position hybrid control method and system
JP2006281330A (en) Robot simulation device
TWI594858B (en) Robotic arm teaching system
Campbell et al. Superpositioning of behaviors learned through teleoperation
JPH0991004A (en) Method for estimating load weight
CN115008468B (en) Mechanical arm attitude speed planning control method
CN111152231A (en) Robot teaching method based on force control
JP4134369B2 (en) Robot control device
JPH08190433A (en) Load weight estimating method
JP2645866B2 (en) Manipulator control method and device
JPS618236A (en) Moving body control unit
Yu et al. Research on manipulator motion control system based on preset performance control algorithm
JP2019214105A (en) Robot control apparatus and robot control method

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