CN117407630A - 协作机械臂求解肩肘角度逆运动学解析解系统 - Google Patents

协作机械臂求解肩肘角度逆运动学解析解系统 Download PDF

Info

Publication number
CN117407630A
CN117407630A CN202311713282.5A CN202311713282A CN117407630A CN 117407630 A CN117407630 A CN 117407630A CN 202311713282 A CN202311713282 A CN 202311713282A CN 117407630 A CN117407630 A CN 117407630A
Authority
CN
China
Prior art keywords
joint
mechanical arm
robot
axis
roty
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
CN202311713282.5A
Other languages
English (en)
Other versions
CN117407630B (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.)
Star Ape Philosophy Technology Shanghai Co ltd
Xingyuanzhe Technology Shenzhen Co ltd
Original Assignee
Star Ape Philosophy Technology Shanghai Co ltd
Xingyuanzhe Technology Shenzhen 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 Star Ape Philosophy Technology Shanghai Co ltd, Xingyuanzhe Technology Shenzhen Co ltd filed Critical Star Ape Philosophy Technology Shanghai Co ltd
Priority to CN202311713282.5A priority Critical patent/CN117407630B/zh
Publication of CN117407630A publication Critical patent/CN117407630A/zh
Application granted granted Critical
Publication of CN117407630B publication Critical patent/CN117407630B/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
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • 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
    • 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)
  • Mechanical Engineering (AREA)
  • Robotics (AREA)
  • General Physics & Mathematics (AREA)
  • Software Systems (AREA)
  • Data Mining & Analysis (AREA)
  • Theoretical Computer Science (AREA)
  • Databases & Information Systems (AREA)
  • Mathematical Optimization (AREA)
  • General Engineering & Computer Science (AREA)
  • Pure & Applied Mathematics (AREA)
  • Mathematical Analysis (AREA)
  • Computational Mathematics (AREA)
  • Algebra (AREA)
  • Artificial Intelligence (AREA)
  • Evolutionary Computation (AREA)
  • Fuzzy Systems (AREA)
  • Manipulator (AREA)

Abstract

本发明提供了一种协作机械臂求解肩肘角度逆运动学解析解系统，包括：处理单元、传感器单元、驱动单元，传感器单元用以确定机器人末端的位置，处理单元中的空间坐标系建立单元，用于建立机械臂关节坐标系；关节角度调整单元，用于在机械臂的朝向确定之后，分别获取第4轴、第5轴、第6轴的旋度变化量，得到第4关节、第5关节、第6关节对应的关节角度；关节角度运算单元，用于根据机器人末端坐标，以及第4关节、第5关节、第6关节对应的关节角度建立逆运动学方程，计算第1关节、第2关节、第3关节对应的角度。从而能够非常迅速地求解出机械臂所有关节所对应的角度，控制机械臂顺利通过奇异点，保证机械臂运动的连贯性和稳定性。

Description

