CN113722864B - 7-degree-of-freedom redundant mechanical arm inverse kinematics solving method and system - Google Patents

7-degree-of-freedom redundant mechanical arm inverse kinematics solving method and system Download PDF

Info

Publication number
CN113722864B
CN113722864B CN202111069374.5A CN202111069374A CN113722864B CN 113722864 B CN113722864 B CN 113722864B CN 202111069374 A CN202111069374 A CN 202111069374A CN 113722864 B CN113722864 B CN 113722864B
Authority
CN
China
Prior art keywords
joint
equation
coordinate system
row
matrix
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202111069374.5A
Other languages
Chinese (zh)
Other versions
CN113722864A (en
Inventor
朱跃
仲军
龚锦
韩震峰
于振中
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
HRG International Institute for Research and Innovation
Original Assignee
HRG International Institute for Research and Innovation
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by HRG International Institute for Research and Innovation filed Critical HRG International Institute for Research and Innovation
Priority to CN202111069374.5A priority Critical patent/CN113722864B/en
Publication of CN113722864A publication Critical patent/CN113722864A/en
Application granted granted Critical
Publication of CN113722864B publication Critical patent/CN113722864B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F30/00—Computer-aided design [CAD]
    • G06F30/10—Geometric CAD
    • G06F30/17—Mechanical parametric or variational design
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/11—Complex mathematical operations for solving equations, e.g. nonlinear equations, general mathematical optimization problems
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F30/00—Computer-aided design [CAD]
    • G06F30/20—Design optimisation, verification or simulation
    • 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

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

Abstract

The invention discloses a 7-degree-of-freedom redundant mechanical arm inverse kinematics solving method, which relates to the field of industrial robots. The invention greatly reduces the number of inverse kinematics solutions of the redundant mechanical arms with 7 degrees of freedom, effectively improves the solving speed and reduces the solving difficulty.

Description

7-degree-of-freedom redundant mechanical arm inverse kinematics solving method and system
Technical Field
The invention relates to the technical field of industrial robots, in particular to a 7-degree-of-freedom redundant mechanical arm inverse kinematics solving method and system.
Background
The inverse kinematics solving problem of the mechanical arm aims at solving the angles of all joints according to the known pose of the tail end coordinate system of the mechanical arm relative to the base coordinate system and all connecting rod parameters, and the inverse kinematics is a basis for application research of an industrial robot and plays a very important role in the problems of teleoperation, track planning, real-time control, collision prevention and the like of the mechanical arm. The existing inverse kinematics solving method is mainly divided into two main types, namely a closed solving method and a numerical solving method, and because the iterative principle of the numerical solving method, the solving speed of the numerical solving method is much slower than that of the closed solving method, the existing inverse kinematics solving method of the industrial robot is mainly divided into an algebraic method and a geometric method by adopting the closed solving method.
Compared with a 6-degree-of-freedom mechanical arm, the 7-degree-of-freedom mechanical arm is difficult to solve in the inverse kinematics, and the existence of more than 1 redundant degrees of freedom enables numerous theoretical kinematic inverse solutions of the mechanical arm to exist, so that the calculation is extremely complex, and the calculation time is greatly increased.
The application number 202011075243.3 discloses a seven-degree-of-freedom mechanical arm limiting optimization method based on position-level inverse kinematics, and relates to a seven-degree-of-freedom mechanical arm limiting optimization method. The invention aims to solve the problems that the existing numerical solution cannot obtain a closed solution and has final state self-movement; the analytic solution cannot be aimed at the bias configuration, and the problem that motion optimization cannot be achieved exists. Firstly, based on a parameterized solving method for fixing a certain joint angle, an analytical solution of inverse kinematics of a 7-degree-of-freedom mechanical arm is obtained; then taking the fixed joint angle parameter as input, taking joint limit as an optimization index, and establishing an optimal control problem; converting the constraint problem into an unconstrained problem based on the Lagrangian multiplier method; and finally, solving the optimal joint angle parameters based on a Newton iteration method, and obtaining 7 joint space tracks considering joint limit optimization through giving an initial configuration, an expected terminal pose and Cartesian path planning. Although the invention employs fixing one of the joints to achieve optimal control, the specific algorithm provided is not applicable to the 7-degree-of-freedom robotic arm configuration provided by the invention.
Disclosure of Invention
The technical problem to be solved by the invention is that in the prior art, the inverse kinematics solving process of the redundant mechanical arm with 7 degrees of freedom is too many in inverse solution, and the calculation speed is slow.
The invention solves the technical problems by the following technical means:
the inverse kinematics solving method of the redundant mechanical arm with 7 degrees of freedom comprises the following steps: 1 st joint coordinate system x 1 z 1 y 1 And a base coordinate system x 0 z 0 y 0 Coincidence, 2 nd passNode coordinate system x 2 z 2 y 2 Around x by a first joint coordinate system 1 The axis rotates by 90 DEG and then winds z 1 Rotated 90 deg. and along x 1 Axis movement a1, 3 rd joint coordinate System x 3 z 3 y 3 Z is surrounded by the 2 nd joint coordinate system 2 The axis rotates 90 DEG and along x 2 Axis movement a2, 4 th joint coordinate system x 4 z 4 y 4 From joint 3 along x 3 Axis movement a3, 5 th joint coordinate System x 5 z 5 y 5 From joint 4 along x 4 Axis-shifting a4, rotation of the 6 th joint coordinate system by 90 degrees along the x-axis, rotation by 90 degrees along the z-axis, and rotation along the x-axis from the 5 th joint coordinate system 5 Move a5, along y 5 -d6, rotation of the 7 th joint coordinate system by 90 ° about the x-axis from the 5 th joint coordinate system; all joints rotate by taking the respective z-axis as the rotation center; the method comprises the following steps:
s1, establishing a D-H coordinate system and a kinematic matrix according to the configuration and parameters of the mechanical arm;
s2, solving the joint angles of the mechanical arm which do not form redundancy by adopting an algebraic method;
s3, drawing a connecting rod plan according to the redundant joint of the mechanical arm;
s4, fixing a redundant joint to enable the angle of the redundant joint to be a known value;
and S5, solving the kinematic inverse solution of the redundant joint by adopting a method of combining a geometric method and an algebraic method.
The method comprises the steps of firstly establishing a D-H coordinate system and a kinematic matrix equation of the mechanical arm according to the configuration and parameters of the mechanical arm, then solving the angles of joints which do not form redundancy through algebraic methods, drawing a connecting rod plan according to the redundant joints, and finally solving kinematic inverse solutions of the angles of other redundant joints through the combination of algebraic methods and geometric methods by adopting a method for fixing one joint. The invention greatly reduces the number of inverse kinematics solutions of the redundant mechanical arms with 7 degrees of freedom, effectively improves the solving speed and reduces the solving difficulty.
Further, the solving method of the S1 comprises the following steps:
s11, obtaining the terminal seat according to a second transformation equation between the coordinate systems of the connecting rodsHomogeneous coordinate transformation matrix of standard system relative to basic coordinate systemSatisfy formula (1):
wherein ,representing the secondary coordinate transformation matrix of the adjacent two-link coordinate system; the connecting rod is a connecting piece between two adjacent joints; wherein n, o, a represent the pose of the terminal joint coordinate system relative to the polar coordinate system, and p represents the position of the terminal joint coordinate system relative to the polar coordinate system.
Further, the solving method of the S2 comprises the following steps:
s21, firstly, multiplying the two ends of the equation (1) by the left side simultaneouslyObtaining:
the equality of the elements of row 2 and column 4 of the matrix at both ends according to equation (2) can establish the equation:
-s 1 p x +c 1 p y ＝0(3)
the 1 st joint angle θ can be obtained according to equation (3) 1 There are two solutions, respectively: atan2 (p) y ,p x ) And Atan2 (-p) y ,-p x )；
The equality of the elements of row 3 and column 2 of the matrix at both ends according to equation (2) can establish the equation:
c 6 ＝-s 1 a x +c 1 a y ＝k 1 (4)
the 6 th joint angle theta can be obtained by the method (4) 6 The value of (2) is divided into two solutions, namelyDue to theta 1 There are two solutions, see θ 6 There are 4 solutions to the value of (2);
then multiplying at both ends of equation (2) with the right endThe method can obtain:
according to equation (5), the 1 st row and 1 st column elements and the 3 rd row and 3 rd column elements of the two-end matrix are equal, it is obtained that:
(n x +c 1 c 6 o z )c 7 +(c 1 c 6 n z -o x )s 7 ＝-s 1 s 6 (6)
let n x +c 1 c 6 o z ＝k 2 ，c 1 c 6 n z -o x ＝k 3 The 7 th joint angle theta can be obtained 7 Solution to (1)
Further, the solving method of the S3 comprises the following steps:
s31, obtaining the L according to the connecting rod plan OA ＝a 2 ，l AB ＝a 3 ，l BC ＝a 4 ，D point coordinates are
Further, the invention fixes the joint angle of the 5 th joint; the solving method of the S5 comprises the following steps:
s51, right multiplying at both ends of equation (1)The method can obtain:
the coordinates of the point B in the basic coordinate system are made to be% 4 p x ， 4 p y ， 4 p z ) The coordinates of the point B can be obtained asThereby get->Then->Can obtain the 3 rd joint angle theta 3 Solution of->
S52, according to the equation (7), the elements of the 1 st row and the 3 rd column of the two-end matrix are equal to the elements of the 2 nd row and the 3 rd column, and the following can be obtained:
4 p x ＝a 1 c 1 +a 2 c 1 c 2 +a 3 c 1 c 23 (8)
let k 5 ＝a 2 c 1 +a 3 c 1 c 3 ,k 6 ＝-a 3 c 1 s 3 ,k 7 ＝ 4 p x -a 1 c 1 The method can obtain:
k 7 ＝k 5 c 2 +k 6 s 2 (9)
the 2 nd joint angle can be obtained by the formula (9)
S53, according to the equation (7), the elements of the 1 st row and the 3 rd column of the matrix are equal to the elements of the 3 rd row and the 3 rd column, and the following can be obtained:
s 234 ＝-(c 5 c 6 c 7 -s 5 s 7 )n z +(c 5 c 6 s 7 +s 5 c 7 )o z +(c 5 s 6 )a z (10)
c 234 ＝(s 5 c 6 c 7 +c 5 s 7 )n z -(-s 5 c 6 s 7 +s 5 c 7 )o z -(s 5 s 6 )a z (11)
θ 234 ＝atan2(s 234 ,c 234 )(12)
the 4 th joint angle θ can be obtained by the formula (12) 4 ＝θ 234 -θ 2 -θ 3 。
Corresponding to the method, the invention also provides a 7-degree-of-freedom redundant mechanical arm inverse kinematics solving system, which comprises:
the D-H coordinate system and kinematics matrix establishment module is used for establishing a D-H coordinate system and a kinematics matrix according to the configuration and parameters of the mechanical arm;
the joint angle solving module is used for solving the joint angle of the mechanical arm which does not form redundancy by adopting algebraic method;
the connecting rod plan drawing module is used for drawing a connecting rod plan according to the redundant joint of the mechanical arm;
the redundant joint fixing module is used for fixing a redundant joint to enable the angle of the redundant joint to be a known value;
and the calculation module is used for solving the kinematic inverse solution of the redundant joint by adopting a method of combining a geometric method and algebraic correlation.
Further, the solution method of the D-H coordinate system and the kinematics matrix establishment module comprises the following steps:
s11, obtaining a homogeneous coordinate transformation matrix of the terminal coordinate system relative to the base coordinate system according to a second transformation equation between the connecting rod coordinate systemsSatisfy formula (1):
wherein ,representing the secondary coordinate transformation matrix of the adjacent two-link coordinate system; the connecting rod is a connecting piece between two adjacent joints; n, o, a represent the pose of the end joint coordinate system relative to the polar coordinate system, and p represents the position of the end joint coordinate system relative to the polar coordinate system.
Further, the solving method of the joint angle solving module comprises the following steps:
s21, firstly, multiplying the two ends of the equation (1) by the left side simultaneouslyObtaining:
the equality of the elements of row 2 and column 4 of the matrix at both ends according to equation (2) can establish the equation:
-s 1 p x +c 1 p y ＝0(3)
the 1 st joint angle θ can be obtained according to equation (3) 1 There are two solutions, respectively: atan2 (p) y ,p x ) And Atan2 (-p) y ,-p x )；
The equality of the elements of row 3 and column 2 of the matrix at both ends according to equation (2) can establish the equation:
c 6 ＝-s 1 a x +c 1 a y ＝k 1 (4)
the 6 th joint angle theta can be obtained by the method (4) 6 The value of (2) is divided into two solutions, namelyDue to theta 1 There are two solutions, see θ 6 There are 4 solutions to the value of (2);
then multiplying at both ends of equation (2) with the right endThe method can obtain:
according to equation (5), the 1 st row and 1 st column elements and the 3 rd row and 3 rd column elements of the two-end matrix are equal, it is obtained that:
(n x +c 1 c 6 o z )c 7 +(c 1 c 6 n z -o x )s 7 ＝-s 1 s 6 (6)
let n x +c 1 c 6 o z ＝k 2 ，c 1 c 6 n z -o x ＝k 3 The 7 th joint angle theta can be obtained 7 Solution to (1)
Further, the solving method of the connecting rod plan drawing module comprises the following steps:
s31, obtaining the L according to the connecting rod plan OA ＝a 2 ，l AB ＝a 3 ，l BC ＝a 4 ，D point coordinates are
Further, the invention fixes the joint angle of the 5 th joint; the solving method of the computing module comprises the following steps:
s51, at both ends of equation (1)At the same time take advantage of rightThe method can obtain:
the coordinates of the point B in the basic coordinate system are made to be% 4 p x ， 4 p y ， 4 p z ) The coordinates of the point B can be obtained asThereby get->Then->Can obtain the 3 rd joint angle theta 3 Solution of->
S52, according to the equation (7), the elements of the 1 st row and the 3 rd column of the two-end matrix are equal to the elements of the 2 nd row and the 3 rd column, and the following can be obtained:
4 p x ＝a 1 c 1 +a 2 c 1 c 2 +a 3 c 1 c 23 (8)
let k 5 ＝a 2 c 1 +a 3 c 1 c 3 ,k 6 ＝-a 3 c 1 s 3 ,k 7 ＝ 4 p x -a 1 c 1 The method can obtain:
k 7 ＝k 5 c 2 +k 6 s 2 (9)
the 2 nd joint angle can be obtained by the formula (9)
S53, according to the equation (7), the elements of the 1 st row and the 3 rd column of the matrix are equal to the elements of the 3 rd row and the 3 rd column, and the following can be obtained:
s 234 ＝-(c 5 c 6 c 7 -s 5 s 7 )n z +(c 5 c 6 s 7 +s 5 c 7 )o z +(c 5 s 6 )a z (10)
c 234 ＝(s 5 c 6 c 7 +c 5 s 7 )n z -(-s 5 c 6 s 7 +s 5 c 7 )o z -(s 5 s 6 )a z (11)
θ 234 ＝atan2(s 234 ,c 234 )(12)
the 4 th joint angle θ can be obtained by the formula (12) 4 ＝θ 234 -θ 2 -θ 3 。
The invention has the advantages that:
the method comprises the steps of firstly establishing a D-H coordinate system and a kinematic matrix equation of the mechanical arm according to the configuration and parameters of the mechanical arm, then solving the angles of joints which do not form redundancy through algebraic methods, drawing a connecting rod plan according to the redundant joints, and finally solving kinematic inverse solutions of the angles of other redundant joints through the combination of algebraic methods and geometric methods by adopting a method for fixing one joint. The invention greatly reduces the number of inverse kinematics solutions of the redundant mechanical arms with 7 degrees of freedom, effectively improves the solving speed and reduces the solving difficulty.
The principle of the invention is to select the joint with smaller influence on other joints as much as possible to fix by fixing the 5 th joint. The analytic solution of each position can be obtained by adopting algebraic method and geometric method.
Drawings
FIG. 1 is a schematic diagram of a 7-degree-of-freedom redundant manipulator according to an embodiment of the present invention;
FIG. 2 is a graph of a 7-degree-of-freedom redundant manipulator in an embodiment of the present invention;
FIG. 3 is a flow chart of a kinematic inverse solution of a 7-degree-of-freedom redundant manipulator according to an embodiment of the present invention;
fig. 4 is a plan view of a redundant joint link of a 7-degree-of-freedom redundant manipulator according to an embodiment of the present invention.
In the figure, 1, a swivel base; 2. a large arm joint; 3. two arm joints; 4. a three-arm joint; 5. swing arm rotary joint; 6. swing arm swing joint; 7. a distal revolute joint; a, a 1 Along x 1 An axis from z 1 Move to z 2 Is a distance of (2); a, a 2 Along x 2 An axis from z 2 Move to z 3 Is a distance of (2); a, a 3 Along x 3 An axis from z 3 Move to z 4 Is a distance of (2); a, a 4 Along x 4 Axis from x 4 Move to z 5 Is a distance of (2); a, a 5 Along x 5 An axis from z 5 Move to z 6 Is a distance of (2); d, d 6 Along z 6 Axis from x 5 Move to x 6 Is a distance of (2); θ 2 Around z 2 Axis from x 1 Rotate to x 2 Is a function of the angle of (2); θ 3 Around z 3 Axis from x 2 Rotate to x 3 Is a function of the angle of (2); θ 4 Around z 4 Axis from x 3 Rotate to x 4 Is a function of the angle of (2); θ 5 Around z 5 Axis from x 4 Rotate to x 5 Is a function of the angle of (a).
Detailed Description
For the purpose of making the objects, technical solutions and advantages of the embodiments of the present invention more apparent, the technical solutions in the embodiments of the present invention will be clearly and completely described in the following in conjunction with the embodiments of the present invention, and it is apparent that the described embodiments are some embodiments of the present invention, but not all embodiments. All other embodiments, which can be made by those skilled in the art based on the embodiments of the invention without making any inventive effort, are intended to be within the scope of the invention.
As shown in fig. 1 and 2, the redundant mechanical arm with 7 degrees of freedom is formed by sequentially rotationally connecting a rotary base 1, a large arm joint 2, a two-arm joint 3, a three-arm joint 4, a swing arm rotary joint 5, a swing arm swing joint 6 and a tail end rotary joint 7; 1 st joint coordinate system x 1 z 1 y 1 And a base coordinate system x 0 z 0 y 0 Coincidence, 2 nd joint coordinate system x 2 z 2 y 2 Around x by a first joint coordinate system 1 The axis rotates by 90 DEG and then winds z 1 Rotated 90 deg. and along x 1 Axis movement a1, 3 rd joint coordinate System x 3 z 3 y 3 Z is surrounded by the 2 nd joint coordinate system 2 The axis rotates 90 DEG and along x 2 Axis movement a2, 4 th joint coordinate system x 4 z 4 y 4 From joint 3 along x 3 Axis movement a3, 5 th joint coordinate System x 5 z 5 y 5 From joint 4 along x 4 Axis-shifting a4, rotation of the 6 th joint coordinate system by 90 degrees along the x-axis, rotation by 90 degrees along the z-axis, and rotation along the x-axis from the 5 th joint coordinate system 5 Move a5, along y 5 Moving-d 6, the 7 th joint coordinate system is rotated 90 ° about the x-axis from the 5 th joint coordinate system. The main parameters of the mechanical arm are shown in table 1.
Specific parameters of redundant mechanical arm with 1 7 degrees of freedom
As shown in fig. 3, the method for solving inverse kinematics of the 7-degree-of-freedom redundant manipulator includes the following steps:
s1, establishing a D-H coordinate system and a kinematic matrix according to the configuration and parameters of the mechanical arm;
s2, solving the joint angles of the mechanical arm which do not form redundancy by adopting an algebraic method;
s3, drawing a connecting rod plan according to the redundant joint of the mechanical arm;
s4, fixing a redundant joint to enable the angle of the redundant joint to be a known value;
and S5, solving the kinematic inverse solution of the redundant joint by adopting a method of combining a geometric method and an algebraic method.
As shown in fig. 4, the method for establishing the link plan of the redundant joint of the redundant mechanical arm with 7 degrees of freedom is to simplify the large arm joint 2, the two arm joint 3, the three arm joint 4 and the swing arm rotating joint 5 into a straight line and take a coordinate system X 2 O 2 Y 2 Is the origin, wherein l OA ＝a 2 ，l AB ＝a 3 ，l BC ＝a 4 ，The D point coordinates are +.>
The working process comprises the following steps: the homogeneous coordinate transformation matrix of the terminal coordinate system relative to the base coordinate system can be obtained according to the second transformation equation between the connecting rod coordinate systemsSatisfy formula (1):
wherein ,representing the secondary coordinate transformation matrix of the adjacent two-link coordinate system; n, o, a represent the pose of the end joint coordinate system relative to the polar coordinate system, and p represents the position of the end joint coordinate system relative to the polar coordinate system.
First, the two ends of equation (1) are multiplied byObtaining:
the equality of the elements of row 2 and column 4 of the matrix at both ends according to equation (2) can establish the equation:
-s 1 p x +c 1 p y ＝0(3)
the 1 st joint angle θ can be obtained according to equation (3) 1 There are two solutions, respectively: atan2 (p) y ,p x ) And Atan2 (-p) y ,-p x ) The method comprises the steps of carrying out a first treatment on the surface of the s represents sin and c represents cos.
The equality of the elements of row 3 and column 2 of the matrix at both ends according to equation (2) can establish the equation:
c 6 ＝-s 1 a x +c 1 a y ＝k 1 (4)
the 6 th joint angle theta can be obtained by the method (4) 6 The value of (2) is divided into two solutions, namelyDue to theta 1 There are two solutions, see θ 6 There are 4 solutions to the value of (2).
Then multiplying at both ends of equation (2) with the right endThe method can obtain:
according to equation (5), the 1 st row and 1 st column elements and the 3 rd row and 3 rd column elements of the two-end matrix are equal, it is obtained that:
(n x +c 1 c 6 o z )c 7 +(c 1 c 6 n z -o x )s 7 ＝-s 1 s 6 (6)
let n x +c 1 c 6 o z ＝k 2 ，c 1 c 6 n z -o x ＝k 3 The 7 th joint angle theta can be obtained 7 Solution to (1)
Selecting and fixing the joint angle of the 5 th joint and setting theta 5 ＝90°。
Right multiplication is performed simultaneously at both ends of equation (1)The method can obtain:
the coordinates of the point B in the basic coordinate system are made to be% 4 p x ， 4 p y ， 4 p z ) The coordinates of the point B can be obtained asThereby get->Then->Can obtain the 3 rd joint angle theta 3 Solution of->
Let the 1 st row and 3 rd column elements and the 2 nd row and 3 rd column elements of the matrix at both ends of equation (7) be equal, it can be obtained that:
4 p x ＝a 1 c 1 +a 2 c 1 c 2 +a 3 c 1 c 23 (8)
let k 5 ＝a 2 c 1 +a 3 c 1 c 3 ,k 6 ＝-a 3 c 1 s 3 ,k 7 ＝ 4 p x -a 1 c 1 The method can obtain:
k 7 ＝k 5 c 2 +k 6 s 2 (9)
the 2 nd joint angle can be obtained by the formula (9)
Equaling the equation (7) matrix row 1, column 3 elements and row 3, column 3 elements, we can obtain:
s 234 ＝-(c 5 c 6 c 7 -s 5 s 7 )n z +(c 5 c 6 s 7 +s 5 c 7 )o z +(c 5 s 6 )a z (10)
c 234 ＝(s 5 c 6 c 7 +c 5 s 7 )n z -(-s 5 c 6 s 7 +s 5 c 7 )o z -(s 5 s 6 )a z (11)
θ 234 ＝atan2(s 234 ,c 234 )(12)
the 4 th joint angle θ can be obtained by the formula (12) 4 ＝θ 234 -θ 2 -θ 3 。
Corresponding to the method, the embodiment also provides a 7-degree-of-freedom redundant mechanical arm inverse kinematics solving system, which comprises the following steps:
the D-H coordinate system and kinematics matrix establishment module is used for establishing a D-H coordinate system and a kinematics matrix according to the configuration and parameters of the mechanical arm;
the joint angle solving module is used for solving the joint angle of the mechanical arm which does not form redundancy by adopting algebraic method;
the connecting rod plan drawing module is used for drawing a connecting rod plan according to the redundant joint of the mechanical arm;
the redundant joint fixing module is used for fixing a redundant joint to enable the angle of the redundant joint to be a known value;
and the calculation module is used for solving the kinematic inverse solution of the redundant joint by adopting a method of combining a geometric method and algebraic correlation.
As shown in fig. 4, the method for establishing the link plan of the redundant joint of the redundant mechanical arm with 7 degrees of freedom is to simplify the large arm joint 2, the two arm joint 3, the three arm joint 4 and the swing arm rotating joint 5 into a straight line and take a coordinate system X 2 O 2 Y 2 Is the origin, wherein l OA ＝a 2 ，l AB ＝a 3 ，l BC ＝a 4 ，The D point coordinates are +.>
The working process comprises the following steps: the homogeneous coordinate transformation matrix of the terminal coordinate system relative to the base coordinate system can be obtained according to the second transformation equation between the connecting rod coordinate systemsSatisfy formula (1):
wherein ,representing the secondary coordinate transformation matrix of the adjacent two-link coordinate system.
First, the two ends of equation (1) are multiplied byObtaining:
the equality of the elements of row 2 and column 4 of the matrix at both ends according to equation (2) can establish the equation:
-s 1 p x +c 1 p y ＝0(3)
the 1 st joint angle θ can be obtained according to equation (3) 1 There are two solutions, respectively: atan2 (p) y ,p x ) And Atan2 (-p) y ,-p x )；
The equality of the elements of row 3 and column 2 of the matrix at both ends according to equation (2) can establish the equation:
c 6 ＝-s 1 a x +c 1 a y ＝k 1 (4)
the 6 th joint angle theta can be obtained by the method (4) 6 The value of (2) is divided into two solutions, namelyDue to theta 1 There are two solutions, see θ 6 There are 4 solutions to the value of (2).
Then multiplying at both ends of equation (2) with the right endThe method can obtain:
according to equation (5), the 1 st row and 1 st column elements and the 3 rd row and 3 rd column elements of the two-end matrix are equal, it is obtained that:
(n x +c 1 c 6 o z )c 7 +(c 1 c 6 n z -o x )s 7 ＝-s 1 s 6 (6)
let n x +c 1 c 6 o z ＝k 2 ，c 1 c 6 n z -o x ＝k 3 The 7 th joint angle theta can be obtained 7 Solution to (1)
Selecting and fixing the joint angle of the 5 th joint and setting theta 5 ＝90°。
Right multiplication is performed simultaneously at both ends of equation (1)The method can obtain:
the coordinates of the point B in the basic coordinate system are made to be% 4 p x ， 4 p y ， 4 p z ) The coordinates of the point B can be obtained asThereby get->Then->Can obtain the 3 rd joint angle theta 3 Solution of->
Let the 1 st row and 3 rd column elements and the 2 nd row and 3 rd column elements of the matrix at both ends of equation (7) be equal, it can be obtained that:
4 p x ＝a 1 c 1 +a 2 c 1 c 2 +a 3 c 1 c 23 (8)
let k 5 ＝a 2 c 1 +a 3 c 1 c 3 ,k 6 ＝-a 3 c 1 s 3 ,k 7 ＝ 4 p x -a 1 c 1 The method can obtain:
k 7 ＝k 5 c 2 +k 6 s 2 (9)
the 2 nd joint angle can be obtained by the formula (9)
Equaling the equation (7) matrix row 1, column 3 elements and row 3, column 3 elements, we can obtain:
s 234 ＝-(c 5 c 6 c 7 -s 5 s 7 )n z +(c 5 c 6 s 7 +s 5 c 7 )o z +(c 5 s 6 )a z (10)
c 234 ＝(s 5 c 6 c 7 +c 5 s 7 )n z -(-s 5 c 6 s 7 +s 5 c 7 )o z -(s 5 s 6 )a z (11)
θ 234 ＝atan2(s 234 ,c 234 )(12)
the 4 th joint angle θ can be obtained by the formula (12) 4 ＝θ 234 -θ 2 -θ 3 。
The above embodiments are only for illustrating the technical solution of the present invention, and are not limiting; although the invention has been described in detail with reference to the foregoing embodiments, it will be understood by those of ordinary skill in the art that: the technical scheme described in the foregoing embodiments can be modified or some technical features thereof can be replaced by equivalents; such modifications and substitutions do not depart from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims (6)

