CN108763151A - 一种任意三关节的逆运动学求解方法 - Google Patents

一种任意三关节的逆运动学求解方法 Download PDF

Info

Publication number
CN108763151A
CN108763151A CN201810323427.3A CN201810323427A CN108763151A CN 108763151 A CN108763151 A CN 108763151A CN 201810323427 A CN201810323427 A CN 201810323427A CN 108763151 A CN108763151 A CN 108763151A
Authority
CN
China
Prior art keywords
theta
sinθ
cosθ
formula
rotation
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN201810323427.3A
Other languages
English (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.)
Shandong University of Science and Technology
Original Assignee
Shandong University of Science and 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 Shandong University of Science and Technology filed Critical Shandong University of Science and Technology
Priority to CN201810323427.3A priority Critical patent/CN108763151A/zh
Priority to PCT/CN2018/095639 priority patent/WO2019196229A1/zh
Publication of CN108763151A publication Critical patent/CN108763151A/zh
Pending legal-status Critical Current

Links

Classifications

    • 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

Landscapes

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

Abstract

本发明公开了一种任意三关节的逆运动学求解方法，属于机器人逆运动学领域，本发明在指数积模型的基础上，利用简单的几何约束方程、旋量理论的基本性质以及旋转矩阵的Rodrigues表达，将问题转化成关于三角函数的线性方程进行求解，实现了任意三个关节轴线的逆解问题，使机器人逆解的求解不再局限于相交、平行、垂直的约束关系中，可根据需求设计结构，安装或加工中存在的误差也不会影响最终的计算结果。本发明是一种灵活、方便、实用的机器人逆解方法，为机器人在实际中的应用提供了方便。

Description

