四足机器人——3自由度机械臂正逆解(肘式+膝式)
项目介绍
最近在做一个12自由度四足机器人的项目,每条腿3个自由度。涉及到足端位姿关节逆解的问题,于是进行了简单的推导,并且编写了Matlab仿真程序,有需要的童鞋可以点击下载。
四足机器人腿步有肘式和膝式之分,很好理解,顾名思义。因为想要探索一下前肘后膝四足机器人和双肘式、双膝式的不同,因此推导了两种腿部关节的逆解。为了简单起见,没有用到机器人学复杂的坐标转换,只需要基础的初高中几何知识就可以理解。
说明:以下推导x,y,z值取正方向为正值,反方向为负值;角度值逆时针为正值,顺时针为负值。
O、A、B、C 表示点,a、b、c、l 表示边长,q1、q2、q3表示角度。
肘式
正解推导(已知关节角度求坐标)
A
x
=
0
Ax=0
Ax=0
A
y
=
a
∗
s
i
n
(
q
3
)
Ay = a*sin(q3)
Ay=a∗sin(q3)
A
z
=
−
a
∗
c
o
s
(
q
3
)
Az = -a*cos(q3)
Az=−a∗cos(q3)
B
x
=
−
b
∗
s
i
n
(
−
q
2
)
Bx = -b*sin(-q2)
Bx=−b∗sin(−q2)
B
y
=
[
b
∗
c
o
s
(
−
q
2
)
+
a
]
∗
s
i
n
(
q
3
)
By=[b*cos(-q2)+a]*sin(q3)
By=[b∗cos(−q2)+a]∗sin(q3)
B
z
=
−
[
b
∗
c
o
s
(
−
q
2
)
+
a
]
∗
c
o
s
(
q
3
)
Bz=-[b*cos(-q2)+a]*cos(q3)
Bz=−[b∗cos(−q2)+a]∗cos(q3)
C
x
=
−
b
∗
s
i
n
(
−
q
2
)
+
c
∗
c
o
s
(
−
q
2
−
q
1
)
Cx=-b*sin(-q2)+c*cos(-q2-q1)
Cx=−b∗sin(−q2)+c∗cos(−q2−q1)
C
y
=
[
b
∗
c
o
s
(
−
q
2
)
+
a
+
c
∗
s
i
n
(
−
q
2
−
q
1
)
]
∗
s
i
n
(
q
3
)
Cy=[b*cos(-q2)+a+c*sin(-q2-q1)]*sin(q3)
Cy=[b∗cos(−q2)+a+c∗sin(−q2−q1)]∗sin(q3)
C
z
=
−
[
b
∗
c
o
s
(
−
q
2
)
+
a
+
c
∗
s
i
n
(
−
q
2
−
q
1
)
]
∗
c
o
s
(
q
3
)
Cz=-[b*cos(-q2)+a+c*sin(-q2-q1)]*cos(q3)
Cz=−[b∗cos(−q2)+a+c∗sin(−q2−q1)]∗cos(q3)
逆解推导(已知坐标求关节角度)
q
3
=
a
r
c
t
a
n
(
−
y
/
z
)
q3=arctan(-y/z)
q3=arctan(−y/z)
l
=
(
x
2
+
(
y
−
a
∗
s
i
n
(
q
3
)
)
2
+
(
z
+
a
∗
c
o
s
(
q
3
)
)
2
)
l=\sqrt{(x^2+(y-a*sin(q3))^2+(z+a*cos(q3))^2)}
l=(x2+(y−a∗sin(q3))2+(z+a∗cos(q3))2)
q
2
=
−
a
r
c
c
o
s
(
(
b
2
+
l
2
−
c
2
)
/
(
2
∗
b
∗
l
)
)
+
a
r
c
s
i
n
(
x
/
l
)
q2=-arccos((b^2+l^2-c^2)/(2*b*l))+arcsin(x/l)
q2=−arccos((b2+l2−c2)/(2∗b∗l))+arcsin(x/l)
q
1
=
p
i
/
2
−
a
r
c
c
o
s
(
(
b
2
+
c
2
−
l
2
)
/
(
2
∗
b
∗
c
)
)
q1=pi/2-arccos((b^2+c^2-l^2)/(2*b*c))
q1=pi/2−arccos((b2+c2−l2)/(2∗b∗c))
膝式
正解推导(已知关节角度求坐标)
A
x
=
0
Ax=0
Ax=0
A
y
=
a
∗
s
i
n
(
q
3
)
Ay = a*sin(q3)
Ay=a∗sin(q3)
A
z
=
−
a
∗
c
o
s
(
q
3
)
Az = -a*cos(q3)
Az=−a∗cos(q3)
B
x
=
b
∗
s
i
n
(
q
2
)
Bx = b*sin(q2)
Bx=b∗sin(q2)
B
y
=
[
b
∗
c
o
s
(
q
2
)
+
a
]
∗
s
i
n
(
q
3
)
By=[b*cos(q2)+a]*sin(q3)
By=[b∗cos(q2)+a]∗sin(q3)
B
z
=
−
[
b
∗
c
o
s
(
q
2
)
+
a
]
∗
c
o
s
(
q
3
)
Bz=-[b*cos(q2)+a]*cos(q3)
Bz=−[b∗cos(q2)+a]∗cos(q3)
C
x
=
b
∗
s
i
n
(
q
2
)
−
c
∗
c
o
s
(
q
2
+
q
1
)
Cx=b*sin(q2)-c*cos(q2+q1)
Cx=b∗sin(q2)−c∗cos(q2+q1)
C
y
=
[
b
∗
c
o
s
(
q
2
)
+
a
+
c
∗
s
i
n
(
q
2
+
q
1
)
]
∗
s
i
n
(
q
3
)
Cy=[b*cos(q2)+a+c*sin(q2+q1)]*sin(q3)
Cy=[b∗cos(q2)+a+c∗sin(q2+q1)]∗sin(q3)
C
z
=
−
[
b
∗
c
o
s
(
q
2
)
+
a
+
c
∗
s
i
n
(
q
2
+
q
1
)
]
∗
c
o
s
(
q
3
)
Cz=-[b*cos(q2)+a+c*sin(q2+q1)]*cos(q3)
Cz=−[b∗cos(q2)+a+c∗sin(q2+q1)]∗cos(q3)
逆解推导(已知坐标求关节角度)
q
3
=
a
r
c
t
a
n
(
−
y
/
z
)
q3=arctan(-y/z)
q3=arctan(−y/z)
l
=
(
x
2
+
(
y
−
a
∗
s
i
n
(
q
3
)
)
2
+
(
z
+
a
∗
c
o
s
(
q
3
)
)
2
)
l=\sqrt{(x^2+(y-a*sin(q3))^2+(z+a*cos(q3))^2)}
l=(x2+(y−a∗sin(q3))2+(z+a∗cos(q3))2)
q
2
=
a
r
c
c
o
s
(
(
b
2
+
l
2
−
c
2
)
/
(
2
∗
b
∗
l
)
)
−
a
r
c
s
i
n
(
−
x
/
l
)
q2=arccos((b^2+l^2-c^2)/(2*b*l))-arcsin(-x/l)
q2=arccos((b2+l2−c2)/(2∗b∗l))−arcsin(−x/l)
q
1
=
a
r
c
c
o
s
(
(
b
2
+
c
2
−
l
2
)
/
(
2
∗
b
∗
c
)
)
−
p
i
/
2
q1=arccos((b^2+c^2-l^2)/(2*b*c))-pi/2
q1=arccos((b2+c2−l2)/(2∗b∗c))−pi/2