# A Head Control Strategy of the Snake Robot Based on Segmented Kinematics
## 3 The Control Process of the Head
### The Stability of the Motion  
After the joint angles of the snake robot are obtained, the stability of the motion should be considered. In Section 2.1, the support polygon and zero-moment point are introduced. They are widely applied in the humanoid robot [32]. Moreover, [PERSON] et al. introduced the ZMP method to the head raising of the snake robot [9] for the first time. In this section, we combine the support polygon method and the zeros-moment point method for stability analysis. Before the head of the snake robot is lifted, we need to calculate the final configuration of the snake robot body and determine whether the mass center of the body inside the support polygon. Once the center of mass is inside the support polygon, the zero-moment point method needs to be used to judge the dynamic stability of the snake robot in the movement process. For the support polygon, the equations are as follows [32],  
\[\mathbf{c}=\frac{\sum_{i=1}^{N}m_{i}\mathbf{P}_{i}}{\sum_{i=1}^{n}m_{i}} \tag{23}\]  
\[\mathbf{C}=\text{polygon}(\mathbf{p}_{0},\ \mathbf{p}_{1},\ \ldots,\ \mathbf{p}_{k-1}) \tag{24}\]  
Figure 9: Several desired poses are generated by rotating a frame along the desired direction \(X_{d}\) evenly. Each of the solutions of these desired poses guarantees that the x-axis and origin \(O\) of the frame [X, Y, Z] are equal to the origin \(O_{d}\) and \(X_{d}\) axis of these desired poses. where \(m_{i}\) indicates the mass of the \(i\)-th link of the snake robot and \(\mathbf{P}_{i}\) represents the center of mass of \(i\)-th link. \(N\) denotes the number of links of the robot. \(\mathbf{c}\) is the center of the mass of the snake robot. \(\mathbf{p}_{i}\) is the endpoint of the link for the base part. The function polygon outputs a polygon constructed by the input points. The robot is statically stable if \(\mathbf{c}\in\mathbf{C}\). For the process of the motion for the robot, the ZMP method is used for dynamically stability for the snake robot. Since the ZMP method is based on the torque equilibrium, the linear velocity and the angular velocity of the link should be calculated first. Assuming that the linear velocity and angular velocity of the \(i\)-th link are known [32], then  
\[{}^{0}\mathbf{v}_{j}={}^{0}\mathbf{v}_{i}+{}^{0}\mathbf{\omega}_{i}\times{}^{0}\mathbf{R}_{i}{} ^{i}\mathbf{b}_{j} \tag{25}\]  
\[{}^{0}\mathbf{\omega}_{j}={}^{0}\mathbf{\omega}_{i}+{}^{0}\mathbf{R}_{i}\times{}^{i}\mathbf{z }_{j}\mathbf{\mu}_{j} \tag{26}\]  
where \({}^{0}\mathbf{v}_{j}\) and \({}^{0}\mathbf{\omega}_{j}\) are the linear velocity and angular velocity of the \(j\)-th link represented in the world frame, frame 0. The \({}^{i}\mathbf{z}_{j}\) is the z-axis of frame \(j\) represented in frame \(i\). \({}^{0}\mathbf{R}_{i}\) is the rotation matrix of the frame \(i\) relative to the frame 0. \({}^{i}\mathbf{b}_{j}\) is the joint position vector for \(j\)-th link represented in the \(i\)-th frame. \(q_{j}\) is the derivative of \(j\)-th joint angle relative to time. Next the velocities of the links are used to calculate the linear momentum and angular momentum of the robot [32],  
\[{}^{0}\dot{\mathbf{c}}_{j}={}^{0}\mathbf{v}_{j}+{}^{0}\mathbf{\omega}_{j}\times({}^{0}R_{ j}\overline{\varepsilon}_{j}) \tag{27}\]  
\[{}^{0}\mathcal{P}_{j}=m_{j}{}^{0}\dot{\mathbf{c}}_{j} \tag{28}\]  
\[{}^{0}\mathcal{P}=\sum_{j=1}^{N}{}^{0}\mathcal{P}_{j} \tag{29}\]  
\[{}^{0}\mathcal{L}_{j}={}^{0}\mathbf{c}_{j}\times{}^{0}\mathcal{P}_{j}+{}^{0}\mathbf{R }_{j}\overline{\mathbf{I}}{}^{0}\mathbf{R}_{j}^{\mathsf{T}0}\mathbf{\omega}_{j} \tag{30}\]  
\[{}^{0}\mathcal{L}=\sum_{j=1}^{N}{}^{0}\mathcal{L}_{j} \tag{31}\]  
where \(\overline{\varepsilon}_{j}\) indicates the coordinate of the center of mass for the \(j\)-th link represented in frame \(j\). In addition, \({}^{0}\dot{\mathbf{c}}_{j}\) denotes the derivative of the center of mass of the \(j\)-th respect to time represented in the world frame. \({}^{0}\mathcal{P}_{j}\) and \({}^{0}\mathcal{P}\) represent the linear momentum for \(i\)-th link and the robot respectively.