Locomotion Control of Snake-Like Robot with Rotational Elastic Actuators Utilizing Observer

Abstract

The purpose of this paper is designing a head control system capable of adapting to passive side-slipping. The environments in which snake-like robots are expected to be utilized generally have ground surface conditions with nonuniform frictional coefficients. In such conditions, the passive wheels of the snake-like robot have a chance of side-slipping. To locomote the snake-like robot dexterously, a control system which adapts to such side-slipping is desired. There are two key points to realizing such a system: First, a dynamic model capable of representing the passive side-slipping must be formulated. A solution for the first key point is to develop a switching dynamic model for the snake-like robot, which switches depending on the occurrence of the side-slipping, by utilizing a projection method. The second key point is to adapt the control system’s behavior to side-slipping. An idea for such a solution is to include the side-slipping velocity in the weighting matrices. An algorithm to estimate the occurrence of side-slipping and the particular side-slipping link is constructed, to formulate the dynamic model depending on the actual side-slipping situation. The effectiveness of the designed Luenberger observer and the head control system for side-slipping adaptation is verified through numerical simulation.

1. Introduction

A real snake has a simple, string-like figure and can locomote by utilizing the difference between friction in the propulsive direction and that in the normal direction. It can locomote not only on flat land, but also irregular terrains, such as those of deserts, wildlands, and grasslands, by choosing its motion and posture depending on the environment/task. Moreover, it can realize skilled motions, such as swimming, climbing, and squeezing. Overall, a real snake possesses high adaptability, corresponding to the given environments/tasks. A snake-like robot mimics the high level of adaptability of a real snake and is expected to be adaptable to given environments/tasks.

There have been many studies in the literature focused on the locomotion control of snake-like robots. Hirose, a pioneer in the study of snake-like robots, found that the curvature of a snake changes sinusoidally along its body axis. He named this the “Serpenoid Curve” and applied it to trajectory generation for snake-like robots [1]. Endo et al. implemented the propulsive locomotion control of snake-like robots by utilizing the Serpenoid Curve [2]. In studies extending the Serpenoid Curve, Yamada et al. proposed the kinematics to stabilize the head direction during tracking of the robot to the Serpenoid Curve and implemented the kinematics in a robot [3]. Ma et al. implemented a slope climbing control adjusting the amplitude of the Serpenoid Curve by a simulator [4]. In our previous study, we designed a servo control system for tracking to the Serpenoid Curve [5]. Bing et al. implemented smooth walking by adjusting the Central Pattern Generator (CPG) parameters [6]. Dao et al. proposed a head trajectory tracking control method using CPG and the Amplitude Modulation Method (AMM) [7]. Wang et al. implemented locomotion control of a serpentine crawling robot using CPG and a Fuzzy Logic Controller (FLC) [8]. Manzoor et al. implemented motion control of a modular robot using the CPG algorithm [9]. These studies focused on the kinematics and morphology of the snake, but did not address the dynamics involving interactions with the ground.

To consider such dynamics, locomotion control utilizing the difference of friction, like that of a real snake, has been proposed. Izu et al. [10], Yamakita et al. [11], and Matsuno et al. [12] implemented locomotion control avoiding singular configurations, based on dynamic manipulability, by regarding friction in the axial direction as a constraint force. Nakajima et al. realized the locomotion control of a snake-like robot by considering a transition between two nonparallel planes [13]. Fukushima et al. proposed a front-unit tracking control theory utilizing only the current target velocity for the front-unit of a snake-like robot using a screw-driving mechanism [14]. Ariizumi et al. proposed a head position tracking control system, which was capable of locomotion even when a joint suddenly lost energy [15]. In addition, Tanaka et al. implemented a semi-autonomous step climbing control by utilizing both sensor information and a three-dimensional manipulation method [16], and proposed a workspace control system for a redundant robot with a gripper on its head part [17]. They also proposed a shape control method for a snake robot that maintains the head position and orientation as well as avoiding joint limits and self-collision [18], and proposed a smooth controller for an articulated mobile robot with switching constraints [19]. In our previous study, locomotion control of a snake-like robot was realized and a head position control [20,21,22], force control [23], and slope climbing control [24] were designed. Xiao et al. designed a walking method for moving pipes and curved pipes with different diameters and proposed continuous-time dynamical models which integrated an extended Kalman filter (EKF) and an Inertial Measurement Unit (IMU) [25,26]. Bai et al. developed a snake-like robot for coal mine rescue and proposed an algorithm to avoid obstacles using the particle swarm optimization weight coefficient of extreme learning machine (PSOELM) method [27]. Hongyan et al. proposed a real-time obstacle avoidance algorithm based on multiobjective optimization [28]. Suhara et al. proposed an algorithm which runs through a curved pipe that connects straight pipes by spiral rotation [29,30]. Ariizumi et al. analyzed the three actions of sidewinding locomotion, sinus-lifting motion, and lateral undulation of a snake-like robot [31]. Rollinson et al. used an extended Kalman filter (EKF) to move a pipe that cannot be passed by a wheeled robot [32]. Takagi et al. proposed a control design method using side-thrust links, in which the amplitude varies with time [33,34]. Malayjerdi et al. analyzed three-dimensional motion, based on the Gibbs–Appell method [35,36]. These studies have supposed either a uniform or infinite friction coefficient. However, the friction coefficient is generally nonuniform in an actual environment and is finite. Therefore, side-slipping normally occurs, even if the robot features passive wheels.

To address such an issue, control systems based on a dynamic model with side-slipping have also proposed. Bando et al. realized a localization on pipes by sounds during locomotion [37]. Takemori et al. implemented locomotion control over irregular ground, such as a flanged pipe or rubble terrain, by utilizing a dynamic model without considering side-slipping [38,39]. Additionally, Takemori et al. designed a control system to fit configurations of a snake-like robot to targets, by connecting simple shapes, such as straight lines, arc shapes, and spirals [40]. In the work by the authors of [40], ladder climbing locomotion was implemented by the proposed method [40]. For another example, Tanaka et al. proposed a shape-control system for a snake-like robot which preserves the head coordinate [18]. In the work by the authors of [18], the side-slipping was actively controlled by lifting up part of the robot’s body. However, side-slipping occurs unpredictably and is uncontrollable. Thus, it can be regarded as passive for the control system.

