文章目录
正运动学求解
syms thetai alphai_1 ai_1 di ci si %此处是alphai_1为alpha_{i-1},ai为a_{i-1}
ci = cos(thetai);
si = sin(thetai);
Ti = [ci -si 0 ai_1
si*cos(alphai_1) ci*cos(alphai_1) -sin(alphai_1) -sin(alphai_1)*di
si*sin(alphai_1) ci*sin(alphai_1) cos(alphai_1) cos(alphai_1)*di
0 0 0 1];
%根据DH参数定义变量
syms alpha0 a0 d1 theta1 ...
alpha1 a1 d2 theta2 ...
alpha2 a2 d3 theta3 ...
alpha3 a3 d4 theta4 ...
alpha4 a4 d5 theta5 ...
alpha5 a5 d6 theta6
% 测试参数
alpha0=0; a0=0;
alpha1=pi/2; a1=0; d2=0;
alpha2=0; d3=-0;
alpha3=0;
alpha4=-pi/2; a4=0;
alpha5=pi/2; a5=0;
T01 = subs(Ti,[alphai_1 ai_1 di thetai],[alpha0 a0 d1 theta1]);
T12 = subs(Ti,[alphai_1 ai_1 di thetai],[alpha1 a1 d2 theta2]);
T23 = subs(Ti,[alphai_1 ai_1 di thetai],[alpha2 a2 d3 theta3]);
T34 = subs(Ti,[alphai_1 ai_1 di thetai],[alpha3 a3 d4 theta4]);
T45 = subs(Ti,[alphai_1 ai_1 di thetai],[alpha4 a4 d5 theta5]);
T56 = subs(Ti,[alphai_1 ai_1 di thetai],[alpha5 a5 d6 theta6]);
T13 = simplify(T12*T23);
T46 = simplify(T45*T56);
T36 = simplify(T34*T46);
T16 = simplify(T13*T36);
T06 = simplify(T01*T16);
求 6 0 T ^0_6T 60T,具体结果不展开。
逆解求解
1.求 θ 1 \theta_1 θ1
syms nx ox ax px ny oy ay py nz oz az pz
T = [nx ox ax px
ny oy ay py
nz oz az pz
0 0 0 1];
MT16 = simplify(inv(T01)*T)
6
1
T
=
(
n
x
c
o
s
(
θ
1
)
+
n
y
s
i
n
(
θ
1
)
o
x
c
o
s
(
θ
1
)
+
o
y
s
i
n
(
θ
1
)
a
x
c
o
s
(
θ
1
)
+
a
y
s
i
n
(
θ
1
)
p
x
c
o
s
(
θ
1
)
+
p
y
s
i
n
(
θ
1
)
n
y
c
o
s
(
θ
1
)
−
n
x
s
i
n
(
θ
1
)
o
y
c
o
s
(
θ
1
)
−
o
x
s
i
n
(
θ
1
)
a
y
c
o
s
(
θ
1
)
−
a
x
s
i
n
(
θ
1
)
p
y
c
o
s
(
θ
1
)
−
p
x
s
i
n
(
θ
1
)
n
z
o
z
a
z
p
z
−
d
1
0
0
0
1
)
^1_6T=\left(\begin{array}{cccc} \mathrm{nx}\,\mathrm{cos}\left(\theta_1 \right)+\mathrm{ny}\,\mathrm{sin}\left(\theta_1 \right) & \mathrm{ox}\,\mathrm{cos}\left(\theta_1 \right)+\mathrm{oy}\,\mathrm{sin}\left(\theta_1 \right) & \mathrm{ax}\,\mathrm{cos}\left(\theta_1 \right)+\mathrm{ay}\,\mathrm{sin}\left(\theta_1 \right) & \mathrm{px}\,\mathrm{cos}\left(\theta_1 \right)+\mathrm{py}\,\mathrm{sin}\left(\theta_1 \right)\\ \mathrm{ny}\,\mathrm{cos}\left(\theta_1 \right)-\mathrm{nx}\,\mathrm{sin}\left(\theta_1 \right) & \mathrm{oy}\,\mathrm{cos}\left(\theta_1 \right)-\mathrm{ox}\,\mathrm{sin}\left(\theta_1 \right) & \mathrm{ay}\,\mathrm{cos}\left(\theta_1 \right)-\mathrm{ax}\,\mathrm{sin}\left(\theta_1 \right) & \mathrm{py}\,\mathrm{cos}\left(\theta_1 \right)-\mathrm{px}\,\mathrm{sin}\left(\theta_1 \right)\\ \mathrm{nz} & \mathrm{oz} & \mathrm{az} & \mathrm{pz}-d_1 \\ 0 & 0 & 0 & 1 \end{array}\right)
61T=
nxcos(θ1)+nysin(θ1)nycos(θ1)−nxsin(θ1)nz0oxcos(θ1)+oysin(θ1)oycos(θ1)−oxsin(θ1)oz0axcos(θ1)+aysin(θ1)aycos(θ1)−axsin(θ1)az0pxcos(θ1)+pysin(θ1)pycos(θ1)−pxsin(θ1)pz−d11
用thetai表示的
6
1
T
^1_6T
61T在此不展开，在正解的求解过程中可以求出。
6
1
T
(
2
,
3
)
=
−
c
o
s
(
θ
5
)
=
a
z
^1_6T(2,3)=-cos(\theta_5)=az
61T(2,3)=−cos(θ5)=az，且
6
1
T
(
2
,
4
)
=
−
d
4
−
d
6
∗
c
o
s
(
θ
5
)
=
p
x
c
o
s
(
θ
1
)
+
p
y
s
i
n
(
θ
1
)
^1_6T(2,4)=- d4 - d6*cos(\theta_5)=\mathrm{px}\,\mathrm{cos}\left(\theta_1 \right)+\mathrm{py}\,\mathrm{sin}\left(\theta_1 \right)
61T(2,4)=−d4−d6∗cos(θ5)=pxcos(θ1)+pysin(θ1)
则
p
x
cos
(
θ
1
)
+
p
y
sin
(
θ
1
)
=
−
d
4
+
d
6
∗
a
z
px \cos(\theta_1)+py \sin(\theta_1)=-d4+d6*az
pxcos(θ1)+pysin(θ1)=−d4+d6∗az
θ
1
=
a
t
a
n
2
(
−
p
y
,
p
x
)
−
a
t
a
n
2
(
d
6
∗
a
z
−
d
4
,
±
p
x
2
+
p
y
2
−
(
d
6
∗
a
z
−
d
4
)
2
)
\theta_1=atan2(-py,px)-atan2(d6*az-d4,\pm\sqrt{px^2+py^2-(d6*az-d4)^2})
θ1=atan2(−py,px)−atan2(d6∗az−d4,±px2+py2−(d6∗az−d4)2)
2. 求 θ 6 \theta_6 θ6
6
1
T
(
2
,
1
)
=
cos
(
θ
6
)
sin
(
θ
5
)
^1_6T(2,1)=\cos(\theta_6)\sin(\theta_5)
61T(2,1)=cos(θ6)sin(θ5)
6
1
T
(
2
,
2
)
=
−
sin
(
θ
6
)
sin
(
θ
5
)
^1_6T(2,2)=-\sin(\theta_6)\sin(\theta_5)
61T(2,2)=−sin(θ6)sin(θ5)
θ
6
=
a
t
a
n
2
(
−
6
1
T
(
2
,
2
)
,
6
1
T
(
2
,
1
)
)
=
a
t
a
n
2
(
−
o
y
c
o
s
(
θ
1
)
+
o
x
s
i
n
(
θ
1
)
,
n
y
c
o
s
(
θ
1
)
−
n
x
s
i
n
(
θ
1
)
)
\theta_6=atan2(-^1_6T(2,2),^1_6T(2,1))\\=atan2(-\mathrm{oy}\,\mathrm{cos}\left(\theta_1 \right)+\mathrm{ox}\,\mathrm{sin}\left(\theta_1 \right),\mathrm{ny}\,\mathrm{cos}\left(\theta_1 \right)-\mathrm{nx}\,\mathrm{sin}\left(\theta_1 \right))
θ6=atan2(−61T(2,2),61T(2,1))=atan2(−oycos(θ1)+oxsin(θ1),nycos(θ1)−nxsin(θ1))
3.求 θ 5 \theta_5 θ5
θ 5 = a r c c o s ( − a z ) \theta_5=arccos(-az) θ5=arccos(−az)
4.求 θ 3 \theta_3 θ3
4
1
T
(
1
,
4
)
=
a
3
c
o
s
(
θ
2
+
θ
3
)
+
a
2
c
o
s
(
θ
2
)
^1_4T(1,4)=a_3 \,\mathrm{cos}\left(\theta_2 +\theta_3 \right)+a_2 \,\mathrm{cos}\left(\theta_2 \right)
41T(1,4)=a3cos(θ2+θ3)+a2cos(θ2)
4
1
T
(
3
,
4
)
=
a
3
s
i
n
(
θ
2
+
θ
3
)
+
a
2
s
i
n
(
θ
2
)
^1_4T(3,4)=a_3 \,\mathrm{sin}\left(\theta_2 +\theta_3 \right)+a_2 \,\mathrm{sin}\left(\theta_2 \right)
41T(3,4)=a3sin(θ2+θ3)+a2sin(θ2)
4
1
T
(
1
,
4
)
2
+
4
1
T
(
3
,
4
)
2
=
a
2
2
+
2
∗
c
o
s
(
θ
3
)
∗
a
2
∗
a
3
+
a
3
2
=
(
d
1
−
p
z
+
a
z
∗
d
6
+
d
5
∗
o
z
∗
c
o
s
(
θ
6
)
+
d
5
∗
n
z
∗
s
i
n
(
θ
6
)
)
2
+
(
a
y
∗
d
6
∗
s
i
n
(
θ
1
)
−
p
y
∗
s
i
n
(
θ
1
)
−
p
x
∗
c
o
s
(
θ
1
)
+
a
x
∗
d
6
∗
c
o
s
(
θ
1
)
+
d
5
∗
n
y
∗
s
i
n
(
θ
1
)
∗
s
i
n
(
θ
6
)
+
d
5
∗
o
x
∗
c
o
s
(
θ
1
)
∗
c
o
s
(
θ
6
)
+
d
5
∗
n
x
∗
c
o
s
(
θ
1
)
∗
s
i
n
(
θ
6
)
+
d
5
∗
o
y
∗
c
o
s
(
θ
6
)
∗
s
i
n
(
θ
1
)
)
2
^1_4T(1,4)^2+^1_4T(3,4)^2=a2^2 + 2*cos(\theta_3)*a2*a3 + a3^2\\=(d1 - pz + az*d6 + d5*oz*cos(\theta_6) + d5*nz*sin(\theta_6))^2 \\+ (ay*d6*sin(\theta_1) - py*sin(\theta_1) - px*cos(\theta_1) + ax*d6*cos(\theta_1) + d5*ny*sin(\theta_1)*sin(\theta_6) + d5*ox*cos(\theta_1)*cos(\theta_6) + d5*nx*cos(\theta_1)*sin(\theta_6) + d5*oy*cos(\theta_6)*sin(\theta_1))^2
41T(1,4)2+41T(3,4)2=a22+2∗cos(θ3)∗a2∗a3+a32=(d1−pz+az∗d6+d5∗oz∗cos(θ6)+d5∗nz∗sin(θ6))2+(ay∗d6∗sin(θ1)−py∗sin(θ1)−px∗cos(θ1)+ax∗d6∗cos(θ1)+d5∗ny∗sin(θ1)∗sin(θ6)+d5∗ox∗cos(θ1)∗cos(θ6)+d5∗nx∗cos(θ1)∗sin(θ6)+d5∗oy∗cos(θ6)∗sin(θ1))2
5. 求 θ 2 \theta_2 θ2
把求得的 θ 3 \theta_3 θ3带入 4 1 T ( 1 , 4 ) = a 3 c o s ( θ 2 + θ 3 ) + a 2 c o s ( θ 2 ) = a 2 ∗ c o s ( θ 2 ) + a 3 ∗ c o s ( θ 2 ) ∗ c o s ( θ 3 ) − a 3 ∗ s i n ( θ 2 ) ∗ s i n ( θ 3 ) ^1_4T(1,4)=a_3 \,\mathrm{cos}\left(\theta_2 +\theta_3 \right)+a_2 \,\mathrm{cos}\left(\theta_2 \right)=a2*cos(\theta_2) + a3*cos(\theta_2)*cos(\theta_3) - a3*sin(\theta_2)*sin(\theta_3) 41T(1,4)=a3cos(θ2+θ3)+a2cos(θ2)=a2∗cos(θ2)+a3∗cos(θ2)∗cos(θ3)−a3∗sin(θ2)∗sin(θ3)，得到关于 θ 2 \theta_2 θ2的式子进行求解
6.求 θ 4 \theta_4 θ4
θ
2
+
θ
3
+
θ
4
=
a
t
a
n
2
(
6
1
T
(
3
,
3
)
,
6
1
T
(
1
,
3
)
)
=
a
t
a
n
2
(
a
z
,
a
x
sin
(
θ
1
)
+
a
y
sin
(
θ
1
)
)
\theta_2+\theta_3+\theta_4=atan2(^1_6T(3,3),^1_6T(1,3))\\=atan2(az,ax\sin(\theta_1)+ay\sin(\theta_1))
θ2+θ3+θ4=atan2(61T(3,3),61T(1,3))=atan2(az,axsin(θ1)+aysin(θ1))
将
θ
2
\theta_2
θ2和
θ
3
\theta_3
θ3带入求解。
扫一扫
2万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
