不考虑横向髋关节运动时，四足机器人的腿部可以简化成二连杆机构，如上图。如果已知
α
,
β
\alpha,\beta
α , β
x
=
−
L
1
sin
α
−
L
2
sin
(
α
−
β
)
x =- L_1\sin\alpha - L_2\sin(\alpha-\beta )
x = − L 1 sin α − L 2 sin ( α − β )
y
=
−
L
1
cos
α
−
L
2
c
o
s
(
α
−
β
)
y = - L_1\cos\alpha - L_2cos(\alpha - \beta)
y = − L 1 cos α − L 2 c o s ( α − β )
已知P[x,y]位置，求
α
,
β
\alpha,\beta
α , β
我们用代数的方法求逆解，首先两边平方相加
x
2
+
y
2
=
(
l
1
s
i
n
(
α
)
+
l
2
s
i
n
(
α
−
β
)
)
2
+
(
l
1
c
o
s
(
α
)
+
l
2
c
o
s
(
α
−
β
)
)
2
x^2 + y^2 = (l_1sin(\alpha) + l_2sin(\alpha - \beta))^2 + (l_1cos(\alpha) + l_2cos(\alpha - \beta))^2
x 2 + y 2 = ( l 1 s i n ( α ) + l 2 s i n ( α − β ) ) 2 + ( l 1 c o s ( α ) + l 2 c o s ( α − β ) ) 2
拆开，并写成更简洁的形式,其中
c
o
s
α
=
c
1
,
c
o
s
β
=
c
2
cos\alpha=c1, cos\beta=c2
c o s α = c 1 , c o s β = c 2
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^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 s 1 2 + 2 l 1 l 2 s 1 s 1 2 + l 1 2 c 1 + 2 l 1 l 2 c 1 c 1 2 + l 2 2 s 1 2 2 + l 2 2 c 1 2 2
x
2
+
y
2
=
l
1
2
+
l
2
2
+
2
l
1
l
2
(
s
1
s
12
+
c
1
c
12
)
x^2 + y^2 = l_1^2 + l_2^2 + 2l_1l_2(s_1s_{12} +c_1c_{12})
x 2 + y 2 = l 1 2 + l 2 2 + 2 l 1 l 2 ( s 1 s 1 2 + c 1 c 1 2 )
根据和差公式：
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
s 1 s 1 2 = 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
c_1c_{12} = c_1^2 c_2 + s_1c_1 s_2
c 1 c 1 2 = c 1 2 c 2 + s 1 c 1 s 2
最终得到：
l
1
2
+
l
2
2
+
2
l
1
l
2
c
2
l_1^2 + l_2^2 + 2l_1l_2c_2
l 1 2 + l 2 2 + 2 l 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 ( result)
我们能够得到同样结果
L1** 2 + 2 * L1* L2* cos( b) + L2** 2
求得
cos
β
=
−
l
1
2
−
l
2
2
+
x
2
+
y
2
2
l
1
l
2
\cos\beta = \frac{- l_{1}^{2} - l_{2}^{2} + x^{2} + y^{2}}{2 l_{1} l_{2}}
cos β = 2 l 1 l 2 − l 1 2 − l 2 2 + x 2 + y 2
sin
β
=
±
1
−
cos
2
β
\sin\beta = \pm\sqrt{1-\cos^2\beta}
sin β = ± 1 − cos 2 β
这里在求解时需要注意x，y值，验证其是否超过腿部动作空间。
sin
β
\sin\beta
sin β
α
\alpha
α
接下来我们通过正解来求
α
\alpha
α
x
=
k
1
s
1
+
k
2
c
1
x = k_1s_1 + k_2c_1
x = k 1 s 1 + k 2 c 1
y
=
k
1
c
1
−
k
2
s
1
y = k_1c_1 - k_2s_1
y = k 1 c 1 − k 2 s 1
其中
k
1
=
−
(
l
1
+
l
2
c
2
)
k
2
=
l
2
s
2
\begin{matrix} k_1 =& -(l_1 + l2c_2)\\ \\ k_2 =& l_2s_2 \end{matrix}
k 1 = k 2 = − ( l 1 + l 2 c 2 ) l 2 s 2
对
k
1
,
k
2
k_1,k_2
k 1 , k 2
k
1
=
r
cos
γ
k_1 = r\cos\gamma
k 1 = r cos γ
k
2
=
r
sin
γ
k_2 = r\sin\gamma
k 2 = r sin γ
其中
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 = k 1 2 + k 2 2
, γ = a t a n 2 ( k 2 , k 1 )
代入正解方程：
x
r
=
−
cos
γ
sin
α
+
sin
γ
cos
α
\frac{x}{r}=-\cos\gamma\sin\alpha +\sin\gamma\cos\alpha
r x = − cos γ sin α + sin γ cos α
y
r
=
−
cos
γ
cos
α
−
sin
γ
sin
α
\frac{y}{r}=-\cos\gamma\cos\alpha -\sin\gamma\sin\alpha
r y = − cos γ cos α − sin γ sin α
最终化简成：
sin
(
γ
+
α
)
=
x
r
\sin(\gamma + \alpha) = \frac{x}{r}
sin ( γ + α ) = r x
cos
(
γ
+
α
)
=
y
r
\cos(\gamma + \alpha) = \frac{y}{r}
cos ( γ + α ) = r y
同样使用atan2函数计算得到：
γ
+
α
=
a
t
a
n
2
(
x
,
y
)
\gamma + \alpha = atan2(x, y)
γ + α = a t a n 2 ( x , y )
最终:
α
=
a
t
a
n
2
(
x
,
y
)
−
a
t
a
n
2
(
k
2
,
k
1
)
=
a
t
a
n
2
(
x
,
y
)
−
a
t
a
n
2
(
l
2
s
2
,
−
l
1
−
l
2
c
2
)
\alpha = atan2(x,y) - atan2(k_2,k_1)= atan2(x,y) - atan2( l_2s_2 ,-l_1 - l2c_2)
α = a t a n 2 ( x , y ) − a t a n 2 ( k 2 , k 1 ) = a t a n 2 ( x , y ) − a t a n 2 ( l 2 s 2 , − l 1 − l 2 c 2 )
整理一些写成可计算的python程序如下：
c2 = ( - L1** 2 - L2** 2 + x** 2 + y** 2 ) / ( 2 * L1* L2)
s2 = np. sqrt( 1 - c2** 2 )
beta = np. arctan2( s2, c2)
alpha = np. arctan2( x, y) - np. arctan2( L1* s2, - L1- L2* c2)
