CN115488860A - 基于正向运动学确定并联机器人位置的方法及装置 - Google Patents

基于正向运动学确定并联机器人位置的方法及装置 Download PDF

Info

Publication number
CN115488860A
CN115488860A CN202211214966.6A CN202211214966A CN115488860A CN 115488860 A CN115488860 A CN 115488860A CN 202211214966 A CN202211214966 A CN 202211214966A CN 115488860 A CN115488860 A CN 115488860A
Authority
CN
China
Prior art keywords
joint
transformation matrix
coordinate system
determining
joint angle
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
CN202211214966.6A
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.)
Beijing CHL Robotics Co ltd
Original Assignee
Beijing CHL Robotics 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 Beijing CHL Robotics Co ltd filed Critical Beijing CHL Robotics Co ltd
Priority to CN202211214966.6A priority Critical patent/CN115488860A/zh
Publication of CN115488860A publication Critical patent/CN115488860A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • 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/003—Programme-controlled manipulators having parallel kinematics
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J17/00—Joints
    • B25J17/02—Wrist joints
    • B25J17/0258—Two-dimensional joints
    • 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)
  • General Physics & Mathematics (AREA)
  • Mathematical Physics (AREA)
  • Pure & Applied Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Mathematical Optimization (AREA)
  • Mechanical Engineering (AREA)
  • Computational Mathematics (AREA)
  • Robotics (AREA)
  • Data Mining & Analysis (AREA)
  • Mathematical Analysis (AREA)
  • Software Systems (AREA)
  • General Engineering & Computer Science (AREA)
  • Databases & Information Systems (AREA)
  • Algebra (AREA)
  • Computing Systems (AREA)
  • Manipulator (AREA)
  • Numerical Control (AREA)

Abstract

本发明实施例公开了一种基于正向运动学确定并联机器人位置的方法及装置，该方法首先将并联机器人的闭链结构划分为第一开链结构和第二开链结构；第一开链结构和第二开链结构均是由各关节连接形成的；其次基于第三关节角的位置和自身轴长，以及第七关节角的位置和自身轴长，计算得到第四关节角在基础坐标系中的位置坐标；最后基于基础坐标系、世界坐标系、第四关节角的位置坐标第一开链结构的各关节角，以及并联机器人的标准DH参数，确定变换矩阵
Figure DDA0003875803660000011
并从变换矩阵

Description

