# A Head Control Strategy of the Snake Robot Based on Segmented Kinematics
## 2 The Strategy of the Head Control
### The Workspace of Head Part
\begin{split}\arg\underset{\mathbf{q}\in\mathbb{R}^{n}}{\text{minsumsqr} }\text{(}\begin{bmatrix}\mathbf{R}_{d}&\mathbf{p}_{d}\\ \mathbf{0}&1\end{bmatrix}^{-1}\mathbf{f}(\mathbf{q})-k\mathbf{I})\\ s.t.&q_{i}\leq q_{\text{lim}}^{i},\quad i=1,\;2,\;3,\;\ldots,\;n\\ &\mathbf{f}(\mathbf{q})={{}^{0}\mathbf{T}_{1}}^{1}\mathbf{T}_{2}\cdots{{}^{n-1}\mathbf{T}_{n}} \end{split}  \tag{14}\]  
where \(\mathbf{R}_{d}\) is the \(3\times 3\) rotation matrix, which represents the desired orientation. In addition, \(\mathbf{p}_{d}\) is the desired position vector. \(\mathbf{f}(\mathbf{q})\) is a function of the forward kinematics with joint angle vector \(\mathbf{q}\), which is used to obtain the transformation matrix of the end effector of the robot described in the world frame. The symbol \({{}^{n-1}\mathbf{T}_{n}}\) indicates the transformation matrix of the \(n\)-th link relative to the \((n-1)\)-th link. In addition, the frame 0 represent the world frame in Equation (15). \(\mathbf{I}\), \(k\) and \(q_{\text{lim}}^{i}\) are an identity matrix, a gain coefficient and the joint limit of the \(i\)-th joint, respectively. Function sumsqr calculates sum of squared elements of the input matrix. For the nonlinear optimization problem, the active-set algorithm, SQP algorithm and interior-point algorithm can be selected [25]. For the configuration of the head part with 6 DOFs, the active-set algorithm, the interior-point algorithm and the SQP algorithm have 57.7%, 67.5% and 61.1% solve rate, respectively. For the configuration of the head part with 7 DOFs, the active-set algorithm, the interior-point algorithm and the SQP algorithm have 59.8%, 77.1% and 65.8% solve rate, respectively. The desired pose sample size and current pose sample size are both 1000 samples generated by the random joint angles within the joint limit, respectively. However, the optimization algorithm is difficult to guarantee that it always has global convergence. For the TRAC-IK inverse kinematics algorithm [18], the random restart of KDL called KDL-RR is used to solve the problem that it is stuck in local minima. TRAC-IK spawns the KDL-RR and SQP optimization inverse kinematics solver at the same time and run them for solving the inverse kinematics. The TRAC-IK solver will stop when either of the two solvers finishes with a solution or both exceed the set run time. The TRAC-IK enlarges the solution rate and shortens the solution time. For the configuration of the head part with 6 DOFs, TRAC-IK algorithm has 99.8% solve rate. For the configuration of the head part with 7 DOFs, TRAC-IK algorithm has 99.3% solve rate. The desired pose sample size and current pose sample size are also both 1000 samples generated by the random joint angles within the joint limit. By comparing the above results, we chose the TRAC-IK algorithm to calculate the workspace for the head part. The specific calculation method of the workspace is introduced in the literature [14]. However, for the heart part, as we know, it needs to have at least six DOFs to reach a specific pose. For simplifying the control of the head, we decide that the head part has six or seven DOFs. Then we calculate the workspace of the head part with a reachability measure. The algorithm is shown in Algorithm 1, and the symbols in Algorithm 1 are shown in Table 1.  
Through Algorithm 1, we calculate the reachability of the workspace of the head part with 7 DOFs and 6 DOFs, respectively. The workspace is calculated in the base frame of the head part. The reachability of the workspace with 6 DOFs is shown in Figure 4. The reachability of the workspace with 7 DOFs is shown in Figure 5. The gradient color from red to blue indicates that the reachability increases. The distributions of the reachability of the head part with 7 DOFs and 6 DOFs are shown in Table 2. The head part with 6 DOFs has the largest reachability index 76, then one with 7 DOFs has largest reachability index of 100 and 14.79% of the workspace has a reachability index above 90. Moreover, 3.69% of the workspace has a reachability index of 100.