# Path following control of planar snake robots using virtual holonomic constraints: theory and experiments

- Ehsan Rezapour
^{1}Email author, - Kristin Y Pettersen
^{1}, - Pål Liljebäck
^{1, 2}, - Jan T Gravdahl
^{1}and - Eleni Kelasidi
^{1}

**1**:3

**DOI: **10.1186/s40638-014-0003-6

© Rezapour et al.; licensee Springer. 2014

**Received: **14 April 2014

**Accepted: **18 July 2014

**Published: **28 August 2014

## Abstract

This paper considers path following control of planar snake robots using virtual holonomic constraints. In order to present a model-based path following control design for the snake robot, we first derive the Euler-Lagrange equations of motion of the system. Subsequently, we define geometric relations among the generalized coordinates of the system, using the method of virtual holonomic constraints. These appropriately defined constraints shape the geometry of a constraint manifold for the system, which is a submanifold of the configuration space of the robot. Furthermore, we show that the constraint manifold can be made invariant by a suitable choice of feedback. In particular, we analytically design a smooth feedback control law to exponentially stabilize the constraint manifold. We show that enforcing the appropriately defined virtual holonomic constraints for the configuration variables implies that the robot converges to and follows a desired geometric path. Numerical simulations and experimental results are presented to validate the theoretical approach.

### Keywords

Path following Snake robots Biologically inspired robots Virtual holonomic constraints## Introduction

Although wheels and legs are extensively used in mobile robots and are known as the conventional locomotion tools, we sometimes need more adaptable and flexible locomotion systems in order to carry out tasks in complex, narrow, and unstructured environments. In such situations, snake robots, which have significant adaptability and structural flexibility properties, are a potentially useful alternative to conventional types of mobile robots. Snake robots are relevant for applications where restriction of human involvement is important due to safety (e.g. in firefighting operations [1]) and in applications where human presence is impossible (e.g. in narrow pipe inspection tasks [2],[3]).

This paper considers path following control of snake robots. In the path following problem, the goal is to ensure that the error between the system output and a desired geometric path is asymptotically less than any pre-specified constant, while guaranteeing a forward motion along the path and boundedness of the states of the controlled system [4]. This control problem is particularly challenging for snake robots. This is due to the fact that such mechanisms are generally hyper-redundant, i.e. they have a large degree of kinematic redundancy, and this gives rise to a complicated dynamical behaviour for the system. Moreover, snake robots are underactuated, i.e. they have fewer independent control inputs than degrees of freedom (DOF), and this complicates the control design for these robots.

## Background and literature review

In general, snake robots can be categorized into two classes: snake robots which are subject to nonholonomic velocity constraints and snake robots without nonholonomic velocity constraints. Path following control of both classes of snake robots has been considered in several previous works. The majority of these works consider snake robots with nonholonomic velocity constraints, which is inspired by the world’s first snake robot developed in 1972 [5]. Nonholonomic constraints are in the form of sideslip constraints on the links of the robot, i.e. where each link is constrained from moving sideways. These constraints allow the control input to be specified directly in terms of the desired propulsion of the snake robot, which is employed in [6]-[8] for computed torque control of the position and heading of snake robots with nonholonomic velocity constraints. In [9], position and path following controllers are proposed for the case where some, but not all, of the snake robot links are subject to sideslip constraints. These constrained links can be lifted from the ground, which give the system more DOF that can be utilized to follow a trajectory while simultaneously maintaining a high manipulability. Similar approaches are considered in [10], where strategies for sinus lifting during the lateral undulatory motion are proposed. In [11], a path following controller for a snake robot with nonholonomic velocity constraints is proposed, and Lyapunov analysis is employed in order to analyse the controller.

Path following control of snake robots without nonholonomic velocity constraints is only considered in a few previous works. In [12], path following control of swimming snake robots is achieved by moving the joints according to a predetermined gait pattern while introducing an angular offset in each joint to steer the robot to some desired path. Methods based on numerical optimal control are considered in [13] for determining optimal gaits during positional control of snake robots. In [14], a control strategy is proposed for sinus lifting during lateral undulation by solving a quadratic optimization problem. In [15], numerical simulations are used to study the properties of lateral undulation that are related to the optimality of motion of the snake robot. In [16],[17], cascaded systems theory is employed to achieve path following control of a snake robot described by a simplified model. In this simplified model of the snake robot, the motion of the links is approximated as translational motion instead of rotational motion, which is valid for small joint angles. In [18], a dynamic feedback control law is proposed which controls the body shape and orientation of the snake robot. Controllability analysis of planar snake locomotion is presented in [19], and a controller for straight line path following control of snake robots is proposed and a Poincare map is investigated to prove that the resulting state variables of the snake robot, except for the position in the forward direction, trace out an exponentially stable periodic orbit.

## Research design and methodology

The contribution of this paper is a solution to the path following control problem for a snake robot without nonholonomic velocity constraints, by using virtual holonomic constraints, which is a particularly useful concept for control of oscillations (see, e.g. [20]-[24]). Using this approach, we constrain the state evolution of the system to an appropriately defined submanifold of the configuration space, which is called the constraint manifold. This manifold is defined based on the specified geometric relations among the generalized coordinates of the system which are called virtual holonomic constraints. The proposed feedback control law is designed to exponentially stabilize the constraint manifold, i.e. to enforce the virtual holonomic constraints, which allows the convergence of the snake robot to the desired path.