The purpose of this study is to design a head position control system which is capable of adapting to passive side-slipping. There are two key points to realize this: First, a dynamic model capable of representing passive side-slipping must be formulated. Second, the control system must be able to adapt to the side-slipping. A solution for the first key point is to switch a dynamic model of the snake-like robot, depending on occurrence of side-slipping. In this paper, the Projection Method [41,42,43] is utilized for this solution. The linear complementarity problem (LCP) is a general method for switching the dynamic model [44]. Likjebäck et al. [45] and Suzuki [46], as well as our previous study, formulated a dynamic model capable of switching model depending on environmental load forces or frictional forces. However, Suzuki noted that there is an issue in real-time processing, due to its heavy calculation load. The Projection Method provides a means to provide a further solution to this issue. The Projection Method is a system modeling method that can be used to derive a dynamic model focusing on constraints of its system, and is capable of formulating constraint forces in parallel with the dynamic model of the system. In addition, the constraint forces can be added to change the dynamics (i.e., is able to switch the dynamic model). Static frictional forces generated in the axial direction of the passive wheel can be regarded as a constraint force in the Projection Method. Therefore, by switching the added constraint forces, depending on the side-slipping, the dynamic model is switched. Based on this idea, the dynamic model of a snake-like robot without constraints, with respect to the side-slipping of the passive wheel, is derived first. Note that the model addressed in this paper has a Rotational Elastic Actuator (REA) installed [47], in order to consider interaction with the environment. Second, the constraint forces in the axial direction are formulated and are added into the dynamic model. A solution for the second key point is to tune the weighting matrices, depending on the side-slipping, in order to converge the side-slipping as soon as possible. An idea for this solution is to include the side-slipping velocity into the weighting matrices. The control system designed in this paper consists of a state observer and a nonlinear quadratic regulator, both based on the State-Dependent Riccati Equation (SDRE) [48,49]. As the state observer, the Luenberger observer is adopted, in order to verify the theoretical effectiveness of our ideas without any noise. To estimate the state accurately, establishing an accurate model is crucially important (i.e., the model must include accurate constraint forces corresponding to the actual side-slipping situation). To accomplish this, an algorithm to estimate the occurrence of side-slipping and the side-slipping link is constructed. The algorithm discriminates both the occurrence of side-slipping and the side-slipping link from the estimated state and observed output. The weighting matrices of the Luenverger observer are tuned depending on the discrimination result. Rapid and accurate state estimation are obtained. Additionally, in the nonlinear quadratic regulator, the discrimination result is utilized to construct an accurate state-space representation. Furthermore, control inputs are obtained by including the velocities in the axial direction into the weighting matrices.

This paper is organized as follows: In Section 2, the dynamic model of a five-link snake-like robot with passive side-slipping switching is derived. The passive side-slipping is represented by utilizing the constraint forces formulated by the Projection Method. Next, the Luenberger observer based on SDRE is designed in Section 3. The algorithm to estimate the generation of side-slipping is also constructed. In Section 4, the head position control system which adapts to the side-slipping is designed. The idea to include the velocities in the axial direction into the weighting matrices of SDRE, which is also proposed in detail. The effectiveness of the designed Luenberger observer and the designed head position control are verified through numerical simulation in Section 5. Section 6 concludes the paper.

2. Modeling of the Snake-Like Robot Switching Side-Slipping Passively

In this section, the dynamic model of a five-link snake-like robot with passive side-slipping is derived. As a modeling method, the Projection Method is utilized. The Projection Method provides a dynamic model of the system, as well as its constraint forces. For the constraint forces, forces to connect the robot’s link and the ones to prevent side-slipping are listed. In this paper, the forces to prevent side-slipping are calculated depending on the environment (i.e., passively) and, by adding them into the model, side-slipping of the model occurs passively. According to this, the dynamic model of the snake-like robot without constraints, with respect to the side-slipping of the passive wheel, is derived first. After that, the constraint forces to prevent side-slipping, which passively switches corresponding to the environment, are derived and the initial model is extended.

2.1. Modeling of the Five-Link Snake-Like Robot with a Rotational Elastic Actuator

First, a dynamic model of the snake-like robot without a constraint for the passive wheels without side-slipping is derived. A schematic figure of the model is shown in Figure 1, and its physical parameters are defined in

Table 1. Physical parameters ($i=1,\cdots ,5$, $j=2,\cdots ,5$).

Parameters	Notation	Value
Inertia moment [kg·m${}^2$]	$J_i$	0.1
	$J_{js}$	0.001
Viscous friction coefficient [Nm·s/rad]	$C_i$	0.1
	$C_{js}$	0.001
Mass [kg]	$m_i$	1.0
	$m_{js}$	0.1
Length from extremity to center of gravity [m]	$d_i$	0.2
Length from center of gravity to end [m]	$l_i$	0.2
Spring constant [Nm/rad]	$k_j$	5.0

. From Figure 1, the passive wheel is installed at the center of gravity of each link. The subscript s represents the element related to the spring; ${\theta }_i(i=1,\cdots ,5)$ and ${\theta }_{js}(j=2,\cdots ,5)$ represent the absolute angle of each link and spring, respectively; ${\omega }_i,v_{xi},v_{yi}$ and ${\omega }_{js},v_{xjs},v_{yjs}(i=1,\cdots ,5,j=2,\cdots ,5)$ represent quasi-velocities at each center of gravity.

First, a model of the constraint-free system is derived. From Figure 1, the generalized coordinate ${\mathit{x}}_{\mathit{p}}$ of this system is defined as

$$\begin{array}{cc}\hfill {\mathit{x}}_{\mathit{p}}& ={\left[\begin{array}{cccccc}{\mathit{x}}_{\mathit{p}\mathbf{1}}& {\mathit{x}}_{\mathit{p}\mathbf{2}\mathit{s}}& {\mathit{x}}_{\mathit{p}\mathbf{2}}& \cdots & {\mathit{x}}_{\mathit{p}\mathbf{5}\mathit{s}}& {\mathit{x}}_{\mathit{p}\mathbf{5}}\end{array}\right]}^T,\hfill \\ \hfill {\mathit{x}}_{\mathit{pi}}:& =\left[\begin{array}{ccc}{\theta }_i& x_i& y_i\end{array}\right],i=1,\cdots ,5,\hfill \\ \hfill {\mathit{x}}_{\mathit{pjs}}:& =\left[\begin{array}{ccc}{\theta }_{js}& x_{js}& y_{js}\end{array}\right],j=2,\cdots ,5.\hfill \end{array}$$

(1)

As deriving the model of the constraint free system is equivalent to formulating motion equations, both in the rotational direction and in the translation direction of (1) (according to Newton’s equation of motion), the model is derived as follows,

$${\mathit{M}}_{\mathit{p}}{\ddot{\mathit{x}}}_{\mathit{p}}={\mathit{h}}_{\mathit{p}},$$

(2)

where ${\mathit{M}}_{\mathit{p}}$ and ${\mathit{h}}_{\mathit{p}}$ represent the generalized mass matrix and the generalized force matrix, respectively, which are defined as follows,

$$\begin{array}{cc}\hfill {\mathit{M}}_{\mathit{p}}& =\mathrm{diag}({\mathit{M}}_{\mathit{p}\mathbf{1}},{\mathit{M}}_{\mathit{p}\mathbf{2}\mathit{s}},{\mathit{M}}_{\mathit{p}\mathbf{2}},\cdots ,{\mathit{M}}_{\mathit{p}\mathbf{5}\mathit{s}},{\mathit{M}}_{\mathit{p}\mathbf{5}}),\hfill \\ \hfill {\mathit{h}}_{\mathit{p}}& ={\left[\begin{array}{cccccc}{\mathit{h}}_{\mathit{p}\mathbf{1}}& {\mathit{h}}_{\mathit{p}\mathbf{2}\mathit{s}}& {\mathit{h}}_{\mathit{p}\mathbf{2}}& \cdots & {\mathit{h}}_{\mathit{p}\mathbf{5}\mathit{s}}& {\mathit{h}}_{\mathit{p}\mathbf{5}}\end{array}\right]}^T,\hfill \\ \hfill {\mathit{M}}_{\mathit{pi}}:& =\mathrm{diag}(J_i,m_i,m_i),i=1,\cdots ,5,\hfill \\ \hfill {\mathit{M}}_{\mathit{pjs}}:& =\mathrm{diag}(J_{js},m_{js},m_{js}),j=2,\cdots ,5,\hfill \\ \hfill {\mathit{h}}_{\mathit{pi}}:& =\left[\begin{array}{ccc}-{\tau }_{i+1}-k_i({\theta }_i-{\theta }_{is})-C_i({\dot{\theta }}_i-{\dot{\theta }}_{is})+C_{i+1s}({\dot{\theta }}_{i+1s}-{\dot{\theta }}_i)& 0& 0\end{array}\right],\hfill \\ \hfill {\mathit{h}}_{\mathit{pjs}}:& =\left[\begin{array}{ccc}{\tau }_j+k_j({\theta }_j-{\theta }_{js})-C_j({\dot{\theta }}_{js}-{\dot{\theta }}_{j-1})+C_j({\dot{\theta }}_j-{\dot{\theta }}_{js})& 0& 0\end{array}\right].\hfill \end{array}$$

