# Locomotion Efficiency Optimization of Biologically Inspired Snake Robots
## 2 Modeling of Snake Robots
### Dynamic Model of Underwater Snake Robot  
The robot is assumed to consist of \(n\) rigid links with each of them having length \(2l\), mass \(m\) and moment of inertia \(J=\frac{1}{3}ml^{2}\). The links are interconnected by \(n-1\) joints. The center of mass (CM) of the link is defined as the middle point, since it is assumed that the mass is uniformly distributed, with the total mass of the USR given by \(nm\). The kinematics and dynamics of the robot will be described in terms of the mathematical symbols described in Table 1 and illustrated in Figures 1 and 2. The following vectors and matrices are used in the subsequent sections:  
\[\mathbf{A}=\left[ \begin{array}{cccc}1&1&&\\ &\ddots&\ddots&\\ &&1&1\end{array} \right]\,,\,\mathbf{D}=\left[ \begin{array}{cccc}1&-1&&\\ &\ddots&\ddots&\\ &&1&-1\end{array} \right]\,,\]  
where \(\mathbf{A},\mathbf{D}\in\mathbb{R}^{(n-1)\times n}\). Furthermore,  
\[\mathbf{e}=\left[ \begin{array}{cccc}1,&\ldots,&1\end{array} \right]^{T}\in \mathbb{R}^{n},\,\mathbf{E}=\left[ \begin{array}{cc}\mathbf{e}&\mathbf{0}_{n \times 1}\\ \mathbf{0}_{n\times 1}&\mathbf{e}\end{array} \right]\in\mathbb{R}^{2n\times 2}\,,\]  
\[\mathbf{S}_{\theta} = \mathrm{diag}(\sin\theta)\in\mathbb{R}^{n\times n},\qquad \mathbf{C}_{\theta}=\mathrm{diag}(\cos\theta)\in\mathbb{R}^{n\times n},\] \[\dot{\theta}^{2} = \left[ \begin{array}{cc}\theta_{1}^{\,2},&\ldots,&\theta_{n}^{\, 2}\end{array} \right]^{T}\in\mathbb{R}^{n}\,,\mathbf{K}=\mathbf{A}^{T}\left( \mathbf{D}\mathbf{D}^{T}\right)^{-1}\mathbf{D}.\]  
Figure 1: Kinematic parameters of the underwater snake robot. Figure 2: Forces and torques acting on each link of the underwater snake robot. The kinematics of the robot is derived for a motion in horizontal plane. We assume that the robot is fully immersed in water and has in total \(n+2\) degrees of freedom (DOF). The parameters \(\theta_{i}\) and \(\phi_{i}=\theta_{i}-\theta_{i-1}\) represent the link angle and the joint angle of each link, respectively. The link angles and the joint angles are grouped in \(\mathbf{\theta}=[\theta_{1},\dots,\theta_{n}]^{T}\in\mathbb{R}^{n}\) and \(\mathbf{\phi}=[\phi_{1},\dots,\phi_{n-1}]^{T}\in\mathbb{R}^{n-1}\). The position of the CM of the USR \(\mathbf{p}_{\text{CM}}\in\mathbb{R}^{2}\) can be calculated as:  
\[\mathbf{p}_{\text{CM}}=\left[ \begin{array}{c}p_{x}\\ p_{y}\end{array} \right]=\left[ \begin{array}{c}\frac{1}{nm}\sum_{i=1}^{n}mx_{ i}\\ \frac{1}{nm}\sum_{i=1}^{n}my_{i}\end{array} \right]=\frac{1}{n}\left[ \begin{array}[ ]{c}\mathbf{e}^{T}\mathbf{x}\\ \mathbf{e}^{T}\mathbf{y}\end{array} \right], \tag{1}\]  
where \((x_{i},y_{i})\) are the global frame coordinates of the CM of link \(i\), \(\mathbf{X}=[x_{1},\dots,x_{n}]^{T}\in\mathbb{R}^{n}\) and \(\mathbf{Y}=[y_{1},\dots,y_{n}]^{T}\in\mathbb{R}^{n}\). Regarding the hydrodynamic modeling, in [6], it is shown that the global frame forces due to interaction of the links with the water can be assembled in vector form as follows:  
\[\mathbf{f}=\left[ \begin{array}{c}\mathbf{f}_{\mathbf{x}}\\ \mathbf{f}_{\mathbf{y}}\end{array} \right]=\left[ \begin{array}{c}\mathbf{f}_{ \mathbf{A}_{\mathbf{x}}}\\ \mathbf{f}_{\mathbf{A}_{\mathbf{y}}}\end{array} \right]+\left[ \begin{array}{ c}\mathbf{f}_{\mathbf{D}_{\mathbf{x}}}^{\mathbf{\Pi}}\\ \mathbf{f}_{\mathbf{D}_{\mathbf{y}}}^{\mathbf{\Pi}}\end{array} \right], \tag{2}\]  
where the added mass effects \(\mathbf{f}_{\mathbf{A}_{\mathbf{x}}}\) and \(\mathbf{f}_{\mathbf{A}_{\mathbf{y}}}\) in \(x\) and \(y\) direction, respectively, are given by  
\[\left[ \begin{array}{c}\mathbf{f}_{\mathbf{A}_{\mathbf{x}}}\\ \mathbf{f}_{\mathbf{A}_{\mathbf{y}}}\end{array} \right]=-\left[ \begin{array}[] {cc}\mu_{n}\left(\mathbf{S}_{\theta}\right)^{2}&-\mu_{n}\mathbf{S}_{\theta} \mathbf{C}_{\theta}\\ -\mu_{n}\mathbf{S}_{\theta}\mathbf{C}_{\theta}&\mu_{n}\left(\mathbf{C}_{ \theta}\right)^{2}\end{array} \right]\left[ \begin{array}{c}\hat{\mathbf{X}} \\ \hat{\mathbf{Y}}\end{array}