Head trajectory tracking control of an extendable snake-like robot

Abstract

In recent years, extendable snake-like robots are attracting researchers’ interest. A large number of degrees of freedom make it possible for the robots to perform new gaits. It is expected that the telescopic motion can also give the robots higher adaptability for the environment. In this paper, we propose a controller of an extendable snake-like robot for head trajectory tracking and show the effectiveness of the extendable snake-like robot by comparison to an ordinary snake-like robot.

1 Introduction

Living snakes are highly adaptive to the environment. They can climb trees, swim in the water, and move on sand even though they have no limbs. The mysterious locomotion has stimulated researchers’ interests. Hirose’s development of snake-like robots is one of the notable studies. He performed experiments with living snakes and built the world’s first snake-like robot [1].

Most snake-like robots consist of serial rigid links coupled with revolutionary joints that generate a large number of degrees of freedom. This enables a snake-like robot to perform various movements allowing it to perform interesting locomotions like climbing up pipes [2]. Moreover, the large number of degrees of freedom also gives robustness to the robot for joint malfunctions [3]. Because of the mobility and the robustness mentioned above, snake-like robots are expected to work as rescue robots or maintenance robots [4].

Research on extendable snake-like robots with prismatic joints is also progressing. The additional degrees of freedom by the prismatic joints make it possible for the snake-like robots to perform new gaits. One of these new gaits enables extendable snake-like robots to move into such a narrow path that normal snake-like robots cannot move forward [5]. Another study shows that the extension of the prismatic joints helps the robot climb a step [6]. Besides, general snake-like robots can fall into a singular posture, i.e., the posture in which there exists a direction that the robots cannot move [7]. It is expected that the prismatic joints enable the robot to avoid falling into the singular posture.

However, most previous studies on the extendable snake-like robots use prebuilt motion patterns to actuate the joints. In reality, the robot works in an environment unknown to the developer. It is difficult to develop a set of prebuilt motion patterns covering all environment cases. In addition, the robot is often required to move its head along a trajectory given by the operator. Usually, a snake-like robot has its sensors on its head, and moving the head to the desired position is a fundamental task, for example in rescue and maintenance missions. Although prebuilt motions with adjustable parameters may achieve the head tracking task, the appropriate choice of parameters is far from obvious. A head trajectory tracking controller can become a good solution for generating the joints’ movement based on the operator’s command. Moreover, such a controller can develop into a controller that enhances the robot’s adaptivity to the environment.

In the current study, we propose a controller using the partial feedback linearization technique for head trajectory tracking. The controller can utilize the additional degrees of freedom introduced by the prismatic joints appropriately to achieve tasks that an unextendable snake-like robot cannot complete. The validity and efficacy of the proposed controller are shown by simulations. In what follows, Sect. 2 introduces the model of the robot briefly and illustrates the derivation of the controller. Then Sect. 3 shows the results of the simulations.

2 Head tracking control

As mentioned in the introduction, moving the head along the desired trajectory is an important task for snake-like robots. Thus, we set our control objective as head trajectory tracking of an extendable snake-like robot.

In this section, we introduce the model and the controller. The equation of motion of the extendable snake-like robot is shown in Subsection 2.1. The controller for head-tracking control is derived in Subsection 2.2.

2.1 The model of a snake-like robot with prismatic joints

Fig. 1

The structure of the extendable snake-like robot

Figure 1 shows the structure of the extendable snake-like robot. It consists of 2n rigid links serially connected by $n-1$ yaw joints and n prismatic joints. All the links have the same length L, and viscous friction acts on each link. We assumed that the viscous coefficient of the longitudinal direction of the link is smaller than that of the lateral direction. The previous study shows that the anisotropy of frictional force plays a major role in generating thrust force [8]. Note that we use “simple model” to indicate the simple mathematical model (1), which represents the system depicted in Fig. 1.

Let $\varvec{r}_h(t)=[x_h(t), y_h(t)]^T$ and $\theta _1$ be the position and the posture of the head respectively. The angle of the i-th yaw joint is represented as $\phi _i(t)$ and the extension of the i-th prismatic joint is represented as $d_i(t)$. Let the torque of yaw joints and the force of prismatic joints as the inputs of the robot. The torque and the force corresponding $\phi _i(t)$ and $d_i(t)$ are denoted as $T_i(t)$ and $F_i(t)$ respectively. We use the vectorized notations for these variables defined as follows:

$$\begin{aligned} \begin{aligned} {\phi }&=[\phi _1, \cdots , \phi _{n-1}]^T, \varvec{d}=[d_1, \cdots , d_n]^T,\\ \varvec{T}&=[T_1, \cdots , T_{n-1}]^T, \varvec{F}=[F_1, \cdots , F_n]^T, \\ \varvec{u}&=[\varvec{T}^T, \varvec{F}^T]^T, \varvec{q}=[{\phi }^T, \varvec{d}^T]^T. \end{aligned} \end{aligned}$$

Application of Newton’s second law and Euler’s equation gives the following equations of motion [9]:

$$\begin{aligned} \begin{bmatrix} \varvec{E}_2 &{}\varvec{M}_{12} &{}\varvec{M}_{13} \\ \varvec{0}_{1\!\times \!2} &{}M_{22} &{}\varvec{M}_{23} \\ \varvec{0}_{(2n\!-\!1)\!\times \!2} &{}\varvec{M}_{32} &{}\varvec{M}_{33} \end{bmatrix} \begin{bmatrix} \ddot{\varvec{r}}_h \\ \ddot{\theta }_1 \\ \ddot{\varvec{q}} \end{bmatrix} = \begin{bmatrix} \varvec{0} \\ 0 \\ \varvec{B}_q\varvec{u} \end{bmatrix} + \begin{bmatrix} \varvec{b}_{h} \\ b_\theta \\ \varvec{b}_q \end{bmatrix}, \end{aligned}$$

(1)

$$\begin{aligned} \begin{aligned}&\varvec{M}_{12}\in \varvec{R}^{2\!\times \!1}, \varvec{M}_{13}\in \varvec{R}^{2\!\times \!(2n\!-\!1)},\\&M_{22}\in \varvec{R}, \varvec{M}_{23}\in \varvec{R}^{1\!\times \!(2n\!-\!1)}, \\&\varvec{M}_{32}\in \varvec{R}^{(2n\!-\!1)\!\times \!1}, \varvec{M}_{33}\in \varvec{R}^{(2n\!-\!1)\!\times \!(2n\!-\!1)}, \\&\varvec{b}_h\in \varvec{R}^2, b_\theta \in \varvec{R}, \varvec{b}_q\in \varvec{R}^{2n\!-\!1}, \end{aligned} \end{aligned}$$

where $\varvec{E}_m$ is the $m\times m$ identity matrix and $\varvec{B}_q$ is a regular diagonal matrix. The matrices $\varvec{M}_{ij}$ and the scalar value $M_{22}$ are determined by $\theta _1$ and $\varvec{q}$. The vectors $\varvec{b}_h$, $b_\theta$ and $\varvec{b}_q$ consist of the friction force, the centrifugal force and the Coriolis force. They depend on $\theta _1$, $\varvec{q}$, $\dot{\theta }_1$, $\dot{\varvec{q}}$ and $\dot{\varvec{r}}_h$. Note that the inertia matrix in the above equations of motion is not symmetric. Although we can transform it into a symmetric one, we leave it as it is, because symmetry is not required in the following discussions.

In the equation above, we make the following assumption.

Assumption 1

$M_{22}\ne 0$ and matrices ${\mathcal {M}}, \bar{\varvec{M}}_{12}$ defined below are full rank.

$$\begin{aligned} \begin{aligned} {\mathcal {M}}&=\varvec{M}_{33}-M_{22}^{-1}\varvec{M}_{32}\varvec{M}_{23}\\ \varvec{\bar{M}}_{12}&=\varvec{M}_{13}-M_{22}^{-1}\varvec{M}_{12}\varvec{M}_{23} \end{aligned} \end{aligned}$$

This assumption means that the robot does not take a singular posture, in which there exists a direction where the robot cannot accelerate its head. We have confirmed that the assumption fails to be true if $d_i = -2L$ and $\phi _i = 0$ for any i. This situation means that all revolutionary joints gather at the same point. However, it is still unclear whether it is always true when $d_i > 0$.

