CN114474068A - 一种puma构型机器人腕关节奇异规避方法及系统 - Google Patents

一种puma构型机器人腕关节奇异规避方法及系统 Download PDF

Info

Publication number
CN114474068A
CN114474068A CN202210243517.8A CN202210243517A CN114474068A CN 114474068 A CN114474068 A CN 114474068A CN 202210243517 A CN202210243517 A CN 202210243517A CN 114474068 A CN114474068 A CN 114474068A
Authority
CN
China
Prior art keywords
cooperative robot
robot
pose
joint
cartesian space
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
CN202210243517.8A
Other languages
English (en)
Other versions
CN114474068B (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.)
Shenzhen Niuer Robot Co ltd
Original Assignee
Shenzhen Niuer Robot 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 Shenzhen Niuer Robot Co ltd filed Critical Shenzhen Niuer Robot Co ltd
Priority to CN202210243517.8A priority Critical patent/CN114474068B/zh
Publication of CN114474068A publication Critical patent/CN114474068A/zh
Application granted granted Critical
Publication of CN114474068B publication Critical patent/CN114474068B/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
    • B25J9/161—Hardware, e.g. neural networks, fuzzy logic, interfaces, processor
    • 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/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning

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

本申请实施例提供的一种puma构型机器人腕关节奇异规避方法及系统，通过首先判断协作机器人t时刻一关节位姿是否进入奇异区域，如果进入奇异区域，则通过奇异算法模块计算协作机器人末端的笛卡尔空间当前位姿，通过协作机器人末端的笛卡尔空间期望位姿和笛卡尔空间当前位姿计算笛卡尔空间误差，通过笛卡尔空间误差与第一预设阈值的比较结果，判断未进入奇异区域协作机器人末端状态，根据机器人末端状态选择对应的计算方式计算协作机器人所有关节的运动速度，根据协作机器人所有关节的运动速度计算t+1时刻所有关节位姿，最后通过协作机器人控制器控制协作机器人根据t+1时刻所有关节位姿进行关节奇异位姿规避，通过本申请实施例可以实现协作机器人笛卡尔空间较小误差的奇异关节位姿规避，并使得机器人运动满足关节速度约束。

Description

