CN115648177A - 基于逆向运动学确定并联机器人姿态的方法及装置 - Google Patents

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

Info

Publication number
CN115648177A
CN115648177A CN202211214223.9A CN202211214223A CN115648177A CN 115648177 A CN115648177 A CN 115648177A CN 202211214223 A CN202211214223 A CN 202211214223A CN 115648177 A CN115648177 A CN 115648177A
Authority
CN
China
Prior art keywords
joint
transformation matrix
angle theta
joint angle
theta
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
CN202211214223.9A
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 CN202211214223.9A priority Critical patent/CN115648177A/zh
Publication of CN115648177A publication Critical patent/CN115648177A/zh
Pending legal-status Critical Current

Links

Images

Landscapes

  • Manipulator (AREA)
  • Numerical Control (AREA)

Abstract

本发明实施例公开了一种基于逆向运动学确定并联机器人姿态的方法及装置，该方法包括：将并联机器人闭链结构划分为第一开链结构和第二开链结构；基于法兰末端、各关节末端以及标准DH参数，运动学逆解求解，得到各关节角；针对第一关节、第二关节、第三关节、第四关节、第五关节连接形成的第一开链结构中任一关节角的两组解：从两组解中选取满足第一预设条件的解作为唯一解；针对第一关节、第六关节、第七关节、第四关节和第五关节连接形成的第一开链结构中任一关节角的两组解：从两组解中选取满足第二预设条件的解作为唯一解。由此，将闭链结构拆分成两个开链结构，并基于机器人特定结构确定各关节角，提高了并联机器人姿态计算的速率和准确性。

Description

