一种机器人姿态插补方法、装置及存储介质与流程

1.本技术涉及机器人领域，尤其涉及一种机器人姿态插补方法、装置及存储介质。


背景技术：

2.轨迹规划是机器人控制器的核心功能之一，而直线运动、圆弧运动、关节运动是轨迹规划需重点研究的内容。对于机器人关节运动，起点关节角度和终点关节角度已知，则其插补过程唯一确定。而对于直线运动、圆弧运动，虽起点位姿和结束点位姿均已知，可以将起点姿态到终点姿态的变换过程等价为绕着空间的一个轴线旋转一个转角，即姿态的等效角/轴表达方式，但是由于等效角度通常有两组解，通过起始点的位姿和结束点的位姿态通常是无法判断姿态插补应该取哪组解，这意味着其插补过程并不唯一确定。
3.现有的机器人姿态插补方法是按照最短姿态路径插补来选择的，即等效旋转角度应小于等于180
°
。对于大多直线运动和圆弧运动，由于等效旋转角是小于等于180
°
，该方法是可行的。但是对于等效旋转角大于180度的情况，以机器人激光切割圆管为例，若用户编写了两段圆弧运动，则必有一段圆弧的等效旋转角度大于180度，但是显然现有的姿态插补方法无法满足上述要求。


技术实现要素：

