CN113722864B - 一种7自由度冗余机械臂逆运动学求解方法及*** - Google Patents

一种7自由度冗余机械臂逆运动学求解方法及*** Download PDF

Info

Publication number
CN113722864B
CN113722864B CN202111069374.5A CN202111069374A CN113722864B CN 113722864 B CN113722864 B CN 113722864B CN 202111069374 A CN202111069374 A CN 202111069374A CN 113722864 B CN113722864 B CN 113722864B
Authority
CN
China
Prior art keywords
joint
equation
coordinate system
row
matrix
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202111069374.5A
Other languages
English (en)
Other versions
CN113722864A (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.)
HRG International Institute for Research and Innovation
Original Assignee
HRG International Institute for Research and Innovation
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by HRG International Institute for Research and Innovation filed Critical HRG International Institute for Research and Innovation
Priority to CN202111069374.5A priority Critical patent/CN113722864B/zh
Publication of CN113722864A publication Critical patent/CN113722864A/zh
Application granted granted Critical
Publication of CN113722864B publication Critical patent/CN113722864B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F30/00—Computer-aided design [CAD]
    • G06F30/10—Geometric CAD
    • G06F30/17—Mechanical parametric or variational design
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/11—Complex mathematical operations for solving equations, e.g. nonlinear equations, general mathematical optimization problems
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F30/00—Computer-aided design [CAD]
    • G06F30/20—Design optimisation, verification or simulation
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02P—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN THE PRODUCTION OR PROCESSING OF GOODS
    • Y02P90/00—Enabling technologies with a potential contribution to greenhouse gas [GHG] emissions mitigation
    • Y02P90/02—Total factory control, e.g. smart factories, flexible manufacturing systems [FMS] or integrated manufacturing systems [IMS]

Landscapes

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

Abstract

本发明公开了一种7自由度冗余机械臂逆运动学求解方法，涉及工业机器人领域，本方法首先根据机械臂构型和参数建立机械臂的D‑H坐标系以及运动学矩阵方程，随后通过代数法求解不构成冗余的关节角度，并根据冗余关节绘制连杆平面图，最后采用固定一个关节的方法，通过代数法和几何法相结合，求解其他冗余关节角度的运动学逆解。本发明极大地较少了7自由度冗余机械臂逆运动学解的个数，有效地提高了求解速度，降低了求解难度。

Description

一种7自由度冗余机械臂逆运动学求解方法及***
技术领域
本发明涉及工业机器人技术领域，具体来说是一种7自由度冗余机械臂逆运动学求解方法及***。
背景技术
机械臂的逆运动学求解问题旨在根据已知的机械臂末端坐标系相对于基坐标系的位姿以及各个连杆参数的情况下，求解各个关节的角度，逆运动学是工业机器人应用研究的基础，在机械臂的遥操作、轨迹规划、实时控制以及防碰撞等问题中起着非常重要的作用。现有的逆运动学求解方法主要分为两大类，封闭解法与数值解法，由于数值解法的迭代性原理，数值解法的求解速度要比封闭解法慢很多，因此现有的工业机器人的逆运动学求解大多采用封闭解法进行求解，封闭解法又可以分为代数法和几何法两种方法。
相较于6自由度机械臂，7自由度机械臂的逆运动学求解较为困难，由于多了1个冗余自由度的存在，使得机械臂的理论运动学逆解有无数个，计算极为复杂，极大地增加了计算时间。
申请号为202011075243.3公开的一种基于位置级逆运动学的七自由度机械臂限位优化方法，它涉及一种七自由度机械臂限位优化方法。该发明为了解决现有的数值解无法得到封闭解，存在终态自运动；解析解无法针对偏置构型，存在无法实现对运动优化的问题。该发明首先基于固定某一关节角的参数化求解方法，得到7自由度机械臂逆运动学的解析解；然后将该固定的关节角度参数作为输入，关节限位作为优化指标，建立最优控制问题；再基于拉格朗日乘子法，将有约束问题转化为无约束问题；最后基于牛顿迭代法实现对最优的关节角参数的求解，通过给定初始构型、期望末端位姿和笛卡尔的路径规划，得到考虑关节限位优化的7个关节空间轨迹。虽然该发明采用固定其中一个关节实现最优控制，但提供的具体算法并不适用本发明所提供的7自由度机械臂构型。
发明内容
本发明所要解决的技术问题在于现有技术中关于7自由度冗余机械臂的逆运动学求解过程中逆解太多，计算速度慢。
本发明通过以下技术手段实现解决上述技术问题的：
一种7自由度冗余机械臂逆运动学求解方法，7自由度冗余机械臂的结构为：第1关节坐标系x1z1y1与基坐标系x0z0y0重合，第2关节坐标系x2z2y2由第一关节坐标系绕x1轴旋转90°，再绕z1转动90°，并沿x1轴移动a1，第3关节坐标系x3z3y3由第2关节坐标系绕z2轴旋转90°，并沿x2轴移动a2，第4关节坐标系x4z4y4由第3关节沿x3轴移动a3，第5关节坐标系x5z5y5由第4关节沿x4轴移动a4，第6关节坐标系由第5关节坐标系沿x轴转90°，再沿z轴转90°，并沿x5移动a5，沿y5移动-d6，第7关节坐标系由第5关节坐标系绕x轴转动90°；所有关节都以各自的z轴为旋转中心转动；方法包括以下步骤：
S1、根据机械臂构型和参数建立D-H坐标系及运动学矩阵；
S2、采用代数法求解机械臂不构成冗余的关节角度；
S3、根据机械臂的冗余关节绘制连杆平面图；
S4、固定一个冗余关节，令其角度为已知量；
S5、采用几何法和代数法相关结合的方法求解冗余关节的运动学逆解。
本方法首先根据机械臂构型和参数建立机械臂的D-H坐标系以及运动学矩阵方程，随后通过代数法求解不构成冗余的关节角度，并根据冗余关节绘制连杆平面图，最后采用固定一个关节的方法，通过代数法和几何法相结合，求解其他冗余关节角度的运动学逆解。本发明极大地较少了7自由度冗余机械臂逆运动学解的个数，有效地提高了求解速度，降低了求解难度。
进一步的，所述S1的求解方法包括以下步骤：
S11、根据连杆坐标系之间的其次变换方程可以得出末端坐标系相对于基坐标系的齐次坐标变换矩阵满足公式(1)：
其中，表示相邻两连杆坐标系的其次坐标变换矩阵；所述连杆为相邻两个关节之间的连接件；其中，n，o，a代表的是末端关节坐标系相对于极坐标系的姿态，p代表的是末端关节坐标系相对于极坐标系的位置。
进一步的，所述S2的求解方法包括以下步骤：
S21、首先在等式(1)两端同时左乘得：
根据等式(2)两端矩阵第2行第4列元素相等可建立方程：
-s1px+c1py＝0(3)
根据等式(3)可以得到第1个关节角θ1有两个解，分别为：Atan2(py,px)和Atan2(-py,-px)；
根据等式(2)两端矩阵第3行第2列元素相等可建立方程：
c6＝-s1ax+c1ay＝k1(4)
由式(4)可得第6个关节角θ6的值由两个解，分别为由于θ1有两个解，可知θ6的值有4个解；
随后在等式(2)两端同时右端乘可得：
根据等式(5)两端矩阵第1行第1列元素以及第3行第3列元素相等，可得：
(nx+c1c6oz)c7+(c1c6nz-ox)s7＝-s1s6(6)
令nx+c1c6oz＝k2，c1c6nz-ox＝k3，则可得第7个关节角θ7的解为
进一步的，所述S3的求解方法包括以下步骤：
S31、根据连杆平面图可以得到lOA＝a2，lAB＝a3，lBC＝a4，D点坐标为
进一步的，本发明固定第5关节的关节角；所述S5的求解方法包括以下步骤：
S51、在等式(1)两端同时右乘可得：
令B点在基坐标系中的坐标为(4px，4py，4pz)，可得到B点的坐标为由此可得/>则/>可得到第3个关节角θ3的解为/>
S52、根据等式(7)两端矩阵第1行第3列元素以及第2行第3列元素相等，可得：
4px＝a1c1+a2c1c2+a3c1c23(8)
令k5＝a2c1+a3c1c3,k6＝-a3c1s3,k7＝4px-a1c1，可得：
k7＝k5c2+k6s2(9)
由式(9)可得到第2个关节角
S53、根据等式(7)矩阵第1行第3列元素以及第3行第3列元素相等，可得：
s234＝-(c5c6c7-s5s7)nz+(c5c6s7+s5c7)oz+(c5s6)az(10)
c234＝(s5c6c7+c5s7)nz-(-s5c6s7+s5c7)oz-(s5s6)az(11)
θ234＝atan2(s234,c234)(12)
由式(12)可得到第4个关节角θ4＝θ234-θ2-θ3。
与上述方法对应的，本发明还提供一种7自由度冗余机械臂逆运动学求解***，包括：
D-H坐标系和运动学矩阵建立模块，用以根据机械臂构型和参数建立D-H坐标系及运动学矩阵；
关节角度求解模块，用以采用代数法求解机械臂不构成冗余的关节角度；
连杆平面图绘制模块，用以根据机械臂的冗余关节绘制连杆平面图；
冗余关节固定模块，用以固定一个冗余关节，令其角度为已知量；
计算模块，用以采用几何法和代数法相关结合的方法求解冗余关节的运动学逆解。
进一步的，所述D-H坐标系和运动学矩阵建立模块的求解方法包括以下步骤：
S11、根据连杆坐标系之间的其次变换方程可以得出末端坐标系相对于基坐标系的齐次坐标变换矩阵满足公式(1)：
其中，表示相邻两连杆坐标系的其次坐标变换矩阵；所述连杆为相邻两个关节之间的连接件；n，o，a代表的是末端关节坐标系相对于极坐标系的姿态，p代表的是末端关节坐标系相对于极坐标系的位置。
进一步的，所述关节角度求解模块的求解方法包括以下步骤：
S21、首先在等式(1)两端同时左乘得：
根据等式(2)两端矩阵第2行第4列元素相等可建立方程：
-s1px+c1py＝0(3)
根据等式(3)可以得到第1个关节角θ1有两个解，分别为：Atan2(py,px)和Atan2(-py,-px)；
根据等式(2)两端矩阵第3行第2列元素相等可建立方程：
c6＝-s1ax+c1ay＝k1(4)
由式(4)可得第6个关节角θ6的值由两个解，分别为由于θ1有两个解，可知θ6的值有4个解；
随后在等式(2)两端同时右端乘可得：
根据等式(5)两端矩阵第1行第1列元素以及第3行第3列元素相等，可得：
(nx+c1c6oz)c7+(c1c6nz-ox)s7＝-s1s6(6)
令nx+c1c6oz＝k2，c1c6nz-ox＝k3，则可得第7个关节角θ7的解为
进一步的，所述连杆平面图绘制模块的求解方法包括以下步骤：
S31、根据连杆平面图可以得到lOA＝a2，lAB＝a3，lBC＝a4，D点坐标为
进一步的，本发明固定第5关节的关节角；所述计算模块的求解方法包括以下步骤：
S51、在等式(1)两端同时右乘可得：
令B点在基坐标系中的坐标为(4px，4py，4pz)，可得到B点的坐标为由此可得/>则/>可得到第3个关节角θ3的解为/>
S52、根据等式(7)两端矩阵第1行第3列元素以及第2行第3列元素相等，可得：
4px＝a1c1+a2c1c2+a3c1c23(8)
令k5＝a2c1+a3c1c3,k6＝-a3c1s3,k7＝4px-a1c1，可得：
k7＝k5c2+k6s2(9)
由式(9)可得到第2个关节角
S53、根据等式(7)矩阵第1行第3列元素以及第3行第3列元素相等，可得：
s234＝-(c5c6c7-s5s7)nz+(c5c6s7+s5c7)oz+(c5s6)az(10)
c234＝(s5c6c7+c5s7)nz-(-s5c6s7+s5c7)oz-(s5s6)az(11)
θ234＝atan2(s234,c234)(12)
由式(12)可得到第4个关节角θ4＝θ234-θ2-θ3。
本发明的优点在于：
本方法首先根据机械臂构型和参数建立机械臂的D-H坐标系以及运动学矩阵方程，随后通过代数法求解不构成冗余的关节角度，并根据冗余关节绘制连杆平面图，最后采用固定一个关节的方法，通过代数法和几何法相结合，求解其他冗余关节角度的运动学逆解。本发明极大地较少了7自由度冗余机械臂逆运动学解的个数，有效地提高了求解速度，降低了求解难度。
本发明通过固定第5个关节的原理在于尽量选择对其他关节影响较小的关节进行固定。采用代数法和几何法，能得到每个位置的解析解。
附图说明
图1为本发明实施例中7自由度冗余机械臂的结构示意图；
图2为本发明实施例中7自由度冗余机械臂的坐标图；
图3为本发明实施例中7自由度冗余机械臂运动学逆解的流程图；
图4为本发明实施例中7自由度冗余机械臂冗余关节连杆平面图。
图中，1、回转底座；2、大臂关节；3、二臂关节；4、三臂关节；5、摆动臂转动关节；6、摆动臂摆动关节；7、末端回转关节；a1、沿x1轴，从z1移动到z2的距离；a2、沿x2轴，从z2移动到z3的距离；a3、沿x3轴，从z3移动到z4的距离；a4、沿x4轴，从x4移动到z5的距离；a5、沿x5轴，从z5移动到z6的距离；d6、沿z6轴，从x5移动到x6的距离；θ2、绕z2轴，从x1旋转到x2的角度；θ3、绕z3轴，从x2旋转到x3的角度；θ4、绕z4轴，从x3旋转到x4的角度；θ5、绕z5轴，从x4旋转到x5的角度。
具体实施方式
为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
如图1、图2所示，该7自由度冗余机械臂由回转底座1、大臂关节2、二臂关节3、三臂关节4、摆动臂转动关节5、摆动臂摆动关节6、末端回转关节7依次转动连接组成；第1关节坐标系x1z1y1与基坐标系x0z0y0重合，第2关节坐标系x2z2y2由第一关节坐标系绕x1轴旋转90°，再绕z1转动90°，并沿x1轴移动a1，第3关节坐标系x3z3y3由第2关节坐标系绕z2轴旋转90°，并沿x2轴移动a2，第4关节坐标系x4z4y4由第3关节沿x3轴移动a3，第5关节坐标系x5z5y5由第4关节沿x4轴移动a4，第6关节坐标系由第5关节坐标系沿x轴转90°，再沿z轴转90°，并沿x5移动a5，沿y5移动-d6，第7关节坐标系由第5关节坐标系绕x轴转动90°。该机械臂主要参数如表1所示。
表1 7自由度冗余机械臂具体参数
如图3所示，该7自由度冗余机械臂逆运动学求解方法，包括以下步骤：
S1、根据机械臂构型和参数建立D-H坐标系及运动学矩阵；
S2、采用代数法求解机械臂不构成冗余的关节角度；
S3、根据机械臂的冗余关节绘制连杆平面图；
S4、固定一个冗余关节，令其角度为已知量；
S5、采用几何法和代数法相关结合的方法求解冗余关节的运动学逆解。
如图4所示，该7自由度冗余机械臂冗余关节连杆平面图的建立方法为，将大臂关节2、二臂关节3、三臂关节4、摆动臂转动关节5简化为一条直线，并以坐标系X2O2Y2为原点，其中lOA＝a2，lAB＝a3，lBC＝a4，D点坐标为/>
工作过程：根据连杆坐标系之间的其次变换方程可以得出末端坐标系相对于基坐标系的齐次坐标变换矩阵满足公式(1)：
其中，表示相邻两连杆坐标系的其次坐标变换矩阵；n，o，a代表的是末端关节坐标系相对于极坐标系的姿态，p代表的是末端关节坐标系相对于极坐标系的位置。
首先在等式(1)两端同时左乘得：
根据等式(2)两端矩阵第2行第4列元素相等可建立方程：
-s1px+c1py＝0(3)
根据等式(3)可以得到第1个关节角θ1有两个解，分别为：Atan2(py,px)和Atan2(-py,-px)；s表示sin，c表示cos。
根据等式(2)两端矩阵第3行第2列元素相等可建立方程：
c6＝-s1ax+c1ay＝k1(4)
由式(4)可得第6个关节角θ6的值由两个解，分别为由于θ1有两个解，可知θ6的值有4个解。
随后在等式(2)两端同时右端乘可得：
根据等式(5)两端矩阵第1行第1列元素以及第3行第3列元素相等，可得：
(nx+c1c6oz)c7+(c1c6nz-ox)s7＝-s1s6(6)
令nx+c1c6oz＝k2，c1c6nz-ox＝k3，则可得第7个关节角θ7的解为
选择固定第5关节的关节角，并设θ5＝90°。
在等式(1)两端同时右乘可得：
令B点在基坐标系中的坐标为(4px，4py，4pz)，可得到B点的坐标为由此可得/>则/>可得到第3个关节角θ3的解为/>
令等式(7)两端矩阵第1行第3列元素以及第2行第3列元素相等，可得：
4px＝a1c1+a2c1c2+a3c1c23(8)
令k5＝a2c1+a3c1c3,k6＝-a3c1s3,k7＝4px-a1c1，可得：
k7＝k5c2+k6s2(9)
由式(9)可得到第2个关节角
令等式(7)矩阵第1行第3列元素以及第3行第3列元素相等，可得：
s234＝-(c5c6c7-s5s7)nz+(c5c6s7+s5c7)oz+(c5s6)az(10)
c234＝(s5c6c7+c5s7)nz-(-s5c6s7+s5c7)oz-(s5s6)az(11)
θ234＝atan2(s234,c234)(12)
由式(12)可得到第4个关节角θ4＝θ234-θ2-θ3。
与上述方法对应的，本实施例还提供一种7自由度冗余机械臂逆运动学求解***，包括以下步骤：
D-H坐标系和运动学矩阵建立模块，用以根据机械臂构型和参数建立D-H坐标系及运动学矩阵；
关节角度求解模块，用以采用代数法求解机械臂不构成冗余的关节角度；
连杆平面图绘制模块，用以根据机械臂的冗余关节绘制连杆平面图；
冗余关节固定模块，用以固定一个冗余关节，令其角度为已知量；
计算模块，用以采用几何法和代数法相关结合的方法求解冗余关节的运动学逆解。
如图4所示，该7自由度冗余机械臂冗余关节连杆平面图的建立方法为，将大臂关节2、二臂关节3、三臂关节4、摆动臂转动关节5简化为一条直线，并以坐标系X2O2Y2为原点，其中lOA＝a2，lAB＝a3，lBC＝a4，D点坐标为/>
工作过程：根据连杆坐标系之间的其次变换方程可以得出末端坐标系相对于基坐标系的齐次坐标变换矩阵满足公式(1)：
其中，表示相邻两连杆坐标系的其次坐标变换矩阵。
首先在等式(1)两端同时左乘得：
根据等式(2)两端矩阵第2行第4列元素相等可建立方程：
-s1px+c1py＝0(3)
根据等式(3)可以得到第1个关节角θ1有两个解，分别为：Atan2(py,px)和Atan2(-py,-px)；
根据等式(2)两端矩阵第3行第2列元素相等可建立方程：
c6＝-s1ax+c1ay＝k1(4)
由式(4)可得第6个关节角θ6的值由两个解，分别为由于θ1有两个解，可知θ6的值有4个解。
随后在等式(2)两端同时右端乘可得：
根据等式(5)两端矩阵第1行第1列元素以及第3行第3列元素相等，可得：
(nx+c1c6oz)c7+(c1c6nz-ox)s7＝-s1s6(6)
令nx+c1c6oz＝k2，c1c6nz-ox＝k3，则可得第7个关节角θ7的解为
选择固定第5关节的关节角，并设θ5＝90°。
在等式(1)两端同时右乘可得：
令B点在基坐标系中的坐标为(4px，4py，4pz)，可得到B点的坐标为由此可得/>则/>可得到第3个关节角θ3的解为/>
令等式(7)两端矩阵第1行第3列元素以及第2行第3列元素相等，可得：
4px＝a1c1+a2c1c2+a3c1c23(8)
令k5＝a2c1+a3c1c3,k6＝-a3c1s3,k7＝4px-a1c1，可得：
k7＝k5c2+k6s2(9)
由式(9)可得到第2个关节角
令等式(7)矩阵第1行第3列元素以及第3行第3列元素相等，可得：
s234＝-(c5c6c7-s5s7)nz+(c5c6s7+s5c7)oz+(c5s6)az(10)
c234＝(s5c6c7+c5s7)nz-(-s5c6s7+s5c7)oz-(s5s6)az(11)
θ234＝atan2(s234,c234)(12)
由式(12)可得到第4个关节角θ4＝θ234-θ2-θ3。
以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。

