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

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

【技术实现步骤摘要】
一种puma构型机器人腕关节奇异规避方法及系统


[0001]本专利技术实施例涉及机器人关节奇异点规避
，具体涉及一种puma构型机器人腕关节奇异规避方法及系统。

技术介绍

[0002]腕关节奇异点在是协作机器人常见的问题之一，它可能出现在笛卡尔空间的任何位置，且只与机器人五号关节趋于零时的特殊的空间位型相关，难以通过限制工作空间的方式进行排除。
[0003]本专利提出了一种基于QP控制器的奇异位姿规避方法，能够让机器人在经过腕关节奇异位姿时，在奇异方向上有二次型代价函数层面定义的最优位置控制精度。其关节位置精度达到同时满足实时性要求。

技术实现思路

[0004]为此，本专利技术实施例提供一种puma构型机器人腕关节奇异规避方法及系统。能够让机器人在经过腕关节奇异位姿时，在奇异方向上有二次型代价函数层面定义的最优位置控制精度，其关节位姿精度达到同时满足实时性要求。
[0005]为了实现上述目的，本申请实施例提供了一种协作机器人关节奇异位姿规避方法，包括：
[0006]获取协作机器人t时刻一关节位姿，并判断协作机器人t时刻一关节位姿是否进入奇异区域；
[0007]如果协作机器人t时刻一关节位姿进入奇异区域内，启动奇异位姿算法模块，并向奇异位姿算法模块中输入协作机器人末端的笛卡尔空间期望位姿和当前所有关节位姿，根据当前所有关节位姿计算协作机器人末端的笛卡尔空间当前位姿；
[0008]根据协作机器人末端的笛卡尔空间期望位姿和笛卡尔空间当前位姿计算笛卡尔空间误差；<br/>[0009]如果协作机器人末端在奇异区域外，通过笛卡尔空间误差与第一预设阈值的比较结果，确定协作机器人末端状态；
[0010]根据协作机器人末端状态采用相对应计算方式计算协作机器人所有关节的运动速度；
[0011]根据所有关节的运动速度计算t+1时刻所有关节位姿；
[0012]控制协作机器人根据所述t+1所有关节位姿进行关节奇异位姿规避。
[0013]作为本申请一优选实施例，所述获取协作机器人t时刻一关节位姿，并判断协作机器人t时刻一关节位姿是否进入奇异区域包括；
[0014]获取协作机器人在t时刻一关节位姿q
t
，如果q
t
∈[0，a1]或q
t
∈[
‑
a1，0]，其中，a1为第二预设阈值，则协作机器人t时刻一关节位姿进入奇异区域，否则，协作机器人t时刻一关节位姿未进入奇异区域。
[0015]作为本申请一优选实施例，所述根据当前所有关节位姿计算协作机器人末端的笛卡尔空间当前位姿通过以下公式计算：
[0016]Tr
t
＝fkin(q
t
)
[0017]其中，Tr
t
为协作机器人末端的笛卡尔空间当前位姿，q
t
为当前所有关节位姿。
[0018]作为本申请一优选实施例，所述根据协作机器人末端的笛卡尔空间期望位姿和笛卡尔空间当前位姿计算笛卡尔空间误差通过以下公式计算笛卡尔空间误差：
[0019]e
Tr
＝Tr
d
‑
Tr
t
[0020]其中，e
Tr
为笛卡尔空间误差，Tr
d
为协作机器人末端的笛卡尔空间期望位姿，Tr
t
为协作机器人末端的笛卡尔空间当前位姿。
[0021]作为本申请一优选实施例，所述如果协作机器人末端在奇异区域外，通过笛卡尔空间误差与第一预设阈值的比较结果，确定协作机器人末端状态包括：
[0022]如果协作机器人末端的笛卡尔空间位姿小于第一设定阈值a2，则协作机器人末端处于第一状态；如果协作机器人末端的笛卡尔空间位姿大于第一设定阈值a2，则协作机器人末端处于第二状态。
[0023]作为本申请一优选实施例，所述方法还包括：如果协作机器人末端在奇异区域内，则协作机器人末端处于第三状态。
[0024]作为本申请一优选实施例，所述协作机器人一关节位姿为puma机器人的五号关节位姿。
[0025]作为本申请一优选实施例，所述方法还包括：所述根据笛卡尔空间误差计算协作机器人末端期望运行线速度和角速度，具体通过以下公式计算：
[0026][0027]P(e
Tr
)＝p
d
‑
p
t
[0028][0029]其中，v
d
为协作机器人末端期望运行线速度和角速度；w
d
为协作机器人末端期望运行线速度，p
d
和R
d
分别为Tr
d
的偏移矢量和旋转矢量，p
t
和R
t
分别为Tr
t
的偏移矢量和旋转矢量，其中log(
·
)是矩阵的log变换，上标v是对角速度矩阵的向量化，e
Tr
为笛卡尔空间误差，Δt为协作机器人控制器的控制周期。
[0030]作为本申请一优选实施例，所述根据协作机器人末端状态采用相对应计算方式计算协作机器人所有关节的运动速度，具体通过以下公式计算：
[0031][0032]其中，qd
t
为协作机器人所有关节运动速度；
[0033]通过以下公式计算：
[0034][0035]其中，5J
v
取自以下矩阵：
[0036][0037]其中，为puma协作机器人五号关节坐标系表示的机器人腕部坐标系的雅克比矩阵，为puma协作机器人五号关节坐标系表示的机器人腕部坐标系的线速度，通过以下公式计算：
[0038][0039][0040][0041]其中，为t时刻协作机器人五号关节坐标系相对于基座标系的变换矩阵的旋转矩阵，p
E，W
为t时刻协作机器人腕关节坐标系相对于协作机器人末端执行器坐标系的齐次变换矩阵的旋转矩阵；
[0042]所述如果协作机器人末端在奇异区域外，通过笛卡尔空间误差与第一预设阈值的比较结果，确定协作机器人末端状态包括：
[0043]如果协作机器人末端的笛卡尔空间位姿小于第一设定阈值，则协作机器人末端处于第一状态；如果协作机器人末端的笛卡尔空间位姿大于设定阈值，则协作机器人末端处于第二状态；
[0044]所述如果协作机器人末端在奇异区域内，则协作机器人末端处于第三状态；
[0045]如果协作机器人末端处于第一状态，通过以下公式计算：
[0046][0047]如果协作机器人末端处于第二状态，通过以下公式计算：
[0048][0049][0050]如果协作机器人末端处于第三状态，通过以下公式计算：
[0051][0052][0053][0054]其中，5J
w2
是一个3x3的矩阵，5J
sin
是一个1x3的矩阵，且是矩阵的第一行，5本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种puma构型机器人腕关节奇异规避方法，其特征在于，包括：获取协作机器人t时刻一关节位姿，并判断协作机器人t时刻一关节位姿是否进入奇异区域；如果协作机器人t时刻一关节位姿进入奇异区域内，启动奇异位姿算法模块，并向奇异位姿算法模块中输入协作机器人末端的笛卡尔空间期望位姿和当前所有关节位姿，根据当前所有关节位姿计算协作机器人末端的笛卡尔空间当前位姿；根据协作机器人末端的笛卡尔空间期望位姿和笛卡尔空间当前位姿计算笛卡尔空间误差；如果协作机器人末端在奇异区域外，通过笛卡尔空间误差与第一预设阈值的比较结果，确定协作机器人末端状态；根据协作机器人末端状态采用相对应计算方式计算协作机器人所有关节的运动速度；根据所有关节的运动速度计算t+1时刻所有关节位姿；控制协作机器人根据所述t+1所有关节位姿进行一关节奇异位姿规避运动。2.如权利要求1所述的一种puma构型机器人腕关节奇异规避方法，其特征在于，所述获取协作机器人t时刻一关节位姿，并判断协作机器人t时刻一关节位姿是否进入奇异区域包括；获取协作机器人在t时刻一关节位姿q
t
，如果q
t
∈[0，a1]或q
t
∈[
‑
a1，0]，其中，a1为第二预设阈值，则协作机器人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构型机器人腕关节奇异规避方法，其特征在于，所述如果协作机器人末端在奇异区域外，通过笛卡尔空间误差与第一预设阈值的比较结果，确定协作机器人末端状态包括：如果协作机器人末端的笛卡尔空间位姿小于第二设定阈值a2，则协作机器人末端处于第一状态；如果协作机器人末端的笛卡尔空间位姿大于第二设定阈值a2，则协作机器人末端处于第二状态。6.如权利要求5所述的一种puma构型机器人腕关节奇异规避方法，其特征在于，所述方法还包括：如果协作机器人末端在奇异区域内，则协作机器人末端处于第三状态。7.如权利要求1至6任一项所述的一种puma构型机器人腕关节奇异规避方法，其特征在
于，所述协作机器人一关节位姿为puma机器人的五号关节位姿。8.如权利要求7所述的一种puma构型机器人腕关节奇异规避方法，其特征在于，所述方法还包括：所述根据笛卡尔空间误差计算协作机器人末端期望运行线速度和角速度，具体通过以下公式计算：P(e
Tr
)＝p
d
‑
p
t
其中，v
d
为协作机器人末端期望运行线速度和角速度；w
d
为协作机器人末端期望运行线速度，p
d
和R
d
...

【专利技术属性】
技术研发人员：黄彦玮，张鹏，张国平，王光能，
申请(专利权)人：深圳市牛耳机器人有限公司，
类型：发明
国别省市：