四足机器人单腿逆运动学几何计算

目标：由给定腿部几何参数和计算出的x/y/z坐标求取关节转动的角度，得到关节转角才能计算电机需要的转角。

三关节腿部概念图

如下图是单腿零位示意图，设电机顺时针转动，角度为正，逆时针转动，角度为负。

看一下一点简单的转动。

几何计算

现给一个又一定x/y/z坐标的三关节腿。

图1

$$\tan {\alpha }_1=\frac{y}{z}$$
$$\tan ({\theta }_0+{\alpha }_2)=\frac{L_{yz}}{h}$$
变换一下得到θ0.
$${\theta }_0=\arctan \frac{L_{yz}}{h}-(\frac{\pi }{2}-\arctan \frac{y}{z})$$
其中，Lyz是 大腿根部 到 足端投影到yoz平面点 的长度，后面转换成大腿小腿二维平面时计算。

本来在代码中我在只有大腿小腿的二维平面中，计算(x’, z’)来求θ1和θ2，在三维坐标下不能再使用这个z’

图2

图3

从原点到足端，有一个三棱柱的几何关系图，后面会用到

图4

图5

$$x^2+y^2+z^2=d^2$$
$$y^2+z^2-h^2=z^{\prime 2}$$
$$z^{\prime 2}+x^2=L^2$$
这样就求得大腿根部到足端的距离L(其实看髋部垂直大腿小腿平面就得到这个勾股式子)：
$$L=\sqrt{x^2+y^2+z^2-h^2}$$
再回到上面的式子，就可以求Lyz了：
$$L_{yz}=\sqrt{L^2-x^2}$$
有了Lyz就可以从上面的计算得到θ0。接下来利用余弦定理求θ1和θ2：
$$\begin{gathered} cos(\frac{\pi }{2}-{\theta }_2)=\frac{L_1^2+L_2^2-L^2}{2L_1{L}_2} \\ {\theta }_2=\pi -arccos\frac{L_1^2+L_2^2-L^2}{2L_1{L}_2} \\ {\theta }_1=arccos\frac{L_1^2+L^2-L_2^2}{2L_{1}L}+arctan\frac{x}{L_{yz}} \end{gathered}$$