【關於四足機器人那些事】運動學建模及其基於運動學的姿態調節（彙總）

一、效果演示

〔四足機器人〕姿態調節

[四足機器人]動態調整仿真測試

二、平面運動學建模

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

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

1、幾何建模

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

2、運動學正解

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

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

$y = L_1\sin\theta_1 + L_2\sin(\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{aligned} &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{aligned}$

根據和差公式：
$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)

4、注意

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

三、3維運動學建模

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

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

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

1、正視圖：

首先來回一下小學知識：

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

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

1.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$的值

1.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$，在正視圖我們需要的公式就這麼多，接下來轉到右視圖

2、右視圖

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

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

3、整理

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

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

3.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$進行一定相加減

四、俯仰角

通過一到三章，我們已經建立了四足機器人的運動學模型，下面將會對姿態調節中的俯仰角進行數學建模，求解其正逆運動學解，包含詳細公式推導與計算

1、幾何模型

假設俯仰角爲$\lambda$,對於座標系A足端位置$P_A[x, y]$，我們求出其在B座標系下的位置$P_B=[x',y']$，然後通過逆運動學求解關節角度$\theta_1,\theta_2$即可。假定機器人半身長爲L，腿長分別爲L1， L2

2、座標變換

前腿

a、座標系A-O變換矩陣：
$T_1 = \begin{bmatrix} 1 & 0 & L\\ 0 & 1 & 0\\ 0 & 0 & 1 \end{bmatrix}$

b、座標系O-B變換矩陣：
$T_2 = \begin{bmatrix} \cos\lambda & -\sin\lambda & -L\\ \sin\lambda & \cos\lambda & 0\\ 0 & 0 & 1 \end{bmatrix}$

根據變換矩陣，我們可以求出$P_B$座標：
$P_B = T_2T_1P_A$

即：

$\begin{bmatrix}x'\\ y'\\1 \end{bmatrix} = \begin{bmatrix} \cos\lambda & -\sin\lambda & -L\\ \sin\lambda & \cos\lambda & 0\\ 0 & 0 & 1 \end{bmatrix}\begin{bmatrix} 1 & 0 & L\\ 0 & 1 & 0\\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} x\\ y \\1 \end{bmatrix}$

化簡之後：

$\begin{bmatrix}x'\\ y'\\1 \end{bmatrix} =\left[\begin{matrix}\cos\lambda & - \sin\lambda & L \cos\lambda - L\\\sin\lambda & \cos\lambda & L \sin\lambda \\0 & 0 & 1\end{matrix}\right]\begin{bmatrix} x\\ y \\1 \end{bmatrix}$

後腿

對於後腿實際上只是L取反即可：
$\begin{bmatrix}x'\\ y'\\1 \end{bmatrix} =\left[\begin{matrix}\cos\lambda & - \sin\lambda & -L \cos\lambda +L\\\sin\lambda & \cos\lambda & -L \sin\lambda \\0 & 0 & 1\end{matrix}\right]\begin{bmatrix} x\\ y \\1 \end{bmatrix}$

3、逆運動學求解

根據離地高度確定初始位置$P0$，再根據變換矩陣將其轉換致參考系B中得到$P_B$，通過逆運動學求解兩個關節角度，公式如下（推導可回看前三章）：

$\begin{matrix} \cos\beta = \frac{- l_{1}^{2} - l_{2}^{2} + x^{2} + y^{2}}{2 l_{1} l_{2}} \\ \\ \sin\beta = \sqrt{1-\cos^2\beta} \\ \\ \beta = atan2(sin\beta, cos\beta) \\ \\ \alpha = atan2(y,x) - atan2( l_2s_2 ,l_1 + l2c_2)\end{matrix}$

4、最大俯仰角

對於給定離地高度h，俯仰角存在一個最大值，在實際控制時，我們需要保證俯仰角在允許範圍內。

如上圖，當L1， L1共線時，俯仰角最大，此時有以下關係：

$\cos(r+\varphi) = \frac{L^2+(h^2+L^2) - (L1+L2)^2}{2L(\sqrt{L^2+h^2})}$

其中$\varphi = atan2(h, L)$，因此我們可以解出此時的$r$

五、滾轉角

5.1 幾何模型

從正面觀看我們的四足機器人時，可以簡化成以下幾何圖形，接下來我們就根據該模型來分析四足機器人的滾轉角調節

我們設定符號：

• 機身寬度$W$，
• 髖關節偏移$a$，
• 滾轉角$R$
• 腿長$L1，L2$
• $L_{12}$爲髖關節點$P_H$到足端$P_E$的距離，是個變量

根據結構，我們可以推測出以下固有的幾何關係：

• L1，L2共面
• 且與$a$垂直

5.2、座標變換

已知$P_E$相對於{H}的位置爲$[x, y, z]^T$。我們同樣通過座標變換的思路來求解$P_E$相對於{H’}的位置

a、$P_E$相對於機身質心位置$O$的位置可以通過以下變換矩陣得到：
$T_1 = \left[\begin{matrix}1 & 0 & 0 & 0\\0 & 1 & 0 & a\\0 & 0 & 1 & 0\\0 & 0 & 0 & 1\end{matrix}\right]$

b、{O}座標系下的位置轉換到{H’}下的位置：

$T_2 = \left[\begin{matrix}1 & 0 & 0 & 0\\0 & \cos{\left(R \right)} & - \sin{\left(R \right)} & - a\\0 & \sin{\left(R \right)} & \cos{\left(R \right)} & 0\\0 & 0 & 0 & 1\end{matrix}\right]$

因此我們有以下公式：

$P'_E = T_2T_1P_E$

化簡後即：

$\begin{bmatrix} x'\\ y'\\ z'\\ 1 \end{bmatrix} = \left[\begin{matrix}1 & 0 & 0 & 0\\0 & \cos{\left(R \right)} & - \sin{\left(R \right)} & a \cos{\left(R \right)} - a\\0 & \sin{\left(R \right)} & \cos{\left(R \right)} & a \sin{\left(R \right)}\\0 & 0 & 0 & 1\end{matrix}\right]\begin{bmatrix} x\\ y\\ z\\ 1 \end{bmatrix}$

得到變換後的座標點$P'_E$即可代進逆運動學模型（第三章）中求解出關節角度即可

六、偏航角

6.1、幾何模型

從正上方觀看我們的四足機器人時，可以簡化成以下幾何圖形，接下來我們就根據該模型來分析四足機器人的偏航調節以右前腿爲例

我們設定符號：

機身寬度$W$，
機身長度$L$
偏航角$Y$
​

6.2、座標變換

假設A點爲初始狀態足端位置$[x,y,z]^T$，$B$爲機身沿Z軸旋轉Y度後，髖關節位姿，計算A，相對於B的位置，我們可以通過以下變換矩陣求出：

{A}相對於{O}的位置：

$T_1 = \left[\begin{matrix}1 & 0 & 0 & L\\0 & 1 & 0 & W\\0 & 0 & 1 & 0\\0 & 0 & 0 & 1\end{matrix}\right]$

{O}相對於{B}的位置：
$T_2 = \left[\begin{matrix}\cos{\left(Y \right)} & - \sin{\left(Y \right)} & 0 & - L\\\sin{\left(Y \right)} & \cos{\left(Y \right)} & 0 & - W\\0 & 0 & 1 & 0\\0 & 0 & 0 & 1\end{matrix}\right]$

因此{A}相對於{O}的位置可以用下式表示：

$P_B = T_2T_1P_A$

即，

$\begin{bmatrix}x'\\ y'\\ z'\\ 1' \\ \end{bmatrix} = \left[\begin{matrix}\cos{\left(Y \right)} & - \sin{\left(Y \right)} & 0 & L \cos{\left(Y \right)} - L - W \sin{\left(Y \right)}\\\sin{\left(Y \right)} & \cos{\left(Y \right)} & 0 & L \sin{\left(Y \right)} + W \cos{\left(Y \right)} - W\\0 & 0 & 1 & 0\\0 & 0 & 0 & 1\end{matrix}\right] \begin{bmatrix}x\\ y\\ z\\ 1 \\ \end{bmatrix}$

注意，我們這裏求的右前腿的變換，其餘不同位置的腿需要適時對$W,L$取反。

最後將{P_B}點位置代入逆運動學方程即可求解關節角度。

七、姿態調節矩陣

四到六章分別介紹了繞不同軸的姿態調節,爲了方便描述，我們分別用$\Omega =[\alpha,\beta,\gamma]$表示機身旋轉角，其中

• 俯仰角（$\beta$）變換

$R(\beta) = \left[\begin{matrix}\cos\beta & - \sin\beta & L \cos\beta - L\\\sin\beta & \cos\beta & L \sin\beta \\0 & 0 & 1\end{matrix}\right]$

• 滾轉角($\alpha$)變換
  $R(\alpha) = \left[\begin{matrix}1 & 0 & 0 & 0\\0 & \cos{\left(\alpha\right)} & - \sin{\left(\alpha \right)} & l_0 \cos{\left(\alpha \right)} - l_0\\0 & \sin{\left(\alpha \right)} & \cos{\left(\alpha \right)} & l_0 \sin{\left(\alpha \right)}\\0 & 0 & 0 & 1\end{matrix}\right]$

• 偏航角($\gamma$)調節
  $R(\gamma) = \left[\begin{matrix}\cos{\left(\gamma \right)} & - \sin{\left(\gamma \right)} & 0 & L \cos{\left(\gamma \right)} - L - W \sin{\left(\gamma \right)}\\\sin{\left(Y \right)} & \cos{\left(\gamma \right)} & 0 & L \sin{\left(\gamma \right)} + W \cos{\left(\gamma \right)} - W\\0 & 0 & 1 & 0\\0 & 0 & 0 & 1\end{matrix}\right]$

以roll-pitch-yaw的順序進行旋轉，得到變換矩陣爲

$T = R(\alpha)R(\beta)R(\gamma)$

---

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