# A Head Control Strategy of the Snake Robot Based on Segmented Kinematics
## 3 The Control Process of the Head
### The Trajectory Planning of the Overall Body in Cartesian Space  
After the joint positions of the robot are solved, A multi-segment trajectories interpolation method [17] for joint angles of the robot is adopted. However, the head of the snake robot sometimes will hit the ground and lose the balance of the body. The specific situation is shown in Figure 11. The initial state of the snake robot is like State 1, and then the body of the snake robot moves to State 2. The head of the robot will hit the ground since only the interpolations of the joint angles are considered.  
To solve the above problem, an interpolation method of the overall body of the snake robot based on the Cartesian space is proposed. The joint positions of the neck part and head part in the Cartesian space for the initial configuration are assumed \(\mathbf{p}_{s0}\), \(\mathbf{p}_{s1}\),..., \(\mathbf{p}_{sn-1}\), and ones for the final  
Figure 11: The initial state of the snake robot is like State 1 and the head of the robot hits the ground in State 2.  
Figure 10: The illustration of the ZMP and the torque for the stability analysis.  
configuration are assumed \(\mathbf{p}_{f0}\), \(\mathbf{p}_{f1}\),..., \(\mathbf{p}_{fn-1}\). First, \(\mathbf{p}_{si}\) and \(\mathbf{p}_{fi}\) is linear interpolated. However, the other interpolation methods, such as quadratic interpolation and cubic interpolation, can also be adopted. For simplifying the calculation, the linear interpolation is adopted. Then we have  
\[\begin{split}\mathbf{p}_{ij}&=\mathbf{p}_{fi}+(1-t_{j}) \mathbf{p}_{si},\quad i=0,1,2,\ldots,n-1\quad j=0,1,\\ 2,\ldots,m\quad 0\leq t_{0}\leq t_{1}\leq\ldots\leq t_{j}\leq \ldots\leq t_{m}\leq 1\end{split} \tag{41}\]  
where \(\mathbf{p}_{ij}\) represents the interpolation position of \(i\)-th joint corresponding parameter \(t_{j}\). In addition, every \(t_{j}\) can be used to interpolate a configuration of the snake robot.  
Assuming the transformation matrix of the starting frame of the neck part is \(\mathbf{T}_{0}\) and the D-H frames \(\{x_{i},\ y_{i},\ z_{i}\}\) are constructed as shown in Figure 12, the links of the neck part and head part approach the line segments generated by the linear interpolation. Then we have  
\[\theta_{i}=\pi/2-\arccos(\mathbf{T}_{y}\cdot\frac{\mathbf{l}_{\text{proj}}^{i}}{\|\mathbf{l }_{\text{proj}}^{i}\|})\quad i=0,\ 1,\ 2,\ \ldots,\ n-1 \tag{42}\]  
\[\mathbf{l}_{\text{proj}}^{i}=\mathbf{l}_{i}-(\mathbf{l}_{i}\cdot\mathbf{T}_{z})\mathbf{T}_{z}\quad i =0,\ 1,\ 2,\ \ldots,\ n-1 \tag{43}\]  
\[\mathbf{l}_{i}=\frac{\mathbf{p}_{i+1j}-\mathbf{p}_{ij}}{\|\mathbf{p}_{i+1j}-\mathbf{p}_{ij}\|}\quad i =0,\ 1,\ 2,\ \ldots,\ n-1 \tag{44}\]  
where \(\theta_{i}\) denotes the \(i\)-th joint angles from the neck part, and \(\mathbf{T}_{i}\) indicates the transformation matrix of the \(i\)-th joint frame relative to the world frame. \(\mathbf{T}_{y}\) and \(\mathbf{T}_{z}\) are the y-axis vector and z-axis vector of the local frame of \(\mathbf{T}_{i}\) for \(i\)-th joint. \".\" is the dot product operator. arccos is represents the inverse trigonometric function of cosine. The interpolation result of the proposed method is shown in Figure 13.  
Figure 12: The D-H convention of the neck part and head part for the snake robot, and the illustration of the line segments generated by the linear interpolation and the links.  
Figure 13: The illustration of the proposed interpolation method of the overall body of the snake robot in Cartesian space. The direction of the movement is shown as the dotted line.