基于逆向运动学确定并联机器人姿态的方法及装置
技术领域
本发明属于工业机器人技术领域，尤其涉及一种基于逆向运动学确定并联机器人姿态的方法及装置。
背景技术
串联机器人或者并联机器人的运动学分析包括正运动学问题和逆运动学问题。逆运动学问题定义是已知动平台的位置和姿态信息，求解若干独立的输入运动，对于串联机器人或并联机器人而言，多个输入运动的表达式是独立的，能并行计算，快速求解。正运动学问题则是在已知多个输入运动的条件下，求解动平台的位置和姿态。
针对并联机器人的逆解求解法主要有数值法和解析法。数值法主要是利用迭代算法，通过大量的迭代计算来获得机器人的关节值。数值法的优点是适用于任意构型的并联机器人，但是需要花费大量的时间在迭代计算上，而且存在一定的误差。解析法虽然对于高自由度的并联机器人效果不佳，无法计算奇异点以及冗余机器人的解，但是对于某些特定结构的并联机器人，解析法的计算速度明显优于数值解法，而且在理论上不存在误差。
对于并联机器人，逆运动学求解往往相对明确(例如，能够根据平台的形状有效确定机器人的关节值)；而正向运动学求解可能相当复杂，无法根据平台形状准确确定机器人的关节值。
发明内容
本发明提供一种基于逆向运动学确定并联机器人姿态的方法及装置。该方法能够提高并联机器人姿态计算的速率和准确性。
为实现上述目的，根据本申请实施例第一方面提供一种基于逆向运动学确定并联机器人姿态的方法，该方法包括将并联机器人的闭链结构划分为第一开链结构和第二开链结构；所述闭链结构是由第一关节、第二关节、第三关节、第四关节、第五关节、第六关节以及第七关节连接形成的闭合结构；所述第一开链结构和所述第二开链结构均是通过若干关节连接形成的结构；基于法兰末端以及所述闭链结构各关节对应的标准DH参数，进行运动学逆解求解，得到第一关节沿机器人基础坐标系Z轴的移动距离d1、第二关节角θ2、第三关节角θ3、第四关节角θ4、第五关节角θ5、第六关节角θ6和第七关节角θ7；当所述第一开链结构是由第一关节、第二关节、第三关节、第四关节、第五关节连接形成时，针对所述第一开链结构中任一关节角：若所述关节角存在两组解，则从所述两组解中选取满足第一预设条件的解作为该关节角的唯一解；当所述第一开链结构是由第一关节、第六关节、第七关节、第四关节和第五关节连接形成时，针对所述第二开链结构中任一关节角：若所述关节角存在两组解，则从所述两组解中选取满足第二预设条件的解作为该关节角的唯一解。
可选的，所述基于法兰末端以及所述闭链结构各关节对应的标准DH参数，进行运动学逆解求解，得到第一关节沿机器人基础坐标系Z轴的移动距离d1、第二关节角θ2、第三关节角θ3、第四关节角θ4、第五关节角θ5、第六关节角θ6和第七关节角θ7，包括：
基于法兰末端，以及由第一关节、第二关节、第三关节、第四关节、第五关节连接形成的第一开链结构中各关节对应的第一标准DH参数，进行运动学逆解求解，得到第一关节沿机器人基础坐标系Z轴的移动距离d1、第二关节角θ2、第三关节角θ3、第四关节角θ4和第五关节角θ5；基于法兰末端，以及由第一关节、第四关节、第五关节、第六关节和第七关节连接形成的第一开链结构中各关节对应的第二标准DH参数，进行运动学逆解求解，得到第六关节角θ6和第七关节角θ7。
可选的，所述基于法兰末端，以及由第一关节、第二关节、第三关节、第四关节、第五关节连接形成的第一开链结构中各关节对应的第一标准DH参数，进行运动学逆解求解，得到第一关节沿机器人基础坐标系Z轴的移动距离d1、第二关节角θ2、第三关节角θ3、第四关节角θ4和第五关节角θ5，包括：
确定第五关节到机器人基础坐标系的已知变换矩阵
Figure BDA0003875649630000031
基于所述并联机器人的第一标准DH参数，确定含有d1变量的变换矩阵
Figure BDA0003875649630000032
含有θ2变量的变换矩阵
Figure BDA0003875649630000033
含有θ3变量的变换矩阵
Figure BDA0003875649630000034
含有θ4变量的变换矩阵
Figure BDA0003875649630000035
以及含有θ5变量的变换矩阵
Figure BDA0003875649630000036
其中，
Figure BDA0003875649630000037
表示第一关节到机器人基础坐标系的变换矩阵；
Figure BDA0003875649630000038
表示第二关节到第一关节的变换矩阵，
Figure BDA0003875649630000039
表示第三关节到第二关节的变换矩阵，
Figure BDA00038756496300000310
表示第四关节到第三关节的变换矩阵，
Figure BDA00038756496300000311
表示第五关节到第四关节的变换矩阵；基于所述含有d1变量的变换矩阵
Figure BDA00038756496300000312
所述含有θ2变量的变换矩阵
Figure BDA00038756496300000313
所述含有θ3变量的变换矩阵
Figure BDA00038756496300000314
所述含有θ4变量的变换矩阵
Figure BDA00038756496300000315
以及所述含有θ5变量的变换矩阵
Figure BDA00038756496300000316
确定含有关节变量的变换矩阵
Figure BDA00038756496300000317
基于所述已知变换矩阵
Figure BDA00038756496300000318
和所述含有关节变量的变换矩阵
Figure BDA00038756496300000319
之间的元素对应关系建立方程，得到第五关节角θ5；基于所述已知变换矩阵
Figure BDA00038756496300000320
所述含有θ5变量的变换矩阵
Figure BDA00038756496300000321
以及所述第五关节角θ5，确定第一关节沿机器人基础坐标系Z轴的移动距离d1、第二关节角θ2、第三关节角θ3以及第四关节角θ4。
可选的，所述确定第五关节到机器人基础坐标系的已知变换矩阵
Figure BDA0003875649630000041
包括：获取法兰末端到世界坐标系的已知变换矩阵
Figure BDA0003875649630000042
以及机器人基础坐标系到世界坐标系的已知变换矩阵
Figure BDA0003875649630000043
基于并联机器人的第一标准DH参数，确定法兰末端到第五关节的已知变换矩阵
Figure BDA0003875649630000044
基于
Figure BDA0003875649630000045
以及
Figure BDA0003875649630000046
确定第五关节到机器人基础坐标系的已知变换矩阵
Figure BDA0003875649630000047
可选的，基于已知变换矩阵
Figure BDA0003875649630000048
含有θ5变量的变换矩阵
Figure BDA0003875649630000049
以及第五关节角θ5，确定第一关节沿机器人基础坐标系Z轴的移动距离d1、第二关节角θ2、第三关节角θ3以及第四关节角θ4，包括：基于第五关节角θ5，确定已知变换矩阵
Figure BDA00038756496300000410
基于已知变换矩阵
Figure BDA00038756496300000411
和已知变换矩阵
Figure BDA00038756496300000412
确定已知变换矩阵
Figure BDA00038756496300000413
确定含有变量的变换矩阵
Figure BDA00038756496300000414
基于已知变换矩阵
Figure BDA00038756496300000415
和含有变量的变换矩阵
Figure BDA00038756496300000416
之间的元素对应关系，确定第一关节沿机器人基础坐标系Z轴的移动距离d1、第二关节角θ2、第三关节角θ3，以及第四关节角θ4。
可选的，确定含有变量的变换矩阵
Figure BDA00038756496300000417
包括：基于所述含有d1变量的变换矩阵
Figure BDA00038756496300000418
所述含有θ2变量的变换矩阵
Figure BDA00038756496300000419
所述含有θ3变量的变换矩阵
Figure BDA00038756496300000420
所述含有θ4变量的变换矩阵
Figure BDA00038756496300000421
确定含有变量的变换矩阵
Figure BDA00038756496300000422
可选的，所述基于所述已知变换矩阵
Figure BDA00038756496300000423
和所述含有变量的变换矩阵
Figure BDA00038756496300000424
之间的元素对应关系，确定第一关节沿机器人基础坐标系Z轴的移动距离d1、第二关节角θ2、第三关节角θ3，以及第四关节角θ4，包括：从所述已知变换矩阵
Figure BDA00038756496300000425
中提取第四关节在机器人基础坐标系下的位置坐标；并从所述含有变量的变换矩阵
Figure BDA0003875649630000051
中提取第四关节在机器人基础坐标系下的位置函数坐标；基于所述位置坐标和所述位置函数坐标之间的元素对应关系建立方程组，得到第一关节沿机器人基础坐标系Z轴的移动距离d1、第二关节角θ2和第三关节角θ3；将所述移动距离d1、所述第二关节角θ2、所述第三关节角θ3代入所述含有变量的变换矩阵
Figure BDA0003875649630000052
得到θ4作为变量的变换矩阵
Figure BDA0003875649630000053
基于所述已知变换矩阵
Figure BDA0003875649630000054
和所述θ4作为变量的变换矩阵
Figure BDA0003875649630000055
之间的元素对应关系，确定第四关节角θ4。
为实现上述目的，根据本申请实施例第二方面提一种基于逆向运动学确定并联机器人姿态的装置，所述装置包括：划分模块，用于将并联机器人的闭链结构划分为第一开链结构和第二开链结构；所述闭链结构是由第一关节、第二关节、第三关节、第四关节、第五关节、第六关节以及第七关节连接形成的闭合结构；所述第一开链结构和所述第二开链结构均是通过若干关节连接形成的结构；拟解求解模块，用于基于法兰末端以及所述闭链结构各关节对应标准DH参数，进行运动学逆解求解，得到第一关节沿机器人基础坐标系Z轴的移动距离d1、第二关节角θ2、第三关节角θ3、第四关节角θ4、第五关节角θ5、第六关节角θ6和第七关节角θ7；第一选取模块，用于当所述第一开链结构是由第一关节、第二关节、第三关节、第四关节、第五关节连接形成时，针对所述第一开链结构中任一关节角：若所述关节角存在两组解，则从所述两组解中选取满足第一预设条件的解作为该关节角的唯一解；第二选取模块，用于当所述第一开链结构是由第一关节、第六关节、第七关节、第四关节和第五关节连接形成时，针对所述第二开链结构中任一关节角：若所述关节角存在两组解，则从所述两组解中选取满足第二预设条件的解作为该关节角的唯一解。
可选的，所述拟解求解模块，包括：第一拟解求解模块，用于基于法兰末端，以及由第一关节、第二关节、第三关节、第四关节、第五关节连接形成的第一开链结构中各关节对应的第一标准DH参数，进行运动学逆解求解，得到第一关节沿机器人基础坐标系Z轴的移动距离d1、第二关节角θ2、第三关节角θ3、第四关节角θ4和第五关节角θ5；第二拟解求解模块，用于基于法兰末端，以及由第一关节、第四关节、第五关节、第六关节和第七关节连接形成的第一开链结构中各关节对应第二标准DH参数，进行运动学逆解求解，得到第六关节角θ6和第七关节角θ7。
为实现上述目的，根据本申请实施例第三方面提供一种计算机可读介质，其上存储有计算机程序，所述程序被处理器执行时实现如第一方面所述的方法。
与现有技术相比，本发明实施例提供一种基于逆向运动学确定并联机器人姿态的方法及装置，该方法包括：首先，将并联机器人的闭链结构划分为第一开链结构和第二开链结构；所述闭链结构是由第一关节、第二关节、第三关节、第四关节、第五关节、第六关节以及第七关节连接形成的闭合结构；所述第一开链结构和所述第二开链结构均是通过若干关节连接形成的结构；其次，基于法兰末端以及所述闭链结构各关节对应的标准DH参数，进行运动学逆解求解，得到第一关节沿机器人基础坐标系Z轴的移动距离d1、第二关节角θ2、第三关节角θ3、第四关节角θ4、第五关节角θ5、第六关节角θ6和第七关节角θ7；最后，当所述第一开链结构是由第一关节、第二关节、第三关节、第四关节、第五关节连接形成时，针对所述第一开链结构中任一关节角：若所述关节角存在两组解，则从所述两组解中选取满足第一预设条件的解作为该关节角的唯一解；当所述第一开链结构是由第一关节、第六关节、第七关节、第四关节和第五关节连接形成时，针对所述第二开链结构中任一关节角：若所述关节角存在两组解，则从所述两组解中选取满足第二预设条件的解作为该关节角的唯一解。由此，通过将闭链结构拆分成两个开链结构，并基于串并混合解法和机器人特定结构确定机器人各关节角，提高了并联机器人姿态计算的速率和准确性。
附图说明
后文将参照附图以示例性而非限制性的方式详细描述本发明的一些具体实施例。附图中相同的附图标记标示了相同或类似的部件或部分。本领域技术人员应该理解，这些附图未必是按比例绘制的。附图中：
图1为本发明一实施例提供的基于逆向运动学确定并联机器人姿态的方法的流程示意图；
图2为本发明一实施例提供的并联机器人第一标准DH参数的结构示意图；
图3为发明一实施例提供的并联机器人第二标准DH参数的结构示意图；
图4为本发明一实施例提供的并联机器人的结构示意图；
图5为本发明一实施例提供的基于逆向运动学确定并联机器人姿态的装置的结构示意图。
具体实施方式
为使本发明的目的、特征、优点能够更加的明显和易懂，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而非全部实施例。基于本发明中的实施例，本领域技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
如图1所示，为本发明一实施例提供的基于逆向运动学确定并联机器人姿态的方法的流程示意图；如图2所示，为本发明一实施例提供的并联机器人的第一标准DH参数的结构示意图；如图3所示，为发明一实施例提供的并联机器人第二标准DH参数的结构示意图；图4所示，为本发明一实施例提供的并联机器人的结构示意图。
一种基于逆向运动学确定并联机器人姿态的方法，所述方法至少包括如下步骤：
S101，将并联机器人的闭链结构划分为第一开链结构和第二开链结构；所述闭链结构是由第一关节、第二关节、第三关节、第四关节、第五关节、第六关节以及第七关节连接形成的闭合结构；第一开链结构和所述第二开链结构均是通过若干关节连接形成的结构；
S102，基于法兰末端以及所述闭链结构各关节对应的标准DH参数，进行运动学逆解求解，得到第一关节沿机器人基础坐标系Z轴的移动距离d1、第二关节角θ2、第三关节角θ3、第四关节角θ4、第五关节角θ5、第六关节角θ6和第七关节角θ7；
S103，当第一开链结构是由第一关节、第二关节、第三关节、第四关节、第五关节连接形成时，针对第一开链结构中任一关节角：若关节角存在两组解，则从两组解中选取满足第一预设条件的解作为该关节角的唯一解；
S104，当第一开链结构是由第一关节、第六关节、第七关节、第四关节和第五关节连接形成时，针对第二开链结构中任一关节角：若关节角存在两组解，则从两组解中选取满足第二预设条件的解作为该关节角的唯一解。
在S101中，如图3所示，例如：将并联机器人的第一关节、第二关节、第三关节、第四关节、第五关节、第六关节以及第七关节组合后形成闭链结构；将闭链结构划分成由第一关节、第二关节、第三关节、第四关节和第五关节连接后形成的第一开链结构与由第六关节和第七关节连接后形成的第二开链结构；或者，将闭链结构划分成由第一关节、第六关节、第七关节、第四关节和第五关节连接后形成的第一开链结构与由第二关节和第三关节连接后形成的第二开链结构。
由此，将闭链结构拆分成两个开链结构，有利于提高并联机器人姿态计算的效率。
需要说明的是，第三关节和第七关节是从动轴；第一关节、第二关节、第四关节、第五关节和第六关节是主动轴。
在S102至S104中，当第一关节、第二关节、第三关节、第四关节和第五关节连接后形成第一开链结构，且第六关节和第七关节连接后形成第二开链结构时，并联机器人的标准DH参数为第一标准DH参数，如图2所示。当第一关节、第六关节、第七关节、第四关节和第五关节连接后形成第一开链结构，且第二关节和第三关节连接后形成第二开链结构时，并联机器人的DH参数为第二标准DH参数。
而且，θdh，x(°)、ddh,x(mm)、adh，x(mm)中出现的x表示关节轴的编号，例如x＝1,2,3,4,5,6,7。
基于法兰末端以及所述闭链结构各关节对应的标准DH参数，进行运动学逆解求解，得到第一关节沿机器人基础坐标系Z轴的移动距离d1、第二关节角θ2、第三关节角θ3、第四关节角θ4、第五关节角θ5、第六关节角θ6和第七关节角θ7，包括如下步骤：
S1，基于法兰末端，以及由第一关节、第二关节、第三关节、第四关节、第五关节连接形成的第一开链结构中各关节对应的的第一标准DH参数，进行运动学逆解求解，得到第一关节沿机器人基础坐标系Z轴的移动距离d1、第二关节角θ2、第三关节角θ3、第四关节角θ4和第五关节角θ5；
S2，基于法兰末端，以及由第一关节、第四关节、第五关节、第六关节和第七关节连接形成的第一开链结构中各关节对应的的第二标准DH参数，进行运动学逆解求解，得到第六关节角θ6和第七关节角θ7。
在S1中，以第一关节、第二关节、第三关节、第四关节和第五关节连接后形成第一开链结构为主轴，并以第六关节和第七关节连接后形成第二开链结构为副轴，并联机器人的标准参数为第一标准DH参数。
在图2所示的第二标准DH参数的示意图中，以法兰为分界线分别进行说明，位于法兰之前的各关节：第二关节是从第一关节派生且第二关节对应的DH参数是以第一关节为参考计算的，第三关节是从第二关节派生且第三关节对应的DH参数是以第二关节为参考计算的，第四关节是从第三关节派生且第四关节对应的DH参数是以第三关节为参考计算的，第五关节是从第四关节派生且第五关节对应的DH参数是以第四关节为参考计算的；
位于法兰之后的各关节：第六关节是从第一关节派生且第六关节对应的DH参数是以第一关节为参考计算的，第七关节是以第六关节派生且第七关节对应的DH参数是以第六关节为参考计算的。
S1至少通过如下步骤实现：
S11，确定第五关节到机器人基础坐标系的已知变换矩阵
Figure BDA0003875649630000101
S12，基于并联机器人的第一标准DH参数，确定含有d1变量的变换矩阵
Figure BDA0003875649630000102
含有θ2变量的变换矩阵
Figure BDA0003875649630000103
含有θ3变量的变换矩阵
Figure BDA0003875649630000104
含有θ4变量的变换矩阵
Figure BDA0003875649630000105
以及含有θ5变量的变换矩阵
Figure BDA0003875649630000106
其中，
Figure BDA0003875649630000107
表示第一关节到机器人基础坐标系的变换矩阵；
Figure BDA0003875649630000108
表示第二关节到第一关节的变换矩阵，
Figure BDA0003875649630000109
表示第三关节到第二关节的变换矩阵，
Figure BDA00038756496300001010
表示第四关节到第三关节的变换矩阵，
Figure BDA00038756496300001011
表示第五关节到第四关节的变换矩阵；
S13，基于含有d1变量的变换矩阵
Figure BDA00038756496300001012
含有θ2变量的变换矩阵
Figure BDA00038756496300001013
含有θ3变量的变换矩阵
Figure BDA00038756496300001014
含有θ4变量的变换矩阵
Figure BDA00038756496300001015
以及含有θ5变量的变换矩阵
Figure BDA00038756496300001016
确定含有关节变量的变换矩阵
Figure BDA00038756496300001017
S14，基于已知变换矩阵
Figure BDA0003875649630000111
和含有关节变量的变换矩阵
Figure BDA0003875649630000112
之间的元素对应关系建立方程，得到第五关节角θ5；
S15，基于已知变换矩阵
Figure BDA0003875649630000113
含有θ5变量的变换矩阵
Figure BDA0003875649630000114
以及第五关节角θ5，确定第一关节沿机器人基础坐标系Z轴的移动距离d1、第二关节角θ2、第三关节角θ3以及第四关节角θ4。
S11具体实现过程如下：获取法兰末端到世界坐标系的已知变换矩阵
Figure BDA0003875649630000115
以及机器人基础坐标系到世界坐标系的已知变换矩阵
Figure BDA0003875649630000116
基于并联机器人的第一标准DH参数，确定法兰末端到第五关节的已知变换矩阵
Figure BDA0003875649630000117
基于所述
Figure BDA0003875649630000118
以及所述
Figure BDA0003875649630000119
确定第五关节到机器人基础坐标系的已知变换矩阵
Figure BDA00038756496300001110
例如：已知变换矩阵
Figure BDA00038756496300001111
的计算公式如下式(1)所示：
Figure BDA00038756496300001112
在这里，已知变换矩阵中每个元素都是已知的。
在S12中，含有d1变量的变换矩阵
Figure BDA00038756496300001113
如式(2)所示：
Figure BDA00038756496300001114
含有θ2变量的变换矩阵
Figure BDA00038756496300001115
如式(3)所示：
Figure BDA0003875649630000121
含有θ3变量的变换矩阵
Figure BDA0003875649630000122
如式(4)所示：
Figure BDA0003875649630000123
含有θ4变量的变换矩阵
Figure BDA0003875649630000124
如式(5)所示：
Figure BDA0003875649630000125
将第一标准DH参数中第五关节的α＝90°代入含有θ5变量的变换矩阵
Figure BDA0003875649630000126
中，得到如下式(6)：
Figure BDA0003875649630000127
其中，θdh，2，ddh，2，θdh，3，θdh，4，ddh，3，ddh，4，θdh.5，ddh.5均属于第一标准DH参数，可参见图3。
在S13中，含有关节变量的变换矩阵
Figure BDA0003875649630000131
可以基于如式(7)所示的公式计算，具体如下：
Figure BDA0003875649630000132
最后得到的含有关节变量的变换矩阵
Figure BDA0003875649630000133
的表达式如式(8)所示：
Figure BDA0003875649630000134
在S14中，将含有关节变量的变换矩阵
Figure BDA0003875649630000135
的第三行第一列元素和第三行第二列元素相除得到tanθ5，并结合已知变换矩阵
Figure BDA0003875649630000136
中第三行第一列元素和第三行第二列元素相除得到tanθ5的具体数值，利用反三角函数，得到第五关节角θ5。
在S15中，基于已知变换矩阵
Figure BDA0003875649630000137
含有θ5变量的变换矩阵
Figure BDA0003875649630000138
以及第五关节角θ5，确定第一关节沿机器人基础坐标系Z轴的移动距离d1、第二关节角θ2、第三关节角θ3以及第四关节角θ4，至少包括如下步骤：
基于第五关节角θ5，确定已知变换矩阵
Figure BDA0003875649630000139
基于已知变换矩阵
Figure BDA00038756496300001310
和已知变换矩阵
Figure BDA0003875649630000141
确定已知变换矩阵
Figure BDA0003875649630000142
确定含有变量的变换矩阵
Figure BDA0003875649630000143
基于已知变换矩阵
Figure BDA0003875649630000144
和含有变量的变换矩阵
Figure BDA0003875649630000145
之间的元素对应关系，确定第一关节沿机器人基础坐标系Z轴的移动距离d1、第二关节角θ2、第三关节角θ3，以及第四关节角θ4。
例如：将求解得到的θ5代入含有θ5变量的变换矩阵
Figure BDA0003875649630000146
中，得到已知变换矩阵
Figure BDA0003875649630000147
例如：已知变换矩阵
Figure BDA0003875649630000148
的计算公式如式(9)所示：
Figure BDA0003875649630000149
其中，
Figure BDA00038756496300001410
是
Figure BDA00038756496300001411
的逆矩阵。
在这里，已知变换矩阵
Figure BDA00038756496300001412
中各个元素是已知的。
获得含有变量的变换矩阵
Figure BDA00038756496300001413
的表达式如下式(10)所示：
Figure BDA00038756496300001414
之后，从已知变换矩阵
Figure BDA00038756496300001415
中提取第四关节在机器人基础坐标系下的位置坐标；并从含有变量的变换矩阵
Figure BDA00038756496300001416
中提取第四关节在机器人基础坐标系下的位置函数坐标；基于位置坐标和位置函数坐标之间的元素对应关系建立方程组，得到第一关节沿机器人基础坐标系Z轴的移动距离d1、第二关节角θ2和第三关节角θ3。将移动距离d1、第二关节角θ2、第三关节角θ3代入所述含有变量的变换矩阵
Figure BDA0003875649630000151
得到θ4作为变量的变换矩阵
Figure BDA0003875649630000152
基于已知变换矩阵
Figure BDA0003875649630000153
和θ4作为变量的变换矩阵
Figure BDA0003875649630000154
之间的元素对应关系，确定第四关节角θ4。
例如：从已知变换矩阵
Figure BDA0003875649630000155
中提取第四关节在机器人基础坐标系下的位置坐标为(x，y，z)。在这里，x、y、z分别代表
Figure BDA0003875649630000156
最后一列的前三行中的数值。
从含有变量的变换矩阵
Figure BDA0003875649630000157
中提取第四关节在机器人基础坐标系下的位置函数坐标如下式(11)所示：
Figure BDA0003875649630000158
基于位置坐标和位置函数坐标之间的元素对应关系建立方程组，例如：
Figure BDA0003875649630000159
由此可以直接计算出d1＝z-ddh，2-ddh，3-ddh，4。
而x，y各自减去一个常数后的平方和只和θ3有关，如下式(12)所示：
Figure BDA00038756496300001510
通过x、y就能求得θ3的两组解，但是此方程能解出θ3的两组解，一组为左手系的解，一组为右手系的解。结合并联机器人的实际使用情况，为了保证并联机器人的闭链结构中所形成的多边形形状为凸多边形，在选择关节角的解时，对于第一开链结构中任一关节角选用左手系的解，而对于第二开链结构中任一关节角选择右手系的解。由此，结合图4中并联机器人的结构从θ3的两组解中选取满足左手系规则的解作为第三关节的唯一解。
获得了θ3后就能通过x，y求解出θ2；求解过程不能只靠一个x或者y，需要联立求解。因为一个x或者y都能求出两组解。需要选取其中重复的那个作为θ2的解；从而获得了满足物理意义的两组解。之后从θ2的两组解中选取满足左手系规则的解作为第二关节的唯一解。
将第二关节角θ2、第三关节角θ3代入含有变量的变换矩阵
Figure BDA0003875649630000161
所属元素σ1＝sin(θ2+θ3+θ4+θdh，2+θdh，3+θdh，4)中，得到θ4。
在S2中，第一关节、第六关节、第七关节、第四关节和第五关节连接后形成的第一开链结构为主轴，且以第二关节和第三关节连接后形成第二开链结构为副轴，并联机器人的标准参数为第二标准DH参数。
在图3所示的第二标准DH参数的示意图中，以法兰为分界线分别进行说明，位于法兰之前的各关节：第六关节是从第一关节派生且第六关节对应的DH参数是以第一关节为参考计算的，第七关节是从第六关节派生且第七关节对应的DH参数是以第六关节为参考计算的，第四关节是从第七关节派生且第四关节对应的DH参数是以第七关节为参考计算的，第五关节是以第四关节派生且第五关节对应的DH参数是以第四关节为参考计算的；
位于法兰之后的各关节：第二关节是从第一关节派生且第二关节对应的DH参数是以第一关节为参考计算的，第三关节是以第二关节派生且第三关节对应的DH参数是以第二关节为参考计算的。
例如：第六关节角θ6和第七关节角θ7的求解过程与第二关节角θ2和第三关节角θ3的求解过程相类似，唯一不同的是：将第一标准DH参数替换成第二标准DH参数，并将上述求解过程中的第二关节替换成第六关节，第三关节替换成第七关节。
具体地，基于并联机器人的第二标准DH参数，确定含有d1变量的变换矩阵
Figure BDA0003875649630000171
含有θ6变量的变换矩阵
Figure BDA0003875649630000172
含有θ7变量的变换矩阵
Figure BDA0003875649630000173
含有θ4变量的变换矩阵
Figure BDA0003875649630000174
其中，
Figure BDA0003875649630000175
表示第六关节到第一关节的变换矩阵，
Figure BDA0003875649630000176
表示第七关节到第六关节的变换矩阵，
Figure BDA0003875649630000177
表示第四关节到第七关节的变换矩阵。基于含有d1变量的变换矩阵
Figure BDA0003875649630000178
含有θ6变量的变换矩阵
Figure BDA0003875649630000179
含有θ7变量的变换矩阵
Figure BDA00038756496300001710
含有θ4变量的变换矩阵
Figure BDA00038756496300001711
确定含有关节变量的
Figure BDA00038756496300001712
从已知变换矩阵
Figure BDA00038756496300001713
中提取第四关节在机器人基础坐标系下的位置坐标；并从所述含有变量的变换矩阵
Figure BDA00038756496300001714
中提取第四关节在机器人基础坐标系下的位置函数坐标；基于所述位置坐标和所述位置函数坐标之间的元素对应关系建立方程组，得到第六关节角θ6和第七关节角θ7。例如：变换矩阵
Figure BDA00038756496300001715
如下式(13)所示：
Figure BDA00038756496300001716
变换矩阵
Figure BDA00038756496300001717
如下式(14)所示：
Figure BDA0003875649630000181
从所述含有变量的变换矩阵
Figure BDA0003875649630000182
中提取第四关节在机器人基础坐标系下的位置函数坐标如下式(15)所示：
Figure BDA0003875649630000183
基于位置坐标和位置函数坐标之间的元素对应关系建立方程组，化简后得到；
Figure BDA0003875649630000184
通过x、y就能求得θ6的两组解。但是此方程能解出θ6的两组解，一组为左手系的解，一组为右手系的解。结合并联机器人的实际使用情况，为了保证并联机器人的闭链结构中间所形成的多边形形状为凸多边形，对于第二开链结构中任一关节角选择右手系的解；由此，从θ6的两组解中选取满足左手系规则的解作为第六关节的唯一解。
在这里，在求解θ6时确定已知变换矩阵
Figure BDA0003875649630000185
的方法与求解θ3时确定已知变换矩阵
Figure BDA0003875649630000186
方法相同。
获得了θ6后就能通过x，y求解出θ7。求解过程不能只靠一个x或者y，需要联立求解。因为一个x或者y都能求出两组解。需要选取其中重复的那个作为θ7的解；之后从θ7的两组解中选取满足左手系规则的解作为第七关节的唯一解。
需要注意的是，虽然第一标准参数和第二标准参数中都出现了θdh，x(°)、ddh,x(mm)、adh，x(mm)，但是所属的主轴结构不同，因此在第一标准DH参数和第二标准DH参数中对于相同表达形式的参数所对应的具体数值是不同的。
本实施例基于串并混合的解析法对并联机器人进行姿态求解，不仅计算速度快；而且从动轴旋转角的计算不依赖主动轴旋转角的计算，从动轴和主动轴都是单独计算，提高了机器人姿态计算的准确性。
需要说明的是，本实施例提到的机器人基础坐标系均是指机器人base坐标系。
在一优选的实施方式中，确定含有变量的变换矩阵
Figure BDA0003875649630000191
至少包括如下步骤：
S201，获取含有d1变量的变换矩阵
Figure BDA0003875649630000192
含有θ2变量的变换矩阵
Figure BDA0003875649630000193
含有θ3变量的变换矩阵
Figure BDA0003875649630000194
以及含有θ4变量的变换矩阵
Figure BDA0003875649630000195
S202，基于含有d1变量的变换矩阵
Figure BDA0003875649630000196
含有θ2变量的变换矩阵
Figure BDA0003875649630000197
含有θ3变量的变换矩阵
Figure BDA0003875649630000198
含有θ4变量的变换矩阵
Figure BDA0003875649630000199
确定含有变量的变换矩阵
Figure BDA00038756496300001910
例如：含有变量的变换矩阵
Figure BDA00038756496300001911
的表达式如下式(17)所示：
Figure BDA00038756496300001912
由此，能够基于第二标准DH参数确定含有变量的变换矩阵
Figure BDA00038756496300001913
需要说明的是，θ2表示第二关节的旋转角度，θ3表示第三关节的旋转角度，θ4表示第四关节的旋转角度，θ5表示第五关节的旋转角度，θ6表示第六关节的旋转角度，θ7表示第七关节的旋转角度。
应理解，在本发明的各种实施例中，上述各过程的序号的大小并不意味着执行顺序的先后，各过程的执行顺序应以其功能和内在的逻辑确定，而不应对本发明实施例的实施过程构成任何限定。
如图5所示，为本发明一实施例提供的一种基于逆向运动学确定并联机器人姿态的装置的结构示意图。一种基于逆向运动学确定并联机器人姿态的装置，该装置500具体包括：划分模块501，用于将并联机器人的闭链结构划分为第一开链结构和第二开链结构；所述闭链结构是由第一关节、第二关节、第三关节、第四关节、第五关节、第六关节以及第七关节连接形成的闭合结构；所述第一开链结构和所述第二开链结构均是通过若干关节连接形成的结构；拟解求解模块502，用于基于法兰末端以及所述闭链结构各关节对应的标准DH参数，进行运动学逆解求解，得到第一关节沿机器人基础坐标系Z轴的移动距离d1、第二关节角θ2、第三关节角θ3、第四关节角θ4、第五关节角θ5、第六关节角θ6和第七关节角θ7；第一选取模块503，用于当所述第一开链结构是由第一关节、第二关节、第三关节、第四关节、第五关节连接形成时，针对所述第一开链结构中任一关节角：若所述关节角存在两组解，则从所述两组解中选取满足第一预设条件的解作为该关节角的唯一解；第二选取模块504，用于当所述第一开链结构是由第一关节、第六关节、第七关节、第四关节和第五关节连接形成时，针对所述第二开链结构中任一关节角：若所述关节角存在两组解，则从所述两组解中选取满足第二预设条件的解作为该关节角的唯一解。
在可选的实施方式中，所述拟解求解模块包括：第一拟解求解模块，用于基于法兰末端，以及由第一关节、第二关节、第三关节、第四关节、第五关节连接形成的第一开链结构中各关节对应的第一标准DH参数，进行运动学逆解求解，得到第一关节沿机器人基础坐标系Z轴的移动距离d1、第二关节角θ2、第三关节角θ3、第四关节角θ4和第五关节角θ5；第二拟解求解模块，用于基于法兰末端，以及由第一关节、第四关节、第五关节、第六关节和第七关节连接形成的第一开链结构中各关节对应的的第二标准DH参数，进行运动学逆解求解，得到第六关节角θ6和第七关节角θ7。
在可选的实施方式中，第一拟解求解模块包括：第一确定单元，用于确定第五关节到机器人基础坐标系的已知变换矩阵
Figure BDA0003875649630000211
第二确定单元，用于基于所述并联机器人的第一标准DH参数，确定含有d1变量的变换矩阵
Figure BDA0003875649630000212
含有θ2变量的变换矩阵
Figure BDA0003875649630000213
含有θ3变量的变换矩阵
Figure BDA0003875649630000214
含有θ4变量的变换矩阵
Figure BDA0003875649630000215
以及含有θ5变量的变换矩阵
Figure BDA0003875649630000216
其中，
Figure BDA0003875649630000217
表示第一关节到机器人基础坐标系的变换矩阵；
Figure BDA0003875649630000218
表示第二关节到第一关节的变换矩阵，
Figure BDA0003875649630000219
表示第三关节到第二关节的变换矩阵，
Figure BDA00038756496300002110
表示第四关节到第三关节的变换矩阵，
Figure BDA00038756496300002111
表示第五关节到第四关节的变换矩阵；第三确定单元，用于基于含有d1变量的变换矩阵
Figure BDA00038756496300002112
含有θ2变量的变换矩阵
Figure BDA00038756496300002113
含有θ3变量的变换矩阵
Figure BDA00038756496300002114
含有θ4变量的变换矩阵
Figure BDA00038756496300002115
以及含有θ5变量的变换矩阵
Figure BDA00038756496300002116
确定含有关节变量的变换矩阵
Figure BDA00038756496300002117
求解单元，用于基于已知变换矩阵
Figure BDA00038756496300002118
和含有关节变量的变换矩阵
Figure BDA00038756496300002119
之间的元素对应关系建立方程，得到第五关节角θ5；第四确定单元，用于基于已知变换矩阵
Figure BDA00038756496300002120
含有θ5变量的变换矩阵
Figure BDA00038756496300002121
以及第五关节角θ5，确定第一关节沿机器人基础坐标系Z轴的移动距离d1、第二关节角θ2、第三关节角θ3以及第四关节角θ4。
在可选的实施方式中，第一确定单元包括：获取子单元，用于获取法兰末端到世界坐标系的已知变换矩阵
Figure BDA0003875649630000221
以及机器人基础坐标系到世界坐标系的已知变换矩阵
Figure BDA0003875649630000222
第一确定子单元，用于基于并联机器人的第一标准DH参数，确定法兰末端到第五关节的已知变换矩阵
Figure BDA0003875649630000223
第二确定子单元，用于基于
Figure BDA0003875649630000224
以及
Figure BDA0003875649630000225
确定第五关节到机器人基础坐标系的已知变换矩阵
Figure BDA0003875649630000226
在可选的实施方式中，第四确定单元包括：第一确定子单元，用于基于第五关节角θ5，确定已知变换矩阵
Figure BDA0003875649630000227
第二确定子单元，用于基于已知变换矩阵
Figure BDA0003875649630000228
和已知变换矩阵
Figure BDA0003875649630000229
确定已知变换矩阵
Figure BDA00038756496300002210
第三确定子单元，用于确定含有变量的变换矩阵
Figure BDA00038756496300002211
第四确定子单元，用于基于已知变换矩阵
Figure BDA00038756496300002212
和含有变量的变换矩阵
Figure BDA00038756496300002213
之间的元素对应关系，确定第一关节沿机器人基础坐标系Z轴的移动距离d1、第二关节角θ2、第三关节角θ3，以及第四关节角θ4。
在可选的实施方式中，第三确定子单元包括：基于含有d1变量的变换矩阵
Figure BDA00038756496300002214
含有θ2变量的变换矩阵
Figure BDA00038756496300002215
含有θ3变量的变换矩阵
Figure BDA00038756496300002216
含有θ4变量的变换矩阵
Figure BDA00038756496300002217
确定含有变量的变换矩阵
Figure BDA00038756496300002218
在可选的实施方式中，第四确定子单元包括：提取单元，用于从已知变换矩阵
Figure BDA00038756496300002219
中提取第四关节在机器人基础坐标系下的位置坐标；并从含有变量的变换矩阵
Figure BDA00038756496300002220
中提取第四关节在机器人基础坐标系下的位置函数坐标；确定单元，用于基于位置坐标和位置函数坐标之间的元素对应关系建立方程组，得到第一关节沿机器人基础坐标系Z轴的移动距离d1、第二关节角θ2和第三关节角θ3；代入求解单元，用于将移动距离d1、第二关节角θ2、第三关节角θ3代入含有变量的变换矩阵
Figure BDA00038756496300002221
得到θ4作为变量的变换矩阵
Figure BDA0003875649630000231
对应单元，用于基于已知变换矩阵
Figure BDA0003875649630000232
和θ4作为变量的变换矩阵
Figure BDA0003875649630000233
之间的元素对应关系，确定第四关节角θ4。
上述装置可执行本发明一实施例所提供的基于逆向运动学确定并联机器人姿态的方法，具备执行基于逆向运动学确定并联机器人姿态的方法相应的功能模块和有益效果。未在本实施例中详尽描述的技术细节，可参见本发明实施例所提供的基于逆向运动学确定并联机器人姿态的方法。
本发明还提供一种电子设备，包括：处理器；用于存储所述处理器可执行指令的存储器；所述处理器，用于从所述存储器中读取所述可执行指令，并执行所述指令以实现本发明所述的基于逆向运动学确定并联机器人姿态的方法。
除了上述方法和设备以外，本申请的实施例还可以是计算机程序产品，其包括计算机程序指令，所述计算机程序指令在被处理器运行时使得所述处理器执行本说明书上述“示例性方法”部分中描述的根据本申请各种实施例的方法中的步骤。
所述计算机程序产品可以以一种或多种程序设计语言的任意组合来编写用于执行本申请实施例操作的程序代码，所述程序设计语言包括面向对象的程序设计语言，诸如Java、C++等，还包括常规的过程式程序设计语言，诸如“C”语言或类似的程序设计语言。程序代码可以完全地在用户计算设备上执行、部分地在用户设备上执行、作为一个独立的软件包执行、部分在用户计算设备上部分在远程计算设备上执行、或者完全在远程计算设备或服务器上执行。
此外，本申请的实施例还可以是计算机可读存储介质，其上存储有计算机程序指令，所述计算机程序指令在被处理器运行时使得所述处理器执行本说明书上述“示例性方法”部分中描述的根据本申请如下各实施例的方法中的步骤。
所述计算机可读存储介质可以采用一个或多个可读介质的任意组合。可读介质可以是可读信号介质或者可读存储介质。可读存储介质例如可以包括但不限于电、磁、光、电磁、红外线、或半导体的***、装置或器件，或者任意以上的组合。可读存储介质的更具体的例子(非穷举的列表)包括：具有一个或多个导线的电连接、便携式盘、硬盘、随机存取存储器(RAM)、只读存储器(ROM)、可擦式可编程只读存储器(EPROM或闪存)、光纤、便携式紧凑盘只读存储器(CD-ROM)、光存储器件、磁存储器件、或者上述的任意合适的组合。
以上结合具体实施例描述了本申请的基本原理，但是，需要指出的是，在本申请中提及的优点、优势、效果等仅是示例而非限制，不能认为这些优点、优势、效果等是本申请的各个实施例必须具备的。另外，上述公开的具体细节仅是为了示例的作用和便于理解的作用，而非限制，上述细节并不限制本申请为必须采用上述具体的细节来实现。
本申请中涉及的器件、装置、设备、***的方框图仅作为例示性的例子并且不意图要求或暗示必须按照方框图示出的方式进行连接、布置、配置。如本领域技术人员将认识到的，可以按任意方式连接、布置、配置这些器件、装置、设备、***。诸如“包括”、“包含”、“具有”等等的词语是开放性词汇，指“包括但不限于”，且可与其互换使用。这里所使用的词汇“或”和“和”指词汇“和/或”，且可与其互换使用，除非上下文明确指示不是如此。这里所使用的词汇“诸如”指词组“如但不限于”，且可与其互换使用。
还需要指出的是，在本申请的装置、设备和方法中，各部件或各步骤是可以分解和/或重新组合的。这些分解和/或重新组合应视为本申请的等效方案。
提供所公开的方面的以上描述以使本领域的任何技术人员能够做出或者使用本申请。对这些方面的各种修改对于本领域技术人员而言是非常显而易见的，并且在此定义的一般原理可以应用于其他方面而不脱离本申请的范围。因此，本申请不意图被限制到在此示出的方面，而是按照与在此公开的原理和新颖的特征一致的最宽范围。
为了例示和描述的目的已经给出了以上描述。此外，此描述不意图将本申请的实施例限制到在此公开的形式。尽管以上已经讨论了多个示例方面和实施例，但是本领域技术人员将认识到其某些变型、修改、改变、添加和子组合。
在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。而且，描述的具体特征、结构、材料或者特点可以在任一个或多个实施例或示例中以合适的方式结合。此外，在不相互矛盾的情况下，本领域的技术人员可以将本说明书中描述的不同实施例或示例以及不同实施例或示例的特征进行结合和组合。
此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或隐含地包括至少一个该特征。在本发明的描述中，“多个”的含义是两个或两个以上，除非另有明确具体的限定。
以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以所述权利要求的保护范围为准。