2.2 Controller

To achieve the control objective $\Vert \varvec{r}_h-\varvec{r}_{hr}\Vert \rightarrow 0$ $(t\rightarrow \infty )$ , where $\varvec{r}_{hr}(t)$ is the reference trajectory of the head, we used the partial feedback linearization technique. From (1), we have

$$\begin{aligned} \begin{aligned} \ddot{\varvec{r}}_h=&-\bar{\varvec{M}}_{12}{\mathcal {M}}^{-1}(\varvec{B}_q\varvec{u}+\varvec{b}_q-M_{22}^{-1}\varvec{M}_{32}b_\theta )\\&-M_{22}^{-1}\varvec{M}_{12}b_\theta +\varvec{b}_h. \end{aligned} \end{aligned}$$

(2)

Define the weighted pseudo-inverse matrix $\bar{\varvec{M}}_{12}^+$ of $\bar{\varvec{M}}_{12}$ as follows:

$$\begin{aligned} \bar{\varvec{M}}_{12}^+\!=\!\varvec{W}^{-1}\bar{\varvec{M}}_{12}^T(\bar{\varvec{M}}_{12}\varvec{W}^{-1}\bar{\varvec{M}}_{12}^T)^{-1}, \end{aligned}$$

where $\varvec{W}$ is a $(2n\!-\!1)\!\times \!(2n\!-\!1)$ diagonal matrix with positive diagonal entries. It is easy to see that $\bar{\varvec{M}}_{12}\bar{\varvec{M}}_{12}^+=\varvec{E}_2$ holds. The weighted pseudo-inverse matrix regulates the movement of the joints. The details will be described later.

Using $\bar{\varvec{M}}_{12}^+$, let the input $\varvec{u}$ be calculated as

$$\begin{aligned}&\varvec{u}=\varvec{B}_q^{-1}({\mathcal {M}}\varvec{v}-\varvec{b}_q+M_{22}^{-1}\varvec{M}_{32}b_\theta ), \end{aligned}$$

(3)

$$\begin{aligned}&\begin{aligned} \varvec{v}=&-\bar{\varvec{M}}_{12}^+(\bar{\varvec{v}}+M_{22}^{-1}\varvec{M}_{12}b_\theta -\varvec{b}_h)\\&+(\varvec{E}_{2n\!-\!1}\!-\!\bar{\varvec{M}}_{12}^+\bar{\varvec{M}}_{12})\varvec{k}, \\ \end{aligned} \end{aligned}$$

(4)

$$\begin{aligned}&\bar{\varvec{v}}=\ddot{\varvec{r}}_{hr}-\varvec{K}_d(\dot{\varvec{r}}_{h}-\dot{\varvec{r}}_{hr})-\varvec{K}_p(\varvec{r}_h-\varvec{r}_{hr}), \end{aligned}$$

(5)

where $\varvec{k}\in \varvec{R}^{2n\!-\!1}$ is an arbitrary vector, $\varvec{K}_p, \varvec{K}_d\in \varvec{R}^{2\times 2}$ are positive difinite matrices.

Substituting the above expression for $\varvec{u}$ in the (2), the following equation is derived:

$$\begin{aligned} \ddot{\varvec{r}}_{h}-\ddot{\varvec{r}}_{hr}+\varvec{K}_d(\dot{\varvec{r}}_{h}-\dot{\varvec{r}}_{hr})+\varvec{K}_p(\varvec{r}_h-\varvec{r}_{hr})=\varvec{0}, \end{aligned}$$

(6)

which implies $\Vert \varvec{r}_h-\varvec{r}_{hr}\Vert \rightarrow 0$ as $t \rightarrow \infty$.

The weight $\varvec{W}$ determines which joint mainly contributes to the acceleration of the head. The (i, i) entry $(1 \le i \le n-1)$ and the $(n+j-1, n+j-1)$ entry $(1 \le j \le n)$ of $\varvec{W}$ correspond to the weight of the i-th revolutionary joint and the weight of the j-th prismatic joint respectively. For example, a relatively large (1, 1) element to other elements restrains the movement of the first revolutionary joint $\phi _1$.

