【關於四足機器人那些事】姿態調節-俯仰角

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

一、數學建模

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$

---

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