Claims (10)

1.基于逆向运动学确定并联机器人姿态的方法，其特征在于，包括：
将并联机器人的闭链结构划分为第一开链结构和第二开链结构；所述闭链结构是由第一关节、第二关节、第三关节、第四关节、第五关节、第六关节以及第七关节连接形成的闭合结构；所述第一开链结构和所述第二开链结构均是通过若干关节连接形成的结构；
基于法兰末端以及所述闭链结构各关节对应的标准DH参数，进行运动学逆解求解，得到第一关节沿机器人基础坐标系Z轴的移动距离d1、第二关节角θ2、第三关节角θ3、第四关节角θ4、第五关节角θ5、第六关节角θ6和第七关节角θ7；
当所述第一开链结构是由第一关节、第二关节、第三关节、第四关节、第五关节连接形成时，针对所述第一开链结构中任一关节角：若所述关节角存在两组解，则从所述两组解中选取满足第一预设条件的解作为该关节角的唯一解；
当所述第一开链结构是由第一关节、第六关节、第七关节、第四关节和第五关节连接形成时，针对所述第二开链结构中任一关节角：若所述关节角存在两组解，则从所述两组解中选取满足第二预设条件的解作为该关节角的唯一解。
2.根据权利要求1所述的方法，其特征在于，所述基于法兰末端以及所述闭链结构各关节对应的标准DH参数，进行运动学逆解求解，得到第一关节沿机器人基础坐标系Z轴的移动距离d1、第二关节角θ2、第三关节角θ3、第四关节角θ4、第五关节角θ5、第六关节角θ6和第七关节角θ7，包括：
基于法兰末端，以及由第一关节、第二关节、第三关节、第四关节、第五关节连接形成的第一开链结构中各关节对应的第一标准DH参数，进行运动学逆解求解，得到第一关节沿机器人基础坐标系Z轴的移动距离d1、第二关节角θ2、第三关节角θ3、第四关节角θ4和第五关节角θ5；
基于法兰末端，以及由第一关节、第四关节、第五关节、第六关节和第七关节连接形成的第一开链结构中各关节对应的第二标准DH参数，进行运动学逆解求解，得到第六关节角θ6和第七关节角θ7。
3.根据权利要求2所述的方法，其特征在于，所述基于法兰末端，以及由第一关节、第二关节、第三关节、第四关节、第五关节连接形成的第一开链结构中各关节对应的第一标准DH参数，进行运动学逆解求解，得到第一关节沿机器人基础坐标系Z轴的移动距离d1、第二关节角θ2、第三关节角θ3、第四关节角θ4和第五关节角θ5，包括：
确定第五关节到机器人基础坐标系的已知变换矩阵
Figure FDA0003875649620000021
基于所述并联机器人的第一标准DH参数，确定含有d1变量的变换矩阵
Figure FDA0003875649620000022
含有θ2变量的变换矩阵
Figure FDA0003875649620000023
含有θ3变量的变换矩阵
Figure FDA0003875649620000024
含有θ4变量的变换矩阵
Figure FDA0003875649620000025
以及含有θ5变量的变换矩阵
Figure FDA0003875649620000026
其中，
Figure FDA0003875649620000027
表示第一关节到机器人基础坐标系的变换矩阵；
Figure FDA0003875649620000028
表示第二关节到第一关节的变换矩阵，
Figure FDA0003875649620000029
表示第三关节到第二关节的变换矩阵，
Figure FDA00038756496200000210
表示第四关节到第三关节的变换矩阵，
Figure FDA00038756496200000211
表示第五关节到第四关节的变换矩阵；
基于所述含有d1变量的变换矩阵
Figure FDA00038756496200000212
所述含有θ2变量的变换矩阵
Figure FDA00038756496200000213
所述含有θ3变量的变换矩阵
Figure FDA00038756496200000214
所述含有θ4变量的变换矩阵
Figure FDA00038756496200000215
以及所述含有θ5变量的变换矩阵
Figure FDA00038756496200000216
确定含有关节变量的变换矩阵
Figure FDA00038756496200000217
基于所述已知变换矩阵
Figure FDA00038756496200000218
和所述含有关节变量的变换矩阵
Figure FDA00038756496200000219
之间的元素对应关系建立方程，得到第五关节角θ5；
基于所述已知变换矩阵
Figure FDA00038756496200000220
所述含有θ5变量的变换矩阵
Figure FDA00038756496200000221
以及所述第五关节角θ5，确定第一关节沿机器人基础坐标系Z轴的移动距离d1、第二关节角θ2、第三关节角θ3以及第四关节角θ4。
4.根据权利要求3所述的方法，其特征在于，所述确定第五关节到机器人基础坐标系的已知变换矩阵
Figure FDA0003875649620000031
包括：
获取法兰末端到世界坐标系的已知变换矩阵
Figure FDA0003875649620000032
以及机器人基础坐标系到世界坐标系的已知变换矩阵
Figure FDA0003875649620000033
基于所述并联机器人的第一标准DH参数，确定法兰末端到第五关节的已知变换矩阵
Figure FDA0003875649620000034
基于所述
Figure FDA0003875649620000035
以及所述
Figure FDA0003875649620000036
确定第五关节到机器人基础坐标系的已知变换矩阵
Figure FDA0003875649620000037
5.根据权利要求3所述的方法，其特征在于，所述基于所述已知变换矩阵
Figure FDA0003875649620000038
所述含有θ5变量的变换矩阵
Figure FDA0003875649620000039
以及所述第五关节角θ5，确定第一关节沿机器人基础坐标系Z轴的移动距离d1、第二关节角θ2、第三关节角θ3以及第四关节角θ4，包括：
基于所述第五关节角θ5，确定已知变换矩阵
Figure FDA00038756496200000310
基于所述已知变换矩阵
Figure FDA00038756496200000311
和所述已知变换矩阵
Figure FDA00038756496200000312
确定已知变换矩阵
Figure FDA00038756496200000313
确定含有变量的变换矩阵
Figure FDA00038756496200000314
基于所述已知变换矩阵
Figure FDA00038756496200000315
和所述含有变量的变换矩阵
Figure FDA00038756496200000316
之间的元素对应关系，确定第一关节沿机器人基础坐标系Z轴的移动距离d1、第二关节角θ2、第三关节角θ3，以及第四关节角θ4。
6.根据权利要求5所述的方法，其特征在于，所述确定含有变量的变换矩阵
Figure FDA00038756496200000317
包括：
基于所述含有d1变量的变换矩阵
Figure FDA0003875649620000041
所述含有θ2变量的变换矩阵
Figure FDA0003875649620000042
所述含有θ3变量的变换矩阵
Figure FDA0003875649620000043
所述含有θ4变量的变换矩阵
Figure FDA0003875649620000044
确定含有变量的变换矩阵
Figure FDA0003875649620000045
7.根据权利要求5所述的方法的，其特征在于，所述基于所述已知变换矩阵
Figure FDA0003875649620000046
和所述含有变量的变换矩阵
Figure FDA0003875649620000047
之间的元素对应关系，确定第一关节沿机器人基础坐标系Z轴的移动距离d1、第二关节角θ2、第三关节角θ3，以及第四关节角θ4，包括：
从所述已知变换矩阵
Figure FDA0003875649620000048
中提取第四关节在机器人基础坐标系下的位置坐标；并从所述含有变量的变换矩阵
Figure FDA0003875649620000049
中提取第四关节在机器人基础坐标系下的位置函数坐标；
基于所述位置坐标和所述位置函数坐标之间的元素对应关系建立方程组，得到第一关节沿机器人基础坐标系Z轴的移动距离d1、第二关节角θ2和第三关节角θ3；
将所述移动距离d1、所述第二关节角θ2、所述第三关节角θ3代入所述含有变量的变换矩阵
Figure FDA00038756496200000410
得到θ4作为变量的变换矩阵
Figure FDA00038756496200000411
基于所述已知变换矩阵
Figure FDA00038756496200000412
和所述θ4作为变量的变换矩阵
Figure FDA00038756496200000413
之间的元素对应关系，确定第四关节角θ4。
8.基于逆向运动学确定并联机器人姿态的装置，其特征在于，包括：
划分模块，用于将并联机器人的闭链结构划分为第一开链结构和第二开链结构；所述闭链结构是由第一关节、第二关节、第三关节、第四关节、第五关节、第六关节以及第七关节连接形成的闭合结构；所述第一开链结构和所述第二开链结构均是通过若干关节连接形成的结构；
拟解求解模块，用于基于法兰末端以及所述闭链结构各关节对应的标准DH参数，进行运动学逆解求解，得到第一关节沿机器人基础坐标系Z轴的移动距离d1、第二关节角θ2、第三关节角θ3、第四关节角θ4、第五关节角θ5、第六关节角θ6和第七关节角θ7；
第一选取模块，用于当所述第一开链结构是由第一关节、第二关节、第三关节、第四关节、第五关节连接形成时，针对所述第一开链结构中任一关节角：若所述关节角存在两组解，则从所述两组解中选取满足第一预设条件的解作为该关节角的唯一解；
第二选取模块，用于当所述第一开链结构是由第一关节、第六关节、第七关节、第四关节和第五关节连接形成时，针对所述第二开链结构中任一关节角：若所述关节角存在两组解，则从所述两组解中选取满足第二预设条件的解作为该关节角的唯一解。
9.根据权利要求8所述的装置，其特征在于，所述拟解求解模块包括：
第一拟解求解模块，用于基于法兰末端，以及由第一关节、第二关节、第三关节、第四关节、第五关节连接形成的第一开链结构中各关节对应的第一标准DH参数，进行运动学逆解求解，得到第一关节沿机器人基础坐标系Z轴的移动距离d1、第二关节角θ2、第三关节角θ3、第四关节角θ4和第五关节角θ5；
第二拟解求解模块，用于基于法兰末端，以及由第一关节、第四关节、第五关节、第六关节和第七关节连接形成的第一开链结构中各关节对应的第二标准DH参数，进行运动学逆解求解，得到第六关节角θ6和第七关节角θ7。
10.一种计算机可读介质，其上存储有计算机程序，所述程序被处理器执行时实现如权利要求1-7中任一项所述的方法。
CN202211214223.9A 2022-09-30 2022-09-30 基于逆向运动学确定并联机器人姿态的方法及装置 Pending CN115648177A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211214223.9A CN115648177A (zh) 2022-09-30 2022-09-30 基于逆向运动学确定并联机器人姿态的方法及装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211214223.9A CN115648177A (zh) 2022-09-30 2022-09-30 基于逆向运动学确定并联机器人姿态的方法及装置

