# A Head Control Strategy of the Snake Robot Based on Segmented Kinematics
## 2 The Strategy of the Head Control
### The Calculation of Joint Angles for the Neck Part  
The neck part connects the base part and the head part, so the joint angles of the neck part are determined by the base part and the head part. To ensure that the head link of the head part achieves the desired pose, we need to make sure that the dexterous workspace covers the desired pose. The position of the workspace varies with the base frame of the head part in the world frame. Thus,  
Figure 6: The workspace whose reachability index equals 100.  
Figure 7: The dexterous workspace can be divided 3 clusters using k-means++ method and the centroids of each cluster can be calculated.  
the determination of the base frame of the head part is essential. Meanwhile, the end frame of the neck part is the base frame of the head part.  
To calculate the end frame of the neck part, we denote the center of the dexterous point, the base of the head part and the end point of the base part as \(\mathbf{G}_{0}\), \(\mathbf{B}_{0}\) and \(\mathbf{P}_{0}\), shown in Figure 8, then  
\[L_{c}=\|\mathbf{B}_{0}-\mathbf{G}_{0}\| \tag{18}\]  
\[\mathbf{r}_{d}=\frac{\mathbf{G}_{0}-\mathbf{P}_{0}}{\|\mathbf{G}_{0}-\mathbf{P}_{0}\|} \tag{19}\]  
\[\mathbf{P}_{d}=\mathbf{G}_{0}-L_{c}\mathbf{r}_{d} \tag{20}\]  
where \(\mathbf{P}_{d}\) and \(\mathbf{r}_{d}\) are the desired position and desired direction of the end of the neck part, respectively. Once the end frame of the neck part achieves the desired position and direction, the dexterous workspace will cover the desired pose of the head part. So next the sum \(E_{n}\) of the error \(E_{p}\) between the origin of the end frame of the neck part and the desired position and the error \(E_{d}\) between the end link direction and the desire direction needs to be minimized. Suppose that the joint angles of the neck part are \(\theta_{k},\ \theta_{k+1},\ \ldots,\ \theta_{k+n_{c}-1}\), then  
\[\mathbf{T}_{a}={}^{0}\mathbf{T}_{k}(\theta_{k})^{k}\mathbf{T}_{k+1}(\theta_{k+1})\ldots ^{k+n_{c}-2}\mathbf{T}_{k+n_{c}-1}(\theta_{k+n_{c}-1}) \tag{21}\]  
\[\arg\min_{q_{i}\in\mathbb{R}^{n}}E_{n}=\|\mathbf{P}_{a}-\mathbf{P}_{d}\|_{2}^{2}+h\| \mathbf{r}_{d}\times\mathbf{r}_{a}\|_{2}^{2} \tag{22}\]  
\[s.t.\quad q_{i}<=q_{lim}^{i}\quad i=k,k+1,\ldots,k+n_{c}-1\]  
where \(\mathbf{T}_{a}\) is the transformation matrix of the frame for the end link of the neck part relative to the world frame, and \({}^{0}\mathbf{T}_{k}\) is the transformation matrix of \(k\)-th link relative to the world frame. The neck part has \(n_{c}\) links. \({}^{k-1}\mathbf{T}_{k}\) is the transformation matrix of \(k\)-th link relative to \((k-1)\)-th link for the neck part. Then \(\mathbf{P}_{a}\) and \(\mathbf{r}_{a}\) are the position and the x-axis vector of \(\mathbf{T}_{a}\) in the world frame. In addition, \(h\) is a coefficient. \(h\) can be set 0.01.  
The illustration of the strategy is shown in Figure 8. (1) Determine the endpoint of the base part, \(\mathbf{P}_{0}\). \(\mathbf{P}_{0}\) is determined by the discretization of the backbone curve of the base part and it is used as the base point of the neck part. (2) Calculate the desired direction for the x component of the end frame of the neck part. The direction from \(\mathbf{P}_{0}\) to \(\mathbf{G}_{0}\) is used as the desired direction, which is the desired x-axis direction of the end frame for the neck part. (3) Use the center of the dexterous workspace to estimate the desired position of the origin of the end frame for the neck part. The calculation of the desired  
Figure 8: The origin and x-axis of the end frame of the neck part approaches the desire position \(\mathbf{P}_{d}\) and the desired direction \(\mathbf{r}_{d}\).  
position refers to Equation (20). (4) Minimize the error \(E_{n}\) to determine the joint angles of the neck part. The objective function and the limitation conditions of the optimization refer to Equation (22). (5) Use the TRAC-IK algorithm to solve the joint angles of the head part considering the end frame of the neck part as the base frame of the head part.