一种puma构型机器人腕关节奇异规避方法及系统
技术领域
本发明实施例涉及机器人关节奇异点规避技术领域，具体涉及一种puma构型机器人腕关节奇异规避方法及系统。
背景技术
腕关节奇异点在是协作机器人常见的问题之一，它可能出现在笛卡尔空间的任何位置，且只与机器人五号关节趋于零时的特殊的空间位型相关，难以通过限制工作空间的方式进行排除。
本专利提出了一种基于QP控制器的奇异位姿规避方法，能够让机器人在经过腕关节奇异位姿时，在奇异方向上有二次型代价函数层面定义的最优位置控制精度。其关节位置精度达到同时满足实时性要求。
发明内容
为此，本发明实施例提供一种puma构型机器人腕关节奇异规避方法及系统。能够让机器人在经过腕关节奇异位姿时，在奇异方向上有二次型代价函数层面定义的最优位置控制精度，其关节位姿精度达到同时满足实时性要求。
为了实现上述目的，本申请实施例提供了一种协作机器人关节奇异位姿规避方法，包括：
获取协作机器人t时刻一关节位姿，并判断协作机器人t时刻一关节位姿是否进入奇异区域；
如果协作机器人t时刻一关节位姿进入奇异区域内，启动奇异位姿算法模块，并向奇异位姿算法模块中输入协作机器人末端的笛卡尔空间期望位姿和当前所有关节位姿，根据当前所有关节位姿计算协作机器人末端的笛卡尔空间当前位姿；
根据协作机器人末端的笛卡尔空间期望位姿和笛卡尔空间当前位姿计算笛卡尔空间误差；
如果协作机器人末端在奇异区域外，通过笛卡尔空间误差与第一预设阈值的比较结果，确定协作机器人末端状态；
根据协作机器人末端状态采用相对应计算方式计算协作机器人所有关节的运动速度；
根据所有关节的运动速度计算t+1时刻所有关节位姿；
控制协作机器人根据所述t+1所有关节位姿进行关节奇异位姿规避。
作为本申请一优选实施例，所述获取协作机器人t时刻一关节位姿，并判断协作机器人t时刻一关节位姿是否进入奇异区域包括；
获取协作机器人在t时刻一关节位姿qt，如果qt∈[0，a1]或qt∈[-a1，0]，其中，a1为第二预设阈值，则协作机器人t时刻一关节位姿进入奇异区域，否则，协作机器人t时刻一关节位姿未进入奇异区域。
作为本申请一优选实施例，所述根据当前所有关节位姿计算协作机器人末端的笛卡尔空间当前位姿通过以下公式计算：
Trt＝fkin(qt)
其中，Trt为协作机器人末端的笛卡尔空间当前位姿，qt为当前所有关节位姿。
作为本申请一优选实施例，所述根据协作机器人末端的笛卡尔空间期望位姿和笛卡尔空间当前位姿计算笛卡尔空间误差通过以下公式计算笛卡尔空间误差：
eTr＝Trd-Trt
其中，eTr为笛卡尔空间误差，Trd为协作机器人末端的笛卡尔空间期望位姿，Trt为协作机器人末端的笛卡尔空间当前位姿。
作为本申请一优选实施例，所述如果协作机器人末端在奇异区域外，通过笛卡尔空间误差与第一预设阈值的比较结果，确定协作机器人末端状态包括：
如果协作机器人末端的笛卡尔空间位姿小于第一设定阈值a2，则协作机器人末端处于第一状态；如果协作机器人末端的笛卡尔空间位姿大于第一设定阈值a2，则协作机器人末端处于第二状态。
作为本申请一优选实施例，所述方法还包括：如果协作机器人末端在奇异区域内，则协作机器人末端处于第三状态。
作为本申请一优选实施例，所述协作机器人一关节位姿为puma机器人的五号关节位姿。
作为本申请一优选实施例，所述方法还包括：所述根据笛卡尔空间误差计算协作机器人末端期望运行线速度和角速度，具体通过以下公式计算：
Figure BDA0003543854620000031
P(eTr)＝pd-pt
Figure BDA0003543854620000032
其中，vd为协作机器人末端期望运行线速度和角速度；wd为协作机器人末端期望运行线速度，pd和Rd分别为Trd的偏移矢量和旋转矢量，pt和Rt分别为Trt的偏移矢量和旋转矢量，其中log(·)是矩阵的log变换，上标v是对角速度矩阵的向量化，eTr为笛卡尔空间误差，Δt为协作机器人控制器的控制周期。
作为本申请一优选实施例，所述根据协作机器人末端状态采用相对应计算方式计算协作机器人所有关节的运动速度，具体通过以下公式计算：
Figure BDA0003543854620000033
其中，qdt为协作机器人所有关节运动速度；
Figure BDA0003543854620000034
通过以下公式计算：
Figure BDA0003543854620000035
其中，5Jv取自以下矩阵：
Figure BDA0003543854620000036
其中，
Figure BDA0003543854620000041
为puma协作机器人五号关节坐标系表示的机器人腕部坐标系的雅克比矩阵，
Figure BDA0003543854620000042
为puma协作机器人五号关节坐标系表示的机器人腕部坐标系的线速度，
Figure BDA0003543854620000043
通过以下公式计算：
Figure BDA0003543854620000044
Figure BDA0003543854620000045
Figure BDA0003543854620000046
其中，
Figure BDA0003543854620000047
为t时刻协作机器人五号关节坐标系相对于基座标系的变换矩阵的旋转矩阵，pE，W为t时刻协作机器人腕关节坐标系相对于协作机器人末端执行器坐标系的齐次变换矩阵的旋转矩阵；
所述如果协作机器人末端在奇异区域外，通过笛卡尔空间误差与第一预设阈值的比较结果，确定协作机器人末端状态包括：
如果协作机器人末端的笛卡尔空间位姿小于第一设定阈值，则协作机器人末端处于第一状态；如果协作机器人末端的笛卡尔空间位姿大于设定阈值，则协作机器人末端处于第二状态；
所述如果协作机器人末端在奇异区域内，则协作机器人末端处于第三状态；
如果协作机器人末端处于第一状态，
Figure BDA0003543854620000048
通过以下公式计算：
Figure BDA0003543854620000049
如果协作机器人末端处于第二状态，
Figure BDA00035438546200000410
通过以下公式计算：
Figure BDA00035438546200000411
Figure BDA00035438546200000412
如果协作机器人末端处于第三状态，
Figure BDA00035438546200000413
通过以下公式计算：
Figure BDA0003543854620000051
Figure BDA0003543854620000052
Figure BDA0003543854620000053
其中，5Jw2是一个3x3的矩阵，5Jsin是一个1x3的矩阵，且是矩阵的第一行，5Jnonsin一个2x3的矩阵，且是矩阵的第二和第三行；
Figure BDA0003543854620000054
通过以下公式计算：
Figure BDA0003543854620000055
Figure BDA0003543854620000056
Figure BDA0003543854620000057
Figure BDA0003543854620000058
和
Figure BDA0003543854620000059
分别代表矢量
Figure BDA00035438546200000510
的奇异和非奇异分离，5Jw1是
Figure BDA00035438546200000511
矩阵的左下角的矩阵。
作为本申请一优选实施例，所述根据所有关节的运动速度计算t+1时刻所有关节位姿，通过以下公式计算：
qc＝qt+Δt·qdt
其中，qc为协作机器人控制器下发至所有关节t+1时刻的位姿。
与现有技术相比，本申请实施例提供的一种协作机器人关节奇异位姿规避方法，通过首先判断协作机器人t时刻一关节位姿是否进入奇异区域，如果进入奇异区域，则通过奇异算法模块计算协作机器人末端的笛卡尔空间当前位姿，通过协作机器人末端的笛卡尔空间期望位姿和笛卡尔空间当前位姿计算笛卡尔空间误差，通过笛卡尔空间误差与第一预设阈值的比较结果，判断未进入奇异区域协作机器人末端状态，根据机器人末端状态选择对应的计算方式计算协作机器人所有关节的运动速度，根据协作机器人所有关节的运动速度计算t+1时刻所有关节位姿，最后通过协作机器人控制器控制协作机器人根据t+1时刻所有关节位姿进行一关节奇异位姿规避，通过本申请实施例可以实现协作机器人笛卡尔空间较小误差的奇异关节位姿规避，并使得机器人运动满足关节速度约束。
第二方面，本申请实施例还提供了一种协作机器人关节奇异位姿规避系统，包括：
判断模块，用于获取协作机器人t时刻一关节位姿，并判断协作机器人t时刻一关节位姿是否进入奇异区域；
第一计算模块，用于如果协作机器人t时刻一关节位姿进入奇异区域内，启动奇异位姿算法模块，并向奇异位姿算法模块中输入协作机器人末端的笛卡尔空间期望位姿和当前所有关节位姿，根据当前所有关节位姿计算协作机器人末端的笛卡尔空间当前位姿；
用于根据协作机器人末端的笛卡尔空间期望位姿和笛卡尔空间当前位姿计算笛卡尔空间误差；
比较模块，用于如果协作机器人末端在奇异区域外，通过笛卡尔空间误差与第一预设阈值的比较结果，确定协作机器人末端状态；
第二计算模块，用于根据协作机器人末端状态采用相对应计算方式计算协作机器人所有关节的运动速度；
第三计算模块，用于根据所有关节的运动速度计算t+1时刻所有关节位姿；
控制模块，用于控制协作机器人根据所述t+1所有关节位姿进行一关节奇异位姿规避。
与现有技术相比，第二方面，本申请实施例提供的一种协作机器人关节奇异位姿规避系统的有益效果与第一方面提供的一种协作机器人关节奇异位姿规避方法有益效果相同，在此不再赘述。
附图说明
为了更清楚地说明本发明的实施方式或现有技术中的技术方案，下面将对实施方式或现有技术描述中所需要使用的附图作简单地介绍。显而易见地，下面描述中的附图仅仅是示例性的，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据提供的附图引伸获得其它的实施附图。
本说明书所绘示的结构、比例、大小等，均仅用以配合说明书所揭示的内容，以供熟悉此技术的人士了解与阅读，并非用以限定本发明可实施的限定条件，故不具技术上的实质意义，任何结构的修饰、比例关系的改变或大小的调整，在不影响本发明所能产生的功效及所能达成的目的下，均应仍落在本发明所揭示的技术内容得能涵盖的范围内。
图1为本发明实施例提供的一种协作机器人关节奇异位姿规避方法流程示意图；
图2为本发明实施例提供协作机器人末端运动轨迹示意图；
图3为本发明实施例提供的一种协作机器人关节奇异位姿规避系统流程示意图。
具体实施方式
以下由特定的具体实施例说明本发明的实施方式，熟悉此技术的人士可由本说明书所揭露的内容轻易地了解本发明的其他优点及功效，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
如图1所示，本申请实施例提供了一种协作机器人关节奇异位姿规避方法，包括以下步骤：
步骤S01，获取协作机器人t时刻一关节位姿，并判断协作机器人t时刻一关节位姿是否进入奇异区域；
需要说明的是，协作机器人控制器会实时计算奇异区域，根据此关节位姿判断是否进入奇异区域，在此需要说明的是，关节位姿为关节角度。
在本申请实施例中，此处的关节位姿为puma协作机器人第五关节，本领域技术都熟知puma机器人构型，在此不在赘述。
获取协作机器人在t时刻一关节位姿qt，如果qt∈[0，a1]或qt∈[-a1，0]，其中，a1为第二预设阈值，则协作机器人t时刻一关节位姿进入奇异区域，否则，协作机器人t时刻一关节位姿未进入奇异区域。
步骤S02，如果协作机器人t时刻一关节位姿进入奇异区域内，启动奇异位姿算法模块，并向奇异位姿算法模块中输入协作机器人末端的笛卡尔空间期望位姿和当前所有关节位姿，根据当前所有关节位姿计算协作机器人末端的笛卡尔空间当前位姿；
需要说明的是，当通过步骤S01判断协作机器人一关节进入奇异区域，则启动奇异算法模块且向奇异算法模块中输入协作机器人末端的笛卡尔空间期望位姿Trd和当前所有关节位姿qt，通过正运动学公式利用当前所有关节位姿qt计算协作机器人末端的笛卡尔空间当前位姿Trt，其中，协作机器人末端的笛卡尔空间期望位姿Trd和协作机器人末端的笛卡尔空间当前位姿Trt在进入奇异区域之前是相等的，进入奇异区域之后协作机器人末端的笛卡尔空间期望位姿Trd和协作机器人末端的笛卡尔空间当前位姿Trt不相等，这是由于误差将在奇异区域S内部累积导致。
步骤S03，根据协作机器人末端的笛卡尔空间期望位姿和笛卡尔空间当前位姿计算笛卡尔空间误差；
需要说明的是，通过当前所有关节位姿qt计算机器人末端的笛卡尔空间当前位姿Trt公式如下：
Trt＝fkin(qt)
笛卡尔空间误差为：
eTr＝Trd-Trt
其中，eTr为笛卡尔空间误差，Trd为协作机器人末端的笛卡尔空间期望位姿，Trt为协作机器人末端的笛卡尔空间当前位姿。
步骤S04，如果协作机器人末端在奇异区域外，通过笛卡尔空间误差与第一预设阈值的比较结果，确定协作机器人末端状态；
需要说明的是，根据协作机器人末端是否在奇异区域内，确定协作机器人末端状态，如图2所示，具体分为以下几种情况：
如果协作机器人末端的笛卡尔空间位姿小于第一设定阈值a2，则协作机器人末端处于第一状态；如果协作机器人末端的笛卡尔空间位姿大于第一设定阈值a2，则协作机器人末端处于第二状态。如果协作机器人末端在奇异区域内，则协作机器人末端处于第三状态。
第一状态为常规状态，第二状态为误差释放阶段，第三状态为修正阶段，协作机器人末端依次经历第一状态，第三状态到第二状态。
步骤S05，根据协作机器人末端状态采用相对应计算方式计算协作机器人所有关节的运动速度；
需要说明的是，所述根据笛卡尔空间误差计算协作机器人末端期望运行线速度和角速度，具体通过以下公式计算：
Figure BDA0003543854620000091
P(eTr)＝pd-pt
Figure BDA0003543854620000092
其中，vd为协作机器人末端期望运行线速度和角速度；wd为协作机器人末端期望运行线速度，pd和Rd分别为Trd的偏移矢量和旋转矢量，pt和Rt分别为Trt的偏移矢量和旋转矢量，其中log(·)是矩阵的log变换，上标v是对角速度矩阵的向量化，eTr为笛卡尔空间误差，Δt为协作机器人控制器的控制周期。
所述根据协作机器人末端状态采用相对应计算方式计算协作机器人所有关节的运动速度，具体通过以下公式计算：
Figure BDA0003543854620000093
其中，qdt为协作机器人所有关节运动速度；
Figure BDA0003543854620000094
通过以下公式计算：
Figure BDA0003543854620000095
其中，5Jv取自以下矩阵：
Figure BDA0003543854620000096
其中，
Figure BDA0003543854620000101
为puma协作机器人五号关节坐标系表示的机器人腕部坐标系的雅克比矩阵，
Figure BDA0003543854620000102
为puma协作机器人五号关节坐标系表示的机器人腕部坐标系的线速度，
Figure BDA0003543854620000103
通过以下公式计算：
Figure BDA0003543854620000104
Figure BDA0003543854620000105
Figure BDA0003543854620000106
其中，
Figure BDA0003543854620000107
为t时刻协作机器人五号关节坐标系相对于基座标系的变换矩阵的旋转矩阵，pE，W为t时刻协作机器人腕关节坐标系相对于协作机器人末端执行器坐标系的齐次变换矩阵的旋转矩阵；
所述如果协作机器人末端在奇异区域外，通过笛卡尔空间误差与第一预设阈值的比较结果，确定协作机器人末端状态包括：
如果协作机器人末端的笛卡尔空间位姿小于第一设定阈值a2，则协作机器人末端处于第一状态；如果协作机器人末端的笛卡尔空间位姿大于第一设定阈值a2，则协作机器人末端处于第二状态。
所述如果协作机器人末端在奇异区域内，则协作机器人末端处于第三状态；
如果协作机器人末端处于第二状态，
Figure BDA0003543854620000108
通过以下公式计算：
Figure BDA0003543854620000109
Figure BDA00035438546200001010
如果协作机器人末端处于第三状态，
Figure BDA00035438546200001011
通过以下公式计算：
Figure BDA00035438546200001012
Figure BDA00035438546200001013
Figure BDA00035438546200001014
其中，5Jw2是一个3x3的矩阵，5Jsin是一个1x3的矩阵，且是矩阵的第一行，5Jnonsin一个2x3的矩阵，且是矩阵的第二和第三行；
Figure BDA0003543854620000111
通过以下公式计算：
Figure BDA0003543854620000112
Figure BDA0003543854620000113
Figure BDA0003543854620000114
Figure BDA0003543854620000115
和
Figure BDA0003543854620000116
分别代表矢量
Figure BDA0003543854620000117
的奇异和非奇异分离，5Jw1是
Figure BDA0003543854620000118
矩阵的左下角的矩阵。
步骤S06，根据所有关节的运动速度计算t+1时刻所有关节位姿；
需要说明的是，所述根据所有关节的运动速度计算t+1时刻所有关节位姿，通过以下公式计算：
qc＝qt+Δt·qdt
其中，qc为协作机器人控制器下发至所有关节t+1时刻的位姿。
步骤S07，控制协作机器人根据所述t+1所有关节位姿进行一关节奇异位姿规避。
需要说明的是，通过步骤S06计算获取在t+1时刻所有关节位姿qc，然后将t+1时刻所有关节位姿qc至协作机器人控制器，通过协作机器人控制控制所述关节根据t+1时刻所有关节位姿qc进行奇异点规避运动。
第二方面，如图3所示，本申请实施例还提供了一种puma构型机器人腕关节奇异规避方法及系统，包括：
判断模块21，用于获取协作机器人t时刻一关节位姿，并判断协作机器人t时刻一关节位姿是否进入奇异区域；
第一计算模块22，用于如果协作机器人t时刻一关节位姿进入奇异区域内，启动奇异位姿算法模块，并向奇异位姿算法模块中输入协作机器人末端的笛卡尔空间期望位姿和当前所有关节位姿，根据当前所有关节位姿计算协作机器人末端的笛卡尔空间当前位姿；
用于根据协作机器人末端的笛卡尔空间期望位姿和笛卡尔空间当前位姿计算笛卡尔空间误差；
比较模块23，用于如果协作机器人末端在奇异区域外，通过笛卡尔空间误差与第一预设阈值的比较结果，确定协作机器人末端状态；
第二计算模块24，用于根据协作机器人末端状态采用相对应计算方式计算协作机器人所有关节的运动速度；
第三计算模块25，用于根据所有关节的运动速度计算t+1时刻所有关节位姿；
控制模块26，用于控制协作机器人根据所述t+1所有关节位姿进行一关节奇异位姿规避运动。
与现有技术相比，第二方面，本申请实施例提供的一种协作机器人关节奇异位姿规避系统的有益效果与第一方面提供的一种协作机器人关节奇异位姿规避方法有益效果相同，同时，第二方面提供的系统具体执行的方法步骤与第一方面提供的技术方案相同，在此不再赘述。
虽然，上文中已经用一般性说明及具体实施例对本发明作了详尽的描述，但在本发明基础上，可以对之作一些修改或改进，这对本领域技术人员而言是显而易见的。因此，在不偏离本发明精神的基础上所做的这些修改或改进，均属于本发明要求保护的范围。

