## 2 Dynamic Modeling of 6R Robot System
### Dynamic Parameter Identification of 6R Robot System  
The dynamic parameters of the 6R robot include the mass \(m_{i}\) of each rigid body and the three principal moments of inertia \(J_{i_{1}i}^{(i)},J_{i_{2}i}^{(i)},I_{i_{3}i}^{(i)}\). The force element \(\mathbf{F}_{i}^{(i)}\) exerted by each joint and the moment \(\mathbf{M}_{i}^{(i)}\) of the force element \(\mathbf{F}_{i}^{(i)}\) to the center of mass and the angular velocity and angular acceleration of each rigid body and the acceleration of the center of mass of each rigid body are regarded as known quantities, dynamic parameters are regarded to be unknown quantities. We rewrote the dynamic equation into parameter identification form and identified it by using the least squares method. First, the center of mass motion equation Equation (22) is rewritten. The expansion of \(\mathbf{F}_{i}\) in Equation (22) is written as:  
\[\mathbf{F}_{i}=\mathbf{F}_{i}^{\rm g}-\sum\limits_{k=1}^{n_{e}}S_{ik}^{e}\mathbf{F}_{k}^{e} =\left[ \begin{array}{c}-\sum\limits_{k=1}^{n_{e}}S_{ik}^{e}\mathbf{F}_{k1}^{e}\\ -\sum\limits_{k=1}^{n_{e}}S_{ik}^{e}\mathbf{F}_{k2}^{e}\\ m_{i}g-\sum\limits_{k=1}^{n_{e}}S_{ik}^{e}\mathbf{F}_{k3}^{e}\end{array} \right]. \tag{40}\]  
Here, \(\mathbf{F}_{k1}^{e}\), \(\mathbf{F}_{k2}^{e}\) and \(\mathbf{F}_{k3}^{e}\) are the components of \(\mathbf{F}_{k}^{e}\) on \(\mathbf{e}_{1}^{(0)}\), \(\mathbf{e}_{2}^{(0)}\) and \(\mathbf{e}_{3}^{(0)}\) respectively. The expansion of Equation (22) is written as:  
\[m_{i}\left[ \begin{array}{cc}\ddot{\mathbf{x}}_{i}&\\ \ddot{\mathbf{y}}_{i}&\\ &\ddot{\mathbf{z}}_{i}\end{array} \right]=\left[ \begin{array}{c}-\sum\limits_{k= 1}^{n_{e}}S_{ik}^{e}\mathbf{F}_{k1}^{e}\\ -\sum\limits_{k=1}^{n_{e}}S_{ik}^{e}\mathbf{F}_{k2}^{e}\\ m_{i}g-\sum\limits_{k=1}^{n_{e}}S_{ik}^{e}\mathbf{F}_{k3}^{e}\end{array} \right] (i=1,2,\cdots,6). \tag{41}\]  
Since gravity \(\mathbf{F}_{i}^{\rm g}\) is a function of the unknown quantity \(m_{i}\) to be identified, move it to the left. Equation (41) is rewritten in parameter identification form as follows:  
\[\left[ \begin{array}{cc}\ddot{\mathbf{x}}_{i}&\\ \ddot{\mathbf{y}}_{i}&\\ &\ddot{\mathbf{z}}_{i}-g\end{array} \right]\left[ \begin{array}{c}m_{i}\\ m_{i}\\ m_{i}\end{array} \right]=\left[ \begin{array}{c}-\sum\limits_{k=1}^{n_{e}}S_{ ik}^{e}\mathbf{F}_{k1}^{e}\\ -\sum\limits_{k=1}^{n_{e}}S_{ik}^{e}\mathbf{F}_{k2}^{e}\\ -\sum\limits_{k=1}^{n_{e}}S_{ik}^{e}\mathbf{F}_{k3}^{e}\end{array} \right] (i=1,2,\cdots,6). \tag{42}\]  
Equation (42) is written in matrix form as follows:  
\[ \begin{array}{c}\dot{\mathbf{R}}_{i}\ \dot{\mathbf{u}}\dot{\mathbf{u}}_{i}=F_{-i}^{R}=A _{-}^{(0i)}F_{-i}^{R(i)}\ \ (i=1,2,\cdots,6).\end{array}  \tag{43}\]  
Here,  
\[ \begin{array}{c}\dot{\mathbf{u}}\underline{R}_{i}=\left[\begin{array}{cc}\ddot {\mathbf{x}}_{i}&\\ &\ddot{\mathbf{y}}_{i}&\\ &\ddot{\mathbf{z}}_{i}+g\end{array}\right],\ \ \dot{\mathbf{u}}\underline{m}_{i}=\left[ \begin{array}{c}m_{i}\\ m_{i}\end{array}\right],\ \ \dot{\mathbf{F}}_{-i}^{R}=\left[\begin{array}{c}-\sum\limits_{k=1}^{n_{e}}S_{ ik}^{e}\mathbf{F}_{k1}^{e}\\ -\sum\limits_{k=1}^{n_{e}}S_{ik}^{e}\mathbf{F}_{k2}^{e}\\ -\sum\limits_{k=1}^{n_{e}}S_{ik}^{e}\mathbf{F}_{k3}^{e}\end{array}\right].\end{array}  \tag{44}\]  
Second, the posture motion equation, Equation (30), is rewritten. The expansion of Equation (30) is written as:\[\left[ \begin{array}{ccc}I_{i_{11}}^{(i)}&0&0\\ 0&I_{i_{22}}^{(i)}&0\\ 0&0&I_{i_{33}}^{(i)}\end{array} \right]\left[ \begin{array}{c}\dot{\omega}_{i_{1 }}^{(i)}\\ \dot{\omega}_{i_{2}}^{(i)}\\ \dot{\omega}_{i_{3}}^{(i)}\end{array} \right]\;+\;\left[ \begin{array}{ccc}0&- \omega_{i3}^{(i)}&\omega_{i2}^{(i)}\\ \omega_{i3}^{(i)}&0&-\omega_{i1}^{(i)}\\ -\omega_{i2}^{(i)}&\omega_{i1}^{(i)}&0\end{array} \right]\left[ \begin{array}{ ccc}J_{i_{11}}^{(i)}&0&0\\ 0&I_{i_{22}}^{(i)}&0\\ 0&0&I_{i_{33}}^{(i)}\end{array} \right]\left[ \begin{array}{c}\omega_{i_{1}}^{ (i)}\\ \omega_{i2}^{(i)}\\ \omega_{i3}^{(i)}\end{array}