If the reference of joint values $\varvec{q}_r$ is given, $\varvec{k}$ can be used to reduce $\Vert \varvec{q}-\varvec{q}_r\Vert$ at every t as described in [10]. From (1) and the property of $\bar{\varvec{M}}_{12}^+$, the closed loop system is expressed as follows:

$$\begin{aligned} \begin{aligned} \ddot{\varvec{q}}=&-\bar{\varvec{M}}_{12}^+(\bar{\varvec{v}}+M_{22}^{-1}\varvec{M}_{12}b_\theta -\varvec{b}_h)\\&+(\varvec{E}_{2n\!-\!1}\!-\!\bar{\varvec{M}}_{12}^+\bar{\varvec{M}}_{12})\varvec{k}, \\ \end{aligned} \end{aligned}$$

(7)

$$\begin{aligned} (\varvec{E}_{2n-1}-\bar{\varvec{M}}_{12}^+\bar{\varvec{M}}_{12})^2=(\varvec{E}_{2n-1}-\bar{\varvec{M}}_{12}^+\bar{\varvec{M}}_{12}), \end{aligned}$$

(8)

$$\begin{aligned} (\varvec{E}_{2n-1}-\bar{\varvec{M}}_{12}^+\bar{\varvec{M}}_{12})\bar{\varvec{M}}_{12}^+=\varvec{0}. \end{aligned}$$

(9)

Multiplying (7) by $(\varvec{E}_{2n-1}-\bar{\varvec{M}}_{12}^+\bar{\varvec{M}}_{12})$ from the left and applying (8) and (9) gives the following relation:

$$\begin{aligned} (\varvec{E}_{2n\!-\!1}\!-\!\bar{\varvec{M}}_{12}^+\bar{\varvec{M}}_{12})\ddot{\varvec{q}}=(\varvec{E}_{2n\!-\!1}\!-\!\bar{\varvec{M}}_{12}^+\bar{\varvec{M}}_{12})\varvec{k}. \end{aligned}$$

(10)

Therefore, if $\varvec{k}$ is defined by

$$\begin{aligned} \varvec{k}=\ddot{\varvec{q}}_{r}-\varvec{K}_{qd}(\dot{\varvec{q}}-\dot{\varvec{q}}_{r})-\varvec{K}_{qp}(\varvec{q}-\varvec{q}_{r}), \end{aligned}$$

(11)

with positive definite matrices $\varvec{K}_{qp}$ and $\varvec{K}_{qd}$, $\varvec{q}$ will get closer to $\varvec{q}_r$.

The technique using $\varvec{k}$ to achieve a sub-objective is mentioned in [11]. The authors calculate $\varvec{k}$ as the gradient of a cost function to avoid the singular posture. However, the method requires a carefully designed cost function, therefore we employ the method in [10] with a modification for an extendable snake-like robot.

3 Simulation

We carried out four simulations as shown in Table 1.

Table 1 Simulated robots and trajectory

In Table 1, “Sine curve*” represents a sine curve trajectory containing discontinuous reference velocity. The mathematical definitions of these trajectories are given in Subsection 3.1.

Subsection 3.2 explains the parameters of the extendable snake-like robots. In Subsection 3.3, the simple model of the unextendable snake-like robot is introduced briefly. The simulation results of Sim1 and Sim2 are shown in Subsection 3.4, and the result of Sim3 and Sim4 are shown in Subsection 3.5.

We used COPPELIA ROBOTICS V-REP version 3.6.2 and set the time step 10 ms. The physics engine was Open Dynamics Engine [12] and “Dynamics settings” was “Accurate”. On the setting, the output of our controller was updated every two calculations of the movement of the robot.

3.1 The reference trajectories

In Sim1 and Sim3, we used a sine curve as the reference trajectory to examine the trackability on the path where the robot’s head swings from side to side. The trajectory was defined as follows:

$$\begin{aligned} \begin{aligned} \varvec{r}_{hr}&= \begin{bmatrix} -v_xt,&-0.2\sin (-0.75t) \end{bmatrix}^T,\\ v_x&=0.15. \end{aligned} \end{aligned}$$

(12)

