基于空间解析几何的并联机器人正解及逆解方法与流程

1.本发明涉及机器人技术领域，具体涉及一种基于空间解析几何的四轴delta并联机器人正解方法及逆解方法。


背景技术：

2.并联机器人具有刚度高、速度快、柔性强、重量轻等优点，在食品、医药、电子等轻工业中应用最为广泛，在物料的搬运、包装、分拣等方面有着无可比拟的优势，已成为工业机器人领域不可忽视的一股新兴力量。然而目前针对delta型并联机器人的运动学算法，由于引入过多参数，及求解过程过于繁杂等问题，致使算法运算效率低下。


技术实现要素：

3.针对现有技术存在的问题，本发明的目的在于提供一种基于空间解析几何的四轴delta并联机器人正解方法及逆解方法，以提高正解及逆解的运算效率。
4.为实现上述目的，本发明采用的技术方案是：
5.一种基于空间解析几何的四轴delta并联机器人逆解方法，所述并联机器人包括一静平台、三主动臂、三从动臂、一动平台、一末端机构、一可伸缩传动轴，其中，静平台与主动臂通过通过电机和减速机连接，主动臂和从动臂通过铰链连接，从动臂与动平台通过铰链连接；
6.所述逆解方法包括以下步骤：
7.步骤1、以静平台中心为原点建立基坐标系o-xyz，并设定机器人结构参数如下：
8.[0009][0010]
步骤2、求取基坐标系原点指向动平台中心的向量r；
[0011]
r＝bi+l1iui+l2iwiꢀꢀ
(1)
[0012]
其中：
[0013][0014]
则
[0015]
步骤3、令与y重合，与y轴重合，然后绕z轴转动α
i-π/2，再绕yi转动βi，然后再绕xi'轴转动θi，在机器人中向下为正，所以代入-θi，得到：
[0016][0017]
步骤4、将式(1)进行改写得：
[0018]
r-b
i-l1iui＝l2iwiꢀꢀ
(4)；
[0019]
利用式(4)两边模量相等，并化简得到，
[0020][0021]
步骤5、将代入式(5)；
[0022]
令常数项：
[0023]
则式(5)可写成：
[0024]-2l1i(r-bi)
t
ui+ci＝0
[0025]
代入已知项：
[0026][0027]
得：
[0028]-2l1i(x-b
ix
)(cosαicosθ
i-sinαisinβisinθi)-2l1i(y-b
iy
)(sinαicosθi+cosαisinβisinθ)+2l1i(z-b
iz
)(-cosβisinθi)+ci＝0
[0029]
进一步化简得：
[0030]-2l1i((x-b
ix
)cosαi+(y-b
iy
)sinαi)cosθ+2l1i((x-b
ix
)sinαisinβ
i-(y-b
iy
)cosαisinβ
i-(z-b
iz
)cosβi)sinθi+ci＝0
[0031]ai
sinθi+bicosθ+ci＝0
ꢀꢀ
(6)
[0032]
公式中：
[0033]ai
＝2l1i((x-b
ix
)sinαisinβ
i-(y-b
iy
)cosαisinβ
i-(z-b
iz
)cosβi)
[0034]bi
＝-2l1i((x-b
ix
)cosαi+(y-b
iy
)sinαi)
[0035][0036]
已知：
[0037][0038][0039]
令：
[0040]
t＝tan(θi/2)
[0041]
则式(6)可写成：
[0042][0043]
2ait+bi(1-t2)+ci(1+t2)＝0
[0044]
(c
i-bi)t2+2ait+(bi+ci)＝0
[0045]
解得：
[0046][0047]
步骤6、由于关节角只限于0-90
°
活动，则t不能为负数，故舍去负数项得：
[0048][0049]
所求关节角为：
[0050]
[0051]
对上述z进行修正：z＝z+d，其中，d为机器人末端中心与动平台中心的距离。
[0052]
一种基于空间解析几何的四轴delta并联机器人正解方法，所述并联机器人包括一静平台、三主动臂、三从动臂、一动平台、一末端机构、一可伸缩传动轴，其中，静平台与主动臂通过通过电机和减速机连接，主动臂和从动臂通过铰链连接，从动臂与动平台通过铰链连接；
[0053]
所述正解方法包括以下步骤：
[0054]
步骤1、以静平台中心为原点建立基坐标系o-xyz，并设定机器人结构参数如下：
[0055][0056][0057]
步骤2、求取基坐标系原点指向动平台中心的向量r；
[0058]
r＝bi+l1iui+l2iwiꢀꢀ
(1)
[0059]
其中：
[0060][0061]
则
[0062]
步骤3、令ui与y重合，ui与y轴重合，然后ui绕z轴转动α
i-π/2，再绕yi转动βi，然后再绕xi'轴转动θi，在机器人中向下为正，所以代入-θi，得到：
[0063][0064]
步骤4、将式(1)进行改写得：
[0065]
r-b
i-l1iui＝l2iwiꢀꢀ
(4)；
[0066]
利用式(4)两边模量相等，并化简得到，
[0067][0068]
步骤5、将公式(5)整理得：
[0069][0070]
即：
[0071][0072]
其中：
[0073]
r＝[x,y,z]
t
；
[0074]
步骤6、将式(9)中的等式两两相减得：
[0075][0076][0077][0078]
即:
[0079][0080]
[0081][0082]
令：
[0083]
g1＝c
1-c2[0084]
g2＝c
2-c3[0085]
g3＝c
1-c3[0086]
其中：
[0087][0088]
步骤7、选取式(12)中的任意两式，得到线性方程组，结合式(9)进行解方程，得到x,y,z；
[0089]
步骤8、机器人末端机构与动平台距离为d，则末端机构中心位姿可表示为
[0090][0091]
即得到末端机构位姿为：
[0092][0093]
所述步骤7中，选取式(12)中的第1式和第2式建立方程组，令
[0094]a11
＝2(b
1x-b
2x
+l11u
1x-l12u
2x
)
[0095]a12
＝2(b
1y-b
2y
+l11u
1y-l12u
2y
)
[0096]a13
＝2(b
1z-b
2z
+l11u
1z-l12u
2z
)
[0097]a21
＝2(b
2x-b
3x
+l12u
2x-l13u
3x
)
[0098]a22
＝2(b
2y-b
3y
+l12u
2y-l13u
3y
)
[0099]a23
＝2(b
2z-b
3z
+l12u
2z-l13u
3z
)
[0100]
则得到
[0101]a11
x+a
12
y+a
13
z＝g1[0102]a21
x+a
22
y+a
23
z＝g2[0103]
g1＝c
1-c2[0104]
g2＝c
2-c3[0105]
其中：
[0106]
[0107]
令：
[0108]
x＝m2+n2z
[0109]
y＝m1+n1z
[0110]
则可得到：
[0111]
m1＝-(a
11g2-a
21
g1)/(a
12a21-a
11a22
)
[0112]
m2＝-(a
22g1-a
12
g2)/(a
12a21-a
11a22
)
[0113]
n1＝(a
11a23-a
13a21
)/(a
12a21-a
11a22
)
[0114]
n2＝(a
22a13-a
12a23
)/(a
12a21-a
11a22
)
[0115]
将x,y,z代入式(9)的第1式：
[0116][0117]
即：
[0118][0119]
由于：
[0120][0121]
则上式可变为：
[0122]
x2+y2+z
2-2{(b
1x
+l11u
1x
)x+(b
1y
+l11u
1y
)y+(b
1z
+l11u
1z
)z}+c1＝0
[0123]
其中：
[0124]
x＝m2+n2z
[0125]
y＝m1+n1z
[0126]
上式中m1、n1、m2、n2已知
[0127]
代入化简得到：
[0128]
(m2+n2z)2+(m1+n1z)2+z
2-2{(b
1x
+l11u
1x
)(m2+n2z)+(b
1y
+l11u
1y
)(m1+n1z)+(b
1z
+l11u
1z
)z}+c1＝0
ꢀꢀ
(13)
[0129]
展开得：
[0130]
(n
12
+n
22
+1)z2+2{m2n2+m1n
1-(b
1x
+l11u
1x
)n
2-(b
1y
+l11u
1y
)n
1-(b
1z
+l11u
1z
)}z+(m
22
+m
12-2(b
1x
+l11u
1x
)m
2-2(b
1y
+l11u
1y
)m1)+c1＝0
[0131]
令：
[0132]
k＝(n
12
+n
22
+1)
[0133]
e＝2{m2n2+m1n
1-(b
1x
+l11u
1x
)n
2-(b
1y
+l11u
1y
)n
1-(b
1z
+l11u
1z
)}
[0134]
f＝(m
22
+m
12-2(b
1x
+l11u
1x
)m
2-2(b
1y
+l11u
1y
)m1)+c1[0135]
得到二次方程：
[0136]
kz2+ez+f＝0
[0137][0138]
x＝m2+n2z
[0139]
y＝m1+n1z
[0140]
式中除x、y、z外均已知，已知项如下：
[0141][0142]
e＝2m2n2+2m1n
1-2(b
1x
+l11u
1x
)n
2-2(b
1y
+l11u
1y
)n
1-2(b
1z
+l11u
1z
)
[0143][0144]
m1＝-(a
11g2-a
21
g1)/(a
12a21-a
11a22
)
[0145]
m2＝-(a
22g1-a
12
g2)/(a
12a21-a
11a22
)
[0146]
n1＝(a
11a23-a
13a21
)/(a
12a21-a
11a22
)
[0147]
n2＝(a
22a13-a
12a23
)/(a
12a21-a
11a22
)
[0148]a11
＝2(b
1x-b
2x
+l11u
1x-l12u
2x
)
[0149]a12
＝2(b
1y-b
2y
+l11u
1y-l12u
2y
)
[0150]a13
＝2(b
1z-b
2z
+l11u
1z-l12u
2z
)
[0151]a21
＝2(b
2x-b
3x
+l12u
2x-l13u
3x
)
[0152]a22
＝2(b
2y-b
3y
+l12u
2y-l13u
3y
)
[0153]a23
＝2(b
2z-b
3z
+l12u
2z-l13u
3z
)
[0154]
g1＝c
1-c2[0155]
g2＝c
2-c3[0156][0157]
至此，x、y、z均可代入求出。
[0158]
采用上述方案后，本发明构建了多个关键参数，这些关键参数比几何法能更加详细准确的表征机器人的运动学特性，基于这些参数构建的数学模型，通过空间解析解析几何数学变换方法，避免了求解四次方程一般解的问题，有效提高了机器人正解及逆解的求解效率。并且，本发明在正解求解过程中采用拼凑齐次方程组，求解齐次方程组的方法，以避免高次方程求解，进一步提高了求解效率。
附图说明
[0159]
图1为四轴delta并联机器人结构示意图；
[0160]
图2为四轴delta并联机器人单轴机构简图；
[0161]
图3为四轴delta并联机器人的逆解流程图；
[0162]
图4为四轴delta并联机器人的正解流程图。
具体实施方式
[0163]
如图1所示，本发明所应用的四轴delta并联机器人包括一静平台1、三主动臂4、三从动臂5、一动平台8、一末端机构9、一可伸缩传动轴10，其中，静平台1与主动臂4通过通过电机3和减速机2连接，主动臂4和从动臂5通过铰链连接，从动臂5与动平台8通过铰链连接。
[0164]
如图2和图3所示，基于上述四轴delta并联机器人，本发明提出了一种基于空间解析几何的四轴delta并联机器人逆解方法，其包括以下步骤：
[0165]
步骤1、以静平台中心为原点建立基坐标系o-xyz，并设定机器人结构参数如下：
[0166][0167][0168]
步骤2、求取基坐标系原点指向动平台中心的向量r；
[0169]
r＝bi+l1iui+l2iwiꢀꢀ
(1)
[0170]
其中：
[0171][0172]
则
[0173]
其中，bi表示主动臂铰链的位置误差，l1i表示第i个主动臂的长度，l2i表示第i个从动臂的长度，ui为主动臂的单位方向向量，wi为从动臂的单位方向向量，为三个主动臂与静平台铰链的位置在基坐标系o下的位置的向量表示，为三个从动臂与动平台中心的向量在基坐标系o下的表示。
[0174]
步骤3、求主动臂单位向量：令ui与y重合，ui与y轴重合，然后ui绕z轴转动α
i-π/2，再绕yi转动βi，然后再绕xi'轴转动θi，在机器人中向下为正，所以代入-θi，得到
[0175][0176]
其中，βi为三个主动臂轴向与静平台的夹角，默认为0度；rot(参数1，参数2)，表示绕参数2轴逆时针转动参数1度。
[0177]
步骤4、将式(1)进行改写得：
[0178]
r-b
i-l1iui＝l2iwiꢀꢀ
(4)；
[0179]
其中：r＝(x,y,z)
t
,bi＝(b
ix
,b
iy
,b
iz
)
t
[0180][0181]
式(4)两端向量模长相等，则：
[0182]
(r-b
i-l1iui)
t
(r-b
i-l1iui)＝(l2iwi)
t
(l2iwi)
[0183]
进一步化简得：
[0184]rt
r-r
tbi-r
t
(l1iui)-b
it
r+b
it
(bi)+b
it
(l1iui)-(l1iui)
t
r+(l1iui)
tbi
+(l1iui)
t
(l1iui)-(l2iwi)
t
(l2iwi)＝0
[0185]rt
r-2b
it
r-l1i(r
t
ui+u
it
r)+l1i(b
it
ui+u
itbi
)+l1
i2
+b
it
(bi)-l2
i2
＝0
[0186]rt
r-2b
it
r-l1i(r
t
ui+u
it
r)+l1i(b
it
ui+u
itbi
)+l1
i2
+b
it
(bi)-l2
i2
＝0
[0187]rt
r-2b
it
r-2l1
irt
ui+2l1
ibit
ui+l1
i2
+b
it
(bi)-l2
i2
＝0
[0188]
得到，
[0189][0190]
步骤5、将代入式(5)；
[0191]
令常数项：
[0192]
则式(5)可写成：
[0193]-2l1i(r-bi)
t
ui+ci＝0
[0194]
代入已知项：
[0195][0196]
得：
[0197]-2l1i(x-b
ix
)(cosαicosθ
i-sinαisinβisinθi)-2l1i(y-b
iy
)(sinαicosθi+cosαisinβisinθ)+2l1i(z-b
iz
)(-cosβisinθi)+ci＝0
[0198]
进一步化简得：
[0199]-2l1i((x-b
ix
)cosαi+(y-b
iy
)sinαi)cosθ+2l1i((x-b
ix
)sinαisinβ
i-(y-b
iy
)cosαisinβ
i-(z-b
iz
)cosβi)sinθi+ci＝0
[0200]ai
sinθi+bicosθ+ci＝0
ꢀꢀ
(6)
[0201]
公式中：
[0202]ai
＝2l1i((x-b
ix
)sinαisinβ
i-(y-b
iy
)cosαisinβ
i-(z-b
iz
)cosβi)
[0203]bi
＝-2l1i((x-b
ix
)cosαi+(y-b
iy
)sinαi)
[0204][0205]
已知：
[0206][0207][0208]
令：
[0209]
t＝tan(θi/2)
[0210]
则式(6)可写成：
[0211][0212]
2ait+bi(1-t2)+ci(1+t2)＝0
[0213]
(c
i-bi)t2+2ait+(bi+ci)＝0
[0214]
解得：
[0215][0216]
步骤6、由于关节角只限于0-90
°
活动，则t不能为负数，故舍去负数项得：
[0217][0218]
所求关节角为：
[0219][0220]
上述逆解的求解过程是以动平台的中心为变量的，事实上，机器人末端指的是法兰盘的末端中心，两中心相差正好是d，因此在计算中要对z修正：z＝z+d。
[0221]
结合图2和图4所示，基于同一发明构思，本发明还揭示了一种基于空间解析几何的四轴delta并联机器人正解方法，其包括以下步骤：
[0222]
步骤1、以静平台中心为原点建立基坐标系o-xyz，并设定机器人结构参数；步骤2、求取基坐标系原点指向动平台中心的向量r；
[0223]
步骤3、令与y重合，与y轴重合，然后绕z轴转动α
i-π/2，再绕yi转动βi，然后再绕xi'轴转动θi，在机器人中向下为正，所以代入-θi。得到：
[0224][0225]
步骤4、将式(1)进行改写得
[0226][0227]
上述步骤1-4的内容与正解方法中的步骤1-4相同，此处不进行过多赘述。
[0228]
步骤5、将公式(5)整理得：
[0229][0230]
即：
[0231][0232]
其中：
[0233]
r＝[x,y,z]
t
；
[0234]
步骤6、将式(9)中的等式两两相减得：
[0235][0236][0237][0238]
即:
[0239][0240]
[0241][0242]
令：
[0243]
g1＝c
1-c2[0244]
g2＝c
2-c3[0245]
g3＝c
1-c3[0246]
其中：
[0247][0248]
步骤7、选取式(12)中的任意两式，得到线性方程组，结合式(9)进行解方程，得到x,y,z。
[0249]
注意方程组的奇异性问题，与坐标系的建立有关，构建坐标系或者选取式(12)中的公式构建方程组时，避免系数为零，物理上即任意两电机连线的向量与坐标轴平行的情况。
[0250]
本实施例中，选取式(12)中的第1式和第2式建立方程组，令
[0251]a11
＝2(b
1x-b
2x
+l11u
1x-l12u
2x
)
[0252]a12
＝2(b
1y-b
2y
+l11u
1y-l12u
2y
)
[0253]a13
＝2(b
1z-b
2z
+l11u
1z-l12u
2z
)
[0254]a21
＝2(b
2x-b
3x
+l12u
2x-l13u
3x
)
[0255]a22
＝2(b
2y-b
3y
+l12u
2y-l13u
3y
)
[0256]a23
＝2(b
2z-b
3z
+l12u
2z-l13u
3z
)
[0257]
则得到
[0258]a11
x+a
12
y+a
13
z＝g1[0259]a21
x+a
22
y+a
23
z＝g2[0260]
g1＝c
1-c2[0261]
g2＝c
2-c3[0262]
其中：
[0263][0264]
令：
[0265]
x＝m2+n2z
[0266]
y＝m1+n1z
[0267]
则可得到：
[0268]
m1＝-(a
11g2-a
21
g1)/(a
12a21-a
11a22
)
[0269]
m2＝-(a
22g1-a
12
g2)/(a
12a21-a
11a22
)
[0270]
n1＝(a
11a23-a
13a21
)/(a
12a21-a
11a22
)
[0271]
n2＝(a
22a13-a
12a23
)/(a
12a21-a
11a22
)
[0272]
将x,y,z代入式(9)的第1式：
[0273][0274]
即：
[0275][0276]
由于：
[0277][0278]
则上式可变为：
[0279]
x2+y2+z
2-2{(b
1x
+l11u
1x
)x+(b
1y
+l11u
1y
)y+(b
1z
+l11u
1z
)z}+c1＝0
[0280]
其中：
[0281]
x＝m2+n2z
[0282]
y＝m1+n1z
[0283]
上式中m1、n1、m2、n2已知
[0284]
代入化简得到：
[0285]
(m2+n2z)2+(m1+n1z)2+z
2-2{(b
1x
+l11u
1x
)(m2+n2z)+(b
1y
+l11u
1y
)(m1+n1z)+(b
1z
+l11u
1z
)z}+c1＝0
ꢀꢀ
(13)展开得：
[0286]
(n
12
+n
22
+1)z2+2{m2n2+m1n
1-(b
1x
+l11u
1x
)n
2-(b
1y
+l11u
1y
)n
1-(b
1z
+l11u
1z
)}z+(m
22
+m
12-2(b
1x
+l11u
1x
)m
2-2(b
1y
+l11u
1y
)m1)+c1＝0
[0287]
令：
[0288]
k＝(n
12
+n
22
+1)
[0289]
e＝2{m2n2+m1n
1-(b
1x
+l11u
1x
)n
2-(b
1y
+l11u
1y
)n
1-(b
1z
+l11u
1z
)}
[0290]
f＝(m
22
+m
12-2(b
1x
+l11u
1x
)m
2-2(b
1y
+l11u
1y
)m1)+c1[0291]
得到二次方程：
[0292]
kz2+ez+f＝0
[0293][0294]
x＝m2+n2z
[0295]
y＝m1+n1z
[0296]
式中除x、y、z外均已知，已知项如下：
[0297][0298]
e＝2m2n2+2m1n
1-2(b
1x
+l11u
1x
)n
2-2(b
1y
+l11u
1y
)n
1-2(b
1z
+l11u
1z
)
[0299][0300]
m1＝-(a
11g2-a
21
g1)/(a
12a21-a
11a22
)
[0301]
m2＝-(a
22g1-a
12
g2)/(a
12a21-a
11a22
)
[0302]
n1＝(a
11a23-a
13a21
)/(a
12a21-a
11a22
)
[0303]
n2＝(a
22a13-a
12a23
)/(a
12a21-a
11a22
)
[0304]a11
＝2(b
1x-b
2x
+l11u
1x-l12u
2x
)
[0305]a12
＝2(b
1y-b
2y
+l11u
1y-l12u
2y
)
[0306]a13
＝2(b
1z-b
2z
+l11u
1z-l12u
2z
)
[0307]a21
＝2(b
2x-b
3x
+l12u
2x-l13u
3x
)
[0308]a22
＝2(b
2y-b
3y
+l12u
2y-l13u
3y
)
[0309]a23
＝2(b
2z-b
3z
+l12u
2z-l13u
3z
)
[0310]
g1＝c
1-c2[0311]
g2＝c
2-c3[0312][0313]
至此，x、y、z均可代入求出。
[0314]
步骤8、机器人末端机构与动平台距离为d，则末端机构中心位姿可表示为
[0315][0316]
即得到末端机构位姿为：
[0317][0318]
本发明利用matlab实现上述正解、逆解计算。matlab程序如下：
[0319]
(1)正解
[0320]
[0321]
[0322]
[0323]
[0324]
[0325][0326]
(2)逆解
[0327]
[0328][0329]
综上，本发明构建了多个关键参数，这些关键参数比几何法能更加详细准确的表征机器人的运动学特性，基于这些参数构建的数学模型，通过空间解析解析几何数学变换方法，避免了求解四次方程一般解的问题，有效提高了机器人正解及逆解的求解效率。并且，本发明在正解求解过程中采用拼凑齐次方程组，求解齐次方程组的方法，以避免高次方程求解，进一步提高了求解效率。
[0330]
以上所述，仅是本发明实施例而已，并非对本发明的技术范围作任何限制，故凡是依据本发明的技术实质对以上实施例所作的任何细微修改、等同变化与修饰，均仍属于本
发明技术方案的范围内。