一种任意三关节的逆运动学求解方法
技术领域
本发明属于机器人逆运动学领域，具体涉及一种任意三关节的逆运动学求解方法。
背景技术
在机器人指数积模型中，其逆解的核心问题就是求解三阶子问题，因为一般的高维机器人无法直接获得其逆解，往往采用消元方法将其化简为三阶以下的问题来解决，而目前的三阶子问题都是通过进一步化简得到二阶子问题和一阶子问题来求解，很少有直接对其进行求解的方法，即使有这样的方法求解也是很复杂的，甚至得不到封闭解。而目前所采用的二阶子问题都是利用了特殊的几何关系：平行、相交、垂直等约束条件来求解，但实际中这些几何关系难以保证，同时这些方法也限制了机器人机械结构的设计。所以，能够有一种直接针对三阶子问题进行求解，得到一种统一的、不受机器人几何结构的约束求解方法具有重要的理论意义和实际意义。
发明内容
针对现有技术中存在的上述技术问题，本发明提出了一种任意三关节的逆运动学求解方法，设计合理，克服了现有技术的不足，具有良好的效果。
为了实现上述目的，本发明采用如下技术方案：
一种任意三关节的逆运动学求解方法，包括以下步骤：
步骤1：求解θ1和θ3
空间点p绕轴ω3旋转角度θ3到点p1，再绕轴ω2旋转角度θ2到点p2，最后点p2绕轴ω1旋转角度θ1到q点，这一过程可表示为：
其中，是p,q的齐次坐标，为第i关节的运动旋量，包括关节轴的轴方向向量和轴上一点ωi和ri被称为旋量参数，的表达形式如下：
其中，是ωi的反对称矩阵，如果ωi＝[ωix,ωiy,ωiz]T，则可表示成：
其中，和i＝1,3均已知；
是刚体变换的指数表达，对于转动关节其表达式为：
其中，I3×3为3×3的单位矩阵，是旋转矩阵，可用Rodrigues表示为：
根据旋量理论的基本性质可得：
其中，r21和r22分别为第二个轴上的两个点，将 以及和的Rodrigues公式，带入式(4)整理得：
a1 sinθ1+b1 cosθ1+c1 sinθ3+d1 cosθ3＝k1 (6)；
a2 sinθ1+b2 cosθ1+c2 sinθ3+d2 cosθ3＝k2 (7)；
其中，a1,b1,c1,d1,a2,b2,c2,d2,k1,k2均为已知参数；
当a1b2-b1a2≠0，对式(6)、(7)进行化简可得：
其中，
当c1d2-d1c2≠0时，公式(6)和(7)可整理为：
其中，公式(10)中的系数可根据公式(9)中的a,b分别与c,d互换，下标不变得到：
根据三角函数性质，将式(8)带入sin2θ1+cos2θ1＝1中，整理可得：
(fs1-us1 sinθ3-vs1 cosθ3)2+(fc1-uc1 sinθ3-vc1 cosθ3)2＝1 (11)；
设则将其带入sin2θ3+cos2θ3＝1中，整理可得：
m1t4+m2t3+m3t2+m4t+m5＝0 (12)；
其中，
m1＝(fs1+vs1)2+(fc1+vc1)2-1
m2＝-4[(fs1+vs1)us1+(fc1+vc1)uc1]
m4＝-4[(fs1-vs1)us1+(fc1-vc1)uc1]
m5＝(fs1-vs1)2+(fc1-vc1)2-1
根据费拉里法求解式(12)一元四次方程可得t的解，根据角度取值范围，可进一步确定θ3的值：
θ3＝2arc tan(t) (13)；
将θ3的值带入公式(8)，可得θ1：
θ1＝a tan2(fs1-us1 sinθ3-vs1 cosθ3,fc1-uc1 sinθ3-vc1 cosθ3) (14)；
步骤2：求解θ2
当θ1和θ3已知时，由和可获得p1和p2，而p2和p1之间有：
将旋转矩阵的Rodrigues公式带入公式(15)可得：
x2 sinθ2+y2 cosθ2＝z2 (16)；
其中，
由于在公式(16)两边分别同乘以和可得：
则可得θ2的值：
本发明所带来的有益技术效果：
1、计算效率高；直接针对三关节机器人进行求解，不需要进行降阶来实现；
2、实现简单；只需要求解一个一元四次方程和两个反正切函数获得三个关节的封闭解；
3、应用范围广；可应用于任意关系的RRR机器人中，不需要考虑其轴线之间的几何关系。
附图说明
图1为任意关系的RRR逆解示意图。
具体实施方式
下面结合附图以及具体实施方式对本发明作进一步详细说明：
任意关系的RRR逆解如图1所示。
一种任意三关节的逆运动学求解方法，包括以下步骤：
步骤1：求解θ1和θ3
空间点p绕轴ω3旋转角度θ3到点p1，再绕轴ω2旋转角度θ2到点p2，最后点p2绕轴ω1旋转角度θ1到q点，这一过程可表示为：
其中，是p,q的齐次坐标，为第i关节的运动旋量，包括关节轴的轴方向向量和轴上一点ωi和ri被称为旋量参数，的表达形式如下：
其中，是ωi的反对称矩阵，如果ωi＝[ωix,ωiy,ωiz]T，则可表示成：
其中，和i＝1,3均已知；
是刚体变换的指数表达，对于转动关节其表达式为：
其中，I3×3为3×3的单位矩阵，是旋转矩阵，可用Rodrigues表示为：
根据旋量理论的基本性质可得：
其中，r21和r22分别为第二个轴上的两个点，将 以及和的Rodrigues公式，带入式(4)整理得：
a1 sinθ1+b1 cosθ1+c1 sinθ3+d1 cosθ3＝k1 (6)；
a2 sinθ1+b2 cosθ1+c2 sinθ3+d2 cosθ3＝k2 (7)；
其中，a1,b1,c1,d1,a2,b2,c2,d2,k1,k2均为已知参数；
当a1b2-b1a2≠0，对式(6)、(7)进行化简可得：
其中，
当c1d2-d1c2≠0时，公式(6)和(7)可整理为：
其中，公式(10)中的系数可根据公式(9)中的a,b分别与c,d互换，下标不变得到：
根据三角函数性质，将式(8)带入sin2θ1+cos2θ1＝1中，整理可得：
(fs1-us1 sinθ3-vs1 cosθ3)2+(fc1-uc1 sinθ3-vc1 cosθ3)2＝1 (11)；
设则将其带入sin2θ3+cos2θ3＝1中，整理可得：
m1t4+m2t3+m3t2+m4t+m5＝0 (12)；
其中，
m1＝(fs1+vs1)2+(fc1+vc1)2-1
m2＝-4[(fs1+vs1)us1+(fc1+vc1)uc1]
m4＝-4[(fs1-vs1)us1+(fc1-vc1)uc1]
m5＝(fs1-vs1)2+(fc1-vc1)2-1
根据费拉里法求解式(12)一元四次方程可得t的解，根据角度取值范围，可进一步确定θ3的值：
θ3＝2arc tan(t) (13)；
将θ3的值带入公式(8)，可得θ1：
θ1＝a tan2(fs1-us1 sinθ3-vs1 cosθ3,fc1-uc1 sinθ3-vc1 cosθ3) (14)；
步骤2：求解θ2
当θ1和θ3已知时，由和可获得p1和p2，而p2和p1之间有：
将旋转矩阵的Rodrigues公式带入公式(15)可得：
x2 sinθ2+y2 cosθ2＝z2 (16)；
其中，
由于在公式(16)两边分别同乘以和可得：
则可得θ2的值：
当然，上述说明并非是对本发明的限制，本发明也并不仅限于上述举例，本技术领域的技术人员在本发明的实质范围内所做出的变化、改型、添加或替换，也应属于本发明的保护范围。