基于正向运动学确定并联机器人位置的方法及装置
技术领域
本发明属于并联机器人技术领域，尤其涉及一种基于正向运动学确定并联机器人位置的方法及装置。
背景技术
串联机器人或者并联机器人的运动学分析包括正运动学问题和逆运动学问题。逆运动学问题定义是已知动平台的位置和姿态信息，求解若干独立的输入运动，对于串联机器人或并联机器人而言，多个输入运动的表达式是独立的，能并行计算，快速求解。正运动学问题则是在已知多个输入运动的条件下，求解动平台的位置和姿态。
对于并联机器人，逆运动学求解往往相对明确，例如：能够根据平台的形状有效确定机器人的关节值；而正向运动学求解可能相当复杂，无法根据平台形状准确确定机器人的关节值，例如：不论多自由度机器人还是少自由度机器人，机构都存在一定程度的耦合现象，这就导致并联机器人的正运动学解可能对应平台多种位形。
发明内容
本发明提供一种基于正向运动学确定并联机器人位置的方法及装置。该方法能够提高并联机器人位置计算的速率和准确性。
为实现上述目的，根据本申请实施例第一方面提供一种基于正向运动学确定并联机器人位置的方法，该方法包括将并联机器人由第一关节、第二关节、第三关节、第四关节、第五关节、第六关节以及第七关节连接形成的闭链结构划分为第一开链结构和第二开链结构；所述第一开链结构和所述第二开链结构均是由各关节连接形成的；将第三关节角位置为圆心和第三关节自身轴长为半径形成的第一圆，与第七关节角位置为圆心和第七关节自身轴长为半径形成的第二圆，进行相交，计算得到第四关节角在基础坐标系中的位置坐标；基于基础坐标系、世界坐标系、所述第四关节角在基础坐标系中的位置坐标、所述第一开链结构的各关节角，以及并联机器人的标准DH参数，确定法兰末端到世界坐标系的变换矩阵
Figure BDA0003875803640000021
从所述变换矩阵
Figure BDA0003875803640000022
中提取法兰末端对应的位置坐标。
可选的，当所述第一开链结构是由第一关节、第二关节、第三关节、第四关节，以及第五关节连接形成时；所述基于基础坐标系、世界坐标系、所述第四关节角在基础坐标系中的位置坐标、所述第一开链结构的各关节角，以及所述并联机器人的标准DH参数，确定法兰末端到世界坐标系的变换矩阵
Figure BDA0003875803640000023
包括：将所述第四关节角在基础坐标系中的位置坐标与所述第三关节角作为变量的变换矩阵
Figure BDA0003875803640000024
建立元素对应关系，计算得到第三关节角θ3；获取第一关节沿基础坐标系Z轴的移动距离d1、第二关节角θ2、第四关节角θ4，以及第五关节角θ5；基于基础坐标系、世界坐标系、所述并联机器人的第一标准DH参数、所述第一关节沿基础坐标系Z轴的移动距离d1、所述第二关节角θ2、所述第三关节角θ3、所述第四关节角θ4，以及所述第五关节角θ5，确定法兰末端到世界坐标系的变换矩阵
Figure BDA0003875803640000025
可选的，所述基于基础坐标系、世界坐标系、所述并联机器人的第一标准DH参数、所述第一关节沿基础坐标系Z轴的移动距离d1、所述第二关节角θ2、所述第三关节角θ3、所述第四关节角θ4，以及所述第五关节角θ5，确定法兰末端到世界坐标系的变换矩阵
Figure BDA0003875803640000026
包括：基于所述第一关节沿基础坐标系Z轴的移动距离d1，确定第一关节到基础坐标系的变换矩阵
Figure BDA0003875803640000031
基于所述第二关节角θ2和所述第二关节对应的第一标准DH参数，确定第二关节到第一关节的变换矩阵
Figure BDA0003875803640000032
基于所述第三关节角θ3和所述第三关节对应第一标准DH参数，确定第三关节到第二关节的变换矩阵
Figure BDA0003875803640000033
基于所述第四关节角θ4和所述第四关节对应的第一标准DH参数，确定第四关节到第三关节的变换矩阵
Figure BDA0003875803640000034
基于第五关节角θ5和所述第五关节对应的第一标准DH参数，确定第五关节到第四关节的变换矩阵
Figure BDA0003875803640000035
基于法兰对应的第一标准DH参数，确定法兰末端到第五关节的变换矩阵
Figure BDA0003875803640000036
基于所述基础坐标系到世界坐标系的变换矩阵
Figure BDA0003875803640000037
所述变换矩阵
Figure BDA0003875803640000038
所述变换矩阵
Figure BDA0003875803640000039
所述变换矩阵
Figure BDA00038758036400000310
所述变换矩阵
Figure BDA00038758036400000311
所述变换矩阵
Figure BDA00038758036400000312
以及变换矩阵
Figure BDA00038758036400000313
确定法兰末端到世界坐标系的变换矩阵
Figure BDA00038758036400000314
可选的，当所述第一开链结构是由第一关节、第六关节、第七关节、第四关节，以及第五关节连接形成时；所述基于基础坐标系、世界坐标系、所述第四关节角在基础坐标系中的位置坐标、所述第一开链结构的各关节角，以及所述并联机器人的第二标准DH参数，确定法兰末端到世界坐标系的变换矩阵
Figure BDA00038758036400000315
包括：将所述位置坐标与所述第七关节角作为变量的变换矩阵
Figure BDA00038758036400000316
建立元素对应关系，得到第七关节角θ7；获取第一关节沿基础坐标系Z轴的移动距离d1、第六关节角θ6、第四关节角θ4，以及第五关节角θ5；基于基础坐标系、世界坐标系、所述并联机器人的第二标准DH参数，所述第一关节沿基础坐标系Z轴的移动距离d1、所述第六关节角θ6、所述第七关节角θ7、所述第四关节角θ4，以及所述第五关节角θ5，确定法兰末端到世界坐标系的变换矩阵
Figure BDA00038758036400000317
可选的，所述基于基础坐标系、世界坐标系、所述并联机器人的第二标准DH参数，所述第一关节沿基础坐标系Z轴的移动距离d1、所述第六关节角θ6、所述第七关节角θ7、所述第四关节角θ4，以及所述第五关节角θ5，确定法兰末端到世界坐标系的变换矩阵
Figure BDA0003875803640000041
包括：基于所述第一关节沿基础坐标系Z轴的移动距离d1，确定第一关节到基础坐标系的变换矩阵
Figure BDA0003875803640000042
基于所述第六关节角θ6和所述第六关节对应的第二标准DH参数，确定第六关节到第一关节的变换矩阵
Figure BDA0003875803640000043
基于所述第七关节角θ7和所述第七关节对应第二标准DH参数，确定第七关节到第六关节的变换矩阵
Figure BDA0003875803640000044
基于所述第四关节角θ4和所述第四关节对应的第二标准DH参数，确定第四关节到第七关节的变换矩阵
Figure BDA0003875803640000045
基于第五关节角θ5和所述第五关节对应的第二标准DH参数，确定第五关节到第四关节的变换矩阵
Figure BDA0003875803640000046
基于法兰对应的第二标准DH参数，确定法兰末端到第五关节的变换矩阵
Figure BDA0003875803640000047
基于所述变换矩阵
Figure BDA0003875803640000048
所述变换矩阵
Figure BDA0003875803640000049
所述变换矩阵
Figure BDA00038758036400000410
所述变换矩阵
Figure BDA00038758036400000411
所述变换矩阵
Figure BDA00038758036400000412
所述变换矩阵
Figure BDA00038758036400000413
以及变换矩阵
Figure BDA00038758036400000414
确定法兰末端到世界坐标系的变换矩阵
Figure BDA00038758036400000415
可选的，所述从所述法兰末端到世界坐标系的变换矩阵
Figure BDA00038758036400000416
中提取法兰末端对应的位置坐标，包括：获取法兰末端到世界坐标系的变换矩阵
Figure BDA00038758036400000417
将所述变换矩阵
Figure BDA00038758036400000418
中前三行元素按列划分成第一列、第二列、第三列和第四列；将所述第四列的元素确定为法兰末端对应的位置坐标。
为实现上述目的，本实施例第二方面还提供一种基于正向运动学确定并联机器人位置的装置，所述装置包括：划分模块，用于将并联机器人由第一关节、第二关节、第三关节、第四关节、第五关节、第六关节以及第七关节连接形成的闭链结构划分为第一开链结构和第二开链结构；所述第一开链结构和所述第二开链结构均是由各关节连接形成的；计算模块，用于将第三关节角位置为圆心和第三关节自身轴长为半径形成的第一圆，与第七关节角位置为圆心和第七关节自身轴长为半径形成的第二圆，进行相交，计算得到第四关节角在基础坐标系中的位置坐标；确定模块，用于基于基础坐标系、世界坐标系、所述第四关节角在基础坐标系中的位置坐标、所述第一开链结构的各关节角，以及并联机器人的标准DH参数，确定法兰末端到世界坐标系的变换矩阵
Figure BDA0003875803640000051
提取模块，用于从所述变换矩阵
Figure BDA0003875803640000052
中提取法兰末端对应的位置坐标。
可选的，当所述第一开链结构是由第一关节、第二关节、第三关节、第四关节，以及第五关节连接形成时；所述确定模块包括：计算单元，用于将所述第四关节角在基础坐标系中的位置坐标与所述第三关节角作为变量的变换矩阵
Figure BDA0003875803640000053
建立元素对应关系，计算得到第三关节角θ3；获取单元，用于获取第一关节沿基础坐标系Z轴的移动距离d1、第二关节角θ2、第四关节角θ4，以及第五关节角θ5；确定单元，用于基于基础坐标系、世界坐标系、所述并联机器人的第一标准DH参数、所述第一关节沿基础坐标系Z轴的移动距离d1、所述第二关节角θ2、所述第三关节角θ3、所述第四关节角θ4，以及所述第五关节角θ5，确定法兰末端到世界坐标系的变换矩阵
Figure BDA0003875803640000054
可选的，所述确定单元包括：第一确定子单元，用于基于所述第一关节沿基础坐标系Z轴的移动距离d1，确定第一关节到基础坐标系的变换矩阵
Figure BDA0003875803640000055
基于所述第二关节角θ2和所述第二关节对应的第一标准DH参数，确定第二关节到第一关节的变换矩阵
Figure BDA0003875803640000056
基于所述第三关节角θ3和所述第三关节对应第一标准DH参数，确定第三关节到第二关节的变换矩阵
Figure BDA0003875803640000057
基于所述第四关节角θ4和所述第四关节对应的第一标准DH参数，确定第四关节到第三关节的变换矩阵
Figure BDA0003875803640000061
基于第五关节角θ5和所述第五关节对应的第一标准DH参数，确定第五关节到第四关节的变换矩阵
Figure BDA0003875803640000062
基于法兰对应的第一标准DH参数，确定法兰末端到第五关节的变换矩阵
Figure BDA0003875803640000063
第二确定子单元，用于基于所述基础坐标系到世界坐标系的变换矩阵
Figure BDA0003875803640000064
所述变换矩阵
Figure BDA0003875803640000065
所述变换矩阵
Figure BDA0003875803640000066
所述变换矩阵
Figure BDA0003875803640000067
所述变换矩阵
Figure BDA0003875803640000068
所述变换矩阵
Figure BDA0003875803640000069
以及变换矩阵
Figure BDA00038758036400000610
确定法兰末端到世界坐标系的变换矩阵
Figure BDA00038758036400000611
为实现上述目的，根据本申请实施例第三方面提供一种计算机可读介质，其上存储有计算机程序，所述程序被处理器执行时实现如第一方面所述的方法。
与现有技术相比，本发明实施例提供一种基于逆向运动学确定并联机器人姿态的方法及装置，该方法包括：首先，将并联机器人由第一关节、第二关节、第三关节、第四关节、第五关节、第六关节以及第七关节连接形成的闭链结构划分为第一开链结构和第二开链结构；所述第一开链结构和所述第二开链结构均是由各关节连接形成的；其次，将第三关节角位置为圆心和第三关节自身轴长为半径形成的第一圆，与第七关节角位置为圆心和第七关节自身轴长为半径形成的第二圆，进行相交，计算得到第四关节角在基础坐标系中的位置坐标；之后，基于基础坐标系、世界坐标系、所述第四关节角在基础坐标系中的位置坐标、所述第一开链结构的各关节角，以及并联机器人的标准DH参数，确定法兰末端到世界坐标系的变换矩阵
Figure BDA00038758036400000612
最后，从所述变换矩阵
Figure BDA00038758036400000613
中提取法兰末端对应的位置坐标。由此，通过将闭链结构拆分成两个开链结构，并基于第四关节在基础坐标系中的位置坐标确定从动轴的关节角，从而提高了并联机器人位置计算的速率和准确性。
附图说明
后文将参照附图以示例性而非限制性的方式详细描述本发明的一些具体实施例。附图中相同的附图标记标示了相同或类似的部件或部分。本领域技术人员应该理解，这些附图未必是按比例绘制的。附图中：
图1为本发明一实施例提供的基于正向运动学确定并联机器人位置的方法的流程示意图；
图2为本发明一实施例中基于第一种情况确定法兰末端到世界坐标系的变换矩阵
Figure BDA0003875803640000071
的流程示意图；
图3为本发明一实施例中基于第二种情况确定法兰末端到世界坐标系的变换矩阵
Figure BDA0003875803640000072
的流程示意图；
图4为本发明一实施例提供的并联机器人的第一标准DH参数的结构示意图；
图5为本发明一实施例提供的并联机器人的第二标准DH参数的结构示意图；
图6为本发明一实施例提供的并联机器人的结构示意图；
图7为本发明一实施例提供的基于正向运动学确定并联机器人位置的装置的结构示意图。
具体实施方式
为使本发明的目的、特征、优点能够更加的明显和易懂，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而非全部实施例。基于本发明中的实施例，本领域技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
如图1所示，为本发明一实施例提供的基于正向运动学确定并联机器人位置的方法的流程示意图；如图4所示，为本发明一实施例提供的并联机器人第一标准DH参数的结构示意图；图5为本发明一实施例提供的并联机器人的第二标准DH参数的结构示意图；如图6所示，为本发明一实施例提供的并联机器人的结构示意图。
一种基于正向运动学确定并联机器人位置的方法，所述方法至少包括如下步骤：
S101，将并联机器人由第一关节、第二关节、第三关节、第四关节、第五关节、第六关节以及第七关节连接形成的闭链结构划分为第一开链结构和第二开链结构；所述第一开链结构和所述第二开链结构均是由各关节连接形成的；
S102，将第三关节角位置为圆心和第三关节自身轴长为半径形成的第一圆，与第七关节角位置为圆心和第七关节自身轴长为半径形成的第二圆，进行相交，计算得到第四关节角在基础坐标系中的位置坐标；
S103，基于基础坐标系、世界坐标系、所述第四关节角在基础坐标系中的位置坐标、所述第一开链结构的各关节角，以及并联机器人的标准DH参数，确定法兰末端到世界坐标系的变换矩阵；
S104，从法兰末端到世界坐标系的变换矩阵中提取法兰末端对应的位置坐标。
在S101中，如图5所示，例如：将并联机器人的第一关节、第二关节、第三关节、第四关节、第五关节、第六关节以及第七关节组合后形成闭链结构。在闭链结构中，由第一关节、第二关节、第三关节、第四关节和第五关节依次连接形成的第一开链结构与由第六关节和第七关节依次连接形成的第二开链结构是并联关系；或者，在闭链结构中，由第一关节、第六关节、第七关节、第四关节和第五关节依次连接形成的第一开链结构和由第二关节和第三关节依次连接形成的第二开链结构是并联关系；其中，第一开链结构是主轴，第二开链结构是副轴。
由此，将闭链结构拆分成两个开链结构，有利于提高并联机器人位置计算的速度。
需要说明的是，第三关节和第七关节是从动轴；第一关节、第二关节、第四关节、第五关节和第六关节是主动轴。
在S102中，第一标准DH参数如图4所示，其中，θdh，x(°)、ddh,x、dah,x与adh，x(°)中出现的x表示关节轴的编号，例如x＝1,2,3,4,5,6,7。
当第一开链结构是由第一关节、第二关节、第三关节、第四关节和第五关节依次连接后形成的结构时，则获取并联机器人第一标准DH参数、第一关节沿基础坐标系Z轴的移动距离d1、第二关节角θ2、第四关节角θ4、第五关节角θ5和第六关节角θ6；之后，基于并联机器人第一关节对应的第一标准DH参数和移动距离d1，确定变换矩阵
Figure BDA0003875803640000091
基于并联机器人第二关节对应的第一标准DH参数和第二关节角θ2，确定变换矩阵
Figure BDA0003875803640000092
基于并联机器人第三关节对应的第一标准DH参数和作为变量的第三关节角θ3，确定含有θ3变量的变换矩阵
Figure BDA0003875803640000093
基于并联机器人第四关节对应的第一标准DH参数和第四关节角θ4，确定变换矩阵
Figure BDA0003875803640000094
其中，
Figure BDA0003875803640000095
表示第一关节到基础坐标系的变换矩阵；
Figure BDA0003875803640000096
表示第二关节到第一关节的变换矩阵，
Figure BDA0003875803640000097
表示第三关节到第二关节的变换矩阵，
Figure BDA0003875803640000098
表示第四关节到第三关节的变换矩阵。最后基于变换矩阵
Figure BDA0003875803640000099
变换矩阵
Figure BDA00038758036400000910
含有θ3变量的变换矩阵
Figure BDA00038758036400000911
以及变换矩阵
Figure BDA00038758036400000912
确定含有θ3变量的变换矩阵
Figure BDA00038758036400000913
例如：含有θ3变量的变换矩阵
Figure BDA00038758036400000914
的具体计算公式如下式(1)所示：
Figure BDA00038758036400000915
当第一开链结构是由第一关节、第六关节、第七关节、第四关节和第五关节连接后形成的结构时，则获取并联机器人第二标准DH参数、第一关节沿基础坐标系Z轴的移动距离d1、第四关节角θ4、第五关节角θ5和第六关节角θ6；之后，基于并联机器人第一关节对应的第二标准DH参数和移动距离d1，确定变换矩阵
Figure BDA0003875803640000101
基于并联机器人第六关节对应的第二标准参数和第六关节角θ6，确定变换矩阵
Figure BDA0003875803640000102
基于并联机器人第七关节对应的第二标准参数和作为变量的第七关节角θ7，确定含有θ7变量的变换矩阵
Figure BDA0003875803640000103
基于并联机器人第四关节对应的第二标准参数和第四关节角θ4，确定变换矩阵
Figure BDA0003875803640000104
其中，
Figure BDA0003875803640000105
表示第六关节到第一关节的变换矩阵，
Figure BDA0003875803640000106
表示第七关节到第六关节的变换矩阵，
Figure BDA0003875803640000107
表示第四关节到第七关节的变换矩阵；最后基于变换矩阵
Figure BDA0003875803640000108
变换矩阵
Figure BDA0003875803640000109
含有θ7变量的变换矩阵
Figure BDA00038758036400001010
以及变换矩阵
Figure BDA00038758036400001011
确定含有θ7变量的变换矩阵
Figure BDA00038758036400001012
变换矩阵
Figure BDA00038758036400001013
的具体计算公式如下式(2)所示：
Figure BDA00038758036400001014
假设第四关节在基础坐标系中的位置坐标为(x，y，z)；从含有θ3变量的变换矩阵
Figure BDA00038758036400001015
中提取第四列元素如下式(3)所述：
Figure BDA00038758036400001016
通过式(3)可知，第四关节在基础坐标系中的位置坐标为(
Figure BDA00038758036400001017
Figure BDA0003875803640000111
其中：
Figure BDA0003875803640000112
代表式(3)第一行元素，
Figure BDA0003875803640000113
代表式(3)第二行元素，
Figure BDA0003875803640000114
代表式(3)第三行元素。
由此，建立如下式(4)所示的方程组：
Figure BDA0003875803640000115
之后，x和y减去所有不带θ3的数后，平方和则为
Figure BDA0003875803640000116
具体公式如下式(5)所示：
Figure BDA0003875803640000117
同理，从含有θ7变量的变换矩阵
Figure BDA0003875803640000118
中提取第四关节在基础坐标系中的位置坐标为
Figure BDA0003875803640000119
由此，建立如下式(6)所示的方程组：
Figure BDA00038758036400001110
式(6)化简得到如下公式(7)所示：
Figure BDA00038758036400001111
将式(5)和式(7)进行联立方程组求解，得到交点(x，y)；从而获得第四关节在基础坐标系中的位置坐标(x，y，z)。
在S103中，第一种情况：以第一关节、第二关节、第三关节、第四关节，以及第五关节连接后形成的第一开链结构为主轴，并以第六关节和第七关节连接后形成的第二开链结构为副轴，，并联机器人的标准参数为第一标准DH参数。在图2所示的第二标准DH参数的示意图中，以法兰为分界线分别进行说明，位于法兰之前的各关节：第二关节是从第一关节派生且第二关节对应的DH参数是以第一关节为参考计算的，第三关节是从第二关节派生且第三关节对应的DH参数是以第二关节为参考计算的，第四关节是从第三关节派生且第四关节对应的DH参数是以第三关节为参考计算的，第五关节是从第四关节派生且第五关节对应的DH参数是以第四关节为参考计算的。
示例性的，首先，根据求取的位置坐标(x，y，z)，结合公式(4)，求解第三关节角θ3；其次，基于基础坐标系、世界坐标系、第四关节角在基础坐标系中的位置坐标、第一关节角、第二关节角、第三关节角、第四关节角、第五关节角，以及并联机器人的第一标准DH参数，确定法兰末端到世界坐标系的变换矩阵。
第二种情况：以第一关节、第六关节、第七关节、第四关节和第五关节连接后形成的第一开链结构为主轴，且以第二关节和第三关节连接后形成第二开链结构为副轴，并联机器人的标准参数为第二标准DH参数。
在图3所示的第二标准DH参数的示意图中，以法兰为分界线分别进行说明，位于法兰之前的各关节：第六关节是从第一关节派生且第六关节对应的DH参数是以第一关节为参考计算的，第七关节是从第六关节派生且第七关节对应的DH参数是以第六关节为参考计算的，第四关节是从第七关节派生且第四关节对应的DH参数是以第七关节为参考计算的，第五关节是以第四关节派生且第五关节对应的DH参数是以第四关节为参考计算的。
示例性的，首先，根据求取的位置坐标(x，y，z)，结合公式(6)，求解第七关节角θ7；其次，基于基础坐标系、世界坐标系、第四关节角在基础坐标系中的位置坐标、第一关节角、第六关节角、第七关节角、第四关节角、第五关节角，以及并联机器人的第二标准参数，确定法兰末端到世界坐标系的变换矩阵。
在S104中，获取法兰末端到世界坐标系的变换矩阵
Figure BDA0003875803640000131
将变换矩阵
Figure BDA0003875803640000132
中前三行元素按列划分成第一列、第二列、第三列和第四列；将第四列的元素确定为法兰末端对应的位置坐标。
由此，本实施例通过将闭链结构拆解成两个开链结构，并基于第三关节和第七关节的交点确定第四关节在基础坐标系中的位置坐标，从而有利于从动轴的求解，提高了并联机器人位置计算的准确率和效率。
如图2所示，为本发明一实施例中基于第一种情况确定法兰末端到世界坐标系的变换矩阵
Figure BDA0003875803640000133
的流程示意图。在本实施例一优选的实施方式中，当第一开链结构是由第一关节、第二关节、第三关节、第四关节，以及第五关节连接形成时；确定法兰末端到世界坐标系的变换矩阵
Figure BDA0003875803640000134
至少包括如下步骤：
S201，将位置坐标与第三关节角作为变量的用于第四关节到基础坐标系的变换矩阵建立元素对应关系，计算得到第三关节角；
S202，获取第一关节沿基础坐标系Z轴的移动距离、第二关节角、第四关节角，以及第五关节角；
S203，基于基础坐标系、世界坐标系、并联机器人的第一标准DH参数、第一关节沿基础坐标系Z轴的移动距离、第二关节角、所述第三关节角、第四关节角，以及第五关节角，确定法兰末端到世界坐标系的变换矩阵。
具体地，基于所述第一关节沿基础坐标系Z轴的移动距离d1，确定第一关节到基础坐标系的变换矩阵
Figure BDA0003875803640000141
基于所述第二关节角θ2和所述第二关节对应的第一标准DH参数，确定第二关节到第一关节的变换矩阵
Figure BDA0003875803640000142
基于所述第三关节角θ3和所述第三关节对应第一标准DH参数，确定第三关节到第二关节的变换矩阵
Figure BDA0003875803640000143
基于所述第四关节角θ4和所述第四关节对应的第一标准DH参数，确定第四关节到第三关节的变换矩阵
Figure BDA0003875803640000144
基于第五关节角θ5和所述第五关节对应的第一标准DH参数，确定第五关节到第四关节的变换矩阵
Figure BDA0003875803640000145
基于法兰对应的第一标准DH参数，确定法兰末端到第五关节的变换矩阵
Figure BDA0003875803640000146
基于所述基础坐标系到世界坐标系的变换矩阵
Figure BDA0003875803640000147
所述变换矩阵
Figure BDA0003875803640000148
所述变换矩阵
Figure BDA0003875803640000149
所述变换矩阵
Figure BDA00038758036400001410
所述变换矩阵
Figure BDA00038758036400001411
所述变换矩阵
Figure BDA00038758036400001412
以及变换矩阵
Figure BDA00038758036400001413
确定法兰末端到世界坐标系的变换矩阵
Figure BDA00038758036400001414
例如：变换矩阵
Figure BDA00038758036400001415
的计算公式如下式(8)所示：
Figure BDA00038758036400001416
由此，本实施方式将第一关节、第二关节、第三关节、第四关节和第五关节形成的第一开链结构作为主轴，并基于主轴对应的第二标准DH参数确定法兰末端到世界坐标系的变换矩阵
Figure BDA00038758036400001417
提高了并联机器人位置计算的准确率和效率。
如图3所示，为本发明一实施例中基于第二种情况确定法兰末端到世界坐标系的变换矩阵
Figure BDA00038758036400001418
的流程示意图。在本实施例一优选的实施方式中，当第一开链结构是由第一关节、第六关节、第七关节、第四关节，以及第五关节连接形成时；确定法兰末端到世界坐标系的变换矩阵
Figure BDA0003875803640000151
至少包括如下步骤：
S301，将位置坐标与第七关节角作为变量的用于第四关节到基础坐标系的变换矩阵建立元素对应关系，得到第七关节角；
S302，获取第一关节沿基础坐标系Z轴的移动距离、第六关节角、第四关节角，以及第五关节角；
S303，基于基础坐标系、世界坐标系、并联机器人的第二标准DH参数，第一关节沿基础坐标系Z轴的移动距离、第六关节角、第七关节角、第四关节角，以及第五关节角，确定法兰末端到世界坐标系的变换矩阵。
在S303中，基于所述第一关节沿基础坐标系Z轴的移动距离d1，确定第一关节到基础坐标系的变换矩阵
Figure BDA0003875803640000152
基于所述第六关节角θ6和所述第六关节对应的第二标准DH参数，确定第六关节到第一关节的变换矩阵
Figure BDA0003875803640000153
基于所述第七关节角θ7和所述第七关节对应第二标准DH参数，确定第七关节到第六关节的变换矩阵
Figure BDA0003875803640000154
基于所述第四关节角θ4和所述第四关节对应的第二标准DH参数，确定第四关节到第七关节的变换矩阵
Figure BDA0003875803640000155
基于第五关节角θ5和所述第五关节对应的第二标准DH参数，确定第五关节到第四关节的变换矩阵
Figure BDA0003875803640000156
基于法兰对应的第二标准DH参数，确定法兰末端到第五关节的变换矩阵
Figure BDA0003875803640000157
基于所述变换矩阵
Figure BDA0003875803640000158
所述变换矩阵
Figure BDA0003875803640000159
所述变换矩阵
Figure BDA00038758036400001510
所述变换矩阵
Figure BDA00038758036400001511
所述变换矩阵
Figure BDA00038758036400001512
所述变换矩阵
Figure BDA00038758036400001513
以及变换矩阵
Figure BDA00038758036400001514
确定法兰末端到世界坐标系的变换矩阵
Figure BDA00038758036400001515
例如：变换矩阵
Figure BDA00038758036400001516
的计算公式如下式(9)所示：
Figure BDA00038758036400001517
由此，本实施方式将第一关节、第六关节、第七关节、第四关节和第五关节形成的第一开链结构作为主轴，并基于主轴对应的第二标准DH参数确定法兰末端到世界坐标系的变换矩阵
Figure BDA0003875803640000161
提高了并联机器人位置计算的准确率和效率。
需要注意的是，虽然第一标准DH参数和第二标准参数中都出现了θdh，x(°)、ddh,x、adh，x(mm)，但是所属所属的主轴结构不同，因此在第一标准DH参数和第二标准DH参数中对于相同表达形式的参数所对应的具体数值是不同的。
应理解，在本发明的各种实施例中，上述各过程的序号的大小并不意味着执行顺序的先后，各过程的执行顺序应以其功能和内在的逻辑确定，而不应对本发明实施例的实施过程构成任何限定。
如图7所示，为本发明一实施例提供的一种基于正向运动学确定并联机器人位置的装置的结构示意图。一种基于正向运动学确定并联机器人位置的装置，该装置700具体包括：划分模块701，用于将并联机器人由第一关节、第二关节、第三关节、第四关节、第五关节、第六关节以及第七关节连接形成的闭链结构划分为第一开链结构和第二开链结构；所述第一开链结构和所述第二开链结构均是由各关节连接形成的；计算模块702，用于将第三关节角位置为圆心和第三关节自身轴长为半径形成的第一圆，与第七关节角位置为圆心和第七关节自身轴长为半径形成的第二圆，进行相交，计算得到第四关节角在基础坐标系中的位置坐标；确定模块703，用于基于基础坐标系、世界坐标系、所述第四关节角在基础坐标系中的位置坐标、所述第一开链结构的各关节角，以及并联机器人的标准DH参数，确定法兰末端到世界坐标系的变换矩阵
Figure BDA0003875803640000162
提取模块704，用于从所述变换矩阵
Figure BDA0003875803640000163
中提取法兰末端对应的位置坐标。
在可选的实施方式中，当所述第一开链结构是由第一关节、第二关节、第三关节、第四关节，以及第五关节连接形成时；所述确定模块包括：计算单元，用于将所述第四关节角在基础坐标系中的位置坐标与所述第三关节角作为变量的变换矩阵
Figure BDA0003875803640000171
建立元素对应关系，计算得到第三关节角θ3；获取单元，用于获取第一关节沿基础坐标系Z轴的移动距离d1、第二关节角θ2、第四关节角θ4，以及第五关节角θ5；确定单元，用于基于基础坐标系、世界坐标系、所述并联机器人的第一标准DH参数、所述第一关节沿基础坐标系Z轴的移动距离d1、所述第二关节角θ2、所述第三关节角θ3、所述第四关节角θ4，以及所述第五关节角θ5，确定法兰末端到世界坐标系的变换矩阵
Figure BDA0003875803640000172
在可选的实施方式中，所述确定单元包括：第一确定子单元，用于基于所述第一关节沿基础坐标系Z轴的移动距离d1，确定第一关节到基础坐标系的变换矩阵
Figure BDA0003875803640000173
基于所述第二关节角θ2和所述第二关节对应的第一标准DH参数，确定第二关节到第一关节的变换矩阵
Figure BDA0003875803640000174
基于所述第三关节角θ3和所述第三关节对应第一标准DH参数，确定第三关节到第二关节的变换矩阵
Figure BDA0003875803640000175
基于所述第四关节角θ4和所述第四关节对应的第一标准DH参数，确定第四关节到第三关节的变换矩阵
Figure BDA0003875803640000176
基于第五关节角θ5和所述第五关节对应的第一标准DH参数，确定第五关节到第四关节的变换矩阵
Figure BDA0003875803640000177
基于法兰对应的第一标准DH参数，确定法兰末端到第五关节的变换矩阵
Figure BDA0003875803640000178
第二确定子单元，用于基于所述基础坐标系到世界坐标系的变换矩阵
Figure BDA0003875803640000179
所述变换矩阵
Figure BDA00038758036400001710
所述变换矩阵
Figure BDA00038758036400001711
所述变换矩阵
Figure BDA00038758036400001712
所述变换矩阵
Figure BDA00038758036400001713
所述变换矩阵
Figure BDA00038758036400001714
以及变换矩阵
Figure BDA00038758036400001715
确定法兰末端到世界坐标系的变换矩阵
Figure BDA00038758036400001716
在可选的实施方式中，当所述第一开链结构是由第一关节、第六关节、第七关节、第四关节，以及第五关节连接形成时；确定模块包括：
计算单元还用于，将所述位置坐标与所述第七关节角作为变量的变换矩阵
Figure BDA0003875803640000181
建立元素对应关系，计算得到第七关节角θ7；获取单元还用于，获取第一关节沿基础坐标系Z轴的移动距离d1、第六关节角θ6、第四关节角θ4，以及第五关节角θ5；确定单元还用于，基于基础坐标系、世界坐标系、所述并联机器人的第二标准DH参数，所述第一关节沿基础坐标系Z轴的移动距离d1、所述第六关节角θ6、所述第七关节角θ7、所述第四关节角θ4，以及所述第五关节角θ5，确定法兰末端到世界坐标系的变换矩阵
Figure BDA0003875803640000182
在可选的实施方式中，确定单元包括：第一确定子单元还用于，基于所述第一关节沿基础坐标系Z轴的移动距离d1，确定第一关节到基础坐标系的变换矩阵
Figure BDA0003875803640000183
基于所述第六关节角θ6和所述第六关节对应的第二标准DH参数，确定第六关节到第一关节的变换矩阵
Figure BDA0003875803640000184
基于所述第七关节角θ7和所述第七关节对应第二标准DH参数，确定第七关节到第六关节的变换矩阵
Figure BDA0003875803640000185
基于所述第四关节角θ4和所述第四关节对应的第二标准DH参数，确定第四关节到第七关节的变换矩阵
Figure BDA0003875803640000186
基于第五关节角θ5和所述第五关节对应的第二标准DH参数，确定第五关节到第四关节的变换矩阵
Figure BDA0003875803640000187
基于法兰对应的第二标准DH参数，确定法兰末端到第五关节的变换矩阵
Figure BDA0003875803640000188
第二确定子单元还用于，基于所述变换矩阵
Figure BDA0003875803640000189
所述变换矩阵
Figure BDA00038758036400001810
所述变换矩阵
Figure BDA00038758036400001811
所述变换矩阵
Figure BDA00038758036400001812
所述变换矩阵
Figure BDA00038758036400001813
所述变换矩阵
Figure BDA00038758036400001814
以及变换矩阵
Figure BDA00038758036400001815
确定法兰末端到世界坐标系的变换矩阵
Figure BDA00038758036400001816
在可选的实施方式中，提取模块包括：获取单元，用于获取法兰末端到世界坐标系的变换矩阵
Figure BDA00038758036400001817
划分单元，用于将所述变换矩阵
Figure BDA00038758036400001818
中前三行元素按列划分成第一列、第二列、第三列和第四列；确定单元，用于将所述第四列的元素确定为法兰末端对应的位置坐标。
上述装置可执行本发明一实施例所提供的基于正向运动学确定并联机器人位置的方法，具备执行基于正向运动学确定并联机器人位置的方法相应的功能模块和有益效果。未在本实施例中详尽描述的技术细节，可参见本发明实施例所提供的基于正向运动学确定并联机器人位置的方法。
本发明还提供一种电子设备，包括：处理器；用于存储所述处理器可执行指令的存储器；所述处理器，用于从所述存储器中读取所述可执行指令，并执行所述指令以实现本发明所述的基于正向运动学确定并联机器人位置的方法。
除了上述方法和设备以外，本申请的实施例还可以是计算机程序产品，其包括计算机程序指令，所述计算机程序指令在被处理器运行时使得所述处理器执行本说明书上述“示例性方法”部分中描述的根据本申请各种实施例的方法中的步骤。
所述计算机程序产品可以以一种或多种程序设计语言的任意组合来编写用于执行本申请实施例操作的程序代码，所述程序设计语言包括面向对象的程序设计语言，诸如Java、C++等，还包括常规的过程式程序设计语言，诸如“C”语言或类似的程序设计语言。程序代码可以完全地在用户计算设备上执行、部分地在用户设备上执行、作为一个独立的软件包执行、部分在用户计算设备上部分在远程计算设备上执行、或者完全在远程计算设备或服务器上执行。
此外，本申请的实施例还可以是计算机可读存储介质，其上存储有计算机程序指令，所述计算机程序指令在被处理器运行时使得所述处理器执行本说明书上述“示例性方法”部分中描述的根据本申请如下各实施例的方法中的步骤。
所述计算机可读存储介质可以采用一个或多个可读介质的任意组合。可读介质可以是可读信号介质或者可读存储介质。可读存储介质例如可以包括但不限于电、磁、光、电磁、红外线、或半导体的***、装置或器件，或者任意以上的组合。可读存储介质的更具体的例子(非穷举的列表)包括：具有一个或多个导线的电连接、便携式盘、硬盘、随机存取存储器(RAM)、只读存储器(ROM)、可擦式可编程只读存储器(EPROM或闪存)、光纤、便携式紧凑盘只读存储器(CD-ROM)、光存储器件、磁存储器件、或者上述的任意合适的组合。
以上结合具体实施例描述了本申请的基本原理，但是，需要指出的是，在本申请中提及的优点、优势、效果等仅是示例而非限制，不能认为这些优点、优势、效果等是本申请的各个实施例必须具备的。另外，上述公开的具体细节仅是为了示例的作用和便于理解的作用，而非限制，上述细节并不限制本申请为必须采用上述具体的细节来实现。
本申请中涉及的器件、装置、设备、***的方框图仅作为例示性的例子并且不意图要求或暗示必须按照方框图示出的方式进行连接、布置、配置。如本领域技术人员将认识到的，可以按任意方式连接、布置、配置这些器件、装置、设备、***。诸如“包括”、“包含”、“具有”等等的词语是开放性词汇，指“包括但不限于”，且可与其互换使用。这里所使用的词汇“或”和“和”指词汇“和/或”，且可与其互换使用，除非上下文明确指示不是如此。这里所使用的词汇“诸如”指词组“如但不限于”，且可与其互换使用。
还需要指出的是，在本申请的装置、设备和方法中，各部件或各步骤是可以分解和/或重新组合的。这些分解和/或重新组合应视为本申请的等效方案。
提供所公开的方面的以上描述以使本领域的任何技术人员能够做出或者使用本申请。对这些方面的各种修改对于本领域技术人员而言是非常显而易见的，并且在此定义的一般原理可以应用于其他方面而不脱离本申请的范围。因此，本申请不意图被限制到在此示出的方面，而是按照与在此公开的原理和新颖的特征一致的最宽范围。
为了例示和描述的目的已经给出了以上描述。此外，此描述不意图将本申请的实施例限制到在此公开的形式。尽管以上已经讨论了多个示例方面和实施例，但是本领域技术人员将认识到其某些变型、修改、改变、添加和子组合。
在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。而且，描述的具体特征、结构、材料或者特点可以在任一个或多个实施例或示例中以合适的方式结合。此外，在不相互矛盾的情况下，本领域的技术人员可以将本说明书中描述的不同实施例或示例以及不同实施例或示例的特征进行结合和组合。
此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或隐含地包括至少一个该特征。在本发明的描述中，“多个”的含义是两个或两个以上，除非另有明确具体的限定。
以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以所述权利要求的保护范围为准。

Claims (10)

1.基于正向运动学确定并联机器人位置的方法，其特征在于，包括：
将并联机器人由第一关节、第二关节、第三关节、第四关节、第五关节、第六关节以及第七关节连接形成的闭链结构划分为第一开链结构和第二开链结构；所述第一开链结构和所述第二开链结构均是由各关节连接形成的；
将第三关节角位置为圆心和第三关节自身轴长为半径形成的第一圆，与第七关节角位置为圆心和第七关节自身轴长为半径形成的第二圆，进行相交，计算得到第四关节角在基础坐标系中的位置坐标；
基于基础坐标系、世界坐标系、所述第四关节角在基础坐标系中的位置坐标、所述第一开链结构的各关节角，以及并联机器人的标准DH参数，确定法兰末端到世界坐标系的变换矩阵
Figure FDA0003875803630000011
从所述变换矩阵
Figure FDA0003875803630000012
中提取法兰末端对应的位置坐标。
2.根据权利要求1所述的方法，其特征在于，当所述第一开链结构是由第一关节、第二关节、第三关节、第四关节，以及第五关节连接形成时；所述基于基础坐标系、世界坐标系、所述第四关节角在基础坐标系中的位置坐标、所述第一开链结构的各关节角，以及并联机器人的标准DH参数，确定法兰末端到世界坐标系的变换矩阵
Figure FDA0003875803630000013
包括：
将所述第四关节角在基础坐标系中的位置坐标与所述第三关节角作为变量的变换矩阵
Figure FDA0003875803630000014
建立元素对应关系，计算得到第三关节角θ3；
获取第一关节沿基础坐标系Z轴的移动距离d1、第二关节角θ2、第四关节角θ4，以及第五关节角θ5；
基于基础坐标系、世界坐标系、所述并联机器人的第一标准DH参数、所述第一关节沿基础坐标系Z轴的移动距离d1、所述第二关节角θ2、所述第三关节角θ3、所述第四关节角θ4，以及所述第五关节角θ5，确定法兰末端到世界坐标系的变换矩阵
Figure FDA0003875803630000021
3.根据权利要求2所述的方法，其特征在于，所述基于基础坐标系、世界坐标系、所述并联机器人的第一标准DH参数、所述第一关节沿基础坐标系Z轴的移动距离d1、所述第二关节角θ2、所述第三关节角θ3、所述第四关节角θ4，以及所述第五关节角θ5，确定法兰末端到世界坐标系的变换矩阵
Figure FDA0003875803630000022
包括：
基于所述第一关节沿基础坐标系Z轴的移动距离d1，确定第一关节到基础坐标系的变换矩阵
Figure FDA0003875803630000023
基于所述第二关节角θ2和所述第二关节对应的第一标准DH参数，确定第二关节到第一关节的变换矩阵
Figure FDA0003875803630000024
基于所述第三关节角θ3和所述第三关节对应第一标准DH参数，确定第三关节到第二关节的变换矩阵
Figure FDA0003875803630000025
基于所述第四关节角θ4和所述第四关节对应的第一标准DH参数，确定第四关节到第三关节的变换矩阵
Figure FDA0003875803630000026
基于第五关节角θ5和所述第五关节对应的第一标准DH参数，确定第五关节到第四关节的变换矩阵
Figure FDA0003875803630000027
基于法兰对应的第一标准DH参数，确定法兰末端到第五关节的变换矩阵
Figure FDA0003875803630000028
基于所述基础坐标系到世界坐标系的变换矩阵
Figure FDA0003875803630000029
所述变换矩阵
Figure FDA00038758036300000210
所述变换矩阵
Figure FDA00038758036300000211
所述变换矩阵
Figure FDA00038758036300000212
所述变换矩阵
Figure FDA00038758036300000213
所述变换矩阵
Figure FDA00038758036300000214
以及变换矩阵
Figure FDA00038758036300000215
确定法兰末端到世界坐标系的变换矩阵
Figure FDA00038758036300000216
4.根据权利要求1所述的方法，其特征在于，当所述第一开链结构是由第一关节、第六关节、第七关节、第四关节，以及第五关节连接形成时；所述基于基础坐标系、世界坐标系、所述第四关节角在基础坐标系中的位置坐标、所述第一开链结构的各关节角，以及所述并联机器人的标准DH参数，确定法兰末端到世界坐标系的变换矩阵
Figure FDA00038758036300000217
包括：
将所述位置坐标与所述第七关节角作为变量的变换矩阵
Figure FDA0003875803630000031
建立元素对应关系，得到第七关节角θ7；
获取第一关节沿基础坐标系Z轴的移动距离d1、第六关节角θ6、第四关节角θ4，以及第五关节角θ5；
基于基础坐标系、世界坐标系、所述并联机器人的第二标准DH参数，所述第一关节沿基础坐标系Z轴的移动距离d1、所述第六关节角θ6、所述第七关节角θ7、所述第四关节角θ4，以及所述第五关节角θ5，确定法兰末端到世界坐标系的变换矩阵
Figure FDA0003875803630000032
5.根据权利要求4所述的方法，其特征在于，所述基于基础坐标系、世界坐标系、所述并联机器人的第二标准DH参数，所述第一关节沿基础坐标系Z轴的移动距离d1、所述第六关节角θ6、所述第七关节角θ7、所述第四关节角θ4，以及所述第五关节角θ5，确定法兰末端到世界坐标系的变换矩阵
Figure FDA0003875803630000033
包括：
基于所述第一关节沿基础坐标系Z轴的移动距离d1，确定第一关节到基础坐标系的变换矩阵
Figure FDA0003875803630000034
基于所述第六关节角θ6和所述第六关节对应的第二标准DH参数，确定第六关节到第一关节的变换矩阵
Figure FDA0003875803630000035
基于所述第七关节角θ7和所述第七关节对应第二标准DH参数，确定第七关节到第六关节的变换矩阵
Figure FDA0003875803630000036
基于所述第四关节角θ4和所述第四关节对应的第二标准DH参数，确定第四关节到第七关节的变换矩阵
Figure FDA0003875803630000037
基于第五关节角θ5和所述第五关节对应的第二标准DH参数，确定第五关节到第四关节的变换矩阵
Figure FDA0003875803630000038
基于法兰对应的第二标准DH参数，确定法兰末端到第五关节的变换矩阵
Figure FDA0003875803630000039
基于所述变换矩阵
Figure FDA00038758036300000310
所述变换矩阵
Figure FDA00038758036300000311
所述变换矩阵
Figure FDA00038758036300000312
所述变换矩阵
Figure FDA0003875803630000041
所述变换矩阵
Figure FDA0003875803630000042
所述变换矩阵
Figure FDA0003875803630000043
以及变换矩阵
Figure FDA0003875803630000044
确定法兰末端到世界坐标系的变换矩阵
Figure FDA0003875803630000045
6.根据权利要求1所述的方法，其特征在于，所述从所述法兰末端到世界坐标系的变换矩阵
Figure FDA0003875803630000046
中提取法兰末端对应的位置坐标，包括：
获取法兰末端到世界坐标系的变换矩阵
Figure FDA0003875803630000047
将所述变换矩阵
Figure FDA0003875803630000048
中前三行元素按列划分成第一列、第二列、第三列和第四列；
将所述第四列的元素确定为法兰末端对应的位置坐标。
7.基于正向运动学确定并联机器人位置的装置，其特征在于，包括：
划分模块，用于将并联机器人由第一关节、第二关节、第三关节、第四关节、第五关节、第六关节以及第七关节连接形成的闭链结构划分为第一开链结构和第二开链结构；所述第一开链结构和所述第二开链结构均是由各关节连接形成的；
计算模块，用于将第三关节角位置为圆心和第三关节自身轴长为半径形成的第一圆，与第七关节角位置为圆心和第七关节自身轴长为半径形成的第二圆，进行相交，计算得到第四关节角在基础坐标系中的位置坐标；
确定模块，用于基于基础坐标系、世界坐标系、所述第四关节角在基础坐标系中的位置坐标、所述第一开链结构的各关节角，以及并联机器人的标准DH参数，确定法兰末端到世界坐标系的变换矩阵
Figure FDA0003875803630000049
提取模块，用于从所述变换矩阵
Figure FDA00038758036300000410
中提取法兰末端对应的位置坐标。
8.根据权利要求7所述的装置，其特征在于，当所述第一开链结构是由第一关节、第二关节、第三关节、第四关节，以及第五关节连接形成时；所述确定模块包括：
计算单元，用于将所述第四关节角在基础坐标系中的位置坐标与所述第三关节角作为变量的变换矩阵
Figure FDA0003875803630000051
建立元素对应关系，计算得到第三关节角θ3；
获取单元，用于获取第一关节沿基础坐标系Z轴的移动距离d1、第二关节角θ2、第四关节角θ4，以及第五关节角θ5；
确定单元，用于基于基础坐标系、世界坐标系、所述并联机器人的第一标准DH参数、所述第一关节沿基础坐标系Z轴的移动距离d1、所述第二关节角θ2、所述第三关节角θ3、所述第四关节角θ4，以及所述第五关节角θ5，确定法兰末端到世界坐标系的变换矩阵
Figure FDA0003875803630000052
9.根据权利要求8所述的装置，其特征在于，所述确定单元包括：
第一确定子单元，用于基于所述第一关节沿基础坐标系Z轴的移动距离d1，确定第一关节到基础坐标系的变换矩阵
Figure FDA0003875803630000053
基于所述第二关节角θ2和所述第二关节对应的第一标准DH参数，确定第二关节到第一关节的变换矩阵
Figure FDA0003875803630000054
基于所述第三关节角θ3和所述第三关节对应第一标准DH参数，确定第三关节到第二关节的变换矩阵
Figure FDA0003875803630000055
基于所述第四关节角θ4和所述第四关节对应的第一标准DH参数，确定第四关节到第三关节的变换矩阵
Figure FDA0003875803630000056
基于第五关节角θ5和所述第五关节对应的第一标准DH参数，确定第五关节到第四关节的变换矩阵
Figure FDA0003875803630000057
基于法兰对应的第一标准DH参数，确定法兰末端到第五关节的变换矩阵
Figure FDA0003875803630000058
第二确定子单元，用于基于所述基础坐标系到世界坐标系的变换矩阵
Figure FDA0003875803630000059
所述变换矩阵
Figure FDA00038758036300000510
所述变换矩阵
Figure FDA00038758036300000511
所述变换矩阵
Figure FDA00038758036300000512
所述变换矩阵
Figure FDA00038758036300000513
所述变换矩阵
Figure FDA00038758036300000514
以及变换矩阵
Figure FDA00038758036300000515
确定法兰末端到世界坐标系的变换矩阵
Figure FDA00038758036300000516
10.一种计算机可读介质，其上存储有计算机程序，所述程序被处理器执行时实现如权利要求1-7中任一项所述的方法。
CN202211214966.6A 2022-09-30 2022-09-30 基于正向运动学确定并联机器人位置的方法及装置 Pending CN115488860A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211214966.6A CN115488860A (zh) 2022-09-30 2022-09-30 基于正向运动学确定并联机器人位置的方法及装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211214966.6A CN115488860A (zh) 2022-09-30 2022-09-30 基于正向运动学确定并联机器人位置的方法及装置

Publications (1)

Publication Number Publication Date
CN115488860A true CN115488860A (zh) 2022-12-20

Family

ID=84472588

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211214966.6A Pending CN115488860A (zh) 2022-09-30 2022-09-30 基于正向运动学确定并联机器人位置的方法及装置

Country Status (1)

Country Link
CN (1) CN115488860A (zh)

Similar Documents

Publication Publication Date Title
Yin et al. A vision-based self-calibration method for robotic visual inspection systems
JP2020046427A (ja) マルチセンサーのキャリブレーション方法とその装置、コンピュータ機器、媒体及び車両
WO2017129093A1 (zh) 一种基于共形几何代数的机械臂运动规划的形式化分析方法及***
CN113211445B (zh) 一种机器人参数标定方法、装置、设备及存储介质
EP2407902A2 (en) System and method for configuring a simulation model utilizing a tool for automatic input/output assignment
Zhao et al. LiDAR-based non-cooperative tumbling spacecraft pose tracking by fusing depth maps and point clouds
Wong et al. Manipulation planning for object re-orientation based on semantic segmentation keypoint detection
Kang et al. High-speed autonomous robotic assembly using in-hand manipulation and re-grasping
Liu et al. A geometric approach to obtain the closed-form forward kinematics of H4 parallel robot
CN114661028A (zh) 智能驾驶控制器测试方法、装置、计算机设备和存储介质
Wang et al. Research on door opening operation of mobile robotic arm based on reinforcement learning
CN112536792B (zh) 一种基于球面几何解析法的机器臂逆解方法
CN114193444A (zh) 一种机器人手眼标定方法、***、设备及介质
CN115488860A (zh) 基于正向运动学确定并联机器人位置的方法及装置
CN111414508B (zh) 一种在设计模型中搜索并实现可视化的方法及终端
CN113084791B (zh) 机械臂控制方法、机械臂控制装置及终端设备
CN114516048B (zh) 机器人的零点调试方法、装置、控制器及存储介质
US20220111531A1 (en) Robot globally optimal visual positioning method and device based on point-line features
Zhang et al. Hand–eye calibration and grasping pose calculation with motion error compensation and vertical-component correction for 4-R (2-SS) parallel robot
Feng et al. Calibration of a six-DOF parallel manipulator for chromosome dissection
Wang et al. Robot Grasp Planning: A Learning from Demonstration-Based Approach
CN115648177A (zh) 基于逆向运动学确定并联机器人姿态的方法及装置
CN115741666A (zh) 机器人手眼标定方法、机器人及机器人作业方法
CN112270066B (zh) 一种计算卫星刚性耦合系数的优化方法和计算机设备
Vithani et al. Estimation of object kinematics from point data

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