2.1. Kinematics of the underwater snake robot

Figure 2 shows the motion of a snake robot with $n$ links and $(n-1)$ joints in the X-Y plane.

Figure 2. Motion of the snake robot in X-Y plane.

The planar kinematics is similar to the existing snake robot [Reference Pettersen4]. The snake robot motion in the X-Y plane can be considered as shown in Fig. 2. The angle $\theta _i$ is the orientation of link ‘ $i$ ’ with respect to global $X$ -axis, angle $\phi _i$ is the joint angle between two adjacent links and the centre of mass (CM) position of a link ‘ $i$ ’ describes as $(x_i,y_i)$ . So, it has $(n+2)$ DOF which can be described as $n$ -link angles $(\theta _i)$ and $(p_x,p_y)$ CM position of snake robot. The CM position $(p_x,p_y)$ of the snake robot can be expressed as follows:

(1) \begin{equation} \mathbf{p_{xy}} = \begin{bmatrix} p_x \\ p_y \end{bmatrix} = \frac{1}{n} \begin{bmatrix} \mathbf{e}^{T}\mathbf{X} \\ \mathbf{e}^{T}\mathbf{Y} \end{bmatrix}, \end{equation}

where $\mathbf{X} = [x_1, \cdots \cdots, x_n]^T\in \mathbb{R}^{n}$ , $\mathbf{Y} = [y_1, \cdots \cdots, y_n]^T \in \mathbb{R}^{n}$ and $\mathbf{e} = [1, \cdots \cdots, 1]^T, \in \mathbb{R}^{n}$ are addition vectors. The geometric constraint of the robot can be described as

(2a) \begin{align} \mathbf{DX} + l\mathbf{A}\cos{\boldsymbol{\theta }}=0, \end{align}

(2b) \begin{align} \mathbf{DY} + l\mathbf{A}\sin{\boldsymbol{\theta }}=0, \end{align}

where $\mathbf{A} = [\mathbf{I}_{(n-1)}, \mathbf{0}_{(n-1,1)}] + [\mathbf{0}_{(n-1,1)}, \mathbf{I}_{(n-1)}]\in \mathbb{R}^{(n-1)\times n}$ is the addition matrix, $\mathbf{D} = [\mathbf{I}_{(n-1)}, \mathbf{0}_{(n-1,1)}] - [\mathbf{0}_{(n-1,1)}, \mathbf{I}_{(n-1)}]\in \mathbb{R}^{(n-1)\times n}$ is the subtraction matrix, $\sin \boldsymbol{\theta } = [\sin \theta _1, \cdots \cdots, \sin \theta _n]^T \in \mathbb{R}^n \quad \text{and}\quad \cos \boldsymbol{\theta } = [\cos \theta _1, \cdots \cdots, \cos \theta _n]^T \in \mathbb{R}^{n}$ . By using Eqs. (1) and (2), the final position of each link in the X-Y plane can be written as

(3a) \begin{align} \mathbf{X} = -l\mathbf{K}^{T}\cos{\boldsymbol{\theta }} + \mathbf{e}p_x, \end{align}

(3b) \begin{align} \mathbf{Y} = -l\mathbf{K}^{T}\sin{\boldsymbol{\theta }} + \mathbf{e}p_y, \end{align}

where $\mathbf{K} = \mathbf{A}^{T}(\mathbf{DD}^T)^{-1}\mathbf{D}, \in \mathbb{R}^{n \times n}$ . Therefore, the velocity and acceleration of the links are given below:

(4a) \begin{align} \dot{\mathbf{X}} &= l\mathbf{K}^{T}\mathbf{S}_{\theta }{\dot{\boldsymbol{\theta }}} + \mathbf{e}{\dot{p}_x}, \end{align}

(4b) \begin{align} \dot{\mathbf{Y}} &= -l\mathbf{K}^{T}\mathbf{C}_{\theta }\dot{\boldsymbol{\theta }} + \mathbf{e}{\dot{p}_y}, \end{align}

(4c) \begin{align} \ddot{\mathbf{X}} &= l\mathbf{K}^{T}\mathbf{C}_{\theta }diag(\boldsymbol{\dot{\theta }})\boldsymbol{\dot{\theta }} +l\mathbf{K}^{T}\mathbf{S}_{\theta }\boldsymbol{\ddot{\theta }} + \mathbf{e}\ddot{p}_x, \end{align}

(4d) \begin{align} \ddot{\mathbf{Y}} &= l\mathbf{K}^{T}\mathbf{S}_{\theta }diag(\boldsymbol{\dot{\theta }})\boldsymbol{\dot{\theta }} -l\mathbf{K}^{T}\mathbf{C}_{\theta }\boldsymbol{\ddot{\theta }} + \mathbf{e}\ddot{p}_y, \end{align}

where $\boldsymbol{S_\theta } = diag(\sin \boldsymbol{\theta }) \in \mathbb{R}^{n\times n}$ is the sine diagonal matrix, $\boldsymbol{C_\theta } = diag(\cos \boldsymbol{\theta }) \in \mathbb{R}^{n\times n}$ is the cosine diagonal matrix and $\boldsymbol{\theta } = [\theta _1\ldots \ldots .\theta _n]^T \in \mathbb{R}^n$ is the link angle vector. The relative tangential velocity ( $v_{t,r}$ ) and normal velocity ( $v_{n,r}$ ) of the snake robot head link in the X-Y direction can be given as follows:

(5) \begin{equation} \begin{bmatrix} v_{t,r}\\ v_{n,r} \end{bmatrix}= \mathbf{R}_{\theta _n}^T\left (\dot{\mathbf{p}}_{xy}-\mathbf{v}_c\right ), \end{equation}

