Dawid Bębenek gr. 29

Dynamika Robotów

Manipulator 1

Określenie współrzędnych uogólnionych:

q₁ = d₁

q₂ = θ₂ 

q₃ = d₄

J_2Z = J₂ + m₂l₂²

Energia kinetyczna:

$$E_{1} = \frac{1}{2}m_{1}{\dot{d_{1}}}^{2}$$

$$E_{2} = \frac{1}{2}J_{2Z}{\dot{\theta}}_{2}^{2} + \frac{1}{2}m_{2}{\dot{d}}_{1}^{2}$$

$$E_{3} = \frac{1}{2}{(J}_{3Z} + \ m_{3}l_{2}^{}{)\dot{\theta}}_{2}^{2} + \frac{1}{2}m_{3}{\dot{d}}_{1}^{2} + \frac{1}{2}m_{3}{\dot{d}}_{4}^{2}$$

E  =  E₁  +  E₂  +  E₃ 

Energia potencjalna:

V₁ =  m₁gd₁

V₂ =  m₂g(d₁ + l₂)

V₃ =  m₃g(d₁ + l₂)

V  =  V₁  +  V₂  +  V₃

$$E\ = \ \frac{1}{2}m_{1}\dot{{d_{1}}^{2}} + \frac{1}{2}J_{2Z}{\dot{\theta}}_{2}^{2} + \frac{1}{2}m_{2}{\dot{d}}_{1}^{2} + \frac{1}{2}{(J}_{3Z} + \ m_{3}l_{2}^{}{)\dot{\theta}}_{2}^{2} + \frac{1}{2}m_{3}{\dot{d}}_{1}^{2} + \frac{1}{2}m_{3}{\dot{d}}_{4}^{2}$$

V  =  m₁gd₁ + m₂g(d₁ + l₂) + m₃g(d₁ + l₂)

$L\ = \ E\ \ V\ = \frac{1}{2}m_{1}{\dot{d_{1}}}^{2} + \frac{1}{2}J_{2Z}{\dot{\theta}}_{2}^{2} + \frac{1}{2}m_{2}{\dot{d}}_{1}^{2} + \frac{1}{2}{(J}_{3Z} + \ m_{3}l_{2}^{}{)\dot{\theta}}_{2}^{2} + \frac{1}{2}m_{3}{\dot{d}}_{1}^{2} + \frac{1}{2}m_{3}{\dot{d}}_{4}^{2} - m_{1}gd_{1} - m_{2}g{(d}_{1} + l_{2}) - m_{3}g(d_{1} + l_{2})$

$$\frac{\partial L}{\partial\theta_{2}} = 0$$

$$\frac{\partial L}{\partial\dot{\theta_{2}}} = \dot{\theta_{2}}(J_{2Z} + J_{3Z} + \ m_{3}l_{2}^{})$$

$$\frac{\partial L}{\partial d_{1}} = - g(m_{1} + m_{2} + m_{3})$$

$$\frac{\partial L}{\partial\dot{d_{1}}} = \dot{d_{1}}(m_{1} + m_{2} + m_{3})$$

$$\frac{\partial L}{\partial d_{4}} = 0$$

$$\frac{\partial L}{\partial\dot{d_{4}}} = \dot{d_{4}}m_{3}$$

$$\frac{d}{\text{dt}}\left\lbrack \frac{\partial L}{\partial\dot{\theta_{1}}} \right\rbrack = \ddot{\theta_{2}(}J_{2Z} + J_{3Z} + \ m_{3}l_{2}^{})$$

$$\frac{d}{\text{dt}}\left\lbrack \frac{\partial L}{\partial\dot{d_{1}}} \right\rbrack = \ddot{d_{1}}(m_{1} + m_{2} + m_{3})$$

$$\frac{d}{\text{dt}}\left\lbrack \frac{\partial L}{\partial\dot{d_{4}}} \right\rbrack = \ddot{d_{4}}m_{3}$$

Równania Lagrange’a

$$\mathbf{\tau}_{\mathbf{1}}\mathbf{\lbrack Nm\rbrack = \ }\ddot{\mathbf{\theta}_{\mathbf{1}}}\mathbf{(J}_{\mathbf{1}\mathbf{Z}}\mathbf{+}\mathbf{J}_{\mathbf{2}\mathbf{Z}}\mathbf{+}\mathbf{J}_{\mathbf{3}\mathbf{Z}_{\mathbf{1}}}\mathbf{+ \ }\mathbf{m}_{\mathbf{3}}\mathbf{l}_{\mathbf{2}}^{\mathbf{2}}\mathbf{)}$$

$$\mathbf{\tau}_{\mathbf{2}}\left\lbrack \mathbf{N} \right\rbrack\mathbf{=}\ddot{\mathbf{d}_{\mathbf{1}}}\left( \mathbf{m}_{\mathbf{1}}\mathbf{+}\mathbf{m}_{\mathbf{2}}\mathbf{+}\mathbf{m}_{\mathbf{3}} \right)\mathbf{+ g(}\mathbf{m}_{\mathbf{1}}\mathbf{+}\mathbf{m}_{\mathbf{2}}\mathbf{+}\mathbf{m}_{\mathbf{3}}\mathbf{)}$$

$$\mathbf{\tau}_{\mathbf{3}}\mathbf{\lbrack N\rbrack =}\ddot{\mathbf{d}_{\mathbf{2}}}\mathbf{m}_{\mathbf{3}}$$

