CN107756400A - A kind of 6R Robotic inverse kinematics geometry solving methods based on spinor theory - Google Patents

A kind of 6R Robotic inverse kinematics geometry solving methods based on spinor theory Download PDF

Info

Publication number
CN107756400A
CN107756400A CN201710953599.4A CN201710953599A CN107756400A CN 107756400 A CN107756400 A CN 107756400A CN 201710953599 A CN201710953599 A CN 201710953599A CN 107756400 A CN107756400 A CN 107756400A
Authority
CN
China
Prior art keywords
msub
mrow
mtd
mtr
msup
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
Application number
CN201710953599.4A
Other languages
Chinese (zh)
Other versions
CN107756400B (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.)
Beijing University of Technology
Original Assignee
Beijing University of Technology
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 Beijing University of Technology filed Critical Beijing University of Technology
Priority to CN201710953599.4A priority Critical patent/CN107756400B/en
Publication of CN107756400A publication Critical patent/CN107756400A/en
Application granted granted Critical
Publication of CN107756400B publication Critical patent/CN107756400B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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/1605—Simulation of manipulator lay-out, design, modelling of manipulator
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J19/00—Accessories fitted to manipulators, e.g. for monitoring, for viewing; Safety devices combined with or specially adapted for use in connection with manipulators
    • B25J19/007—Means or methods for designing or fabricating manipulators
    • 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

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Mathematical Physics (AREA)
  • Data Mining & Analysis (AREA)
  • Theoretical Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Computational Mathematics (AREA)
  • Mathematical Analysis (AREA)
  • Mathematical Optimization (AREA)
  • Pure & Applied Mathematics (AREA)
  • Operations Research (AREA)
  • Algebra (AREA)
  • Databases & Information Systems (AREA)
  • Software Systems (AREA)
  • General Engineering & Computer Science (AREA)
  • Automation & Control Theory (AREA)
  • Manipulator (AREA)
  • Numerical Control (AREA)
  • External Artificial Organs (AREA)

Abstract

The invention discloses a kind of 6R Robotic inverse kinematics geometry solving methods based on spinor theory, belong to the inverse solution technique study field of robot kinematics.Basis coordinates system and tool coordinates system are established, 6R robot kinematics' parameters are determined by basis coordinates system and tool coordinates system, and establish positive kinematics model.The inverse solution Kinematic Decomposition in first three joint of 6R robots is described and establishes hexa-atomic quadratic equation group.Based on spinor positive kinematics model solution initial position qs1Corresponding target location qe1.This method is combined geometric description with spinor theory, and geometric meaning definitely, is solved for Algebraic Equation set by simplifying inverse arithmetic, effectively improves computational efficiency, can be that robot motion controls a kind of new inverse solution processing method of offer in real time.

Description

A kind of 6R Robotic inverse kinematics geometry solving methods based on spinor theory
Technical field
The invention belongs to the inverse solution technique study field of robot kinematics, more particularly to a kind of 6R machines based on spinor theory Device people's inverse kinematics geometry solving method.
Background technology
Kinematics analysis is to realize the basis of motion control, mainly establishes the mapping model of joint variable and end pose. Wherein against solution problem, i.e. known end pose solution joint variable the problem of, be one of robot field's study hotspot, it is solved Efficiency directly affects the real-time performance of motion planning and robot control.The inverse solution modeling of robot kinematics at present is based primarily upon D-H methods And spinor theory, researchers compared for two methods and find that the application of the latter has advantages below：It can avoid establishing part Coordinate system, simplified mathematical model simultaneously overcome singularity caused by local parameter；Its geometric meaning is clear and definite, can easily determine to produce The condition and number of raw more solutions.The Paden-Kahan subproblems for being currently based on spinor description are Converse solved by extensive robot In problem, but this method is not suitable for the six-DOF robot of arbitrary configuration, therefore most scholars are asked based on three kinds of basic sons Topic extend and has proposed some new subproblem models, as Tan subproblem two improve establish for " around The mathematical modeling of the rotary motion of two non-intersect axles " subproblem；Chen describe it is a kind of " around the motion of three axis rotations, Two of which diameter parallel and with the 3rd axis antarafacial " subproblem, carried out detailed spinor against solution for it and described.It is based on In six degree of freedom serial manipulator against being widely applied in solution problem, research shows to work as 6R robots these subproblem models Meet that there is its Inverse Kinematics Problem closing to solve during Piper criterions.Sariyildi etc., which is based on three subproblems, realizes 6R machines The inverse kinematics of people；Lv Shizeng etc. reduces dependence to subproblem by introducing Wu Ritt's method in spinor method, and will The inverse solution problem of 6R robots is converted into the Solve problems of hexa-atomic eight power journey group.In order to further simplify 6R robot inverse solution moulds The inverse solution problem of 6R robots is converted into a hexa-atomic quadratic equation group and one three by type, this method by Describing Method The solution of first quadratic equation group, so that its geometric meaning becomes apparent from, solution procedure is simpler.
The content of the invention
The purpose of the present invention aims to provide a kind of 6R Robotic inverse kinematics geometry solving methods based on spinor theory.Should Method is mainly characterized by by combining Describing Method and spinor theory, is proposed for 6R robot kinematics against solution problem A kind of geometric meaning is clear, the simple inverse kinematics model of solution procedure.
To achieve the above object, the technological means that the present invention uses is a kind of 6R robot inverses motion based on spinor theory Geometry solving method is learned, the implementation process of this method is as follows：
S1, basis coordinates system and tool coordinates system are established, 6R robot motions are determined by basis coordinates system and tool coordinates system Parameter is learned, and establishes positive kinematics model.
S2, such as Fig. 1, the inverse solution Kinematic Decomposition in first three joint of 6R robots is described：By point qeTo point c2=[x2 y2 z2] rotary motion around joint 1；Point c2To point c1=[x1 y1 z1] rotary motion around joint 2；Point c1To point qsAround joint 3 Rotary motion.Wherein qs, qeThe respectively initial and target location of robot end, the geometrical relationship description according to Fig. 1 are built Stand with x1,y1,z1,x2,y2And z2For the hexa-atomic quadratic equation group of variable.
S3, using in MATLAB " SOLVE " function solve above equation group, obtain c1And c2Coordinate vector.
S4, since then, the inverse solution in first three joint move whole story position, it is known that being built respectively based on Paden-Kahan subproblems 1 First three joint angle variable θ of vertical 6R robots1, θ2And θ3Explicit solution model, and solved.
S5, some initial position q as after in the inverse solution motion in three joints is taken on the axis in joint 6s1, and based on rotation Measure positive kinematics model solution initial position qs1Corresponding target location qe1。
S6, such as Fig. 2, the inverse solution Kinematic Decomposition in joint 4 and 5 is described：By point qe1To point c3=[x3y3z3] rotation fortune It is dynamic；By point c3To point qs1Rotary motion.Shown according to Fig. 2, established according to geometrical relationship with x3,y3And z3For the ternary two of variable Equation of n th order n group, and solved using " SOLVE " function, obtain c3Coordinate vector.
S7, since then, the inverse solution in joint 4 and joint 5 move whole story position, it is known that distinguishing based on Paden-Kahan subproblems 1 Establish the joint angle variable θ in joint 4 and joint 54And θ5Explicit solution model, and solved.
S8, take any point q not on the axis of joint 6s2, based on spinor positive kinematics model solution qs2Corresponding mesh Cursor position qe2, then known to the inverse solution motion whole story position in joint 6.θ is similarly solved based on subproblem 16。
The method have the characteristics that the reverse kinematics formula of 6R robots is converted into one based on Describing Method The solution of hexa-atomic quadratic equation group and a ternary quadratic equation group, inverse solution model is simplified, and geometric meaning is definitely, is The real time kinematics control of 6R robots provides certain method support.This method is combined geometric description with spinor theory, geometry Meaning definitely, is solved for Algebraic Equation set by simplifying inverse arithmetic, effectively improves computational efficiency, can be robot Motion control in real time provides a kind of new inverse solution processing method.
Brief description of the drawings
The inverse solution kinematic geometry description in Fig. 1 joints 1,2 and 3；
The inverse solution kinematic geometry description in Fig. 2 joints 4 and 5；
Fig. 3 6R robots parameter coordinate system.
Embodiment
Below in conjunction with accompanying drawing 1-3, the present invention is described in detail.
S1 determines 6R robot kinematics parameter and establishes positive kinematics model
As shown in Figure 3, it is known that each joint position vector of original state and rotating vector of the 6R robots are as follows：
Wherein ri, 1≤i≤6 represent i joints in the position vector of basis coordinates system, ωiRepresent the rotating vector in i joints.
Based on spinor theory, the 6R robots positive kinematics model is expressed as,
Wherein gst(θ),gst(0) the initial pose and object pose of robot end is represented respectively,Represent that i is closed The exponent product form of rotary motion is saved,
θ in formulaiFor i-th of joint angular displacement；It is i joints rotating vector ωiAnother representation, by ωi= [ω1 ω2 ω3] be defined asThenνi It is the linear velocity of i joint motions, νi=-ωi×ri。
Given object pose,
S2 establishes hexa-atomic quadratic equation group for first three joint
Rotary motion based on first three joint of spinor theory is described as,
Q ' in formulasWith q 'eInitial position and target location of the 6R robot ends in the rotary motion are represented respectively.By S1 Understand,
q′s=[xs ys zs1]=[0 744 940 1]
q′e=[xe ye ze1]=[- 936.6611 631.7859 570.0752 1]
If move across point c1And c2, the geometric description according to Fig. 1 establishes relationship below group,
Wherein q1, q2And q3Any point respectively on joint 1, joint 2 and the rotation axis of joint 3, takes for simplified model q1=[0 0 0], q2=[0 150 250] and q3=[0 150 800], then formula (7) is expressed as equation group,
Equation group (8) is solved using " SOLVE " function in MATLAB, obtains x1,y1,z1,x2,y2And z2Solution.
S3 calculates joint angular displacement1, θ2And θ3
Obtain process point coordinates c1=[x1 y1 z1] and c2=[x2 y2 z2] after, by around joint 1, joint 2 and joint 3 Rotary motion is described as follows respectively,
Based on Paden-Kahan subproblems 1, it is as follows to obtain the display expression formula of joint angular displacement,
Step (4) establishes ternary quadratic equation group for joint 4 and joint 5
Known by formula (3),
WhereinOne is taken on the axis in joint 6 Point qs1=[xs1 ys1 zs1]=[0 744 0], its rotary motion around joint 4 and joint 5 is described as follows,
Wherein q 'e1=g1q′s1=[xe1 ye1 ze1 1]。
If it is c to move across point coordinates3=[x3 y3 z3], the geometric description according to Fig. 2 establishes relationship below group,
Wherein q4=[0 744 940].Then formula (14) is expressed as equation,
Equation group (15) is solved using " SOLVE " function in MATLAB, obtains x3,y3And z3Solution.
Step (5) calculates joint angular displacement4And θ5
Obtain process point coordinates c3=[x3 y3 z3] after, it will be described as follows respectively around the rotary motion in joint 4 and joint 5,
Based on Paden-Kahan subproblems 1, joint angular displacement is obtained4And θ5Display expression formula it is as follows,
Step (6) calculates joint angular displacement6
Take a not point q on the axis of joint 6s2=[0 750 940], its rotary motion around joint 6 are described as,
WhereinSimilarly it is based on Paden- Kahan subproblems 1, obtain joint angular displacement6Display expression formula it is as follows,
It is as shown in table 1 to obtain eight groups of Inverse Kinematics Solutions for solution more than.
1 eight groups of Inverse Kinematics Solutions of table

Claims (2)

  1. A kind of 1. 6R Robotic inverse kinematics geometry solving methods based on spinor theory, it is characterised in that：The realization of this method Process is as follows：
    S1, basis coordinates system and tool coordinates system are established, determine that 6R robot kinematics join by basis coordinates system and tool coordinates system Number, and establish positive kinematics model；
    S2, the inverse solution Kinematic Decomposition description by first three joint of 6R robots：By point qeTo point c2=[x2 y2 z2] around joint 1 Rotary motion；Point c2To point c1=[x1 y1 z1] rotary motion around joint 2；Point c1To point qsRotary motion around joint 3； Wherein qs, qeThe respectively initial and target location of robot end, established according to geometrical relationship description with x1,y1,z1,x2,y2 And z2For the hexa-atomic quadratic equation group of variable；
    S3, using in MATLAB " SOLVE " function solve above equation group, obtain c1And c2Coordinate vector；
    S4, since then, the inverse solution in first three joint move whole story position, it is known that establishing 6R respectively based on Paden-Kahan subproblems 1 First three joint angle variable θ of robot1, θ2And θ3Explicit solution model, and solved；
    S5, some initial position q as after in the inverse solution motion in three joints is taken on the axis in joint 6s1, and based on spinor just Kinematics model solves initial position qs1Corresponding target location qe1；
    S6, the inverse solution Kinematic Decomposition description by joint 4 and 5：By point qe1To point c3=[x3 y3 z3] rotary motion；By point c3Arrive Point qs1Rotary motion；Established according to geometrical relationship with x3,y3And z3For the ternary quadratic equation group of variable, and use " SOLVE " Function solves, and obtains c3Coordinate vector；
    S7, since then, the inverse solution in joint 4 and joint 5 move whole story position, it is known that being established respectively based on Paden-Kahan subproblems 1 Joint 4 and the joint angle variable θ in joint 54And θ5Explicit solution model, and solved；
    S8, take any point q not on the axis of joint 6s2, based on spinor positive kinematics model solution qs2Corresponding target location qe2, then known to the inverse solution motion whole story position in joint 6；θ is similarly solved based on subproblem 16。
  2. 2. a kind of 6R Robotic inverse kinematics geometry solving methods based on spinor theory according to claim 1, it is special Sign is：S1 determines 6R robot kinematics parameter and establishes positive kinematics model
    Each joint position vector of original state and rotating vector of the known 6R robots are as follows：
    <mrow> <mtable> <mtr> <mtd> <mtable> <mtr> <mtd> <mrow> <msub> <mi>r</mi> <mn>1</mn> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> </mtable> </mfenced> </mrow> </mtd> <mtd> <mrow> <msub> <mi>r</mi> <mn>2</mn> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>150</mn> </mtd> <mtd> <mn>250</mn> </mtd> </mtr> </mtable> </mfenced> </mrow> </mtd> <mtd> <mrow> <msub> <mi>r</mi> <mn>3</mn> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>150</mn> </mtd> <mtd> <mn>800</mn> </mtd> </mtr> </mtable> </mfenced> </mrow> </mtd> </mtr> </mtable> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>r</mi> <mn>4</mn> </msub> <mo>=</mo> <msub> <mi>r</mi> <mn>5</mn> </msub> <mo>=</mo> <msub> <mi>r</mi> <mn>6</mn> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>744</mn> </mtd> <mtd> <mn>940</mn> </mtd> </mtr> </mtable> </mfenced> </mrow> </mtd> </mtr> </mtable> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>1</mn> <mo>)</mo> </mrow> </mrow>
    <mrow> <mtable> <mtr> <mtd> <mrow> <msub> <mi>&amp;omega;</mi> <mn>1</mn> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>1</mn> </mtd> </mtr> </mtable> </mfenced> </mrow> </mtd> <mtd> <mrow> <msub> <mi>&amp;omega;</mi> <mn>2</mn> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mn>1</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> </mtable> </mfenced> </mrow> </mtd> <mtd> <mrow> <msub> <mi>&amp;omega;</mi> <mn>3</mn> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mn>1</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> </mtable> </mfenced> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>&amp;omega;</mi> <mn>4</mn> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>1</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> </mtable> </mfenced> </mrow> </mtd> <mtd> <mrow> <msub> <mi>&amp;omega;</mi> <mn>5</mn> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mn>1</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> </mtable> </mfenced> </mrow> </mtd> <mtd> <mrow> <msub> <mi>&amp;omega;</mi> <mn>6</mn> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>1</mn> </mtd> </mtr> </mtable> </mfenced> </mrow> </mtd> </mtr> </mtable> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>2</mn> <mo>)</mo> </mrow> </mrow>
    Wherein ri, 1≤i≤6 represent i joints in the position vector of basis coordinates system, ωiRepresent the rotating vector in i joints；
    Based on spinor theory, the 6R robots positive kinematics model is expressed as,
    <mrow> <msub> <mi>g</mi> <mrow> <mi>s</mi> <mi>t</mi> </mrow> </msub> <mrow> <mo>(</mo> <mi>&amp;theta;</mi> <mo>)</mo> </mrow> <mo>=</mo> <mi>exp</mi> <mrow> <mo>(</mo> <msub> <mover> <mi>&amp;xi;</mi> <mo>^</mo> </mover> <mn>1</mn> </msub> <msub> <mi>&amp;theta;</mi> <mn>1</mn> </msub> <mo>)</mo> </mrow> <mi>exp</mi> <mrow> <mo>(</mo> <msub> <mover> <mi>&amp;xi;</mi> <mo>^</mo> </mover> <mn>2</mn> </msub> <msub> <mi>&amp;theta;</mi> <mn>2</mn> </msub> <mo>)</mo> </mrow> <mo>...</mo> <mi>exp</mi> <mrow> <mo>(</mo> <msub> <mover> <mi>&amp;xi;</mi> <mo>^</mo> </mover> <mn>6</mn> </msub> <msub> <mi>&amp;theta;</mi> <mn>6</mn> </msub> <mo>)</mo> </mrow> <msub> <mi>g</mi> <mrow> <mi>s</mi> <mi>t</mi> </mrow> </msub> <mrow> <mo>(</mo> <mn>0</mn> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>3</mn> <mo>)</mo> </mrow> </mrow>
    Wherein gst(θ),gst(0) the initial pose and object pose of robot end is represented respectively,Represent the rotation of i joints The dynamic exponent product form of transhipment,
    <mrow> <mi>exp</mi> <mrow> <mo>(</mo> <mrow> <msub> <mover> <mi>&amp;xi;</mi> <mo>^</mo> </mover> <mi>i</mi> </msub> <msub> <mi>&amp;theta;</mi> <mi>i</mi> </msub> </mrow> <mo>)</mo> </mrow> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <mi>exp</mi> <mrow> <mo>(</mo> <mrow> <msub> <mover> <mi>&amp;omega;</mi> <mo>^</mo> </mover> <mi>i</mi> </msub> <msub> <mi>&amp;theta;</mi> <mi>i</mi> </msub> </mrow> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <mrow> <mo>(</mo> <mrow> <mi>I</mi> <mo>-</mo> <mi>exp</mi> <mrow> <mo>(</mo> <mrow> <msub> <mover> <mi>&amp;omega;</mi> <mo>^</mo> </mover> <mi>i</mi> </msub> <msub> <mi>&amp;theta;</mi> <mi>i</mi> </msub> </mrow> <mo>)</mo> </mrow> </mrow> <mo>)</mo> </mrow> <mrow> <mo>(</mo> <mrow> <msub> <mi>&amp;omega;</mi> <mi>i</mi> </msub> <mo>&amp;times;</mo> <msub> <mi>v</mi> <mi>i</mi> </msub> </mrow> <mo>)</mo> </mrow> <mo>+</mo> <msub> <mi>&amp;theta;</mi> <mi>i</mi> </msub> <msub> <mi>&amp;omega;</mi> <mi>i</mi> </msub> <msup> <msub> <mi>&amp;omega;</mi> <mi>i</mi> </msub> <mi>T</mi> </msup> <msub> <mi>v</mi> <mi>i</mi> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>1</mn> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>4</mn> <mo>)</mo> </mrow> </mrow>
    θ in formulaiFor i-th of joint angular displacement；It is i joints rotating vector ωiAnother representation, by ωi=[ω1 ω2 ω3] be defined asThenνiIt is that i is closed Save the linear velocity of motion, νi=-ωi×ri；
    Given object pose,
    <mrow> <msub> <mi>g</mi> <mrow> <mi>s</mi> <mi>t</mi> </mrow> </msub> <mrow> <mo>(</mo> <mi>&amp;theta;</mi> <mo>)</mo> </mrow> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mn>0.5234</mn> </mtd> <mtd> <mn>0.4033</mn> </mtd> <mtd> <mrow> <mo>-</mo> <mn>0.7506</mn> </mrow> </mtd> <mtd> <mrow> <mo>-</mo> <mn>936.6611</mn> </mrow> </mtd> </mtr> <mtr> <mtd> <mn>0.5979</mn> </mtd> <mtd> <mn>0.4537</mn> </mtd> <mtd> <mn>0.6608</mn> </mtd> <mtd> <mn>631.7859</mn> </mtd> </mtr> <mtr> <mtd> <mn>0.6071</mn> </mtd> <mtd> <mrow> <mo>-</mo> <mn>0.7946</mn> </mrow> </mtd> <mtd> <mrow> <mo>-</mo> <mn>0.0036</mn> </mrow> </mtd> <mtd> <mn>570.0752</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>1</mn> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>5</mn> <mo>)</mo> </mrow> </mrow>
    S2 establishes hexa-atomic quadratic equation group for first three joint
    Rotary motion based on first three joint of spinor theory is described as,
    <mrow> <mi>exp</mi> <mrow> <mo>(</mo> <msub> <mover> <mi>&amp;xi;</mi> <mo>^</mo> </mover> <mn>1</mn> </msub> <msub> <mi>&amp;theta;</mi> <mn>1</mn> </msub> <mo>)</mo> </mrow> <mi>exp</mi> <mrow> <mo>(</mo> <msub> <mover> <mi>&amp;xi;</mi> <mo>^</mo> </mover> <mn>2</mn> </msub> <msub> <mi>&amp;theta;</mi> <mn>2</mn> </msub> <mo>)</mo> </mrow> <mi>exp</mi> <mrow> <mo>(</mo> <msub> <mover> <mi>&amp;xi;</mi> <mo>^</mo> </mover> <mn>3</mn> </msub> <msub> <mi>&amp;theta;</mi> <mn>3</mn> </msub> <mo>)</mo> </mrow> <msubsup> <mi>q</mi> <mi>s</mi> <mo>&amp;prime;</mo> </msubsup> <mo>=</mo> <msubsup> <mi>q</mi> <mi>e</mi> <mo>&amp;prime;</mo> </msubsup> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>6</mn> <mo>)</mo> </mrow> </mrow>
    Q ' in formulasWith q 'eInitial position and target location of the 6R robot ends in the rotary motion are represented respectively；From S1,
    q′s=[xs ys zs1]=[0 744 940 1]
    q′e=[xe ye ze1]=[- 936.6611 631.7859 570.0752 1]
    If move across point c1And c2, relationship below group is established according to geometric description,
    <mrow> <mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <mo>|</mo> <mo>|</mo> <msub> <mi>q</mi> <mn>1</mn> </msub> <mo>-</mo> <msub> <mi>q</mi> <mi>e</mi> </msub> <mo>|</mo> <mo>|</mo> <mo>=</mo> <mo>|</mo> <mo>|</mo> <msub> <mi>q</mi> <mn>1</mn> </msub> <mo>-</mo> <msub> <mi>c</mi> <mn>2</mn> </msub> <mo>|</mo> <mo>|</mo> <mo>,</mo> </mrow> </mtd> <mtd> <mrow> <mo>(</mo> <msub> <mi>q</mi> <mi>e</mi> </msub> <mo>-</mo> <msub> <mi>c</mi> <mn>2</mn> </msub> <mo>)</mo> <msup> <msub> <mi>&amp;omega;</mi> <mn>1</mn> </msub> <mi>T</mi> </msup> <mo>=</mo> <mn>0</mn> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>|</mo> <mo>|</mo> <msub> <mi>q</mi> <mn>2</mn> </msub> <mo>-</mo> <msub> <mi>c</mi> <mn>2</mn> </msub> <mo>|</mo> <mo>|</mo> <mo>=</mo> <mo>|</mo> <mo>|</mo> <msub> <mi>q</mi> <mn>2</mn> </msub> <mo>-</mo> <msub> <mi>c</mi> <mn>1</mn> </msub> <mo>|</mo> <mo>|</mo> <mo>,</mo> </mrow> </mtd> <mtd> <mrow> <mo>(</mo> <msub> <mi>c</mi> <mn>2</mn> </msub> <mo>-</mo> <msub> <mi>c</mi> <mn>1</mn> </msub> <mo>)</mo> <msup> <msub> <mi>&amp;omega;</mi> <mn>2</mn> </msub> <mi>T</mi> </msup> <mo>=</mo> <mn>0</mn> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>|</mo> <mo>|</mo> <msub> <mi>q</mi> <mn>3</mn> </msub> <mo>-</mo> <msub> <mi>c</mi> <mn>1</mn> </msub> <mo>|</mo> <mo>|</mo> <mo>=</mo> <mo>|</mo> <mo>|</mo> <msub> <mi>q</mi> <mn>3</mn> </msub> <mo>-</mo> <msub> <mi>q</mi> <mi>s</mi> </msub> <mo>|</mo> <mo>|</mo> <mo>,</mo> </mrow> </mtd> <mtd> <mrow> <mo>(</mo> <msub> <mi>c</mi> <mn>1</mn> </msub> <mo>-</mo> <msub> <mi>q</mi> <mi>s</mi> </msub> <mo>)</mo> <msup> <msub> <mi>&amp;omega;</mi> <mn>3</mn> </msub> <mi>T</mi> </msup> <mo>=</mo> <mn>0</mn> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>7</mn> <mo>)</mo> </mrow> </mrow>
    Wherein q1, q2And q3Any point respectively on joint 1, joint 2 and the rotation axis of joint 3, q is taken for simplified model1= [0 0 0], q2=[0 150 250] and q3=[0 150 800], then formula (7) is expressed as equation group,
    <mrow> <mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <msup> <msub> <mi>x</mi> <mn>2</mn> </msub> <mn>2</mn> </msup> <mo>+</mo> <msup> <msub> <mi>y</mi> <mn>2</mn> </msub> <mn>2</mn> </msup> <mo>=</mo> <msup> <msub> <mi>x</mi> <mi>e</mi> </msub> <mn>2</mn> </msup> <mo>+</mo> <msup> <msub> <mi>y</mi> <mi>e</mi> </msub> <mn>2</mn> </msup> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msup> <msub> <mi>y</mi> <mn>1</mn> </msub> <mn>2</mn> </msup> <mo>-</mo> <mn>300</mn> <msub> <mi>y</mi> <mn>1</mn> </msub> <mo>+</mo> <msup> <msub> <mi>z</mi> <mn>1</mn> </msub> <mn>2</mn> </msup> <mo>-</mo> <mn>500</mn> <msub> <mi>z</mi> <mn>1</mn> </msub> <mo>=</mo> <msup> <msub> <mi>y</mi> <mn>2</mn> </msub> <mn>3</mn> </msup> <mo>-</mo> <mn>300</mn> <msub> <mi>y</mi> <mn>2</mn> </msub> <mo>+</mo> <msup> <msub> <mi>z</mi> <mn>2</mn> </msub> <mn>2</mn> </msup> <mo>-</mo> <mn>500</mn> <msub> <mi>z</mi> <mn>2</mn> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msup> <msub> <mi>y</mi> <mn>1</mn> </msub> <mn>2</mn> </msup> <mo>-</mo> <mn>300</mn> <msub> <mi>y</mi> <mn>1</mn> </msub> <mo>+</mo> <msup> <msub> <mi>z</mi> <mn>1</mn> </msub> <mn>2</mn> </msup> <mo>-</mo> <mn>1600</mn> <msub> <mi>z</mi> <mn>1</mn> </msub> <mo>=</mo> <msup> <msub> <mi>y</mi> <mi>s</mi> </msub> <mn>2</mn> </msup> <mo>-</mo> <mn>300</mn> <msub> <mi>y</mi> <mi>s</mi> </msub> <mo>+</mo> <msup> <msub> <mi>z</mi> <mi>s</mi> </msub> <mn>2</mn> </msup> <mo>-</mo> <mn>1600</mn> <msub> <mi>z</mi> <mi>s</mi> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>z</mi> <mn>2</mn> </msub> <mo>=</mo> <msub> <mi>z</mi> <mi>e</mi> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>x</mi> <mn>1</mn> </msub> <mo>=</mo> <mn>0</mn> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>x</mi> <mn>2</mn> </msub> <mo>=</mo> <mn>0</mn> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>8</mn> <mo>)</mo> </mrow> </mrow>
    Equation group (8) is solved using " SOLVE " function in MATLAB, obtains x1,y1,z1,x2,y2And z2Solution；
    S3 calculates joint angular displacement1, θ2And θ3
    Obtain process point coordinates c1=[x1 y1 z1] and c2=[x2 y2 z2] after, by around the rotation in joint 1, joint 2 and joint 3 Motion is described as follows respectively,
    <mrow> <mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <mi>exp</mi> <mrow> <mo>(</mo> <msub> <mover> <mi>&amp;xi;</mi> <mo>^</mo> </mover> <mn>1</mn> </msub> <msub> <mi>&amp;theta;</mi> <mn>1</mn> </msub> <mo>)</mo> </mrow> <msubsup> <mi>c</mi> <mn>2</mn> <mo>&amp;prime;</mo> </msubsup> <mo>=</mo> <msubsup> <mi>q</mi> <mi>e</mi> <mo>&amp;prime;</mo> </msubsup> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>exp</mi> <mrow> <mo>(</mo> <msub> <mover> <mi>&amp;xi;</mi> <mo>^</mo> </mover> <mn>2</mn> </msub> <msub> <mi>&amp;theta;</mi> <mn>2</mn> </msub> <mo>)</mo> </mrow> <msubsup> <mi>c</mi> <mn>1</mn> <mo>&amp;prime;</mo> </msubsup> <mo>=</mo> <msubsup> <mi>c</mi> <mn>2</mn> <mo>&amp;prime;</mo> </msubsup> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>exp</mi> <mrow> <mo>(</mo> <msub> <mover> <mi>&amp;xi;</mi> <mo>^</mo> </mover> <mn>3</mn> </msub> <msub> <mi>&amp;theta;</mi> <mn>3</mn> </msub> <mo>)</mo> </mrow> <msubsup> <mi>q</mi> <mi>s</mi> <mo>&amp;prime;</mo> </msubsup> <mo>=</mo> <msubsup> <mi>c</mi> <mn>1</mn> <mo>&amp;prime;</mo> </msubsup> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>9</mn> <mo>)</mo> </mrow> </mrow>
    Based on Paden-Kahan subproblems 1, it is as follows to obtain the display expression formula of joint angular displacement,
    <mrow> <msub> <mi>&amp;theta;</mi> <mn>1</mn> </msub> <mo>=</mo> <mi>a</mi> <mi> </mi> <mi>t</mi> <mi>a</mi> <mi>n</mi> <mrow> <mo>(</mo> <mfrac> <mrow> <msub> <mi>x</mi> <mi>e</mi> </msub> <msub> <mi>y</mi> <mn>2</mn> </msub> <mo>-</mo> <msub> <mi>y</mi> <mi>e</mi> </msub> <msub> <mi>x</mi> <mn>2</mn> </msub> </mrow> <mrow> <msub> <mi>x</mi> <mn>2</mn> </msub> <msub> <mi>x</mi> <mi>e</mi> </msub> <mo>+</mo> <msub> <mi>y</mi> <mn>2</mn> </msub> <msub> <mi>y</mi> <mi>e</mi> </msub> </mrow> </mfrac> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>10</mn> <mo>)</mo> </mrow> </mrow>
    <mrow> <msub> <mi>&amp;theta;</mi> <mn>2</mn> </msub> <mo>=</mo> <mi>a</mi> <mi> </mi> <mi>t</mi> <mi>a</mi> <mi>n</mi> <mrow> <mo>(</mo> <mfrac> <mrow> <mo>(</mo> <msub> <mi>y</mi> <mn>1</mn> </msub> <mo>-</mo> <mn>150</mn> <mo>)</mo> <mo>(</mo> <msub> <mi>z</mi> <mn>2</mn> </msub> <mo>-</mo> <mn>250</mn> <mo>)</mo> <mo>-</mo> <mo>(</mo> <msub> <mi>y</mi> <mn>2</mn> </msub> <mo>-</mo> <mn>150</mn> <mo>)</mo> <mo>(</mo> <msub> <mi>z</mi> <mn>1</mn> </msub> <mo>-</mo> <mn>250</mn> <mo>)</mo> </mrow> <mrow> <mo>(</mo> <msub> <mi>y</mi> <mn>1</mn> </msub> <mo>-</mo> <mn>150</mn> <mo>)</mo> <mo>(</mo> <msub> <mi>y</mi> <mn>2</mn> </msub> <mo>-</mo> <mn>150</mn> <mo>)</mo> <mo>+</mo> <mo>(</mo> <msub> <mi>z</mi> <mn>1</mn> </msub> <mo>-</mo> <mn>250</mn> <mo>)</mo> <mo>(</mo> <msub> <mi>z</mi> <mn>2</mn> </msub> <mo>-</mo> <mn>250</mn> <mo>)</mo> </mrow> </mfrac> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>11</mn> <mo>)</mo> </mrow> </mrow>
    <mrow> <msub> <mi>&amp;theta;</mi> <mn>3</mn> </msub> <mo>=</mo> <mi>a</mi> <mi> </mi> <mi>t</mi> <mi>a</mi> <mi>n</mi> <mrow> <mo>(</mo> <mfrac> <mrow> <mo>(</mo> <msub> <mi>y</mi> <mi>s</mi> </msub> <mo>-</mo> <mn>150</mn> <mo>)</mo> <mo>(</mo> <msub> <mi>z</mi> <mn>1</mn> </msub> <mo>-</mo> <mn>800</mn> <mo>)</mo> <mo>-</mo> <mo>(</mo> <msub> <mi>y</mi> <mn>1</mn> </msub> <mo>-</mo> <mn>150</mn> <mo>)</mo> <mo>(</mo> <msub> <mi>z</mi> <mi>s</mi> </msub> <mo>-</mo> <mn>800</mn> <mo>)</mo> </mrow> <mrow> <mo>(</mo> <msub> <mi>y</mi> <mi>s</mi> </msub> <mo>-</mo> <mn>150</mn> <mo>)</mo> <mo>(</mo> <msub> <mi>y</mi> <mn>1</mn> </msub> <mo>-</mo> <mn>150</mn> <mo>)</mo> <mo>+</mo> <mo>(</mo> <msub> <mi>z</mi> <mi>s</mi> </msub> <mo>-</mo> <mn>800</mn> <mo>)</mo> <mo>(</mo> <msub> <mi>z</mi> <mn>1</mn> </msub> <mo>-</mo> <mn>800</mn> <mo>)</mo> </mrow> </mfrac> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>12</mn> <mo>)</mo> </mrow> </mrow>
    Step (4) establishes ternary quadratic equation group for joint 4 and joint 5
    Known by formula (3),
    <mrow> <mi>exp</mi> <mrow> <mo>(</mo> <msub> <mover> <mi>&amp;xi;</mi> <mo>^</mo> </mover> <mn>4</mn> </msub> <msub> <mi>&amp;theta;</mi> <mn>4</mn> </msub> <mo>)</mo> </mrow> <mi>exp</mi> <mrow> <mo>(</mo> <msub> <mover> <mi>&amp;xi;</mi> <mo>^</mo> </mover> <mn>5</mn> </msub> <msub> <mi>&amp;theta;</mi> <mn>5</mn> </msub> <mo>)</mo> </mrow> <mi>exp</mi> <mrow> <mo>(</mo> <msub> <mover> <mi>&amp;xi;</mi> <mo>^</mo> </mover> <mn>6</mn> </msub> <msub> <mi>&amp;theta;</mi> <mn>6</mn> </msub> <mo>)</mo> </mrow> <mo>=</mo> <msub> <mi>g</mi> <mn>1</mn> </msub> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>12</mn> <mo>)</mo> </mrow> </mrow>
    WhereinA point q is taken on the axis in joint 6s1 =[xs1 ys1 zs1]=[0 744 0], its rotary motion around joint 4 and joint 5 is described as follows,
    <mrow> <mi>exp</mi> <mrow> <mo>(</mo> <msub> <mover> <mi>&amp;xi;</mi> <mo>^</mo> </mover> <mn>4</mn> </msub> <msub> <mi>&amp;theta;</mi> <mn>4</mn> </msub> <mo>)</mo> </mrow> <mi>exp</mi> <mrow> <mo>(</mo> <msub> <mover> <mi>&amp;xi;</mi> <mo>^</mo> </mover> <mn>5</mn> </msub> <msub> <mi>&amp;theta;</mi> <mn>5</mn> </msub> <mo>)</mo> </mrow> <msubsup> <mi>q</mi> <mrow> <mi>s</mi> <mn>1</mn> </mrow> <mo>&amp;prime;</mo> </msubsup> <mo>=</mo> <msubsup> <mi>q</mi> <mrow> <mi>e</mi> <mn>1</mn> </mrow> <mo>&amp;prime;</mo> </msubsup> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>13</mn> <mo>)</mo> </mrow> </mrow>
    Wherein
    If it is c to move across point coordinates3=[x3 y3 z3], relationship below group is established according to geometric description,
    <mrow> <mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <mo>|</mo> <mo>|</mo> <msub> <mi>c</mi> <mn>3</mn> </msub> <mo>-</mo> <msub> <mi>q</mi> <mn>4</mn> </msub> <mo>|</mo> <mo>|</mo> <mo>=</mo> <mo>|</mo> <mo>|</mo> <msub> <mi>q</mi> <mrow> <mi>e</mi> <mn>1</mn> </mrow> </msub> <mo>-</mo> <msub> <mi>q</mi> <mn>4</mn> </msub> <mo>|</mo> <mo>|</mo> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>(</mo> <msub> <mi>c</mi> <mn>3</mn> </msub> <mo>-</mo> <msub> <mi>q</mi> <mrow> <mi>e</mi> <mn>1</mn> </mrow> </msub> <mo>)</mo> <msup> <msub> <mi>&amp;omega;</mi> <mn>4</mn> </msub> <mi>T</mi> </msup> <mo>=</mo> <mn>0</mn> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>(</mo> <msub> <mi>c</mi> <mn>3</mn> </msub> <mo>-</mo> <msub> <mi>q</mi> <mrow> <mi>s</mi> <mn>1</mn> </mrow> </msub> <mo>)</mo> <msup> <msub> <mi>&amp;omega;</mi> <mn>5</mn> </msub> <mi>T</mi> </msup> <mo>=</mo> <mn>0</mn> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>14</mn> <mo>)</mo> </mrow> </mrow>
    Wherein q4=[0 744 940]；Then formula (14) is expressed as equation,
    <mrow> <mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <msup> <msub> <mi>x</mi> <mn>3</mn> </msub> <mn>2</mn> </msup> <mo>+</mo> <msup> <msub> <mi>z</mi> <mn>3</mn> </msub> <mn>2</mn> </msup> <mo>-</mo> <mn>1880</mn> <msub> <mi>z</mi> <mn>3</mn> </msub> <mo>=</mo> <msup> <msub> <mi>x</mi> <mrow> <mi>e</mi> <mn>1</mn> </mrow> </msub> <mn>2</mn> </msup> <mo>+</mo> <msup> <msub> <mi>z</mi> <mrow> <mi>e</mi> <mn>1</mn> </mrow> </msub> <mn>2</mn> </msup> <mo>-</mo> <mn>1880</mn> <msub> <mi>z</mi> <mrow> <mi>e</mi> <mn>1</mn> </mrow> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>x</mi> <mn>3</mn> </msub> <mo>=</mo> <msub> <mi>x</mi> <mrow> <mi>s</mi> <mn>1</mn> </mrow> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>y</mi> <mn>3</mn> </msub> <mo>=</mo> <msub> <mi>y</mi> <mrow> <mi>e</mi> <mn>1</mn> </mrow> </msub> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>15</mn> <mo>)</mo> </mrow> </mrow>
    Equation group (15) is solved using " SOLVE " function in MATLAB, obtains x3,y3And z3Solution；
    Step (5) calculates joint angular displacement4And θ5
    Obtain process point coordinates c3=[x3 y3 z3] after, it will be described as follows respectively around the rotary motion in joint 4 and joint 5,
    <mrow> <mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <mi>exp</mi> <mrow> <mo>(</mo> <msub> <mover> <mi>&amp;xi;</mi> <mo>^</mo> </mover> <mn>4</mn> </msub> <msub> <mi>&amp;theta;</mi> <mn>4</mn> </msub> <mo>)</mo> </mrow> <msubsup> <mi>c</mi> <mn>3</mn> <mo>&amp;prime;</mo> </msubsup> <mo>=</mo> <msubsup> <mi>q</mi> <mrow> <mi>e</mi> <mn>1</mn> </mrow> <mo>&amp;prime;</mo> </msubsup> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>exp</mi> <mrow> <mo>(</mo> <msub> <mover> <mi>&amp;xi;</mi> <mo>^</mo> </mover> <mn>5</mn> </msub> <msub> <mi>&amp;theta;</mi> <mn>5</mn> </msub> <mo>)</mo> </mrow> <msubsup> <mi>q</mi> <mrow> <mi>s</mi> <mn>1</mn> </mrow> <mo>&amp;prime;</mo> </msubsup> <mo>=</mo> <msubsup> <mi>c</mi> <mn>3</mn> <mo>&amp;prime;</mo> </msubsup> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>16</mn> <mo>)</mo> </mrow> </mrow>
    Based on Paden-Kahan subproblems 1, joint angular displacement is obtained4And θ5Display expression formula it is as follows,
    <mrow> <msub> <mi>&amp;theta;</mi> <mn>4</mn> </msub> <mo>=</mo> <mi>a</mi> <mi> </mi> <mi>t</mi> <mi>a</mi> <mi>n</mi> <mrow> <mo>(</mo> <mfrac> <mrow> <mo>(</mo> <msub> <mi>z</mi> <mn>3</mn> </msub> <mo>-</mo> <mn>940</mn> <mo>)</mo> <msub> <mi>x</mi> <mrow> <mi>e</mi> <mn>1</mn> </mrow> </msub> <mo>-</mo> <msub> <mi>x</mi> <mn>3</mn> </msub> <mo>(</mo> <msub> <mi>z</mi> <mrow> <mi>e</mi> <mn>1</mn> </mrow> </msub> <mo>-</mo> <mn>940</mn> <mo>)</mo> </mrow> <mrow> <msub> <mi>x</mi> <mn>3</mn> </msub> <msub> <mi>x</mi> <mrow> <mi>e</mi> <mn>1</mn> </mrow> </msub> <mo>+</mo> <mrow> <mo>(</mo> <msub> <mi>z</mi> <mn>3</mn> </msub> <mo>-</mo> <mn>940</mn> <mo>)</mo> </mrow> <mrow> <mo>(</mo> <msub> <mi>z</mi> <mrow> <mi>e</mi> <mn>1</mn> </mrow> </msub> <mo>-</mo> <mn>940</mn> <mo>)</mo> </mrow> </mrow> </mfrac> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>17</mn> <mo>)</mo> </mrow> </mrow>
    <mrow> <msub> <mi>&amp;theta;</mi> <mn>5</mn> </msub> <mo>=</mo> <mi>a</mi> <mi> </mi> <mi>t</mi> <mi>a</mi> <mi>n</mi> <mrow> <mo>(</mo> <mfrac> <mrow> <mo>(</mo> <msub> <mi>y</mi> <mrow> <mi>s</mi> <mn>1</mn> </mrow> </msub> <mo>-</mo> <mn>744</mn> <mo>)</mo> <mo>(</mo> <msub> <mi>z</mi> <mn>3</mn> </msub> <mo>-</mo> <mn>940</mn> <mo>)</mo> <mo>-</mo> <mo>(</mo> <msub> <mi>z</mi> <mrow> <mi>s</mi> <mn>1</mn> </mrow> </msub> <mo>-</mo> <mn>940</mn> <mo>)</mo> <mo>(</mo> <msub> <mi>y</mi> <mn>3</mn> </msub> <mo>-</mo> <mn>744</mn> <mo>)</mo> </mrow> <mrow> <mo>(</mo> <msub> <mi>y</mi> <mrow> <mi>s</mi> <mn>1</mn> </mrow> </msub> <mo>-</mo> <mn>744</mn> <mo>)</mo> <mo>(</mo> <msub> <mi>y</mi> <mn>3</mn> </msub> <mo>-</mo> <mn>744</mn> <mo>)</mo> <mo>+</mo> <mo>(</mo> <msub> <mi>z</mi> <mn>3</mn> </msub> <mo>-</mo> <mn>940</mn> <mo>)</mo> <mo>(</mo> <msub> <mi>z</mi> <mrow> <mi>s</mi> <mn>1</mn> </mrow> </msub> <mo>-</mo> <mn>940</mn> <mo>)</mo> </mrow> </mfrac> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>18</mn> <mo>)</mo> </mrow> </mrow>
    Step (6) calculates joint angular displacement6
    Take a not point q on the axis of joint 6s2=[0 750 940], its rotary motion around joint 6 are described as,
    <mrow> <mi>exp</mi> <mrow> <mo>(</mo> <msub> <mover> <mi>&amp;xi;</mi> <mo>^</mo> </mover> <mn>6</mn> </msub> <msub> <mi>&amp;theta;</mi> <mn>6</mn> </msub> <mo>)</mo> </mrow> <msubsup> <mi>q</mi> <mrow> <mi>s</mi> <mn>2</mn> </mrow> <mo>&amp;prime;</mo> </msubsup> <mo>=</mo> <msubsup> <mi>q</mi> <mrow> <mi>e</mi> <mn>2</mn> </mrow> <mo>&amp;prime;</mo> </msubsup> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>19</mn> <mo>)</mo> </mrow> </mrow>
    WhereinSimilarly it is based on Paden-Kahan Subproblem 1, obtain joint angular displacement6Display expression formula it is as follows,
    <mrow> <msub> <mi>&amp;theta;</mi> <mn>5</mn> </msub> <mo>=</mo> <mi>a</mi> <mi> </mi> <mi>t</mi> <mi>a</mi> <mi>n</mi> <mrow> <mo>(</mo> <mfrac> <mrow> <msub> <mi>x</mi> <mrow> <mi>s</mi> <mn>2</mn> </mrow> </msub> <mrow> <mo>(</mo> <msub> <mi>y</mi> <mrow> <mi>e</mi> <mn>2</mn> </mrow> </msub> <mo>-</mo> <mn>744</mn> <mo>)</mo> </mrow> <mo>-</mo> <msub> <mi>x</mi> <mrow> <mi>e</mi> <mn>2</mn> </mrow> </msub> <mrow> <mo>(</mo> <msub> <mi>y</mi> <mrow> <mi>s</mi> <mn>2</mn> </mrow> </msub> <mo>-</mo> <mn>744</mn> <mo>)</mo> </mrow> </mrow> <mrow> <msub> <mi>x</mi> <mrow> <mi>s</mi> <mn>2</mn> </mrow> </msub> <msub> <mi>x</mi> <mrow> <mi>e</mi> <mn>2</mn> </mrow> </msub> <mo>+</mo> <mrow> <mo>(</mo> <msub> <mi>y</mi> <mrow> <mi>s</mi> <mn>2</mn> </mrow> </msub> <mo>-</mo> <mn>744</mn> <mo>)</mo> </mrow> <mrow> <mo>(</mo> <msub> <mi>y</mi> <mrow> <mi>e</mi> <mn>2</mn> </mrow> </msub> <mo>-</mo> <mn>744</mn> <mo>)</mo> </mrow> </mrow> </mfrac> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>20</mn> <mo>)</mo> </mrow> </mrow>
    Solved more than and obtain eight groups of Inverse Kinematics Solutions.