To our best knowledge, the application of model-based motion control approaches which rely on formal stability proofs for snake robots is very restricted in the previous literature. In particular, the only previous works which formally prove the stability of a path following controller for a snake robot without nonholonomic velocity constraints are presented in [17],[18]. In [17], cascaded systems theory is used to stabilize a desired straight path for the position of the center of mass (CM) of a snake robot which is described based on simplified kinematic and dynamic models. In contrast with [17], in this paper, we consider a more complex and accurate model of the snake robot, where the motion of the links is not modelled based on the simplifying assumptions of [17]. Preliminary results of this paper are presented in [18], where the method of virtual holonomic constraints is used for path following control of a planar snake robot. In this paper, these results are extended with a new simulation study of a 11-link snake robot. Furthermore, we extend the results presented in [18] by an experimental investigation of the performance of the control approach. A back-to-back comparison between simulations and experimental results is given, in order to contribute to bridge the gap between advanced control theory and practice.

The paper is organized as follows. First, we derive the Euler-Lagrange equations of motion of the robot. Subsequently, we state the control design objectives. Afterwards, we use the virtual holonomic constraints approach for path following control design for the snake robot. Finally, we present the results of numerical simulations and real-time experiments which illustrate the performance of the theoretical control design.

## Methods

In this section, we derive the kinematic model along with the dynamic equations of motion of the snake robot in a Lagrangian framework. Moreover, we use partial feedback linearization to write the model in a simpler form for model-based control design.

*N*-link snake robot as $x={\left[{q}_{1},{q}_{2},\dots ,{q}_{N-1},{\theta}_{N},{p}_{x},{p}_{y}\right]}^{T}\in {\mathbb{R}}^{N+2}$, where

*q*

_{ i }with

*i*∈{1,…,

*N*−1} denotes the

*i*th joint angle,

*θ*

_{ N }denotes the head angle, and the pair (

*p*

_{ x },

*p*

_{ y }) describes the position of the CM of the robot w.r.t. the global

*x*−

*y*axes. Since the robot is not subject to nonholonomic velocity constraints, the vector of the generalized velocities is defined as $\stackrel{\u0307}{x}={\left[{\stackrel{\u0307}{q}}_{1},{\stackrel{\u0307}{q}}_{2},\dots ,{\stackrel{\u0307}{q}}_{N-1},{\stackrel{\u0307}{\theta}}_{N},{\stackrel{\u0307}{p}}_{x},{\stackrel{\u0307}{p}}_{y}\right]}^{T}\in {\mathbb{R}}^{N+2}$. Using these coordinates, it is possible to specify the kinematic map of the robot. In this paper, we denote the first

*N*elements of the vector

*x*, i.e. (

*q*

_{1},…,

*q*

_{N−1},

*θ*

_{ N }), as the angular coordinates, and the corresponding dynamics as the angular dynamics of the system.

### The geometry of the problem

The (*N*+2)-dimensional configuration space of the snake robot is denoted as $\mathcal{Q}=\mathcal{S}\times \mathcal{G}$, which is composed of the shape space and a Lie group which is freely and properly acting on the configuration space. In particular, the shape variables, i.e. *q*_{
a
}=(*q*_{1},…,*q*_{N−1}), which define the internal configuration of the robot and which we have direct control on, take values in . Moreover, the position variables, i.e. *q*_{
u
}=(*θ*_{
N
},*p*_{
x
},*p*_{
y
}), which are passive DOF of the system, lie in . The velocity space of the robot is defined as the differentiable (2*N*+4)-dimensional tangent bundle of as $T\mathcal{Q}={\mathbb{\mathbb{T}}}^{N}\times {\mathbb{R}}^{N+4}$, where ${\mathbb{\mathbb{T}}}^{N}$ denotes the *N*-torus in which the angular coordinates live. The free Lagrangian function of the robot $\mathcal{\mathcal{L}}:T\mathcal{Q}\to \mathbb{R}$ is invariant under the given action of on . The coupling between the shape and the position variables causes the net displacement of the position variables, according to the cyclic motion of the shape variables, i.e. the *gait pattern*. Note that for simplicity of presentation, in this paper, we consider local representation of $T\mathcal{Q}$ embedded in an (2*N*+4)-dimensional open subset of the Euclidean space ${\mathbb{R}}^{2N+4}$.

### The forward kinematic map of the snake robot

*θ*

_{ i }and the relative joint angles

*q*

_{ i }is given by

*i*th link w.r.t. the global

*x*-

*y*axes can be, respectively, given as

*l*denotes the length of each link, and (

*p*

_{x,0},

*p*

_{y,0}) denotes the tail position (

*cf.*Figure 1). The linear velocities of the CM of the

*i*th link w.r.t. the global

*x*-

*y*axes can be found by taking the time derivative of (2)-(3) which gives

*p*

_{x,0},

*p*

_{y,0}) in (2)-(3) with the position of the CM of the robot (

*p*

_{ x },

*p*

_{ y }) using the following change of coordinates:

Substituting (7)-(8) along with their time derivatives into (2)-(5) completes the derivation of the forward kinematic map of the snake robot w.r.t. the desired specified coordinate chart $(x,\stackrel{\u0307}{x})$.

### Equations of motion of the snake robot