Publications (1)

Publication Number Publication Date
CN115648177A true CN115648177A (zh) 2023-01-31

Family

ID=84986164

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211214223.9A Pending CN115648177A (zh) 2022-09-30 2022-09-30 基于逆向运动学确定并联机器人姿态的方法及装置

Country Status (1)

Country Link
CN (1) CN115648177A (zh)

Similar Documents

Publication Publication Date Title
EP4068150A1 (en) Hand key point detection method, gesture recognition method, and related devices
US8185265B2 (en) Path planning device, path planning method, and computer program
CN109732593B (zh) 一种机器人的远端控制方法、装置及终端设备
Gu et al. Automated assembly skill acquisition and implementation through human demonstration
CN1832670A (zh) 虚拟环境下的线缆布线方法
Uchida et al. Triangularizing kinematic constraint equations using Gröbner bases for real-time dynamic simulation
CN113119104A (zh) 机械臂控制方法、机械臂控制装置、计算设备及***
Le et al. Deformation-aware data-driven grasp synthesis
CN113084791B (zh) 机械臂控制方法、机械臂控制装置及终端设备
CN115648177A (zh) 基于逆向运动学确定并联机器人姿态的方法及装置
CN115771146A (zh) 一种机器人姿态插补方法、装置及存储介质
Wu et al. Formal kinematic analysis of a general 6R manipulator using the screw theory
Zhao et al. Model and simulation of the mitsubishi rv-m1 robot using matlab
CN115488860A (zh) 基于正向运动学确定并联机器人位置的方法及装置
JP5083992B1 (ja) 把持姿勢生成装置、把持姿勢生成方法及び把持姿勢生成プログラム
CN115798030A (zh) 基于旋量的手势识别方法、装置、电子设备和存储介质
Coscia et al. 3-d hand pose estimation from kinect’s point cloud using appearance matching
CN113103239A (zh) 一种机器人姿态轨迹生成方法、装置及存储介质
CN113400310B (zh) 三指机器人灵巧手逆运动学的形式化验证方法及电子设备
CN114378860B (zh) 软体机器人操纵器的生成式设计技术
Mayyas et al. A method for the automatic generation of inverse kinematic maps in modular robotic systems
Eizicovits et al. Automatic graspability map generation based on shape-primitives for unknown and familiar objects
CN110532704B (zh) 一种动力数据获取方法及装置
Zhao et al. Planning for grasping cluttered objects based on obstruction degree
Wong et al. Manipulator system designs for drawing random objects through point cloud posture estimation

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