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.

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 *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

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

{\theta}_{i}=\sum _{n=i}^{N-1}{q}_{n}+{\theta}_{N}

(1)

The position of the CM of the *i* th link w.r.t. the global *x*-*y* axes can be, respectively, given as

\begin{array}{lcr}{p}_{x,i}& =& {p}_{x,0}+2l\sum _{j=1}^{i-1}cos{\theta}_{j}+lcos{\theta}_{i}\end{array}

(2)

\begin{array}{lcr}{p}_{y,i}& =& {p}_{y,0}+2l\sum _{j=1}^{i-1}sin{\theta}_{j}+lsin{\theta}_{i}\end{array}

(3)

where 2*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

\begin{array}{lcr}{\stackrel{\u0307}{p}}_{x,i}& =& {\stackrel{\u0307}{p}}_{x,0}-2l\sum _{j=1}^{i-1}sin{\theta}_{j}{\stackrel{\u0307}{\theta}}_{j}-lsin{\theta}_{i}{\stackrel{\u0307}{\theta}}_{i}\end{array}

(4)

\begin{array}{lcr}{\stackrel{\u0307}{p}}_{y,i}& =& {\stackrel{\u0307}{p}}_{y,0}+2l\sum _{j=1}^{i-1}cos{\theta}_{j}{\stackrel{\u0307}{\theta}}_{j}+lcos{\theta}_{i}{\stackrel{\u0307}{\theta}}_{i}\end{array}

(5)

Since all the links have equal length and mass, the position of the CM for the whole structure of the robot is defined as

\begin{array}{l}\left({p}_{x},{p}_{y}\right)=\left(\frac{1}{N}\sum _{i=1}^{N}{p}_{x,i},\frac{1}{N}\sum _{i=1}^{N}{p}_{y,i}\right)\end{array}

(6)

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:

{p}_{x,0}={p}_{x}-\frac{1}{N}\sum _{i=1}^{N}\left(2l\sum _{j=1}^{i-1}cos{\theta}_{j}+lcos{\theta}_{i}\right)

(7)

{p}_{y,0}={p}_{y}-\frac{1}{N}\sum _{i=1}^{N}\left(2l\sum _{j=1}^{i-1}sin{\theta}_{j}+lsin{\theta}_{i}\right)

(8)

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.

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:

\phantom{\rule{-12.0pt}{0ex}}\mathcal{\mathcal{L}}\left({q}_{a},\stackrel{\u0307}{x}\right)=\mathcal{K}\left({q}_{a},\stackrel{\u0307}{x}\right)=\frac{1}{2}m\sum _{i=1}^{N}\left({\stackrel{\u0307}{p}}_{x,i}^{2}+{\stackrel{\u0307}{p}}_{y,i}^{2}\right)+\frac{1}{2}J\sum _{i=1}^{N}{\stackrel{\u0307}{\theta}}_{i}^{2}

(9)

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 Euler-Lagrange equations of motion of the controlled system as

\frac{d}{\mathit{\text{dt}}}\left[\frac{\partial \mathcal{\mathcal{L}}\left({q}_{a},\stackrel{\u0307}{x}\right)}{\partial {\stackrel{\u0307}{x}}_{i}}\right]-\frac{\partial \mathcal{\mathcal{L}}\left({q}_{a},\stackrel{\u0307}{x}\right)}{\partial {x}_{i}}={\left(B\left(x\right)\tau -{\tau}_{f}\right)}_{i}

(10)

where *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

M\left({q}_{a}\right)\stackrel{\u0308}{x}+C\left(x,\stackrel{\u0307}{x}\right)\stackrel{\u0307}{x}=B\left(x\right)\tau -{\tau}_{f}

(11)

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

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

\begin{array}{l}{R}_{i}=\left[\begin{array}{cc}cos{\theta}_{i}& -sin{\theta}_{i}\\ sin{\theta}_{i}& cos{\theta}_{i}\end{array}\right]\end{array}

(12)

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

\begin{array}{l}{v}^{\text{link},i}={\left[\begin{array}{cc}{v}_{t}^{\text{link},i}& {v}_{n}^{\text{link},i}\end{array}\right]}^{T}={R}_{i}^{T}{\left[\begin{array}{cc}{\stackrel{\u0307}{p}}_{x,i}& {\stackrel{\u0307}{p}}_{y,i}\end{array}\right]}^{T}\end{array}