where $\mathbf{v}_c = [v_{cx},v_{cy}]^T$ . $v_{cx}$ and $v_{cy}$ are the water current velocity in $X$ and $Y$ direction, respectively, and $\dot{\mathbf{p}}_{xy}$ is the velocity of the centre of mass. The rotation matrix $ R_{\theta _n} = \begin{bmatrix} \cos \theta _n & -\sin \theta _n\\ \sin \theta _n & \cos \theta _n \end{bmatrix}$ . These kinematic relations are for the snake robot’s planar motion, which moves forward in the X-Y plane. The relative velocity of each link in the X-Y plane can be given as follows:

(6) \begin{equation} \begin{bmatrix} \mathbf{V}_{rx}\\ \mathbf{V}_{ry} \end{bmatrix}= \begin{bmatrix} \mathbf{C}_{\theta } & \mathbf{S}_{\theta }\\ -\mathbf{S}_{\theta } & \mathbf{C}_{\theta } \end{bmatrix} \begin{bmatrix} \dot{\mathbf{X}}-\mathbf{V}_{cx}\\ \dot{\mathbf{Y}}-\mathbf{V}_{cy} \end{bmatrix}, \end{equation}

where the relative velocity of each link can be described as $\mathbf{V}_{rx} = [v_{rx,1}, \dots,v_{rx,n}] \in \mathbb{R}^n$ , $\mathbf{V}_{ry} = [v_{ry,1}, \dots,v_{ry,n}]\in \mathbb{R}^n$ , and the water current velocity in X-Y direction of each link can be expressed as $\mathbf{V}_{cx} = [v_{cx,1}, \dots,v_{cx,n}] \in \mathbb{R}^n$ , $\mathbf{V}_{cy} = [v_{cy,1}, \dots,v_{cy,n}]\in \mathbb{R}^n$ . The relative acceleration is the derivative of Eq. (6), and it can be expressed as follows:

(7) \begin{equation} \dot{\mathbf{V}}_r = \mathbf{K}_1 \mathbf{N}_2 + \mathbf{K}_1 \mathbf{SC}_{\theta } \ddot{\boldsymbol{\theta }} + \mathbf{K}_1 \mathbf{E}\ddot{\mathbf{p}}_{xy} - \mathbf{K}_1 + \mathbf{K}_2 \mathbf{N}_1, \end{equation}

where

\begin{align*} \dot{\mathbf{V}}_{r} &=[\dot{\mathbf{V}}_{rx},\dot{\mathbf{V}}_{ry}]^T,\\ \mathbf{K}_1 &= \begin{bmatrix} \mathbf{C}_{\theta } & \mathbf{S}_{\theta }\\ -\mathbf{S}_{\theta } & \mathbf{C}_{\theta } \end{bmatrix} \begin{bmatrix} \dot{\mathbf{V}}_x\\ \dot{\mathbf{V}}_y \end{bmatrix},\in \mathbb{R}^{2n \times 2n}\\ \mathbf{K}_2 &= \begin{bmatrix} -\mathbf{S}_{\theta } & \mathbf{C}_{\theta } \\ -\mathbf{C}_{\theta } & -\mathbf{S}_{\theta } \end{bmatrix} \begin{bmatrix} diag(\dot{\boldsymbol{\theta }}) & \mathbf{0} \\ \mathbf{0} & diag(\dot{\boldsymbol{\theta }}) \end{bmatrix}\in \mathbb{R}^{2n \times 2n}, \end{align*}

\begin{align*} \mathbf{N}_1 &= \begin{bmatrix} l\mathbf{K}^T\mathbf{S}_{\theta }\dot{\boldsymbol{\theta }}+\mathbf{e}\dot{p}_x - \mathbf{V}_x\\ -l\mathbf{K}^T\mathbf{C}_{\theta }\dot{\boldsymbol{\theta }}+\mathbf{e}\dot{p}_y - \mathbf{V}_y \end{bmatrix}\in \mathbb{R}^{2n \times 1},\\ \mathbf{N}_2 &= \begin{bmatrix} l\mathbf{K}^T\mathbf{C}_{\theta }diag(\dot{\theta })\dot{\theta }\\ l\mathbf{K}^T\mathbf{S}_{\theta }diag(\dot{\theta })\dot{\theta } \end{bmatrix}\in \mathbb{R}^{2n \times 1},\\ \mathbf{SC}_{\theta } &= \begin{bmatrix} l\mathbf{K}^T \boldsymbol{S_\theta }\\ -l\mathbf{K}^T \boldsymbol{C_\theta } \end{bmatrix} \in \mathbb{R}^{2n \times n}. \end{align*}

The kinematic relation for the Z-direction motion is as follows:

(9) \begin{equation} {p_z} = \frac{1}{n}\mathbf{e}^{T}\mathbf{Z}, \end{equation}

where $Z =[z_1, \dots,z_n]\in \mathbb{R}^n$ is the position vector in Z-direction. The derivative of Eq. (9) gives the velocity in Z-direction as $\dot{p}_z$ . The 3D motion dynamics of the underwater snake robot are given in the following subsection.

2.2. 3D motion dynamics of the underwater snake robot

The free body diagram for a ‘ $i^{th}$ ’ link of a snake robot is given in Fig. 3. Figure 3(a) shows the force and moment balance in the X-Y plane (horizontal), and Fig. 3(b) shows the force balance in Z-direction (vertical).

Figure 3. Free body diagram of link ‘ $i$ ’ . (a) Force and moment balance in X-Y plane. (b) Force balance in Z-direction.

The force balance equation is using the newton $2^{nd}$ law given for a link ‘ $i$ ’ as

(10a) \begin{align} m\ddot{x}_i &= f_{x,i}+h_{x,i}-h_{x,i-1}, \end{align}