4.为了解决上述技术问题或者至少部分地解决上述技术问题，本技术提供了一种机器人姿态插补方法、装置及存储介质。
5.第一方面，本技术提供了一种机器人姿态插补方法，所述方法包括步骤：
6.获取机器人的起点关节角度和终点关节角度；
7.根据所述起点关节角度和所述终点关节角度计算所述机器人从起点到终点的关节角度差值；
8.根据所述关节角度差值计算所述机器人的起点姿态到终点姿态变化的等效四元素表达式；
9.根据所述等效四元素表达式计算所述机器人的起点姿态到终点姿态变化的等效旋转轴线和等效旋转角度；
10.根据所述起点关节角度、所述终点关节角度、所述等效旋转轴线和所述等效旋转角度计算所述机器人的插补姿态。
11.优选地，所述根据所述起点关节角度和所述终点关节角度计算所述机器人从起点到终点的关节角度差值包括步骤：
12.对所述机器人进行dh参数建模并得到所述机器人的运动学模型；
13.根据所述运动学模型确定所述机器人的关节dh参数；
14.计算所述机器人的起点的各关节轴线方向；
15.计算终点关节角度与起点关节角度的关节角度差值。
16.优选地，所述计算所述机器人的起点的各关节轴线方向包括步骤：
17.定义所述机器人的基坐标系、关节坐标系和机器人末端法兰坐标系；
18.根据坐标变换方法计算所述基坐标系到所述关节坐标系的齐次变换矩阵；
19.根据所述齐次变换矩阵计算关节的旋转轴线在所述基坐标系下的关节轴线方向。
20.优选地，所述关节轴线方向的表达式为：
[0021][0022]
其中，表示i个关节轴线正方向在基座表系{0}上的投影，表示i个关节轴线正方向在基座表系{0}的x轴的投影，表示i个关节轴线正方向在基座表系{0}的y轴的投影，表示第i个关节轴线正方向在基座表系{0}的x轴的投影，表示从基座表系{0}到第i个关节坐标{i-1}的变换矩阵，表示的第1行第3列的元素，表示的第2行第3列的元素，表示的第3行第3列的元素。
[0023]
优选地，所述关节角度差值的表达式为：
[0024][0025]
其中，δq表示机器人直线运动或者圆弧运动起点与终点关节坐标的差值，q
(2)
表示机器人直线运动或者圆弧运动的终点关节坐标，q
(1)
表示机器人直线运动或者圆弧运动的起点关节坐标，表示终点第n个关节的坐标，表示起点第n个关节的坐标，δq1表示机器人直线运动或者圆弧运动起点与终点第1个关节坐标的差值，δq2表示机器人直线运动或者圆弧运动起点与终点第2个关节坐标的差值，δq3表示机器人直线运动或者圆弧运动起点与终点第3个关节坐标的差值。
[0026]
优选地，所述等效四元素表达式的表达式为：
[0027][0028]
其中，表示从基座表系到第n个关节坐标姿态变化的四元素表达方式，表示从基座表系到第1个关节坐标姿态变化的四元素表达方式，表示从第1个关节坐标到第2个关节坐标姿态变化的四元素表达方式，表示从第n-1个关节坐标到第n个关节坐标姿态变化的四元素表达方式。
[0029]
优选地，所述等效旋转轴线的表达式为：
[0030][0031]
其中，k表示机器人直线运动或者圆弧运动从起点到终点的姿态变化等效旋转轴
线，k
x
表示等效旋转轴线在x方向的投影，ky表示等效旋转轴线在y方向的投影，kz表示等效旋转轴线在z方向的投影，表示的第2、3、4个元素。
[0032]
优选地，所述等效旋转角度的表达式为：
[0033][0034]
其中，θ表示机器人直线运动或者圆弧运动从起点到终点的姿态变化等效旋转角度，acos表示反余弦，表示的第1个元素。
[0035]
第二方面，本技术提供了一种机器人姿态插补装置，包括：
[0036]
角度获取模块，用于获取机器人的起点关节角度和终点关节角度；
[0037]
角度差值计算模块，用于根据所述起点关节角度和所述终点关节角度计算所述机器人从起点到终点的关节角度差值；
[0038]
等效四元素表达式计算模块，用于根据所述关节角度差值计算所述机器人的起点姿态到终点姿态变化的等效四元素表达式；
[0039]
等效旋转计算模块，用于根据所述等效四元素表达式计算所述机器人的起点姿态到终点姿态变化的等效旋转轴线和等效旋转角度；
[0040]
插补姿态计算模块，用于根据所述起点关节角度、所述终点关节角度、所述等效旋转轴线和所述等效旋转角度计算所述机器人的插补姿态。
[0041]
第三方面，提供了一种非暂态计算机可读存储介质，该非暂态计算机可读存储介质存储计算机指令，该计算机指令用于使该计算机执行前述任一权利要求1-8所述机器人姿态插补方法。
[0042]
本技术实施例提供的上述技术方案与现有技术相比具有如下优点：
[0043]
本技术提供的一种机器人姿态插补方法、装置及存储介质可以解决笛卡尔空间的直线运动、圆弧运动姿态插补问题，不论笛卡尔姿态等效旋转角度小于180度还是大于180度(小于360度)均适用。
附图说明
[0044]
此处的附图被并入说明书中并构成本说明书的一部分，示出了符合本发明的实施例，并与说明书一起用于解释本发明的原理。
[0045]
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，对于本领域普通技术人员而言，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
[0046]
图1为本技术实施例提供的一种机器人姿态插补方法的流程示意图；
[0047]
图2是本发明实施例提供的一种机器人姿态插补装置的结构示意图；
[0048]
图3是本发明提供的一种非暂态计算机可读存储介质的结构示意图
[0049]
图4为本技术实施例提供的一种机器人姿态插补方法的插补状态示意图。
具体实施方式
[0050]
为使本技术实施例的目的、技术方案和优点更加清楚，下面将结合本技术实施例中的附图，对本技术实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是
本技术的一部分实施例，而不是全部的实施例。基于本技术中的实施例，本领域普通技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本技术保护的范围。
[0051]
图1为本技术实施例提供的一种机器人姿态插补方法的流程示意图。
[0052]
本技术提供了一种机器人姿态插补方法，所述方法包括步骤：
[0053]
s1：获取机器人的起点关节角度和终点关节角度；
[0054]
具体地，以n个自由度的串联机器人为例，机器人从起点p
(1)
直线运动或者圆弧运动到终点p
(2)
，通过对示教程序指令解析可得到机器人起点p
(1)
点的关节坐标q
(1)
和基坐标系到tcp的齐次变换矩阵t
(1)
,以及机器人终点p
(2)
的关节坐标q
(2)
和齐次变换矩阵t
(2)
，其中，q
(1)
和q
(2)
为n维矢量，t
(1)
和t
(2)
为4
×
4的矩阵，表达式分别如下：
[0055][0056][0057][0058][0059]
s2：根据所述起点关节角度和所述终点关节角度计算所述机器人从起点到终点的关节角度差值；
[0060]
在本技术实施例中，所述根据所述起点关节角度和所述终点关节角度计算所述机器人从起点到终点的关节角度差值包括步骤：
[0061]
对所述机器人进行dh参数建模并得到所述机器人的运动学模型；
[0062]
根据所述运动学模型确定所述机器人的关节dh参数；
[0063]
计算所述机器人的起点的各关节轴线方向；
[0064]
计算终点关节角度与起点关节角度的关节角度差值。
[0065]
具体地，采用标准d-h法建立机器人的运动学模型，可以得到机器人每个关节d-h参数：di、ai、αi和qi，其中i＝(1,2...n)。进一步地，首先在第i个关节轴线处建立关节坐标系{i-1}，其中关节坐标系{i-1}的x轴、轴y、z轴的构建方法如下：z
i-1
表示第i个关节轴线，正方向满足右手螺旋定则；x
i-1
表示第i-1个关节轴线和i个关节轴线的公垂线，正方向从第
i-1个关节轴线指向i个关节轴线；y
i-1
表示z
i-1
叉乘x
i-1
。接着确定第i个关节的dh参数，其中，第i个关节的dh参数含义具体为：di：连杆偏置，表示将x
i-1
沿着z
i-1
轴平移至xi的距离；ai：连杆长度，表示将z
i-1
沿着xi轴平移至zi的距离；αi:连杆扭转角，表示将z
i-1
沿着xi轴旋转至zi的角度；qi：表示第i个关节的关节角度。
[0066]
在本技术实施例中，所述计算所述机器人的起点的各关节轴线方向包括步骤：
[0067]
定义所述机器人的基坐标系、关节坐标系和机器人末端法兰坐标系；
[0068]
根据坐标变换方法计算所述基坐标系到所述关节坐标系的齐次变换矩阵；
[0069]
根据所述齐次变换矩阵计算关节的旋转轴线在所述基坐标系下的关节轴线方向。
[0070]
具体地，计算起点的各关节轴线方向的步骤具体为：首先研究机器人在起点位置的p
(1)
的关节轴向的方向，根据上述公式(1)已知起点的关节角度为q
(1)
；接着设机器人基坐标系为{0}，第i个关节坐标系为{i-1}，机器人末端法兰坐标系为坐标系为{n}；根据坐标变换方法可得到基坐标系{0}到坐标系{i-1}的齐次变换矩阵，表达式为：
[0071][0072]
其中，表示从第i-1个关节的坐标系{i-2}到第i个关节的坐标系{i-1}的齐次变换矩阵，i＝2,3...n，的计算公式如下，
[0073][0074]
当i＝1时，为4
×
4的单位矩阵，如下：
[0075][0076]
则在起点时第i个关节的旋转轴线在基坐标系下{0}的方向为：
[0077][0078]
其中，表示i个关节轴线正方向在基座表系{0}上的投影，表示i个关节轴线正方向在基座表系{0}的x轴的投影，表示i个关节轴线正方向在基座表系{0}的y轴的投影，表示第i个关节轴线正方向在基座表系{0}的x轴的投影，表示从基座表系{0}到第i个关节坐标{i-1}的变换矩阵，表示的第1行第3列的元素，表示的第2行第3列的元素，表示的第3行第3列的元素。
[0079]
在本技术实施例中，所述关节角度差值的表达式为：
[0080][0081]
其中，δq表示机器人直线运动或者圆弧运动起点与终点关节坐标的差值，q
(2)
表示机器人直线运动或者圆弧运动的终点关节坐标，q
(1)
表示机器人直线运动或者圆弧运动的起点关节坐标，表示终点第n个关节的坐标，表示起点第n个关节的坐标，δq1表示机器人直线运动或者圆弧运动起点与终点第1个关节坐标的差值，δq2表示机器人直线运动或者圆弧运动起点与终点第2个关节坐标的差值，δq3表示机器人直线运动或者圆弧运动起点与终点第3个关节坐标的差值。
[0082]
s3：根据所述关节角度差值计算所述机器人的起点姿态到终点姿态变化的等效四元素表达式；
[0083]
具体地，机器人起点到终点的姿态变化时由于起点到终点的n个角度差值δq导致，即n次旋转共同作用，可以得到所述等效四元素表达式的表达式为：
[0084][0085]
其中，表示从基座表系到第n个关节坐标姿态变化的四元素表达方式，表示从基座表系到第1个关节坐标姿态变化的四元素表达方式，表示从第1个关节坐标到第2个关节坐标姿态变化的四元素表达方式，表示从第n-1个关节坐标到第n个关节坐标姿态变化的四元素表达方式。
[0086]
根据四元数的定义，其表达式如下:
[0087][0088][0089][0090]
公式(8)中涉及四元素的乘法，根据四元素的乘法运算法则，以为例：
[0091][0092][0093][0094]
s4：根据所述等效四元素表达式计算所述机器人的起点姿态到终点姿态变化的等
效旋转轴线和等效旋转角度；
[0095]
具体地，根据上述公式(8)可得到起点姿态到终点姿态的等效四元素表达式为：则等效旋转轴线的表达式为：
[0096][0097]
其中，k表示待求解变量，表示机器人直线运动或者圆弧运动从起点到终点的姿态变化等效旋转轴线，k
x
表示等效旋转轴线在x方向的投影，ky表示等效旋转轴线在y方向的投影，kz表示等效旋转轴线在z方向的投影，表示的第2、3、4个元素。
[0098]
等效旋转角度的表达式为：
[0099][0100]
其中，θ表示待求解变量，表示机器人直线运动或者圆弧运动从起点到终点的姿态变化等效旋转角度，acos表示反余弦，表示的第1个元素。
[0101]
s5：根据所述起点关节角度、所述终点关节角度、所述等效旋转轴线和所述等效旋转角度计算所述机器人的插补姿态。
[0102]
具体地，已知得到机器人在起点p
(1)
时，基坐标系到tcp的齐次变换矩阵t
(1)
，则其旋转矩阵r
(1)
为:
[0103][0104]
对于从起点p
(1)
到终点p
(2)
的任意位置的插补运动运动p，其等效旋转轴线k与公式(11)相同，等效旋转角度计算公式如下：
[0105]
δθ＝tθ(0≤t≤1)
[0106][0107]
其中，p
(1)
p和p
(1)
p
(2)
表示对应的两点间的直线距离或者圆弧距离。
[0108]
可知从起点p
(1)
到插补点p的姿态变化矩阵矩δr取决于k和δθ，根据罗德里格斯公式有:
[0109]
δr＝i sin(δθ)m (1-cos(δθ))mm
[0110]
[0111][0112]
则在插补点p时，基坐标系到tcp的旋转矩阵为：
[0113]
r＝δrr
(1)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(15)。
[0114]
如图4，通过本技术提供的姿态插补方法，对于直线运动或者圆弧，不论起点到终点的姿态小于180度还是大于180度两种情况，该方法均适用，以直线运动姿态逆时针变化270度和顺时针变化90度为例，两种情况下起点的姿态和关节角度均相同，终点姿态相同，但是关节角度不同，相差360度。由于本技术是通过关节角度来确定姿态变化的等效旋转轴和角度，等效角度可在0～360度内，因此可以区分姿态插补是路径1和还是路径2。
[0115]
如图2，本技术提供了一种机器人姿态插补装置，包括：
[0116]
角度获取模块10，用于获取机器人的起点关节角度和终点关节角度；
[0117]
角度差值计算模块20，用于根据所述起点关节角度和所述终点关节角度计算所述机器人从起点到终点的关节角度差值；
[0118]
等效四元素表达式计算模块30，用于根据所述关节角度差值计算所述机器人的起点姿态到终点姿态变化的等效四元素表达式；
[0119]
等效旋转计算模块40，用于根据所述等效四元素表达式计算所述机器人的起点姿态到终点姿态变化的等效旋转轴线和等效旋转角度；
[0120]
插补姿态计算模块50，用于根据所述起点关节角度、所述终点关节角度、所述等效旋转轴线和所述等效旋转角度计算所述机器人的插补姿态。
[0121]
本技术提供的一种机器人姿态插补装置可以执行上述提供的一种机器人姿态插补方法。
[0122]
下面参考图3，其示出了适于用来实现本公开实施例的计算机可读存储介质的结构示意图，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器执行时能够实现如上述中任一所述的机器人姿态插补方法。
[0123]
本技术提供的一种机器人姿态插补方法、装置及存储介质通过机器人起点的关节角度和终点关节角度得到从起点到终点的关节角度差值，同时在起点时利用机器人的正运动学得到在基坐标系下各个关节旋转轴线方向，起点到终点的姿态变化可表达为绕着起点的各个关节轴线方向旋转关节角度差值的，采用四元素方法表示每次旋转，利用四元素的四则运算计算起点姿态到终点姿态的等效旋转轴线和等效旋转角度。
[0124]
本技术提供的一种机器人姿态插补方法、装置及存储介质可以解决笛卡尔空间的直线运动、圆弧运动姿态插补问题，不论笛卡尔姿态等效旋转角度小于180度还是大于180度(小于360度)均适用。
[0125]
需要说明的是，在本文中，诸如“第一”和“第二”等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设
备所固有的要素。在没有更多限制的情况下，由语句“包括一个”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。
[0126]
以上所述仅是本发明的具体实施方式，使本领域技术人员能够理解或实现本发明。对这些实施例的多种修改对本领域的技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所申请的原理和新颖特点相一致的最宽的范围。