【關於四足機器人那些事】腿部運動學代數求解方法

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

一、運動學

不考慮橫向髖關節運動時，四足機器人的腿部可以簡化成二連桿機構

1、幾何建模

我們將位置點P擺到第一象限，以便符合我們的直覺：

2、運動學正解

如果已知$\theta_1, \theta_2$，可以通過下式求P[x,y]位置：

$x = L_1\sin\theta_1 + L_2\sin(\theta_1 + \theta_2 )$

$y = L_1\cos\theta_1+ L_2cos(\theta_1 + \theta_2)$

如果不明白上面兩條公式如何來的，畫一條輔助線就能夠明白了，如下圖：

3、逆解

已知P[x,y]位置，求$\theta_1, \theta_2$, 我們用代數的方法求逆解

首先兩邊平方相加：

$x^2 + y^2 = (l_1sin(\theta_1) + l_2sin(\theta_1 :+ \theta_2))^2 + (l_1cos(\theta_1) + l_2cos(\theta_1 - \theta_2))^2$

將表達式展開，並寫成更簡潔的形式,其中$cos\theta_1=c1, cos\theta_2=c2$以此類推：

$\begin{matrix} x^2 + y^2 =& l_1^2s_1^2 + 2l_1l_2s_1s_{12} + l_1^2c_1 + 2l_1l_2c_1c_{12} + l_2^2s_{12}^2 + l_2^2c_{12}^2\\ \\ x^2 + y^2 =& l_1^2 + l_2^2 + 2l_1l_2(s_1s_{12} +c_1c_{12}) \end{matrix}$

根據和差公式：
$s_1s_{12} = s_1^2 c_2 - s_1c_1 s_2$

$c_1c_{12} = c_1^2 c_2 + s_1c_1 s_2$

最終得到：

$x^2 + y^2=l_1^2 + l_2^2 + 2l_1l_2c_2$

或者直接利用python對錶達式進行化簡

L1 = symbols('L1')
L2 = symbols('L2')
b = symbols('b')
a = symbols('a')

x = - L1 * sin(a) - L2 * sin(a - b)
y = - L1 * cos(a) - L2 * cos(a - b)

temp = x**2 + y ** 2
result = simplify(temp)
print('x**2 + y ** 2 = ', result)

我們能夠得到同樣結果

x**2 + y ** 2 = L1**2 + 2*L1*L2*cos(b) + L2**2

求得
$\begin{matrix} \cos\theta_2 =& \frac{- l_{1}^{2} - l_{2}^{2} + x^{2} + y^{2}}{2 l_{1} l_{2}}\\ \\ \sin\theta_2 =& \pm\sqrt{1-\cos^2\theta_2} \end{matrix}$

這裏在求解時需要注意x，y值，驗證其是否超過腿部動作空間。

$\sin\theta_2$有兩個取值，會對$\theta_1$的值產生影響，在我們當前使用是關節配置方式，$\sin\theta_2$取正值, 即$\sin\theta_2 = \sqrt{1-\cos^2\theta_2}$

接下來我們通過正解來求$\theta_1$，先對公式進行以下變換：

$x = k_1s_1 + k_2c_1$

$y = k_1c_1 - k_2s_1$

其中
$\begin{matrix} k_1 =& l_1 + l_2c_2\\ \\ k_2 =& l_2s_2 \end{matrix}$

再對$k_1,k_2$進行變量替換：

$k_1 = r\cos\gamma$

$k_2 = r\sin\gamma$

其中$r=\sqrt{k_1^2 + k_2^2},\gamma = atan2(k_2,k_1)$，同樣，我們畫個圖方便理解，$k_1,k_2$相當於座標軸上的點P，$\gamma$是$r$與$x$軸的夾角。

代入正解方程：
$\frac{x}{r}=\cos\gamma\sin\theta_1-\sin\gamma\cos\alpha$

$\frac{y}{r}=\cos\gamma\cos\theta_1 +\sin\gamma\sin\theta_1$

最終化簡成：

$\sin(\gamma + \theta_1) = \frac{x}{r}$

$\cos(\gamma + \theta_1) = \frac{y}{r}$

同樣使用atan2函數計算得到：$\gamma + \alpha = atan2(y, x)$

最終:
$\theta_1 = atan2(y,x) - atan2(k_2,k_1)= atan2(y,x) - atan2( l_2s_2 ,l_1 + l2c_2)$

整理一些寫成可計算的python程序如下：

c2 = (-L1**2 - L2**2 + x**2 + y**2)/(2*L1*L2)
s2 = np.sqrt(1-c2**2)
# 根據關節配置方式我們取beta爲正
theta_2 = np.arctan2(s2, c2)
theta_1 = np.arctan2(y, x) - np.arctan2(L2*s2, L1+L2*c2)

注意

以上求出的$\theta_1$是與x軸正半軸的夾角，需要根據機器人的腿部初始角度進行一定變換，例如圖中這樣腿部初始角度是與地面垂直的，$\theta_1 = \theta_1 + 90°$，$\theta_1$是與y軸負半軸的夾角，如最開始的那張圖所示

---

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