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

Abstract

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

Description

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

技术领域

本发明实施例涉及机器人关节奇异点规避技术领域，具体涉及一种puma构型机器人腕关节奇异规避方法及系统。

背景技术

腕关节奇异点在是协作机器人常见的问题之一，它可能出现在笛卡尔空间的任何位置，且只与机器人五号关节趋于零时的特殊的空间位型相关，难以通过限制工作空间的方式进行排除。

本专利提出了一种基于QP控制器的奇异位姿规避方法，能够让机器人在经过腕关节奇异位姿时，在奇异方向上有二次型代价函数层面定义的最优位置控制精度。其关节位置精度达到同时满足实时性要求。

发明内容

为此，本发明实施例提供一种puma构型机器人腕关节奇异规避方法及系统。能够让机器人在经过腕关节奇异位姿时，在奇异方向上有二次型代价函数层面定义的最优位置控制精度，其关节位姿精度达到同时满足实时性要求。

为了实现上述目的，本申请实施例提供了一种协作机器人关节奇异位姿规避方法，包括：

获取协作机器人t时刻一关节位姿，并判断协作机器人t时刻一关节位姿是否进入奇异区域；

如果协作机器人t时刻一关节位姿进入奇异区域内，启动奇异位姿算法模块，并向奇异位姿算法模块中输入协作机器人末端的笛卡尔空间期望位姿和当前所有关节位姿，根据当前所有关节位姿计算协作机器人末端的笛卡尔空间当前位姿；

根据协作机器人末端的笛卡尔空间期望位姿和笛卡尔空间当前位姿计算笛卡尔空间误差；

如果协作机器人末端在奇异区域外，通过笛卡尔空间误差与第一预设阈值的比较结果，确定协作机器人末端状态；

根据协作机器人末端状态采用相对应计算方式计算协作机器人所有关节的运动速度；

根据所有关节的运动速度计算t+1时刻所有关节位姿；

控制协作机器人根据所述t+1所有关节位姿进行关节奇异位姿规避。

作为本申请一优选实施例，所述获取协作机器人t时刻一关节位姿，并判断协作机器人t时刻一关节位姿是否进入奇异区域包括；

获取协作机器人在t时刻一关节位姿q_t，如果q_t∈[0，a₁]或q_t∈[-a₁，0]，其中，a₁为第二预设阈值，则协作机器人t时刻一关节位姿进入奇异区域，否则，协作机器人t时刻一关节位姿未进入奇异区域。

作为本申请一优选实施例，所述根据当前所有关节位姿计算协作机器人末端的笛卡尔空间当前位姿通过以下公式计算：

Tr_t＝fkin(q_t)

其中，Tr_t为协作机器人末端的笛卡尔空间当前位姿，q_t为当前所有关节位姿。

作为本申请一优选实施例，所述根据协作机器人末端的笛卡尔空间期望位姿和笛卡尔空间当前位姿计算笛卡尔空间误差通过以下公式计算笛卡尔空间误差：

e_Tr＝Tr_d-Tr_t

其中，e_Tr为笛卡尔空间误差，Tr_d为协作机器人末端的笛卡尔空间期望位姿，Tr_t为协作机器人末端的笛卡尔空间当前位姿。

作为本申请一优选实施例，所述如果协作机器人末端在奇异区域外，通过笛卡尔空间误差与第一预设阈值的比较结果，确定协作机器人末端状态包括：

如果协作机器人末端的笛卡尔空间位姿小于第一设定阈值a₂，则协作机器人末端处于第一状态；如果协作机器人末端的笛卡尔空间位姿大于第一设定阈值a₂，则协作机器人末端处于第二状态。

作为本申请一优选实施例，所述方法还包括：如果协作机器人末端在奇异区域内，则协作机器人末端处于第三状态。

作为本申请一优选实施例，所述协作机器人一关节位姿为puma机器人的五号关节位姿。

作为本申请一优选实施例，所述方法还包括：所述根据笛卡尔空间误差计算协作机器人末端期望运行线速度和角速度，具体通过以下公式计算：

P(e_Tr)＝p_d-p_t

