Maciej Górka gr. 29

Dynamika Robotów

Manipulator 1

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

q₁ = θ₁

q₂ = d₁

q₃ = d₂

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}{(J}_{3Z_{1}} + \ m_{3}d_{2}^{2}{)\dot{\theta}}_{1}^{2} + \frac{1}{2}m_{3}{\dot{d}}_{1}^{2} + \frac{1}{2}m_{3}{\dot{d}}_{2}^{2}$$

E  =  E₁  +  E₂  +  E₃ 

Energia potencjalna:

V₁ =  m₁gh₁

V₂ =  m₂gd₁

V₃ =  m₃g(d₁ − h₂)

V  =  V₁  +  V₂  +  V₃

$$E\ = \ \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}{(J}_{3Z_{1}} + \ m_{3}d_{2}^{2}{)\dot{\theta}}_{1}^{2} + \frac{1}{2}m_{3}{\dot{d}}_{1}^{2} + \frac{1}{2}m_{3}{\dot{d}}_{2}^{2}$$

V  =  m₁gh₁ + m₂gd₁ + m₃g(d₁ − h₂)

$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}d_{2}^{2}) + \frac{1}{2}m_{3}{\dot{d}}_{1}^{2} + \frac{1}{2}m_{3}{\dot{d}}_{2}^{2} - m_{1}gh_{1} - m_{2}gd_{1} - \ m_{3}g({d_{1} - h}_{2})$

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

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

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

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

$$\frac{\partial L}{\partial d_{2}} = {{\dot{\theta}}_{1}^{2}m}_{3}d_{2}$$

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

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

$$\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{d_{2}}} \right\rbrack = \ddot{d_{2}}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{d}_{\mathbf{2}}^{\mathbf{2}}\mathbf{) + \ 2}\dot{\mathbf{\theta}_{\mathbf{1}}}\dot{\mathbf{d}_{\mathbf{2}}}{\mathbf{d}_{\mathbf{2}}\mathbf{m}}_{\mathbf{3}}$$

$$\mathbf{\tau}_{\mathbf{2}}\mathbf{\lbrack N\rbrack =}\ddot{\mathbf{d}_{\mathbf{1}}}\mathbf{(}\mathbf{m}_{\mathbf{2}}\mathbf{+}\mathbf{m}_{\mathbf{3}}\mathbf{) + g(}\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}}{\mathbf{\ }{\mathbf{- \ }\dot{\mathbf{\theta}}}_{\mathbf{1}}^{\mathbf{2}}\mathbf{m}}_{\mathbf{3}}\mathbf{d}_{\mathbf{2}}$$

Manipulator 2

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

q₁ = θ₁

q₂ = d₂

q₃ = θ₂

J_2Z = J₂ + m₂l₁²

J_4Z1 =  J₄ + m₄l₂²

Energia kinetyczna:

E₁ = 0

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

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

$$E_{4} = \frac{1}{2}{{\dot{\theta}}_{1}^{2}(J}_{4Z_{1}} + \ m_{4}d_{2}^{2}) + \frac{1}{2}m_{4}{\dot{d}}_{2}^{2} + \frac{1}{2}J_{4Z_{1}}{\dot{\theta}}_{2}^{2}$$

E  =  E₁  +  E₂  +  E₃ + E₄

Energia potencjalna:

V₁ =  m₁gh₁

V₂ =  m₂gd₁

V₃ =  m₃gh₂

V₄ =m₄g(d₁ + d₃)

V  =  V₁  +  V₂  +  V₃ + V₄ 

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

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

$$\frac{\partial L}{\partial\dot{\theta_{1}}} = \dot{\theta_{1}}(J_{2Z} + J_{3Z_{1}} + \ m_{3}d_{2}^{2} + J_{4Z_{1}} + \ m_{4}d_{2}^{2})$$

$$\frac{\partial L}{\partial d_{2}} = d_{2}{{(\dot{\theta}}_{1}^{2}m}_{3} + {{\dot{\theta}}_{1}^{2}m}_{4})$$

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

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

$$\frac{\partial L}{\partial\dot{\theta_{2}}} = \dot{\theta_{2}}J_{4Z_{1}}$$

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

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

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

Równania Lagrange’a

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

$$\mathbf{\tau}_{\mathbf{2}}\mathbf{\lbrack N\rbrack =}\ddot{\mathbf{d}_{\mathbf{2}}}\left( \mathbf{m}_{\mathbf{3}}\mathbf{+}\mathbf{m}_{\mathbf{4}} \right)\mathbf{-}\mathbf{d}_{\mathbf{2}}{{\mathbf{(}\dot{\mathbf{\theta}}}_{\mathbf{1}}^{\mathbf{2}}\mathbf{m}}_{\mathbf{3}}\mathbf{+}{{\dot{\mathbf{\theta}}}_{\mathbf{1}}^{\mathbf{2}}\mathbf{m}}_{\mathbf{4}}\mathbf{)}$$

$$\mathbf{\tau}_{\mathbf{3}}\mathbf{\lbrack Nm\rbrack =}\ddot{\mathbf{\theta}_{\mathbf{2}}}\mathbf{J}_{\mathbf{4}\mathbf{Z}_{\mathbf{1}}}$$