The majority of the previous literature on snake robots and similar mobile multi-body robotic structures, such as eel-like robots, have derived the equations of motion of these robots with a Newton-Euler formulation, i.e. where the equations describing the linear and angular motion of individual links are written separately (see, e.g. [15],[16]). This is due to the fact that it is usually not straightforward to integrate the anisotropic external dissipative forces, i.e. ground friction forces, acting on these complex robotic structures into their Euler-Lagrange equations of motion. However, ground friction forces have been proved to play a fundamental role in snake robot locomotion (see, e.g. [16]). In this paper, we derive the equations of motion of the snake robot in a Lagrangian framework, i.e. treating the robot as a whole and performing the analysis using a Lagrangian function, which is simple to follow and better suited for studying advanced mechanical phenomena such as elastic link deformations [25], which might be insightful for future research challenges on snake robots. Moreover, we integrate the anisotropic friction forces into these equations using the Jacobian matrices of the links, which gives a straightforward mapping of these forces for the equations of motion.

*simple*mechanical systems, where the Lagrangian $\mathcal{\mathcal{L}}\left({q}_{a},\stackrel{\u0307}{x}\right)$ is defined as the difference between the kinetic energy $\mathcal{K}({q}_{a},\stackrel{\u0307}{x})$ and potential energy $\mathcal{P}\left(x\right)$ of the system [26]. Since the planar snake robot is not subject to any potential field, i.e. $-\nabla \mathcal{P}\left(x\right)=0$, we may write the Lagrangian equal to the kinetic energy, which is the sum of the translational and the rotational kinetic energy of the robot:

*m*and

*J*denote the uniformly distributed mass and moment of inertia of the links, respectively. Using the Lagrangian function (9), we write the Euler-Lagrange equations of motion of the controlled system as

*i*∈{1,…,

*N*+2}, $B\left(x\right)=\left[{e}_{j}\right]\in {\mathbb{R}}^{(N+2)\times (N-1)}$ is the full column rank actuator configuration matrix, where

*e*

_{ j }denotes the

*j*th standard basis vector in ${\mathbb{R}}^{N+2}$. Moreover, $B\left(x\right)\tau \in {\mathbb{R}}^{N+2}$ with $\tau ={\left[{\tau}_{1},\dots ,{\tau}_{N-1}\right]}^{T}\in {\mathbb{R}}^{N-1}$ stands for the generalized forces resulting from the control inputs. Furthermore, ${\tau}_{f}={\left[{\tau}_{f}^{1},\dots ,{\tau}_{f}^{N+2}\right]}^{T}\in {\mathbb{R}}^{N+2}$ denotes viscous and Coulomb friction forces acting on (

*N*+2) DOF of the system. The controlled Euler-Lagrange equations (10) can also be written in the form of a second-order differential equation as

where $M\left({q}_{a}\right)\in {\mathbb{R}}^{(N+2)\times (N+2)}$ is the positive definite symmetric inertia matrix, $C(x,\stackrel{\u0307}{x})\stackrel{\u0307}{x}\in {\mathbb{R}}^{N+2}$ denotes the generalized Coriolis and centripetal forces, and the right-hand side terms denote the external forces acting on the system. The fact that the inertia matrix is only a function of the directly actuated shape variables *q*_{
a
} is a direct consequence of the invariance of the Lagrangian function (9) w.r.t. the position variables. Moreover, since rank[*B*(*x*)]<dim(*x*), the system is underactuated. This underactuation represents the lack of direct control on the head angle and the position of the CM of the robot.

The dynamic model (11) perfectly agrees with the equations of motion which are derived based on the Newton-Euler formulation in previous works (see, e.g. [16]). In order to validate the model, in the last section of this paper, we present simulation results which are obtained using the dynamic model (11) together with experimental results for the locomotion of the robot which are obtained using a physical snake robot. The agreement between simulations and experiments shows that the dynamic model (11) accurately represents the motion of the robot.

### The ground friction model

*i*(

*cf.*Figure 1) as

*i*th link in the tangential (along link

*x*-axis) and normal (along link

*y*-axis) direction of the link, respectively. The total friction force acting on link

*i*is defined as the sum of the viscous and Coulomb friction forces, which are denoted by ${f}_{{v}_{i}}$ and ${f}_{{c}_{i}}$, respectively, as

*i*as

*i*∈{1,…,

*N*},

*m*denotes the mass of a link,

*g*denotes the acceleration due to gravity, and

*μ*

_{ t }and

*μ*

_{ n }denote Coulomb friction coefficients in the tangential and normal direction of the link, respectively. Furthermore,

*c*

_{ t }and

*c*

_{ n }denote viscous friction coefficients in the tangential and normal direction of the link, respectively. Thus, we map the friction force acting on the

*i*th link to the global

*x*-

*y*frame as

*τ*

_{ f }in (11) as

denotes the transpose of the Jacobian matrix of the CM of the *i* th link.

**Remark** **1**.

As argued in [16], the motion of a snake robot with anisotropic viscous ground friction is qualitatively (but not quantitatively) similar as with anisotropic Coulomb friction. However, a viscous friction model is less complex w.r.t. control design and analysis. Accordingly, we employ a viscous friction model for the control design in this paper.

### Partial feedback linearization of the dynamic model

*β*

_{ i }is negative at any configuration ${q}_{a}\in \mathcal{Q}$. Furthermore, $\phantom{\rule{0.3em}{0ex}}{f}_{{\theta}_{N}}$,

*f*