其中，v_d为协作机器人末端期望运行线速度和角速度；w_d为协作机器人末端期望运行线速度，p_d和R_d分别为Tr_d的偏移矢量和旋转矢量，p_t和R_t分别为Tr_t的偏移矢量和旋转矢量，其中log(·)是矩阵的log变换，上标v是对角速度矩阵的向量化，e_Tr为笛卡尔空间误差，Δt为协作机器人控制器的控制周期。

作为本申请一优选实施例，所述根据协作机器人末端状态采用相对应计算方式计算协作机器人所有关节的运动速度，具体通过以下公式计算：

其中，qd_t为协作机器人所有关节运动速度；

通过以下公式计算：

其中，⁵J_v取自以下矩阵：

其中，为puma协作机器人五号关节坐标系表示的机器人腕部坐标系的雅克比矩阵，/>为puma协作机器人五号关节坐标系表示的机器人腕部坐标系的线速度，通过以下公式计算：

其中，为t时刻协作机器人五号关节坐标系相对于基座标系的变换矩阵的旋转矩阵，p^E，W为t时刻协作机器人腕关节坐标系相对于协作机器人末端执行器坐标系的齐次变换矩阵的旋转矩阵；

所述如果协作机器人末端在奇异区域外，通过笛卡尔空间误差与第一预设阈值的比较结果，确定协作机器人末端状态包括：

如果协作机器人末端的笛卡尔空间位姿小于第一设定阈值，则协作机器人末端处于第一状态；如果协作机器人末端的笛卡尔空间位姿大于设定阈值，则协作机器人末端处于第二状态；

所述如果协作机器人末端在奇异区域内，则协作机器人末端处于第三状态；

如果协作机器人末端处于第一状态，通过以下公式计算：

如果协作机器人末端处于第二状态，通过以下公式计算：

如果协作机器人末端处于第三状态，通过以下公式计算：

其中，⁵J_w2是一个3x3的矩阵，⁵J_sin是一个1x3的矩阵，且是矩阵的第一行，⁵J_nonsin一个2x3的矩阵，且是矩阵的第二和第三行；

通过以下公式计算：

和/>分别代表矢量/>的奇异和非奇异分离，⁵J_w1是/>矩阵的左下角的矩阵。

作为本申请一优选实施例，所述根据所有关节的运动速度计算t+1时刻所有关节位姿，通过以下公式计算：

q_c＝q_t+Δt·qd_t

其中，q_c为协作机器人控制器下发至所有关节t+1时刻的位姿。

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

第二方面，本申请实施例还提供了一种协作机器人关节奇异位姿规避系统，包括：

判断模块，用于获取协作机器人t时刻一关节位姿，并判断协作机器人t时刻一关节位姿是否进入奇异区域；

第一计算模块，用于如果协作机器人t时刻一关节位姿进入奇异区域内，启动奇异位姿算法模块，并向奇异位姿算法模块中输入协作机器人末端的笛卡尔空间期望位姿和当前所有关节位姿，根据当前所有关节位姿计算协作机器人末端的笛卡尔空间当前位姿；

用于根据协作机器人末端的笛卡尔空间期望位姿和笛卡尔空间当前位姿计算笛卡尔空间误差；

比较模块，用于如果协作机器人末端在奇异区域外，通过笛卡尔空间误差与第一预设阈值的比较结果，确定协作机器人末端状态；

第二计算模块，用于根据协作机器人末端状态采用相对应计算方式计算协作机器人所有关节的运动速度；

第三计算模块，用于根据所有关节的运动速度计算t+1时刻所有关节位姿；

控制模块，用于控制协作机器人根据所述t+1所有关节位姿进行一关节奇异位姿规避。

与现有技术相比，第二方面，本申请实施例提供的一种协作机器人关节奇异位姿规避系统的有益效果与第一方面提供的一种协作机器人关节奇异位姿规避方法有益效果相同，在此不再赘述。

附图说明

为了更清楚地说明本发明的实施方式或现有技术中的技术方案，下面将对实施方式或现有技术描述中所需要使用的附图作简单地介绍。显而易见地，下面描述中的附图仅仅是示例性的，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据提供的附图引伸获得其它的实施附图。