Claims (1)

1.一种任意三关节的逆运动学求解方法，其特征在于：包括以下步骤：
步骤1：求解θ1和θ3
空间点p绕轴ω3旋转角度θ3到点p1，再绕轴ω2旋转角度θ2到点p2，最后点p2绕轴ω1旋转角度θ1到q点，这一过程可表示为：
其中，是p,q的齐次坐标，为第i关节的运动旋量，包括关节轴的轴方向向量和轴上一点ωi和ri被称为旋量参数，的表达形式如下：
其中，是ωi的反对称矩阵，如果ωi＝[ωix,ωiy,ωiz]T，则可表示成：
其中，和i＝1,3均已知；
是刚体变换的指数表达，对于转动关节其表达式为：
其中，I3×3为3×3的单位矩阵，是旋转矩阵，可用Rodrigues表示为：
根据旋量理论的基本性质可得：
其中，r21和r22分别为第二个轴上的两个点，将 以及和的Rodrigues公式，带入式(4)整理得：
a1sinθ1+b1cosθ1+c1sinθ3+d1cosθ3＝k1 (6)；
a2sinθ1+b2cosθ1+c2sinθ3+d2cosθ3＝k2 (7)；
其中，a1,b1,c1,d1,a2,b2,c2,d2,k1,k2均为已知参数；
当a1b2-b1a2≠0，对式(6)、(7)进行化简可得：
其中，
当c1d2-d1c2≠0时，公式(6)和(7)可整理为：
其中，公式(10)中的系数可根据公式(9)中的a,b分别与c,d互换，下标不变得到：
根据三角函数性质，将式(8)带入sin2θ1+cos2θ1＝1中，整理可得：
(fs1-us1sinθ3-vs1cosθ3)2+(fc1-uc1sinθ3-vc1cosθ3)2＝1 (11)；
设则将其带入sin2θ3+cos2θ3＝1中，整理可得：
m1t4+m2t3+m3t2+m4t+m5＝0 (12)；
其中，
m1＝(fs1+vs1)2+(fc1+vc1)2-1
m2＝-4[(fs1+vs1)us1+(fc1+vc1)uc1]
m4＝-4[(fs1-vs1)us1+(fc1-vc1)uc1]
m5＝(fs1-vs1)2+(fc1-vc1)2-1
根据费拉里法求解式(12)一元四次方程可得t的解，根据角度取值范围，可进一步确定θ3的值：
θ3＝2arctan(t) (13)；
将θ3的值带入公式(8)，可得θ1：
θ1＝atan2(fs1-us1sinθ3-vs1cosθ3,fc1-uc1sinθ3-vc1cosθ3) (14)；
步骤2：求解θ2
当θ1和θ3已知时，由和可获得p1和p2，而p2和p1之间有：
将旋转矩阵的Rodrigues公式带入公式(15)可得：
x2sinθ2+y2cosθ2＝z2 (16)；
其中，
由于在公式(16)两边分别同乘以和可得：
则可得θ2的值：
CN201810323427.3A 2018-04-12 2018-04-12 一种任意三关节的逆运动学求解方法 Pending CN108763151A (zh)

Priority Applications (2)

Application Number Priority Date Filing Date Title
CN201810323427.3A CN108763151A (zh) 2018-04-12 2018-04-12 一种任意三关节的逆运动学求解方法
PCT/CN2018/095639 WO2019196229A1 (zh) 2018-04-12 2018-07-13 一种任意三关节的逆运动学求解方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201810323427.3A CN108763151A (zh) 2018-04-12 2018-04-12 一种任意三关节的逆运动学求解方法

Publications (1)

Publication Number Publication Date
CN108763151A true CN108763151A (zh) 2018-11-06

Family

