CN113733037B - Seven-degree-of-freedom master-slave isomorphic teleoperation master hand - Google Patents

Seven-degree-of-freedom master-slave isomorphic teleoperation master hand Download PDF

Info

Publication number
CN113733037B
CN113733037B CN202111069388.7A CN202111069388A CN113733037B CN 113733037 B CN113733037 B CN 113733037B CN 202111069388 A CN202111069388 A CN 202111069388A CN 113733037 B CN113733037 B CN 113733037B
Authority
CN
China
Prior art keywords
joint
arm
rotating
shaft hole
rotating shaft
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202111069388.7A
Other languages
Chinese (zh)
Other versions
CN113733037A (en
Inventor
朱跃
仲军
龚锦
韩震峰
于振中
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
HRG International Institute for Research and Innovation
Original Assignee
HRG International Institute for Research and Innovation
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by HRG International Institute for Research and Innovation filed Critical HRG International Institute for Research and Innovation
Priority to CN202111069388.7A priority Critical patent/CN113733037B/en
Publication of CN113733037A publication Critical patent/CN113733037A/en
Application granted granted Critical
Publication of CN113733037B publication Critical patent/CN113733037B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • 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
    • 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
    • B25J9/00—Programme-controlled manipulators
    • B25J9/06—Programme-controlled manipulators characterised by multi-articulated arms
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1602—Programme controls characterised by the control system, structure, architecture
    • B25J9/1607—Calculation of inertia, jacobian matrixes and inverses
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/11—Complex mathematical operations for solving equations, e.g. nonlinear equations, general mathematical optimization problems
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F30/00—Computer-aided design [CAD]
    • G06F30/10—Geometric CAD
    • G06F30/17—Mechanical parametric or variational design
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F30/00—Computer-aided design [CAD]
    • G06F30/20—Design optimisation, verification or simulation
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F2119/00—Details relating to the type or aim of the analysis or the optimisation
    • G06F2119/14—Force analysis or force optimisation, e.g. static or dynamic forces

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Mathematical Physics (AREA)
  • Computational Mathematics (AREA)
  • Mathematical Analysis (AREA)
  • Mathematical Optimization (AREA)
  • Pure & Applied Mathematics (AREA)
  • Data Mining & Analysis (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Geometry (AREA)
  • General Engineering & Computer Science (AREA)
  • Software Systems (AREA)
  • Computer Hardware Design (AREA)
  • Evolutionary Computation (AREA)
  • Algebra (AREA)
  • Databases & Information Systems (AREA)
  • Computing Systems (AREA)
  • Operations Research (AREA)
  • Automation & Control Theory (AREA)
  • Manipulator (AREA)

Abstract

The invention discloses a master-slave isomorphic teleoperation master hand with seven degrees of freedom, which is characterized in that a base component, a base rotating joint component, a large-arm rotating joint component, a two-arm rotating joint component, a three-arm rotating joint component, a swing frame swing joint component and a tail end rotating joint component are sequentially fixed in an end-to-end rotating mode to form seven joints, and each joint is driven by a motor; the motor integrates a driver and an encoder into a whole; according to the method, a D-H coordinate system and a kinematics matrix equation of the main hand are established according to the configuration and parameters of the main hand, redundant joint angles which are not formed are solved through an algebraic method, a connecting rod plane graph is drawn according to redundant joints, finally, a method for fixing one joint is adopted, and the kinematics inverse solution of other redundant joint angles is solved through the combination of the algebraic method and a geometric method. The invention greatly reduces the number of 7-degree-of-freedom redundant main hand inverse kinematics solutions and effectively improves the solving speed.

Description

Seven-degree-of-freedom master-slave isomorphic teleoperation master hand
Technical Field
The invention relates to the technical field of teleoperation robots, in particular to a seven-degree-of-freedom master-slave isomorphic teleoperation master hand.
Background
The master-slave teleoperation robot system can help operators to remotely operate the master hand to carry out rescue operation, can control the master hand in real time at a distance, can immediately react according to the change of the surrounding environment of the master hand at the slave end, greatly improves the rescue efficiency, and ensures the life safety of rescuers.
At present, the research of a master-slave teleoperation master hand is still in a primary stage, a force feedback teleoperation system which takes a force feedback handle PHATOM Omni as a master end device and a hydraulic excavator as a slave end device is built in Zhejiang university, and the force feedback teleoperation system has a force feedback function and can better control the motion of the slave end master hand.
In the prior art, a master-slave isomorphic teleoperation robot system for rescue after earthquake does not exist, and most teleoperation masters in China currently adopt existing master-end equipment to research the master-slave isomerous teleoperation robot system, so that the system delay is increased, and the transparency of master-slave teleoperation is reduced.
Compared with a 6-degree-of-freedom master hand, the inverse kinematics solution of a 7-degree-of-freedom master hand is difficult, and due to the existence of 1 more redundant degree of freedom, the theoretical inverse kinematics solution of the master hand has infinite number, the calculation is extremely complex, and the calculation time is greatly increased.
The application number 202011075243.3 discloses a seven-degree-of-freedom master hand limit optimization method based on position-level inverse kinematics, and relates to the seven-degree-of-freedom master hand limit optimization method. The method aims to solve the problems that the existing numerical solution cannot obtain a closed solution, and final state self-motion exists; the analytical solution cannot aim at the offset configuration, and the problem that the motion optimization cannot be realized exists. Firstly, obtaining an analytical solution of 7-degree-of-freedom main hand inverse kinematics based on a parameterization solving method for fixing a certain joint angle; then, the fixed joint angle parameter is used as input, joint limit is used as an optimization index, and an optimal control problem is established; based on Lagrange multiplier method, converting the constrained problem into unconstrained problem; and finally, solving the optimal joint angle parameters based on a Newton iteration method, and obtaining 7 joint space tracks considering joint limit optimization by giving an initial configuration, an expected end pose and Cartesian path planning. Although the invention achieves optimal control by fixing one of the joints, the specific algorithm provided is not applicable to the 7-degree-of-freedom master hand configuration provided by the invention.
Disclosure of Invention
The invention aims to solve the technical problem of how to provide a 7-degree-of-freedom master hand with a master-slave isomorphism.
The invention solves the technical problems through the following technical means:
a seven-degree-of-freedom master-slave isomorphic teleoperation master hand comprises a base component (1), a base rotating joint component (2), a large-arm rotating joint component (3), a two-arm rotating joint component (8), a three-arm rotating joint component (4), a swing frame rotating joint component (5), a swing frame swing joint component (6) and a tail end rotating joint component (7);
the base component (1), the base rotating joint component (2), the large-arm rotating joint component (3), the two-arm rotating joint component (8), the three-arm rotating joint component (4), the swing frame rotating joint component (5), the swing frame swing joint component (6) and the tail end rotating joint component (7) are sequentially fixed in an end-to-end rotating mode to form seven joints, and each joint is driven by a motor; the motor integrates a driver and an encoder into a whole;
the relationship of the 7 joints is: 1 st joint coordinate system x 1 z 1 y 1 And a base coordinate system x 0 z 0 y 0 Coincident, 2 nd joint coordinate system x 2 z 2 y 2 By the first joint coordinate system around x 1 The axis is rotated by 90 DEG and then wound by z 1 Rotated 90 deg. and taken along x 1 Axis movement a1, 3 rd joint coordinate system x 3 z 3 y 3 By the 2 nd joint coordinate system around z 2 The axis being rotated 90 and directed along x 2 Axis movement a2, 4 th joint coordinate system x 4 z 4 y 4 From the 3 rd joint along x 3 Axis movement a3, 5 th joint coordinate system x 5 z 5 y 5 Along x by the 4 th joint 4 Axial movement a4 thThe 6-joint coordinate system rotates 90 degrees along the x axis from the 5 th joint coordinate system, then rotates 90 degrees along the z axis and rotates along the x axis 5 Move a5 along y 5 Moving-d 6, the 7 th joint coordinate system is rotated 90 degrees around the x-axis from the 5 th joint coordinate system; all joints rotate by taking respective z-axis as a rotation center;
the inverse kinematics solving method of the teleoperation master hand comprises the following steps:
s1, establishing a D-H coordinate system and a kinematic matrix according to the configuration and the parameters of the master hand;
s2, solving the joint angle of which the main hand does not form redundancy by adopting an algebraic method;
s3, drawing a connecting rod plane diagram according to the redundant joints of the main hand;
s4, fixing a redundant joint, and enabling the angle of the redundant joint to be a known quantity;
and S5, solving the kinematic inverse solution of the redundant joint by adopting a method of relevant combination of a geometric method and an algebraic method.
The main hand provided by the invention adopts the motor to drive each joint, reduces the complexity of the structure of the main hand, and integrates the encoder in the motor, so that the joint angle can be obtained in real time, and a data basis is provided for control. The method comprises the steps of firstly establishing a D-H coordinate system and a kinematics matrix equation of a main hand according to the configuration and parameters of the main hand, then solving the joint angles which do not form redundancy through an algebraic method, drawing a connecting rod plane diagram according to the redundant joints, and finally solving the kinematics inverse solution of other redundant joint angles by combining the algebraic method and a geometric method through a method of fixing one joint. The invention greatly reduces the number of 7-degree-of-freedom redundant main hand inverse kinematics solutions, effectively improves the solving speed and reduces the solving difficulty.
Further, the base component (1) comprises a base (9), a base rotary motor (10) and a base end cover (11); an accommodating cavity for accommodating a base rotary motor (10) is arranged in the base (9); the base end cover (11) is fixed on the top of the base (9); an output shaft of the rotary motor (10) extends upwards out of the base (9) and the base end cover (11);
the base rotating joint assembly (2) comprises a base rotating main shaft (12) and a base rotating frame; the base revolving frame is provided with a first revolving shaft hole; the main shaft (12) is vertically fixedly connected with an output shaft of a first rotary motor (10) of the base, and the first rotary shaft hole is horizontally arranged.
Furthermore, the large arm rotating joint assembly (3) comprises a large arm joint motor (14) and a large arm connecting rod; one end of the big arm connecting rod is provided with a second rotating shaft hole, and the other end of the big arm connecting rod is provided with a third rotating shaft hole; the large arm joint motor (14) is fixed on the base revolving frame, and an output shaft of the large arm joint motor (14) penetrates through the first rotating shaft hole and the second rotating shaft hole to rotationally fix the large arm connecting rod and the base rotating joint component (2); the third rotating shaft hole is horizontally arranged;
the two-arm rotating joint component (8) comprises a two-arm joint motor (21) and a two-arm connecting rod; a fourth rotating shaft hole and a fifth rotating shaft hole are respectively formed in two ends of the two-arm connecting rod; the two-arm joint motor (21) is fixed on the large arm connecting rod, and an output shaft of the two-arm joint motor (21) penetrates through the third rotating shaft hole and the fourth rotating shaft hole to fix the two-arm connecting rod and the large arm connecting rod in a rotating mode; the fifth rotating shaft hole is horizontally arranged.
Furthermore, the three-arm rotating joint assembly (4) comprises a three-arm joint motor (31) and a three-arm connecting rod; a sixth rotating shaft hole and a seventh rotating shaft hole are respectively formed in two ends of the three-arm connecting rod; the three-arm joint motor (31) is fixed on the two-arm connecting rod, and an output shaft of the three-arm joint motor (31) penetrates through the fifth rotating shaft hole and the sixth rotating shaft hole to fix the three-arm connecting rod and the two-arm connecting rod in a rotating mode; the seventh rotating shaft hole is horizontally arranged;
the swing frame rotating joint assembly (5) comprises a rotating joint motor (44) and a first swing frame; an eighth rotating shaft hole and a ninth rotating shaft hole are respectively formed at the two ends of the first swinging frame; the rotary joint motor (44) is fixed on the three-arm connecting rod, and an output shaft of the rotary joint motor (44) penetrates through the seventh rotating shaft hole and the eighth rotating shaft hole to fix the first swinging frame and the three-arm connecting rod in a rotating mode; the ninth rotating shaft hole is horizontally arranged and is vertical to the eighth rotating shaft hole.
Furthermore, the swing joint component (6) of the swing frame comprises a swing joint motor (53) and a second swing frame; a tenth rotating shaft hole and an eleventh rotating shaft hole are formed in the second swinging frame; the swing joint motor (53) is fixed on the first swing frame, and an output shaft of the swing joint motor (53) penetrates through the ninth rotating shaft hole and the tenth rotating shaft hole to rotationally fix the second swing frame and the first swing frame; the eleventh rotating shaft hole is vertical to the tenth rotating shaft hole;
the tail end rotary joint assembly (7) comprises a second rotary motor (57) and a rotary frame; a twelfth rotating shaft hole is formed in the rotating frame; the second rotary motor (57) is fixed on the second swing frame, and an output shaft of the second rotary motor (57) penetrates through the eleventh rotating shaft hole and the twelfth rotating shaft hole to rotationally fix the swing frame and the second swing frame.
Furthermore, the large arm connecting rod, the two-arm connecting rod and the three-arm connecting rod are all hollow frame bodies.
Further, the solving method of S1 includes the following steps:
s11, obtaining a homogeneous coordinate transformation matrix of the end coordinate system relative to the base coordinate system according to the secondary transformation equation between the link coordinate systems
Figure GDA0003733663480000041
Satisfies formula (1):
Figure GDA0003733663480000042
wherein the content of the first and second substances,
Figure GDA0003733663480000043
a secondary coordinate transformation matrix representing coordinate systems of two adjacent connecting rods; the connecting rod is a connecting piece between two adjacent joints; where n, o, a represents the attitude of the end joint coordinate system relative to the polar coordinate system, and p represents the position of the end joint coordinate system relative to the polar coordinate system.
Further, the solving method of S2 includes the following steps:
s21, first, the two ends of equation (1) are simultaneously multiplied by
Figure GDA0003733663480000044
Obtaining:
Figure GDA0003733663480000045
the equation can be established by equaling the elements in row 2 and column 4 of the two-terminal matrix of equation (2):
-s 1 p x +c 1 p y ＝0 (3)
the 1 st joint angle θ can be obtained from equation (3) 1 There are two solutions, respectively: atan2 (p) y ,p x ) And Atan2 (-p) y ,-p x )；
The equation can be established by equality of the elements in the 3 rd row and the 2 nd column of the matrix at both ends of equation (2):
c 6 ＝-s 1 a x +c 1 a y ＝k 1 (4)
the 6 th joint angle theta can be obtained from the formula (4) 6 The value of (A) is given by two solutions, respectively
Figure GDA0003733663480000046
Due to theta 1 With two solutions, theta is known 6 There are 4 solutions to the value of (c);
then multiply right at both ends of equation (2) simultaneously
Figure GDA0003733663480000047
The following can be obtained:
Figure GDA0003733663480000048
from equation (5), the two-terminal matrix has the same 1 st row and 1 st column element and 3 rd row and 3 rd column element, we can obtain:
(n x +c 1 c 6 o z )c 7 +(c 1 c 6 n z -o x )s 7 ＝-s 1 s 6 (6)
let n be x +c 1 c 6 o z ＝k 2 ，c 1 c 6 n z -o x ＝k 3 Then the 7 th joint angle theta can be obtained 7 Is solved as
Figure GDA0003733663480000051
Further, the solving method of S3 includes the following steps:
s31, obtaining l according to the plane view of the connecting rod OA ＝a 2 ，l AB ＝a 3 ，l BC ＝a 4 ，
Figure GDA0003733663480000052
Coordinates of point D are
Figure GDA0003733663480000053
The seven-degree-of-freedom master-slave isomorphic teleoperation master hand is characterized in that the joint angle of the 5 th joint is fixed; the solving method of S5 comprises the following steps:
s51, simultaneous right multiplication at both ends of equation (1)
Figure GDA0003733663480000054
The following can be obtained:
Figure GDA0003733663480000055
let the coordinates of point B in the base coordinate system be ( 4 p x ， 4 p y ， 4 p z ) The coordinates of the point B can be obtained as
Figure GDA0003733663480000056
Thereby obtaining
Figure GDA0003733663480000057
Then
Figure GDA0003733663480000058
The 3 rd joint angle theta can be obtained 3 Is solved as
Figure GDA0003733663480000059
S52, the two-terminal matrix has the same row 1, column 3 element and row 2, column 3 element according to equation (7), so that:
4 p x ＝a 1 c 1 +a 2 c 1 c 2 +a 3 c 1 c 23 (8)
let k 5 ＝a 2 c 1 +a 3 c 1 c 3 ,k 6 ＝-a 3 c 1 s 3 ,k 7 ＝ 4 p x -a 1 c 1 The following can be obtained:
k 7 ＝k 5 c 2 +k 6 s 2 (9)
the 2 nd joint angle can be obtained from the formula (9)
Figure GDA00037336634800000510
S53, the matrix is equal in row 1, column 3 and row 3, column 3 elements according to equation (7):
s 234 ＝-(c 5 c 6 c 7 -s 5 s 7 )n z +(c 5 c 6 s 7 +s 5 c 7 )o z +(c 5 s 6 )a z (10)
c 234 ＝(s 5 c 6 c 7 +c 5 s 7 )n z -(-s 5 c 6 s 7 +s 5 c 7 )o z -(s 5 s 6 )a z (11)
θ 234 ＝atan2(s 234 ,c 234 ) (12)
the 4 th joint angle θ can be obtained from the formula (12) 4 ＝θ 234 -θ 2 -θ 3 。
The invention has the advantages that:
1. the main hand provided by the invention adopts the motor to drive each joint, reduces the complexity of the structure of the main hand, and integrates the encoder in the motor, so that the joint angle can be obtained in real time, and a data basis is provided for control. The method comprises the steps of firstly establishing a D-H coordinate system and a kinematics matrix equation of a main hand according to the configuration and parameters of the main hand, then solving angles which do not form redundant joints through an algebraic method, drawing a connecting rod plane graph according to the redundant joints, and finally solving kinematics inverse solutions of other redundant joint angles by combining the algebraic method with a geometric method through a method of fixing one joint. The invention greatly reduces the number of 7-degree-of-freedom redundant main inverse kinematics solutions, effectively improves the solving speed and reduces the solving difficulty.
The invention relates to a principle of fixing the 5 th joint, which is to select the joint which has less influence on other joints as much as possible to fix. An analytic solution for each position can be obtained by using an algebraic method and a geometric method.
2. Each joint adopts the butt joint of pivot hole, and structural design is simple easily to assemble, and mutual interference when simple structure avoids the motion as far as can satisfy the continuity and the real-time nature of action by the at utmost.
3. The hollow connecting rod is adopted, so that the weight of the main hand can be further reduced, and the power output is reduced.
Drawings
FIG. 1 is a diagram illustrating an appearance of a master hand in master-slave isomorphic teleoperation according to an embodiment of the present invention;
FIG. 2 is an illustration of an embodiment of the present invention showing the effect of a master-slave isomorphic teleoperation master hand base;
FIG. 3 is an illustration showing the effect of the master-slave isomorphic teleoperation of the rotary joint of the master hand base according to the embodiment of the present invention;
FIG. 4 is a diagram illustrating the effect of the two-arm rotation joint of the master hand in master-slave isomorphic teleoperation according to the embodiment of the present invention;
FIG. 5 is a diagram illustrating the effect of the three-arm rotary joint of the master hand in master-slave isomorphic teleoperation according to the embodiment of the present invention;
FIG. 6 is an illustration showing the effect of the rotational joint of the master-slave isomorphic teleoperation master hand swing frame in the embodiment of the present invention;
FIG. 7 is a diagram illustrating the effect of the swing joint and the end swing joint of the master-slave isomorphic teleoperation master hand swing frame according to the embodiment of the present invention;
FIG. 8 is a diagram illustrating the effect of the master-slave isomorphic teleoperation of the master hand end revolute joint in the embodiment of the present invention;
FIG. 9 is a schematic diagram illustrating the relationship between joints of a master-slave isomorphic teleoperation master hand according to an embodiment of the present invention;
FIG. 10 is a graph of a master-slave isomorphic teleoperation master hand of an embodiment of the present invention;
FIG. 11 is a flowchart illustrating a master-slave isomorphic teleoperation master hand kinematics inverse solution in accordance with an embodiment of the present invention;
fig. 12 is a plan view of a master-slave isomorphic teleoperation master-hand redundant joint link according to an embodiment of the present invention.
In the figure: 1. a base assembly; 2. a base revolute joint assembly; 3. a two-arm rotating joint assembly; 4. a three-arm revolute joint assembly; 5. the swing frame rotates the joint assembly; 6. a swing joint assembly of the swing frame; 7. a distal revolute joint assembly; 8. A boom swivel joint assembly; 9. a base; 10. a base rotary motor; 11. a base fixing cover; 12. the base rotates the main shaft; 13. a rear support joint plate; 14. a large arm joint motor; 15. a left swivel joint plate; 16. a large arm bearing; 17. A right swivel joint plate; 18. a left large arm joint plate; 19. an upper support plate of the big arm; 20. a right large arm joint plate; 21. A two-arm joint motor; 22. a two-arm bearing; 23. a two-arm bearing end cover; 24. a two-arm bearing support; 25. a lower support plate of the upper arm; 26. a second large arm connecting plate; 27. a large arm right shaft; 28. a first connecting plate of the big arm; 29. a three-arm bearing; 30. A left two-arm joint plate; 31. a three-arm joint motor; 32. two arm upper support plates; 33. a two-arm lower joint plate; 34. a second arm and a second connecting plate; 35. a second-arm third-connecting plate; 36. a two-arm right shaft; 37. a two-arm lower support plate; 38. a right two-arm joint plate; 39. a three-arm first connecting plate; 40. a three-arm left shaft; 41. a three-arm second connecting plate; 42. a left three-arm joint plate; 43. a three-arm upper support plate; 44. the swing frame rotates the joint motor; 45. a swing arm bearing; 46. a swing arm bearing support; 47. a three-arm outer bearing end cap; 48. the three-arm shaft end is fixed; 49. a three-arm lower support plate; 50. a right three-arm joint plate; 51 left swing frame joint plate; 52 a swing frame top plate; 53. a swing joint motor of the swing frame; 54. a right swing frame knuckle board; 55. a swing arm left side connecting shaft; 56. a swing frame rotating shaft; 57. a terminal joint motor 7; 58. A swing joint; 59. and (4) a rotary joint end cover.
a 1 Along x 1 Axis from z 1 Move to z 2 The distance of (a); a is 2 Along x 2 Axis from z 2 Move to z 3 The distance of (a); a is a 3 Along x 3 Axis from z 3 Move to z 4 The distance of (d); a is a 4 Along x 4 Axis from x 4 Move to z 5 The distance of (d); a is 5 Along x 5 Axis from z 5 Move to z 6 The distance of (a); d 6 Along z 6 Axis from x 5 Move to x 6 The distance of (d); theta.theta. 2 Z around z 2 Axis from x 1 Rotate to x 2 The angle of (d); theta.theta. 3 Winding z 3 Axis from x 2 Rotate to x 3 The angle of (d); theta.theta. 4 Z around z 4 Axis from x 3 Rotate to x 4 The angle of (d); theta 5 Winding z 5 Axis from x 4 Rotate to x 5 The angle of (c).
Detailed Description
In order to make the objects, technical solutions and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the embodiments of the present invention, and it is obvious that the described embodiments are some embodiments of the present invention, but not all embodiments. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.
Example 1
As shown in figure 1, a seven-degree-of-freedom master-slave isomorphic teleoperation master hand is disclosed, and the structural proportion of the master hand is consistent with that of a slave hand. The device is composed of a base 1, a base rotating joint component 2, a large arm rotating joint component 8, a two-arm rotating joint component 3, a three-arm rotating joint component 4, a swing frame rotating joint component 5, a swing frame swing joint component 6 and a tail end rotating joint component 7 which are rotationally fixed end to form 7 joints, and each joint is driven by a motor; the motor in this embodiment integrates a driver and an encoder.
As shown in fig. 2, the base 1 includes a base 9, a first rotary motor, and a base end cover 11. The base 9 is provided with a containing cavity for containing a base exhibitions motor 10, the first rotary motor is installed in the base 9, and the base end cover 11 is respectively fixed with the first rotary motor and the base 9 through bolt connection. The output shaft of the first rotary motor upwards penetrates through the base end cover 11 to drive the upper part to rotate in the circumferential direction.
As shown in fig. 3, the base rotary joint assembly 2 includes a base rotary main shaft 12 and a base turret; the base revolving frame is provided with a first revolving shaft hole; the main shaft 12 is vertically fixedly connected with an output shaft of the first rotary motor 10 of the base, and the first rotary shaft hole is horizontally arranged.
The base revolving frame comprises a left revolving joint plate 15, a right revolving joint plate 17 and a rear supporting joint plate 13, the left revolving joint plate 15 and the right revolving joint plate 17 are respectively fixed on two sides of the base rotating main shaft 12 through bolt connection, and two ends of the rear supporting joint plate 13 are respectively connected with the left revolving joint plate 15 and the right revolving joint plate 17 through bolts; the large arm joint motor 14 is fixed on the left rotary joint plate 15 through bolt connection, and the large arm bearing 16 is installed in the first rotating shaft hole of the right rotary joint plate 17.
As shown in fig. 4, the upper arm rotary joint assembly 3 includes an upper arm joint motor 14, an upper arm link; one end of the large arm connecting rod is provided with a second rotating shaft hole, and the other end of the large arm connecting rod is provided with a third rotating shaft hole; the large arm joint motor 14 is fixed on the base revolving frame, and an output shaft of the large arm joint motor 14 penetrates through the first rotating shaft hole and the second rotating shaft hole to fix the large arm connecting rod and the base rotating joint component 2 in a rotating mode; the third rotating shaft hole is horizontally arranged.
The large arm connecting rod comprises a right large arm joint plate 20, a left large arm joint plate 18, a large arm upper supporting plate 19 and a large arm lower supporting plate 25. The left large arm joint plate 18 is connected with a two-arm joint motor 21 through bolts, the right large arm joint plate 20 and the left large arm joint plate 18 are respectively fixedly connected with a large arm upper support plate 19 and a large arm lower support plate 25 through bolts, a large arm second connecting plate 26 is connected with the right large arm joint plate 20 through bolts, a large arm first connecting plate 28 is connected with a large arm second connecting plate 26 through bolts, and a large arm right shaft 27 is connected with a large arm first connecting plate 28 through bolts; the two-arm bearing support 24 is fixed in a third rotating shaft hole on the right large-arm joint plate 20 through bolt connection, the two-arm bearing 22 is installed in the two-arm bearing support 24, and the two-arm bearing end cover 23 is fixed on the two-arm bearing support 24 through bolt connection so as to fix the two-arm bearing 22 and prevent falling.
As shown in fig. 5, the two-arm rotating joint assembly 8 includes a two-arm joint motor 21, a two-arm connecting rod; a fourth rotating shaft hole and a fifth rotating shaft hole are respectively formed at two ends of the two-arm connecting rod; the two-arm joint motor 21 is fixed on the large arm connecting rod, and an output shaft of the two-arm joint motor 21 penetrates through the third rotating shaft hole and the fourth rotating shaft hole to fix the two-arm connecting rod and the large arm connecting rod in a rotating mode; the fifth rotating shaft hole is horizontally arranged.
The two-arm connecting rod comprises a left two-arm joint plate 30, a right two-arm joint plate 38, a two-arm upper supporting plate 32 and a two-arm lower joint plate 33. The three-arm joint motor 31 and the left two-arm joint plate 30 are fixed through bolts, the left two-arm joint plate 30 and the right two-arm joint plate 38 are respectively connected with the two-arm upper supporting plate 32 and the two-arm lower joint plate 33 through bolts, the three-arm bearing 29 is installed in a fifth rotating shaft hole of the right two-arm joint plate 38, the two-arm second connecting plate 34 is connected with the left two-arm joint plate 30 through bolts, the two-arm first connecting plate 35 is connected with the two-arm second connecting plate 34 through bolts, and the two-arm right shaft 36 is connected with the two-arm first connecting plate 35 through bolts.
As shown in fig. 6, the three-arm revolute joint assembly 4 includes a three-arm joint motor 31, a three-arm link; a sixth rotating shaft hole and a seventh rotating shaft hole are respectively formed in two ends of the three-arm connecting rod; the three-arm joint motor 31 is fixed on the two-arm connecting rod, and an output shaft of the three-arm joint motor 31 penetrates through the fifth rotating shaft hole and the sixth rotating shaft hole to fix the three-arm connecting rod and the two-arm connecting rod in a rotating mode; the seventh rotating shaft hole is horizontally arranged.
The three-arm connecting rod comprises a three-arm second connecting plate 41, a left three-arm joint plate 42, a right three-arm joint plate 50 and a three-arm upper supporting plate 43. The swing frame rotary joint motor 44 is connected with a left three-arm joint plate 42 through a bolt, the left three-arm joint plate 42 and a right three-arm joint plate 50 are respectively connected with a three-arm upper supporting plate 43 and a two-arm lower joint plate 49 through bolts, a three-arm second connecting plate 41 is connected with the left three-arm joint plate 42 through a bolt, a three-arm first connecting plate 39 is connected with a two-arm second connecting plate 41 through a bolt, a three-arm left shaft 40 is connected with a three-arm first connecting plate 39 through a bolt, a swing arm bearing 45 is installed in a bearing hole of the swing arm bearing support 46, and the swing arm bearing support 46 and a three-arm outer bearing end cover 47 are respectively installed on two sides of the right three-arm joint plate 50 and are fixed through bolts.
As shown in fig. 7, the swing frame revolute joint assembly 5 includes a revolute joint motor 44, a first swing frame; an eighth rotating shaft hole and a ninth rotating shaft hole are respectively formed at the two ends of the first swinging frame; the rotary joint motor 44 is fixed on the three-arm connecting rod, and an output shaft of the rotary joint motor 44 penetrates through the seventh rotating shaft hole and the eighth rotating shaft hole to fix the first swing frame and the three-arm connecting rod in a rotating mode; the ninth rotating shaft hole is horizontally arranged and is vertical to the eighth rotating shaft hole.
The first swing frame includes a left swing frame knuckle plate 51, a right swing frame knuckle plate 54, and a swing frame top plate 52. The left swing frame joint plate 51 and the right swing frame joint plate 54 are respectively fixed on two sides of the swing frame top plate 52 through bolts, the swing frame swing joint motor 53 is fixed in the swing frame top plate 52 through bolts, the swing frame rotating shaft 56 is fixed on the left swing frame joint plate 51 through bolts, and the swing arm left connecting shaft 55 is fixed on the right swing frame joint plate 54 through bolts.
As shown in fig. 7, the swing frame swing joint assembly 6 includes a swing joint motor 53, a second swing frame; a tenth rotating shaft hole and an eleventh rotating shaft hole are formed in the second swinging frame; the swing joint motor 53 is fixed on the first swing frame, and an output shaft of the swing joint motor 53 passes through the ninth rotating shaft hole and the tenth rotating shaft hole to rotationally fix the second swing frame and the first swing frame; the eleventh rotating shaft hole is perpendicular to the tenth rotating shaft hole.
As shown in fig. 8, the end swing joint assembly 7 includes a second swing motor 57, a swing frame 58; a twelfth rotating shaft hole is formed in the rotating frame; the second rotating motor 57 is fixed to the second swing frame, and an output shaft of the second rotating motor 57 passes through the eleventh rotating shaft hole and the twelfth rotating shaft hole to rotationally fix the swing frame 58 and the second swing frame.
The end joint motor 57 is fixed in the revolving frame 58 through a bolt connection, and the revolving joint end cover 59 is fixed on the rotating shaft of the end joint motor 57 through a bolt connection. In this embodiment, the turret 58 is L-shaped.
In this embodiment, big arm connecting rod, two arm connecting rods, three arm connecting rods are the fretwork support body to alleviate the dead weight, improve stability.
As shown in fig. 1 to 8, the base rotating main shaft 12 is fixed to a rotating shaft of the first rotating motor by means of bolt connection; the position B of the left large arm joint plate 18 is fixed on a rotating shaft of the large arm joint motor 14 through a bolt, and a large arm right shaft 27 is arranged in a hole A of a large arm bearing 16 of the base rotary joint component 2; the position D of the right two-arm joint plate 38 is connected with a two-arm joint motor through a bolt, and a two-arm right shaft 36 is arranged in a hole C of the two-arm bearing 22; the position F of the right three-arm joint plate 50 is fixed on the rotating shaft of the three-arm joint motor 31 through a bolt, and the three-arm left shaft 40 is arranged in the hole E of the three-arm bearing 29; a swing frame rotating shaft 56 is fixedly connected to a rotating shaft of the swing frame rotating joint motor 44 through a bolt, and a swing arm left connecting shaft 55 is installed in a G hole of the swing arm bearing 45; the swing joint 58 is fixed to the rotation shaft of the swing joint motor 53 of the swing frame by a bolt.
The working process is as follows: after the motor is connected with a power supply, an operator drags the tail end of the master hand by hands, an absolute value encoder in the master hand motor detects the rotating angle of each joint respectively, then the slave end master hand is controlled to move through the angle one-to-one correspondence, meanwhile, the slave end master hand feeds back the torque change of each joint in the moving process to the encoder in the master hand motor through equal scaling, and the output torque of the motor is changed in real time.
Example 2
This embodiment provides an inverse kinematics solution method with the dominant hand of embodiment 1 as the target. As shown in fig. 9 and 10, the relationship between the 7 joints of the master hand is: 1 st joint coordinate system x 1 z 1 y 1 And a base coordinate system x 0 z 0 y 0 Coincident, 2 nd joint coordinate system x 2 z 2 y 2 By a first joint coordinate system around x 1 The axis is rotated by 90 DEG and then wound by z 1 Rotated by 90 deg. and taken along x 1 Axis movement a1, 3 rd joint coordinate system x 3 z 3 y 3 By the 2 nd joint coordinate system around z 2 The axis being rotated 90 and along x 2 Axis movement a2, 4 th joint coordinate system x 4 z 4 y 4 From the 3 rd joint along x 3 Axis movement a3, 5 th joint coordinate system x 5 z 5 y 5 Along x by the 4 th joint 4 The axis is moved a4, the 6 th joint coordinate system is rotated 90 along the x-axis, 90 along the z-axis and along the x-axis from the 5 th joint coordinate system 5 Moving a5 along y 5 Moving-d 6, the 7 th joint coordinate system is rotated 90 around the x-axis from the 5 th joint coordinate system. The main parameters of the master hand are shown in table 1.
TABLE 17 redundant Primary DOF specific parameters
Figure GDA0003733663480000111
As shown in fig. 11, the 7-degree-of-freedom redundant master inverse kinematics solution method includes the following steps:
s1, establishing a D-H coordinate system and a kinematics matrix according to the configuration and the parameters of the master hand;
s2, solving the joint angle which does not form redundancy by the main hand by an algebraic method;
s3, drawing a connecting rod plan according to the redundant joints of the main hand;
s4, fixing a redundant joint, and enabling the angle of the redundant joint to be a known quantity;
and S5, solving the kinematic inverse solution of the redundant joint by adopting a method of relevant combination of a geometric method and an algebraic method.
As shown in FIG. 12, the 7-DOF redundant master hand redundant joint connecting rod plan view is established by simplifying the large arm joint, the two-arm joint, the three-arm joint and the swing arm rotating joint into a straight line and using a coordinate system X 2 O 2 Y 2 Is an origin, where OA ＝a 2 ，l AB ＝a 3 ，l BC ＝a 4 ，
Figure GDA0003733663480000112
Coordinates of point D are
Figure GDA0003733663480000113
The working process comprises the following steps: according to the secondary transformation equation between the connecting rod coordinate systems, a homogeneous coordinate transformation matrix of the tail end coordinate system relative to the base coordinate system can be obtained
Figure GDA0003733663480000114
Satisfies formula (1):
Figure GDA0003733663480000115
wherein the content of the first and second substances,
Figure GDA0003733663480000116
and a secondary coordinate transformation matrix representing the coordinate systems of two adjacent connecting rods, wherein n, o and a represent the postures of the terminal joint coordinate system relative to the polar coordinate system, and p represents the position of the terminal joint coordinate system relative to the polar coordinate system.
First, left-multiply simultaneously at both ends of equation (1)
Figure GDA0003733663480000121
Obtaining:
Figure GDA0003733663480000122
the equation can be established by equaling the elements in row 2 and column 4 of the two-terminal matrix of equation (2):
-s 1 p x +c 1 p y ＝0 (3)
the 1 st joint angle θ can be obtained from equation (3) 1 There are two solutions, respectively: atan2 (p) y ,p x ) And Atan2 (-p) y ,-p x ) (ii) a s represents sin, cRepresenting cos.
The equation can be established by equaling the elements in row 3 and column 2 of the matrix at both ends of equation (2):
c 6 ＝-s 1 a x +c 1 a y ＝k 1 (4)
the 6 th joint angle theta can be obtained from the formula (4) 6 Is defined by two solutions, respectively
Figure GDA0003733663480000123
Due to theta 1 With two solutions, theta is known 6 There are 4 solutions to the value of (c).
Then multiply right at both ends of equation (2) simultaneously
Figure GDA0003733663480000124
The following can be obtained:
Figure GDA0003733663480000125
from equation (5), the two-terminal matrix has the same 1 st row and 1 st column element and 3 rd row and 3 rd column element, we can obtain:
(n x +c 1 c 6 o z )c 7 +(c 1 c 6 n z -o x )s 7 ＝-s 1 s 6 (6)
let n be x +c 1 c 6 o z ＝k 2 ，c 1 c 6 n z -o x ＝k 3 Then the 7 th joint angle theta can be obtained 7 Is solved as
Figure GDA0003733663480000126
Selecting and fixing joint angle of the 5 th joint and setting theta 5 ＝90°。
Simultaneous right multiplication across equation (1)
Figure GDA0003733663480000127
The following can be obtained:
Figure GDA0003733663480000128
let the coordinates of point B in the base coordinate system be ( 4 p x ， 4 p y ， 4 p z ) The coordinates of the point B can be obtained as
Figure GDA0003733663480000129
Thereby obtaining
Figure GDA00037336634800001210
Then
Figure GDA00037336634800001211
The 3 rd joint angle theta can be obtained 3 Is solved as
Figure GDA00037336634800001212
Let equation (7) be equal for the 1 st row, 3 rd column element and the 2 nd row, 3 rd column element of the two-terminal matrix:
4 p x ＝a 1 c 1 +a 2 c 1 c 2 +a 3 c 1 c 23 (8)
let k be 5 ＝a 2 c 1 +a 3 c 1 c 3 ,k 6 ＝-a 3 c 1 s 3 ,k 7 ＝ 4 p x -a 1 c 1 The following can be obtained:
k 7 ＝k 5 c 2 +k 6 s 2 (9)
the 2 nd joint angle can be obtained from the formula (9)
Figure GDA0003733663480000131
Let the 1 st row, 3 rd column element and the 3 rd row, 3 rd column element of the matrix of equation (7) be equal, we can obtain:
s 234 ＝-(c 5 c 6 c 7 -s 5 s 7 )n z +(c 5 c 6 s 7 +s 5 c 7 )o z +(c 5 s 6 )a z (10)
c 234 ＝(s 5 c 6 c 7 +c 5 s 7 )n z -(-s 5 c 6 s 7 +s 5 c 7 )o z -(s 5 s 6 )a z (11)
θ 234 ＝atan2(s 234 ,c 234 ) (12)
the 4 th joint angle theta can be obtained from the equation (12) 4 ＝θ 234 -θ 2 -θ 3 。
Corresponding to the above method, this embodiment further provides a 7-degree-of-freedom redundant master inverse kinematics solving system, including the following steps:
the D-H coordinate system and kinematic matrix establishing module is used for establishing a D-H coordinate system and a kinematic matrix according to the configuration and the parameters of the main hand;
the joint angle solving module is used for solving the joint angle which does not form redundancy by the main hand by adopting an algebraic method;
the connecting rod plan drawing module is used for drawing a connecting rod plan according to the redundant joints of the main hand;
the redundant joint fixing module is used for fixing a redundant joint to enable the angle of the redundant joint to be a known quantity;
and the calculation module is used for solving the inverse kinematics solution of the redundant joint by adopting a method of relevant combination of a geometric method and an algebraic method.
As shown in FIG. 12, the 7-DOF redundant master hand redundant joint connecting rod plan view is established by simplifying the large arm joint, the two-arm joint, the three-arm joint and the swing arm rotating joint into a straight line and using a coordinate system X 2 O 2 Y 2 Is an origin, where OA ＝a 2 ，l AB ＝a 3 ，l BC ＝a 4 ，
Figure GDA0003733663480000132
D point coordinate is
Figure GDA0003733663480000133
The working process is as follows: according to the secondary transformation equation between the coordinate systems of the connecting rods, a homogeneous coordinate transformation matrix of the terminal coordinate system relative to the base coordinate system can be obtained
Figure GDA0003733663480000134
Satisfies formula (1):
Figure GDA0003733663480000141
wherein, the first and the second end of the pipe are connected with each other,
Figure GDA0003733663480000142
and a secondary coordinate transformation matrix representing the coordinate systems of two adjacent connecting rods.
First, left-multiply simultaneously at both ends of equation (1)
Figure GDA0003733663480000143
Obtaining:
Figure GDA0003733663480000144
the equation can be established by equaling the elements in row 2 and column 4 of the two-terminal matrix of equation (2):
-s 1 p x +c 1 p y ＝0 (3)
the 1 st joint angle θ can be obtained from equation (3) 1 There are two solutions, respectively: atan2 (p) y ,p x ) And Atan2 (-p) y ,-p x )；
The equation can be established by equality of the elements in the 3 rd row and the 2 nd column of the matrix at both ends of equation (2):
c 6 ＝-s 1 a x +c 1 a y ＝k 1 (4)
the 6 th joint angle theta can be obtained from the formula (4) 6 The value of (A) is given by two solutions, respectively
Figure GDA0003733663480000145
Due to theta 1 With two solutions, theta can be known 6 There are 4 solutions to the value of (c).
Then multiply right at both ends of equation (2) simultaneously
Figure GDA0003733663480000146
The following can be obtained:
Figure GDA0003733663480000147
from equation (5), the two-terminal matrix has the 1 st row, 1 st column element and the 3 rd row, 3 rd column element equal to each other:
(n x +c 1 c 6 o z )c 7 +(c 1 c 6 n z -o x )s 7 ＝-s 1 s 6 (6)
let n be x +c 1 c 6 o z ＝k 2 ，c 1 c 6 n z -o x ＝k 3 Then, the 7 th joint angle θ can be obtained 7 Is solved as
Figure GDA0003733663480000148
Selecting and fixing joint angle of the 5 th joint and setting theta 5 ＝90°。
Simultaneous right multiplication at both ends of equation (1)
Figure GDA0003733663480000149
The following can be obtained:
Figure GDA00037336634800001410
let the coordinates of point B in the base coordinate system be: ( 4 p x ， 4 p y ， 4 p z ) The coordinates of the point B can be obtained as
Figure GDA0003733663480000151
Thus, can obtain
Figure GDA0003733663480000152
Then the
Figure GDA0003733663480000153
The 3 rd joint angle theta can be obtained 3 Is solved as
Figure GDA0003733663480000154
Let equation (7) be equal for the 1 st row, 3 rd column element and the 2 nd row, 3 rd column element of the two-terminal matrix:
4 p x ＝a 1 c 1 +a 2 c 1 c 2 +a 3 c 1 c 23 (8)
let k 5 ＝a 2 c 1 +a 3 c 1 c 3 ,k 6 ＝-a 3 c 1 s 3 ,k 7 ＝ 4 p x -a 1 c 1 The following can be obtained:
k 7 ＝k 5 c 2 +k 6 s 2 (9)
the 2 nd joint angle can be obtained by the formula (9)
Figure GDA0003733663480000155
Let the 1 st row, 3 rd column element and the 3 rd row, 3 rd column element of the matrix of equation (7) be equal, we can obtain:
s 234 ＝-(c 5 c 6 c 7 -s 5 s 7 )n z +(c 5 c 6 s 7 +s 5 c 7 )o z +(c 5 s 6 )a z (10)
c 234 ＝(s 5 c 6 c 7 +c 5 s 7 )n z -(-s 5 c 6 s 7 +s 5 c 7 )o z -(s 5 s 6 )a z (11)
θ 234 ＝atan2(s 234 ,c 234 ) (12)
the 4 th joint angle θ can be obtained from the formula (12) 4 ＝θ 234 -θ 2 -θ 3 。
The above examples are only intended to illustrate the technical solution of the present invention, but not to limit it; although the present invention has been described in detail with reference to the foregoing embodiments, it will be understood by those of ordinary skill in the art that: the technical solutions described in the foregoing embodiments may still be modified, or some technical features may be equivalently replaced; and such modifications or substitutions do not depart from the spirit and scope of the corresponding technical solutions of the embodiments of the present invention.