1. A7-degree-of-freedom redundant mechanical arm inverse kinematics solving method is characterized in that the 7-degree-of-freedom redundant mechanical arm has the following structure: 1 st joint coordinate system x 1 z 1 y 1 And a base coordinate system x 0 z 0 y 0 Coincidence, 2 nd joint coordinate system x 2 z 2 y 2 Around x by a first joint coordinate system 1 The axis rotates by 90 DEG and then winds z 1 Rotated 90 deg. and along x 1 Axis movement a1, 3 rd joint coordinate System x 3 z 3 y 3 Z is surrounded by the 2 nd joint coordinate system 2 The axis rotates 90 DEG and along x 2 Axis movement a2, 4 th joint coordinate system x 4 z 4 y 4 From joint 3 along x 3 Axis movement a3, 5 th joint coordinate System x 5 z 5 y 5 From joint 4 along x 4 Axis-shifting a4, rotation of the 6 th joint coordinate system by 90 degrees along the x-axis, rotation by 90 degrees along the z-axis, and rotation along the x-axis from the 5 th joint coordinate system 5 Move a5, along y 5 -d6, rotation of the 7 th joint coordinate system by 90 ° about the x-axis from the 5 th joint coordinate system; all joints rotate by taking the respective z-axis as the rotation center; the method comprises the following steps:
s1, establishing a D-H coordinate system and a kinematic matrix according to the configuration and parameters of the mechanical arm;
s2, solving the joint angles of the mechanical arm which do not form redundancy by adopting an algebraic method;
s3, drawing a connecting rod plan according to the redundant joint of the mechanical arm;
s4, fixing a redundant joint to enable the angle of the redundant joint to be a known value;
s5, solving the kinematic inverse solution of the redundant joint by adopting a method of combining a geometric method and algebraic method correlation;
the solving method of the S1 comprises the following steps:
s11, obtaining a homogeneous coordinate transformation matrix of the terminal coordinate system relative to the base coordinate system according to a second transformation equation between the connecting rod coordinate systemsSatisfy formula (1):
wherein ,representing the secondary coordinate transformation matrix of the adjacent two-link coordinate system; the connecting rod is a connecting piece between two adjacent joints;
fixing the joint angle of the 5 th joint; the solving method of the S5 comprises the following steps:
s51, right multiplying at both ends of equation (1)The method can obtain:
the coordinates of the point B in the basic coordinate system are made to be% 4 p x ， 4 p y ， 4 p z ) The coordinates of the point B can be obtained asThereby get->Then->Can obtain the 3 rd joint angle theta 3 Solution to (1)
S52, according to the equation (7), the elements of the 1 st row and the 3 rd column of the two-end matrix are equal to the elements of the 2 nd row and the 3 rd column, and the following can be obtained:
4 p x ＝a 1 c 1 +a 2 c 1 c 2 +a 3 c 1 c 23 (8)
let k 5 ＝a 2 c 1 +a 3 c 1 c 3 ,k 6 ＝-a 3 c 1 s 3 ,k 7 ＝ 4 p x -a 1 c 1 The method can obtain:
k 7 ＝k 5 c 2 +k 6 s 2 (9)
the 2 nd joint angle can be obtained by the formula (9)
S53, according to the equation (7), the elements of the 1 st row and the 3 rd column of the matrix are equal to the elements of the 3 rd row and the 3 rd column, and the following can be obtained:
s 234 ＝-(c 5 c 6 c 7 -s 5 s 7 )n z +(c 5 c 6 s 7 +s 5 c 7 )o z +(c 5 s 6 )a z (10)
c 234 ＝(s 5 c 6 c 7 +c 5 s 7 )n z -(-s 5 c 6 s 7 +s 5 c 7 )o z -(s 5 s 6 )a z (11)
θ 234 ＝atan2(s 234 ,c 234 )(12)
the 4 th joint angle θ can be obtained by the formula (12) 4 ＝θ 234 -θ 2 -θ 3 。
2. The 7-degree-of-freedom redundant manipulator inverse kinematics solution of claim 1, wherein the solution of S2 comprises the steps of:
s21, firstly, multiplying the two ends of the equation (1) by the left side simultaneouslyObtaining:
the equality of the elements of row 2 and column 4 of the matrix at both ends according to equation (2) can establish the equation:
-s 1 p x +c 1 p y ＝0(3)
the 1 st joint angle θ can be obtained according to equation (3) 1 There are two solutions, respectively: atan2 (p) y ,p x ) And Atan2 (-p) y ,-p x )；
The equality of the elements of row 3 and column 2 of the matrix at both ends according to equation (2) can establish the equation:
c 6 ＝-s 1 a x +c 1 a y ＝k 1 (4)
the 6 th joint angle theta can be obtained by the method (4) 6 The value of (2) is divided into two solutions, namelyDue to theta 1 There are two solutions, see θ 6 There are 4 solutions to the value of (2);
then multiplying at both ends of equation (2) with the right endThe method can obtain:
according to equation (5), the 1 st row and 1 st column elements and the 3 rd row and 3 rd column elements of the two-end matrix are equal, it is obtained that:
(n x +c 1 c 6 o z )c 7 +(c 1 c 6 n z -o x )s 7 ＝-s 1 s 6 (6)
let n x +c 1 c 6 o z ＝k 2 ，c 1 c 6 n z -o x ＝k 3 The 7 th joint angle theta can be obtained 7 Solution to (1)
3. The 7-degree-of-freedom redundant manipulator inverse kinematics solution of claim 2, wherein the solution of S3 comprises the steps of:
s31, obtaining the L according to the connecting rod plan OA ＝a 2 ，l AB ＝a 3 ，l BC ＝a 4 ，D point coordinates are
4. A7-degree-of-freedom redundant mechanical arm inverse kinematics solving system is characterized by comprising:
the D-H coordinate system and kinematics matrix establishment module is used for establishing a D-H coordinate system and a kinematics matrix according to the configuration and parameters of the mechanical arm;
the joint angle solving module is used for solving the joint angle of the mechanical arm which does not form redundancy by adopting algebraic method;
the connecting rod plan drawing module is used for drawing a connecting rod plan according to the redundant joint of the mechanical arm;
the redundant joint fixing module is used for fixing a redundant joint to enable the angle of the redundant joint to be a known value;
the calculation module is used for solving the kinematic inverse solution of the redundant joint by adopting a method of combining a geometric method and algebraic correlation;
the solving method of the D-H coordinate system and kinematics matrix building module comprises the following steps:
s11, obtaining a homogeneous coordinate transformation matrix of the terminal coordinate system relative to the base coordinate system according to a second transformation equation between the connecting rod coordinate systemsSatisfy formula (1):
wherein ,representing the secondary coordinate transformation matrix of the adjacent two-link coordinate system; the connecting rod is a connecting piece between two adjacent joints;
fixing the joint angle of the 5 th joint; the solving method of the computing module comprises the following steps:
s51, right multiplying at both ends of equation (1)The method can obtain:
let the point B be at the base coordinatesThe coordinates in the system are% 4 p x ， 4 p y ， 4 p z ) The coordinates of the point B can be obtained asThereby get->Then->Can obtain the 3 rd joint angle theta 3 Solution to (1)
S52, according to the equation (7), the elements of the 1 st row and the 3 rd column of the two-end matrix are equal to the elements of the 2 nd row and the 3 rd column, and the following can be obtained:
4 p x ＝a 1 c 1 +a 2 c 1 c 2 +a 3 c 1 c 23 (8)
let k 5 ＝a 2 c 1 +a 3 c 1 c 3 ,k 6 ＝-a 3 c 1 s 3 ,k 7 ＝ 4 p x -a 1 c 1 The method can obtain:
k 7 ＝k 5 c 2 +k 6 s 2 (9)
the 2 nd joint angle can be obtained by the formula (9)
S53, according to the equation (7), the elements of the 1 st row and the 3 rd column of the matrix are equal to the elements of the 3 rd row and the 3 rd column, and the following can be obtained:
s 234 ＝-(c 5 c 6 c 7 -s 5 s 7 )n z +(c 5 c 6 s 7 +s 5 c 7 )o z +(c 5 s 6 )a z (10)
c 234 ＝(s 5 c 6 c 7 +c 5 s 7 )n z -(-s 5 c 6 s 7 +s 5 c 7 )o z -(s 5 s 6 )a z (11)
θ 234 ＝atan2(s 234 ,c 234 )(12)
the 4 th joint angle θ can be obtained by the formula (12) 4 ＝θ 234 -θ 2 -θ 3 。
5. The 7-degree-of-freedom redundant manipulator inverse kinematics solution of claim 4, wherein the solution method of the joint angle solution module comprises the steps of:
s21, firstly, multiplying the two ends of the equation (1) by the left side simultaneouslyObtaining:
the equality of the elements of row 2 and column 4 of the matrix at both ends according to equation (2) can establish the equation:
-s 1 p x +c 1 p y ＝0(3)
the 1 st joint angle θ can be obtained according to equation (3) 1 There are two solutions, respectively: atan2 (p) y ,p x ) And Atan2 (-p) y ,-p x )；
The equality of the elements of row 3 and column 2 of the matrix at both ends according to equation (2) can establish the equation:
c 6 ＝-s 1 a x +c 1 a y ＝k 1 (4)
the 6 th joint angle theta can be obtained by the method (4) 6 The value of (2) is divided into two solutions, namelyDue to theta 1 There are two solutions, see θ 6 There are 4 solutions to the value of (2);
then multiplying at both ends of equation (2) with the right endThe method can obtain:
according to equation (5), the 1 st row and 1 st column elements and the 3 rd row and 3 rd column elements of the two-end matrix are equal, it is obtained that:
(n x +c 1 c 6 o z )c 7 +(c 1 c 6 n z -o x )s 7 ＝-s 1 s 6 (6)
let n x +c 1 c 6 o z ＝k 2 ，c 1 c 6 n z -o x ＝k 3 The 7 th joint angle theta can be obtained 7 Solution to (1)
6. The 7-degree-of-freedom redundant manipulator inverse kinematics solution of claim 5, wherein the solution method of the link plan drawing module comprises the steps of:
s31, obtaining the L according to the connecting rod plan OA ＝a 2 ，l AB ＝a 3 ，l BC ＝a 4 ，D point coordinates are
CN202111069374.5A 2021-09-13 2021-09-13 7-degree-of-freedom redundant mechanical arm inverse kinematics solving method and system Active CN113722864B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111069374.5A CN113722864B (en) 2021-09-13 2021-09-13 7-degree-of-freedom redundant mechanical arm inverse kinematics solving method and system

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111069374.5A CN113722864B (en) 2021-09-13 2021-09-13 7-degree-of-freedom redundant mechanical arm inverse kinematics solving method and system