Claims (6)

1.一种7自由度冗余机械臂逆运动学求解方法，其特征在于，7自由度冗余机械臂的结构为：第1关节坐标系x1z1y1与基坐标系x0z0y0重合，第2关节坐标系x2z2y2由第一关节坐标系绕x1轴旋转90°，再绕z1转动90°，并沿x1轴移动a1，第3关节坐标系x3z3y3由第2关节坐标系绕z2轴旋转90°，并沿x2轴移动a2，第4关节坐标系x4z4y4由第3关节沿x3轴移动a3，第5关节坐标系x5z5y5由第4关节沿x4轴移动a4，第6关节坐标系由第5关节坐标系沿x轴转90°，再沿z轴转90°，并沿x5移动a5，沿y5移动-d6，第7关节坐标系由第5关节坐标系绕x轴转动90°；所有关节都以各自的z轴为旋转中心转动；方法包括以下步骤：
S1、根据机械臂构型和参数建立D-H坐标系及运动学矩阵；
S2、采用代数法求解机械臂不构成冗余的关节角度；
S3、根据机械臂的冗余关节绘制连杆平面图；
S4、固定一个冗余关节，令其角度为已知量；
S5、采用几何法和代数法相关结合的方法求解冗余关节的运动学逆解；
所述S1的求解方法包括以下步骤：
S11、根据连杆坐标系之间的其次变换方程可以得出末端坐标系相对于基坐标系的齐次坐标变换矩阵满足公式(1)：
其中，表示相邻两连杆坐标系的其次坐标变换矩阵；所述连杆为相邻两个关节之间的连接件；
固定第5关节的关节角；所述S5的求解方法包括以下步骤：
S51、在等式(1)两端同时右乘可得：
令B点在基坐标系中的坐标为(4px，4py，4pz)，可得到B点的坐标为由此可得/>则/>可得到第3个关节角θ3的解为
S52、根据等式(7)两端矩阵第1行第3列元素以及第2行第3列元素相等，可得：
4px＝a1c1+a2c1c2+a3c1c23(8)
令k5＝a2c1+a3c1c3,k6＝-a3c1s3,k7＝4px-a1c1，可得：
k7＝k5c2+k6s2(9)
由式(9)可得到第2个关节角
S53、根据等式(7)矩阵第1行第3列元素以及第3行第3列元素相等，可得：
s234＝-(c5c6c7-s5s7)nz+(c5c6s7+s5c7)oz+(c5s6)az(10)
c234＝(s5c6c7+c5s7)nz-(-s5c6s7+s5c7)oz-(s5s6)az(11)
θ234＝atan2(s234,c234)(12)
由式(12)可得到第4个关节角θ4＝θ234-θ2-θ3。
2.根据权利要求1所述的7自由度冗余机械臂逆运动学求解方法，其特征在于，所述S2的求解方法包括以下步骤：
S21、首先在等式(1)两端同时左乘得：
根据等式(2)两端矩阵第2行第4列元素相等可建立方程：
-s1px+c1py＝0(3)
根据等式(3)可以得到第1个关节角θ1有两个解，分别为：Atan2(py,px)和Atan2(-py,-px)；
根据等式(2)两端矩阵第3行第2列元素相等可建立方程：
c6＝-s1ax+c1ay＝k1(4)
由式(4)可得第6个关节角θ6的值由两个解，分别为由于θ1有两个解，可知θ6的值有4个解；
随后在等式(2)两端同时右端乘可得：
根据等式(5)两端矩阵第1行第1列元素以及第3行第3列元素相等，可得：
(nx+c1c6oz)c7+(c1c6nz-ox)s7＝-s1s6(6)
令nx+c1c6oz＝k2，c1c6nz-ox＝k3，则可得第7个关节角θ7的解为
3.根据权利要求2所述的7自由度冗余机械臂逆运动学求解方法，其特征在于，所述S3的求解方法包括以下步骤：
S31、根据连杆平面图可以得到lOA＝a2，lAB＝a3，lBC＝a4，D点坐标为
4.一种7自由度冗余机械臂逆运动学求解***，其特征在于，包括：
D-H坐标系和运动学矩阵建立模块，用以根据机械臂构型和参数建立D-H坐标系及运动学矩阵；
关节角度求解模块，用以采用代数法求解机械臂不构成冗余的关节角度；
连杆平面图绘制模块，用以根据机械臂的冗余关节绘制连杆平面图；
冗余关节固定模块，用以固定一个冗余关节，令其角度为已知量；
计算模块，用以采用几何法和代数法相关结合的方法求解冗余关节的运动学逆解；
所述D-H坐标系和运动学矩阵建立模块的求解方法包括以下步骤：
S11、根据连杆坐标系之间的其次变换方程可以得出末端坐标系相对于基坐标系的齐次坐标变换矩阵满足公式(1)：
其中，表示相邻两连杆坐标系的其次坐标变换矩阵；所述连杆为相邻两个关节之间的连接件；
固定第5关节的关节角；所述计算模块的求解方法包括以下步骤：
S51、在等式(1)两端同时右乘可得：
令B点在基坐标系中的坐标为(4px，4py，4pz)，可得到B点的坐标为由此可得/>则/>可得到第3个关节角θ3的解为
S52、根据等式(7)两端矩阵第1行第3列元素以及第2行第3列元素相等，可得：
4px＝a1c1+a2c1c2+a3c1c23(8)
令k5＝a2c1+a3c1c3,k6＝-a3c1s3,k7＝4px-a1c1，可得：
k7＝k5c2+k6s2(9)
由式(9)可得到第2个关节角
S53、根据等式(7)矩阵第1行第3列元素以及第3行第3列元素相等，可得：
s234＝-(c5c6c7-s5s7)nz+(c5c6s7+s5c7)oz+(c5s6)az(10)
c234＝(s5c6c7+c5s7)nz-(-s5c6s7+s5c7)oz-(s5s6)az(11)
θ234＝atan2(s234,c234)(12)
由式(12)可得到第4个关节角θ4＝θ234-θ2-θ3。
5.根据权利要求4所述的7自由度冗余机械臂逆运动学求解***，其特征在于，所述关节角度求解模块的求解方法包括以下步骤：
S21、首先在等式(1)两端同时左乘得：
根据等式(2)两端矩阵第2行第4列元素相等可建立方程：
-s1px+c1py＝0(3)
根据等式(3)可以得到第1个关节角θ1有两个解，分别为：Atan2(py,px)和Atan2(-py,-px)；
根据等式(2)两端矩阵第3行第2列元素相等可建立方程：
c6＝-s1ax+c1ay＝k1(4)
由式(4)可得第6个关节角θ6的值由两个解，分别为由于θ1有两个解，可知θ6的值有4个解；
随后在等式(2)两端同时右端乘可得：
根据等式(5)两端矩阵第1行第1列元素以及第3行第3列元素相等，可得：
(nx+c1c6oz)c7+(c1c6nz-ox)s7＝-s1s6(6)
令nx+c1c6oz＝k2，c1c6nz-ox＝k3，则可得第7个关节角θ7的解为
6.根据权利要求5所述的7自由度冗余机械臂逆运动学求解***，其特征在于，所述连杆平面图绘制模块的求解方法包括以下步骤：
S31、根据连杆平面图可以得到lOA＝a2，lAB＝a3，lBC＝a4，D点坐标为
CN202111069374.5A 2021-09-13 2021-09-13 一种7自由度冗余机械臂逆运动学求解方法及*** Active CN113722864B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111069374.5A CN113722864B (zh) 2021-09-13 2021-09-13 一种7自由度冗余机械臂逆运动学求解方法及***

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111069374.5A CN113722864B (zh) 2021-09-13 2021-09-13 一种7自由度冗余机械臂逆运动学求解方法及***

Publications (2)

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

Family

ID=78683579

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111069374.5A Active CN113722864B (zh) 2021-09-13 2021-09-13 一种7自由度冗余机械臂逆运动学求解方法及***

Country Status (1)

Country Link
CN (1) CN113722864B (zh)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114714335B (zh) * 2022-03-09 2023-07-25 泉州华中科技大学智能制造研究院 一种七关节冗余自由度机器人的逆解方法及装置
CN117959146B (zh) * 2024-04-02 2024-06-25 江西求是高等研究院 一种六自由度的上肢康复机器人控制方法及***

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2012007014A1 (en) * 2010-07-13 2012-01-19 Abdallah Ezzat Abdallah Abozaied Joint
CN107589934A (zh) * 2017-07-24 2018-01-16 大连理工大学 一种关节型机械臂逆运动学解析解的求取方法
CN111844005A (zh) * 2020-07-08 2020-10-30 哈尔滨工业大学 一种应用于隧道湿喷的2r-p-2r-p-2r机械臂运动规划方法
CN111914980A (zh) * 2020-05-30 2020-11-10 中南大学 一种基于解析解和改进遗传算法的冗余机械臂运动学解算方法
CN112223291A (zh) * 2020-10-21 2021-01-15 哈工大机器人（合肥）国际创新研究院 一种基于三维任务空间约束的机械臂避障方法及装置
CN112536792A (zh) * 2020-11-24 2021-03-23 河南理工大学 一种基于球面几何解析法的机器臂逆解方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2012007014A1 (en) * 2010-07-13 2012-01-19 Abdallah Ezzat Abdallah Abozaied Joint
CN107589934A (zh) * 2017-07-24 2018-01-16 大连理工大学 一种关节型机械臂逆运动学解析解的求取方法
CN111914980A (zh) * 2020-05-30 2020-11-10 中南大学 一种基于解析解和改进遗传算法的冗余机械臂运动学解算方法
CN111844005A (zh) * 2020-07-08 2020-10-30 哈尔滨工业大学 一种应用于隧道湿喷的2r-p-2r-p-2r机械臂运动规划方法
CN112223291A (zh) * 2020-10-21 2021-01-15 哈工大机器人（合肥）国际创新研究院 一种基于三维任务空间约束的机械臂避障方法及装置
CN112536792A (zh) * 2020-11-24 2021-03-23 河南理工大学 一种基于球面几何解析法的机器臂逆解方法

Non-Patent Citations (1)

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

Also Published As

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

Similar Documents

Publication Publication Date Title
CN107589934B (zh) 一种关节型机械臂逆运动学解析解的求取方法
CN113722864B (zh) 一种7自由度冗余机械臂逆运动学求解方法及***
Singh et al. Forward and inverse kinematic analysis of robotic manipulators
Duka Neural network based inverse kinematics solution for trajectory tracking of a robotic arm
CN107584474B (zh) 一种基于几何投影的球面关节双臂机器人协调运动方法
CN108241339A (zh) 仿人机械臂的运动求解和构型控制方法
WO2021184655A1 (zh) 一种超冗余机械臂末端轨迹运动规划方法
US9321175B2 (en) Robotic manipulator articulation techniques
CN108908347B (zh) 一种面向冗余移动机械臂容错型重复运动规划方法
CN107791248B (zh) 基于不满足Pieper准则的六自由度串联机器人的控制方法
Zheng et al. Kinematics analysis and trajectory planning of 6-DOF robot
CN111791234A (zh) 一种狭窄空间内多机器人空间工作位置防撞控制算法
Zhang et al. Inverse kinematics of modular Cable-driven Snake-like Robots with flexible backbones
Yang et al. General inverse kinematics method for 7-DOF offset manipulators based on arm angle parameterization
CN114589701A (zh) 一种基于阻尼最小二乘的多关节机械臂避障逆运动学方法
CN116330267A (zh) 一种基于工业机器人腕部奇异点计算的控制方法
Valsamos et al. Introduction of the high performance area measure for the evaluation of metamorphic manipulator anatomies
CN111515954A (zh) 一种机械臂高质量运动路径生成方法
Andrade et al. A comparative analysis of kinematics of industrial robot KUKA KR 60–3 using scientific computing languages
CN117325143A (zh) 一种冗余机械臂奇异位型下运动学优化方法
Wang et al. Kinematics solution of snake-like manipulator based on improved backbone mode method
Sharma et al. Generalized unified closed form inverse kinematics for mobile manipulators with reusable redundancy parameters
JP2019093487A (ja) ロボットの制御装置及びロボットの逆変換処理方法
Qizhi et al. On the kinematics analysis and motion planning of the manipulator of a mobile robot
Zhang et al. A new analytical inverse kinematics model for seven degrees of freedom redundant manipulators

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