【关于四足机器人那些事】腿部运动学代数求解方法

本篇将会对四足机器人的腿部进行数学建模，求解器正逆运动学解，包含详细公式推导与计算

一、运动学

不考虑横向髋关节运动时，四足机器人的腿部可以简化成二连杆机构

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_{2}cos({\theta }_1+{\theta }_2)$$

如果不明白上面两条公式如何来的，画一条辅助线就能够明白了，如下图：

3、逆解

已知P[x,y]位置，求${\theta }_1,{\theta }_2$, 我们用代数的方法求逆解

首先两边平方相加：

$$x^2+y^2=(l_{1}sin({\theta }_1)+l_{2}sin({\theta }_1:+{\theta }_2){)}^2+(l_{1}cos({\theta }_1)+l_{2}cos({\theta }_1-{\theta }_2){)}^2$$

将表达式展开，并写成更简洁的形式,其中$cos{\theta }_1=c1,cos{\theta }_2=c2$以此类推：

$$\begin{array}{cc}{x}^2+y^2=& l_1^2{s}_1^2+2l_1{l}_2{s}_1{s}_{12}+l_1^2{c}_1+2l_1{l}_2{c}_1{c}_{12}+l_2^2{s}_{12}^2+l_2^2{c}_{12}^2\\ & \\ x^2+y^2=& l_1^2+l_2^2+2l_1{l}_2(s_1{s}_{12}+c_1{c}_{12})\end{array}$$

根据和差公式：
$$s_1{s}_{12}=s_1^2{c}_2-s_1{c}_1{s}_2$$

$$c_1{c}_{12}=c_1^2{c}_2+s_1{c}_1{s}_2$$

最终得到：

$$x^2+y^2=l_1^2+l_2^2+2l_1{l}_2{c}_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{array}{cc}\cos {\theta }_2=& \frac{-l_1^2-l_2^2+x^2+y^2}{2l_1{l}_2}\\ & \\ \sin {\theta }_2=& \pm \sqrt{1-{\cos }^2{\theta }_2}\end{array}$$

这里在求解时需要注意x，y值，验证其是否超过腿部动作空间。

$\sin {\theta }_2$有两个取值，会对${\theta }_1$的值产生影响，在我们当前使用是关节配置方式，$\sin {\theta }_2$取正值, 即$\sin {\theta }_2=\sqrt{1-{\cos }^2{\theta }_2}$

接下来我们通过正解来求${\theta }_1$，先对公式进行以下变换：

$$x=k_1{s}_1+k_2{c}_1$$

$$y=k_1{c}_1-k_2{s}_1$$

其中
$$\begin{array}{cc}{k}_1=& l_1+l_2{c}_2\\ & \\ k_2=& l_2{s}_2\end{array}$$

再对$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_2{s}_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，点个赞，点个关注，也欢迎给个打赏支持一下编者的工作