CN117817659A - Inverse kinematics analysis solution method for solving shoulder and elbow angles by cooperative mechanical arm - Google Patents
Inverse kinematics analysis solution method for solving shoulder and elbow angles by cooperative mechanical arm Download PDFInfo
- Publication number
- CN117817659A CN117817659A CN202311724301.4A CN202311724301A CN117817659A CN 117817659 A CN117817659 A CN 117817659A CN 202311724301 A CN202311724301 A CN 202311724301A CN 117817659 A CN117817659 A CN 117817659A
- Authority
- CN
- China
- Prior art keywords
- joint
- mechanical arm
- robot
- roty
- axis
- 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.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 55
- 238000004458 analytical method Methods 0.000 title claims description 7
- 238000006073 displacement reaction Methods 0.000 claims description 55
- 230000015654 memory Effects 0.000 claims description 33
- 206010069747 Burkholderia mallei infection Diseases 0.000 claims description 30
- 238000003860 storage Methods 0.000 claims description 25
- 238000004422 calculation algorithm Methods 0.000 claims description 19
- 238000004364 calculation method Methods 0.000 claims description 13
- 230000008569 process Effects 0.000 claims description 10
- 238000006467 substitution reaction Methods 0.000 claims description 5
- 238000004590 computer program Methods 0.000 description 12
- 238000010586 diagram Methods 0.000 description 6
- 239000011159 matrix material Substances 0.000 description 6
- 230000003287 optical effect Effects 0.000 description 4
- 239000012636 effector Substances 0.000 description 3
- 238000004519 manufacturing process Methods 0.000 description 3
- 230000009466 transformation Effects 0.000 description 3
- 210000000707 wrist Anatomy 0.000 description 3
- 238000004891 communication Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 239000013307 optical fiber Substances 0.000 description 2
- 239000002245 particle Substances 0.000 description 2
- 230000000644 propagated effect Effects 0.000 description 2
- 230000001360 synchronised effect Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000005094 computer simulation Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000006870 function Effects 0.000 description 1
- 230000002068 genetic effect Effects 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
- 238000005192 partition Methods 0.000 description 1
- 230000000750 progressive effect Effects 0.000 description 1
- 239000004065 semiconductor Substances 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
Classifications
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02P—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN THE PRODUCTION OR PROCESSING OF GOODS
- Y02P90/00—Enabling technologies with a potential contribution to greenhouse gas [GHG] emissions mitigation
- Y02P90/02—Total factory control, e.g. smart factories, flexible manufacturing systems [FMS] or integrated manufacturing systems [IMS]
Landscapes
- Numerical Control (AREA)
Abstract
The invention provides a method for solving inverse kinematics of shoulder and elbow angles by a cooperative mechanical arm, which comprises the following steps: establishing a mechanical arm joint coordinate system, configuring a shoulder-elbow-wrist structure according to 6 degrees of freedom of the robot, and respectively defining the 0 th to 6 th joints from a base to the tail end; the direction of the mechanical arm is controlled through the 4 th shaft, the 5 th shaft and the 6 th shaft of the robot, and the joint angles corresponding to the 4 th joint, the 5 th joint and the 6 th joint are obtained; and establishing an inverse kinematics equation according to the tail end coordinates of the robot and the angles of the joints corresponding to the 4 th joint, the 5 th joint and the 6 th joint, and calculating the angles corresponding to the 1 st joint, the 2 nd joint and the 3 rd joint. Therefore, the angles corresponding to the 1 st joint, the 2 nd joint and the 3 rd joint of the mechanical arm can be solved very rapidly, the mechanical arm is controlled to pass through the singular point smoothly, and the continuity and the stability of the movement of the mechanical arm are ensured.
Description
Technical Field
The invention relates to intelligent manufacturing and high-end manufacturing, in particular to a method for solving inverse kinematics analysis of angles of shoulder and elbow by a cooperative mechanical arm.
Background
At present, the inverse kinematics problem of the articulated mechanical arm is that under the condition that the position and the gesture of the mechanical arm end effector coordinate system relative to the base coordinate system and the geometrical parameters of all mechanical arm joints are given, the angle values of all mechanical arm joints are obtained, and the inverse process of the forward kinematics is realized. The forward kinematics can obtain a homogeneous coordinate transformation matrix T between the front and rear adjacent joint coordinate systems according to the structural geometric parameters of the mechanical arm, namely a pose matrix between two joints; if the rotation angle theta of each joint is known, the pose matrix of the mechanical arm end effector coordinate system can be obtained by sequentially and continuously right multiplying the joint homogeneous transformation matrix T, and the obtained result is unique. The solution of inverse kinematics is relatively complex and may have no solution or multiple solutions, such as when the end effector coordinate system is at a singular point, and the periodicity of the inverse trigonometric function results in the mechanical arm joint rotation angle resolving solution to the theoretical multiple solution problem.
The wrist of most articulated six-degree-of-freedom mechanical arms meets the Pieper criterion, i.e. the axes of rotation of the 3 joints adjacent to the wrist intersect at a point, then the 3 joints adjacent to the wrist are decoupled, while the co-operating mechanical arm has 3 parallel axes. The general coordinate system modeling method in the robot field is mainly a Denavit-Hartenberg parameter method, namely a DH parameter method for short, namely a matrix method for establishing a coordinate system for each rod piece in a joint chain of a mechanical arm, wherein the method describes coordinate directions and geometric parameters between adjacent joints, and is observed. For example, tao Maosheng et al discloses a "inverse kinematics solution method for 6-degree-of-freedom industrial robot", patent No. CN105573143a, which requires that a solution with the smallest sum of the norms of the difference values of the rotation axis angles of the joints corresponding to the position of the previous joint space be dissociated from eight sets of results corresponding to the rotation angles of the rotation axes of the joints obtained. However, the obtained norm cannot be brought into calculation again, but is just taken as a judgment basis for selecting a better solution from a plurality of inverse solutions, and the calculation accuracy and the calculation speed cannot meet the requirements of modern production. Zhu Jidan et al discloses a quick and simple method for solving inverse kinematics solutions of a 6-degree-of-freedom mechanical arm, which adopts an Euler angle transformation matrix to solve the problem of universal joint locking of the method, and patent number CN 103942427A. In addition, "service robot inverse kinematics solving method and device under intelligent space" disclosed by Congress et al, and "robot inverse kinematics solving method based on particle swarm optimization algorithm" disclosed by Lv Yahui et al, adopt intelligent algorithms such as genetic algorithm and particle swarm algorithm to carry out inverse kinematics solving, have complex program, require high-performance computing configuration, and have the problem of insufficient stability.
Disclosure of Invention
Aiming at the defects in the prior art, the invention aims to provide a method for solving inverse kinematics analysis of shoulder and elbow angles by a cooperative mechanical arm.
In a first aspect, an embodiment of the present application provides a method for solving inverse kinematics of a shoulder and elbow angle by using a cooperative mechanical arm, including:
step 1: establishing a mechanical arm joint coordinate system, configuring a shoulder-elbow-wrist structure according to six degrees of freedom of the robot, and defining the 0 th to 6 th joints from the base to the tail end respectively;
step 2: the direction of the mechanical arm is controlled through the 4 th shaft, the 5 th shaft and the 6 th shaft of the robot, and the joint angles corresponding to the 4 th joint, the 5 th joint and the 6 th joint are obtained;
step 3: and establishing an inverse kinematics equation according to the tail end coordinates of the robot and the angles of the joints corresponding to the 4 th joint, the 5 th joint and the 6 th joint, and calculating the angles corresponding to the 1 st joint, the 2 nd joint and the 3 rd joint.
Optionally, the step 1 includes:
establishing a rectangular coordinate system by taking the position corresponding to the robot base as an origin; the tail end coordinates of the robot are target coordinate positions which the executing end of the mechanical arm needs to reach.
Optionally, the step 2 includes:
according to the known tail end coordinates of the robot, the orientation of the mechanical arm is controlled by adjusting the 4 th axis, the 5 th axis and the 6 th axis of the robot, and when the mechanical arm is oriented to a preset orientation, the joint angles corresponding to the 4 th joint, the 5 th joint and the 6 th joint are recorded respectively.
Optionally, the step 3 includes:
constructing a positive kinematic equation of the robot, wherein the calculation formula of the tail end coordinates of the robot is as follows:
t 0T ＝t 01 +rotz(q 1 )*(t 12 +roty(q 2 )*(t 23 +roty(q 3 )*(t 34 +roty(q 4 )*(t 45 +rotz(q 5 )*(t 56 +roty(q 6 )*t 6T )))))
wherein: t is t 0T Representing the displacement of the end flange relative to joint 0, t 01 Represents the displacement of the 1 st joint relative to the 0 th joint, rotz (q 1 ) Representing rotation q about the Z-axis 1 Angle, t 12 Represents the displacement of the 2 nd joint relative to the 1 st joint, the roty (q 2 ) Representing rotation q about the Y-axis 2 Angle, t 23 Represents the displacement of the 3 rd joint relative to the 2 nd joint, the roty (q 3 ) Representing rotation q about the Y-axis 3 Angle, t 34 Represents the displacement of the 4 th joint relative to the 3 rd joint, the roty (q 4 ) Representing rotation q about the Y-axis 4 Angle, t 45 Represents the displacement of the 5 th joint relative to the 4 th joint, rotz (q 5 ) Representing rotation q about the Z-axis 5 Angle, t 56 Represents the displacement of the 6 th joint relative to the 5 th joint, the roty (q 6 ) Representing rotation q about the Y-axis 6 Angle, t 6T Representing the displacement of the end flange relative to joint 6; wherein:
when the joint angles q corresponding to the 4 th joint, the 5 th joint and the 6 th joint are 4 、q 5 、q 6 When it is known that the number of the cells,
t 4T ＝roty(q 4 )*(t 45 +rotz(q 5 )*(t 56 +roty(q 6 )*t 6T ))
wherein: t is t 4T Representing the displacement of the end flange relative to joint 4, will be t 4T Substitution into t 0T The calculation formula of (2) is obtained as follows:
t 0T ＝t 01 +rotz(q 1 )*(t 12 +roty(q 2 )*(t 23 +roty(q 3 )*(t 34 +t 4T )))
the process of solving inverse kinematics: reversely calculating the joint angles q corresponding to the 1 st joint, the 2 nd joint and the 3 rd joint 1 、q 2 、q 3 ；
Let t 1T ＝t 0T -t 01 ；
Wherein: t is t 1T Representing the displacement of the end flange relative to joint 1, will be t 1T Is marked as [ t ] 1x ，t 1y ，t 1z ]，t 1x Representing coordinates about the X-axis, t 1y Representing the coordinates about the Y-axis, t 1z Representing coordinates about the Z-axis;
let T 1T ＝t 12 +roty(q 2 )*(t 23 +roty(q 3 )*t 3T )；
Wherein: t is t 3T Representing the displacement of the end flange relative to joint 3, will be T 1T Is marked as [ T ] 1x ，T 1y ，T 1z ]；
Since the component of the corresponding axis remains unchanged when a certain 1-coordinate rotates around the corresponding axis, let t be 12 、t 23 、t 3T The variables about the Y-axis direction are t respectively 12y 、t 23y 、t 3Ty T is then 1T The coordinates of (a) have the following relationship:
T 1z ＝t 1z ；
T 1y ＝t 12y +t 23y +t 3Ty ；
wherein T is 1x ＝+/-sqrt(t 1y 2 +t 1x 2 -T 1y 2 )；
When T is 1T When known, q is calculated according to Paden-Kahan subsubbreem algorithm 1 ；
When q 1 When known, there is ||T 1T -t 12 ‖＝‖t 23 +roty(q 3 )*t 3T II, q is calculated according to Paden-Kahan subsuble algorithm 3 ；
When q 3 When known, according to T 1T -t 12 ＝roty(q 2 )*(t 23 +roty(q 3 )*t 3T ) And q is calculated by Paden-Kahan subsubbreb algorithm 2 。
Alternatively, q 1 There are at most two sets of solutions, q 3 There are at most two sets of solutions, q 2 Only 1 set of solutions, thus yielding a maximum of 4 setsDifferent q 1 、q 2 、q 3 Angle.
In a second aspect, an embodiment of the present application provides a device for solving inverse kinematics of a shoulder and elbow angle by using a cooperative mechanical arm, including:
the joint coordinate system building module is used for building a mechanical arm joint coordinate system, configuring a shoulder-elbow-wrist structure according to 6 degrees of freedom of the robot, and defining the 0 th to 6 th joints from the base to the tail end respectively;
the mechanical arm orientation adjusting module is used for controlling the orientation of the mechanical arm through a 4 th shaft, a 5 th shaft and a 6 th shaft of the robot to obtain joint angles corresponding to a 4 th joint, a 5 th joint and a 6 th joint;
the joint angle operation module is used for establishing an inverse kinematics equation according to the tail end coordinates of the robot and the joint angles corresponding to the 4 th joint, the 5 th joint and the 6 th joint, and calculating the angles corresponding to the 1 st joint, the 2 nd joint and the 3 rd joint.
Optionally, the joint coordinate system establishment module is specifically configured to:
establishing a rectangular coordinate system by taking the position corresponding to the robot base as an origin; the tail end coordinates of the robot are target coordinate positions which the executing end of the mechanical arm needs to reach.
Optionally, the mechanical arm orientation adjusting module is specifically configured to:
according to the known tail end coordinates of the robot, the orientation of the mechanical arm is controlled by adjusting the 4 th axis, the 5 th axis and the 6 th axis of the robot, and when the mechanical arm is oriented to a preset orientation, the joint angles corresponding to the 4 th joint, the 5 th joint and the 6 th joint are recorded respectively.
Optionally, the joint angle operation module is specifically configured to:
constructing a positive kinematic equation of the robot, wherein the calculation formula of the tail end coordinates of the robot is as follows:
t 0T ＝t 01 +rotz(q 1 )*(t 12 +roty(q 2 )*(t 23 +roty(q 3 )*(t 34 +roty(q 4 )*(t 45 +rotz(q 5 )*(t 56 +roty(q 6 )*t 6T )))))
wherein: t is t 0T Representing the displacement of the end flange relative to joint 0, t 01 Represents the displacement of the 1 st joint relative to the 0 th joint, rotz (q 1 ) Representing rotation q about the Z-axis 1 Angle, t 12 Represents the displacement of the 2 nd joint relative to the 1 st joint, the roty (q 2 ) Representing rotation q about the Y-axis 2 Angle, t 23 Represents the displacement of the 3 rd joint relative to the 2 nd joint, the roty (q 3 ) Representing rotation q about the Y-axis 3 Angle, t 34 Represents the displacement of the 4 th joint relative to the 3 rd joint, the roty (q 4 ) Representing rotation q about the Y-axis 4 Angle, t 45 Represents the displacement of the 5 th joint relative to the 4 th joint, rotz (q 5 ) Representing rotation q about the Z-axis 5 Angle, t 56 Represents the displacement of the 6 th joint relative to the 5 th joint, the roty (q 6 ) Representing rotation q about the Y-axis 6 Angle, t 6T Representing the displacement of the end flange relative to joint 6; wherein:
when the joint angles q corresponding to the 4 th joint, the 5 th joint and the 6 th joint are 4 、q 5 、q 6 When it is known that the number of the cells,
t 4T ＝roty(q 4 )*(t 45 +rotz(q 5 )*(t 56 +roty(q 6 )*t 6T ))
wherein: t is t 4T Representing the displacement of the end flange relative to joint 4, will be t 4T Substitution into t 0T The calculation formula of (2) is obtained as follows:
t 0T ＝t 01 +rotz(q 1 )*(t 12 +roty(q 2 )*(t 23 +roty(q 3 )*(t 34 +t 4T )))
the process of solving inverse kinematics: reversely calculating the joint angles q corresponding to the 1 st joint, the 2 nd joint and the 3 rd joint 1 、q 2 、q 3 ；
Let t 1T ＝t 0T -t 01 ；
Wherein: t is t 1T Representing the displacement of the end flange relative to joint 1, will be t 1T Is marked as [ t ] 1x ，t 1y ，t 1z ]，t 1x Representing coordinates about the X-axis, t 1y Representing the coordinates about the Y-axis, t 1z Representing coordinates about the Z-axis;
let T 1T ＝t 12 +roty(q 2 )*(t 23 +roty(q 3 )*t 3T )；
Wherein: t is t 3T Representing the displacement of the end flange relative to joint 3, will be T 1T Is marked as [ T ] 1x ，T 1y ，T 1z ]；
Since the component of the corresponding axis remains unchanged when a certain coordinate rotates around the corresponding axis, let t be 12 、t 23 、t 3T The variables about the Y-axis direction are t respectively 12y 、t 23y 、t 3Ty T is then 1T The coordinates of (a) have the following relationship:
T 1z ＝t 1z ；
T 1y ＝t 12y +t 23y +t 3Ty ；
wherein T is 1x ＝+/-sqrt(t 1y 2 +t 1x 2 -T 1y 2 )；
When T is 1T When known, q is calculated according to Paden-Kahan subsubbreem algorithm 1 ；
When q 1 When known, there is ||T 1T -t 12 ‖＝‖t 23 +roty(q 3 )*t 3T II, q is calculated according to Paden-Kahan subsuble algorithm 3 ；
When q 3 When known, according to T 1T -t 12 ＝roty(q 2 )*(t 23 +roty(q 3 )*t 3T ) And q is calculated by Paden-Kahan subsubbreb algorithm 2 。
Alternatively, q 1 There are at most two sets of solutions, q 3 There are at most two sets of solutions, q 2 There is only one set of solutions, thus yielding a maximum of four different sets of q 1 、q 2 、q 3 Angle.
In a third aspect, an embodiment of the present application provides a collaborative mechanical arm solution shoulder elbow angle inverse kinematics solution device, including: the system comprises a processor and a memory, wherein executable program instructions are stored in the memory, and when the processor calls the program instructions in the memory, the processor is used for:
a step of performing a collaborative robotic arm solution shoulder-elbow angle inverse kinematics solution as described in any of the first aspects.
In a fourth aspect, embodiments of the present application provide a robot, including: the robot comprises a robot body, a driving system and a mechanical arm, wherein a processor and a memory are arranged in the robot body, executable program instructions are stored in the memory, and when the processor calls the program instructions in the memory, the processor is used for controlling the driving system to drive the mechanical arm to achieve the step of solving the inverse kinematics analysis method of the shoulder and elbow angles through the cooperative mechanical arm according to any one of the first aspect.
In a fifth aspect, embodiments of the present application provide a computer readable storage medium storing a program, where the program is executed to implement the steps of the collaborative robotic arm solution shoulder-elbow angle inverse kinematics solution method according to any of the first aspects.
In a sixth aspect, embodiments of the present application provide a program product comprising a computer program stored in a readable storage medium, from which the computer program can be read by at least one processor of a robot, the at least one processor executing the computer program causing the robot to implement a method of solving a shoulder-elbow angle inverse kinematics solution as in the first aspect.
Compared with the prior art, the invention has the following beneficial effects:
in the method, a mechanical arm joint coordinate system is established, a shoulder-elbow-wrist structure is configured according to 6 degrees of freedom of a robot, and 1 st to 6 th joints are defined from a base to the tail end respectively; the direction of the mechanical arm is controlled through the 4 th shaft, the 5 th shaft and the 6 th shaft of the robot, and the joint angles corresponding to the 4 th joint, the 5 th joint and the 6 th joint are obtained; and establishing an inverse kinematics equation according to the tail end coordinates of the robot and the angles of the joints corresponding to the 4 th joint, the 5 th joint and the 6 th joint, and calculating the angles corresponding to the 1 st joint, the 2 nd joint and the 3 rd joint. Therefore, the angles corresponding to the 1 st joint, the 2 nd joint and the 3 rd joint of the mechanical arm can be solved very rapidly, the mechanical arm is controlled to pass through the singular point smoothly, and the continuity and the stability of the movement of the mechanical arm are ensured.
Drawings
In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings that are required to be used in the embodiments or the description of the prior art will be briefly described below, and it is obvious that the drawings in the following description are only embodiments of the present invention, and that other drawings can be obtained according to the provided drawings without inventive effort for a person skilled in the art. Other features, objects and advantages of the present invention will become more apparent upon reading of the detailed description of non-limiting embodiments, given with reference to the accompanying drawings in which:
fig. 1 is a schematic diagram of an articulated six-degree-of-freedom robot according to an embodiment of the present disclosure;
fig. 2 is a flowchart of a method for solving inverse kinematics of shoulder and elbow angles by using a cooperative mechanical arm according to an embodiment of the present application;
fig. 3 is a schematic structural diagram of a device for resolving inverse kinematics of shoulder and elbow angles by using a cooperative mechanical arm according to an embodiment of the present application;
fig. 4 is a schematic structural diagram of a device for resolving inverse kinematics of shoulder and elbow angles by using a cooperative mechanical arm according to an embodiment of the present application;
fig. 5 is a schematic structural view of a computer-readable storage medium in an embodiment of the present invention.
In the figure: 1-1 st joint, 2-2 nd joint, 3-3 rd joint, 4-4 th joint, 5-5 th joint, 6-6 th joint, 7-robot tip.
Detailed Description
For the purposes of making the objects, technical solutions and advantages of the embodiments of the present application more clear, the technical solutions of the embodiments of the present application will be clearly and completely described below with reference to the drawings in the embodiments of the present application, and it is apparent that the described embodiments are some embodiments of the present application, but not all embodiments. All other embodiments, which can be made by one of ordinary skill in the art based on the embodiments herein without making any inventive effort, are intended to be within the scope of the present application.
It will be understood that when an element is referred to as being "fixed to" another element, it can be directly on the other element or intervening elements may also be present. When a component is considered to be "connected" to another component, it can be directly connected to the other component or intervening components may also be present.
Unless defined otherwise, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this application belongs. The terminology used herein in the description of the application is for the purpose of describing particular embodiments only and is not intended to be limiting of the application. The term "and/or" as used herein includes any and all combinations of one or more of the associated listed items.
The terms "first," "second," "third," "fourth" and the like in the description and in the claims and in the above drawings, if any, are used for distinguishing between similar objects and not necessarily for describing a particular sequential or chronological order. It is to be understood that the data so used may be interchanged where appropriate such that the embodiments of the invention described herein may be implemented, for example, in sequences other than those illustrated or otherwise described herein. Furthermore, the terms "comprises," "comprising," and "having," and any variations thereof, are intended to cover a non-exclusive inclusion, such that a process, method, system, article, or apparatus that comprises a list of steps or elements is not necessarily limited to those steps or elements expressly listed but may include other steps or elements not expressly listed or inherent to such process, method, article, or apparatus.
The following describes the technical scheme of the present invention and how the technical scheme of the present application solves the above technical problems in detail with specific embodiments. The following embodiments may be combined with each other, and the same or similar concepts or processes may not be described in detail in some embodiments.
Some embodiments of the present application are described in detail below with reference to the accompanying drawings. The following embodiments and features of the embodiments may be combined with each other without conflict.
Fig. 1 is a schematic diagram of a joint type six-degree-of-freedom robot according to an embodiment of the present application, as shown in fig. 1, a robot arm joint coordinate system is first established, a shoulder-elbow-wrist structure is configured according to six degrees of freedom of the robot, and joints 0 to 6 are defined from a base to a tip, namely, a 1 st joint 1, a 2 nd joint 2, a 3 rd joint 3, a 4 th joint 4, a 5 th joint 5, a 6 th joint 6, and a robot tip 7; secondly, taking the position corresponding to the robot base as an origin, and establishing a rectangular coordinate system; the tail end coordinates of the robot are the target coordinate positions which the executing end of the mechanical arm needs to reach.
Illustratively, the distance between the joints of the robotic arm is known, as is the robot tip coordinates. The direction of the mechanical arm is controlled through the 4 th shaft, the 5 th shaft and the 6 th shaft of the robot, so that the joint angles corresponding to the 4 th joint, the 5 th joint and the 6 th joint can be obtained. Then, according to the end coordinates of the robot and the angles of the joints corresponding to the 4 th joint, the 5 th joint and the 6 th joint, an inverse kinematics equation is established, and the angles corresponding to the 1 st joint, the 2 nd joint and the 3 rd joint are calculated. Therefore, the angles corresponding to the 1 st joint, the 2 nd joint and the 3 rd joint of the mechanical arm can be solved very rapidly, the mechanical arm is controlled to pass through the singular point smoothly, and the continuity and the stability of the movement of the mechanical arm are ensured.
Fig. 2 is a flowchart of a method for solving inverse kinematics of shoulder and elbow angles by using a cooperative mechanical arm according to an embodiment of the present application, as shown in fig. 2, the method in this embodiment may include:
step S201: and establishing a mechanical arm joint coordinate system, configuring a shoulder-elbow-wrist structure according to six degrees of freedom of the robot, and respectively defining the 0 th to 6 th joints from the base to the tail end.
The joint coordinates of the mechanical arm established in the embodiment can be shown in fig. 1, and a rectangular coordinate system is established by taking the corresponding position of the robot base as the origin; the tail end coordinates of the robot are target coordinate positions which the executing end of the mechanical arm needs to reach. The six joints from the base to the tip are then defined, respectively.
Step S202: the direction of the mechanical arm is controlled through the 4 th shaft, the 5 th shaft and the 6 th shaft of the robot, and the joint angles corresponding to the 4 th joint, the 5 th joint and the 6 th joint are obtained.
In this embodiment, according to the known coordinates of the tail end of the robot, the direction of the mechanical arm can be controlled by adjusting the 4 th axis, the 5 th axis and the 6 th axis of the robot, and when the mechanical arm is oriented to a preset direction, the joint angles corresponding to the 4 th joint, the 5 th joint and the 6 th joint are recorded respectively.
Step S203: and establishing an inverse kinematics equation according to the tail end coordinates of the robot and the angles of the joints corresponding to the 4 th joint, the 5 th joint and the 6 th joint, and calculating the angles corresponding to the 1 st joint, the 2 nd joint and the 3 rd joint.
In this embodiment, a positive kinematic equation of the robot is constructed, where the calculation formula of the robot end coordinates is as follows:
t 0T ＝t 01 +rotz(q 1 )*(t 12 +roty(q 2 )*(t 23 +roty(q 3 )*(t 34 +roty(q 4 )*(t 45 +rotz(q 5 )*(t 56 +roty(q 6 )*t 6T )))))
wherein: t is t 0T Representing the displacement of the end flange relative to joint 0, t 01 Represents the displacement of the 1 st joint relative to the 0 th joint, rotz (q 1 ) Representing rotation q about the Z-axis 1 Angle, t 12 Indicating the 2 nd jointRelative to the displacement of joint 1, the roty (q 2 ) Representing rotation q about the Y-axis 2 Angle, t 23 Represents the displacement of the 3 rd joint relative to the 2 nd joint, the roty (q 3 ) Representing rotation q about the Y-axis 3 Angle, t 34 Represents the displacement of the 4 th joint relative to the 3 rd joint, the roty (q 4 ) Representing rotation q about the Y-axis 4 Angle, t 45 Represents the displacement of the 5 th joint relative to the 4 th joint, rotz (q 5 ) Representing rotation q about the Z-axis 5 Angle, t 56 Represents the displacement of the 6 th joint relative to the 5 th joint, the roty (q 6 ) Representing rotation q about the Y-axis 6 Angle, t 6T Representing the displacement of the end flange relative to joint 6; wherein:
when the joint angles q corresponding to the 4 th joint, the 5 th joint and the 6 th joint are 4 、q 5 、q 6 When it is known that the number of the cells,
t 4T ＝roty(q 4 )*(t 45 +rotz(q 5 )*(t 56 +roty(q 6 )*t 6T ))
wherein: t is t 4T Representing the displacement of the end flange relative to joint 4, will be t 4T Substitution into t 0T The calculation formula of (2) is obtained as follows:
t 0T ＝t 01 +rotz(q 1 )*(t 12 +roty(q 2 )*(t 23 +roty(q 3 )*(t 34 +t 4T )))
the process of solving inverse kinematics: reversely calculating the joint angles q corresponding to the 1 st joint, the 2 nd joint and the 3 rd joint 1 、q 2 、q 3 ；
Let t 1T ＝t 0T -t 01 ；
Wherein: t is t 1T Representing the displacement of the end flange relative to joint 1, will be t 1T Is marked as [ t ] 1x ，t 1y ，t 1z ]，t 1x Representing coordinates about the X-axis, t 1y Representing the coordinates about the Y-axis, t 1z Representing coordinates about the Z-axis;
let T 1T ＝t 12 +roty(q 2 )*(t 23 +roty(q 3 )*t 3T )；
Wherein: t is t 3T Representing the displacement of the end flange relative to joint 3, will be T 1T Is marked as [ T ] 1x ，T 1y ，T 1z ]The method comprises the steps of carrying out a first treatment on the surface of the Since the component of the corresponding axis remains unchanged when a certain coordinate rotates around the corresponding axis, let t be 12 、t 23 、t 3T The variables about the Y-axis direction are t respectively 12y 、t 23y 、t 3Ty T is then 1T The coordinates of (a) have the following relationship:
T 1z ＝t 1z ；
T 1y ＝t 12y +t 23y +t 3Ty ；
wherein T is 1x ＝+/-sqrt(t 1y 2 +t 1x 2 -T 1y 2 )；
When T is 1T When known, q is calculated according to Paden-Kahan subsubbreem algorithm 1 ；
When q 1 When known, there is ||T 1T -t 12 ‖＝‖t 23 +roty(q 3 )*t 3T II, q is calculated according to Paden-Kahan subsuble algorithm 3 ；
When q 3 When known, according to T 1T -t 12 ＝roty(q 2 )*(t 23 +roty(q 3 )*t 3T ) And q is calculated by Paden-Kahan subsubbreb algorithm 2 。
Exemplary, according to the methods in embodiments of the present application, q 1 There are at most two sets of solutions, q 3 There are at most two sets of solutions, q 2 There is only one set of solutions, thus yielding a maximum of four different sets of q 1 、q 2 、q 3 Angle.
In the embodiment, the 0 th to 6 th joints are respectively defined from the base to the tail end by establishing a mechanical arm joint coordinate system and configuring a shoulder-elbow-wrist structure according to six degrees of freedom of the robot; the direction of the mechanical arm is controlled through the 4 th shaft, the 5 th shaft and the 6 th shaft of the robot, and the joint angles corresponding to the 4 th joint, the 5 th joint and the 6 th joint are obtained; and establishing an inverse kinematics equation according to the tail end coordinates of the robot and the angles of the joints corresponding to the 4 th joint, the 5 th joint and the 6 th joint, and calculating the angles corresponding to the 1 st joint, the 2 nd joint and the 3 rd joint. Therefore, the angles corresponding to the 1 st joint, the 2 nd joint and the 3 rd joint of the mechanical arm can be solved very rapidly, the mechanical arm is controlled to pass through the singular point smoothly, and the continuity and the stability of the movement of the mechanical arm are ensured.
Fig. 3 is a schematic structural diagram of a device for resolving inverse kinematics of shoulder and elbow angles by using a cooperative mechanical arm according to an embodiment of the present application, as shown in fig. 3, the device in this embodiment may include: the joint coordinate system building module 310 is used for building a mechanical arm joint coordinate system, configuring a shoulder-elbow-wrist structure according to six degrees of freedom of the robot, and defining the 0 th to 6 th joints from the base to the tail end respectively; the mechanical arm orientation adjusting module 320 is configured to control the orientation of the mechanical arm through a 4 th axis, a 5 th axis, and a 6 th axis of the robot, so as to obtain joint angles corresponding to the 4 th joint, the 5 th joint, and the 6 th joint; the joint angle operation module 330 is configured to establish an inverse kinematics equation according to the coordinates of the robot end and the joint angles corresponding to the 4 th joint, the 5 th joint and the 6 th joint, and calculate the angles corresponding to the 1 st joint, the 2 nd joint and the 3 rd joint.
Illustratively, the joint coordinate system establishment module 310 is specifically configured to: establishing a rectangular coordinate system by taking the position corresponding to the robot base as an origin; the tail end coordinates of the robot are target coordinate positions which the executing end of the mechanical arm needs to reach.
Illustratively, the robotic arm orientation adjustment module 320 is specifically configured to: according to the known tail end coordinates of the robot, the orientation of the mechanical arm is controlled by adjusting the 4 th axis, the 5 th axis and the 6 th axis of the robot, and when the mechanical arm is oriented to a preset orientation, the joint angles corresponding to the 4 th joint, the 5 th joint and the 6 th joint are recorded respectively.
Exemplary, the joint angle operation module 330 is specifically configured to: constructing a positive kinematic equation of the robot, wherein the calculation formula of the tail end coordinates of the robot is as follows:
t 0T ＝t 01 +rotz(q 1 )*(t 12 +roty(q 2 )*(t 23 +roty(q 3 )*(t 34 +roty(q 4 )*(t 45 +rotz(q 5 )*(t 56 +roty(q 6 )*t 6T )))))
wherein: t is t 0T Representing the displacement of the end flange relative to joint 0, t 01 Represents the displacement of the 1 st joint relative to the 0 th joint, rotz (q 1 ) Representing rotation q about the Z-axis 1 Angle, t 12 Represents the displacement of the 2 nd joint relative to the 1 st joint, the roty (q 2 ) Representing rotation q about the Y-axis 2 Angle, t 23 Represents the displacement of the 3 rd joint relative to the 2 nd joint, the roty (q 3 ) Representing rotation q about the Y-axis 3 Angle, t 34 Represents the displacement of the 4 th joint relative to the 3 rd joint, the roty (q 4 ) Representing rotation q about the Y-axis 4 Angle, t 45 Represents the displacement of the 5 th joint relative to the 4 th joint, rotz (q 5 ) Representing rotation q about the Z-axis 5 Angle, t 56 Represents the displacement of the 6 th joint relative to the 5 th joint, the roty (q 6 ) Representing rotation q about the Y-axis 6 Angle, t 6T Representing the displacement of the end flange relative to joint 6; wherein:
when the joint angles q corresponding to the 4 th joint, the 5 th joint and the 6 th joint are 4 、q 5 、q 6 When it is known that the number of the cells,
t 4T ＝roty(q 4 )*(t 45 +rotz(q 5 )*(t 56 +roty(q 6 )*t 6T ))
wherein: t is t 4T Representing the displacement of the end flange relative to joint 4, will be t 4T Substitution into t 0T The calculation formula of (2) is obtained as follows:
t 0T ＝t 01 +rotz(q 1 )*(t 12 +roty(q 2 )*(t 23 +roty(q 3 )*(t 34 +t 4T )))
the process of solving inverse kinematics: reversely calculating the joint angles q corresponding to the 1 st joint, the 2 nd joint and the 3 rd joint 1 、q 2 、q 3 ；
Let t 1T ＝t 0T -t 01 ；
Wherein: t is t 1T Representing the displacement of the end flange relative to joint 1, will be t 1T Is marked as [ t ] 1x ，t 1y ，t 1z ]，t 1x Representing coordinates about the X-axis, t 1y Representing the coordinates about the Y-axis, t 1z Representing coordinates about the Z-axis;
let T 1T ＝t 12 +roty(q 2 )*(t 23 +roty(q 3 )*t 3T )；
Wherein: t is t 3T Representing the displacement of the end flange relative to joint 3, will be T 1T Is marked as [ T ] 1x ，T 1y ，T 1z ]The method comprises the steps of carrying out a first treatment on the surface of the Since the component of the corresponding axis remains unchanged when a certain coordinate rotates around the corresponding axis, let t be 12 、t 23 、t 3T The variables about the Y-axis direction are t respectively 12y 、t 23y 、t 3Ty T is then 1T The coordinates of (a) have the following relationship:
T 1z ＝t 1z ；
T 1y ＝t 12y +t 23y +t 3Ty ；
wherein T is 1x ＝+/-sqrt(t 1y 2 +t 1x 2 -T 1y 2 )；
When T is 1T When known, q is calculated according to Paden-Kahan subsubbreem algorithm 1 ；
When q 1 When known, there is ||T 1T -t 12 ‖＝‖t 23 +roty(q 3 )*t 3T II, q is calculated according to Paden-Kahan subsuble algorithm 3 ；
When q 3 When known, according to T 1T -t 12 ＝roty(q 2 )*(t 23 +roty(q 3 )*t 3T ) And q is calculated by Paden-Kahan subsubbreb algorithm 2 。
Exemplary, q in this embodiment 1 There are at most two sets of solutions, q 3 There are at most two sets of solutions, q 2 Only 1 set of solutions, thus yielding a maximum of 4 different q 1 、q 2 、q 3 Angle.
In the embodiment, the 0 th to 6 th joints are respectively defined from the base to the tail end by establishing a mechanical arm joint coordinate system and configuring a shoulder-elbow-wrist structure according to six degrees of freedom of the robot; the direction of the mechanical arm is controlled through the 4 th shaft, the 5 th shaft and the 6 th shaft of the robot, and the joint angles corresponding to the 4 th joint, the 5 th joint and the 6 th joint are obtained; and establishing an inverse kinematics equation according to the tail end coordinates of the robot and the angles of the joints corresponding to the 4 th joint, the 5 th joint and the 6 th joint, and calculating the angles corresponding to the 1 st joint, the 2 nd joint and the 3 rd joint. Therefore, the angles corresponding to the 1 st joint, the 2 nd joint and the 3 rd joint of the mechanical arm can be solved very rapidly, the mechanical arm is controlled to pass through the singular point smoothly, and the continuity and the stability of the movement of the mechanical arm are ensured.
Fig. 4 is a schematic structural diagram of a device for resolving inverse kinematics of shoulder and elbow angles by using a cooperative mechanical arm according to an embodiment of the present application, where the device 400 for resolving inverse kinematics of shoulder and elbow angles by using a cooperative mechanical arm in this embodiment may include: a processor 401 and a memory 402.
A memory 402 for storing a program; memory 402, which may include volatile memory (English: volatile memory), such as random-access memory (RAM), such as static random-access memory (SRAM), double data rate synchronous dynamic random-access memory (Double Data Rate Synchronous Dynamic Random Access Memory, DDR SDRAM), etc.; the memory may also include a non-volatile memory (English) such as a flash memory (English). The memory 402 is used to store computer programs (e.g., application programs, functional modules, etc. that implement the methods described above), computer instructions, etc., which may be stored in one or more of the memories 402 in a partitioned manner. And the above-described computer programs, computer instructions, data, etc. may be called by the processor 401.
The computer programs, computer instructions, etc., described above may be stored in partitions in one or more memories 402. And the above-described computer programs, computer instructions, data, etc. may be called by the processor 401.
A processor 401 for executing a computer program stored in a memory 402 to implement the steps of the method according to the above-mentioned embodiment.
Reference may be made in particular to the description of the embodiments of the method described above.
The processor 401 and the memory 402 may be separate structures or may be integrated structures integrated together. When the processor 401 and the memory 402 are separate structures, the memory 402 and the processor 401 may be coupled by a bus 403.
The collaborative mechanical arm solution shoulder-elbow angle inverse kinematics analysis device 400 of the present embodiment may execute the technical scheme in the method shown in fig. 2, and the specific implementation process and technical principle thereof refer to the related description in the method shown in fig. 2, which is not repeated here.
Those skilled in the art will appreciate that the various aspects of the invention may be implemented as a system, method, or program product. Accordingly, aspects of the invention may be embodied in the following forms, namely: an entirely hardware embodiment, an entirely software embodiment (including firmware, micro-code, etc.) or an embodiment combining hardware and software aspects may be referred to herein as a "circuit," module "or" platform.
In addition, the embodiment of the application further provides a computer-readable storage medium, in which computer-executable instructions are stored, when the at least one processor of the user equipment executes the computer-executable instructions, the user equipment performs the above possible methods.
Among them, computer-readable media include computer storage media and communication media including any medium that facilitates transfer of a computer program from one place to another. A storage media may be any available media that can be accessed by a general purpose or special purpose computer. An exemplary storage medium is coupled to the processor such the processor can read information from, and write information to, the storage medium. In the alternative, the storage medium may be integral to the processor. The processor and the storage medium may reside in an ASIC. In addition, the ASIC may reside in a user device. The processor and the storage medium may reside as discrete components in a communication device.
The present application also provides a program product comprising a computer program stored in a readable storage medium, from which the computer program can be read by at least one processor of a server, the at least one processor executing the computer program causing the server to implement the method according to any one of the embodiments of the present invention described above.
Those of ordinary skill in the art will appreciate that: all or part of the steps for implementing the above method embodiments may be implemented by hardware associated with program instructions, where the foregoing program may be stored in a computer readable storage medium, and when executed, the program performs steps including the above method embodiments; and the aforementioned storage medium includes: read-Only Memory (ROM), random access Memory (Random Access Memory, RAM), magnetic or optical disk, and the like, which can store program codes.
Fig. 5 is a schematic structural view of a computer-readable storage medium in an embodiment of the present invention. Referring to fig. 5, a program product 500 for implementing the above-described method according to an embodiment of the present invention is described, which may employ a portable compact disc read only memory (CD-ROM) and include program code, and may be run on a terminal device, such as a personal computer. However, the program product of the present invention is not limited thereto, and in this document, a readable storage medium may be any tangible medium that can contain, or store a program for use by or in connection with an instruction execution system, apparatus, or device.
The program product may employ any combination of one or more readable media. The readable medium may be a readable signal medium or a readable storage medium. The readable storage medium can be, for example, but is not limited to, an electronic, magnetic, optical, electromagnetic, infrared, or semiconductor system, apparatus, or device, or a combination of any of the foregoing. More specific examples (a non-exhaustive list) of the readable storage medium would include the following: an electrical connection having one or more wires, a portable disk, a hard disk, random Access Memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM or flash memory), optical fiber, portable compact disk read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the foregoing.
The computer readable storage medium may include a data signal propagated in baseband or as part of a carrier wave, with readable program code embodied therein. Such a propagated data signal may take any of a variety of forms, including, but not limited to, electro-magnetic, optical, or any suitable combination of the foregoing. A readable storage medium may also be any readable medium that can communicate, propagate, or transport a program for use by or in connection with an instruction execution system, apparatus, or device. Program code embodied on a readable storage medium may be transmitted using any appropriate medium, including but not limited to wireless, wireline, optical fiber cable, RF, etc., or any suitable combination of the foregoing.
Program code for carrying out operations of the present invention may be written in any combination of one or more programming languages, including an object oriented programming language such as Java, C++ or the like and conventional procedural programming languages, such as the "C" programming language or similar programming languages. The program code may execute entirely on the user's computing device, partly on the user's device, as a stand-alone software package, partly on the user's computing device, partly on a remote computing device, or entirely on the remote computing device or server. In the case of remote computing devices, the remote computing device may be connected to the user computing device through any kind of network, including a Local Area Network (LAN) or a Wide Area Network (WAN), or may be connected to an external computing device (e.g., connected via the Internet using an Internet service provider).
In the present specification, each embodiment is described in a progressive manner, and each embodiment is mainly described in a different point from other embodiments, and identical and similar parts between the embodiments are all enough to refer to each other. The previous description of the disclosed embodiments is provided to enable any person skilled in the art to make or use the present invention. Various modifications to these embodiments will be readily apparent to those skilled in the art, and the generic principles defined herein may be applied to other embodiments without departing from the spirit or scope of the invention. Thus, the present invention is not intended to be limited to the embodiments shown herein but is to be accorded the widest scope consistent with the principles and novel features disclosed herein.
The foregoing describes specific embodiments of the present invention. It is to be understood that the invention is not limited to the particular embodiments described above, and that various changes and modifications may be made by one skilled in the art within the scope of the claims without affecting the spirit of the invention.
Claims (10)
1. The inverse kinematics analysis solution method for solving the shoulder and elbow angles by the cooperative mechanical arm is characterized by comprising the following steps of:
step 1: establishing a mechanical arm joint coordinate system, configuring a shoulder-elbow-wrist structure according to six degrees of freedom of the robot, and defining the 0 th to 6 th joints from the base to the tail end respectively;
step 2: the direction of the mechanical arm is controlled through the 4 th shaft, the 5 th shaft and the 6 th shaft of the robot, and the joint angles corresponding to the 4 th joint, the 5 th joint and the 6 th joint are obtained;
step 3: and establishing an inverse kinematics equation according to the tail end coordinates of the robot and the angles of the joints corresponding to the 4 th joint, the 5 th joint and the 6 th joint, and calculating the angles corresponding to the 1 st joint, the 2 nd joint and the 3 rd joint.
2. The method for solving inverse kinematics of shoulder and elbow angles by using a cooperative mechanical arm according to claim 1, wherein the step 1 comprises:
establishing a rectangular coordinate system by taking the position corresponding to the robot base as an origin; the tail end coordinates of the robot are target coordinate positions which the executing end of the mechanical arm needs to reach.
3. The method for solving inverse kinematics of shoulder and elbow angles by using a cooperative mechanical arm according to claim 1, wherein the step 2 comprises:
according to the known tail end coordinates of the robot, the orientation of the mechanical arm is controlled by adjusting the 4 th axis, the 5 th axis and the 6 th axis of the robot, and when the mechanical arm is oriented to a preset orientation, the joint angles corresponding to the 4 th joint, the 5 th joint and the 6 th joint are recorded respectively.
4. The method for solving inverse kinematics of shoulder and elbow angles by using a cooperative mechanical arm according to claim 1, wherein the step 3 comprises:
constructing a positive kinematic equation of the robot, wherein the calculation formula of the tail end coordinates of the robot is as follows:
t 0T ＝t 01 +rotz(q 1 )*(t 12 +roty(q 2 )*(t 23 +roty(q 3 )*(t 34 +roty(q 4 )*(t 45 +rotz(q 5 )*(t 56 +roty(q 6 )*t 6T )))))
wherein: t is t 0T Representing the displacement of the end flange relative to joint 0, t 01 Represents the displacement of the 1 st joint relative to the 0 th joint, rotz (q 1 ) Indicating rotation about the Z axisq 1 Angle, t 12 Represents the displacement of the 2 nd joint relative to the 1 st joint, the roty (q 2 ) Representing rotation q about the Y-axis 2 Angle, t 23 Represents the displacement of the 3 rd joint relative to the 2 nd joint, the roty (q 3 ) Representing rotation q about the Y-axis 3 Angle, t 34 Represents the displacement of the 4 th joint relative to the 3 rd joint, the roty (q 4 ) Representing rotation q about the Y-axis 4 Angle, t 45 Represents the displacement of the 5 th joint relative to the 4 th joint, rotz (q 5 ) Representing rotation q about the Z-axis 5 Angle, t 56 Represents the displacement of the 6 th joint relative to the 5 th joint, the roty (q 6 ) Representing rotation q about the Y-axis 6 Angle, t 6T Representing the displacement of the end flange relative to joint 6; wherein:
when the joint angles q corresponding to the 4 th joint, the 5 th joint and the 6 th joint are 4 、q 5 、q 6 When it is known that the number of the cells,
t 4T ＝roty(q 4 )*(t 45 +rotz(q 5 )*(t 56 +roty(q 6 )*t 6T ))
wherein: t is t 4T Representing the displacement of the end flange relative to joint 4, will be t 4T Substitution into t 0T The calculation formula of (2) is obtained as follows:
t 0T ＝t 01 +rotz(q 1 )*(t 12 +roty(q 2 )*(t 23 +roty(q 3 )*(t 34 +t 4T )))
the process of solving inverse kinematics: reversely calculating the joint angles q corresponding to the 1 st joint, the 2 nd joint and the 3 rd joint 1 、q 2 、q 3 ；
Let t 1T ＝t 0T -t 01 ；
Wherein: t is t 1T Representing the displacement of the end flange relative to joint 1, will be t 1T Is marked as [ t ] 1x ，t 1y ，t 1z ]，t 1x Representing coordinates about the X-axis, t 1y Representing the coordinates about the Y-axis, t 1z Representing coordinates about the Z-axis;
let T 1T ＝t 12 +roty(q 2 )*(t 23 +roty(q 3 )*t 3T )；
Wherein: t is t 3T Representing the displacement of the end flange relative to joint 3, will be T 1T Is marked as [ T ] 1x ，T 1y ，T 1z ]；
Since the component of the corresponding axis remains unchanged when a certain coordinate rotates around the corresponding axis, let t be 12 、t 23 、t 3T The variables about the Y-axis direction are t respectively 12y 、t 23y 、t 3Ty T is then 1T The coordinates of (a) have the following relationship:
T 1z ＝t 1z ；
T 1y ＝t 12y +t 23y +t 3Ty ；
wherein T is 1x ＝+/-sqrt(t 1y 2 +t 1x 2 -T 1y 2 )；
When T is 1T When known, q is calculated according to Paden-Kahan subsubbreem algorithm 1 ；
When q 1 When known, there is ||T 1T -t 12 ‖＝‖t 23 +roty(q 3 )*t 3T II, q is calculated according to Paden-Kahan subsuble algorithm 3 ；
When q 3 When known, according to T 1T -t 12 ＝roty(q 2 )*(t 23 +roty(q 3 )*t 3T ) And q is calculated by Paden-Kahan subsubbreb algorithm 2 。
5. The collaborative mechanical arm solution shoulder and elbow angle inverse kinematics analytical solution method according to claim 4Characterized in that q 1 There are at most two sets of solutions, q 3 There are at most two sets of solutions, q 2 There is only one set of solutions, thus yielding a maximum of four different sets of q 1 、q 2 、q 3 Angle.
6. A device for solving inverse kinematics of shoulder and elbow angles by a cooperative mechanical arm, the device comprising:
the joint coordinate system building module is used for building a mechanical arm joint coordinate system, configuring a shoulder-elbow-wrist structure according to six degrees of freedom of the robot, and defining the 0 th to 6 th joints from the base to the tail end respectively;
the mechanical arm orientation adjusting module is used for controlling the orientation of the mechanical arm through a 4 th shaft, a 5 th shaft and a 6 th shaft of the robot to obtain joint angles corresponding to a 4 th joint, a 5 th joint and a 6 th joint;
the joint angle operation module is used for establishing an inverse kinematics equation according to the tail end coordinates of the robot and the joint angles corresponding to the 4 th joint, the 5 th joint and the 6 th joint, and calculating the angles corresponding to the 1 st joint, the 2 nd joint and the 3 rd joint.
7. The collaborative mechanical arm solution shoulder and elbow angle inverse kinematics solution apparatus of claim 6, wherein the joint coordinate system establishment module is specifically configured to:
establishing a rectangular coordinate system by taking the position corresponding to the robot base as an origin; the tail end coordinates of the robot are target coordinate positions which the executing end of the mechanical arm needs to reach.
8. The collaborative mechanical arm solution shoulder and elbow angle inverse kinematics solution apparatus of claim 6, wherein the mechanical arm orientation adjustment module is specifically configured to:
according to the known tail end coordinates of the robot, the orientation of the mechanical arm is controlled by adjusting the 4 th axis, the 5 th axis and the 6 th axis of the robot, and when the mechanical arm is oriented to a preset orientation, the joint angles corresponding to the 4 th joint, the 5 th joint and the 6 th joint are recorded respectively.
9. The utility model provides a collaborative mechanical arm solves shoulder elbow angle inverse kinematics and solves equipment which characterized in that includes: the system comprises a processor and a memory, wherein executable program instructions are stored in the memory, and when the processor calls the program instructions in the memory, the processor is used for:
a step of performing the collaborative mechanical arm solution shoulder elbow angle inverse kinematics solution method of any one of claims 1-5.
10. A computer readable storage medium storing a program, wherein the program when executed implements the steps of the collaborative robotic arm solution shoulder-elbow angle inverse kinematics solution method of any one of claims 1-5.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311724301.4A CN117817659A (en) | 2023-12-14 | 2023-12-14 | Inverse kinematics analysis solution method for solving shoulder and elbow angles by cooperative mechanical arm |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311724301.4A CN117817659A (en) | 2023-12-14 | 2023-12-14 | Inverse kinematics analysis solution method for solving shoulder and elbow angles by cooperative mechanical arm |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN117817659A true CN117817659A (en) | 2024-04-05 |
Family
ID=90520047
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202311724301.4A Pending CN117817659A (en) | 2023-12-14 | 2023-12-14 | Inverse kinematics analysis solution method for solving shoulder and elbow angles by cooperative mechanical arm |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117817659A (en) |
-
2023
- 2023-12-14 CN CN202311724301.4A patent/CN117817659A/en active Pending
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110370256B (en) | Robot and path planning method, device and controller thereof | |
| CN111360827A (en) | Visual servo switching control method and system | |
| CN113715016B (en) | Robot grabbing method, system, device and medium based on 3D vision | |
| CN108406769A (en) | The joint rotation angle of serial manipulator determines method and device | |
| CN107253191B (en) | Double-mechanical-arm system and coordination control method thereof | |
| WO2022205844A1 (en) | Robot forward kinematics solution method and apparatus, readable storage medium, and robot | |
| WO2020093253A1 (en) | Robot motion control method, control system and storage device | |
| CN110561419B (en) | Arm-shaped line constraint flexible robot track planning method and device | |
| CN111113429B (en) | Action simulation method, action simulation device and terminal equipment | |
| WO2023024278A1 (en) | Robot joint pose optimization method, robot control method, and robot | |
| CN112356032B (en) | Posture smooth transition method and system | |
| CN109366486B (en) | Flexible robot inverse kinematics solving method, system, equipment and storage medium | |
| CN117817659A (en) | Inverse kinematics analysis solution method for solving shoulder and elbow angles by cooperative mechanical arm | |
| CN117407630B (en) | Inverse kinematics analysis and solution system for solving shoulder and elbow angles through cooperative mechanical arm | |
| CN111958602A (en) | Real-time inverse solution method for wrist offset type 6-axis robot | |
| Zhao et al. | Model and simulation of the mitsubishi rv-m1 robot using matlab | |
| Al-Faiz et al. | Inverse kinematics analysis for manipulator robot with wrist offset based on the closed-form algorithm | |
| CN113722864A (en) | 7-degree-of-freedom redundant manipulator inverse kinematics solving method and system | |
| CN111761586A (en) | Remote control robot based on big data | |
| Seo et al. | Contact-rich SE (3)-Equivariant Robot Manipulation Task Learning via Geometric Impedance Control | |
| Zhang et al. | Trajectory planning of collaborative robot for 3c products assembly | |
| CN118061169A (en) | Robot control system and robot through singular point | |
| CN117428791B (en) | Inverse kinematics solving method and system for shoulder four-axis rehabilitation robot | |
| CN114918923B (en) | Method and system for avoiding human body by mechanical arm in near-distance man-machine cooperation environment | |
| CN114290331B (en) | Robot motion control method, robot and computer readable storage device |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication |