CN114147720A - 一种多自由度机械臂的逆运动学通用求解方法及装置 - Google Patents

一种多自由度机械臂的逆运动学通用求解方法及装置 Download PDF

Info

Publication number
CN114147720A
CN114147720A CN202111521893.0A CN202111521893A CN114147720A CN 114147720 A CN114147720 A CN 114147720A CN 202111521893 A CN202111521893 A CN 202111521893A CN 114147720 A CN114147720 A CN 114147720A
Authority
CN
China
Prior art keywords
freedom
mechanical arm
degree
angle
steps
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
CN202111521893.0A
Other languages
English (en)
Other versions
CN114147720B (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.)
Hangzhou Haixi Technology Co.,Ltd.
Original Assignee
Hangzhou Ruiwo Robot Technology Co ltd
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 Hangzhou Ruiwo Robot Technology Co ltd filed Critical Hangzhou Ruiwo Robot Technology Co ltd
Priority to CN202111521893.0A priority Critical patent/CN114147720B/zh
Publication of CN114147720A publication Critical patent/CN114147720A/zh
Application granted granted Critical
Publication of CN114147720B publication Critical patent/CN114147720B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

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/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
    • 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/1607—Calculation of inertia, jacobian matrixes and inverses
    • 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/161—Hardware, e.g. neural networks, fuzzy logic, interfaces, processor
    • 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)
  • Mathematical Physics (AREA)
  • Automation & Control Theory (AREA)
  • General Physics & Mathematics (AREA)
  • Mathematical Analysis (AREA)
  • Theoretical Computer Science (AREA)
  • Mechanical Engineering (AREA)
  • Pure & Applied Mathematics (AREA)
  • Robotics (AREA)
  • Data Mining & Analysis (AREA)
  • Computational Mathematics (AREA)
  • Software Systems (AREA)
  • Mathematical Optimization (AREA)
  • General Engineering & Computer Science (AREA)
  • Databases & Information Systems (AREA)
  • Algebra (AREA)
  • Computing Systems (AREA)
  • Artificial Intelligence (AREA)
  • Evolutionary Computation (AREA)
  • Fuzzy Systems (AREA)
  • Numerical Control (AREA)
  • Manipulator (AREA)

Abstract

本发明涉及一种多自由度机械臂的逆运动学通用求解方法及装置，使用指数积方法建立机械臂末端当前及目标状态下的齐次变换矩阵；定义机械臂当前各自由度角度；使用牛顿‑拉弗森方法，通过比较机械臂末端运动旋量对机械臂各自由度角度进行迭代，从而求出机械臂的运动学逆解；本方法空间构型简洁、末端控制精度高、有利于动力学模型建立，实现了多自由度机械臂的逆运动学通用求解。

Description

