【關於四足機器人那些事】腿部運動學建模（三維）

本篇將會三維空間中，對四足機器人的腿部進行數學建模，求解器正逆運動學解，包含詳細公式推導與計算

首先，我們來看三維空間中簡圖：

其中a表示髖關節距離主體得偏移，L1，L2共面，與a保持垂直關係，[x,y,z]表示足端相對於髖關節的位置，爲了更容易大家理解，我們將視角轉換到不同的視圖當中去計算幾何關係。

一、正視圖：

首先來回一下小學知識：

• 互餘：如果兩個銳角的和等於90度,那麼這兩個銳角互爲餘角

通過互餘我們可以證明a與y軸的夾角與圖中所示$\theta_0$相等。通過其數量關係，我們可以得出：

1、正解（已知$\theta_0$）

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

$y = a\cos(\theta_0) + H\sin(\theta_0) \tag{1-2}$
此時，我們只要求出$H$，即可得到$z$與$y$的值

2、逆解（已知x，y，z）

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

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

在已知xyz的情況下，通過勾股定理我們能夠輕易求出$H$，在正視圖我們需要的公式就這麼多，接下來轉到右視圖

右視圖

雖說是右視圖，但我們最好不要將其想象成水平向右觀看的視圖，這裏的右視圖指的是L1，L2所在平面的視圖。這裏其實跟二維情況下無疑。H的長度相當於原本的Y軸上的距離。這裏就不再重新推導，詳情請查看文章

1、正解（已知$\theta_1,\theta_2$）

此時$H$是在L1，L2組成的平面當中，如果空間感比較好的小夥伴應該會發現這時候其實跟二維情況下的幾何模型是一致的。因此我們有以下關係：
$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、逆解（已知xyz）

$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)$

整理

我們將所有公式整理一下：

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}$

$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$

$\theta_0 = atan2(H, |z|) - atan2(|z|, y)$

$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)$

同樣，需要根據機器人各個關節的初始角度對$\theta_0, \theta_1, \theta_2$進行一定相加減

---

如果覺得ok，點個贊，點個關注，也歡迎給個打賞支持一下編者的工作