_{ x }, and

*f*

_{ y }denote the friction forces acting on

*θ*

_{ N },

*p*

_{ x }, and

*p*

_{ y }, respectively ($\phantom{\rule{0.3em}{0ex}}{f}_{{\theta}_{N}}$ also contains Coriolis forces besides the friction forces). For the aim of analysis and model-based control design, we write (25)-(26) in a more detailed form:

are called the control vector fields, which enable us to control the internal configuration and consequently the orientation and the position of the robot in the plane.

**Remark** **2**.

The last two rows of the control vectors in (34) are composed of zero elements. This implies that the control forces have no direct effect on the dynamics of the position of the CM of the robot, i.e. (31)-(32). Furthermore, the dynamics of the position of the CM are coupled with the dynamics of the directly actuated shape variables *q*_{
a
}, i.e. (29), only through the friction forces. Accordingly, in the absence of the friction forces, the linear momentum of the robot is a conserved quantity, and the position of the CM of the robot is not controllable.

## Control design objectives and the track-follow problem formulation

In this section, we state our control design objectives which will be followed throughout the remaining sections of the paper. In particular, we stress that for a complex mobile multi-link robotic structure such as a snake robot, formulating a pure path following, trajectory tracking, or maneuvering problem is unusual (for definitions of these problem formulations, see [27]). This is due to the fact that for a part of the state variables of the system (particularly the shape variables and the head angle), it is most natural to formulate the control problem as a trajectory tracking problem, while for the other state variables (particularly the position of the CM), we may formulate the problem as a path following or a maneuvering one.

*track-follow*problem for the snake robot, which we define as a trajectory tracking formulation for a subset of the state variables, together with a path following formulation for the remaining subset, we introduce the error variable for the

*i*th joint of the robot as

*i*∈{1,…,

*N*−1}, and ${\Phi}_{i}\in \mathbb{R}$ denotes a function that defines the reference trajectory for the

*i*th joint which will be chosen through the control design in the next section. The head angle error is defined as

where ${\Phi}_{N}\in \mathbb{R}$ denotes the reference head angle for the robot.

*i*∈{1,…,

*N*−1}. Furthermore, we seek to control the head angle of the robot. The second part of the control objective is thus to make the head angle of the robot track a desired head angle such that

*x*-

*y*plane given by the pair (

*p*

_{ xd },

*p*

_{ yd }), which are parameterized by a scalar time-dependent variable

*Θ*(

*t*) as

where $\epsilon \in {\mathbb{R}}_{>0}$ is an arbitrary positive scalar. Moreover, we require that $\stackrel{\u0307}{\Theta}\left(t\right)\ge 0$ and ${lim}_{t\to \infty}\Theta \left(t\right)=\infty $ (forward motion along the path), and boundedness of the states of the controlled system.

## Path following control with virtual holonomic constraints

The idea of virtual holonomic constraints is particularly a useful concept for control of oscillations (see, e.g. [20]-[24]). We will in this section show how this approach can be used to solve the path following control problem of snake robots. In particular, we will show how, by designing the joint reference trajectories in (35) using virtual holonomic constraints and by combining this with virtual holonomic constraints motivated by line-of-sight (LOS) guidance for the head angle in (36), we are able to solve the path following control problem, i.e. achieving (40). Our main motivation for using this approach is the fact that while performing the gait pattern lateral undulation which consists of fixed periodic body motions, all the solutions of the snake robot dynamics have inherent oscillatory behaviour. Moreover, we will show how this behaviour can be analytically and *constructively* controlled based on virtual holonomic constraints. In particular, we use the word ‘constructive’ in the sense that through the feedback action, we shape the dynamics of the system such that it possesses the desired structural properties, i.e. positive invariance and exponential stability of an appropriately defined constraint manifold. To this end, we define a constraint manifold for the system, and we design the control input of (29) to exponentially stabilize the constraint manifold. The geometry of this manifold is defined based on specified geometric relations among the generalized coordinates of the system which are called virtual holonomic constraints. In particular, we call them virtual constraints because they do not arise from a physical connection between two variables but rather from the actions of a feedback controller [20].

### Trajectory planning by virtual holonomic constraints

*C*

^{1}coordinate-dependent functions ${\Phi}_{i}:\mathcal{Q}\to \mathbb{R}$ which are called the constraint functions, in the relations of the form

*Φ*

_{ i }(

*x*)=0, which can be enforced through the feedback action. In particular, for the snake robot, we define a vector-valued function

in which every element defines one constraint function for the corresponding angular coordinate of the system.

- 1.
We introduce two new states ${\left[{\varphi}_{o},{\stackrel{\u0307}{\varphi}}_{o}\right]}^{T}\in {\mathbb{R}}^{2}$ where the second-order time derivative of

*ϕ*_{ o }will be used as an additional control input that drives the snake robot towards the desired path by modifying the orientation of the robot in accordance with a path following guidance law. - 2.
In the previous section, we defined the control objective for the joints and the head angle of the robot as a trajectory tracking problem. However, it is known that holonomic constraints are coordinate-dependent equality constraints of the form

*Φ*_{ i }(*x*)=0, where*Φ*_{ i }is a time-independent function [25]. Thus, we remove this explicit time dependency from the reference joint trajectories by augmenting the state vector of the system with a new variable*η*, with $\stackrel{\u0307}{\eta}=2\pi /T$ and*η*(0)=0, where*T*denotes the period of the cyclic motion of the shape variables of the robot.

