【数学问题】俯仰角

一、数学建模

假设俯仰角为 $\lambda$,对于坐标系A足端位置 $P_A[x, y]$，我们求出其在B坐标系下的位置 $P_B=[x',y']$，然后通过逆运动学求解关节角度 $\theta_1,\theta_2$即可。

假定机器人身长为L，腿长分别为L1， L2

1、坐标变换

前腿

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

2、逆运动学求解

$\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(x,y) - atan2(k_2,k_1)= atan2(x,y) - atan2( l_2s_2 ,-l_1 - l2c_2)$

最终我们就能得到两个关节角度

---

如果觉得ok，点个赞，点个关注，也欢迎给个打赏支持一下编者的工作