Exemple Numèric de Dinàmica de Braç Robòtic

1. Paràmetres del Sistema

2. Càlcul de l'Energia Cinètica

2.1. Posicions:

\[ x_1 = l_1 \sin(\theta_1) = 1 \cdot \sin\left(\frac{\pi}{4}\right) = \frac{\sqrt{2}}{2} \]

\[ y_1 = l_1 \cos(\theta_1) = 1 \cdot \cos\left(\frac{\pi}{4}\right) = \frac{\sqrt{2}}{2} \]

\[ x_2 = l_1 \sin(\theta_1) + l_2 \sin(\theta_1 + \theta_2) = 1 \cdot \sin\left(\frac{\pi}{4}\right) + 0.5 \cdot \sin\left(\frac{\pi}{4} + \frac{\pi}{6}\right) \]

\[ y_2 = -l_1 \cos(\theta_1) - l_2 \cos(\theta_1 + \theta_2) = -1 \cdot \cos\left(\frac{\pi}{4}\right) - 0.5 \cdot \cos\left(\frac{\pi}{4} + \frac{\pi}{6}\right) \]

2.2. Velocitat:

\[ v_1 = \begin{bmatrix} l_1 \cos(\theta_1) \dot{\theta_1} \\ -l_1 \sin(\theta_1) \dot{\theta_1} \end{bmatrix} = \begin{bmatrix} 1 \cdot \cos\left(\frac{\pi}{4}\right) \cdot 1 \\ -1 \cdot \sin\left(\frac{\pi}{4}\right) \cdot 1 \end{bmatrix} = \begin{bmatrix} \frac{\sqrt{2}}{2} \\ -\frac{\sqrt{2}}{2} \end{bmatrix} \]

\[ v_2 = \begin{bmatrix} l_2 \cos(\theta_1 + \theta_2) \dot{\theta_2} \\ -l_2 \sin(\theta_1 + \theta_2) \dot{\theta_2} \end{bmatrix} + 2 l_1 l_2 \cos(\theta_2) \dot{\theta_1} \dot{\theta_2} \]

2.3. Energia Cinètica:

\[ K_1 = \frac{1}{2} m_1 v_1^2 = \frac{1}{2} \cdot 1 \cdot \left( \frac{\sqrt{2}}{2} \right)^2 = \frac{1}{2} \cdot \frac{2}{4} = \frac{1}{4} \]

\[ K_2 = \frac{1}{2} m_2 v_2^2 = \frac{1}{2} \cdot 0.5 \cdot \left( \frac{\sqrt{2}}{2} \right)^2 = \frac{0.5}{2} \cdot \frac{2}{4} = \frac{0.25}{2} = \frac{0.125} \]

\[ K = K_1 + K_2 = \frac{1}{4} + \frac{0.125} = 0.375 \]

3. Energia Potencial

3.1. Energia Potencial:

\[ U = - (m_1 + m_2) g l_1 \cos(\theta_1) - m_2 g l_2 \cos(\theta_1 + \theta_2) \]

\[ U = - (1 + 0.5) \cdot 9.81 \cdot 1 \cdot \cos\left(\frac{\pi}{4}\right) - 0.5 \cdot 9.81 \cdot 0.5 \cdot \cos\left(\frac{\pi}{4} + \frac{\pi}{6}\right) \]

\[ U = - 1.5 \cdot 9.81 \cdot \frac{\sqrt{2}}{2} - 0.5 \cdot 9.81 \cdot 0.5 \cdot \cos\left(\frac{5\pi}{12}\right) \]

4. Equacions de Lagrange

4.1. Lagrangià:

\[ L = K - U \]

\[ L = 0.375 - \left[ - (1.5 \cdot 9.81 \cdot \frac{\sqrt{2}}{2}) - (0.5 \cdot 9.81 \cdot 0.5 \cdot \cos\left(\frac{5\pi}{12}\right)) \right] \]

4.2. Equació de Lagrange per \( \theta_1 \):

\[ \frac{\partial L}{\partial \theta_1} = (m_1 + m_2) l_1^2 \dot{\theta_1} + m_2 l_1 l_2 (\dot{\theta_1} + \dot{\theta_2}) + 2 m_2 l_1 l_2 \cos \theta_2 \dot{\theta_1} + m_2 l_1 l_2 \cos \theta_2 \dot{\theta_1} \]

4.3. Derivada temporal de \( \frac{\partial L}{\partial \dot{\theta_1}} \):

\[ \frac{d}{dt} \left[ \frac{\partial L}{\partial \dot{\theta_1}} \right] = \left[(m_1 + m_2) l_1^2 + m_2 l_1 l_2 \cos \theta_2 \right] \ddot{\theta_1} + \left[ m_2 l_1 l_2 \cos \theta_2 \right] \ddot{\theta_2} - 2 m_2 l_1 l_2 \sin \theta_2 \dot{\theta_1} \dot{\theta_2} \]

4.4. Equació de Lagrange per \( \theta_2 \):

\[ \frac{\partial L}{\partial \theta_2} = m_2 l_2^2 (\dot{\theta_1} + \dot{\theta_2}) + m_2 l_1 l_2 \cos \theta_1 \dot{\theta_2} \]

4.5. Derivada temporal de \( \frac{\partial L}{\partial \dot{\theta_2}} \):

\[ \frac{d}{dt} \left[ \frac{\partial L}{\partial \dot{\theta_2}} \right] = m_2 l_2 (\ddot{\theta_1} + \ddot{\theta_2}) + m_2 l_1 l_2 \cos \theta_1 \ddot{\theta_2} - m_2 l_1 l_2 \sin \theta_2 \dot{\theta_1} \dot{\theta_2} \]

4.6. Equació Dinàmica:

\[ M(\theta) \ddot{\theta} + C(\theta, \dot{\theta}) \dot{\theta} + G(\theta) = \tau \]

On: \[ M(\theta) = \begin{bmatrix} (m_1 + m_2) l_1^2 + m_2 l_1 l_2 \cos \theta_2 & m_2 l_1 l_2 \cos \theta_2 \\ m_2 l_1 l_2 \cos \theta_2 & m_2 l_2^2 \end{bmatrix} \]

\[ C(\theta, \dot{\theta}) = \begin{bmatrix} -2 m_2 l_1 l_2 \sin \theta_2 \dot{\theta_2} - m_2 l_1 l_2 \sin \theta_2 \dot{\theta_2} \\ m_2 l_1 l_2 \sin \theta_2 \dot{\theta_1} \end{bmatrix} \]

\[ G(\theta) = \begin{bmatrix} - (m_1 + m_2) g l_1 \sin \theta_1 - m_2 g l_2 \sin (\theta_1 + \theta_2) \\ - m_2 g l_2 \sin (\theta_1 + \theta_2) \end{bmatrix} \]

On: \[ \tau = \text{torque applied} \]