(10b) \begin{align} m\ddot{y}_i &= f_{y,i}+h_{y,i}-h_{y,i-1}, \end{align}

(10c) \begin{align} m\ddot{z}_i &= m_b g -f_{z,i}, \end{align}

where $(f_{x,i},f_{y,i})$ are the environmental forces acting on the link ‘ $i$ ’, $(h_{x,i},h_{y,i})$ are the joint constraint forces between link ‘ $i$ ’ and ‘ $i+1$ ’, $(h_{x,i-1},h_{y,i-1})$ is the joint constraint force between link ‘ $i-1$ ’ and ‘ $i$ ’ as shown in Fig. 3, $m_b$ is considered buoyancy mass that ‘ $m_b$ ’ kg water may enter or exist from the system by using the pumps, ‘ $g$ ’ is acceleration due to gravity and ‘ $f_{z,i}$ ’ is the environmental force in the vertical direction during vertical motion. It may be noted that the total mass ‘ $m$ ’ can be calculated as $m = m_s + m_w \pm m_b$ in which $m_s$ is the mass of the snake robot and $m_w$ is the mass of water. Some amount of the water ( $m_w$ ) is initially considered inside the link to make the robot neutral buoyant. When the robot gets down, the $m_b$ amount of water is pumped inside the link. To get some position in the ‘ $Z$ ’-direction, the $m_b$ amount of water is discharged from the link using $2^{nd}$ pump, and the robot becomes neutral buoyant at that position level. When the robot moves upward, the $m_b$ amount of the water discharged from the initial $m_w$ water becomes a positive buoyant, and it lifts due to buoyancy force. A robot can become neutral buoyant by taking water inside the link as the exact amount of water discharged earlier. That means Eq. (10c) works when the robot moves in the vertical direction. The shematic diagram of this discussion is shown in Fig. 4. Thus, the snake robot would get 3D motion in underwater conditions.

Figure 4. Internal components of a link.

The moment balance equation of link ‘ $i$ ’ in the X-Y plane is given below:

(11) \begin{equation} J\ddot{\theta }_i = u_i-u_{i-1}-l\sin{\theta _i}(h_{x,i}+h_{x,i-1})+l\cos{\theta _i}(h_{y,i}+h_{y,i-1})+\tau _i \end{equation}

where $J$ is the mass moment of inertia of link ‘ $i$ ’; $u_i$ is the actuator torque between link ‘ $i$ ’ and ‘ $i+1$ ’; $u_{i-1}$ is the actuator torque between link ‘ $i-1$ ’ and ‘ $i$ ’; $\tau _i = -\lambda _1 \dot{\theta }_i-\lambda _2 \dot{\theta }_i^2-\lambda _3 \ddot{\theta }$ , in which $\lambda _1$ and $\lambda _2$ are the linear and non-linear rotational damping parameters, respectively; and $\lambda _3$ is a rotational added mass effect coefficient. No moment in the $Z$ -direction occurs because all links are symmetric and have equal mass. Equations 10 and 11 can be extended for the whole body of the snake robot as follows:

(12a) \begin{align} m\ddot{\mathbf{X}} &= \mathbf{F}_{x}+\mathbf{D}^T\mathbf{h}_x, \end{align}

(12b) \begin{align} m\ddot{\mathbf{Y}} &= \mathbf{F}_{y}+\mathbf{D}^T\mathbf{h}_y, \end{align}

(12c) \begin{align} m\ddot{\mathbf{Z}} & = m_bg\mathbf{e}-\mathbf{F}_{z}, \end{align}

(12d) \begin{align} J\mathbf{I}_n &=\mathbf{D}^T\mathbf{u}-l\mathbf{S}_{\theta }\mathbf{A}^T\mathbf{h}_x + l\mathbf{C}_{\theta }\mathbf{A}^T\mathbf{h}_y + \boldsymbol{\tau } \end{align}

where $\mathbf{u} = [u_1, \cdots \cdots,u_{n-1}]^T$ , $\mathbf{F}_{x} = [f_{x,1}, \cdots \cdots, f_{x,n}]^T$ , $\mathbf{F}_{y} = [f_{y,1}, \cdots \cdots, f_{y,n}]^T$ , $\mathbf{F}_{z} = [f_{z,1}, \cdots \cdots, f_{z,n}]^T$ , $\mathbf{h}_{x} = [h_{x,1}, \cdots \cdots, h_{x,n}]^T$ , $\mathbf{h}_{y} = [h_{y,1}, \cdots \cdots, h_{y,n}]^T$ , $\boldsymbol{\tau } = [\tau _{1},\cdots \cdots, \tau _{n}]^T$ , . The total force in X-Y direction $\mathbf{F} = [\mathbf{F}_x, \mathbf{F}_y]^T = \mathbf{F_a} + \mathbf{F_d} \in \mathbb{R}^{2n}$ , where $\mathbf{F_d} \in \mathbb{R}^{2n}$ is the drag force and $\mathbf{F_a} \in \mathbb{R}^{2n}$ is the force due to added mass effect. The force $\mathbf{F}_a$ acting on the snake robot due to the added mass effect can be written below:

(13) \begin{equation} \mathbf{F}_a = \mathbf{D}_1\mathbf{K}_1\mathbf{SC}_{\theta }\ddot{\boldsymbol{\theta }} + \mathbf{D}_1\mathbf{K}_1\mathbf{E}\ddot{\mathbf{p}}_{xy} + \mathbf{D}_1 \mathbf{K}_3, \end{equation}

where