本说明书所绘示的结构、比例、大小等，均仅用以配合说明书所揭示的内容，以供熟悉此技术的人士了解与阅读，并非用以限定本发明可实施的限定条件，故不具技术上的实质意义，任何结构的修饰、比例关系的改变或大小的调整，在不影响本发明所能产生的功效及所能达成的目的下，均应仍落在本发明所揭示的技术内容得能涵盖的范围内。

图1为本发明实施例提供的一种协作机器人关节奇异位姿规避方法流程示意图；

图2为本发明实施例提供协作机器人末端运动轨迹示意图；

图3为本发明实施例提供的一种协作机器人关节奇异位姿规避系统流程示意图。

具体实施方式

以下由特定的具体实施例说明本发明的实施方式，熟悉此技术的人士可由本说明书所揭露的内容轻易地了解本发明的其他优点及功效，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

如图1所示，本申请实施例提供了一种协作机器人关节奇异位姿规避方法，包括以下步骤：

步骤S01，获取协作机器人t时刻一关节位姿，并判断协作机器人t时刻一关节位姿是否进入奇异区域；

需要说明的是，协作机器人控制器会实时计算奇异区域，根据此关节位姿判断是否进入奇异区域，在此需要说明的是，关节位姿为关节角度。

在本申请实施例中，此处的关节位姿为puma协作机器人第五关节，本领域技术都熟知puma机器人构型，在此不在赘述。

获取协作机器人在t时刻一关节位姿q_t，如果q_t∈[0，a₁]或q_t∈[-a₁，0]，其中，a1为第二预设阈值，则协作机器人t时刻一关节位姿进入奇异区域，否则，协作机器人t时刻一关节位姿未进入奇异区域。

步骤S02，如果协作机器人t时刻一关节位姿进入奇异区域内，启动奇异位姿算法模块，并向奇异位姿算法模块中输入协作机器人末端的笛卡尔空间期望位姿和当前所有关节位姿，根据当前所有关节位姿计算协作机器人末端的笛卡尔空间当前位姿；

需要说明的是，当通过步骤S01判断协作机器人一关节进入奇异区域，则启动奇异算法模块且向奇异算法模块中输入协作机器人末端的笛卡尔空间期望位姿Tr_d和当前所有关节位姿q_t，通过正运动学公式利用当前所有关节位姿q_t计算协作机器人末端的笛卡尔空间当前位姿Tr_t，其中，协作机器人末端的笛卡尔空间期望位姿Tr_d和协作机器人末端的笛卡尔空间当前位姿Tr_t在进入奇异区域之前是相等的，进入奇异区域之后协作机器人末端的笛卡尔空间期望位姿Tr_d和协作机器人末端的笛卡尔空间当前位姿Tr_t不相等，这是由于误差将在奇异区域S内部累积导致。

步骤S03，根据协作机器人末端的笛卡尔空间期望位姿和笛卡尔空间当前位姿计算笛卡尔空间误差；

需要说明的是，通过当前所有关节位姿q_t计算机器人末端的笛卡尔空间当前位姿Tr_t公式如下：

Tr_t＝fkin(q_t)

笛卡尔空间误差为：

e_Tr＝Tr_d-Tr_t

其中，e_Tr为笛卡尔空间误差，Tr_d为协作机器人末端的笛卡尔空间期望位姿，Tr_t为协作机器人末端的笛卡尔空间当前位姿。

步骤S04，如果协作机器人末端在奇异区域外，通过笛卡尔空间误差与第一预设阈值的比较结果，确定协作机器人末端状态；

需要说明的是，根据协作机器人末端是否在奇异区域内，确定协作机器人末端状态，如图2所示，具体分为以下几种情况：

如果协作机器人末端的笛卡尔空间位姿小于第一设定阈值a₂，则协作机器人末端处于第一状态；如果协作机器人末端的笛卡尔空间位姿大于第一设定阈值a₂，则协作机器人末端处于第二状态。如果协作机器人末端在奇异区域内，则协作机器人末端处于第三状态。

第一状态为常规状态，第二状态为误差释放阶段，第三状态为修正阶段，协作机器人末端依次经历第一状态，第三状态到第二状态。