and the corresponding augmented state space by $T\widehat{\mathcal{Q}}$.

### Virtual holonomic constraints for the joint angles

*x*-

*y*plane along the curve at arc length

*s*given by

*a*,

*b*, and

*c*are positive scalars. Locomotion of a snake-like structure in accordance with the serpenoid curve, i.e. lateral undulation, is achieved if the joints of the robot move according to the reference joint trajectories in the form of a sinusoidal function with specified amplitude, frequency, and phase shift. In particular, using the foregoing defined new states, we define a constraint function for the

*i*th joint of the snake robot by

where *i*∈{1,…,*N*−1}, *α* denotes the amplitude of the sinusoidal joint motion, and *δ* is a phase shift that is used to keep the joints *out of phase*. Moreover, *ϕ*_{
o
} is an offset value that is identical for all of the joints. It was illustrated in [16] how the offset value *ϕ*_{
o
} affects the orientation of the snake robot in the plane. Building further on this insight, we consider the second-order time derivative of *ϕ*_{
o
} in the form of a dynamic compensator, which will be used to control the orientation of the robot. In particular, through this control term, we modify the orientation of the robot in accordance with a reference orientation. This will be done by adding an offset angle to the reference trajectory of each joint. We will show that this will steer the position of the CM of the robot towards the desired path. The constraint function (45) is dynamic, since it depends on the solution of a dynamic compensator.

### Virtual holonomic constraint for the head link angle

In this subsection, we define a constraint function for the head angle of the robot. In particular, we use a line-of-sight (LOS) guidance law as the reference angle for the head link. LOS guidance is a much-used method in marine control systems (see, e.g. [27]). In general, guidance-based control strategies are based on defining a reference heading angle for the vehicle through a guidance law and designing a controller to track this angle [27]. Motivated by marine control literature, in [17] based on a simplified model of the snake robot, using cascade systems theory, it was proved that if the heading angle of the snake robot was controlled to the LOS angle, then also the position of the CM of the robot would converge to the desired path. We will show that a similar guidance-based control strategy can successfully steer the robot towards the desired path. However, we perform the model-based control design based on a more accurate model of the snake robot which does not contain the simplifying assumptions of [17] which are valid for small joint angles.

*x*-axis is aligned with the desired path. Consequently, the position of the CM of the robot along the

*y*-axis, denoted by

*p*

_{ y }, defines the shortest distance between the robot and the desired path, often referred to as the

*cross-track error*. In order to solve the path following problem, we use the LOS guidance law as a virtual holonomic constraint, which defines the desired head angle as a function of the cross-track error as

where *Δ*>0 is a design parameter known as the look-ahead distance. The idea is that steering the head angle of the snake robot such that it is headed towards a point located at a distance *Δ* ahead of the robot along the desired path will make the snake robot move towards the path and follow it.

### Defining a constraint manifold

*i*∈{1,…,

*N*−1}. The constraint manifold (48) is a six-dimensional submanifold of $\widehat{\mathcal{Q}}$, since we have three different constraint variables, i.e.

*η*,

*ϕ*

_{ o },

*p*

_{ y }. The goal of the control input is to enforce the virtual holonomic constraints (47), by making

*Γ*exponentially stable for the closed-loop system and thereby achieving the control objectives (37)-(38). To this end, we define the elements of a controlled output vector $y\in {\mathbb{R}}^{N}$ for the system (29)-(32) as the difference between the angular coordinates and their corresponding constraint functions as

*𝜗*and ${\stackrel{\u0308}{\varphi}}_{o}$ such that $\left({y}_{i},{\stackrel{\u0307}{y}}_{i}\right)\to (0,0)$ for all

*i*∈{1,…,

*N*}. To this end, we first need to ensure that the given relations in (47) are stabilizable, i.e. a suitable choice of feedback can make the constraint manifold exponentially stable for the closed-loop system. For simplicity of notation, we denote the following differentials:

*i*∈{1,…,

*N*−1}. The Lie derivative of (49) along the solutions of (29)-(32) is of the form

which contains the control inputs. Consequently, the controlled output vector (49) yields a well-defined vector relative degree {2,2,…,2} everywhere on the constraint manifold *Γ*. The virtual holonomic constraints satisfying this vector relative degree condition are called regular, and regular constraints are always feasible [22], i.e. there exists a smooth feedback such that *Γ* is positively invariant for the closed-loop system. Furthermore, regular constraints in parametric form (47) are always stabilizable [21].

The well-defined vector relative degree {2,2,…,2} on *Γ* implies that the system (29)-(32) with the controlled output function (47) is input-output feedback linearizable. Consequently, we can stabilize *Γ* with an input-output feedback linearizing controller.

### Output regulation via input-output linearization

In this subsection, we will derive a control law for (29) such that the constraint manifold (48) with the constraint functions defined in (47) is globally exponentially stable for the closed-loop system. In particular, we use input-output linearization to stabilize the constraint manifold (48).

*Φ*

_{ i }(

*η*,

*ϕ*

_{ o }) for the

*i*th joint, i.e. to make $({y}_{i},{\stackrel{\u0307}{y}}_{i})\to (0,0)$ for all

*i*∈{1,…,

*N*−1}, we define an exponentially stabilizing joint control law. The second-order time derivative of the

*i*th joint tracking error, i.e. the