\begin{align*} \mathbf{E} &= \begin{bmatrix} \mathbf{e} & \mathbf{0}_{(n,1)}\\ \mathbf{0}_{(n,1)} & \mathbf{e} \end{bmatrix} \in \mathbb{R}^{2n \times 2},\\ \mathbf{D}_1 &= - \begin{bmatrix} \mathbf{C}_{\theta } & -\mathbf{S}_{\theta }\\ \mathbf{S}_{\theta } & \mathbf{C}_{\theta } \end{bmatrix} \begin{bmatrix} \mathbf{0} & \mathbf{0}\\ \mathbf{0} & \boldsymbol{\mu } \end{bmatrix} \in \mathbb{R}^{2n \times 2n},\\ \boldsymbol{\mu } &= \mu \mathbf{I}_n,\\ \mathbf{K}_3 &= \mathbf{K}_1 \mathbf{N}_2 - \mathbf{K}_1 \dot{\mathbf{V}}_c + \mathbf{K}_2 \mathbf{N}_1 \in \mathbb{R}^{2n \times 1}, \end{align*}

and $\mu$ is the added mass coefficient. The force due to the drag effect ( $\mathbf{F}_d$ ) can be expressed in terms of linear drag force ( $\mathbf{F}_{Ld}$ ) and non-linear drag force ( $\mathbf{F}_{NLd}$ ) such as $\mathbf{F}_d = \mathbf{F}_{Ld} + \mathbf{F}_{NLd}$ . The linear drag force can be expressed as

(14) \begin{equation} \mathbf{F}_{Ld} = l \mathbf{Q}_{\theta }\mathbf{SC}_{\theta }\dot{\boldsymbol{\theta }}+ \mathbf{Q}_{\theta }\mathbf{E}\mathbf{v}_r, \end{equation}

where $ \mathbf{v}_r = \dot{\mathbf{p}}-\mathbf{v}_c$ , $\mathbf{Q}_{\theta } = - \begin{bmatrix} c_t(\mathbf{C}_{\theta })^2+c_n(\mathbf{S}_{\theta })^2 & (c_t-c_n)\mathbf{S}_{\theta }\mathbf{C}_{\theta }\\ (c_t-c_n)\mathbf{S}_{\theta }\mathbf{C}_{\theta } & c_t(\mathbf{S}_{\theta })^2+c_n(\mathbf{C}_{\theta })^2 \end{bmatrix}$ . The non-linear drag force can be expressed as

(15) \begin{equation} \mathbf{F}_{NLd} = - \begin{bmatrix} c_t\mathbf{C}_{\theta } & -c_n\mathbf{S}_{\theta }\\ c_t\mathbf{S}_{\theta } & c_n\mathbf{C}_{\theta } \end{bmatrix} diag\left (abs \begin{bmatrix} \mathbf{V}_{rx}\\ \mathbf{V}_{ry} \end{bmatrix}\right ) \begin{bmatrix} \mathbf{V}_{rx}\\ \mathbf{V}_{ry} \end{bmatrix}. \end{equation}

The force acting on the robot during motion in $Z$ -direction (vertical) can be explained as follows:

(16) \begin{equation} \mathbf{F}_z = \frac{1}{2m} \rho \mathbf{C_D} \mathbf{A_p} diag(\mid \dot{\mathbf{Z}}\mid )\dot{\mathbf{Z}}, \end{equation}

where $\mathbf{A}_p = a_p \mathbf{I}_n$ , $\mathbf{C_D} = c_d \mathbf{I}_n$ . $a_p$ is the projected area of link, and $c_d$ is the vertical drag coefficient. The robot changes its position during motion. So, the acceleration of CM in the X-Y plane can be obtained using Eq. (12a,b) as follows:

(17) \begin{equation} \ddot{\mathbf{p}}_{xy} =\frac{1}{n}\begin{bmatrix} \mathbf{e}^T\ddot{\mathbf{X}}\\ \mathbf{e}^T\ddot{\mathbf{Y}} \end{bmatrix}= \frac{1}{nm}\mathbf{E}^T\mathbf{F}. \end{equation}

$\because \mathbf{e}^T\mathbf{D}^T = 0$ . The substitution of Eq. (16) with Eq. (12c) gives the motion equation in Z-direction as follows:

(18) \begin{equation} \ddot{\mathbf{Z}} = \frac{m_bg}{m}\mathbf{e} - \frac{1}{2m} \rho \mathbf{C_D} \mathbf{A_p} diag(\mid \dot{\mathbf{Z}}\mid )\dot{\mathbf{Z}}. \end{equation}

The motion equations of the multi-link snake robot are given by substituting Eq. (4) with Eq. (12a,b,d), and the angular dynamic equation becomes as in Eq. (19a). The acceleration in X-Y can be deduced after substituting Eqs. (13, 14, 15) with Eq. (17), and the resulting Eq. (19b) becomes the acceleration equation. Vertical CM acceleration of the snake robot (ref. Eq. 19c) can be derived using Eqs. (18) and (9). Therefore, the equation of motion for the snake robot in 3D by using the drag forces and added mass effect are as follows:

(19a) \begin{align} &\mathbf{M}_{\theta }\ddot{\boldsymbol{\theta }} + \mathbf{W}_{\theta }diag(\dot{\boldsymbol{\theta }})\dot{\boldsymbol{\theta }}-\mathbf{SC}_{\theta }^T (\mathbf{f}_d + \mathbf{A}_g) + \boldsymbol{\Lambda }_1\boldsymbol{\dot{\theta }}+\boldsymbol{\Lambda }_2 diag(\mid \dot{\boldsymbol{\theta }}\mid )\dot{\boldsymbol{\theta }} = \mathbf{D}^T\mathbf{u}+\mathbf{\Delta }, \end{align}