一种多自由度机械臂的逆运动学通用求解方法及装置
技术领域
本发明涉及一种机器人逆运动学通用求解方法，具体来说是一种多自由度机械臂的逆运动学通用求解方法及装置。
背景技术
随着机器人行业的发展，机械臂作为机器人行业至关重要的一部分，在未来将占据越来越重要的作用；机械臂的运动逆解，是当机械臂末端处于某一点及某一姿态，求出机械臂每个自由度旋转角度的一种方法；机械臂的运动学逆解是机器人学中最基础也是最重要的一部分，它直接关系到机器人的运动控制、路径规划等一系列问题。目前传统机器人行业中广泛应用的机械臂逆解方法是以D-H参数法为基础的解析法，该方法实时性较高，但是其计算过程相当复杂；解析法与机械臂的自由度数量和构型关系较大，如研发过程中改变结构构型或增减自由度数量，则需重新进行计算。
发明内容
为了克服目前已有的逆解方法复杂性、通用性等不足，本发明提供了一种适用于多自由度机械臂的通用逆运动学通用求解方法。
本发明所采用的技术方案是：
提供一种多自由度机械臂的逆运动学通用求解方法，其特征在于：包括以下步骤：
1）将所述机械臂各关节自由度当前角度
Figure 694007DEST_PATH_IMAGE002
作为初始迭代角度，计算所述机械臂当 前状态下的末端齐次变换矩阵
Figure 830590DEST_PATH_IMAGE004
及运动至目标位置
Figure 686419DEST_PATH_IMAGE005
的运动旋量
Figure 883045DEST_PATH_IMAGE006
；
2）判断所述运动旋量
Figure 824325DEST_PATH_IMAGE006
是否小于等于误差
Figure 217129DEST_PATH_IMAGE007
，如果是，此时的关节自由度角度即为 所述机械臂的逆运动学解，计算终止；如果否，则进入步骤3）;
3）计算所述机械臂各关节的下一迭代自由度角度
Figure 678198DEST_PATH_IMAGE008
，将新各自由度角度
Figure 294993DEST_PATH_IMAGE008
带入 步骤1）和2），进行迭代计算，直到其满足计算终止条件。
进一步地，所述步骤1）还包括以下步骤：
1.1）利用指数积方法表示空间构型，在三维空间笛卡尔坐标系下构建各自由度的 旋转轴矩阵
Figure 474301DEST_PATH_IMAGE009
，构建末端正解
Figure 936376DEST_PATH_IMAGE010
的表达公式。
进一步地，所述步骤1.1）还包括以下步骤：
1.1.1） 利用指数积方法表示空间构型，在三维空间笛卡尔坐标系下构建各自由 度的旋转轴矩阵；
Figure 517530DEST_PATH_IMAGE011
自由度机械臂的单位旋转轴矩阵为：
Figure 39647DEST_PATH_IMAGE012
其中
Figure 440672DEST_PATH_IMAGE014
为
Figure 972017DEST_PATH_IMAGE011
自由度机械臂各自由度旋转轴单位角速度在三维空间下的表示，
Figure 142098DEST_PATH_IMAGE015
为
Figure 648166DEST_PATH_IMAGE011
自由度机械臂各自由度旋转轴单位线速度在三维空间下的表示；
1.1.2） 通过指数积法表示空间构型，机械臂末端正解公式为：
Figure 989017DEST_PATH_IMAGE016
其中
Figure 605943DEST_PATH_IMAGE018
为机械臂初始状态下末端的齐次变换矩阵。
进一步地，所述步骤1）还包括以下步骤：
1.2） 令
Figure 145378DEST_PATH_IMAGE020
为机械臂各关节自由度当前角度，通过上述步骤1.1.2）得到机械臂末端 当前状态下的齐次变换矩阵
Figure 25609DEST_PATH_IMAGE022
。
进一步地，所述步骤1）还包括以下步骤：
1.3）输入机械臂末端目标状态下的齐次变换矩阵
Figure 650494DEST_PATH_IMAGE023
，计算机械臂从
Figure 274374DEST_PATH_IMAGE024
至
Figure 668315DEST_PATH_IMAGE023
的运动 旋量
Figure 195415DEST_PATH_IMAGE025
；
将机械臂正解对时间求导得到
Figure 58329DEST_PATH_IMAGE026
为：
Figure 735167DEST_PATH_IMAGE027
将机械臂正解求逆得到
Figure 203188DEST_PATH_IMAGE028
为：
Figure 408910DEST_PATH_IMAGE029
其运动旋量
Figure 759120DEST_PATH_IMAGE030
为：
Figure 974070DEST_PATH_IMAGE031
其中
Figure 93336DEST_PATH_IMAGE032
为机械臂第
Figure 735538DEST_PATH_IMAGE033
个自由度的单位旋转矩阵,
Figure 307465DEST_PATH_IMAGE034
为第
Figure 60526DEST_PATH_IMAGE036
次迭代的第
Figure 299878DEST_PATH_IMAGE037
个自由度 的旋转角度,
Figure 909720DEST_PATH_IMAGE038
为第
Figure 968942DEST_PATH_IMAGE036
次迭代的第
Figure 525695DEST_PATH_IMAGE033
个自由度的旋转速度。
进一步地，所述步骤3）还包括以下步骤：
3.1） 计算机械臂各自由度的空间雅可比矩阵
Figure 619553DEST_PATH_IMAGE039
，表示为关于
Figure 603558DEST_PATH_IMAGE020
的函数：
Figure 150077DEST_PATH_IMAGE040
3.2） 其空间旋量
Figure 979362DEST_PATH_IMAGE025
表示为：
Figure 927726DEST_PATH_IMAGE041
，
单位时间内，各自由度角度的变化
Figure 630103DEST_PATH_IMAGE042
表示为：
Figure 913185DEST_PATH_IMAGE043
其中
Figure 562473DEST_PATH_IMAGE044
为其机械臂空间雅可比的伪逆；
3.3）下一迭代自由度角度
Figure 349032DEST_PATH_IMAGE045
表示为：
Figure 425572DEST_PATH_IMAGE046
将新的各自由度角度
Figure 195951DEST_PATH_IMAGE045
代入步骤2）中进行迭代计算。
本发明还提供了一种多自由度机械臂的逆运动学通用求解装置，包括运动计算单元、判断单元和迭代计算单元，其特征在于：
所述运动计算单元，用于将所述机械臂各关节自由度当前角度
Figure 445667DEST_PATH_IMAGE048
作为初始迭代角 度，计算所述机械臂当前状态下的末端齐次变换矩阵
Figure 86733DEST_PATH_IMAGE050
及运动至目标位置
Figure 334174DEST_PATH_IMAGE051
的运动旋量
Figure 591849DEST_PATH_IMAGE006
；
所述判断单元，用于判断所述运动旋量
Figure 379677DEST_PATH_IMAGE006
是否小于等于误差
Figure 157140DEST_PATH_IMAGE007
，是否计算终止；
所述迭代计算单元，用于计算所述机械臂各关节的下一迭代自由度角度
Figure 559171DEST_PATH_IMAGE052
，将新 各自由度角度
Figure 54874DEST_PATH_IMAGE052
带入所述运动计算单元进行迭代计算，直到满足计算终止条件。
进一步地，所述运动计算单元还用于：利用指数积方法表示空间构型，在三维空间 笛卡尔坐标系下构建各自由度的旋转轴矩阵
Figure 833344DEST_PATH_IMAGE009
，构建末端正解
Figure 262051DEST_PATH_IMAGE010
的表达公式。
进一步地，所述利用指数积方法表示空间构型，在三维空间笛卡尔坐标系下构建 各自由度的旋转轴矩阵
Figure 366142DEST_PATH_IMAGE009
为：
Figure 83562DEST_PATH_IMAGE012
其中
Figure 400143DEST_PATH_IMAGE054
为
Figure 948936DEST_PATH_IMAGE011
自由度机械臂各自由度旋转轴单位角速度在三维空间下的表示，
Figure 709082DEST_PATH_IMAGE015
为
Figure 163065DEST_PATH_IMAGE011
自由度机械臂各自由度旋转轴单位线速度在三维空间下的表示；
所述通过指数积法表示空间构型，机械臂末端正解公式为：
Figure 34070DEST_PATH_IMAGE055
其中
Figure 889899DEST_PATH_IMAGE057
为机械臂初始状态下末端的齐次变换矩阵。
进一步地，所述运动计算单元还用于：令
Figure 820946DEST_PATH_IMAGE059
为机械臂各关节自由度当前角度，通过 所述机械臂末端正解公式得到机械臂末端当前状态下的齐次变换矩阵
Figure 558964DEST_PATH_IMAGE061
。
进一步地，所述运动计算单元还用于：接收输入机械臂末端目标状态下的齐次变 换矩阵
Figure 233659DEST_PATH_IMAGE062
，计算机械臂从
Figure 678415DEST_PATH_IMAGE061
至
Figure 45943DEST_PATH_IMAGE062
的运动旋量
Figure 474519DEST_PATH_IMAGE025
；
将机械臂正解对时间求导得到
Figure 484063DEST_PATH_IMAGE026
为：
Figure 65217DEST_PATH_IMAGE063
将机械臂正解求逆得到
Figure 587334DEST_PATH_IMAGE064
为：
Figure 175310DEST_PATH_IMAGE065
其运动旋量
Figure 191808DEST_PATH_IMAGE066
为：
Figure 876736DEST_PATH_IMAGE067
，其中
Figure 586066DEST_PATH_IMAGE068
为机械臂第
Figure 520393DEST_PATH_IMAGE033
个 自由度的单位旋转矩阵,
Figure 340581DEST_PATH_IMAGE069
为第
Figure 614437DEST_PATH_IMAGE036
次迭代的第
Figure 494668DEST_PATH_IMAGE037
个自由度的旋转角度,
Figure 667023DEST_PATH_IMAGE070
为第
Figure 274591DEST_PATH_IMAGE036
次迭代的第
Figure 684844DEST_PATH_IMAGE033
个自由度的旋转速度。
进一步地，所述迭代计算单元用于： 计算机械臂各自由度的空间雅可比矩阵
Figure 985244DEST_PATH_IMAGE039
，表 示为关于
Figure 848158DEST_PATH_IMAGE059
的函数：
Figure 993837DEST_PATH_IMAGE071
其空间旋量
Figure 993017DEST_PATH_IMAGE025
表示为：
Figure 487756DEST_PATH_IMAGE072
，
单位时间内，各自由度角度的变化
Figure 837966DEST_PATH_IMAGE042
表示为：
Figure 849653DEST_PATH_IMAGE043
其中
Figure 968919DEST_PATH_IMAGE044
为其机械臂空间雅可比的伪逆；
下一迭代自由度角度
Figure 345543DEST_PATH_IMAGE045
表示为：
Figure 917470DEST_PATH_IMAGE046
将新的各自由度角度
Figure 670531DEST_PATH_IMAGE045
代入所述运动计算单元中进行迭代计算。
本发明还提供一种电子设备，包括存储器及处理器，所述存储器中储存有计算机程序，其特征在于，所述计算机程序被所述处理器执行时，使得所述处理器执行上述的多自由度机械臂的逆运动学通用求解方法的步骤。
本发明还提供一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现上述的多自由度机械臂的逆运动学通用求解方法的步骤。
本发明还提供一种机械臂控制装置，其特征在于：包括一个或多个处理器；存储器；以及一个或多个计算机程序，其中所述一个或多个计算机程序被存储在所述存储器中，所述一个或多个计算机程序包括指令，当所述指令被所述装置执行时，使得所述装置执行上述求解方法；
机械臂，用于受控于所述控制装置，以执行运动操作。
本发明还提供一种机器人，其特征在于，包括：机械臂和如上所述的机械控制装置。
本发明的有益效果是：本方法巧妙利用了数值求解法对机械臂各自由度旋转角度进行迭代，求得机械臂末端当前位置至目标位置的运动旋量，利用空间雅可比矩阵求得个自由度角度增量，从而求得新的迭代点，通过计算并判断机械臂末端新位置至目标位置的运动旋量，不断地更新迭代点，直到满足迭代终止条件。本方法计算量小，收敛速度快，精度高，并不受机械臂构型与自由度数的影响，可通用于多自由度的机械臂中。
本方法的运动学模型利用了李群李代数的概念对末端齐次变换矩阵进行定义，利用指数积方法构建多自由度机械臂空间构型，不同于传统的机械臂D-H参数法的多坐标系建立，该方法只需要构建初始坐标系与目标坐标系，利用旋量的概念描述刚体的几何运动，使机械臂运动学计算更系统化。
附图说明
图1：计算机械臂的逆运动学通用求解方法流程图；
图2：六自由度机械臂初始状态示意图；
图3： 六自由度机械臂运动旋量示意图。
图4：八自由度机械臂运动旋量示意图。
具体实施方式
现结合实施例、附图对本发明进行进一步描述：
图1示出本发明计算机械臂的逆运动学通用求解方法流程图。如图1所示，该求解方法包括以下步骤：
1）将所述机械臂各关节自由度当前角度
Figure 706620DEST_PATH_IMAGE048
作为初始迭代角度，计算所述机械臂当 前状态下的末端齐次变换矩阵
Figure 4877DEST_PATH_IMAGE050
及运动至目标位置
Figure 578947DEST_PATH_IMAGE051
的运动旋量
Figure 620852DEST_PATH_IMAGE006
；2）判断所述运动旋量
Figure 698399DEST_PATH_IMAGE006
是否小于等于误差
Figure 433136DEST_PATH_IMAGE007
，如果是，此时的关节自由度角度即为所述机械臂的逆运动学解，计算 终止；如果否，则进入步骤3）;
3）计算所述机械臂各关节的下一迭代自由度角度
Figure 25661DEST_PATH_IMAGE052
，将新各自由度角度
Figure 136836DEST_PATH_IMAGE052
带入 步骤1）和2），进行迭代计算，直到其满足计算终止条件。
以图2所示六自由机械臂为例，其单位旋转矩阵为：
Figure 68889DEST_PATH_IMAGE073
其中
Figure 974528DEST_PATH_IMAGE074
为六自由度机械臂各自由度旋转轴单位角速度在三维空间下的表示，
Figure 257611DEST_PATH_IMAGE015
为 六自由度机械臂各自由度旋转轴单位线速度在三维空间下的表示。
本发明中角度
Figure 703635DEST_PATH_IMAGE075
可表示为第
Figure 506506DEST_PATH_IMAGE077
次迭代运算中第
Figure 566735DEST_PATH_IMAGE078
个自由度的角度。
如图3所示，
Figure 87846DEST_PATH_IMAGE079
为机械臂当前状态下各关节角度，
Figure 321250DEST_PATH_IMAGE081
为机械臂末端当前状态下的齐 次变换矩阵，
Figure 244207DEST_PATH_IMAGE051
为机械臂末端目标状态下的齐次变换矩阵，
Figure 740916DEST_PATH_IMAGE006
为机械臂从
Figure 483744DEST_PATH_IMAGE081
至
Figure 724102DEST_PATH_IMAGE051
的运动旋 量。
通过指数积法表示空间构型，坐标原点至第一个自由度的齐次变换矩阵
Figure 501565DEST_PATH_IMAGE082
表示 为：
Figure 716646DEST_PATH_IMAGE083
其中
Figure 196037DEST_PATH_IMAGE084
为即为指数积坐标，表示为：
Figure 990818DEST_PATH_IMAGE085
其中
Figure 872055DEST_PATH_IMAGE086
为
Figure 195720DEST_PATH_IMAGE087
的李代数。
同理可得机械臂各自由度间的齐次变换矩阵，即可知机械臂第
Figure 427987DEST_PATH_IMAGE077
次迭代过程中末端 正解表示为：
Figure 557617DEST_PATH_IMAGE088
其中
Figure 293361DEST_PATH_IMAGE089
为机械臂初始状态下末端的齐次变换矩阵，
Figure 319086DEST_PATH_IMAGE090
为机械臂第
Figure 773070DEST_PATH_IMAGE036
个自由度的单 位旋转矩阵,
Figure 112916DEST_PATH_IMAGE092
为第
Figure 499903DEST_PATH_IMAGE036
个自由度的旋转角度。
其机械臂的运动旋量
Figure 899792DEST_PATH_IMAGE093
表示为
Figure 841072DEST_PATH_IMAGE066
：
Figure 781346DEST_PATH_IMAGE095
将机械臂正解对时间求导得到
Figure 491682DEST_PATH_IMAGE096
为：
Figure 859209DEST_PATH_IMAGE097
将机械臂正解求逆得到
Figure 756627DEST_PATH_IMAGE098
为：
Figure 235013DEST_PATH_IMAGE099
其运动旋量
Figure 65435DEST_PATH_IMAGE066
为：
Figure 135022DEST_PATH_IMAGE100
其中
Figure 785315DEST_PATH_IMAGE068
为机械臂第
Figure 801812DEST_PATH_IMAGE033
个自由度的单位旋转矩阵,
Figure 486741DEST_PATH_IMAGE069
为第
Figure 727229DEST_PATH_IMAGE036
次迭代的第
Figure 881130DEST_PATH_IMAGE037
个自由度 的旋转角度,
Figure 685007DEST_PATH_IMAGE070
为第
Figure 975174DEST_PATH_IMAGE036
次迭代的第
Figure 104673DEST_PATH_IMAGE033
个自由度的旋转速度。
根据以上公式可得，当
Figure 277028DEST_PATH_IMAGE101
时，其运动旋量
Figure 884596DEST_PATH_IMAGE006
与各关节角度变换的关系：
Figure DEST_PATH_IMAGE102
其中
Figure DEST_PATH_IMAGE103
定义为各个自由度的空间雅可比矩阵，表示为关于
Figure 153903DEST_PATH_IMAGE079
的函数：
Figure DEST_PATH_IMAGE104
通过上述计算，旋量
Figure 923145DEST_PATH_IMAGE006
可表示为：
Figure DEST_PATH_IMAGE105
单位时间内，各自由度角度的变化
Figure DEST_PATH_IMAGE106
表示为：
Figure DEST_PATH_IMAGE107
其中
Figure DEST_PATH_IMAGE108
为其机械臂空间雅可比的伪逆。
此时下一组迭代法角度
Figure DEST_PATH_IMAGE110
表示为：
Figure DEST_PATH_IMAGE111
Figure DEST_PATH_IMAGE112
为机械臂末端当前状态下的齐次变换矩阵，
Figure 448427DEST_PATH_IMAGE051
为机械臂末端目标状态下的齐次 变换矩阵，
Figure 407156DEST_PATH_IMAGE006
为机械臂从
Figure 671915DEST_PATH_IMAGE112
至
Figure 877638DEST_PATH_IMAGE051
的运动旋量。
当运动旋量
Figure 227847DEST_PATH_IMAGE006
小于等于可接受误差
Figure 177218DEST_PATH_IMAGE007
时停止迭代，此时角度组
Figure 296483DEST_PATH_IMAGE110
即为末端其次变化 矩阵
Figure 735424DEST_PATH_IMAGE051
的逆解。
如
Figure DEST_PATH_IMAGE113
时，继续进行迭代至
Figure DEST_PATH_IMAGE114
，此时角度组
Figure 431984DEST_PATH_IMAGE048
即为末端其次变化矩阵
Figure 450625DEST_PATH_IMAGE051
的逆 解。
该方法可通用于市面上其他六自由度机械臂。
以图3为例，当所示六自由度机械臂每个自由度角度旋转
Figure DEST_PATH_IMAGE115
时，其机械臂末端齐次 变换矩阵（正解）
Figure 611348DEST_PATH_IMAGE051
为：
Figure DEST_PATH_IMAGE116
通过改变初始角度组
Figure DEST_PATH_IMAGE118
的值，当机械臂末端齐次变换矩阵
Figure 503080DEST_PATH_IMAGE051
为上式的情况下，可 接受误差
Figure 77150DEST_PATH_IMAGE007
为0.001时，其全部逆解可求为：
Figure DEST_PATH_IMAGE119
将求得所有逆解结果代入正解公式验证，全部逆解都是正确的。
第7组的角度为输入角度组，即每个自由度角度为
Figure 102744DEST_PATH_IMAGE115
。
求得目标齐次变换矩阵与初始齐次变换矩阵变化最小的一组逆解为第7组：
Figure DEST_PATH_IMAGE120
即六自由度机械臂逆解可以求出。
对于更多自由度的冗余机械臂，该方法同样有效。
以图4为例，当所示八自由度机械臂每个自由度角度旋转
Figure 117973DEST_PATH_IMAGE115
时，其机械臂末端齐次 变换矩阵（正解）
Figure 852711DEST_PATH_IMAGE051
为：
Figure DEST_PATH_IMAGE121
通过选择50组初始角度组
Figure 382918DEST_PATH_IMAGE118
的值，当机械臂末端齐次变换矩阵
Figure 494094DEST_PATH_IMAGE051
为上式的情况 下，可接受误差
Figure 426147DEST_PATH_IMAGE007
为0.001时，共解得45组逆解，其中前10组逆解为：
Figure DEST_PATH_IMAGE122
将求得所有逆解结果代入正解公式验证，全部逆解都是正确的。
第27组的角度为输入角度组，即每个自由度角度为
Figure 315474DEST_PATH_IMAGE115
。
求得目标齐次变换矩阵与初始齐次变换矩阵变化最小的一组逆解为第27组：
自由度1 自由度2 自由度3 自由度4 自由度5 自由度6 自由度7 自由度8
0.5236 0.5236 0.5236 0.5236 0.5236 0.5236 0.5236 0.5236
即逆解可以求出。
本发明还提供了一种多自由度机械臂的逆运动学通用求解装置，包括运动计算单元、判断单元和迭代计算单元，其特征在于：
所述运动计算单元，用于将所述机械臂各关节自由度当前角度
Figure DEST_PATH_IMAGE123
作为初始迭代角 度，计算所述机械臂当前状态下的末端齐次变换矩阵
Figure 270661DEST_PATH_IMAGE050
及运动至目标位置
Figure 654369DEST_PATH_IMAGE051
的运动旋量
Figure 519556DEST_PATH_IMAGE006
；
所述判断单元，用于判断所述运动旋量
Figure 845364DEST_PATH_IMAGE006
是否小于等于误差
Figure 366476DEST_PATH_IMAGE007
，是否计算终止；
所述迭代计算单元，用于计算所述机械臂各关节的下一迭代自由度角度
Figure 537563DEST_PATH_IMAGE052
，将新 各自由度角度
Figure 460519DEST_PATH_IMAGE052
带入所述运动计算单元进行迭代计算，直到满足计算终止条件。
各个单元的功能与上述逆解计算方法相对应，此不赘述。
本发明还提供一种电子设备，包括存储器及处理器，所述存储器中储存有计算机程序，其特征在于，所述计算机程序被所述处理器执行时，使得所述处理器执行如权利要求1至6中任一项所述的多自由度机械臂的逆运动学通用求解方法的步骤。
本发明还提供一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1至6中任一项所述的多自由度机械臂的逆运动学通用求解方法的步骤。
本发明还提供一种机械臂控制装置，其特征在于：包括一个或多个处理器；存储器；以及一个或多个计算机程序，其中所述一个或多个计算机程序被存储在所述存储器中，所述一个或多个计算机程序包括指令，当所述指令被所述装置执行时，使得所述装置执行如权利要求1至6任一项所述的方法；
机械臂，用于受控于所述控制装置，以执行运动操作。
本发明还提供一种机器人，其特征在于，包括：机械臂和如权利要求15所述的机械控制装置。
本领域普通技术人员可以意识到，本文中公开的实施例中描述的各单元及算法步骤，能够以电子硬件、计算机软件和电子硬件的结合来实现。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本申请的范围。
所属领域的技术人员可以清楚地了解到，为描述的方便和简洁，上述描述的系统、装置和单元的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。
在本申请所提供的几个实施例中，任一功能如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本申请的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本申请各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(Read-Only Memory；以下简称：ROM)、随机存取存储器(Random Access Memory；以下简称：RAM)、磁碟或者光盘等各种可以存储程序代码的介质。
以上所述，仅为本申请的具体实施方式，任何熟悉本技术领域的技术人员在本申请揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本申请的保护范围之内。本申请的保护范围应以所述权利要求的保护范围为准。

