六足机器人腿部逆运动解算
这里我们使用几何法进行逆运动解算, L 1 , L 2 , L 3 L_1,L_2,L_3 L1,L2,L3分别为连杆1,2,3的长度
由三角关系,易得:
θ
1
=
a
t
a
n
(
P
y
/
P
x
)
R
=
P
y
2
+
P
x
2
\theta_1 = atan(Py/Px)\\ R = \sqrt{Py^2+Px^2}
θ1=atan(Py/Px)R=Py2+Px2
这里借由
α
R
和
α
1
\alpha_R和\alpha_1
αR和α1求得第二个关节角
θ
2
\theta_2
θ2:
α
R
=
a
t
a
n
(
−
z
R
−
L
1
)
α
1
=
a
c
o
s
(
(
L
2
)
2
+
(
L
r
)
2
−
(
L
3
)
2
2
L
r
L
2
)
(
余弦定理
)
θ
2
=
α
1
−
α
R
\alpha_R = atan(\frac{-z}{R-L_1}) \\ \alpha_1 = acos(\frac{(L_2)^2+(L_r)^2-(L_3)^2}{2L_rL_2}) \qquad(余弦定理)\\ \theta_2 = \alpha_1 - \alpha_R
αR=atan(R−L1−z)α1=acos(2LrL2(L2)2+(Lr)2−(L3)2)(余弦定理)θ2=α1−αR
最后借助
α
2
\alpha_2
α2求得第三个关节角
θ
3
\theta_3
θ3:
α
2
=
a
c
o
s
(
(
L
r
)
2
+
(
L
3
)
2
−
(
L
2
)
2
2
L
r
L
3
)
θ
3
=
α
1
+
α
2
\alpha_2 = acos(\frac{(L_r)^2+(L_3)^2-(L_2)^2}{2L_rL_3})\\ \theta_3 = \alpha_1+\alpha_2
α2=acos(2LrL3(Lr)2+(L3)2−(L2)2)θ3=α1+α2