CN114147720B - 一种多自由度机械臂的逆运动学通用求解方法及装置 - Google Patents
一种多自由度机械臂的逆运动学通用求解方法及装置 Download PDFInfo
- Publication number
- CN114147720B CN114147720B CN202111521893.0A CN202111521893A CN114147720B CN 114147720 B CN114147720 B CN 114147720B CN 202111521893 A CN202111521893 A CN 202111521893A CN 114147720 B CN114147720 B CN 114147720B
- Authority
- CN
- China
- Prior art keywords
- mechanical arm
- freedom
- degree
- rotation
- motion
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 70
- 239000011159 matrix material Substances 0.000 claims abstract description 62
- 230000009466 transformation Effects 0.000 claims abstract description 35
- 238000004364 calculation method Methods 0.000 claims description 43
- 238000004590 computer program Methods 0.000 claims description 21
- 230000008859 change Effects 0.000 claims description 9
- 230000006870 function Effects 0.000 claims description 7
- 230000002349 favourable effect Effects 0.000 abstract 1
- 238000010586 diagram Methods 0.000 description 3
- 230000008569 process Effects 0.000 description 3
- 238000004458 analytical method Methods 0.000 description 2
- 238000012795 verification Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004422 calculation algorithm Methods 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 230000003247 decreasing effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000012827 research and development Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 230000009897 systematic effect Effects 0.000 description 1
- 230000001131 transforming effect Effects 0.000 description 1
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)将所述机械臂各关节自由度当前角度θi作为初始迭代角度，计算所述机械臂当前状态下的末端齐次变换矩阵Ti及运动至目标位置Tf的运动旋量V；
2)判断所述运动旋量V是否小于等于误差ε，如果是，此时的关节自由度角度即为所述机械臂的逆运动学解，计算终止；如果否，则进入步骤3)；
3)计算所述机械臂各关节的下一迭代自由度角度θi+1，将新各自由度角度θi+1带入步骤1)和2)，进行迭代计算，直到其满足计算终止条件。
进一步地，所述步骤1)还包括以下步骤：
1.1)利用指数积方法表示空间构型，在三维空间笛卡尔坐标系下构建各自由度的旋转轴矩阵S，构建末端正解T的表达公式。
进一步地，所述步骤1.1)还包括以下步骤：
1.1.1)利用指数积方法表示空间构型，在三维空间笛卡尔坐标系下构建各自由度的旋转轴矩阵；n自由度机械臂的单位旋转轴矩阵为：
其中ω为n自由度机械臂各自由度旋转轴单位角速度在三维空间下的表示，v为n自由度机械臂各自由度旋转轴单位线速度在三维空间下的表示；
1.1.2)通过指数积法表示空间构型，机械臂末端正解公式为：
其中M为机械臂初始状态下末端的齐次变换矩阵。
进一步地，所述步骤1)还包括以下步骤：
1.2)令θi为机械臂各关节自由度当前角度，通过上述步骤1.1.2)得到机械臂末端当前状态下的齐次变换矩阵Ti。
进一步地，所述步骤1)还包括以下步骤：
1.3)输入机械臂末端目标状态下的齐次变换矩阵Tf，计算机械臂从Ti至Tf的运动旋量V；
将机械臂正解对时间求导得到为：
将机械臂正解求逆得到T-1为：
其运动旋量[V]∈se(3)为：
其中[Sj]为机械臂第j个自由度的单位旋转矩阵,θik为第i次迭代的第k个自由度的旋转角度,为第i次迭代的第j个自由度的旋转速度。
进一步地，所述步骤3)还包括以下步骤：
3.1)计算机械臂各自由度的空间雅可比矩阵J，表示为关于θi的函数：
3.2)其空间旋量V表示为：
单位时间内，各自由度角度的变化Δθ表示为：
其中J+为其机械臂空间雅可比的伪逆；
3.3)下一迭代自由度角度θi+1表示为：θi+1＝θi+Δθ
将新的各自由度角度θi+1代入步骤2)中进行迭代计算。
本发明还提供了一种多自由度机械臂的逆运动学通用求解装置，包括运动计算单元、判断单元和迭代计算单元，其特征在于：
所述运动计算单元，用于将所述机械臂各关节自由度当前角度θi作为初始迭代角度，计算所述机械臂当前状态下的末端齐次变换矩阵Ti及运动至目标位置Tf的运动旋量V；
所述判断单元，用于判断所述运动旋量V是否小于等于误差ε，是否计算终止；
所述迭代计算单元，用于计算所述机械臂各关节的下一迭代自由度角度θi+1，将新各自由度角度θi+1带入所述运动计算单元进行迭代计算，直到满足计算终止条件。
进一步地，所述运动计算单元还用于：利用指数积方法表示空间构型，在三维空间笛卡尔坐标系下构建各自由度的旋转轴矩阵S，构建末端正解T的表达公式。
进一步地，所述利用指数积方法表示空间构型，在三维空间笛卡尔坐标系下构建各自由度的旋转轴矩阵S为：
其中ω为n自由度机械臂各自由度旋转轴单位角速度在三维空间下的表示，v为n自由度机械臂各自由度旋转轴单位线速度在三维空间下的表示；
所述通过指数积法表示空间构型，机械臂末端正解公式为：
其中M为机械臂初始状态下末端的齐次变换矩阵。
进一步地，所述运动计算单元还用于：令θi为机械臂各关节自由度当前角度，通过所述机械臂末端正解公式得到机械臂末端当前状态下的齐次变换矩阵Ti。
进一步地，所述运动计算单元还用于：接收输入机械臂末端目标状态下的齐次变换矩阵Tf，计算机械臂从Ti至Tf的运动旋量V；
将机械臂正解对时间求导得到为：
将机械臂正解求逆得到T-1为：
其运动旋量[V]∈se(3)为：
其中[Sj]为机械臂第j个自由度的单位旋转矩阵,θik为第i次迭代的第k个自由度的旋转角度,/>为第i次迭代的第j个自由度的旋转速度。
进一步地，所述迭代计算单元用于：计算机械臂各自由度的空间雅可比矩阵J，表示为关于θi的函数：
其空间旋量V表示为：
单位时间内，各自由度角度的变化Δθ表示为：
其中J+为其机械臂空间雅可比的伪逆；
下一迭代自由度角度θi+1表示为：θi+1＝θi+Δθ
将新的各自由度角度θi+1代入所述运动计算单元中进行迭代计算。
本发明还提供一种电子设备，包括存储器及处理器，所述存储器中储存有计算机程序，所述计算机程序被所述处理器执行时，使得所述处理器执行上述的多自由度机械臂的逆运动学通用求解方法的步骤。
本发明还提供一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现上述的多自由度机械臂的逆运动学通用求解方法的步骤。
本发明还提供一种机械臂控制装置，包括一个或多个处理器；存储器；以及一个或多个计算机程序，其中所述一个或多个计算机程序被存储在所述存储器中，所述一个或多个计算机程序包括指令，当所述指令被所述处理器执行时，使得所述装置执行上述求解方法；
机械臂，用于受控于所述控制装置，以执行运动操作。
本发明还提供一种机器人，其特征在于，包括：机械臂和如上所述的机械控制装置。
本发明的有益效果是：本方法巧妙利用了数值求解法对机械臂各自由度旋转角度进行迭代，求得机械臂末端当前位置至目标位置的运动旋量，利用空间雅可比矩阵求得个自由度角度增量，从而求得新的迭代点，通过计算并判断机械臂末端新位置至目标位置的运动旋量，不断地更新迭代点，直到满足迭代终止条件。本方法计算量小，收敛速度快，精度高，并不受机械臂构型与自由度数的影响，可通用于多自由度的机械臂中。
本方法的运动学模型利用了李群李代数的概念对末端齐次变换矩阵进行定义，利用指数积方法构建多自由度机械臂空间构型，不同于传统的机械臂D-H参数法的多坐标系建立，该方法只需要构建初始坐标系与目标坐标系，利用旋量的概念描述刚体的几何运动，使机械臂运动学计算更系统化。
附图说明
图1：计算机械臂的逆运动学通用求解方法流程图；
图2：六自由度机械臂初始状态示意图；
图3：六自由度机械臂运动旋量示意图。
图4：八自由度机械臂运动旋量示意图。
具体实施方式
现结合实施例、附图对本发明进行进一步描述：
图1示出本发明计算机械臂的逆运动学通用求解方法流程图。如图1所示，该求解方法包括以下步骤：
1)将所述机械臂各关节自由度当前角度θi作为初始迭代角度，计算所述机械臂当前状态下的末端齐次变换矩阵Ti及运动至目标位置Tf的运动旋量V；
2)判断所述运动旋量V是否小于等于误差ε，如果是，此时的关节自由度角度即为所述机械臂的逆运动学解，计算终止；如果否，则进入步骤3)；
3)计算所述机械臂各关节的下一迭代自由度角度θi+1，将新各自由度角度θi+1带入步骤1)和2)，进行迭代计算，直到其满足计算终止条件。
以图2所示六自由机械臂为例，其单位旋转矩阵为：
其中ω为六自由度机械臂各自由度旋转轴单位角速度在三维空间下的表示，v为六自由度机械臂各自由度旋转轴单位线速度在三维空间下的表示。
本发明中角度θij可表示为第i次迭代运算中第j个自由度的角度。
如图3所示，θ0j为机械臂当前状态下各关节角度，T0为机械臂末端当前状态下的齐次变换矩阵，Tf为机械臂末端目标状态下的齐次变换矩阵，V为机械臂从T0至Tf的运动旋量。
通过指数积法表示空间构型，坐标原点至第一个自由度的齐次变换矩阵T0-1表示为：
其中为即为指数积坐标，表示为：
其中[S1]θ1∈se(3)为T0-1∈SE(3)的李代数。
同理可得机械臂各自由度间的齐次变换矩阵，即可知机械臂第i次迭代过程中末端正解表示为：
其中M为机械臂初始状态下末端的齐次变换矩阵，[Si]为机械臂第i个自由度的单位旋转矩阵,θi为第i个自由度的旋转角度。
其机械臂的运动旋量V∈R6表示为[V]∈se(3)：
将机械臂正解对时间求导得到为：
将机械臂正解求逆得到T-1为：
其运动旋量[V]∈se(3)为：
其中[Sj]为机械臂第j个自由度的单位旋转矩阵,θik为第i次迭代的第k个自由度的旋转角度,为第i次迭代的第j个自由度的旋转速度。
根据以上公式可得，当i＝0时，其运动旋量V与各关节角度变换的关系：
其中J0j定义为各个自由度的空间雅可比矩阵，表示为关于θ0j的函数：
通过上述计算，旋量V可表示为：
单位时间内，各自由度角度的变化Δθ表示为：
其中J+为其机械臂空间雅可比的伪逆。
此时下一组迭代法角度θ1表示为：θ1＝θ0+Δθ
T1为机械臂末端当前状态下的齐次变换矩阵，Tf为机械臂末端目标状态下的齐次变换矩阵，V为机械臂从T1至Tf的运动旋量。
当运动旋量V小于等于可接受误差ε时停止迭代，此时角度组θ1即为末端其次变化矩阵Tf的逆解。
如V>ε时，继续进行迭代至V≤ε，此时角度组θi即为末端其次变化矩阵Tf的逆解。
该方法可通用于市面上其他六自由度机械臂。
以图3为例，当所示六自由度机械臂每个自由度角度旋转时，其机械臂末端齐次变换矩阵(正解)Tf为：
通过改变初始角度组θ0的值，当机械臂末端齐次变换矩阵Tf为上式的情况下，可接受误差ε为0.001时，其全部逆解可求为：
| 组号 | 自由度1 | 自由度2 | 自由度3 | 自由度4 | 自由度5 | 自由度6 |
| 1 | -2.6180 | -0.5236 | -2.6180 | 0.5236 | 0.5236 | 0.5236 |
| 2 | 2.0069 | 0.2708 | 0.1600 | 0.5236 | -0.5236 | -0.0385 |
| 3 | -2.6180 | -0.5236 | 0.5236 | 2.6180 | -2.6180 | 0.5236 |
| 4 | -1.1347 | -0.2708 | 0.1600 | 2.6180 | 2.6180 | -0.0385 |
| 5 | 0.5236 | 0.5236 | -2.6180 | 2.6180 | -2.6180 | 0.5236 |
| 6 | -1.1347 | -0.2708 | -2.9816 | 0.5236 | -0.5236 | -0.0385 |
| 7 | 0.5236 | 0.5236 | 0.5236 | 0.5236 | 0.5236 | 0.5236 |
| 8 | 2.0069 | 0.2708 | -2.9816 | 2.6180 | 2.6180 | -0.0385 |
将求得所有逆解结果代入正解公式验证，全部逆解都是正确的。
第7组的角度为输入角度组，即每个自由度角度为
求得目标齐次变换矩阵与初始齐次变换矩阵变化最小的一组逆解为第7组：
| 自由度1 | 自由度2 | 自由度3 | 自由度4 | 自由度5 | 自由度6 |
| 0.5236 | 0.5236 | 0.5236 | 0.5236 | 0.5236 | 0.5236 |
即六自由度机械臂逆解可以求出。
对于更多自由度的冗余机械臂，该方法同样有效。
以图4为例，当所示八自由度机械臂每个自由度角度旋转时，其机械臂末端齐次变换矩阵(正解)Tf为：
通过选择50组初始角度组θ0的值，当机械臂末端齐次变换矩阵Tf为上式的情况下，可接受误差ε为0.001时，共解得45组逆解，其中前10组逆解为：
| 组号 | 自由度1 | 自由度2 | 自由度3 | 自由度4 | 自由度5 | 自由度6 | 自由度7 | 自由度8 |
| 1 | -2.2689 | -0.7758 | 1.8333 | 0.2954 | 1.8601 | 0.7132 | 0.7204 | 0.6644 |
| 2 | -2.2645 | -0.7601 | -2.9502 | -0.4721 | 1.7349 | 0.7062 | -0.7269 | 0.0649 |
| 3 | -1.9684 | -0.6004 | 1.0247 | 0.4360 | 0.5969 | 2.2475 | 2.5304 | 0.0393 |
| 4 | 1.0991 | 0.7472 | -2.4714 | 0.5342 | 1.0078 | 2.2233 | 2.5026 | 0.1222 |
| 5 | 0.7596 | 0.6750 | -0.0853 | -0.3581 | 2.1258 | 0.5944 | -0.7039 | -0.0040 |
| 6 | 1.2395 | 0.4032 | 2.0826 | -0.5927 | -1.2501 | 0.9167 | 0.2144 | 0.2104 |
| 7 | -2.7168 | -0.8690 | -1.1387 | 0.4206 | 2.3201 | 2.5751 | -2.8790 | 0.6048 |
| 8 | -1.8609 | -0.4937 | -1.3287 | -0.6121 | -1.1237 | 0.9668 | 0.3160 | 0.2714 |
| 9 | -2.3451 | -0.7486 | 0.0150 | 0.4518 | -1.1727 | 0.6429 | -0.7122 | 0.0500 |
| 10 | -2.8184 | -0.4991 | -2.0799 | 0.6058 | 0.4102 | 0.5151 | 0.2949 | 0.3888 |
将求得所有逆解结果代入正解公式验证，全部逆解都是正确的。
第27组的角度为输入角度组，即每个自由度角度为
求得目标齐次变换矩阵与初始齐次变换矩阵变化最小的一组逆解为第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 |
即逆解可以求出。
本发明还提供了一种多自由度机械臂的逆运动学通用求解装置，包括运动计算单元、判断单元和迭代计算单元，其特征在于：
所述运动计算单元，用于将所述机械臂各关节自由度当前角度θi作为初始迭代角度，计算所述机械臂当前状态下的末端齐次变换矩阵Ti及运动至目标位置Tf的运动旋量V；
所述判断单元，用于判断所述运动旋量V是否小于等于误差ε，是否计算终止；
所述迭代计算单元，用于计算所述机械臂各关节的下一迭代自由度角度θi+1，将新各自由度角度θi+1带入所述运动计算单元进行迭代计算，直到满足计算终止条件。
各个单元的功能与上述逆解计算方法相对应，此不赘述。
本发明还提供一种电子设备，包括存储器及处理器，所述存储器中储存有计算机程序，其特征在于，所述计算机程序被所述处理器执行时，使得所述处理器执行如前所述的多自由度机械臂的逆运动学通用求解方法的步骤。
本发明还提供一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如前所述的多自由度机械臂的逆运动学通用求解方法的步骤。
本发明还提供一种机械臂控制装置，其特征在于：包括一个或多个处理器；存储器；以及一个或多个计算机程序，其中所述一个或多个计算机程序被存储在所述存储器中，所述一个或多个计算机程序包括指令，当所述指令被所述装置执行时，使得所述处理器执行如前所述的方法；
机械臂，用于受控于所述控制装置，以执行运动操作。
本发明还提供一种机器人，其特征在于，包括：机械臂和如前所述的机械臂控制装置。
本领域普通技术人员可以意识到，本文中公开的实施例中描述的各单元及算法步骤，能够以电子硬件、计算机软件和电子硬件的结合来实现。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本申请的范围。
所属领域的技术人员可以清楚地了解到，为描述的方便和简洁，上述描述的系统、装置和单元的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。
在本申请所提供的几个实施例中，任一功能如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本申请的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本申请各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(Read-Only Memory；以下简称：ROM)、随机存取存储器(Random Access Memory；以下简称：RAM)、磁碟或者光盘等各种可以存储程序代码的介质。
以上所述，仅为本申请的具体实施方式，任何熟悉本技术领域的技术人员在本申请揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本申请的保护范围之内。本申请的保护范围应以所述权利要求的保护范围为准。
Claims (6)
1.一种多自由度机械臂的逆运动学通用求解方法，其特征在于：包括以下步骤：
1)将所述机械臂各关节自由度当前角度θi作为初始迭代角度，计算所述机械臂当前状态下的末端齐次变换矩阵Ti及运动至目标位置Tf的运动旋量V；
2)判断所述运动旋量V是否小于等于误差ε，如果是，此时的关节自由度角度即为所述机械臂的逆运动学解，计算终止；如果否，则进入步骤3)；
3)计算所述机械臂各关节的下一迭代自由度角度θi+1，将新各自由度角度θi+1带入步骤1)和2)，进行迭代计算，直到其满足计算终止条件；
所述步骤1)还包括以下步骤：
1.1)利用指数积方法表示空间构型，在三维空间笛卡尔坐标系下构建各自由度的旋转轴矩阵S，构建末端正解T的表达公式；
所述步骤1.1)还包括以下步骤：
1.1.1)利用指数积方法表示空间构型，在三维空间笛卡尔坐标系下构建各自由度的旋转轴矩阵；n自由度机械臂的单位旋转轴矩阵为：
其中ω为n自由度机械臂各自由度旋转轴单位角速度在三维空间下的表示，v为n自由度机械臂各自由度旋转轴单位线速度在三维空间下的表示；
1.1.2)通过指数积法表示空间构型，机械臂末端正解公式为：
其中M为机械臂初始状态下末端的齐次变换矩阵，其中[Si]为机械臂第i个自由度的单位旋转矩阵,θi为第i个自由度的旋转角度；
所述步骤1)还包括以下步骤：
1.2)令θi为机械臂各关节自由度当前角度，通过上述步骤1.1.2)得到机械臂末端当前状态下的齐次变换矩阵Ti；
所述步骤1)还包括以下步骤：
1.3)输入机械臂末端目标状态下的齐次变换矩阵Tf，计算机械臂从Ti至Tf的运动旋量V；
将机械臂正解对时间求导得到为：
将机械臂正解求逆得到T-1为：
其运动旋量[V]∈se(3)为：
其中[Sj]为机械臂第j个自由度的单位旋转矩阵,θik为第i次迭代的第k个自由度的旋转角度,为第i次迭代的第j个自由度的旋转速度；
所述步骤3)还包括以下步骤：
3.1)计算机械臂各自由度的空间雅可比矩阵J，表示为关于θi的函数：
3.2)其空间旋量V表示为：
单位时间内，各自由度角度的变化Δθ表示为：
其中J+为其机械臂空间雅可比的伪逆；
3.3)下一迭代自由度角度θi+1表示为：θi+1＝θi+Δθ
将新的各自由度角度θi+1代入步骤2)中进行迭代计算。
2.一种多自由度机械臂的逆运动学通用求解装置，包括运动计算单元、判断单元和迭代计算单元，其特征在于：
所述运动计算单元，用于将所述机械臂各关节自由度当前角度θi作为初始迭代角度，计算所述机械臂当前状态下的末端齐次变换矩阵Ti及运动至目标位置Tf的运动旋量V；
所述判断单元，用于判断所述运动旋量V是否小于等于误差ε，是否计算终止；
所述迭代计算单元，用于计算所述机械臂各关节的下一迭代自由度角度θi+1，将新各自由度角度θi+1带入所述运动计算单元进行迭代计算，直到满足计算终止条件；
所述运动计算单元还用于，利用指数积方法表示空间构型，在三维空间笛卡尔坐标系下构建各自由度的旋转轴矩阵S，构建末端正解T的表达公式；
所述利用指数积方法表示空间构型，在三维空间笛卡尔坐标系下构建各自由度的旋转轴矩阵S为：
其中ω为n自由度机械臂各自由度旋转轴单位角速度在三维空间下的表示，v为n自由度机械臂各自由度旋转轴单位线速度在三维空间下的表示；
所述通过指数积法表示空间构型，机械臂末端正解公式为：
其中M为机械臂初始状态下末端的齐次变换矩阵，[Si]为机械臂第i个自由度的单位旋转矩阵,θi为第i个自由度的旋转角度；
所述运动计算单元还用于，令θi为机械臂各关节自由度当前角度，通过所述机械臂末端正解公式得到机械臂末端当前状态下的齐次变换矩阵Ti；
所述运动计算单元还用于，接收输入机械臂末端目标状态下的齐次变换矩阵Tf，计算机械臂从Ti至Tf的运动旋量V；
将机械臂正解对时间求导得到为：
将机械臂正解求逆得到T-1为：
其运动旋量[V]∈se(3)为：
其中[Sj]为机械臂第j个自由度的单位旋转矩阵,θik为第i次迭代的第k个自由度的旋转角度,为第i次迭代的第j个自由度的旋转速度；
所述迭代计算单元用于：计算机械臂各自由度的空间雅可比矩阵Jn，表示为关于θi的函数：
其空间旋量V表示为：
单位时间内，各自由度角度的变化Δθ表示为：
其中J+为其机械臂空间雅可比的伪逆；
下一迭代自由度角度θi+1表示为：θi+1＝θi+Δθ
将新的各自由度角度θi+1代入所述运动计算单元中进行迭代计算。
3.一种电子设备，包括存储器及处理器，所述存储器中储存有计算机程序，其特征在于：所述计算机程序被所述处理器执行时，使得所述处理器执行如权利要求1所述的多自由度机械臂的逆运动学通用求解方法的步骤。
4.一种计算机可读存储介质，其上存储有计算机程序，其特征在于：所述计算机程序被处理器执行时实现如权利要求1所述的多自由度机械臂的逆运动学通用求解方法的步骤。
5.一种机械臂控制装置，其特征在于：包括一个或多个处理器；存储器；以及一个或多个计算机程序，其中所述一个或多个计算机程序被存储在所述存储器中，所述一个或多个计算机程序包括指令，当所述指令被所述处理器执行时，使得所述装置执行如权利要求1所述的方法；
机械臂，用于受控于所述控制装置，以执行运动操作。
6.一种机器人，其特征在于，包括：机械臂和如权利要求5所述的机械臂控制装置。
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 CN114147720A (zh) | 2022-03-08 |
| CN114147720B true 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) |
Families Citing this family (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114888815A (zh) * | 2022-07-04 | 2022-08-12 | 北京航空航天大学 | 一种连续体机器人逆运动学求解方法 |
| CN115982893B (zh) * | 2023-03-20 | 2023-07-18 | 广东工业大学 | 多自由度机构运动学建模方法、装置、设备和存储介质 |
Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108555904A (zh) * | 2018-01-10 | 2018-09-21 | 苏州工业职业技术学院 | 一种表面改性机器人操作性能的优化方法 |
| CN110181480A (zh) * | 2019-07-08 | 2019-08-30 | 济南大学 | 一种多关节助老助残轮椅机械臂及其基于旋量理论的运动学建模方法 |
| CN111914416A (zh) * | 2020-07-24 | 2020-11-10 | 之江实验室 | 一种高能效轻量化结构双足机器人的逆运动学求解方法 |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| EP3436876B1 (en) * | 2016-03-29 | 2021-07-07 | Cognibotics AB | Method, constraining device and system for determining geometric properties of a manipulator |
-
2021
- 2021-12-14 CN CN202111521893.0A patent/CN114147720B/zh active Active
Patent Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| 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)
| Title |
|---|
| 基于模糊神经网络与旋量理论的6R机械臂运动和动力研究;王波;《中国优秀硕士学位论文全文数据库 信息科技辑》(第7期);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN114147720A (zh) | 2022-03-08 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107589934B (zh) | 一种关节型机械臂逆运动学解析解的求取方法 | |
| Zhang et al. | Variable joint-velocity limits of redundant robot manipulators handled by quadratic programming | |
| CN114147720B (zh) | 一种多自由度机械臂的逆运动学通用求解方法及装置 | |
| CN111538949A (zh) | 冗余机器人逆运动学求解方法、装置和冗余机器人 | |
| CN104772773B (zh) | 一种机械臂运动学形式化分析方法 | |
| CN109344477B (zh) | 一种6自由度机械臂逆运动学求解方法 | |
| CN110370256A (zh) | 机器人及其路径规划方法、装置和控制器 | |
| CN104991448B (zh) | 一种基于构型平面的水下机械臂运动学的求解方法 | |
| WO2017132905A1 (zh) | 控制运动系统的方法和装置 | |
| CN113618742A (zh) | 一种机器人避障方法、装置和机器人 | |
| CN109176487A (zh) | 一种联动关节段标定方法、系统、设备、存储介质 | |
| CN110900608B (zh) | 基于最优测量构型选择的机器人运动学标定方法 | |
| CN117245640A (zh) | 机器人的控制方法、装置、设备及存储介质 | |
| Angerer et al. | Industrial versatility of inverse kinematics algorithms for general 6R manipulators | |
| CN111801630A (zh) | 变位机轴坐标系标定方法、机器人系统及存储装置 | |
| CN112380655A (zh) | 一种基于rs-cmsa算法的机器人逆运动学求解方法 | |
| Wu et al. | Inverse kinematics of a class of 7R 6-DOF robots with non-spherical wrist | |
| Valsamos et al. | Introduction of the high performance area measure for the evaluation of metamorphic manipulator anatomies | |
| Moon et al. | Real-time UVMS torque distribution algorithm based on weighting matrix | |
| CN117162080A (zh) | 机械臂的控制方法、装置、设备、存储介质及程序产品 | |
| CN113021345A (zh) | 机械臂逆运动学控制方法、装置、设备及可读存储介质 | |
| Masajedi et al. | Verification of bee algorithm based path planning for a 6DOF manipulator using ADAMS | |
| CN112613196A (zh) | 一种五轴机器人姿态逆解方法及相关设备 | |
| CN117733872B (zh) | 一种基于方向性能的串联机器人逆运动学控制方法 | |
| Zhou et al. | The research on direct kinematic problem of a special class of the Stewart-Gough 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 | ||
| 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 |
|
| TR01 | Transfer of patent right |