CN114227638B - Master-slave mapping method and correction method for robot operating lever and robot - Google Patents
Master-slave mapping method and correction method for robot operating lever and robot Download PDFInfo
- Publication number
- CN114227638B CN114227638B CN202111438196.9A CN202111438196A CN114227638B CN 114227638 B CN114227638 B CN 114227638B CN 202111438196 A CN202111438196 A CN 202111438196A CN 114227638 B CN114227638 B CN 114227638B
- Authority
- CN
- China
- Prior art keywords
- encoder
- angle
- joint
- linear motor
- robot
- 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
Links
- 238000013507 mapping Methods 0.000 title claims abstract description 70
- 238000000034 method Methods 0.000 title claims abstract description 34
- 238000009954 braiding Methods 0.000 claims 1
- 230000009286 beneficial effect Effects 0.000 abstract description 3
- 238000006073 displacement reaction Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000006467 substitution reaction Methods 0.000 description 2
- 230000001360 synchronised effect Effects 0.000 description 2
- 238000001356 surgical procedure Methods 0.000 description 1
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J3/00—Manipulators of master-slave type, i.e. both controlling unit and controlled unit perform corresponding spatial movements
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J13/00—Controls for manipulators
- B25J13/02—Hand grip control means
Abstract
The invention belongs to the technical field of master-slave control of robots, and discloses a master-slave mapping method and a correction method of a robot operating rod and a robot. The beneficial effects are that: the principle of the control robot of the operating lever is convenient for operators to learn and understand, the robot is operated through the operating lever, the robot operating lever can be guaranteed to accurately control the robot, the phenomenon that the robot moves inaccurately due to the fact that the robot operating lever and the robot operating lever are asynchronous is avoided, and the preset precise operation cannot be completed is avoided.
Description
Technical Field
The invention relates to the technical field of master-slave control of robots, in particular to a master-slave mapping method and a correction method of a robot operating lever and a robot.
Background
The mapping relationship between master and slave is a core problem in the field of master-slave control of robots. In a master-slave robot, a corresponding control method is designed according to the mapping relation to realize complete control of a master hand to a slave hand. In the existing master-slave mapping technology, corresponding mapping modes are formulated according to the structural configuration of master-slave hands, namely incremental mapping and absolute mapping are respectively carried out, wherein the variable quantity of the respective degrees of freedom of the master hands is converted into the control quantity of the slave hands to realize control, and the absolute position tracking of the respective degrees of freedom of the slave hands to the master hands is realized.
The existing mapping method is difficult for an operator to operate when controlling the robot, and is difficult to learn the movement of the robot corresponding to the operation rod. And when the operating lever is initialized, the state of the operating lever is inconsistent with the state of the robot possibly caused by unexpected situations, namely when the angle deviation exists, the position of the operating lever is required to be corrected and synchronized so as to ensure the accuracy of the operating lever to the control of the robot.
Disclosure of Invention
The purpose of the invention is that: the master-slave mapping method and the correction method for the robot operating rod and the robot are provided, so that an operator can operate the robot conveniently, and correction of the robot operating rod and the robot is realized.
In order to achieve the above object, the present invention provides a master-slave mapping method of a robot lever and a robot, including: a robot lever and a robot; the robot operating lever comprises a first operating shaft, a second operating shaft, a third operating shaft, a first encoder, a second encoder and a third encoder, and each operating shaft corresponds to one encoder; the robot includes first joint, second joint and terminal slide rail joint that connect gradually, first joint includes: a first linear motor and a second linear motor, the second joint comprising a third linear motor and a fourth linear motor, the end rail joint comprising a fifth linear motor, one for each operating shaft; when the operation shaft rotates, an encoder corresponding to the operation shaft acquires a rotation angle of the operation shaft and controls an articulation corresponding to the operation shaft.
Further, when the operation shaft rotates, the encoder corresponding to the operation shaft obtains the rotation angle of the operation shaft and controls the joint movement corresponding to the operation shaft, specifically:
controlling articulation according to a predefined mapping relationship between the rotation angle of the encoder and articulation;
the mapping parameter of the first encoder and the first joint angle is delta theta 1 The delta theta 1 A ratio of the first joint angle to the first encoder reading;
the mapping parameter of the second encoder and the second joint angle is delta theta 2 The delta theta 2 For a second joint angle and a second codeA ratio of readings of the device;
the mapping parameter of the third encoder and the fifth linear motor of the end sliding rail joint is delta Z, and delta Z is the ratio of the position of the end sliding rail joint to the reading of the third encoder.
The invention also discloses a robot operating rod and a correction method of the robot, comprising the following steps:
when the robot lever and the robot are mapped by applying the master-slave mapping method, the robot lever and the robot are corrected by the steps comprising:
acquiring the position of each linear motor, and acquiring a first angle formed by a first joint according to the position of the first linear motor, the position of the second linear motor and the vertical distance between the first linear motor and the second linear motor; acquiring a second angle formed by a second joint according to the position of the third linear motor, the position of the fourth linear motor and the vertical distance between the third linear motor and the fourth linear motor;
obtaining a first initial angle of the first encoder according to the first angle, a pre-stored initial reading of the first encoder at the initial position of the first joint and a pre-stored mapping relation between the first encoder and the first joint angle; obtaining a second initial angle of the second encoder according to the second angle, a pre-stored mapping relation between the initial reading of the second encoder and the second joint angle when the second joint is at the initial position; obtaining an initial value of the third encoder according to the position of the fifth linear motor, a pre-stored initial position of the fifth linear motor, a pre-stored mapping relation between the third encoder and the position of the fifth linear motor and a pre-stored initial reading of the third encoder when the initial position of the tail end sliding rail joint is stored;
and controlling the robot operating rod to rotate and acquiring real-time values of a first real-time angle of the first encoder, a second real-time angle of the second encoder and a third encoder in real time, and completing correction of the robot operating rod and the robot when a difference value between the first real-time angle and the first initial angle is smaller than or equal to a first deviation range, a difference value between the second real-time angle and the second initial angle is smaller than or equal to a second deviation range, and a real-time value of the third encoder and an initial value of the third encoder are smaller than a third deviation range.
Further, the first angle formed by the first joint is obtained according to the position of the first linear motor, the position of the second linear motor and the vertical distance between the first linear motor and the second linear motor, specifically:
obtaining a first angle according to a first formula, wherein the first formula specifically comprises:
wherein θ slave_1 For the first angle, L1 is the position of the first linear motor, L2 is the position of the second linear motor, dm1 is the vertical distance between the first joint and the second joint.
Further, the second angle formed by the second joint is obtained according to the position of the third linear motor, the position of the fourth linear motor and the vertical distance between the third linear motor and the fourth linear motor, specifically:
obtaining a second angle according to a second formula, wherein the second formula specifically comprises:
wherein θ slave_1 For the second angle, L3 is the position of the third linear motor, L4 is the position of the fourth linear motor, dm2 is the vertical distance between the second joint and the end rail joint.
Further, the first initial angle of the first encoder is obtained according to the first angle, the pre-stored initial reading of the first encoder at the initial position of the first joint, and the pre-stored mapping parameters of the first encoder and the first joint angle, specifically:
obtaining a first initial angle according to a third formula, wherein the third formula specifically comprises:
wherein θ init_1 For a first initial angle, θ slave_1 At a first angle of delta theta 1 B for pre-stored mapping parameters of the first encoder and the first joint angle 1 Initial readings of the first encoder when the initial position of the first joint is pre-stored; Δθ 1 In particular toθ stick_1 For the last corrected reading of the first encoder, θ slave_1 The first joint angle of the first joint of the robot, which is corrected last time.
Further, the second initial angle of the second encoder is obtained according to the second angle, the pre-stored initial reading of the second encoder at the initial position of the second joint, and the mapping parameter of the second joint angle, specifically:
obtaining a second initial angle according to a fourth formula, wherein the fourth formula specifically comprises:
wherein θ init_2 For a second initial angle, θ slave_2 At a second angle of delta theta 1 B) mapping parameters for a pre-stored second encoder and second joint angle 1 Initial readings of the second encoder when the initial positions of the second joints are prestored; Δθ 2 In particular toθ slave_2 For the last correction reading of the second encoder, θ slave_2 And the second joint angle of the second joint of the robot which is corrected last time.
Further, the initial value of the third encoder is obtained according to the position of the fifth linear motor, the pre-stored initial position of the fifth linear motor, the pre-stored mapping parameters of the third encoder and the position of the fifth linear motor, and the pre-stored initial reading of the third encoder when the initial position of the end sliding rail joint is pre-stored, specifically:
obtaining an initial value of the third encoder according to a fifth formula, wherein the fifth formula specifically comprises:
wherein the Z is init L5 is the position of the fifth linear motor, b mid B, pre-storing a mapping relation between the position of the third encoder and the position of the fifth linear motor, which is pre-stored as an initial position of the fifth linear motor, delta Z z Pre-storing initial readings of a third encoder when the initial position of the tail end sliding rail joint is pre-stored; ΔZ is specificallyZ stick For the reading of the third encoder at the last correction.
Further, the Z stick ω (r+ (NR)), where ω represents a step speed index, N is the number of turns maintained during the reading, R is the range of readings for each revolution of the encoder, r=xmod R, and when the third operating axis is moving, R is modulo if the reading x of the third encoder exceeds R.
Further, when the difference value between the first real-time angle and the first initial angle is smaller than or equal to a first deviation range, the difference value between the second real-time angle and the second initial angle is smaller than or equal to a second deviation range, and the real-time value of the third encoder and the initial value of the third encoder are smaller than the third deviation range, the correction of the robot operating lever and the robot is completed, specifically:
judging whether the difference value between the first real-time angle and the first initial angle is smaller than or equal to a first deviation range, whether the difference value between the second real-time angle and the second initial angle is smaller than or equal to a second deviation range, and whether the real-time value of the third encoder and the initial value of the third encoder are smaller than or equal to a third deviation range according to the first inequality, the second inequality and the third inequality;
the first inequality is specifically: theta (theta) init_1 -θ tmp_1 |≤t θ1 Wherein θ init _ 1 For a first initial angle, θ tmp_1 For a first real-time angle, t θ1 Is a first deviation range;
the second inequality is specifically: theta (theta) init_2 -θ tmp_2 |≤t θ2 Wherein θ init _ 2 For a first initial angle, θ tmp_2 At a second real-time angle t θ2 Is a second deviation range;
the third inequality is specifically: z init -Z tmp |≤t Z Wherein Z is init Initial value of third encoder, Z tmp Is the real-time value of the third encoder, t Z Is a third deviation range;
when the first inequality, the second inequality, and the third inequality are all satisfied, the correction of the robot lever and the robot is completed.
Compared with the prior art, the robot operating lever and the robot correction method have the beneficial effects that: according to the master-slave mapping method, an operator can easily understand the actions of the robot corresponding to the operating rod, and is convenient for the operator to understand and learn to control the robot through the operating rod. The correction method can ensure the accurate control of the robot by the robot operating rod, and avoid inaccurate movement of the robot caused by the asynchronous movement of the robot operating rod and the robot operating rod, so that the preset precise operation can not be finished.
Drawings
FIG. 1 is a schematic view of the structure of a robotic lever of the present invention;
fig. 2 is a schematic structural view of the robot of the present invention.
In the figure, 1, a first encoder; 2. a second encoder; 3. a third encoder; 4. a first operation shaft; 5. a second operation shaft; 6. a third operating shaft 7, a first linear motor; 8. a second linear motor; 9. a third linear motor; a fourth linear motor; 11. a fifth linear motor; 12. a first joint; 13. a second joint; 14. and the tail end sliding rail joint.
Detailed Description
The following describes in further detail the embodiments of the present invention with reference to the drawings and examples. The following examples are illustrative of the invention and are not intended to limit the scope of the invention.
The invention discloses a master-slave mapping method of a robot operating lever and a robot, which comprises the following steps: a robot lever and a robot; the robot operating lever comprises a first operating shaft 4, a second operating shaft 5, a third operating shaft 6, a first encoder 1, a second encoder 2 and a third encoder 3, wherein each operating shaft corresponds to one encoder; the robot includes first joint 12, second joint 13 and terminal slide rail joint that connects gradually, first joint 12 includes: a first linear motor 7 and a second linear motor 8, the second joint 13 comprising a third linear motor 9 and a fourth linear motor 10, the end rail joint comprising a fifth linear motor 11, one for each operating axis; when the operation shaft rotates, an encoder corresponding to the operation shaft acquires a rotation angle of the operation shaft and controls an articulation corresponding to the operation shaft.
In this embodiment, when the operation shaft rotates, the encoder corresponding to the operation shaft acquires the rotation angle of the operation shaft and controls the joint movement corresponding to the operation shaft, specifically:
controlling articulation according to a predefined mapping relationship between the rotation angle of the encoder and articulation;
the mapping parameter of the first encoder 1 and the first joint angle is delta theta 1 The delta theta 1 Is the ratio of the first joint angle to the first encoder 1 reading;
the mapping parameter of the second encoder 2 and the second joint angle is delta theta 2 The delta theta 2 Is the ratio of the angle of the second joint 13 to the reading of the second encoder 2;
the mapping parameter of the third encoder 3 and the fifth linear motor 11 of the end rail joint 14 is Δz, which is the ratio of the position of the end rail joint and the reading of the third encoder 3.
Referring to fig. 1 and 2, the present invention discloses a robot lever that can be applied to control an ophthalmic surgical machine, and a robot that is a robot arm that can be used to perform an ophthalmic surgery.
Referring to fig. 2, the robot is composed of a first joint 12, a second joint 13, and a terminal rail joint, the first joint 12 is provided with a first linear motor 7 and a second linear motor 8, the second joint 13 is provided with a third linear motor 9 and a fourth linear motor 10, and the terminal rail joint is provided with a fifth linear motor 11. The robot operating lever comprises a first operating shaft 4, a second operating shaft 5 and a third operating shaft 6; each shaft end point is provided with a rotary encoder for reading a control input, and for the first operation shaft 4, the control input acquired by the first encoder 1 corresponds to the rotary motion of the first joint 12 of the robot; for the second operating shaft 5, the control input obtained by the second encoder 2 corresponds to the rotational movement of the second joint 13 of the robot; for the third operation shaft 6, the control input acquired by the third encoder 3 corresponds to the translational movement of the robot end sliding rail joint.
The mapping parameters of the operating axis, encoder and joint are defined as follows:
mapping parameters of the first joint 12 and the second joint 13:
recording readings theta of the first and second operation shafts 4 and 5 of the current operation rod corresponding to the first and second encoders 1 and 2 stick_1 、θ stick_2 。
Defining the mapping parameter Δθ 1 、Δθ 2 The readings theta of the first shaft and the second shaft of the operating rod corresponding to the first encoder 1 and the second encoder 2 are respectively shown stick_1 、θ stick_2 Angle theta with first joint of robot slave_1 Second joint angle theta slave_2 Is a mapping relation of (a) to (b).
The first shaft and the second shaft of the operating rod correspond to the readings theta of the first encoder 1 and the second encoder 2 according to the real-time reading stick_1 、θ stick_2 Finish the first joint angle theta of the robot slave_1 Second joint angle theta slave_2 Mapping of (3) specific mapping relationThe method comprises the following steps:
mapping parameters of the distal sled joint 14:
record the reading Z of the third encoder 3 corresponding to the third axis of the current operating lever stick 。
Defining a mapping parameter DeltaZ representing the reading Z of the third shaft of the lever corresponding to the third encoder 3 stick And the mapping relation with the joint displacement L5 of the tail end sliding rail of the robot.
The number of turns statistic N is maintained during the reading. The reading range of each rotation of the rotary encoder is recorded as follows: [0, R ], then when the lever is pushed in the z direction, if the reading x exceeds R, then R is modulo, i.e. the reading r=xmod R is actually updated.
The updating method of N is as follows:
wherein r is tmp A value r representing the current third axis of the lever corresponding to the third encoder 3 pre Indicating the value of the third encoder 3 corresponding to the third axis of the operating lever at the last reading, t r E (0, r) is the turn update threshold.
The third axis of the operating lever corresponds to the reading Z of the third encoder 3 according to the real-time reading stick The mapping of the displacement of the sliding rail joint at the tail end of the robot is completed, and the specific mapping relation is as follows:
where ω represents the step speed index.
The invention also discloses a robot operating rod and a correction method of the robot, comprising the following steps:
when the robot lever and the robot are mapped by applying the master-slave mapping method, the robot lever and the robot are corrected by the steps comprising:
acquiring the position of each linear motor, and acquiring a first angle formed by a first joint 12 according to the position of a first linear motor 7, the position of a second linear motor 8 and the vertical distance between the first linear motor 7 and the second linear motor 8; acquiring a second angle made by the second joint 13 according to the position of the third linear motor 9, the position of the fourth linear motor 10 and the vertical distance between the third linear motor 9 and the fourth linear motor 10;
obtaining a first initial angle of the first encoder 1 according to the first angle, a pre-stored initial reading of the first encoder 1 at the initial position of the first joint 12 and a pre-stored mapping relation between the first encoder 1 and the first joint 12; obtaining a second initial angle of the second encoder 2 according to the second angle, a pre-stored mapping relation between the initial reading of the second encoder 2 and the angle of the second joint 13 when the initial position of the second joint 13 is stored; obtaining an initial value of the third encoder 3 according to the position of the fifth linear motor 11, the pre-stored initial position of the fifth linear motor 11, the pre-stored mapping parameters of the positions of the third encoder 3 and the fifth linear motor 11 and the pre-stored initial reading of the third encoder 3 at the initial position of the end sliding rail joint 14;
and controlling the robot operating rod to rotate and acquiring real-time values of a first real-time angle of the first encoder 1, a second real-time angle of the second encoder 2 and a third encoder 3 in real time, and completing correction of the robot operating rod and the robot when a difference value between the first real-time angle and the first initial angle is smaller than or equal to a first deviation range, a difference value between the second real-time angle and the second initial angle is smaller than or equal to a second deviation range, and a real-time value of the third encoder 3 and an initial value of the third encoder 3 are smaller than a third deviation range.
In this embodiment, when the operation lever is initialized, or when the state of the operation lever is inconsistent with the state of the robot due to an unexpected situation, that is, there is an angular deviation, the position of the operation lever needs to be corrected and synchronized.
In the present embodiment, the state of the robot is obtained as l= { L1, L2, L3, L4, L5}, where L1, L2, L3, L4, L5 represent the first linear motor 7, the second linear motor 8 position on the first joint 12, the third linear motor 9, the fourth linear motor 10 position on the second joint 13, and the fifth linear motor 11 position on the end rail joint, respectively.
In this embodiment, the first angle formed by the first joint 12 is obtained according to the position of the first linear motor 7, the position of the second linear motor 8, and the vertical distance between the first linear motor 7 and the second linear motor 8, specifically:
obtaining a first angle according to a first formula, wherein the first formula specifically comprises:
wherein θ slave_1 For the first angle, L1 is the position of the first linear motor 7, L2 is the position of the second linear motor 8, dm1 is the vertical distance between the first joint 12 and the second joint 13.
In this embodiment, the second angle formed by the second joint 13 is obtained according to the position of the third linear motor 9, the position of the fourth linear motor 10, and the vertical distance between the third linear motor 9 and the fourth linear motor 10, specifically:
obtaining a second angle according to a second formula, wherein the second formula specifically comprises:
wherein θ slave_2 For the second angle, L3 is the position of the third linear motor 9, L4 is the position of the fourth linear motor 10, dm2 is the vertical distance between the second joint 13 and the end rail joint 14.
In this embodiment, the first initial angle of the first encoder 1 is obtained according to the first angle, the pre-stored initial reading of the first encoder 1 at the initial position of the first joint 12, and the pre-stored mapping parameters of the first encoder 1 and the first joint 12 angle, which specifically is:
obtaining a first initial angle according to a third formula, wherein the third formula specifically comprises:
wherein θ init_1 For a first initial angle, θ slave_1 At a first angle of delta theta 1 B for pre-stored mapping parameters of the first encoder 1 and the first joint angle 1 Initial readings of the first encoder 1 at the initial position of the first joint 12 pre-stored; Δθ 1 In particular toθ stick_1 For the last correction reading of the first encoder 1, θ slave_1 The first joint angle of the first joint 12 of the robot, which was corrected last time.
In this embodiment, the second initial angle of the second encoder 2 is obtained according to the second angle, the pre-stored initial reading of the second encoder 2 at the initial position of the second joint 13, and the mapping parameter of the second joint angle, which specifically is:
obtaining a second initial angle according to a fourth formula, wherein the fourth formula specifically comprises:
wherein θ init_2 For a second initial angle, θ slave_2 At a second angle of delta theta 1 B for pre-stored mapping parameters of the second encoder 2 and the second joint angle 1 Initial readings of the second encoder 2 at the initial position of the second joint 13 pre-stored; Δθ 2 In particular toθ slave_2 For the last correction reading of the second encoder 2, θ slave_2 The second joint angle of the robot second joint 13, which was corrected last time.
In this embodiment, the initial value of the third encoder 3 is obtained according to the position of the fifth linear motor 11, the pre-stored initial position of the fifth linear motor 11, the pre-stored mapping parameters of the positions of the third encoder 3 and the fifth linear motor 11, and the pre-stored initial reading of the third encoder 3 at the initial position of the end rail joint 14, specifically:
the initial value of the third encoder 3 is obtained according to a fifth formula, which is specifically:
wherein the Z is init L5 is the position of the fifth linear motor 11, b, which is the initial value of the third encoder 3 mid A pre-stored mapping relationship between the positions of the third encoder 3 and the fifth linear motor 11, b, Δz, for the pre-stored initial position of the fifth linear motor 11 z Pre-stored initial readings of the third encoder 3 at the initial position of the end rail joint 14; ΔZ is specificallyZ stick For the reading of the third encoder 3 at the last correction.
In the present embodiment, the Z stick ω (r+ (NR)), where ω represents a step speed index, N is the number of turns maintained during the reading, R is the range of readings for each revolution of the encoder, r=xmod R, and when the third operating shaft 6 is moving, R is modulo if the reading x of the third encoder 3 exceeds R.
In this embodiment, the updating method of N is as follows:
wherein r is tmp A value r representing the current third axis of the lever corresponding to the third encoder 3 pre Indicating the value of the third encoder 3 corresponding to the third axis of the operating lever at the last reading, t r E (0, r) is the turn update threshold.
In this embodiment, when the difference between the first real-time angle and the first initial angle is less than or equal to the first deviation range, the difference between the second real-time angle and the second initial angle is less than or equal to the second deviation range, and the real-time value of the third encoder 3 and the initial value of the third encoder 3 are less than or equal to the third deviation range, the correction of the robot lever and the robot is completed, specifically:
judging whether the difference value between the first real-time angle and the first initial angle is smaller than or equal to a first deviation range, whether the difference value between the second real-time angle and the second initial angle is smaller than or equal to a second deviation range, and whether the real-time value of the third encoder 3 and the initial value of the third encoder 3 are smaller than or equal to a third deviation range according to the first inequality, the second inequality and the third inequality;
the first inequality is specifically: theta (theta) init_1 -θ tmp_1 |≤t θ1 Wherein θ init_1 For a first initial angle, θ tmp_1 For a first real-time angle, t θ1 Is a first deviation range;
the second inequality is specifically: theta (theta) init_2 -θ tmp_2 |≤t θ2 Wherein θ init_2 For a first initial angle, θ tmp_2 At a second real-time angle t θ2 Is a second deviation range;
the third inequality is specifically: z init -Z tmp |≤t Z Wherein Z is init Initial value, Z, of the third encoder 3 tmp Is the real-time value, t, of the third encoder 3 Z Is a third deviation range;
when the first inequality, the second inequality, and the third inequality are all satisfied, the correction of the robot lever and the robot is completed.
In summary, compared with the prior art, the robot operating lever and the robot correction method have the beneficial effects that: according to the master-slave mapping method, an operator can easily understand the actions of the robot corresponding to the operating rod, and is convenient for the operator to understand and learn to control the robot through the operating rod. The correction method can ensure the accurate control of the robot by the robot operating rod, and avoid inaccurate movement of the robot caused by the asynchronous movement of the robot operating rod and the robot operating rod, so that the preset precise operation can not be finished.
The foregoing is merely a preferred embodiment of the present invention, and it should be noted that modifications and substitutions can be made by those skilled in the art without departing from the technical principles of the present invention, and these modifications and substitutions should also be considered as being within the scope of the present invention.
Claims (5)
1. A master-slave mapping method of a robot lever and a robot, comprising: a robot lever and a robot; the robot operating lever comprises a first operating shaft, a second operating shaft, a third operating shaft, a first encoder, a second encoder and a third encoder, and each operating shaft corresponds to one encoder; the robot includes first joint, second joint and terminal slide rail joint that connect gradually, first joint includes: a first linear motor and a second linear motor, the second joint comprising a third linear motor and a fourth linear motor, the end rail joint comprising a fifth linear motor, one for each operating shaft; when the operation shaft rotates, an encoder corresponding to the operation shaft acquires a rotation angle of the operation shaft and controls an articulation corresponding to the operation shaft;
when the operation shaft rotates, the encoder corresponding to the operation shaft acquires the rotation angle of the operation shaft and controls the joint movement corresponding to the operation shaft, specifically:
controlling articulation according to a predefined mapping relationship between the rotation angle of the encoder and articulation;
the mapping parameter of the first encoder and the first joint angle is delta theta 1 The delta theta 1 A ratio of the first joint angle to the first encoder reading;
the mapping parameter of the second encoder and the second joint angle is delta theta 2 The delta theta 2 A ratio of the second joint angle to the second encoder reading;
the mapping parameter of the third encoder and the fifth linear motor of the tail end sliding rail joint is delta Z, and delta Z is the ratio of the position of the tail end sliding rail joint to the reading of the third encoder;
obtaining a first initial angle of the first encoder according to the first angle, a pre-stored initial reading of the first encoder at the initial position of the first joint and a pre-stored mapping relation between the first encoder and the first joint angle; obtaining a second initial angle of the second encoder according to the second angle, a pre-stored mapping relation between the initial reading of the second encoder and the second joint angle when the second joint is at the initial position; obtaining an initial value of the third encoder according to the position of the fifth linear motor, a pre-stored initial position of the fifth linear motor, a pre-stored mapping relation between the third encoder and the position of the fifth linear motor and a pre-stored initial reading of the third encoder when the initial position of the tail end sliding rail joint is stored;
the method comprises the steps of obtaining a first initial angle of a first encoder according to the first angle, a pre-stored initial reading of the first encoder when the first joint is at an initial position and a pre-stored mapping parameter of the first encoder and the first joint angle, wherein the first initial angle of the first encoder is specifically as follows:
obtaining a first initial angle according to a third formula, wherein the third formula specifically comprises:
wherein θ init_1 For a first initial angle, θ slave_1 At a first angle of delta theta 1 B for pre-stored mapping parameters of the first encoder and the first joint angle 1 Initial readings of the first encoder when the initial position of the first joint is pre-stored; Δθ 1 In particular toθ stick_1 For the last corrected reading of the first encoder, θ slave_1 A first joint angle of a first joint of the robot, which is corrected last time;
the second initial angle of the second encoder is obtained according to the second angle, the pre-stored initial reading of the second encoder when the initial position of the second joint is the second angle and the mapping parameter of the second joint angle, specifically:
obtaining a second initial angle according to a fourth formula, wherein the fourth formula specifically comprises:
wherein θ init_2 For a second initial angle, θ slave_2 At a second angle of delta theta 2 B) mapping parameters for a pre-stored second encoder and second joint angle 2 Initial readings of the second encoder when the initial positions of the second joints are prestored; Δθ 2 In particular toθ slave_2 For the last correction reading of the second encoder, θ slave_2 A second joint angle of a second joint of the robot, which is corrected last time;
the initial value of the third encoder is obtained according to the position of the fifth linear motor, the pre-stored initial position of the fifth linear motor, the pre-stored mapping parameters of the third encoder and the position of the fifth linear motor, and the pre-stored initial reading of the third encoder when the initial position of the tail end sliding rail joint is obtained, specifically:
obtaining an initial value of the third encoder according to a fifth formula, wherein the fifth formula specifically comprises:
wherein the Z is init L5 is the position of the fifth linear motor, b mid B, pre-storing a mapping relation between the position of the third encoder and the position of the fifth linear motor, which is pre-stored as an initial position of the fifth linear motor, delta Z z Third braiding at pre-stored initial position of tail end sliding rail jointInitial reading of the encoder; ΔZ is specificallyZ stick Reading of the third encoder at the time of the last correction;
the Z is stick ω (r+ (NR)), where ω represents a step speed index, N is the number of turns maintained during the reading, R is the range of readings for each revolution of the encoder, r=xmod R, and when the third operating axis is moving, R is modulo if the reading x of the third encoder exceeds R.
2. A robot lever and a robot correction method are provided, comprising:
when the robot lever and the robot are mapped by applying the master-slave mapping method of claim 1, the robot lever and the robot are corrected by the steps comprising:
acquiring the position of each linear motor, and acquiring a first angle formed by a first joint according to the position of the first linear motor, the position of the second linear motor and the vertical distance between the first linear motor and the second linear motor; acquiring a second angle formed by a second joint according to the position of the third linear motor, the position of the fourth linear motor and the vertical distance between the third linear motor and the fourth linear motor;
and controlling the robot operating rod to rotate and acquiring real-time values of a first real-time angle of the first encoder, a second real-time angle of the second encoder and a third encoder in real time, and completing correction of the robot operating rod and the robot when a difference value between the first real-time angle and the first initial angle is smaller than or equal to a first deviation range, a difference value between the second real-time angle and the second initial angle is smaller than or equal to a second deviation range, and a real-time value of the third encoder and an initial value of the third encoder are smaller than a third deviation range.
3. The method for calibrating a robot lever and a robot according to claim 2, wherein the first angle of the first joint is obtained according to the position of the first linear motor, the position of the second linear motor, and the vertical distance between the first linear motor and the second linear motor, specifically:
obtaining a first angle according to a first formula, wherein the first formula specifically comprises:
wherein θ slave_1 For the first angle, L1 is the position of the first linear motor, L2 is the position of the second linear motor, dm1 is the vertical distance between the first joint and the second joint.
4. The method for calibrating a robot lever and a robot according to claim 2, wherein the second angle of the second joint is obtained according to the position of the third linear motor, the position of the fourth linear motor, and the vertical distance between the third linear motor and the fourth linear motor, specifically:
obtaining a second angle according to a second formula, wherein the second formula specifically comprises:
wherein θ slave_2 For the second angle, L3 is the position of the third linear motor, L4 is the position of the fourth linear motor, dm2 is the vertical distance between the second joint and the end rail joint.
5. The method for calibrating a robot lever and a robot according to claim 2, wherein when the difference between the first real-time angle and the first initial angle is equal to or smaller than a first deviation range, the difference between the second real-time angle and the second initial angle is equal to or smaller than a second deviation range, and the real-time value of the third encoder and the initial value of the third encoder are equal to or smaller than a third deviation range, the calibration of the robot lever and the robot is completed, specifically:
judging whether the difference value between the first real-time angle and the first initial angle is smaller than or equal to a first deviation range, whether the difference value between the second real-time angle and the second initial angle is smaller than or equal to a second deviation range, and whether the real-time value of the third encoder and the initial value of the third encoder are smaller than or equal to a third deviation range according to the first inequality, the second inequality and the third inequality;
the first inequality is specifically: theta (theta) init_1 -θ tmp_1 |≤t θ1 Wherein θ init_1 For a first initial angle, θ tmp_1 For a first real-time angle, t θ1 Is a first deviation range;
the second inequality is specifically: theta (theta) init_2 -θ tmp_2 |≤t θ2 Wherein θ init_2 For a first initial angle, θ tmp_2 At a second real-time angle t θ2 Is a second deviation range;
the third inequality is specifically: z init -Z tmp |≤t Z Wherein Z is init Initial value of third encoder, Z tmp Is the real-time value of the third encoder, t Z Is a third deviation range;
when the first inequality, the second inequality, and the third inequality are all satisfied, the correction of the robot lever and the robot is completed.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111438196.9A CN114227638B (en) | 2021-11-29 | 2021-11-29 | Master-slave mapping method and correction method for robot operating lever and robot |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111438196.9A CN114227638B (en) | 2021-11-29 | 2021-11-29 | Master-slave mapping method and correction method for robot operating lever and robot |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN114227638A CN114227638A (en) | 2022-03-25 |
| CN114227638B true CN114227638B (en) | 2023-11-28 |
Family
ID=80752003
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111438196.9A Active CN114227638B (en) | 2021-11-29 | 2021-11-29 | Master-slave mapping method and correction method for robot operating lever and robot |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114227638B (en) |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN1487675A (en) * | 2002-08-21 | 2004-04-07 | 株式会社Ntt都科摩 | Walking robot remote-control system, apparatus and method |
| CN102141468A (en) * | 2010-12-20 | 2011-08-03 | 中国商用飞机有限责任公司 | Two-freedom-degree joystick-driven experiment device and control method thereof |
| CN102339086A (en) * | 2010-07-27 | 2012-02-01 | 巍世科技有限公司 | Rotating control device having displacement sensor free of movable electrical contact point |
| CN103862465A (en) * | 2014-02-20 | 2014-06-18 | 三一汽车制造有限公司 | Multi-joint mechanical arm coordinate correction method and system |
| CN105116961A (en) * | 2015-07-21 | 2015-12-02 | 东南大学 | Intelligent force feedback handle and control method thereof |
| CN106341025A (en) * | 2016-10-19 | 2017-01-18 | 山东大学 | Two-degree-freedom mixed type stepping motor with spherical structure and application thereof |
| CN208451644U (en) * | 2018-07-13 | 2019-02-01 | 深圳市创客工场科技有限公司 | Teaching machine and its rotary encoder |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP7118628B2 (en) * | 2017-12-01 | 2022-08-16 | キヤノン株式会社 | ROBOT DEVICE AND ROBOT DEVICE CONTROL METHOD |
-
2021
- 2021-11-29 CN CN202111438196.9A patent/CN114227638B/en active Active
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN1487675A (en) * | 2002-08-21 | 2004-04-07 | 株式会社Ntt都科摩 | Walking robot remote-control system, apparatus and method |
| CN102339086A (en) * | 2010-07-27 | 2012-02-01 | 巍世科技有限公司 | Rotating control device having displacement sensor free of movable electrical contact point |
| CN102141468A (en) * | 2010-12-20 | 2011-08-03 | 中国商用飞机有限责任公司 | Two-freedom-degree joystick-driven experiment device and control method thereof |
| CN103862465A (en) * | 2014-02-20 | 2014-06-18 | 三一汽车制造有限公司 | Multi-joint mechanical arm coordinate correction method and system |
| CN105116961A (en) * | 2015-07-21 | 2015-12-02 | 东南大学 | Intelligent force feedback handle and control method thereof |
| CN106341025A (en) * | 2016-10-19 | 2017-01-18 | 山东大学 | Two-degree-freedom mixed type stepping motor with spherical structure and application thereof |
| CN208451644U (en) * | 2018-07-13 | 2019-02-01 | 深圳市创客工场科技有限公司 | Teaching machine and its rotary encoder |
Also Published As
| Publication number | Publication date |
|---|---|
| CN114227638A (en) | 2022-03-25 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| JP6924142B2 (en) | Robot system | |
| US20040093119A1 (en) | Pathcorrection for an industrial robot | |
| CN107309884B (en) | Robot calibration system and method | |
| CN114227638B (en) | Master-slave mapping method and correction method for robot operating lever and robot | |
| US20040010345A1 (en) | Method of calibrating robot | |
| CN114176902B (en) | Robot automatic compensation motion control method | |
| US5241249A (en) | Process for controlling a robot arm by defining substitution paths | |
| CN109311158B (en) | Operating device for a link actuating device and link actuating system | |
| CN112060129A (en) | Robot joint origin correcting device and correcting method thereof, and robot | |
| CN111113457B (en) | Wearable mechanical arm controller | |
| JP2011036956A (en) | Accuracy adjusting method for robot and robot | |
| JPH07100781A (en) | Articulated robot | |
| CN114740735A (en) | Variable-length feedback-assisted PD type iterative learning control method of single-joint robot | |
| EP4219087A1 (en) | Flexible mechanical arm control method and robot system | |
| CN114131607A (en) | Method and system for calibrating kinematics of generalized kinematics error of industrial robot | |
| CN211615626U (en) | Industrial grade arm allies oneself with control system more | |
| JP2000263477A (en) | Calibration method of robot | |
| JP2011131304A (en) | Control device for joint mechanism of robot | |
| TW201607707A (en) | Rotation angle calibration system and calibration method for a swing arm apparatus | |
| CN219543238U (en) | Multi-degree-of-freedom manipulator | |
| CN111975788B (en) | Calibration method of SCARA manipulator glue dispensing system | |
| JP2727542B2 (en) | Arm position detection method | |
| CN115816448A (en) | Mechanical arm calibration method, device, equipment and medium based on optical position indicator | |
| Wang et al. | Singularity Avoidance Algorithm Based on Bounded Deviation Path Correction Method | |
| JPH05177565A (en) | Calibration device for robot |
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 | ||
| TA01 | Transfer of patent application right |
Effective date of registration: 20220507 Address after: 510000 803, floor 8, building 8 (Building 9), No. 6, Nanjiang Second Road, Zhujiang street, Nansha District, Guangzhou, Guangdong Province Applicant after: Guangzhou Weimou Medical Instrument Co.,Ltd. Address before: 510275 No. 135 West Xingang Road, Guangzhou, Guangdong, Haizhuqu District Applicant before: SUN YAT-SEN University |
|
| TA01 | Transfer of patent application right | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |