CN107756400A - 一种基于旋量理论的6r机器人逆运动学几何求解方法 - Google Patents

一种基于旋量理论的6r机器人逆运动学几何求解方法 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
English (en)
Other versions
CN107756400B (zh
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/zh
Publication of CN107756400A publication Critical patent/CN107756400A/zh
Application granted granted Critical
Publication of CN107756400B publication Critical patent/CN107756400B/zh
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

本发明公开了一种基于旋量理论的6R机器人逆运动学几何求解方法，属于机器人运动学逆解方法研究领域。建立基坐标系与工具坐标系，通过基坐标系与工具坐标系确定6R机器人运动学参数，并建立正运动学模型。将6R机器人的前三个关节的逆解运动分解描述并建立六元二次方程组。基于旋量正运动学模型求解初始位置qs1对应的目标位置qe1。该方法将几何描述与旋量理论结合，几何意义更加明确，通过简化逆解算法为代数方程组求解，有效提升了计算效率，能够为机器人运动实时控制提供一种新的逆解处理方法。

Description

一种基于旋量理论的6R机器人逆运动学几何求解方法
技术领域
本发明属于机器人运动学逆解方法研究领域，尤其涉及一种基于旋量理论的6R机器人逆运动学几何求解方法。
背景技术
运动学分析是实现运动控制的基础，主要建立关节变量与末端位姿的映射模型。其中逆解问题，即已知末端位姿求解关节变量的问题，是机器人领域研究热点之一，其求解效率直接影响着机器人运动控制的实时性能。目前机器人运动学逆解建模主要基于D-H法和旋量理论，研究者们对比了两种方法并发现后者的应用具有以下优点：可避免建立局部坐标系，简化计算模型并克服了局部参数产生的奇异性；其几何意义明确，可方便的确定产生多解的条件和个数。目前基于旋量描述的Paden-Kahan子问题被广泛的机器人逆向求解问题中，但该方法不适用于任意构型的六自由度机器人，因此多数学者基于三种基本子问题进行了延伸并提出了一些新的子问题模型，如Tan对子问题二进行了改进建立了针对“绕两个不相交轴的旋转运动”子问题的数学模型；Chen描述了一种“绕三个轴线旋转的运动，其中两个轴线平行且与第三轴线异面”的子问题，针对其逆解进行了详细的旋量描述。基于这些子问题模型在六自由度串联机器人逆解问题中得到了广泛应用，研究表明当6R机器人满足Piper准则时其逆运动学问题具有封闭解。Sariyildi等基于三个子问题实现了6R机器人的逆运动学求解；吕世增等通过在旋量法中引入吴方法减少了对子问题的依赖性，并将6R机器人的逆解问题转化为六元八次方程组的求解问题。为了进一步简化6R机器人逆解模型，本方法通过几何描述方法将6R机器人的逆解问题转化为一个六元二次方程组和一个三元二次方程组的求解，从而使其几何意义更加清晰，求解过程更加简单。
发明内容
本发明的目的旨在提供一种基于旋量理论的6R机器人逆运动学几何求解方法。该方法的主要特点是通过结合几何描述方法和旋量理论，针对6R机器人运动学逆解问题提出一种几何意义清晰、求解过程简单的逆运动学模型。
为实现上述目的，本发明采用的技术手段为一种基于旋量理论的6R机器人逆运动学几何求解方法，该方法的实现过程如下：
S1、建立基坐标系与工具坐标系，通过基坐标系与工具坐标系确定6R机器人运动学参数，并建立正运动学模型。
S2、如图1，将6R机器人的前三个关节的逆解运动分解描述：由点qe到点c2＝[x2 y2z2]绕关节1的旋转运动；点c2到点c1＝[x1 y1 z1]绕关节2的旋转运动；点c1到点qs绕关节3的旋转运动。其中qs，qe分别为机器人末端的初始和目标位置，根据图1所示的几何关系描述建立以x1,y1,z1,x2,y2和z2为变量的六元二次方程组。
S3、采用MATLAB中“SOLVE”函数求解以上方程组，得到c1和c2的坐标矢量。
S4、自此，前三个关节的逆解运动始末位置已知，基于Paden-Kahan子问题1分别建立6R机器人的前三个关节角变量θ1，θ2和θ3的显式求解模型，并进行求解。
S5、在关节6的轴线上取一点作为后三个关节逆解运动中的初始位置qs1，并基于旋量正运动学模型求解初始位置qs1对应的目标位置qe1。
S6、如图2，将关节4和5的逆解运动分解描述：由点qe1到点c3＝[x3y3z3]的旋转运动；由点c3到点qs1的旋转运动。根据图2示，根据几何关系建立以x3,y3和z3为变量的三元二次方程组，并采用“SOLVE”函数求解，得到c3的坐标矢量。
S7、自此，关节4和关节5的逆解运动始末位置已知，基于Paden-Kahan子问题1分别建立关节4和关节5的关节角变量θ4和θ5显式求解模型，并进行求解。
S8、取不在关节6轴线上的任意一点qs2，基于旋量正运动学模型求解qs2对应的目标位置qe2，则关节6的逆解运动始末位置已知。同理基于子问题1求解θ6。
本发明的特点在于基于几何描述方法将6R机器人的运动学逆解问题转化为一个六元二次方程组和一个三元二次方程组的求解，简化了逆解模型，且几何意义更加明确，为6R机器人的实时运动控制提供一定的方法支撑。该方法将几何描述与旋量理论结合，几何意义更加明确，通过简化逆解算法为代数方程组求解，有效提升了计算效率，能够为机器人运动实时控制提供一种新的逆解处理方法。
附图说明
图1关节1、2和3的逆解运动几何描述；
图2关节4和5的逆解运动几何描述；
图3某6R机器人参数坐标系。
具体实施方式
以下结合附图1-3对本发明进行详细说明。
S1确定6R机器人运动学参数并建立正运动学模型
如图3所示，已知该6R机器人的初始状态各关节所在位置矢量及旋转矢量如下：
其中ri，1≤i≤6表示i关节在基坐标系的位置矢量，ωi表示i关节的旋转矢量。
基于旋量理论，该6R机器人正运动学模型表示为，
其中gst(θ),gst(0)分别表示机器人末端的初始位姿与目标位姿，表示i关节旋转运动的指数积形式，
式中θi为第i个关节角位移；是i关节旋转矢量ωi的另一种表示形式，由ωi＝[ω1 ω2 ω3]定义为则νi是i关节运动的旋转线速度，νi＝-ωi×ri。
给定目标位姿，
S2针对前三个关节建立六元二次方程组
基于旋量理论前三个关节的旋转运动描述为，
式中q′s和q′e分别表示6R机器人末端在该旋转运动的初始位置与目标位置。由S1可知，
q′s＝[xs ys zs 1]＝[0 744 940 1]
q′e＝[xe ye ze 1]＝[-936.6611 631.7859 570.0752 1]
设运动经过点c1和c2,根据图1所示几何描述建立以下关系式组，
其中q1，q2和q3分别为关节1、关节2和关节3旋转轴线上的任意一点，为简化模型取q1＝[0 0 0]，q2＝[0 150 250]和q3＝[0 150 800]，那么式(7)表示为如下方程组，
采用MATLAB中“SOLVE”函数求解方程组(8)，得到x1,y1,z1,x2,y2和z2的解。
S3计算关节角位移θ1，θ2和θ3
得到过程点坐标c1＝[x1 y1 z1]和c2＝[x2 y2 z2]后，将绕关节1、关节2和关节3的旋转运动分别描述如下，
基于Paden-Kahan子问题1，得到关节角位移的显示表达式如下，
步骤(4)针对关节4和关节5建立三元二次方程组
由式(3)知，
其中在关节6的轴线上取一点qs1＝[xs1 ys1 zs1]＝[0 744 0]，其绕关节4与关节5的旋转运动描述如下，
其中q′e1＝g1q′s1＝[xe1 ye1 ze1 1]。
设运动经过点坐标为c3＝[x3 y3 z3]，根据图2所示几何描述建立以下关系式组，
其中q4＝[0 744 940]。则式(14)表示为如下方程，
采用MATLAB中“SOLVE”函数求解方程组(15)，得到x3,y3和z3的解。
步骤(5)计算关节角位移θ4和θ5
得到过程点坐标c3＝[x3 y3 z3]后，将绕关节4和关节5的旋转运动分别描述如下，
基于Paden-Kahan子问题1，得到关节角位移θ4和θ5的显示表达式如下，
步骤(6)计算关节角位移θ6
取不在关节6轴线上一点qs2＝[0 750 940]，其绕关节6的旋转运动描述为，
其中同理基于Paden-Kahan子问题1，得到关节角位移θ6的显示表达式如下，
通过以上求解得到八组运动学逆解如表1所示。
表1八组运动学逆解

Claims (2)

1.一种基于旋量理论的6R机器人逆运动学几何求解方法，其特征在于：该方法的实现过程如下：
S1、建立基坐标系与工具坐标系，通过基坐标系与工具坐标系确定6R机器人运动学参数，并建立正运动学模型；
S2、将6R机器人的前三个关节的逆解运动分解描述：由点qe到点c2＝[x2 y2 z2]绕关节1的旋转运动；点c2到点c1＝[x1 y1 z1]绕关节2的旋转运动；点c1到点qs绕关节3的旋转运动；其中qs，qe分别为机器人末端的初始和目标位置，根据几何关系描述建立以x1,y1,z1,x2,y2和z2为变量的六元二次方程组；
S3、采用MATLAB中“SOLVE”函数求解以上方程组，得到c1和c2的坐标矢量；
S4、自此，前三个关节的逆解运动始末位置已知，基于Paden-Kahan子问题1分别建立6R机器人的前三个关节角变量θ1，θ2和θ3的显式求解模型，并进行求解；
S5、在关节6的轴线上取一点作为后三个关节逆解运动中的初始位置qs1，并基于旋量正运动学模型求解初始位置qs1对应的目标位置qe1；
S6、将关节4和5的逆解运动分解描述：由点qe1到点c3＝[x3 y3 z3]的旋转运动；由点c3到点qs1的旋转运动；根据几何关系建立以x3,y3和z3为变量的三元二次方程组，并采用“SOLVE”函数求解，得到c3的坐标矢量；
S7、自此，关节4和关节5的逆解运动始末位置已知，基于Paden-Kahan子问题1分别建立关节4和关节5的关节角变量θ4和θ5显式求解模型，并进行求解；
S8、取不在关节6轴线上的任意一点qs2，基于旋量正运动学模型求解qs2对应的目标位置qe2，则关节6的逆解运动始末位置已知；同理基于子问题1求解θ6。
2.根据权利要求1所述的一种基于旋量理论的6R机器人逆运动学几何求解方法，其特征在于：S1确定6R机器人运动学参数并建立正运动学模型
已知该6R机器人的初始状态各关节所在位置矢量及旋转矢量如下：
<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>
其中ri，1≤i≤6表示i关节在基坐标系的位置矢量，ωi表示i关节的旋转矢量；
基于旋量理论，该6R机器人正运动学模型表示为，
<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>
其中gst(θ),gst(0)分别表示机器人末端的初始位姿与目标位姿，表示i关节旋转运动的指数积形式，
<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>
式中θi为第i个关节角位移；是i关节旋转矢量ωi的另一种表示形式，由ωi＝[ω1ω2 ω3]定义为则νi是i关节运动的旋转线速度，νi＝-ωi×ri；
给定目标位姿，
<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针对前三个关节建立六元二次方程组
基于旋量理论前三个关节的旋转运动描述为，
<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′s和q′e分别表示6R机器人末端在该旋转运动的初始位置与目标位置；由S1可知，
q′s＝[xs ys zs 1]＝[0 744 940 1]
q′e＝[xe ye ze 1]＝[-936.6611 631.7859 570.0752 1]
设运动经过点c1和c2,根据几何描述建立以下关系式组，
<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>
其中q1，q2和q3分别为关节1、关节2和关节3旋转轴线上的任意一点，为简化模型取q1＝[0 0 0]，q2＝[0 150 250]和q3＝[0 150 800]，那么式(7)表示为如下方程组，
<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>
采用MATLAB中“SOLVE”函数求解方程组(8)，得到x1,y1,z1,x2,y2和z2的解；
S3计算关节角位移θ1，θ2和θ3
得到过程点坐标c1＝[x1 y1 z1]和c2＝[x2 y2 z2]后，将绕关节1、关节2和关节3的旋转运动分别描述如下，
<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>
基于Paden-Kahan子问题1，得到关节角位移的显示表达式如下，
<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>
步骤(4)针对关节4和关节5建立三元二次方程组
由式(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>
其中在关节6的轴线上取一点qs1＝[xs1 ys1 zs1]＝[0 744 0]，其绕关节4与关节5的旋转运动描述如下，
<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>
其中
设运动经过点坐标为c3＝[x3 y3 z3]，根据几何描述建立以下关系式组，
<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>
其中q4＝[0 744 940]；则式(14)表示为如下方程，
<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>
采用MATLAB中“SOLVE”函数求解方程组(15)，得到x3,y3和z3的解；
步骤(5)计算关节角位移θ4和θ5
得到过程点坐标c3＝[x3 y3 z3]后，将绕关节4和关节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>
基于Paden-Kahan子问题1，得到关节角位移θ4和θ5的显示表达式如下，
<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>
步骤(6)计算关节角位移θ6
取不在关节6轴线上一点qs2＝[0 750 940]，其绕关节6的旋转运动描述为，
<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>
其中同理基于Paden-Kahan子问题1，得到关节角位移θ6的显示表达式如下，
<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>
通过以上求解得到八组运动学逆解。
CN201710953599.4A 2017-10-13 2017-10-13 一种基于旋量理论的6r机器人逆运动学几何求解方法 Active CN107756400B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201710953599.4A CN107756400B (zh) 2017-10-13 2017-10-13 一种基于旋量理论的6r机器人逆运动学几何求解方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201710953599.4A CN107756400B (zh) 2017-10-13 2017-10-13 一种基于旋量理论的6r机器人逆运动学几何求解方法

Publications (2)

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

Family

ID=61269419

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201710953599.4A Active CN107756400B (zh) 2017-10-13 2017-10-13 一种基于旋量理论的6r机器人逆运动学几何求解方法

Country Status (1)

Country Link
CN (1) CN107756400B (zh)

Cited By (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108406770A (zh) * 2018-03-09 2018-08-17 首都师范大学 串联机器人的关节旋转角度确定方法及装置
CN109015641A (zh) * 2018-08-16 2018-12-18 居鹤华 基于轴不变量的通用6r机械臂逆解建模与解算方法
CN109291047A (zh) * 2018-08-16 2019-02-01 居鹤华 基于轴不变量及dh参数1r/2r/3r逆解建模方法
CN109483529A (zh) * 2018-10-12 2019-03-19 华南智能机器人创新研究院 一种基于螺旋理论的机械臂伺服控制方法、***及装置
CN109968358A (zh) * 2019-03-28 2019-07-05 北京工业大学 一种考虑运动平稳性的冗余机器人全关节避障轨迹优化方法
CN110385716A (zh) * 2018-04-18 2019-10-29 B和R工业自动化有限公司 用于控制运动学机构的方法和运动学机构
CN111452041A (zh) * 2020-03-17 2020-07-28 湖南工业大学 一种非球腕6r机器人逆运动学求取方法
CN111496783A (zh) * 2020-03-26 2020-08-07 天津大学 一种6r工业机器人逆运动学求解方法
CN113400310A (zh) * 2021-06-28 2021-09-17 首都师范大学 三指机器人灵巧手逆运动学的形式化验证方法及电子设备
CN114290331A (zh) * 2021-12-14 2022-04-08 中国科学院深圳先进技术研究院 机器人运动控制方法、机器人及计算机可读存储装置
CN115741672A (zh) * 2022-10-21 2023-03-07 杭州邦杰星医疗科技有限公司 一种基于刚体变换的dh推导方法

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102243620A (zh) * 2011-06-02 2011-11-16 安凯 六关节机械臂逆运动学问题的快速求解方法
CN102637158A (zh) * 2012-04-28 2012-08-15 谷菲 一种六自由度串联机器人运动学逆解的求解方法
CN106647282A (zh) * 2017-01-19 2017-05-10 北京工业大学 一种考虑末端运动误差的六自由度机器人轨迹规划方法
CN106845037A (zh) * 2017-03-21 2017-06-13 山东科技大学 一种五自由度串联机器人的逆运动学通用求解方法
CN107203653A (zh) * 2017-04-12 2017-09-26 山东科技大学 一种六自由度串联机器人的逆运动学通用求解方法

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102243620A (zh) * 2011-06-02 2011-11-16 安凯 六关节机械臂逆运动学问题的快速求解方法
CN102637158A (zh) * 2012-04-28 2012-08-15 谷菲 一种六自由度串联机器人运动学逆解的求解方法
CN106647282A (zh) * 2017-01-19 2017-05-10 北京工业大学 一种考虑末端运动误差的六自由度机器人轨迹规划方法
CN106845037A (zh) * 2017-03-21 2017-06-13 山东科技大学 一种五自由度串联机器人的逆运动学通用求解方法
CN107203653A (zh) * 2017-04-12 2017-09-26 山东科技大学 一种六自由度串联机器人的逆运动学通用求解方法

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 (zh) * 2018-03-09 2020-12-18 首都师范大学 6自由度串联机器人的关节旋转角度确定方法及装置
CN108406770A (zh) * 2018-03-09 2018-08-17 首都师范大学 串联机器人的关节旋转角度确定方法及装置
CN110385716A (zh) * 2018-04-18 2019-10-29 B和R工业自动化有限公司 用于控制运动学机构的方法和运动学机构
CN110385716B (zh) * 2018-04-18 2024-03-12 B和R工业自动化有限公司 用于控制运动学机构的方法和运动学机构
CN109015641A (zh) * 2018-08-16 2018-12-18 居鹤华 基于轴不变量的通用6r机械臂逆解建模与解算方法
CN109291047A (zh) * 2018-08-16 2019-02-01 居鹤华 基于轴不变量及dh参数1r/2r/3r逆解建模方法
CN109483529A (zh) * 2018-10-12 2019-03-19 华南智能机器人创新研究院 一种基于螺旋理论的机械臂伺服控制方法、***及装置
CN109483529B (zh) * 2018-10-12 2021-02-26 华南智能机器人创新研究院 一种基于螺旋理论的机械臂伺服控制方法、***及装置
CN109968358A (zh) * 2019-03-28 2019-07-05 北京工业大学 一种考虑运动平稳性的冗余机器人全关节避障轨迹优化方法
CN109968358B (zh) * 2019-03-28 2021-04-09 北京工业大学 一种考虑运动平稳性的冗余机器人全关节避障轨迹优化方法
CN111452041A (zh) * 2020-03-17 2020-07-28 湖南工业大学 一种非球腕6r机器人逆运动学求取方法
CN111496783A (zh) * 2020-03-26 2020-08-07 天津大学 一种6r工业机器人逆运动学求解方法
CN113400310A (zh) * 2021-06-28 2021-09-17 首都师范大学 三指机器人灵巧手逆运动学的形式化验证方法及电子设备
CN113400310B (zh) * 2021-06-28 2022-07-05 首都师范大学 三指机器人灵巧手逆运动学的形式化验证方法及电子设备
CN114290331A (zh) * 2021-12-14 2022-04-08 中国科学院深圳先进技术研究院 机器人运动控制方法、机器人及计算机可读存储装置
CN115741672A (zh) * 2022-10-21 2023-03-07 杭州邦杰星医疗科技有限公司 一种基于刚体变换的dh推导方法
CN115741672B (zh) * 2022-10-21 2024-04-19 杭州邦杰星医疗科技有限公司 一种基于刚体变换的dh推导方法

Also Published As

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

Similar Documents

Publication Publication Date Title
CN107756400A (zh) 一种基于旋量理论的6r机器人逆运动学几何求解方法
CN108621158B (zh) 一种关于机械臂的时间最优轨迹规划控制方法及装置
CN102207988B (zh) 一种多自由度机械臂高效动力学建模方法
Gong et al. Analytical inverse kinematics and self-motion application for 7-DOF redundant manipulator
CN104573255B (zh) 一种基于改进多体***传递矩阵的机械臂建模与求解方法
CN108241339A (zh) 仿人机械臂的运动求解和构型控制方法
CN110421564A (zh) 一种基于关节能耗评估的机器人工作单元布局优化方法
CN103792885B (zh) 一种数控弯管加工仿真方法及装置
CN113084821A (zh) 一种基于动力学的喷涂机器人时间最优轨迹规划方法
CN107203653A (zh) 一种六自由度串联机器人的逆运动学通用求解方法
CN104552247B (zh) 一种三自由度混联机器人机构
CN103481288A (zh) 一种5关节机器人末端工具位姿控制方法
CN108406769A (zh) 串联机器人的关节旋转角度确定方法及装置
CN107791248A (zh) 基于不满足pipper准则的六自由度串联机器人的控制方法
CN106406097A (zh) 多机械臂***的分布式自适应协调控制方法
CN106346480A (zh) 一种基于ug和matlab的多自由度注塑机械臂建模方法
CN106845037A (zh) 一种五自由度串联机器人的逆运动学通用求解方法
CN106371442A (zh) 一种基于张量积模型变换的移动机器人控制方法
Al Mashhadany Virtual reality trajectory of modified PUMA 560 by hybrid intelligent controller
CN108687774B (zh) 一种混联机器人工作空间的求解方法
CN110450154A (zh) 一种基于动力学的机械臂柔性控制方法
CN109866224A (zh) 一种机器人雅可比矩阵计算方法、装置及存储介质
CN109434838A (zh) 线驱动连续机器人内窥操作的协同运动规划方法及***
Zhao et al. Trajectory planning for 6-DOF robotic arm based on quintic polynormial
CN103529856A (zh) 5关节机器人末端工具位姿控制方法

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