柚子快報邀請碼778899分享：輪腿機器人-五連桿正運動學(xué)解算
柚子快報邀請碼778899分享：輪腿機器人-五連桿正運動學(xué)解算
輪腿機器人-五連桿與VMC
1.五連桿正運動學(xué)分析2.參考文獻
1.五連桿正運動學(xué)分析
如圖所示為五連桿結(jié)構(gòu)圖，其中A，E為機器人腿部控制的兩個電機，θ1,θ4可以通過電機的編碼器測得。五連桿控制任務(wù)主要關(guān)注機構(gòu)末端C點位置，其位置用直角坐標表示為(Cx,Cy)，極坐標系用(L0,θ0)表示。 根據(jù)上述五連桿結(jié)構(gòu)圖可以列出以下等式：
{
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)內(nèi)部兩個等式相加并移項有：
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?)?L2?M=2?(Bx??Dx?)?L2?P=2?[(L3?)2?(L2?)2]LBD?=(Bx??Dx?)2+(By??Dy?)2
?C=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)? 根據(jù)公式(6)得到了一個關(guān)于
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)? 進一步推導(dǎo)得到極坐標為：
{
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??2L5??Cy????(8)?
2.參考文獻
https://zhuanlan.zhihu.com/p/613007726 [1]于紅英,唐德威,王建宇.平面五桿機構(gòu)運動學(xué)和動力學(xué)特性分析[J].哈爾濱工業(yè)大學(xué)學(xué)報,2007(06):940-943. [2]謝惠祥.四足機器人對角小跑步態(tài)虛擬模型直覺控制方法研究[D].國防科學(xué)技術(shù)大學(xué),2015.
柚子快報邀請碼778899分享：輪腿機器人-五連桿正運動學(xué)解算
文章來源
本文內(nèi)容根據(jù)網(wǎng)絡(luò)資料整理，出于傳遞更多信息之目的，不代表金鑰匙跨境贊同其觀點和立場。
轉(zhuǎn)載請注明，如有侵權(quán)，聯(lián)系刪除。
Target跨境目標站