Claims (6)

1. The seven-degree-of-freedom master-slave isomorphic teleoperation master hand is characterized by comprising a base component (1), a base rotating joint component (2), a large-arm rotating joint component (3), a two-arm rotating joint component (8), a three-arm rotating joint component (4), a swing frame rotating joint component (5), a swing frame swing joint component (6) and a tail end rotating joint component (7);
the base component (1), the base rotating joint component (2), the large-arm rotating joint component (3), the two-arm rotating joint component (8), the three-arm rotating joint component (4), the swing frame rotating joint component (5), the swing frame swing joint component (6) and the tail end rotating joint component (7) are sequentially fixed in an end-to-end rotating mode to form seven joints, and each joint is driven by a motor; the motor integrates a driver and an encoder into a whole;
the relationship of the 7 joints is: 1 st joint coordinate system x 1 z 1 y 1 And a base coordinate system x 0 z 0 y 0 Coincident, 2 nd joint coordinate system x 2 z 2 y 2 By the first joint coordinate system around x 1 The axis is rotated by 90 DEG and then wound around z 1 Rotated 90 deg. and taken along x 1 Axis movement a1, 3 rd joint coordinate system x 3 z 3 y 3 Around z by the 2 nd joint coordinate system 2 The axis being rotated 90 and along x 2 Axis movement a2, 4 th joint coordinate system x 4 z 4 y 4 From the 3 rd joint along x 3 Axis movement a3, 5 th joint coordinate system x 5 z 5 y 5 Along x by the 4 th joint 4 The axis is moved a4, the 6 th joint coordinate system is rotated 90 along the x-axis, 90 along the z-axis and 90 along the x-axis from the 5 th joint coordinate system 5 Moving a5 along y 5 Moving-d 6, the 7 th joint coordinate system is rotated 90 degrees around the x-axis from the 6 th joint coordinate system; all joints rotate by taking respective z-axis as a rotation center;
the inverse kinematics solving method of the teleoperation master hand comprises the following steps:
s1, establishing a D-H coordinate system and a kinematic matrix according to the configuration and the parameters of the master hand;
s2, solving the joint angle of which the main hand does not form redundancy by adopting an algebraic method;
s3, drawing a connecting rod plane diagram according to the redundant joints of the main hand;
s4, fixing a redundant joint, and enabling the angle of the redundant joint to be a known quantity;
s5, solving the inverse kinematics solution of the redundant joint by adopting a method of relevant combination of a geometric method and an algebraic method;
the solving method of S1 comprises the following steps:
s11, obtaining a homogeneous coordinate transformation matrix of the end coordinate system relative to the base coordinate system according to the secondary transformation equation between the link coordinate systems
Figure FDA0003733663470000011
Satisfies the equation (1):
Figure FDA0003733663470000012
wherein the content of the first and second substances,
Figure FDA0003733663470000013
a secondary coordinate transformation matrix representing coordinate systems of two adjacent connecting rods; the connecting rod is a connecting piece between two adjacent joints; n, o, a represents the attitude of the terminal joint coordinate system relative to the polar coordinate system, and p represents the position of the terminal joint coordinate system relative to the polar coordinate system;
the solving method of S2 comprises the following steps:
s21, first, the two ends of equation (1) are simultaneously multiplied by
Figure FDA0003733663470000021
Obtaining:
Figure FDA0003733663470000022
the equation can be established by equaling the elements in row 2 and column 4 of the two-terminal matrix of equation (2):
-s 1 p x +c 1 p y ＝0(3)
the 1 st joint angle θ can be obtained from equation (3) 1 There are two solutions, respectively: atan2 (p) y ,p x ) And Atan2 (-p) y ,-p x )；s 1 Denotes sin (theta1), c 1 Represents cos (theta 1);
the equation can be established by equality of the elements in the 3 rd row and the 2 nd column of the matrix at both ends of equation (2):
c 6 ＝-s 1 a x +c 1 a y ＝k 1 (4)
the 6 th joint angle θ can be obtained from equation (4) 6 Has two solutions of
Figure FDA0003733663470000023
Due to theta 1 With two solutions, theta is known 6 There are 4 solutions to the value of (c);
then multiply right at both ends of equation (2) simultaneously
Figure FDA0003733663470000024
The following can be obtained:
Figure FDA0003733663470000025
from equation (5), the two-terminal matrix has the 1 st row, 1 st column element and the 3 rd row, 3 rd column element equal to each other:
(n x +c 1 c 6 o z )c 7 +(c 1 c 6 n z -o x )s 7 ＝-s 1 s 6 (6)
let n be x +c 1 c 6 o z ＝k 2 ，c 1 c 6 n z -o x ＝k 3 Then the 7 th joint angle theta can be obtained 7 Is solved as
Figure FDA0003733663470000026
The solving method of S3 comprises the following steps:
s31, obtaining l according to the plane view of the connecting rod OA ＝a 2 ，l AB ＝a 3 ，l BC ＝a 4 ，
Figure FDA0003733663470000027
Coordinates of point D are
Figure FDA0003733663470000028
S4 fixing the joint angle of the 5 th joint; the solving method of S5 comprises the following steps:
s51, right multiplication at both ends of equation (1) simultaneously
Figure FDA0003733663470000029
The following can be obtained:
Figure FDA00037336634700000210
let the coordinates of point B in the base coordinate system be: ( 4 p x ， 4 p y ， 4 p z ) The coordinates of the point B can be obtained as
Figure FDA0003733663470000031
Thereby can beTo obtain
Figure FDA0003733663470000032
Then
Figure FDA0003733663470000033
The 3 rd joint angle theta can be obtained 3 Is solved as
Figure FDA0003733663470000034
S52, the two-terminal matrix is equal in the 1 st row, 3 rd column element and the 2 nd row, 3 rd column element according to equation (7), so that:
4 p x ＝a 1 c 1 +a 2 c 1 c 2 +a 3 c 1 c 23 (8)
let k 5 ＝a 2 c 1 +a 3 c 1 c 3 ,k 6 ＝-a 3 c 1 s 3 ,k 7 ＝ 4 p x -a 1 c 1 The following can be obtained:
k 7 ＝k 5 c 2 +k 6 s 2 (9)
the 2 nd joint angle can be obtained by the equation (9)
Figure FDA0003733663470000035
S53, the matrix is equal in row 1, column 3 and row 3, column 3 according to equation (7):
s 234 ＝-(c 5 c 6 c 7 -s 5 s 7 )n z +(c 5 c 6 s 7 +s 5 c 7 )o z +(c 5 s 6 )a z (10)
c 234 ＝(s 5 c 6 c 7 +c 5 s 7 )n z -(-s 5 c 6 s 7 +s 5 c 7 )o z -(s 5 s 6 )a z (11)
θ 234 ＝atan2(s 234 ,c 234 )(12)
the 4 th joint angle theta can be obtained from equation (12) 4 ＝θ 234 -θ 2 -θ 3 。
2. The seven-degree-of-freedom master-slave isomorphic teleoperational master hand according to claim 1, characterized in that the base assembly (1) comprises a base (9), a base rotary motor (10), a base end cap (11); an accommodating cavity for accommodating a base rotary motor (10) is arranged in the base (9); the base end cover (11) is fixed at the top of the base (9); an output shaft of the rotary motor (10) extends upwards out of the base (9) and the base end cover (11);
the base rotating joint component (2) comprises a base rotating main shaft (12) and a base rotating frame; the base revolving frame is provided with a first revolving shaft hole; the base rotates the vertical output shaft fixed connection with first rotating electrical machines (10) of base of main shaft (12), first pivot hole is the level setting.
3. The seven-degree-of-freedom master-slave isomorphic teleoperation master hand according to claim 2, wherein the big arm rotation joint assembly (3) comprises a big arm joint motor (14), a big arm connecting rod; one end of the big arm connecting rod is provided with a second rotating shaft hole, and the other end of the big arm connecting rod is provided with a third rotating shaft hole; the large arm joint motor (14) is fixed on the base revolving frame, and an output shaft of the large arm joint motor (14) penetrates through the first rotating shaft hole and the second rotating shaft hole to rotationally fix the large arm connecting rod and the base rotating joint component (2); the third rotating shaft hole is horizontally arranged;
the two-arm rotating joint component (8) comprises a two-arm joint motor (21) and a two-arm connecting rod; a fourth rotating shaft hole and a fifth rotating shaft hole are respectively formed in two ends of the two-arm connecting rod; the two-arm joint motor (21) is fixed on the large arm connecting rod, and an output shaft of the two-arm joint motor (21) penetrates through the third rotating shaft hole and the fourth rotating shaft hole to fix the two-arm connecting rod and the large arm connecting rod in a rotating mode; the fifth rotating shaft hole is horizontally arranged.
4. A seven degree-of-freedom master-slave isomorphic teleoperational master hand according to claim 3, characterized in that the three arm revolute joint assembly (4) comprises a three arm joint motor (31), a three arm link; a sixth rotating shaft hole and a seventh rotating shaft hole are respectively formed in two ends of the three-arm connecting rod; the three-arm joint motor (31) is fixed on the two-arm connecting rod, and an output shaft of the three-arm joint motor (31) penetrates through the fifth rotating shaft hole and the sixth rotating shaft hole to fix the three-arm connecting rod and the two-arm connecting rod in a rotating mode; the seventh rotating shaft hole is horizontally arranged;
the swing frame rotating joint assembly (5) comprises a rotating joint motor (44) and a first swing frame; an eighth rotating shaft hole and a ninth rotating shaft hole are respectively formed at the two ends of the first swinging frame; the rotary joint motor (44) is fixed on the three-arm connecting rod, and an output shaft of the rotary joint motor (44) penetrates through the seventh rotating shaft hole and the eighth rotating shaft hole to fix the first swinging frame and the three-arm connecting rod in a rotating mode; the ninth rotating shaft hole is horizontally arranged and is vertical to the eighth rotating shaft hole.
5. The seven degree-of-freedom master-slave isomorphic teleoperation master hand of claim 4, wherein the swing frame swing joint assembly (6) comprises a swing joint motor (53), a second swing frame; a tenth rotating shaft hole and an eleventh rotating shaft hole are formed in the second swinging frame; the swing joint motor (53) is fixed on the first swing frame, and an output shaft of the swing joint motor (53) penetrates through the ninth rotating shaft hole and the tenth rotating shaft hole to rotationally fix the second swing frame and the first swing frame; the eleventh rotating shaft hole is vertical to the tenth rotating shaft hole;
the tail end rotary joint assembly (7) comprises a second rotary motor (57) and a rotary frame; a twelfth rotating shaft hole is formed in the rotating frame; the second rotating motor (57) is fixed on the second swinging frame, and an output shaft of the second rotating motor (57) penetrates through the eleventh rotating shaft hole and the twelfth rotating shaft hole to fix the rotating frame and the second swinging frame in a rotating mode.
6. The seven-degree-of-freedom master-slave isomorphic teleoperation master hand according to claim 5, wherein the large arm connecting rod, the two-arm connecting rod and the three-arm connecting rod are all hollow frame bodies.
CN202111069388.7A 2021-09-13 2021-09-13 Seven-degree-of-freedom master-slave isomorphic teleoperation master hand Active CN113733037B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111069388.7A CN113733037B (en) 2021-09-13 2021-09-13 Seven-degree-of-freedom master-slave isomorphic teleoperation master hand

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111069388.7A CN113733037B (en) 2021-09-13 2021-09-13 Seven-degree-of-freedom master-slave isomorphic teleoperation master hand