*i*th element of (55), is of the form

*i*th joint in (29) as

*k*

_{ p }>0 and

*k*

_{ d }>0 are the joint controller gains. These gains are chosen similar for all the joints since the links have similar inertial parameters. Inserting (57) into (56) yields

The tracking error dynamics of the *i* th joint angle (58) clearly has a globally exponentially stable equilibrium at the origin $\left({y}_{i},{\stackrel{\u0307}{y}}_{i})=(0,0\right)$, which implies that every *i* th control input (57) exponentially stabilizes the constraint manifold for the *i* th joint, and the control objective (37) is achieved.

*N*th element of the controlled output vector (49), and its second-order time derivative (i.e. the head angle error dynamics) is given by

*𝜗*

_{ i }from (57) into (59) gives

*i*th joint angle of the robot by

*Φ*

_{ i }=

*S*

_{ i }+

*ϕ*

_{ o }, where

*S*

_{ i }=

*α*sin(

*η*+(

*i*−1)

*δ*). Subsequently, based on the specified constraint functions in (47), i.e. since $\stackrel{\u0308}{\eta}=0$ and $\frac{{\partial}^{2}{\varphi}_{i}}{\partial {\varphi}_{o}^{2}}=0$, we may write (61) as

*Φ*

_{ N }(

*p*

_{ y }) for the head angle, we define the second-order time derivative of the augmented coordinate

*ϕ*

_{ o }in the form of a dynamic compensator as

*β*

_{ i }is negative-valued in any configuration, (63) is globally well-defined. Through numerical simulations, we show that the states of the dynamic compensator (63), i.e. $({\varphi}_{o},{\stackrel{\u0307}{\varphi}}_{o})$, remain bounded. By inserting (63) into (62), the error dynamics of the head angle takes the form

which clearly has a globally exponentially stable equilibrium at the origin $({y}_{N},{\stackrel{\u0307}{y}}_{N})=(0,0)$. Consequently, we have that $({y}_{N},{\stackrel{\u0307}{y}}_{N})\to (0,0)$ from any initial condition, and the control objective (38) will be achieved.

converge to and follow the desired planar path. Analytically investigating the convergence of the snake robot position to the desired path is a topic of future work. As a preliminary support of this conjecture, we provide simulation and experimental results which show that the snake robot successfully converges to and follows the desired path.

## Simulation results

*N*=11 links,

*m*=1 kg,

*l*=0.07 m, and

*J*=0.0016 kg m

^{2}(Figure 2). The friction coefficients were

*c*

_{ n }=10 and

*c*

_{ t }=1. The parameters of the joint constraint functions (45) were

*α*=

*π*/6 rad,

*η*=70

*π*

*t*/180 rad, and

*δ*=36

*π*/180 rad. The controller gains in (57) and (63) were tuned as

*k*

_{ p }=10,

*k*

_{ d }=5, ${k}_{p,{\theta}_{N}}=20$, ${k}_{d,{\theta}_{N}}=1$, and

*Δ*=1.4 m. In order to calculate ${\stackrel{\u0307}{\Phi}}_{N}$ and ${\stackrel{\u0308}{\Phi}}_{N}$, we employed the approach taken in [27] by passing

*Φ*

_{ N }through a low-pass filter of the form

*ω*

_{ n }=

*π*/2 rad, damping ratio

*ψ*

_{ f }=1, and initial condition

*Ω*(0)=[0,0]

^{ T }. As seen from the simulation results which are presented in Figures 3, 4, 5, 6, 7, the snake robot successfully converges to and follows the desired path. In particular, Figure 3 shows that the solutions of the dynamic compensator (63) remain bounded. Figure 4 shows that the joint angles track the reference angles provided by the constraint functions (45), while the tracking errors converge exponentially to zero. Figure 5 shows that the head angle tracks the reference head angle provided by the constraint function (46), while the tracking error converges to zero exponentially fast. Finally, Figure 6 shows that the CM of the robot converges to and follows the desired straight path. Moreover, in order to show the performance of the proposed tracking control law (57) in the presence of angular position measurement noise, we subjected every

*i*th joint angle

*q*

_{ i }to an additive noise by using Matlab function

*r*

*a*

*n*

*d*

*n*() which generates normally distributed pseudorandom numbers that can be considered as measurement noise for the joint angles. In particular, we added

*r*

*a*

*n*

*d*

*n*(1) to each joint angle

*q*

_{ i }in each integration step. The result of the simulation is presented in Figure 7, which shows that the joint tracking errors converge to a very small neighbourhood of zero in the presence of measurement noise.

## Experimental results

In this section, we present results from an experimental investigation of the real-time performance of the proposed control strategy using a mechanical snake robot.

### Experimental setup