In Sim2 and Sim4, we examined the trackability when the reference trajectory contained discontinuous velocity change. We changed $v_x$ in (12) as follows:

$$\begin{aligned} v_x= {\left\{ \begin{array}{ll} 0.15 &{}(0\le t \le 5, 6< t),\\ 0.45 &{}(5 < t \le 6). \end{array}\right. } \end{aligned}$$

The discontinuous velocity reference is one of the difficult references for a normal snake-like robot because it will lead the robot into a singular posture, e.g., a line shape posture and an arc shape posture. It will be discussed later how the prismatic joints ease the difficulty of the discontinuous velocity reference.

In all the simulations, the robot stood still at the initial time.

3.2 The parameters of the extendable snake-like robot

Fig. 2

3D model of the robot (16 links)

Figure 2 shows the 3D model of the extendable snake-like robot. The grey rectangle parts represent the prismatic joints and the red cylindrical parts represent the rotational joints. We use “simulation model” to indicate the 3D model in the physical simulator. Note that the physical simulator can simulate three-dimensional movement, however, we let the robot move slow and kept the robot on the floor. In addition, the simulator handles the friction as Coulomb friction, and to represent the anisotropy of friction in the physical simulator, we equipped passive wheels on the simulation model.

The number of links was 16, the link length was 8 cm, the weight of the link was 0.25 kg, and the maximum extension of the prismatic joint was 10 cm. We set $\varvec{K}_p$, $\varvec{K}_d$, $\varvec{q}_r$, $\varvec{K}_{qp}$, $\varvec{K}_{qd}$ and $\varvec{W}$ as follows:

$$\begin{aligned} \begin{aligned} \varvec{K}_p&= 5000\varvec{E}_2, \\ \varvec{K}_d&=100\sqrt{2},\\ \varvec{q}_r&= \begin{bmatrix} {\phi }_r^T&0,&0,&0,&0,&0,&0,&0,&0 \end{bmatrix}^T, \\ {\phi }_r&=[\mathrm{Ser}(t,1), \cdots , \mathrm{Ser}(t,7)]^T,\\ \mathrm{Ser}(t,i)&=\frac{\pi ^2}{14}\sin \left( \frac{\pi }{4}t-\frac{2\pi i}{7}\right) ,\\ \varvec{K}_{qp}&= \mathrm{block~diag}\left( 200\varvec{E}_7,10\varvec{E}_8\right) ,\\ \varvec{K}_{qd}&= \mathrm{block~diag}\left( 20\sqrt{2}\varvec{E}_7,2\sqrt{10}\varvec{E}_8\right) , \\ \varvec{W}&=\mathrm{block~diag}\left( \varvec{E}_7, 1000\varvec{E}_8\right) . \end{aligned} \end{aligned}$$

If the reference angle (${\phi }_r$) is not given, i.e., if we set $\varvec{k}=\varvec{0}$, the robot tends to solely use extensions to follow the reference path of the head. That causes the robot to converge to a singular posture: the fully stretched robot can be regarded as an ordinary snake-like robot without prismatic joints and the straight line is known to be a singular posture for the ordinary snake-like robot. To avoid such a problem, we set ${\phi }_r$ as above. In addition, when the robot does not take the singular posture, the head tracking is achieved without using ${\phi }_r$. Therefore, the difference of the frequencies between ${\phi }_r$ and $\varvec{r}_{hr}$ does not cause problems.

Note that, due to a constraint of the simulator, it was difficult to make the joints output specified torques and forces. Therefore, in the physical simulator, we controlled the joints by a position controller. To make it applicable, we converted our controller’s output to the reference position and the reference velocity. Let the reference value of $\varvec{q}$ and $\dot{\varvec{q}}$ to be calculated as follows:

$$\begin{aligned} \begin{aligned}&\ddot{\varvec{q}}={\mathcal {M}}^{-1}(\varvec{B}_q\varvec{u}+\varvec{b}_q-M_{22}^{-1}\varvec{M}_{32}b_\theta ),\\&\dot{\varvec{q}}_{\mathrm{sim}}=\dot{\varvec{q}}+\ddot{\varvec{q}}\varDelta t, \\&\varvec{q}_{\mathrm{sim}}=\varvec{q}+\dot{\varvec{q}}\varDelta t+\frac{1}{2}\ddot{\varvec{q}}\varDelta t^2, \end{aligned} \end{aligned}$$

where $\varvec{q}_{\mathrm{sim}}$ is the reference value of the joints, $\dot{\varvec{q}}_{\mathrm{sim}}$ is the reference velocity and $\varDelta t$ is the step time of the simulator (10 ms). A similar calculation is also used in Sim3 and Sim4. Position servo motors are often used as the joints of real snake-like robots. Using the method above, our controller can be applied to such real snake-like robots.

3.3 The parameters of the ordinary snake-like robot

Fig. 3

Ordinary snake-like robot used in Sim3 and Sim4

The simulation model of the ordinary snake-like robot is shown in Fig. 3. Before mentioning its parameters, we introduce the equation of motion of the ordinary snake-like robot briefly.

$$\begin{aligned} \begin{bmatrix} \varvec{E}_2 &{}\tilde{\varvec{M}}_{12} &{}\tilde{\varvec{M}}_{13} \\ \varvec{0}_{1\!\times \!2} &{}\tilde{M}_{22} &{}\tilde{\varvec{M}}_{23} \\ \varvec{0}_{(n\!-\!1)\!\times \!2} &{}\tilde{\varvec{M}}_{32} &{}\tilde{\varvec{M}}_{33} \end{bmatrix} \begin{bmatrix} \ddot{\varvec{r}}_h \\ \ddot{\theta }_1 \\ \ddot{{\phi }} \end{bmatrix} = \begin{bmatrix} \varvec{0} \\ 0 \\ -\varvec{u} \end{bmatrix} + \begin{bmatrix} \tilde{\varvec{b}}_{h} \\ \bar{b}_\theta \\ \tilde{\varvec{b}}_\phi \end{bmatrix}, \end{aligned}$$

$$\begin{aligned} \begin{aligned}&\tilde{\varvec{M}}_{12}\in \varvec{R}^{2\!\times \!1}, \tilde{\varvec{M}}_{13}\in \varvec{R}^{2\!\times \!(n\!-\!1)},\\&\tilde{M}_{22}\in \varvec{R}, \tilde{\varvec{M}}_{23}\in \varvec{R}^{1\!\times \!(n\!-\!1)}, \\&\tilde{\varvec{M}}_{32}\in \varvec{R}^{(n\!-\!1)\!\times \!1}, \tilde{\varvec{M}}_{33}\in \varvec{R}^{(n\!-\!1)\!\times \!(n\!-\!1)}, \\&\tilde{\varvec{b}}_h\in \varvec{R}^2, \bar{b}_\theta \in \varvec{R}, \tilde{\varvec{b}}_\phi \in \varvec{R}^{n\!-\!1}. \end{aligned} \end{aligned}$$

This equation is almost the same as (1). Because of the absence of prismatic joints, the variable $\varvec{q}$ in (1) turned into ${\phi }$, and the input $\varvec{u}$ consists only of the torque.

We designed the controller for this robot in the same way as Section 2 using the partial feedback linearization technique.

$$\begin{aligned}&\varvec{u}=-(\tilde{{\mathcal {M}}}\varvec{v}-\tilde{\varvec{b}}_\phi +\tilde{M}_{22}^{-1}\tilde{\varvec{M}}_{32}\bar{b}_\theta ),\\&\begin{aligned} \varvec{v}=&-\bar{\tilde{\varvec{M}}}_{12}^+(\bar{\varvec{v}}+\tilde{M}_{22}^{-1}\tilde{\varvec{M}}_{12}\bar{b}_\theta -\tilde{\varvec{b}}_h)\\&+(\varvec{E}_{2n\!-\!1}\!-\!\tilde{\bar{\varvec{M}}}_{12}^+\tilde{\bar{\varvec{M}}}_{12})\varvec{k}, \\ \end{aligned} \\&\bar{\varvec{v}}=\ddot{\varvec{r}}_{hr}-\tilde{\varvec{K}}_d(\dot{\varvec{r}}_{h}-\dot{\varvec{r}}_{hr})-\tilde{\varvec{K}}_p(\varvec{r}_h-\varvec{r}_{hr}), \end{aligned}$$

where

$$\begin{aligned} \begin{aligned} \tilde{{\mathcal {M}}}&=\tilde{\varvec{M}}_{33}-\tilde{M}_{22}^{-1}\tilde{\varvec{M}}_{32}\tilde{\varvec{M}}_{23}, \\ \tilde{\bar{\varvec{M}}}_{12}&=\tilde{\varvec{M}}_{13}-\tilde{M}_{22}^{-1}\tilde{\varvec{M}}_{12}\tilde{\varvec{M}}_{23}, \\ \tilde{\bar{\varvec{M}}}_{12}^+\!&=\!\tilde{\varvec{M}}_{12}^T(\tilde{\varvec{M}}_{12}\tilde{\varvec{M}}_{12}^T)^{-1}, \\ \varvec{k}&=\ddot{{\phi }}_{r}-\tilde{\varvec{K}}_{\phi d}(\dot{{\phi }}-\dot{{\phi }}_{r})-\tilde{\varvec{K}}_{\phi p}({\phi }-{\phi }_{r}), \end{aligned} \end{aligned}$$

and $\tilde{\varvec{K}}_p$, $\tilde{\varvec{K}}_d$, $\tilde{\varvec{K}}_{\phi p}$, $\tilde{\varvec{K}}_{\phi d}$ are arbitrary positive definite matrices.

To reduce the difference of inertial parameters of the two simulation models, the simulation model of the ordinary snake-like robot is made from the extendable snake-like robot’s simulation model by fixing the prismatic joints’ extension to 0. We can regard a pair of two links of the extendable snake-like robot connected by a prismatic joint as one link of the ordinary snake-like robot. Thus the number of links was 8, the link length was 16 cm, and the weight of the link was 0.5 kg. The gain and ${\phi }_r$ are defined as follows:

$$\begin{aligned} \begin{aligned} \tilde{\varvec{K}}_p&= 10000\varvec{E}_2, \\ \tilde{\varvec{K}}_d&=200,\\ {\phi }_r&=[\mathrm{Ser}(t,1), \cdots , \mathrm{Ser}(t,7)]^T,\\ \tilde{\varvec{K}}_{\phi p}&= 9.5\varvec{E}_7,\\ \tilde{\varvec{K}}_{\phi d}&= 6.1644\varvec{E}_7.\\ \end{aligned} \end{aligned}$$

Note that different gains were used from the ones for the extendable snake-like robot. In the present study, we chose the gain which gave almost the same error value in Sim1 and Sim3.

3.4 The results of Sim1 and Sim2

Fig. 4

The motion of the robot ($t = 0$ – 10 s): Sim1

Fig. 5

The motion of the robot ($t = 0$ – 10 s): Sim2

Fig. 6

The tracking error $\Vert \!\varvec{r}_h\!-\!\varvec{r}_{hr}\!\Vert$: Sim1

Fig. 7

The tracking error $\Vert \!\varvec{r}_h\!-\!\varvec{r}_{hr}\!\Vert$: Sim2

Fig. 8

The extension of the prismatic joints: Sim1

Fig. 9

The extension of the prismatic joints: Sim2

Figure 4 shows the position and the posture of the extendable snake-like robot at $t = 0$, 5, 6, 10 s in Sim1. Figure 5 shows the position and the posture in Sim2. The red ball shows the reference position at the time. The robot seemed to track the reference well in both simulations.

Figure 6 shows the discrepancy between the reference and the actual head positions $\Vert \varvec{r}_h - \varvec{r}_{hr}\Vert$ in Sim1. It shows that the error value stayed around 1 cm. The reason why $\Vert \varvec{r}_h-\varvec{r}_{hr}\Vert \rightarrow 0$ $(t\rightarrow \infty )$ was not achieved will be the difference in the friction between the simple model and the simulator. In the simple model, we assumed that anisotropic friction force was distributed over the link. Because of a constraint on the physical simulator, we used passive wheels to represent anisotropy. This makes the frictional torque acting on links close to zero because the wheel lifts the link in the air. Besides, the friction is treated as Coulomb friction in the physical simulator. Thus, using the wheels and the property of the simulator led to a modeling error. However, even the largest error value (about 2.8 cm) was only 2.2 % of the length of the robot. Thus, we conclude that the controller worked well even with a modeling error. Moreover, Fig. 8 shows that the prismatic joints gradually extended. This is because the robot tried catching up with the reference position from the initial head velocity (0 m/s) using both the movement of the entire body and extension of the prismatic joints. That behavior can seem reasonable and effective to catch up with the reference. Therefore, we conclude that the controller successfully generated the prismatic joints’ motion based on the given task.

Figure 7 shows the error in Sim2. As in Sim1, the error value stayed around 1 cm most of the time. The difference is that the error value between 5 s and 6 s was relatively large. Furthermore, Fig. 9 shows that sharp extension of the prismatic joints occurred around $t \simeq 6$ s, which was not observed in Sim1. This implies that the controller generated the extension to track the sudden velocity change. Taking a closer look, around $t \simeq 6$ s in Fig. 9, $d_1$ and $d_2$ reached the maximum extension (10 cm) and could not extend further. Thus, the large error around $t \simeq 6$ s was caused by both the discontinuous change of the reference velocity and the limit of the extension. However, the largest error (about 4.5 cm) was also relatively small to the length of the robot (3.5 %). Therefore, we conclude that the proposed controller worked well in the situation.

3.5 The results of Sim3 and Sim4

Fig. 10

The motion of the robot ($t = 0$ – 10 s): Sim3

Fig. 11

The motion of the robot ($t = 0$ – 10 s): Sim4

Fig. 12

The tracking error $\Vert \!\varvec{r}_h\!-\!\varvec{r}_{hr}\!\Vert$: Sim3

Fig. 13

The tracking error $\Vert \!\varvec{r}_h\!-\!\varvec{r}_{hr}\!\Vert$: Sim4

Figures 10 and 11 show the position and the posture of the ordinary unextendable snake-like robot at $t = 0$, 5, 6, 10 s in Sim3 and Sim4 respectively.

Figure 12 shows the error value during Sim3. Unlike Sim1, the error between $t=5$ s and $t=10$ s was relatively large. As can be seen in Fig. 10, the posture of the robot is close to an arc at $t=5$ s and $t=6$ s, which is known to be a singular posture. This suggests that it was difficult for the robot to track the reference trajectory without the aid of additional degrees of freedom at those time moments. Considering this result, in Sim1, the prismatic joints helped to keep the robot from falling into an arc shape by extending the body length. Except for the error between $t = 5$ s and $t = 6$ s, the error value stayed around 1 cm as the extendable snake-like robot (Sim1). Thus, when (12) is given as a reference, both controllers exhibit almost the same performance.

Figure 13 shows the error value during Sim4. It indicates that the ordinary snake-like robot failed in tracking the reference trajectory when the discontinuous velocity input was given. Figure 11 also shows that the robot was left behind at $t = 6$ s and coiled itself up at $t = 10$ s far behind the reference position at the time. This result illustrates the effectiveness of equipping snake-like robots with prismatic joints.

4 Conclusion

In this paper, we developed the controller for the extendable snake-like robot and verified the effectiveness of the controller and the prismatic joint by the simulations. Our new controller successfully generates the motion of the joints based on the given trajectory of the robot. That will enhance the use of extendable snake-like robots. Furthermore, the results also showed the superiority of extendable snake-like robots against ordinary unextendable snake-like robots.

However, the following problems remain. First, the sufficient condition for Assumption 1 to hold is unclear. Though we found that some negative extension of the prismatic joints makes Assumption 1 fail, however, it is not quite sure that the assumption always holds when all the joints’ lengths take positive values. In addition, the sufficient condition under the existence of the movable limit of the joints also needs to be discussed. Our future work needs to consider the movable limit. Besides, the robustness of the controller against the environmental change has not been examined in this paper. We assumed the robot moved on a floor where the frictional property did not change. In the real world, the friction can vary from position to position, which can cause a source of an error. Future works include the application of robust control techniques to our controller. Regarding the gains, our controller has numerous gain parameters to be tuned. We will work on not only the application of robust control but also data-driven gain tuning methods.