${\tau }_i(i=1,\cdots ,6)$ denotes the input torques into each joint; note that ${\tau }_1={\tau }_6=0$ and $C_1=C_{6s}=0$, from Figure 1, at the first and fifth links, have no joint at their front and rear, respectively. Furthermore, from Figure 1b, the quasi-velocity $\mathit{v}$ is defined as follows,

$$\begin{array}{cc}\hfill \mathit{v}& ={\left[\begin{array}{cccccc}{\mathit{v}}_{\mathbf{1}}& {\mathit{v}}_{\mathbf{2}\mathit{s}}& {\mathit{v}}_{\mathbf{2}}& \cdots & {\mathit{v}}_{\mathbf{5}\mathit{s}}& {\mathit{v}}_{\mathbf{5}}\end{array}\right]}^T,\hfill \\ \hfill {\mathit{v}}_{\mathit{i}}:& =\left[\begin{array}{ccc}{\omega }_i& v_{xi}& v_{yi}\end{array}\right],i=1,\cdots ,5,\hfill \\ \hfill {\mathit{v}}_{\mathit{js}}:& =\left[\begin{array}{ccc}{\omega }_{js}& v_{xjs}& v_{yjs}\end{array}\right],j=2,\cdots ,5.\hfill \end{array}$$

From Figure 1, the velocity transformation matrix $\mathit{A}$ relates the generalized coordinate ${\mathit{x}}_{\mathit{p}}$ and the quasi-velocity $\mathit{v}$ as follows,

$$\begin{array}{cc}\hfill \mathit{A}& =\mathrm{diag}({\mathit{A}}_{\mathbf{1}},{\mathit{A}}_{\mathbf{2}\mathit{s}},{\mathit{A}}_{\mathbf{2}},\cdots ,{\mathit{A}}_{\mathbf{5}\mathit{s}},{\mathit{A}}_{\mathbf{5}})\hfill \\ \hfill {\mathit{A}}_{\mathit{i}}:& =\left[\begin{array}{ccc}1& 0& 0\\ 0& cos{\theta }_i& -sin{\theta }_i\\ 0& sin{\theta }_i& cos{\theta }_i\end{array}\right],i=1,\cdots ,5,\hfill \\ \hfill {\mathit{A}}_{\mathit{js}}:& =\left[\begin{array}{ccc}1& 0& 0\\ 0& cos{\theta }_{js}& -sin{\theta }_{js}\\ 0& sin{\theta }_{js}& cos{\theta }_{js}\end{array}\right],j=2,\cdots ,5,\hfill \end{array}$$

(3)

By Equation (3), Equation (2) is transformed as follows:

$$\mathit{M}\dot{\mathit{v}}=\mathit{h},$$

where, $\mathit{M}={\mathit{A}}^T{\mathit{M}}_{\mathit{p}}\mathit{A},\mathit{h}={\mathit{A}}^T({\mathit{h}}_{\mathit{p}}-{\mathit{M}}_{\mathit{p}}\dot{\mathit{A}}\mathit{v})$.

Second, a constraint matrix $\mathit{C}$ of the system satisfying $\mathit{Cv}=0$ is derived. With respect to the positions of the centers of gravity, a holonomic constraint is represented as follows,

$$\begin{array}{cc}\hfill {\mathsf{\Phi }}_{\mathit{h}}& ={\left[\begin{array}{ccccc}{{\mathsf{\Phi }}_{\mathit{h}\mathbf{2}\mathit{s}}}^T& {{\mathsf{\Phi }}_{\mathit{h}\mathbf{3}}}^T& \cdots & {{\mathsf{\Phi }}_{\mathit{h}\mathbf{5}\mathit{s}}}^T& {{\mathsf{\Phi }}_{\mathit{h}\mathbf{5}}}^T\end{array}\right]}^T=\mathbf{0},\hfill \\ \hfill {\mathsf{\Phi }}_{\mathit{hj}}& =\left[\begin{array}{c}{x}_{js}-x_{j-1}-l_{j-1}cos{\theta }_{j-1}\\ y_{js}-y_{j-1}-l_{j-1}sin{\theta }_{j-1}\end{array}\right],\hfill \\ \hfill {\mathsf{\Phi }}_{\mathit{hjs}}& =\left[\begin{array}{c}{x}_j-x_{j-1s}-d_{j-1}cos{\theta }_j\\ y_j-y_{j-1s}-d_{j-1}sin{\theta }_j\end{array}\right].\hfill \end{array}$$

(4)

In this paper, the dynamic model without the constraint, in which the passive wheels are not side-slipping, is derived, to design a control system which allows side-slipping. Therefore, the constraint matrix of the system is defined as follows,

$$\begin{array}{cc}\hfill \mathit{C}& =\frac{\partial {\mathsf{\Phi }}_{\mathit{h}}}{\partial {\mathit{x}}_{\mathit{p}}}\mathit{A}.\hfill \end{array}$$

(5)

By utilizing (5), the model satisfying the constraint conditions is derived as follows:

$$\mathit{M}\dot{\mathit{v}}=\mathit{h}+{\mathit{C}}^T\mathit{\lambda },$$

(6)

where $\mathit{\lambda }$ represents the Lagrange multipliers and denotes the constraint forces of the system in the Projection Method.

Finally, Equation (6) is projected into the tangent speed space and the dynamic model is obtained. From (1), the constraint-free system has 27 DOF. From (4), the system is constrained by 16 holonomic constraints. Therefore, the constrained system has 11 DOF. Now, we set the tangent speed as follows,

$$\dot{\mathit{q}}={\left[\begin{array}{ccccccccccc}{\omega }_1& v_{x1}& {\omega }_{2s}& {\omega }_{3s}& {\omega }_{4s}& {\omega }_{5s}& v_{y1}& v_{y2}& v_{y3}& v_{y4}& v_{y5}\end{array}\right]}^T.$$

The tangent speeds are chosen to be suitable for the control system. In this case, ${\omega }_1$ and $v_{x1}$ correspond to the first link, ${\omega }_{js}(j=2,\cdots ,5)$ are the motor velocities and $v_{yi}(i=1,\cdots ,5)$ are velocities in the axial direction. The quasi-velocity is replaced such that ${\mathit{v}}^{\prime }={\left[\begin{array}{c}{\dot{\mathit{q}}}^T,{{\mathit{v}}_{\mathit{q}}}^T\end{array}\right]}^T$ is satisfied, where ${\mathit{v}}_{\mathit{q}}$ represents the dependent velocities with respect to $\dot{\mathit{q}}$. Furthermore, the constraint matrix $\mathit{C}$ is decomposed into $\mathit{C}=\left[\begin{array}{c}{\mathit{C}}_{\mathbf{1}},{\mathit{C}}_{\mathbf{2}}\end{array}\right]$, satisfying $\mathit{C}{\mathit{v}}^{\prime }={\mathit{C}}_{\mathbf{1}}\dot{\mathit{q}}+{\mathit{C}}_{\mathbf{2}}{\mathit{v}}_{\mathit{q}}=0$. The complementary orthogonal matrix $\mathit{D}$ satisfying both $\mathit{CD}=0$ and $\mathit{v}=\mathit{D}\dot{\mathit{q}}$ is derived as follows,