The experiment was carried out using the snake robot Wheeko [16]. The robot, which is shown in Figure 2, has 10 identical joint modules, i.e. *N*=11 links. Each joint module is equipped with a set of passive wheels which give the robot anisotropic ground friction properties during motion on flat surfaces. The wheels are able to slip sideways and thus do not introduce nonholonomic velocity constraints in the system. Each joint is driven by a Hitec servo motor (HS-5955TG; Hitec RCD USA, Inc., Poway, CA, USA), and the joint angles are measured using magnetic rotary encoders. The motion of the snake robot was measured using a camera-based motion capture system from OptiTrack of type Flex 13 (NaturalPoint, Inc., Corvallis, OR, USA). The system consists of 16 cameras which are sampled at 120 frames per second and which allow reflective markers to be tracked on a submillimetre level. During the experiment, reflective markers were mounted on the head link of the snake robot in order to measure the position (*p*_{x,N},*p*_{y,N}) and orientation (*θ*_{
N
}) of the head. These measurements were combined with the measured joint angles (*q*_{1},…,*q*_{N−1}) of the snake robot in order to measure the absolute link angles (1) and the position of the CM (*p*_{
x
},*p*_{
y
}) of the robot. In order to obtain the derivatives of the reference head angle (46), we used the same technique as in the simulations, i.e. passing *Φ*_{
N
} through a low-pass filter of the form (67). The parameters of the low-pass filter were set to *ω*_{
n
}=*π*/2 and *ψ*_{
f
}=1.

*i*∈{1,…,10}. We conjecture that eliminating the joint angular velocity terms from (57) did not significantly change the dynamic behaviour of the system since the joint motion was relatively slow during the experiment. The main consequence of excluding the velocity terms from (57) is that we potentially introduce a steady-state error in the tracking of the joint angles. Consequently, since with the joint control law (68) the derivative terms in (63) are identically zero, they need not to be linearized in the head angle dynamics by the dynamic compensator. As the result, we implemented the dynamic compensator of the form

where the controller gains were ${k}_{p,{\theta}_{N}}=20$, ${k}_{d,{\theta}_{N}}=1$, *k*_{
p
}=10, and *k*_{
d
}=5. We saturated the joint angle offset *ϕ*_{
o
} according to *ϕ*_{
o
}∈[−*π*/6,*π*/6], in order to keep the joint reference angles within reasonable bounds w.r.t the maximum allowable joint angles of the physical snake robot. Moreover, from Figure 2, it can be seen that the head link of the physical snake robot does not touch the ground since the ground contact points occur at the location of the joints. As a results, we implemented (69) with $\phantom{\rule{0.3em}{0ex}}{f}_{{\theta}_{N}}\equiv 0$. The solutions of the dynamic compensator (69) were obtained by numerical integration in *LabVIEW* which was used as the development environment.

We chose the look-ahead distance of the path following controller as *Δ*=1.4 m. The initial values for the configuration variables of the snake robot were *q*_{
i
}=0 rad, *θ*_{
N
}=−*π*/2 rad, *p*_{
x
}=0.3 m, and *p*_{
y
}=1.7 m, i.e. the snake robot was initially headed towards the desired path (the *x*-axis), and the initial distance from the CM to the desired path was 1.7 m. Furthermore, the parameters of the constraint functions for the joint angles, i.e. (45), were *α*=*π*/6, *η*=70*π* *t*/180, and *δ*=36*π*/180, while the ground friction coefficients were *c*_{
t
}=1 and *c*_{
n
}=10 (i.e identical to the simulation parameters).

### Results and discussion

*x*-

*y*plane, which converged to and followed the desired path. Figure 12 compares the motion of the CM during the simulation and the experiment, which were performed using the same controller parameters in order to obtain comparable results. In particular, from Figure 12, it can be seen that the physical snake and the simulated snake follow almost the same path. However, due to precise measurement and a more accurate joint control law for the simulated snake, the path following error converges to a smaller neighbourhood of the origin. Figure 13 shows screenshots from a video recording of the experiment.

## Conclusions

This paper has considered path following control of planar snake robots by using virtual holonomic constraints. The equations of motion of the snake robot were derived using a Lagrangian framework. We then introduced virtual holonomic constraints that defined the geometry of a constraint manifold for the robot. We showed that the constraint manifold can be made positively invariant by a suitable choice of feedback, and we designed an input-output feedback linearizing control law to exponentially stabilize the constraint manifold for the system. We presented simulation and experimental results which validated the theoretical design. In particular, the robot successfully converged to and followed a desired straight path.

As a topic of future work, we aim to prove the practical stability of the desired path with the proposed control approach. Furthermore, a formal proof for boundedness of the solutions of the dynamic compensator remains as a topic of future work. Moreover, application of the proposed control strategy for more complex paths such as curved paths, using different path following guidance laws, remains as a topic of future work.

## Declarations

### Acknowledgements

This research was partly supported by the Research Council of Norway through the Centres of Excellence funding scheme, project no. 223254 AMOS and project no. 205622.

## Authors’ Affiliations

## References