Claims (10)

1.一种多自由度机械臂的逆运动学通用求解方法，其特征在于：包括以下步骤：
1）将所述机械臂各关节自由度当前角度
Figure 408135DEST_PATH_IMAGE001
作为初始迭代角度，计算所述机械臂当前状 态下的末端齐次变换矩阵
Figure 989289DEST_PATH_IMAGE002
及运动至目标位置
Figure 262139DEST_PATH_IMAGE003
的运动旋量
Figure 928744DEST_PATH_IMAGE004
；
2）判断所述运动旋量
Figure 178197DEST_PATH_IMAGE004
是否小于等于误差
Figure 613858DEST_PATH_IMAGE005
，如果是，此时的关节自由度角度即为所述 机械臂的逆运动学解，计算终止；如果否，则进入步骤3）;
3）计算所述机械臂各关节的下一迭代自由度角度
Figure 57608DEST_PATH_IMAGE006
，将新各自由度角度
Figure 178886DEST_PATH_IMAGE006
带入步 骤1）和2），进行迭代计算，直到其满足计算终止条件。
2.根据权利要求1所述的一种多自由度机械臂的逆运动学通用求解方法，其特征在于：所述步骤1）还包括以下步骤：
1.1）利用指数积方法表示空间构型，在三维空间笛卡尔坐标系下构建各自由度的旋转 轴矩阵
Figure 733495DEST_PATH_IMAGE007
，构建末端正解
Figure 758083DEST_PATH_IMAGE008
的表达公式。
3.根据权利要求2所述的多自由度机械臂的逆运动学通用求解方法，其特征在于：所述步骤1.1）还包括以下步骤：
1.1.1） 利用指数积方法表示空间构型，在三维空间笛卡尔坐标系下构建各自由度的 旋转轴矩阵；
Figure 638314DEST_PATH_IMAGE009
自由度机械臂的单位旋转轴矩阵为：
Figure 512467DEST_PATH_IMAGE010
其中
Figure 870767DEST_PATH_IMAGE011
为
Figure 281020DEST_PATH_IMAGE009
自由度机械臂各自由度旋转轴单位角速度在三维空间下的表示，
Figure 332153DEST_PATH_IMAGE012
为
Figure 445601DEST_PATH_IMAGE009
自由 度机械臂各自由度旋转轴单位线速度在三维空间下的表示；
1.1.2） 通过指数积法表示空间构型，机械臂末端正解公式为：
Figure 607592DEST_PATH_IMAGE013
其中
Figure 606772DEST_PATH_IMAGE014
为机械臂初始状态下末端的齐次变换矩阵，其中
Figure 327341DEST_PATH_IMAGE015
为机械臂第
Figure 677551DEST_PATH_IMAGE016
个自由度的单位 旋转矩阵,
Figure 377653DEST_PATH_IMAGE017
为第
Figure 496919DEST_PATH_IMAGE016
个自由度的旋转角度。
4.根据权利要求3所述的多自由度机械臂的逆运动学通用求解方法，其特征在于：所述步骤1）还包括以下步骤：
1.2） 令
Figure 122810DEST_PATH_IMAGE018
为机械臂各关节自由度当前角度，通过上述步骤1.1.2）得到机械臂末端当前 状态下的齐次变换矩阵
Figure 960316DEST_PATH_IMAGE019
。
5.根据权利要求4所述的多自由度机械臂的逆运动学通用求解方法，其特征在于：所述步骤1）还包括以下步骤：
1.3）输入机械臂末端目标状态下的齐次变换矩阵
Figure 198531DEST_PATH_IMAGE020
，计算机械臂从
Figure 437882DEST_PATH_IMAGE019
至
Figure 234675DEST_PATH_IMAGE020
的运动旋量
Figure 559477DEST_PATH_IMAGE021
；
将机械臂正解对时间求导得到
Figure 866961DEST_PATH_IMAGE022
为：
Figure 695240DEST_PATH_IMAGE023
将机械臂正解求逆得到
Figure 934372DEST_PATH_IMAGE024
为：
Figure 480891DEST_PATH_IMAGE025
其运动旋量
Figure 592067DEST_PATH_IMAGE026
为：
Figure 540431DEST_PATH_IMAGE027
其中
Figure 944605DEST_PATH_IMAGE028
为机械臂第
Figure 978421DEST_PATH_IMAGE029
个自由度的单位旋转矩阵, 为第
Figure 893287DEST_PATH_IMAGE016
次迭代的第
Figure 696158DEST_PATH_IMAGE030
个自由度的旋转角 度,
Figure 271233DEST_PATH_IMAGE031
为第
Figure 792345DEST_PATH_IMAGE016
次迭代的第
Figure 979743DEST_PATH_IMAGE029
个自由度的旋转速度。
6.根据权利要求5所述的多自由度机械臂的逆运动学通用求解方法，其特征在于：所述步骤3）还包括以下步骤：
3.1） 计算机械臂各自由度的空间雅可比矩阵
Figure 902700DEST_PATH_IMAGE032
，表示为关于
Figure 648677DEST_PATH_IMAGE018
的函数：
Figure 922664DEST_PATH_IMAGE033
3.2） 其空间旋量
Figure 648174DEST_PATH_IMAGE021
表示为：
Figure 425637DEST_PATH_IMAGE034
，
单位时间内，各自由度角度的变化
Figure 336656DEST_PATH_IMAGE035
表示为：
Figure 566780DEST_PATH_IMAGE036
其中
Figure 627140DEST_PATH_IMAGE037
为其机械臂空间雅可比的伪逆；
3.3）下一迭代自由度角度
Figure 993530DEST_PATH_IMAGE038
表示为：
Figure 346889DEST_PATH_IMAGE039
将新的各自由度角度
Figure 329889DEST_PATH_IMAGE038
代入步骤2）中进行迭代计算。
7.一种多自由度机械臂的逆运动学通用求解装置，包括运动计算单元、判断单元和迭代计算单元，其特征在于：
所述运动计算单元，用于将所述机械臂各关节自由度当前角度
Figure 397202DEST_PATH_IMAGE040
作为初始迭代角度，计 算所述机械臂当前状态下的末端齐次变换矩阵
Figure 883678DEST_PATH_IMAGE041
及运动至目标位置
Figure 407938DEST_PATH_IMAGE042
的运动旋量
Figure 612654DEST_PATH_IMAGE043
；
所述判断单元，用于判断所述运动旋量
Figure 749238DEST_PATH_IMAGE043
是否小于等于误差
Figure 90220DEST_PATH_IMAGE044
，是否计算终止；
所述迭代计算单元，用于计算所述机械臂各关节的下一迭代自由度角度
Figure 50961DEST_PATH_IMAGE045
，将新各自 由度角度
Figure 742973DEST_PATH_IMAGE045
带入所述运动计算单元进行迭代计算，直到满足计算终止条件。
8.一种电子设备，包括存储器及处理器，所述存储器中储存有计算机程序，其特征在于：所述计算机程序被所述处理器执行时，使得所述处理器执行如权利要求1至6中任一项所述的多自由度机械臂的逆运动学通用求解方法的步骤。
9.一种机械臂控制装置，其特征在于：包括一个或多个处理器；存储器；以及一个或多个计算机程序，其中所述一个或多个计算机程序被存储在所述存储器中，所述一个或多个计算机程序包括指令，当所述指令被所述装置执行时，使得所述装置执行如权利要求1至6任一项所述的方法；
机械臂，用于受控于所述控制装置，以执行运动操作。
10.一种机器人，其特征在于，包括：机械臂和如权利要求15所述的机械臂控制装置。
CN202111521893.0A 2021-12-14 2021-12-14 一种多自由度机械臂的逆运动学通用求解方法及装置 Active CN114147720B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111521893.0A CN114147720B (zh) 2021-12-14 2021-12-14 一种多自由度机械臂的逆运动学通用求解方法及装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111521893.0A CN114147720B (zh) 2021-12-14 2021-12-14 一种多自由度机械臂的逆运动学通用求解方法及装置