步骤S05，根据协作机器人末端状态采用相对应计算方式计算协作机器人所有关节的运动速度；

需要说明的是，所述根据笛卡尔空间误差计算协作机器人末端期望运行线速度和角速度，具体通过以下公式计算：

P(e_Tr)＝p_d-p_t

其中，v_d为协作机器人末端期望运行线速度和角速度；w_d为协作机器人末端期望运行线速度，p_d和R_d分别为Tr_d的偏移矢量和旋转矢量，p_t和R_t分别为Tr_t的偏移矢量和旋转矢量，其中log(·)是矩阵的log变换，上标v是对角速度矩阵的向量化，e_Tr为笛卡尔空间误差，Δt为协作机器人控制器的控制周期。

所述根据协作机器人末端状态采用相对应计算方式计算协作机器人所有关节的运动速度，具体通过以下公式计算：

其中，qd_t为协作机器人所有关节运动速度；

通过以下公式计算：

其中，⁵J_v取自以下矩阵：

其中，为puma协作机器人五号关节坐标系表示的机器人腕部坐标系的雅克比矩阵，/>为puma协作机器人五号关节坐标系表示的机器人腕部坐标系的线速度，通过以下公式计算：

其中，为t时刻协作机器人五号关节坐标系相对于基座标系的变换矩阵的旋转矩阵，p^E，W为t时刻协作机器人腕关节坐标系相对于协作机器人末端执行器坐标系的齐次变换矩阵的旋转矩阵；

所述如果协作机器人末端在奇异区域外，通过笛卡尔空间误差与第一预设阈值的比较结果，确定协作机器人末端状态包括：

如果协作机器人末端的笛卡尔空间位姿小于第一设定阈值a₂，则协作机器人末端处于第一状态；如果协作机器人末端的笛卡尔空间位姿大于第一设定阈值a₂，则协作机器人末端处于第二状态。

所述如果协作机器人末端在奇异区域内，则协作机器人末端处于第三状态；

如果协作机器人末端处于第二状态，通过以下公式计算：

如果协作机器人末端处于第三状态，通过以下公式计算：

其中，⁵J_w2是一个3x3的矩阵，⁵J_sin是一个1x3的矩阵，且是矩阵的第一行，⁵J_nonsin一个2x3的矩阵，且是矩阵的第二和第三行；

通过以下公式计算：

和/>分别代表矢量/>的奇异和非奇异分离，⁵J_w1是/>矩阵的左下角的矩阵。

步骤S06，根据所有关节的运动速度计算t+1时刻所有关节位姿；

需要说明的是，所述根据所有关节的运动速度计算t+1时刻所有关节位姿，通过以下公式计算：

q_c＝q_t+Δt·qd_t

其中，q_c为协作机器人控制器下发至所有关节t+1时刻的位姿。

步骤S07，控制协作机器人根据所述t+1所有关节位姿进行一关节奇异位姿规避。

需要说明的是，通过步骤S06计算获取在t+1时刻所有关节位姿q_c，然后将t+1时刻所有关节位姿q_c至协作机器人控制器，通过协作机器人控制控制所述关节根据t+1时刻所有关节位姿q_c进行奇异点规避运动。

第二方面，如图3所示，本申请实施例还提供了一种puma构型机器人腕关节奇异规避方法及系统，包括：

判断模块21，用于获取协作机器人t时刻一关节位姿，并判断协作机器人t时刻一关节位姿是否进入奇异区域；

第一计算模块22，用于如果协作机器人t时刻一关节位姿进入奇异区域内，启动奇异位姿算法模块，并向奇异位姿算法模块中输入协作机器人末端的笛卡尔空间期望位姿和当前所有关节位姿，根据当前所有关节位姿计算协作机器人末端的笛卡尔空间当前位姿；

用于根据协作机器人末端的笛卡尔空间期望位姿和笛卡尔空间当前位姿计算笛卡尔空间误差；

比较模块23，用于如果协作机器人末端在奇异区域外，通过笛卡尔空间误差与第一预设阈值的比较结果，确定协作机器人末端状态；