$$\mathit{D}=\left[\begin{array}{c}{\mathit{I}}^{11\times 11}\\ -{{\mathit{C}}_{\mathbf{2}}}^{-1}{\mathit{C}}_{\mathbf{1}}\end{array}\right],$$

where ${\mathit{I}}^{m\times n}$ represents the $m\times n$ identity matrix. Then, Equation (6) is projected into the tangent speed space by the derived complementary orthogonal matrix $\mathit{D}$, and we obtain the dynamic model of the five-link snake-like robot as follows,

$${\mathit{D}}^T\mathit{MD}\ddot{\mathit{q}}+{\mathit{D}}^T\mathit{M}\dot{\mathit{D}}\dot{\mathit{q}}={\mathit{D}}^T\mathit{h}.$$

(7)

2.2. Constraint Forces to Prevent Side-Slipping

In this section, the constraint forces to prevent side-slipping of the passive wheels of the snake-like robot are derived. Suzuki utilized the Linear Complementarity Problem (LCP) [44] to formulate these. However, they also noted that there was an issue of high computation times preventing efficient real-time control [46]. To overcome this issue in this paper, the constraint forces to prevent passive wheel side-slipping are formulated by extending the constraint matrix derived using the Projection Method. First, the constraint matrix is extended. Second, using the extended constraint matrix, the constraint forces to prevent side-slipping are formulated. Finally, by utilizing the formulated constraint forces, Equation (7) is extended.

First, the constraint matrix of the system is extended. As shown in Figure 1, let the constraint force in the axial direction and maximum static frictional force of i-th link be denoted by $f_{ni}$ and $f_{max}$, respectively. In the axial direction of the passive wheel of a snake-like robot, the constraint forces preventing side-slipping from occurring satisfy the following condition,

$$v_{yi}=0\cap f_{ni}\le f_{max},(i=1,\cdots ,5).$$

In the above equation, as $v_{yi}=0,(i=1,\cdots ,5)$ can be described as non-holonomic constraints, we define the following new constraint matrix,