Publications (2)

Publication Number Publication Date
CN113733037A CN113733037A (en) 2021-12-03
CN113733037B true CN113733037B (en) 2022-09-09

Family

ID=78738423

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111069388.7A Active CN113733037B (en) 2021-09-13 2021-09-13 Seven-degree-of-freedom master-slave isomorphic teleoperation master hand

Country Status (1)

Country Link
CN (1) CN113733037B (en)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115284319A (en) * 2022-08-22 2022-11-04 北京航空航天大学 Control method of manual control equipment and manual controller

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106844951A (en) * 2017-01-18 2017-06-13 哈尔滨工业大学深圳研究生院 The method and system of super redundant robot's inverse kinematics are solved based on segmentation geometric method
CN107589934A (en) * 2017-07-24 2018-01-16 大连理工大学 A kind of acquiring method of articulated manipulator inverse kinematics parsing solution
CN108121833A (en) * 2016-11-29 2018-06-05 沈阳新松机器人自动化股份有限公司 A kind of redundant degree of freedom robot is against solution method
CN109048897A (en) * 2018-08-14 2018-12-21 清华大学深圳研究生院 A kind of method of principal and subordinate's teleoperation of robot
CN109129469A (en) * 2018-08-01 2019-01-04 珠海格力电器股份有限公司 The inverse solution method, apparatus of Mechanical transmission test and mechanical arm
CN112091979A (en) * 2020-10-10 2020-12-18 哈尔滨工业大学 Seven-degree-of-freedom mechanical arm limiting optimization method based on position-level inverse kinematics
CN112828916A (en) * 2021-01-20 2021-05-25 清华大学深圳国际研究生院 Remote operation combined interaction device for redundant mechanical arm and remote operation system for redundant mechanical arm
CN113127989A (en) * 2021-04-22 2021-07-16 中国科学院沈阳自动化研究所 Six-degree-of-freedom mechanical arm inverse kinematics analysis solution control method

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP4735795B2 (en) * 2003-12-26 2011-07-27 独立行政法人 宇宙航空研究開発機構 Redundant manipulator control method

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108121833A (en) * 2016-11-29 2018-06-05 沈阳新松机器人自动化股份有限公司 A kind of redundant degree of freedom robot is against solution method
CN106844951A (en) * 2017-01-18 2017-06-13 哈尔滨工业大学深圳研究生院 The method and system of super redundant robot's inverse kinematics are solved based on segmentation geometric method
CN107589934A (en) * 2017-07-24 2018-01-16 大连理工大学 A kind of acquiring method of articulated manipulator inverse kinematics parsing solution
CN109129469A (en) * 2018-08-01 2019-01-04 珠海格力电器股份有限公司 The inverse solution method, apparatus of Mechanical transmission test and mechanical arm
CN109048897A (en) * 2018-08-14 2018-12-21 清华大学深圳研究生院 A kind of method of principal and subordinate's teleoperation of robot
CN112091979A (en) * 2020-10-10 2020-12-18 哈尔滨工业大学 Seven-degree-of-freedom mechanical arm limiting optimization method based on position-level inverse kinematics
CN112828916A (en) * 2021-01-20 2021-05-25 清华大学深圳国际研究生院 Remote operation combined interaction device for redundant mechanical arm and remote operation system for redundant mechanical arm
CN113127989A (en) * 2021-04-22 2021-07-16 中国科学院沈阳自动化研究所 Six-degree-of-freedom mechanical arm inverse kinematics analysis solution control method