ID=63981525

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201810323427.3A Pending CN108763151A (zh) 2018-04-12 2018-04-12 一种任意三关节的逆运动学求解方法

Country Status (2)

Country Link
CN (1) CN108763151A (zh)
WO (1) WO2019196229A1 (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110362785A (zh) * 2019-06-13 2019-10-22 上海交通大学 基于多文件流的任意对角线性方程组并行求解方法和系统

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115741672B (zh) * 2022-10-21 2024-04-19 杭州邦杰星医疗科技有限公司 一种基于刚体变换的dh推导方法

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104866722A (zh) * 2015-05-26 2015-08-26 宁波韦尔德斯凯勒智能科技有限公司 一种七轴工业机械臂的逆动学求解方法
CN106991277A (zh) * 2017-03-21 2017-07-28 山东科技大学 一种任意关系的二阶子问题逆运动学求解方法
CN107203653A (zh) * 2017-04-12 2017-09-26 山东科技大学 一种六自由度串联机器人的逆运动学通用求解方法

Family Cites Families (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US8977394B2 (en) * 2012-12-31 2015-03-10 King Fahd University Of Petroleum And Minerals Control method for mobile parallel manipulators
CN106228260A (zh) * 2016-01-26 2016-12-14 西北工业大学 一种平面三自由度空间机器人逆运动学求解方法
CN106845037B (zh) * 2017-03-21 2018-07-24 山东科技大学 一种五自由度串联机器人的逆运动学通用求解方法

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104866722A (zh) * 2015-05-26 2015-08-26 宁波韦尔德斯凯勒智能科技有限公司 一种七轴工业机械臂的逆动学求解方法
CN106991277A (zh) * 2017-03-21 2017-07-28 山东科技大学 一种任意关系的二阶子问题逆运动学求解方法
CN107203653A (zh) * 2017-04-12 2017-09-26 山东科技大学 一种六自由度串联机器人的逆运动学通用求解方法

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110362785A (zh) * 2019-06-13 2019-10-22 上海交通大学 基于多文件流的任意对角线性方程组并行求解方法和系统

Also Published As

Publication number Publication date
WO2019196229A1 (zh) 2019-10-17

Similar Documents

Publication Publication Date Title
CN107589934B (zh) 一种关节型机械臂逆运动学解析解的求取方法
CN110757454B (zh) 一种双机器人协同旋转的路径规划方法和装置
CN107598919B (zh) 一种基于五点标定法的两轴变位机标定方法
CN107756400B (zh) 一种基于旋量理论的6r机器人逆运动学几何求解方法
CN107203653B (zh) 一种六自由度串联机器人的逆运动学通用求解方法
CN107462154A (zh) 机器人末端工具的位姿测量方法
WO2020020073A1 (zh) 一种球面渐开线直齿锥齿轮副及其齿廓修形方法
CN103481288B (zh) 一种5关节机器人末端工具位姿控制方法
CN106845037B (zh) 一种五自由度串联机器人的逆运动学通用求解方法
CN108763151A (zh) 一种任意三关节的逆运动学求解方法
CN103558808A (zh) 复杂曲面五轴数控加工刀矢的运动学控制方法
CN110722562B (zh) 一种用于机器人参数辨识的空间雅克比矩阵构造方法
CN108621130A (zh) 具有2r1t和2t1r两种运动模式的并联机构
CN107633152A (zh) 基于Creo、ADAMS环境的弧面分度凸轮机构运动学仿真方法
CN107657134A (zh) 一种基于Creo的弧面分度凸轮建模方法
CN104070523B (zh) 基于空间坐标转换的工业机器人实时圆弧插补实现方法
CN111283682A (zh) 一种4-upu四自由度并联机器人正向运动学的几何投影解法
CN109366486B (zh) 柔性机器人逆运动学求解方法、系统、设备、存储介质
CN104608146A (zh) 基于双斜面偏转关节的新型机械臂
CN204487596U (zh) 基于双斜面偏转关节的新型机械臂
CN207415403U (zh) 一种机械臂滑轨
WO2018171467A1 (zh) 一种任意关系的二阶子问题逆运动学求解方法
CN111590570A (zh) 一种同步交叉耦合机器人轮廓控制方法
CN203556931U (zh) 垂直超扁型铣削装置
CN114833848B (zh) 一种刚度约束的机器人铣削加工刀轴矢量和冗余度集成规划方法

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
RJ01 Rejection of invention patent application after publication

Application publication date: 20181106

RJ01 Rejection of invention patent application after publication