Publications (2)

Publication Number Publication Date
CN114147720A true CN114147720A (zh) 2022-03-08
CN114147720B CN114147720B (zh) 2024-04-02

Family

ID=80450703

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111521893.0A Active CN114147720B (zh) 2021-12-14 2021-12-14 一种多自由度机械臂的逆运动学通用求解方法及装置

Country Status (1)

Country Link
CN (1) CN114147720B (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114888815A (zh) * 2022-07-04 2022-08-12 北京航空航天大学 一种连续体机器人逆运动学求解方法
CN115982893A (zh) * 2023-03-20 2023-04-18 广东工业大学 多自由度机构运动学建模方法、装置、设备和存储介质

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108555904A (zh) * 2018-01-10 2018-09-21 苏州工业职业技术学院 一种表面改性机器人操作性能的优化方法
CN110181480A (zh) * 2019-07-08 2019-08-30 济南大学 一种多关节助老助残轮椅机械臂及其基于旋量理论的运动学建模方法
US20200298403A1 (en) * 2016-03-29 2020-09-24 Cognibotics Ab Method, constraining device and system for determining geometric properties of a manipulator
CN111914416A (zh) * 2020-07-24 2020-11-10 之江实验室 一种高能效轻量化结构双足机器人的逆运动学求解方法

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20200298403A1 (en) * 2016-03-29 2020-09-24 Cognibotics Ab Method, constraining device and system for determining geometric properties of a manipulator
CN108555904A (zh) * 2018-01-10 2018-09-21 苏州工业职业技术学院 一种表面改性机器人操作性能的优化方法
CN110181480A (zh) * 2019-07-08 2019-08-30 济南大学 一种多关节助老助残轮椅机械臂及其基于旋量理论的运动学建模方法
CN111914416A (zh) * 2020-07-24 2020-11-10 之江实验室 一种高能效轻量化结构双足机器人的逆运动学求解方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
王波: "基于模糊神经网络与旋量理论的6R机械臂运动和动力研究", 《中国优秀硕士学位论文全文数据库 信息科技辑》, no. 7 *

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114888815A (zh) * 2022-07-04 2022-08-12 北京航空航天大学 一种连续体机器人逆运动学求解方法
CN115982893A (zh) * 2023-03-20 2023-04-18 广东工业大学 多自由度机构运动学建模方法、装置、设备和存储介质
CN115982893B (zh) * 2023-03-20 2023-07-18 广东工业大学 多自由度机构运动学建模方法、装置、设备和存储介质

Also Published As

Publication number Publication date
CN114147720B (zh) 2024-04-02

Similar Documents

Publication Publication Date Title
CN110076772B (zh) 一种机械臂的抓取方法及装置
CN107738254B (zh) 一种机械臂坐标系的转换标定方法与系统
CN114147720A (zh) 一种多自由度机械臂的逆运动学通用求解方法及装置
CN105773620B (zh) 基于倍四元数的工业机器人自由曲线的轨迹规划控制方法
CN110682286B (zh) 一种协作机器人实时避障方法
US8560122B2 (en) Teaching and playback method based on control of redundancy resolution for robot and computer-readable medium controlling the same
CN110489707B (zh) 一种基于gan网络的机器人逆运动学求解方法
CN113618742B (zh) 一种机器人避障方法、装置和机器人
CN107038275B (zh) 一种机械臂误差分析方法
CN111958602B (zh) 一种手腕偏置型6轴机器人实时逆解算法
WO2017132905A1 (zh) 控制运动系统的方法和装置
CN113043286B (zh) 一种多自由度机械臂实时避障路径规划系统及方法
CN116038702B (zh) 一种七轴机器人逆解方法及七轴机器人
CN109366486B (zh) 柔性机器人逆运动学求解方法、系统、设备、存储介质
CN109311155B (zh) 一种工业机器人的工具坐标系原点的标定方法及装置
CN111482969A (zh) 一种基于bas算法的六自由度偏置机器人逆解方法
CN113580135B (zh) 一种有偏置七轴机器人实时逆解算法
CN109333520A (zh) 一种四轴scara机器人逆解方法
Angerer et al. Industrial versatility of inverse kinematics algorithms for general 6R manipulators
Wu et al. Inverse kinematics of a class of 7R 6-DOF robots with non-spherical wrist
CN111482968A (zh) 一种基于bfs算法的六自由度偏置机器人逆解方法
CN116276990A (zh) 基于神经网络训练的两自由度并联结构运动学正解方法
CN115525863A (zh) 一种七自由度遥操作机械臂系统运动学求解方法及系统
CN112917479B (zh) 五轴机器人的近似位姿计算方法、装置和存储介质
CN113021345A (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
GR01 Patent grant
GR01 Patent grant
TR01 Transfer of patent right

Effective date of registration: 20240611

Address after: Station 61, 2nd Floor, Building 4, Haiju Center, No. 501, 2nd Street, Baiyang Street, Qiantang District, Hangzhou City, Zhejiang Province, 310000

Patentee after: Hangzhou Haixi Technology Co.,Ltd.

Country or region after: China

Address before: 310051 712, floor 7, building h, Haichuang Park, CLP Haikang Group Co., Ltd., No. 198, aicheng street, Wuchang Street, Yuhang District, Hangzhou, Zhejiang Province

Patentee before: Hangzhou Ruiwo Robot Technology Co.,Ltd.

Country or region before: China