(13)

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 *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

{f}^{\text{link},i}={f}_{{c}_{i}}+{f}_{{v}_{i}}

(14)

Assuming equal friction coefficients for all the links, we write the model of the friction for each individual link *i* as

{f}_{{c}_{i}}=\mathit{\text{mg}}{\left[{\mu}_{t}\text{sgn}\left({v}_{t}^{\text{link},i}\right)\phantom{\rule{1em}{0ex}}{\mu}_{n}\text{sgn}\left({v}_{n}^{\text{link},i}\right)\right]}^{T}\phantom{\rule{0.3em}{0ex}}\phantom{\rule{0.3em}{0ex}}\in {\mathbb{R}}^{2}

(15)

{f}_{{v}_{i}}={\left[{c}_{t}{v}_{t}^{\text{link},i}{c}_{n}{v}_{n}^{\text{link},i}\right]}^{T}\in {\mathbb{R}}^{2}

(16)

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 *x*-*y* frame as

{f}_{\text{global}}^{\text{link},i}={R}_{i}{f}^{\text{link},i}

(17)

Finally, we can write *τ*_{
f
} in (11) as

{\tau}_{f}=\sum _{i=1}^{N}{\mathcal{J}}_{i}^{T}\left(x\right)\phantom{\rule{0.3em}{0ex}}{f}_{\text{global}}^{\text{link},i}

(18)

where

\phantom{\rule{-15.0pt}{0ex}}{\mathcal{J}}_{i}^{T}\left(x\right)=\left[\phantom{\rule{0.3em}{0ex}}\frac{\partial {\stackrel{\u0307}{p}}_{x,i}}{\partial {\stackrel{\u0307}{x}}_{j}},\frac{\partial {\stackrel{\u0307}{p}}_{y,i}}{\partial {\stackrel{\u0307}{x}}_{j}}\right]\in {\mathbb{R}}^{(N+2)\times 2},\phantom{\rule{2em}{0ex}}j\in \{1,\dots ,N+2\}

(19)

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 full-state 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}}^{N-1} and {q}_{u}\in {\mathbb{R}}^{3} which were defined in the subsection describing the geometry of the problem:

\begin{array}{l}\phantom{\rule{-19.0pt}{0ex}}{m}_{11}\left({q}_{a}\right){\stackrel{\u0308}{q}}_{a}+{m}_{12}\left({q}_{a}\right){\stackrel{\u0308}{q}}_{u}+{h}_{1}(x,\stackrel{\u0307}{x})=\psi \in {\mathbb{R}}^{N-1}\end{array}

(20)

\begin{array}{l}\phantom{\rule{-19.0pt}{0ex}}{m}_{21}\left({q}_{a}\right){\stackrel{\u0308}{q}}_{a}+{m}_{22}\left({q}_{a}\right){\stackrel{\u0308}{q}}_{u}+{h}_{2}(x,\stackrel{\u0307}{x})={0}_{3\times 1}\in {\mathbb{R}}^{3}\end{array}

(21)

where {m}_{11}\in {\mathbb{R}}^{(N-1)\times (N-1)}, {m}_{12}\in {\mathbb{R}}^{(N-1)\times 3}, {m}_{21}\in {\mathbb{R}}^{3\times (N-1)}, 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}}^{N-1} 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}}^{N-1} denotes the non-zero 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

\begin{array}{l}{\stackrel{\u0308}{q}}_{u}=-{m}_{22}^{-1}\left({h}_{2}+{m}_{21}{\stackrel{\u0308}{q}}_{a}\right)\in {\mathbb{R}}^{3}\end{array}

(22)

Substituting (22) into (20) yields

\left({m}_{11}-{m}_{12}{m}_{22}^{-1}{m}_{21}\right){\stackrel{\u0308}{q}}_{a}-\left({m}_{12}{m}_{22}^{-1}\right){h}_{2}+{h}_{1}=\psi

(23)

For linearizing the dynamics of the directly actuated DOF, we apply the global transformation of the vector of control inputs as

\psi =\left({m}_{11}-{m}_{12}{m}_{22}^{-1}{m}_{21}\right)\mathit{\vartheta}-\left({m}_{12}{m}_{22}^{-1}\right){h}_{2}+{h}_{1}

(24)