(19b) \begin{align} &\ddot{\mathbf{p}}_{xy} = \mathbf{H}_{x}^{-1}\left \lbrace \mathbf{E}^T\mathbf{f}_d + \mathbf{E}^T \mathbf{D}_1 \mathbf{K}_1 \mathbf{SC}_{\theta }\ddot{\boldsymbol{\theta }}+\mathbf{E}^T\mathbf{D}_1\mathbf{K}_3\right \rbrace, \end{align}

(19c) \begin{align} &\ddot{p}_z = \frac{m_bg}{nm}\mathbf{e}^T\mathbf{e} - \frac{1}{2nm} \rho \mathbf{e}^T\mathbf{C_D} \mathbf{A_p} diag(\mid \dot{\mathbf{Z}}\mid )\dot{\mathbf{Z}}, \end{align}

where

\begin{align*} &\mathbf{M}_{\theta } = J\mathbf{I_n} + ml^2\mathbf{S}_{\theta }\mathbf{V}\mathbf{S}_{\theta } + ml^2\mathbf{C}_{\theta }\mathbf{V}\mathbf{C}_{\theta }-\mathbf{SC}_{\theta }^T \mathbf{A}_k + \mathbf{\Lambda }_3,\\ &\mathbf{A}_k = \mathbf{D}_1 \mathbf{K}_1 \mathbf{SC}_{\theta } + \mathbf{D}_1 \mathbf{K}_1 \mathbf{E} \mathbf{H}_{x}^{-1} \mathbf{E}^T \mathbf{D}_1 \mathbf{K}_1 \mathbf{SC}_{\theta },\\ &\mathbf{A}_g = \mathbf{D}_1\mathbf{K}_1\mathbf{E}\mathbf{H}_{x}^{-1}\mathbf{E}^T\mathbf{f}_d + \mathbf{D}_1 \mathbf{K}_1 \mathbf{E} \mathbf{H}_{x}^{-1} \mathbf{E}^T \mathbf{D}_1 \mathbf{K}_3 + \mathbf{D}_1 \mathbf{K}_3,\\ &\mathbf{H}_{x} = nm\mathbf{I}_2 - \mathbf{E}^T\mathbf{D}_1\mathbf{K}_1\mathbf{E},\\ &\mathbf{W}_{\theta } = ml^2\mathbf{S}_{\theta }\mathbf{V}\mathbf{C}_{\theta } - ml^2\mathbf{C}_{\theta }\mathbf{V}\mathbf{S}_{\theta },\\ &\mathbf{V} = \mathbf{A}^T(\mathbf{DD}^T)^{-1}\mathbf{A}, \in \mathbb{R}^{n \times n},\\ &\boldsymbol{\Lambda _1} = \lambda _1\mathbf{I}_n,\quad \boldsymbol{\Lambda _2} = \lambda _2\mathbf{I}_n, \quad \boldsymbol{\Lambda _3} = \lambda _3\mathbf{I}_n, \end{align*}

$\Delta$ is the external disturbances. The equation of motion in Eq. (19a, b) shows the motion of a snake robot in the X-Y plane, and Eq. (19c) shows the motion of a snake robot along the Z-axis. This equation of motion expresses the snake robot moving in the 3D, namely, X-Y-Z motion. Hence, two independent controllers must be designed for the snake robot, one for X-Y plane motion and Z-axis motions. Here, it is noted that the torque $\mathbf{u}$ is responsible for the horizontal motion, and $m_b$ is responsible for the vertical motion. The VHCs are designed for motion control in the X-Y plane, and the event-based controller/PD controller is designed for the motion in the Z-axis. The control approach for the motion of a snake robot for a given dynamics is explained in the next section.

3.1. Virtual holonomic constraints (VHCs)

In this method, the actuator torque ‘u’ in Eq. (19) is regulated using the virtual constraint of the undulation gait pattern as mentioned in Eq. (20). In this, $\omega t$ is replaced by a state $\gamma$ . The state offset angle $\phi _0$ affects all the joints, unlike in [Reference Mukherjee, Mukherjee and Kar25, Reference Mukherjee, Kar and Mukherjee26] where it affects only the head. So, the evaluation compensator of the state is described below:

(21a) \begin{align} \ddot{\gamma } &= u_{\gamma }, \end{align}

(21b) \begin{align} \ddot{\phi _{0}} &= u_{\phi _{0}}. \end{align}

These VHCs enforce an autonomous (time-independent) relation involving configuration variables $(\gamma, \phi _0)$ which does not exist in a mechanical system but is enforced via feedback. The stabilise relation can be written for $n$ generalised coordinates and $n-1$ inputs from Eq. (20).

(22) \begin{equation} \theta _i-\theta _{i+1} = \alpha \sin (\gamma + (i -1)\beta ) + \phi _0. \end{equation}

Let $\Phi _i(\gamma ) = \alpha \sin (\gamma + (i -1)\beta )$ . Therefore, the $\boldsymbol{\Phi }(\gamma ) =[\Phi _1,\Phi _2,\cdots,\Phi _{n-1}]^T$ . Using Eq. (22) $\boldsymbol{\theta }$ can be expressed as

(23) \begin{equation} \boldsymbol{\theta } = \mathbf{e} \theta _n + \mathbf{H}\boldsymbol{\Phi }(\gamma ) + \mathbf{H}\mathbf{b}\phi _0, \end{equation}

where $\mathbf{b}= [1,\cdots \cdots,n-1]^T \in \mathbb{R}^{n-1}$ , . The VHCs from the above can be expressed by following [Reference Mohammadi, Rezapour, Maggiore and Pettersen21] as

(24) \begin{equation} \mathbf{g}(\gamma,\phi _0,\boldsymbol{\theta }) = \mathbf{D}\boldsymbol{\theta }-\Phi (\gamma )-\mathbf{b}\phi _0 = 0. \end{equation}

