一種求UR機械臂逆運動學不受奇異位置影響的方法
- 1 逆運動學求解
- 1.1 求解 θ 1 \theta_1 θ1?
- 1.2 求解 θ 6 \theta_6 θ6?
- 1.3 求解 θ 2 \theta_2 θ2?和 θ 3 \theta_3 θ3?
- 1.4 求解 θ 5 \theta_5 θ5?
- 1.5 求解 θ 4 \theta_4 θ4?
- 2. 驗證求奇異位置逆解
- 3. 代碼
- 3.1 標準DH方法的齊次變換矩陣
- 3.2 正向運動學
- 3.3 逆運動學
- 3.4 總運行m程式
- 4 參考文獻
本文將介紹一種新方法求解UR機械臂的逆運動學,這個方法來自我最近發表在《Mechanism and Machine Theory》上的文章,文末有該文的下載地址,文章中有很多細節將會隱去,各位可以下載原文細讀,
網上已經有很多文章提供了UR機械臂的逆解方法,這些方法仍存在一些不足,主要是腕部關節的計算公式會受奇異位置的影響,即第四個關節與第六個關節的軸線共線時,這兩個關節將難以計算,文章以鏈接中的UR機械臂為例,講解求解方法,UR機械臂的圖以及DH引數表,都來源于這篇博文,https://blog.csdn.net/fengyu19930920/article/details/81144042/
圖1
圖2
表1
需要注意的是,參考的這篇博文中用的是標準DH方法建立的正向運動學公式,
T = T 1 ( θ 1 ) T 2 ( θ 2 ) T 3 ( θ 3 ) T 4 ( θ 4 ) T 5 ( θ 5 ) T 6 ( θ 6 ) T =T_1(\theta_1)T_2(\theta_2)T_3(\theta_3)T_4(\theta_4)T_5(\theta_5)T_6(\theta_6) T=T1?(θ1?)T2?(θ2?)T3?(θ3?)T4?(θ4?)T5?(θ5?)T6?(θ6?)(1)
1 逆運動學求解
首先根據文獻[1]中的方法,我們可以對公式(1)進行一下變形:
T
L
=
T
2
(
θ
2
)
T
3
(
θ
3
)
T
4
(
θ
4
)
R
o
t
(
?
α
4
,
x
4
)
T_L=T_2(\theta_2)T_3(\theta_3)T_4(\theta_4)Rot(-\alpha_4,x_4)
TL?=T2?(θ2?)T3?(θ3?)T4?(θ4?)Rot(?α4?,x4?)(2)
T
R
=
T
?
1
(
θ
1
)
T
T
?
1
(
θ
6
)
T
?
1
(
θ
5
)
T_R =T^{-1}(\theta_1)TT^{-1}(\theta_6)T^{-1}(\theta_5)
TR?=T?1(θ1?)TT?1(θ6?)T?1(θ5?)(3)
TL =[ cos(q2 + q3 + q4), -sin(q2 + q3 + q4), 0, a3*cos(q2 + q3) + a2*cos(q2)
sin(q2 + q3 + q4), cos(q2 + q3 + q4), 0, a3*sin(q2 + q3) + a2*sin(q2)
0, 0, 1, d4
0, 0, 0, 1]
TR =[ - cos(q1)*(ax*sin(q5) - nx*cos(q5)*cos(q6) + ox*cos(q5)*sin(q6)) - sin(q1)*(ay*sin(q5) - ny*cos(q5)*cos(q6) + oy*cos(q5)*sin(q6)), cos(q1)*(ax*cos(q5) + nx*cos(q6)*sin(q5) - ox*sin(q5)*sin(q6)) + sin(q1)*(ay*cos(q5) + ny*cos(q6)*sin(q5) - oy*sin(q5)*sin(q6)), - ox*cos(q1)*cos(q6) - nx*cos(q1)*sin(q6) - oy*cos(q6)*sin(q1) - ny*sin(q1)*sin(q6), cos(q1)*(px - ax*d6 + d5*ox*cos(q6) + d5*nx*sin(q6)) + sin(q1)*(py - ay*d6 + d5*oy*cos(q6) + d5*ny*sin(q6))
nz*cos(q5)*cos(q6) - az*sin(q5) - oz*cos(q5)*sin(q6), az*cos(q5) + nz*cos(q6)*sin(q5) - oz*sin(q5)*sin(q6), - oz*cos(q6) - nz*sin(q6), pz - d1 - az*d6 + d5*oz*cos(q6) + d5*nz*sin(q6)
cos(q1)*(ay*sin(q5) - ny*cos(q5)*cos(q6) + oy*cos(q5)*sin(q6)) - sin(q1)*(ax*sin(q5) - nx*cos(q5)*cos(q6) + ox*cos(q5)*sin(q6)), sin(q1)*(ax*cos(q5) + nx*cos(q6)*sin(q5) - ox*sin(q5)*sin(q6)) - cos(q1)*(ay*cos(q5) + ny*cos(q6)*sin(q5) - oy*sin(q5)*sin(q6)), oy*cos(q1)*cos(q6) + ny*cos(q1)*sin(q6) - ox*cos(q6)*sin(q1) - nx*sin(q1)*sin(q6), sin(q1)*(px - ax*d6 + d5*ox*cos(q6) + d5*nx*sin(q6)) - cos(q1)*(py - ay*d6 + d5*oy*cos(q6) + d5*ny*sin(q6))
0, 0, 0, 1];
正運動學方程的變形可以看作是把原來的機械臂拆分為兩段子鏈,分別為 T L T_L TL?和 T R T_R TR?中,而 T L T_L TL?中乘以 R o t ( ? α 4 , x 4 ) Rot(-\alpha_4,x_4) Rot(?α4?,x4?)表示將子鏈 T L T_L TL?的末端姿態繞 x 4 x_4 x4?軸旋轉 ? α 4 -\alpha_4 ?α4?弧度,這樣做的目的是為了讓子鏈 T L T_L TL?末端姿態的z軸與第四個關節的軸線重合,因為從圖中我們可以知道第四個關節和第五個關節的軸線是相互垂直的,另外,采用(2)-(3)的方式變形,可以充分利用機械臂的幾何資訊,我們可以直觀地看出子鏈 T L T_L TL?只包含關節2~關節4,這三個關節組成一個平面結構,它的末端姿態中 z L = [ 0 , 0 , 1 ] T z_L=[0,0,1]^T zL?=[0,0,1]T,這樣后面推導逆解公式將會變得很方便,
1.1 求解 θ 1 \theta_1 θ1?
由前面的變形可以知道,子鏈
T
L
T_L
TL?末端姿態的分向量與關節2至4無關,位置
P
L
Z
P_{LZ}
PLZ?也與這三個關節無關,而子鏈
T
L
T_L
TL?中對應的部分至于關節1和關節6有關,因此可以得到下列等式:
P
L
Z
=
P
R
Z
P_{LZ}=P_{RZ}
PLZ?=PRZ?
s
1
(
p
x
?
a
x
d
6
+
d
5
o
x
c
6
)
+
d
5
n
x
s
6
)
?
c
1
(
p
y
?
a
y
d
6
+
d
5
o
y
c
6
+
d
5
n
y
s
6
)
=
d
4
s_1(p_x - a_xd_6 + d_5o_xc_6) + d_5n_xs_6) - c_1(p_y - a_yd_6 + d_5o_yc_6+ d_5n_ys_6)=d_4
s1?(px??ax?d6?+d5?ox?c6?)+d5?nx?s6?)?c1?(py??ay?d6?+d5?oy?c6?+d5?ny?s6?)=d4? (4)
z
L
z
R
=
0
z_Lz_R=0
zL?zR?=0
(
o
y
c
6
+
n
y
s
6
)
c
1
?
(
o
x
c
6
+
n
x
s
6
)
s
1
=
0
(o_yc_6 + n_ys_6)c_1 - (o_xc_6 + n_xs_6)s_1=0
(oy?c6?+ny?s6?)c1??(ox?c6?+nx?s6?)s1?=0 (5)
將(5)代入(4)中可以消掉
θ
6
\theta_6
θ6?,得到只含
θ
1
\theta_1
θ1?的三角函式式子:
s
1
(
p
x
?
a
x
d
6
)
?
c
1
(
p
y
?
a
y
d
6
)
=
d
4
s_1(p_x - a_xd_6 ) - c_1(p_y - a_yd_6)=d_4
s1?(px??ax?d6?)?c1?(py??ay?d6?)=d4? (6)
解得
θ
1
\theta_1
θ1?,
A1=px - ax*d6;
B1=py - ay*d6;
q1=atan2(d4,k1*sqrt(A1^2+B1^2-d4^2))+atan2(B1,A1)
θ 1 \theta_1 θ1?運算式中根號部分,表示目標位姿點是否在機械臂的作業空間內,若不在,則根號內為負號無解, k 1 = ± 1 k1=\pm1 k1=±1,代碼中的q表示 θ \theta θ,下同,
1.2 求解 θ 6 \theta_6 θ6?
根據兩個子鏈末端的z軸互相垂直
z
L
⊥
z
R
z_L⊥z_R
zL?⊥zR?,有
o
y
c
1
c
6
+
n
y
c
1
s
6
?
o
x
c
6
s
1
?
n
x
s
1
s
6
=
0
o_yc_1c_6 + n_yc_1s_6 - o_xc_6s_1 - n_xs_1s_6=0
oy?c1?c6?+ny?c1?s6??ox?c6?s1??nx?s1?s6?=0 (7)
解得
A6=ny*cos(q1) - nx*sin(q1);
B6=oy*cos(q1) - ox*sin(q1);
q6=atan2(0,k2)-atan2(B6,A6)
式中 k 2 = ± 1 k2=\pm1 k2=±1,
1.3 求解 θ 2 \theta_2 θ2?和 θ 3 \theta_3 θ3?
由
P
L
x
=
P
R
x
P_{Lx}=P_{Rx}
PLx?=PRx? 和
P
L
y
=
P
R
y
P_{Ly}=P_{Ry}
PLy?=PRy? 得到方程組
a
3
c
23
+
a
2
c
2
=
c
1
(
p
x
?
a
x
d
6
+
d
5
o
x
c
6
+
d
5
n
x
s
6
)
+
s
1
(
p
y
?
a
y
d
6
+
d
5
o
y
c
6
+
d
5
n
y
s
6
)
a_3c_{23} + a_2c_2=c_1(p_x - a_xd_6 + d_5o_xc_6 + d_5n_xs_6) + s_1(p_y - a_yd_6 + d_5o_yc_6 + d_5n_ys_6)
a3?c23?+a2?c2?=c1?(px??ax?d6?+d5?ox?c6?+d5?nx?s6?)+s1?(py??ay?d6?+d5?oy?c6?+d5?ny?s6?) (8)
a
3
s
23
+
a
2
s
2
=
p
z
?
d
1
?
a
z
d
6
+
d
5
o
z
c
6
+
d
5
n
z
s
6
a_3s_{23}+ a_2s_2= p_z - d_1 - a_zd_6 + d_5o_zc_6 + d_5n_zs_6
a3?s23?+a2?s2?=pz??d1??az?d6?+d5?oz?c6?+d5?nz?s6? (9)
首先將式(8)和(9)兩邊同時平方相加可得到
θ
3
\theta_3
θ3?,
p1= cos(q1)*(px - ax*d6 + d5*ox*cos(q6) + d5*nx*sin(q6)) + sin(q1)*(py - ay*d6 + d5*oy*cos(q6) + d5*ny*sin(q6));
p2= pz - d1 - az*d6 + d5*oz*cos(q6) + d5*nz*sin(q6);
c3=(p1^2+p2^2-a2^2 - a3^2)/(2*a2*a3);
s3=k3*sqrt(1-c3^2);
q3=atan2(s3,c3)
式中 p 1 和 p 2 p_1和p_2 p1?和p2?分別表示式(8)和(9)等號右邊的部分, k 3 = ± 1 k3=\pm1 k3=±1,
然后將式(8)和(9)中 c 23 和 s 23 c_{23}和s_{23} c23?和s23?都移到等號右邊,接著兩邊平方相加,得到
A23=2*a3*p1;
B23=2*a3*p2;
C23=a3^2 + p1^2 + p2^2-a2^2;
q23=atan2(k3*sqrt(A23^2+B23^2-C23^2),C23)+atan2(B23,A23)
式中 p 1 和 p 2 , k 3 p_1和p_2,k3 p1?和p2?,k3同上,
最后 θ 2 = θ 23 ? θ 3 \theta_2=\theta_{23}-\theta_3 θ2?=θ23??θ3?,
1.4 求解 θ 5 \theta_5 θ5?
根據參考文獻[1]中的條件式(9)有,
z
L
T
y
R
=
1
\bm{z_L^T}\bm{y_R}=1
zLT?yR?=1,之前我們知道
z
L
=
[
0
,
0
,
1
]
T
z_L=[0,0,1]^T
zL?=[0,0,1]T,因此可以很容易得到式子
s
1
(
a
x
c
5
+
n
x
c
6
s
5
?
o
x
s
5
s
6
)
?
c
1
(
a
y
c
5
+
n
y
c
6
s
5
?
o
y
s
5
s
6
)
=
1
s_1(a_xc_5+ n_xc_6s_5 - o_xs_5s_6) - c_1(a_yc_5 + n_yc_6s_5 - o_ys_5s_6)=1
s1?(ax?c5?+nx?c6?s5??ox?s5?s6?)?c1?(ay?c5?+ny?c6?s5??oy?s5?s6?)=1 (10)
由該式可以得到
θ
5
\theta_5
θ5?,
A5=( - ny*cos(q1)*cos(q6) + nx*cos(q6)*sin(q1) + oy*cos(q1)*sin(q6) - ox*sin(q1)*sin(q6));
B5= ax*sin(q1) - ay*cos(q1);
q5=atan2(0,1)+atan2(A5,B5);
需要注意的是,由文獻[1]中的方法得到之前的四個關節角后,剩余兩個角都將是唯一對應,求解程序中帶有正負符號的部分直接等于0,大家可以自己把兩個子鏈的末端姿態畫出來看就能明白,我在我的碩士畢業論文中也給出了證明,因為剛畢業,現在還沒在知網上出版,各位先記住這個結論即可,當然也可以嘗試證明,我用的是三余弦定理證明的,
1.5 求解 θ 4 \theta_4 θ4?
θ
4
\theta_4
θ4?的求解也可以按照文獻[1]中的條件式(8)來,但是我們觀察兩個子鏈姿態中
x
L
和
x
R
x_L和x_R
xL?和xR?的運算式,會發現
c
234
=
?
c
1
(
a
x
s
5
?
n
x
c
5
c
6
+
o
x
c
5
s
6
)
?
s
1
(
a
y
s
5
?
n
y
c
5
c
6
+
o
y
c
5
s
6
)
c_{234}= - c_1(a_xs_5 - n_xc_5c_6 + o_xc_5s_6) - s_1(a_ys_5 - n_yc_5c_6 + o_yc_5s_6)
c234?=?c1?(ax?s5??nx?c5?c6?+ox?c5?s6?)?s1?(ay?s5??ny?c5?c6?+oy?c5?s6?)(11)
s
234
=
n
z
c
5
c
6
?
a
z
s
5
?
o
z
c
5
s
6
s_{234}= n_zc_5c_6- a_zs_5 - o_zc_5s_6
s234?=nz?c5?c6??az?s5??oz?c5?s6?(12)
解得,
A4= nz*cos(q5)*cos(q6) - az*sin(q5) - oz*cos(q5)*sin(q6);
B4=-cos(q1)*(ax*sin(q5) - nx*cos(q5)*cos(q6) + ox*cos(q5)*sin(q6)) - sin(q1)*(ay*sin(q5) - ny*cos(q5)*cos(q6) + oy*cos(q5)*sin(q6));
q234=atan2(A4,B4);
q4=q234-q23;
至此UR機械臂的所有逆解都求出來了,接下來我們驗證奇異位置時能否得到逆運動學解,
2. 驗證求奇異位置逆解
從圖以及建立的DH引數表可以知道, θ 5 = 0 \theta_5=0 θ5?=0時,關節4與關節6的軸線是共線的,此時UR機械臂處于腕部奇異位置,我們令目標位姿由以下關節角組合得到 θ d = [ 1 , 1 , 1 , 1 , 0 , 1 ] \theta_d=[1,1,1,1,0,1] θd?=[1,1,1,1,0,1],代入正向運動學公式得到目標位姿 T d T_d Td?
Td =
1.0e+02 *
-0.003531651555686 0.004089021333016 0.008414709848079 1.324431340991907
-0.005500221413615 0.006368273410318 -0.005403023058681 -1.480707090264286
-0.007568024953079 -0.006536436208636 0.000000000000000 -5.311352944124976
0 0 0 0.010000000000000
它所對應的所有逆解為
AQ =
0.095363758584843 0.850264373394017 1.736680792136842 -2.586945165530858 -0.904636241415157 -2.283185307179587
0.095363758584843 2.492335031398676 -1.736680792136842 -0.755654239261834 -0.904636241415157 -2.283185307179587
0.095363758584843 1.166295711457801 1.000859439511193 0.974437502620798 0.904636241415157 0.858407346410206
0.095363758584843 2.123333020453949 -1.000859439511193 2.019119072647037 0.904636241415157 0.858407346410206
1.000000000000001 1.006084796303796 0.999365806146063 1.038416022677409 -0.000000000000001 0.956133374872731
1.000000000000001 1.961706157981659 -0.999365806146063 2.081526273291673 -0.000000000000001 0.956133374872731
1.000000000000001 0.666163375380177 1.737955362927514 -2.501844766770216 0.000000000000001 -2.185459278717062
1.000000000000001 2.309386429160488 -1.737955362927514 -0.669157094695499 0.000000000000001 -2.185459278717062
這些解對應的位姿與目標位姿之間的誤差為
delta =
1.0e-12 *
0.236088728961565 0.130244884890050 0.236088466354214 0.212690026999602 0.153055985103290 0.360631166302336 0.102475995454713 0.170530682241551
從結果可以看出,求解的位姿誤差范數達到10的負-12次方數量級,且求解公式不受奇異位置的影響,
3. 代碼
正向運動學的代碼,
3.1 標準DH方法的齊次變換矩陣
function [ T ] = stdtrans( q,d,a,alfa )
%TRANS 此處顯示有關此函式的摘要
% 此處顯示詳細說明
T=[cos(q) -sin(q)*cos(alfa) sin(q)*sin(alfa) a*cos(q);
sin(q) cos(q)*cos(alfa) -cos(q)*sin(alfa) a*sin(q);
0 sin(alfa) cos(alfa) d;
0 0 0 1];
end
3.2 正向運動學
function [T] = FW(q)
alfa=pi/2;
d1=89.459;
a2=-425 ;
a3=-392.25 ;
d4=109.15;
d5=94.65;
d6=82.3;
T1=stdtrans(q(1), d1, 0, alfa);
T2=stdtrans(q(2), 0, a2, 0);
T3=stdtrans(q(3), 0, a3, 0);
T4=stdtrans(q(4), d4, 0, alfa);
T5=stdtrans(q(5), d5, 0, -alfa);
T6=stdtrans(q(6), d6, 0, 0);
T=T1*T2*T3*T4*T5*T6;
end
3.3 逆運動學
function [Q] = IK(T,K)
d1=89.459;
a2=-425 ;
a3=-392.25 ;
d4=109.15;
d5=94.65;
d6=82.3;
k1=K(1);
k2=K(2);
k3=K(3);
px=T(1,4); py=T(2,4); pz=T(3,4);
ax=T(1,3); ay=T(2,3); az=T(3,3);
ox=T(1,2); oy=T(2,2); oz=T(3,2);
nx=T(1,1); ny=T(2,1); nz=T(3,1);
A1=px - ax*d6;
B1=py - ay*d6;
q1=atan2(d4,k1*sqrt(A1^2+B1^2-d4^2))+atan2(B1,A1);
% % + (ny*cos(q1) - nx*sin(q1))*sin(q6)+(oy*cos(q1)- ox*sin(q1))*cos(q6)
A6=ny*cos(q1) - nx*sin(q1);
B6=oy*cos(q1) - ox*sin(q1);
q6=atan2(0,k2)-atan2(B6,A6);
%%%%%%%%%%%%%%%
p1= cos(q1)*(px - ax*d6 + d5*ox*cos(q6) + d5*nx*sin(q6)) + sin(q1)*(py - ay*d6 + d5*oy*cos(q6) + d5*ny*sin(q6));
p2= pz - d1 - az*d6 + d5*oz*cos(q6) + d5*nz*sin(q6);
c3=(p1^2+p2^2-a2^2 - a3^2)/(2*a2*a3);
s3=k3*sqrt(1-c3^2);
q3=atan2(s3,c3);
% % % a3^2 - 2*cos(q2 + q3)*a3*p1 - 2*sin(q2 + q3)*a3*p2 + p1^2 + p2^2-a2^2
A23=2*a3*p1;
B23=2*a3*p2;
C23=a3^2 + p1^2 + p2^2-a2^2;
q23=atan2(k3*sqrt(A23^2+B23^2-C23^2),C23)+atan2(B23,A23);
%%%%%%%%%%%%%%%
q2=q23-q3;
% % % ax*cos(q5)*sin(q1) - ay*cos(q1)*cos(q5) - ny*cos(q1)*cos(q6)*sin(q5) + nx*cos(q6)*sin(q1)*sin(q5) + oy*cos(q1)*sin(q5)*sin(q6) - ox*sin(q1)*sin(q5)*sin(q6)
%%%%%%%%%%%%%%%
% % (ax*sin(q1) - ay*cos(q1))*cos(q5)+( - ny*cos(q1)*cos(q6) + nx*cos(q6)*sin(q1) + oy*cos(q1)*sin(q6) - ox*sin(q1)*sin(q6))*sin(q5)
A5=( - ny*cos(q1)*cos(q6) + nx*cos(q6)*sin(q1) + oy*cos(q1)*sin(q6) - ox*sin(q1)*sin(q6));
B5= ax*sin(q1) - ay*cos(q1);
q5=atan2(0,1)+atan2(A5,B5);
%%%%%%%%%%%%%
A4= nz*cos(q5)*cos(q6) - az*sin(q5) - oz*cos(q5)*sin(q6);
B4=-cos(q1)*(ax*sin(q5) - nx*cos(q5)*cos(q6) + ox*cos(q5)*sin(q6)) - sin(q1)*(ay*sin(q5) - ny*cos(q5)*cos(q6) + oy*cos(q5)*sin(q6));
q234=atan2(A4,B4);
q4=q234-q23;
Q=[q1 q2 q3 q4 q5 q6];
end
3.4 總運行m程式
clear
clc
K=[1 1 1
1 1 -1
1 -1 1
1 -1 -1
-1 1 1
-1 1 -1
-1 -1 1
-1 -1 -1];
Td=FW([1 1 1 1 0 1])
for i=1:8
Q=IK(Td,K(i,:));
%%%%%%將各個關節角的值轉換到[-pi,pi]
for j=1:6
if Q(j)<-pi
Q(j)=Q(j)+2*pi;
elseif Q(j)>pi
Q(j)=Q(j)-2*pi;
else
Q(j)=Q(j);
end
end
T1=FW(Q);
delta(i)=norm(T1-Td);
AQ(i,:)=Q;
end
AQ
4 參考文獻
[1] Xiao F, Li G, Jiang D, et al. An effective and unified method to derive the inverse kinematics formulas of general six-DOF manipulator with simple geometry[J]. Mechanism and Machine Theory, 2021, 159: 104265.
這篇論文目前是online階段,希望里面的內容能幫助國內各位同行,也衷心希望各位寫論文時,提到逆運動學的部分能把這篇參考進去,有問題的朋友可以留言,謝謝各位!
轉載請註明出處,本文鏈接:https://www.uj5u.com/ruanti/264872.html
標籤:其他
上一篇:手機訪問本地Tomcat服務器
