CN109129469A - Mechanical arm kinematics inverse solution method and device and mechanical arm - Google Patents
Mechanical arm kinematics inverse solution method and device and mechanical arm Download PDFInfo
- Publication number
- CN109129469A CN109129469A CN201810864277.7A CN201810864277A CN109129469A CN 109129469 A CN109129469 A CN 109129469A CN 201810864277 A CN201810864277 A CN 201810864277A CN 109129469 A CN109129469 A CN 109129469A
- Authority
- CN
- China
- Prior art keywords
- joint
- angle
- determining
- type
- rotary
- 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.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 92
- 239000011159 matrix material Substances 0.000 claims abstract description 34
- 238000011426 transformation method Methods 0.000 claims abstract description 8
- 239000012636 effector Substances 0.000 claims description 16
- 210000000707 wrist Anatomy 0.000 claims description 14
- 230000009466 transformation Effects 0.000 claims description 10
- 238000004590 computer program Methods 0.000 claims description 4
- 238000010586 diagram Methods 0.000 description 4
- 230000036544 posture Effects 0.000 description 2
- 230000006978 adaptation Effects 0.000 description 1
- 238000004458 analytical method Methods 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000006870 function Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000001131 transforming effect Effects 0.000 description 1
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1602—Programme controls characterised by the control system, structure, architecture
- B25J9/1607—Calculation of inertia, jacobian matrixes and inverses
-
- 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
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Physics & Mathematics (AREA)
- Mathematical Physics (AREA)
- Automation & Control Theory (AREA)
- Manipulator (AREA)
- Numerical Control (AREA)
Abstract
The invention discloses a mechanical arm kinematics inverse solution method, a device and a mechanical arm, wherein the method comprises the following steps: determining geometrical parameters of a mechanical arm, wherein the mechanical arm comprises a plurality of rotary joints, and the rotary joints are divided into a first type of rotary joint and a second type of rotary joint; determining a first type joint angle corresponding to a first type rotary joint by adopting a geometric algebra method according to the geometric parameters of the mechanical arm; and determining a second type joint angle corresponding to the second type rotary joint by adopting an Euler angle rotation matrix transformation method according to the first type joint angle, wherein the first type joint angle and the second type joint angle are used for controlling the operation of the mechanical arm. The invention solves the problem of complex solving process in the prior art, and realizes simple and understandable solving process, simplified code amount, fast computing speed and high precision.
Description
Technical Field
The invention relates to the technical field of robots, in particular to a method and a device for inverse kinematics solution of a mechanical arm and the mechanical arm.
Background
And calculating a joint angle meeting the expected requirement according to the expected position and the expected posture of the known tool coordinate system relative to the base coordinate system, wherein the process is called as a kinematic inverse solution. Since industrial robots are of the 6R type, the existence of multiple solutions and solutions occurs in the kinematics inversion problem, and the non-existence of solutions occurs when the end effector encounters a singular point. At present, an industrial robot is realized by a D-H parameter method, a three-axis intersected PIEPER method and an algebraic geometric analysis method in an inverse kinematics solving process. Such a method has the following disadvantages: the D-H parameter method causes the robot to be single in modeling and limited, the solution process of the algebraic geometry method is complex and difficult to understand, the calculation formula is complex, and the solution speed is relatively slow.
Aiming at the problem of complex solving process in the related technology, no effective solution is provided at present.
Disclosure of Invention
The invention provides a method and a device for inverse solution of kinematics of a mechanical arm and the mechanical arm, which are used for at least solving the problem of complex solution process in the prior art.
To solve the above technical problem, according to an aspect of an embodiment of the present invention, there is provided a method for inverse kinematics solution of a robot arm, including: determining geometrical parameters of a mechanical arm, wherein the mechanical arm comprises a plurality of rotary joints, and the rotary joints are divided into a first type of rotary joint and a second type of rotary joint; determining a first type joint angle corresponding to a first type rotary joint by adopting a geometric algebra method according to the geometric parameters of the mechanical arm; and determining a second type joint angle corresponding to the second type rotary joint by adopting an Euler angle rotation matrix transformation method according to the first type joint angle, wherein the first type joint angle and the second type joint angle are used for controlling the operation of the mechanical arm.
Further, the mechanical arm comprises six rotary joints, the first type of rotary joint comprises first to third rotary joints, the first type of joint angle comprises first to third joint angles, the second type of rotary joint comprises fourth to sixth rotary joints, and the second type of joint angle comprises fourth to sixth joint angles.
Further, the mechanical arm also comprises five connecting rod structures and an end effector; determining geometrical parameters of the mechanical arm, comprising: establishing a reference coordinate system by taking the first connecting rod structure as a reference, wherein the reference coordinate system comprises an X axis, a Y axis and a Z axis, and the origin of the reference coordinate system is positioned in the center of the bottom of the first connecting rod structure; and determining the coordinate position and the Euler attitude angle of the reference structure in the reference coordinate system, wherein the reference structure is an end effector, or a fourth rotary joint, or a third rotary joint.
Further, according to the geometric parameters of the mechanical arm, a first-class joint angle corresponding to the first-class rotary joint is determined by adopting a geometric algebra method, and the method comprises the following steps: determining the coordinate position of a fifth rotary joint according to the coordinate position of the reference structure and the Euler attitude angle; determining a first joint angle according to the coordinate position of the fifth rotary joint; and determining the second joint angle and the third joint angle by adopting a geometric algebra method according to the coordinate position of the fifth rotary joint.
Further, determining the coordinate position of the fifth rotational joint from the coordinate position of the reference structure and the euler attitude angle includes: converting the Euler attitude angles (A, B, C) of the reference structure into X-Y-Z Euler angular moments of rotationArray R; translating the coordinate system of the reference structure to the coordinate system of the fifth rotary joint, and determining a translation operator P5 a(ii) a According to the X-Y-Z Euler angle rotation matrix R, the coordinate position Pa (X, Y, Z) of the reference structure and the translation operator P5 aDetermining the coordinate position P5 (P) of the fifth rotary joint5.X，P5.Y，P5.Z)。
Further, X-Y-Z Euler angle rotation matrix
Coordinate position of the fifth rotary joint
P5.X＝X-5.X*r12；P5.Y＝Y-5.X*r22；P5.Z＝Z-5.X*r32
Wherein a is 6 when the reference structure is the end effector, 4 when the reference structure is the fourth rotary joint, and 3 when the reference structure is the third rotary joint.
Further, determining the first joint angle from the coordinate position of the fifth rotational joint includes: from the X-axis coordinate position P of the fifth rotary joint5X and Y axis coordinate position P5Y determines a first joint angle Q1; wherein, Q1 ═ arctan2 (P)5.Y，P5.X)。
Further, determining the second and third joint angles by using a geometric algebra method according to the coordinate position of the fifth rotary joint, comprising: establishing a second rotary joint coordinate system by taking the second rotary joint as a reference, wherein the second rotary joint coordinate system comprises an X axis, a Y axis and a Z axis, and the origin of the second rotary joint coordinate system is positioned in the center of the bottom of the second rotary joint; judging the combination form of the second connecting rod structure and the third connecting rod structure in a second rotary joint coordinate system; and determining a second joint angle and a third joint angle corresponding to different forms according to the combined forms.
Further, the combination modality includes: a convex structure and a concave structure; determining a second joint angle and a third joint angle corresponding to different forms according to the combined forms, wherein the method comprises the following steps: determining each side length in a triangle formed by the second rotary joint, the third rotary joint and the fifth rotary joint; determining an included angle in the triangle and an included angle between the side length and a second rotary joint coordinate system according to the side length; and determining a second joint angle and a third joint angle in the convex structure and a second joint angle and a third joint angle in the concave structure according to the included angle and the side length in the triangle and the included angle of the second rotary joint coordinate system.
Further, determining each side length of a triangle formed by the second rotary joint, the third rotary joint and the fifth rotary joint comprises: determining a side AB between the second rotary joint and the third rotary joint, a side AD between the second rotary joint and the fifth rotary joint, and a side BD between the third rotary joint and the fifth rotary joint; wherein,
AB＝2.Z
wherein,
further, determining the included angle in the triangle and the included angle between the side length and the second rotary joint coordinate system according to the side length comprises determining the included angle β between AB and AD, the included angle Y, AD between AB and BD and the X-axis of the second rotary joint coordinate system, and the included angle theta between BD and the Z-axis of the second rotary joint coordinate system according to AB, AD and BD, wherein,
θ＝arctan2(ED，EB)。
further, according to the included angle and the included angle between the side length in the triangle and the second rotating joint coordinate system, determining a second joint angle and a third joint angle in the convex structure and a second joint angle and a third joint angle in the concave structure, including: in the convex configuration, the second joint angleAnd a third joint angleIn the concave configuration, the second joint angleAnd a third joint angle
Further, determining fourth to sixth joint angles corresponding to fourth to sixth rotary joints by using an euler angle rotation matrix transformation method according to the first to third joint angles, includes: determining a wrist rotation matrix Rw1 according to the first to third joint angles; determining an X-Y-X Euler angle rotation matrix Rw2 according to the fourth to sixth joint angles; solving for a fourth joint angle Q4, a fifth joint angle Q5 and a sixth joint angle Q6 according to Rw1 and Rw 2.
Further, the wrist rotation matrix Rw1 ═ Ra ^ T ^ R, wherein,
Qz-Q1, Qy-Q2 + Q3, wherein Q1 is a first joint angle, Q2 is a second joint angle, and Q3 is a third joint angle; X-Y-X Euler angle rotation matrix
Let Rw1 be Rw2, Rw1 and Rw2 have a one-to-one correspondence of elements in the matrix, Q5 gets two solutions:
q4 yields a solution:
Q4.1＝arctan2(rw21，rw31)
q6 yields a solution:
Q6.1＝arctan2(rw12，rw13)。
further, determining fourth to sixth joint angles from Rw1 and Rw2, comprising: respectively calculating the difference values of the Q5.1, the Q5.2 and the joint angle of the fifth rotary joint in the last rotation; and comparing the difference values, and determining a fifth joint angle with a small difference value as an optimal solution.
Further, determining fourth to sixth joint angles from Rw1 and Rw2, further comprising: when the fifth joint angle is a singular point, the joint angle at the time of the last rotation of the fourth rotary joint is set as the fourth joint angle Q4; the sixth joint angle Q6 is arctan2(r23, r33) -Q4.
According to another aspect of the embodiments of the present invention, there is provided a robot kinematic inverse solution apparatus, including: the detector is used for determining the geometric parameters of the mechanical arm, wherein the mechanical arm comprises a plurality of rotary joints, and the rotary joints are divided into a first type of rotary joint and a second type of rotary joint; the first processor is used for determining a first type of joint angle corresponding to a first type of rotary joint by adopting a geometric algebra method according to the geometric parameters of the mechanical arm; and the second processor is used for determining a second type joint angle corresponding to the second type rotary joint by adopting an Euler angle rotation matrix transformation method according to the first type joint angle, wherein the first type joint angle and the second type joint angle are used for controlling the operation of the mechanical arm.
According to another aspect of the embodiments of the present invention, there is provided a robot arm including the inverse kinematics solution device as described above.
According to a further aspect of embodiments of the present invention, there is provided a computer device comprising a memory, a processor and a computer program stored on the memory and executable on the processor, the processor implementing the method as described in the above embodiments when executing the program.
According to a further aspect of embodiments of the present invention there is provided a storage medium containing computer executable instructions which when executed by a computer processor are for performing the method of the above embodiments.
In the invention, the robot mechanical arm is divided into a front end three-arm part and a rear end three-arm part, each joint angle is solved by respectively utilizing a geometric algebra method and wrist Euler angle rotation transformation, the traditional D-H parameter modeling is not relied on, the problems of complex and difficult solving process, large calculated amount and low solving efficiency in the prior art are effectively solved, the solving process is simple and easy to understand, the code amount is simplified, the calculating speed is high and the precision is high.
Drawings
Fig. 1 is an alternative flowchart of a method for inverse kinematics solution of a robot arm according to embodiment 1 of the present invention;
fig. 2 is a schematic structural view of a robot arm according to embodiment 1 of the present invention;
fig. 3 is a geometric configuration diagram of a convex-structured robot arm according to embodiment 1 of the present invention;
fig. 4 is a geometric block diagram of a concave structure robot arm according to example 1 of the present invention;
fig. 5 is another alternative flowchart of the inverse kinematics solution method of the robot arm according to embodiment 1 of the present invention; and
fig. 6 is an alternative block diagram of the inverse kinematics solution apparatus of a robot arm according to embodiment 2 of the present invention.
Detailed Description
Reference will now be made in detail to the exemplary embodiments, examples of which are illustrated in the accompanying drawings. When the following description refers to the accompanying drawings, like numbers in different drawings represent the same or similar elements unless otherwise indicated. The embodiments described in the following exemplary embodiments do not represent all embodiments consistent with the present invention. Rather, they are merely examples of apparatus and methods consistent with certain aspects of the invention, as detailed in the appended claims.
Example 1
In a preferred embodiment 1 of the present invention, a kinematic inverse solution method for a robot arm is provided, which can be directly applied to various industrial robots, and can also be applied to other devices having partial functions of an industrial robot, and in particular, when the kinematic inverse solution method is implemented, the kinematic inverse solution method can be implemented by installing software, APP, or writing a program into a controller of the industrial robot or other devices. In particular, fig. 1 shows an alternative flow chart of the method, which, as shown in fig. 1, comprises the following steps S102-S106:
s102: determining geometrical parameters of a mechanical arm, wherein the mechanical arm comprises a plurality of rotary joints, and the rotary joints are divided into a first type of rotary joint and a second type of rotary joint;
s104: determining a first type joint angle corresponding to a first type rotary joint by adopting a geometric algebra method according to the geometric parameters of the mechanical arm;
s106: and determining a second type joint angle corresponding to the second type rotary joint by adopting an Euler angle rotation matrix transformation method according to the first type joint angle, wherein the first type joint angle and the second type joint angle are used for controlling the operation of the mechanical arm.
In the above embodiment, the robot arm is divided into the front three-arm part and the rear three-arm part, and each joint angle is solved by using a geometric algebra method and a wrist euler angle rotation transformation, without depending on the traditional D-H parameter modeling, so that the problems of complicated and difficult solving process, large calculated amount and low solving efficiency in the prior art are effectively solved, the solving process is simple and easy to understand, the code amount is simplified, the calculating speed is high, and the precision is high.
In a preferred embodiment of the present invention, as shown in fig. 2, the robot arm includes six rotary joints, five link structures and one end effector, the first type of rotary joint includes first to third rotary joints, the first type of joint angle includes first to third joint angles, the second type of rotary joint includes fourth to sixth rotary joints, and the second type of joint angle includes fourth to sixth joint angles. And establishing a reference coordinate system by taking the first connecting rod structure as a reference, wherein the reference coordinate system comprises an X axis, a Y axis and a Z axis, and the origin of the reference coordinate system is positioned in the center of the bottom of the first connecting rod structure. Further, the mechanical arm also comprises five connecting rod structures and an end effector; determining geometrical parameters of the mechanical arm, comprising: establishing a reference coordinate system by taking the first connecting rod structure as a reference, wherein the reference coordinate system comprises an X axis, a Y axis and a Z axis, and the origin of the reference coordinate system is positioned in the center of the bottom of the first connecting rod structure; and determining the coordinate position and the Euler attitude angle of the reference structure in the reference coordinate system, wherein the reference structure is an end effector, or a fourth rotary joint, or a third rotary joint. In the above embodiment, the end effector may be replaced by a fourth joint angle, or a third joint angle, after the end effector is changed, the triangles of the convex structure and the concave structure are changed, and the corresponding included angles of the triangles and the included angles of the coordinate axes are determined in the changed triangles. This increases the flexibility of calculating the joint angles, which can be calculated independently of the position of the end effector.
Preferably, according to the geometric parameters of the mechanical arm, determining a first type of joint angle corresponding to the first type of rotation joint by using a geometric algebra method, including: determining the coordinate position of a fifth rotary joint according to the coordinate position of the reference structure and the Euler attitude angle; determining a first joint angle according to the coordinate position of the fifth rotary joint; and determining the second joint angle and the third joint angle by adopting a geometric algebra method according to the coordinate position of the fifth rotary joint.
In the above embodiment, determining the coordinate position of the fifth rotational joint from the coordinate position of the reference structure and the euler attitude angle includes: converting the Euler attitude angles (A, B, C) of the reference structure into an X-Y-Z Euler angle rotation matrix R; translating the coordinate system of the reference structure to the coordinate system of the fifth rotary joint, and determining a translation operator P5 a(ii) a According to the X-Y-Z Euler angle rotation matrix R, the coordinate position Pa (X, Y, Z) of the reference structure and the translation operator P5 aDetermining the coordinate position P5 (P) of the fifth rotary joint5.X，P5.Y，P5.Z)。
Further, X-Y-Z Euler angle rotation matrix
Coordinate position of the fifth rotary joint
P5.X＝X-5.X*r12；P5.Y＝Y-5.X*r22；P5.Z＝Z-5.X*r32
And X is the coordinate movement distance when the coordinate system moves from the end effector to the fifth rotational joint.
Wherein a is 6 when the reference structure is the end effector, 4 when the reference structure is the fourth rotary joint, and 3 when the reference structure is the third rotary joint.
Further, determining the first joint angle from the coordinate position of the fifth rotational joint includes: from the X-axis coordinate position P of the fifth rotary joint5X and Y axis coordinate position P5Y determines a first joint angle Q1; wherein, Q1 ═ arctan2 (P)5.Y，P5.X)。
In another preferred embodiment of the present invention, determining the second and third joint angles using geometric algebra based on the coordinate position of the fifth rotational joint comprises: establishing a second rotary joint coordinate system by taking the second rotary joint as a reference, wherein the second rotary joint coordinate system comprises an X axis, a Y axis and a Z axis, and the origin of the second rotary joint coordinate system is positioned in the center of the bottom of the second rotary joint; judging the combination form of the second connecting rod structure and the third connecting rod structure in a second rotary joint coordinate system; and determining a second joint angle and a third joint angle corresponding to different forms according to the combined forms.
In the above embodiment, the combination form includes: convex and concave structures, as shown in fig. 3 and 4, fig. 3 is a convex structure, and fig. 4 is a concave structure; determining a second joint angle and a third joint angle corresponding to different forms according to the combined forms, wherein the method comprises the following steps: determining each side length in a triangle formed by the second rotary joint, the third rotary joint and the fifth rotary joint; determining an included angle in the triangle and an included angle between the side length and a second rotary joint coordinate system according to the side length; and determining a second joint angle and a third joint angle in the convex structure and a second joint angle and a third joint angle in the concave structure according to the included angle and the side length in the triangle and the included angle of the second rotary joint coordinate system.
Wherein, confirm each length of side in the triangle-shaped that second rotation joint, third rotation joint and fifth rotation joint constitute, include: determining a side AB between the second rotary joint and the third rotary joint, a side AD between the second rotary joint and the fifth rotary joint, and a side BD between the third rotary joint and the fifth rotary joint; wherein,
AB＝2.Z
wherein,in the above formula, 2.Z, 3.X, 3.Z and 4.X are the moving distances of the coordinate system,. X is the moving distance of the X axis,. Z is the moving distance of the Z axis.
Preferably, the determination of the included angle in the triangle and the included angle between the side length and the second rotary joint coordinate system according to the side length comprises determining the included angle β between AB and AD, the included angle Y, AD between AB and BD and the X-axis of the second rotary joint coordinate system, and the included angle theta between BD and the Z-axis of the second rotary joint coordinate system according to AB, AD and BD, wherein,
θ＝arctan2(ED，EB)。
optionally, determining the second joint angle and the third joint angle in the convex structure and the second joint angle and the third joint angle in the concave structure according to the included angle and the side length in the triangle and the included angle of the second rotating joint coordinate system comprises: in the convex configuration, the second joint angleAnd a third joint angleIn the concave configuration, the second joint angleAnd a third joint angle
In still another preferred embodiment of the present invention, determining fourth to sixth joint angles corresponding to fourth to sixth rotary joints by using euler's angle rotation matrix transformation based on the first to third joint angles includes: determining a wrist rotation matrix Rw1 according to the first to third joint angles; determining an X-Y-X Euler angle rotation matrix Rw2 according to the fourth to sixth joint angles; solving for a fourth joint angle Q4, a fifth joint angle Q5 and a sixth joint angle Q6 according to Rw1 and Rw 2.
Wherein the wrist rotation matrix Rw1 ^ Ra ^ T ^ R,
Qz-Q1, Qy-Q2 + Q3, wherein Q1 is a first joint angle, Q2 is a second joint angle, and Q3 is a third joint angle; X-Y-X Euler angle rotation matrix
Let Rw1 be Rw2, Rw1 and Rw2 have a one-to-one correspondence of elements in the matrix, Q5 gets two solutions:
q4 yields a solution:
Q4.1＝arctan2(rw21，rw31)
q6 yields a solution:
Q6.1＝arctan2(rw12，rw13)。
in the above embodiments, the fourth to sixth joint angles are determined from Rw1 and Rw2, including: respectively calculating the difference values of the Q5.1, the Q5.2 and the joint angle of the fifth rotary joint in the last rotation; and comparing the difference values, and determining a fifth joint angle with a small difference value as an optimal solution. Preferably, the fourth to sixth joint angles are determined from Rw1 and Rw2, further comprising: when the fifth joint angle is a singular point, the joint angle at the time of the last rotation of the fourth rotary joint is set as the fourth joint angle Q4; the sixth joint angle Q6 is arctan2(r23, r33) -Q4.
In a preferred embodiment 1 of the present invention, there is further provided a method for inverse kinematics solution of a robot arm, and specifically, fig. 5 shows an alternative flowchart of the method, as shown in fig. 5, the method includes the following steps:
starting;
s1: determining geometric parameters of all connecting rod structures of the whole industrial robot, and positions and postures (X, Y, Z, A, B and C) of the end effector;
s2: transforming the terminal attitude Euler angles A, B and C into a rotation matrix R of 3x 3;
s3: converting the position coordinates of the joint 5 from the tail end through a translation operator and a rotation operator, then determining the joint angle of the joint 1, converting a coordinate system into the joint 2 through coordinate transformation, and solving the joint angles of the joint 2 and the joint 3 through a geometric algebra method;
s4: converting the joint angles of the joints 1, 2 and 3 obtained in the previous step into wrist rotation matrixes of the joints 4, 5 and 6 through rotation matrixes, and then respectively obtaining the joint angles of the joints 4, 5 and 6 through parameter definition in the rotation matrixes converted by Euler;
s5: two solutions can occur due to the joint angles 4, 5 and 6, and the optimal solution is selected by comparing with the previous joint angle;
and (6) ending.
In the above embodiment, the robot arm is divided into the front three-arm part and the rear three-arm part, and each joint angle is solved by using a geometric algebra method and a wrist euler angle rotation transformation, without depending on the traditional D-H parameter modeling, so that the problems of complicated and difficult solving process, large calculated amount and low solving efficiency in the prior art are effectively solved, the solving process is simple and easy to understand, the code amount is simplified, the calculating speed is high, and the precision is high.
Example 2
Based on the inverse solution method for kinematics of the mechanical arm provided in the foregoing embodiment 1, in a preferred embodiment 2 of the present invention, there is further provided an inverse solution apparatus for kinematics of the mechanical arm, and specifically, fig. 6 shows an alternative structural block diagram of the apparatus, and as shown in fig. 6, the apparatus includes:
a detector 602 for determining a geometric parameter of a robotic arm, wherein the robotic arm comprises a plurality of rotational joints, and the rotational joints are classified into a first type rotational joint and a second type rotational joint;
the first processor 604 is connected with the detector 602 and is used for determining a first type joint angle corresponding to a first type rotary joint by adopting a geometric algebra method according to the geometric parameters of the mechanical arm;
and the second processor 606 is connected with the first processor 604 and is used for determining a second type joint angle corresponding to a second type rotary joint by using an euler angle rotation matrix transformation method according to the first type joint angle, wherein the first type joint angle and the second type joint angle are used for controlling the operation of the mechanical arm.
In the above embodiment, the robot arm is divided into the front three-arm part and the rear three-arm part, and each joint angle is solved by using a geometric algebra method and a wrist euler angle rotation transformation, without depending on the traditional D-H parameter modeling, so that the problems of complicated and difficult solving process, large calculated amount and low solving efficiency in the prior art are effectively solved, the solving process is simple and easy to understand, the code amount is simplified, the calculating speed is high, and the precision is high.
With regard to the apparatus in the above embodiments, the specific manner in which each unit and each module performs operations has been described in detail in the embodiments related to the method, and will not be described in detail herein.
Example 3
Based on the inverse kinematics solution device of the mechanical arm provided in the above embodiment 2, in a preferred embodiment 3 of the present invention, a mechanical arm is also provided, which includes the inverse kinematics solution device of the mechanical arm as described above.
In the above embodiment, the robot arm is divided into the front three-arm part and the rear three-arm part, and each joint angle is solved by using a geometric algebra method and a wrist euler angle rotation transformation, without depending on the traditional D-H parameter modeling, so that the problems of complicated and difficult solving process, large calculated amount and low solving efficiency in the prior art are effectively solved, the solving process is simple and easy to understand, the code amount is simplified, the calculating speed is high, and the precision is high.
Example 4
Based on the inverse solution method for the kinematics of the mechanical arm provided in the above embodiment 1, in a preferred embodiment 4 of the present invention, there is further provided a computer device, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement the inverse solution method for the kinematics of the mechanical arm as in the above embodiment 1.
In the above embodiment, the robot arm is divided into the front three-arm part and the rear three-arm part, and each joint angle is solved by using a geometric algebra method and a wrist euler angle rotation transformation, without depending on the traditional D-H parameter modeling, so that the problems of complicated and difficult solving process, large calculated amount and low solving efficiency in the prior art are effectively solved, the solving process is simple and easy to understand, the code amount is simplified, the calculating speed is high, and the precision is high.
Example 5
Based on the inverse solution method for robot arm kinematics provided in embodiment 1 above, there is also provided in a preferred embodiment 5 of the present invention a storage medium containing computer-executable instructions for performing the inverse solution method for robot arm kinematics as in embodiment 1 above when executed by a computer processor.
In the above embodiment, the robot arm is divided into the front three-arm part and the rear three-arm part, and each joint angle is solved by using a geometric algebra method and a wrist euler angle rotation transformation, without depending on the traditional D-H parameter modeling, so that the problems of complicated and difficult solving process, large calculated amount and low solving efficiency in the prior art are effectively solved, the solving process is simple and easy to understand, the code amount is simplified, the calculating speed is high, and the precision is high.
Other embodiments of the invention will be apparent to those skilled in the art from consideration of the specification and practice of the invention disclosed herein. This application is intended to cover any variations, uses, or adaptations of the invention following, in general, the principles of the invention and including such departures from the present disclosure as come within known or customary practice within the art to which the invention pertains. It is intended that the specification and examples be considered as exemplary only, with a true scope and spirit of the invention being indicated by the following claims.
It will be understood that the invention is not limited to the precise arrangements described above and shown in the drawings and that various modifications and changes may be made without departing from the scope thereof. The scope of the invention is limited only by the appended claims.
Claims (20)
1. A kinematic inverse solution method of a mechanical arm is characterized by comprising the following steps:
determining geometrical parameters of a mechanical arm, wherein the mechanical arm comprises a plurality of rotary joints, and the rotary joints are divided into a first type of rotary joint and a second type of rotary joint;
determining a first class joint angle corresponding to the first class rotary joint by adopting a geometric algebra method according to the geometric parameters of the mechanical arm;
and determining a second type joint angle corresponding to the second type rotary joint by adopting an Euler angle rotation matrix transformation method according to the first type joint angle, wherein the first type joint angle and the second type joint angle are used for controlling the operation of the mechanical arm.
2. The method of claim 1, wherein the robotic arm comprises six rotational joints, the first type of rotational joint comprises first through third rotational joints, the first type of joint angle comprises first through third joint angles, the second type of rotational joint comprises fourth through sixth rotational joints, and the second type of joint angle comprises fourth through sixth joint angles.
3. The method of claim 2, wherein the robotic arm further comprises five linkages and one end effector; the determining the geometric parameters of the mechanical arm comprises the following steps:
establishing a reference coordinate system by taking a first connecting rod structure as a reference, wherein the reference coordinate system comprises an X axis, a Y axis and a Z axis, and the origin of the reference coordinate system is positioned in the center of the bottom of the first connecting rod structure;
and determining the coordinate position and the Euler attitude angle of a reference structure in the reference coordinate system, wherein the reference structure is the end effector, or the fourth rotary joint, or the third rotary joint.
4. The method according to claim 3, wherein the determining the first type of joint angle corresponding to the first type of rotary joint by using a geometric algebra method according to the geometric parameters of the mechanical arm comprises:
determining the coordinate position of a fifth rotary joint according to the coordinate position of the reference structure and the Euler attitude angle;
determining the first joint angle according to the coordinate position of the fifth rotary joint;
and determining the second joint angle and the third joint angle by adopting a geometric algebra method according to the coordinate position of the fifth rotary joint.
5. The method of claim 4, wherein determining the coordinate position of the fifth rotational joint from the coordinate position of the reference structure and the Euler pose angle comprises:
converting the Euler attitude angles (A, B, C) of the reference structure into an X-Y-Z Euler angular rotation matrix R;
translating the coordinate system of the reference structure to the coordinate system of the fifth rotary joint, and determining a translation operator P5 a；
According to the X-Y-Z Euler angular rotation matrix R, the coordinate position Pa (X, Y, Z) of the reference structure and the translation operator P5 aDetermining the coordinate position P5 (P) of the fifth rotary joint5.X，P5.Y，P5.Z)。
6. The method of claim 5,
the X-Y-Z Euler angle rotation matrix
Coordinate position of the fifth rotary joint
P5.X＝X-5.X*r12；P5.Y＝Y-5.X*r22；P5.Z＝Z-5.X*r32
Wherein a is 6 when the reference structure is the end effector, a is 4 when the reference structure is the fourth rotational joint, and a is 3 when the reference structure is the third rotational joint.
7. The method of claim 5, wherein determining the first joint angle from the coordinate position of the fifth rotational joint comprises:
according to the X-axis coordinate position P of the fifth rotary joint5X and Y axis coordinate position P5Y determines the first joint angle Q1; wherein,
Q1＝arctan2(P5.Y，P5.X)。
8. the method of claim 5, wherein determining the second and third joint angles using geometric algebra from the coordinate position of the fifth rotational joint comprises:
establishing a second rotary joint coordinate system by taking a second rotary joint as a reference, wherein the second rotary joint coordinate system comprises an X axis, a Y axis and a Z axis, and the origin of the second rotary joint coordinate system is positioned in the center of the bottom of the second rotary joint;
judging the combination state of the second connecting rod structure and the third connecting rod structure in the second rotary joint coordinate system;
and determining the second joint angle and the third joint angle corresponding to different forms according to the combined form.
9. The method of claim 8, wherein the combined modality comprises: a convex structure and a concave structure; the determining the second joint angle and the third joint angle corresponding to different forms according to the combined form comprises:
determining each side length of a triangle formed by the second rotary joint, the third rotary joint and the fifth rotary joint;
determining an included angle in the triangle and an included angle between the side length and the second rotary joint coordinate system according to the side length;
and determining the second joint angle and the third joint angle in the convex structure and the second joint angle and the third joint angle in the concave structure according to the included angle in the triangle, the included angle between the side length and a second rotary joint coordinate system.
10. The method of claim 9, wherein determining the side length of each of the triangle formed by the second rotational joint, the third rotational joint, and the fifth rotational joint comprises:
determining a side AB between the second and third rotational joints, a side AD between the second and fifth rotational joints, and a side BD between the third and fifth rotational joints; wherein,
AB＝2.Z
wherein AH-OG-2. X HD-P5.Z-2.Z
11. The method of claim 10, wherein determining the included angle in the triangle and the included angle of the side length and the second rotational joint coordinate system according to the side length comprises:
determining an included angle β between AB and AD, an included angle gamma between AB and BD, an included angle psi between AD and the X axis of the second rotary joint coordinate system and an included angle theta between BD and the Z axis of the second rotary joint coordinate system according to AB, AD and BD,
cosγ＝(BD2+AB2-AD2)/(2*BD*AB)
cosγ＝(BD2+AB2-AD2)/(2*BD*AB)
θ＝arctan2(ED，EB)。
12. the method of claim 11, wherein determining the second and third joint angles in the convex structure and the second and third joint angles in the concave structure from the included angle in the triangle and the included angle of the side length to a second rotational joint coordinate system comprises:
in the convex structure, the second joint angleAnd a third joint angle Q3-180- γ - θ;
in the concave structure, the second joint angleAnd a third joint angle Q3 ═ γ + θ -180.
13. The method according to claim 1, wherein determining fourth to sixth joint angles corresponding to fourth to sixth rotary joints by using euler angle rotation matrix transformation according to the first to third joint angles comprises:
determining a wrist rotation matrix Rw1 according to the first to third joint angles;
determining an X-Y-X Euler angle rotation matrix Rw2 according to the fourth to sixth joint angles;
solving for the fourth joint angle Q4, fifth joint angle Q5, and sixth joint angle Q6 from the Rw1 and the Rw 2.
14. The method according to claim 13, wherein the wrist rotation matrix Rw1 ═ Ra T ^ R, wherein,
Qz-Q1, Qy-Q2 + Q3, wherein Q1 is the first joint angle, Q2 is the second joint angle, and Q3 is the third joint angle;
the X-Y-X Euler angle rotation matrix
Let Rw1 be Rw2, and Rw1 and the elements in the Rw2 matrix correspond one-to-one, Q5 gets two solutions:
q4 yields a solution:
Q4.1＝arctan2(rw21，rw31)
q6 yields a solution:
Q6.1＝arctan2(rw12，rw13)。
15. the method of claim 14, wherein said determining said fourth-sixth joint angles from said Rw1 and said Rw2 comprises:
respectively calculating the difference values of the joint angles of the Q5.1, the Q5.2 and the fifth rotary joint in the last rotation;
and comparing the difference values, and determining a fifth joint angle with a small difference value as an optimal solution.
16. The method of claim 15, wherein said determining said fourth through sixth joint angles from said Rw1 and said Rw2 further comprises:
when the fifth joint angle is a singular point, setting the joint angle at the last rotation of the fourth rotary joint as the fourth joint angle Q4;
the sixth joint angle Q6 is arctan2(r23, r33) -Q4.
17. A kinematic inverse solution device for a mechanical arm is characterized by comprising:
a detector for determining a geometric parameter of a robotic arm, wherein the robotic arm comprises a plurality of rotational joints, the rotational joints being classified into a first type of rotational joint and a second type of rotational joint;
the first processor is used for determining a first type of joint angle corresponding to the first type of rotary joint by adopting a geometric algebra method according to the geometric parameters of the mechanical arm;
and the second processor is used for determining a second type joint angle corresponding to the second type rotary joint by adopting an Euler angle rotation matrix transformation method according to the first type joint angle, wherein the first type joint angle and the second type joint angle are used for controlling the operation of the mechanical arm.
18. A robot arm comprising the inverse kinematics resolution assembly of the robot arm of claim 17.
19. A computer device comprising a memory, a processor and a computer program stored on the memory and executable on the processor, wherein the processor when executing the program implements the inverse kinematics solution of a robotic arm according to any of claims 1 to 16.
20. A storage medium containing computer executable instructions for performing the inverse kinematics solution of a robotic arm as claimed in any one of claims 1 to 16 when executed by a computer processor.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810864277.7A CN109129469B (en) | 2018-08-01 | 2018-08-01 | Mechanical arm kinematics inverse solution method and device and mechanical arm |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810864277.7A CN109129469B (en) | 2018-08-01 | 2018-08-01 | Mechanical arm kinematics inverse solution method and device and mechanical arm |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN109129469A true CN109129469A (en) | 2019-01-04 |
| CN109129469B CN109129469B (en) | 2020-01-21 |
Family
ID=64798639
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201810864277.7A Active CN109129469B (en) | 2018-08-01 | 2018-08-01 | Mechanical arm kinematics inverse solution method and device and mechanical arm |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN109129469B (en) |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111430022A (en) * | 2020-03-20 | 2020-07-17 | 苏州新医智越机器人科技有限公司 | Puncture algorithm of puncture surgical robot |
| CN112621713A (en) * | 2020-11-30 | 2021-04-09 | 珠海格力电器股份有限公司 | Robot demonstrator, controller, robot demonstrator and robot demonstrator control method, robot demonstrator control device and robot demonstrator control medium |
| CN113733037A (en) * | 2021-09-13 | 2021-12-03 | 哈工大机器人（合肥）国际创新研究院 | Seven-degree-of-freedom master-slave isomorphic teleoperation master hand |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20050143860A1 (en) * | 2003-12-26 | 2005-06-30 | Japan Aerospace Exploration Agency | Method of controlling redundant manipulator |
| CN102243620A (en) * | 2011-06-02 | 2011-11-16 | 安凯 | Rapid solving method for inverse kinematics problem of six-joint mechanical arm |
| CN103942427A (en) * | 2014-04-11 | 2014-07-23 | 哈尔滨工程大学 | Quick and simple method for solving inverse kinematics of six-degree-of-freedom mechanical arm |
| CN105856231A (en) * | 2016-01-25 | 2016-08-17 | 广东工业大学 | Movement control method for six-shaft industrial robot of specific structure |
-
2018
- 2018-08-01 CN CN201810864277.7A patent/CN109129469B/en active Active
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20050143860A1 (en) * | 2003-12-26 | 2005-06-30 | Japan Aerospace Exploration Agency | Method of controlling redundant manipulator |
| CN102243620A (en) * | 2011-06-02 | 2011-11-16 | 安凯 | Rapid solving method for inverse kinematics problem of six-joint mechanical arm |
| CN103942427A (en) * | 2014-04-11 | 2014-07-23 | 哈尔滨工程大学 | Quick and simple method for solving inverse kinematics of six-degree-of-freedom mechanical arm |
| CN105856231A (en) * | 2016-01-25 | 2016-08-17 | 广东工业大学 | Movement control method for six-shaft industrial robot of specific structure |
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111430022A (en) * | 2020-03-20 | 2020-07-17 | 苏州新医智越机器人科技有限公司 | Puncture algorithm of puncture surgical robot |
| CN112621713A (en) * | 2020-11-30 | 2021-04-09 | 珠海格力电器股份有限公司 | Robot demonstrator, controller, robot demonstrator and robot demonstrator control method, robot demonstrator control device and robot demonstrator control medium |
| CN113733037A (en) * | 2021-09-13 | 2021-12-03 | 哈工大机器人（合肥）国际创新研究院 | Seven-degree-of-freedom master-slave isomorphic teleoperation master hand |
| CN113733037B (en) * | 2021-09-13 | 2022-09-09 | 哈工大机器人（合肥）国际创新研究院 | Seven-degree-of-freedom master-slave isomorphic teleoperation master hand |
Also Published As
| Publication number | Publication date |
|---|---|
| CN109129469B (en) | 2020-01-21 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US10926414B2 (en) | System and method for calibrating tool center point of robot | |
| JP5114019B2 (en) | Method for controlling the trajectory of an effector | |
| JP3207728B2 (en) | Control method of redundant manipulator | |
| CN103942427B (en) | Quick and simple method for solving inverse kinematics of six-degree-of-freedom mechanical arm | |
| CN104635762B (en) | A kind of autokinesis angle computational methods towards SRS copy man arms | |
| CN113319857B (en) | Mechanical arm force and position hybrid control method and device, electronic equipment and storage medium | |
| WO2018090323A1 (en) | Method, system, and device for calibrating coordinate system | |
| CN103692433B (en) | Model decoupling three-arm-lever five-freedom-degree translation welding robot and decoupling method thereof | |
| CN107443382A (en) | Industrial robot structure parameter error recognizes and compensation method | |
| CN108621162A (en) | A kind of manipulator motion planning method | |
| CN109129469B (en) | Mechanical arm kinematics inverse solution method and device and mechanical arm | |
| CN105014677A (en) | Visual mechanical arm control device and method based on Camshift visual tracking and D-H modeling algorithms | |
| CN111113425B (en) | Inverse solution method for kinematics of five-degree-of-freedom series-parallel robot with parasitic motion | |
| CN109311155B (en) | Method and device for calibrating tool coordinate system origin of industrial robot | |
| CN107253191B (en) | Double-mechanical-arm system and coordination control method thereof | |
| CN114888812B (en) | Mobile mechanical arm station planning method based on rigidity performance optimization | |
| CN105643619B (en) | A kind of industrial robot instrument posture control method of use framework description | |
| CN109866224B (en) | A robot Jacobian matrix calculation method, device and storage medium | |
| CN108908347A (en) | One kind is towards redundancy mobile mechanical arm error-tolerance type repetitive motion planning method | |
| CN107885916A (en) | A kind of drill jumbo drill boom Analytical Methods of Kinematics based on CFDH methods | |
| JP5223407B2 (en) | Redundant robot teaching method | |
| CN114055467B (en) | Space pose online simulation system based on five degrees of freedom robot | |
| Wen et al. | Singularities in three-legged platform-type parallel mechanisms | |
| CN107791248A (en) | Control method based on the six degree of freedom serial manipulator for being unsatisfactory for pipper criterions | |
| KR20220099535A (en) | Inverse kinematics solver for wrist offset robots |
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 |