轮腿机器人-五连杆与VMC
- 1.五连杆正运动学分析
- 2.参考文献
1.五连杆正运动学分析
如图所示为五连杆结构图，其中A，E为机器人腿部控制的两个电机，θ1,θ4可以通过电机的编码器测得。五连杆控制任务主要关注机构末端C点位置，其位置用直角坐标表示为(Cx,Cy)，极坐标系用(L0,θ0)表示。
根据上述五连杆结构图可以列出以下等式：
{
B
x
+
L
2
∗
c
o
s
(
θ
2
)
=
D
x
+
L
3
∗
c
o
s
(
θ
3
)
B
y
+
L
2
∗
s
i
n
(
θ
2
)
=
D
y
+
L
3
∗
s
i
n
(
θ
3
)
\begin{equation} \begin{cases} B_{x}+L_{2}*{\color{Green} cos(\theta _{2})} =D_{x}+L_{3}*{\color{Green} cos(\theta _{3})} \\ B_{y}+L_{2}*{\color{Orange} sin(\theta _{2})} =D_{y}+L_{3}*{\color{Orange} sin(\theta _{3})} \end{cases} \tag{1} \end{equation}
{Bx+L2∗cos(θ2)=Dx+L3∗cos(θ3)By+L2∗sin(θ2)=Dy+L3∗sin(θ3)(1)
对公式(1)移项，并在等式两边进行平方有：
{
(
B
x
+
L
2
∗
c
o
s
(
θ
2
)
−
D
x
)
2
=
(
L
3
∗
c
o
s
(
θ
3
)
)
2
(
B
y
+
L
2
∗
s
i
n
(
θ
2
)
−
D
y
)
2
=
(
L
3
∗
s
i
n
(
θ
3
)
)
2
\begin{equation} \begin{cases} (B_{x}+L_{2}*{\color{Green} cos(\theta _{2})} -D_{x})^{2}=(L_{3}*{\color{Green} cos(\theta _{3})})^{2} \\ (B_{y}+L_{2}*{\color{Orange} sin(\theta _{2})} - D_{y})^{2}=(L_{3}*{\color{Orange} sin(\theta _{3})})^{2} \end{cases} \tag{2} \end{equation}
{(Bx+L2∗cos(θ2)−Dx)2=(L3∗cos(θ3))2(By+L2∗sin(θ2)−Dy)2=(L3∗sin(θ3))2(2)
将平方展开有：
{
(
B
x
−
D
x
)
2
+
2
∗
(
B
x
−
D
x
)
∗
L
2
∗
c
o
s
(
θ
2
)
+
(
L
2
∗
c
o
s
(
θ
2
)
)
2
=
(
L
3
∗
c
o
s
(
θ
3
)
)
2
(
B
y
−
D
y
)
2
+
2
∗
(
B
y
−
D
y
)
∗
L
2
∗
s
i
n
(
θ
2
)
+
(
L
2
∗
s
i
n
(
θ
2
)
)
2
=
(
L
3
∗
s
i
n
(
θ
3
)
)
2
\begin{equation} \begin{cases} (B_{x}-D_{x})^{2}+2*(B_{x}-D_{x})*L_{2}*{\color{Green} cos(\theta _{2})}+ (L_{2}*{\color{Green} cos(\theta _{2})})^{2}=(L_{3}*{\color{Green} cos(\theta _{3})})^{2} \\ (B_{y} - D_{y})^{2}+2*(B_{y} - D_{y})*L_{2}*{\color{Orange} sin(\theta _{2})}+(L_{2}*{\color{Orange} sin(\theta _{2})})^{2}=(L_{3}*{\color{Orange} sin(\theta _{3})})^{2} \end{cases} \tag{3} \end{equation}
{(Bx−Dx)2+2∗(Bx−Dx)∗L2∗cos(θ2)+(L2∗cos(θ2))2=(L3∗cos(θ3))2(By−Dy)2+2∗(By−Dy)∗L2∗sin(θ2)+(L2∗sin(θ2))2=(L3∗sin(θ3))2(3)
对公式(3)内部两个等式相加并移项有：
K
∗
s
i
n
(
θ
2
)
+
M
∗
c
o
s
(
θ
2
)
=
C
\begin{equation} K*{\color{Orange} sin(\theta _{2})}+M*{\color{Green} cos(\theta _{2})}=C \tag{4} \end{equation}
K∗sin(θ2)+M∗cos(θ2)=C(4)
{
K
=
2
∗
(
B
y
−
D
y
)
∗
L
2
M
=
2
∗
(
B
x
−
D
x
)
∗
L
2
P
=
2
∗
[
(
L
3
)
2
−
(
L
2
)
2
]
L
B
D
=
(
B
x
−
D
x
)
2
+
(
B
y
−
D
y
)
2
C
=
P
−
(
L
B
D
)
2
\begin{cases} K=2*(B_{y} - D_{y})*L_{2} \\M=2*(B_{x}-D_{x})*L_{2} \\P=2*[(L_{3})^{2}-(L_{2})^{2}] \\L_{BD}=\sqrt{(B_{x}-D_{x})^{2}+(B_{y} - D_{y})^{2}} \\C=P-(L_{BD} )^2 \end{cases}
⎩
⎨
⎧K=2∗(By−Dy)∗L2M=2∗(Bx−Dx)∗L2P=2∗[(L3)2−(L2)2]LBD=(Bx−Dx)2+(By−Dy)2C=P−(LBD)2
使用二倍角法对公式(4)进一步化简，已知：
{
t
a
n
θ
2
=
s
i
n
(
θ
)
1
+
c
o
s
(
θ
)
c
o
s
(
θ
)
=
c
o
s
2
θ
2
−
s
i
n
2
θ
2
=
2
∗
c
o
s
2
θ
2
−
1
c
o
s
2
θ
2
−
s
i
n
2
θ
2
=
1
\begin{cases} {\color{Purple} tan\frac{\theta }{2}} = \frac{{\color{Orange} sin(\theta )} }{1+{\color{Green} cos(\theta )} } \\{\color{Green} cos(\theta )} {\color{Green} ={\color{Green} cos^2\frac{\theta }{2}}} - {\color{Orange} sin^2\frac{\theta }{2}} =2*{\color{Green} cos^2\frac{\theta }{2}} -1 \\{\color{Green} cos^2\frac{\theta }{2}} - {\color{Orange} sin^2\frac{\theta }{2}} =1 \end{cases}
⎩
⎨
⎧tan2θ=1+cos(θ)sin(θ)cos(θ)=cos22θ−sin22θ=2∗cos22θ−1cos22θ−sin22θ=1
当
1
+
c
o
s
(
θ
)
≠
0
1+{\color{Green} cos(\theta )} \ne 0
1+cos(θ)=0，对公式(4)进行如下变化，其中
τ
=
1
+
c
o
s
(
θ
)
\tau=1+{\color{Green}cos(\theta)}
τ=1+cos(θ):
τ
2
∗
(
2
∗
K
∗
s
i
n
(
θ
2
)
τ
+
2
∗
M
∗
c
o
s
(
θ
2
)
τ
−
2
∗
C
τ
)
=
0
\begin{equation} \frac{\tau}{2} *(\frac{2*K*{\color{Green} sin(\theta_{2})} }{\tau}+\frac{2*M*{\color{Orange} cos(\theta_{2})} }{\tau}-\frac{2*C}{\tau} )=0 \tag{5} \end{equation}
2τ∗(τ2∗K∗sin(θ2)+τ2∗M∗cos(θ2)−τ2∗C)=0(5)
使用二倍角对公式(5)进行展开并进行化简得：
1
+
c
o
s
(
θ
2
)
2
∗
[
(
C
−
M
)
∗
t
a
n
2
θ
2
2
+
2
∗
K
∗
t
a
n
(
θ
2
2
)
+
(
M
+
C
)
]
\begin{equation} \frac{1+{\color{Green} cos(\theta_{2} )} }{2}*[(C-M)*{\color{Purple} tan^2\frac{\theta_{2} }{2}} +2*K*{\color{Purple} tan(\frac{\theta_{2} }{2})} +(M+C) ] \tag{6} \end{equation}
21+cos(θ2)∗[(C−M)∗tan22θ2+2∗K∗tan(2θ2)+(M+C)](6)
根据公式(6)得到了一个关于
t
a
n
(
θ
2
2
)
{\color{Purple} tan(\frac{\theta_{2} }{2})}
tan(2θ2)的一元二次方程，其求根判别式为：
△
=
(
2
∗
K
)
2
−
4
∗
(
C
−
M
)
∗
(
M
+
C
)
=
4
(
K
2
+
M
2
−
C
2
)
\bigtriangleup =(2*K)^2-4*(C-M)*(M+C)=4(K^2+M^2-C^2)
△=(2∗K)2−4∗(C−M)∗(M+C)=4(K2+M2−C2)
当
△
≥
0
\bigtriangleup\ge 0
△≥0时，可以解出
θ
2
\theta_{2}
θ2:
θ
2
=
2
∗
a
r
c
t
a
n
(
K
±
(
K
2
+
M
2
−
C
2
)
M
−
C
)
\theta _{2}=2*arctan(\frac{K\pm \sqrt{(K^2+M^2-C^2)} }{M-C} )
θ2=2∗arctan(M−CK±(K2+M2−C2))
通过
θ
1
\theta_{1}
θ1即可解算出
C
C
C点的直角坐标有：
{
C
x
=
L
1
∗
c
o
s
(
θ
1
)
+
L
2
∗
c
o
s
(
θ
2
)
C
y
=
L
1
∗
s
i
n
(
θ
1
)
+
L
2
∗
s
i
n
(
θ
2
)
\begin{equation} \begin{cases} C_{x}=L_{1}*{\color{Orange} cos(\theta _{1})} +L_{2}*{\color{Orange} cos(\theta_{2})} \\C_{y}=L_{1}*{\color{Green} sin(\theta _{1})} +L_{2}*{\color{Green} sin(\theta_{2})} \end{cases} \tag{7} \end{equation}
{Cx=L1∗cos(θ1)+L2∗cos(θ2)Cy=L1∗sin(θ1)+L2∗sin(θ2)(7)
进一步推导得到极坐标为：
{
L
0
=
(
C
x
−
L
5
)
2
+
C
y
2
θ
0
=
a
r
c
t
a
n
C
y
C
x
−
L
5
2
\begin{equation} \begin{cases} L_{0}=\sqrt{(C_{x}-L_{5})^2+C_{y}^2} \\\theta_{0}=arctan\frac{C_{y}}{C_{x}-\frac{L_{5}}{2} } \end{cases} \tag{8} \end{equation}
⎩
⎨
⎧L0=(Cx−L5)2+Cy2θ0=arctanCx−2L5Cy(8)
2.参考文献
https://zhuanlan.zhihu.com/p/613007726
[1]于红英,唐德威,王建宇.平面五杆机构运动学和动力学特性分析[J].哈尔滨工业大学学报,2007(06):940-943.
[2]谢惠祥.四足机器人对角小跑步态虚拟模型直觉控制方法研究[D].国防科学技术大学,2015.