Also Published As

Publication number Publication date
CN113733037A (en) 2021-12-03

Similar Documents

Publication Publication Date Title
CN103753601B (en) Teleoperation mechanical arm of space cascade rotary joint type and combination thereof
CN109397244B (en) Integrated double-7-degree-of-freedom mechanical arm omnidirectional mobile robot system and control method
CN104965517B (en) A kind of planing method of robot cartesian space track
CN203649702U (en) Three-lever five-DOF enlarged range shifting welding robot with function of model decoupling
CN113733037B (en) Seven-degree-of-freedom master-slave isomorphic teleoperation master hand
CN103737582A (en) High-precision advanced welding robot mechanism with six degrees of freedom
Bogdanov et al. Control system of a manipulator of the anthropomorphic robot fedor
CN114714327A (en) Fusion system of mechanical arm and dexterous hand and motion control method
KR101787865B1 (en) Inverse kinematic solution for multi-joint link mechanism, and device for creating instructional data by using inverse kinematic solution
JP2006198703A (en) Arm driving device of human type robot
CN113722864B (en) 7-degree-of-freedom redundant mechanical arm inverse kinematics solving method and system
CN207223974U (en) A kind of six degree of freedom biomimetic manipulator
CN215881611U (en) Seven-degree-of-freedom master-slave isomorphic teleoperation master hand
Esque et al. The use of virtual prototyping and simulation in ITER maintenance device development
US6978193B2 (en) Positioning data calculating procedure, positioning data calculating apparatus and articulated manipulator
JP4268035B2 (en) Industrial robot and control method thereof
CN108068108B (en) Method for controlling motion of in-plane swinging telescopic robot
Zhang et al. Design and implementation of a 6 DOF delta force feedback controller for tele-operation
CN102909504A (en) Intersecting line cutting robot mechanism for container adapter tube
Li et al. Design of a Control system Based on Beck Hoff TwinCAT Nursing Robot
Yan et al. A modular manipulator system using rotary joints toward helping the elderly and the handicapped
Anirudh et al. An Application of a 3-RRRS 6 DOF Parallel Manipulator
CN117733862A (en) High-freedom-degree control equipment
KR101297071B1 (en) Multi-joint manipulator
Liu et al. INVERSE DYNAMIC MODELING AND SIMULATION OF MULTIPLED-EGREE-OF-FREEDOM HEAVY-DUTY HYDRAULIC MANIPULATOR

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