Claims (11)

1.一种puma构型机器人腕关节奇异规避方法，其特征在于，包括：
获取协作机器人t时刻一关节位姿，并判断协作机器人t时刻一关节位姿是否进入奇异区域；
如果协作机器人t时刻一关节位姿进入奇异区域内，启动奇异位姿算法模块，并向奇异位姿算法模块中输入协作机器人末端的笛卡尔空间期望位姿和当前所有关节位姿，根据当前所有关节位姿计算协作机器人末端的笛卡尔空间当前位姿；
根据协作机器人末端的笛卡尔空间期望位姿和笛卡尔空间当前位姿计算笛卡尔空间误差；
如果协作机器人末端在奇异区域外，通过笛卡尔空间误差与第一预设阈值的比较结果，确定协作机器人末端状态；
根据协作机器人末端状态采用相对应计算方式计算协作机器人所有关节的运动速度；
根据所有关节的运动速度计算t+1时刻所有关节位姿；
控制协作机器人根据所述t+1所有关节位姿进行一关节奇异位姿规避运动。
2.如权利要求1所述的一种puma构型机器人腕关节奇异规避方法，其特征在于，所述获取协作机器人t时刻一关节位姿，并判断协作机器人t时刻一关节位姿是否进入奇异区域包括；
获取协作机器人在t时刻一关节位姿qt，如果qt∈[0，a1]或qt∈[-a1，0]，其中，a1为第二预设阈值，则协作机器人t时刻一关节位姿进入奇异区域，否则，协作机器人t时刻一关节位姿未进入奇异区域。
3.如权利要求1所述的一种puma构型机器人腕关节奇异规避方法，其特征在于，所述根据当前所有关节位姿计算协作机器人末端的笛卡尔空间当前位姿通过以下公式计算：
Trt＝fkin(qt)
其中，Trt为协作机器人末端的笛卡尔空间当前位姿，qt为当前所有关节位姿。
4.如权利要求3所述的一种puma构型机器人腕关节奇异规避方法，其特征在于，所述根据协作机器人末端的笛卡尔空间期望位姿和笛卡尔空间当前位姿计算笛卡尔空间误差通过以下公式计算笛卡尔空间误差：
eTr＝Trd-Trt
其中，eTr为笛卡尔空间误差，Trd为协作机器人末端的笛卡尔空间期望位姿，Trt为协作机器人末端的笛卡尔空间当前位姿。
5.如权利要求1所述的一种puma构型机器人腕关节奇异规避方法，其特征在于，所述如果协作机器人末端在奇异区域外，通过笛卡尔空间误差与第一预设阈值的比较结果，确定协作机器人末端状态包括：
如果协作机器人末端的笛卡尔空间位姿小于第二设定阈值a2，则协作机器人末端处于第一状态；如果协作机器人末端的笛卡尔空间位姿大于第二设定阈值a2，则协作机器人末端处于第二状态。
6.如权利要求5所述的一种puma构型机器人腕关节奇异规避方法，其特征在于，所述方法还包括：
如果协作机器人末端在奇异区域内，则协作机器人末端处于第三状态。
7.如权利要求1至6任一项所述的一种puma构型机器人腕关节奇异规避方法，其特征在于，所述协作机器人一关节位姿为puma机器人的五号关节位姿。
8.如权利要求7所述的一种puma构型机器人腕关节奇异规避方法，其特征在于，所述方法还包括：所述根据笛卡尔空间误差计算协作机器人末端期望运行线速度和角速度，具体通过以下公式计算：
Figure FDA0003543854610000021
P(eTr)＝pd-pt
Figure FDA0003543854610000022
其中，vd为协作机器人末端期望运行线速度和角速度；wd为协作机器人末端期望运行线速度，pd和Rd分别为Trd的偏移矢量和旋转矢量，pt和Rt分别为Trt的偏移矢量和旋转矢量，其中log(·)是矩阵的log变换，上标v是对角速度矩阵的向量化，eTr为笛卡尔空间误差，Δt为协作机器人控制器的控制周期。
9.如权利要求8所述的一种puma构型机器人腕关节奇异规避方法，其特征在于，所述根据协作机器人末端状态采用相对应计算方式计算协作机器人所有关节的运动速度，具体通过以下公式计算：
Figure FDA0003543854610000031
其中，qdt为协作机器人所有关节运动速度；
Figure FDA0003543854610000032
通过以下公式计算：
Figure FDA0003543854610000033
其中，5Jv取自以下矩阵：
Figure FDA0003543854610000034
其中，
Figure FDA0003543854610000035
为puma协作机器人五号关节坐标系表示的机器人腕部坐标系的雅克比矩阵，
Figure FDA0003543854610000036
为puma协作机器人五号关节坐标系表示的机器人腕部坐标系的线速度，
Figure FDA0003543854610000037
通过以下公式计算：
Figure FDA0003543854610000038
Figure FDA0003543854610000039
Figure FDA00035438546100000310
其中，
Figure FDA00035438546100000311
为t时刻协作机器人五号关节坐标系相对于基座标系的变换矩阵的旋转矩阵，pE，W为t时刻协作机器人腕关节坐标系相对于协作机器人末端执行器坐标系的齐次变换矩阵的旋转矩阵；
所述如果协作机器人末端在奇异区域外，通过笛卡尔空间误差与第一预设阈值的比较结果，确定协作机器人末端状态包括：
如果协作机器人末端的笛卡尔空间位姿小于第一设定阈值，则协作机器人末端处于第一状态；如果协作机器人末端的笛卡尔空间位姿大于第二设定阈值，则协作机器人末端处于第二状态；
所述如果协作机器人末端在奇异区域内，则协作机器人末端处于第三状态；
如果协作机器人末端处于第一状态，
Figure FDA0003543854610000041
通过以下公式计算：
Figure FDA0003543854610000042
如果协作机器人末端处于第二状态，
Figure FDA0003543854610000043
通过以下公式计算：
Figure FDA0003543854610000044
Figure FDA0003543854610000045
如果协作机器人末端处于第三状态，
Figure FDA0003543854610000046
通过以下公式计算：
Figure FDA0003543854610000047
Figure FDA0003543854610000048
Figure FDA0003543854610000049
其中，5Jw2是一个3x3的矩阵，5Jsin是一个1x3的矩阵，且是矩阵的第一行，5Jnonsin一个2x3的矩阵，且是矩阵的第二和第三行；
Figure FDA00035438546100000410
通过以下公式计算：
Figure FDA00035438546100000411
Figure FDA00035438546100000412
Figure FDA0003543854610000051
Figure FDA0003543854610000052
和
Figure FDA0003543854610000053
分别代表矢量
Figure FDA0003543854610000054
的奇异和非奇异分离，5Jw1是
Figure FDA0003543854610000055
矩阵的左下角的矩阵。
10.如权利要求9所述的一种puma构型机器人腕关节奇异规避方法，其特征在于，所述根据所有关节的运动速度计算t+1时刻所有关节位姿，通过以下公式计算：
qc＝qt+Δt·qdt
其中，qc为协作机器人控制器下发至所有关节t+1时刻的位姿。
11.一种puma构型机器人腕关节奇异规避系统，其特征在于，包括：
判断模块，用于获取协作机器人t时刻一关节位姿，并判断协作机器人t时刻一关节位姿是否进入奇异区域；
第一计算模块，用于如果协作机器人t时刻一关节位姿进入奇异区域内，启动奇异位姿算法模块，并向奇异位姿算法模块中输入协作机器人末端的笛卡尔空间期望位姿和当前所有关节位姿，根据当前所有关节位姿计算协作机器人末端的笛卡尔空间当前位姿；
用于根据协作机器人末端的笛卡尔空间期望位姿和笛卡尔空间当前位姿计算笛卡尔空间误差；
比较模块，用于如果协作机器人末端在奇异区域外，通过笛卡尔空间误差与第一预设阈值的比较结果，确定协作机器人末端状态；
第二计算模块，用于根据协作机器人末端状态采用相对应计算方式计算协作机器人所有关节的运动速度；
第三计算模块，用于根据所有关节的运动速度计算t+1时刻所有关节位姿；
控制模块，用于控制协作机器人根据所述t+1所有关节位姿进行一关节奇异位姿规避运动。
CN202210243517.8A 2022-03-11 2022-03-11 一种puma构型机器人腕关节奇异规避方法及系统 Active CN114474068B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210243517.8A CN114474068B (zh) 2022-03-11 2022-03-11 一种puma构型机器人腕关节奇异规避方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210243517.8A CN114474068B (zh) 2022-03-11 2022-03-11 一种puma构型机器人腕关节奇异规避方法及系统