$\mathbf{g}(\gamma,\phi _0,\boldsymbol{\theta })$ is an error function for the system Eq. (19a) with compensator Eq. (21), and it is needed to be zero. In most snake robots, RC servo motors have an inbuilt Proportional Integral Derivative (PID) controller. Therefore, the torque equation can be written as follows [Reference Patel and Dwivedy32, Reference Patel and Dwivedy34]:

(25) \begin{equation} \begin{aligned} \mathbf{u}=&-{k_p}\mathbf{g}(\gamma,\phi _0,\boldsymbol{\theta })-{k_d}\dot{\mathbf{g}}(\gamma,\phi _0,\boldsymbol{\theta })\dot{\gamma }\dot{\phi }_0\dot{\boldsymbol{\theta }}-{k_i}\int \mathbf{g}(\gamma,\phi _0,\boldsymbol{\theta })dt, \end{aligned} \end{equation}

where $k_p$ , $k_d$ , ${k_i} \gt 0$ are positive gains. On the satisfaction of VHCs, the dynamical system corresponds to the manifold by substituting Eq. (24) with Eq. (19). Hence, the reduced-ordered dynamic system is written as follows:

(26a) \begin{align} \ddot{\theta }_{n} &= \hat{\Upsilon }_1 + \hat{\Upsilon }_2 u_{\gamma } + \hat{\Upsilon }_3 u_{\phi _0} + \tilde{\Upsilon }_{\theta _n}, \end{align}

(26b) \begin{align} \ddot{\mathbf{p}}_{xy} &=\hat{\Upsilon }_4 + \hat{\Upsilon }_5 u_{\gamma } + \hat{\Upsilon }_6 u_{\phi _0} + \tilde{\Upsilon }_p, \end{align}

where

\begin{align*} \hat{\Upsilon }_1 &= -\frac{\mathbf{e}^T\mathbf{M}_{\theta }\mathbf{H}\ddot{\boldsymbol{\Phi }}(\gamma )\dot{\gamma }^2}{\mathbf{e}^T\mathbf{M}_{\theta }\mathbf{e}}-\frac{\mathbf{e}^T}{\mathbf{e}^T\mathbf{M}_{\theta }\mathbf{e}}\left (\mathbf{W}_{\theta }\dot{\theta }^2-\mathbf{SC}_{\theta }(\mathbf{f}_d+\mathbf{A}_g) + \boldsymbol{\Lambda _1}\dot{\boldsymbol{\theta }} + \boldsymbol{\Lambda _2}diag(\mid \dot{\boldsymbol{\theta }}\mid )\dot{\boldsymbol{\theta }} \right ), \\ \hat{\Upsilon }_2 &= -\frac{\mathbf{e}^T\mathbf{M}_{\theta }\mathbf{H}\dot{\boldsymbol{\Phi }}(\gamma )}{\mathbf{e}^T\mathbf{M}_{\theta }\mathbf{e}},\\ \hat{\Upsilon }_3 &=-\frac{\mathbf{e}^T\mathbf{M}_{\theta }\mathbf{H}\mathbf{b}}{\mathbf{e}^T\mathbf{M}_{\theta }\mathbf{e}},\\ \hat{\Upsilon }_4 &=\mathbf{H}_{x}^{-1}\mathbf{E}^T\mathbf{f}_d + \mathbf{H}_{x}^{-1}\mathbf{E}^T\mathbf{D}_1\mathbf{K}_1\mathbf{SC}_{\theta }\mathbf{H}\ddot{\boldsymbol{\phi }}(\gamma )\dot{\gamma }^2 + \mathbf{H}_{x}^{-1}\mathbf{E}^T\mathbf{D}_1\mathbf{K}_3 + \mathbf{H}_{x}^{-1} \mathbf{E}^T \mathbf{D}_1 \mathbf{K}_1 \mathbf{SC}_{\theta } \mathbf{e} \hat{\Upsilon }_1, \\ \hat{\Upsilon }_5 &= \mathbf{H}_{x}^{-1} \mathbf{E}^T \mathbf{D}_1 \mathbf{K}_1 \mathbf{SC}_{\theta } \mathbf{e} \hat{\Upsilon }_2 + \mathbf{H}_{x}^{-1} \mathbf{E}^T \mathbf{D}_1 \mathbf{K}_1 \mathbf{SC}_{\theta } \mathbf{H} \dot{\boldsymbol{\phi }}(\gamma ),\\ \hat{\Upsilon }_6 &= \mathbf{H}_{x}^{-1} \mathbf{E}^T \mathbf{D}_1 \mathbf{K}_1 \mathbf{SC}_{\theta } \mathbf{e} \hat{\Upsilon }_3 + \mathbf{H}_{x}^{-1} \mathbf{E}^T \mathbf{D}_1 \mathbf{K}_1 \mathbf{SC}_{\theta } \mathbf{H}\mathbf{b}. \end{align*}

$\tilde{\Upsilon }_{\theta _n}, \tilde{\Upsilon }_{p}$ are the lumped disturbances in reduced-ordered dynamics of head angle and CM acceleration, respectively. Based on the reduced-order system, the controller would be designed. The benefit of a reduced-ordered system is that the entire robot can be controlled by designing a controller for a single joint. It is noted that the reduced-ordered dynamic system does not have a control input term. Here, the planar motion is controlled by the STSMC, and an event-based control technique controls vertical motion.

4.1. Horizontal motion control

The reduced-order governing equation for a snake robot is given in Eq. (26) in terms of head angle ( $\theta _n$ ) and tangential velocity ( $v_t$ ). Considering ( $\theta _{ref}, v_{ref}$ ) reference head angle and tangential velocity, the error equation can be written as follows:

