轮式机器人五连杆正运动学解决方案
最编程
2024-04-14 16:24:26
...
{
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)∗
使用二倍角法对公式(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)∗
上一篇： Perlin noise
下一篇： 3.15 Python 逻辑操作符
推荐阅读