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

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

一、运动学

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

1、几何建模

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

2、运动学正解

如果已知 θ 1 , θ 2 \theta_1, \theta_2 θ1,θ2,可以通过下式求P[x,y]位置:

x = L 1 sin ⁡ θ 1 + L 2 sin ⁡ ( θ 1 + θ 2 ) x = L_1\sin\theta_1 + L_2\sin(\theta_1 + \theta_2 ) x=L1sinθ1+L2sin(θ1+θ2)

y = L 1 cos ⁡ θ 1 + L 2 c o s ( θ 1 + θ 2 ) y = L_1\cos\theta_1+ L_2cos(\theta_1 + \theta_2) y=L1cosθ1+L2cos(θ1+θ2)

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

3、逆解

已知P[x,y]位置,求 θ 1 , θ 2 \theta_1, \theta_2 θ1,θ2, 我们用代数的方法求逆解

首先两边平方相加:

x 2 + y 2 = ( l 1 s i n ( θ 1 ) + l 2 s i n ( θ 1 : + θ 2 ) ) 2 + ( l 1 c o s ( θ 1 ) + l 2 c o s ( θ 1 − θ 2 ) ) 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 x2+y2=(l1sin(θ1)+l2sin(θ1:+θ2))2+(l1cos(θ1)+l2cos(θ1θ2))2

将表达式展开,并写成更简洁的形式,其中 c o s θ 1 = c 1 , c o s θ 2 = c 2 cos\theta_1=c1, cos\theta_2=c2 cosθ1=c1,cosθ2=c2以此类推:

x 2 + y 2 = l 1 2 s 1 2 + 2 l 1 l 2 s 1 s 12 + l 1 2 c 1 + 2 l 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 + 2 l 1 l 2 ( s 1 s 12 + c 1 c 12 ) \begin{matrix} 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{matrix} x2+y2=x2+y2=l12s12+2l1l2s1s12+l12c1+2l1l2c1c12+l22s122+l22c122l12+l22+2l1l2(s1s12+c1c12)

根据和差公式:
s 1 s 12 = s 1 2 c 2 − s 1 c 1 s 2 s_1s_{12} = s_1^2 c_2 - s_1c_1 s_2 s1s12=s12c2s1c1s2

c 1 c 12 = c 1 2 c 2 + s 1 c 1 s 2 c_1c_{12} = c_1^2 c_2 + s_1c_1 s_2 c1c12=c12c2+s1c1s2

最终得到:

x 2 + y 2 = l 1 2 + l 2 2 + 2 l 1 l 2 c 2 x^2 + y^2=l_1^2 + l_2^2 + 2l_1l_2c_2 x2+y2=l12+l22+2l1l2c2

或者直接利用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

求得
cos ⁡ θ 2 = − l 1 2 − l 2 2 + x 2 + y 2 2 l 1 l 2 sin ⁡ θ 2 = ± 1 − cos ⁡ 2 θ 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} cosθ2=sinθ2=2l1l2l12l22+x2+y2±1cos2θ2

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

sin ⁡ θ 2 \sin\theta_2 sinθ2有两个取值,会对 θ 1 \theta_1 θ1的值产生影响,在我们当前使用是关节配置方式, sin ⁡ θ 2 \sin\theta_2 sinθ2取正值, 即 sin ⁡ θ 2 = 1 − cos ⁡ 2 θ 2 \sin\theta_2 = \sqrt{1-\cos^2\theta_2} sinθ2=1cos2θ2

接下来我们通过正解来求 θ 1 \theta_1 θ1,先对公式进行以下变换:

x = k 1 s 1 + k 2 c 1 x = k_1s_1 + k_2c_1 x=k1s1+k2c1

y = k 1 c 1 − k 2 s 1 y = k_1c_1 - k_2s_1 y=k1c1k2s1

其中
k 1 = l 1 + l 2 c 2 k 2 = l 2 s 2 \begin{matrix} k_1 =& l_1 + l_2c_2\\ \\ k_2 =& l_2s_2 \end{matrix} k1=k2=l1+l2c2l2s2

再对 k 1 , k 2 k_1,k_2 k1,k2进行变量替换

k 1 = r cos ⁡ γ k_1 = r\cos\gamma k1=rcosγ

k 2 = r sin ⁡ γ k_2 = r\sin\gamma k2=rsinγ

其中 r = k 1 2 + k 2 2 , γ = a t a n 2 ( k 2 , k 1 ) r=\sqrt{k_1^2 + k_2^2},\gamma = atan2(k_2,k_1) r=k12+k22 ,γ=atan2(k2,k1),同样,我们画个图方便理解, k 1 , k 2 k_1,k_2 k1,k2相当于坐标轴上的点P, γ \gamma γ r r r x x x轴的夹角。

代入正解方程
x r = cos ⁡ γ sin ⁡ θ 1 − sin ⁡ γ cos ⁡ α \frac{x}{r}=\cos\gamma\sin\theta_1-\sin\gamma\cos\alpha rx=cosγsinθ1sinγcosα

y r = cos ⁡ γ cos ⁡ θ 1 + sin ⁡ γ sin ⁡ θ 1 \frac{y}{r}=\cos\gamma\cos\theta_1 +\sin\gamma\sin\theta_1 ry=cosγcosθ1+sinγsinθ1

最终化简成:

sin ⁡ ( γ + θ 1 ) = x r \sin(\gamma + \theta_1) = \frac{x}{r} sin(γ+θ1)=rx

cos ⁡ ( γ + θ 1 ) = y r \cos(\gamma + \theta_1) = \frac{y}{r} cos(γ+θ1)=ry

同样使用atan2函数计算得到: γ + α = a t a n 2 ( y , x ) \gamma + \alpha = atan2(y, x) γ+α=atan2(y,x)

最终:
θ 1 = a t a n 2 ( y , x ) − a t a n 2 ( k 2 , k 1 ) = a t a n 2 ( y , x ) − a t a n 2 ( l 2 s 2 , l 1 + l 2 c 2 ) \theta_1 = atan2(y,x) - atan2(k_2,k_1)= atan2(y,x) - atan2( l_2s_2 ,l_1 + l2c_2) θ1=atan2(y,x)atan2(k2,k1)=atan2(y,x)atan2(l2s2,l1+l2c2)

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

注意

以上求出的 θ 1 \theta_1 θ1是与x轴正半轴的夹角,需要根据机器人的腿部初始角度进行一定变换,例如图中这样腿部初始角度是与地面垂直的, θ 1 = θ 1 + 90 ° \theta_1 = \theta_1 + 90° θ1=θ1+90° θ 1 \theta_1 θ1是与y轴负半轴的夹角,如最开始的那张图所示
在这里插入图片描述


如果觉得ok,点个赞,点个关注,也欢迎给个打赏支持一下编者的工作
相关推荐
©️2020 CSDN 皮肤主题: 技术黑板 设计师:CSDN官方博客 返回首页