# A Head Control Strategy of the Snake Robot Based on Segmented Kinematics
## 2 The Strategy of the Head Control
### The Workspace of Head Part  
To simplify the control of the snake robot and ensure the head of the snake robot can achieve the desired position and posture, we consider the head part has six or seven DOFs. Furthermore, the head of the snake robot can approach it if the workspace of the head part can cover the desired pose. Then the joint angles of the head part can be determined. To get the workspace of the head part of the snake robot, we need a reliable inverse kinematics algorithm. Aim to get the ability of motion in the 3-dimension space and imitate the biological snake, the snake robot has the configuration that the joint axes are orthogonal to each other. However, the inverse kinematics of the head part with six or seven DOFs has no analytical solution for this configuration. Then some numerical algorithms such as the KDL algorithm [15; 16], the inverse kinematics algorithm based on optimization [17] and TRAC-IK algorithm [18], are chosen. For the KDL algorithm, it is a feedback and iteration process. (1) Forward Kinematics is used to compute the current pose of the end effector in the Cartesian space through the current joint angles \(q_{0}\), (2) the linear interpolation method and the spherical linear interpolation method are used for the interpolation of the Cartesian position and the orientation represented by the unit quaternion and the desired linear velocity \(\mathbf{v}_{d}\) and angular velocity \(\mathbf{\omega}_{d}\) can be obtained, (3) the error \(E_{p}\) between the desired pose and the current pose of the end effector is calculated, \(E_{p}=[\mathbf{e}_{p},\mathbf{e}_{0}]\), where \(\mathbf{e}_{p}\) and \(\mathbf{e}_{o}\) represent the position and orientation error of the end effector, (4) the inverse of the Jacobian, \(\mathbf{J}^{-1}\) convert the velocity of the end effector to the joint angular velocity. The equations used in the KDL iteration algorithm is given [16],  
\[\dot{q}_{m-1}=\mathbf{J}^{-1}(q_{m-1}) \begin{pmatrix}\mathbf{v}_{d}+\mathbf{K}_{p}\mathbf{e}_{ p}\\ \mathbf{\omega}_{d}+\mathbf{K}_{o}\mathbf{e}_{o}\end{pmatrix}  \tag{10}\]  
\[\mathbf{q}_{m}=\mathbf{q}_{m-1}+\dot{q}_{m-1}\Delta t \tag{11}\]  
\[\mathbf{e}_{p}=\mathbf{p}_{d}-\mathbf{p}_{e}(\mathbf{q}_{m}) \tag{12}\]  
\[\mathbf{e}_{o}=\eta_{e}(\mathbf{q}_{m})\mathbf{e}_{d}-\eta_{d}\mathbf{e}_{e}(\mathbf{q}_{m})-\mathbf{e} _{d}\times\mathbf{e}_{e}(\mathbf{q}_{m}) \tag{13}\]  
where \(\mathbf{p}_{d}\) and \(\mathbf{p}_{e}(\mathbf{q}_{m})\) are the desired position and the calculated position of the end effector, respectively. \(\{\eta_{d},\mathbf{e}_{d}\}\) and \(\{\eta_{e},\mathbf{e}_{e}\}\) are the desired orientation and computed orientation of the end effector represented by the unit quaternion, respectively. \(\mathbf{K}_{p}\) and \(\mathbf{K}_{o}\) are gain matrices, which are \(n\times n\) diagonal matrices. They should be selected properly to avoid the divergence of the iteration process. \(m\) represents the times of the iteration. When \(\mathbf{E}_{p}\approx 0\), the iteration of the algorithm stops, and the result is output. However, result of the joint angle is usually beyond the joint limit though the classical avoiding joint limit method, the gradient projection method is adopted [24]. Moreover, the KDL method is also easily stuck in the local minima. For the configuration of the head part with 6 DOFs, KDL algorithm has 62.9% solve rate. For the configuration of the head part with 7 DOFs, KDL algorithm has 73.5%  
Figure 3: The discretization result of the plane arc backbone curve. The symbol ’bc’ means backbone curve. solve rate. The desired pose sample size and current pose sample size are both 1000 samples generated by the random joint angles within the joint limit. For the inverse kinematics algorithm based on the optimization, it can guarantee that the joint angles are within the joint limit. At first, we need to consider an objective function [17],  
\[