Manipulator 2

Określenie współrzędnych uogólnionych:

q₁ = θ₁

q₂ = d₁

q₃ = θ₂

J_2Z = J₂ + m₂l₁²

Energia kinetyczna:

$$E_{1} = \frac{1}{2}J_{1Z}{\dot{\theta}}_{1}^{2}$$

$$E_{2} = \frac{1}{2}J_{2Z}{\dot{\theta}}_{1}^{2} + \frac{1}{2}m_{2}{\dot{d}}_{1}^{2}$$

$$E_{3} = \frac{1}{2}{{\dot{\theta}}_{1}^{2}(J}_{3Z_{1}} + \ m_{3}r_{1}sin(\theta_{2})) + \frac{1}{2}m_{3}{\dot{d}}_{1}^{2} + \frac{1}{2}J_{3Z_{1}}{\dot{\theta}}_{2}^{2}$$

E  =  E₁  +  E₂  +  E₃

Energia potencjalna:

V₁ =  m₁gh₁

V₂ =  m₂gd₁

V₃ =  m₃g(d₁ − l₂cos(θ₂))

V  =  V₁  +  V₂  +  V₃

$$L\ = \ E\ \ V = \frac{1}{2}J_{1Z}{\dot{\theta}}_{1}^{2} + \frac{1}{2}J_{2Z}{\dot{\theta}}_{1}^{2} + \frac{1}{2}m_{2}{\dot{d}}_{1}^{2} + \frac{1}{2}{{\dot{\theta}}_{1}^{2}(J}_{3Z_{1}} + \ m_{3}r_{1}sin(\theta_{2})) + \frac{1}{2}m_{3}{\dot{d}}_{1}^{2} + \frac{1}{2}J_{3Z_{1}}{\dot{\theta}}_{2}^{2} - m_{1}gh_{1} - m_{2}gd_{1} - m_{3}g({d_{1} - l}_{2}cos(\theta_{2}))$$

$$\frac{\partial L}{\partial\theta_{1}} = 0$$

$$\frac{\partial L}{\partial\dot{\theta_{1}}} = {\dot{\theta_{1}}(J_{1Z} + J_{2Z} + J}_{3Z} + \ m_{3}r_{1}sin(\theta_{2}))$$

$$\frac{\partial L}{\partial d_{1}} = - g\left( m_{2} + m_{3} \right)$$

$$\frac{\partial L}{\partial\dot{d_{1}}} = (m_{2} + m_{3})\dot{d_{1}}$$

$$\frac{\partial L}{\partial\theta_{2}} = \ m_{3}r_{1}\cos\left( \theta_{2} \right) - m_{3}gl_{2}cos(\theta_{2})$$

$$\frac{\partial L}{\partial\dot{\theta_{2}}} = \dot{\theta_{2}}J_{3Z}$$

$$\frac{d}{\text{dt}}\left\lbrack \frac{\partial L}{\partial\dot{\theta_{1}}} \right\rbrack = \ddot{\theta_{1}}({J_{1Z} + J_{2Z} + J}_{3Z} + \ m_{3}d_{1}^{2})$$

$$\frac{d}{\text{dt}}\left\lbrack \frac{\partial L}{\partial\dot{d_{1}}} \right\rbrack = \ddot{d_{1}}(m_{2} + m_{3})$$

$$\frac{d}{\text{dt}}\left\lbrack \frac{\partial L}{\partial\dot{\theta_{2}}} \right\rbrack = \ddot{\theta_{2}}J_{3Z}$$

Równania Lagrange’a

$$\mathbf{\tau}_{\mathbf{1}}\mathbf{\lbrack Nm\rbrack = \ }\ddot{\mathbf{\theta}_{\mathbf{1}}}\mathbf{(}{\mathbf{J}_{\mathbf{1}\mathbf{Z}}\mathbf{+}\mathbf{J}_{\mathbf{2}\mathbf{Z}}\mathbf{+ J}}_{\mathbf{3}\mathbf{Z}}\mathbf{+ \ }\mathbf{m}_{\mathbf{3}}\mathbf{d}_{\mathbf{2}}^{\mathbf{2}}\mathbf{)}$$

$$\mathbf{\tau}_{\mathbf{2}}\left\lbrack \mathbf{N} \right\rbrack\mathbf{=}\ddot{\mathbf{d}_{\mathbf{1}}}\left( \mathbf{m}_{\mathbf{2}}\mathbf{+}\mathbf{m}_{\mathbf{3}} \right)\mathbf{+ g}\left( \mathbf{m}_{\mathbf{2}}\mathbf{+}\mathbf{m}_{\mathbf{3}} \right)$$

$$\mathbf{\tau}_{\mathbf{3}}\left\lbrack \mathbf{\text{Nm}} \right\rbrack\mathbf{=}\ddot{\mathbf{\theta}_{\mathbf{2}}}\mathbf{J}_{\mathbf{3}\mathbf{Z}}\mathbf{-}\mathbf{m}_{\mathbf{3}}\mathbf{r}_{\mathbf{1}}\mathbf{\text{co}}\operatorname{s}\left( \mathbf{\theta}_{\mathbf{2}} \right)\mathbf{+}\mathbf{m}_{\mathbf{3}}\mathbf{g}\mathbf{l}_{\mathbf{2}}\mathbf{cos(}\mathbf{\theta}_{\mathbf{2}}\mathbf{)}$$