【关于四足机器人那些事】腿部运动学建模（三维）

本篇将会三维空间中，对四足机器人的腿部进行数学建模，求解器正逆运动学解，包含详细公式推导与计算

首先，我们来看三维空间中简图：

其中a表示髋关节距离主体得偏移，L1，L2共面，与a保持垂直关系

正视图：

我们可以得出以下几何关系：

1、正解

$z = -H\cos(\theta_0) + a\sin(\theta_0) \tag{1-1}$

$y = a\cos(\theta_0) + H\sin(\theta_0) \tag{1-2}$

2、逆解

$H^2 = z^2 + y^2 - a^2 \tag{2-1}$

$\alpha = atan2(H, |z|) - atan2(|z|, y) \tag{2-2}$

右视图

虽说是右视图，但我们最好不要将其想象成水平向右观看的视图，这里的右视图指的是L1，L2所在平面的视图。这里其实跟二维情况下无疑。H的长度相当于原本的Y轴上的距离。这里就不再重新推导，详情请查看文章

1、正解

$H = L_1\cos(\theta_1) + L_2\cos(\theta_1+\theta_2) \tag{1-3}$

$x = L_1\sin(\theta_1) + L_2\sin(\theta_1+\theta_2) \tag{1-4}$

2、逆解

$c_2 = \frac{-L_1^2 - L_2 ^2 + x ^2 + H ^2)}{2 L1 L2}$

$s_2 = \sqrt{1-c_2^2}$

$\theta_2 = atan2(s_2, c_2)$

$\theta_1 = atan2(-z_, x) - atan2(L_2 s_2, L_1 + L_2 * c2)$