协作机械臂求解肩肘角度逆运动学解析解系统
技术领域
本发明涉及智能制造和高端制造，具体地，涉及一种协作机械臂求解肩肘角度逆运动学解析解系统。
背景技术
目前，关节型机械臂的逆向运动学问题是在给定机械臂末端执行器坐标系相对于基坐标系的位置和姿态，以及所有机械臂关节几何参数的情况下，求取所有机械臂关节转动的角度值，是正向运动学的逆过程。正向运动学根据机械臂的结构几何参数可以得到前后相邻关节坐标系之间的齐次坐标变换矩阵T，也即两关节之间的位姿矩阵；若已知每个关节转动角度θ，通过将各关节齐次变换矩阵T依次连续右乘即可得到机械臂末端执行器坐标系的位姿矩阵，所得结果唯一。而逆向运动学的求解则相对复杂，且可能具有无解或多解情况，比如末端执行器坐标系在奇异点处时无解，而反三角函数的周期性导致机械臂关节旋转角度解析解理论上的多解问题。现有的逆运动学求解方法，程序复杂，需要高性能的计算配置，且存在稳定性不足的问题。
发明内容
针对现有技术中的缺陷，本发明的目的是提供一种协作机械臂求解肩肘角度逆运动学解析解系统。
第一方面，本申请实施例提供一种协作机械臂求解肩肘角度逆运动学解析解系统，应用于具备六个自由度的机器人，所述系统包括：处理单元、传感器单元、驱动单元，所述传感器单元用以确定机器人末端的位置，所述处理单元包括：空间坐标系建立单元、关节角度调整单元、关节角度运算单元，其中：所述空间坐标系建立单元，用于建立机械臂关节坐标系，并根据机器人的六个自由度配置肩-肘-腕结构，由基座至末端分别定义第0~6关节；所述驱动单元，用于通过机器人的第4轴、第5轴、第6轴来控制机械臂的朝向；所述关节角度调整单元，用于在机械臂的朝向确定之后，分别获取第4轴、第5轴、第6轴的旋度变化量，得到第4关节、第5关节、第6关节对应的关节角度；所述关节角度运算单元，用于根据机器人末端坐标，以及第4关节、第5关节、第6关节对应的关节角度建立逆运动学方程，计算第1关节、第2关节、第3关节对应的角度。
可选地，所述空间坐标系建立单元，具体用于：以机器人基座对应的位置为原点，建立直角坐标系；其中，机器人末端坐标为机械臂执行端需要到达的目标坐标位置。
可选地，所述关节角度调整单元，具体用于：根据已知的机器人末端坐标，向所述驱动单元发送运动控制指令，以使得所述驱动单元通过调节机器人的第4轴、第5轴、第6轴来控制机械臂的朝向；在机械臂朝向预设方位时，分别记录下第4关节、第5关节、第6关节对应的旋度变化量，并基于关节初始状态时的关节角度，确定当前状态下第4关节、第5关节、第6关节对应的关节角度。
可选地，所述关节角度运算单元，具体用于：构建机器人末端坐标的正运动学方程；基于所述正运动学方程求解逆运动学的过程，反向计算出第1关节、第2关节、第3关节对应的关节角度。
可选地，机器人末端坐标的正运动方程如下：
t0T=t01+ rotz(q1)(t12+ roty(q2)(t23+ roty(q3)(t34+ roty(q4)(t45+ rotz(q5)(t56+ roty(q6)t6T)))))
式中：t0T表示末端法兰相对于第0关节的位移，t01表示第1关节相对于第0关节的位移，rotz(q1)表示绕Z轴旋转角度q1，t12表示第2关节相对于第1关节的位移，roty(q2)表示绕Y轴旋转角度q2，t23 表示第3关节相对于第2关节的位移，roty(q3)表示绕Y轴旋转角度q3，t34表示第4关节相对于第3关节的位移，roty(q4))表示绕Y轴旋转角度q4，t45表示第5关节相对于第4关节的位移，rotz(q5)表示绕Z轴旋转角度q5，t56表示第6关节相对于第5关节的位移，roty(q6)表示绕Y轴旋转角度q6，t6T表示末端法兰相对于第6关节的位移。
可选地，当第4关节、第5关节、第6关节对应的关节角度q4、q5、q6已知时，
t4T= roty(q4)(t45+ rotz(q5)(t56+ roty(q6)t6T))
式中：t4T表示末端法兰相对于第4关节的位移，将t4T代入t0T的计算公式，得到如下公式：
t0T= t01+ rotz(q1)(t12+ roty(q2)(t23 + roty(q3)(t34+ t4T)
求解逆运动学的过程：反向计算出第1关节、第2关节、第3关节对应的关节角度q1、q2、q3；
令t1T= t0T- t01 ；
式中：t1T表示末端法兰相对于第1关节的位移，将t1T的坐标记为[t1x，t1y ，t1z]，t1x表示关于X轴的坐标，t1y表示关于Y轴的坐标，t1z表示关于Z轴的坐标；
令T1T= t12+ roty(q2)(t23+ roty(q3)(t3T) ；
式中：t3T表示末端法兰相对于第3关节的位移，将T1T的坐标记为[T1x，T1y ，T1z]；
由于某一坐标绕相应轴旋转时，对应轴的分量保持不变，假设t12 、t23 、t3T关于Y轴方向变量分别为t12y 、t23y 、t3Ty，则的坐标存在如下关系：
T1z= t1z；
T1y= t12y+ t23y+ t3Ty；
其中，T1x = +/-sqrt(t1y* t1y+ t1x* t1x- T1y* T1y)；
当T1T已知时，根据Paden-Kahan subproblem算法计算得到q1；
当q1已知时，存在T1T= t12+ roty(q2)(t23+ roty(q3)t3T)，根据Paden-Kahansubproblem算法计算得到q3；
当q3已知时，根据T1T- T12= roty(q2)(t23+ roty(q3)t3T)，以及Paden-Kahansubproblem算法计算得到q2。
可选地，q1最多存在有两组解，q3最多存在有两组解，q2 只有一组解，因此得到最多有四组不同的q1、q2、q3角度。
可选地，还包括：运动控制单元，所述运动控制单元，用于根据求解得到的第1关节、第2关节、第3关节对应的角度，以及第4关节、第5关节、第6关节对应的关节角度，控制机械臂通过奇异点。
第二方面，本申请实施例还提供一种具备协作机械臂的机器人，包括：基座、驱动系统、第一关节组件、第二关节组件、第三关节组件、第四关节组件、第五关节组件、第六关节组件，以及如第一方面中任一项所述的协作机械臂求解肩肘角度逆运动学解析解系统，其中：所述驱动系统根据求解得到的第1关节、第2关节、第3关节对应的角度，以及已知的机器人末端坐标，第4关节、第5关节、第6关节对应的角度，控制机械臂运动。
可选地，还包括：工控机和/或远程通讯的上位机，用于生成协作机械臂的初始运动轨迹，并基于求解得到的第1关节、第2关节、第3关节对应的角度，对所述初始运动轨迹进行插值处理，以使得插值后的运动轨迹能够平稳通过奇异点。
与现有技术相比，本发明具有如下的有益效果：本申请提供的协作机械臂求解肩肘角度逆运动学解析解系统设置有处理单元、传感器单元、驱动单元，传感器单元用以确定机器人末端的位置，处理单元包括：空间坐标系建立单元、关节角度调整单元、关节角度运算单元。通过空间坐标系建立单元建立机械臂关节坐标系，并根据机器人的六个自由度配置肩-肘-腕结构，由基座至末端分别定义第0~6关节；通过驱动单元通过机器人的第4轴、第5轴、第6轴来控制机械臂的朝向；通过关节角度调整单元在机械臂的朝向确定之后，分别获取第4轴、第5轴、第6轴的旋度变化量，得到第4关节、第5关节、第6关节对应的关节角度；关节角度运算单元，用于根据机器人末端坐标，以及第4关节、第5关节、第6关节对应的关节角度建立逆运动学方程，计算第1关节、第2关节、第3关节对应的角度。从而能够非常迅速地求解出机械臂所有关节所对应的角度，控制机械臂平稳顺利地通过奇异点，保证机械臂运动的连贯性和稳定性。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据提供的附图获得其他的附图。通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：
图1为本申请实施例提供的关节型六自由度机器人的结构简图；
图2为本申请实施例提供的一种协作机械臂求解肩肘角度逆运动学解析解系统的结构示意图；
图3为本申请实施例提供的另一种协作机械臂求解肩肘角度逆运动学解析解系统的结构示意图；
图4为本申请一实施例提供的协作机械臂求解肩肘角度逆运动学解析解设备的结构示意图；
图5是本发明实施例中的计算机可读存储介质的结构示意图。
图中：1-第1关节，2-第2关节，3-第3关节，4-第4关节，5-第5关节，6-第6关节，7-机器人末端。
具体实施方式
为使本申请实施例的目的、技术方案和优点更加清楚，下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。需要说明的是，当组件被称为“固定于”另一个组件，它可以直接在另一个组件上或者也可以存在居中的组件。当一个组件被认为是“连接”另一个组件，它可以是直接连接到另一个组件或者可能同时存在居中组件。
除非另有定义，本文所使用的所有的技术和科学术语与属于本申请的技术领域的技术人员通常理解的含义相同。本文中在本申请的说明书中所使用的术语只是为了描述具体的实施例的目的，不是旨在于限制本申请。本文所使用的术语“及／或”包括一个或多个相关的所列项目的任意的和所有的组合。
本发明的说明书和权利要求书及上述附图中的术语“第一”、“第二”、“第三”、“第四”等(如果存在)是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的本发明的实施例，例如能够以除了在这里图示或描述的那些以外的顺序实施。此外，术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或单元的过程、方法、系统、产品或设备不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。
下面以具体地实施例对本发明的技术方案以及本申请的技术方案如何解决上述技术问题进行详细说明。下面这几个具体的实施例可以相互结合，对于相同或相似的概念或过程可能在某些实施例中不再赘述。
下面结合附图，对本申请的一些实施方式作详细说明。在不冲突的情况下，下述的实施例及实施例中的特征可以相互组合。
图1为本申请实施例提供的关节型六自由度机器人的结构简图，如图1所示，首先建立机械臂关节坐标系，并根据机器人的六个自由度配置肩-肘-腕结构，由基座至末端分别定义第0~6关节，其中基座所在位置定义为第0关节，从下往上分别为第1关节1、第2关节2、第3关节3、第4关节4、第5关节5、第6关节6、机器人末端7；其次，以机器人基座对应的位置为原点，建立直角坐标系；其中，机器人末端坐标即为机械臂执行端需要到达的目标坐标位置。
示例性的，机械臂各个关节之间的距离是已知的，机器人末端坐标也是已知的。通过机器人的第4轴、第5轴、第6轴来控制机械臂的朝向，可以得到第4关节、第5关节、第6关节对应的关节角度。然后，根据机器人末端坐标，以及第4关节、第5关节、第6关节对应的关节角度建立逆运动学方程，计算第1关节、第2关节、第3关节对应的角度。从而能够非常迅速地求解出机械臂第1关节、第2关节、第3关节所对应的角度，控制机械臂顺利通过奇异点，保证机械臂运动的连贯性和稳定性。
图2为本申请实施例提供的一种协作机械臂求解肩肘角度逆运动学解析解系统的结构示意图，如图2所示，本实施例中的协作机械臂求解肩肘角度逆运动学解析解系统200可以包括：处理单元210、传感器单元220、驱动单元230，传感器单元220用以确定机器人末端的位置，处理单元210包括：空间坐标系建立单元211、关节角度调整单元212、关节角度运算单元213，其中：空间坐标系建立单元211，用于建立机械臂关节坐标系，并根据机器人的六个自由度配置肩-肘-腕结构，由基座至末端分别定义第0~6关节；驱动单元230，用于通过机器人的第4轴、第5轴、第6轴来控制机械臂的朝向；关节角度调整单元212，用于在机械臂的朝向确定之后，分别获取第4轴、第5轴、第6轴的旋度变化量，得到第4关节、第5关节、第6关节对应的关节角度；关节角度运算单元213，用于根据机器人末端坐标，以及第4关节、第5关节、第6关节对应的关节角度建立逆运动学方程，计算第1关节、第2关节、第3关节对应的角度。
示例性的，本实施例中的空间坐标系建立单元211，具体用于：以机器人基座对应的位置为原点，建立直角坐标系；其中，机器人末端坐标为机械臂执行端需要到达的目标坐标位置。
示例性的，本实施例中的关节角度调整单元212，具体用于：根据已知的机器人末端坐标，向驱动单元230发送运动控制指令，以使得驱动单元230通过调节机器人的第4轴、第5轴、第6轴来控制机械臂的朝向；在机械臂朝向预设方位时，分别记录下第4关节、第5关节、第6关节对应的旋度变化量，并基于关节初始状态时的关节角度，确定当前状态下第4关节、第5关节、第6关节对应的关节角度。
示例性的，本实施例中的关节角度运算单元213，具体用于：构建机器人末端坐标的正运动学方程；基于正运动学方程求解逆运动学的过程，反向计算出第1关节、第2关节、第3关节对应的关节角度。
在一种可选的实施方式中，机器人末端坐标的正运动方程如下：
t0T=t01+ rotz(q1)(t12+ roty(q2)(t23+ roty(q3)(t34+ roty(q4)(t45+ rotz(q5)(t56+ roty(q6)t6T)))))
式中：t0T表示末端法兰相对于第0关节的位移，t01表示第1关节相对于第0关节的位移，rotz(q1)表示绕Z轴旋转角度q1，t12表示第2关节相对于第1关节的位移，roty(q2)表示绕Y轴旋转角度q2，t23 表示第3关节相对于第2关节的位移，roty(q3)表示绕Y轴旋转角度q3，t34表示第4关节相对于第3关节的位移，roty(q4))表示绕Y轴旋转角度q4，t45表示第5关节相对于第4关节的位移，rotz(q5)表示绕Z轴旋转角度q5，t56表示第6关节相对于第5关节的位移，roty(q6)表示绕Y轴旋转角度q6，t6T表示末端法兰相对于第6关节的位移。
当第4关节、第5关节、第6关节对应的关节角度q4、q5、q6已知时，
t4T= roty(q4)(t45+ rotz(q5)(t56+ roty(q6)t6T))
式中：t4T表示末端法兰相对于第4关节的位移，将t4T代入t0T的计算公式，得到如下公式：
t0T= t01+ rotz(q1)(t12+ roty(q2)(t23 + roty(q3)(t34+ t4T)
求解逆运动学的过程：反向计算出第1关节、第2关节、第3关节对应的关节角度q1、q2、q3；
令t1T= t0T- t01 ；
式中：t1T表示末端法兰相对于第1关节的位移，将t1T的坐标记为[t1x，t1y ，t1z]，t1x表示关于X轴的坐标，t1y表示关于Y轴的坐标，t1z表示关于Z轴的坐标；
令T1T= t12+ roty(q2)(t23+ roty(q3)(t3T) ；
式中：t3T表示末端法兰相对于第3关节的位移，将T1T的坐标记为[T1x，T1y ，T1z]；
由于某一坐标绕相应轴旋转时，对应轴的分量保持不变，假设t12 、t23 、t3T关于Y轴方向变量分别为t12y 、t23y 、t3Ty，则的坐标存在如下关系：
T1z= t1z；
T1y= t12y+ t23y+ t3Ty；
其中，T1x = +/-sqrt(t1y* t1y+ t1x* t1x- T1y* T1y)；
当T1T已知时，根据Paden-Kahan subproblem算法计算得到q1；
当q1已知时，存在T1T= t12+ roty(q2)(t23+ roty(q3)t3T)，根据Paden-Kahansubproblem算法计算得到q3；
当q3已知时，根据T1T- T12= roty(q2)(t23+ roty(q3)t3T)，以及Paden-Kahansubproblem算法计算得到q2。
本实施例中，q1最多存在有两组解，q3最多存在有两组解，q2 只有一组解，因此得到最多有四组不同的q1、q2、q3角度。从而能够极大地增加运算速度，非常迅速地求解出机械臂所有关节所对应的角度。
图3为本申请实施例提供的另一种协作机械臂求解肩肘角度逆运动学解析解系统的结构示意图，如图3所示，本实施例中的系统可以包括：处理单元210、传感器单元220、驱动单元230、运动控制单元240，传感器单元220用以确定机器人末端的位置，处理单元210包括：空间坐标系建立单元211、关节角度调整单元212、关节角度运算单元213，其中，运动控制单元240，用于根据求解得到的第1关节、第2关节、第3关节对应的角度，以及第4关节、第5关节、第6关节对应的关节角度，控制机械臂通过奇异点。
本实施例中，由于可以快速计算出所有关节的角度，因此能够控制机械臂顺利通过奇异点，保证机械臂运动的连贯性和稳定性。
本申请实施例还提供一种具备协作机械臂的机器人，包括：基座、驱动系统、第一关节组件、第二关节组件、第三关节组件、第四关节组件、第五关节组件、第六关节组件，以及如图2、图3所示的协作机械臂求解肩肘角度逆运动学解析解系统，其中：驱动系统根据求解得到的第1关节、第2关节、第3关节对应的角度，以及已知的机器人末端坐标，第4关节、第5关节、第6关节对应的角度，控制机械臂运动。
示例性的，上述具备协作机械臂的机器人，还包括工控机和/或远程通讯的上位机，用于生成协作机械臂的初始运动轨迹，并基于求解得到的第1关节、第2关节、第3关节对应的角度，对初始运动轨迹进行插值处理，以使得插值后的运动轨迹能够平稳通过奇异点。
本申请实施例还提供的一种协作机械臂求解肩肘角度逆运动学解析解方法，可以包括如下步骤：
步骤S1：建立机械臂关节坐标系，并根据机器人的六个自由度配置肩-肘-腕结构，由基座至末端分别定义第0~6关节。
本实施例中所建立的机械臂关节坐标可以参见图1所示，以机器人基座对应的位置为原点，建立直角坐标系；其中，机器人末端坐标为机械臂执行端需要到达的目标坐标位置。然后分别定义基座至末端的关节。
步骤S2：通过机器人的第4轴、第5轴、第6轴来控制机械臂的朝向，得到第4关节、第5关节、第6关节对应的关节角度。
本实施例中，可以根据已知的机器人末端坐标，通过调节机器人的第4轴、第5轴、第6轴来控制机械臂的朝向，并在机械臂朝向预设方位时，分别记录下第4关节、第5关节、第6关节对应的关节角度。
步骤S3：根据机器人末端坐标，以及第4关节、第5关节、第6关节对应的关节角度建立逆运动学方程，计算第1关节、第2关节、第3关节对应的角度。
本实施例，通过建立机械臂关节坐标系，并根据机器人的六个自由度配置肩-肘-腕结构，由基座至末端分别定义第0~6关节；通过机器人的第4轴、第5轴、第6轴来控制机械臂的朝向，得到第4关节、第5关节、第6关节对应的关节角度；根据机器人末端坐标，以及第4关节、第5关节、第6关节对应的关节角度建立逆运动学方程，计算第1关节、第2关节、第3关节对应的角度。从而能够非常迅速地求解出机械臂第1关节、第2关节、第3关节所对应的角度，控制机械臂顺利通过奇异点，保证机械臂运动的连贯性和稳定性。
图4为本申请一实施例提供的协作机械臂求解肩肘角度逆运动学解析解设备的结构示意图，本实施例中的协作机械臂求解肩肘角度逆运动学解析解设备400可以包括：处理器401和存储器402。
存储器402，用于存储程序；存储器402，可以包括易失性存储器（英文：volatilememory），例如随机存取存储器（英文：random-access memory，缩写：RAM），如静态随机存取存储器（英文：static random-access memory，缩写：SRAM），双倍数据率同步动态随机存取存储器（英文：Double Data Rate Synchronous Dynamic Random Access Memory，缩写：DDR SDRAM）等；存储器也可以包括非易失性存储器（英文：non-volatile memory），例如快闪存储器（英文：flash memory）。存储器402用于存储计算机程序（如实现上述方法的应用程序、功能模块等）、计算机指令等，上述的计算机程序、计算机指令等可以分区存储在一个或多个存储器402中。并且上述的计算机程序、计算机指令、数据等可以被处理器401调用。
上述的计算机程序、计算机指令等可以分区存储在一个或多个存储器402中。并且上述的计算机程序、计算机指令、数据等可以被处理器401调用。
处理器401，用于执行存储器402存储的计算机程序，以实现上述实施例涉及的方法中的各个步骤。
具体可以参见前面方法实施例中的相关描述。
处理器401和存储器402可以是独立结构，也可以是集成在一起的集成结构。当处理器401和存储器402是独立结构时，存储器402、处理器401可以通过总线403耦合连接。
本实施例的协作机械臂求解肩肘角度逆运动学解析解设备400可以执行上述方法中的技术方案，其具体实现过程和技术原理参见上述方法中的相关描述，此处不再赘述。
所属技术领域的技术人员能够理解，本发明的各个方面可以实现为系统、方法或程序产品。因此，本发明的各个方面可以具体实现为以下形式，即：完全的硬件实施方式、完全的软件实施方式（包括固件、微代码等），或硬件和软件方面结合的实施方式，这里可以统称为“电路”、“模块”或“平台”。
此外，本申请实施例还提供一种计算机可读存储介质，计算机可读存储介质中存储有计算机执行指令，当用户设备的至少一个处理器执行该计算机执行指令时，用户设备执行上述各种可能的方法。
其中，计算机可读介质包括计算机存储介质和通信介质，其中通信介质包括便于从一个地方向另一个地方传送计算机程序的任何介质。存储介质可以是通用或专用计算机能够存取的任何可用介质。一种示例性的存储介质耦合至处理器，从而使处理器能够从该存储介质读取信息，且可向该存储介质写入信息。当然，存储介质也可以是处理器的组成部分。处理器和存储介质可以位于ASIC中。另外，该ASIC可以位于用户设备中。当然，处理器和存储介质也可以作为分立组件存在于通信设备中。
本申请还提供一种程序产品，程序产品包括计算机程序，计算机程序存储在可读存储介质中，服务器的至少一个处理器可以从可读存储介质读取计算机程序，至少一个处理器执行计算机程序使得服务器实施上述本发明实施例任一的方法。
本领域普通技术人员可以理解：实现上述方法实施例的全部或部分步骤可以通过程序指令相关的硬件来完成，前述的程序可以存储于一计算机可读取存储介质中，该程序在执行时，执行包括上述方法实施例的步骤；而前述的存储介质包括：只读内存（Read-OnlyMemory，ROM）、随机存取存储器（Random Access Memory，RAM）、磁碟或者光盘等各种可以存储程序代码的介质。
图5是本发明实施例中的计算机可读存储介质的结构示意图。参考图5所示，描述了根据本发明的实施方式的用于实现上述方法的程序产品500，其可以采用便携式紧凑盘只读存储器（CD-ROM）并包括程序代码，并可以在终端设备，例如个人电脑上运行。然而，本发明的程序产品不限于此，在本文件中，可读存储介质可以是任何包含或存储程序的有形介质，该程序可以被指令执行系统、装置或者器件使用或者与其结合使用。
程序产品可以采用一个或多个可读介质的任意组合。可读介质可以是可读信号介质或者可读存储介质。可读存储介质例如可以为但不限于电、磁、光、电磁、红外线、或半导体的系统、装置或器件，或者任意以上的组合。可读存储介质的更具体的例子（非穷举的列表）包括：具有一个或多个导线的电连接、便携式盘、硬盘、随机存取存储器（RAM）、只读存储器（ROM）、可擦式可编程只读存储器（EPROM或闪存）、光纤、便携式紧凑盘只读存储器（CD-ROM）、光存储器件、磁存储器件、或者上述的任意合适的组合。
计算机可读存储介质可以包括在基带中或者作为载波一部分传播的数据信号，其中承载了可读程序代码。这种传播的数据信号可以采用多种形式，包括但不限于电磁信号、光信号或上述的任意合适的组合。可读存储介质还可以是可读存储介质以外的任何可读介质，该可读介质可以发送、传播或者传输用于由指令执行系统、装置或者器件使用或者与其结合使用的程序。可读存储介质上包含的程序代码可以用任何适当的介质传输，包括但不限于无线、有线、光缆、RF等等，或者上述的任意合适的组合。
可以以一种或多种程序设计语言的任意组合来编写用于执行本发明操作的程序代码，程序设计语言包括面向对象的程序设计语言—诸如Java、C++等，还包括常规的过程式程序设计语言—诸如“C”语言或类似的程序设计语言。程序代码可以完全地在用户计算设备上执行、部分地在用户设备上执行、作为一个独立的软件包执行、部分在用户计算设备上部分在远程计算设备上执行、或者完全在远程计算设备或服务器上执行。在涉及远程计算设备的情形中，远程计算设备可以通过任意种类的网络，包括局域网（LAN）或广域网（WAN），连接到用户计算设备，或者，可以连接到外部计算设备（例如利用因特网服务提供商来通过因特网连接）。
本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。
以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变形或修改，这并不影响本发明的实质内容。

Claims (10)

1.一种协作机械臂求解肩肘角度逆运动学解析解系统，应用于具备六个自由度的机器人，所述系统包括：处理单元、传感器单元、驱动单元，所述传感器单元用以确定机器人末端的位置，其特征在于，所述处理单元包括：空间坐标系建立单元、关节角度调整单元、关节角度运算单元，其中：
所述空间坐标系建立单元，用于建立机械臂关节坐标系，并根据机器人的六个自由度配置肩-肘-腕结构，由基座至末端分别定义第0~6关节；
所述驱动单元，用于通过机器人的第4轴、第5轴、第6轴来控制机械臂的朝向；
所述关节角度调整单元，用于在机械臂的朝向确定之后，分别获取第4轴、第5轴、第6轴的旋度变化量，得到第4关节、第5关节、第6关节对应的关节角度；
所述关节角度运算单元，用于根据机器人末端坐标，以及第4关节、第5关节、第6关节对应的关节角度建立逆运动学方程，计算第1关节、第2关节、第3关节对应的角度。
2.根据权利要求1所述的协作机械臂求解肩肘角度逆运动学解析解系统，其特征在于，所述空间坐标系建立单元，具体用于：
以机器人基座对应的位置为原点，建立直角坐标系；其中，机器人末端坐标为机械臂执行端需要到达的目标坐标位置。
3.根据权利要求1所述的协作机械臂求解肩肘角度逆运动学解析解系统，其特征在于，所述关节角度调整单元，具体用于：
根据已知的机器人末端坐标，向所述驱动单元发送运动控制指令，以使得所述驱动单元通过调节机器人的第4轴、第5轴、第6轴来控制机械臂的朝向；
在机械臂朝向预设方位时，分别记录下第4关节、第5关节、第6关节对应的旋度变化量，并基于关节初始状态时的关节角度，确定当前状态下第4关节、第5关节、第6关节对应的关节角度。
4.根据权利要求1所述的协作机械臂求解肩肘角度逆运动学解析解系统，其特征在于，所述关节角度运算单元，具体用于：
构建机器人末端坐标的正运动学方程；
基于所述正运动学方程求解逆运动学的过程，反向计算出第1关节、第2关节、第3关节对应的关节角度。
5.根据权利要求4所述的协作机械臂求解肩肘角度逆运动学解析解系统，其特征在于，机器人末端坐标的正运动方程如下：
t0T=t01 + rotz(q1)(t12 + roty(q2)(t23 + roty(q3)(t34 + roty(q4)(t45 + rotz(q5)(t56 + roty(q6)t6T)))))
式中：t0T表示末端法兰相对于第0关节的位移，t01表示第1关节相对于第0关节的位移，rotz(q1)表示绕Z轴旋转角度q1，t12表示第2关节相对于第1关节的位移，roty(q2)表示绕Y轴旋转角度q2，t23 表示第3关节相对于第2关节的位移，roty(q3)表示绕Y轴旋转角度q3，t34表示第4关节相对于第3关节的位移，roty(q4))表示绕Y轴旋转角度q4，t45表示第5关节相对于第4关节的位移，rotz(q5)表示绕Z轴旋转角度q5，t56表示第6关节相对于第5关节的位移，roty(q6)表示绕Y轴旋转角度q6，t6T表示末端法兰相对于第6关节的位移。
6.根据权利要求5所述的协作机械臂求解肩肘角度逆运动学解析解系统，其特征在于，当第4关节、第5关节、第6关节对应的关节角度q4、q5、q6已知时，
t4T = roty(q4)(t45 + rotz(q5)(t56 + roty(q6)t6T))
式中：t4T表示末端法兰相对于第4关节的位移，将t4T代入t0T的计算公式，得到如下公式：
t0T = t01 + rotz(q1)(t12 + roty(q2)(t23 + roty(q3)(t34 + t4T)
求解逆运动学的过程：反向计算出第1关节、第2关节、第3关节对应的关节角度q1、q2、q3；
令t1T = t0T - t01 ；
式中：t1T表示末端法兰相对于第1关节的位移，将t1T的坐标记为[t1x，t1y ，t1z]，t1x表示关于X轴的坐标，t1y表示关于Y轴的坐标，t1z表示关于Z轴的坐标；
令T1T = t12 + roty(q2)(t23 + roty(q3)(t3T) ；
式中：t3T表示末端法兰相对于第3关节的位移，将T1T的坐标记为[T1x，T1y ，T1z]；
由于某一坐标绕相应轴旋转时，对应轴的分量保持不变，假设t12 、t23 、t3T关于Y轴方向变量分别为t12y 、t23y 、t3Ty，则的坐标存在如下关系：
T1z = t1z；
T1y = t12y + t23y + t3Ty；
其中，T1x = +/-sqrt(t1y * t1y + t1x * t1x - T1y * T1y)；
当T1T已知时，根据Paden-Kahan subproblem算法计算得到q1；
当q1已知时，存在T1T = t12 + roty(q2)(t23 + roty(q3)t3T)，根据Paden-Kahansubproblem算法计算得到q3；
当q3已知时，根据T1T - T12 = roty(q2)(t23 + roty(q3)t3T)，以及Paden-Kahansubproblem算法计算得到q2。
7.根据权利要求6所述的协作机械臂求解肩肘角度逆运动学解析解系统，其特征在于，q1最多存在有两组解，q3最多存在有两组解，q2 只有一组解，因此得到最多有四组不同的q1、q2、q3角度。
8.根据权利要求1-7任一项所述的协作机械臂求解肩肘角度逆运动学解析解系统，其特征在于，还包括：运动控制单元，所述运动控制单元，用于根据求解得到的第1关节、第2关节、第3关节对应的角度，以及第4关节、第5关节、第6关节对应的关节角度，控制机械臂通过奇异点。
9.一种具备协作机械臂的机器人，其特征在于，包括：基座、驱动系统、第一关节组件、第二关节组件、第三关节组件、第四关节组件、第五关节组件、第六关节组件，以及如权利要求1-8中任一项所述的协作机械臂求解肩肘角度逆运动学解析解系统，其中：
所述驱动系统根据求解得到的第1关节、第2关节、第3关节对应的角度，以及已知的机器人末端坐标，第4关节、第5关节、第6关节对应的角度，控制机械臂运动。
10.根据权利要求9所述的具备协作机械臂的机器人，其特征在于，还包括：工控机和/或远程通讯的上位机，用于生成协作机械臂的初始运动轨迹，并基于求解得到的第1关节、第2关节、第3关节对应的角度，对所述初始运动轨迹进行插值处理，以使得插值后的运动轨迹能够平稳通过奇异点。
CN202311713282.5A 2023-12-14 2023-12-14 协作机械臂求解肩肘角度逆运动学解析解系统 Active CN117407630B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202311713282.5A CN117407630B (zh) 2023-12-14 2023-12-14 协作机械臂求解肩肘角度逆运动学解析解系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202311713282.5A CN117407630B (zh) 2023-12-14 2023-12-14 协作机械臂求解肩肘角度逆运动学解析解系统

Publications (2)

Publication Number Publication Date
CN117407630A true CN117407630A (zh) 2024-01-16
CN117407630B CN117407630B (zh) 2024-04-30

Family

ID=89492845

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202311713282.5A Active CN117407630B (zh) 2023-12-14 2023-12-14 协作机械臂求解肩肘角度逆运动学解析解系统

Country Status (1)

Country Link
CN (1) CN117407630B (zh)

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20050143860A1 (en) * 2003-12-26 2005-06-30 Japan Aerospace Exploration Agency Method of controlling redundant manipulator
CN104635762A (zh) * 2015-01-13 2015-05-20 北京航空航天大学 一种面向srs仿人手臂的自运动角计算方法
CN107589934A (zh) * 2017-07-24 2018-01-16 大连理工大学 一种关节型机械臂逆运动学解析解的求取方法
CN107943283A (zh) * 2017-11-08 2018-04-20 浙江工业大学 基于手势识别的机械臂位姿控制系统
CN108638069A (zh) * 2018-05-18 2018-10-12 南昌大学 一种机械臂末端精确运动控制方法
CN112597437A (zh) * 2020-12-29 2021-04-02 深圳市优必选科技股份有限公司 机械臂的逆运动学解析方法、装置及设备

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20050143860A1 (en) * 2003-12-26 2005-06-30 Japan Aerospace Exploration Agency Method of controlling redundant manipulator
CN104635762A (zh) * 2015-01-13 2015-05-20 北京航空航天大学 一种面向srs仿人手臂的自运动角计算方法
CN107589934A (zh) * 2017-07-24 2018-01-16 大连理工大学 一种关节型机械臂逆运动学解析解的求取方法
CN107943283A (zh) * 2017-11-08 2018-04-20 浙江工业大学 基于手势识别的机械臂位姿控制系统
CN108638069A (zh) * 2018-05-18 2018-10-12 南昌大学 一种机械臂末端精确运动控制方法
CN112597437A (zh) * 2020-12-29 2021-04-02 深圳市优必选科技股份有限公司 机械臂的逆运动学解析方法、装置及设备

Also Published As

Publication number Publication date
CN117407630B (zh) 2024-04-30

Similar Documents

Publication Publication Date Title
CN111360827A (zh) 一种视觉伺服切换控制方法及系统
CN108406769A (zh) 串联机器人的关节旋转角度确定方法及装置
CN112959326B (zh) 机器人正运动学求解方法、装置、可读存储介质及机器人
CN107253191B (zh) 一种双机械臂系统及其协调控制方法
CN113715016B (zh) 一种基于3d视觉的机器人抓取方法、系统、装置及介质
WO2023024278A1 (zh) 机器人关节位姿优化方法、机器人控制方法和机器人
KR20220155921A (ko) 로봇 디바이스를 제어하기 위한 방법
CN113182681A (zh) 一种壳盖侧缝的焊接轨迹获取方法及焊接方法
CN117407630B (zh) 协作机械臂求解肩肘角度逆运动学解析解系统
CN113199481B (zh) 机器人运动控制方法、装置、电子设备及介质
CN112356032A (zh) 一种姿态平滑过渡方法及系统
CN113246131A (zh) 运动捕捉方法、装置、电子设备以及机械臂控制系统
CN116214510A (zh) 一种机械臂导纳控制方法和系统
CN117817659A (zh) 协作机械臂求解肩肘角度逆运动学解析解方法
CN108115672B (zh) 一种斜交腕喷涂机器人的运动控制系统及其方法
CN111761586A (zh) 一种基于大数据的远程控制机器人
CN114494446A (zh) 外参标定方法及其系统和电子设备
Zhao et al. Model and simulation of the mitsubishi rv-m1 robot using matlab
CN111823215A (zh) 一种工业机器人的同步控制方法及装置
CN118061169A (zh) 通过奇异点的机器人控制系统和机器人
Zhang et al. Trajectory planning of collaborative robot for 3c products assembly
CN118061168A (zh) 通过奇异点的机器人控制方法、装置、设备及存储介质
CN117428791B (zh) 一种用于肩部四轴康复机器人的逆运动学求解方法及系统
CN110919639A (zh) 一种关节式示教臂及基于该示教臂的示教方法
CN117182932B (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