第二计算模块24，用于根据协作机器人末端状态采用相对应计算方式计算协作机器人所有关节的运动速度；

第三计算模块25，用于根据所有关节的运动速度计算t+1时刻所有关节位姿；

控制模块26，用于控制协作机器人根据所述t+1所有关节位姿进行一关节奇异位姿规避运动。

与现有技术相比，第二方面，本申请实施例提供的一种协作机器人关节奇异位姿规避系统的有益效果与第一方面提供的一种协作机器人关节奇异位姿规避方法有益效果相同，同时，第二方面提供的系统具体执行的方法步骤与第一方面提供的技术方案相同，在此不再赘述。

虽然，上文中已经用一般性说明及具体实施例对本发明作了详尽的描述，但在本发明基础上，可以对之作一些修改或改进，这对本领域技术人员而言是显而易见的。因此，在不偏离本发明精神的基础上所做的这些修改或改进，均属于本发明要求保护的范围。

Claims (10)

1.一种puma构型机器人腕关节奇异规避方法，其特征在于，所述方法包括：

获取协作机器人t时刻一关节位姿，并判断协作机器人t时刻一关节位姿是否进入奇异区域；

如果协作机器人t时刻一关节位姿进入奇异区域内，启动奇异位姿算法模块，并向奇异位姿算法模块中输入协作机器人末端的笛卡尔空间期望位姿和当前所有关节位姿，根据当前所有关节位姿计算协作机器人末端的笛卡尔空间当前位姿；

根据协作机器人末端的笛卡尔空间期望位姿和笛卡尔空间当前位姿计算笛卡尔空间误差；

如果协作机器人末端在奇异区域外，通过笛卡尔空间误差与第一预设阈值的比较结果，确定协作机器人末端状态；

根据协作机器人末端状态采用相对应计算方式计算协作机器人所有关节的运动速度，通过以下公式计算：

其中，qd_t为协作机器人所有关节运动速度；

根据所有关节的运动速度计算t+1时刻所有关节位姿；

控制协作机器人根据所述t+1时刻所有关节位姿进行一关节奇异位姿规避运动，所述协作机器人一关节位姿为puma机器人的五号关节位姿。

2.如权利要求1所述的一种puma构型机器人腕关节奇异规避方法，其特征在于，所述获取协作机器人t时刻一关节位姿，并判断协作机器人t时刻一关节位姿是否进入奇异区域包括；

获取协作机器人在t时刻一关节位姿q_t，如果q_t∈[0,a₁]或q_t∈[-a₁,0]，其中，a₁为第二预设阈值，则协作机器人t时刻一关节位姿进入奇异区域，否则，协作机器人t时刻一关节位姿未进入奇异区域。

3.如权利要求1所述的一种puma构型机器人腕关节奇异规避方法，其特征在于，所述根据当前所有关节位姿计算协作机器人末端的笛卡尔空间当前位姿通过以下公式计算：

Tr_t＝fkin(q_t)

其中，Tr_t为协作机器人末端的笛卡尔空间当前位姿，q_t为当前所有关节位姿。

4.如权利要求3所述的一种puma构型机器人腕关节奇异规避方法，其特征在于，所述根据协作机器人末端的笛卡尔空间期望位姿和笛卡尔空间当前位姿计算笛卡尔空间误差通过以下公式计算笛卡尔空间误差：

e_Tr＝Tr_d-Tr_t

其中，e_Tr为笛卡尔空间误差，Tr_d为协作机器人末端的笛卡尔空间期望位姿，Tr_t为协作机器人末端的笛卡尔空间当前位姿。

5.如权利要求1所述的一种puma构型机器人腕关节奇异规避方法，其特征在于，所述如果协作机器人末端在奇异区域外，通过笛卡尔空间误差与第一预设阈值的比较结果，确定协作机器人末端状态包括：

如果协作机器人末端的笛卡尔空间位姿小于第一设定阈值a₂，则协作机器人末端处于第一状态；如果协作机器人末端的笛卡尔空间位姿大于第一设定阈值a₂，则协作机器人末端处于第二状态。

6.如权利要求5所述的一种puma构型机器人腕关节奇异规避方法，其特征在于，所述方法还包括：