where \mathit{\vartheta}={\left[{\mathit{\vartheta}}_{1},{\mathit{\vartheta}}_{2},\dots ,{\mathit{\vartheta}}_{N-1}\right]}^{T}\in {\mathbb{R}}^{N-1} is the vector of new control inputs. Consequently, the dynamic model (20)-(21) can be written in the following partially feedback linearized form

\begin{array}{lcr}{\stackrel{\u0308}{q}}_{a}& =& \mathit{\vartheta}\in {\mathbb{R}}^{N-1}\end{array}

(25)

\begin{array}{lcr}{\stackrel{\u0308}{q}}_{u}& =& \mathcal{D}(x,\stackrel{\u0307}{x})+\mathcal{C}\left({q}_{a}\right)\mathit{\vartheta}\in {\mathbb{R}}^{3}\end{array}

(26)

with

\begin{array}{lcr}\phantom{\rule{-20.0pt}{0ex}}\mathcal{D}(x,\stackrel{\u0307}{x})& =& -{m}_{22}^{-1}\left({q}_{a}\right){h}_{2}(x,\stackrel{\u0307}{x})={\left[\phantom{\rule{0.3em}{0ex}}{f}_{{\theta}_{N}},{f}_{x},{f}_{y}\right]}^{T}\in {\mathbb{R}}^{3}\end{array}

(27)

\begin{array}{lcr}\phantom{\rule{-20.0pt}{0ex}}\mathcal{C}\left({q}_{a}\right)& =& -{m}_{22}^{-1}\left({q}_{a}\right){m}_{21}\left({q}_{a}\right)\\ =& {\left[{\beta}_{i}\left({q}_{a}\right),0,0\right]}^{T}\in {\mathbb{R}}^{3\times (N-1)}\end{array}

(28)

where {\beta}_{i}\left({q}_{a}\right):\mathcal{Q}\to \mathbb{R} is a smooth scalar-valued 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 model-based control design, we write (25)-(26) in a more detailed form:

\begin{array}{lcr}{\stackrel{\u0308}{q}}_{a}& =& \mathit{\vartheta}\in {\mathbb{R}}^{N-1}\end{array}

(29)

\begin{array}{lcr}{\stackrel{\u0308}{\theta}}_{N}& =& {f}_{{\theta}_{N}}(x,\stackrel{\u0307}{x})+{\beta}_{i}\left({q}_{a}\right){\mathit{\vartheta}}^{i}\in \mathbb{R}\end{array}

(30)

\begin{array}{lcr}{\stackrel{\u0308}{p}}_{x}& =& {f}_{x}(x,\stackrel{\u0307}{x})\in \mathbb{R}\end{array}

(31)

\begin{array}{lcr}{\stackrel{\u0308}{p}}_{y}& =& {f}_{y}(x,\stackrel{\u0307}{x})\in \mathbb{R}\end{array}

(32)

where the summation convention is applied in (30), and henceforth, to all the equations which contain repeated upper-lower 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 control-affine system with drift. In particular, the term

\mathcal{A}(x,\stackrel{\u0307}{x})={\left[{\stackrel{\u0307}{q}}_{a},{\stackrel{\u0307}{q}}_{u},{0}_{(N-1)\times 1},\mathcal{D}(x,\stackrel{\u0307}{x})\right]}^{T}\in {\mathbb{R}}^{2N+4}

(33)

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

\begin{array}{l}\phantom{\rule{-16.0pt}{0ex}}\mathcal{\mathcal{B}}\left({q}_{a}\right)\phantom{\rule{0.3em}{0ex}}=\phantom{\rule{0.3em}{0ex}}\left[\phantom{\rule{0.3em}{0ex}}\phantom{\rule{0.3em}{0ex}}\phantom{\rule{0.3em}{0ex}}\begin{array}{c}{0}_{(N+2)\times (N-1)}\\ {I}_{N-1}\\ \left[{\beta}_{1}\left({q}_{a}\right),\dots ,{\beta}_{N-1}\left({q}_{a}\right)\right]\\ {0}_{2\times (N-1)}\end{array}\phantom{\rule{0.3em}{0ex}}\phantom{\rule{0.3em}{0ex}}\phantom{\rule{0.3em}{0ex}}\right]\phantom{\rule{0.3em}{0ex}}\phantom{\rule{0.3em}{0ex}}\in {\mathbb{R}}^{(2N+4)\times (N-1)}\end{array}

(34)

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.