(27a) \begin{align} \ddot{\tilde{\theta }}_{n} &= \hat{\Upsilon }_1 + \hat{\Upsilon }_2 u_{\gamma } + \hat{\Upsilon }_3 u_{\phi _0} - \ddot{\theta }_{ref}, \end{align}

(27b) \begin{align} \dot{\tilde{v}}_{t} &= \mathbf{u}_{\theta _n}^T \hat{\Upsilon }_4 + \mathbf{u}_{\theta _n}^T \hat{\Upsilon }_5 u_{\gamma } + \mathbf{u}_{\theta _n}^T \hat{\Upsilon }_6 u_{\phi _0} + \dot{\theta }_n v_n - \dot{v}_{ref}. \end{align}

These error equations for the state vector $\bar{\xi } = [\tilde{\theta }_n, \dot{\tilde{\theta }}_n, \tilde{v}_{t,r}, v_{n,r}, \gamma, \dot{\gamma }, \phi _0, \dot{\phi }_0]^T$ are used in the designing of a sliding surface where the expression is given below:

(28) \begin{equation} \boldsymbol{\sigma }(\bar{\xi }) = \begin{bmatrix} \sigma _1(\bar{\xi })\\ \sigma _2(\bar{\xi }) \end{bmatrix}= \begin{bmatrix} \dot{\tilde{\theta }}_n + k_n \tilde{\theta }_n\\ \dot{\gamma } + k_v\tilde{v}_{t,r} \end{bmatrix} \end{equation}

where $k_n,k_v\gt 0$ is the design parameter. The derivative of sliding surface is given below:

(29a) \begin{align} \dot{\sigma }_1(\bar{\xi })&=\ddot{\tilde{\theta }}_n +k_n \dot{\tilde{\theta }}_n, \end{align}

(29b) \begin{align} \dot{\sigma }_2(\bar{\xi })&= \ddot{\gamma } + k_v \dot{\tilde{v}}_{t,r}. \end{align}