Publications (2)

Publication Number Publication Date
CN114474068A true CN114474068A (zh) 2022-05-13
CN114474068B CN114474068B (zh) 2023-10-31

Family

ID=81485824

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210243517.8A Active CN114474068B (zh) 2022-03-11 2022-03-11 一种puma构型机器人腕关节奇异规避方法及系统

Country Status (1)

Country Link
CN (1) CN114474068B (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116038686A (zh) * 2022-10-14 2023-05-02 深圳市大族机器人有限公司 机器人奇异点规避方法、装置、计算机设备和存储介质

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103909522A (zh) * 2014-03-19 2014-07-09 华南理工大学 一种六自由度工业机器人通过奇异域的方法
CN105082134A (zh) * 2015-08-06 2015-11-25 珞石（北京）科技有限公司 一种基于多判据的六自由度串联机器人奇异性处理方法
CN107214701A (zh) * 2017-06-12 2017-09-29 南京理工大学 一种基于运动基元库的带电作业机械臂自主避障路径规划方法
CN112847373A (zh) * 2021-01-12 2021-05-28 成都卡诺普自动化控制技术有限公司 一种机器人轨迹同步控制方法、计算机可读存储介质
US20210308864A1 (en) * 2020-04-02 2021-10-07 X Development Llc Robot control for avoiding singular configurations

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103909522A (zh) * 2014-03-19 2014-07-09 华南理工大学 一种六自由度工业机器人通过奇异域的方法
CN105082134A (zh) * 2015-08-06 2015-11-25 珞石（北京）科技有限公司 一种基于多判据的六自由度串联机器人奇异性处理方法
CN107214701A (zh) * 2017-06-12 2017-09-29 南京理工大学 一种基于运动基元库的带电作业机械臂自主避障路径规划方法
US20210308864A1 (en) * 2020-04-02 2021-10-07 X Development Llc Robot control for avoiding singular configurations
CN112847373A (zh) * 2021-01-12 2021-05-28 成都卡诺普自动化控制技术有限公司 一种机器人轨迹同步控制方法、计算机可读存储介质

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116038686A (zh) * 2022-10-14 2023-05-02 深圳市大族机器人有限公司 机器人奇异点规避方法、装置、计算机设备和存储介质
CN116038686B (zh) * 2022-10-14 2023-12-08 深圳市大族机器人有限公司 机器人奇异点规避方法、装置、计算机设备和存储介质

Also Published As

Publication number Publication date
CN114474068B (zh) 2023-10-31

Similar Documents

Publication Publication Date Title
CN110116407B (zh) 柔性机器人位姿测量方法及装置
JP5114019B2 (ja) エフェクタの軌道を制御するための方法
US6845295B2 (en) Method of controlling a robot through a singularity
Baeten et al. Hybrid vision/force control at corners in planar robotic-contour following
JP4265088B2 (ja) ロボット装置及びその制御方法
CN109605369B (zh) 机械臂奇异点控制方法及系统
JP5223407B2 (ja) 冗長ロボットの教示方法
Li et al. A hybrid visual servo control method for simultaneously controlling a nonholonomic mobile and a manipulator
JP2767417B2 (ja) ロボツト制御装置
CN114474068A (zh) 一种puma构型机器人腕关节奇异规避方法及系统
CN110802600A (zh) 一种六自由度关节型机器人的奇异性处理方法
JP2020171989A (ja) ロボット教示システム
CN114378830B (zh) 一种机器人腕关节奇异规避方法及系统
CN113084797B (zh) 一种基于任务分解的双臂冗余机械臂动态协同控制方法
Hafez Visual servo control by optimizing hybrid objective function with visibility and path constraints
CN107520838B (zh) 机械手臂及其控制方法和装置
JP4329257B2 (ja) ロボット装置
Suh et al. A visual servoing algorithm using fuzzy logics and fuzzy-neural networks
Shu et al. Dynamic path tracking of industrial robots with high accuracy by visual servoing
CN112847362A (zh) 一种基于图像矩的搬运机器人视觉伺服控制方法
Tonko et al. Visually servoed gripping of a used car battery
JP2552271B2 (ja) ロボツトの適応制御方法
CN113867260B (zh) 一种采用数值积分的机器人曲面加工关节轨迹生成方法
CN116100562B (zh) 多机器人协同上下料的视觉引导方法及系统
JP7159505B1 (ja) 塗装ロボット

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