# Head-Raising of Snake Robots Based on a Predefined Spiral Curve Method
## 2 Modeling of Head-Raising Motion
### Coordinate System and Angle-Solving Algorithm  
The line segments cannot contain all the information of the snake robot links or modules. Therefore, we propose an approach on establishing the coordinate system fixed on line segments. Figure 5 shows the reference coordinate \(O_{x_{xyz}}\), and its origin is \([0,0,0]\), with \(x_{o}\)-axis \([1,0,0]\), \(y_{o}\)-axis \([0,1,0]\), and \(z_{o}\)-axis \([0,0,1]\). In determining the local coordinates, several rules are needed. Figure 6 shows how these rules are established and illustrates one link at step \(i\) during the phase-shifting process. The \({}^{i}y_{j}\) -axis is calculated as follows:  
\[{}^{i}y_{j}=\frac{{}^{i}p_{j+1}-{}^{i}p_{j}}{\left\|{}^{i}p_{j+1}-{}^{i}p_{j} \right\|} \tag{10}\]  
The \({}^{i}x_{j}\)- and \({}^{i}z_{j}\)-axes are on the plane that is vertical to the link vector shown in the left part of Figure 6. Therefore, the \({}^{i}x_{j}\)- and \({}^{i}z_{j}\)-axes are not unique, and the horizontal plane is introduced to solve this problem. Moving the horizontal plane to point \({}^{i}P_{j}\), the intersecting line between two planes is used to select the \({}^{i}x_{j}\)-axis, as shown in the right part of Figure 6. The intersecting line has two directions. Thus, the direction is selected by determining whether the angle between \({}^{i}x_{j}\)- and \(x_{o}\)-axes is acute. Afterward, the \({}^{i}z_{j}\)-axis is determined using the right-hand rule. Figure 7 shows the result of the local coordinates during the entire phase-shifting process. The simulation without showing the coordinates is discussed in the subsequent chapter. After the determination of the coordinate system, the relative rotation matrix between adjacent links can be obtained using Equation (11). \[{}^{i}R_{P\_rel_{j}}=(^{i}R_{P_{j+1}})^{-1}(^{i}R_{P_{j}})\quad j=2,3...,n\quad i =1,2,...,n_{step}\] \[{}^{i}R_{P_{j}}=\left[ \begin{array}{ccc}i_{x}j&{}^{i}y_{j}&{}^{i}z _{j}\end{array} \right]=\left[ \begin{array}{ccc}r_{11}&r_{12}&r_{13}\\ r_{21}&r_{22}&r_{23}\\ r_{31}&r_{32}&r_{33}\end{array} \right] \tag{11}\]  
where \({}^{i}R_{P_{j}}\in R^{3\times 3}\) is the orientation matrix of link \(j\) relative to the reference coordinate and \({}^{i}R_{P\_rel_{j}}\in R^{3\times 3}\) is the orientation matrix of link \(j\) relative to \({}^{i}R_{P_{j+1}}\). Then, the relative roll \((^{i}\gamma_{j})\), pitch \((^{i}\beta_{j})\), and yaw \((^{i}\alpha_{j})\) angle sequences can be calculated using Equation (12). \[{}^{i}\beta_{j} =A\tan 2(-r_{31},\sqrt{r_{11}{}^{2}+r_{21}{}^{2}})\quad\quad j=2,3..., n\quad i=1,2,...,n_{step}\] \[{}^{i}\alpha_{j} =Atan2(r_{21}/cos(^{i}\beta_{j}),r_{11}/cos(^{i}\beta_{j})) \tag{12}\] \[{}^{i}\gamma_{j} =Atan2(r_{32}/cos(^{i}\beta_{j}),r_{33}/cos(^{i}\beta_{j}))\]  
Figure 6: Establishment of local coordinates. Figure 7: Local coordinates during entire head-raising process. The line segments model for the snake robot is abstract, therefore we introduce a simplified Solidworks model for the subsequent simulation in Adams environment, as shown in Figure 8. It is part of Solidworks model and contains three modules. This figure provides us with the line segments model (right part) and Solidworks model (left part), and it is convenient for us to understanding where local coordinates are established and how the roll, pitch, and yaw angles are solved. As mentioned before, a line segment (\({}^{i}\!I_{j}\)) and a local coordinate (\({}^{i}\!P_{j}-{}^{i}\!x_{j}\!y_{j}\!^{i}\!z_{j}\)) represent a snake robot module, and a point (\({}^{i}\!P_{j}\)) represents a joint with three degrees of freedom. The three axes are located at centers of rotating shafts and intersected at the \({}^{i}\!P_{j}\). Connecting the \({}^{i}\!P_{j}\) and the \({}^{i}\!P_{j+1}\), the \({}^{i}\!I_{j}\) can be determined. Equation (11) and (12) provide us with the solution of roll, pitch and yaw angles.