Therefore, Eq. (29) can be rewritten by substituting Eq. ((27) as

(30a) \begin{align} \dot{\sigma }_1(\bar{\xi }) &= \hat{\Upsilon }_1 + \hat{\Upsilon }_2 u_{\gamma } + \hat{\Upsilon }_3 u_{\phi _0} - \ddot{\theta }_{ref} + +k_n\dot{\tilde{\theta }}_n + \tilde{\Upsilon }_{\theta _n}, \end{align}

(30b) \begin{align} \dot{\sigma }_2(\bar{\xi }) &= u_{\gamma }+k_v(\mathbf{u}_{\theta _n}^T \hat{\Upsilon }_4 + \mathbf{u}_{\theta _n}^T \hat{\Upsilon }_5 u_{\gamma } + \mathbf{u}_{\theta _n}^T \hat{\Upsilon }_6 u_{\phi _0} + \dot{\theta }_n v_n - \dot{v}_{ref}) +u_{\theta _n}^T\tilde{\Upsilon }_p. \end{align}

Now, Eq. (30) can represent in sliding vector form as

(31) \begin{equation} \boldsymbol{\dot{\sigma }}(\bar{\xi }) = \hat{\mathbf{f}}_{\sigma } +\mathbf{f}_e +\hat{\mathbf{g}}_{\sigma }\mathbf{u}_{\sigma } + \bar{\mathbf{f}}_{\sigma }, \end{equation}

where

\begin{align*} &\hat{\mathbf{f}}_{\sigma }= \begin{bmatrix} \hat{\Upsilon }_1 \\ k_v (\mathbf{u}_{\theta _n}^T \hat{\Upsilon }_4+ \dot{\theta }_n v_n) \end{bmatrix}, \mathbf{f}_e = \begin{bmatrix} k_n \dot{\tilde{\theta }}_n-\ddot{\theta }_{ref}\\ - k_v\dot{v}_{ref} \end{bmatrix}, \hat{\mathbf{g}}_{\sigma }= \begin{bmatrix} \hat{\Upsilon }_2 & \hat{\Upsilon }_3\\ k_v(1+\mathbf{u}_{\theta _n}^T \hat{\Upsilon }_5) & k_v\mathbf{u}_{\theta _n}^T \hat{\Upsilon }_6 \end{bmatrix}, \\&\mathbf{u}_{\sigma } = \begin{bmatrix} u_{\gamma }\\ u_{\phi _0} \end{bmatrix}, \bar{\mathbf{f}}_{\sigma } = \begin{bmatrix} \tilde{\Upsilon }_{\theta _n}\\ u_{\theta _n}^T\tilde{\Upsilon }_p \end{bmatrix}. \end{align*}

The STSMC [Reference Patel and Dwivedy32] scheme is used for the motion control in the X-Y plane. The equivalent control law can be written for $(\dot{\boldsymbol{\sigma }}=0)$ as follows:

(32) \begin{equation} \mathbf{u}_{eq} = -\hat{\mathbf{g}}_{\sigma }^{-1}(\hat{\mathbf{f}}_{\sigma }+\mathbf{f}_e). \end{equation}

The reaching law that helps to reach the initial error function on the sliding surface is given below [Reference Patel and Dwivedy32]:

(33) \begin{equation} \mathbf{u}_{sw}=-\hat{\mathbf{g}}_{\sigma }^{-1}\Big (k_1sgn(\boldsymbol{\sigma })\Vert \boldsymbol{\sigma }\Vert ^{\frac{1}{2}}+k_2 \boldsymbol{\sigma }\Vert \boldsymbol{\sigma }\Vert ^{\frac{3}{2}}+ \int _{0}^{t} k_3 sgn(\boldsymbol{\sigma })\,dt\Big ). \end{equation}

Therefore, the control law for the horizontal motion is given as follows:

(34) \begin{equation} \mathbf{u}_{\sigma } = \mathbf{u}_{eq} + \mathbf{u}_{sw}. \end{equation}

The stability analysis by considering the given control scheme is explained in ref. [Reference Patel and Dwivedy32] and given in appendix using Lyapunov stability analysis. The vertical motion control technique is explained in the next section.

4.2. Motion control in Z-direction

Z-direction motion is open-loop control as it is independent from the X-Y motion control. Z-direction motion is controlled using an event-based technique. As discussed earlier, the Z-direction motion can be controlled by using the inlet and outlet pumps. The schematic diagram of link ‘ $i$ ’ is shown in Fig. 4. To operate these pumps, the following given logic control algorithm can be used. When one pump starts, the mass of the robot gets changes and the robot starts motion in the Z-direction. In this work, the 3V Direct Current (DC) pump has a flow rate of 10 ml/s. It is assumed that the clean water after small filtering goes inside the pump which prevents the pump from blockages. Let us consider that Pump-1 is used for taking water inside the link and Pump-2 for discharging water from the link as shown in Fig. 4. Triggering conditions for the pump based on the feedback system are explained in Algorithm 1. It is noted that at a particular instant, either Pump-1 or Pump-2 operates. Both pumps do not work simultaneously to keep the balance of the snake robot in underwater conditions.

Algorithm 1 Z-direction motion control algorithm

This event-based control technique is triggered when the condition is fulfilled. This event-based controller is responsible for the motion in the vertical direction. Another way to control the Z-direction motion is by using PD control which is described in the following subsection. In this, reference Z-position is required that robot would be reached.

4.3. Z-direction control using PD controller

It is cleared from the previous subsection discussion that the Z-direction motion is possible using the event-based algorithm. The PD-controller can be useful to reach the exact position that is explained further.

In this, the reference Z-position is given by the slider. The PD-based control scheme is used for the controlling of the pump. The controlled signal operates the pumps. The PD controller is given below:

(35) \begin{equation} \mathbf{F_z} = k_{pz}(\mathbf{z}_{ref}-\mathbf{z})+k_{dz}(\dot{\mathbf{z}}_{ref}-\dot{\mathbf{z}}), \end{equation}

where $k_{pz}$ and $k_{dz}$ are the proportional and derivative gains, respectively. The pump is directly dealt with buoyancy, so the saturation function is used, in which the generated control signal starts the pumps on changing the error sign or achieving the reference position and changing the mass of the link which would help to achieve the reference position in Z-direction.

4.4. Reference trajectory generation for horizontal motion

The reference trajectory can be generated by observing the current velocity in the horizontal direction of the water condition. The water current observer is briefly described in Eq. (36). More detail can be found in ref. [Reference Kohl, Kelasidi, Mohammadi, Maggiore and Pettersen22].

(36a) \begin{align} \mathbf{R}_{\theta } &= [\mathbf{u}_{\theta },\mathbf{v}_{\theta }], \end{align}

(36b) \begin{align} \mathbf{v}_{rel} &= [v_{t,r}, v_{n,r}]^T, \end{align}

(36c) \begin{align} \dot{\mathbf{q}} &= -k\mathbf{q}-k^2\mathbf{p}_{xy}-k\mathbf{R}_{\theta }\mathbf{v}_{rel}, \end{align}

(36d) \begin{align} \bar{\mathbf{v}}_c &= \mathbf{q}+k\mathbf{p}_{xy}, \end{align}

where $\bar{\mathbf{v}}_{c}$ is an estimated current velocity of the water and $k$ is a parameter. Using Eq. (36) reference head angle $(\theta _{ref})$ and velocity $(v_{ref})$ to be calculated for the different geometry of the path following:

(37a) \begin{align} \boldsymbol{\mu }_0 &= -\frac{\nabla h(p_{xy})^{T}}{\Vert \nabla h(p_{xy})\Vert ^2}(k_t h(p_{xy})+k_h \rho ) +\frac{v}{\Vert \nabla h(p_{xy}) \Vert }\begin{bmatrix} 0 & 1\\ -1 & 0 \end{bmatrix} \nabla h(p_{xy})', \end{align}

(37b) \begin{align} \boldsymbol{\mu } &= \frac{v\boldsymbol{\mu }_0}{\Vert \boldsymbol{\mu }_0\Vert }-\bar{\mathbf{v}}_c, \end{align}

(37c) \begin{align} \dot{\rho } &= h(p_{xy}), \end{align}

(37d) \begin{align} \theta _{ref} &= arctan2(\mu _y,\mu _x), \end{align}

(37e) \begin{align} v_{ref} &= \Vert \mu \Vert, \end{align}

where $v, h(p_{xy})$ denotes the desired velocity and path, respectively. $k_t, k_h$ are the design parameters.

4.5. Control law implementation

There are two controllers that work simultaneously. The control law implementation on the robot dynamics is explained in Fig. 5. This gives a clear idea about the implication of STSMC scheme and event-based controller/PD controller simultaneously as shown in the block diagram. The saturation block limits the torque input to the robot dynamics because the motor has limited torque output in real time. The demanding torque of the controller might not be fulfilled by the actuator. Therefore, the saturation function is used to meet the real-time condition of the actuator. The complete implementation gives the 3D motion of the snake robot in underwater conditions.

Figure 5. Block diagram of the control law implementation.

Figure 6. Uncertainties in dynamical system parameters.

Figure 7. Environmental disturbances. (a) No disturbance condition. (b) Random disturbance condition. (c) Pulse disturbance. (d) High-frequency disturbance.