 Research
 Open Access
 Published:
Path following control of planar snake robots using virtual holonomic constraints: theory and experiments
Robotics and Biomimetics volume 1, Article number: 3 (2014)
Abstract
This paper considers path following control of planar snake robots using virtual holonomic constraints. In order to present a modelbased path following control design for the snake robot, we first derive the EulerLagrange 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.
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 prespecified 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 hyperredundant, 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 modelbased 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 11link snake robot. Furthermore, we extend the results presented in [18] by an experimental investigation of the performance of the control approach. A backtoback 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 EulerLagrange 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 realtime 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 modelbased control design.
In order to perform control design, we need to write the governing equations of the system in an implementable way. This is often done by choosing a local coordinate chart and writing the system equations with respect to (w.r.t.) these coordinates. According to the illustration of the snake robot in Figure 1, we choose the vector of the generalized coordinates of the Nlink snake robot as $x={\left[{q}_{1},{q}_{2},\dots ,{q}_{N1},{\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}}_{N1},{\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 (2N+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 Ntorus 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 (2N+4)dimensional open subset of the Euclidean space ${\mathbb{R}}^{2N+4}$.
The forward kinematic map of the snake robot
Based on the kinematic parameters of the snake robot given in Figure 1, it is possible to write the coordinate representation of the forward kinematic map. The map between the absolute link angles θ_{ i } and the relative joint angles q_{ i } is given by
The position of the CM of the i th link w.r.t. the global xy axes can be, respectively, given as
where 2l 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 xy axes can be found by taking the time derivative of (2)(3) which gives
Since all the links have equal length and mass, the position of the CM for the whole structure of the robot is defined as
To facilitate path following control of the CM of the snake robot, we replace the tail position (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 multibody robotic structures, such as eellike robots, have derived the equations of motion of these robots with a NewtonEuler 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 EulerLagrange 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.
Snake robots are a class of 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:
where m and J denote the uniformly distributed mass and moment of inertia of the links, respectively. Using the Lagrangian function (9), we write the EulerLagrange equations of motion of the controlled system as
where i∈{1,…,N+2}, $B\left(x\right)=\left[{e}_{j}\right]\in {\mathbb{R}}^{(N+2)\times (N1)}$ 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}_{N1}\right]}^{T}\in {\mathbb{R}}^{N1}$ 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 EulerLagrange equations (10) can also be written in the form of a secondorder 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 righthand 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 NewtonEuler 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
In this subsection, both viscous and Coulomb friction models are used for capturing the essential properties of the anisotropic ground friction forces. For modelling the friction, we first define the rotation matrix for mapping from the global frame to the local frame of link i (cf. Figure 1) as
Using (4)(5) and (12), the velocities of the links in the local link frames can be written in terms of the velocities of the links in the global frame as
where ${v}_{t}^{\text{link},i}$ and ${v}_{n}^{\text{link},i}$ denote the linear velocity of the CM of the i th link in the tangential (along link xaxis) and normal (along link yaxis) 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
Assuming equal friction coefficients for all the links, we write the model of the friction for each individual link i as
where 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 xy frame as
Finally, we can write τ_{ f } in (11) as
where
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
A common method for control of mechanical systems is fullstate feedback linearization. This approach is not applicable for snake robots due to the underactuation. However, it is still possible to linearize the dynamics of the actuated DOF of the robot, which is called collocated partial feedback linearization, and can simplify the analysis as well as the control design. A similar approach is considered in [16], but for the sake of completeness, we present the approach here. To this end, we separate the dynamic equations of the robot given by (11) into two subsets by taking $x={\left[{q}_{a},{q}_{u}\right]}^{T}\in {\mathbb{R}}^{N+2}$, with ${q}_{a}\in {\mathbb{R}}^{N1}$ and ${q}_{u}\in {\mathbb{R}}^{3}$ which were defined in the subsection describing the geometry of the problem:
where ${m}_{11}\in {\mathbb{R}}^{(N1)\times (N1)}$, ${m}_{12}\in {\mathbb{R}}^{(N1)\times 3}$, ${m}_{21}\in {\mathbb{R}}^{3\times (N1)}$, and ${m}_{22}\in {\mathbb{R}}^{3\times 3}$ denote the corresponding submatrices of the inertia matrix, and ${0}_{3\times 1}={\left[0,0,0\right]}^{T}\in {\mathbb{R}}^{3}$. Furthermore, ${h}_{1}(x,\stackrel{\u0307}{x})\in {\mathbb{R}}^{N1}$ and ${h}_{2}(x,\stackrel{\u0307}{x})\in {\mathbb{R}}^{3}$ include all the contributions of the Coriolis, centripetal, and friction forces. Moreover, $\psi \in {\mathbb{R}}^{N1}$ denotes the nonzero part of the vector of control forces, i.e. $B\left(x\right)\tau ={\left[\psi ,{0}_{3\times 1}\right]}^{T}\in {\mathbb{R}}^{N+2}$. From (21), we have
Substituting (22) into (20) yields
For linearizing the dynamics of the directly actuated DOF, we apply the global transformation of the vector of control inputs as
where $\mathit{\vartheta}={\left[{\mathit{\vartheta}}_{1},{\mathit{\vartheta}}_{2},\dots ,{\mathit{\vartheta}}_{N1}\right]}^{T}\in {\mathbb{R}}^{N1}$ is the vector of new control inputs. Consequently, the dynamic model (20)(21) can be written in the following partially feedback linearized form
with
where ${\beta}_{i}\left({q}_{a}\right):\mathcal{Q}\to \mathbb{R}$ is a smooth scalarvalued function. It can be numerically shown that the value of β_{ 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 modelbased control design, we write (25)(26) in a more detailed form:
where the summation convention is applied in (30), and henceforth, to all the equations which contain repeated upperlower indices (i.e. whenever an expression contains a repeated index, one as a subscript and the other as a superscript, summation is implied over this index [26]). The dynamical system (29)(32) is in the form of a controlaffine system with drift. In particular, the term
is called the drift vector field, which specifies the dynamics of the robot when the control input is zero. Furthermore, the columns of the matrix
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 trackfollow 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 multilink 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.
To formulate a combinational trackfollow 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
where 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.
We divide the control objectives into three main parts. In the first part, the goal is to make the shape variables of the robot track given bounded smooth timevarying references, i.e. asymptotic trajectory tracking problem, such that
for all 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
Moreover, we define a desired straight path that we want the CM of the snake robot to follow. This is defined as a smooth onedimensional manifold $\mathcal{P}\subset {\mathbb{R}}^{2}$, with coordinates in the xy plane given by the pair (p_{ xd },p_{ yd }), which are parameterized by a scalar timedependent variable Θ(t) as
We define the vector of the path following error variables for the position of the CM of the robot as $\stackrel{~}{p}={\left[{p}_{x}\left(t\right){p}_{\mathit{\text{xd}}}\left(\Theta \right),{p}_{y}\left(t\right){p}_{\mathit{\text{yd}}}\left(\Theta \right)\right]}^{T}\in {\mathbb{R}}^{2}$. Subsequently, the third part of the control objectives is defined as practical convergence (see, e.g. [4]) of the position of the CM of the robot to the desired path such that
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 lineofsight (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
Virtual holonomic constraints are specified through C^{1} coordinatedependent 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 vectorvalued function
in which every element defines one constraint function for the corresponding angular coordinate of the system.
At this point, we augment the state vector of the system with three new states that in the following will be used in the control design. The introduction of these new variables to the state vector of the system, which will be used as constraint variables, is inspired by the notion of dynamic virtual holonomic constraints [21], i.e. virtual holonomic constraints which depend on the solutions of a dynamic compensator. The idea is to make the virtual holonomic constraints to depend on the variations of a dynamic parameter, which is used for controlling the system on the constraint manifold. The purpose of these additional states is explained below.

1.
We introduce two new states ${\left[{\varphi}_{o},{\stackrel{\u0307}{\varphi}}_{o}\right]}^{T}\in {\mathbb{R}}^{2}$ where the secondorder 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 coordinatedependent equality constraints of the form Φ _{ i }(x)=0, where Φ _{ i } is a timeindependent 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.
Subsequently, we denote the augmented coordinate vector of the system by
and the corresponding augmented state space by $T\widehat{\mathcal{Q}}$.
Virtual holonomic constraints for the joint angles
A fundamental work in the area of snake robots was presented by Hirose [5]. In this work, Hirose considers empirical studies of biological snakes to derive a mathematical approximation of the most common gait pattern among biological snakes, known as lateral undulation. In particular, the shape of a snake conducting lateral undulation can be described by a planar curve (the serpenoid curve) with coordinates in the xy plane along the curve at arc length s given by
where a, b, and c are positive scalars. Locomotion of a snakelike 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 secondorder 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 lineofsight (LOS) guidance law as the reference angle for the head link. LOS guidance is a muchused method in marine control systems (see, e.g. [27]). In general, guidancebased 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 guidancebased control strategy can successfully steer the robot towards the desired path. However, we perform the modelbased 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.
To define the guidance law, without loss of generality, we assign the global coordinate system such that the global xaxis is aligned with the desired path. Consequently, the position of the CM of the robot along the yaxis, denoted by p_{ y }, defines the shortest distance between the robot and the desired path, often referred to as the crosstrack 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 crosstrack error as
where Δ>0 is a design parameter known as the lookahead 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
We collect all the foregoing defined constraint functions in the following vectorvalued function
For trajectory planning using virtual holonomic constraints, we define the constraint manifold associated with the constraint functions (47) as
where i∈{1,…,N−1}. The constraint manifold (48) is a sixdimensional 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 closedloop 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
We will achieve our control design objectives which we defined in the previous section, by designing the control inputs 𝜗 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 closedloop system. For simplicity of notation, we denote the following differentials:
where i∈{1,…,N−1}. The Lie derivative of (49) along the solutions of (29)(32) is of the form
which lacks the control inputs. The Lie derivative of (54) along the solutions of (29)(32) is of the form
which contains the control inputs. Consequently, the controlled output vector (49) yields a welldefined 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 closedloop system. Furthermore, regular constraints in parametric form (47) are always stabilizable [21].
The welldefined vector relative degree {2,2,…,2} on Γ implies that the system (29)(32) with the controlled output function (47) is inputoutput feedback linearizable. Consequently, we can stabilize Γ with an inputoutput feedback linearizing controller.
Output regulation via inputoutput 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 closedloop system. In particular, we use inputoutput linearization to stabilize the constraint manifold (48).
To stabilize Φ_{ 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 secondorder time derivative of the i th joint tracking error, i.e. the i th element of (55), is of the form
We define the control input for the i th joint in (29) as
where 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.
In the following, we aim to stabilize the constraint manifold for the head angle, i.e. to make $\left({y}_{N},{\stackrel{\u0307}{y}}_{N}\right)\to (0,0)$. The head angle error corresponds to the N th element of the controlled output vector (49), and its secondorder time derivative (i.e. the head angle error dynamics) is given by
Inserting 𝜗_{ i } from (57) into (59) gives
which is equivalent to
For simplicity of notation, we denote the constraint function for the 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
In order to stabilize the constraint function Φ_{ N }(p_{ y }) for the head angle, we define the secondorder time derivative of the augmented coordinate ϕ_{ o } in the form of a dynamic compensator as
where ${k}_{p,{\theta}_{N}}>0$ and ${k}_{d,{\theta}_{N}}>0$ are the head angle controller gains. Notice that since β_{ i } is negativevalued in any configuration, (63) is globally welldefined. 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.
Finally, we conjecture that while the output trajectories are evolving on the constraint manifold (48), the internal dynamics (31)(32), which has the form
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
In this section, we present simulation results which illustrate the performance of the proposed path following controller. We considered a snake robot with 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 lowpass filter of the form
with natural frequency ω_{ 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 realtime 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 (HS5955TG; 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 camerabased 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 lowpass filter of the form (67). The parameters of the lowpass filter were set to ω_{ n }=π/2 and ψ_{ f }=1.
In the following, we elaborate on a few adjustments that were made in the implemented path following controller in order to comply with the particular properties and capabilities of the physical snake robot employed in the experiment. We conjecture that these adjustments only marginally affected the overall motion of the robot. The successful path following behaviour of the robot demonstrated below supports this claim. Since the experimental setup only provided measurements of the joint angles and the position and orientation of the head link, we chose to implement the joint controller in (57) as
where 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 steadystate 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 lookahead 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 xaxis), 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
The results of the experiments are illustrated in Figures 8, 9, 10, 11, 12, 13. In particular, Figure 8 shows that the solution of the dynamic compensator remained bounded. Figure 9 shows that the joints of the robot tracked the sinusoidal reference angles provided by the constraint functions (45) and that the tracking error converged to a neighbourhood of the origin. As discussed above, this is probably due to the modification of the joint controller (68) due to the lack of velocity measurements in the lab. Figure 10 shows that the head angle of the robot tracked the reference head angle defined by the constraint function (46) and that the tracking error converged to a neighbourhood of the origin. Figure 11 shows the motion of the CM of the robot in the xy 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 inputoutput 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.
References
 1.
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.
 2.
Wang Z, Appleton E (2003) The concept and research of a pipe crawling rescue robot. Adv Robot 17.4: 339–358.
 3.
Fjerdingen SA, Liljebäck P, Transeth AA (2009) A snakelike robot for internal inspection of complex pipe structures (PIKo). In: Proc. IEEE/RSJ international conference on intelligent robots and systems, St. Louis, MO, USA.
 4.
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.2105130
 5.
Hirose S: Biologically inspired robots: snakelike locomotors and manipulators. Oxford University Press, Oxford, England; 1993.
 6.
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.
 7.
Date H, Hoshi Y, Sampei M (2000) Locomotion control of a snakelike robot based on dynamic manipulability. In: Proc. IEEE/RSJ international conference on intelligent robots and systems, Takamatsu, Japan.
 8.
Tanaka M, Matsuno F (2008) Control of 3dimensional snake robots by using redundancy. In: Proc. IEEE international conference on robotics and automation, 1156–1161, Pasadena, CA.
 9.
Ma S, Ohmameuda Y, Inoue K, Li B (2003) Control of a 3dimensional snakelike robot. In: Proc. IEEE international conference on robotics and automation, vol. 2, 2067–2072, Taipei, Taiwan.
 10.
Tanaka M, Matsuno F (2009) A study on sinuslifting motion of a snake robot with switching constraints. In: Proc. IEEE international conference on robotics and automation, 2270–2275. 12–17 May 2009.
 11.
Prautsch P, Mita T, Iwasaki T (2000) Analysis and control of a gait of snake robot. Trans IEE J Ind Appl Soc 120D: 372–381.
 12.
McIsaac K, Ostrowski J: Motion planning for anguilliform locomotion. IEEE Trans Robot Automation 2003, 19: 637–652. 10.1109/TRA.2003.814495
 13.
Hicks G, Ito K: A method for determination of optimal gaits with application to a snakelike seriallink structure. IEEE Trans Automatic Control 2005, 50: 1291–1306. 10.1109/TAC.2005.854583
 14.
Ma S, Ohmameuda Y, Inoue K (2004) Dynamic analysis of 3dimensional snake robots. In: Proc. IEEE/RSJ international conference on intelligent robots and systems, 767–772. 28 Sept.2 Oct. 2004.
 15.
Ma S: Analysis of creeping locomotion of a snakelike robot. Adv Robot 2001, 15(2):205–224. 10.1163/15685530152116236
 16.
Liljebäck P, Pettersen KY, Stavdahl Ø, Gravdahl JT (2013) Snake robots  modelling, mechatronics, and control. Advances in industrial control. Springer.
 17.
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.
 18.
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.
 19.
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.2088830
 20.
Westervelt ER, Grizzle JW, Chevallereau C, Choi JH, Morris B: Feedback control of dynamic bipedal robot locomotion. CRC press, Boca Raton; 2007.
 21.
Maggiore M, Consolini L: Virtual holonomic constraints for EulerLagrange systems. IEEE Trans on Automatic Control 2013, 58(4):1001–1008. 10.1109/TAC.2012.2215538
 22.
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.
 23.
Shiriaev A, Perram JW, CanudasdeWit 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.852568
 24.
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.011
 25.
Spong MW, Hutchinson S, Vidyasagar M: Robot modeling and control. John Wiley and Sons, New York; 2006.
 26.
Bullo F, Lewis A (2005) Geometric control of mechanical systems. Springer.
 27.
Fossen TI: Marine control systems: guidance, navigation and control of ships, rigs and underwater vehicles. Trondheim, Norway, Marine Cybernetics; 2002.
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.
Author information
Additional information
Competing interests
The authors declare that they have no competing interests.
Authors’ contributions
All authors contributed equally and significantly in the writing of this paper. All authors read and approved the final manuscript.
Authors’ original submitted files for images
Below are the links to the authors’ original submitted files for images.
Rights and permissions
Open Access This article is distributed under the terms of the Creative Commons Attribution 4.0 International License (https://creativecommons.org/licenses/by/4.0), which permits use, duplication, adaptation, distribution, and reproduction in any medium or format, as long as you give appropriate credit to the original author(s) and the source, provide a link to the Creative Commons license, and indicate if changes were made.
About this article
Received
Accepted
Published
DOI
Keywords
 Path following
 Snake robots
 Biologically inspired robots
 Virtual holonomic constraints