CN113814988B - 7自由度srs型机械臂逆解的解析方法、装置及电子设备 - Google Patents

7自由度srs型机械臂逆解的解析方法、装置及电子设备 Download PDF

Info

Publication number
CN113814988B
CN113814988B CN202111406791.4A CN202111406791A CN113814988B CN 113814988 B CN113814988 B CN 113814988B CN 202111406791 A CN202111406791 A CN 202111406791A CN 113814988 B CN113814988 B CN 113814988B
Authority
CN
China
Prior art keywords
joint
coordinate
shoulder
joints
wrist
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
Application number
CN202111406791.4A
Other languages
English (en)
Other versions
CN113814988A (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.)
Zhejiang Lab
Original Assignee
Zhejiang Lab
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 Zhejiang Lab filed Critical Zhejiang Lab
Priority to CN202111406791.4A priority Critical patent/CN113814988B/zh
Publication of CN113814988A publication Critical patent/CN113814988A/zh
Application granted granted Critical
Publication of CN113814988B publication Critical patent/CN113814988B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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/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/161—Hardware, e.g. neural networks, fuzzy logic, interfaces, processor

Landscapes

  • Engineering & Computer Science (AREA)
  • Automation & Control Theory (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Physics & Mathematics (AREA)
  • Artificial Intelligence (AREA)
  • Evolutionary Computation (AREA)
  • Fuzzy Systems (AREA)
  • Mathematical Physics (AREA)
  • Software Systems (AREA)
  • Manipulator (AREA)

Abstract

本发明公开了一种7自由度SRS型机械臂逆解的解析方法、装置及电子设备，所述方法包括：建立机械臂的世界坐标系、肩部固定坐标系以及各关节的局部坐标系；将机械臂腕部在世界坐标系下的第一位置和第一姿态转换为在肩部固定坐标系下的第二位置和第二姿态；根据第二位置、机械臂的上臂长和下臂长，计算肘关节的关节角；在肩部的三个关节中，选择一个关节的关节角作为优化变量；根据肘关节的关节角、优化变量、第二位置、第一坐标和第二坐标，计算肩部其余两个关节的关节角；根据优化变量、肩部其余两个关节的关节角、肘关节的关节角和第二姿态，计算腕部三个关节的关节角。

Description

7自由度SRS型机械臂逆解的解析方法、装置及电子设备
技术领域
本申请涉及机器人机械臂逆解领域，尤其涉及一种7自由度SRS型机械臂逆解的解析方法、装置及电子设备。
背景技术
6个自由度的机械臂由于其关节限位、奇异构型等原因，其灵活性不高，操作空间较小。因此，实际应用中，多采用7个甚至更多自由度的机械臂，来提升灵活性和操作空间。
机械臂在执行任务时，任务信息是在笛卡尔空间中进行描述的，然而在控制实际机械臂时，是通过控制机械臂各个关节的电机来完成任务的，因此需要将任务在笛卡尔空间的描述转换为机械臂的关节空间，这种转换即是机械臂的逆运动学。笛卡尔空间的任务描述为位置和姿态共六维数据，因此对于由7个关节组成的7自由度机械臂而言，有一个冗余度，即根据已知的六个位姿参数，求解机械臂的7个关节角，缺少了一个已知参数。
在实现本发明的过程中，发明人发现现有技术中至少存在如下问题：
目前，冗余机械臂的逆解，多采用数值解法，然而数值解法存在的问题是迭代时间较长，精度不高，且同一目标位姿，每次迭代出的逆解有可能是不同的，因此，对于精度和计算时间要求较高的应用场景，数值解法并不适用。
发明内容
本申请实施例的目的是提供一种7自由度SRS型机械臂逆解的解析方法、装置及电子设备，以解决相关技术中存在的机械臂逆解精度和效率不足的技术问题。
根据本申请实施例的第一方面，提供一种7自由度SRS型机械臂逆解的解析方法，包括：
建立机械臂的世界坐标系、肩部固定坐标系以及各关节的局部坐标系；
将机械臂腕部在世界坐标系下的第一位置和第一姿态转换为在肩部固定坐标系下的第二位置和第二姿态；
根据所述第二位置、机械臂的上臂长和下臂长，计算肘关节的关节角；
在肩部的三个关节中，选择一个关节的关节角作为优化变量；
根据所述肘关节的关节角、优化变量、第二位置、第一坐标和第二坐标，计算肩部其余两个关节的关节角，其中所述第一坐标为机械臂处于零位状态时腕部在肘关节的局部坐标系下的坐标，所述第二坐标为机械臂处于零位状态时肘部在所述优化变量对应的关节的局部坐标系下的坐标；
根据所述优化变量、肩部其余两个关节的关节角、肘关节的关节角和第二姿态，计算腕部三个关节的关节角。
进一步地，所述方法还包括：
根据所述优化变量，计算肘关节和优化变量对应的关节之外的关节角，重复此步骤直至遍历完所述优化变量对应的关节的运动范围；
在遍历过程中得到的关节角的多组逆解中，选择最优解。
进一步地，将机械臂腕部在世界坐标系下的第一位置和第一姿态转换为在肩部固定坐标系下的第二位置和第二姿态，包括：
根据肩部固定坐标系与世界坐标系之间的转换关系，将所述第一姿态转换为第二姿态；
根据所述转换关系，将所述第一位置转换为计算第二位置。
进一步地，根据所述第二位置、机械臂的上臂长和下臂长，计算肘关节的关节角，包括：
根据所述第二位置，计算肩部与腕部连线的长度；
根据所述机械臂的上臂长、下臂长和肩部与腕部连线的长度，计算肘关节的关节角。
进一步地，根据所述肘关节的关节角、优化变量、第二位置、第一坐标和第二坐标，计算肩部其余两个关节的关节角，包括：
根据所述肘关节的关节角和第一坐标，计算第三坐标，所述第三坐标为肘关节旋转后腕部在肘关节的局部坐标系下的坐标；
根据所述优化变量、第二坐标以及所述第三坐标，计算第四坐标，所述第四坐标为使用肩部其余两个关节的关节角描述的腕部在肩固定坐标系下的坐标；
根据所述第四坐标和第二位置，计算肩部其余两个关节的关节角。
进一步地，根据所述优化变量、肩部其余两个关节的关节角、肘关节的关节角和第二姿态，计算腕部三个关节的关节角，包括：
根据所述优化变量、肩部其余两个关节的关节角、肘关节的关节角，计算第三姿态，所述第三姿态为使用腕部三个关节的关节角描述的腕部在肩固定坐标系下的姿态；
根据所述第三姿态和第二姿态，计算腕部三个关节的关节角。
根据本申请实施例的第二方面，提供一种7自由度SRS型机械臂逆解的解析装置，包括：
建立模块，用于建立机械臂的世界坐标系、肩部固定坐标系以及各关节的局部坐标系；
转换模块，用于将机械臂腕部在世界坐标系下的第一位置和第一姿态转换为在肩部固定坐标系下的第二位置和第二姿态；
第一计算模块，用于根据所述第二位置、机械臂的上臂长和下臂长，计算肘关节的关节角；
选择模块，用于在肩部的三个关节中，选择一个关节的关节角作为优化变量；
第二计算模块，用于根据所述肘关节的关节角、优化变量、第二位置、第一坐标和第二坐标，计算肩部其余两个关节的关节角，其中所述第一坐标为机械臂处于零位状态时腕部在肘关节的局部坐标系下的坐标，所述第二坐标为机械臂处于零位状态时肘部在所述优化变量对应的关节的局部坐标系下的坐标；
第三计算模块，用于根据所述优化变量、肩部其余两个关节的关节角、肘关节的关节角和第二姿态，计算腕部三个关节的关节角。
根据本申请实施例的第三方面，提供一种电子设备，包括：
一个或多个处理器；
存储器，用于存储一个或多个程序；
当所述一个或多个程序被所述一个或多个处理器执行，使得所述一个或多个处理器实现如第一方面所述的方法。
根据本申请实施例的第四方面，提供一种计算机可读存储介质，其上存储有计算机指令，其特征在于，该指令被处理器执行时实现如第一方面所述方法的步骤。
本申请的实施例提供的技术方案可以包括以下有益效果：
由上述实施例可知，本申请建立7自由度机械臂的世界坐标系、肩部固定坐标系和关节局部坐标系，通过坐标转换得到的机械臂腕部在肩部固定坐标系下的第二位置和机械臂上臂长、下臂长，计算肘关节的关节角；通过选择肩部的三个关节中的一个关节作为优化变量，即在该关节的旋转范围内，进行遍历，每次遍历时，可认为该关节角已知；通过肘关节的关节角和优化变量的已知，计算出机械臂其余五个关节的关节角，解决了7自由度SRS型机械臂逆解的冗余问题。
应当理解的是，以上的一般描述和后文的细节描述仅是示例性和解释性的，并不能限制本申请。
附图说明
此处的附图被并入说明书中并构成本说明书的一部分，示出了符合本申请的实施例，并与说明书一起用于解释本申请的原理。
图1是根据一示例性实施例示出的一种7自由度SRS型机械臂逆解的解析方法的流程图。
图2是根据一示例性实施例示出的一种具有7自由度SRS型机械臂的机器人的结构示意图。
图3是根据一示例性实施例示出的步骤S12的流程图。
图4是根据一示例性实施例示出的步骤S13的流程图。
图5是根据一示例性实施例示出的步骤S15的流程图。
图6是根据一示例性实施例示出的步骤S16的流程图。
图7是根据一示例性实施例示出的一种7自由度SRS型机械臂逆解的解析方法的流程图。
图8是根据一示例性实施例示出的一种7自由度SRS型机械臂逆解的解析装置的框图。
具体实施方式
这里将详细地对示例性实施例进行说明，其示例表示在附图中。下面的描述涉及附图时，除非另有表示，不同附图中的相同数字表示相同或相似的要素。以下示例性实施例中所描述的实施方式并不代表与本申请相一致的所有实施方式。相反，它们仅是与如所附权利要求书中所详述的、本申请的一些方面相一致的装置和方法的例子。
在本申请使用的术语是仅仅出于描述特定实施例的目的，而非旨在限制本申请。在本申请和所附权利要求书中所使用的单数形式的“一种”、“所述”和“该”也旨在包括多数形式，除非上下文清楚地表示其他含义。还应当理解，本文中使用的术语“和/或”是指并包含一个或多个相关联的列出项目的任何或所有可能组合。
应当理解，尽管在本申请可能采用术语第一、第二、第三等来描述各种信息，但这些信息不应限于这些术语。这些术语仅用来将同一类型的信息彼此区分开。例如，在不脱离本申请范围的情况下，第一信息也可以被称为第二信息，类似地，第二信息也可以被称为第一信息。取决于语境，如在此所使用的词语“如果”可以被解释成为“在……时”或“当……时”或“响应于确定”。
图1是根据一示例性实施例示出的一种7自由度SRS型机械臂逆解的解析方法的流程图，需要说明的是，如图2所示，7自由度SRS机械臂包括：旋转轴互相垂直的肩部三个自由度、肘部一个自由度和旋转轴互相垂直的腕部三个自由度。如图1所示，该方法可以包括以下步骤：
步骤S11：建立机械臂的世界坐标系、肩部固定坐标系以及各关节的局部坐标系；
步骤S12：将机械臂腕部在世界坐标系下的第一位置和第一姿态转换为在肩部固定坐标系下的第二位置和第二姿态；
步骤S13：根据所述第二位置、机械臂的上臂长和下臂长，计算肘关节的关节角；
步骤S14：在肩部的三个关节中，选择一个关节的关节角作为优化变量；
步骤S15：根据所述肘关节的关节角、优化变量、第二位置、第一坐标和第二坐标，计算肩部其余两个关节的关节角，其中所述第一坐标为机械臂处于零位状态时腕部在肘关节的局部坐标系下的坐标，所述第二坐标为机械臂处于零位状态时肘部在所述优化变量对应的关节的局部坐标系下的坐标；
步骤S16：根据所述优化变量、肩部其余两个关节的关节角、肘关节的关节角和第二姿态，计算腕部三个关节的关节角。
由上述实施例可知，本申请建立7自由度机械臂的世界坐标系、肩部固定坐标系和关节局部坐标系，通过坐标转换得到的机械臂腕部在肩部固定坐标系下的第二位置和机械臂上臂长、下臂长，计算肘关节的关节角；通过选择肩部的三个关节中的一个关节作为优化变量，即在该关节的旋转范围内，进行遍历，每次遍历时，可认为该关节角已知；通过肘关节的关节角和优化变量的已知，计算出机械臂其余五个关节的关节角，解决了7自由度SRS型机械臂逆解的冗余问题。
在步骤S11的具体实施中，建立机械臂的世界坐标系、肩部固定坐标系以及各关节的局部坐标系；
具体地，参考图2，建立世界坐标系
Figure 836550DEST_PATH_IMAGE001
、肩部固定坐标系
Figure 118627DEST_PATH_IMAGE002
以及机械臂各关节的局部坐标系
Figure 711544DEST_PATH_IMAGE003
，所有坐标系符合右手法则。世界坐标系的原点位于机器人底盘中心，X轴正方向指向机器人正前方，Y轴正方向指向机器人左侧，Z轴正方向指向机器人上方。肩部固定坐标系的坐标原点位于肩部中心，三个坐标轴分别与世界坐标系的三个坐标轴相平行。肩部三个关节的局部坐标系的坐标原点位于肩部中心，肘部局部坐标系坐标原点位于肘部中心，腕部三个关节的局部坐标系坐标原点位于腕部中心，所有局部坐标系的三个坐标轴方向分别与世界坐标系的三个坐标轴平行。世界坐标系与肩部固定坐标系始终不变，每个局部坐标系的坐标轴会随着上一个关节的转动而转动。
在步骤S12的具体实施中，将机械臂腕部在世界坐标系下的第一位置和第一姿态转换为在肩部固定坐标系下的第二位置和第二姿态；
具体地，如图3所示，此步骤包括以下子步骤：
步骤S21：根据肩部固定坐标系与世界坐标系之间的转换关系，将所述第一姿态转换为第二姿态；
具体地，根据下式计算第二姿态
Figure 952033DEST_PATH_IMAGE004
：
Figure 496147DEST_PATH_IMAGE005
其中，
Figure 847494DEST_PATH_IMAGE006
为第一姿态，
Figure 465557DEST_PATH_IMAGE007
是肩部固定坐标系与世界坐标系之间的转换关系，其中
Figure 1580DEST_PATH_IMAGE008
为肩部固定坐标系的原点在世界坐标系下的坐标，
Figure 642777DEST_PATH_IMAGE009
为肩部固定坐标系相对于世界坐标系的旋转矩阵。
步骤S22：根据所述转换关系，将所述第一位置转换为计算第二位置；
具体地，根据下式计算第二位置
Figure 656870DEST_PATH_IMAGE010
：
Figure 598281DEST_PATH_IMAGE011
其中，
Figure 977310DEST_PATH_IMAGE012
为第一位置，
Figure 997480DEST_PATH_IMAGE007
根据肩部固定坐标系与世界坐标系之间的转换关系，其中
Figure 690630DEST_PATH_IMAGE008
为肩部固定坐标系的原点在世界坐标系下的坐标，
Figure 80023DEST_PATH_IMAGE009
为肩部固定坐标系相对于世界坐标系的旋转矩阵。
在步骤S13的具体实施中，根据所述第二位置、机械臂的上臂长和下臂长，计算肘关节的关节角；
具体地，如图4所示，此步骤包括以下子步骤：
步骤S31：根据所述第二位置，计算肩部与腕部连线的长度；
具体地，根据下式计算肩部与腕部连线的长度
Figure 833215DEST_PATH_IMAGE013
：
Figure 511321DEST_PATH_IMAGE014
其中，
Figure 132796DEST_PATH_IMAGE015
、
Figure 783220DEST_PATH_IMAGE016
、
Figure 300789DEST_PATH_IMAGE017
分别为所述第二位置的x坐标、y坐标与z坐标。
步骤S32：根据所述机械臂的上臂长、下臂长和肩部与腕部连线的长度，计算肘关节的关节角。
在一实施例中，根据下式计算肘关节的关节角
Figure 669453DEST_PATH_IMAGE018
：
Figure 330504DEST_PATH_IMAGE019
其中，
Figure 897752DEST_PATH_IMAGE020
为上臂长，
Figure 727167DEST_PATH_IMAGE021
为下臂长，
Figure 442182DEST_PATH_IMAGE013
为肩部与腕部连线的长度。
在另一实施例中，根据下式计算肘关节的关节角
Figure 280825DEST_PATH_IMAGE018
：
Figure 764896DEST_PATH_IMAGE022
其中，
Figure 827530DEST_PATH_IMAGE020
为上臂长，
Figure 905208DEST_PATH_IMAGE021
为下臂长，
Figure 406596DEST_PATH_IMAGE013
为肩部与腕部连线的长度。
在步骤S14的具体实施中，肩部的三个关节中，选择一个关节的关节角作为优化变量；
具体地，取第三个关节角
Figure 886119DEST_PATH_IMAGE023
作为优化变量，即在
Figure 949015DEST_PATH_IMAGE023
的旋转范围内，进行遍历，每次遍历时，可认为该关节角已知，在具体实施过程中，可对该关节的取值在其范围内进行循环离散遍历，如每隔0.01弧度循环一次，即每个循环内，该关节角的值取一个范围内的离散值，从而使每次循环内的计算，从7自由度退化为6个自由度，解决了冗余度的问题。
在步骤S15的具体实施中，根据所述肘关节的关节角、优化变量、第二位置、第一坐标和第二坐标，计算肩部其余两个关节的关节角，其中所述第一坐标为机械臂处于零位状态时腕部在肘关节的局部坐标系下的坐标，所述第二坐标为机械臂处于零位状态时肘部在所述优化变量对应的关节的局部坐标系下的坐标；
具体地，如图5所示，此步骤包括以下子步骤：
步骤S41：根据所述肘关节的关节角和第一坐标，计算第三坐标，所述第三坐标为肘关节旋转后腕部在肘关节的局部坐标系下的坐标；
具体地，根据所述肘关节的关节角和所述肘关节的旋转轴，计算所述肘关节的旋转矩阵
Figure 310726DEST_PATH_IMAGE024
：
旋转轴为X轴时，肘关节的旋转矩阵为：
Figure 491172DEST_PATH_IMAGE025
旋转轴为Y轴时，肘关节的旋转矩阵为：
Figure 684256DEST_PATH_IMAGE026
旋转轴为Z轴时，肘关节的旋转矩阵为：
Figure 291955DEST_PATH_IMAGE027
根据所述机械臂的下臂长，计算机械臂处于零位状态时腕部在肘关节的局部坐标系下的坐标，即第一坐标，所述第一坐标表示为
Figure 468858DEST_PATH_IMAGE028
-1；
根据肘关节的旋转矩阵和第一坐标，依据齐次坐标变换可得第三坐标
Figure 984153DEST_PATH_IMAGE029
：
Figure 438268DEST_PATH_IMAGE030
步骤S42：根据所述优化变量、第二坐标以及所述第三坐标，计算第四坐标，所述第四坐标为使用肩部其余两个关节的关节角描述的腕部在肩固定坐标系下的坐标；
具体地，由肩部三个关节的旋转矩阵
Figure 341502DEST_PATH_IMAGE031
，
Figure 615489DEST_PATH_IMAGE032
，
Figure 763836DEST_PATH_IMAGE033
和第二坐标、第三坐标，结合齐次坐标变换可得：
Figure 869195DEST_PATH_IMAGE034
步骤S43：根据所述第四坐标和第二位置，计算肩部其余两个关节的关节角；
具体地，本实施用例中肩部第一个关节的旋转轴为Y轴，第二个关节的旋转轴为X轴，第三个关节的旋转轴为Z轴，肘部关节的旋转轴为X轴，因此：
Figure 818696DEST_PATH_IMAGE035
Figure 704613DEST_PATH_IMAGE036
Figure 30552DEST_PATH_IMAGE037
Figure 52735DEST_PATH_IMAGE038
因此，前述公式可转换为：
Figure 173137DEST_PATH_IMAGE039
其中，
Figure 484033DEST_PATH_IMAGE040
分别表示
Figure 472718DEST_PATH_IMAGE041
,
Figure 224773DEST_PATH_IMAGE042
,
Figure 396036DEST_PATH_IMAGE043
。
进一步可以得出以下等式：
Figure 131911DEST_PATH_IMAGE044
设
Figure 596390DEST_PATH_IMAGE045
，
Figure 593165DEST_PATH_IMAGE046
，则上述等式可简化为：
Figure 55370DEST_PATH_IMAGE047
进一步地，上述公式可转化为：
Figure 403175DEST_PATH_IMAGE048
根据三角函数公式，上述公式可转化为：
Figure 609029DEST_PATH_IMAGE049
进一步地，上述公式可转化为：
Figure 397993DEST_PATH_IMAGE050
设
Figure 421313DEST_PATH_IMAGE051
，
Figure 866201DEST_PATH_IMAGE052
，
Figure 236264DEST_PATH_IMAGE053
，则，
Figure 348577DEST_PATH_IMAGE054
设
Figure 949322DEST_PATH_IMAGE055
，
Figure 271719DEST_PATH_IMAGE056
，
Figure 819375DEST_PATH_IMAGE057
，则，
Figure 645249DEST_PATH_IMAGE058
在步骤S16的具体实施中，根据所述优化变量、肩部其余两个关节的关节角、肘关节的关节角和第二姿态，计算腕部三个关节的关节角；
具体地，如图6所示，此步骤包括以下子步骤：
步骤S51：根据所述优化变量、肩部其余两个关节的关节角、肘关节的关节角，计算第三姿态，所述第三姿态为使用腕部三个关节的关节角描述的腕部在肩固定坐标系下的姿态；
具体地，根据7个关节角的旋转矩阵，以及齐次坐标变换，可得腕部在肩固定坐标系下的第三姿态为：
Figure 620158DEST_PATH_IMAGE059
步骤S52：根据所述第三姿态和第二姿态，计算腕部三个关节的关节角。
具体地，用7个关节角的旋转矩阵描述的第三姿态，其实际值即是已知的第二姿态，即：
Figure 429851DEST_PATH_IMAGE060
进一步地，
Figure 577936DEST_PATH_IMAGE061
其中，本实施用例中，机械臂的第5、6、7个关节的关节轴分别为Z轴、Y轴和X轴，因此其旋转矩阵分别为：
Figure 399261DEST_PATH_IMAGE062
Figure 436750DEST_PATH_IMAGE063
Figure 77946DEST_PATH_IMAGE064
最终可得如下等式：
Figure 92039DEST_PATH_IMAGE065
由上述公式可知，当
Figure 33450DEST_PATH_IMAGE066
时：
Figure 146900DEST_PATH_IMAGE067
Figure 665606DEST_PATH_IMAGE068
Figure 93176DEST_PATH_IMAGE069
当
Figure 13727DEST_PATH_IMAGE070
，
Figure 766920DEST_PATH_IMAGE071
时：
Figure 445026DEST_PATH_IMAGE072
设
Figure 302386DEST_PATH_IMAGE073
,
Figure 952810DEST_PATH_IMAGE074
Figure 735958DEST_PATH_IMAGE075
Figure 104622DEST_PATH_IMAGE076
Figure 264208DEST_PATH_IMAGE077
Figure 565877DEST_PATH_IMAGE078
Figure 660872DEST_PATH_IMAGE079
Figure 641466DEST_PATH_IMAGE080
Figure 948951DEST_PATH_IMAGE081
当
Figure 200066DEST_PATH_IMAGE070
，
Figure 465962DEST_PATH_IMAGE071
时：
Figure 340377DEST_PATH_IMAGE082
解法同上。
具体地，如图7所示，该方法还可以包括以下步骤：
步骤S17：根据所述优化变量，计算肘关节和优化变量对应的关节之外的关节角，重复此步骤直至遍历完所述优化变量对应的关节的运动范围；
具体地，针对选取的优化变量，在其范围内进行离散遍历，每遍历一次，进行一次如上的计算。
步骤S18：在遍历过程中得到的关节角的多组逆解中，选择最优解。
具体地，求解均为求解三角函数的解，并且求解过程时在优化变量
Figure 107345DEST_PATH_IMAGE023
的范围内进行遍历，因此存在多组逆解，需要在多组解中选择一组最优解。本专利中选择距离机械臂当前构型最近的构型作为最优解，即选择使得如下参数最小的一组解作为最优解：
Figure 321288DEST_PATH_IMAGE083
其中，
Figure 882720DEST_PATH_IMAGE084
为7个关节角的当前角度，当机械臂处于零位时，当前角度均为零，否则可根据实际应用，保存上一次移动的角度作为当前角度，或直接读取电机当前状态并转换为当前角度。
与前述的7自由度SRS型机械臂逆解的解析方法的实施例相对应，本申请还提供了7自由度SRS型机械臂逆解的解析装置的实施例。
图8是根据一示例性实施例示出的一种7自由度SRS型机械臂逆解的解析装置框图。参照图8，该装置包括：
建立模块21，用于建立机械臂的世界坐标系、肩部固定坐标系以及各关节的局部坐标系；
转换模块22，用于将机械臂腕部在世界坐标系下的第一位置和第一姿态转换为在肩部固定坐标系下的第二位置和第二姿态；
第一计算模块23，用于根据所述第二位置、机械臂的上臂长和下臂长，计算肘关节的关节角；
选择模块24，用于在肩部的三个关节中，选择一个关节的关节角作为优化变量；
第二计算模块25，用于根据所述肘关节的关节角、优化变量、第二位置、第一坐标和第二坐标，计算肩部其余两个关节的关节角，其中所述第一坐标为机械臂处于零位状态时腕部在肘关节的局部坐标系下的坐标，所述第二坐标为机械臂处于零位状态时肘部在所述优化变量对应的关节的局部坐标系下的坐标；
第三计算模块26，用于根据所述优化变量、肩部其余两个关节的关节角、肘关节的关节角和第二姿态，计算腕部三个关节的关节角。
关于上述实施例中的装置，其中各个模块执行操作的具体方式已经在有关该方法的实施例中进行了详细描述，此处将不做详细阐述说明。
对于装置实施例而言，由于其基本对应于方法实施例，所以相关之处参见方法实施例的部分说明即可。以上所描述的装置实施例仅仅是示意性的，其中所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本申请方案的目的。本领域普通技术人员在不付出创造性劳动的情况下，即可以理解并实施。
相应的，本申请还提供一种电子设备，包括：一个或多个处理器；存储器，用于存储一个或多个程序；当所述一个或多个程序被所述一个或多个处理器执行，使得所述一个或多个处理器实现如上述的7自由度SRS型机械臂逆解的解析方法。
相应的，本申请还提供一种计算机可读存储介质，其上存储有计算机指令，其特征在于，该指令被处理器执行时实现如上述的7自由度SRS型机械臂逆解的解析方法。
本领域技术人员在考虑说明书及实践这里公开的内容后，将容易想到本申请的其它实施方案。本申请旨在涵盖本申请的任何变型、用途或者适应性变化，这些变型、用途或者适应性变化遵循本申请的一般性原理并包括本申请未公开的本技术领域中的公知常识或惯用技术手段。说明书和实施例仅被视为示例性的，本申请的真正范围和精神由下面的权利要求指出。
应当理解的是，本申请并不局限于上面已经描述并在附图中示出的精确结构，并且可以在不脱离其范围进行各种修改和改变。本申请的范围仅由所附的权利要求来限制。

Claims (7)

1.一种7自由度SRS型机械臂逆解的解析方法，其特征在于，包括：
建立机械臂的世界坐标系、肩部固定坐标系以及各关节的局部坐标系；
将机械臂腕部在世界坐标系下的第一位置和第一姿态分别转换为在肩部固定坐标系下的第二位置和第二姿态；
根据所述第二位置、机械臂的上臂长和下臂长，计算肘关节的关节角；
在肩部的三个关节中，选择一个关节的关节角作为优化变量；
根据所述肘关节的关节角、优化变量、第二位置、第一坐标和第二坐标，计算肩部其余两个关节的关节角，其中所述第一坐标为机械臂处于零位状态时腕部在肘关节的局部坐标系下的坐标，所述第二坐标为机械臂处于零位状态时肘部在所述优化变量对应的关节的局部坐标系下的坐标；
根据所述优化变量、肩部其余两个关节的关节角、肘关节的关节角和第二姿态，计算腕部三个关节的关节角；
其中，在肩部的三个关节中，选择一个关节的关节角作为优化变量，即在所述关节角的旋转范围内，进行遍历，每次遍历时，可认为所述关节角已知；
根据所述肘关节的关节角、优化变量、第二位置、第一坐标和第二坐标，计算肩部其余两个关节的关节角，包括：
根据所述肘关节的关节角和第一坐标，计算第三坐标，所述第三坐标为肘关节旋转后腕部在肘关节的局部坐标系下的坐标；
根据所述优化变量、第二坐标以及所述第三坐标，计算第四坐标，所述第四坐标为使用肩部其余两个关节的关节角描述的腕部在肩部固定坐标系下的坐标；
根据所述第四坐标和第二位置，计算肩部其余两个关节的关节角；
其中，根据所述优化变量、第二坐标以及所述第三坐标，计算第四坐标，包括：
由肩部三个关节的旋转矩阵
Figure DEST_PATH_IMAGE002
，
Figure DEST_PATH_IMAGE004
，
Figure DEST_PATH_IMAGE006
和第二坐标、第三坐标
Figure DEST_PATH_IMAGE008
，结合齐次坐标变换可得第四坐标
Figure DEST_PATH_IMAGE010
；
Figure DEST_PATH_IMAGE012
为机械臂的上臂长；
根据所述优化变量、肩部其余两个关节的关节角、肘关节的关节角和第二姿态，计算腕部三个关节的关节角，包括：
根据所述优化变量、肩部其余两个关节的关节角、肘关节的关节角，计算第三姿态，所述第三姿态为使用腕部三个关节的关节角描述的腕部在肩部固定坐标系下的姿态；
根据所述第三姿态和第二姿态，计算腕部三个关节的关节角；
其中，根据所述优化变量、肩部其余两个关节的关节角、肘关节的关节角，计算第三姿态，包括：
根据肩部的三个关节角、肘关节的关节角和腕部的三个关节角的旋转矩阵，以及齐次坐标变换，可得腕部在肩部固定坐标系下的第三姿态为
Figure DEST_PATH_IMAGE014
，
Figure DEST_PATH_IMAGE016
为肘关节的关节角的旋转矩阵，
Figure DEST_PATH_IMAGE018
分别为腕部的三个关节角的旋转矩阵。
2.根据权利要求1所述的方法，其特征在于，还包括：
根据所述优化变量，计算肘关节和优化变量对应的关节之外的关节角，重复此步骤直至遍历完所述优化变量对应的关节的运动范围；
在遍历过程中得到的关节角的多组逆解中，选择最优解。
3.根据权利要求1所述的方法，其特征在于，将机械臂腕部在世界坐标系下的第一位置和第一姿态转换为在肩部固定坐标系下的第二位置和第二姿态，包括：
根据肩部固定坐标系与世界坐标系之间的转换关系，将所述第一姿态转换为第二姿态；
根据所述转换关系，将所述第一位置转换为计算第二位置。
4.根据权利要求1所述的方法，其特征在于，根据所述第二位置、机械臂的上臂长和下臂长，计算肘关节的关节角，包括：
根据所述第二位置，计算肩部与腕部连线的长度；
根据所述机械臂的上臂长、下臂长和肩部与腕部连线的长度，计算肘关节的关节角。
5.一种7自由度SRS型机械臂逆解的解析装置，其特征在于，包括：
建立模块，用于建立机械臂的世界坐标系、肩部固定坐标系以及各关节的局部坐标系；
转换模块，用于将机械臂腕部在世界坐标系下的第一位置和第一姿态分别转换为在肩部固定坐标系下的第二位置和第二姿态；
第一计算模块，用于根据所述第二位置、机械臂的上臂长和下臂长，计算肘关节的关节角；
选择模块，用于在肩部的三个关节中，选择一个关节的关节角作为优化变量；
第二计算模块，用于根据所述肘关节的关节角、优化变量、第二位置、第一坐标和第二坐标，计算肩部其余两个关节的关节角，其中所述第一坐标为机械臂处于零位状态时腕部在肘关节的局部坐标系下的坐标，所述第二坐标为机械臂处于零位状态时肘部在所述优化变量对应的关节的局部坐标系下的坐标；
第三计算模块，用于根据所述优化变量、肩部其余两个关节的关节角、肘关节的关节角和第二姿态，计算腕部三个关节的关节角；
其中，在肩部的三个关节中，选择一个关节的关节角作为优化变量，即在所述关节角的旋转范围内，进行遍历，每次遍历时，可认为所述关节角已知；
所述第二计算模块包括：
第一计算子模块，用于根据所述肘关节的关节角和机械臂处于零位状态时腕部在肘关节的局部坐标系下的第一坐标，计算肘关节旋转后腕部在肘关节的局部坐标系下的第三坐标；
第二计算子模块，用于根据所述优化变量、第二坐标以及所述第三坐标，计算第四坐标，所述第四坐标为使用肩部其余两个关节的关节角描述的腕部在肩部固定坐标系下的坐标；
第三计算子模块，用于根据所述第四坐标和第二位置，计算肩部其余两个关节的关节角；
其中，根据所述优化变量、第二坐标以及所述第三坐标，计算第四坐标，包括：
由肩部三个关节的旋转矩阵
Figure 456524DEST_PATH_IMAGE002
，
Figure 639244DEST_PATH_IMAGE004
，
Figure 684560DEST_PATH_IMAGE006
和第二坐标、第三坐标
Figure 345349DEST_PATH_IMAGE008
，结合齐次坐标变换可得第四坐标
Figure 690879DEST_PATH_IMAGE010
；
Figure 778921DEST_PATH_IMAGE012
为机械臂的上臂长；
根据所述优化变量、肩部其余两个关节的关节角、肘关节的关节角和第二姿态，计算腕部三个关节的关节角，包括：
根据所述优化变量、肩部其余两个关节的关节角、肘关节的关节角，计算第三姿态，所述第三姿态为使用腕部三个关节的关节角描述的腕部在肩部固定坐标系下的姿态；
根据所述第三姿态和第二姿态，计算腕部三个关节的关节角；
其中，根据所述优化变量、肩部其余两个关节的关节角、肘关节的关节角，计算第三姿态，包括：
根据肩部的三个关节角、肘关节的关节角和腕部的三个关节角的旋转矩阵，以及齐次坐标变换，可得腕部在肩部固定坐标系下的第三姿态为
Figure 311534DEST_PATH_IMAGE014
，
Figure 776013DEST_PATH_IMAGE016
为肘关节的关节角的旋转矩阵，
Figure 976050DEST_PATH_IMAGE018
分别为腕部的三个关节角的旋转矩阵。
6.一种电子设备，其特征在于，包括：
一个或多个处理器；
存储器，用于存储一个或多个程序；
当所述一个或多个程序被所述一个或多个处理器执行，使得所述一个或多个处理器实现如权利要求1-4任一项所述的方法。
7.一种计算机可读存储介质，其上存储有计算机指令，其特征在于，该指令被处理器执行时实现如权利要求1-4中任一项所述方法的步骤。
CN202111406791.4A 2021-11-24 2021-11-24 7自由度srs型机械臂逆解的解析方法、装置及电子设备 Active CN113814988B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111406791.4A CN113814988B (zh) 2021-11-24 2021-11-24 7自由度srs型机械臂逆解的解析方法、装置及电子设备

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111406791.4A CN113814988B (zh) 2021-11-24 2021-11-24 7自由度srs型机械臂逆解的解析方法、装置及电子设备

Publications (2)

Publication Number Publication Date
CN113814988A CN113814988A (zh) 2021-12-21
CN113814988B true CN113814988B (zh) 2022-02-22

Family

ID=78918198

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111406791.4A Active CN113814988B (zh) 2021-11-24 2021-11-24 7自由度srs型机械臂逆解的解析方法、装置及电子设备

Country Status (1)

Country Link
CN (1) CN113814988B (zh)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114952868B (zh) * 2022-07-26 2022-11-11 之江实验室 7自由度srs型机械臂控制方法及装置、弹琴机器人
CN115922728B (zh) * 2023-01-03 2023-06-30 之江实验室 机器人指向动作控制方法、装置、电子设备和存储介质

Family Cites Families (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US8423190B1 (en) * 2009-06-11 2013-04-16 Kabushiki Kaisha Yaskawa Denki Manipulator system
CN103955619B (zh) * 2014-05-09 2017-12-22 大连大学 一种七自由度空间机械臂最小基座扰动解析逆运动学求解方法
CN104635762B (zh) * 2015-01-13 2017-06-06 北京航空航天大学 一种面向srs仿人手臂的自运动角计算方法
CN109291046A (zh) * 2017-07-25 2019-02-01 中国科学院沈阳自动化研究所 一种七自由度拟人构型机械臂逆运动学规划方法
CN110712203B (zh) * 2019-09-26 2021-03-26 苏州苏相机器人智能装备有限公司 一种7自由度机械臂逆运动学求解算法
CN112650079B (zh) * 2020-12-22 2022-02-15 华中科技大学 一种七自由度机器人的运动学反解方法

Also Published As

Publication number Publication date
CN113814988A (zh) 2021-12-21

Similar Documents

Publication Publication Date Title
CN113814988B (zh) 7自由度srs型机械臂逆解的解析方法、装置及电子设备
CN114952868B (zh) 7自由度srs型机械臂控制方法及装置、弹琴机器人
US5590034A (en) Method for controlling the movement of an industrial robot at and near singularities
JP2020179466A (ja) 軌道計画装置、軌道計画方法及びプログラム
CN110370256B (zh) 机器人及其路径规划方法、装置和控制器
CN112318506A (zh) 机械臂自动标定方法、装置、设备、机械臂和介质
JPH09109072A (ja) 冗長マニピュレータの制御方法
CN113715016B (zh) 一种基于3d视觉的机器人抓取方法、系统、装置及介质
CN116370088B (zh) 机械臂的运动控制方法及相关装置
Cong et al. A new decoupled control law for image-based visual servoing control of robot manipulators
CN114571452A (zh) 工业机器人轨迹规划方法、电子设备及可读存储介质
CN115213905A (zh) 冗余度机械臂位姿控制方法、系统及机器人
CN115179297A (zh) 手术机器人位置姿态控制联合避障关节极限的方法及系统
CN117428791B (zh) 一种用于肩部四轴康复机器人的逆运动学求解方法及系统
CN117207200A (zh) 机械臂的工作空间生成方法、装置和计算机设备
CN111709095A (zh) 一种面向复杂曲面6d虚拟夹具构造方法
CN116141341A (zh) 满足笛卡尔空间约束的五自由度机械臂指向动作实现方法
CN117325143A (zh) 一种冗余机械臂奇异位型下运动学优化方法
CN112643674B (zh) 机器人跟随加工工件表面补偿方法、机器人及存储装置
CN110815230B (zh) 一种基于虚拟采样的六轴码垛机器人位姿适配方法
CN115570562B (zh) 机器人装配位姿确定方法、装置、机器人及存储介质
Dong et al. A novel algorithm combined with single mapping workspace and Genetic Algorithm for solving inverse kinematic problem of redundant manipulators
CN118143948B (zh) 一种七自由度机械臂运动的控制方法、装置及电子设备
CN113103239B (zh) 一种机器人姿态轨迹生成方法、装置及存储介质
CN117348577B (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