4.3 Lagrange Equations
4.3.1 Lagrange Eqations
여기서 $U = U(\textbf{q})$이므로,
\[\frac{d}{dt}(\frac{\partial K}{\partial \dot{\textbf{q}}})-\frac{\partial K}{\partial \textbf{q}} + \frac{\partial U}{\partial \textbf{q}} = \boldsymbol{\tau}\]좌변의 왼쪽 두 항은 inertial forces, 세 번째 항은 gravity vector가 된다. Inertial force들만을 남기면,
\[\frac{d}{dt}(\frac{\partial K}{\partial \dot{\textbf{q}}})-\frac{\partial K}{\partial \textbf{q}} = \boldsymbol{\tau} - \textbf{G}, \qquad \textbf{G}=\frac{\partial U}{\partial \textbf{q}}\]이제 우리가 원하던 식이 나왔다.
\[\textbf{M}(\textbf{q})\ddot{\textbf{q}} + \textbf{V}(\textbf{q}, \dot{\textbf{q}}) = \boldsymbol{\tau} - \textbf{G}(\textbf{q})\]4.3.2 Kinetic Energy
초기 위치에서 현재 위치까지 system을 움직인 외력이 한 일을 나타내자. 이는 Kinetic energy로 쓰일 수 있으며, quadratic form으로 표현된다.
\[K_{trans} = \frac{1}{2}m \textbf{v}^T \textbf{v} \\ K_{rot} = \frac{1}{2} \boldsymbol{\omega}^T \textbf{I}_c \boldsymbol{\omega}\]이는 각각 link의 center of mass의 liner velocity에 기인한 kinetic energy와 link의 angular velocity에 기인한 kinetic energy. 전체 manipulator의 kinetic energy는
\[K_i = \frac{1}{2}(m_i \textbf{v}_{ci}^T \textbf{v}_{ci} + \boldsymbol{\omega}_i^T \textbf{I}_{ci} \boldsymbol{\omega}_i) \\ K = \sum_{i=1}^n K_i\]4.3.3 2R robot
Kinetic energy를 계산하자.
\[K_i = \frac{1}{2}(m_i \textbf{v}_{ci}^T \textbf{v}_{ci} + \boldsymbol{\omega}_i^T \textbf{I}_{ci} \boldsymbol{\omega}_i)\]- $i=1$
- $i=2$
Link 2의 center of mass부터,
\[{}^{0}\textbf{P}_{g2} = \begin{bmatrix} l_1c_1 + l_{g2}c_{12} \\ l_1s_1 + l_{g2}s_{12} \\ 0 \end{bmatrix}\]미분하고 제곱.
\[\textbf{v}_{c2}^T \textbf{v} = l_1^2 \dot{\theta}_1^2 + l_{g2}^2(\dot{\theta}_1 + \dot{\theta}_2)^2 + 2l_1l_{g2}c_2(\dot{\theta}_1^2 + \dot{\theta}_1\dot{\theta}_2)\]그러므로 kinetic energy는
\[K_2 = \frac{1}{2}m_2[ l_1^2 \dot{\theta}_1^2 + l_{g2}^2(\dot{\theta}_1 + \dot{\theta}_2)^2 + 2l_1l_{g2}c_2(\dot{\theta}_1^2 + \dot{\theta}_1\dot{\theta}_2)] + \frac{1}{2} I_2 (\dot{\theta}_1 + \dot{\theta}_2)^2\]다음은 potential energy다.
\[U_i = m_igh_i\] \[U_1 = m_1gl_{g1}s_1 \\ U_2 = m_2g(l_1s_1 + l_{g2}s_{12})\]이로부터 Lagrangian을 얻는다.
\[L=K_1+K_2-U_1-U_2\]이제 운동 방정식을 해결하자. 2R manipulator이므로 미분에 사용되는 변수는 $\theta_1$과 $\theta_2$이다.
\[\boldsymbol{\tau}_1 = \frac{d}{dt}(\frac{\partial K}{\partial \dot{\theta}_1})-\frac{\partial K}{\partial \theta_1} + \frac{\partial U}{\partial \theta_1} \\ \boldsymbol{\tau}_2 = \frac{d}{dt}(\frac{\partial K}{\partial \dot{\theta}_2})-\frac{\partial K}{\partial \theta_2} + \frac{\partial U}{\partial \theta_2}\]열심히 계산하면…
\[\tau_1 =\]4.3.4 Generalization to N DOF Manipulator
전체 kinetic energy를 어떤 generalized coordinate에서의 quadratic form으로 표현하자.
\[K_i = \frac{1}{2}(m_i \textbf{v}_{ci}^T \textbf{v}_{ci} + \boldsymbol{\omega}_i^T \textbf{I}_{ci} \boldsymbol{\omega}_i) = \frac{1}{2} \dot{\textbf{q}}^T \textbf{M} \dot{\textbf{q}}\]여기서 $\textbf{q}$는 generalized coordinates, $\dot{\textbf{q}}$는 generalized velocity를 의미한다. $\textbf{M}$가 뭔지는 잠시 제쳐 두자. 아마 generalized mass 비슷한 무언가일 것이다.
각 link의 속도와 각속도를 generalized velocity를 사용해 표현할 수 있다. Jacobian이 필요.
\[\textbf{v}_{ci} = \textbf{J}_{vi}\dot{\textbf{q}} \\ \boldsymbol{\omega}_{ci} = \textbf{J}_{\omega i}\dot{\textbf{q}}\] \[\textbf{J}_{vi} = \begin{bmatrix} \frac{\partial \textbf{P}_{c1}}{\partial q_1} & \cdots & \frac{\partial \textbf{P}_{ci}}{\partial q_i} & \textbf{0} & \cdots & \textbf{0} \end{bmatrix} \\ \\ \textbf{J}_{\omega i} = \begin{bmatrix} \hat{\textbf{z}}_0 & \cdots & \hat{\textbf{z}}_{i-1} & \textbf{0} & \cdots & \textbf{0} \end{bmatrix}\]Link $i$의 속도 및 가속도는 Link $1$부터 Link $i$까지의 generalized velocity에 의해 결정될 테니까. 아무튼 이로부터 kinetic energy가 generalized coordinate의 quadratic form 표현된다.
\[K = \frac{1}{2} \dot{\textbf{q}}^T \textbf{M} \dot{\textbf{q}} = \frac{1}{2} \dot{\textbf{q}}^T \left[ \sum_{i=1}^n(m_i\textbf{J}_{vi}^T\textbf{J}_{vi} + \textbf{J}_{\omega i}^T \textbf{I}_{ci}\textbf{J}_{\omega i}) \right]\dot{\textbf{q}}\]여기서 등장하는 것이 Mass matrix $\textbf{M}$.
\[\textbf{M} = \sum_{i=1}^n(m_i\textbf{J}_{vi}^T\textbf{J}_{vi} + \textbf{J}_{\omega i}^T \textbf{I}_{ci}\textbf{J}_{\omega i})\]이 mass matrix가 manipulator의 운동 방정식에 등장한 바로 그거 맞다.
\[\boldsymbol{\tau} = \textbf{M}(\textbf{q})\ddot{\textbf{q}} + \textbf{V}(\textbf{q}, \dot{\textbf{q}}) + \textbf{G}(\textbf{q})\]4.3.5 Mass Matrix
여기서 잠시 mass matrix에 대해 짚고 넘어가자.
\[\begin{align*} \textbf{M} &= \sum_{i=1}^n(m_i\textbf{J}_{vi}^T\textbf{J}_{vi} + \textbf{J}_{\omega i}^T \textbf{I}_{ci}\textbf{J}_{\omega i}) \\ &= \begin{bmatrix} m_{11} & m_{12} & \cdots & m_{1n} \\ m_{21} & m_{22} & \cdots & m_{2n} \\ \vdots & \vdots & \vdots & \vdots \\ m_{n1} & m_{n2} & \cdots & m_{nn} \\ \end{bmatrix} \end{align*}\]Mass matrix의 대각 성분은 무엇을 의미할까? 바로 manipulator가 움직이지 않았을 때 각 link의 moment of inertia.
4.3.6 Inertial Forces
Generalized coordiate에서 계산한 kinetic energy를 Lagrangian formulation에 집어넣자.
\[\frac{d}{dt}(\frac{\partial K}{\partial \dot{\textbf{q}}})-\frac{\partial K}{\partial \textbf{q}} = \boldsymbol{\tau} - \textbf{G} \\\] \[\frac{\partial K}{\partial \dot{\textbf{q}}} = \frac{\partial}{\partial \dot{\textbf{q}}} \left[ \frac{1}{2} \dot{\textbf{q}}^T \textbf{M} \dot{\textbf{q}} \right] = \textbf{M} \dot{\textbf{q}} \\ \frac{d}{dt}(\frac{\partial K}{\partial \dot{\textbf{q}}}) = \frac{d}{dt}(\textbf{M} \dot{\textbf{q}}) = \textbf{M}\ddot{\textbf{q}} + \dot{\textbf{M}}\dot{\textbf{q}}\] \[\frac{d}{dt}(\frac{\partial K}{\partial \dot{\textbf{q}}})-\frac{\partial K}{\partial \textbf{q}} = \textbf{M}\ddot{\textbf{q}} + \dot{\textbf{M}}\dot{\textbf{q}} - \frac{1}{2} \begin{bmatrix} \dot{\textbf{q}}^T \frac{\partial \textbf{M}}{\partial q_1} \dot{\textbf{q}} \\ \vdots \\ \dot{\textbf{q}}^T \frac{\partial \textbf{M}}{\partial q_n} \dot{\textbf{q}} \end{bmatrix}\]뒤쪽 두 항이 바로 Centrifugal & Coriolis term이 된다.
\[\textbf{V}(\textbf{q}, \dot{\textbf{q}}) = \dot{\textbf{M}}\dot{\textbf{q}} - \frac{1}{2} \begin{bmatrix} \dot{\textbf{q}}^T \frac{\partial \textbf{M}}{\partial q_1} \dot{\textbf{q}} \\ \vdots \\ \dot{\textbf{q}}^T \frac{\partial \textbf{M}}{\partial q_n} \dot{\textbf{q}} \end{bmatrix}\]이렇게 운동 방정식이 완성.
\[\textbf{M}(\textbf{q})\ddot{\textbf{q}} + \textbf{V}(\textbf{q}, \dot{\textbf{q}}) + \textbf{G}(\textbf{q}) = \boldsymbol{\tau}\]4.3.7 Centrifugal and Coriolis Force
2 DOF planar manipulator의 경우 equations of motion을 행렬 형태로 표현하면
\[\begin{bmatrix} m_{11} & m_{12} \\ m_{21} & m_{22} \end{bmatrix} \begin{bmatrix} \ddot{q}_1 \\ \ddot{q}_2 \end{bmatrix} + \begin{bmatrix} V_1 \\ V_2 \end{bmatrix} + \begin{bmatrix} G_1 \\ G_2 \end{bmatrix} = \begin{bmatrix} \tau_1 \\ \tau_2 \end{bmatrix}\]Centrifugal and coriolis force $\textbf{V}$에 대해 자세히 파헤쳐보자.
\[\textbf{V} = \dot{\textbf{M}}\dot{\textbf{q}} - \frac{1}{2} \begin{bmatrix} \dot{\textbf{q}}^T \frac{\partial \textbf{M}}{\partial q_1} \dot{\textbf{q}} \\ \vdots \\ \dot{\textbf{q}}^T \frac{\partial \textbf{M}}{\partial q_n} \dot{\textbf{q}} \end{bmatrix} = \begin{bmatrix} \dot{m_{11}} & \dot{m_{12}} \\ \dot{m_{21}} & \dot{m_{22}} \end{bmatrix} \dot{\textbf{q}} - \frac{1}{2} \begin{bmatrix} \dot{\textbf{q}}^T \begin{bmatrix} m_{111} & m_{121} \\ m_{211} & m_{221} \end{bmatrix} \dot{\textbf{q}} \\ \dot{\textbf{q}}^T \begin{bmatrix} m_{112} & m_{122} \\ m_{212} & m_{222} \end{bmatrix} \dot{\textbf{q}} \end{bmatrix}\]여기서
\[\dot{m}_{11}=m_{111}\dot{q}_1+m_{112}\dot{q}_2 \\ m_{111} = \frac{\partial m_{11}}{\partial q_1}\]위 행렬을 풀면,
\[\textbf{V} = \begin{bmatrix} \frac{1}{2}(m_{111}+m_{111}-m_{111}) & \frac{1}{2}(m_{122}+m_{122}-m_{221}) \\ \frac{1}{2}(m_{211}+m_{211}-m_{112}) & \frac{1}{2}(m_{222}+m_{222}-m_{222}) \end{bmatrix} \begin{bmatrix} \dot{q}_1^2 \\ \dot{q}_2^2 \end{bmatrix} + \begin{bmatrix} m_{112} + m_{121} - m_{121} \\ m_{212} + m_{221} - m_{122} \end{bmatrix} \begin{bmatrix} \dot{q}_1 \dot{q}_2 \end{bmatrix}\]식을 깔끔하게 하기 위해 Christoffel Symbol을 사용한다.
\[b_{ijk} = \frac{1}{2}(m_{ijk}+m_{ikj}-m_{jki})\] \[\textbf{V} = \begin{bmatrix} b_{111} & b_{122} \\ b_{211} & b_{222} \end{bmatrix} \begin{bmatrix} \dot{q}_1^2 \\ \dot{q}_2^2 \end{bmatrix} + \begin{bmatrix} 2b_{112} \\ 2b_{212} \end{bmatrix} \begin{bmatrix} \dot{q}_1 \dot{q}_2 \end{bmatrix} = \textbf{C}(\textbf{q}) \begin{bmatrix} \dot{q}_1^2 \\ \dot{q}_2^2 \end{bmatrix} + \textbf{B}(\textbf{q}) \begin{bmatrix} \dot{q}_1 \dot{q}_2 \end{bmatrix}\]일반적인 $N$ DOF manipulator에서는,
\[\textbf{C}(\textbf{q}) \begin{bmatrix} \dot{q}_1^2 \\ \dot{q}_2^2 \\ \vdots \\ \dot{q}_n^2 \end{bmatrix} = \begin{bmatrix} b_{111} & b_{122} & \cdots & b_{1nn} \\ b_{211} & b_{222} & \cdots & b_{2nn} \\ \vdots & \vdots & \vdots & \vdots \\ b_{n11} & b_{n22} & \cdots & b_{nnn} \\ \end{bmatrix} \begin{bmatrix} \dot{q}_1^2 \\ \dot{q}_2^2 \\ \vdots \\ \dot{q}_n^2 \end{bmatrix}\] \[\textbf{B}(\textbf{q}) \begin{bmatrix} \dot{q}_1 \dot{q}_2 \\ \dot{q}_2 \dot{q}_3 \\ \vdots \\ \dot{q}_{(n-1)} \dot{q}_n \end{bmatrix} = \begin{bmatrix} 2b_{112} & 2b_{113} & \cdots & 2b_{1(n-1)n} \\ 2b_{212} & 2b_{213} & \cdots & 2b_{2(n-1)n} \\ \vdots & \vdots & \vdots & \vdots \\ 2b_{n12} & 2b_{n13} & \cdots & 2b_{n(n-1)n} \\ \end{bmatrix} \begin{bmatrix} \dot{q}_1 \dot{q}_2 \\ \dot{q}_2 \dot{q}_3 \\ \vdots \\ \dot{q}_{(n-1)} \dot{q}_n \end{bmatrix}\]이렇게 centrifugal term과 Coriolis term을 얻는다.
4.3.8 Potential Energy
마지막으로 Link $i$의 potential energy $\textbf{p}_i$.는 다음과 같다.
\[\begin{align*} U_i &= m_ig_oh_i + U_o \\ &= m_i(-\textbf{g}^T \textbf{p}_{ci}) \end{align*} \\ U = \sum_{i-1}^n U_i\]여기서 gravity vector는
\[G_j = \frac{\partial U}{\partial q_j} = - \sum_{i=1}^n (m_i \textbf{g}^T \frac{\partial \textbf{p}_{ci}}{\partial q_i})\] \[\textbf{G} = - \begin{bmatrix} \textbf{J}_{\nu 1}^T & \textbf{J}_{\nu 2}^T & \cdots & \textbf{J}_{\nu n}^T \end{bmatrix} \begin{bmatrix} m_1 \textbf{g} \\ m_2 \textbf{g} \\ \vdots \\ m_n \textbf{g} \end{bmatrix}\]
각 link가 받는 gravitational force에 Jacobian을 곱했으므로 joint torque가 된다.