四足机器人单腿逆运动学几何计算
目标：由给定腿部几何参数和计算出的x/y/z坐标求取关节转动的角度，得到关节转角才能计算电机需要的转角。
三关节腿部概念图
如下图是单腿零位示意图，设电机顺时针转动，角度为正，逆时针转动，角度为负。
看一下一点简单的转动。
几何计算
现给一个又一定x/y/z坐标的三关节腿。
tan
α
1
=
y
z
\tan\alpha_1=\frac yz
tanα1=zy
tan
(
θ
0
+
α
2
)
=
L
y
z
h
\tan(\theta_0+\alpha_2)=\frac {L_{yz}}h
tan(θ0+α2)=hLyz
变换一下得到θ0.
θ
0
=
arctan
L
y
z
h
−
(
π
2
−
arctan
y
z
)
\theta_0=\arctan\frac {L_{yz}}h-(\frac\pi2-\arctan\frac yz)
θ0=arctanhLyz−(2π−arctanzy)
其中，Lyz是 大腿根部 到 足端投影到yoz平面点 的长度，后面转换成大腿小腿二维平面时计算。
本来在代码中我在只有大腿小腿的二维平面中，计算(x’, z’)来求θ1和θ2，在三维坐标下不能再使用这个z’
从原点到足端，有一个三棱柱的几何关系图，后面会用到
x
2
+
y
2
+
z
2
=
d
2
x^2+y^2+z^2=d^2
x2+y2+z2=d2
y
2
+
z
2
−
h
2
=
z
′
2
y^2+z^2-h^2=z'^2
y2+z2−h2=z′2
z
′
2
+
x
2
=
L
2
z'^2+x^2=L^2
z′2+x2=L2
这样就求得大腿根部到足端的距离L(其实看髋部垂直大腿小腿平面就得到这个勾股式子)：
L
=
x
2
+
y
2
+
z
2
−
h
2
L=\sqrt{x^2+y^2+z^2-h^2}
L=x2+y2+z2−h2
再回到上面的式子，就可以求Lyz了：
L
y
z
=
L
2
−
x
2
L_{yz}=\sqrt{L^2-x^2}
Lyz=L2−x2
有了Lyz就可以从上面的计算得到θ0。接下来利用余弦定理求θ1和θ2：
c
o
s
(
π
2
−
θ
2
)
=
L
1
2
+
L
2
2
−
L
2
2
L
1
L
2
θ
2
=
π
−
a
r
c
c
o
s
L
1
2
+
L
2
2
−
L
2
2
L
1
L
2
θ
1
=
a
r
c
c
o
s
L
1
2
+
L
2
−
L
2
2
2
L
1
L
+
a
r
c
t
a
n
x
L
y
z
cos({\frac π2} -θ_2)={\frac {L_1^2+L_2^2-L^2}{2L_1L_2}} \\ θ_2=π -arccos{\frac {L_1^2+L_2^2-L^2}{2L_1L_2}} \\ θ_1=arccos{\frac {L_1^2+L^2-L_2^2}{2L_1L}}+arctan{\frac x{L_{yz}}}
cos(2π−θ2)=2L1L2L12+L22−L2θ2=π−arccos2L1L2L12+L22−L2θ1=arccos2L1LL12+L2−L22+arctanLyzx