CN113524154B - Non-contact sucker carrying device with force feedback speed limiting function and control method - Google Patents
Non-contact sucker carrying device with force feedback speed limiting function and control method Download PDFInfo
- Publication number
- CN113524154B CN113524154B CN202110817340.3A CN202110817340A CN113524154B CN 113524154 B CN113524154 B CN 113524154B CN 202110817340 A CN202110817340 A CN 202110817340A CN 113524154 B CN113524154 B CN 113524154B
- Authority
- CN
- China
- Prior art keywords
- motor
- reel
- conveying device
- connecting rod
- joint
- 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
- 238000000034 method Methods 0.000 title claims abstract description 26
- 238000004364 calculation method Methods 0.000 claims abstract description 8
- 230000008859 change Effects 0.000 claims abstract description 8
- 210000002310 elbow joint Anatomy 0.000 claims description 43
- 210000000323 shoulder joint Anatomy 0.000 claims description 42
- 230000001133 acceleration Effects 0.000 claims description 38
- 239000011159 matrix material Substances 0.000 claims description 19
- 230000009466 transformation Effects 0.000 claims description 16
- 229910000831 Steel Inorganic materials 0.000 claims description 14
- 239000010959 steel Substances 0.000 claims description 14
- 230000005540 biological transmission Effects 0.000 claims description 10
- 238000006073 displacement reaction Methods 0.000 claims description 10
- OIGNJSKKLXVSLS-VWUMJDOOSA-N prednisolone Chemical compound O=C1C=C[C@]2(C)[C@H]3[C@@H](O)C[C@](C)([C@@](CC4)(O)C(=O)CO)[C@@H]4[C@@H]3CCC2=C1 OIGNJSKKLXVSLS-VWUMJDOOSA-N 0.000 claims description 10
- 210000001503 joint Anatomy 0.000 claims description 8
- 241000252254 Catostomidae Species 0.000 claims description 6
- 238000005381 potential energy Methods 0.000 claims description 6
- VYPSYNLAJGMNEJ-UHFFFAOYSA-N Silicium dioxide Chemical compound O=[Si]=O VYPSYNLAJGMNEJ-UHFFFAOYSA-N 0.000 claims description 4
- 239000000741 silica gel Substances 0.000 claims description 4
- 229910002027 silica gel Inorganic materials 0.000 claims description 4
- 230000009471 action Effects 0.000 claims description 3
- 238000009795 derivation Methods 0.000 claims description 3
- 238000012933 kinetic analysis Methods 0.000 claims description 3
- 230000008569 process Effects 0.000 abstract description 6
- 230000033001 locomotion Effects 0.000 abstract description 4
- 238000004804 winding Methods 0.000 description 7
- 241000282414 Homo sapiens Species 0.000 description 4
- 238000010586 diagram Methods 0.000 description 2
- 238000006243 chemical reaction Methods 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- NJPPVKZQTLUDBO-UHFFFAOYSA-N novaluron Chemical compound C1=C(Cl)C(OC(F)(F)C(OC(F)(F)F)F)=CC=C1NC(=O)NC(=O)C1=C(F)C=CC=C1F NJPPVKZQTLUDBO-UHFFFAOYSA-N 0.000 description 1
- 230000003252 repetitive effect Effects 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
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/02—Programme-controlled manipulators characterised by movement of the arms, e.g. cartesian coordinate type
- B25J9/04—Programme-controlled manipulators characterised by movement of the arms, e.g. cartesian coordinate type by rotating at least one arm, excluding the head movement itself, e.g. cylindrical coordinate type or polar coordinate type
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J15/00—Gripping heads and other end effectors
- B25J15/06—Gripping heads and other end effectors with vacuum or magnetic holding means
- B25J15/0616—Gripping heads and other end effectors with vacuum or magnetic holding means with vacuum
-
- 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/10—Programme-controlled manipulators characterised by positioning means for manipulator elements
-
- 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)
- Manipulator (AREA)
Abstract
The invention discloses a non-contact sucker carrying device with force feedback speed limiting function and a control method thereof. And the controller performs modeling calculation according to the corner signal acquired by the motor encoder and the acting force acquired by the force sensor on the suction and floating device to obtain the required resisting moment, so as to control the motor to rotate. The invention can detect the motion state of the transported object in real time; in the process of carrying objects by the non-contact sucker carrying device, the motor generates resistance torque, so that the over-fast change of the speed is effectively inhibited, and the stable conveying of the objects is realized.
Description
Technical Field
The invention relates to the field of contactless moving operation of light and thin objects, in particular to a contactless sucker carrying device with a force feedback speed limiting function and a control method.
Background
The conventional rubber type vacuum chuck is easy to generate scratches and cracks on the contact surface of an object due to the contact with the object. The non-contact vacuum chuck is adopted in the current popular non-contact transportation mode, and comprises a Bernoulli type chuck and a cyclone chuck. For example, chinese patent CN108068136A discloses a simple bernoulli chuck, which uses the bernoulli principle that the pressure is low in a place with high flow velocity, an object is grabbed by a vacuum chuck under the action of negative pressure, the supplied air flows out from the gap between the device and the object, and the device and the object are not in contact. For example, chinese patent CN101264844A discloses a spiral-flow type non-contact suction cup, which uses the suction force generated by the swirling airflow to realize the object grabbing. In this way, the suction cup has a vertically upward lifting force on the object, enabling movement in the vertical direction. In addition, the positioning pin is required to be in contact with the object, so that the object is prevented from falling off when moving in the horizontal direction. Although the automatic conveying labor productivity of the equipment is high, the intelligent degree of the existing equipment is not high, the existing equipment can only complete repetitive work, can not be thought like a human, and can not make corresponding reactions according to different conditions. Therefore, in the process of conveying, when the conveyed objects are of a large variety and are not placed at different positions, a manual conveying mode is adopted, but in the process of conveying the objects by the manual conveying device, human beings have defects in accuracy and endurance compared with robots, so that the moving speed of the human beings is unstable when the human beings operate the conveying device, and if the speed changes too fast, the generated acceleration is too large, and the objects can be separated from the conveying device.
Disclosure of Invention
The invention aims to: the invention aims to provide a non-contact sucker conveying device with a force feedback speed limiting function and a control method, which solve the problems that the moving speed is unstable and objects are possibly separated from the conveying device during manual operation.
The technical scheme is as follows: the invention relates to a non-contact sucker carrying device with a force sensing feedback speed limiting function, which comprises a rotary platform, a connecting rod assembly and a controller, wherein the rotary platform comprises a base reel, a first side plate and a second side plate are arranged on the base reel in parallel, a first shaft is rotatably mounted in the first side plate, a second shaft is rotatably mounted in the second side plate, the connecting rod assembly comprises a first connecting rod, a second connecting rod, a third connecting rod and a fourth connecting rod which are sequentially hinged, one end of the first connecting rod is fixedly connected with the first shaft, one end of the fourth connecting rod is fixedly connected with the second shaft, the third connecting rod is rotatably connected with a sucker, the sucker comprises a sucker frame, a hand controller is fixed on the upper side of the sucker frame, suckers are arranged on the lower side of the sucker frame, force sensors are arranged on the suckers, the controller is communicated with the encoders on the motors and the force sensors on the suckers, when a person holds the hand controller to carry, the hand controller drives the joints of the carrying device to rotate, the rotation of the carrying device drives the motors to rotate, the speed limiting device through the reels and the controller collects rotation angle information of the encoders on the motors and controls the force sensors to control the action of the motors, so that the joint of the carrying device to transmit torque to the reels and the motors, and the carrying device.
Wherein, rotary platform sets up on the base, pedestal mounting has the base motor, base motor output shaft base motor reel, the last base reel of rotary platform passes through wire rope and base motor reel winding, forms confined transmission circuit, install the base encoder on the base motor, base encoder and controller communication.
The elbow joint motor and the shoulder joint motor are arranged between the first side plate and the second side plate, encoders are arranged on the elbow joint motor and the shoulder joint motor, and are communicated with the controller through the encoders to record rotating angles, an output shaft of the elbow joint motor is connected with an elbow joint motor reel, the elbow joint motor reel is wound with the elbow joint reel through a steel wire rope to form a closed transmission loop, and the elbow joint reel is connected with the first shaft; the output shaft of the shoulder joint motor is connected with a shoulder joint motor reel, the shoulder joint motor reel is wound with the shoulder joint reel through a steel wire rope to form a closed transmission loop, and the shoulder joint reel is connected with a second shaft.
The first connecting rod is fixedly connected with the first shaft through an elbow joint connector, the elbow joint connector is hinged with the second shaft and rotates around the second shaft, the fourth connecting rod is fixedly connected with the second shaft through a shoulder joint connector, and the shoulder joint connector is hinged with the first shaft and rotates around the first shaft.
First connecting rod, second connecting rod, third connecting rod and fourth connecting rod are the parallelogram and connect, the third connecting rod passes through the articulated hand controller of joint, the sucking disc frame includes sucking disc frame upper plate and sucking disc frame curb plate, the sucking disc frame curb plate sets up in sucking disc frame upper plate bottom both sides, and the sucking disc is fixed in the sucking disc frame curb plate through the fixed block symmetry, and even interval is provided with three force sensor on each sucking disc, is provided with the silica gel contact on each force sensor.
The invention discloses a control method of a non-contact sucker conveying device with a force feedback speed limiting function, which comprises the following steps:
(1) Converting pulses collected by encoders on the motors into rotation angles of joints of the conveying device, and calculating to obtain position coordinates of the tail end of the conveying device through a positive kinematics model;
(2) Calculating to obtain the moving acceleration of the tail end of the conveying device according to the change of the position coordinates of the tail end of the conveying device in a time period;
(3) Calculating to obtain an average value of the acting force according to the acting force between the sucker and the object to be conveyed, which is acquired by each force sensor, judging whether the average value of the acting force is within a preset range, determining a calculation formula of the reverse acceleration required by the tail end of the conveying device, and obtaining the reverse acceleration required by the tail end of the conveying device according to the calculation formula of the reverse acceleration;
(4) Calculating displacement brought by required resisting moment according to the reverse acceleration of the tail end of the conveying device, calculating to obtain the final position coordinate of the tail end of the conveying device under the condition that only the resisting moment exists according to the displacement brought by the resisting moment, obtaining the rotating angle of each joint of the conveying device through an inverse kinematics model according to the final position coordinate of the tail end of the conveying device, and obtaining the rotating angle variable required by each motor according to the rotating angle of each joint;
(5) The required driving torque is calculated through a dynamic model according to the required corner variable of each motor, the value of the required current is calculated according to the linear relation between the required driving torque and the current, each motor is driven to rotate according to the value of the required current, each motor reel drives each reel to rotate through a steel wire rope, so that each joint of the conveying device generates resistance torque, and the over-fast change of the speed is restrained.
Wherein, the step (1) is specifically as follows: establishing each connecting rod coordinate system of the conveying device by using a D-H method, and performing 4x4 homogeneous matrix transformation between adjacent connecting rod coordinate systems to obtain a matrix relation between the adjacent coordinate systems as follows:
wherein i =1,2,3, α i-1 Is X i-1 Axial direction from Z i-1 RotateTo Z i The turning angle of (c); a is i-1 Is X i-1 Axis from Z i-1 Move to Z i The distance of (d); d is a radical of i Is Z i Axis from X i-1 Move to X i The distance of (d); theta i Is Z i Axis from X i-1 Rotate to X i The corner of (d);
multiplying the posture transformation matrixes of the conveying devices to obtain a posture transformation matrix of the tail end of the non-contact sucker conveying device relative to a base coordinate system as follows:
therefore, the terminal position coordinates of the non-contact sucker conveying device can be obtained according to the angles of the three joints of the conveying device.
The step (2) is specifically as follows: at a time range of Δ t 1 Taking the terminal coordinate (x) of the tail end of the non-contact sucker conveying device 2 ,y 2 ,z 2 ) And the coordinates of the starting point (x) 1 ,y1,z 1 ) Taking difference derivation to obtain Δ t 1 Acceleration in time a = (a) x ,a y ,a z ) Wherein, in the process,
whether the average force value F in the step (3) is within the preset value (0, delta F)]In the range of F>δ F, the reverse acceleration is a k When 0 is present<When F is less than or equal to delta F, the object is about to separate from the conveying device, and the reverse acceleration is a k +k(F-F max ) Wherein a is k The reverse acceleration of the acceleration a obtained in the step (2); at Δ t in said step (4) 1 A period of time after Δ t 2 For reverse acceleration a k ＝(Δa x ,Δa y ,Δa z ) The integral is performed to obtain the displacement due to the required resisting moment as follows:
(Δx,Δy,Δz)＝∫∫(Δa x ,Δa y ,Δa z )dt
then get at Δ t 2 At the end of the time, the final position coordinates of the end of the non-contact suction cup handling device in the presence of only the resisting moment are as follows:
(x e ,y e ,z e )＝(x 2 +Δx,y 2 +Δy,z 2 +Δz)；
inverse kinematics for the contactless suction cup handling device is based on positive kinematics
By left-hand multiplication in turn (A) i ) -1 Wherein i =1,2,3. Corner capable of solving three joints of non-contact sucker conveying device
The rotation angle variable of the motor is:
wherein [ gamma ] 1 ,γ 2 ,γ 3 ]At Δ t for each joint 2 The angle of the starting moment in time is,
at Δ t for each joint 2 Angle of termination time, C 1 、C 2 、C 3 The proportional coefficient between the base reel and the base motor reel, the proportional coefficient between the shoulder joint reel and the shoulder joint motor reel, and the proportional coefficient between the elbow joint reel and the elbow joint motor reel are respectively.
The calculation formula of the required driving torque in the step (5) is as follows:
wherein, the driving moment variable of each joint is set as: t = [ T = 1 ,T 2 ,T 3 ]The kinetic energy of the connecting rod of the carrying device is as follows:
in the formula: u shape ij 、U ik For two representations of the derivative of the transformation matrix with respect to the angle of rotation of the joint, J i Is an inertia matrix, [ beta ] 1 ,β 2 ,β 3 ]The rotation angle variable of each motor;
the link potential energy of the handling device may be expressed as:
in the formula: m is i The mass of each rod; g is a radical of formula T Is a matrix of 1x4, which is formed by the projections of the gravitational acceleration of the connecting rod in the directions of x, y and z; 0 T i is the coordinate transformation of the coordinate system i relative to the base coordinate system;
the position of the connecting rod centroid in the coordinate system i;
the method adopts a Lagrange method to carry out kinetic analysis to obtain:
L＝K-P
wherein K is the kinetic energy of the connecting rod of the carrying device; p is the potential energy of the connecting rod of the carrying device.
Has the advantages that: the invention detects the acting force between the conveying device and the conveyed object through the force sensor arranged at the bottom of the sucker, and can detect the motion state of the conveyed object in real time; in the process of carrying objects by the non-contact sucker carrying device, the resistance moment is generated by the motor, so that the over-fast change of the speed is effectively inhibited, the stable conveying of the objects is realized, and the objects are prevented from being separated from the non-contact sucker carrying device.
Drawings
FIG. 1 is a schematic view of the overall assembly structure of the present invention;
FIG. 2 is a schematic view of another aspect of the present invention;
FIG. 3 is a schematic view of the base;
FIG. 4 is an assembled schematic view of the rotary platform and link assembly;
FIG. 5 is an assembled view of the first link;
FIG. 6 is an assembled view of the fourth link;
FIG. 7 is a schematic view of the assembly of the suction and floating device;
FIG. 8 is a schematic bottom view of the assembly of the suction and floating device;
FIG. 9 is a schematic winding diagram of a reel and motor reel;
FIG. 10 is a schematic representation of forward kinematics according to the present invention;
FIG. 11 is a schematic representation of inverse kinematics according to the present invention;
FIG. 12 is a control flow diagram of the present invention;
fig. 13 is a schematic view of the operation of the manually operated non-contact suction cup carrying device.
Detailed Description
The invention is further described below with reference to the accompanying drawings.
As shown in fig. 1-9, the non-contact sucker carrying device with force feedback speed limiting function disclosed by the invention comprises a force sensing device and a suction and floating device, wherein the force sensing device comprises a base, a rotary platform and a connecting rod assembly, the bottom of the rotary platform is hinged to the base, a fourth connecting rod of the connecting rod assembly and a shoulder joint reel are hinged to the rotary platform, a first connecting rod of the connecting rod assembly and a elbow joint reel are hinged to the rotary platform, and a third connecting rod of the connecting rod assembly and a hand controller on the suction and floating device are connected together through a joint; the hand controller is fixedly connected with a sucker frame provided with four suckers and used for driving the sucking and floating device to move, and three force sensors are uniformly arranged on each sucker at intervals and used for detecting acting force between the carrying device and a carried object. The base comprises a base lower plate 1, a base support piece 6, a base upper plate 5, a base encoder 2, a base motor 3 and a base motor reel 4, wherein the base motor 3 is fixed on the base upper plate 5, the base motor reel 4 is arranged on an output shaft of the base motor 3, and the base encoder 2 is arranged on the base motor 3. The rotary platform comprises a base reel 7, a first side plate 9, a second side plate 8, a elbow joint motor reel 17, a elbow joint motor 10, a elbow joint encoder 11, a shoulder joint motor reel 23, a shoulder joint motor 18 and a shoulder joint encoder 19. Wherein the first side plate 9 and the second side plate 8 are arranged on the base reel 7; the elbow joint motor 10 and the shoulder joint motor 18 are arranged between the first side plate 9 and the second side plate 8; the elbow joint encoder 11 and the shoulder joint encoder 19 are provided on the elbow joint motor 10 and the shoulder joint motor 18, respectively. The elbow joint motor reel 17 is arranged on the output shaft of the elbow joint motor 10; the shoulder joint motor reel 23 is arranged on an output shaft of the shoulder joint motor 18, the two ends of the base reel 7 are fixed with steel wire ropes, and the steel wire ropes are wound on the base motor reel 4 through the cambered surface of the base reel 7, so that the base reel 7 and the base motor 4 form a closed transmission loop. The connecting rod assembly comprises four connecting rods which are arranged in a parallelogram mode, a elbow joint 14 is arranged on a first connecting rod 13, the elbow joint 14 is connected to a first shaft 15 through a flange, a elbow joint reel 16 is arranged on the first shaft 15, the first shaft 15 is hinged to a first side plate 9 through a bearing, the elbow joint 14 is hinged to a second shaft 12 through the bearing, and therefore the elbow joint 14 can rotate around the second shaft 12. The steel wire rope is fixed at two ends of the elbow joint reel 16 and is wound on the elbow joint motor reel 17 through the cambered surface of the elbow joint reel 16 to form a closed transmission loop; the fourth link 20 is provided with a shoulder joint 21, the shoulder joint 21 is flanged to the second shaft 12, and a shoulder reel 22 is provided on the second shaft 12. The second shaft 12 is hinged to the second side plate 8 by a bearing, and the shoulder joint 21 is hinged to the first shaft 15 by a bearing, so that the shoulder joint 21 can rotate about the first shaft 15. The steel wire rope is fixed at two ends of the shoulder joint reel 22 and is wound on the shoulder joint motor reel 23 through the cambered surface of the shoulder joint reel 22 to form a closed transmission loop. The third connecting rod 25 is connected with the suction and floating device to drive the suction and floating device to move. The second link 24 is hinged to the first link 13, one end of the third link 25 is hinged to the second link 24, and the other end is hinged to the fourth link 20. When the non-contact sucker carrying device is operated, the rotation of the third connecting rod 25 can drive the first connecting rod 13 to rotate through the second connecting rod 24; similarly, the rotation of the third link 25 can also rotate the fourth link 20; when the operation speed is changed and the controller controls the rotation of each motor to make the non-contact sucker transporting device generate resistance, the rotation of the first link 13 drives the third link 25 to rotate through the second link 24, and the rotation of the fourth link 20 can also drive the third link 25 to rotate. The sucking and floating device mainly comprises a manual controller 30, a connector 29, a sucker 28, a sucker frame side plate 27, a sucker frame upper plate 31, a fixed block 26, a silica gel contact 33 and a force sensor 32. Wherein, the hand controller 30 is connected with the suction cup frame upper plate 31 through a screw 34. And the suction cup frame side plates 27 are respectively arranged on two sides of the suction cup frame upper plate 31. The four suction cups 28 are symmetrically fixed on the suction cup frame side plate 27 through the fixing blocks 26, three force sensors 32 are arranged on each suction cup, and a silica gel contact 33 is arranged on each force sensor. The hand controller 30 on the suction and floating device is connected with the connecting rod component on the force sensing device through a joint 29.
The base reel and the base motor reel, the shoulder joint reel and the shoulder joint motor reel, and the elbow joint reel and the elbow joint motor reel of the carrying device are driven by steel wire ropes. The steel wire rope is fixed at two ends of the winding wheel and is wound with the motor winding wheel in a crossing mode through the cambered surface of the winding wheel to form a closed transmission loop. When the motor is rotating, the rotating directions of the reel and the motor reel are opposite, the enveloping angle is small by adopting the cross winding mode, the tension is large, and the driving performance can be well kept.
As shown in fig. 10 to 12, the method for suppressing the speed change from being too fast when the non-contact suction cup transfer device of the present invention is operated is as follows:
(1) In the process that the floated object is conveyed from one point to another point, the rotation of each joint drives each reel to rotate, further drives each motor shaft to rotate, and the pulses collected by the encoder on the motor are transmitted to the controller and converted into the rotation angles of each joint of the non-contact sucker conveying device.
(2) Performing kinematic modeling to obtain the tail end pose of the non-contact sucker conveying device, analyzing the pose coordinate system transformation relation of each joint, establishing the corresponding relation between the angle of each joint of the non-contact sucker conveying device and the tail end pose, inputting the rotating angle of each joint obtained in the step (1) into the established model, and calculatingObtaining the position coordinates of the tail end of the conveying device, which comprises the following specific steps: establishing a coordinate system of each connecting rod of the non-contact sucker carrying device by using a D-H method, wherein the position relation of adjacent connecting rods can be represented by four parameters of theta, alpha, a and D, and each parameter is defined as follows: alpha (alpha) ("alpha") i-1 Is X i-1 Axis from Z i-1 Rotate to Z i The corner of (d); a is i-1 Is X i-1 Axial direction from Z i-1 Move to Z i The distance of (d); d is a radical of i Is Z i Axial direction from X i-1 Move to X i The distance of (d); theta i Is Z i Axial direction from X i-1 Rotate to X i The corner of (c). 4x4 homogeneous matrix transformation is carried out between adjacent coordinate systems, a motion equation of the non-contact sucker conveying device can be established, and a matrix relation between the adjacent coordinate systems is obtained:
wherein i =1,2,3;
multiplying the pose transformation matrixes of the non-contact sucker conveying devices to obtain a pose transformation matrix of the tail end of the non-contact sucker conveying device relative to a base coordinate system:
thus, the terminal position (x, y, z) of the non-contact sucker conveying device can be obtained according to the angles of the three joints of the conveying device.
(3) At an extremely short time Δ t 1 Taking the terminal coordinate (x) of the tail end of the non-contact sucker conveying device in a time range 2 ,y 2 ,z 2 ) And starting point coordinates (x) 1 ,y 1 ,z 1 ) Difference derivation to obtain Δ t 1 Acceleration in time a = (a) x ,a y ,a z ) Wherein:
(4) Force sensors are arranged on all suckers of the non-contact sucker conveying device, and when the non-contact sucker conveying device sucks and floats an object, acting force between the non-contact sucker conveying device and the conveyed object, which is acquired by the force sensors, is transmitted to the controller.
(5) The acting force between each contact on the suction cup of the non-contact suction cup conveying device and the conveyed object is F i (i =1,2,3 \823012; 12), the average force F can be found as:
when the conveying device is in a static state, the average value of the acting force between each contact point on the sucker and the conveyed object is maximum, and F = F max ；
Judging whether the average value F of the acting force is close to 0, namely whether the average value F is (0, delta F)]Within the range, δ F is 0.05 × fmax, as the average value of the forces F is greater, i.e. F>δ F, the reverse acceleration is a k May be represented by Δ a x 、Δa y 、Δa z Representing components of the reverse acceleration in the x direction, the y direction and the z direction; when the average value F of the forces is close to 0, i.e. 0<If F is less than or equal to delta F, the object is about to be separated from the conveying device, and the reverse acceleration is a k +k(F-F max ) May be represented by Δ a x 、Δa y 、Δa z Representing the components of the reverse acceleration in the three x, y, z directions. a is k A reverse acceleration of the acceleration a determined in step (3), a k Is smaller than the mode of a.
(6) The final acceleration of the tail end of the conveying device is the sum of the acceleration brought by the hand holding the hand controller and the reverse acceleration brought by the resistance moment, so that the driving moment of the motor can be solved according to the reverse acceleration, and the tail end is in an extremely short time delta t 2 The displacement within the range can be considered to be the sum of the displacement due to hand control and the displacement due to resistive torque, assumed to be at Δ t 1 A period of time after Δ t 2 For reverse acceleration (Δ a) x ,Δa y ,Δa z ) Integrating to obtain the resisting momentThe resulting displacement is:
(Δx,Δy,Δz)＝∫∫(Δa x ,Δa y ,Δa z )dt
then at Δ t 2 At the end of the time, the final coordinate (x) of the end of the non-contact suction cup handling device in the presence of only the resisting moment e ,y e ,z e ) Comprises the following steps:
(x e ,y e ,z e )＝(x 2 +Δx,y 2 +Δy,z 2 +Δz)
in the formula: (x) 2 ,y 2 ,z 2 ) For the end of the handling device at Δ t 2 Coordinates of the starting moment, i.e. Δ t 1 Coordinates of the termination time;
inverse kinematics for a non-contact chuck handling device is the known end coordinate (x) e ,y e ,z e ) And calculating the rotation angle of each joint. According to positive kinematics
By left-hand multiplication in turn (A) i ) -1 Wherein i =1,2,3. Required rotation angles of three joints of non-contact sucker carrying device can be solved
The required rotational angle variables of the motor are:
wherein [ gamma ] 1 ,γ 2 ,γ 3 ]At Δ t for each joint 2 Angle of start time (Δ t) 1 Angle of termination time);
at Δ t for each joint 2 Angle of the termination time. C 1 、C 2 、C 3 Proportional coefficient between the base reel and the base motor reel, proportional coefficient between the shoulder joint reel and the shoulder joint motor reel, and elbow joint winding wireThe proportionality coefficient between the wheel and the elbow joint motor reel.
(7) The kinetic analysis was performed using the Lagrangian method. The driving moment variable of each joint is set as follows: t = [ T = 1 ,T 2 ,T 3 ]The kinetic energy of the connecting rod of the carrying device is as follows:
in the formula: u shape ij 、U ik -the derivative of the transformation matrix with respect to the joint rotation angle;
J i -a pseudo-inertia matrix; [ beta ] 1 ,β 2 ,β 3 ]The rotation angle variable of each motor;
the link potential energy of the handling apparatus can be expressed as:
in the formula: m is i -each rod mass;
g T -a matrix of 1x4, which is composed of projections of the gravitational acceleration of the connecting rod in the x, y, z directions;
0 T i -a coordinate transformation of the coordinate system i with respect to the base coordinate system;
-the position of the connecting rod centroid in the coordinate system i;
in summary, the lagrange equation can be derived as:
L＝K-P
wherein K is the kinetic energy of the connecting rod of the carrying device; p is the potential energy of the connecting rod of the carrying device;
the driving moment equation of each joint of the non-contact sucker conveying device is as follows:
where i =1,2,3, the required drive torque can be calculated from the drive torque equation.
(8) Current I i (i =1,2,3) and the drive torque T of the electric machine i (i =1,2,3) is in a linear relationship, and the controller obtains the value of the required current according to the magnitude of the driving torque that is sought.
(9) The current is sent to each motor to drive each motor to rotate, and each motor reel drives each reel to rotate through a steel wire rope, so that when an operator operates the non-contact sucker conveying device, each joint generates resistance moment, and the over-fast change of the speed is restrained.
As shown in fig. 13, when the present invention is used, the suction cup of the carrying device sucks and floats the object 3, and the hand 1 of the person holds the hand controller 2 of the carrying device, so that the carrying device can be operated to move, and the object 3 can be carried.
Claims (10)
1. The non-contact suction disc carrying device with the force sensing feedback speed limiting function is characterized by comprising a rotary platform, a connecting rod assembly and a controller, wherein the rotary platform comprises a base reel (7), a first side plate (9) and a second side plate (8) are arranged on the base reel (7) in parallel, a first shaft (15) is rotatably mounted on the first side plate (9), a second shaft (12) is rotatably mounted on the second side plate (8), the connecting rod assembly comprises a first connecting rod (13), a second connecting rod (24), a third connecting rod (25) and a fourth connecting rod (20) which are sequentially hinged, one end of the first connecting rod (13) is fixedly connected with the first shaft (15), one end of the fourth connecting rod (20) is fixedly connected with the second shaft (12), the third connecting rod (25) is rotatably connected with a suction floating device, the suction floating device comprises a suction disc frame, a hand controller (30) is fixed on the upper side of the suction disc frame, a suction disc (28) is arranged on the lower side of the suction disc frame, a force sensor (32) is arranged on the suction disc (28), the controller is connected with encoders on each motor and each joint (28), when the controller is communicated with the force sensor on each motor, the rotary controller controls each motor, the power sensor and the rotary controller controls each joint (32) and the motor, and the rotary controller controls each motor to control the rotary reel, and each motor, and each joint controller controls each motor, the motor actions are transmitted to each joint of the conveying device through each motor reel and each reel, so that each joint of the non-contact sucker conveying device generates resistance moment to limit the speed.
2. The contactless sucker conveying device with force sensing feedback speed limiting function according to claim 1, is characterized in that the rotary platform is arranged on a base, the base is provided with a base motor (3), an output shaft of the base motor (3) is connected with a base motor reel (4), the base reel (7) on the rotary platform is wound with the base motor reel through a steel wire rope to form a closed transmission loop, the base motor is provided with a base encoder (2), and the base encoder (2) is communicated with a controller.
3. The non-contact sucker carrying device with the force-sensing feedback speed-limiting function is characterized in that a elbow joint motor (10) and a shoulder joint motor (18) are arranged between the first side plate (9) and the second side plate (8), encoders are arranged on the elbow joint motor (10) and the shoulder joint motor (18) and are communicated with a controller through the encoders to record rotation angles, an output shaft of the elbow joint motor (10) is connected with an elbow joint motor reel (17), the elbow joint motor reel (17) is wound with an elbow joint reel (16) through a steel wire rope to form a closed transmission loop, and the elbow joint reel (16) is connected with a first shaft; the output shaft of the shoulder joint motor (18) is connected with a shoulder joint motor reel (23), the shoulder joint motor reel (23) is wound with the shoulder joint reel (22) through a steel wire rope to form a closed transmission loop, and the shoulder joint reel (22) is connected with a second shaft.
4. The non-contact sucker carrying device with the force-sensing feedback speed-limiting function according to claim 1, wherein the first connecting rod (13) is fixedly connected with the first shaft (15) through an elbow joint (14), the elbow joint (14) is hinged with the second shaft (12) and rotates around the second shaft, the fourth connecting rod (20) is fixedly connected with the second shaft (12) through a shoulder joint (21), and the shoulder joint (21) is hinged with the first shaft (15) and rotates around the first shaft (15).
5. The contactless sucker conveying device with force sensing feedback speed limiting function according to claim 1, wherein the first connecting rod (13), the second connecting rod (24), the third connecting rod (25) and the fourth connecting rod (20) are connected in a parallelogram shape, the third connecting rod is hinged to a hand controller (30) through a joint (29), the sucker frame comprises a sucker frame upper plate (31) and sucker frame side plates (27), the sucker frame side plates (27) are arranged on two sides of the bottom surface of the sucker frame upper plate (31), the suckers (28) are symmetrically fixed on the sucker frame side plates (27) through fixing blocks (26), three force sensors (32) are uniformly arranged on each sucker at intervals, and a silica gel contact (33) is arranged on each force sensor.
6. The method for controlling a non-contact sucker conveying device with force feedback speed limiting function according to any one of claims 1 to 5, characterized by comprising the following steps:
(1) Converting pulses collected by encoders on the motors into joint corners of the conveying device, and calculating to obtain a position coordinate of the tail end of the conveying device through a positive kinematics model;
(2) Calculating to obtain the moving acceleration of the tail end of the conveying device according to the change of the position coordinates of the tail end of the conveying device in a time period;
(3) Calculating to obtain an average value of the acting force according to the acting force between the sucker and the object to be conveyed, which is acquired by each force sensor, judging whether the average value of the acting force is within a preset range, determining a calculation formula of the reverse acceleration required by the tail end of the conveying device, and obtaining the reverse acceleration required by the tail end of the conveying device according to the calculation formula of the reverse acceleration;
(4) Calculating displacement brought by required resisting moment according to the reverse acceleration of the tail end of the conveying device, calculating to obtain the final position coordinate of the tail end of the conveying device under the condition that only the resisting moment exists according to the displacement brought by the resisting moment, obtaining the rotating angle of each joint of the conveying device through an inverse kinematics model according to the final position coordinate of the tail end of the conveying device, and obtaining the rotating angle variable required by each motor according to the rotating angle of each joint;
(5) According to the required rotation angle variable of each motor, required driving torque is obtained through calculation of a dynamic model, a value of required current is obtained through calculation of a linear relation between the required driving torque and the current, each motor is driven to rotate according to the value of the required current, each motor reel drives each reel to rotate through a steel wire rope, so that each joint of the conveying device generates resistance torque, and excessive change of speed is restrained.
7. The method for controlling a non-contact sucker conveying device with force feedback speed limiting function according to claim 6, wherein the step (1) specifically comprises: establishing each connecting rod coordinate system of the carrying device by using a D-H method, and carrying out 4x4 homogeneous matrix transformation between adjacent connecting rod coordinate systems to obtain a matrix relation between the adjacent coordinate systems as follows:
wherein i =1,2,3, α i-1 Is X i-1 Axial direction from Z i-1 Rotate to Z i The turning angle of (c); a is a i-1 Is X i-1 Axial direction from Z i-1 Move to Z i The distance of (a); d is a radical of i Is Z i Axial direction from X i-1 Move to X i The distance of (d); theta.theta. i Is Z i Axis from X i-1 Is rotated to X i The corner of (d);
multiplying each pose transformation matrix of the conveying device to obtain a pose transformation matrix of the tail end of the non-contact sucker conveying device relative to a base coordinate system as follows:
therefore, the terminal position coordinates of the non-contact sucker conveying device can be obtained according to the angles of the three joints of the conveying device.
8. Having force of claim 6The control method of the non-contact sucker conveying device with the speed limiting function is characterized in that the step (2) specifically comprises the following steps: at a time range of Δ t 1 Taking the terminal coordinate (x) of the tail end of the non-contact sucker conveying device 2 ,y 2 ,z 2 ) And the coordinates of the starting point (x) 1 ,y 1 ,z 1 ) Taking difference derivation to obtain Δ t 1 Acceleration in time a = (a) x ,a y ,a z ) Wherein, in the step (A),
9. the method for controlling a non-contact sucker conveying device with force feedback speed limiting function as claimed in claim 8, wherein the average value F of the acting force in the step (3) is within a preset value (0, δ F)]In the range of F>δ F, the reverse acceleration is a k When 0 is present<If F is less than or equal to delta F, the object is about to be separated from the conveying device, and the reverse acceleration is a k +k(F-F max ) Wherein a is k The reverse acceleration of the acceleration a obtained in the step (2); at Δ t in said step (4) 1 A period of time after Δ t 2 For reverse acceleration a k ＝(Δa x ,Δa y ,Δa z ) By integrating, the displacement by the required resisting moment can be obtained as follows:
(Δx,Δy,Δz)＝∫∫(Δa x ,Δa y ,Δa z )dt
then get at Δ t 2 At the end time, the final position coordinates of the tail end of the non-contact suction cup conveying device under the condition that only the resisting moment exists are as follows:
(x e ,y e ,z e )＝(x 2 +Δx,y 2 +Δy,z 2 +Δz)；
inverse kinematics of the non-contact suction-cup handling device is based on positive kinematics
By sequential left multiplication (A) i ) -1 Wherein i =1,2,3; corner capable of solving three joints of non-contact sucker carrying device
The rotation angle variable of the motor is:
wherein [ gamma ] 1 ,γ 2 ,γ 3 ]At Δ t for each joint 2 The angle of the starting moment in time is,
at Δ t for each joint 2 Angle of termination time, C 1 、C 2 、C 3 The proportional coefficient between the base reel and the base motor reel, the proportional coefficient between the shoulder joint reel and the shoulder joint motor reel, and the proportional coefficient between the elbow joint reel and the elbow joint motor reel are respectively.
10. The method for controlling a non-contact suction cup carrying device having a force feedback speed limiting function according to claim 9, wherein the driving torque required in the step (5) is calculated by the following equation:
wherein, the driving moment variable of each joint is set as: t = [ T = 1 ,T 2 ,T 3 ]The kinetic energy of the connecting rod of the carrying device is as follows:
in the formula: u shape ij 、U ik For two representations of the derivative of the transformation matrix with respect to the angle of rotation of the joint, J i Is an inertia matrix, [ beta ] 1 ,β 2 ,β 3 ]The rotation angle variable of each motor;
the link potential energy of the handling device may be expressed as:
in the formula: m is a unit of i The mass of each rod; g is a radical of formula T Is a matrix of 1x4, which is formed by the projections of the gravitational acceleration of the connecting rod in the directions of x, y and z; 0 T i is the coordinate transformation of the coordinate system i relative to the base coordinate system;
the position of the connecting rod centroid in the coordinate system i;
the method adopts a Lagrange method to carry out kinetic analysis to obtain:
L＝K-P
wherein K is the kinetic energy of the connecting rod of the carrying device; p is the potential energy of the connecting rod of the carrying device.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110817340.3A CN113524154B (en) | 2021-07-20 | 2021-07-20 | Non-contact sucker carrying device with force feedback speed limiting function and control method |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110817340.3A CN113524154B (en) | 2021-07-20 | 2021-07-20 | Non-contact sucker carrying device with force feedback speed limiting function and control method |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113524154A CN113524154A (en) | 2021-10-22 |
| CN113524154B true CN113524154B (en) | 2022-11-08 |
Family
ID=78128923
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110817340.3A Active CN113524154B (en) | 2021-07-20 | 2021-07-20 | Non-contact sucker carrying device with force feedback speed limiting function and control method |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113524154B (en) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114014139B (en) * | 2021-11-24 | 2024-04-19 | 江苏科技大学 | Marine three-dimensional curved plate carrying device |
Family Cites Families (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPS57145722A (en) * | 1981-03-04 | 1982-09-08 | Central Glass Co Ltd | Taking out device for sheet member |
| CN203045725U (en) * | 2012-08-23 | 2013-07-10 | 杭州永创智能设备股份有限公司 | Mechanical arm |
| CN103433921B (en) * | 2013-09-11 | 2015-07-08 | 北京邮电大学 | Three-degree-of-freedom translation force feedback hand controller |
| CN204339777U (en) * | 2014-11-19 | 2015-05-20 | 沃科诺（福建）环保能源有限公司 | A kind of sucker manipulator |
| CN205060921U (en) * | 2015-09-21 | 2016-03-02 | 重庆新铝城幕墙工程有限公司 | Glass production is with transportation frame |
| CN105171734A (en) * | 2015-10-22 | 2015-12-23 | 哈尔滨联科自动化技术开发有限公司 | High-speed four-axis palletizing robot and palletizing method |
| CN205325679U (en) * | 2015-12-28 | 2016-06-22 | 长沙师范学院 | Intelligent transfer robot |
| CN109955225A (en) * | 2019-03-28 | 2019-07-02 | 东南大学 | A kind of force feedback hand controlled device of parallel Three Degree Of Freedom and its control method |
-
2021
- 2021-07-20 CN CN202110817340.3A patent/CN113524154B/en active Active
Also Published As
| Publication number | Publication date |
|---|---|
| CN113524154A (en) | 2021-10-22 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107263486A (en) | A kind of robot overturning-preventing method, device and overturning-preventing robot | |
| CN113524154B (en) | Non-contact sucker carrying device with force feedback speed limiting function and control method | |
| CN107414823A (en) | According to the method and device of torque adjustment balance | |
| CN108557450A (en) | A kind of logistic storage robot | |
| CN109591906B (en) | Control system and control method for transmission tower climbing robot | |
| CN111203849A (en) | Mobile robot grabbing operation system and control method | |
| CN111941422A (en) | Tile pasting robot system | |
| CN108818586B (en) | Object gravity center detection method suitable for automatic grabbing by manipulator | |
| CN210819597U (en) | Automatic change manipulator and system thereof | |
| CN108372512B (en) | But intelligent automatic control's rotatory grabbing device on robotic arm | |
| CN113561199A (en) | Transformer substation inspection robot with lifting type holder and mechanical arm | |
| CN105729487B (en) | One kind embraces formula mining elevator crusing robot | |
| CN111175770B (en) | Automatic container grabbing device and method based on point laser ranging | |
| CN109795879B (en) | Posture adjusting butt joint device, system and method based on six-degree-of-freedom parallel mechanism | |
| WO2019109672A1 (en) | Jacking robot and robot system | |
| CN115258972A (en) | Double-arm cooperative flexible cable parallel hoisting robot and control method thereof | |
| CN113601485A (en) | Intelligent spraying robot for high-rise building outer wall | |
| CN212352030U (en) | Tile pasting robot system | |
| EP0328650A4 (en) | Method and apparatus for directly teaching horizontal arm-type multi-articulated robot | |
| CN113246115B (en) | Industrial manipulator based on machine vision technology and control method thereof | |
| CN212500367U (en) | Obstacle crossing device of intelligent inspection robot | |
| CN207071931U (en) | According to the device of torque adjustment balance | |
| CN216328388U (en) | Stacking mechanical arm and detection device | |
| CN113119163A (en) | Pole-climbing robot control system and control method thereof | |
| CN112975896A (en) | Building material transfer 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 | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant | ||
| EE01 | Entry into force of recordation of patent licensing contract |
Application publication date: 20211022 Assignee: HANGZHOU GOWE AUTOMATION CO.,LTD. Assignor: JIANGSU University OF SCIENCE AND TECHNOLOGY Contract record no.: X2024980000156 Denomination of invention: Contactless suction cup handling device and control method with force feedback speed limit function Granted publication date: 20221108 License type: Common License Record date: 20240109 Application publication date: 20211022 Assignee: Sichuan Jinyuan Pipe Co.,Ltd. Assignor: JIANGSU University OF SCIENCE AND TECHNOLOGY Contract record no.: X2024980000155 Denomination of invention: Contactless suction cup handling device and control method with force feedback speed limit function Granted publication date: 20221108 License type: Common License Record date: 20240109 |
|
| EE01 | Entry into force of recordation of patent licensing contract |