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

一、运动学

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

1、几何建模

我们将位置点P摆到第一象限，以便符合我们的直觉：

2、运动学正解

如果已知 $θ_{1}, θ_{2}$ ，可以通过下式求P[x,y]位置： $x = L_{1} sin θ_{1} + L_{2} sin (θ_{1} + θ_{2})$ $y = L_{1} cos θ_{1} + L_{2} c o s (θ_{1} + θ_{2})$ 如果不明白上面两条公式如何来的，画一条辅助线就能够明白了，如下图：

3、逆解

已知P[x,y]位置，求 $θ_{1}, θ_{2}$ , 我们用代数的方法求逆解 首先两边平方相加：
【关于四足机器人那些事】腿部运动学代数求解方法将表达式展开，并写成更简洁的形式,其中 $c o s θ_{1} = c 1, c o s θ_{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

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

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

$sin θ_{2}$ 有两个取值，会对 $θ_{1}$ 的值产生影响，在我们当前使用是关节配置方式， $s$ $in θ_{2}$ 取正值, 即

接下来我们通过正解来求 $θ_{1}$ ，先对公式进行以下变换： $x = k_{1} s_{1} + k_{2} c_{1}$ $y = k_{1} c_{1} - k_{2} s_{1}$ 其中 $\begin{matrix} k_{1} = & l_{1} + l_{2} c_{2} \\ k_{2} = & l_{2} s_{2} \end{matrix}$ k2=l2s2 再对 $k_{1}, k_{2}$ 进行变量替换： $k_{1} = r cos γ$ $k_{2} = r sin γ$ 其中
同样，我们画个图方便理解， $k_{1}, k_{2}$ 相当于坐标轴上的点P， $γ$ 是与轴的夹角。

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

最终化简成：

同样使用atan2函数计算得到：

γ + α = a t a n 2 (y, x)

最终:

整理一些写成可计算的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)