如果协作机器人末端在奇异区域内，则协作机器人末端处于第三状态。

7.如权利要求1所述的一种puma构型机器人腕关节奇异规避方法，其特征在于，所述方法还包括：根据笛卡尔空间误差计算协作机器人末端期望运行线速度和角速度，具体通过以下公式计算：

P(e_Tr)＝p_d-p_t

其中，v_d为协作机器人末端期望运行线速度和角速度；w_d为协作机器人末端期望运行线速度，p_d和R_d分别为Tr_d的偏移矢量和旋转矢量，p_t和R_t分别为Tr_t的偏移矢量和旋转矢量，其中log(·)是矩阵的log变换，上标v是对角速度矩阵的向量化，e_Tr为笛卡尔空间误差，Δt为协作机器人控制器的控制周期。

8.如权利要求7所述的一种puma构型机器人腕关节奇异规避方法，其特征在于，通过以下公式计算：

其中，⁵J_v取自以下矩阵：

其中，为puma协作机器人五号关节坐标系表示的机器人腕部坐标系的雅克比矩阵，/>为puma协作机器人五号关节坐标系表示的机器人腕部坐标系的线速度，通过以下公式计算：

其中，为t时刻协作机器人五号关节坐标系相对于基座标系的变换矩阵的旋转矩阵，p^E,W为t时刻协作机器人腕关节坐标系相对于协作机器人末端执行器坐标系的齐次变换矩阵的旋转矩阵；

所述如果协作机器人末端在奇异区域外，通过笛卡尔空间误差与第一预设阈值的比较结果，确定协作机器人末端状态包括：

如果协作机器人末端的笛卡尔空间位姿小于第一设定阈值，则协作机器人末端处于第一状态；如果协作机器人末端的笛卡尔空间位姿大于第二设定阈值，则协作机器人末端处于第二状态；

所述如果协作机器人末端在奇异区域内，则协作机器人末端处于第三状态；

如果协作机器人末端处于第一状态，通过以下公式计算：

如果协作机器人末端处于第二状态，通过以下公式计算：

如果协作机器人末端处于第三状态，通过以下公式计算：

其中，⁵J_w2是一个3x3的矩阵，⁵J_sin是一个1x3的矩阵，且是矩阵的第一行，⁵J_nonsin一个2x3的矩阵，且是矩阵的第二和第三行；

通过以下公式计算：

和/>分别代表矢量/>的奇异和非奇异分离，⁵J_w1是/>矩阵的左下角的矩阵。

9.如权利要求8所述的一种puma构型机器人腕关节奇异规避方法，其特征在于，所述根据所有关节的运动速度计算t+1时刻所有关节位姿，通过以下公式计算：

q_c＝q_t+Δt·qd_t

其中，q_c为协作机器人控制器下发至所有关节t+1时刻的位姿。

10.一种puma构型机器人腕关节奇异规避系统，其特征在于，所述系统包括：

判断模块，用于获取协作机器人t时刻一关节位姿，并判断协作机器人t时刻一关节位姿是否进入奇异区域；

第一计算模块，用于如果协作机器人t时刻一关节位姿进入奇异区域内，启动奇异位姿算法模块，并向奇异位姿算法模块中输入协作机器人末端的笛卡尔空间期望位姿和当前所有关节位姿，根据当前所有关节位姿计算协作机器人末端的笛卡尔空间当前位姿；

用于根据协作机器人末端的笛卡尔空间期望位姿和笛卡尔空间当前位姿计算笛卡尔空间误差；

比较模块，用于如果协作机器人末端在奇异区域外，通过笛卡尔空间误差与第一预设阈值的比较结果，确定协作机器人末端状态；

第二计算模块，用于根据协作机器人末端状态采用相对应计算方式计算协作机器人所有关节的运动速度，具体通过以下公式计算：

其中，qd_t为协作机器人所有关节运动速度；第三计算模块，用于根据所有关节的运动速度计算t+1时刻所有关节位姿；

控制模块，用于控制协作机器人根据所述t+1时刻所有关节位姿进行一关节奇异位姿规避运动；

所述协作机器人一关节位姿为puma机器人的五号关节位姿。