CN201710953599.4A 2017-10-13 2017-10-13 6R robot inverse kinematics geometric solving method based on momentum theory Active CN107756400B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201710953599.4A CN107756400B (en) 2017-10-13 2017-10-13 6R robot inverse kinematics geometric solving method based on momentum theory

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201710953599.4A CN107756400B (en) 2017-10-13 2017-10-13 6R robot inverse kinematics geometric solving method based on momentum theory

Publications (2)

Publication Number Publication Date
CN107756400A true CN107756400A (en) 2018-03-06
CN107756400B CN107756400B (en) 2020-12-04

Family

ID=61269419

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201710953599.4A Active CN107756400B (en) 2017-10-13 2017-10-13 6R robot inverse kinematics geometric solving method based on momentum theory

Country Status (1)

Country Link
CN (1) CN107756400B (en)

Cited By (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108406770A (en) * 2018-03-09 2018-08-17 首都师范大学 The joint rotation angle of serial manipulator determines method and device
CN109015641A (en) * 2018-08-16 2018-12-18 居鹤华 The inverse solution modeling of general 6R mechanical arm based on axis invariant and calculation method
CN109291047A (en) * 2018-08-16 2019-02-01 居鹤华 Based on axis invariant and the inverse solution modeling method of DH parameter 1R/2R/3R
CN109483529A (en) * 2018-10-12 2019-03-19 华南智能机器人创新研究院 A kind of mechanical arm method of servo-controlling, system and device based on screw theory
CN109968358A (en) * 2019-03-28 2019-07-05 北京工业大学 A kind of redundant robot's total joint avoidance track optimizing method considering robust motion
CN110385716A (en) * 2018-04-18 2019-10-29 B和R工业自动化有限公司 For controlling the method and kinematic mechanisms of kinematic mechanisms
CN111452041A (en) * 2020-03-17 2020-07-28 湖南工业大学 Inverse kinematics solving method for non-spherical wrist 6R robot
CN111496783A (en) * 2020-03-26 2020-08-07 天津大学 Inverse kinematics solving method for 6R industrial robot
CN113400310A (en) * 2021-06-28 2021-09-17 首都师范大学 Formal verification method for dexterous hand inverse kinematics of three-finger robot and electronic equipment
CN114290331A (en) * 2021-12-14 2022-04-08 中国科学院深圳先进技术研究院 Robot motion control method, robot and computer readable storage device
CN115741672A (en) * 2022-10-21 2023-03-07 杭州邦杰星医疗科技有限公司 DH derivation method based on rigid body transformation

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102243620A (en) * 2011-06-02 2011-11-16 安凯 Rapid solving method for inverse kinematics problem of six-joint mechanical arm
CN102637158A (en) * 2012-04-28 2012-08-15 谷菲 Inverse kinematics solution method for six-degree-of-freedom serial robot
CN106647282A (en) * 2017-01-19 2017-05-10 北京工业大学 Six-freedom-degree robot track planning method giving consideration to tail end motion error
CN106845037A (en) * 2017-03-21 2017-06-13 山东科技大学 A kind of inverse kinematics general method for solving of five degree of freedom serial manipulator
CN107203653A (en) * 2017-04-12 2017-09-26 山东科技大学 A kind of inverse kinematics general method for solving of six degree of freedom serial manipulator

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102243620A (en) * 2011-06-02 2011-11-16 安凯 Rapid solving method for inverse kinematics problem of six-joint mechanical arm
CN102637158A (en) * 2012-04-28 2012-08-15 谷菲 Inverse kinematics solution method for six-degree-of-freedom serial robot
CN106647282A (en) * 2017-01-19 2017-05-10 北京工业大学 Six-freedom-degree robot track planning method giving consideration to tail end motion error
CN106845037A (en) * 2017-03-21 2017-06-13 山东科技大学 A kind of inverse kinematics general method for solving of five degree of freedom serial manipulator
CN107203653A (en) * 2017-04-12 2017-09-26 山东科技大学 A kind of inverse kinematics general method for solving of six degree of freedom serial manipulator

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
洪磊: "ABB1410工业机器人的旋量运动学逆解方法", 《机械设计与制造》 *

Cited By (17)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108406770B (en) * 2018-03-09 2020-12-18 首都师范大学 Joint rotation angle determination method and device of 6-degree-of-freedom series robot
CN108406770A (en) * 2018-03-09 2018-08-17 首都师范大学 The joint rotation angle of serial manipulator determines method and device
CN110385716A (en) * 2018-04-18 2019-10-29 B和R工业自动化有限公司 For controlling the method and kinematic mechanisms of kinematic mechanisms
CN110385716B (en) * 2018-04-18 2024-03-12 B和R工业自动化有限公司 Method for controlling a kinematic mechanism and kinematic mechanism
CN109015641A (en) * 2018-08-16 2018-12-18 居鹤华 The inverse solution modeling of general 6R mechanical arm based on axis invariant and calculation method
CN109291047A (en) * 2018-08-16 2019-02-01 居鹤华 Based on axis invariant and the inverse solution modeling method of DH parameter 1R/2R/3R
CN109483529A (en) * 2018-10-12 2019-03-19 华南智能机器人创新研究院 A kind of mechanical arm method of servo-controlling, system and device based on screw theory
CN109483529B (en) * 2018-10-12 2021-02-26 华南智能机器人创新研究院 Mechanical arm servo control method, system and device based on spiral theory
CN109968358A (en) * 2019-03-28 2019-07-05 北京工业大学 A kind of redundant robot's total joint avoidance track optimizing method considering robust motion
CN109968358B (en) * 2019-03-28 2021-04-09 北京工业大学 Redundant robot full-joint obstacle avoidance track optimization method considering motion stability
CN111452041A (en) * 2020-03-17 2020-07-28 湖南工业大学 Inverse kinematics solving method for non-spherical wrist 6R robot
CN111496783A (en) * 2020-03-26 2020-08-07 天津大学 Inverse kinematics solving method for 6R industrial robot
CN113400310A (en) * 2021-06-28 2021-09-17 首都师范大学 Formal verification method for dexterous hand inverse kinematics of three-finger robot and electronic equipment
CN113400310B (en) * 2021-06-28 2022-07-05 首都师范大学 Formal verification method for dexterous hand inverse kinematics of three-finger robot and electronic equipment
CN114290331A (en) * 2021-12-14 2022-04-08 中国科学院深圳先进技术研究院 Robot motion control method, robot and computer readable storage device
CN115741672A (en) * 2022-10-21 2023-03-07 杭州邦杰星医疗科技有限公司 DH derivation method based on rigid body transformation
CN115741672B (en) * 2022-10-21 2024-04-19 杭州邦杰星医疗科技有限公司 DH deducing method based on rigid body transformation

Also Published As

Publication number Publication date
CN107756400B (en) 2020-12-04

Similar Documents

Publication Publication Date Title
CN107756400A (en) A kind of 6R Robotic inverse kinematics geometry solving methods based on spinor theory
CN108621158B (en) Time optimal trajectory planning control method and device for mechanical arm
CN102207988B (en) Efficient dynamic modeling method for multi-degree of freedom (multi-DOF) mechanical arm
CN104573255B (en) It is a kind of to be modeled and method for solving based on the mechanical arm for improving multi-body system transfer matrix
CN108241339A (en) The movement solution of apery mechanical arm and configuration control method
CN110421564A (en) A kind of robot working unit&#39;s layout optimization method based on joint energy consumption assessment
CN106845037B (en) A kind of general method for solving of inverse kinematics of five degree of freedom serial manipulator
CN113084821A (en) Spraying robot time optimal trajectory planning method based on dynamics
CN107203653A (en) A kind of inverse kinematics general method for solving of six degree of freedom serial manipulator
CN103481288A (en) 5-joint robot end-of-arm tool pose controlling method
CN108406769A (en) The joint rotation angle of serial manipulator determines method and device
CN107791248A (en) Control method based on the six degree of freedom serial manipulator for being unsatisfactory for pipper criterions
CN106406097A (en) Distributed adaptive coordinated control method for multi-manipulator systems
CN106346480A (en) Multi-freedom-degree injection mold mechanical arm modeling method based on UG and MATLAB
CN106371442A (en) Tensor-product-model-transformation-based mobile robot control method
Al Mashhadany Virtual reality trajectory of modified PUMA 560 by hybrid intelligent controller
CN108687774B (en) Solving method for working space of hybrid robot
CN110450154A (en) One kind being based on dynamic (dynamical) mechanical arm flexible control method
CN109866224A (en) A kind of robot Jacobian matrix calculation method, device and storage medium
CN109434838A (en) Coordinated motion planning method and system for endoscopic operation of line-driven continuous robot
Zhao et al. Trajectory planning for 6-DOF robotic arm based on quintic polynormial
CN103529856A (en) 5-joint robot end tool position and posture control method
CN106055519B (en) A kind of Singularity Analysis of Stewart parallel institutions
CN111283682A (en) Geometric projection solution of forward kinematics of 4-UPU four-degree-of-freedom parallel robot
CN111531532A (en) Robot climbing movement speed modeling method based on momentum theory

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