- Liljebäck P, Stavdahl Ø, Beitnes A (2006) SnakeFighter - development of a water hydraulic fire fighting snake robot. In: Proc. IEEE international conference on control, automation, robotics, and vision ICARCV, Singapore.Google Scholar
- Wang Z, Appleton E (2003) The concept and research of a pipe crawling rescue robot. Adv Robot 17.4: 339–358.View ArticleGoogle Scholar
- Fjerdingen SA, Liljebäck P, Transeth AA (2009) A snake-like robot for internal inspection of complex pipe structures (PIKo). In: Proc. IEEE/RSJ international conference on intelligent robots and systems, St. Louis, MO, USA.Google Scholar
- Dacic DB, Nesic D, Teel AR, Wang W: Path following for nonlinear systems with unstable zero dynamics: an averaging solution.
*IEEE Trans Automatic Control*2011, 56: 880–886. 10.1109/TAC.2011.2105130View ArticleMathSciNetGoogle Scholar - Hirose S:
*Biologically inspired robots: snake-like locomotors and manipulators*. Oxford University Press, Oxford, England; 1993.Google Scholar - Matsuno F, Sato H (2005) Trajectory tracking control of snake robots based on dynamic model. In: Proc. IEEE international conference on robotics and automation, 3029–3034. 18–22 April 2005.Google Scholar
- Date H, Hoshi Y, Sampei M (2000) Locomotion control of a snake-like robot based on dynamic manipulability. In: Proc. IEEE/RSJ international conference on intelligent robots and systems, Takamatsu, Japan.Google Scholar
- Tanaka M, Matsuno F (2008) Control of 3-dimensional snake robots by using redundancy. In: Proc. IEEE international conference on robotics and automation, 1156–1161, Pasadena, CA.Google Scholar
- Ma S, Ohmameuda Y, Inoue K, Li B (2003) Control of a 3-dimensional snake-like robot. In: Proc. IEEE international conference on robotics and automation, vol. 2, 2067–2072, Taipei, Taiwan.Google Scholar
- Tanaka M, Matsuno F (2009) A study on sinus-lifting motion of a snake robot with switching constraints. In: Proc. IEEE international conference on robotics and automation, 2270–2275. 12–17 May 2009.Google Scholar
- Prautsch P, Mita T, Iwasaki T (2000) Analysis and control of a gait of snake robot. Trans IEE J Ind Appl Soc 120-D: 372–381.Google Scholar
- McIsaac K, Ostrowski J: Motion planning for anguilliform locomotion.
*IEEE Trans Robot Automation*2003, 19: 637–652. 10.1109/TRA.2003.814495View ArticleGoogle Scholar - Hicks G, Ito K: A method for determination of optimal gaits with application to a snake-like serial-link structure.
*IEEE Trans Automatic Control*2005, 50: 1291–1306. 10.1109/TAC.2005.854583View ArticleMathSciNetGoogle Scholar - Ma S, Ohmameuda Y, Inoue K (2004) Dynamic analysis of 3-dimensional snake robots. In: Proc. IEEE/RSJ international conference on intelligent robots and systems, 767–772. 28 Sept.-2 Oct. 2004.Google Scholar
- Ma S: Analysis of creeping locomotion of a snake-like robot.
*Adv Robot*2001, 15(2):205–224. 10.1163/15685530152116236View ArticleGoogle Scholar - Liljebäck P, Pettersen KY, Stavdahl Ø, Gravdahl JT (2013) Snake robots - modelling, mechatronics, and control. Advances in industrial control. Springer.View ArticleGoogle Scholar
- Liljebäck P, Haugstuen IU, Pettersen KY: Path following control of planar snake robots using a cascaded approach.
*IEEE Trans Control Syst Technol*2012, 20: 111–126.Google Scholar - Rezapour E, Pettersen KY, Liljebäck P, Gravdahl JT (2013) Path following control of planar snake robots using virtual holonomic constraints. Paper presented at the IEEE international conference on robotics and biomimetics, Shenzhen, China.View ArticleGoogle Scholar
- Liljebäck P, Pettersen KY, Stavdahl Ø, Gravdahl JT: Controllability and stability analysis of planar snake robot locomotion.
*IEEE Trans Automatic Control*2013, 56(6):1365–1380. 10.1109/TAC.2010.2088830View ArticleGoogle Scholar - Westervelt ER, Grizzle JW, Chevallereau C, Choi JH, Morris B:
*Feedback control of dynamic bipedal robot locomotion*. CRC press, Boca Raton; 2007.View ArticleGoogle Scholar - Maggiore M, Consolini L: Virtual holonomic constraints for Euler-Lagrange systems.
*IEEE Trans on Automatic Control*2013, 58(4):1001–1008. 10.1109/TAC.2012.2215538View ArticleMathSciNetGoogle Scholar - Consolini L, Maggiore M (2010) Control of a bicycle using virtual holonomic constraints. In: Proc. 49th IEEE conference on decision and control, Atlanta, Georgia, USA, December 15–17, 2010.Google Scholar
- Shiriaev A, Perram JW, Canudas-de-Wit C: Constructive tool for orbital stabilization of underactuated nonlinear systems: virtual constraints approach.
*IEEE Trans Automatic Control*2005, 50(8):1164–1176. 10.1109/TAC.2005.852568View ArticleMathSciNetGoogle Scholar - Freidovich L, Robertsson A, Shiriaev A, Johansson R: Periodic motions of the Pendubot via virtual holonomic constraints: theory and experiments.
*Automatica*2008, 44(3):785–791. 10.1016/j.automatica.2007.07.011View ArticleMathSciNetGoogle Scholar - Spong MW, Hutchinson S, Vidyasagar M:
*Robot modeling and control*. John Wiley and Sons, New York; 2006.Google Scholar - Bullo F, Lewis A (2005) Geometric control of mechanical systems. Springer.View ArticleGoogle Scholar
- Fossen TI:
*Marine control systems: guidance, navigation and control of ships, rigs and underwater vehicles*. Trondheim, Norway, Marine Cybernetics; 2002.Google Scholar

## Copyright

This article is published under license to BioMed Central Ltd. This is an Open Access article distributed under the terms of the Creative Commons Attribution License (http://creativecommons.org/licenses/by/2.0), which permits unrestricted use, distribution, and reproduction in any medium, provided the original work is properly credited.