$$\begin{array}{cc}\hfill {\mathit{C}}_{\mathit{n}}& ={\left[\begin{array}{ccccc}{{\mathit{C}}_{\mathit{n}\mathbf{1}}}^T& {{\mathit{C}}_{\mathit{n}\mathbf{2}}}^T& {{\mathit{C}}_{\mathit{n}\mathbf{3}}}^T& {{\mathit{C}}_{\mathit{n}\mathbf{4}}}^T& {{\mathit{C}}_{\mathit{n}\mathbf{5}}}^T\end{array}\right]}^T,\hfill \\ \hfill {\mathit{C}}_{\mathit{ni}}& =\left\{\begin{array}{cc}\frac{\partial v_{yi}}{\partial \mathit{v}},\hfill & (v_{yi}=0\cap f_{ni}\le f_{max}),\hfill \\ \varnothing ,\hfill & \left(\mathrm{others}\right),\hfill \end{array}\right.(i=1,\cdots ,5).\hfill \end{array}$$

Moreover, both the constraint matrix of the system and the Lagrange multipliers, as the constraint forces of the system are extended as ${\mathit{C}}^{\prime }={\left[\begin{array}{cc}{\mathit{C}}^T& {{\mathit{C}}_{\mathit{n}}}^T\end{array}\right]}^T$ and ${\mathit{\lambda }}^{\prime }={\left[\begin{array}{cc}{\mathit{\lambda }}^T& {{\mathit{\lambda }}_{\mathit{n}}}^T\end{array}\right]}^T$, respectively; note that ${\mathit{\lambda }}_{\mathit{n}}$ represents the constraint force to prevent side-slipping. By utilizing the extended constraint matrix and the Projection method, the extended constraint force ${\mathit{\lambda }}^{\prime }$, including the constraint forces to prevent side-slipping ${\mathit{\lambda }}_{\mathit{n}}$, is formulated as follows,

$${\mathit{\lambda }}^{\prime }={\left({\mathit{C}}^{\prime }{\mathit{M}}^{-1}{\mathit{C}}^{\prime T}\right)}^{-1}{\mathit{C}}^{\prime }(\dot{\mathit{D}}\dot{\mathit{q}}-{\mathit{M}}^{-1}\mathit{h}).$$

Finally, by utilizing the formulated constraint force in the axial direction, Equation (7) is extended. The motion (6) in the quasi-velocity coordinate is extended, as follows:

$$\mathit{M}\dot{\mathit{v}}=\mathit{h}+{\mathit{C}}^{\prime }{\mathit{\lambda }}^{\prime }.$$

(8)

Note that (8) is equivalent to the dynamics model of the snake-like robot without side-slipping, unless any link is side-slipping. By projecting (8) into the tangent speed space, by utilizing the complementary orthogonal matrix $\mathit{D}$, we obtain the extended dynamics model shown in (9):

$$\begin{array}{c}\hfill {\mathit{D}}^T\mathit{MD}\ddot{\mathit{q}}+{\mathit{D}}^T\mathit{M}\dot{\mathit{D}}\dot{\mathit{q}}={\mathit{D}}^T\mathit{h}+{\mathit{D}}^T{{\mathit{C}}_{\mathit{n}}}^T{\mathit{\lambda }}_{\mathit{n}}.\end{array}$$

(9)

3. Design of the Luenberger Observer

In this section, a Luenberger observer is designed based on SDRE. The key functions of the observer designed in this section are to estimate the passive side-slipping occurring and to tune the observer gains to adapt to it. First, the observable states are discussed quantitatively. Second, the Luenberger observer is designed, based on SDRE. Finally, an algorithm for state estimation for adaptive side-slipping determination is constructed, and the tuning algorithm of observer gain, corresponding to the result of the state estimation algorithm, is constructed.

First, the observable states are discussed. As installation of a rotary encoder in each joint and each elastic part was realized in our previous study [47], the related angles of each joint ${\varphi }_j(j=2,\cdots ,5)$ and twist angles of the springs $s_j(j=2,\cdots ,5)$ are observable. Additionally, as the installation of a camera on the head link has also been realized in related studies [50,51], the absolute angle ${\theta }_1$ and position ($x_1,y_1$) of the robot’s head are observable. On the other hand, velocity sensors are not installed, so as to save cost and energy; thus, the tangent speed is unobservable. From the above discussion, the observable states (i.e., the output of the system) $\mathit{y}$ are

$$\begin{array}{cc}\hfill \mathit{y}& ={\left[\begin{array}{cc}{{\mathit{y}}_{\mathit{\varphi }}}^T& {\mathit{s}}^T\end{array}\right]}^T\hfill \\ \hfill & ={\left[\begin{array}{ccccccccccc}{\theta }_1& x_1& y_1& {\varphi }_2& {\varphi }_3& {\varphi }_4& {\varphi }_5& s_2& s_3& s_4& s_5\end{array}\right]}^T.\hfill \end{array}$$

Second, the Luenberger observer is designed, based on SDRE. The generalized force matrix $\mathit{h}$ is resolved as follows,

$$\begin{array}{cc}\hfill \mathit{h}& =\mathit{Eu}+\mathit{F}\dot{\mathit{\theta }}+\mathit{Gs},\hfill \\ \hfill \mathit{u}& ={\left[\begin{array}{cccc}{\tau }_2& {\tau }_3& {\tau }_4& {\tau }_5\end{array}\right]}^T,\hfill \\ \hfill \dot{\mathit{\theta }}& ={\left[\begin{array}{ccccc}{\dot{\theta }}_1& {\dot{\theta }}_{2s}& {\dot{\theta }}_2& \cdots & {\dot{\theta }}_5\end{array}\right]}^T,\hfill \\ \hfill \mathit{s}& ={\left[\begin{array}{cccc}{s}_2& s_3& s_4& s_5\end{array}\right]}^T\hfill \\ \hfill & ={\left[\begin{array}{cccc}{\theta }_2-{\theta }_{2s}& {\theta }_3-{\theta }_{3s}& {\theta }_4-{\theta }_{4s}& {\theta }_5-{\theta }_{5s}\end{array}\right]}^T,\hfill \end{array}$$

where

$$\begin{array}{cc}\hfill \mathit{E}& =\frac{\mathit{\partial }\mathit{h}}{\mathit{\partial }\mathit{u}},\hfill \\ \hfill \mathit{F}& =\frac{\mathit{\partial }(\mathit{h}-\mathit{Eu})}{\partial \dot{\mathit{\theta }}},\hfill \\ \hfill \mathit{G}& =\frac{\mathit{\partial }(\mathit{h}-\mathit{Eu}-\mathit{F}\dot{\mathit{\theta }})}{\mathit{\partial }\mathit{s}}.\hfill \end{array}$$

By utilizing the relation $\mathit{v}=\mathit{D}\dot{\mathit{q}}$, the matrices $\mathit{H}$, ${\mathit{H}}_{\mathit{s}}$, and ${\mathit{H}}_{\mathit{y}}$ satisfying $\dot{\mathit{\theta }}=\mathit{H}\dot{\mathit{q}}$, $\dot{\mathit{s}}={\mathit{H}}_{\mathit{s}}\dot{\mathit{q}}$, and ${\dot{\mathit{y}}}_{\mathit{\varphi }}={\mathit{H}}_{\mathit{y}}\dot{\mathit{q}}$, respectively, are defined as follows,

$$\begin{array}{cc}\hfill \mathit{H}& ={\left[\begin{array}{ccccccccc}\mathit{D}\left(1\right)& \mathit{D}\left(4\right)& \mathit{D}\left(7\right)& \mathit{D}\left(10\right)& \mathit{D}\left(13\right)& \mathit{D}\left(16\right)& \mathit{D}\left(19\right)& \mathit{D}\left(22\right)& \mathit{D}\left(25\right)\end{array}\right]}^T,\hfill \\ \hfill {\mathit{H}}_{\mathit{s}}& ={\left[\begin{array}{cccc}\mathit{D}\left(7\right)-\mathit{D}\left(4\right)& \mathit{D}\left(13\right)-\mathit{D}\left(10\right)& \mathit{D}\left(19\right)-\mathit{D}\left(16\right)& \mathit{D}\left(25\right)-\mathit{D}\left(22\right)\end{array}\right]}^T,\hfill \\ \hfill {\mathit{H}}_{\mathit{y}}& ={\left[\begin{array}{ccccc}{\mathit{D}}_{\mathbf{1}}& \mathit{D}\left(7\right)-\mathit{D}\left(1\right)& \mathit{D}\left(13\right)-\mathit{D}\left(7\right)& \mathit{D}\left(19\right)-\mathit{D}\left(13\right)& \mathit{D}\left(25\right)-\mathit{D}\left(19\right)\end{array}\right]}^T,\hfill \end{array}$$

where

$${\mathit{D}}_{\mathbf{1}}=\left[\begin{array}{ccc}1& 0& 0\\ 0& cos{\theta }_1& -sin{\theta }_1\\ 0& sin{\theta }_1& cos{\theta }_1\end{array}\right]\left[\begin{array}{c}\mathit{D}\left(1\right)\\ \mathit{D}\left(2\right)\\ \mathit{D}\left(3\right)\end{array}\right].$$

Note that $\mathit{D}\left(i\right)$ denotes the i-th row vector of the complementary orthogonal matrix $\mathit{D}$. Furthermore, the constraint matrix of the system is resolved as follows,

$$\begin{array}{cc}\hfill {\mathit{\lambda }}^{\prime }& =\mathit{\alpha }\dot{\mathit{q}}+\mathit{\beta }\mathit{u}+\mathit{\gamma }\mathit{s},\hfill \\ \hfill \mathit{\alpha }& ={\mathit{C}}^{\prime T}{\left({\mathit{C}}^{\prime }{\mathit{M}}^{-1}{{\mathit{C}}^{\prime }}^T\right)}^{-1}{\mathit{C}}^{\prime }(\dot{\mathit{D}}-{\mathit{M}}^{-1}\mathit{FH}),\hfill \\ \hfill \mathit{\beta }& =-{\mathit{C}}^{\prime T}{\left({\mathit{C}}^{\prime }{\mathit{M}}^{-1}{{\mathit{C}}^{\prime }}^T\right)}^{-1}{\mathit{C}}^{\prime }{\mathit{M}}^{-1}\mathit{E},\hfill \\ \hfill \mathit{\gamma }& =-{\mathit{C}}^{\prime T}{\left({\mathit{C}}^{\prime }{\mathit{M}}^{-1}{{\mathit{C}}^{\prime }}^T\right)}^{-1}{\mathit{C}}^{\prime }{\mathit{M}}^{-1}\mathit{G}.\hfill \end{array}$$

By defining the output error $\mathit{e}$ and the state of the Luenberger observer $\widehat{\mathit{x}}$ as follows,

$$\begin{array}{cc}\hfill \mathit{e}& =\mathit{y}-\widehat{\mathit{y}},\hfill \\ \hfill \widehat{\mathit{x}}& ={\left[\begin{array}{ccc}{{\mathit{y}}_{\mathit{\varphi }}}^T& {\mathit{s}}^T& {\dot{\mathit{q}}}^T\end{array}\right]}^T,\hfill \end{array}$$

the Luenberger observer is obtained as

$$\begin{array}{cc}\hfill \dot{\widehat{\mathit{x}}}& ={\mathit{A}}_{\mathit{o}}\left(\widehat{\mathit{x}}\right)\widehat{\mathit{x}}+{\mathit{B}}_{\mathit{o}}\left(\widehat{\mathit{x}}\right)\mathit{u}+\mathit{L}{\left(\widehat{\mathit{x}}\right)}^T\mathit{e},\hfill \\ \hfill \widehat{\mathit{y}}& ={\mathit{C}}_{\mathit{o}}\widehat{\mathit{x}},\hfill \\ \hfill {\mathit{A}}_{\mathit{o}}\left(\widehat{\mathit{x}}\right)& =\left[\begin{array}{ccc}{\mathit{Z}}^{7\times 7}& {\mathit{Z}}^{7\times 4}& {\mathit{H}}_{\mathit{y}}\\ {\mathit{Z}}^{4\times 7}& {\mathit{Z}}^{4\times 4}& {\mathit{H}}_{\mathit{s}}\\ {\mathit{Z}}^{11\times 7}& \tilde{\mathit{K}}& \tilde{\mathit{M}}\end{array}\right],\hfill \\ \hfill {\mathit{B}}_{\mathit{o}}(\widehat{\mathit{x}}& )=\left[\begin{array}{c}{\mathit{Z}}^{7\times 4}\\ {\mathit{Z}}^{4\times 4}\\ \tilde{\mathit{N}}\end{array}\right],\hfill \\ \hfill {\mathit{C}}_{\mathit{o}}& =\left[\begin{array}{cc}{\mathit{I}}^{11\times 11}& {\mathit{Z}}^{11\times 11}\end{array}\right],\hfill \end{array}$$

where ${\mathit{Z}}^{m\times n}$ represents the zero matrix with $m\times n$; in addition,

$$\begin{array}{cc}\hfill \tilde{\mathit{M}}& ={\left({\mathit{D}}^T\mathit{MD}\right)}^{-1}{\mathit{D}}^T(\mathit{FH}-\mathit{M}\dot{\mathit{D}}+\mathit{\alpha }),\hfill \\ \hfill \tilde{\mathit{N}}& ={\left(\mathit{D}\mathit{MD}\right)}^{-1}{\mathit{D}}^T(\mathit{E}+\mathit{\beta }),\hfill \\ \hfill \tilde{\mathit{K}}& ={\left({\mathit{D}}^T\mathit{MD}\right)}^{-1}{\mathit{D}}^T(\mathit{G}+\mathit{\gamma }).\hfill \end{array}$$

Note that an accent mark ∧ represents the estimated state of the observer. Moreover, $\mathit{L}\left(\widehat{\mathit{x}}\right)$ denotes the observer gain and

$$\mathit{L}\left(\widehat{\mathit{x}}\right)=-{\mathit{R}}_{\mathit{o}}\left(\widehat{\mathit{x}}\right){\mathit{C}}_{\mathit{o}}{\mathit{P}}_{\mathit{o}}\left(\widehat{\mathit{x}}\right),$$

where ${\mathit{P}}_{\mathit{o}}\left(\widehat{\mathit{x}}\right)$ is a symmetric positive definite matrix of following state-dependent Riccati equation,

$${\mathit{A}}_{\mathit{o}}\left(\widehat{\mathit{x}}\right)\mathit{P}\left(\widehat{\mathit{x}}\right)+\mathit{P}\left(\widehat{\mathit{x}}\right){\mathit{A}}_{\mathit{o}}{\left(\widehat{\mathit{x}}\right)}^T+{\mathit{Q}}_{\mathit{o}}\left(\widehat{\mathit{x}}\right)-\mathit{P}\left(\widehat{\mathit{x}}\right){{\mathit{C}}_{\mathit{o}}}^T{\mathit{R}}_{\mathit{o}}{\left(\widehat{\mathit{x}}\right)}^{-1}{\mathit{C}}_{\mathit{o}}\mathit{P}\left(\widehat{\mathit{x}}\right)=0,$$

where ${\mathit{Q}}_{\mathit{o}}\left(\widehat{\mathit{x}}\right)$ and ${\mathit{R}}_{\mathit{o}}\left(\widehat{\mathit{x}}\right)$ represent the weighting matrices for the observer, and are chosen as positive semidefinite and positive definite, respectively.

Finally, the algorithm for adaptive state estimation of side-slipping determination is constructed. The estimation precision of the designed observer depends greatly on the accuracy of the dynamic model; that is, to appropriately extend the dynamic model according to passive side-slipping is a key issue. To extend the model appropriately, slipping links are discriminated by utilizing the estimated state of the observer. Corresponding to the discriminant result, the weighting matrices of the observer are tuned. First, the generation of side-slipping is discriminated. An error vector for it is defined as follows,

$$\begin{array}{cc}\hfill {\mathit{e}}_{\mathit{d}}:& ={\left[\begin{array}{ccccc}{d}_1& d_2& d_3& d_4& d_5\end{array}\right]}^T,\hfill \end{array}$$

(10)

$$\begin{array}{cc}\hfill d_i& =\frac{\left|\begin{array}{c}{y}_i-{\widehat{\theta }}_i{x}_i-({\widehat{y}}_i-{\widehat{\theta }}_i{\widehat{x}}_i)\end{array}\right|}{\sqrt{1+{\widehat{\theta }}_i^2}},(i=1,\cdots ,5).\hfill \end{array}$$

(11)

Note that $d_i$ represents perpendicular distance from the center of gravity of the i-th link to estimated value of the i-th link. For the error vector, it is expected that, with a decrease in the norm of ${\mathit{e}}_{\mathit{d}}$, a corresponding decrease in the error $\mathit{e}$ between observation of output and the estimated state will be observed when the side-slipping link is discriminated appropriately. On the other hand, when side-slipping happens, an increase in the norm of ${\mathit{e}}_{\mathit{d}}$ and a corresponding increase in the estimation error $\mathit{e}$ are expected, due to difference between the observer model and actual model. Thus, the generation of side-slipping is discriminated when the following condition is satisfied,

$$\left|\begin{array}{c}{\dot{\mathit{e}}}_{\mathit{d}}\left[k\right]\end{array}\right|\ge \left|\begin{array}{c}{\dot{\mathit{e}}}_{\mathit{d}}[k-1]\end{array}\right|.$$

When the above condition is satisfied, a link which possesses a maximum value of $d_i$ is discriminated as a side-slipping link, and (9) is reformulated. In addition, the weighting matrices of the observer ${\mathit{Q}}_{\mathit{o}}\left(\widehat{\mathit{x}}\right)$ and ${\mathit{R}}_{\mathit{o}}\left(\widehat{\mathit{x}}\right)$ are tuned, according to the discriminant result of side-slipping:

$$\begin{array}{cc}\hfill {\mathit{Q}}_{\mathit{o}}& =\mathrm{diag}(q_{\theta 1},q_{x1},q_{y1},q_{\varphi 2},\cdots ,q_{\varphi 5},q_{s2},\cdots ,q_{s5},q_{\omega 1},q_{vx1},q_{\omega 2s},\cdots ,q_{\omega 5s},q_{vy1},\cdots ,q_{vy5}),\hfill \\ \hfill {\mathit{R}}_{\mathit{o}}& =\mathrm{diag}(r_1,r_2,r_3,r_4),\hfill \end{array}$$

where

$$\begin{array}{cc}\hfill q_{{\theta }_1},q_{x1},q_{y1},q_{\varphi 2},\cdots ,q_{\varphi 5}& =1000,\hfill \\ \hfill q_{s2},\cdots ,q_{s5}& =1,\hfill \\ \hfill q_{\omega 1},q_{vx1},q_{\omega 2s},\cdots ,q_{\omega 5s}& =1,000,000,\hfill \\ \hfill q_{vy1},\cdots ,q_{vy5}& =\left\{\begin{array}{cc}1\hfill & \left(\right|{\widehat{v}}_{yi}|\le 1.0\times {10}^{-5}),\hfill \\ 100,000\hfill & \left(\right|{\widehat{v}}_{yi}|>1.0\times {10}^{-5}).\hfill \end{array}\right.\hfill \end{array}$$

4. Design of Head Position Control System Considering Side-Slipping

In this section, the head position control system for a snake-like robot switching the side-slipping passively is designed. A strategy for the control system design is to converge side-slipping immediately. To converge the side-slipping, the weighting matrix of SDRE is tuned corresponding to the side-slipping situation. To design such a control system, a state-space representation including the head position of the snake-like robot is first formulated. Second, the system is reduced, corresponding to the state of side-slipping. Next, the head position control system is designed, based on SDRE. Finally, a tuning algorithm for the weighting matrix to converge the side-slipping is constructed.

First, the state-space representation including the head position of the snake-like robot is formulated. The head position of the snake-like robot ($x_h,y_h$) is represented as follows,

$$\begin{array}{cc}\hfill x_h& =x_1-d_{1}cos{\theta }_1,\hfill \\ \hfill y_h& =y_1-d_{1}sin{\theta }_1.\hfill \end{array}$$

By utilizing the relation ${\dot{\mathit{x}}}_{\mathit{p}}=\mathit{Av}$ and $\mathit{v}=\mathit{D}\dot{\mathit{q}}$, the velocity of the head position is related with the tangent speed as follows:

$$\begin{array}{cc}\hfill \left[\begin{array}{c}{\dot{x}}_h\\ {\dot{y}}_h\end{array}\right]& =\mathit{S}\dot{\mathit{q}},\hfill \\ \hfill \mathit{S}& =\left[\begin{array}{ccc}{d}_{1}sin{\theta }_1& 1& 0\\ -d_{1}cos{\theta }_1& 0& 1\end{array}\right]{\mathit{D}}_{\mathbf{1}}.\hfill \end{array}$$

The state vector is set as follows,

$$\mathit{x}={\left[\begin{array}{cccc}{x}_h& y_h& {\mathit{s}}^T& {\mathit{q}}^T\end{array}\right]}^T,$$

and the state-space representation including the head position of the snake-like robot is formulated, as (12), by utilizing the estimated state of the observer $\widehat{\mathit{x}}$:

$$\dot{\mathit{x}}=\mathit{A}\left(\widehat{\mathit{x}}\right)\mathit{x}+\mathit{B}\left(\widehat{\mathit{x}}\right)\mathit{u},$$

(12)

where

$$\begin{array}{cc}\hfill \mathit{A}\left(\widehat{\mathit{x}}\right)& =\left[\begin{array}{ccc}{\mathit{Z}}^{2\times 2}& {\mathit{Z}}^{2\times 4}& \mathit{S}\\ {\mathit{Z}}^{4\times 2}& {\mathit{Z}}^{4\times 4}& {\mathit{H}}_{\mathit{s}}\\ {\mathit{Z}}^{11\times 2}& \tilde{\mathit{K}}& \tilde{\mathit{M}}\end{array}\right],\hfill \\ \hfill \mathit{B}\left(\widehat{\mathit{x}}\right)& =\left[\begin{array}{c}{\mathit{Z}}^{2\times 4}\\ {\mathit{Z}}^{4\times 4}\\ \tilde{\mathit{N}}\end{array}\right].\hfill \end{array}$$

Second, the state-space representation is reduced, corresponding to the state of side-slipping. In (12), when no side-slipping link exists, the velocity in the axial direction of its link is uncontrollable, because in a link without side-slipping, it is regarded that there is a constraint with respect to slip prevention (i.e., the velocity in the axial direction is fixed to zero). To avoid uncontrollable links, the state-space representation is appropriately reduced, corresponding to the side-slipping situation. The reduced system is represented as follows,

$$\dot{\overline{\mathit{x}}}=\overline{\mathit{A}}\left(\widehat{\mathit{x}}\right)\overline{\mathit{x}}+\overline{\mathit{B}}\left(\widehat{\mathit{x}}\right)\mathit{u}.$$

For example, when all links are slipping,

$$\overline{\mathit{x}}=\mathit{x}={\left[\begin{array}{cccc}{x}_h& y_h& {\mathit{s}}^T& {\mathit{q}}^T\end{array}\right]}^T.$$

For another instance, when no links are side-slipping,

$$\overline{\mathit{x}}=\mathit{x}={\left[\begin{array}{ccccc}{x}_h& y_h& {\mathit{s}}^T& {\omega }_1& v_{x1}\end{array}\right]}^T.$$

When the first, second, and fourth links are side-slipping,

$$\overline{\mathit{x}}=\mathit{x}={\left[\begin{array}{cccccccc}{x}_h& y_h& {\mathit{s}}^T& {\omega }_1& v_{x1}& v_{y1}& v_{y2}& v_{y4}\end{array}\right]}^T.$$

Next, the head position control system is designed, based on SDRE. Now, we consider a following quadratic cost function for the reduced system:

$$J={\int }_0^{\infty }{\overline{\widehat{\mathit{x}}}}^T\mathit{Q}\left(\mathit{x}\right)\overline{\mathit{x}}+{\mathit{u}}^T\mathit{R}\left(\widehat{\mathit{x}}\right)\mathit{u}dt,$$

where ${\mathit{Q}}_{\mathit{o}}\left({\mathit{\xi }}_{\mathit{e}}\right)$ and ${\mathit{R}}_{\mathit{o}}\left({\mathit{\xi }}_{\mathit{e}}\right)$ are state-dependent weighting matrices, which are chosen to be positive semidefinite and positive definite, respectively. The input $\mathit{u}$ that quasi-minimizes the quadratic cost function is given by utilizing the estimated state of the observer as follows,

$$\mathit{u}=-\mathit{R}{\left(\widehat{\mathit{x}}\right)}^{-1}\overline{\mathit{B}}{\left(\widehat{\mathit{x}}\right)}^T\mathit{P}\left(\widehat{\mathit{x}}\right)\widehat{\overline{\mathit{x}}},$$

where $\mathit{P}\left(\widehat{\mathit{x}}\right)$ is a symmetric positive definite matrix of the following state-dependent Riccati equation,

$$\overline{\mathit{A}}{\left(\widehat{\mathit{x}}\right)}^T\mathit{P}\left(\widehat{\mathit{x}}\right)+\mathit{P}\left(\widehat{\mathit{x}}\right)\overline{\mathit{A}}\left(\widehat{\mathit{x}}\right)+\mathit{Q}\left(\widehat{\mathit{x}}\right)-\mathit{P}\left(\widehat{\mathit{x}}\right)\overline{\mathit{B}}\left(\widehat{\mathit{x}}\right)\mathit{R}{\left(\widehat{\mathit{x}}\right)}^{-1}\overline{\mathit{B}}{\left(\widehat{\mathit{x}}\right)}^T\mathit{P}\left(\widehat{\mathit{x}}\right)=0.$$

(13)

Last, the tuning algorithm to immediately converge the side-slipping is constructed. An idea to achieve this is to change the weight for the velocity in the axial direction, with respect to each sampling period. In particular, the velocity in the axial direction is incorporated into the weighting matrix. We set the weighting matrices ${\mathit{Q}}_{\mathit{o}}\left({\mathit{\xi }}_{\mathit{e}}\right)$ and ${\mathit{R}}_{\mathit{o}}\left({\mathit{\xi }}_{\mathit{e}}\right)$ as follows,

$$\begin{array}{cc}\hfill \mathit{Q}& =\mathrm{diag}(Q_{xh},Q_{yh},Q_{s2},\cdots ,Q_{s5},Q_{\omega 1},Q_{vx1},Q_{\omega j2},\cdots ,Q_{\omega j5},Q_{vy1},\cdots ,Q_{vy5}),\hfill \\ \hfill \mathit{R}& =\mathrm{diag}(R_1,R_2,R_3,R_4).\hfill \end{array}$$

where each of the elements are as follows,

$$\begin{array}{cc}\hfill Q_{xh},Q_{yh}& =1,\hfill \\ \hfill Q_{s2},\cdots ,Q_{s5},Q_{\omega 1},Q_{vx1},Q_{\omega j2},\cdots ,Q_{\omega j5}& =0.1,\hfill \\ \hfill R_1,R_2,R_3,R_4& =0.1.\hfill \end{array}$$

The elements for the velocity in the axial direction are set, below, according to the idea:

$$\begin{array}{cc}\hfill q_{vyi}& =1+\sigma \left[n\right]|{\widehat{v}}_{yi}|,(|{\widehat{v}}_{yi}|>1\times {10}^{-5}),\hfill \\ \hfill \sigma \left[n\right]& =k\sigma [n-1],\sigma \left[0\right]=1000,k=1.2.\hfill \end{array}$$

By setting the elements for the velocity in the axial direction, as above, the weights for both a link which is slipping fast and a link which continues slipping are tuned greater than others. Thus, the control system converges the side-slipping adroitly and locomotes the system effectively.

5. Numerical Simulations

The effectiveness of both the designed observer and the designed head position control system were verified through numerical simulations. The numerical simulations were performed by MaTX (Visual C++ 2005 version 5.3.37) [52]. The physical parameters utilized in the numerical simulations are shown in Table 1. The simulation time was 15 s, sampling interval was 0.01 s, and the algorithm used to solve the ordinary differential equation utilized the function “rkf45”. The initial configuration of the robot was ${\theta }_1,{\theta }_{3s},{\theta }_3,{\theta }_{5s},{\theta }_5=-\pi /6$ rad and ${\theta }_{2s},{\theta }_2,{\theta }_{4s},{\theta }_4=\pi /6$ rad. The static friction coefficient was set as in Figure 2. The set meshed pattern friction coefficients resulted in the generation of side-slipping in an unexpected fashion. The simulations were aimed at verifying adaptability to side-slipping occurring unpredictably, rather than to simulate a practical case. Thus, the simulations were performed in the artificial environment shown in Figure 2. In the simulations, the case studies in three situations, corresponding to different initial positions, were addressed.

5.1. Case 1

In case 1, the initial position of the robot’s head was ($x_h,y_h$) = (5, −5) m. The simulation results are shown in Figure 3, Figure 4, Figure 5, Figure 6, Figure 7, Figure 8, Figure 9, Figure 10, Figure 11, Figure 12, Figure 13 and Figure 14.

Figure 5, Figure 6 and Figure 7, Figure 11, and Figure 13 clearly show that the designed observer estimated all states accurately and the head position of the robot was controlled by the designed control system even when passive side-slipping occurred, as all states converged. Figure 3 shows that head position control was accomplished, as the head position converged to the origin. From Figure 13, it can be seen, as the constraint forces in the axial direction became greater than the maximum static frictional force at 5.8 s, that side-slipping occurred. However, since side-slipping converged in 0.5 s from the onset of side-slipping, it has been confirmed that the control system considering passive side-slipping functioned appropriately. In addition, Figure 8, Figure 10, Figure 12, and Figure 14 show that the state of the snake-like robot was estimated by the designed observer and the side-slipping link estimation algorithm.

5.2. Case 2

In case 2, the initial position of the robot’s head was ($x_h,y_h$) = (5, 1) m. The simulation results are shown in Figure 15, Figure 16, Figure 17, Figure 18, Figure 19, Figure 20, Figure 21, Figure 22, Figure 23, Figure 24, Figure 25 and Figure 26.

In case 2, Figure 15, Figure 16, Figure 17, Figure 18, Figure 19, Figure 20, Figure 21, Figure 22, Figure 23, Figure 24, Figure 25 and Figure 26 also show that the designed observer estimated all states accurately, and the head position of the robot was controlled by the designed control system, even when passive side-slipping occurred. In case 2, side-slipping occurred for the first link at 4.6 s, which converged in 0.6 s.

5.3. Case 3

In case 3, the initial position of the robot’s head was ($x_h,y_h$) = (−1, −2) m. The simulation results are shown in Figure 27, Figure 28, Figure 29, Figure 30, Figure 31, Figure 32, Figure 33, Figure 34, Figure 35, Figure 36, Figure 37 and Figure 38.

In case 3, Figure 27, Figure 28, Figure 29, Figure 30, Figure 31, Figure 32, Figure 33, Figure 34, Figure 35, Figure 36, Figure 37 and Figure 38 also show that the designed observer estimated all states accurately, and that the head position of the robot was controlled by the designed control system, even when passive side-slipping occurred. In case 3, side-slipping occurred for the fifth link at 0.7 s, which converged in 0.4 s.

5.4. Discussion

As a result of the three simulations, we observed the following three facts.

First, the model was switched appropriately by the Luenberger observer. Figure 14, Figure 26, and Figure 38 show that the estimation error of the velocities in the axial direction converged immediately; in particular, “converging the only slipping link” was one of the main characteristics of the designed observer. This means that the designed Luenberger observer worked as expected. Otherwise, the estimated state diverged, or the estimated velocity for other links was non-zero, as the normal Luenberger observer converges the estimation by adjusting all elements impartially. On the other hand, the observer designed in this paper changes its weighting matrices depending on the discrimination result for individual slipping links. This idea gives the observer inequality, and a specific state (i.e., the velocity of the slipping link) was estimated preferentially over others. This characteristic resulted in the simulation results.

Second, the designed control system was able to converge the side-slipping immediately, as was found in Figure 13, Figure 25, and Figure 37. This means that the proposed tuning method worked as expected. Otherwise, the robot continued locomotion with side-slipping longer than that observed in the results. Such a controller might be able to stabilize the system; however, this decreases energy efficiency.

Third, the system generated serpentine locomotion, similar to a real snake. Serpentine locomotion comprises the very basic kinematics of a real snake. Figure 9, Figure 21, and Figure 33 show that the relative angles varied sinusoidally with almost the same phase difference. This is an important characteristic of serpentine locomotion; that is, the control system based on SDRE was able to generate a snake-like morphology without any trajectory generation.

6. Conclusions

In this paper, a head position control system of a snake-like robot capable of adapting to passive side-slipping has been designed. The environments expected for utilizing such snake-like robots are generally ground surfaces featuring nonuniform frictional coefficients. On such surfaces, the passive wheels of the snake-like robot generate side-slipping unpredictably. To locomote the snake-like robot dexterously, a control system which adapts to passive side-slipping was designed. We have regarded the side-slipping which occurs unpredictably to be that which occurs passively, and have proposed the dynamic model. The proposed model represents the presence of (or lack of) side-slipping by passively switching the constraint forces in the axial direction of the passive wheels. The constraint forces in the axial direction were formulated in parallel to the dynamic model of the robot by utilizing the Projection Method. Second, a Luenberger observer to estimate the state of the snake-like robot was designed, based on SDRE. To improve the estimation precision, an algorithm for estimating passive side-slipping was constructed; corresponding to its result, a tuning method for the observer gain was proposed. Next, a head position control system was designed, based on SDRE. In addition, the tuning method to immediately converge the passively generated side-slipping has been proposed. Finally, the effectiveness of the designed observer and the designed head position control system were verified through numerical simulation. The results show that head position control with immediate side-slipping convergence was accomplished, even with unpredictably generated side-slipping.