串联六关节机械手的姿态控制方法及系统与流程

1.本发明涉及机械手姿态控制技术领域，尤其涉及一种串联六关节机械手的姿态控制方法及系统。


背景技术：

2.传统的机械手姿态控制为通过改变机械手正解算法中由矩阵转换的欧拉角数据，并将新的欧拉角数据输入机械手逆解中得到机械手各轴角度，从而进行机械手姿态控制操作。这种姿态控制方式会遇到万向锁问题(gimbal lock)，以及对同一方位有无数多种表达方式的多解问题，这些问题的存在使得使用传统姿态控制算法的六关节机械手牺牲了第4和第5关节的部分自由度，缩小了六关节机械手的可移动范围，且无法满足部分特定的姿态运动轨迹。


技术实现要素：

3.本发明实施例所要解决的技术问题在于，提供一种串联六关节机械手的姿态控制方法及系统，以使六关节机械手能够达到机械设计上的最大运动范围。
4.为了解决上述技术问题，本发明实施例提出了一种串联六关节机械手的姿态控制方法，包括：
5.位姿矩阵计算步骤s1：读取串联六关节机械手的各关节伺服电机角度，根据读取的各关节伺服电机角度，使用连杆变换矩阵进行正解，得到机械手位姿矩阵a
16
；
6.实际点计算步骤s2：根据位姿矩阵a
16
来确定机械手空间坐标x、y、z，将位姿矩阵a
16
中的旋转矩阵转换为四元数q1、q2、q3、q4，得到实际点p1(x,y,z,q1,q2,q3,q4)；
7.设置步骤s3：根据用户手动示教或修改实际点p1得到目标点p2；
8.前三轴角度计算步骤s4：将p2的四元数转换至旋转矩阵并与xyz组合成目标点p2的位姿矩阵t
16
；根据位姿矩阵t
16
求出前三个关节机械手的轴角度θ1、θ2、θ3；
9.后三轴角度计算步骤s5：根据轴角度θ1、θ2、θ3得到前三个关节的逆矩阵表达式再计算得到后三个关节机械手的轴角度θ4、θ5、θ6；
10.发送步骤s6：将得到的各轴角度对应发送至各轴伺服电机。
11.相应地，本发明实施例还提供了一种串联六关节机械手的姿态控制系统，包括：
12.位姿矩阵计算模块：读取串联六关节机械手的各关节伺服电机角度，根据读取的各关节伺服电机角度，使用连杆变换矩阵进行正解，得到机械手位姿矩阵a
16
；
13.实际点计算模块：根据位姿矩阵a
16
来确定机械手空间坐标x、y、z，将位姿矩阵a
16
中的旋转矩阵转换为四元数q1、q2、q3、q4，得到实际点p1(x,y,z,q1,q2,q3,q4)；
14.设置模块：根据用户手动示教或修改实际点p1得到目标点p2；
15.前三轴角度计算模块：将p2的四元数转换至旋转矩阵并与xyz组合成目标点p2的位姿矩阵t
16
；根据位姿矩阵t
16
求出前三个关节机械手的轴角度θ1、θ2、θ3；
16.后三轴角度计算模块：根据轴角度θ1、θ2、θ3得到前三个关节的逆矩阵表达式
再计算得到后三个关节机械手的轴角度θ4、θ5、θ6；
17.发送模块：将得到的各轴角度对应发送至各轴伺服电机。
18.本发明的有益效果为：本发明使用四元数对机械手姿态进行控制，姿态和轨迹控制更灵活，正逆解运算更快，能让六关节机械手能够达到机械设计上的最大运动范围，而不会受到传统方法的限制。
附图说明
19.图1是本发明实施例的串联六关节机械手的姿态控制方法的流程示意图。
20.图2是本发明实施例的串联六关节机械手的姿态控制系统的结构示意图。
21.图3是本发明实施例的串联六关节机械手的姿态控制方法的应用原理示意图。
具体实施方式
22.需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互结合，下面结合附图和具体实施例对本发明作进一步详细说明。
23.本发明实施例中若有方向性指示(诸如上、下、左、右、前、后
……
)仅用于解释在某一特定姿态(如附图所示)下各部件之间的相对位置关系、运动情况等，如果该特定姿态发生改变时，则该方向性指示也相应地随之改变。
24.另外，在本发明中若涉及“第一”、“第二”等的描述仅用于描述目的，而不能理解为指示或暗示其相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括至少一个该特征。
25.请参照图1，本发明实施例的串联六关节机械手的姿态控制方法包括位姿矩阵计算步骤s1、实际点计算步骤s2、设置步骤s3、前三轴角度计算步骤s4、后三轴角度计算步骤s5。
26.位姿矩阵计算步骤s1：根据串联六关节机械手的各关节伺服电机角度，使用连杆变换矩阵进行正解，经解算得出机械手位姿矩阵a
16
(位姿矩阵即若干旋转矩阵和平移矩阵)。
27.实际点计算步骤s2：根据位姿矩阵a
16
来确定机械手空间坐标中的xyz，并从位姿矩阵a
16
中得到旋转矩阵，再将旋转矩阵转换为四元数q1,q2,q3,q4替换传统的uvw来表示机械手姿态，从而得到实际点p1(x,y,z,q1,q2,q3,q4)。本发明实施例计算出完整的当前坐标点p1，用于在交互界面显示，便于用户知道当前机械手末端的空间坐标位置。
28.设置步骤s3：用户根据实际点p1修改或者手动示教取点得到目标点p2，对p2进行逆解求出机械手各轴目标角度。
29.前三轴角度计算步骤s4：求逆解时先将四元数转换至旋转矩阵并与xyz组合成目标点的位姿矩阵t
16
，然后求出第四关节末端xyz坐标进而陆续求出机械手1、2、3轴角度θ1、θ2、θ3。
30.后三轴角度计算步骤s5：求出θ1、θ2、θ3后，可得到前三个关节的逆矩阵表达式进而求出机械手4、5、6轴角度θ4、θ5、θ6。
31.发送步骤s6：将得到的各轴角度对应发送至各轴伺服电机。
32.作为一种实施方式，位姿矩阵计算步骤s1包括：
33.子步骤s11：根据矩阵计算结果得出连杆变换4x4矩阵表达式为：
[0034][0035]
其中alpha、a、d、均为机械手参数，alpha为机械手两个相邻关节轴的夹角，a为机械手两个相邻关节轴公垂线的长度，d为沿两个相邻连杆公共轴线方向的距离，alpha
n
是机械手第n个关节与相邻的后一个关节轴的夹角，a
n
是表示机械手第n个关节与相邻的后一个关节轴公垂线的长度，d
n
是机械手第n个关节沿两个相邻连杆公共轴线方向的距离，以上参数均为已知常量，n代表第几个关节；
[0036]
子步骤s12：将各轴角度代入连杆变换矩阵，经过矩阵乘法计算可得出机械手位姿矩阵a
16
，a
16
表达式为：
[0037]
a
16
＝a1*a2*a3*a4*a5*a6；
[0038]
最终得到的a
16
矩阵表达式为：
[0039][0040]
其中为机械手在当前位置下的旋转矩阵，px、py、pz分别为当前位置下的xyz坐标。
[0041]
作为一种实施方式，实际点计算步骤s2包括：
[0042]
子步骤s21：空间坐标中的xyz分别等于a
16
中的px、py、pz，而旋转矩阵
[0043][0044]
子步骤s 22：由四元数转矩阵得，旋转矩阵r的另一表达式为：
[0045][0046]
其中w＝q1，x＝q2，y＝q3，z＝q4；
[0047]
子步骤s23：由s21和s22的表达式可知：
[0048]
ox+ny＝4xy；
[0049]
ox
‑
ny＝4wz；
[0050]
nz+ax＝4xz；
[0051]
nz
‑
ax＝4wy；
[0052]
ay+oz＝4yz；
[0053]
ay+oz＝4wx；
[0054]
由这组公式可以求出w、x、y、z，即q1，q2，q3，q4从而得到实际点p1(x，y，z，q1，q2，q3，q4)。
[0055]
作为一种实施方式，前三轴角度计算步骤s4包括：
[0056]
子步骤s41：将目标点p2(x，y，z，q1，q2，q3，q4)通过s2.1和s2.2的表达式转换组合成目标点的位姿矩阵t
16
，t
16
表达式与a
16
矩阵表达式相同；
[0057]
根据机械手参数以及t16求得第四关节末端的xyz坐标：
[0058]
x＝px
‑
ax*d6+ox*[cos(θ6)*d5+a5*sin(θ6)]
‑
nx*[a6*cos(θ6)2+a5*cos(θ6)+a6*sin(θ6)2‑
d5*sin(θ6)]；
[0059]
y＝py
‑
ay*d6+oy*[cos(θ6)*d5+a5*sin(θ6)]
‑
ny*[a6*cos(θ6)2+a5*cos(θ6)+a6*sin(θ6)2‑
d5*sin(θ6)]；
[0060]
z＝pz
‑
az*d6+oz*[cos(θ6)*d5+a5*sin(θ6)]
‑
nz*[a6*cos(θ6)2+a5*cos(θ6)+a6*sin(θ6)2‑
d5*sin(θ6)]；
[0061]
其中，常规机械手的a5、a6、d5均等于0，因此常规机械手可以不取实际轴6的角度即可计算出第四关节末端的xyz坐标，而当a5、a6、d5存在非等于0的参数时，就需要取实际位置进行微分法解算；
[0062]
子步骤s42：当计算出第四关节末端的xyz坐标时，即可计算出目标点p2所对应的θ1，其表达式如下：
[0063]
θ1＝atan2(x，y)；
[0064]
子步骤s43：当计算出第四关节末端的xyz坐标时，即可计算出目标点p2所对应的θ3，其表达式如下：
[0065][0066][0067]
fθ3＝asin(
‑
(((z
‑
d1)2+(p
‑
a1)2‑
fd
42
‑
a22)/(2*fd4*a2)))
[0068]
θ3＝90
‑
atan2(d4，a3)+fθ3；
[0069]
当a3为0时θ3＝fθ3；
[0070]
子步骤s44：当计算出θ3时，利用计算θ3过程中的参数可求出θ2，其表达式如下：
[0071]
θ2＝
‑
atan(((fd4*sin(fθ3)
‑
a2)*(p
‑
a1)+(z
‑
d1)*(fd4*cos(fθ3)))/((fd4*cos(fθ3))*(p
‑
a1)
‑
(z
‑
d1)*(fd4*sin(fθ3)
‑
a2)))。
[0072]
作为一种实施方式，后三轴角度计算步骤s5中，当计算出θ1、θ2、θ3后，根据连杆变换4x4矩阵可以得出a1*a2*a3的逆矩阵且当前机械手所在位置的位姿矩阵t
16
已知，可以求得机械手所在位置轴4至轴6的位姿矩阵t
46
，t
46
的表达式为：
[0073][0074]
而t
46
＝a4*a5*a6；
[0075]
以t
46
(x，y)代表t
46
矩阵的第x行、第y列，可得出：
[0076]
θ5＝acos(t
46
(3，3))；
[0077]
θ6＝atan(
‑
t
46
(3，2)/t
46
(3，1))；
[0078]
θ4＝atan(
‑
t
46
(2，3)/t
46
(1，3))；
[0079]
其中，存在两种特殊情况：
[0080]
当t
46
(3，1)＝0时，θ6＝0；
[0081]
当t
46
(1，3)＝0时，θ4＝0。
[0082]
请参照图2，本发明实施例的串联六关节机械手的姿态控制系统包括位姿矩阵计算模块、实际点计算模块、设置模块、前三轴角度计算模块及后三轴角度计算模块。
[0083]
位姿矩阵计算模块：读取串联六关节机械手的各关节伺服电机角度，根据读取的各关节伺服电机角度，使用连杆变换矩阵进行正解，得到机械手位姿矩阵a
16
；
[0084]
实际点计算模块：根据位姿矩阵a
16
来确定机械手空间坐标x、y、z，将位姿矩阵a
16
中的旋转矩阵转换为四元数q1、q2、q3、q4，得到实际点p1(x，y，z，q1，q2，q3，q4)；
[0085]
设置模块：根据用户手动示教或修改实际点p1得到目标点p2；
[0086]
前三轴角度计算模块：将p2的四元数转换至旋转矩阵并与xyz组合成目标点p2的位姿矩阵t
16
；根据位姿矩阵t
16
求出前三个关节机械手的轴角度θ1、θ2、θ3；
[0087]
后三轴角度计算模块：根据轴角度θ1、θ2、θ3得到前三个关节的逆矩阵表达式再计算得到后三个关节机械手的轴角度θ4、θ5、θ6；
[0088]
发送模块：将得到的各轴角度对应发送至各轴伺服电机，各轴伺服电机将关节角度旋转至对应轴角度，即可将移动机械手末端移动到p2目标点的位置。
[0089]
作为一种实施方式，位姿矩阵计算模块包括：
[0090]
子模块11：根据矩阵计算结果得出连杆变换4x4矩阵a
n
表达式：
[0091][0092]
其中，alpha
n
是机械手第n个关节与相邻的后一个关节轴的夹角，a
n
是表示机械手第n个关节与相邻的后一个关节轴公垂线的长度，d
n
是机械手第n个关节沿两个相邻连杆公共轴线方向的距离，n代表第几个关节；
[0093]
子模块12：将各轴角度代入连杆变换矩阵，经过矩阵乘法计算得出机械手位姿矩阵a
16
，a
16
表达式为：
[0094]
a
16
＝a1*a2*a3*a4*a5*a6；
[0095]
再计算得到a
16
矩阵表达式为：
[0096][0097]
其中，为机械手在当前位置下的旋转矩阵，px、py、pz分别为当前位置下的三轴坐标。
[0098]
作为一种实施方式，实际点计算模块包括：
[0099]
子模块21：由a
16
矩阵表达式得到旋转矩阵r的表达式和空间坐标，其中，空间坐标中的xyz分别等于a
16
中的px、py、pz，
[0100]
子模块22：由四元数转矩阵得到旋转矩阵r的另一表达式：
[0101][0102]
其中w＝q1，x＝q2，y＝q3，z＝q4；
[0103]
子模块23：由子步骤s21和s22中旋转矩阵r的表达式得到：
[0104]
ox+ny＝4xy；
[0105]
ox
‑
ny＝4wz；
[0106]
nz+ax＝4xz；
[0107]
nz
‑
ax＝4wy；
[0108]
ay+oz＝4yz；
[0109]
ay+oz＝4wx；
[0110]
由上式计算得到w、x、y、z，从而得到实际点p1(x，y，z，q1，q2，q3，q4)。
[0111]
作为一种实施方式，前三轴角度计算模块包括：
[0112]
子模块41：将目标点p2(x，y，z，q1，q2，q3，q4)通过子步骤s21和s22中旋转矩阵r的表达式转换组合成目标点的位姿矩阵t
16
，根据下式计算第四关节末端的三轴坐标：
[0113]
x＝px
‑
ax*d6+ox*[cos(θ6)*d5+a5*sin(θ6)]
‑
nx*[a6*cos(θ6)2+a5*cos(θ6)+a6*sin(θ6)2‑
d5*sin(θ6)]；
[0114]
y＝py
‑
ay*d6+oy*[cos(θ6)*d5+a5*sin(θ6)]
‑
ny*[a6*cos(θ6)2+a5*cos(θ6)+a6*sin(θ6)2‑
d5*sin(θ6)]；
[0115]
z＝pz
‑
az*d6+oz*[cos(θ6)*d5+a5*sin(θ6)]
‑
nz*[a6*cos(θ6)2+a5*cos(θ6)+a6*sin(θ6)2‑
d5*sin(θ6)]；
[0116]
子模块42：根据下式计算出目标点p2所对应的θ1：
[0117]
θ1＝atan2(x，y)；
[0118]
子模块43：根据下式计算计算出目标点p2所对应的θ3：
[0119][0120][0121]
fθ3＝asin(
‑
(((z
‑
d1)2+(p
‑
a1)2
‑
fd
42
‑
a
22
)/(2*fd4*a2)))；
[0122]
θ3＝90
‑
atan2(d4，a3)+fθ3；
[0123]
当a3为0时θ3＝fθ3；
[0124]
子模块44：根据下式计算出θ2：
[0125]
θ2＝
‑‑
atan(((fd4*sin(fθ3)
‑
a2)*(p
‑
a1)+(z
‑
d1)*(fd4*cos(fθ3)))/((fd4*cos(fθ3))*(p
‑
a1)
‑
(z
‑
d1)*(fd4*sin(fθ3)
‑
a2)))。
[0126]
作为一种实施方式，后三轴角度计算模块中，根据连杆变换4x4矩阵计算得出a1*a2*a3的逆矩阵再根据以下表达式计算机械手所在位置轴4至轴6的位姿矩阵t
46
：
[0127][0128]
而t
46
＝a4*a5*a6；以t
46
(x，y)代表t
46
矩阵的第x行、第y列，可得出：
[0129]
θ5＝acos(t
46
(3，3))；
[0130]
θ6＝atan(
‑
t
46
(3，2)/t
46
(3，1))；
[0131]
θ4＝atan(
‑
t
46
(2，3)/t
46
(1，3))；
[0132]
其中，当t
46
(3，1)＝0时，θ6＝0；当t
46
(1，3)＝0时，θ4＝0。
[0133]
示例：请参照图3，本发明应用于机械手的工业控制器中，六关节机械手由机械手本体和六个伺服驱动器组成。假设有一台机械手的数据如表1所示：
[0134]
表1
[0135] adalphaθ偏移角度190521.5
‑
900251700
‑
90300
‑
900403939005
‑
28108.75909060116.5700
[0136]
控制器中软件已知当前六个关节角度分别为(0，0，0，0，0，0)，用户需要知道当前机械手的空间坐标p1，然后根据p1位置给出目标点p2的位置，控制器中软件需要根据p2求出机械手六个关节的目标角度进行移动。
[0137]
首先可以求得：
[0138][0139]
从a
16
中得到xyz和旋转矩阵
[0140]
进而可以得到当前点p1(455，108.75，921.93，0，1，0，0)；再通过交互界面显示p1。
[0141]
用户根据现场实际情况觉得机械手需要沿x轴移动10mm同时姿态不需要发生变化，于是输入目标点p2(555，108.75，921.93，0，1，0，0)，要控制器控制机械手各个关节移动，最终末端位置到达p2，于是可计算得出机械手前三个关节角度分别为(0，1.1083，
‑
1.1224)。
[0142]
最后再求出余下后三个关节角度后得到目标点所对应的关节角度为(0，11.1664，
‑
12.5935，0，1.4271，0)；之后再由控制器控制机械手各个伺服电机将关节角度旋
转至计算出的角度后，机械手末端就能到达用户指定的目标点p2了。
[0143]
尽管已经示出和描述了本发明的实施例，对于本领域的普通技术人员而言，可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由所附权利要求及其等同范围限定。