Publications (2)

Publication Number Publication Date
CN113722864A CN113722864A (en) 2021-11-30
CN113722864B true CN113722864B (en) 2023-09-05

Family

ID=78683579

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111069374.5A Active CN113722864B (en) 2021-09-13 2021-09-13 7-degree-of-freedom redundant mechanical arm inverse kinematics solving method and system

Country Status (1)

Country Link
CN (1) CN113722864B (en)

Families Citing this family (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114714335B (en) * 2022-03-09 2023-07-25 泉州华中科技大学智能制造研究院 Inverse solution method and device for seven-joint redundancy degree-of-freedom robot
CN114818256A (en) * 2022-03-11 2022-07-29 上海大学 Inverse solution optimization method and system for coupled motion of water jet machine tool
CN116258015A (en) * 2023-03-14 2023-06-13 中国地质大学（武汉） Rope-driven snake-shaped mechanical arm inverse kinematics macro-micro double-stage hybrid analysis solving method
CN117959146B (en) * 2024-04-02 2024-06-25 江西求是高等研究院 Six-degree-of-freedom upper limb rehabilitation robot control method and system

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2012007014A1 (en) * 2010-07-13 2012-01-19 Abdallah Ezzat Abdallah Abozaied Joint
CN107589934A (en) * 2017-07-24 2018-01-16 大连理工大学 A kind of acquiring method of articulated manipulator inverse kinematics parsing solution
CN111844005A (en) * 2020-07-08 2020-10-30 哈尔滨工业大学 2R-P-2R-P-2R mechanical arm movement planning method applied to tunnel wet spraying
CN111914980A (en) * 2020-05-30 2020-11-10 中南大学 Redundant mechanical arm kinematics resolving method based on analytical solution and improved genetic algorithm
CN112223291A (en) * 2020-10-21 2021-01-15 哈工大机器人（合肥）国际创新研究院 Mechanical arm obstacle avoidance method and device based on three-dimensional task space constraint
CN112536792A (en) * 2020-11-24 2021-03-23 河南理工大学 Robot arm inverse solution method based on spherical geometric analysis method

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2012007014A1 (en) * 2010-07-13 2012-01-19 Abdallah Ezzat Abdallah Abozaied Joint
CN107589934A (en) * 2017-07-24 2018-01-16 大连理工大学 A kind of acquiring method of articulated manipulator inverse kinematics parsing solution
CN111914980A (en) * 2020-05-30 2020-11-10 中南大学 Redundant mechanical arm kinematics resolving method based on analytical solution and improved genetic algorithm
CN111844005A (en) * 2020-07-08 2020-10-30 哈尔滨工业大学 2R-P-2R-P-2R mechanical arm movement planning method applied to tunnel wet spraying
CN112223291A (en) * 2020-10-21 2021-01-15 哈工大机器人（合肥）国际创新研究院 Mechanical arm obstacle avoidance method and device based on three-dimensional task space constraint
CN112536792A (en) * 2020-11-24 2021-03-23 河南理工大学 Robot arm inverse solution method based on spherical geometric analysis method

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
一种冗余自由度机械臂逆运动学解析算法;王鹏程等;《机械科学与技术》;第35卷(第5期);第706-710页 *

Also Published As

Publication number Publication date
CN113722864A (en) 2021-11-30

Similar Documents

Publication Publication Date Title
CN113722864B (en) 7-degree-of-freedom redundant mechanical arm inverse kinematics solving method and system
CN107589934B (en) Solving method for inverse kinematics analytic solution of joint type mechanical arm
Singh et al. Forward and inverse kinematic analysis of robotic manipulators
CN107584474B (en) Spherical joint double-arm robot coordinated movement method based on geometric projection
WO2021184655A1 (en) Method for planning motion along trajectory of end of hyper-redundant mechanical arm
US9321175B2 (en) Robotic manipulator articulation techniques
CN109291046A (en) A kind of seven freedom personification configuration mechanical arm inverse kinematics planing method
CN108908347B (en) Fault-tolerant repetitive motion planning method for redundant mobile mechanical arm
CN107791248B (en) Control method of six-degree-of-freedom series robot based on criterion of not meeting Pieper
Zheng et al. Kinematics analysis and trajectory planning of 6-DOF robot
Zhang et al. Inverse kinematics of modular Cable-driven Snake-like Robots with flexible backbones
CN111791234A (en) Anti-collision control algorithm for working positions of multiple robots in narrow space
Yang et al. General inverse kinematics method for 7-DOF offset manipulators based on arm angle parameterization
CN114589701A (en) Multi-joint mechanical arm obstacle avoidance inverse kinematics method based on damping least squares
CN116330267A (en) Control method based on industrial robot wrist singular point calculation
CN117010115A (en) Kinematic reverse solution method and system for serial-parallel mechanism
CN116551693A (en) Rope-driven super-redundancy snake-shaped mechanical arm kinematics inverse solution method
Andrade et al. A comparative analysis of kinematics of industrial robot KUKA KR 60–3 using scientific computing languages
CN117325143A (en) Redundant mechanical arm singular position type lower kinematics optimization method
Wang et al. Kinematics solution of snake-like manipulator based on improved backbone mode method
Zhang et al. A new analytical inverse kinematics model for seven degrees of freedom redundant manipulators
Sharma et al. Generalized unified closed form inverse kinematics for mobile manipulators with reusable redundancy parameters
JP2019093487A (en) Robot control device and robot reverse conversion processing method
Qizhi et al. On the kinematics analysis and motion planning of the manipulator of a mobile robot
Fang et al. RRT-based motion planning with sampling in redundancy space for robots with anthropomorphic arms

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