/smash/get/diva2:123995/FULLTEXT01.pdf

/smash/get/diva2:123995/FULLTEXT01.pdf
Aksel Andreas Transeth
Modelling and Control
of Snake Robots
Thesis for the degree of philosophiae doctor
Trondheim, September 2007
Norwegian University of
Science and Technology
Faculty of Information Technology,
Mathematics and Electrical Engineering
Department of Engineering Cybernetics
NTNU
Norwegian University of Science and Technology
Thesis for the degree of philosophiae doctor
Faculty of Information Technology, Mathematics and Electrical Engineering
Department of Engineering Cybernetics
©Aksel Andreas Transeth
ISBN 978-82-471-4865-5 (printed ver.)
ISBN 978-82-471-4879-2 (electronic ver.)
ISSN 1503-8181
Theses at NTNU, 2008:2
Printed by Tapir Uttrykk
ITK Report 2007-3-W
Summary
Snake robots have the potential of contributing vastly in areas such as rescue
missions, …re-…ghting and maintenance where it may either be too narrow
or too dangerous for personnel to operate. This thesis reports novel results
within modelling and control of snake robots as steps toward developing
snake robots capable of such operations.
A survey of the various mathematical models and motion patterns for
snake robots found in the published literature is presented. Both purely
kinematic models and models including dynamics are investigated. Moreover, di¤erent approaches to both biologically inspired locomotion and arti…cially generated motion patterns for snake robots are discussed.
Snakes utilize irregularities in the terrain, such as rocks and vegetation,
for faster and more e¢ cient locomotion. This motivates the development of
snake robots that actively use the terrain for locomotion, i.e. obstacle aided
locomotion. In order to accurately model and understand this phenomenon,
this thesis presents a novel non-smooth (hybrid) mathematical model for
2D snake robots, which allows the snake robot to push against external
obstacles apart from a ‡at ground. Subsequently, the 2D model is extended
to a non-smooth 3D model. The 2D model o¤ers an e¢ cient platform
for testing and development of planar snake robot motion patterns with
obstacles, while the 3D model can be used to develop motion patterns
where it is necessary to lift parts of the snake robot during locomotion. The
framework of non-smooth dynamics and convex analysis is employed to be
able to systematically and accurately incorporate both unilateral contact
forces (from the obstacles and the ground) and spatial friction forces based
on Coulomb’s law using set-valued force laws. Snake robots can easily be
constructed for forward motion on a ‡at surface by adding passive caster
wheels on the underside of the snake robot body. However, the advantage
of adding wheels su¤ers in rougher terrains. Therefore, the models in this
thesis are developed for wheel-less snake robots to aid future development
of motion patterns that do not rely on passive wheels.
ii
Summary
For numerical integration of the developed models, conventional numerical solvers can not be employed directly due to the set-valued force
laws and possible instantaneous velocity changes. Therefore, we show how
to implement the models for simulation with a numerical integrator called
the time-stepping method. This method helps to avoid explicit changes
between equations during simulation even though the system is hybrid.
Both the 2D and the 3D mathematical models are veri…ed through
experiments. In particular, a back-to-back comparison between numerical
simulations and experimental results is presented. The results compare
very well for obstacle aided locomotion.
The problem of model-based control of the joints of a planar snake robot
without wheels is also investigated. Delicate operations such as inspection
and maintenance in industrial environments or performing search and rescue operations require precise control of snake robot joints. To this end,
we present a controller that asymptotically stabilizes the joints of a snake
robot to a desired reference trajectory. The 2D and 3D model referred to
above are ideal for simulation of various snake robot motion pattern. However, it is also advantageous to model the snake robot based the standard
equations of motion for the dynamics of robot manipulators. This latter
modelling approach is not as suited for simulation of a snake robot due to
its substantial number of degrees of freedom, but a large number of control techniques are developed within this framework and these can now be
employed for a snake robot. We …rst develop a process plant model from
the standard dynamics of a robot manipulator. Then we derive a control
plant model from the process plant model and develop a controller based
on input-output linearization of the control plant model. The control plant
model renders the controller independent of the global orientation of the
snake robot as this is advantageous for the stability analysis. Asymptotic
stability of the desired trajectory of the closed-loop system is shown using a
formal Lyapunov-based proof. Performance of the controller is, …rst, tested
through simulations with a smooth dynamical model and, second, with a
non-smooth snake robot model with set-valued Coulomb friction.
The three main models developed in this thesis all serve important
purposes. First, the 2D model is for investigating planar motion patterns
by e¤ective simulations. Second, the 3D model is for developing motion
patterns that require two degrees of freedom rotational joints on the snake
robot. Finally, the control plant model is employed to investigate stability
and to construct a model-based controller for a planar snake robot so that
its joints are accurately controlled to a desired trajectory.
Preface
This thesis contains the results of my doctoral studies from August 2004
to September 2007 at the Department of Engineering Cybernetics (ITK)
at the Norwegian University of Science and Technology (NTNU) under
the guidance of Professor Kristin Ytterstad Pettersen. The research is
funded by the Strategic University Program on Computational Methods in
Nonlinear Motion Control sponsored by The Research Council of Norway.
I am grateful to my supervisor Professor Kristin Ytterstad Pettersen for
the support and encouragement during my doctoral studies. She has been
a mentor in how to do research and our meetings have always been joyful
ones. I am thankful for her constructive feedback on my research results
and publications which have taught me how to convey scienti…c results in a
to-the-point manner. In addition, I am much obliged to her for introducing
me to various very skilled people around the world, which allowed me to
be a visiting researcher in Zürich and Santa Barbara.
I am thankful for the invitation of Dr. ir. habil. Remco I. Leine and
Professor Christoph Glocker to visit them at the Center of Mechanics at
the Eidgenössische Technische Hochschule (ETH) Zürich in Switzerland.
The introduction to non-smooth dynamics given to me by Dr. ir. habil.
Remco I. Leine together with the guidance I got during my stay there are
invaluable. In addition, I would like to thank the rest of the people at ETH
Center for Mechanics for making my stay there a pleasant one.
I thank Professor João Pedro Hespanha for having me as a visitor at the
Center for Control, Dynamical systems, and Computation (CCDC) at the
University of California Santa Barbara (UCSB) in the USA. I appreciate
the valuable advice and ideas I got from him. In addition, I would like
to thank Professor Nathan van de Wouw at the Eindhoven University of
Technology (TU/e) for a fruitful collaboration and fun time together at
UCSB together with the interesting time I had during my short visit at
TU/e.
I appreciate the discussions with my fellow PhD-students at NTNU,
iv
Preface
and I would particularly like to point out the conversations with my former
o¢ ce mate Svein Hovland and current o¢ ce mate Luca Pivano. In addition,
I greatly acknowledge the constructive debates and advice concerning all
aspects of snake robots I have got from my friend Pål Liljebäck. Moreover,
I thank Dr. Øyvind Stavdahl for sharing his ideas on and enthusiasm for
snake robots.
I express my deepest gratitude to Dr. Alexey Pavlov for guiding me in
the world of non-linear control and for his numerous constructive comments
and valuable feedback on my thesis.
I thank all my colleagues at the department of Engineering Cybernetics
for providing me with a good environment in which it was nice to do research. I thank Terje Haugen and Hans Jørgen Berntsen at the department
workshop for building the snake robot employed in the experiments, and for
sharing hands-on knowledge in the design phase. Also, I thank the students
Kristo¤er Nyborg Gregertsen and Sverre Brovoll who were both involved
in the hardware and software design and implementation needed to get the
snake robot working. Finally, I would like to thank Stefano Bertelli for always helping out with camcorders and movie production for presentations
and Unni Johansen, Eva Amdahl and Tove K. B. Johnsen for taking care of
all the administrative issues that arose during the quest for a PhD-degree.
Finally, I thank my parents for always believing in me, and I thank my
girlfriend Bjørg Riibe Ramskjell for all her love and support.
Trondheim, September 2007
Aksel Andreas Transeth
Publications
The following is a list of publications produced during the work on this
thesis.
Journal papers
Transeth, A. A. and K. Y. Pettersen (2008). A survey on snake robot
modeling and locomotion. Robotica. Submitted.
Transeth, A. A., R. I. Leine, Ch. Glocker and K. Y. Pettersen (2008a).
3D snake robot motion: Modeling, simulations, and experiments.
IEEE Transactions on Robotics. Accepted.
Transeth, A. A., R. I. Leine, Ch. Glocker, K. Y. Pettersen and
P. Liljebäck (2008b). Snake robot obstacle aided locomotion: Modeling, simulations, and experiments. IEEE Transactions on Robotics.
Accepted.
Referred conference proceedings
Transeth, A. A., R. I. Leine, Ch. Glocker and K. Y. Pettersen (2006a).
Non-smooth 3D modeling of a snake robot with external obstacles.
In: Proc. IEEE Int. Conf. Robotics and Biomimetics. Kunming,
China. pp. 1189–1196.
Transeth, A. A., R. I. Leine, Ch. Glocker and K. Y. Pettersen (2006b).
Non-smooth 3D modeling of a snake robot with frictional unilateral
constraints. In: Proc. IEEE Int. Conf. Robotics and Biomimetics.
Kunming, China. pp. 1181–1188
Transeth, A. A. and K. Y. Pettersen (2006). Developments in snake
robot modeling and locomotion. In: Proc. IEEE. Int. Conf. Control,
Automation, Robotics and Vision. Singapore. pp. 1393–1400.
vi
Publications
Transeth, A. A., N. van de Wouw, A. Pavlov, J. P. Hespanha and
K. Y. Pettersen (2007a), Tracking control for snake robot joints. In:
Proc. IEEE/RSJ Int. Conf. Intelligent Robots and Systems. San
Diego, CA, USA. pp. 3539–3546.
Transeth, A. A., P. Liljebäck and K. Y. Pettersen (2007b). Snake robot obstacle aided locomotion: An experimental validation of a nonsmooth modeling approach. In: Proc. IEEE/RSJ Int. Conf. Intelligent Robots and Systems. San Diego, CA, USA. pp. 2582–2589.
Contents
Summary
i
Preface
iii
Publications
v
1 Introduction
1.1 Motivation and Background . . . . . . . . . . . . . . . . . .
1.2 Main Contributions of this Thesis . . . . . . . . . . . . . . .
1.3 Organization of this Thesis . . . . . . . . . . . . . . . . . .
1
1
7
10
2 Developments in Snake Robot Modelling and Locomotion
2.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . .
2.2 Biological Snakes and Inchworms . . . . . . . . . . . . . . .
2.2.1 Snake Skeleton . . . . . . . . . . . . . . . . . . . . .
2.2.2 Snake Skin . . . . . . . . . . . . . . . . . . . . . . .
2.2.3 Locomotion – The Source of Inspiration for Snake
Robots . . . . . . . . . . . . . . . . . . . . . . . . .
2.3 Design and Mathematical Modelling . . . . . . . . . . . . .
2.3.1 Kinematics . . . . . . . . . . . . . . . . . . . . . . .
2.3.2 Dynamics . . . . . . . . . . . . . . . . . . . . . . . .
2.4 Snake Robot Locomotion . . . . . . . . . . . . . . . . . . .
2.4.1 Planar Snake Robot Locomotion . . . . . . . . . . .
2.4.2 3D Snake Robot Locomotion . . . . . . . . . . . . .
2.5 Discussion and Summary . . . . . . . . . . . . . . . . . . .
13
13
14
14
14
3 Non-smooth Model of a 2D Snake Robot for
3.1 Introduction . . . . . . . . . . . . . . . . . . .
3.2 Summary of the Mathematical Model . . . .
3.3 Kinematics . . . . . . . . . . . . . . . . . . .
41
41
42
44
Simulation
. . . . . . . .
. . . . . . . .
. . . . . . . .
15
17
18
21
28
30
34
38
viii
3.4
3.5
3.6
3.7
Contents
3.3.1 Snake Robot Description and Reference Frames . .
3.3.2 Gap Functions for Obstacle Contact . . . . . . . .
3.3.3 Bilateral Constraints: Joints . . . . . . . . . . . .
Contact Constraints on Velocity Level . . . . . . . . . . .
3.4.1 Relative Velocity Between an Obstacle and a Link
3.4.2 Tangential Relative Velocity . . . . . . . . . . . . .
3.4.3 Bilateral Constraints: Joints . . . . . . . . . . . .
Non-smooth Dynamics . . . . . . . . . . . . . . . . . . . .
3.5.1 The Equality of Measures . . . . . . . . . . . . . .
3.5.2 Constitutive Laws for Contact Forces . . . . . . .
Numerical Algorithm: Time-Stepping . . . . . . . . . . .
3.6.1 Time Discretization . . . . . . . . . . . . . . . . .
3.6.2 Solving for the Contact Impulsions . . . . . . . . .
3.6.3 Constraint Violation . . . . . . . . . . . . . . . . .
Summary . . . . . . . . . . . . . . . . . . . . . . . . . . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
4 3D Snake Robot Modelling for Simulation
4.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . .
4.2 Kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . .
4.2.1 Model Description, Coordinates and Reference Frames
4.2.2 Gap Functions for Ground Contact . . . . . . . . . .
4.2.3 Gap Functions for Contact with Obstacles . . . . . .
4.2.4 Gap Functions for Bilateral Constraints . . . . . . .
4.3 Contact Constraints on Velocity Level . . . . . . . . . . . .
4.3.1 Unilateral Contact: Ground Contact . . . . . . . . .
4.3.2 Unilateral Contact: Obstacle Contact . . . . . . . .
4.3.3 Bilateral Constraints: Joints . . . . . . . . . . . . .
4.4 Non-smooth Dynamics . . . . . . . . . . . . . . . . . . . . .
4.4.1 The Equality of Measures . . . . . . . . . . . . . . .
4.4.2 Constitutive Laws for Contact Forces . . . . . . . .
4.4.3 Joint Actuators . . . . . . . . . . . . . . . . . . . . .
4.5 Accessing and Control of Joint Angles . . . . . . . . . . . .
4.6 Numerical Algorithm: Time-stepping . . . . . . . . . . . . .
4.6.1 Time Discretization . . . . . . . . . . . . . . . . . .
4.6.2 Solving for Contact Impulsions . . . . . . . . . . . .
4.6.3 Constraint Violation . . . . . . . . . . . . . . . . . .
4.7 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . .
44
46
48
49
49
51
52
53
53
56
61
62
63
65
65
67
67
68
68
70
71
73
75
75
80
82
83
83
85
88
89
91
91
93
95
97
Contents
ix
5 Obstacle Aided Locomotion
5.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . .
5.2 Motivation . . . . . . . . . . . . . . . . . . . . . . . . . .
5.3 Understanding Obstacle Aided Locomotion . . . . . . . .
5.4 Requirements for Intelligent Obstacle Aided Locomotion .
5.5 Experimental Observation of Obstacle Aided Locomotion
6 Modelling and Control of a Planar Snake Robot
6.1 Introduction . . . . . . . . . . . . . . . . . . . . . .
6.2 Mathematical Modelling . . . . . . . . . . . . . . .
6.2.1 Process Plant Model . . . . . . . . . . . . .
6.2.2 Control Plant Model . . . . . . . . . . . . .
6.3 Joints Control by Input-output Linearization . . .
6.3.1 Control Plant Model Reformulation . . . .
6.3.2 Controller Design . . . . . . . . . . . . . . .
6.3.3 Final Results on Input-output Linearization
6.4 Summary . . . . . . . . . . . . . . . . . . . . . . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
99
99
99
100
102
105
.
.
.
.
.
.
.
.
.
107
107
109
110
116
119
119
120
121
130
7 Simulation and Experimental Results
7.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . .
7.2 Simulation Parameters . . . . . . . . . . . . . . . . . . . . .
7.2.1 A Description of Aiko and Model Parameters . . . .
7.2.2 Simulation Parameters and Reference Joint Angles .
7.3 Simulations without Experimental Validation . . . . . . . .
7.3.1 3D Model: Drop and Lateral Undulation . . . . . . .
7.3.2 3D Model: U-shaped Lateral Rolling . . . . . . . . .
7.3.3 3D Model: Sidewinding . . . . . . . . . . . . . . . .
7.3.4 2D and 3D Model: Obstacle Aided Locomotion . . .
7.3.5 Robot Model: Comparison of Controllers . . . . . .
7.4 Experimental Setup . . . . . . . . . . . . . . . . . . . . . .
7.5 Simulations with Experimental Validation . . . . . . . . . .
7.5.1 3D Model: Sidewinding . . . . . . . . . . . . . . . .
7.5.2 2D and 3D Model: Lateral Undulation with Isotropic
Friction . . . . . . . . . . . . . . . . . . . . . . . . .
7.5.3 2D and 3D Model: Obstacle Aided Locomotion . . .
7.5.4 Discussion of the Experimental Validation . . . . . .
7.6 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . .
131
131
132
132
133
136
137
139
140
141
147
152
153
153
8 Conclusions and Further Work
163
155
156
159
161
x
Bibliography
Contents
167
A An additional Proof and a Theorem for Chapter 6
177
A.1 Theorem on Boundedness . . . . . . . . . . . . . . . . . . . 177
A.2 Positive De…niteness of MV . . . . . . . . . . . . . . . . . . 178
Chapter 1
Introduction
1.1
Motivation and Background
The wheel is an amazing invention, but it does not roll everywhere. Wheeled
mechanisms constitute the backbone of most ground-based means of transportation. On relatively smooth surfaces such mechanisms can achieve high
speeds and have good steering ability. Unfortunately, rougher terrain makes
it harder, if not impossible, for wheeled mechanisms to move. In nature
the snake is one of the creatures that exhibit excellent mobility in various
terrains. It is able to move through narrow passages and climb on rough
ground. This mobility property is attempted to be recreated in robots that
look and move like snakes – snake robots. These robots most often have
a high number of degrees of freedom (DOF) and they are able to move
without using active wheels or legs.
Snake robots may one day play a crucial role in search and rescue operations, …re-…ghting and inspection and maintenance. The highly articulated body allows the snake robot to traverse di¢ cult terrains such as
collapsed buildings or the chaotic environment caused by a car collision in
a tunnel as visualized in Figure 1.1. The snake robot could move within
destroyed buildings looking for people while simultaneously bringing communication equipment together with small amounts of food and water to
anyone trapped by the shattered building. A rescue operation involving a
snake robot is envisioned in Miller (2002). Moreover, the snake robot can
be used for inspection and maintenance of complex and possibly hazardous
areas of industrial plants such as nuclear facilities. In a city it could inspect
the sewer system looking for leaks or aid …re-…ghters. Also, snake robots
with one end …xed to a base may be used as robot manipulators that can
2
Introduction
Figure 1.1: Fire-…ghting snake robots to the rescue after a car accident in
a tunnel. Courtesy of SINTEF (www.sintef.no/Snakefighter).
reach hard-to-get-to places.
Compared to wheeled and legged mobile mechanisms, the snake robot
o¤ers high stability and superior traversability. Moreover, the exterior of a
snake robot can be completely sealed to keep dust and ‡uids out and this
enhances its applicability. Furthermore, the snake robot is more robust to
mechanical failures due to high redundancy and modularity. The downside
is its limited payload capacity, poor power e¢ ciency and a very large number of degrees of freedom that have to be controlled. For a more elaborate
overview of numerous applications of snake robots, the reader is referred to
Dowling (1997) and Choset et al. (2000).
The …rst qualitative research on snake locomotion was presented by
Gray (1946) while the …rst working biologically inspired snake-like robot
was constructed by Hirose almost three decades later in 1972 (Hirose, 1993).
He presented a two-meter long serpentine robot with twenty revolute 1 DOF
joints called the Active Cord Mechanism model ACM III, which is shown
in Figure 1.2. Passive casters were put on the underside of the robot so
that forward planar motion was obtained by moving the joints from side to
side in selected patterns.
Since Hirose presented his Active Cord Mechanism model ACM III,
many multi-link articulated robots intended for undulating locomotion have
been developed and they have been called by many names. Some examples are: multi-link mobile robot (Wiriyacharoensunthorn and Laowattana, 2002), snake-like or snake robot (Kamegawa et al., 2004; Lewis and
1.1 Motivation and Background
3
Figure 1.2: The Active Cord Mechanism model ACM III Hirose (1993). By
permission of Oxford University Press.
Zehnpfennig, 1994; Lu et al., 2003; Ma, 1999; Ma et al., 2001a; Worst
and Linnemann, 1996; Xinyu and Matsuno, 2003), hyper-redundant robot
(Chirikjian and Burdick, 1994b) and G-snake (Krishnaprasad and Tsakiris,
1994). We employ the term ‘snake robot’to emphasize that this thesis deals
with robots whose motion mainly resembles the locomotion of snakes.
Research on snake robots has increased vastly during the past ten to
…fteen years and the published literature is mostly focused on snake robot
modelling and locomotion. In the reminder of this section we will present
a summary of the previously published results most relevant to this thesis.
A more thorough review on snake robot modelling and locomotion is given
in Chapter 2.
The fastest and most common serpentine motion pattern used by biological snakes is called lateral undulation. In short, forward motion is
obtained by this motion pattern by propagating waves from the front to
the rear of the snake while exploiting roughness in the terrain. This has also
been the most implemented motion pattern for snake robots. Note that by
a ‘motion pattern’or a ‘gait’of a snake robot, we mean an (often repetitive) coordinated motion of the snake robot joints employed to move the
snake robot in some direction. Snakes exploit irregularities in the terrain to
push against to move forward by lateral undulation. This method of locomotion is attempted to be recreated for snake robots moving on a smooth
surface by adding passive caster wheels (Ma, 2001; Ma et al., 2003a; Ostrowski and Burdick, 1996; Wiriyacharoensunthorn and Laowattana, 2002;
Ye et al., 2004a) or metal skates (Saito et al., 2002) on the underside of the
4
Introduction
snake robot body. Relatively fast locomotion is obtained for snake robots
with caster wheels travelling on a solid smooth surface. The dependency
on the surface is important since the friction property of the snake robot
links must be such that the links slide easier forward and backward than
sideways for e¢ cient snake robot locomotion by lateral undulation.
The dependency on the ground surface can be relaxed by mimicking
biological snakes and utilizing external objects to move forward. This is
the motivation for developing snake robots that exploit obstacles for locomotion. We de…ne obstacle aided locomotion as the snake robot locomotion
where the snake robot utilizes walls or other external objects, apart from
the ‡at ground, for propulsion. Obstacle aided locomotion for snake robots
was …rst investigated by Hirose in 1976 and experiments with a snake robot
with passive caster wheels moving through a winding track were presented
in Hirose and Umetani (1976) and Hirose (1993). More recently, Bayraktaroglu and Blazevic (2005) and Bayraktaroglu et al. (2006) elaborated
on obstacle aided locomotion for wheel-less snake robots. The dynamics
of such locomotion was simulated with the dynamic simulation software
WorkingModelr in Bayraktaroglu and Blazevic (2005), where the rigid
body contact (i.e. the contact between an obstacle and the snake robot) is
represented by a spring-damper model (i.e. a compliant contact model).
Snake robots capable of 3D motion appeared more recently
(Chirikjian and Burdick, 1993, 1995; Hirose and Morishima, 1990; Liljebäck et al., 2005; Mori and Hirose, 2002) and, together with the robots,
mathematical models of both the kinematics and the dynamics of snake
robots were also developed. Purely kinematic 3D models were presented in
Burdick et al. (1993), Chirikjian and Burdick (1995) and Ma et al. (2003b).
In such models, friction is not considered in the contact between the snake
robot and the ground surface. Instead, it is assumed in Ma et al. (2003b)
that a snake robot link can slide without friction forward and backward.
This can be achieved by adding passive caster wheels, which introduces
non-holonomic constraints, to the underside of the snake robot. A di¤erent
approach presented in Burdick et al. (1993) and Chirikjian and Burdick
(1995) is to assume that the parts of the snake robot in contact with the
ground are anchored to the ground surface. Then the other parts of the
snake robot body may move without being subjected to friction forces.
A model of the motion dynamics is needed to describe the friction forces
a snake robot is subjected to when moving over a surface. Most mathematical models that describe the dynamics of snake robot motion are limited to
planar (2D) motion (Ma, 2001; Ma et al., 2003a; McIsaac and Ostrowski,
1.1 Motivation and Background
5
2003b; Saito et al., 2002) while 3D models of snake robots have only recently
been developed (Liljebäck et al., 2005; Ma et al., 2004) where the contact
between a snake robot and the ground surface is modelled as compliant.
3D models facilitate testing and development of 3D serpentine motion patterns such as sinus-lifting (similar to lateral undulation, but with an additional vertical wave) and sidewinding (a gait commonly employed by desert
snakes). There exists various software to model and simulate dynamical
systems. 3D simulations of a snake robot are presented in Dowling (1999,
1997) where such software is employed to de…ne and simulate a 3D snake
robot. However, details of this implementation are not presented and the
simulations are thus not easily reproducible by the reader.
On a ‡at surface the ability of a snake robot to move forward is dependent on the friction between the ground surface and the body of the
snake robot. Hence, the friction forces and the unilateral (upward) contact forces (which give rise to the friction forces) are important parts of
the mathematical model of a snake robot. The friction forces are usually
based on a Coulomb or viscous-like friction model (McIsaac and Ostrowski,
2003b; Saito et al., 2002), and the Coulomb friction is most often modelled
using the sign-function (Ma et al., 2004; Saito et al., 2002). The unilateral
contact forces as a result of contact with the ground surface are modelled
as a spring-damper system in Liljebäck et al. (2005), where the centre of
gravity of each link is employed as the contact point.
We have focused mostly on modelling in the above review, and we now
proceed to give a short comment on snake robot locomotion. Many authors
base the choice and implementation of the most common serpentine motion
pattern called lateral undulation on the serpenoid curve found in Hirose
(1993). This is a curve that describes the motion of a biological snake while
moving by lateral undulation. A snake robot can follow an approximation to
this curve by setting its joint angles according to a sine-curve that is phaseshifted between adjacent joints. This approach to snake robot locomotion is
widely implemented for snake robots that have either wheels (Ostrowski and
Burdick, 1996; Prautsch and Mita, 1999) or a friction property such that
each link of the snake robot glides easier forward and backward compared to
transversal motion (Ma, 2001; Saito et al., 2002). A no-slip constraint (i.e.
a non-holonomic velocity constraint) on each wheel is sometimes introduced
in the mathematical model and this results in that the snake robot links
cannot slip sideways. Such an approach is presented in Prautsch and Mita
(1999), where the no-slip constraint allows one to signi…cantly reduce the
model. A Lyapunov-based proof for this reduced model together with a
6
Introduction
proposed controller shows that the snake robot is able to move to a position
reference. A velocity controller for a wheel-less snake robot with the friction
property described above is presented in Saito et al. (2002). The simulation
and experimental results presented in that paper indicate that the snake
robot is able to stay within a reasonable o¤set of a desired speed. Results on
controllability based on a kinematic description of snake robot locomotion
are presented in Kelly and Murray (1995) and Ma et al. (2003b).
From the review above we see that very interesting and useful results
are published on snake robot modelling and locomotion. There are many
important aspects to consider when modelling a snake robot. The friction
model has to describe the frictional contact with the ground properly. Moreover, a model to describe a varying normal contact force with the ground
surface is needed to simulate some motion patterns. Other approaches to
snake robot locomotion rely on having objects to push against to move
forward and in this case the contact forces between the snake robot and
the obstacles have to be de…ned in some way. In addition, we need to de…ne some sort of exoskeleton for the snake robot for contact with obstacles
and the ground surface. In order to develop the previously published result on modelling of snake robots further, we note that only unidirectional
Coulomb friction can be described by a sign-function since the direction of
the friction force due to a snake robot link sliding on the ground surface
will not point in the correct direction relative to the direction of the sliding
velocity for spatial friction. Moreover, a sign-function with the commonly
employed de…nition sign (0) = 0 (i.e. not set-valued) will not give a nonzero friction force for zero sliding velocity. However, such a non-zero friction
force is sometimes acting on rigid bodies in the stick-phase of Coulomb friction. Hence, a snake robot on a slightly inclined surface will slowly slide
downward even though it should stick to the surface due to the Coulomb
friction. This is because the friction force becomes zero every time the velocity becomes zero. Hence, the above mentioned properties of Coulomb
friction cannot be modelled with the previously published friction models.
Furthermore, a very high spring coe¢ cient is needed to model a hard obstacle (or ground surface) with a compliant contact model (which is employed
in the previously published literature) and this leads to sti¤ di¤erential
equations which in turn are cumbersome to solve numerically. Moreover,
it is not clear how to determine the dissipation parameters of the contact
unambiguously when using a compliant model (Brogliato, 1999). Hence,
there is a need for a alternative approach to describe Coulomb friction with
an accurate stick-phase, and normal contact forces due to impacts with the
1.2 Main Contributions of this Thesis
7
obstacles and the ground surface with a non-compliant model.
In addition, for 3D models of the dynamics of snake robots presented
in such a way that the model is possible to re-create by the reader, only
one point on each link (most often the centre of gravity) is considered as
the contact point with the ground. Hence, rotational motion around the
longitudinal axis of a snake robot link of such a point does not result in
any translational motion since the point do not have a circumference. This
makes the models unable to perform rolling gaits such as lateral rolling.
Hence, there is a need for a well-described model of a snake robot with some
sort of exoskeleton (instead of just a single stationary point) for contact with
the ground surface.
For wheel-less snake robots it is not su¢ cient to consider a purely kinematic model for the motion pattern lateral undulation, as the friction between the snake robot and the ground surface is essential for locomotion.
Therefore, the friction needs to be considered for wheel-less snake robots
and this motivates including the dynamics in the development of modelbased controllers for snake robots. Some controllers are developed for position control of a single link or a small selection of snake robot links.
Moreover, controllers are proposed for velocity and heading control of a
snake robot. However, for delicate operations such as inspection and maintenance in industrial plants, every point on the snake robot body need to
be accurately controlled so that the snake robot do not unintentionally collide with objects on its path. Such operations require precise control of the
snake robot joints and this can be realized through a model-based design.
However, no such controller is found in the previously published literature.
1.2
Main Contributions of this Thesis
This thesis …rst presents a thorough review of snake robot modelling and
locomotion. Then various mathematical models of a snake robot, together
with a model-based control law are developed. Furthermore, we present
simulation results of a number of motion patterns and some of these results
are experimentally validated.
Review: During the last ten to …fteen years, the published literature on
snake robots has increased vastly. This thesis gives an elaborate
overview and comparison of the various mathematical models and
locomotion principles for snake robots presented during this period.
Both purely kinematic models and models including dynamics are
investigated. Moreover, we provide an introduction to the source of
8
Introduction
inspiration of snake robots: biologically inspired crawling locomotion.
Furthermore, di¤erent approaches to both biologically inspired locomotion and arti…cially generated motion patterns for snake robots are
discussed.
2D model: Propulsion by some sort of obstacle aided locomotion enables
the snake robot to move forward in rough terrain. To this end, we
present a novel non-smooth (hybrid) mathematical model for planar
wheel-less snake robots that allows a snake robot to push against
external obstacles apart from the ‡at ground. The framework of nonsmooth dynamics and convex analysis allows us to systematically and
accurately incorporate both unilateral contact forces (from the obstacles) and friction forces based on Coulomb’s law using set-valued
force laws. Hence, stick-slip transitions with the ground and impacts
with the obstacles are modelled as instantaneous transitions. Moreover, the set-valued force law results in an accurate description of the
Coulomb friction which is important for snake robot locomotion on a
planar surface. Conventional numerical solvers can not be employed
directly for numerical integration of this model due to the set-valued
force laws and possible instantaneous velocity changes. Therefore, we
show how to implement the model for numerical treatment with a
numerical integrator called the time-stepping method. Even though
we model the snake robot as a hybrid system, in this framework we
avoid explicit switching between system equations (for example when
a collision occurs). The description of the model and the method for
numerical integration are presented in such a way that people who
are new to the …eld of non-smooth dynamics can use this thesis and,
in particular, Chapter 3 as an introduction to non-smooth modelling
of robot manipulators with impacts and friction.
3D model: Many snake robot motion patterns require that the snake robot is able to perform more than just planar motion. Therefore, the
2D model is extended to a non-smooth 3D mathematical model of
a snake robot (without wheels) where a particular choice of coordinates results in an e¤ective way of writing the system equations. The
same framework of non-smooth dynamics and convex analysis as in
for the 2D model is employed for the 3D model. Hence, no explicit
changes between equations are necessary when collisions occur. This
is particularly advantageous for 3D motion since the snake robot links
repeatedly collide with the ground surface during, for example, loco-
1.2 Main Contributions of this Thesis
9
motion by sidewinding. Moreover, the set-valued force law employed
to model the normal contact between the snake robot and the ground
surface and obstacles is a bene…cial alternative to the compliant (e.g.
spring-damper) model employed in previously published works. We
de…ne for each link a cylindrical exoskeleton for contact with the
ground surface and the external obstacles. Hence, a rolling motion
will result in a sideways motion which would not be the case if only
the centre of gravity is employed for contact with the environment.
This enables us to simulate motion patterns such as lateral rolling.
Validation of models with experimental results: Both the 2D and
the 3D mathematical model are veri…ed through experiments. In
particular, a back-to-back comparison between numerical simulations
and experimental results is presented. Such a quantitative comparison is not found in any of the previously published works on snake
robots. The simulations show that the numerical treatment of both
models can handle both impacts with the ‡oor and obstacles. The
experiments are performed with the snake robot ‘Aiko’ depicted in
Figure 1.3, which is a wheel-less snake robot with cylindrical links.
Aiko has been developed at the NTNU/SINTEF Advanced Robotics
Laboratory in Trondheim, Norway.
A ‘robot model’and model-based control design: To be able to employ the model-based control techniques developed for robot manipulators, we employ the standard dynamics of a robot manipulator
(found in, e.g., Spong and Vidyasagar (1989)) and develop a process
plant model based on this framework for a planar snake robot. Such a
representation is not favourable for simulation due to the high number
of degrees of freedom of a snake robot so the 2D model mentioned earlier is still important for developing and testing new motion patterns.
We employ one group of the techniques available for the robot manipulator model, called input-output linearization to develop a controller
for tracking control of the snake robot joints. In order to make the
stability analysis manageable, we develop a control plant model from
the process plant model where the global orientation of the snake
robot is removed from the model. This allows us to complete a formal Lyapunov-based proof which shows that the joint angles converge
exponentially to their desired trajectory.
From the list of contributions given above, we see that several mathematical models of snake robots are presented in this thesis. All models
10
Introduction
Figure 1.3: The snake robot ‘Aiko’developed at the NTNU/SINTEF Advanced Robotics Laboratory. Aiko is used as a basis for the mathematical
models.
describe the same snake robot. However, di¤erent aspects of snake robot
locomotion is emphasized in the various models: The 2D model is for simulation of planar snake robot motion with or without obstacles to push
against, while the 3D model is for simulation of 3D gaits like sidewinding
or sinus-lifting where the use of obstacles can also be included. The 2D
model is much faster to simulate and easier to implement since less degrees
of freedom and contact points are considered. Hence, it should be employed
as an e¤ective test bed for the development of new planar motion patterns
and snake robot controller testing. In addition, we have developed a 2D
model of a snake robot based on the standard formulation of the dynamics
of a robot manipulator. This model is denoted the ‘robot model’ and is
in its analytical form is not favourable for simulations due to the intricate
system matrices which arise from employing minimal generalized coordinates for a snake robot (with such a high number of degrees of freedom).
However, the standard formulation provides us with a large range of tools
for controller synthesis and stability analysis for the snake robot.
1.3
Organization of this Thesis
Chapter 2: The main developments in snake robot modelling and locomotion are reviewed.
Chapter 3: A 2D non-smooth model of a snake robot with obstacles to
1.3 Organization of this Thesis
11
push against is developed. The model is based on the framework of
non-smooth dynamics and convex analysis. Instead of presenting the
background theory in a separate chapter, we use the 2D model of a
snake robot as an example and present the necessary theory when it
is appropriate throughout this chapter.
Chapter 4: A 3D non-smooth model of a snake robot with obstacles to
push against is developed. Those familiar with non-smooth dynamics
and convex analysis can read this chapter independently from Chapter 3.
Chapter 5: The concept of obstacle aided locomotion for snake robots is
elaborated on, and we provide an overview of how a biological snake
moves by pushing against irregularities in the ground.
Chapter 6: A planar model of a snake robot based on the standard formulation of the dynamics of a robot manipulator is developed. Moreover, an input-output linearizing controller for asymptotic tracking
for snake robot joints is designed and a formal stability proof is provided.
Chapter 7: Simulation results are given for all models. In addition, backto-back comparisons between experimental results and simulation results with the 2D and 3D model are presented for a selection of serpentine motion patterns.
Chapter 8: Conclusions and suggestions to further work are presented.
Appendix A: An additional proof and a theorem employed in Chapter 6
are described.
12
Introduction
Chapter 2
Developments in Snake
Robot Modelling and
Locomotion
2.1
Introduction
During the last ten to …fteen years, the published literature on snake robots
has increased vastly and the purpose of this chapter is to provide an elaborate survey of the various mathematical models and locomotion principles
of snake robots presented during this period. Both purely kinematic models
and models including dynamics are investigated. Moreover, di¤erent approaches to both biologically inspired locomotion and arti…cially generated
motion patterns for snake robots are discussed. We also provide an introduction to the source of inspiration for snake robot locomotion: serpentine
and crawling locomotion of biological creatures. The speci…c choices of
hardware for e.g. sensors and actuators for snake robots are beyond the
scope of this chapter and will not be discussed. In addition, we do not consider snake robots with active wheels in this thesis. For more information
on such snake robots see, for example, Kamegawa et al. (2004), Yamada
and Hirose (2006a) and Masayuki et al. (2004).
Note that the notation in this chapter sometimes di¤ers from the rest
of this thesis. This is because in this chapter we strive to employ the same
notation as used in the papers we discuss to ease the transition for those
who want to explore these source papers further. However, we balance
between using the exact same notation as in the various original papers
and using a common notation for this chapter, so the correspondence is not
14
Developments in Snake Robot Modelling and Locomotion
always one-to-one.
This chapter is based on Transeth and Pettersen (2006) and Transeth
and Pettersen (2008) and it is arranged as follows: Section 2.2 gives a short
introduction to snakes and serpentine and crawling locomotion of biological
creatures. Various mathematical models of snake robots are presented in
Section 2.3. Section 2.4 provides an overview of numerous motion patterns
implemented on snake robots, while the survey presented in this chapter is
discussed and summarized in Section 2.5.
2.2
Biological Snakes and Inchworms
Biological snakes, inchworms and caterpillars are the source of inspiration
for most of the robots dealt with in this chapter. We will therefore start with
a short introduction to snake physiology and snake locomotion. In addition,
inchworm and caterpillar motion patterns are outlined. Unless otherwise
speci…ed, the contents in this section are based on Mattison (2002), Bauchot
(1994) and Dowling (1997).
2.2.1
Snake Skeleton
The skeleton of a snake often consists of at least 130 vertebrae and can
exceed 400 vertebrae. The range of movement between each joint is limited
to between 10 and 20 degrees for rotation from side to side and to a few
degrees of rotation when moving up and down. A large total curvature of
the snake body is still possible because of the high number of vertebrae.
A very small rotation is also possible around the direction along the
snake body. This property is employed when the snake moves sideways by
sidewinding.
2.2.2
Snake Skin
Since snakes have no legs, the skin surface plays an important role in snake
locomotion (Bauchot, 1994). The snake should experience little friction
when sliding forward, but great friction when pushed backward. The skin is
usually covered with scales with tiny indentations which facilitate forward
locomotion. The scales form an edge to the belly during motion which
results in that the friction between the underside of the snake and the
ground is higher transversal to the snake body than along it (Hirose, 1993).
2.2 Biological Snakes and Inchworms
2.2.3
15
Locomotion – The Source of Inspiration for Snake
Robots
Most motion patterns implemented for snake robots are inspired by locomotion of snakes. However, inchworms and caterpillars are also used as
inspiration. The relevant motion patterns of all these creatures will be
outlined in the following.
Lateral Undulation
Lateral undulation (also denoted serpentine crawling) is a continuous movement of the entire body of the snake relative to the ground. Locomotion is
obtained by propagating waves from the front to the rear of the snake while
exploiting roughness in the terrain. Every part of the body passes the same
part of the ground ideally leaving a single sinus-like track as illustrated in
Figure 2.1 (a). To prevent lateral slipping while moving forward, the snake
‘digs’in to the ground with help of the edge described in Section 2.2.2. In
addition, it may use contours such as rocks on the ground to push against.
All the contact points with the ground constitute possible push-points for
the snake and the snake needs at least three push-points to obtain a continuous forward motion. Two points are needed to generate forces and the
third point is used to balance the forces such that they act forward.
The e¢ ciency of lateral undulation is mainly based on two factors. 1)
The contour of the ground. The more contoured the ground, the more
e¢ cient the locomotion. 2) The ratio between the length of the snake and
its circumference. The fastest snakes have a length that is no longer than 10
to 13 times their circumference. Speeds up to 11 km/h have been observed
in rough terrains.
Figure 2.1: (a) Lateral undulation and (b) concertina locomotion (Mattison,
2002). By permission of Cassell Illustrated.
16
Developments in Snake Robot Modelling and Locomotion
Figure 2.2: Sidewinding locomotion (Burdick et al., 1993) c 1993 IEEE.
Concertina Locomotion
A concertina is a small accordion instrument. The name is used in snake
locomotion to indicate that the snake stretches and folds its body to move
forward. The folded part is kept at a …xed position while the rest of the
body is either pushed or pulled forward as shown in Figure 2.1 (b). Then,
the two parts switch roles. Forward motion is obtained when the force
needed to push back the …xed part of the snake body is higher than the
friction forces acting on the moving part of the body.
Concertina locomotion is employed when the snake moves through narrow passages such as pipes or along branches. If the path is too narrow
compared to the diameter and curving capacity of the snake, the snake is
unable to progress by this motion pattern.
Sidewinding Locomotion
Sidewinding is probably the most astonishing gait to observe and is mostly
used by snakes in the desert. The snake lifts and curves its body leaving
short, parallel marks on the ground while moving at an inclined angle as
shown in Figure 2.2. Unlike lateral undulation there is a brief static contact
between the body of the snake and the ground.
Sidewinding is usually employed on surfaces with low shear such as
sand. The snakes can reach velocities up to 3 km/h during sidewinding
locomotion.
2.3 Design and Mathematical Modelling
17
Other Snake Gaits
Snakes also have gaits that are employed in special situations or by certain
species. These are e.g. rectilinear crawling, burrowing, jumping, sinuslifting, skidding, swimming and climbing. The latter four, which are or
may be used for snake robots, are described as follows.
Sinus-lifting is a modi…cation of lateral undulation where parts of the
trunk are lifted to avoid lateral slippage and to optimise propulsive force
(Hirose, 1993). This gait is employed for high speeds.
A variation of lateral undulation is called skidding (also denoted slidepushing) and is employed when moving past low-friction surfaces. The
snake rests its head on the ground and then sends a ‡exion wave down
through its body. This is repeated in a zigzag pattern and is a very energyine¢ cient way of locomotion.
Almost all snakes can swim. They move forward by undulating laterally
like an eel.
Long and thin bodied snakes can climb trees by vertical lateral undulation. Parts of their body hang freely in the air, while branches are used
as support.
Inchworm and Caterpillar Locomotion
An inchworm moves forward by grabbing the ground with its front legs
while the rear end is pulled forward. The rear legs then grab the ground
and the inchworm lifts its front legs and straightens its body. Caterpillars
send a vertical travelling wave through their body from the end to the front
in order to move forward. Small legs give the necessary friction force while
on the ground.
2.3
Design and Mathematical Modelling
The mathematical model of a snake robot, of course, depends on its design.
To categorize the di¤erent snake robot designs we recognize certain basic
properties: 1) Type of joints, 2) number of degrees of freedom (DOF) and
3) with or without passive caster wheels. Most snake robots consist of links
connected by revolute joints with one or two DOF. On some robots, the
links are extensible (i.e. prismatic joints). To achieve the desired frictional
property for lateral undulation mentioned in Section 2.2, some snake robots
are equipped with passive wheels. When wheels are employed, the dynamics
of the interaction between the robot and the ground surface is often ignored.
18
Developments in Snake Robot Modelling and Locomotion
If no wheels are attached, this friction force needs to be considered for some,
but not all, gaits (see Section 2.4).
In the following, the mathematical modelling of the di¤erent snake robots is divided into kinematics and dynamics.
2.3.1
Kinematics
The kinematics describes the geometrical aspect of motion. Di¤erent modelling techniques ranging from classical methods such as the DenavitHartenberg (D-H) convention (see e.g. Murray et al. (1994) for more on
the D-H convention) to specialized methods for hyper-redundant structures
(structures with a high number of DOF) have been employed. The following
subsections will elaborate on the di¤erent modelling techniques.
The Denavit-Hartenberg Convention
The Denavit-Hartenberg convention is a well established method for describing the position and orientation of the links of a robot manipulator
with respect to a (usually …xed) base frame. Di¤erent solutions are presented to deal with the fact that the base is not …xed on a snake robot
(Liljebäck et al., 2005; Poi et al., 1998).
Poi et al. (1998) present a snake robot that is made of 9 equal modules.
Each module consists of seven revolute 1 DOF joints which are connected
by links of equal length. Three joints and four joints have the axis of rotation perpendicular to the horizontal and vertical plane, respectively, and
the joints are placed alternately. Each module is parameterised with the
D-H convention. A modi…cation to the convention has been proposed by
placing the base coordinate system on the closest motionless link to the part
of structure which is in motion. Hence, the links in motion are described
in an inertial frame. The snake robot described in Poi et al. (1998) moves
only four or …ve modules simultaneously, so giving the position and orientation relative to the closest motionless link prevents traversing through the
complete structure to obtain positions and orientations in an inertial frame
(such traversing is usually necessary when employing the D-H convention
since we are dealing with minimal coordinates).
The motion patterns employed in Liljebäck et al. (2005), sidewinding
and lateral undulation, are based on constant joint movement so we have
to traverse through the whole structure to obtain the inertial position and
orientation of each snake robot link, and hence the previously presented
approach (Poi et al., 1998) will not simplify the mathematical structure.
2.3 Design and Mathematical Modelling
19
Therefore, a virtual structure for orientation and position (VSOP) is introduced to be able to describe the kinematics of the snake robot in an inertial
reference frame. Liljebäck et al. (2005) present a snake robot with 5 revolute 2 DOF joints. The VSOP describes the position and orientation of the
rearmost link of the snake robot relative to an inertial reference frame by 3
orthogonal prismatic joints and 3 orthogonal revolute joints, respectively.
These virtual joints are connected by links with no mass. By employing the
VSOP in the Denavit-Hartenberg convention, the position and orientation
of each joint is given in an inertial coordinate system.
A Backbone Curve
Instead of starting by …nding the position and orientation of each joint directly as with the Denavit-Hartenberg convention, a curve that describes
the shape of the ‘spine’ of the snake robot can be employed (Chirikjian,
1992; Chirikjian and Burdick, 1991, 1994b; Yamada and Hirose, 2006b). The
Frenet-Serret apparatus (see Do Carmo, 1976) is employed in the classical
handling of the geometry of curves (Chirikjian and Burdick, 1994b). However, this approach has some limitations (Chirikjian and Burdick, 1994b).
First, the Frenet-Serret frames assigned along the curve are not de…ned for
straight line segments. Second, the vector function describing the spatial
curve requires the numerical solution of a cumbersome di¤erential equation. The introduction of backbone curves (see e.g. Chirikjian and Burdick,
1994b) is a way of handling these limitations. The backbone curve is de…ned
as ‘a piecewise continuous curve that captures the important macroscopic
geometric features of a hyper-redundant robot’ (Chirikjian and Burdick,
1994b) and it typically runs through the spine of the snake robot. A set of
orthonormal reference frames are found along the backbone curve to specify
the actual snake robot con…guration. The backbone curve parameterisation
together with an associated set of orthonormal reference frames is called a
backbone curve reference set and allows for snake robots built from both
prismatic and revolute joints (Chirikjian and Burdick, 1991).
A recent alternative approach to the methods presented by Chirikjian
and Burdick (Chirikjian and Burdick, 1991, 1994b) has been given by Yamada and Hirose (2006b). This approach is called the bellows model and is
speci…cally designed for distinguishing explicitly between the twisting and
bending of the body of the snake robot. This is advantageous since most
snake robots are designed with joints capable of bending, but not twisting
(for example snake robots with cardan joints). Hence, the ability to twist
can simply be left out of the model of the snake robot. So far, there are no
20
Developments in Snake Robot Modelling and Locomotion
published results on how to …t the continuous bellows model to an actual
snake robot (which has a discrete morphology).
The problem of determining joint angles of a robot manipulator given
the end-e¤ector position is called the inverse kinematics problem. For
hyper-redundant manipulators (such as snake robots) this is a very computationally demanding task. When the backbone curve is employed, the
problem is reduced to determining the proper time varying behaviour of the
backbone reference set and this approach has been employed for controlling
a real snake robot in Chirikjian and Burdick (1994a).
The above methods described in this section are suitable for abstraction,
understanding and development of the geometric aspects of snake robot
motion planning (which may initially be quite complicated due to the high
number of DOF), in particular when the dynamics may be neglected.
Non-holonomic Constraints and Snake Robots with Passive Caster
Wheels
The key to snake robot locomotion is to continuously change the shape
of the robot. This is achieved by rotation and/or elongation of its joints.
Krishnaprasad and Tsakiris (1994) and Ostrowski and Burdick (1996) both
present kinematic approaches on how to link the changes in internal con…guration to the net position change of the robot. The relation is found
by utilizing non-holonomic constraints and di¤erential geometry. Ostrowski
and Burdick (1996) employ Hirose’s Active Cord Mechanism Model 3 (ACM
III) as an example which will be explained in the following. The …rst three
pair of wheels of the ACM III are illustrated in Figure 2.3. The …ve joint
angles 1 ; 2 ; 3 ; 1 and 3 are controlled inputs and is the absolute angle
of the middle link. The kinematic non-holonomic constraints are realized
by adding passive caster wheels on the snake robot and may be written in
the form
x_ i sin i
y_ i cos i = 0;
(2.1)
where (x_ i ; y_ i ) is the translational velocity of the centre of axle i (corresponding the numbering of i , i = 1; 2; : : :) and i is the absolute angle (with
respect to a horizontal axis) of the two wheels connected to axle i. More
on non-holonomic systems can be found in Kolmanovsky and McClamroch
(1995) and Bloch et al. (2003). The wheels are assumed not to slip and
therefore realize an ideal version of the frictional properties of the snake
skin as mentioned in Section 2.2.2.
A local form A of a connection provides the relation between the shape
2.3 Design and Mathematical Modelling
21
Figure 2.3: The …rst three links of the ACM III employed by Ostrowski
and Burdick (1996).
changes of the snake robot and its net locomotion:
g
1
g_ =
A (r) r;
_
(2.2)
where r is the shape variables and g 2 SE (2) gives the overall position
and orientation of the snake robot (Ostrowski and Burdick, 1996). The
connection provides understanding of how shape changes can generate locomotion and can even be used for controllability tests (Kelly and Murray,
1995; Ostrowski and Burdick, 1998). The simple form of (2.2) is dependent
on the kinematic constraints breaking all the symmetries of the Lagrangian
function which may raise dynamic constraints. This is achieved, with the
ACM III as an example by using the …rst three segments to de…ne the net
motion of the snake robot. These segments de…ne the path which is to be
followed by the remaining segments due to the non-holonomic constraints
on the wheels.
This modelling technique have also been used to include the dynamics,
and this is described in Section 2.3.2.
2.3.2
Dynamics
The dynamics of the snake robots presented in the published literature has
been derived by utilizing various modelling techniques such as the NewtonEuler formulation, Lagrange functions and geometric mechanics.
For snake robots without wheels, the friction between the snake robot
and the ground a¤ects the motion of the snake robot signi…cantly. Thus,
for these snake robots, the dynamics should be modelled for locomotion
patterns such as lateral undulation. For snake robots with wheels, however,
the wheels greatly reduce the friction, and hence, make it possible to utilize
a purely kinematic model of the robot. The majority of results presented on
modelling of the dynamics have therefore considered snake robots without
wheels. In the following we will …rst give a short introduction to some of
22
Developments in Snake Robot Modelling and Locomotion
the notation utilized below, then we give a brief overview of a selection of
the results reported on the modelling of dynamics of wheeled snake robots
and …nally we present results on snake robots without wheels.
To ease the presentation of the mathematical models, a common notation for some of the material is presented, which is based in parts on
Prautsch and Mita (1999) and Saito et al. (2002). Denote the mass mi ,
length 2li and moment of inertia Ji for each link i = 1; 2; :::; n. The snake
robot moves in the xy-plane. Denote the angle i between link i and the
inertial (base) x-axis. Denote position of the centre of gravity (CG) of
2
i
link i by (xi ; yi ). Denote the unit vectors tangential eB
t 2 R and norBi
2
i
mal eB
n 2 R to the link i in the horizontal xy-plane. Hence, et points
T
Bi
i
_ i y_ i 2 R2 of
along link i and eB
n ? et . Denote the velocity v i = x
i
i
link i, and tangential and normal velocity of link i v i;t = eB
eB
t
t
T
v i and
T
Bi
i
v i;n = eB
v i , respectively.
n en
The friction forces that act on the CG of link i are denoted
T
2 R2 where fxi , fyi are friction forces between link i
f i = fxi fyi
and the ground along the x- and y-direction of the inertial frame, respec(j)
tively. The coe¢ cients of friction tangential and normal to link i are ci;t
(j)
and ci;n , respectively, where j is used in this chapter to distinguish between
the coe¢ cients in the various friction models.
Snake Robots with Passive Caster Wheels
A desired property for moving by lateral undulation, is to keep the di¤erence
between lateral and longitudinal friction as high as possible. This property
of friction can be obtained by attaching caster wheels to the belly of the
snake robot. The equations of motion of a simpli…ed version of the ACM
III snake robot used by Hirose (1993) are presented by Prautsch and Mita
(1999). The robot is the same as the ACM III shown in Figure 1.2 and
Figure 2.3 except that the wheel axles are …xed. The dynamic model is
derived to utilize acceleration-based control algorithms. It is assumed that
the wheels do not slip sideways.
A snake robot (called the SR#2) has been presented and compared
to the ACM III by Wiriyacharoensunthorn and Laowattana (2002). The
Active Cord Mechanism (ACM) modelling assumes that the wheels do not
slip. This non-slippage introduces non-holonomic constraints. The SR#2
model is based on holonomic framework and is hence without the no-slip
condition. The argument used against assuming no slip is that it is di¢ cult
2.3 Design and Mathematical Modelling
23
to control the torques in the joints such that the assumption is satis…ed.
Simulations show the ACM III build up an error in position while following
a circular path. This is not the case for SR#2, something which makes it
a more accurate model for this scenario.
The above models have all described planar motion. Moreover, a 3D
model of the dynamics of a snake robot with wheels that do not slip has
been presented by Ma et al. (2004). In addition, a system equation for
control of the height of the wheels is given and computer simulations are
presented in that paper.
Snake Robots without Wheels
The use of wheels may decrease traversability (Saito et al., 2002), thus
wheel-less robots may have an advantage. As discussed earlier, friction
plays a signi…cant role for wheel-less snake robots, hence it is necessary to
model the dynamics and not only the kinematics for relatively high speed
motion (for certain motion patterns during low speed motion, a kinematic
model may su¢ ce). First, we will give an overview of the friction and
contact models employed for snake robots, then a selection of dynamic
models derived for snake robots without wheels will be presented.
Friction and Contact Models The friction models presented in literature on snake robots are based on a Coulomb or viscous-like friction
model and such models are explained, for instance, in Egeland and Gravdahl (2002). For 3D models of snake robots, it is necessary to model the
normal contact force due to impacts and sustained contact with the ground,
in addition to the friction force. This force has been described as compliant
by a spring-damper model in Liljebäck et al. (2005) as
fNi =
0
k zi
; zi 0
d z_i ; zi < 0;
(2.3)
+
where zi 2 R is the height of the centre of gravity of link i, z_i = dz
dt , k 2 R
+
is the constant spring coe¢ cient of the ground, and d 2 R is a constant
damping coe¢ cient that serves to damp out the oscillations induced by the
spring. The friction force on link i, based on a simple, viscous-like model,
is now written as
fi =
(1)
ci;t jfNi j v i;t
(1)
ci;n jfNi j v i;n 2 R2 :
(2.4)
The sum of forces acting on link i in the model presented by Liljebäck et
T
al. (2005) is f 3D
= fT
fNi 2 R3 . The spring coe¢ cient k needs to be
i
i
24
Developments in Snake Robot Modelling and Locomotion
set very high to imitate a solid surface. Hence, the total system is sti¤ and
requires a very small simulation step size to be simulated. However, the
constitutive law (2.3) for the normal force provides an intuitive and simple
approach to implementing the normal force. A friction model including
both static and dynamic friction properties for a 3D dynamic model is
given by Ma et al. (2004).
The 2D anisotropic viscous friction model used by Grabec (2002) can
be derived from (2.4) by setting fNi
1. In this case the friction force is
found from
f i = Hi v i ,
(2.5)
where
Hi =
(2)
ci;n
"
1
(2) !
ci;t
(2)
ci;n
i
eB
t
i
eB
t
T
I2
2
#
;
(2.6)
and I2 2 2 R2 2 is a unit matrix.
The e¤ect of rotational motion of the links is introduced in the two 2D
friction models, one with viscous and one with Coulomb friction, presented
by Saito et al. (2002). Both models are derived by integrating the in…nitesimal friction forces on a link. The translational part of the viscous friction
model is given by (2.4) with fNi = mi (i.e. fNi is not an actual force). The
total viscous friction torque due to rotational velocity around the centre of
gravity of link i is found to be
i
=
2
(3) mi li
ci;n
3
_ i 2 R.
(2.7)
For translational motion the friction force based on Coulomb’s law is found
for _ i = 0 as (Saito et al., 2002)
02
31
" (3)
#
T
Bi
cos i
sin i ci;t 0
v
e
i 5A
@4 t
f i = mi g
:
(2.8)
(3) sign
T
sin i cos i
B
0 ci;n
i
e
vi
n
The expression for the Coulomb friction force is slightly di¤erent for _ i 6= 0
(Saito et al., 2002), however we only include the case when _ i = 0 here to
simplify the presentation. Employing Coulomb’s law of dry friction as the
friction model results in a more complicated, but also a more accurate model
for motion on non-lubricated surfaces. The viscous-like friction model (2.4)
does not include dry friction and thus the relative high friction forces which
may arise at low velocities are not modelled.
2.3 Design and Mathematical Modelling
25
For most of the gaits simulated with the above friction models, the
property ci;t < ci;n has been implemented to realize the anisotropic friction
property of a snake moving by lateral undulation. It may be di¢ cult to
design a snake robot with ci;t < ci;n on a general surface. Sidewinding has
been implemented with an isotropic friction model (ci;n = ci;t ) by Liljebäck
(2004) and as a purely kinematic case by Burdick et al. (1993). Special
gaits for planar motion based on an isotropic friction model are detailed by
Chernousko (2005, 2000).
Decoupled Dynamic Model A …ve link snake robot with 1 DOF joints
is modelled and controlled in Saito et al. (2002). The robot is built and experiments performed to validate the theoretical results. Four parallel metal
skates are put on the underside of each link to implement the anisotropic
friction property cti < cni .
The dynamic model of the snake robot is developed from the NewtonEuler equations resulting in two sets of equations: one for translational
motion of the centre of mass w of the snake robot and another for the
rotational motion of each link given in an inertial frame. The …nal equations of motion can be decoupled into two parts: shape motion and inertial
locomotion. The shape motion maps the joint torques to joint angles while
the inertial locomotion relates the joint angles to the inertial position and
orientation of the snake robot. This simpli…es the analysis and synthesis
of locomotion of the snake robot. To achieve decoupling, a vector of relative angles 2 Rn 1 where the i-th element of is i = i
i+1 , and a
quantity _ 2 R which can be thought of as ‘an average angular momentum’
are introduced (Saito et al., 2002). The expressions for shape motion and
inertial motion, respectively, are found to be
_
hs • ; ; _ ; w
hi
_ ; • ; ; w;
_ w;
• _
= Bu;
= 0;
(2.9)
(2.10)
T
where hs ( ) ; hi ( ) 2 Rn are certain functions,
= 1
n , u
are the joint torques and B is an invertible matrix. Control of the snake
robot is now performed in two steps: First, the joint torques u control the
shape of the robot and, second, the relative angles control the average
angular momentum _ and position w. For someone who needs a 2D model
of a snake robot and has a basic knowledge of classical mechanics, this
is probably the easiest 2D model to implement for simulation due to the
concise and comprehensive presentation of the model in the paper.
26
Developments in Snake Robot Modelling and Locomotion
Quasi-Stationary Equations of Motion A 2D model based on the
Newton-Euler formulation of a snake robot with 1 DOF revolute joints
with the viscous friction model (2.5) is presented by Grabec (2002).
Non-dimensional variables are introduced to simulate the dynamics of
the snake robot. The resulting system of second order non-linear equations, which constitute the non-dimensional model of the snake robot, may
become unstable during simulation. To aid the numerical treatment, overcritical damping is introduced by setting accelerations to zero. The result
is a set of ‘quasi-stationary’…rst-order di¤erential equations of motion. By
employing the …rst-order equation for translational motion together with
the friction model in short form (2.5) the velocity of the head of the snake
robot is found to be
Xn
1 Xn
(rel)
v head =
Hi
Hi v i ;
(2.11)
i=1
i=1
(rel)
where v i
is the velocity of link i with respect to the head and Hi is found
from (2.6). Saito et al. (2002) give the relationship between shape changes
from joint angle de‡ection and the position of the CG of the snake robot
(2.10). To investigate locomotion analytically, (2.11) o¤ers an alternative
approach where the direct connection between velocities of each link relative
to head of the snake robot and the head velocity is given.
Creeping on an Inclined Plane A model of a snake robot with n links
and 1 DOF rotational joints has been developed from the Newton-Euler
equations by Ma (2001). The actual snake robot that is modelled has
wheels. However, the friction between the underside of the snake robot and
the ground surface is modelled as anisotropic Coulomb friction. Hence, the
wheels do not constitute non-holonomic constraints (i.e. the wheels may
slip) and that is why we have included the model in this section. The model
of planar motion of the snake robot is extended to motion on an inclined
plane where the angle of inclination e¤ects the motion of the snake robot
in Ma et al. (2003a, 2001b).
The mathematical model is presented in two ways (both for planar motion and the motion on an inclined plane). The …rst alternative is to write
the model in a form where it is assumed that the joint angles together
with the joint angular velocities and accelerations are given (this form is
later employed for something called shape based control). From the speci…ed data, the rotational and translational accelerations of the …rst link can
then be found from the model. Thus, the motion of the snake robot in the
plane is found. Moreover, the joint torques necessary to move the joint in
2.3 Design and Mathematical Modelling
27
a predetermined way can be found from the model. Hence, it is possible
to study the joint torques and e.g. how they change for a speci…ed motion
pattern for various friction scenarios.
The second alternative is the most common for snake robot models:
how does the snake robot move given the commanded joint torques? By
specifying the joint torques, the link angle accelerations are found. Then,
the translational and rotational acceleration of the …rst link can be found
and the necessary velocities and positions are found by integration.
Simulation results are given for both shape based and torque based
control of the snake robot.
The Lagrangian Research on robots that resemble snakes is not only
limited to land-based locomotion. Papers regarding anguilliform (eel-like)
locomotion have also been published (Ayers et al., 2000; McIsaac and Ostrowski, 1999, 2000, 2003b). A …ve link 2D snake robot (called the REEL
II) with 1 DOF revolute joints, which will be used as an example in the
following, is modelled and experimented with in McIsaac and Ostrowski
(2003b). Motion planning for such a robot consists of …rst building up the
momentum to the snake robot and then steering the robot to its desired
location. Hence, it is convenient that the mathematical model includes an
explicit expression for the momentum. The model is formulated from the
Lagrangian of the system and is summarized in the following.
The fact that the energy of the system and the frictional forces acting
on the system are invariant with respect to the position and orientation of
the snake robot (the system exhibits Lie groups symmetries) is exploited to
simplify the mathematical model. The assumption that the joint angles are
controlled directly (the same as saying that the dynamics (2.9) is ignored)
yields two sets of resulting equations. The …rst equation relates the velocity
of the snake robot to its internal shape changes and is similar to (2.2) given
in Section 2.3.1 except for the locked inertia tensor I (r) and generalized
momentum vector p which have been added (we have a case of mixed constraints with both kinematic and dynamic constraints). The dynamics of
the system is described by the generalized momentum equation which is
the second set of resulting equations. The generalized momentum p is associated with the momentum along the directions allowed by the kinematic
constraints. A thorough explanation of the equations is found in Bloch et
al. (1995).
28
Developments in Snake Robot Modelling and Locomotion
The Newton-Euler Algorithm A mathematical model of a snake robot
with …ve 2 DOF revolute joints is presented by Liljebäck et al. (2005). A
snake robot is also constructed for experiments. In addition to the actual
snake robot, a virtual structure of orientation and position (VSOP, see
Section 2.3.1) is included in the dynamic model. The VSOP together with
the snake robot have generalized minimal coordinates q 2 R2(n 1)+6 and
generalised forces 2 R2(n 1)+6 . The Newton-Euler formulation and the
VSOP perspective is employed and the dynamic model is written as
• + C (q; q)
_ q_ + g (q) =
M (q) q
+
ext ;
(2.12)
where M is the mass and inertia matrix, C is the Coriolis and centripetal
matrix, g ( ) is the vector of gravitational forces and torques and ext is
the vector including the external forces (friction and normal contact force).
The matrices are detailed in Liljebäck (2004). The Newton-Euler algorithm
has been employed to simulate the snake robot. Hence, the full analytical
expressions for the system matrices do not need to be found explicitly.
Instead, the necessary matrices and accelerations are found numerically
with the recursive Newton-Euler algorithm. This is advantageous since
the analytical expressions for the system matrices are extremely large for
a large number of joints when minimal coordinates are employed. For a
model with non-minimal coordinates found in Ma et al. (2004), analytical
expressions for the joint torques and head con…guration of a 3D snake robot
model deducted from the Newton-Euler equations are shown.
The Lagrange and the Newton-Euler method are similar for rigid body
dynamics in that the expression obtained by the Lagrange method is found
by running through the Newton-Euler algorithm once. Since the NewtonEuler algorithm deals with the mathematical model as a recursive algorithm, it is a more e¢ cient framework for simulation than the Lagrange
method for large models (Egeland and Gravdahl, 2002).
A modi…cation of the Newton-Euler algorithm has been presented by
Boyer et al. (2006) to numerically evaluate a model of a continuous 3D
underwater snake robot where the modelling approach is based on beam
theory.
2.4
Snake Robot Locomotion
A variety of approaches on how to make a snake robot move has been
proposed. In most of the motion patterns or ‘gaits’used for locomotion, we
…nd a distinct resemblance to the undulating locomotion of biological snakes
2.4 Snake Robot Locomotion
Gait
Concertina
Lateral undulation
With passive wheels
Ma (2001); Ma et al.
(2003a,b); Mori and Hirose (2002); Ohno and
Hirose (2001); Ostrowski
and
Burdick
(1996);
Prautsch
and
Mita
(1999); Wiriyacharoensunthorn and Laowattana
(2002); Ye et al. (2004a)
Sidewinding
Inchworm/
Caterpillar
Climbing
Lateral rolling
Mori and Hirose (2002);
Togawa et al. (2000); Yamada et al. (2005)
29
Without wheels
Shan and Koren (1993)
Bayraktarouglu et al.
(1999); Grabec (2002);
Liljebäck et al. (2005);
Ma (1999); McIsaac and
Ostrowski (1999, 2003b);
Saito et al. (2002)
Burdick et al. (1993); Liljebäck et al. (2005)
Chirikjian and Burdick
(1995); Ohno and Hirose
(2001); Poi et al. (1998);
Rincon and Sotelo (2003)
Nilsson (1997b, 1998)
Chen et al. (2004); Dowling (1999); Ohno and Hirose (2001)
Table 2.1: Overview of gaits implemented for snake robots.
or worms. However, the motion patterns may be altered to compensate
for the fact that the snake robots do not have the exact same anatomy
as biological snakes, inchworms or caterpillars. For example, the snake
robots are not as articulated as their biological counterpart, and this reduces
movability. In addition, snakes use among other their skin to sense their
immediate vicinity. Such a …ne grid of sensors is di¢ cult to implement on
a snake robot.
Early studies of snake locomotion were given by Gray (1946). Later, a
mathematical description of the serpentine motion of snakes was presented
by Hirose (see e.g. Hirose, 1993). An overview of the most frequent gaits
which have been implemented for snake robots is found in Table 2.1 and
we see that lateral undulation is the most common motion pattern.
A description of a number of di¤erent motion patterns both for planar
and 3D motion is given in the following.
30
Developments in Snake Robot Modelling and Locomotion
Figure 2.4: The serpenoid curve in Hirose (1993). By permission of Oxford
University Press.
2.4.1
Planar Snake Robot Locomotion
Planar snake robot locomotion is usually performed with the motion pattern
lateral undulation. First, we will present how to describe the motion pattern
for a snake robot. Then some examples will be given together with other
gaits for planar locomotion.
The Serpenoid Curve
A biological snake that moves by lateral undulation across a uniform surface
displays a periodic creeping motion. The shape of the snake during this
kind of locomotion has been studied by Hirose (see e.g. Hirose, 1993), and
he has presented the ‘serpenoid curve’ as a way of describing the form of
the gliding motion of a snake. The serpenoid curve is interesting for snake
robot locomotion since the snake robot can move forward by moving its
joints such that its body follows the trace of the serpenoid curve. The
curve ensures that the curvature changes smoothly along the body, which
is natural considering the contraction and relaxation of the muscles in a
snake during locomotion (Hirose, 1993).
The serpenoid curve is illustrated in Figure 2.4 where s is the distance
along the curve, l is the length of one quarter period of the curve and
s (s; l) is the ‘winding angle’ along the curve. Denote the tangential ci;t
and normal ci;n frictional coe¢ cient between link i and the ground. The
winding angle s for locomotion of a particular snake robot is determined
by factors such as the link length, the bending angles between adjacent
links and the ratio ci;t =ci;n where the ratio also gives a lower bound for s .
The winding angle s at an arbitrary point Q on the serpenoid curve is
2.4 Snake Robot Locomotion
31
found from
s (s; l)
=
cos
s ,
(2.13)
2l
where
is the initial winding angle as illustrated in Figure 2.4. Unless
otherwise speci…ed, the common assumption is ci;t < ci;n which is necessary
to move forward on a ‡at surface by lateral undulation. However, it needs
to be mentioned that a snake robot will instead move slowly backward for
ci;t = ci;n (Nilsson, 2004).
An alternative to the serpenoid curve has been proposed by Ma (1999)
and is called the serpentine curve. Ma (1999) has shown that a snake
robot following this curve has higher locomotive e¢ ciency. For a snake
robot that does not slip, the locomotive e¢ ciency is de…ned as the ratio
between the forces that act along the snake robot and the forces that act
perpendicular to the body (Ma, 1999). The serpenoid curve is nevertheless
the most frequent employed basis for …nding the desired joint angles for a
snake robot. This is probably partly due to its simplicity compared to the
expression for the serpentine curve.
Lateral Undulation for Snake Robots
The serpenoid curve cannot be exactly reproduced by a snake robot due to
its discrete morphology. However, an approximation to the serpenoid curve
can be recreated by a snake robot by setting its (relative) joint reference
angle hi ;d for joints i = 1; : : : ; n 1 as
hi ;d
= Ah sin (! h t + (i
1)
h)
+
h;
(2.14)
where Ah is the maximum amplitude of oscillation, h is the phase shift
between adjacent links, h determines the orientation of the snake robot
and ! h is the angular frequency of oscillation (see e.g. Saito et al., 2002; Ye
et al., 2004a). The use of h to change the heading of the snake is discussed
in Ye et al. (2004a) and two alternative turning motions are also presented.
The parameters ! h and h are the two most signi…cant parameters in order
to control the speed of the serpentine wave that propagates down the body
of the snake robot.
Sideways slip should be kept as low as possible when moving by lateral
undulation. This is because such slip does not contribute to forward motion.
For wheel-less snake robots, sideways slip can be reduced by changing the
motion pattern parameters in (2.14) according to the ratio ci;t =ci;n . An
example on how to do this has been presented by Saito et al. (2002), where
the desired relative joint angles are found from (2.14) and the corresponding
32
Developments in Snake Robot Modelling and Locomotion
experimental results are presented. In addition, Saito et al. (2002) show
how to control the speed and heading of the snake robot to desired values
by altering ! h and h in (2.14), respectively.
Another possibility to reduce sideways slip is to mechanically alter the
snake robot by adding passive caster wheels to the belly. For the snake robot
ACM III (see Figure 1.2 and Figure 2.3) used as an example by Ostrowski
and Burdick (1996) a particular approach is employed to avoid wheel-slip
while moving forward. To this end, the angles between adjoining links ( 1
and 3 ) and the angles between the wheel axles and the links ( 1 , 2 and
3 ) are found from expressions similar to (2.14) where h is 90 degrees
larger for the joint angles than for the axle angles. A model similar to the
ACM III, but with …xed wheel axles, is employed by Prautsch and Mita
(1999). There a Lyapunov-based approach for position control of the head
of the snake robot is proposed. In addition, it is shown how to minimize
the control torques required for serpentine motion since low control torques
reduce the risk of wheel-slip. Date et al. (2000) introduce an expression for
the constraint forces caused by the wheels. In addition, a control approach
based on dynamic manipulability is proposed to avoid sideways slip in the
same paper. A control strategy to avoid wheel-slip is also presented in Date
et al. (2001). The latter approach uses an automatic generation of the joint
reference signals based on a velocity reference for the head of the snake
robot while keeping a winding shape of the robot. The results given by
Date et al. (2001) are extensions of the methods presented by Prautsch and
Mita (1999) where the snake robot tended to a singular (straight) posture
when the number of links was increased (Date et al., 2000).
Results on controllability of snake robots (and other locomotion systems) are given by Ostrowski and Burdick (1998) and Kelly and Murray
(1995).
McIsaac and Ostrowski (McIsaac and Ostrowski, 2000, 2003a,b) have
derived and implemented closed-loop heading control based on image-based
position feedback for an underwater snake robot, and they have presented
a controller for stopping the underwater snake robot during forward and
circular motion. The motion pattern for underwater locomotion is similar
to lateral undulation.
Ma et al. (2003a) and Ma and Tadokoro (2006) study lateral undulation
on an inclined surface. It is found that for the snake robot to be able to
move up the plane, the upper limit for the initial winding angle should
be decreased and the lower limit should be increased for an increasing inclination angle of the plane.
2.4 Snake Robot Locomotion
33
While most of the approaches to locomotion presented above rely on the
transversal/longitudinal friction property, others have explored alternative
ways to locomote with either the friction model being isotropic (ci;t =
ci;n ) or a purely kinematic model being used. Biological snakes exploit
obstacles such as rocks or contours on the ground to aid locomotion during
lateral undulation as mentioned in Section 2.2.3. This form of locomotion
has also been investigated for snake robots (Bayraktaroglu et al., 2006;
Bayraktaroglu and Blazevic, 2005; Hirose, 1993; Hirose and Umetani, 1976).
A pioneering work on such locomotion has been presented by Hirose and
Umetani (1976), where it is demonstrated how to utilize walls and large
cylinders to generate propulsive forces for a wheeled snake robot. Hirose
(1993) analyses the gliding con…guration of a snake negotiating a corner
within a maze. In addition, he presents a relationship between the angle of
the corner, the friction coe¢ cient between the snake and the walls of the
maze and the distance between the contact points for contact between the
snake and the maze. These analytical results can be employed to determine
how a snake robot should move within a similar maze. Shan and Koren
(1993) present an approach to locomotion where a snake robot is allowed
to come in contact with obstacles and it is discussed how a snake robot can
continue to be mobile while in contact with an obstacle. In this publication
the links of the snake robot presented have ball casters to slide on and
solenoids to push into the ground surface to increase friction when needed.
With this snake robot, the change in number of degrees of freedom of each
link while in contact with the obstacles is investigated and the obstacles
are used as constraints so that the snake robot is able to slide along them.
More recently, Bayraktaroglu and Blazevic (2005) have presented a wheelless snake robot that employs push-points, such as pegs, to create the total
propulsive force during a form of lateral undulation. Here, the ratio between
lateral and longitudinal friction forces is disregarded, and this makes it
possible to build and develop motion patterns for snake robots without the
friction property ci;t < ci;n and hence without wheels. The joints are not
bent to push against the pegs; instead, the snake robot in Bayraktaroglu
and Blazevic (2005) has linear actuators on each side of the links that
push out from the link. This work has been extended by Bayraktaroglu et
al. (2006) where the linear actuators are no longer necessary. Instead, the
wheel-less snake robot localizes push-points with on/o¤ switch sensors along
the sides of its body. The snake robot then calculates a spline between two
adjacent push-points and the joint angles are controlled so that its body
is …tted to the spline and thus uses the push-points to be able to move
34
Developments in Snake Robot Modelling and Locomotion
forward. It is assumed that there is always a push-point available within a
reasonable proximity of the head of the snake robot. Experimental results
are presented for this approach. The serpenoid curve has not been employed
directly in the papers by Bayraktaroglu et al. (Bayraktaroglu et al., 2006;
Bayraktaroglu and Blazevic, 2005) due to the change of shape according
to the placement of the push-points, however, the similarities to the shape
of a biological snake moving by lateral undulation while using contours on
the ground to push against are apparent.
It is important that a snake robot has sensors to detect contours on
the ground that can be pushed against for faster locomotion. In addition,
sensors can be used to make a map of the environment of the snake robot
on-line in order to …nd a feasible path. This is shown in Choset and Lee
(2001) and Henning et al. (1998). Moreover, sensor-based following of a
light source is shown in Worst and Linnemann (1996).
Alternative Approaches to Locomotion
Planar locomotion for a 2-link, 3-link and a multi-link system with 1 DOF
revolute joints (i.e. rotational joints) is studied by Chernousko (2005, 2000).
In those papers, dry friction forces between the links and the ground are
assumed. A combination of fast and slow movements of the joints is used to
move the 2- and 3-link system to any point in the plane. Forward motion of
the multi-link system is obtained by propagating a single wave consisting of
3 or 4 links from the tail to the front of the robot. Very small velocities and
accelerations are necessary for such locomotion which depends on the stickslip phases of dry friction. Subsequently, locomotion is obtained as long as
the moving part of the snake robot is not subjected to a friction force larger
than the non-moving part is able to counteract by lying still. This principle
is similar the concertina locomotion (see Section 2.2.3) except that the snake
robot does not anchor its body by pushing against something other than
the ‡at ground surface such as, for example, the walls of a narrow passage.
2.4.2
3D Snake Robot Locomotion
For snake robots with 2 DOF revolute joints and capable of 3D motion,
a second reference signal is needed to control the vertical wave (i.e. the
lifting and lowering of the links). The vertical joint reference signal can be
written as (Liljebäck et al., 2005):
vi ;d
= Av sin (! v t + (i
1)
v
+
0) ,
(2.15)
2.4 Snake Robot Locomotion
35
where vi ;d is the relative angle of joint i that controls the lift of the adjacent links. The phase di¤erence between the horizontal and vertical wave
is given by 0 and the remaining parameters Av , ! v and v give the amplitude, angular frequency and phase-shift similar to the parameters in (2.14).
The expressions (2.14)-(2.15) will be employed throughout this thesis to implement various motion patterns.
Sinus-lifting is also implemented for snake robots (Hirose and Mori,
2004; Ma et al., 2004; Mori and Hirose, 2002). A 2D model incorporating
a ground contact force as a function of the curvature of the snake body is
used by Ma et al. (2004). It is shown that the snake robot moves forward
faster by sinus-lifting than by lateral undulation. In addition, sinus-lifting
is a way of reducing the risk of side-slip of the wheels. A control method
for determining which links to lift based on a kinematic model has been
proposed by Ohmameuda and Ma (2002). A wheeled snake robot able to
move in 3D is presented by Ma et al. (2003b) and it is shown that for the
robot to be controllable and observable, it requires that 4
m
n 2
where m is the number of pairs of wheels touching the ground simultaneously. Sinus-lifting, like lateral undulation, is dependent on an anisotropic
friction property (ct;i < cn;i ) for e¢ cient locomotion. However, there are
also 3D motion patterns for moving with isotropic friction, and these will
be elaborated on in the following three paragraphs.
Sidewinding was implemented with ct;i = cn;i by Liljebäck (2004) using
(2.14) and (2.15) to …nd the desired joint angles, and it was found that
both h and 0 control the direction of locomotion. A slightly di¤erent
approach to sidewinding locomotion is developed by Burdick et al. (1993)
for 3D motion on a ‡at surface. In this latter work the snake robot lays
its stationary parts ‡at on the ground while the moving parts are curved
above the ground (see Section 2.2.3 for how sidewinding is performed). This
results in a more natural gait compared to a sinus-like curve for the shape of
a snake robot which is obtained by employing (2.14) and (2.15) to …nd the
desired joint angles. The use of such a sinus-like curve results in the fact that
the snake robot only touches the ground surface at isolated points when the
joint angles are accurately controlled. Thus, the snake robot can more easily
fall over since the contact surface with the ground is small. Sidewinding
has also been performed without an explicit mathematical expression for
the shape or joint angles such as e.g. (2.14)-(2.15). One such approach
to develop a form of sidewinding uses genetic programming (Tanev et al.,
2005). Hence, the joint movement of the snake robot is determined based on
some desired criteria. Moreover, the simulation results presented in Tanev
36
Developments in Snake Robot Modelling and Locomotion
et al. (2005) demonstrate that the snake robot manages to move sideways
despite several damaged joints. Such robustness to mechanical failure is
important since the snake robot consists of many joints and it is probable
that one or more of them may fail during challenging operations.
Inchworm-like and caterpillar-like motion patterns are shown with
(Chirikjian and Burdick, 1995) and without (Chirikjian and Burdick, 1995;
Poi et al., 1998; Rincon and Sotelo, 2003) extensible joints (see Figure 2.5
(b) for the latter case). These gaits have been modelled by only considering
the kinematics (and not the dynamics) of the snake robot. The model is
nevertheless valid since it is assumed that the snake robot is moving slowly.
While some results (Chirikjian and Burdick, 1995; Poi et al., 1998) rely on
slow speeds or ratchet wheels to avoid slipping, water can also be pumped
between the links to add weight to the parts that are not moving (Rincon and Sotelo, 2003). This way, the ground contact force and hence the
friction, on the moving parts are reduced.
Various kinds of rolling motion have also been proposed. These motion
patterns are not directly similar to movement of biological snakes. Lateral
rolling is a sub-group of these motion patterns, and one way of implementing lateral rolling consists of moving all the joints in phase while keeping
the snake robot in an U-shape on the ground as depicted in Figure 2.5 (a)
(Dowling, 1999). The reference joint angles for lateral rolling can be found
from (2.14) and (2.15) by setting Ah = Av , ! h = ! v , h = v and 0 = 2
(Ohno and Hirose, 2001). Experimental results for this gait are given by
e.g. Mori and Hirose (2002) and Hirose and Mori (2004). U-shaped lateral rolling is mainly performed with all links in contact with the ground
surface. However, 2 DOF (cardan) joints are still needed so the snake robot has to be capable of 3D motion (the robot can also be equipped with
alternating perpendicular 1 DOF joints instead of 2 DOF joints). Other
forms of lateral rolling are described in Chen et al. (2004). Another way
of moving proposed in the same paper is called the ‘smoke ring’according
to Dowling (1999) and is depicted in Figure 2.5 (e). The snake robot then
shapes its body into a ring and subsequently curls around a vertical or horizontal pipe-like structure in order to ‘roll’along the pipe. The snake robot
can also move like a wheel as shown in Figure 2.5 (c), but it is challenging
to keep the snake robot from falling over while moving. The shape of the
snake robot is described analytically for several motion patterns based on
rolling (or ‘twisting’) by Ye et al. (2004b). In the latter paper sinus-lifting
and other common serpentine motion patterns have been combined with
a twisting motion. Moreover, several of these motion patterns are tested
2.4 Snake Robot Locomotion
37
Figure 2.5: Various motion patterns implemented for snake robots: (a)
U-shaped lateral rolling (moving towards the right), (b) travelling wave/
caterpillar locomotion (moving the same way as the travelling wave, i.e.
left or right), (c) the wheel (moving right or left), (d) bridge mode (moving
right or left), and (e) ‘smoke ring’(moving up- or downward).
38
Developments in Snake Robot Modelling and Locomotion
by experiments and all tests show a mainly sideways motion of the snake
robot. Erkmen et al. (2002) have also elaborated on the twisting mode of
locomotion where an approach on how to make the snake robot walk using
its two ends as feet is described. This form of locomotion is called ‘bridge
mode’and is illustrated in Figure 2.5 (d). In addition, a genetic algorithm
is presented that handles how to move the snake robot from lying ‡at on the
ground to standing on its two ends without falling over. Descriptive …gures
and explanations of the several gaits mentioned above, in addition to other
motion patterns, are given by Dowling (1999), Ohno and Hirose (2001) and
Mori and Hirose (2002). Moreover, movies of various snake robots during
locomotion can be found on the web (Choset, 2007; Miller, 2007).
A snake robot, called the ACM-R5, with …ns with wheels on its link and
thus capable of both swimming and land-based locomotion is presented by
Yamada et al. (2005). On the ground the snake robot moves by lateral
rolling and lateral undulation. In the water it can move up and down,
swim forward and turn. The details on the motion patterns are said to be
reported at a later stage.
A snake robot needs to lift its body when, for example, climbing or
moving over high obstacles. However, the joint actuators can only produce
a limited torque. Hence, the snake robot links should be lifted in some
‘intelligent’manner to keep the necessary joint torques as low as possible.
To this end, Nilsson (1997b, 1998) has developed an algorithm for lifting
several links with limited joint torque. In addition, the snake robot may
utilize poles or other objects for climbing and a variant of the ‘smoke ring’
in Figure 2.5 (e) has been proposed by Nilsson (1997a).
2.5
Discussion and Summary
Research on snake robots has increased the past ten to …fteen years, but
there are still many challenges to face both on modelling and control of
snake robots in order to make them able to locomote intelligently through
unknown terrain. In this chapter we have seen a variety of approaches to
modelling and locomotion of snake robots and these will be summarized in
the following.
Since snake robots have a high number of degrees of freedom, careful
consideration has to be taken to decide what model to use for a snake robot
depending on which aspect of snake robot locomotion is being studied.
A planar and purely kinematic model is sometimes employed to describe
lateral undulation for a snake robot with passive caster wheels that do not
2.5 Discussion and Summary
39
slip. The use of a purely kinematic model simpli…es both the model and the
analysis of locomotion, and factors that contribute to locomotion are more
easily highlighted. 3D models of the kinematics of snake robots without
wheels are also developed and they are employed to study motion patterns
which also requires lifting and lowering (in addition to sideways motion) of
the snake robot links. The kinematic description of snake robot locomotion
without wheels is justi…ed by assuming low velocities and sometimes also
certain friction properties (such as zero friction while gliding forward, but
in…nite friction when pushed backward).
Although passive wheels help to justify the non-slip assumption (on the
wheels) of some kinematic models, it can still be di¢ cult to control the joint
torques such that the wheels do not slip. Moreover, locomotion of snake
robots at high velocities require that the friction between the underside
of the snake robot and the ground surface is considered. Hence, there is
a need for models that include the dynamics of snake robot locomotion.
2D dynamical models are mainly used to describe lateral undulation with
side-slip (i.e. without the idealized assumption of wheels that do not slip,
or with no wheels at all). The friction force between the snake robot and
the ground surface is described as viscous-like friction or Coulomb friction,
and Coulomb friction is usually modelled with sign-functions. However,
such a sign-function does not allow for a non-zero friction force at zero
velocity (the stick-phase). Hence, a snake robot lying on an inclined plane
will slide slowly downward. The directional friction property between the
belly of the snake robot can not always be realized due to varying ground
conditions. To this end, simulation results of a planar snake robot moving
by a promising scheme which consists of the snake robot pushing against
obstacles are presented, and the simulation results are obtained from the
dynamic simulation software WorkingModel. However, the contact between
the snake robot and the obstacles are described with a compliant model, and
it requires a sti¤ spring coe¢ cient to represent the contact between a (hard)
obstacle and a (hard) snake robot link. This results in sti¤ di¤erential
equations which are cumbersome to solve numerically.
Mathematical models that include the dynamics of motion yield a more
accurate description of the behaviour of the system. This is advantageous
with respect to simulation, but the models may get very large and unmanageable. Thus, the simplicity of an analytical analysis su¤ers. In 2D
models, certain properties of the system help to simplify the model, but
not all of these properties persist in 3D models. However, moving in, for
example, a shattered building is an inherently 3D experience and thus 3D
40
Developments in Snake Robot Modelling and Locomotion
dynamical models are necessary to simulate such motion. The complexity
of the models grows a lot from 2D to 3D, and few 3D models have been
presented in the published literature. For the presented models, the contact force between a snake robot and the ground surface is modelled using
a linear spring-damper model. This is a very intuitive approach and easy to
implement. However, the large spring coe¢ cient needed to resemble a hard
ground surface results in a sti¤ mathematical model which requires small
integration steps to avoid instability in the numerical integration. One of
the 3D models have been simpli…ed to describe planar movement with a
varying ground contact force to study sinus-lifting.
By going through the published literature, we see that there are several
aspects of snake robot locomotion. The main concern is to …nd some kind
of (often repetitive) reference for the joint angles such that the snake robot moves in some direction. A large variety of preprogrammed reference
signals are proposed resulting in di¤erent motion patterns. The preprogrammed motion patterns can be improved and extended by incorporating
feedback from external measurements of, for example, velocity, heading,
and/or position of the snake robot in the generation of the joint reference
angles. To some extent, such results are presented in the published literature. Others focus on …nding optimal preprogrammed motion patterns
with respect to energy consumption and distance travelled. Some snake
robots are equipped with sensors and the motion pattern is then generated
based on continuously updated measurements from the sensors. This has
been achieved for a snake robot that moves forward by sensing and pushing
against external objects in an experimental setup for planar motion. A
large variety of 3D motion patterns are implemented for real snake robots
and some of these motion patterns are based on rolling. To simulate such
motion, some sort of exoskeleton is needed for contact with the ground
surface. However, the 3D dynamical models presented only de…ne the contact as a single point for each snake robot link, and thus this renders such
motion impossible.
The above approaches are important contributions to making snake
robots navigate by themselves in an unknown environment. However, there
are still numerous challenges to face before a snake robot can be employed
to be able to search e¢ ciently for people trapped in a collapsed building
or aid …re-…ghters on a dangerous mission. This motivates further research
on snake robots.
Chapter 3
Non-smooth Model of a 2D
Snake Robot for Simulation
3.1
Introduction
The fastest biological snakes exploit roughness in the terrain for locomotion.
They may push against rocks, branches, or other obstacles to move forward
more e¢ ciently (Gray, 1946; Hirose, 1993). Snakes can also exploit the walls
of narrow passages for locomotion. Their mobility properties are interesting
for snake robot locomotion and give the motivation for investigating snake
robot obstacle aided locomotion. We de…ne obstacle aided locomotion as
snake robot locomotion where snake robots utilize walls or other external
objects, apart from the ‡at ground, for propulsion. In order to develop
such an obstacle aided locomotion scheme, we need a mathematical model
that includes the interaction between the snake robot and a terrain with
external objects for the snake robot to push against.
In this chapter we present a non-smooth (hybrid) modelling approach
particularly suitable for modelling snake robot motion in general and obstacle aided locomotion in particular. We use this approach to develop a 2D
mathematical model of a snake robot that can push against external objects
for locomotion. The mathematical model is described in the framework of
non-smooth dynamics and convex analysis (Brogliato, 1999; Glocker, 2001;
Leine and Nijmeijer, 2004). The tools available in this framework that are
necessary for the mathematical modelling of a snake robot will be introduced and explained when necessary.
This chapter is based on Transeth et al. (2007b) and Transeth et al.
(2008b) and it is organized as follows. Section 3.2 gives a short introduc-
42
Non-smooth Model of a 2D Snake Robot for Simulation
Figure 3.1: Planar snake robot with 11 links in an environment with obstacles that it may push against to move forward (the snake robot is moving
towards the right in the illustration).
tion and motivation for the derivation of the mathematical model. The
kinematics of the snake robot with obstacles is given in Section 3.3. Section 3.4 lays the foundation for …nding the obstacle contact and ground
friction forces. Section 3.5 describes the non-smooth dynamics of the snake
robot, while Section 3.6 outlines the numerical treatment of the model. A
summary of the most important results presented in this chapter is given
in Section 3.7.
3.2
Summary of the Mathematical Model
This section contains a brief outline of how to derive a non-smooth mathematical model of a snake robot during planar motion. This preliminary
section is meant to motivate and ease the understanding of the forthcoming
deduction of the system equations.
The 2D model of the snake robot consists of n links connected by n 1
one degree of freedom (DOF) rotational joints. An illustration of the planar
snake robot in an environment with external circular objects (later referred
to as obstacles) is given in Figure 3.1. Let u 2 R3n be a vector containing the translational and rotational velocities of all the links of the snake
robot (the structure of the snake robot together with the coordinates and
reference frames are described further in Section 3.3). Let the di¤erential
measures du and dt be loosely described for now as a ‘possible di¤erential
change’in u and the time t, respectively, while a more precise de…nition is
given in Section 3.5.1. The use of di¤erential measures allows for instantaneous changes in velocities which occur for impacts with obstacles. The
system equations for the snake robot can now be written as
Mdu
hdt
dR = dt;
(3.1)
3.2 Summary of the Mathematical Model
43
where (3.1) is termed the equality of measures (Moreau, 1988). Here M 2
R3n 3n is the mass matrix, 2 R3n contains all the joint actuator torques,
h 2 R3n represents all the smooth forces (for example gravity, note that
h = 03n 1 for the 2D model) and dR 2 R3n accounts for the normal contact
forces/impulses from the obstacles, the Coulomb friction forces/impulses
and the bilateral constraint forces/impulses in the joints. Note: We allow
in this chapter for instantaneous changes in velocities usually associated
with collisions. Hence, the (normal contact/friction/constraint) ‘forces’
are not always de…ned due to the in…nite accelerations. In these cases
we have impulses instead of forces. The non-smooth equality of measures
(3.1) allows us to formulate in a uniform manner both the smooth and
non-smooth phases of motion. This is achieved partly by representing the
contact forces/impulses as contact impulse measures (these are included by
dR in the equality of measures).
A substantial part of the beginning of this chapter is devoted to deducting the force measure dR. Hence, let us brie‡y look at how to derive the
contribution of the normal contact impulse measure between an obstacle
j and the …rst link, in dR. Let dR1 2 R3 be the sum of contact impulse
measures that directly a¤ect link 1 (i.e. the …rst three elements of dR).
Then
friction and joint constraint
;
(3.2)
dR1 = wH dPH +
impulse measures
where dPH 2 R is the normal contact impulse measure from obstacle j
on link 1 and wH 2 R3 is the corresponding generalized force direction
(subscripts ‘j’and ‘1’are omitted for brevity).
Let gH 2 R be a function giving the shortest distance between link 1 and
the obstacle. Such a function is called a gap function (Glocker and Studer,
2005). The gap function is the starting point for the systematic approach of
…nding the impulse measures. The link and obstacle are separated if gH > 0,
are in contact if gH = 0 and are penetrating each other if gH < 0. Hence,
the relative velocity between link 1 and the obstacle along the shortest line
between the two objects can be de…ned as H := g_ H , and it is written, by
calculating g_ H , on the form
H
= wT
H u1 ;
(3.3)
where u1 is the velocity of link 1. Hence, by inspecting the expression for
g_ H and formulating it as (3.3) we …nd an expression for wH in (3.2). The
normal contact impulse measure dPH is related to the relative velocity H
by employing a set-valued force law (see Section 3.5). The set-valuedness
44
Non-smooth Model of a 2D Snake Robot for Simulation
of the force laws allows us to write each constitutive law (force law) with
a single equation and avoid explicit switching between equations (for example when a collision occurs). In addition, this formulation provides an
accurate description of the planar Coulomb friction (see Section 3.5.2). In
the following three sections, we will elaborate on how to derive the elements
that constitute (3.1), that is the various gap functions, relative velocities
and …nally the forces/impulses included in the equality of measures.
3.3
Kinematics
In this section we present the kinematics of the snake robot in an environment with obstacles. From the kinematics, we develop gap functions both
for obstacle contact detection and for conforming to the bilateral joint constraints. The gap functions are later used as a basis for calculating the
normal contact forces and the joint constraint forces.
First, we give an overview of the coordinates used to describe the position and orientation of the snake robot. The coordinates are chosen such
that the mass matrix becomes constant. This is advantageous for the numerical treatment. Subsequently, the gap functions for describing the distance between the snake robot and the obstacles, and the ‘gaps’ in the
joints, are found.
3.3.1
Snake Robot Description and Reference Frames
The model of a planar snake robot consists of n equal links connected by
n 1 one degree of freedom rotational joints. A snake robot with 3 links
is depicted in Figure 3.2. Link i is shaped as a rectangle of length 2LGSi
and width 2LSCi . It is assumed that there is a massless semicircle of radius
LSCi connected to the ends of each link. The semicircles together with the
straight sides de…ne the surface (or contour) of a link used for contact with
the external objects. The distance between two adjacent joints is Li .
Let the inertial reference frame be approximated by an earth-…xed frame
I = O; eIx ; eIy with the origin O attached to the ground surface, and
where the plane eIx ; eIy constitutes the horizontal ground surface which
the snake robot can move on. A general notation used throughout this
chapter is that a vector from the origin of frame I to a point A is given
by r A 2 R2 and a vector from point A to point B is written r AB . Let a
vector r A described in frame I be written as I r A . Denote the point Gi as
the centre of gravity (CG) of link i where the CG is assumed to be in the
3.3 Kinematics
45
Figure 3.2: Snake robot with three links. The positions of the joints are
given by the two small circles in the rightmost drawing.
Bi ,
i
middle of the rectangle. Denote the body-…xed frame Bi = Gi; eB
x ; ey
i
where the origin is at the point Gi , the axis eB
x is pointing along the centre
B
line of link i towards link i + 1 and ey i is pointing transversal to the link
following the right-hand convention. The centre point of the front and
rear sphere which constitutes the ends of link i is denoted SF i and SRi ,
respectively.
The earth-…xed position and orientation of link i is
qi =
I r Gi
;
(3.4)
i
where I r Gi 2 R2 is the position of the CG of link i and i 2 S 1 is the angle
i
between the axes eIx and eB
x . The translational and rotational velocities of
link i are
v
ui = I Gi ;
(3.5)
!i
where I v Gi := I r_ Gi and ! i := _ i when they exist (i.e. for impact free
motion). All the positions and orientations, and velocities, are gathered in
the two vectors
2 3
2 3
q1
u1
6q 7
6 u2 7
6 27
6 7
q = 6 . 7 and u = 6 . 7 :
(3.6)
4 .. 5
4 .. 5
qn
un
The transformation of a vector r between reference frames is given by
46
Non-smooth Model of a 2D Snake Robot for Simulation
Figure 3.3: Snake robot with three links depicted with obstacle j = 1 closest
to link i = 3.
Ir
= RIBi
Bi r
where the rotation matrix is
RIBi =
cos
sin
i
i
sin i
.
cos i
(3.7)
External …xed objects are included in the model for the snake robot to
push against. Each obstacle j, j = 1; 2; : : : ; , is shaped as a circle in the
plane with radius LHj and midpoint Hj , see Figure 3.3. The gap function
gHij in the …gure is described in the following.
3.3.2
Gap Functions for Obstacle Contact
We begin this section by giving an example of what a gap function is used
for, then we introduce the gap functions for contact between the snake
robot and the obstacles.
Example 3.1 (Gap Function) Consider two convex rigid bodies 1 and 2.
Now, we want to determine whether Body 1 is in contact with Body 2. This
can be achieved by determining whether the minimal distance between the
two bodies is equal to zero. The function that gives us the minimal distance
is called a gap function (Glocker and Studer, 2005) and the point on Body
1 that is closest to Body 2 is called a (possible) contact point. Apart from
determining if two rigid bodies are in contact, the gap function will also be
used as a basis for calculating the direction and size of the forces involved
in the contact.
3.3 Kinematics
47
The gap function gHij gives the shortest distance between link i and
obstacle j as illustrated in Figure 3.3 and is used to detect whether the two
bodies are in contact. In addition, this gap function will be used later as a
basis for calculating the forces involved in the contact.
Link i can, at any time-instance, only touch a convex obstacle j at a
single point, resulting in a contact point that may move across the entire
surface of a link. The surface (or contour) of a link consists of three parts:
the rectangle, the front semicircle and the rear semicircle. Hence, we see
that we need to …nd two separate kinds of gap functions for each link
depending on which part of the link is closest to the obstacle. In particular,
we need to …nd the shortest distance between a rectangle and a circle, and
between two circles. This approach provides us with a moving contact
point. Hence, the obstacle can come in contact with the snake robot at any
point and the e¤ect the contact force has on a snake robot link is therefore
modelled accurately.
Distance Between Rectangle Part and Obstacle
When the rectangle-shaped part of link i is closest to obstacle j, we need
i
to investigate the distance between a line (the eB
x -axis) and the centre of
i
the obstacle Hj . The shortest distance between the eB
x -axis and Hj is
dij = (I r Gi
T
Bi
I r Hj ) I ey .
(3.8)
Hence, the gap function when the rectangle part of link i is closest to
obstacle j is
(3.9)
LHj + LSCi .
gHij = jdij j
Distance Between Circle Part and Obstacle
The minimal distance between the circle part of link i and obstacle j is
found from the gap function
gHij =
I r Hj Si
LHj + LSCi
(3.10)
where
r Hj Si
= r Gi + r Gi SF i
r Hj or
(3.11)
r Hj Si
= r Gi + r Gi SRi
r Hj
(3.12)
depending on whether the front or rear part of the link is closest to the
i
i
obstacle, respectively. In addition, r Gi SF i = LGSi eB
LGSi eB
x , r Gi SRi =
x
and we de…ne k k := k k2 .
48
Non-smooth Model of a 2D Snake Robot for Simulation
Figure 3.4: Illustration of how the joint gap function gJi is found. We see
that for the snake (robot with 3 links) in the picture, the vectors are drawn
for i = 2.
A Vector of All Obstacle Gap functions
We now gather the gap functions gHij for the combination of all n links and
obstacles in the vector:
g H = gH11
gHn1
gH12
gHn2
gH1
gHn
T
; (3.13)
where gHij is found from either (3.9) or (3.10) depending on which part of
link i is closest to obstacle j.
3.3.3
Bilateral Constraints: Joints
Each joint introduces two bilateral constraints in the model. These constraints keep the ‘gap’ in a joint equal to zero. An expression for this
‘gap’, a gap function, needs to be found in order to calculate the constraint forces. To …nd the gap function, we need to relate the position of
joint i between link i and i + 1 to both adjacent links. The position of
i
joint i going via link i and i + 1 is given by I r JF i = I r Gi + 12 Li I eB
x and
Bi+1
1
, respectively. Figure 3.4 depicts how these
I r JRi+1 = I r Gi+1
2 Li+1 I ex
position vectors are found. Now, the gap functions for the gaps in the joints
are
gJ i =
for
= x; y and i = 1; : : : ; n
I T
Ie
I r JF i
1.
I r JRi+1
;
(3.14)
3.4 Contact Constraints on Velocity Level
3.4
49
Contact Constraints on Velocity Level
In this section the relative velocities between the snake robot, and the
obstacles and the ground, are found by taking the time-derivative (when
they exist) of the corresponding gap functions. The relative velocities are
employed to calculate the normal contact forces involved in the contact
between the snake robot and the obstacles (Le Saux et al., 2005; Leine
and Nijmeijer, 2004). Also, the relative velocities are employed to …nd the
friction forces between the ground and the snake robot, and the bilateral
constraint forces in the joints. This is shown in Section 3.5.2.
3.4.1
Relative Velocity Between an Obstacle and a Link
The relative velocity between a link i and an obstacle j is de…ned as
Hij := g_ Hij (when the time-derivative exists). As for the gap functions,
the expression for the relative velocity depends on which part of the link is
closest to the obstacle.
Rectangle Part Closest to Obstacle
If the rectangle-shaped part of link i is closest to obstacle j, then the
relative velocity between these two convex objects Hij := g_ Hij is found, by
employing (3.9), as
=
Hij
d
jdij j =)
dt
Hij
= wT
Hij ui ;
(3.15)
where dij is given in (3.8) and the generalized force direction is
wT
Hij = sign (dij )
Bi T
I ey
I r Hj
I r Gi
T
cos ( i )
sin ( i )
ui ;
(3.16)
for i = 1; : : : ; n and j = 1; : : : ; .
Circle Part Closest to Obstacle
The gap function (3.10) is employed to de…ne the relative velocity Hij :=
g_ Hij when one of the circle-shaped parts of link i is closest to obstacle j.
By de…nition, the relative velocity is
Hij
=
d
dt
I r Hj Si
=)
Hij
= wT
Hij ui ;
(3.17)
50
Non-smooth Model of a 2D Snake Robot for Simulation
where the generalized force direction is, for i = 1; : : : ; n and j = 1; : : : ; ,
wT
Hij =
D=
I r Hj Si
I2
I r Hj Si
0
1
1
;
0
RIBi D Bi r Gi Si ;
2
I2
2
=
(3.18)
1 0
;
0 1
(3.19)
and I r Hj Si is given by (3.11) or (3.12) and r Gi Si is equal to either Bi r Gi SF i
or Bi r Gi SRi depending on whether the front (F i) or rear (Ri) part of the
link is closest to the obstacle as described in Section 3.3.2. To …nd the
expression for wT
Hij in (3.18) we have employed the following identities:
d
dt
I r Hj Si
=
I r Hj Si
d I
R
dt Bi
I r Hj Si
d
I r Hj Si ;
dt
(3.20)
= RIBi D:
(3.21)
Notice that we have employed the same notation for wHij in (3.16) and
(3.18) to reduce the number of subscripts.
Vector of All Relative Velocities for Obstacles
We gather the relative velocities Hij for all n links and
vector
T
n
H = WH u 2 R ;
obstacles in the
(3.22)
where
H
=
T
H11
Hn1
H12
Hn2
WH = WH1
and
WHj
2
wH1j
6
6 03 1
=6
6 ..
4 .
03 1
03
H1
WH
03
..
.
1
wH2j
..
.
..
.
..
.
03
2 R3n
1
1
3
Hn
n
;
7
7
7 2 R3n
7
03 1 5
wHnj
; (3.23)
(3.24)
n
.
(3.25)
Depending on which part of the link is closest to the obstacle, wHij is
obtained either from (3.16) or (3.18).
3.4 Contact Constraints on Velocity Level
3.4.2
51
Tangential Relative Velocity
The tangential relative velocities between a link i and the ground is presented here. For the case of the planar snake robot, these velocities are
the velocities of the centre of gravity Gi for each link with respect to the
earth-…xed coordinate axis eIx ; eIy . The relative velocities will later be
employed to …nd the friction forces between the link and the ground. We
start by …nding the tangential relative velocities parallel to the eIx and eIy axis. These are denoted for link i by 0Tix and 0Tiy , respectively. However,
in order to easily …nd the friction forces longitudinal and lateral to each
link, we need to derive the tangential relative velocities longitudinal Tix
and lateral Tiy to each link.
The tangential relative velocities parallel to the eIx and eIy -axis are
0
Ti
=
I T
Ie
I v Gi
0
Ti
= w0Ti
I T
Ie
i
0 ;
=)
T
ui ;
(3.26)
where the generalized force direction is
w0Ti
T
=
h
(3.27)
for = x; y and i = 1; : : : ; n. The two relative velocities are combined in
one vector so that
0
0 T
ui ;
(3.28)
Ti = WTi
where
0
Ti
=
0
Tix
0
Tiy
2 R2 ;
h
WT0 i = w0Tix
i
w0Tiy 2 R3
2
:
(3.29)
Bi
i
The tangential relative velocities along the eB
x and ey -axis can now
be found as follows. De…ne
Ti
=
Tix
:
(3.30)
Tiy
Then, by employing the rotation matrix RIBi in (3.7), we have that
0
I
Ti = RBi Ti . Hence,
T
(3.31)
Ti = WTi ui ;
where
WTi = WT0 i RIBi ;
for i = 1; : : : ; n.
(3.32)
52
Non-smooth Model of a 2D Snake Robot for Simulation
We gather all the tangential relative velocities
T
Ti
as
= WTT u;
(3.33)
where
3
2
03 2
WT1 03 2
6
.. 7
T1
6 03 2 . . .
. 7
6 .. 7
2n
7 2 R3n
6
T = 4 . 5 2 R ; WT = 6 .
7
.
.. 0
4 ..
3 25
Tn
03 2
03 2 WTn
2
3
2n
; (3.34)
and WT i is found from (3.32).
3.4.3
Bilateral Constraints: Joints
We need the relative velocities of the ‘gaps’in the joints to …nd the forces
involved in the bilateral constraint forces acting on the links and imposed
by the joints. The bilateral constraint forces ensure that adjacent links
only moves relative to each other in ways allowed by the 1 DOF joint that
connects the links. The relative joint velocities for joint i along the eIx and eIy -axis are de…ned as Jix := g_ Jix and Jiy := g_ Jiy (when the timederivatives exist). Hence, by employing (3.14) we …nd that
Ji
for
= wT
Ji
ui
;
ui+1
= x; y and i = 1; : : : ; n 1, where
h
Li
I T
I T
wT
=
Jix
I ex
I ex
2 sin ( i )
h
Li
I T
I T
wT
=
I ey
I ey
Jiy
2 cos ( i )
By de…ning
ties as
where
J
=
h
Ji
T
=
Jix
T
J1
T
Jn
2
WJT1
602
6
WJT =6 ..
4 .
02
and WJi = wJix
3
3
Jiy
1
iT
J
02
i
)
i+1 ;
i
cos ( i+1 ) .
Li+1
2
Li+1
2
sin (
(3.36)
(3.37)
, we gather all the relative joint veloci-
= WJT u;
2 R2(n
02
WJT2
(3.35)
1) ,
02
02
3
..
3
(3.38)
.
3
7
72 R3n
02 3 5
WJTn
wJiy for i = 1; : : : ; n
3
1
1.
37
2(n 1)
;
(3.39)
3.5 Non-smooth Dynamics
3.5
53
Non-smooth Dynamics
The starting point for describing the dynamics of the snake robot is the
equality of measures as introduced in Moreau (1988). The equality of measures includes both the equations of motion for impact free motion and the
impact equations. The impact equations give rise to impulsive behaviour
(Glocker and Studer, 2005). In this section we employ all the results from
Section 3.3 and Section 3.4 to …nd the various components of the equality
of measures.
3.5.1
The Equality of Measures
The equality of measures describes the dynamics of the snake robot within
the context of non-smooth dynamics. The non-smoothness allows for instantaneous velocity jumps, usually associated with impacts. The velocity
u(t) is assumed to admit a right u+ and left u limit for all t in the
(short) time-interval It = [tA ; tE ] (Moreau, 1988) and its time-derivative u_
exists for almost all t 2 It . The di¤erential measure du is assumed to be
decomposed into two parts to be able to obtain u from integration:
_ + u+
du = udt
u
d ;
(3.40)
where dt
measure and d denotes the atomic measure
R denotes the Lebesgue
+
where ti d = 1 if u (ti ) u (ti ) 6= 0. The total increment of u over a
compact sub-interval [t1 ; t2 ] of the time-interval It is
Z
du = u+ (t2 ) u (t1 )
(3.41)
[t1 ;t2 ]
and is due to a continuous change (i.e. impact free motion) stemming from
u_ as well as possible discontinuities in u within the time-interval [t1 ; t2 ].
Equation (3.41) is even valid when the time-interval [t1 ; t2 ] reduces to a
singleton ft1 g (i.e. t2 = t1 ):
Z
du = u+ (t2 ) u (t1 );
(3.42)
ft1 g
due to the atomic measure d . Hence, if a velocity jump occurs for t = t1
then (3.41) gives a non-zero result. Note that if the obstacles are removed from the model (thus, no impacts will occur), then we have u+ (t) =
_
u (t) 8t, and (3.40) becomes du = udt.
54
Non-smooth Model of a 2D Snake Robot for Simulation
The Newton-Euler equations as an equality of measures can be written
in a general form as
M (q; t) du
h (q; u; t) dt
dR = dt;
(3.43)
where M (q; t) is the mass matrix, h (q; u; t) is the vector of smooth forces,
dR is the force measure of possibly atomic impact impulsions and
is
the vector of applied torques. For the planar snake robot, the equality of
measures reduces to
Mdu dR = dt;
(3.44)
where dR and
will be described in the following and we note that
h (q; u; t) = 03n . The mass matrix is
2
3
M1 03 3
03 3
6
.. 7
603 3 . . .
. 7
6
7 2 R3n 3n ;
M (q; t) = M = 6 .
(3.45)
7
..
4 ..
. 03 3 5
03 3
03 3 Mn
where
2
mi 0
4
Mi = 0 mi
0
0
3
0
0 5 2 R3
3
;
i = 1; : : : n;
(3.46)
i
and mi 2 R and
i 2 R are the mass and moment of inertia of link i,
respectively. Hence, the mass matrix M is diagonal and constant.
The force measure dR accounts for all contact forces and impulses. Let
H be the set of all active contacts with the obstacles
H (t) = fbjgHb (q (t)) = 0g
f1; 2; : : : ; n g ;
(3.47)
where gHb is the b-th element of the vector g H in (3.13). The force measure
can now be written as
X
dR = WT dP T + WJ dP J +
(WH )b dPHb ;
(3.48)
b2H
where dPHb is the normal contact impulse measure between a link and an
obstacle, dP T is the tangential contact impulse measure (the friction) between the ground and the links and dP J is the contact impulse measures
due to the bilateral constraints in the joints. The order of the elements
in the vectors dP T and dP J corresponds to the vectors of relative velocities (3.33) and (3.38), respectively. The notation (WH )b denotes the b-th
3.5 Non-smooth Dynamics
55
Figure 3.5: Control torques on link i.
column of the WH -matrix in (3.24). The contact impulse measures are,
similarly to the velocity measure, decomposed into two parts. One part
consists of the Lebesgue-measurable force while the other part is a purely
atomic impact impulse. This is written for dPHb as
dPHb =
Hb dt
+
Hb d
;
(3.49)
where Hb is the Lebesgue-measurable force due to a continuous contact
between a link and an obstacle, and Hb is a purely atomic impact impulse
caused by a collision between the two objects. The same decomposition
can be performed for the other contact impulse measures. The Lebesgue
measurable force and the purely atomic impact impulse for the normal
contact with the obstacles and the friction are found from the force laws
given in Section 3.5.2.
The n 1 joint actuators are modelled as applied torques. The total
torque applied to link i (and thus every third element of the vector ) is
3i
=
Ci
Ci
1
;
(3.50)
for i = 1; : : : ; n where Ci 2 R is the torque applied to joint i and C0 =
Cn = 0 (there are no actuators at the two ends of the snake robot). The
torques applied to link i are depicted in Figure 3.5. The desired torques
are found with a PD-controller:
Ci
=
kP
hi ;d
kD _ hi ;d
hi
! Ji ;
(3.51)
where kP 2 R+ ; kD 2 R+ are positive constants, the relative joint angle
and angular velocity are
hi
! Ji
=
i+1
= ! i+1
hi
i;
(3.52)
!i;
(3.53)
56
Non-smooth Model of a 2D Snake Robot for Simulation
respectively, and hi ;d is the desired joint angle for joint i for i = 1; : : : ; n 1.
The joint torques are included in the equality of measures (3.44) through
the vector
= 01
3.5.2
2
3
01
2
01
6
T
2
3n
2 R3n :
(3.54)
Constitutive Laws for Contact Forces
In this section we will introduce set-valued force laws for normal contact
with the obstacles and for Coulomb friction. The set-valuedness of the force
laws allows us to write each constitutive law with a single equation thus
avoiding explicit switching between equations in the numerical treatment.
These laws will all be formulated on velocity level using the relative contact
velocities given by (3.22) and (3.33). Subsequently, the set-valued force
laws are formulated as equalities in the end of this section using the socalled ‘proximal point function’ in order to include the force laws in the
numerical simulation (Leine and Nijmeijer, 2004).
Normal Contact Force
The normal contact between a link and an obstacle is described by the
unilateral constraint
gH
0;
H
0;
gH
H
= 0;
(3.55)
which is known as Signorini’s law (Leine and Nijmeijer, 2004). Here, H is
the normal contact force and gH is the gap function. The subscripts i and
j are temporarily removed for brevity. This set-valued force law states that
the contact is impenetrable, gH 0, the contact can only transmit pressure
forces H 0 and the contact force H does not produce work gH H = 0.
The force law can be expressed on di¤erent kinematic levels: displacement
level (3.55), velocity level and acceleration level. In the following we express
all force laws for a closed contact (i.e. when two objects are in contact)
on velocity level, while all forces vanish for open contacts (i.e. when two
objects are not in contact). Then, by employing concepts of convex analysis,
the relationship between the relative velocity and the Lebesgue measurable
normal contact force (not an impulse) may be written for a closed contact
gH = 0 as an inclusion on velocity level
H
where
CH = f
H
H
2 N CH (
H );
(3.56)
2 R the relative velocity in (3.15) or (3.17), the convex set
j H 0g = R+ is the set of admissible contact forces and NCH
3.5 Non-smooth Dynamics
57
Figure 3.6: (a) Three points in and on the boundary of the set C, and (b)
the normal cones to the set C at the points (Illustration inspired by Leine
and Nijmeijer (2004)).
is the normal cone to the set CH (Leine and Nijmeijer, 2004). The concept
of normal cones will be illustrated shortly by two examples. For the time
being, we note that the inclusion (3.56) is equivalent to the condition
H
0;
H
0;
H H
= 0;
(3.57)
for a closed contact gH = 0. The force law (3.56) describes the impenetrability of sustained contact, i.e. gH = 0 and H = 0, as well as detachment:
H > 0 ) H = 0. However, (3.56) does not cover impacts (where we
have impulses instead of forces). For impacts we need a similar impact law
described at the end of this subsection. We formalize the above comment
on impenetrability and detachment by giving the following two examples:
Example 3.2 (Normal cone) From the de…nition of a normal cone
NC (x) to a convex set C at the point x 2 Rn (Leine and Nijmeijer,
2004; Rockafellar, 1970), we have that NC (x) = f0g for x 2 int C, and
NC (x) = f;g for x 2
= C. If x is on the boundary of C, then NC (x) is,
somewhat loosely described, the set of all vectors y 2 Rn that are normal
to x 2 C. See Figure 3.6 for an illustration of various normal cones.
Example 3.3 (Normal cone) The boundary of the set CH = R+ in (3.56)
is the single point H = 0, and the interior of CH is R+ f0g. Hence, we
have NCH (0) = R
f0g and NCH (2) = f;g.
The force law (3.56) only covers …nite-valued contact e¤orts during impulse free motion, i.e. all velocities are absolutely continuous in time. When
58
Non-smooth Model of a 2D Snake Robot for Simulation
a collision occurs in a rigid-body setting, then the velocities will be locally
discontinuous in order to prevent penetration. The velocity jump is accompanied by an impact impulse H , for which we will set up an impact law.
The relative velocity admits, similarly to the velocities u, a right +
H and a
left H limit. The impact law for a completely inelastic impact at a closed
contact can now be written as
+
H
2 N CH (
H );
CH = f
H
j
H
+
H
H
0g = R+ ;
(3.58)
which is equivalent to the condition
+
H
0;
H
0;
= 0:
(3.59)
Notice that the force law (3.56) and the impact law (3.58) is on the same
form. We have earlier stated that there is no need for an explicit switch
between equations when e.g. an impact occurs in the numerical treatment
of the model. This becomes evident in Section 3.6 with the introduction
of contact impulsions (which include both forces and possible impulses) for
the discretization of the system dynamics.
Coulomb Friction Force
Similarly to the force law (3.56) for normal contact, we describe the constitutive description for friction using an inclusion on a normal cone. The
friction force T 2 R2 between the ground and a link (we omit subscript
‘i’for brevity), in the two-dimensional tangent plane to the contact point,
is modelled with a Coulomb friction law
T
2 N CT (
T );
(3.60)
where T 2 R2 is the relative sliding velocity for link i given in (3.31), CT
is the (convex) set of admissible friction forces and NCT is the normal cone
to the set CT (see Figure 3.7 for an illustration of the force law). In the following, we show how to describe isotropic and anisotropic spatial Coulomb
friction with (3.60) by changing the set CT . First, isotropic friction will be
elaborated on. Then we extend the force law (3.60) to describe a particular
form of anisotropic friction, called orthotropic friction. However, before we
begin, consider the following example that describes the force law (3.60):
Example 3.4 Please refer to Figure 3.7 (a) and Example 3.2, and observe
that the force law (3.60) distinguishes between two cases: if the friction
force is in the interior of the set CT1 , or if it is on the boundary of the set.
3.5 Non-smooth Dynamics
59
Figure 3.7: Relationship between tangential relative velocity and friction
force for (a) isotropic friction with the set CT1 (in grey) given by (3.61) and
(b) anisotropic friction where CT2 is found from (3.63).
If T 2 int CT1 , then it holds that NCT1 ( T ) = 0 from which we conclude
that T = 0. Obviously, this corresponds to the stick phase of the friction
law. If the friction force is on the boundary of CT1 , then the normal cone
NCT1 ( T ) consists of the outward normal ray on the circle CT1 at the point
T . This corresponds to that the link is sliding.
Isotropic friction indicates that the friction forces are not dependent on
the orientation of the snake robot links while sliding. We employ the force
law (3.60) to describe isotropic friction by choosing the convex set CT as
CT1 = f
T
jk
Tk
Ng ;
T
(3.61)
where T > 0 is the friction coe¢ cient and N = mg. The resulting
relationship between the tangential relative velocities and the friction forces
is illustrated in Figure 3.7 (a) where a possible resulting friction force for
T = 0 is included. The set-valued force law (3.60) with (3.61) contains
the cases of stick and slip:
stick :
slip :
T
T
= 0; k T k
6= 0;
T =
T
T
N;
Nk
T
Tk
:
(3.62)
The advantage of formulating the friction law as the inclusion (3.60)
now becomes apparent. A spatial friction law such as (3.60), which is
equivalent to (3.62) for CT = CT1 , can not properly be described by a
set-valued sign-function. Some authors model the spatial contact with two
sign-functions for the two components of the relative sliding velocity using
60
Non-smooth Model of a 2D Snake Robot for Simulation
two friction coe¢ cients Tx and Ty (Ma et al., 2003a; Saito et al., 2002).
This results however directly in an anisotropic friction law, as the friction force and the sliding velocity do no longer point in opposite directions
(even for Tx = Ty ). Then such a double-sign-function force law corresponds to (3.60) with CT being a rectangle with length 2 Tx N and width
2 Ty N . Also, the (set-valued) sign function can be approximated with a
smooth function, for example some arctangent function. This results in a
very steep slope of the friction curve near zero relative velocity. Such an
approach is very cumbersome for two reasons. First of all, stiction can not
properly be described: an object on a slope will with a smoothened friction
law always slide. Secondly, the very steep slope of the friction curve causes
the di¤erential equations of motion to become numerically sti¤. Summarising, we see that (3.60) or (3.62) describes spatial Coulomb friction taking
isotropy and stiction properly into account. We prefer using (3.60) instead
of (3.62), because the latter becomes not well conditioned for very small
T when used in numerics. Note also that the force laws for Coulomb
friction (3.60) and normal contact with the obstacles (3.56) have the same
mathematical form. Moreover, the inclusion (3.60) is much more general
since we can easily change the convex set CT to get a di¤erent (and hence
anisotropic) friction model.
Orthotropic friction (which is a particular form of anisotropic friction)
can be described by the force law (3.60) with the set CT being the equal to
the set
8
9
>
>
2
<
=
2
Ty
Tx
CT2 =
j
+
1
;
(3.63)
T
2
2
>
>
:
;
N
Tx
N
Ty
T
where T = Tx
, and Tx ; Ty > 0 are directional friction coe¢ Ty
B
i
i
cients along the ex - and eB
y -axis, respectively. The relationship between
the direction of the tangential relative velocity and the direction of the corresponding friction force is illustrated in Figure 3.7 (b) for Tx < Ty . Note
that for Tx = Ty = T , the set CT2 is equal to CT1 .
While the force law (3.60) only describes the Coulomb friction during
impulse free motion, we also need a force law for impact impulses T .
These are found from the exact same form as (3.60) by replacing T with
its right limit +
T instead of T both in (3.60) and in the
T and inserting
convex set CT .
3.6 Numerical Algorithm: Time-Stepping
61
Figure 3.8: Illustration of the prox-function for the set C marked in grey.
Notice that x1 = z 1 since x1 2 C.
Constitutive Laws as Projections
An inclusion can not be directly employed in numerical calculations. Hence,
we transform the force laws (3.56) and (3.60), which have been stated as
an inclusion to a normal cone, into an equality. This is achieved through
the so-called proximal point function proxC (x), which equals x if x 2 C
and equals the closest point to x on the boundary of C if x 2
= C. This is
illustrated in Figure 3.8. The set C must be convex. Using the proximal
point function, we transform the force laws into implicit equalities (Leine
and Nijmeijer, 2004):
2 NC (
where r > 0 for
3.6
) ()
= proxC (
r
);
(3.64)
= H; T .
Numerical Algorithm: Time-Stepping
The numerical solution of the equality of measures (3.44) is found with an
algorithm introduced in Moreau (1988) (Glocker and Studer, 2005; Leine
and Nijmeijer, 2004) called the time-stepping method described in the following. By employing this method together with set-valued force laws we
avoid having to explicitly switch between equations when impacts occur.
Sections 3.6.1 and 3.6.2 are based on Le Saux et al. (2005), except for the
novel direct calculation of the bilateral contact impulsions which we have
introduced in Transeth et al. (2006a,b).
62
3.6.1
Non-smooth Model of a 2D Snake Robot for Simulation
Time Discretization
Let tl denote the time at time-step l = 1; 2; 3; : : : where the step size is
t = tl+1 tl . Consider the (usually very short) time interval It = [tA ; tE ]
and let tA = tl . De…ne q A = q(tA ), uA = u(tA ) which are admissible
with respect to both the unilateral and bilateral constraints. Our goal is
now to …nd q E = q(tE )1 . We use the states of the system at the midpoint tM = tA + 12 t of the time-interval It to decide which contact points
are active (i.e. whether or not any links are touching an obstacle). The
coordinates (positions and orientations) at tM are found from
qM = qA +
t
uA :
2
(3.65)
The approximation of the matrices W , where = H; T; J, on the timeinterval It is given as W M := W (q M ). A numerical approximation of
the equality of measures (3.44) over the time-interval It can now be written
as
M (uE
where
S=
uA )
X
S
WJM P J =
t;
(WHM )b PHb + WT M P T ;
(3.66)
(3.67)
b2HM
and PHb , P T and P J are the contact impulsions during the time-interval It .
They consist of forces acting during It and possible impulses acting in
the time-interval It . To …nd the positions and orientations q E at the end of
the time-interval, we need to solve (3.66) for uE and the contact impulsions.
The contact impulsions associated with obstacle and ground frictional contact are found using the prox-functions described in Section 3.5.2 for the
set of active contact points HM found by (3.47) with t = tM . Hence, the
constitutive laws (3.64) for the obstacle contact and friction impulsions may
now be written as
1
PHb = proxCH PHb
rH
HEb
P Ta = proxCT P Ta
rT
T Ea
; b 2 HM ;
; a = 1; : : : ; n;
(3.68)
(3.69)
In order to clarify the use of notation, we note that the subscript letters A and E
refer to the German words ‘Anfang’ which means ‘beginning’, and ‘Ende’ which means
‘end’.
3.6 Numerical Algorithm: Time-Stepping
63
where rH ; rT > 0, HEb is the b-th element of the vector
vector of the (2a 1)-th and 2a-th element of T E and
HE
TE
:=
H
:=
T
HE ,
T Ea
T
(q M ; uE ) = WHM
uE ;
(q M ; uE ) =
WTTM
is the
(3.70)
uE :
(3.71)
The constitutive laws (3.68)-(3.71) are valid for completely inelastic impacts.
The constraints on the joints are bilateral and it therefore holds that
JE
:=
J
T
(q E ; uE ) = WJM
uE = 0; 8t:
(3.72)
This allows us to directly compute the associated contact impulsions P J
by employing (3.66) and (3.72):
PJ =
T
WJM
M
1
WJM
1
T
WJM
uA + M
1
(S +
t) :
(3.73)
The equations (3.66)-(3.71) and (3.73) constitute a set of non-smooth equations where the number of unknowns uE , PHb , P T and P J , equals the number of equations. An algorithm to solve this set of equations is described
in Section 3.6.2. After having solved for uE we …nd the position at the end
of the time-step as
t
qE = qM +
uE :
(3.74)
2
Notice from (3.66)-(3.74) that no explicit change between equations
are needed (when for example an impact between the snake robot and
an obstacle occurs) since both the non-impulsive forces and the impact
impulses are covered by the same constitutive laws (3.68)-(3.69).
3.6.2
Solving for the Contact Impulsions
The numerical integration algorithm used in this chapter is called a timestepping method which allows for a simultaneous treatment of both impulsive and non-impulsive forces during a time-step. The frictional contact
problem, de…ned by (3.66)-(3.71) and (3.73), needs to be solved for each
time-step tl . A Modi…ed Newton Algorithm (Alart and Curnier, 1991) has
been chosen, because of its simplicity, to solve the non-linear problem iteratively. Let the superscript (k) denote the current iteration of the Modi…ed
Newton Algorithm and initialize all contact impulsions (for active contacts)
with the value they had the last time their corresponding contact point was
active. Now, the algorithm may be written as:
64
Non-smooth Model of a 2D Snake Robot for Simulation
Algorithm 3.1 (Modi…ed Newton Algorithm for Planar Model)
1. Solve
(k)
1
T
1
h WJM M WJM
uA + M 1 S (k) +
PJ =
where
X
(k)
S (k) = WT M P T +
T
WJM
i
t
(3.75)
;
(k)
(WHM )b PHb :
(3.76)
b2HM
(k+1)
2. Solve uE
from
(k+1)
M uE
(k)
S (k)
uA
WJM P J =
t:
(3.77)
3. Solve for a = 1; : : : ; n and b 2 HM
(k+1)
= proxCH PHb
(k+1)
= proxCT P Ta
PHb
P Ta
(k)
rH
(k+1)
HEb
;
(3.78)
(k)
rT
(k+1)
T Ea
;
(3.79)
where
(k+1)
HE
(k+1)
T
= WHM
uE
;
(k+1)
TE
(k+1)
= WTTM uE
:
(3.80)
Repeat steps 1. to 3. until
(k+1)
kP H
(k)
(k+1)
P H k + kP T
(k)
PT k < ;
(3.81)
where > 0 is a user-de…ned tolerance. Subsequently, q E is calculated
from (3.74) and the calculation of the time-step is …nished. Usually, a
higher value of the parameters rH , rT yields a faster convergence rate at the
risk of divergence. However, a general convergence result for the Modi…ed
Newton Algorithm does not exist. Note that the inverse of the mass matrix
M needs to be calculated in step 2. in the algorithm in order to solve for
(k+1)
uE . However, the inverse only needs to be calculated once since M is
constant. This is advantageous for the numerical treatment. We give the
following remark on the implementation of Algorithm 3.1:
Remark 3.1 Note that for the implementation of the Modi…ed Newton Algorithm (Algorithm 3.1) it is bene…cial to set an upper bound on the number
of iterations to avoid an in…nite loop. If the algorithm does not converge
within e.g. 10 000 iterations, then this suggests that the parameters rH and
rT should be altered.
3.7 Summary
65
The constitutive laws (3.78)-(3.80) used to describe the contact impulses
are given on velocity level. This means that the bilateral constraints on
position level are in general not satis…ed. A solution to this problems is
suggested in the following.
3.6.3
Constraint Violation
The contact impulses P J are calculated such that the bilateral constraints
are satis…ed on velocity level. In order to prevent that adjacent links drift
apart, we project the positions of the links so that the bilateral constraints
are satis…ed on position level after the Modi…ed Newton Algorithm has
converged and q E has been found from (3.74). The projection is performed
by keeping the position of link 1 …xed together with the orientations of
all links. Then the links are moved so that all the gap functions (3.14)
associated with the joints are equal to zero, i.e. gJi = 0 for = x; y and
i = 1; : : : ; n 1, and the vector q E is updated with the new positions of
the links.
The projection performed to satisfy the bilateral constraints on position
level adds additional computations to the numerical treatment of the snake
robot model. An alternative approach is to reduce the number of generalized coordinates (from a non-minimal description) by expressing the
bilateral constraints analytically and then incorporating them implicitly
into the model. However, some sort of projection algorithm will in some
cases still be necessary to avoid that the links drift apart depending on the
choice of generalized coordinates and the accuracy of the numerical integrator. Another approach would be to develop the model from a set of
minimal coordinates directly, but then the system matrices will grow very
large when the number of links is increased. With the modelling approach
described in this chapter, we avoid having to rewrite the model (from a
non-minimal set of coordinates) to a new (minimal) set of coordinates and
the system matrices are kept simple. Hence, we have the positions of all
links directly available and the appearance of the model makes it easy to
understand. Moreover, it is less demanding to extend the model to 3D
where an implicit incorporation of the bilateral constraints in the model
would prove cumbersome.
3.7
Summary
A novel mathematical 2D model based on the framework of non-smooth
dynamics and convex analysis is developed for a snake robot in an envi-
66
Non-smooth Model of a 2D Snake Robot for Simulation
ronment with external objects in this chapter. The model describes planar
snake robot motion where the snake robot can push against external obstacles over the entire surface of each link. Hence, the e¤ect of the contact
forces between the obstacle and each link is modelled properly and motion
patterns which are based on that the snake robot pushes against external
objects can be tested with even very small objects. It has been shown
how to easily incorporate the normal contact and friction forces into the
system dynamics (i.e. the equality of measures) by set-valued force laws.
The modelling framework allowed for accurately describing rigid body contact and collisions in a hybrid manner without having to explicitly switch
between system equations. Moreover, it has been shown how to include
correct directional Coulomb friction. Hence, the model can be employed
to simulate various planar motion patterns both with and without external
objects to push against. In addition, the concise description of the model
and its numerical treatment give others the opportunity to implement the
model for simulation.
In addition to presenting the 2D model of the snake robot, this chapter
serves as an introduction to the various modelling techniques in the framework of non-smooth dynamics and convex analysis that have been necessary
to employ in this thesis. In the following chapters, we will utilize and refer
to the general theory given in this chapter.
Chapter 4
3D Snake Robot Modelling
for Simulation
4.1
Introduction
Biological snakes may lift parts of their body during locomotion by a form of
lateral undulation (called sinus-lifting) to move faster and more e¢ cient. In
addition, snakes that travel on a surface with uniform friction may employ
a serpentine motion pattern called sidewinding for locomotion. Both of
these motion patterns require that parts of the snake robot body are lifted.
For this reason, a model of a snake robot capable of such motion patterns
is needed to include the possibility of 3D motion.
In this chapter we extend the non-smooth 2D model given in Chapter 3 to 3D motion. We model the snake robot with 2 degrees of freedom
(DOF) cardan joints and cylindrical links. The snake robot is set in an
environment with vertical cylindrical obstacles that it may push against
for locomotion. The general procedure for developing the model is similar
to the development of the 2D model. However, di¤erent coordinates are
chosen and additional constitutive laws need to be added (e.g. a varying
normal force between the snake robot and the ground). The summary of
the 2D model presented in Section 3.2 can be read as an introduction to the
development of the 3D model also since the general approach is the same.
This chapter is based on Transeth et al. (2006a), Transeth et al. (2006b)
and Transeth et al. (2008a) and it is organized as follows. The kinematics
of the snake robot with the ground surface as a unilateral constraint is
described in Section 4.2. Then the groundwork for …nding the friction and
ground contact forces is laid in Section 4.3. The non-smooth dynamics is
68
3D Snake Robot Modelling for Simulation
Figure 4.1: 3D model of snake robot among cylindrical obstacles.
presented in Section 4.4. How to calculate and control the joint angles is
shown in Section 4.5. The numerical treatment of the model is described
in Section 4.6 and a summary of the chapter is given in Section 4.7.
4.2
Kinematics
In this section we describe the kinematics of a 3D model of the snake robot.
We employ the kinematics to develop gap functions for ground contact
detection and obstacle contact detection. These functions are also needed
for calculating the directions of the contact forces.
This section will …rst give an overview of the coordinates used to describe the position and orientation of the snake robot. Subsequently, the
gap functions will be presented.
4.2.1
Model Description, Coordinates and Reference Frames
The 3D model of the snake robot ‘Aiko’consists of n cylindrical links that
are connected by n 1 cardan joints, each having two degrees of freedom
(DOF). Figure 4.1 illustrates the 3D model with obstacles for the snake
robot to push against for locomotion. The distance Li between two adjacent cardan joints equals the length of link i and the radius of each link is
LSCi . Each link is modelled as a cylinder of length 2LGSi with two spheres
of radius LSCi attached to the ends the link. Half of each sphere is covered by the cylinder. See Figure 4.2 for an illustration of link i together
with parts of its two adjacent links. We denote the earth-…xed coordinate
frame I = O; eIx ; eIy ; eIz , see Figure 4.2, as an approximation to an inertial frame where its origin O is …xed to the ground surface and the eIz -axis
4.2 Kinematics
69
Figure 4.2: Link i with two adjacent links and reference frames.
is pointing in the opposite direction of the acceleration of gravity vector
T
g . The remaining coordinate axes are de…ned according to
g= 0 0
the right-hand convention where plane eIx ; eIy constitutes the horizontal
Bi Bi
i
ground surface. We denote the body-…xed frame Bi = Gi ; eB
x ; ey ; ez ,
where Gi is the centre of gravity of link i (which coincides with the geoi
metric centre of the link-cylinder) and eB
z points along the centre line of
B
i
link i towards link i + 1. We de…ne ey to be parallel to eIz when the link is
lying ‡at on the ground surface and has not rolled sideways. The remaining
i
coordinate axis eB
x is de…ned according to the right-hand convention.
The environment of the snake robot consists of a horizontal ground
surface together with external objects. The external objects (referred to
as ‘obstacles’) are included in the model. Each obstacle j = 1; : : : ; is
modelled as a vertical cylinder of radius LHj and in…nite height with its
centre-line parallel to eIz . The vector from O to the point Hj where the
mid-line of obstacle j intersects with the eIx ; eIy -plane is denoted by r Hj .
We see from the above description of the model with obstacles, that the
2D model presented in Chapter 3 is a projection of the 3D model onto
the eIx ; eIy -plane when lying ‡at on the ground surface. The only di¤erence is the change in de…nition of the body-…xed coordinate axes (compare
Figure 3.2 on page 45 and Figure 4.2).
The position and orientation of link i are described by the non-minimal
absolute coordinates
r
q i = I Gi 2 R7 ;
(4.1)
pi
where I r Gi =
xi yi zi
T
2 R3 is the position of the centre of gravity of
70
3D Snake Robot Modelling for Simulation
T
, where eT
link i and the vector pi = ei0 eT
i
i = ei1 ei2 ei3 , contains
the four Euler parameters used to describe the orientation of link i. The
Euler parameters form a unit quaternion vector with the constraint pT
i pi =
1. The coordinates are non-minimal because each link is described with
a non-minimal set of coordinates and absolute because the position and
orientation of link i is given directly relative to frame I. The velocity of
link i is given by
v
ui = I Gi 2 R6 ;
(4.2)
Bi ! IBi
T
2 R3 is the translational velocity of the
where I v Gi = vix viy viz
CG of link i which is I v Gi = I r_ Gi when it exists (i.e. for impact free
motion). Moreover, Bi ! IBi is the angular velocity of frame Bi relative
to frame I, given in frame Bi . The transformation I r = RIBi Bi r can be
performed with the rotation matrix
RIBi = Hi HT
i ;
(4.3)
where
Hi =
~i + ei0 I ;
ei e
Hi =
ei
~i + ei0 I ;
e
(4.4)
and the superscript~denotes the skew-symmetric form of a vector throughout this chapter, i.e.
2
3
0
ei3 ei2
~i = 4 ei3
0
ei1 5 :
e
(4.5)
ei2 ei1
0
The time-derivative of the rotation matrix is found from Egeland and Gravdahl (2002) as
_ I = RI B !
~ IBi RIBi :
R
(4.6)
Bi
Bi i ~ IBi = I !
The coordinates (positions and orientation) and velocities of all links are
gathered in the vectors
2 3
2 3
q1
u1
6 .. 7
6 .. 7
q = 4 . 5; u = 4 . 5:
(4.7)
qn
un
4.2.2
Gap Functions for Ground Contact
The gap functions for contact between the snake robot links and the ground
surface give the minimal distance between the ‡oor and the front and rear
4.2 Kinematics
71
Figure 4.3: Surfaces (solid-drawn circles) on snake robot that constitute
the contact between the robot and the ground.
part of each link. We also saw in Example 3.1 on page 46 that the gap
functions are used as a basis for calculating the forces involved in the contact.
The contact surfaces between a link and the ground are modelled as
two spheres at the ends of the link as illustrated for a three-link robot in
Figure 4.3. Hence, we do not consider the cylindrical part of the link for
ground contact. Moreover, note that with this assumption the cylindrical
part is never penetrated by the ground surfaces since it is assumed that the
ground surface is planar.
We denote the distance between the centre of the two spheres that
belong to link i by 2LGSi and the radius of the spheres by LSCi . The
position of the centre of the front and rear spheres are denoted by r SF i and
r SRi , respectively. The shortest distance (i.e. the gap functions) between
the ground and the points on the front and rear spheres closest to the ground
are denoted by gNF i and gNRi , respectively. The distances are found from
gNF i = (r SF i )T eIz
LSCi ;
gNRi = (r SRi )T eIz
LSCi ;
(4.8)
i
i
where r SF i = r Gi + LGSi eB
LGSi eB
z and r SRi = r Gi
z .
The gap functions are gathered in the vector
g N = gNF 1
4.2.3
gN F n
gNR1
gNRn
T
:
(4.9)
Gap Functions for Contact with Obstacles
For contact between the snake robot and the obstacles, we consider the
entire surface of the link as depicted in Figure 4.2, and not only the two
end-spheres employed for ground contact. This results in a contact point
that may move across the entire surface of a link. The shortest distance
72
3D Snake Robot Modelling for Simulation
Figure 4.4: Illustration of the shortest distance between link i and obstacle
j. The cylindrical part of the link is closest to the obstacle in the picture.
between link i and obstacle j is found with the gap function gHij . Similarly
to the 2D model, link i can only touch a convex obstacle j at one point at
each time-instance. Hence, by inspecting the shape of the surface of a link,
we need to consider the distance between two cylinders, and the distance
between a cylinder and a sphere to calculate the shortest distance (i.e. the
gap function) between the link and the obstacle, see Figure 4.4.
Cylinder - Cylinder Contact
If the part of link i which is shaped as a cylinder is closest to the obstacle
(also being a cylinder), then the gap function for object contact is de…ned
as the distance between these two cylinders. Since it is already established
that it is the cylinder part of the link that is closest to the obstacle, we
can regard the link cylinder to be of in…nite length. The shortest distance
between two in…nitely long cylinders equals the shortest distance between
their centre-lines minus the sum of their respective radii. The normal vector
to the centre-lines of both the obstacle j and the link i cylinder is
nij =
eIz
keIz
i
eB
z
i
eB
z k
:
(4.10)
To …nd the shortest distance between the two centre-lines, we simply project
a vector going from some point on the obstacle centre-line, for example Hj ,
to some point on the link centre-line, for example Gi , onto the normal
vector nij in (4.10). Subsequently, the shortest distance between the two
centre-lines can be found as
dij = r Gi
r Hj
T
nij :
(4.11)
4.2 Kinematics
73
Hence, the gap function for cylinder-cylinder contact is
gHij = jdij j
LHj + LSC :
(4.12)
Cylinder - Sphere Contact
Consider one of the two hemispheres of link i as closest to obstacle j, then
the gap function between the link and the obstacle is
gHij = kr Hj Si k
LHj + LSCi ;
(4.13)
where r Hj Si is a vector from Hj to the projection of either the point SF i or
the point SRi onto the eIx ; eIy -plane depending on which end of the link
is closest to the obstacle. Hence, it holds that
r Hj Si =
Axy (r Gi + r Gi SF i )
Axy (r Gi + r Gi SRi )
r Hj , front part of link i is closest;
r Hj , rear part of link i is closest;
(4.14)
2
3
1 0 0
= 40 1 05 :
0 0 0
(4.15)
where
i
r Gi SF i = LGSi eB
z ;
r Gi SRi =
i
LGSi eB
z ;
Axy
Vector of Gap functions for Contact with External Objects
We now gather the gap functions gHij for all n links and
g H = gH11
gHn1
gH12
gHn2
gH1
vector
gHn
T
; (4.16)
where the elements gHij are found from either (4.12) or (4.13) depending
on which part of link i is closest to obstacle j.
4.2.4
Gap Functions for Bilateral Constraints
Each 2 DOF cardan joint introduces four bilateral constraints. To include
the bilateral constraint forces in the model, we …rst need to develop gap
functions that describe the ‘gap’in the joint.
To …nd the translational ‘gap’in the joints, we need to relate the position
of the joint between link i and i + 1 to both of these links. Then we will get
a non-zero value from the gap function if the adjacent links drift apart. Let
the position of the joint between link i and i + 1 be written relative to these
Bi+1
1
i
,
two links as I r JF i = I r Gi + 12 Li I eB
z and I r JRi+1 = I r Gi+1
2 Li+1 I ez
74
3D Snake Robot Modelling for Simulation
Figure 4.5: Illustration of how the translational gaps in the joint between
link i and link i + 1 is found.
respectively. We see from Figure 4.5 that r JF i = r JRi+1 when the two
adjacent links are in a position and orientation relative to each other that
is allowed by the cardan joint which connects the links. The gap functions
for the translational gaps can now be found from
gJi =
for i = 1; : : : ; n
gJi =
1, and
I r Gi
I r JF i
I r JRi+1
T
I
Ie ;
(4.17)
= x; y; z. Hence, from (4.17) we found that
I r Gi+1
T
I
Ie
B
i+1
I
i
+ 21 Li RIBi Bi eB
z + Li+1 RBi+1 Bi+1 ez
T
I
Ie ;
(4.18)
for i = 1; : : : ; n 1, and = x; y; z.
The next gap function provides the ‘gap’ in rotation around the axis
that the cardan joint is not able to rotate around (one can only perform
i
pitch and yaw movement with a cardan joint and not roll). Let eB
y and
B
ex i+1 be the axes of rotation for the cardan joint between link i and link
i + 1, then
T Bi+1
i
eB
ex
= 0:
(4.19)
y
Hence, a measure for the rotational ‘gap’can be de…ned from (4.19) as the
gap function
T
B
i
RIBi+1 Bi+1 ex i+1 :
(4.20)
gJi = RIBi Bi eB
y
4.3 Contact Constraints on Velocity Level
4.3
75
Contact Constraints on Velocity Level
In this section we calculate the relative velocities between the snake robot
and the ground from the gap functions. The relative velocities are needed
to set up the set-valued contact forces for the closed contacts (Le Saux et
al., 2005).
4.3.1
Unilateral Contact: Ground Contact
Contact between the snake robot and the ground surface involves (vertical) normal forces, which guarantee the unilaterality of the contact, and
(horizontal) tangential contact forces, which are due to friction and are
dependent on the normal contact forces and the relative sliding velocities.
We assumed that the normal contact forces between the snake robot and
the ground surface were constant in the 2D model in Chapter 3. For the
3D model, this normal contact force is varying and we need to …nd the
corresponding relative velocities to calculate it.
Relative Velocities Along eIz
The relative velocities between the front and rear part of link i and the
ground along the eIz -axis are de…ned as NF i := g_ NF i and NRi := g_ NRi
(when they exist), respectively, and they are used later to …nd the normal
contact forces. Before we proceed, note that NF i (or NRi ) should not be
found directly by taking the time-derivative of the expression for gNF i (or
gNF i ) in (4.8). This is the case since the expressions (4.8) have already
been simpli…ed due to the fact that expressions have been inserted for the
various body-…xed vectors which constitute the gap functions. Instead, for
the relative velocities, we must consider the velocity of the body-…xed point
PF i which at time-instant t coincides with the point CF i on the front sphere
closest to the ground (the same principle applies for the rear part of the
link). Note that the position vectors for PFi and CFi will be the same
instantaneously. However, the di¤ erentials will be di¤erent. This is shown
in the following. Let CF i be a point on the front sphere that moves on the
sphere such that the point is always closest to the ground surface. Then
the position of this point is
r CF i = r Gi + r Gi SF i + r SF i CF i :
(4.21)
76
3D Snake Robot Modelling for Simulation
The velocity of the point CF i is obtained by di¤erentiation of (4.21):
I v CF i
( I r~ Gi SF i + I r~ SF i CF i ) RIBi Bi ! IBi + RIBi Bi r_ SF i CF i ; (4.22)
{z
}
= I v Gi
|
I v PF i
where we now can use that
r S F i CF i =
LSCi eIz ;
(4.23)
and we have employed (4.6) and Bi r_ Gi SF i = 0; together with the identities
~ IBi I r SF i CF i = I ! IBi I r~ SF i CF i .
~ IBi I r Gi SF i = I ! IBi I r~ Gi SF i and I !
I!
We see from (4.22) the expression for the velocity I v PF i of the body-…xed
point PF i which at time-instance t coincides with the point CF i . This
velocity will be employed to …nd both the relative velocities along eIz and
the tangential relative velocities. The equivalent velocity I v PRi on the rear
part of the sphere is found similarly to I v PF i in (4.22) by replacing F i by
Ri in (4.22)-(4.23).
Now, the relative velocities along eIz for the front and rear part of link
i can be found as
NQi
= ( I eIz )T I v PQi =)
NQi
= (wNQi )T ui ;
(4.24)
where
wNQi = (I eIz )T
(I eIz )T
+ I r~ SQi CQi RIBi
~ Gi SQi
Ir
T
;
(4.25)
i
i
LGSi eB
for Q = F; R with r Gi SF i = LGSi eB
z . The motiz and r Gi SRi =
vation to use the form (4.24) is that wNQi gives the generalized direction
of the contact force between the ground and the front and rear part of link
i.
A vector gathering all NF i and NRi is
T
= WN
u;
N
(4.26)
where
N
=
NF 1
WN
=
WNF
and
WNQ
for Q = F; R.
2
wNQ1
6
6 06 1
=6
6 ..
4 .
06 1
:::
NF n
WNR 2 R
06
..
NR1
6n 2n
:::
;
06
..
.
1
.
..
06
.
1
T
NRn
;
(4.27)
(4.28)
1
06 1
wNQn
3
7
7
7 2 R6n
7
5
n
;
(4.29)
4.3 Contact Constraints on Velocity Level
77
Tangential Relative Velocities
The friction forces between a link and the ground depend on the relative velocities in the (eIx ; eIy )-plane between the snake robot links and the ground.
These velocities are termed tangential relative velocities. First, the relative
velocities between the front part of link i and the ground along the eIx and eIy -axis, 0TF ix and 0TF iy , will be deducted. Subsequently, the relative
velocities for the front part of link i along the projection of the longitudinal
axis of the link onto the (eIx ; eIy )-plane TF ix and transversal to the link
0
0
TF iy , will be deducted from TF ix and TF iy respectively. The same type
of notation applies for the rear part of link i: 0TRix , 0TRiy , TRix and TRiy .
The tangential relative velocities are found much in the same way as for
NF i and NRi . Consequently, we …nd the velocities of the points PF i and
PRi along the eIx - and eIy -axis. Hence, by looking at (4.24), we see that the
tangential relative velocities of the contact points on the front part of the
link can be found as
0
TF i
for
= (I eI )T I v PF i =)
0
TF i
= (w0TF i )T ui ;
(4.30)
= x; y, where
w0TF i = (I eI )T
T
(I eI )T ( I r~ Gi SF i + I r~ SF i CF i ) RIBi
:
(4.31)
The relative velocities between the rear part of the link and the ground
are found by exchanging I v PF i with I v PRi in (4.30):
0
TRi
for
= (I eI )T I v PRi =)
0
TRi
= (w0TRi )T ui ;
(4.32)
= x; y, where
w0TRi = (I eI )T
(I eI )T ( I r~ Gi SRi +I r~ SRi CRi ) RIBi
T
:
(4.33)
The tangential relative velocities for the front and rear part of the link i
are combined in vectors:
0
0T
(4.34)
TQi = WTQi ui ;
h
iT
0
for Q = F; R, where 0TQi = 0TQix
and
TQiy
h
WT0 Qi = w0TQix
i
w0TQiy 2 R6
2
:
(4.35)
Until now the relative velocities have been given in the directions along the
eIx and eIy axes. In order to calculate the friction forces longitudinal and
78
3D Snake Robot Modelling for Simulation
transversal to a link, we need to know the corresponding relative velocities
in these directions in the eIx ; eIy -plane. To calculate these velocities we
introduce for each link a frame i with axes ex i ; ey i ; ez i , where I ez i =
I
I ez , and
I
i
i
Axy I eB
I ez
I ex
z
i
;
(4.36)
; I ey i =
e
=
I x
Bi
kAxy I ez k
k I eIz I ex i k
where Axy = diag([1; 1; 0]). Hence, it holds that
RI i =
I ex
I ey
i
i
I ez
i
:
(4.37)
Bi
i
i
Notice that I ex i = I eB
z and I ey = I ex when link i is lying ‡at on the
I
I
I
i
ground with I eB
y = I ez . Since only the motion in the ex ; ey -plane is of
interest, we de…ne
RI i = DT RI i D;
D=
1 0 0
0 1 0
T
:
(4.38)
Since we now have that 0TQi = RI i TQi , Q = F; R, the relative velocity
between the ‡oor and the front part of link i, with respect to frame i , can
be found from (4.34) as
T
(4.39)
TQi = WTQi ui ;
h
iT
where TQi = TQix
; WTQi = WT0 Qi RI i ; for Q = F; R.
TQiy
A vector that gathers all TF i and TRi is found from
T
where
2
T
and
WTQ
6
6
6
6
=6
6
6
6
4
TF 1
= WTT u;
(4.40)
3
.. 7
. 7
7
7
4n
TF n 7
72R ;
TR1 7
.. 7
. 5
WT = WTF
WTR 2 R6n
4n
;
(4.41)
TRn
2
WTQ1
6
6 06 2
=6
6 ..
4 .
06 2
06
..
06
..
.
2
.
..
06
.
2
2
06 2
WTQn
3
7
7
7 2 R6n
7
5
2n
;
for Q = F; R: (4.42)
4.3 Contact Constraints on Velocity Level
79
Relative Rolling Velocities
Up until now we have only considered translational relative motion of the
snake robot links. However, we also need to consider rotational relative
motion to add a damping e¤ect on the rotational motion in the form of
rotational friction. In order to do this, we introduce the relative rolling
velocity as
T
(4.43)
I r CQi SQi
I ! IBi ;
VQi = D
iT
h
, I r CQi SQi = LSCi I eIz is the
for Q = F; R, where VQi = VQix
VQiy
vector pointing (upward) from the contact point point CQi on the link endsphere closest to the ground, toward the centre SQi of the end-sphere and
D is given in (4.38). By employing the identity
I r CQi SQi
= I r~ CQi SQi RIBi Bi ! IBi ;
I ! IBi
we …nd that the relative rolling velocity can be written
= WVTQi ui ;
VQi
(4.44)
where
WVTQi = O2
3
DT I r~ CQi SQi RIBi ;
(4.45)
for Q = F; R.
We gather all the relative rolling velocities as
V
= WV u;
(4.46)
where
2
V
6
6
6
6
=6
6
6
6
4
VF 1
3
.. 7
. 7
7
7
4n
VF n 7
72R ;
VR1 7
.. 7
. 5
WV = WVF
WVR 2 R6n
4n
;
(4.47)
VRn
and WVF , WVR are found similarly to (4.42) by replacing WTF i and WTRi
in (4.42) by WVF i and WVRi , respectively.
80
4.3.2
3D Snake Robot Modelling for Simulation
Unilateral Contact: Obstacle Contact
Contact between link i and obstacle j is modelled as a unilateral contact.
To calculate the contact force involved in the contact, the relative velocity
Hij := g_ Hij between obstacle j and the point on link i closest to the
obstacle needs to be found. This is obtained from the gap functions de…ned
in Section 4.2.3. Equivalently to the calculations of the gap functions, the
relative velocity Hij is dependent on which part of the link is closest to
the obstacle.
Cylinder - Cylinder Contact
If the cylinder part of link i is closest to obstacle j, then the gap function
is given by (4.12) and the relative velocity Hij := g_ Hij can be written as
Hij
=
T
I v Gi nij
+ r Gi
T
r Hj
n_ ij sign(dij );
(4.48)
where nij and dij are found from (4.10) and (4.11), respectively. De…ne
k k := k k2 and consider
_ ij
In
=
where
d
dt
d
I
Bi
I ez k
dt kI ez
i 2
kI eIz I eB
z k
I
I ez
d
kI eIz
dt
Bi
I ez
I
I ez
Bi
I ez
+
d
dt
I
I ez
kI eIz
Bi
I ez
Bi
I ez k
;
(4.49)
i
~B
~Iz RIBi Bi e
Ie
z Bi ! IBi ;
=
Bi T
I ez
Bi
I ez k =
(4.50)
i
~B
Axy RIBi Bi e
z Bi ! IBi
kI eIz
Bi
I ez k
;
(4.51)
~Iz denotes the skew-symmetric form of eIz similarly to (4.5) on page 70.
and e
Substitution of (4.49)-(4.51) into (4.48) yields
Hij
where
wHij = sign(dij )
and
cT
ij =
I r Gi
kI eIz
h
T
I r Hj
Bi
I ez k
= wT
Hij ui ;
I
i
~B
(I nij )T cT
z
ij RBi Bi e
Bi T A
xy
I nij I ez
B
i
kI eIz I ez k
(4.52)
iT
;
~Iz
Ie
(4.53)
!
:
(4.54)
The motivation to write the relative velocity in the form (4.52) is that the
vector wHij 2 R6 constitutes the generalized force direction of the contact
force between obstacle j and link i.
4.3 Contact Constraints on Velocity Level
81
Cylinder - Sphere Contact
The gap function in (4.13) is employed to de…ne the relative velocity Hij :=
g_ Hij when one of the two hemispheres on link i is closest to the obstacle j.
In order to …nd Hij , consider the vector I r Hj Si in (4.14) which is a vector
from Hj to the projection of either the point SF i or the point SRi onto the
eIx ; eIy -plane depending on which end of the link is closest to the obstacle.
The time-derivative of I r Hj Si is
_ Hj Si
Ir
where
~ Gi Si
Bi r
= Axy
Axy RIBi Bi r~ Gi Si Bi ! IBi ;
I v Gi
denotes the skew-symmetric form of
, front part of link i is closest,
, rear part of link i is closest.
r Gi SF i
r Gi SRi
r Gi Si =
and
Bi r Gi Si
(4.55)
We now …nd the relative velocity between obstacle j and the point closest
to the obstacle on the front or rear sphere of link i as
Hij
where
wT
Hij
=
T
I r Hj Si
kI r Hj Si k
= wT
Hij ui ;
(4.56)
Axy RIBi Bi r~ Gi Si :
Axy
(4.57)
Vector of Relative Velocities for the Obstacle Contact
We now gather the relative velocities Hij for all n links and
the vector
T
H = WH u;
obstacles in
(4.58)
where
H
=
T
H11
Hn1
H12
Hn2
WH = WH1
and
WHj
2
wH1j
6
6 06 1
=6
6 ..
4 .
06 1
06
..
H1
WH
2 R6n
06
..
.
1
.
..
06
.
1
1
3
Hn
n
;
7
7
7 2 R6n
7
06 1 5
wHnj
; (4.59)
(4.60)
n
:
(4.61)
Depending on which part of the link is closest to the obstacle, wHij is
obtained from either (4.53) or (4.57).
82
4.3.3
3D Snake Robot Modelling for Simulation
Bilateral Constraints: Joints
The bilateral constraints introduced by the cardan joint between two adjacent links prohibit relative motion between the links in four DOF. The
relative velocities between the links along these DOF need to be found in
order to calculate the bilateral constraint forces in the joints. These relative
velocities are found from the gap functions gJi , = x; y; z, in (4.18) and
gJi in (4.20) for i = 1; : : : ; n 1.
The relative velocities for the translational gap between link i and link
i + 1 are de…ned as Ji := g_ Ji for i = 1; : : : ; n 1 where = x; y; z. By
employing (4.18), we …nd that
Ji
where
w Ji
2
6
6
6
=6
6
4
ui
;
ui+1
= wT
Ji
(4.62)
(I eI )
T
i
~B
(I eI )T L2i RIBi Bi e
z
(I eI )
B
I
~z i+1
(I eI )T Li+1
2 RBi+1 Bi+1 e
3
7
7
7
7:
7
T5
The relative velocity for the rotational gap is de…ned as
i = 1; : : : ; n 1. Thus,
Ji
where
w Ji
2
6
6
=6
6
4
ui
;
ui+1
= wT
Ji
03
i
~B
RIBi Bi e
y
T
03
1
B
RIBi+1 Bi+1 ex i+1
1
B
~x i+1
RIBi+1 Bi+1 e
T
i
RIBi Bi eB
y
(4.63)
Ji
:= g_ Ji for
(4.64)
3
7
7
7:
7
5
(4.65)
h
iT
Let Ji = Jix
2 R4 , then we can gather all the relaJiy
Jiz
Ji
tive velocities concerned with the bilateral constraints in one vector
J
= WJT u;
(4.66)
4.4 Non-smooth Dynamics
83
where
J
WJ
2
6
= 4
..
.
Jn
1
7
4(n
52R
1)
;
(4.67)
2 T
3T
WJ1 04 6
04 6
6
.. 7
6 04 6 W T
. 7
J2
6
7 2 R6n
= 6 .
7
.
.
.
4 .
.
04 6 5
T
04 6
04 6 WJn
1
and WJi = wJix
4.4
3
J1
wJiy
wJiz
w Ji
2 R12
4
4(n 1)
for i = 1; : : : ; n
;
(4.68)
1.
Non-smooth Dynamics
In this section we …rst present the various components of the equality of
measures for the 3D model. The equality of measures describes the dynamics of the snake robot as seen in Section 3.5.
4.4.1
The Equality of Measures
Consider the generalized velocity to be a function t 7! u(t) of locally
bounded variation on a time-interval It = [tA ; tE ] (Moreau, 1988). The
velocity function u (t) and its di¤erential measure du for the 3D model
have the same properties as the same function elaborated on for the 2D
model in Section 3.5.1. The only di¤erence is that the velocity function for
the 2D model describes, of course, the velocity of the links in the 2D model,
while the velocity function in this chapter describes the velocities in the 3D
model.
The equality of measures (3.43) on page 54 is written for the 3D model
of the snake robot as
Mdu
h (u) dt
dR = dt;
(4.69)
where the mass matrix M, the vector of smooth forces h (u), the force
measure of possibly atomic impact impulsions dR and the vector of applied torques will be described in the following. Note that the di¤erential
measure du describes both the continuous change and possible discontinuities of the velocity u (see Section 3.5.1).
84
3D Snake Robot Modelling for Simulation
For our choice of coordinates, the mass matrix is diagonal and constant
3
2
M1 06 6
06 6
6
.. 7
606 6 . . .
. 7
7 2 R6n 6n ;
(4.70)
M=6
7
6 ..
.
.
4 .
. 06 6 5
06 6
06 6 Mn
where
Mi =
mi I3 3
03 3
03
Bi
3
;
Gi
with Bi Gi = diag
i , mi is the mass of link i, and
i
i
i
and
are
its
moments
of
inertia.
The
smooth
forces,
here
consisting
of
i
gravity and gyroscopic accelerations, are described by
2
3
h1 (u1 )
6
7
h(u) = 4 ... 5 2 R6n ;
(4.71)
hn (un )
where
2
6
hi (ui ) = 6
4
0
0
mi g
~ IBi Bi
(Bi !
3
7
7 2 R6 :
5
Gi Bi ! IBi )
(4.72)
The force measure dR accounts for all the contact forces and impulses.
The contact e¤orts that constitute dR are found from the force-laws given
in Section 4.4.2. Let I be the set of all active contacts with the ground and
H be the set o¤ all active contacts with the obstacles:
I(t) = fajgNa (q(t)) = 0g
H(t) = fbjgHb (q(t)) = 0g
f1; 2; : : : ; 2ng;
f1; 2; : : : ; n g;
(4.73)
(4.74)
where gNa is the a-th element of the vector g N in (4.9) and gHb is the b-th
element of the vector g H in (4.16).
Now, the force measure is written as
P
dR = WJ dP J + a2I (WN )a dPNa
P
+ a2I (WT )2a 1 dPTax + (WT )2a dPTay
P
+ a2I (WV )2a 1 dPVax + (WV )2a dPVay
P
+ b2H (WH )b dPHb ;
(4.75)
4.4 Non-smooth Dynamics
85
where dPNa is the normal contact impulse measure between the ground
and a link, dPTax and dPTay are the tangential contact impulse measures
(friction) between the ‡oor and a link, longitudinal and transversal to the
link (i.e. along the ex i - and ey i -axis), respectively, dPVax and dPVay are
the rotational contact impulse measures (friction) between the ‡oor and a
link, along the eIx -axis and eIy -axis, respectively, dP J is the contact impulse
measure due to the bilateral constraints in the joints (these constraints are
always active), dPHb is the normal contact impulse measure between a link
and an obstacle and the lower-case subscripts on the W-matrices indicate
which column of the matrix is used. The position of the elements of the
vectors dP N , dP J , dP T , dP V and dP H corresponds to the position of the
elements in their respective vector of relative velocity N , J , T , H and
V . Hence, by looking at for example the expression (4.41) for T , we see
that e.g.
dPT(n+1)x
dP Tn+1 =
(4.76)
dPT(n+1)y
corresponds to TR1 and we know from this that dP Tn+1 is the tangential
contact impulse measure between the ground and the rear part of link 1.
The contact impulse measures are, similarly to the velocity measure,
decomposed into a Lebesgue-measurable force and a purely atomic impact
impulse. This is written for dPNa as
dPNa =
Na dt
+
Na d
;
(4.77)
where Na is the Lebesgue-measurable force due to a continuous contact
between a link and and the ground surface and Na is the purely atomic
impact impulse caused by a collision between the two objects. The same
decomposition can be performed for the other contact impulse measures.
The vector of controlled torques is described in Section (4.4.3) below.
4.4.2
Constitutive Laws for Contact Forces
In this section we will give the set-valued force laws for normal contact with
the ground surface and the obstacles, together with tangential Coulomb
friction and rolling friction. These laws will all be formulated on velocity
level using the relative contact velocities given by N in (4.26), T in (4.40),
V in (4.46) and H in (4.58). Subsequently, the set-valued force laws are
formulated as equalities in Section 4.4.2 using the so-called ‘proximal point
function’ in order to include the force laws in the numerical simulation
(Leine and Nijmeijer, 2004). Subscripts i, j, a and b will be omitted for
brevity.
86
3D Snake Robot Modelling for Simulation
The set-valued force laws for the normal contact forces which arise from
the contact with the ground surface and the obstacles are described in
exactly the same manner as for the 2D model given in Section 3.5.2.
Normal Contact Force
The general form of the set-valued force law for the normal contact forces
that arise from contact with the ground surface and the obstacles are exactly
the same. In addition, this formulation is the same for both the 2D and the
3D model. To this end, we only present the force law for normal contact
brie‡y here. Please see Section 3.5.2 for thorough explanation of the force
law.
The Lebesgue measurable normal contact force (not an impulse) between the front or rear part of a link and the ground surface may be written
for a closed contact gN = 0 as an inclusion on velocity level
N
2 N CN (
N );
(4.78)
where N 2 R is the relative velocity in (4.24), N 2 R is the corresponding
(possible) normal force between a link and the ground surface, the convex
set CN = f N j N
0g = R+ is the set of admissible contact forces and
NCN is the normal cone to CN (see Example 3.2 on page 57 and Example 3.3
for a description of the normal cone).
The Lebesgue measurable normal contact force between a link and an
obstacle may be written for a closed contact gH = 0 as
H
2 N CH (
H );
(4.79)
where H 2 R is the relative velocity in (4.52) or (4.56), H 2 R is the
corresponding (possible) normal force between a link and an obstacle, the
convex set CH = f H j H
0g = R+ is the set of admissible contact
forces and NCH is the normal cone to CH .
The impact laws for a completely inelastic impact at a closed contact
with the ground surface and an obstacle can now be written as
+
N
+
H
2 N CN (
2 N CH (
respectively, where
H.
+
N
N );
H );
CN = f
CH = f
is the right limit of
N
j
0g = R+ ;
N
H
j
N
and
+
0g = R ;
H
+
H
(4.80)
(4.81)
is the right limit of
4.4 Non-smooth Dynamics
87
Coulomb Friction Force
In this section we describe the friction force between the snake robot and
the ground, when the snake robot slides along the ground surface, as setvalued Coulomb friction. Let T 2 R2 be a tangential relative sliding
T
2 R2 , in
velocity found in (4.39). Then the friction force T = Tx
Ty
the two-dimensional tangent plane to the contact point, is modelled with a
Coulomb friction law
(4.82)
T 2 NCT ( T );
where NCT is the normal cone to the convex set CT . See Example 3.4 on
page 58 for an interpretation of the force law (4.82). The friction force is
modelled both as isotropic and orthotropic Coulomb friction by changing
the set CT to either CT1 or CT2 as described below and the friction force
law (4.82) is on the same form as the friction force law (3.60) for the two
models. In addition, the convex sets CT1 in (3.61) and CT2 in (3.63) from
the 2D model are again employed to describe isotropic and orthotropic
friction, respectively. We reproduce the sets here for convenience:
CT1
CT2
= f T jk
8
>
<
=
T j
>
:
Tk
Ng ;
T
2
Tx
2
2
Ty
+
2
Tx N
Ty
N
9
>
=
1 ;
>
;
(4.83)
(4.84)
where N is the (varying) normal force, T > 0 is the friction coe¢ cient for
isotropic friction, and Tx > 0 and Ty > 0 are the directional friction coef…cients along the ex - and ey -axis in (4.36), respectively. See Section 3.5.2
for a more thorough explanation of the model of the set-valued Coulomb
friction and the convex sets CT1 and CT2 .
The force law for the impact impulse T 2 R2 is found from
+
T
where
+
T
2 N CT (
2 R2 is the right limit of
T );
(4.85)
T.
Rolling Friction
The 3D model of the snake robot has cylindrical links which means that it
may roll sideways. The spatial Coulomb friction described in the previous
section arises from translational motion. In addition, there is also a force
that resists the rolling motion of the snake robot. We model this force as a
88
3D Snake Robot Modelling for Simulation
‘rolling friction’ V 2 R2 (additional subscripts omitted for simplicity) in
this chapter by employing the same set-valued force law as for the tangential
Coulomb friction force T in Section 4.4.2. However, we consider the rolling
friction to be isotropic and therefore we …nd the rolling friction as
V
2 N CV (
V
jk
V );
(4.86)
where
CV = f
Vk
V
Ng ;
(4.87)
and V > 0 is the friction coe¢ cient.
The general form of the force law (4.86) is also valid for impact impulses
V in the same way as for the tangential Coulomb friction described in
Section 4.4.2. Subsequently, the impact impulse is found by exchanging V
with +
V in (4.86)-(4.87).
V and V with
Constitutive Laws as Projections
The proximal point function is used to transform the force laws into implicit
equalities (see Section 3.5.2)
2 NC (
where r > 0 for
4.4.3
) ()
= proxC (
r
);
(4.88)
= N; H; T; V .
Joint Actuators
Each cardan joint has 2 DOF that are controlled by two joint actuators.
The actuators are modelled as controlled torques applied around the axes
of rotation for the joint. Figure 4.6 illustrates how the direction of positive
rotation is de…ned. De…ne for link i a positive control torque Cvi to give a
B
positive rotational velocity around ex i+1 and a positive control torque Chi
i
to give a positive rotational velocity around eB
y .
3
The total torque Ci 2 R applied to link i is
Ci
2
=4
0
3
Chi 5
0
i
RB
Bi
1
2
4
0
Ch(i
0
1)
3
5 + RBi
Bi+1
2
Cvi
3
4 0 5
0
for i = 1; : : : ; n, where the relative rotation matrix is
I
i
RB
Bi+1 = RBi
T
RIBi+1 ;
2
4
Cv(i
0
0
1)
3
5 ; (4.89)
(4.90)
4.5 Accessing and Control of Joint Angles
89
Figure 4.6: Control torques for (a) side-view and (b) top-view.
and Ch0 = Cv0 = Chn = Cvn = 0. The total vector of applied torques
2 R6n employed in the equality of measures (4.69) is
2
3
03 1
6 C1 7
6
7
603 1 7
6
7
6
7
= 6 C2 7 :
(4.91)
6 .. 7
6 . 7
6
7
403 1 5
Cn
4.5
Accessing and Control of Joint Angles
In this section we will de…ne the joint angles and show how to control them.
The joint angles are not directly accessible from the non-minimal coordinates, but they can be calculated from the relative rotation matrices
Bi
i
RB
Bi+1 in (4.90). Assume that RBi+1 is constructed from the successive
rotations (i.e. Euler angles with the zyx-convention) zi , hi and vi :
i
RB
Bi+1 = R
Since we have cardan joints, let
2
cos
R zi = 4 sin
0
zi
zi
zi
R
hi
R
sin
cos
0
vi
zi
zi
:
(4.92)
3
0
05
1
i
be the rotation around the eB
z -axis and let
2
3
2
cos hi 0 sin hi
1
0
0
1
0 5; R vi = 40 cos
R hi = 4
sin hi 0 cos hi
0 sin
(4.93)
vi
vi
0
sin
cos
3
vi 5;
vi
(4.94)
90
3D Snake Robot Modelling for Simulation
B
i+1
i
describe the rotation around eB
, respectively. Hence, we should
y and ex
i
have that zi
0 and if the snake robot is lying such that the eB
y -axis is
pointing upwards, then hi describes the DOF of the cardan joint between
link i and link i + 1 that moves link i and i + 1 from side to side (i.e. the
yaw angle) and vi (the pitch angle) describes the lifting and lowering of
the links ( hi = vi = 0 when the snake robot is lying ‡at and straight on
the ground). The relative rotation matrix (4.92) can now be written as
3
2
i
4
RB
Bi+1 =
sin
hi
cos
hi
sin
vi
cos
hi
cos
vi
5:
(4.95)
i
where the elements of RB
Bi+1 which are not interesting for the imminent
computations are marked by .
We see from (4.95) that the joint rotation angles can be found as
vi
hi
for i = 1; : : : ; n
i
RB
Bi+1
= arctan
=
Bi
RB
i+1
arcsin
i
1 where RB
Bi+1
32
i
RB
Bi+1
32
(4.96)
33
31
;
(4.97)
i
is the element of the matrix RB
Bi+1 in
i
row 3 column 2 and the same notation applies for RB
Bi+1
31
i
and RB
Bi+1
33
.
By inspecting (4.95), we note that the expressions (4.96)-(4.97) are only
valid for 2 < vi < 2 and 2 < hi < 2 . However, most snake robot
motion patterns are possible without any larger feasible movement of the
joints and only these motion patterns are considered for simulation and
experiments in this thesis.
The rotational velocities of the joints are found directly from the rotational velocities Bi ! IBi of the links. We de…ne the rotational velocity for
sideways and lifting motion as
! hi
= dT
2 ! Ji ;
(4.98)
! vi
dT
1 ! Ji ;
(4.99)
=
respectively, for the joint between link i and i + 1, where dT
1 = 1 0 0 ,
T
d2 = 0 1 0 and
! Ji = Bi+1 ! IBi+1
i
RB
Bi+1
T
Bi ! IBi :
(4.100)
4.6 Numerical Algorithm: Time-stepping
91
Let the desired values of hi and vi be hi ;d and vi ;d , respectively.
In addition, the reference values for the joint velocities ! hi , ! vi are given
by _ hi ;d and _ vi ;d , respectively. Then the torques applied to the joints are
found by PD-controllers as
for i = 1; : : : ; n
for all i.
4.6
Chi
=
kP
hi ;d
hi
kD
_
Cvi
=
kP
vi ;d
vi
kD
_
hi ;d
! hi ;
(4.101)
vi ;d
! vi ;
(4.102)
1 where kP 2 R+ ; kD 2 R+ are positive constants equal
Numerical Algorithm: Time-stepping
In this section we will present the time-stepping method as the numerical
solution of the equality of measures for the 3D model of the snake robot.
The general form of the algorithm is similar to that presented for the 2D
model in Chapter 3. However, the non-minimal coordinates are found with
a di¤erent function and more contact impulse measures are added, both
since we are now dealing with a 3D model instead of a planar one.
4.6.1
Time Discretization
The time discretization of the equality of measures for the 2D model is
shown in Section 3.6 and the general approach is the same for the 3D model.
Therefore, for the 3D model, we will only state a concise algorithm that
constitutes the time-stepping method for the 3D model. An elaboration on
this procedure is found in Section 3.6. In the following, we will describe
the time-stepping algorithm for the 3D model.
The mid-point q M in the time-stepping method for the 3D model is
found from
t
F(q A )uA
(4.103)
qM = qA +
2
where we have used the equality p_ = 12 HT Bi ! IBi (Egeland and Gravdahl,
2002), and q A and uA are the coordinates and velocities of all links at the
92
3D Snake Robot Modelling for Simulation
beginning of a time-step. Moreover,
2
3
07 6
FH1 07 6
6
.. 7
607 6 FH2
. 7
7
6
F(q A ) = 6 .
7
.
.
.
4 .
. 07 6 5
07 6
07 6 FHn
FHi =
I3
04
3
3
03 3
7
1 T 2R
H
2 i
6
(4.104)
;
(4.105)
where Hi is found from (4.4) by inserting the orientation of link i at tA
found in q A .
The time-stepping method needs to be initialized before it can run. This
is done as follows: Set l = 1. De…ne q l as the initial position and orientation
of all links admissible with respect to the bilateral constraints, the unilateral
constraints and the unit-norm constraint kpi k = 1, i = 1; : : : ; n, where n
is the number of links. De…ne ul as the initial translational and rotational
velocity of all links. De…ne rN , rT , rV , rH > 0. Find the mass matrix
M from (4.70). Initialize all contact impulsions P N;l = 02n , P T;l = 04n ,
P V;l = 04n and P H;l = 0n where is the number of obstacles. De…ne the
start time tl and a small step size t. Now the time-stepping method for
the 3D model given by the following algorithm, can be iterated on.
Algorithm 4.1 (Time-stepping Method for the 3D Model)
1. Initialize time-step: tA = tl ; q A = q l ; uA = ul :
2. Find mid-point: tM = tA +
1
2
3. System vectors: hM = h (uA ) ;
t; q M = q A +
M
=
t
2 F(q A )uA :
(q M ; q A ) (see Remark 4.1).
4. Find active contact points:
IM;l = fajgNa (q M )
0g ;
HM;l = fbjgHb (q M )
0g :
(4.106)
5. Calculate generalized force directions:
WN M = WN (q M ) ;
WT M = WT (q M ) ;
WV M = WV (q M ) ;
WHM = WH (q M ) :
(4.107)
4.6 Numerical Algorithm: Time-stepping
93
6. Initialize and run Algorithm 4.2 with k = 1, P kN = P N;l , P kT = P T;l ,
P kV = P V;l and P kH = P H;l . The algorithm returns uE together with
all the contact forces P N E , P T E , P V E and P HE .
7. Find positions and orientations from uE and q M as
q E;M N A = q M +
t
F(q M )uE :
2
(4.108)
8. Initialize Algorithm 4.3 with q E;M N A and use the algorithm to …nd
q E which is admissible with respect to the bilateral constraints and the
unit-norm kpi k = 1 for i = 1; : : : ; n.
9. Prepare for next iteration: tE = tM +
tl+1 = tE;
1
2
t and
q l+1 = q E ;
ul+1 = uE ;
P N;l+1 = P N E ;
P T;l+1 = P T E ;
P V;l+1 = P V E ;
P H;l+1 = P HE :
(4.109)
Remark 4.1 (Algorithm 4.1 step 3) The torques M = (q M ; q A ) applied to the links are found from both q M and q A . This is because q M is
employed to determine how the control torques applied to the joints a¤ ect
each link (similarly to that WN M de…nes how the normal contact forces
with the ground a¤ ect the links). However, q M is only an internal state
in the numerical algorithm and is therefore not available to the controllers
(4.101)-(4.102) of an actual snake robot. Hence, to determine what torques
to apply in order to let each joint follow its desired trajectory, we need
to employ q A to …nd the relative joint angles and the relative rotational
velocities employed in the controllers.
4.6.2
Solving for Contact Impulsions
The Modi…ed Newton Algorithm employed in Step 6 in Algorithm 4.1 will
be presented for the 3D model in this section. The Modi…ed Newton Algorithm is utilized to …nd the velocities uE at the end of the time-step
l. In addition, the algorithm is employed to …nd the contact impulsions
PNa 2 R, P Ta 2 R2 , P Va 2 R2 and P J 2 R4(n 1) for a 2 IM;l and PHb 2 R
for b 2 HM;l (the subscripts on the contact impulsions refer to the element(s) in their respective vector P N , P T , P V , or P H the same way as
described for (4.76) on page 85). The contact impulsions consist of forces
acting during It and possible impulses acting in the time-interval It .
94
3D Snake Robot Modelling for Simulation
The Modi…ed Newton Algorithm is initialized as presented in Step 6 in
Algorithm 4.1.
Algorithm 4.2 (Modi…ed Newton Algorithm for 3D Model)
1. Solve
1
T
P kJ = WJM
M 1 WJM
h
T
T
WJM
uA WJM
M
1
hM t + S k +
t
i
(4.110)
;
where
Sk =
P
k
a2IM;l (WN M )a PNa
P
+ a2IM;l (WT M )2a 1 (WT M )2a P kTa
P
+ a2IM;l (WV M )2a 1 (WV M )2a P kVa
P
+ b2HM;l (WHM )b PHk b :
(4.111)
from
2. Solve uk+1
E
M uk+1
E
uA
hM t
Sk
WJM P kJ =
t:
(4.112)
3. Solve for a 2 IM;l and b 2 HM;l
PNk+1
= proxCN PNk a
a
rN
k+1
N Ea
;
(4.113)
k
P k+1
Ta = proxCT P Ta
rT
k+1
T Ea
;
(4.114)
k
P k+1
Va = proxCV P Va
rV
k+1
V Ea
;
(4.115)
PHk+1
= proxCH PHk a
a
rN
k+1
HEa
;
(4.116)
where
k+1
NE
k+1
TE
k+1
VE
k+1
HE
k+1
T
= WN
M uE ;
(4.117)
WTTM uk+1
E ;
WVT M uk+1
E ;
T
WHM uk+1
E :
(4.118)
=
=
=
(4.119)
(4.120)
4.6 Numerical Algorithm: Time-stepping
95
4. If
kP k+1
P kN k + kP k+1
P kT k
N
T
k+1
k+1
+kP V
P kV k + kP H
P kH k <
(4.121)
where > 0 is a user-de…ned tolerance, then stop the algorithm and
return
P N E = P k+1
P T E = P k+1
N ;
T ;
(4.122)
k+1
P V E = P V ; P HE = P k+1
H :
If the condition (4.121) is not satis…ed, then start over from Step 1.
Remark 4.2 (Implementation of the time-stepping method) It is
not necessary to calculate the complete W-matrices in Algorithm 4.1, step
5 for implementation of the time-stepping method in, for example, Matlab.
Only the columns of the matrices that correspond to the active contact points
need to be calculated for each time-step. The set of active contact points
also a¤ ects the number of contact impulsions that need to be considered.
Hence, a more e¤ ective way of writing Algorithm 4.1 and Algorithm 4.2
is to only calculate the contact impulsions, W-matrices and relative velocities associated with the active contact points de…ned by the sets IM;l and
HM;l . Moreover, Matlab o¤ ers a convenient programming environment for
implementing such an approach. However, we have chosen to present the
algorithms as they are to ease the understanding of the algorithms and to
avoid introducing additional notation and subscripts.
Most often, a higher value of the parameters rN , rT , rV , rH yields a
faster convergence rate at the risk of divergence. However, a general convergence result for the Modi…ed Newton Algorithm does not exist. See
Remark 3.1 on page 64 for a comment on the implementation of the Modi…ed Newton Algorithm.
The constitutive laws (4.113)-(4.120) used to describe the contact impulses are given on velocity level. This means that both the bilateral constraints on position level and the unit norm constraint on the Euler parameters are in general not satis…ed. A solution to these problems is suggested
in the following.
4.6.3
Constraint Violation
We initialize the link projection algorithm given by Algorithm 4.3 below as
described in Step 8 in Algorithm 4.1 with q E;M N A found in (4.108). The
link projection algorithm returns a vector q E of positions and orientations
96
3D Snake Robot Modelling for Simulation
of the links which are admissible both to the bilateral constraints introduced
by the joints and the unit-norm constraint on the quaternions.
Algorithm 4.3 (Projection of the Links)
1. Satisfy the unit-norm constraint kpi k = 1 by setting
pE;M N Ai
pEi =
; i = 1; : : : ; n;
kpE;M N Ai k
(4.123)
where pE;M N Ai is found from the vector q E;M N A and is the (nonunit-norm) quaternion. Let q E;quat be the new vector with positions
and the update orientations found with (4.123).
2. Employ q E;quat and let the orientation of link 1 be …xed. In addiBi
i
tion, let all the eB
z -axes be kept …xed. Now, change the ex - and
i
1, where
eB
y -axis such that gJi = 0 is satis…ed for i = 1; : : : ; n
the gap function gJi is found in (4.20). This is achieved as follows
for i = 1; : : : ; n 1: The current body axes can be found from the
corresponding rotation matrix as
RIBi q E;quat =
B
Bi
I ex
B
Bi
I ez
Bi
I ey
B
:
(4.124)
B
Denote the new ex i+1 - and ey i+1 -axis e
^x i+1 and e
^y i+1 , respectively.
Then we have that
B
e
^x i+1
B
e
^y i+1
=
=
B
i
e
^B
y
ez i+1
i
e
^B
y
ez i+1
B
;
B
e
^x i+1
B
e
^x i+1
ez i+1
ez i+1
(4.125)
B
B
:
(4.126)
Now, the new rotation matrix for link i + 1 can be constructed as
h
i
Bi+1
Bi+1
Bi+1
RIBi+1 = I e
;
(4.127)
^x
^y
Ie
I ez
and the …nal quaternions describing the orientation of the link are
found from the new rotation matrix. Let q E;rot be the new coordinate
vector with the updated quaternions.
3. Employ q E;rot and let the position of link 1 be …xed. Change the
positions of links i = 2; : : : ; n such that gJi = 0 is satis…ed for i =
1; : : : ; n 1, = x; y; z where the gap function gJi is given by (4.18).
Let q E be the new coordinate vector with the updated positions. Now,
return q E which satis…es the bilateral constraints and the unit-norm
constraint.
4.7 Summary
4.7
97
Summary
We have extended the non-smooth (hybrid) 2D model presented in Chapter 3 to 3D motion. The model enables synthesis and testing of 3D snake
robot motion patterns. It is based on the framework of non-smooth dynamics and convex analysis which allows us to easily and systematically
incorporate unilateral contact forces (i.e. between the snake robot and the
ground surface and the obstacles) and friction forces based on Coulomb’s
law of dry friction. In addition, the particular choice of non-minimal coordinates results in an well arranged structure of the elements in the equality of
measures which describes the motion of the snake robot. Conventional numerical solvers can not be employed directly for numerical treatment of the
non-smooth model due to the set-valued force laws and possible instantaneous velocity changes. Therefore, we show how to implement the model for
numerical treatment with the numerical integrator called the time-stepping
method. This method helps to avoid explicit changes between equations
during simulation even though the system is hybrid.
98
3D Snake Robot Modelling for Simulation
Chapter 5
Obstacle Aided Locomotion
5.1
Introduction
This chapter treats various aspects of obstacle aided locomotion with snake
robots in order to provide a better understanding of the simulation and
experimental results presented later in this thesis. We de…ne obstacle aided
locomotion as snake robot locomotion where the snake robot utilizes walls
or other external objects, apart from the ‡at ground, for propulsion.
This chapter is based on Transeth et al. (2007b) and Transeth et al.
(2008b) and it is organized as follows. We discuss the importance of obstacle aided locomotion in Section 5.2. In Section 5.3 a study by Gray (1946) is
employed to understand how snakes move forward by pushing against irregularities in the ground. Requirements for obstacle aided locomotion for a
snake robot are suggested in Section 5.4, while an approach to experimental
validation of obstacle aided locomotion is presented in Section 5.5.
5.2
Motivation
Biological snakes exploit objects and irregularities in their environment
to achieve more e¢ cient propulsion. A comparison with legged creatures
provides an indication of this statement. While the gait displayed by a
legged creature is highly dependent on the current speed of this creature,
the gait displayed by a snake will mainly be dependent on the properties of
the environment surrounding the snake. The ‡exibility and robust mobility
of snakes in cluttered environments is a signi…cant motivation for research
on snake robots. However, in order to fully bene…t from the advantages of
snake locomotion in such environments, as illustrated in Figure 5.1, snake
100
Obstacle Aided Locomotion
Figure 5.1: A snake robot in a cluttered environment (motivating illustration).
robots must learn to sense and understand the environment surrounding
them at any given time.
In this thesis the word obstacle is used to denote an object or an irregular
surface in the path of the snake robot that can be utilized for propulsion.
This may seem like a contradiction since it is, in fact, not an obstacle
from the point of view of the snake robot. However, the characterization is
valid in the sense of mobile robotics in general. The use of this denotation
therefore helps to emphasize one of the fundamental di¤erences between
snake locomotion and other traditional means of mobility, such as wheeled,
tracked and legged robots. While in traditional mobile robotics the aim is
typically to avoid obstacles, a snake robot should rather seek out and make
contact with obstacles since they represent push-points that can be utilized
for more e¢ cient propulsion. Hence, for snake robots the aim is therefore
not obstacle avoidance, but rather obstacle utilization.
5.3
Understanding Obstacle Aided Locomotion
The gait called lateral undulation represents a common form of snake locomotion (Mattison, 2002). During this motion a series of sinus-like curves
are propagated backwards through the snake body. These waves interact
with irregularities in the environment of the snake in such a way that the
resulting forces propel the snake forward. Progression by lateral undulation
is highly dependent on interaction forces from the environment. The study
of this motion pattern therefore provides a good understanding of how a
5.3 Understanding Obstacle Aided Locomotion
101
Figure 5.2: A snake robot with three links interconnected by two joints. The
resultant of the contact forces from the obstacles propels the snake. The
propelling force points in the forward direction if > , in the backward
direction if < , and is zero if = .
snake robot may use its environment in order to move forward.
A study of lateral undulation and its dependency on external pushpoints was given by Gray (1946). Gray depicted a scenario similar to the
one illustrated in Figure 5.2. It shows an articulated mechanism with three
links interconnected by two joints. Torques are applied to the two joints so
that each link is pushed towards a perfectly smooth obstacle, giving rise to
contact forces acting in the lateral direction of each link. Gray showed that
this mechanism will glide in the forward direction if the angle (between
link 1 and 2) is greater than the angle (between link 2 and 3) and in
the backward direction if
is smaller than . This may be understood
by studying the sum of the contact forces to the right in the …gure. It is
assumed that the resultant of the contact forces F sum is large enough to
overcome the friction forces from the ground.
A snake robot is constructed by serially connecting multiple mechanisms similar to the one in Figure 5.2. The structure of such a snake robot
is depicted in Figure 5.3 (a), where the snake robot posture and obstacles
in Figure 5.2 have been duplicated. Three push-points are needed for locomotion (Mattison, 2002) and we see from the Figure 5.2. that Obstacle
102
Obstacle Aided Locomotion
1 is associated with the normal reaction force that has an advantageous
contribution in the (upwards) direction of propulsion. This suggests that
Obstacle 1 should also be utilized for locomotion of the interconnected
structure in Figure 5.3 (a). Hence, a natural choice of push-points for obstacle aided locomotion by lateral undulation are the three occurrences of
Obstacle 1 depicted in Figure 5.3 (b). Moreover, we see from the …gure
that the push-points are close to the centre line of the snake robot posture.
This corresponds with the utilization of walls by snakes moving by lateral
undulation through a winding track depicted in Gray (1946). The sequence
of pictures of such a scenario in Gray (1946) clearly suggest that the snake
pushes against the walls of the track approximately along the centre line of
its undulatory motion. Hence, the angle between the tangent to the snake
body at the contact point and the direction of locomotion is large (see the
de…nition of in Figure 5.2 for an example of such an angle). This results
in an e¢ cient utilization of the push-points which is explained in the following. The reaction force from an obstacle can be decomposed into two parts:
a longitudinal and a lateral part with respect to the direction of locomotion.
If the push-points are far from the centre line of the snake robot posture
(for example if the two bottom obstacles in Figure 5.3 (b) were located
next to link 3 and link 6, respectively, instead), then the angles between
the links pushing against the obstacles and the centre line are decreased,
and a substantial portion of the normal contact reaction force points in
the lateral direction of locomotion and does not contribute to moving the
snake robot forward. In addition, larger joint torques are needed to obtain
the same longitudinal reaction forces compared to the situation where the
obstacles are located closer to the centre line. Hence, moving the contact
point to the centre line of the snake robot posture optimizes the longitudinal contribution of the normal contact reaction forces and reduces the
necessary joint torques.
5.4
Requirements for Intelligent Obstacle Aided
Locomotion
We suggest that three main physical properties in a snake robot are required
for e¢ cient obstacle aided locomotion in a cluttered and irregular environment. These are a smooth exterior body surface, a contact force sensing
system and an obstacle locating system. The …rst property will give the robot the ability to glide forward along any irregularities in its environment.
Hence, the friction forces between the snake robot and any projection from
5.4 Requirements for Intelligent Obstacle Aided Locomotion 103
Figure 5.3: Snake robot (a) with all obstacles from Figure 5.2, and (b) with
only the necessary and most suited obstacles for locomotion.
104
Obstacle Aided Locomotion
the ground used as a push-point should be as low as possible. Moreover,
it will mimic the skin of a biological snake more closely. Any irregularities
on the body surface of the snake robot will potentially obstruct the gliding
motion of the robot. The second property is crucial for intelligent obstacle
utilization. Contact force sensing allows the snake robot to detect when the
body is in contact with a push-point and also control the force exerted on
a push-point. Since the sum of contact forces along the snake body is what
propels the robot forward, the ability to measure these forces is important
in order to be able to control the propulsion. The third property enables
the snake robot to search for new push-points which it is not currently able
to touch (i.e. sense with the force sensors).
A control strategy for obstacle aided locomotion should in general consist of two continuous tasks running in parallel. The function of the …rst
task should be high-level motion planning. This task should establish some
sort of map of the environment surrounding the snake robot and use it to
plan a path through this environment. In contrast to conventional path
planning strategies for mobile robots, the goal should not be to avoid obstacles, but rather to move relative to these irregularities in a way that
enables the snake robot to utilize push-points for more e¢ cient propulsion.
The actual obstacle utilization should be continuously carried out in the
second task as a function of the current force measurements along the body
and the desired direction of motion. This task should basically control the
joints of the snake robot so that the sum of the contact forces along the
snake body is pointing toward the desired direction of motion.
One of the purposes of this thesis is to present a mathematical framework for the study of obstacle aided locomotion. This thesis will therefore
not treat the speci…c implementation of the motion controller discussed
above. However, we note that all the three above main physical properties
are in e¤ect available in the mathematical model presented in this thesis:
The exterior of the snake robot model is smooth, we are able to calculate the forces between the snake robot and the obstacles and we have an
obstacle locating system since the position of all obstacles are known in advance. Hence, the model can be employed to test intelligent obstacle aided
locomotion when such future control schemes are developed.
5.5 Experimental Observation of Obstacle Aided Locomotion105
5.5
Experimental Observation of Obstacle Aided
Locomotion
As described earlier in this chapter, the study of lateral undulation provides
a good understanding of how a snake robot may utilize interaction forces
from its environment in order to progress forward. The ‘serpenoid curve’
is a sinus-like curve that describes the shape of a snake during undulatory
motion (Hirose, 1993). Inspired by the serpenoid curve, we can imitate
the undulatory motion of a snake by setting the snake robot joint angles
according to
1) h ) ;
(5.1)
hi ;d = Ah sin (! h t + (i
where hi ;d is the desired relative angle between link i and i + 1, Ah is
the amplitude of joint oscillation, ! h determines the angular frequency of
oscillation of the joints, t is the time and h is the phase-shift between
adjacent joints (Saito et al., 2002).
Lateral interaction forces from the environment are needed for this gait
to progress the snake forward. The sum of these forces must act forward for
the snake robot to move in this direction. A simple way to study obstacle
aided locomotion is therefore to curve the snake robot to the initial shape
of this gait (according to (5.1) for t = t0 ) and place obstacles along the
snake robot similar to the position of the obstacles in Figure 5.3 (b). When
the snake robot starts the gait (5.1) from its initial (curved) shape, it will
progress forward by lateral undulation due to the reaction forces from the
obstacles. Note from (5.1) and Figure 5.3 (b) that the chosen amplitude
Ah and phase-shift h depends on the design of the snake robot since it
needs three push-points to move forward. Hence, we can choose from a
bigger variety of Ah and h for a snake robot with many links since it
might be able to utilize the obstacles for many di¤erent sinus-like shapes.
However, for a snake robot with fewer links, the choices of Ah and h are
more predetermined. This is because a small h will not result in a sinuslike shape, but rather a C-like shape. The snake robot will not be able to
utilize the three obstacles in a C-like shape based on the elaboration above.
The approximate distance between two adjacent obstacles are determined such that the snake robot is always in contact with three obstacles
simultaneously. In addition, it is assumed that the contact point with the
obstacles should be close to the centre-line of the serpenoid curve based on
the illustrations in Gray (1946). Note that the obstacles need to be placed
according to the shape the snake robot obtains by controlling its joints according to (5.1). We will see from the simulation results in Section 7.3.4
106
Obstacle Aided Locomotion
that a slight variation of the position of one obstacle does not seriously
a¤ect ability of the snake robot to move forward. However, if an obstacle is
moved far away from its original position, then the snake robot is not able
to complete its track while following the preprogrammed motion pattern
given by (5.1). The snake robot needs to be equipped with sensors to detect
contact with the obstacles for a scenario with randomly placed obstacles
and this is beyond the scope of this thesis.
A real rescue operation in a shattered building will require a snake robot
with a smooth and sealed exterior. This avoids that small pieces of rubble
get stuck in the joints of the snake robot and hinders it to move. However,
the principle motion pattern for how to move in such an environment can
be studied in a laboratory setting without having developed a robust snake
robot design …rst. To this end, we simply choose the obstacles large enough
so that they do not get stuck in the snake robot joints. This is advantageous
since it reduces the development time and cost of the snake robot used in
the experiments. Moreover, the basics of obstacle aided locomotion are
the same for small and large obstacles since the fundamental goal is still
to allocate enough push-points such that the snake robot is able to move
forward.
Chapter 6
Modelling and Control of a
Planar Snake Robot
6.1
Introduction
Many authors base the choice and implementation of the most common
serpentine motion pattern called lateral undulation on the serpenoid curve
found in Hirose (1993). This is a curve that describes the motion of a biological snake while moving by lateral undulation, and a snake robot can
follow an approximation to this curve by setting its joint angles according
to a sine-curve that is phase-shifted between adjacent joints. This approach
to snake robot locomotion has been widely implemented for snake robots
that have either wheels (Ostrowski and Burdick, 1996; Prautsch and Mita,
1999) or a friction property such that each link of the snake robot glides
easier forward and backward compared to transversal motion (Ma, 2001;
Saito et al., 2002). A no-slip constraint (i.e. a non-holonomic velocity constraint) on each wheel is sometimes introduced in the mathematical model,
thus avoiding that a link slips sideways. Such an approach is presented
in Prautsch and Mita (1999), where the no-slip constraint allows one to
signi…cantly reduce the model. A Lyapunov-based proof for this reduced
model together with a proposed controller shows that the snake robot is
able to move to a position reference. A controller for the velocity and heading for a wheel-less snake robot with the friction property described above,
is presented in Saito et al. (2002). The simulation and experimental results
presented in that paper indicate that the snake robot is able to stay within a
reasonable o¤set of the desired speed. However, no formal stability analysis
of the closed-loop system is given. For wheel-less snake robots it is not al-
108
Modelling and Control of a Planar Snake Robot
ways su¢ cient to consider a purely kinematic model for the motion pattern
lateral undulation, as the friction between the snake robot and the ground
surface may be essential for locomotion. Therefore, the friction needs to
be considered for wheel-less snake robots and this motivates including the
dynamics into controller synthesis and stability analysis of the snake robot.
The above published results all consider control of the position or orientation of a selection of snake robot links or some mean value of the velocity
and orientation of all the links of a snake robot. Hence, the main concern
is where some point on the snake robot is in space or where it is heading
with what velocity. However, for delicate operations such as inspection and
maintenance in industrial plants, every point on the snake robot body need
to be accurately controlled. As a …rst step toward designing such a control
scheme, we present in this chapter an input-output linearizing controller
for tracking control of snake robot joints. Such model-based control of the
internal con…guration of the snake robot has, to the best of our knowledge,
not been investigated before, and it results in a challenging stability analysis
due to the large number of degrees of freedom that needs to be considered.
It is hoped that the results presented in this chapter can be combined with
the controllers presented for control of the position and/or orientation of
the snake robot to be able to accurately control both the global position of
the snake robot in the plane together with precise control of the internal
con…guration of the snake robot. This way, every point on the snake robot
body relative to some …xed point in space can be accurately controlled.
In the previous chapters on modelling of a snake robot, we focused
on developing mathematical models for realistic and e¢ cient simulations
of planar and 3D snake robot motion. In this chapter we shift the main
focus to control. To this end, we develop a new 2D model of the snake
robot based on the standard robot manipulator model commonly employed
to model robot manipulators (usually with less degrees of freedom than
a snake robot) and marine vessels (Fossen, 2002; Spong and Vidyasagar,
1989). This model is denoted as the process plant model. By employing this
particular form of the model, a large area of techniques regarding control
and stability analysis of such systems becomes available. However, this
representation of a model of a snake robot (with such a large number of
degrees of freedom) is not advantageous for simulation as will be shown
in Section 7.3.5 so the 2D model developed earlier in this chapter is still
important. The process plant model contains the global orientation of the
snake robot. However, to make the stability analysis manageable we derive
a control plant model from the process plant model where this orientation is
6.2 Mathematical Modelling
109
removed. In this chapter, we employ one group of the techniques available
for the robot manipulator model, called input-output linearization (Khalil,
2002; Nijmeijer and Schaft, 1990) to develop a controller for tracking control
of the snake robot joints, and we show, using the control plant model, that
the joint angles converge exponentially to their desired trajectory. The
non-linear control techniques employed in this chapter are generally only …t
to cope with smooth non-linear systems. Moreover, few control techniques
exists for non-smooth systems such as the 2D model presented in Chapter 3.
Therefore, in order to make the control problem tractable, we approximate
the Coulomb friction with a smooth friction model and design a non-linear
control law for the resulting smooth system. Note that we do not give a
reference for the global position and orientation of the snake robot which
is beyond the scope of this thesis. Instead, we focus on proving that each
joint is able to track a desired reference angle.
This chapter is based on Transeth et al. (2007a) and it is organized
as follows. Section 6.2 presents mathematical models, both based on the
standard dynamics of a robot manipulator, of the snake robot and used
for simulation and stability analysis, respectively. The controller and the
stability analysis are given in Section 6.3 and a summary of the chapter is
found in Section 6.4.
6.2
Mathematical Modelling
In this section we …rst develop the mathematical model of a planar snake
robot without wheels based on the standard model for a robot manipulator with minimal coordinates found in for example Spong and Vidyasagar
(1989). We denote this model the process plant model and implement it
for two di¤erent constitutive laws for Coulomb friction. The process plant
model is necessary for us to be able to proceed to the next step which is to
reformulate the process plant model to obtain a control plant model used
both for designing the joint controllers and for deriving stability results
for the closed-loop system. The process plant model di¤ers mainly from
the 2D model presented in Chapter 3 in that the model is described by an
ordinary di¤erential equation (ODE) instead of an equality of measures, a
smooth approximation to the Coulomb friction is employed, and minimal
coordinates are used to describe the kinematics.
The snake robot di¤ers from a regular robot manipulator in that external forces act on each link instead of just the end-e¤ector. This calls
for a more extensive use of the various manipulator Jacobians employed
110
Modelling and Control of a Planar Snake Robot
to …nd the velocities of each link from the time-derivative of the minimal
coordinates and also to include the friction forces in the system dynamics.
Before we proceed with the modelling, let us …rst sum up the various
planar models we have discussed so far. The ‘2D model’ is presented in
Chapter 3 and is developed for e¢ cient simulations. In this section, we
also develop a 2D model denoted the ‘process plant model’, and this model
can also be employed for simulation. However, it is not advantageous for
simulation partly due to the large number of degrees of freedom of a snake
robot which results in large system matrices. Nonetheless, it may be convenient to employ the process plant model for simulation just in order to test
the controller since many of the system matrices utilized in the controller
are already available in the numerical treatment of the model. From the
process plant model, we will develop a ‘control plant model’for designing
the model-based controller and for stability analysis. We will sometimes
refer to the process plant model as the ‘robot model’throughout this thesis
in order to distinguish it from the ‘2D model’from Chapter 3.
6.2.1
Process Plant Model
In this section we derive a mathematical model of a wheel-less snake robot
based on the Euler-Lagrange equations as found in, for example, Spong
and Vidyasagar (1989). This model is denoted the process plant model.
We present two models for the Coulomb friction between the snake robot
and the ground. The …rst model is a smooth approximation to the Coulomb
friction. This approximation is performed to be able to employ standard
techniques for controller design. The second friction model is a model of setvalued Coulomb friction using a set-valued force law (similar to Chapter 3).
We implement the latter model to check how the controller works for a more
realistic model of a snake robot.
In the following we will …rst describe the kinematics and then we present
the dynamics of the snake robot.
Kinematics and Velocities
The 2D snake robot model presented in this chapter consists of n 2 equal
links connected by n 3 revolute joints, each with one degree of freedom
(DOF). See Figure 6.1 for an illustration of a snake robot with four links.
The inertial position and orientation of the snake robot adds three DOF
to the model. As opposed to n being the number of links in the previous
chapters, notice that n now describes the number of DOF of the system. To
6.2 Mathematical Modelling
111
Figure 6.1: Snake robot with 4 links. Notice that Gi is the centre of gravity
of link i = 5 in the picture.
be able to give a minimal coordinate representation of the snake robot in an
inertial frame, we need to include virtual joints in the model as introduced
for snake robots in Liljebäck et al. (2005). These joints consist of two
prismatic joints and one revolute joint to give the position and orientation,
respectively, of the …rst link of the snake robot with respect to an inertial
frame. Hence, the virtual joints are not physical joints located on the
snake robot, but rather just way to denote the position and orientation of
the …rst link of the snake robot in the plane. Due to these virtual joints,
we also obtain virtual links (link 1 and 2) in the model which do not have
mass. Each (real) link i = 3; : : : ; n has length Li and the position of the
centre of gravity (CG) is given by the point Gi at the middle of link i as
depicted in Figure 6.1. We lump the multi-point contact between link i
and the ground into the two points, Fi and Ri , each of distance LGSi from
Gi . We employ these two contact points to keep the model as simple as
possible while still accounting for the friction forces that arise from a purely
rotational motion of the link (this would not be the case if Gi were chosen
as the contact point). We only included one contact point in the planar
model in Chapter 3 to keep the model as simple as possible. However,
in this chapter the friction forces from a purely rotational motion need to
be included in the model since they become important for the subsequent
stability analysis.
We denote the origin of an earth-…xed frame I = OI ; eIx ; eIy with
origin OI as an approximation to an inertial reference frame. A general
112
Modelling and Control of a Planar Snake Robot
notation used throughout this chapter is that a position vector from the
origin of frame I to a point A is given by r A 2 R2 and a vector from point
A to point B is written as r AB . Vector r A described in frame I is written
Bi , for i = 3; : : : ; n, is
i
as I r A . The body …xed frame Bi = Oi ; eB
x ; ey
B
B
attached to the front of link i and ex i and ey i are pointing as shown in
Figure 6.1. The remaining frames Bi , i = 0; : : : ; 2, are utilized to give the
inertial position and orientation of the …rst (real) link of the snake robot
(later referred to as the ‘tail’ of the snake robot) and will be described
shortly. The transformation of a vector r between frame I and Bi is given
by
I
(6.1)
I r = RBi Bi r;
where RIBi is the corresponding rotation matrix.
T
We de…ne for convenience B0 = B1 = I and let I r O2 = xO2 yO2 be
the position of the rear part of the tail of the snake robot (link 3) where
B2
I
I
2
2 S 1 . Let
eB
x = ex and ey = ey . The orientation of link 3 is given by
1
3 be the relative joint angles between link j + 2 and
j 2 S , j = 1; : : : ; n
T
j + 3 and de…ne I r O0 = I r O1 = I r G1 = I r G2 = 0 0 . Subsequently,
the positions of the origin of each frame and the centre of gravity of each
link are found as
r Oi = r Oi
1
i
+ Li eB
x ;
Li Bi
e ;
2 x
r Gi = r Oi
(6.2)
respectively, for i = 3; : : : ; n.
The generalized coordinates for the snake robot are
q = xO2
yO2
T
1
n 3
2 Rn ;
(6.3)
where j is the joint angle between link j +2 and j +3. Hence, xO2 , yO2 and
represent the virtual joints, while 1 , 2 , . . . , n 3 represent the actual
joints of the snake robot.
A common way to de…ne the mapping from the time-derivative q_ of the
generalized coordinates to the linear v and angular ! velocity of link i is to
employ something called a manipulator Jacobian (Spong and Vidyasagar,
1989). In the following, we will derive these Jacobians for the points Gi in
order obtain the mass matrix, and the Jacobians for Fi and Ri to be able
to include friction in the system dynamics.
The translational velocity I v Gi of the centre of gravity of link i =
3; : : : ; n is found from
P
_
(6.4)
I v Gi = JvGi (q)q;
6.2 Mathematical Modelling
113
where we used the superscript P to denote the process plant model and the
Jacobian JPvGi (q) is found from
JPvGi (q) =
with
J PvGi
k
J PvGi
8
>
0
1
>
>
>
>
< 1 0
Bk 1
=
Ie
>
>
>
0
>
>
: 0
J PvGi
1
I r Gi
J PvGi
2
I r Ok
2 R2
n
n
;
; revolute joint k
1
(6.5)
i
; prismatic joint k
i
(6.6)
; k > i;
and is the axis of elongation for the prismatic joint k. The rotational
velocity ! i of link i = 3; : : : ; n is
_
! i = JP!i (q)q;
where JP!i (q) 2 R1
n
(6.7)
is
JP!i (q) =
where
J!Pi
k
J!Pi
1
J!Pi
J!Pi
2
(6.8)
n
8
< 1 ; revolute joint k i
0 ; prismatic joint k i
=
:
0 ; k > i:
(6.9)
The above Jacobians are employed in the system dynamics to …nd the
mass matrix as will be shown in the subsequent section. Since we have
de…ned the friction to act on the links at positions di¤erent from the centre
of gravity, we also need to de…ne an additional set of Jacobians. The
translational velocities of Fi and Ri are found from
Iv
i
_
= JPv i (q)q;
(6.10)
for i = Fi ; Ri and i = 3; : : : ; n, where the Jacobian JPv i (q) 2 R2
from
h
i
P
P
P
P
J
J
J
Jv i (q) =
;
v i
v i
v i
1
where
J Pv i
k
8
>
0
1
>
>
>
>
< 1 0
Bk 1
=
Ie
>
>
>
0
>
>
: 0
Ir
i
1
; revolute joint k
; prismatic joint k
; k > i;
is found
(6.11)
n
2
I r Ok
n
i
i
(6.12)
114
Modelling and Control of a Planar Snake Robot
and
i
r Fi = r Gi + LGSi eB
x ;
i
LGSi eB
x :
r Ri = r Gi
(6.13)
Dynamics
The equations of motion for the snake robot are based on the Euler-Lagrange
equations and are formulated in a standard matrix form for planar robot
manipulators (see for example Spong and Vidyasagar (1989)) as follows:
• + CP (q; q)
_ q_ = P
MP (q) q
Pn
T
P
P
P
+ i=3 JvF i
I F Fi + JvRi
T
P
I F Ri
;
(6.14)
where the mass matrix MP (q) 2 Rn n , the Coriolis and centripetal matrix
_ 2 Rn n , the applied joint torques P
CP (q; q)
and the friction forces
P
P
I F Fi and I F Ri will be described in the following paragraphs. We have
included the friction forces I F PFi and I F PRi acting on link i by using the
corresponding Jacobians. This is similar to how external forces acting on
the end-e¤ector of a robot manipulator is included in its system dynamics.
The mass matrix is written in a standard way (Spong and Vidyasagar,
1989) as
MP (q) =
n
X
mi JPvGi
T
JPvGi +
i
JP!i
T
JP!i ;
(6.15)
i=3
where mi and
i are the mass and moment of inertia of link i with respect
to its centre of mass, respectively (m1 = m2 =
1 =
2 = 0 due to
the virtualnjointsoand are therefore not included in MP (q)). By de…ning
_ = CPij we can …nd the elements of the
MP (q) = MPij and CP (q; q)
Coriolis and centripetal matrix as
CPij =
n
X
cijk q_k ;
(6.16)
k=1
where
cijk =
1
2
@MPkj
@qi
@MPij
@MPik
+
@qk
@qj
:
(6.17)
The torques 2 Rn 3 applied by the motors in the joints are mapped into
the system dynamics by the constant matrix
P =
03
In
n 3
3 n 3
;
(6.18)
6.2 Mathematical Modelling
115
where In 3 n 3 2 Rn 3 n 3 is the identity matrix. The friction between
the ground surface and the snake robot links is modelled as a combination
PV
of Coulomb friction F PFiC , F PRCi and viscous friction F FPiV , F R
. Hence, the
i
total friction force on the front and rear part of link i is
F Pi = F PC + F PV ;
i
(6.19)
i
for i = Fi ; Ri . We formulate the Coulomb friction force law in two ways.
P
First, we present a smooth approximation to Coulomb friction F Cc and
P
i
then we show how to …nd the set-valued Coulomb friction F Ct .
i
Coulomb friction is often modelled with a sign-function (Egeland and
Gravdahl, 2002; Saito et al., 2002) and the arctan-function can be employed as an approximation to the sign-function. De…ne arctan of a vector
xn as
argument x = x1 x2
2
3
arctan (x1 )
6 arctan (x2 ) 7
6
7
arctan (x) = 6
(6.20)
7:
..
4
5
.
arctan (xn )
P
Then we can …nd a smooth approximation of the Coulomb friction F Cc
i
from
1
Bi v i
PCc
;
(6.21)
= FNi D C arctan
Bi F
i
C
where the total normal force acting on link i is FNi = mi g, g is the acceleration of gravity, C > 0 is a small constant that determines how close the
arctan-function approximates the sign-function, the diagonal matrix
D
C
=
Cx
0
0
Cy
;
(6.22)
and Cx 2 R+ , Cy 2 R+ are the Coulomb friction coe¢ cients longitudinal
and lateral to each link, respectively. Notice that the total normal force FNi
is divided equally between the two contact points on each link in (6.21).
This can be seen from (6.21) since
lim
x!1
2
arctan (x) = 1:
(6.23)
In addition, we employ the common assumption for a planar model that
the normal forces are equally distributed along the snake robot (Ma, 2001;
Saito et al., 2002).
116
Modelling and Control of a Planar Snake Robot
P
Set-valued Coulomb friction F Ct is modelled using a set-valued force
i
law based on the framework of non-smooth dynamics and convex analysis,
and an expression for the friction is given with the algebraic inclusion
Bi v
2 N CT
i
Bi F
PCt
i
;
(6.24)
where NCT ( ) is the normal cone to the set CT given by the ellipse:
8
9
PCt 2
PCt 2
>
>
<
=
Bi F ix
Bi F iy
PCt
CT = Bi F
+
1
;
(6.25)
i
F Ni 2
F Ni 2
>
>
:
;
Cx 2
Cy 2
iT
P
P
P
where we have de…ned Bi F Ct = Bi F Ct Bi F Ct . A more thorough
iy
ix
i
explanation of set-valued force laws is found in Section 3.5.2. However, the
notation is di¤erent. This is because in this chapter we try to conform to
the standard notation employed for modelling of robot manipulators. Note
that when the set-valued force law (6.24) is employed to describe Coulomb
friction, then the time-stepping method with the Modi…ed Newton Algorithm can employed for simulation. Hence, the equation of motion (6.14)
has to be written as an equality of measures, e.g., on the form (3.43) on
page 54 and the same discretization technique as described in Section 3.6.1
can be employed. This procedure will not be repeated here.
The viscous friction force can be expressed in frame Bi as
h
Bi F
PV
i
=
FN i
D
2
V
Bi v
i
;
D
V
=
Vx
0
0
Vy
;
(6.26)
where Vx 2 R+ and Vy 2 R+ are the viscous friction coe¢ cients longitudinal and lateral to each link, respectively.
6.2.2
Control Plant Model
In this section the control plant model is derived from the process plant
model. We approximate the set-valued Coulomb friction law (6.24) by the
smooth non-linear friction model (6.21). This enables us to apply inputoutput linearization control techniques developed for smooth systems. We
T
reformulate the model such that the position I r O2 = xO2 yO2 and the
orientation of the …rst link of the snake robot are removed from the
state equations in (6.14). This allows for an in…nite number of rotations
of the snake robot without a¤ecting the controller (which is based on the
control plant model). We will see in the following that the translational
and rotational velocity of the …rst link will still be included in the model.
6.2 Mathematical Modelling
117
Kinematics
In this section we present the new kinematics where the inertial position
and orientation of the snake robot have been decoupled from the rest of
the system dynamics. To achieve this, we de…ne a new vector of velocity
coordinates that includes the velocity I v O2 = I r_ O2 in frame B3 attached
T
to the …rst link: B3 v O2 = RIB3
I v O2 . Hence,
q_ = Tq
(6.27)
m
_
= TT
q q;
(6.28)
RIB3
(6.29)
where
Tq = Tq ( ) =
2
B 3 v O2
=4
_
_
0n
2 2
3
5 2 Rn ;
02 n 2
;
In 2 n 2
2
3
1
6
7
6 2 7
= 6 . 7 2 Sn
4 .. 5
3
:
(6.30)
n 3
To be able to cancel from the model, we describe all translational velocities
in frame B3 . Let us investigate the new expression for the translational
velocity v Fi of the front part of link i as an example: By employing the
transformation I v Fi = RIB3 B3 v Fi and inserting (6.27) into (6.10), we …nd
that
(6.31)
B 3 v Fi = J vF i ( ) ;
where
JvF i ( ) = RIB3
T
JPvF i Tq :
(6.32)
The expression for B3 v Ri is the same as (6.31) except that Fi needs to be
replaced by Ri . The Jacobian JvRi ( ) is found similarly to JvF i ( ) as
JvRi ( ) = RIB3
T
JPvRi Tq :
(6.33)
The Jacobian JvF i ( ) (or JvRi ( )) which relates the new generalized velocities and the translational velocity B3 v Fi (or B3 v Ri ) does not depend
on the earth-…xed rotation angle of the snake robot tail (link 3). Namely,
the translational velocities of the snake robot tail in are given in frame
B3 and an earth-…xed rotation angle is therefore no longer necessary to …nd
the translational link velocities in frame B3 from .
118
Modelling and Control of a Planar Snake Robot
Dynamics
In this section we employ the transformations above to reduce the model
of the dynamics (6.14). From (6.27), we …nd that
_ q + Tq _ ;
•=T
q
(6.34)
and this equality together with (6.27) and the relation I F Pi = RIB3
are inserted into the process plant model (6.14) to obtain
_ q + CP (q; q)
_ Tq
MP (q) Tq _ + MP (q) T
=P
Pn
T
T
+ i=3 JPvF i RIB3 B3 F PFi + JPvRi RIB3 B3 F PRi :
B3 F
P
i
(6.35)
By inspection Tq in (6.29) and P in (6.18), we see that
TT
qP =P :
(6.36)
We pre-multiply (6.35) with TT
q , and using (6.32) and (6.36), we obtain
where
M ( ) _ + C _; ; _
=P
Pn
T
+ i=3 (JvF i ) B3 F Fi + (JvRi )T
B 3 F Ri
(6.37)
:
M ( ) = TT
q MP (q) Tq ;
C _; ; _
(6.38)
_
_ Tq ;
= TT
q MP (q) Tq + CP (q; q)
(6.39)
and we have used that
F
for
i
i
= F Ci + F Vi = F Pi ;
(6.40)
= Fi ; Ri where the viscous friction term F Vi = F PV is the same
P
i
as in (6.26), while the smooth approximation F Ci = F Cc in (6.21) of
i
the Coulomb friction in the process plant model is employed. We note
from (6.38)-(6.39) that the new system matrices M ( ) and C _ ; ; _
are independent of the global orientation . This is because our choice of
new coordinates renders the new equation of motion (6.37) independent of
since all link velocities are described in frame B3 instead of frame I.
6.3 Joints Control by Input-output Linearization
6.3
119
Joints Control by Input-output Linearization
In this section we employ an input-output (IO) linearizing controller to be
able to accurately track the desired joint angles and prove mathematically
that this closed-loop system is stable. Inspired by Spong (1996), we divide the system (6.37) into two parts: one part for the directly actuated
generalized coordinates called active (or real) joints, and one part for the
unactuated degrees of freedom which are the passive (or virtual) joints. We
will then show that the origin of the tracking error dynamics of the active
joints are asymptotically stable and that the IO-linearizing controller can
be employed.
6.3.1
Control Plant Model Reformulation
We begin by dividing the control plant model (6.37) into two parts (as
performed for the standard robot manipulator model in Spong (1996)):
one part for the directly actuated generalized coordinates
called active
joints and one part for the unactuated generalized velocities w which are
the passive joints. We de…ne the unactuated generalized velocities as
w=
B 3 v O2
2 R3 ;
_
(6.41)
and we de…ne a constant matrix employed often in the following as
Pw =
I3 3
2 Rn
0n 3 3
3
:
(6.42)
De…ne
h = h ( ; ) = C _; ; _
g = g( ; ) =
n
X
(6.43)
;
(JvF i )T
B3 F F i
+ (JvRi )T
B 3 F Ri
;
(6.44)
i=3
where
h( ; ) =
h1
2 Rn ;
h2
g( ; ) =
g1
2 Rn ;
g2
(6.45)
120
Modelling and Control of a Planar Snake Robot
and
_; ; _
h1 = PT
wC
2 R3 ;
h2 = PT C _ ; ; _
2 Rn
3
(JvF i )T
B3 F Fi
+ (JvRi )T
B 3 F Ri
2 R3 ;
(JvF i )T
B3 F Fi
+ (JvRi )T
B 3 F Ri
2 Rn
g1 =
PT
w
g 2 = PT
n
X
i=3
n
X
(6.46)
;
(6.47)
i=3
(6.48)
3
: (6.49)
From (6.30) and (6.41)-(6.49), the control plant model (6.37) may be written as follows:
_ + M12 • + h1 = g 1
M11 w
_ + M22 • + h2 = g 2 + ;
M21 w
(6.50)
(6.51)
where the mass matrix M ( ) in (6.38) is divided as
M( ) =
M11 M12
;
M21 M22
M11 2 R3
3
:
(6.52)
In the forthcoming section, we denote (6.50) as the tracking dynamics and
(6.51) as the input-output dynamics.
6.3.2
Controller Design
In this section we derive an input-output linearizing controller such that
the input-output dynamics (6.51) is able to asymptotically track a bounded
reference d (t), with bounded derivatives _ d (t), • d (t).
Since M ( ) is positive de…nite then the upper left submatrix M11 in
(6.52) of M ( ) is positive de…nite and invertible and we can …nd an ex_ from (6.50):
pression for w
_ = M111 g 1
w
M111 M12 •
M111 h1 ;
(6.53)
_ into (6.51) to obtain
and insert the new expression for w
M22 • + h2 = g 2 + ;
(6.54)
where
M22 = M22
h2 = h2
g2 = g2
M21 M111 M12 ;
M21 M111 h1 ;
M21 M111 g 1 ;
(6.55)
(6.56)
(6.57)
6.3 Joints Control by Input-output Linearization
121
Note that M22 is symmetric and positive de…nite (Gu and Xu, 1993). The
friction and system dynamics may now be cancelled from (6.54) by employing the feedback transformation
= M22 u + h2
g2;
(6.58)
where u 2 Rn 3 is a new control input. The input-output dynamics (6.51)
is now reduced to
• = u:
(6.59)
Hence, by using the stabilizing feedback law for u:
u = •d
KD
_
_
d
KP (
d) ;
(6.60)
we obtain a linear and asymptotically stable error dynamics for the inputoutput dynamics (6.51) for the n 3 n 3 positive de…nite, diagonal
constant matrices KD and KP . We see from (6.44) that g 2 in the controller
(6.58) contains the Coulomb and viscous friction terms. Now, the reason
for the approximation (6.21) of the Coulomb friction becomes apparent.
Namely, due to the set-valued Coulomb friction in the process plant model
we are not able to compensate exactly for the Coulomb friction at zero
velocity since the friction force can be anywhere in the set CT in (6.25).
To be able to include the case of zero velocity in the stability proof we
therefore approximate the Coulomb friction with the smooth friction model
as in (6.21). We see from (6.58) and (6.60) that the computed torque only
depends on , and d and its time-derivatives. However, to implement
the controller (6.58), (6.60) on an actual snake robot we would also have
to …nd and I v O2 since B3 v O2 cannot be measured directly and has to be
T
calculated from B3 v O2 = RIB3
I v O2 .
6.3.3
Final Results on Input-output Linearization
We start this section by stating the following theorem.
Theorem 6.1 Assume that the desired trajectory d (t) 2 Rn 3 for the
joints is bounded with bounded acceleration • d (t) and bounded velocity
_ (t)
c _ for all t for some su¢ ciently small c _ > 0. Along any
d
trajectory of the closed-loop (6.50)-(6.51), (6.58), (6.60), the joint tracking
error e (t), de…ned by
e (t) =
(t)
d (t) ;
(6.61)
converges to zero exponentially and the velocity vector w (t) remains bounded.
122
Modelling and Control of a Planar Snake Robot
Proof. The desired trajectory d together with its time-derivatives typically satis…es the boundedness assumption (for example by employing the
standard expression di (t) = A sin (!t + (i 1) ) for constant A, ! and
).
The exponential convergence to zero of the joint tracking error e (t) is
seen by inserting (6.60) into (6.59) and using (6.61). Thus, we end up with
a linear system for the error dynamics:
• (t) + KD e_ (t) + KP e (t) = 0:
e
(6.62)
Hence, the origin of (6.62) is exponentially stable and the error e (t) converges to zero exponentially. This can easily be shown since (6.62) is a
system of n 3 decoupled linear second order systems of the form
e•
j
+ KDj e_
j
+ KPj e
j
= 0;
j = 1; : : : n
3;
(6.63)
where we have de…ned e = [e 1 ; e 2 ;
; e n 3 ]T and KDj and KPj are the
j-th components along the diagonals of KD and KP , respectively. Each of
these systems is globally exponentially stable, because all KDj and KPj are
positive. Hence, the overall system (6.62) is globally exponentially stable.
Notice from (6.61) that
(t) = e (t) +
d (t) :
(6.64)
Hence, (t) is bounded since both e (t) and d (t) are bounded. The
same applies to the …rst and second derivative of (t). It now remains to
show that the velocity vector w remains bounded1 . In other words, the
so-called tracking dynamics (6.50) produces bounded solutions for timevarying inputs (t), _ (t) and • (t) with (t) satisfying (6.64). To this
_ in (6.50) is locally Lipschitz and
end, we note that the expression for w
employ Theorem 4.18 in Khalil (2002) (see Appendix A.1 for the theorem)
in the following. Hence, we de…ne the continuously di¤erentiable function
1
V (t; w) = wT M11 ( (t)) w:
2
(6.65)
Thus,
1
1
2
2
V (t; w)
(6.66)
M;min kwk
M;max kwk
2
2
where k k denotes the Euclidean norm, M;min is the in…mum over all t of
the smallest eigenvalue of M11 ( (t)) and M;max is the supremum over all
1
For notational simplicity, we omit the dependence on t and
when it is appropriate.
(t) in the following
6.3 Joints Control by Input-output Linearization
123
t of the largest eigenvalue of M11 ( (t)). Since M11 ( ) is continuous and
positive de…nite for all
and since (t) belongs to a compact set for all
t, then such M;max
M;min > 0 do exist. What remains is to …nd a
continuous positive de…nite function W (w) such that
V_ (t; w)
W (w) ;
8 kwk
> 0;
8t
0;
(6.67)
for some > 0 and all w 2 R3 . This will be the task for the remainder of
this section.
Consider
1
_ 11 w:
_ + wT M
V_ = wT M11 w
(6.68)
2
_ from (6.50), we …nd that
Inserting for M11 w
V_ = wT g 1
M12 •
1
_ 11 w:
h1 + wT M
2
(6.69)
The velocities w need to be extracted from g 1 in (6.48) and h1 in (6.46) to
satisfy the above inequality (6.67). To this end, we rewrite in (6.30) as
= Pw w + P _ :
(6.70)
where Pw is found in (6.42) and P is found in (6.18).
We start by investigating h1 which is the term in V_ that needs to be
elaborated the most to prove the inequality (6.67). By inserting the identity
(6.70) into the expression (6.46) for h1 , we …nd that
_ ; ; _ Pw w + PT C _ ; ; _ P _ :
h1 = PT
wC
w
(6.71)
_ ; ; _ Pw w is cancelled from V_
It will later be shown that the term PT
wC
because of skew-symmetry, however _ and thus an expression for w, needs
to be extracted from the second term in h1 and this will be elaborated
_ q P = 0 (I_ n 2 n 2 = 0) together with
on in the following. By using T
the expression for C _ ; ; _ in (6.39), we write a part of the right-most
expression in (6.71) as
T T
PT
w CP = Pw Tq CP P ;
(6.72)
where we also have used that Tq P = P . De…ne cj to be the j-th column
of CP and
01 j 1 1 01 n j 2 Rn
pT
(6.73)
j =
124
Modelling and Control of a Planar Snake Robot
is a row vector of zeros with 1 on its j-th column. From the expression for
the elements of CP in (6.16) we …nd that
_
cj = DPj q;
where
DPj
2
c1j1
(6.74)
c1j2
6
6 c2j1 c2j2
= fcijk g = 6
6 ..
4 .
cnj1
..
.
3
c1jn
.. 7
. 7
7
.. 7 ;
. 5
cnjn
(6.75)
and cijk is given by (6.17). Hence, from (6.74), together with the previous
expression (6.16) for CP , we see that
CP =
n
X
cj pT
j =
j=1
n
X
_ T
DPj qp
j:
(6.76)
j=1
By calculating the partial derivatives of the elements of MP in (6.17) we
…nd from the expression for the elements of DPj that its …rst two columns
are zero. Hence, by comparing q_ in (6.3) with in (6.30) we obtain
DPj q_ = DPj :
(6.77)
We can now continue to expand CP by inserting the identities (6.70) and
(6.77) into (6.76):
CP =
n
X
DPj pT
j =
j=1
n
X
DPj Pw w + P _ pT
j:
(6.78)
j=1
By inserting the new expression for CP (6.78) into (6.72) and noticing that
pT
j P = 01 n 3 for j = 1; : : : ; 3 we obtain
T T
PT
w CP = Pw Tq
n n
o
X
DPj Pw w + DPj P _ pT
jP :
(6.79)
j=4
Inspecting the terms with w in the summation (6.79) for j = 4; : : : ; n we
…nd that
pTj 3 = pT
(6.80)
jP ;
where
pTj
3
= 01
j 3 1
1 01
n j
2 Rn
3
(6.81)
6.3 Joints Control by Input-output Linearization
is a row vector of zeros with 1 on its j
125
3-column, and we note that
_ = pT _ ;
k
k
where
k
2 Rn
is the k-th element of
n
DBj ( ) = TT
q DPj 2 R
3.
n
(6.82)
In addition, let
;
j = 4; : : : ; n;
(6.83)
to explicitly show that we are dealing with a matrix with elements only dependent on . By inserting (6.80) into (6.79), and employing the identities
(6.82) and (6.83), we now …nd the second term of h1 in (6.71) as
PT
w CP
_ = PT
w
n
X
DBj P _ _ j
3
+ Pw _ j
(6.84)
3w
j=4
and we have managed to extract w.
We are now ready to insert h1 into V_ in (6.69):
De…ne
_ 11 w+
V_ = wT g 1 + wT ( M12 ) • + 21 wT M
C) Pw w+
w T PT
w(
Pn
DBj P _ _ j 3 +
w T PT
w
j=4
P
n
DBj Pw _ j 3 w:
w T PT
w
j=4
k1
;•
= ( M12 ) • ;
k2
;_
= PT
w
n
X
(6.85)
(6.86)
DBj P _ _ j
3;
(6.87)
j=4
and note that the following inequality holds:
w T ki
kwk kki k :
(6.88)
We also have that
w T PT
w
where
1
2
PT
w
DBj ( (t)) Pw _ j
DBj ;max (
)
DBj ( ) Pw +
DBj ( ) through cos
such that
j
3 (t)w
is
PT
w
the
j
)
T
kwk2 ;
(6.89)
eigenvalue
of
(t)) _ j
largest
DBj ( ) Pw
and sin
DBj ;max (
DBj ;max (
.
Since
3 (t)
only enters
, then there exists a constant
DBj ;max
8 :
DBj ;max
(6.90)
126
Modelling and Control of a Planar Snake Robot
From (6.90) we de…ne the constant
D
=
n
X
DBj ;max :
(6.91)
j=4
We now have from (6.89)-(6.91) that the latter expression in (6.85) satis…es
w T PT
w
n
P
DBj Pw _ j
j=4
n
P
3w
DBj ;max (
j=4
) _j
3 (t)
D
kwk2 sup max
D
kwk2 sup _ (t) ;
_
t 0 j=4;:::;n
kwk2
j 3 (t)
(6.92)
t 0
where we have employed (see, e.g., Khalil, 2002)
max
_
j=4;:::;n
j 3 (t)
=
_ (t)
1
_ (t)
;
_ (t)
1
2
:
(6.93)
By using the inequality (6.92) together with (6.86)-(6.89), we can simplify
inequality (6.85) as
V_
_ 11
wT g 1 + 21 wT M
2PT
w CPw w+
kwk kk1 k + kwk kk2 k +
D
kwk2 sup _ (t) :
(6.94)
t 0
The above expression (6.94) can be further simpli…ed since
1 T _
w M11
2
1 T T _
2PT
w CPw w = w Pw M
2
2C Pw w = 0;
(6.95)
_ 11 = PT MP
_ w and that M
_
where we have used that M
2C is skeww
symmetric. The latter property is proved in, e.g., Spong and Vidyasagar
(1989).
The remaining term wT g 1 in V_ arises from friction, see (6.48), and will
now be investigated. We start by partitioning the complete friction term g
in (6.44) as
g = g V + g C =) g 1 = g V1 + g C1 2 R3 ;
(6.96)
where
g V1 = PT
w gV ;
g C1 = PT
w gC ;
(6.97)
6.3 Joints Control by Input-output Linearization
127
and the viscous friction g V and Coulomb friction g C are
=
gV
=
gC
n
X
i=3
n
X
(JvF i )T B3 F VFi + (JvRi )T
V
B 3 F Ri
;
(6.98)
T
(JvF i )T B3 F C
Fi + (JvRi )
C
B 3 F Ri
:
(6.99)
i=3
By inserting the constitutive law (6.26) for the viscous friction force and
V
3
employing the identities B3 F Vi = B3 F PV and B3 F Vi = RB
Bi Bi F i for i =
i
Fi ; Ri , we …nd that
gV =
n
X
X FN
i
2
Jv
T
3
RB
Bi D
i
V
Bi v
:
i
(6.100)
i=3 =R;F
Using and in expression (6.31) for
g V in (6.98), we get
gV =
n
X
X
Jv
T
i
3
RB
Bi
i=3 =R;F
|
Bi v
FN i
D
2
{z
i
V
and inserting the result into
3
RB
Bi
T
Jv
MV ( ) =
n
X
(6.101)
}
MVi ( )
We now …nd the pleasant fact that
:
i
MVi ( )
(6.102)
i=3
is positive de…nite as shown in Appendix A.2. Intuitively, this is the case
since MV is the analogy to a mass matrix of interconnected point masses at
any time-instant and such a mass matrix is positive de…nite. We formulate
g V1 de…ned in (6.97) by using the expression for in (6.70) and inserting
MV (6.102) into (6.101), then
g V1 =
PT
w MV Pw w
_:
PT
w MV P
(6.103)
We now move on to the Coulomb friction term g C since we have managed to extract w from g V1 . The Coulomb friction force F Ci is found as the
smooth approximation of the Coulomb friction force in (6.21) on page 115
and by inserting this into the expression for g C in (6.99), we obtain
gC =
n
X
1
i=3
X
i =Fi ;Ri
Jv
T
i
3
FNi RB
Bi D
C
arctan
Bi v
C
i
:
(6.104)
128
Modelling and Control of a Planar Snake Robot
By employing the expressions above that constitute g 1 , we …nd from (6.94)
that the function V_ can now be written
V_
_ +
PT
w MV P
T
w T PT
w MV Pw w + w
wT g C1 + kwk (kk1 k + kk2 k) +
D
kwk2 sup _ (t) :
(6.105)
t 0
De…ne the vectors
k3 =
_;
PT
w MV P
k 4 = g C1
;
Bi v
i
(6.106)
:
(6.107)
In addition, de…ne the positive de…nite matrix
3
MV = PT
w MV Pw 2 R
3
:
(6.108)
The matrix MV is positive de…nite since it is the upper left submatrix of
the positive de…nite matrix MV . By inserting the identities (6.106)-(6.108)
into (6.105), we obtain
V_
wT MV w +
D
kwk2 sup _ (t) +
t 0
(6.109)
kwk (kk1 k + kk2 k + kk3 k + kk4 k) :
Recall that due to (6.64) _(t) and • (t) are bounded, i.e. there exists a
constant c~ _ such that _ (t)
c~ _ and • (t)
c~ _ for all t 0. For the
expressions for k1 , k2 , k3 and k4 in (6.86), (6.87), (6.106) and (6.107), this
implies that there exists a constant k5 > 0 such that
kk1 k + kk2 k + kk3 k + kk4 k
k5 ;
8t
0:
(6.110)
Since enters MV ( ) only through cos j and sin j and MV ( ) is
continuous and positive de…nite, then there exists a constant MV ;min > 0
such that
2
wT MV w; 8 :
(6.111)
MV ;min kwk
Hence,
V_
MV ;min
D
sup _ (t)
t 0
kwk2 + kwk k5 :
(6.112)
Since _ (t) is bounded, the inequality (6.112) implies that V does not
grow faster than an exponent, i.e. solutions w (t) of the system (6.50) are
6.3 Joints Control by Input-output Linearization
129
de…ned for all t 0. Moreover, since _ (t) _ d (t) ! 0 as t ! 0 there exists
a time instant T such that _ (t)
2c _ where c _ is the upper bound of
_ (t) from the formulation in Theorem 6.1. Choose this c _ > 0 such
d
that
D 2c _
MV ;min
> 0 and de…ne the constant
k6 =
Then, for t
MV ;min
D 2c _
> 0:
(6.113)
T we have
V_
k6 kwk2 + k5 kwk :
(6.114)
We are now close to satisfying the inequality (6.67) which we stated in
the beginning of this section. De…ne a constant such that 0 < < 1. We
can then rewrite (6.114) as
V_
(1
)k6 kwk2
k6 kwk2 + k5 kwk :
Hence, the positive de…nite function W (w) in (6.67) is
W (w) = (1
)k6 kwk2 ;
(6.115)
and we obtain
V_
W (w) ;
(6.116)
for
k6 kwk2
k5 kwk ;
(6.117)
The positive constant in (6.67) needs to be found such that (6.116) is
satis…ed. From (6.117) we …nd that
kwk
k5
= :
k6
(6.118)
Hence, we have that
V_
W (w) ;
kwk
> 0;
(6.119)
for t
T . This implies that for t
T the solutions w (t) are bounded.
Moreover, (6.112) ensures that solutions w (t) of the system (6.50) are
de…ned for all t
0, i.e. solutions can not escape to in…nity in the timeinterval from 0 to T . These two latter results together with (6.66) prove
that w is bounded. This concludes the proof of Theorem 6.1.
130
6.4
Modelling and Control of a Planar Snake Robot
Summary
This chapter considers the problem of model-based control of the joints of a
snake robot without wheels. The potential range of applications of snake robots is wide and delicate operations such as inspection and maintenance in
industrial environments or performing search and rescue operations require
precise control of snake robot joints. To this end, we present a controller
that asymptotically stabilizes the joints of the snake robot to a desired reference trajectory. We have developed a planar model of the snake robot based
on the standard dynamics of a robot manipulator. This model is denoted
the process plant model. Such a formulation is advantageous for control
since it allows us to employ the large variety of control techniques available
for robot manipulators. Our controller is based on input-output linearization of a control plant model, which is a reformulation of the process plant
model, of the snake robot dynamics also developed in this chapter. The
reformulation of the process plant model is necessary to make the stability
analysis more manageable and exponential stability of the desired trajectories for the joints is shown with a formal Lyapunov-based stability proof.
The performance of the controller will be tested in simulation on both a
smooth and non-smooth planar snake robot model in Chapter 7.
Chapter 7
Simulation and
Experimental Results
7.1
Introduction
The ability to investigate a system by simulation, as opposed to testing the
actual system, is a powerful tool for testing and evaluating the system as
long as the model represents the actual system dynamics with su¢ ciently
high accuracy. In general, simulations allow one to evaluate the system
without risking damaging it or wearing it out. In addition, it is easier to
try out various initial conditions since the actual system does not need to
be manipulated for each trial.
In this chapter we present simulations of the various snake robot models
presented throughout this thesis. The algorithms for the time-stepping
method presented for the 2D and 3D model have been implemented for
numerical treatment of the models and we investigate whether the timestepping method is suitable for simulating the models. We have qualitative
knowledge from previously published papers on snake robots of how a snake
robot should perform during various motion patterns. In this chapter we
will test a selection of these gaits to see whether the snake robot moves
as expected with our models. Moreover, to further validate the model,
experimental results will be presented for the NTNU/SINTEF snake robot
‘Aiko’ and these results will be compared to the simulation results from
both the 2D model presented in Chapter 3 and the 3D model presented in
Chapter 4. We will also investigate how the 2D and 3D model performs for
obstacle aided locomotion with small obstacles. Such small obstacles may,
in general, cause problems due to numerical issues and that the obstacles
132
Simulation and Experimental Results
may get stuck in the snake robot due to the design of the exterior of the
snake robot. In addition, the 3D model will be tested for a motion pattern
based on rolling to motivate the inclusion of the exoskeleton de…ned for
contact with the ground surface. Moreover, the input-output linearizing
controller presented in Chapter 6 will be evaluated for an idealized snake
robot model and for a model that includes various degrees of noise. These
results will be compared to an ordinary PD-controller. Both of these latter
models are based on the ‘robot model’developed in Chapter 6.
This chapter includes the results from Transeth et al. (2007a), Transeth
et al. (2008b) and Transeth et al. (2008a) and it is organized as follows.
A snake robot called ‘Aiko’ – the basis for the models – is presented in
Section 7.2 together with all simulation parameters. Simulation results are
given in Section 7.3. The experimental setup is presented in Section 7.4
and simulations with back-to-back comparisons to experimental results are
given in Section 7.5. A summary of the most important …ndings in this
chapter is given in Section 7.6.
7.2
Simulation Parameters
The snake robot ‘Aiko’is used as a basis for the mathematical models. In
this section we will describe the most important characteristics of the snake
robot and use these to determine the parameters for the models presented in
Chapters 3, 4 and 6. In addition, we will present the simulation parameters.
7.2.1
A Description of Aiko and Model Parameters
Aiko is depicted in Figure 7.1 and has eleven (red) links connected by ten
two DOF joints. As seen from the …gure, the snake robot has cylindrical links and a half-sphere made of metal as a ‘head’ which may be removed. Aiko weighs 7.5 kg and each link is 12.2 cm long. The snake robot
was developed1 at the NTNU/SINTEF Advanced Robotics Laboratory in
Trondheim, Norway in the year 2006.
1
General requirements for Aiko were formulated by Pål Liljebäck and Aksel Andreas
Transeth. These requirements included that the snake robot should have 2 DOF joints, be
able to lift three or more of its links and have an as smooth as possible exoskeleton to be
able to use obstacles to move forward and/or move over them. Moreover, the exoskeleton
should be cylindrical to be able to easily move by e.g. lateral rolling. Sverre Brovoll
and Kristo¤er Nyborg Gregertsen developed Aiko based on these requirements during
S. Brovoll’s Master’s degree and K. N. Gregertsen’s summer job at SINTEF under the
supervision of P. Liljebäck, A. A. Transeth and Amund Skavhaug. A detailed description
of the design and components of Aiko is found in Brovoll (2006).
7.2 Simulation Parameters
133
Figure 7.1: The NTNU/SINTEF snake robot ‘Aiko’.
As mentioned above, we employ Aiko as a basis for choosing the model
parameters. These parameters are given in Table 7.1 for the 2D model from
Chapter 3, the 3D model from Chapter 4 and the model from Chapter 6.
The latter will be referred to as the ‘robot model’ since it is based on
the standard robot manipulator model form. The moment of inertia for a
link was calculated by assuming that each link was a cylinder of uniform
mass with height Li and radius LSCi . The coe¢ cient of friction between
the snake robot and the ground was calculated from the measurements
obtained by connecting Aiko to a scale and then pulling the scale along one
of the particle boards Aiko is moving on during the experiments.
7.2.2
Simulation Parameters and Reference Joint Angles
The numerical integration of the various models requires …rst deciding the
simulation parameters, such as step-length and di¤erent tolerances. Most
of the remaining parameters needed to perform numerical integration of
the models presented in Chapters 3, 4 and 6 are given in Table 7.2. Some
parameters are varied in the upcoming presentation of the simulations to
test how the snake robot behaves. The new values will then be clearly
stated.
We employ the same expressions to …nd the reference angles for the
joints in this chapter as given in Chapter 2. However, we repeat the expressions here to ease the readability of the upcoming sections. The reference angle for joint i for the sideways (yaw) hi ;d and up-down (pitch) vi ;d
motion of each joint are given by
hi ;d (t)
= Ah sin (! h t + (i
1)
h)
vi ;d (t)
= Av sin (! v t + (i
1)
v
+
(7.1)
0) ;
(7.2)
134
Simulation and Experimental Results
Parameter
Li
LGSi
Value
0:122 m
0:0393 m
LSCi
mi
i
0:0525 m
7:5
0:682 kg
11
1:32 10 3 kg m2
i
9:40
10
Tx
9:81 m/s2
0:2
Ty
0:2
g
Description
4
kg m2
link length
length employed to lump the contact point between the link into
two points for the 3D model and
the ‘robot model’
link radius
link mass
moment of inerita around
transversal axis through CG of
link
moment of inerita around longitudinal axis through CG of link
acceleration of gravity
Coulomb friction coe¢ cient longitudinal to a link
Coulomb friction coe¢ cient lateral to a link
Table 7.1: Parameter values for the various mathematical models.
where the various components are further described in Section 2.4. The
parameter h , which controls the direction of forward motion, is removed
from (7.1) since it will always be set to zero in the following simulations
and experiments.
Remark 7.1 (Choice of motion pattern parameters) In the following sections each motion pattern will most often be simulated with a single
set of motion pattern parameters. In general, the speci…c choice of parameters for a motion pattern is dependent on the design of the snake robot (for
example link length and maximum joint angle), and the desired behaviour of
the snake robot (e.g. desired speed or turning ratio). For example, the snake
robot needs at least three contact points between its body and the obstacles
for obstacle aided locomotion with lateral undulation shown in Section 7.3.4.
Hence, the shape of the snake robot must have ‘enough curves’so that locomotion is possible. The curvature of the snake robot in the horizontal plane
is controlled with the parameters Ah and h .
A ‘soft start’ approach is employed for simulations and experiments
where the initial orientation of each joint is not equal to the desired angles
7.2 Simulation Parameters
Parameter
n
rN
rT
rV
rH
t
V
(
(
Cx ;
Cy )
Vx ;
Vy )
C
kP
kD
KPj
KDj
2D model
11 links
?
1:3
?
0:01
2:5 10 4 s
?
?
?
?
800
2
?
?
2 10 5
135
3D model
11 links
0:1
0:01
0:05
0:01
2:5 10 4 s
0:02
?
?
?
800
2
?
?
2 10 5
Robot model
13 DOF
?
0:2
?
?
2:5 10 4 s
?
(0:2; 0:5)
(0:1; 0:2)
0:001
200
1:2
4:0 105
2:4 103
1 10 5
Table 7.2: Simulation parameters for the 2D model in Chapter 3, the 3D
model in Chapter 4, and the model based on minimal coordinates (the
‘robot model’) presented in Chapter 6. KPj and KDj are the elements on
the diagonal of the matrices KP and KD in (6.60), respectively, and kP ,
kD are the gains of the PD-controllers. See Algorithm 3.1 on page 64 and
Algorithm 4.2 on page 94 for the usage of the parameters rN , rT , rV and
rH . The step size in the numerical integration is given by t. The various
friction coe¢ cients are given by V , Cx , Cy , Vx and Vy . The tolerance
for the Mod…ed Newton Alogorithm is given by . The steepness of the
approximation to the sign-function is given by C .
136
Simulation and Experimental Results
hi ;d (t)
and vi ;d (t) at the time the snake robot is started. An example of
such a situation is if we want the snake robot to move forward by lateral
undulation, but it has a straight posture at t = 0. The ‘soft start’approach
is utilized to avoid large steps in the reference signal which causes large
actuator torques. To this end, we consider all the reference signals hi ;d (t)
and hi ;d (t) separately and calculate them from the expressions (7.1) and
(7.2) for the reference angles, respectively. However, we set hi ;d (t) = 0
until hi ;d (t)
3 =180 and vi ;d (t) = 0 until vi ;d (t)
3 =180 for the
…rst time after start-up. After the reference value is within the allowed
start value, the reference angle is found from (7.1) or (7.2).
7.3
Simulations without Experimental Validation
In this section we present simulations of the models presented in this thesis. First, we simulate the 3D model where the snake robot is raised above
the ground surface and then dropped onto it. This scenario challenges
the numerical stability of the numerical treatment since large sudden contact forces arise simultaneously. Moreover, we check whether the snake
robot moves forward by lateral undulation with anisotropic friction, as it
is expected to. Second, we test if the de…ned exoskeleton works well for Ushaped lateral rolling and sidewinding, and we investigate what happens if
the friction between the snake robot and the ground surface is changed for
these motion patterns. Third, obstacle aided locomotion is simulated with
small obstacles. Such small obstacles pose di¢ culties both in the numerical
treatment and that the snake robot may get stuck. We investigate how our
2D and 3D models perform for these challenges and we also enquire into
how the snake robot behaves if one obstacle that is necessary for forward
motion is moved. Moreover, contact forces between the snake robot and
the obstacles are plotted and compared to the discussion on obstacle aided
locomotion given in Chapter 5. Finally, the input-output linearizing controller is compared to a conventional PD-controller, both implemented on
the ‘robot model’. In addition to the comparison, we will also inquire into
whether the ‘robot model’ is suitable for numerical treatment. Note that
for simulation of the 2D model presented in Chapter 3 we always employ
the PD-controller given in the same chapter due to its simplicity and since
the very high accuracy in the control of the snake robot joints o¤ered by
the input-output linearizing controller is not crucial for testing the validity
of the 2D model. Moreover, the PD-controllers given for the 3D model from
Chapter 4 are always employed when simulating the 3D model.
7.3 Simulations without Experimental Validation
137
Figure 7.2: Snake robot during drop.
These simulations are given without an experimental validation due
to several reasons. For example, the snake robot Aiko does not have the
orthotropic friction property needed to move forward by lateral undulation.
Moreover, we have not focused on producing the necessary measurement
equipment to accurately measure the position of Aiko during 360 degree roll
around the longitudinal axis of the robot or to implement the input-output
linearizing controller. Instead, the focus has been on developing the theory
around these aspects of locomotion and snake robot control.
All simulations are run in Matlab R2006a on an Intel Xeon 3 GHz
processor with 2 GB RAM.
7.3.1
3D Model: Drop and Lateral Undulation
In this section the snake robot is dropped from an inclined angle and then
set to move across the ground surface with lateral undulation. We include
this simulation to show that the numerical treatment is stable even for such
non-ideal situations and to show that the model of the snake robot moves
forward by lateral undulation when the friction coe¢ cient Ty transversal to
the snake robot links is higher than the friction coe¢ cient Tx longitudinal
to each link ( Ty > Tx ).
Particular simulation parameters:
Ty
= 0:5, rN = 0:01.
Motion pattern parameters: Lateral undulation
Ah =
40
180
rad, ! h =
Av = 0 rad,
!v =
80 rad
180 s ,
0 rad
s ,
50
180
h
=
rad,
v
= 0 rad,
0
= 0 rad.
(7.3)
138
Simulation and Experimental Results
Figure 7.3: (a) Position and (b) velocity of link 1 during drop and (c)-(d)
the position of link 6 during lateral undulation after the drop.
The snake robot is dropped at an inclined angle of 30 degrees with
respect to the ‡oor. The centre of gravity of the lowest link is lifted to
z1 = 0:3525 m (i.e. LSCi + 0:3 m) above the ‡oor and all actuators are
turned o¤ until it has come to a resting position on the ground (we set
vi = hi = 0 for t 2 [0; 1) s). Figure 7.2 shows the con…guration of the
snake robot while it drops to the ‡oor. Figure 7.3 depicts (a) the vertical
position z1 of link 1 and (b) its drop velocity v1z (parallel to the eIz -axis).
As seen in Figure 7.3 (b), velocity jumps occur when the link hits the
ground. We observe that the link does not immediately come to a resting
position after having hit the ground even though the impact is modelled
as completely inelastic. This is because the remaining links reaches the
ground shortly after and the impulses that arise from their impacts with
the ground are transmitted through the joints so that v1z also becomes
discontinuous at the impact times. To be able to simulate the hard impact
with the ground, we needed to reduce the value of rN compared to the rest
of the simulations of the 3D model. We see from Figure 7.3 (c) that the
snake robot slides a little backward during, and shortly after, its impact
with the ground. This is because it was dropped from an inclined angle.
At t = 1 the actuators are turned on again and the snake robot begins
to progress forward with the motion pattern lateral undulation where the
‘soft start’approach described in Section 7.2.2 is employed. The position of
the CG of link 6 is depicted in Figure 7.3 (c)-(d) and we see that the snake
robot moves forward along the eIx -axis. This is the expected qualitative
motion of a snake robot moving by lateral undulation with the anisotropic
friction property described above. We note that even though the snake
7.3 Simulations without Experimental Validation
139
robot started from a non-idealized posture (after the drop), the numerical
treatment was stable.
7.3.2
3D Model: U-shaped Lateral Rolling
The motion pattern lateral rolling with a U-shape is simulated in this section. To this end, we show that 360 degree rotations around the longitudinal
axis of the snake robot does not pose a problem for the snake robot model
and that the snake robot moves sideways on the ground surface due to the
exoskeleton. In addition, we investigate what happens for U-shaped lateral
rolling when the friction coe¢ cients are changed.
Particular simulation parameters: Three combinations of friction
coe¢ cients: ( Tx ; T y ) = (0:2; 0:2), ( Tx ; T y ) = (0:2; 0:5) and
( Tx ; T y ) = (0:01; 1).
Motion pattern parameters: U-shaped lateral rolling
Ah =
10
180
Av = Ah ,
rad, ! h =
80 rad
180 s ,
!v = !h,
h
= 0 rad,
v
= 0 rad,
0
=
90
180
rad.
(7.4)
The snake robot is started with all links on the ground in a curved
posture where the joint angles are set initially to hi (0) = hi ;d (0) = 0 rad
and vi (0) = vi ;d (0) = 10 180 rad. With these initial conditions and the
motion pattern parameters in (7.4), the snake robot rolls towards the ‘edge’
of the U-shape (see Figure 2.5 (a) for an illustration of a shape of a snake
robot during U-shaped lateral rolling). We see from Figure 7.4 that the
snake robot moves further for ( Tx ; T y ) = (0:2; 0:5) than for ( Tx ; T y ) =
(0:2; 0:2) in an almost straight line. This suggests that the snake robot slips
less and can utilize more of the rolling motion for locomotion. However, for
( Tx ; T y ) = (0:01; 1) we observe that the path taken by the snake robot
clearly deviates to the left in Figure 7.4 (a). This is because the snake
robot sometimes starts sliding along its longitudinal axis due to the low
friction coe¢ cient Tx and the di¤erence Tx
Ty . This latter results
suggests that for a snake robot with Tx
(implemented,
for example,
Ty
by adding small caster wheels in a tight grid on each link) a di¤erent shape
of the snake robot may be advantageous to better control the direction of
locomotion.
We have seen from the simulations that the snake robot moves sideways
during U-shaped lateral rolling. This is possible since we have chosen to
de…ne an exoskeleton as a contact surface with the ground. If only, for
140
Simulation and Experimental Results
Figure 7.4: Position of link 6 during U-shaped lateral rolling along the eIx and eIy -axis.
example, the centre of gravity would have been chosen as a contact point,
then the rolling motion would not have been possible.
7.3.3
3D Model: Sidewinding
In this section we simulate sidewinding for various friction coe¢ cients.
Particular simulation parameters: Three combinations of friction
coe¢ cients: ( Tx ; T y ) = (0:2; 0:2), ( Tx ; T y ) = (0:2; 0:5) and
( Tx ; T y ) = (0:01; 1).
Motion pattern parameters: Sidewinding
Ah =
Av =
30
180
10
180
rad, ! h =
80 rad
180 s ,
rad, ! v = ! h ,
h
=
50
180
v
=
h,
rad,
0
=
90
180
rad.
(7.5)
We employ the ‘soft start’ approach and set the snake robot to move
by sidewinding from a straight posture. Figure 7.5 depicts the position the
centre of gravity of link 6 during locomotion. We see from Figure 7.5 (a)
and (c) that the snake robot slides backward along the y-axis at certain
intervals for Tx ; T y = (0:01; 1). This is mostly due to the fact that one
of the parts of the snake robot that instantaneously is supposed to be lying
stationary on the ground slides longitudinally due to the large di¤erence
T y > Tx in friction coe¢ cients. In addition, we observe that the direction
of locomotion is dependent on the friction coe¢ cients.
7.3 Simulations without Experimental Validation
141
Figure 7.5: Position of link 6 during sidewinding for three di¤erent friction
scenarios.
j
1
2
3
4
5
LHj [m]
0:02
0:03
0:01
0:02
0:03
Hj ([m]; [m])
(0:087; 0:077)
(0:474; 0:082)
(0:882; 0:101)
(1:201; 0:097)
(1:671; 0:178)
j
6
7
8
9
LHj [m]
0:01
0:02
0:03
0:01
Hj ([m]; [m])
(1:927; 0:110)
(2:394; 0:153)
(2:705; 0:061)
(3:119; 0:128)
Table 7.3: Radii and positions of obstacles in the simulations in Section
7.3.4 for simulations of the 2D and 3D model.
7.3.4
2D and 3D Model: Obstacle Aided Locomotion
In this section we test and compare the 2D and 3D model for obstacle
aided locomotion with small obstacles of various sizes. The contact forces
between the snake robot and obstacles will be investigated and compared to
the discussion on obstacle aided locomotion given in Chapter 5. In addition,
we will investigate what happens if the position of one of the obstacles is
altered.
Particular simulation parameters: We include = 9 obstacles in the
simulation. For each obstacle j = 1; : : : ; 9 (counting from left to right
in Figure 7.6 (a)) the radius LHj and position of the centre Hj in the
eIx ; eIy -plane are as given in Table 7.3. These parameters are used
in the …rst set of simulations. Then all obstacles are rearranged for
a discussion on what happens if the motion pattern parameters are
changed. Finally, the obstacles are placed at their initial position, but
the 3rd obstacle is …rst moved and then removed as explained below.
142
Simulation and Experimental Results
Motion pattern parameters: Lateral undulation is mostly performed
with parameters given by (7.3) and employed for obstacle aided locomotion in this section. However, a short discussion is added for
two other sets of motion pattern parameters. These parameters are
indicated below.
The snake robot is set to move with the same motion pattern parameters as for lateral undulation shown in Section 7.3.1. However, in this section, the snake robot will move forward not due to the orthotropic friction
property employed in Section 7.3.1, but, instead, due to pushing against
obstacles.
The snake robot is initially (at t = 0 s) in a curved posture where its
joints angles are found from hi ;d (0) and vi ;d (0) (the latter only applies
to the 3D model). This posture is the basis for placing the obstacles along
the natural path of the snake robot according to the criteria described in
Section 5.5. In short, that is the obstacles are placed according to the initial
con…guration of the snake robot based on the choice of motion pattern
parameters Ah , ! h and h and they are located close to the centre line of
the sinus-like shape of the snake robot. We employ various small obstacles
to show that problems such as getting stuck do not occur for our choice
of exoskeleton for the snake robot links. This problem is avoided since we
use a moving contact point for contact between a snake robot link and an
obstacle. Hence, every point on the exoskeleton of the snake robot is a
possible contact point with an obstacle and therefore cannot an obstacle
penetrate the snake robot body.
The position of link 6 of the snake robot is depicted in Figure 7.6, where
in Figure 7.6 (a) we also see the obstacles placed according to Table 7.3. We
see that the results from the 2D and 3D model are more or less identical.
Hence, the results suggest that we may choose the 2D model to optimise
the simulation time when purely planar snake robot motion is investigated,
while the 3D model can be employed when additional 3D motion is required.
Moreover, we observe that the snake robot is able to traverse the obstacle
course, and its path is almost sine-like except from some ‘bumps’here and
there. These ‘bumps’ arise from the de…nition of the exoskeleton of the
links. Each link has a hemisphere (or a semicircle for the 2D model) at
the two ends of the link, and the ‘bumps’in the path comes from that the
obstacles are so small that they may slide slightly into the small ‘crack’
between two adjacent links.
We also conclude from the simulation result that the numerical treatment of the 2D and 3D model with the time-stepping method worked well
7.3 Simulations without Experimental Validation
143
Figure 7.6: Position of link 6 during obstacle aided locomotion. The perimeteres of the obstacles j = 1; : : : ; 9 (counting from left to right) are depicted
as black circles in (a).
even though the obstacles were small. Hence, the contact impulsions between the snake robot was calculated accurate enough to keep the obstacles
from penetrating the exoskeleton of the snake robot.
In the following, we will use the simulation results from the 2D model
to investigate the contact forces between the snake robot and the obstacles.
To this end, we quantify the instantaneous value of the contact force that
acts on the snake robot from the obstacles as
FH =
1 X
(WHM )b PHb
t
(7.6)
b2HM
where HM is the set of active contact points, PHb is contact impulsion
during the current time-interval and (WHM )b gives the direction of the
contact impulsion (see the explanation of (3.67) on page 62). Hence, F H
consists of both forces and possible impulses and it is found from the integral
of the forces during each time-step.
Figure 7.7 (a) depicts the snake robot at t = 5:2 s in an environment
with circular obstacles. The snake robot is moving to the right and we
observe that this is also the direction in which the sum of forces, acting on
the snake robot from the obstacles points. The contact forces found from
(7.6) are concurrent with the illustration of obstacle aided locomotion in
Figure 5.3, hence it is the sum of contact forces that results in the forward
(toward the right in Figure 7.7 (a)) motion of the snake robot.
The linear contact forces found from (7.6) are plotted from t = 4:5 s to
t = 5:5 s for links 2, 7 and 10 in Figure 7.7 (b)-(c). Hence, the …gures depict
144
Simulation and Experimental Results
Figure 7.7: (a) Outline of snake robot at t = 5:2 s during obstacle aided
locomotion. The contact forces are indicated with red arrows and the snake
robot is moving toward the right. (b)-(c) The contact forces on links 2, 7
and 10.
how the contact forces develop around the time the snake robot is in the
position illustrated for in Figure 7.7 (a). The spikes in the forces arise from
impulses due to the link colliding with an obstacle. The maximum values of
these spikes have not been included in the plot to depict the contact force
during the rest of the time-interval more accurately. We see from Figure 7.7
(c) that the average applied force to link 7 is higher than to the other two
links. Comparing to Figure 7.7 (a), we see that the di¤erence is because
link 7 is the only link that is subjected to a force pointing in the negative
eIy -direction. Hence, this contact force balances the contact forces acting
on the other two links (links 2 and 10).
We see from Figure 7.7 that the snake robot is barely ‘curled enough’to
be able to reach and take advantage of the necessary three contact points
discussed in Chapter 5. To this end, a comment on the choice of the motion
pattern parameters Ah , h , and ! h is imminent. We see from Figure 7.9 (a)
that a too low value of h results in that the snake robot (with only 11 links)
is no longer able to be in contact with three push-points simultaneously,
and this renders it unable to move forward by lateral undulation. However,
if we increase the value of h as illustrated in Figure 7.9 (b), then snake
robot is able to move forward, but the angle between the link in contact
with an obstacle and the direction of progress becomes small. Hence, most
of the reaction force from the obstacle will point normal to the direction
of motion (since it points transversal to the link), and this leads to very
ine¢ cient locomotion since larger joint torques are needed to propel the
7.3 Simulations without Experimental Validation
145
Figure 7.8: Position of link 6 of the snake robot during a 15 second simulation with the 3rd obstacle (a) moved upwards and (b) removed from the
simulation.
0
-0.2
y [m]
-0.4
-0.6
-0.8
-1
-1.2
0
0.2
0.4
0.6
x [m]
0.8
1
1.2
1.4
Figure 7.9: Posture at t = 0 s of snake robot for (a) h = 30 =180 rad
and (b) h = 70 =180 rad. The posititions of the middle links from t = 0
s to t = 5 s are indicated by the (red) dashed line.
146
Simulation and Experimental Results
snake robot forward (i.e. the contact force needs to be higher to obtain
the same amount of force tangential to the direction of motion with three
push-points). Moreover, the increased contact force will result in that a
real snake robot can get easier stuck since the friction forces between the
obstacles and the snake robot will increase. The increase in contact force
for a posture with ‘more curls’corresponds to the increase in the constraint
forces on the wheels for a wheeled snake robot with the same posture during
lateral undulation (without obstacles) as described in Date et al. (2000).
The angle between the link in contact with the obstacle and the direction
of locomotion will also be smaller if we choose to keep h at its original
value, but lower the amplitude of joint oscillation Ah . Hence, the value
of h should be chosen such that the snake robot is just ‘curled enough’
to utilize the push-points for locomotion, and the amplitude of oscillation
should, up to some extent, be chosen as high as possible to obtain a large
angle between the direction of locomotion and the links in contact with the
push-points. However, it is important to be aware of that if the obstacles
are located far from each other, a larger value of h is necessary to allow
the snake robot to reach all three necessary push-points. Moreover, the
wave propagation speed is dependent on h , so this also has to be taken
into account when choosing the motion pattern parameters.
It now remains to comment on the value of the angular frequency ! h .
For an in…nitely strong snake robot (both with respect to actuator torques
and outer shell) and …xed obstacles that do not yield, a larger ! h will result
in a greater forward speed since the propagating wave is propagated faster
toward the back of the snake robot. However, the larger the speed, the
harder the impact between the snake robot and the obstacles will become.
Also, the speed will be limited by the amount of torque the actuators are
able to produce. Moreover, the obstacles may not always be …xed to the
ground, such as a chair or a small rock, hence it may in some cases be
necessary to limit the forces exerted on the obstacle to keep them …xed.
This is also achieved by limiting the angular frequency ! h . From the discussion above, we choose to keep the motion parameters as they are for the
upcoming experiments.
In addition to the perfect scenario depicted in Figure 7.8 (with respect
to the connection between the motion pattern and obstacle placement)
described above, we set the obstacles according to Table 7.3 and brie‡y
explore the scenario where obstacle 3 (counting from the beginning of the
track) is partly and completely removed. First, obstacle 3 is moved one
snake robot link diameter along the eIy -axis. Thus, its new position in the
7.3 Simulations without Experimental Validation
147
horizontal eIx ; eIy -plane is
H3;moved : (0:882 m; 0:101 m + 2LSC1 ) :
(7.7)
Figure 7.8 (a) illustrates the path of link 6 of the snake robot when it
moves through the altered track. The simulation shows that the snake
robot started sliding to the left and rotating anti-clockwise when it was
supposed to collide with the moved obstacle. However, the deviation from
its original path was not substantial enough to hinder the snake robot in
reaching obstacle 4 su¢ ciently accurate to continue its path along the track.
Secondly, obstacle 3 is completely removed and the new path of the snake
robot is depicted in Figure 7.8 (b). Then the snake robot also rotated anticlockwise and slid to the left when it was supposed to hit obstacle 3, but
this time it was unable to complete the track since it never collided with the
third obstacle, and therefore never regained the third contact point needed
for locomotion as discussed in Section 5.5.
We might expect that the snake robot fails to move through the entire
track with one obstacle moved or missing since the desired joint pattern
is preprogrammed. However, the results displayed in Figure 7.8 show that
some repositioning of the obstacles is possible. Hence, a control algorithm
for intelligent obstacle aided locomotion does not need to provide a motion
pattern fully accurate compared to the position of the obstacles. The snake
robot still manages to move through the track even if it touches some
obstacles in a far from perfect manner. The downside can be that larger
joint torques are needed as discussed in Section 5.3.
7.3.5
Robot Model: Comparison of Controllers
The input-output linearizing controller presented in Chapter 6 will be compared to a PD-controller in this section. The comparison will be performed
for the (planar) process plant model, or ‘robot model’, presented in the
same chapter for both a smooth approximation to Coulomb friction, and
Coulomb friction described by a set-valued force law. In addition, we will
see how the controllers perform when noise is introduced in the system and
the suitability of the ‘robot model’for numerical treatment is discussed.
Particular simulation parameters: None. All parameters are given in
Tables 7.1 and 7.2. Note that ( Cx ; Cy ) and ( Vx ; Vy ) are employed
in the robot model instead of ( Tx ; Ty ) since we here have viscous
friction in addition to Coulomb friction.
148
Simulation and Experimental Results
Figure 7.10: (a) Reference angle for joint 5 during lateral undulation, (b)
the tracking error for the same joint with the smooth approximation to
the Coulomb friction and (c) the tracking error from simulations with the
set-valued Coulomb friction model.
Motion pattern parameters: Lateral undulation with parameters given
by (7.3).
The PD-controller is implemented for the robot model as
=
kD
_
_
d
kP (
d) ;
(7.8)
where the elements of the vector of desired joint angles d 2 R10 are denoted
2 R10 is the vector of snake robot
di and found from hi ;d in (7.1), and
joint angles found directly from the generalized coordinates as shown in
(6.3), (6.30).
The time-stepping method is used for numerical integration of the ‘robot
model’ with set-valued Coulomb friction, while the Matlab solver ode15s
is used for the smooth version of the model. We remember from Chapter 6
that the control torque found from the input-output linearizing controller
is based only on the smooth friction model, even for control of the ‘robot
model’with set-valued Coulomb friction. The initial posture of the snake
robot is straight and then it is set to move with lateral undulation using
the ‘soft start’approach.
First, the simulations were run without noise and the tracking error
for one of the two middle joints is depicted in Figure 7.10 for both the
smooth and the non-smooth model. Figure 7.10 (b) suggests that the
input-output linearization controller yields an asymptotically stable error
dynamics, while the PD-controller results in a periodic tracking error. We
7.3 Simulations without Experimental Validation
149
Figure 7.11: Tracking error for joint 5 from the simulations of the nonsmooth model with added noise according to Table 7.4 and (7.9)-(7.14).
The reference signal for the joint is depicted in (a).
see from Figure 7.10 (c) that the tracking error for the controller based
on input-output linearization (i.e. the ‘IO-controller’) is almost negligible
despite that we are no longer able to cancel the snake robot dynamics correctly with the feedback term (since the feedback term is based on a smooth
friction model and the plant itself is simulated as set-valued). In addition,
we …nd that the tracking error for the PD-controller has a sinus-like shape
and almost similar in form but somewhat smaller in amplitude compared
to the case with smooth Coulomb friction.
We have earlier argued that the ‘robot model’is not suitable for simulating a snake robot due to its high number of degrees of freedom. However,
in this section we employ this model for simulation anyway so this point
naturally needs some attention. The time-stepping method with the Modi…ed Newton Algorithm is designed to simulate systems with set-valued
force laws and possibly discontinuous velocities. However, with the smooth
approximation of the friction model, we no longer have such a system and a
conventional numerical integrator can be employed instead. Moreover, since
the Modi…ed Newton Algorithm is no longer employed, we need a new way
to handle the bilateral constraints (introduced by the joints). One way of
doing this is to employ minimal coordinates instead (as done in the ‘robot
model’). The minimal coordinate representation of the snake robot gives
us the system matrices employed in the feedback law more or less directly.
Hence, we do not have to calculate these matrices from scratch in addition
to system matrices developed for a model with non-minimal coordinates.
The 2D model in Chapter 3 is implemented with a single point on each link
150
Simulation and Experimental Results
for the frictional contact between the snake robot and the ground surface.
This choice has been made to simplify the model. Hence, such an approach
results in that a purely rotational motion of a link does not give rise to
any friction forces. Such friction forces are needed in the stability analysis
provided of the closed-loop system with the input-output linearizing controller. Due to the reasons mentioned above, we decided to try to use the
smooth ‘robot model’for simulation in order to compare the PD-controller
and the input-output linearizing controller. We see from Figure 7.10 that
we were able to simulate the system with this approach, and to avoid having to redesign the 2D model to add a friction point and viscous friction,
we also used the same approach (now with the time-stepping method) for
the ‘robot model’ with set-valued friction. Using this approach was okay,
but only since we needed a very limited number of simulation trials. This is
because the simulations were very time-consuming. This was partly due to
the sti¤ di¤erential equations which was due to the smooth approximation
of the Coulomb friction, but also due to the enormous amount of terms that
had to be calculated in order to …nd the system matrices. To illustrate the
di¤erence in computation time, we mention that it took between 20 and 50
minutes to simulate a single second of snake robot motion with the ‘robot
model’, and with the 2D model it only took about 0.5 minutes to simulate
a second of snake robot motion.
For the second set of simulations, we add various degrees of noise to
the measurements and test the IO-controller and the PD-controller in simulations where the set-valued Coulomb friction model is employed. The
noise is added to the measurements with the Matlab function rand which
produces approximately normally distributed numbers between 0 and 1.
Furthermore, the states utilized in the feedback (i.e. the measurements
with noise) are found from
I r O2 ;m
=
m
=
+ N (2 rand (1; 1)
m
=
+ N (2 rand (10; 1)
=
_m =
_
=
I v O2 ;m
m
I r O2
+ Np (2 rand (2; 1)
1) ;
1) ;
1) ;
+ Nv (2 rand (2; 1) 1) ;
_ + N _ (2 rand (1; 1) 1) ;
_ + N _ (2 rand (10; 1) 1) ;
I v O2
(7.9)
(7.10)
(7.11)
(7.12)
(7.13)
(7.14)
where Np , N , N , Nv , N _ and N _ are positive constants in R, and rand (a; b)
is the Matlab function rand that returns a matrix (or vector) with a rows
and b columns with approximately normally distributed numbers between
0 and 1.
7.3 Simulations without Experimental Validation
151
Figure
7.11
Np [m]
N [rad]
N [rad]
Nv [m/s]
N _ [rad/s]
N _ [rad/s]
(b)
(c)
(d)
0:1
10
10
0:1 180
10 180
10 180
0:1 180
0:1 180
1 180
0:1
10
10
0:1 180
10 180
10 180
0:1 180
0:1 180
1 180
Table 7.4: Amplitude of noise added to the measurement employed in the
feedback control corresponding to Figure 7.11.
Figure 7.11 depicts the reference signal together with the tracking errors
for three simulations with various degrees of noise added to the measurements employed for feedback. Table 7.4 contains the amplitudes of the
added noise. We see from Figure 7.11 (b) and (c) that as long as the noise
added to the joint angle measurements is kept low, then both the results
utilizing the IO-controller and PD-controller, respectively, are not signi…cantly a¤ected. Hence, even though the feedback term in the IO-controller
contains the velocity of the snake robot, it is not signi…cantly a¤ected by
adding noise to these states. We note from Chapter 6 that we assume that
all states are available for feedback.
We see from Table 7.2 that a very high gain is used to control the
joints accurately with the IO-controller. Hence, a large error will yield a
large control torque that may not be possible to realize in an actual snake
robot. Hence, this is an aspect that needs to be considered for future
work. However, when the error is small (which is the case a short time
after the snake robot has been started), then the IO-controller will not
require any substantially larger control torque than the PD-controller. It
is also important to comment on that the gains for the PD-controller and
the IO-controller are not directly comparable since certain elements of the
mass matrix (and its inverse) are included by multiplication before the …nal
control torque is found from the IO-controller (see 6.58 on page 121). A
possible solution to the problem of high control torques during start up, is
to employ a reference generator which accounts for the initial posture of
the snake robot, and then guides the actual reference angle to the desired
reference signal without causing it to jump. To investigate the di¤erence in
gains between the PD-controller and the IO-controller, we tried to increase
the PD-controller gains. However, this resulted in such a sti¤ system that
we were not able to simulate it within a reasonable amount of time (a
20 second simulation was aborted since it was did not complete within
48 hours). We have also tried adding a 10 N saturation on the control
152
Simulation and Experimental Results
torques for both controllers. Then, the results from stationary motion were
the same as Figure 7.10. However, the steps in the reference signal during
the ‘soft start’approach caused a very short, but larger tracking error for
the IO-controller compared to the PD-controller.
The smooth approximation of the Coulomb friction in the ‘robot model’
results in a sti¤ system. This is not advantageous for simulation and the
2D model in Chapter 3 should be employed instead. However, since the
non-linear control techniques employed in this thesis are only …t to cope
with smooth non-linear systems, the smooth approximation is necessary
for the controller synthesis and stability proof. Hence, the system matrices
for the minimal coordinates needed to be found in order to implement the
control law (which is constructed from these matrices). Thus, even though
the simulation time became much longer due to the need for small time step
due to e.g. the sti¤ system equations, it was more e¤ective to simulate the
robot model since few simulations were required to compare the controllers.
7.4
Experimental Setup
The snake robot ‘Aiko’is employed in the experiments, and the snake robot
together with the rest of the equipment used to conduct the experimental
tests are described in the following.
Each 2 DOF cardan joint of the snake robot is actuated by two 6 W
DC-motors. Each the two joint angles are controlled by simple P-controllers
implemented on an Atmel ATmega128 microcontroller. Hence, 10 microcontrollers are employed to control the 10 cardan joints. The snake robot
joint reference angles are sent with a frequency of 10 Hz from a PC running
Matlab, via a CAN-bus (a particular type of …eld bus) to the microcontrollers. The position of the centre of the middle link (link 6) is tracked
using a Vicon MX Motion Capture System with 4 cameras (MX3) together
with Matlab Simulink. The Vicon programme (Vicon iQ 2.0) runs on a
computer with 4 Intel Xeon 3 GHz processors and 2 GB RAM. This was
the same computer employed for the simulations. The logging of motion
data is synchronized in time through a TCP-connection between the PC
that controls Aiko and the Vicon-computer at the start-up of the transmission of the desired snake robot joint angles. Data logging is performed at
20 Hz. We have chosen to let the position of link 6 represent the position
of the snake robot, because then we …lter any transient behaviour of the
snake robot that might occur at its ends and it is thus easier to focus on
the general motion of the snake robot.
7.5 Simulations with Experimental Validation
153
The snake robot moves on particle boards in the experiments. For
obstacle aided locomotion, we attach obstacles cut from a PVC-pipe with
0.25 m outer diameter to the particle boards.
7.5
Simulations with Experimental Validation
In this section we compare the simulation and experimental results for
sidewinding, lateral undulation with isotropic friction and obstacle aided
locomotion. We compare both qualitative and quantitative motion to see
to what degree the models can reproduce the motion of the snake robot
‘Aiko’. Moreover, by investigating obstacle aided locomotion, we can learn
whether the exoskeleton de…ned for the snake robot models resembles the
actual contact between Aiko and the obstacles in the experimental setup.
We employ the PD-controllers described in Chapters 3 and 4 for the 2D and
3D models, respectively. The experimental setup described in Section 7.4
has been employed to conduct and log the experiments.
7.5.1
3D Model: Sidewinding
In this section we compare the simulation results with the 3D model and
the experimental results with Aiko for the sidewinding motion pattern.
Particular simulation parameters: None.
Motion pattern parameters: Sidewinding with the same parameters as
in (7.5).
The resulting motion of sidewinding with isotropic friction for the 3D
model of the snake robot is compared to experimental results found by
letting Aiko move with the same motion pattern. We employ the motion
pattern parameters given by (7.5) and calculate the reference angles for the
joints from (7.1)-(7.2) with the ‘soft start’approach starting from a straight
and ‡at posture along the eIx -axis as performed in the sidewinding simulations above. The motion of the model and Aiko is depicted in Figure 7.12.
As seen from the picture, Aiko was not wearing its metal head. The head
was removed in order to obtain a uniform weight distribution along the
snake robot.
Figure 7.13 displays the position of link 6 for the 3D model and Aiko during sidewinding. We observe from the …gure that the model almost follows
the eIy -axis, while Aiko steadily turns somewhat to the right. In addition,
154
Simulation and Experimental Results
Figure 7.12: Comparison of simulation (left) and experimental (right) results during sidewinding. The pictures are taken at t = 0; 5; 10; 15 s and
the snake robot is moving upwards in the illustration.
Figure 7.13: Position of link 6 of Aiko and the 3D model during sidewinding.
7.5 Simulations with Experimental Validation
155
the model covers a greater distance than Aiko. We see from Figure 7.13 (a)
and (c) that the simulation results have the same trend and approximate
frequency as the experimental results along the eIy -axis. The variance in
trend is more noticeable along the eIx -axis due to the turning motion of
Aiko. Moreover, a slight initial di¤erence between the orientation of Aiko
and the model may contribute to the discrepancy in heading. We discuss
in Section 7.5.4 various reasons for the di¤erences between the simulation
and experimental results. For sidewinding, we believe that one of the most
important di¤erences between the 3D model and Aiko is that Aiko is not
able to control its joints accurately (see Section 7.5.4). This is particularly
the case for vertical (lifting) motion which is needed for sidewinding. By
inspecting Aiko closely during the experiment, we noticed that a large part
of its body was touching the ground at the same time. This was not the
case in the simulation where the snake robot joints were accurately controlled which resulted in that most if its body was lifted from the ground
during locomotion.
We observe from Figure 7.13 (a) and (c) that Aiko sometimes slides
a little backward (at t
5 s, t
9 s and t
14 s). We have tried to
reproduce this phenomenon in the 3D model by lowering and increasing
the friction coe¢ cient. We have also tried a variety of anisotropic friction
properties, but without any luck. The backward motion might be a result
of Aiko not being able to lift its links properly. The links that are supposed
to be lifted and moved forward are instead sometimes dragged along the
particle board. This results in a friction force which acts in the opposite
direction of the desired motion of the snake robot. This friction force might
result in that some of the parts that are supposed to be lying stationary on
the ground are instead pushed slightly backward.
7.5.2
2D and 3D Model: Lateral Undulation with Isotropic
Friction
Although not commonly used, it is possible for a snake robot to use lateral
undulation for locomotion when the friction between the ground and the
snake robot is isotropic (Nilsson, 2004). In this section we compare the
simulation results from the 2D and 3D model with experimental results of
Aiko moving by lateral undulation.
Particular simulation parameters: None.
Motion pattern parameters: Lateral undulation with motion pattern
parameters given by (7.3).
156
Simulation and Experimental Results
We let the snake robot start in a curved posture with its centre line
approximately along the eIx -axis and with its initial joints angles given by
hi ;d (0) and vi ;d (0) (the latter only applies for the 3D model). Figure 7.14
shows both the position of link 6 calculated from the mathematical models
in the simulations and the position logged from the experiment with Aiko.
We see that the snake robot moves slowly backward along the eIx -axis in
both the simulations and the experiment. The average speed of the link is
approximately 1 cm/s. From Figure 7.14, we see that the both the 2D
and the 3D model display the same trend in motion as observed in the experiment. However, the distance travelled is greater in the simulations and
we have tried varying the friction coe¢ cients (while still keeping isotropic
friction), but this did not seem to have a noticeable e¤ect on the simulation
results. The di¤erence in distance travelled may come as a consequence of
that the model of the snake robot is able to control its joints more accurately and thus is able to reach the maximum amplitude (40 degrees) when
it is required to do so. Also, this suggest that the frictional contact between the snake robot and the ground surface is more complex than what
has been accounted for in the 2D and 3D model. This statement is also
backed up by the fact that the simulation results di¤ers between the 2D
and the 3D model. The di¤erences between the two simulations come from
that two contact points are employed for contact between a link and the
ground surface in the 3D model, while only a single point is employed for
contact in the 2D model. We have de…ned two contact points in the 3D
model to avoid that parts of a link penetrates the ground surface while still
keeping the contact model as simple as possible (if just a single point would
have been chosen for the contact point, then the remaining parts of the link
can penetrate the ground surface during 3D motion).
In the following section we will see how the snake robot moves with the
exact same motion pattern in an environment with obstacles. To compare
the two trials, we have kept the metal sphere head on Aiko for the lateral
undulation on an isotropic surface as shown in this section.
7.5.3
2D and 3D Model: Obstacle Aided Locomotion
The simulation and experimental results for obstacle aided locomotion will
be compared in this section. By doing this we can check whether the
models are appropriate test beds for such locomotion, and we can also get
an indication of whether the de…nition of the exoskeleton of the models is
valid for describing Aiko.
7.5 Simulations with Experimental Validation
157
Figure 7.14: Position of link 6 during lateral undulation with isotropic
friction.
Figure 7.15: Experimental results with Aiko (left) and simulation results
from the 3D model (right) during obstacle aided locomotion by lateral undulation.
j
1
2
3
4
LHj [m]
0:125
0:125
0:125
0:125
Hj ([m]; [m])
(0:051; 0:176)
(0:422; 0:003)
(0:804; 0:185)
(1:153; 0:003)
j
5
6
7
LHj [m]
0:125
0:125
0:125
Hj ([m]; [m])
(1:530; 0:178)
(1:888; 0:002)
(2:259; 0:175)
Table 7.5: Radii and positions of obstacles in the simulations and experiments described in Section 7.5.3.
158
Simulation and Experimental Results
Figure 7.16: Position of link 6 during obstacle aided locomotion in the
simulations and the experiment. The circumferences of the obstacles j =
1; : : : ; 7 (counting from left to right) are depicted as black circles in (a).
Particular simulation parameters: We include = 7 obstacles in the
simulation and the experiment. For each obstacle j = 1; : : : ; 7 (counting from left to right in Figure 7.16 (a)) the radius LHj and position
of the centre Hj in the eIx ; eIy -plane are given in Table 7.5.
Motion pattern parameters: Lateral undulation with the same parameters as in (7.3).
The snake robot is set to move with exactly the same motion pattern as
in the previous subsection, however now it moves forward and much faster
because of the obstacles that it pushes against. Moreover, Aiko is wearing
its metal sphere head to easier slide along the obstacles it encounters.
The position of link 6 during the simulations and the experiment are
shown in Figure 7.16. The positions and diameter of the obstacles are decided based on the elaboration in Section 5.5 and this is the same principle
as is employed in Section 7.3.4. The positions of the obstacles were used
to synchronize (in the horizontal plane) the positions logged in the experiment and the positions found in the simulations of link 6. The snake robot
was placed among the obstacles before it started to move as shown in Figure 7.15 for t = 0. Its initial joint angles were set to hi (0) = hi ;d (0)
and vi (0) = vi ;d (0). We see from Figure 7.16 that Aiko traverses its
path among the obstacles along the eIx -axis with approximately the same
speed (ca. 15 cm/s) in the simulation as in the experiments. This is 15
times faster compared to the results without the obstacles. Moreover, the
snake robot moves in the opposite direction (with respect to the wave that
7.5 Simulations with Experimental Validation
159
is propagated down its body) of what it did for isotropic friction without
obstacles. Hence, for isotropic friction without obstacles, the direction of
motion is the same as the direction the undulating wave that is propagated
along the snake robot body. However, for anisotropic friction or with obstacles to push against the direction of motion is opposite to the direction
of the propagating wave. Another way of increasing the velocity of a snake
robot on a ‡at surface is to attach passive caster wheels on the underside
of the robot. However, this approach has some downsides. First of all the
snake robot is more dependent on the ground surface condition for e¤ective
locomotion. Secondly, the speed of the snake robot is dependent on the angular frequency of oscillation ! h in (7.1) for a …xed h and Ah (Saito et al.,
2002), and for a snake robot with passive wheels, the side-ways slip has to
be considered when setting ! h (Wiriyacharoensunthorn and Laowattana,
2002). However, the only practical limitation for the wave propagation
speed during obstacle aided locomotion with obstacles that do not yield
(i.e. move or deform when pushed against) is the speed of the joint motors. It is therefore possible to gain great velocities using obstacle aided
locomotion.
7.5.4
Discussion of the Experimental Validation
We have seen in the previous section (Section 7.4) that the simulation
and experimental results range from being very similar, as for the obstacle
aided locomotion in Figure 7.16, to comparing fairly well for sidewinding
in Figure 7.13. Consequently, the di¤erences between the results need to
be addressed and in the following we will list the most important possible
sources for the discrepancies:
Aiko has a noticeable free play in the joints of about 3 5 degrees.
This results in that the control of the joint angles is not completely
accurate and the joint angle might not be able to reach its desired
angle.
The dynamics of the actuators are not modelled. Hence, the actuators in the models are extremely strong and fast, and we are able
to accurately control the actual angle of each joint to its desired position. This is not the case for Aiko where the joint motors sometimes saturate and are unable to track the desired joint movement
precisely. This particularly the case for vertical motion where larger
joint torques are required.
160
Simulation and Experimental Results
For each cardan joint, there is in fact a 2.2 cm distance between the
rotational axis for the yaw and pitch motion. However, to simplify
the kinematics in the models, we have assumed that the axes cross
each other.
The varnish on the underside of the snake robot has been worn down.
This has resulted in that the snake robot links slide easier when rolled
slightly to one of the sides where the varnish is still present.
The Stribeck e¤ect (see e.g. Leine and Nijmeijer (2004)) is not included in the model of the friction. The Stribeck e¤ect states, roughly
speaking, that the friction forces acting on a body is reduced just after
it is set in motion.
The exoskeleton of Aiko is not modelled precisely since this would
require a great deal of consideration of the geometry of the snake robot
and this would severely complicate the expressions for the contact
with the environment.
There was a time-delay between the start-up of the snake robot and
the start-up of the logging of position data. This delay ranged between 50 and 150 ms.
We note from the simulations with lateral undulation with isotropic
friction that there is a di¤erence between the simulation results of
the 2D and the 3D model. The di¤erence may arise from the fact
that two friction points are employed for each link in the 3D model
to handle the impacts with the ground in an e¤ective manner, while
the 2D model only has one friction point per link to minimize the
complexity of the model and thus increase the simulation speed.
Even though there are several sources of error, we see from the plots
that compare the simulation and experimental results that the models give
a satisfactory qualitative description of the snake robot dynamics. Especially the direction of locomotion is fairly accurate. For the sidewinding
motion pattern, the model is not accurate enough to predict the quantitative motion of the snake robot for an extended period of time, however it
gives a clear indication of how the snake robot will move. This suggests
that the model can be used to develop and test new 3D motion patterns for
snake robots that can later be implemented on physical snake robots such
as Aiko.
7.6 Summary
7.6
161
Summary
In this chapter we present back-to-back comparisons between simulation
and experimental results for a selection of serpentine motion patterns. Furthermore, additional simulation results are given to test motion patterns
and controllers which we have not been able to investigate in the experimental setup with our snake robot Aiko.
We see from the simulations that the time-stepping method with the
Modi…ed Newton Euler Algorithm is suitable for numerical treatment of
the 2D and 3D model. This is veri…ed, partly by simulating that the snake
robot drops to the ground and then moves forward by lateral undulation.
For this scenario the numerical treatment is stable even though the contact
forces are large and many impacts occurs.
The 3D model gives realistic simulation results of both sidewinding and
U-shaped lateral rolling. To this end, the snake robot moves sideways by
lateral rolling due to our choice of an exoskeleton for contact with the
ground surface. Also, our choice of possible contact points between the
snake robot and the ground surface keeps all parts of the snake robot links
from penetrating the ground surface. Sidewinding and U-shaped lateral
rolling are tested for a selection of anisotropic friction properties, and we
have found that the friction coe¢ cients in‡uence the direction of motion
for these motion patterns.
The 2D and 3D model are both simulated for obstacle aided locomotion
with small obstacles. Even though the obstacles are small, the snake robot
does not get stuck since we de…ne the contact between the snake robot and
an obstacle as a moving contact point. This results in that every point on
the body of the snake robot can come in contact with the obstacle and
the obstacle is kept from penetrating the exoskeleton of the snake robot.
We see that the simulation results compare very well for obstacle aided
locomotion simulated with the 2D and the 3D model. For lateral undulation
with isotropic friction (without obstacles), there is a noticeable di¤erence
between the models. However, both models move in the same direction
(backward) and the qualitative motion of the snake robot is the same.
By comparing Figure 7.3 with Figure 7.16, we note that the snake robot
is able to move much faster by pushing against obstacles than while moving
on a planar surface with 2.5 times higher friction coe¢ cient transversal to
the snake robot than along it. To obtain the same speed on a planar surface
without obstacles, the snake robot would probably have to be equipped with
wheels which complicates its design. It addition, the use of obstacles renders
the snake robot less dependent on the condition of the ground surface. This
162
Simulation and Experimental Results
is a desirable mobility property.
The simulations given in Section 7.3.5 show that the input-output linearizing controller proposed in Chapter 6 performs better that a conventional PD-controller for the controller gains used in that section, even when
noise up to a certain level is introduced in the measurements. However,
there is an issue with the choice of controller gains since it was not possible
to simulate very high gains for the PD-controller due to the resulting sti¤
model. However, for stationary motion, the IO-controller performed better
than the PD-controller even though the applied torques were almost the
same in magnitude.
From the back-to-back comparisons between simulation and experimental results, we conclude that the 2D and 3D model both give a realistic
picture of how a real snake robot will move, especially for obstacle aided
locomotion. Many aspects of Aiko have not been included in the mathematical model, which results in noticeable di¤erences between the simulation
and experimental results. However, the goal of the modelling is not to exactly reproduce the behaviour of the speci…c snake robot Aiko. To achieve
such an exact similarity, more elaborate system identi…cation has to be
performed and the exterior of the snake robot has to be more accurately
modelled. Instead of presenting a model with such a very close resemblance to Aiko, we present models for synthesis and testing of planar and
3D motion patterns for snake robots in general.
In this chapter we employ three mathematical models: the 2D model
from Chapter 3, the 3D model from Chapter 4 and the ‘robot model’presented in Chapter 6. Each model is developed for its own speci…c purpose.
First, the 2D model is developed to have an as simple model as possible
for fast implementation and simulation of planar motion pattern. Second,
the 3D model is for synthesis and testing of a combination of planar and
3D motion patterns. Third, the ‘robot model’ is developed to implement
and investigate stability of state-feedback controllers. However, it is not
advantageous to use for simulation, partly since it takes approximately 20
to 50 times more time to simulate the planar ‘robot model’than it takes to
simulate the 2D model. We see in this chapter that the roles of the di¤erent
models are successfully ful…lled.
Chapter 8
Conclusions and Further
Work
The results presented in this thesis are steps towards developing snake
robots that can navigate e¢ ciently in narrow and chaotic environments, for
example, when searching for people in a collapsed building or inspecting
narrow and possibly dangerous areas of industrial plants. In addition, the
results can be used to better understand the working principle of biological
snake locomotion.
A novel 2D mathematical model based on the framework of non-smooth
dynamics and convex analysis is developed for a snake robot in a certain
environment with external objects. The model describes planar snake robot
motion where the snake robot can push against external obstacles over the
entire surface of each link. Hence, the e¤ect of the contact forces between
the obstacle and each link is modelled properly. It is shown how to easily
incorporate the normal contact and friction forces into the system dynamics
(which is represented by a certain equality of measures) by set-valued force
laws. Moreover, it is shown how to include the correct directional Coulomb
friction.
The 2D model is extended to a 3D non-smooth mathematical model
based on the same methodology. However, for the 3D model we need to
add the ability to lift the links above the ground. Moreover, a 3D surface
has to be de…ned for each link. Such a surface is used for contact with
the ground surface and obstacles. This allows us to simulate 3D motion
patterns such as sidewinding and lateral rolling. Even though the resulting
dynamics is hybrid (there may be velocity jumps due to impacts), there is
no need for an explicit switch between system equations (when for example
164
Conclusions and Further Work
an impact between the snake robot and an obstacle occurs) since both the
non-impulsive forces and the impact impulses are covered by the same force
law and since we use the time-stepping method for the numerical treatment.
The same principles apply for the 2D model. The use of the non-minimal
absolute coordinates results in a constant and diagonal mass matrix and an
e¤ective way of writing the system equations. Such a constant mass matrix
is bene…cial in the numerical treatment since it needs only to be inverted
once and not for each time-step during simulation.
A control law for tracking control of the joints of a snake robot without wheels is proposed. Since the friction between the snake robot and
the ground surface is essential for locomotion for wheel-less snake robots,
a kinematic model is not su¢ cient and a non-linear model including both
the kinematics and dynamics of a planar snake robot and its contact with
the ground surface is presented. To this end, we develop a planar model
of the snake robot based on the standard dynamics of a robot manipulator
This model is denoted the process plant model or ‘robot model’ and it is
initially implemented with a smooth approximation of Coulomb friction.
Such a formulation is advantageous for control since it allows us to employ
the large variety of control techniques available for robot manipulators.
Furthermore, a control plant model is developed from the process plant
model. This model allows for an in…nite number of rotations of the snake
robot without a¤ecting the controller. Based on this control plant model,
a non-linear controller is developed using input-output linearization. It is
proved using Lyapunov stability theory that this controller asymptotically
stabilizes the desired motion for the snake robot joints. Furthermore, simulations with this controller applied to the process plant model suggest that
the closed-loop system is able to track the desired joint angles. In addition,
the same process plant model is implemented with a set-valued force law
for describing the Coulomb friction instead of its smooth approximation.
The simulations suggest that the proposed controller is also e¤ective for
the snake robot model with set-valued Coulomb friction. Moreover, it took
a very long time to simulate the process plant model compared to the 2D
model, and this suggests that it is not suitable for simulation.
Experiments are performed with the snake robot ‘Aiko’ built at the
NTNU/SINTEF Advanced Robotics Laboratory and a back-to-back comparison between simulation and experimental results is presented. The
comparison illustrate a quantitative similarity between motion of the 2D
and 3D model and the snake robot ‘Aiko’during obstacle aided locomotion.
Moreover, the results show that the model needs to be extended to accurately describe in particular 3D motion of Aiko. We are able to make a more
Conclusions and Further Work
165
exact model of Aiko, and thus deal with issues such as actuator dynamics
and a more accurate description of the exoskeleton of Aiko. However, this
will require a considerable amount of time and e¤ort and such a speci…c
model is beyond the scope of this thesis. Instead, we have focused on giving
a more general description of a snake robot where Aiko has been employed
as an inspiration for the design. The simulation results of the 2D and 3D
model illustrate that qualitatively the motions of the simulated snake robot
and Aiko are the same. Hence, the 2D and 3D models constitute a test bed
for future controller design and motion pattern design.
This thesis provides a framework based on non-smooth dynamics for
the development, analysis and testing of forthcoming motion planning and
control approaches to obstacle aided locomotion. Further work on snake
robots is important to eventually develop a snake robot that can navigate
by itself through an unknown and cluttered environment. In the following we list a number of challenges to overcome before such a scenario is
realizable. 1) Develop motion patterns based on the mathematical model
and observations of real snakes that enable a snake robot to detect and
exploit obstacles by itself for e¢ cient locomotion. Such a motion pattern
will enable a snake robot to move within, for example, a shattered building
while looking for people. 2) Extend the 3D model to include other ground
shapes such as stairs. This will be advantageous for the development of
the motion pattern mentioned in ‘1)’. 3) Construct a smooth exterior of
the snake robot. This will enable the snake robot to easily slide beside or
over any irregularities on the ground. 4) Construct robust snake robots
with powerful and lightweight joint actuators to be able to continue being
operational even though some of the joint actuators break down. 5) Develop a snake robot exterior that can withstand large temperatures during,
for example, a …re-…ghting operation, while still keeping the temperature
within the snake robot relatively low. 6) Develop control algorithms that
consider the global positions of all parts of a snake robot. This will help
keep the snake robot from unintentionally colliding with its environment
during operations in fragile environments.
A snake robot has limited payload capability, poor power e¢ ciency and a
high number of degrees of freedom. Nevertheless, the snake robot has the
potential of great traversability and capability of inspecting narrow places. It
can also be made very robust to dirt and dust by covering the robot completely
with a shell. These properties make the research on snake robots worth-while. It
is hoped that this thesis will help to promote further research on the fascinating
topic of snake robots.
166
Conclusions and Further Work
Bibliography
Alart, P. and A. Curnier (1991). A mixed formulation for frictional contact
problems prone to Newton like solution methods. Comput. Method. Appl.
M. 92, 353–375.
Ayers, J., C. Wilbur and C. Olcott (2000). Lamprey robots. In: Proc. Int.
Symp. Aqua Biomechanisms.
Bauchot, R. (1994). Snakes: A Natural History. Sterling Publishing Company.
Bayraktaroglu, Z. Y., A. Kilicarslan, A. Kuzucu, V. Hugel and P. Blazevic
(2006). Design and control of biologically inspired wheel-less snake-like
robot. In: Proc. IEEE/RAS-EMBS Int. Conf. Biomedical Robotics and
Biomechatronics. pp. 1001–1006.
Bayraktaroglu, Z. Y. and P. Blazevic (2005). Understanding snakelike locomotion through a novel push-point approach. J. Dyn. Syst. - Trans.
ASME 127(1), 146–152.
Bayraktarouglu, Z. Y., F. Butel, P. Blazevic and V. Pasqui (1999). A geometrical approach to the trajectory planning of a snake-like mechanism.
In: Proc. IEEE/RSJ Int. Conf. Intelligent Robots and Systems. pp. 1322–
1327.
Bloch, A. M., J. Baillieul, P. Crouch and J. Marsden (2003). Nonholonomic
Mechanics and Control. Springer-Verlag.
Bloch, A. M., P. S. Krishnaprasad, J. E. Marsden and R. M. Murray (1995).
Nonholonomic mechanical systems with symmetry. Technical report. California Institute of Technology.
Boyer, F., M. Porez and W. Khalil (2006). Macro-continuous computed
torque algorithm for a three-dimensional eel-like robot. IEEE Trans. Robot. 22(4), 763–775.
168
Bibliography
Brogliato, B. (1999). Nonsmooth Mechanics. 2 ed. Springer. London.
Brovoll, Sverre (2006). Design and implementation of a snake robot actuated by dc-motors. Master’s thesis. Norwegian University of Science and
Technology. In Norwegian.
Burdick, J. W., J. Radford and G. S. Chirikjian (1993). A ‘sidewinding’
locomotion gait for hyper-redundant robots. In: Proc. IEEE Int. Conf.
Robotics and Automation. pp. 101–106.
Chen, L., Y. Wang, S. Ma and B. Li (2004). Studies on lateral rolling
locomotion of a snake robot. In: Proc. IEEE Int. Conf. Robotics and
Automation. pp. 5070–5074.
Chernousko, F. (2005). Modelling of snake-like locomotion. Appl. Math.
Comput. 164(2), 415–434.
Chernousko, F. L. (2000). Snake-like motions of multibody systems over a
rough plane. In: Proc. 2nd Int. Conf. Control of Oscillations and Chaos.
pp. 321–326.
Chirikjian, G. S. (1992). Theory and Applications of Hyper-Redundant
Robotic Manipulators. PhD thesis. California Institute of Technology.
Pasadena, California.
Chirikjian, G. S. and J. W. Burdick (1991). Kinematics of hyper-redundant
robot locomotion with applications to grasping. In: Proc. IEEE Int.
Conf. Robotics and Automation. pp. 720–725.
Chirikjian, G. S. and J. W. Burdick (1993). Design and experiments with
a 30 DOF robot. In: Proc. IEEE Int. Conf. Robotics and Automation.
Vol. 3. pp. 113–119.
Chirikjian, G. S. and J. W. Burdick (1994a). A hyper-redundant manipulator. IEEE Robotics & Automation Magazine 1(4), 22–29.
Chirikjian, G. S. and J. W. Burdick (1994b). A modal approach to
hyper-redundant manipulator kinematics. IEEE Trans. Robot. Autom.
10(3), 343–354.
Chirikjian, G. S. and J. W. Burdick (1995). The kinematics of hyperredundant robot locomotion. IEEE Trans. Robot. Autom. 11(6), 781–
793.
Bibliography
169
Choset, H. (2007). Snake robots at Carnegie Mellon University.
http://www.snakerobot.com/. Online. Accessed 29 September 2007.
Choset, H. and J. Y. Lee (2001). Sensor-based construction of a retract-like
structure for a planar rod robot. IEEE Trans. Robot. Autom. 17(4), 435–
449.
Choset, H., J. Luntz, E. Shammas, T. Rached, D. Hull and C. C. Dent
(2000). Design and motion planning for serpentine robots. In: Proc.
SPIE Smart Structures and Materials: Smart Electronics and MEMS.
Vol. 3990. Newport Beach, CA, USA. pp. 148 –155. Serpentine robots;.
Date, H., Y. Hoshi and M. Sampei (2000). Locomotion control of a snakelike robot based on dynamic manipulability. In: Proc. IEEE/RSJ Int.
Conf. Intelligent Robots and Systems.
Date, H., Y. Hoshi, M. Sampei and N. Shigeki (2001). Locomotion control
of a snake robot with constraint force attenuation. In: Proc. American
Control Conference. pp. 113–118.
Do Carmo, M. P. (1976). Di¤ erential Geometry of Curves and Surfaces.
Prentice-Hall. Englewood Cli¤s, New Jersey.
Dowling, K. (1999). Limbless locomotion: learning to crawl. In: Proc. IEEE
Int. Conf. Robotics and Automation. Vol. 4.
Dowling, K. J. (1997). Limbless Locomotion. Learing to Crawl with a Snake
Robot. PhD thesis. Carnegie Mellon University.
Egeland, O. and J. T. Gravdahl (2002). Modeling and Simulation for Automatic Control. Marine Cybernetics. Trondheim, Norway.
Erkmen, I., A. M. Erkmen, F. Matsuno, R. Chatterjee and T. Kamegawa
(2002). Snake robots to the rescue!. IEEE Robotics & Automation Magazine 9(3), 17–25.
Fossen, T. I. (2002). Marine Control Systems: Guidance, Navigation and
Control of Ships, Rigs and Underwater Vehicles. Marine Cybernetics.
Trondheim, Norway.
Glocker, Ch. (2001). Set-Valued Force Laws, Dynamics of Non-Smooth Systems. Vol. 1 of Lecture Notes in Applied Mechanics. Springer-Verlag.
Berlin.
170
Bibliography
Glocker, Ch. and C. Studer (2005). Formulation and preparation for numerical evaluation of linear complementarity systems in dynamics. Multibody
System Dynamics 13, 447–463.
Grabec, I. (2002). Control of a creeping snake-like robot. In: Proc. 7th Int.
Workshop on Advanced Motion Control. pp. 526–513.
Gray, J. (1946). The mechanism of locomotion in snakes. J. Exp. Biol.
23(2), 101–120.
Gu, Y.-L. and Y. Xu (1993). A normal form augmentation approach to
adaptive control of space robot systems. In: Proc. IEEE Int. Conf. Robotics and Automation. Vol. 2. pp. 731–737.
Henning, W., F. Hickman and H. Choset (1998). Motion planning for serpentine robots. In: Proc. ASCE Space and Robotics.
Hirose, S. (1993). Biologically Inspired Robots: Snake-Like Locomotors and
Manipulators. Oxford University Press. Oxford.
Hirose, S. and A. Morishima (1990). Design and control of a mobil robot
with an articulated body. Int. J. Rob. Res. 9(2), 99–114.
Hirose, S. and M. Mori (2004). Biologically inspired snake-like robots. In:
Proc. IEEE Int. Conf. Robotics and Biomimetics. pp. 1–7.
Hirose, S. and Y. Umetani (1976). Kinematic control of active cord mechanism with tactile sensors. In: Proc. 2nd RoMAnSy Symp. Warsaw.
pp. 241–252.
Kamegawa, T., T. Yarnasaki, H. Igarashi and F. Matsuno (2004). Development of the snake-like rescue robot ‘Kohga’. In: Proc. IEEE Int. Conf.
Robotics and Automation. Vol. 5. pp. 5081–5086.
Kelly, S. D. and R. M. Murray (1995). Geometric phases and robotic locomotion. J. Robotic Systems 12(6), 417–431.
Khalil, H. K. (2002). Nonlinear Systems. 3rd ed. Prentice Hall. International
Edition.
Kolmanovsky, I. and N. H. McClamroch (1995). Developments in nonholonomic control problems. IEEE Contr. Syst. Mag. 15(6), 20–36.
Bibliography
171
Krishnaprasad, P. S. and D. P. Tsakiris (1994). G-snakes: Nonholonomic
kinematic chains on lie groups. In: Proc. 33rd IEEE Conf. Decision and
Control. Vol. 3. Lake Buena Vista, FL USA. pp. 2955–2960.
Le Saux, C., R. I. Leine and Ch. Glocker (2005). Dynamics of a rolling disk
in the presence of dry friction. Journal of Nonlinear Science 15(1), 27–61.
Leine, R. I. and H. Nijmeijer (2004). Dynamics and Bifurcations of NonSmooth Mechanical Systems. Vol. 18 of Lecture Notes in Applied and
Computational Mechanics. Springer Verlag. Berlin.
Lewis, M. A. and D. M. Zehnpfennig (1994). R7: A snake-like robot for
3-D visual inspection. In: Proc. IEEE/RSJ Int. Conf. Intelligent Robots
and Systems. Vol. 2. Munich Germany. pp. 1310–1317.
Liljebäck, P. (2004). Modular snake-robot: Modeling, implementation and
control of a modular and pressure based snake-robot. Master’s thesis.
Norwegian University of Technology and Science. Trondheim, Norway.
In Norwegian.
Liljebäck, P., Ø. Stavdahl and K. Y. Pettersen (2005). Modular pneumatic
snake robot: 3D modelling, implementation and control. In: Proc. 16th
IFAC World Congress.
Lu, Y., S. Ma, B. Li and L. Chen (2003). Ground condition sensing of a
snake-like robot. In: Proc. IEEE Int. Conf. Robotics, Intelligent Systems
and Signal Processing. Vol. 2. pp. 1075–1080.
Ma, S. (1999). Analysis of snake movement forms for realization of snakelike robots. In: Proc. IEEE Int. Conf. Robotics and Automation. Vol. 4.
Detroit, MI USA. pp. 3007–3013.
Ma, S. (2001). Analysis of creeping locomotion of a snake-like robot. Adv.
Robotics 15(2), 205–224.
Ma, S. and N. Tadokoro (2006). Analysis of creeping locomotion of a snakelike robot on a slope. Autonomous Robots 20, 15–23.
Ma, S., H. Araya and L. Li (2001a). Development of a creeping snake-robot.
In: Proc. IEEE Int. Symp. Computational Intelligence in Robotics and
Automation. pp. 77–82.
172
Bibliography
Ma, S., N. Tadokoro, B. Li and K. Inoue (2003a). Analysis of creeping locomotion of a snake robot on a slope. In: Proc. IEEE Int. Conf. Robotics
and Automation. pp. 2073–2078.
Ma, S., W. J. Li and Y. Wang (2001b). A simulator to analyze creeping
locomotion of a snake-like robot. In: Proc. IEEE Int. Conf. Robotics and
Automation. Vol. 4.
Ma, S., Y. Ohmameuda and K. Inoue (2004). Dynamic analysis of 3dimensional snake robots. In: Proc. IEEE/RSJ Int. Conf. Intelligent
Robots and Systems. pp. 767–772.
Ma, S., Y. Ohmameuda, K. Inoue and B. Li (2003b). Control of a 3dimensional snake-like robot. In: Proc. IEEE Int. Conf. Robotics and
Automation. Vol. 2. Taipei, Taiwan. pp. 2067 –2072.
Masayuki, A., T. Takayama and S. Hirose (2004). Development of ‘SouryuIII’: connected crawler vehicle for inspection inside narrow and winding
spaces. In: Proc. IEEE Int. Conf. Intelligent Robots and Systems. Vol. 1.
pp. 52–57.
Mattison, C. (2002). The Encyclopaedia of Snakes. Cassell Paperbacks. London.
McIsaac, K. A. and J. P. Ostrowski (1999). A geometric approach to anguilliform locomotion: Modelling of an underwater eel robot. In: Proc.
IEEE Int. Conf. Robotics and Automation. Vol. 4. pp. 2843–2848.
McIsaac, K. A. and J. P. Ostrowski (2000). Motion planning for dynamic
eel-like robots. In: Proc IEEE Int. Conf. Robotics and Automation.
Vol. 2. pp. 1695–1700.
McIsaac, K. A. and J. P. Ostrowski (2003a). A framework for steering
dynamic robotic locomotion systems. Int. J. Robot. Res. 22(2), 83–97.
McIsaac, K. A. and J. P. Ostrowski (2003b). Motion planning for anguilliform locomotion. IEEE Trans. Robot. Autom. 19(4), 637–625.
Miller, G. (2002). Neurotechnology for Biomimetic Robots. Chap. Snake
Robots for Search and Rescue, pp. 271–284. MIT Press Cambridge, MA,
USA. Cambridge/London.
Miller, G. (2007). Dr. Miller’s snake robots. http://www.snakerobots.com/.
Online. Accessed 29 September 2007.
Bibliography
173
Moreau, J. J. (1988). Unilateral contact and dry friction in …nite freedom
dynamics. In: Non-Smooth Mechanics and Applications (J. J. Moreau
and P. D. Panagiotopoulos, Eds.). Vol. 302 of CISM Courses and Lectures. pp. 1–82. Springer Verlag. Wien.
Mori, M. and S. Hirose (2002). Three-dimensional serpentine motion and
lateral rolling by active cord mechanism ACM-R3. In: Proc. IEEE/RSJ
Int. Conf. Intelligent Robots and Systems. pp. 829–834.
Murray, R. M., Z. Li and S. S. Sastry (1994). A Mathematical Introduction
to Robotic Manipulation. 1st ed. CRC Press. Florida, USA.
Nijmeijer, H. and A. van der Schaft (1990). Nonlinear Dynamical Control
Systems. Springer-Verlag. New York.
Nilsson, M. (1997a). Ripple and roll: Slip-free snake robot locomotion. In:
Proc. Mechatronical Computer Systems for Perception and Action. Piza,
Italy.
Nilsson, M. (1997b). Snake robot free climbing. In: Proc. IEEE Int. Conf.
Robotics and Automation. Vol. 4. pp. 3415–3420.
Nilsson, M. (1998). Snake robot – free climbing. IEEE Contr. Syst. Mag.
18(1), 21–26.
Nilsson, M. (2004). Serpentine locomotion on surfaces with uniform friction.
In: Proc. IEEE/RSJ Int. Conf. Intelligent Robots and Systems. pp. 1751–
1755.
Ohmameuda, Y. and S. Ma (2002). Control of a 3-dimensional snake-like
robot for analysis of sinus-lifting motion. In: Proc. 41st SICE Annual
Conference. Vol. 3. pp. 1487–1491.
Ohno, H. and S. Hirose (2001). Design of slim slime robot and its gait
of locomotion. In: Proc. IEEE/RSJ Int. Conf. Intelligent Robots and
Systems. Vol. 2. pp. 707–715.
Ostrowski, J. and J. Burdick (1996). Gait kinematics for a serpentine robot.
In: Proc. IEEE Int. Conf. Robotics and Automation. Vol. 2. pp. 1294–
1299.
Ostrowski, J. and J. Burdick (1998). The geometric mechanics of undulatory robotic locomotion. Int. J. Robot. Res. 17(7), 683–701.
174
Bibliography
Poi, G., C. Scarabeo and B. Allotta (1998). Traveling wave locomotion
hyper-redundant mobile robot. In: Proc. IEEE Int. Conf. Robotics and
Automation. Vol. 1. pp. 418–423.
Prautsch, P. and T. Mita (1999). Control and analysis of the gait of snake
robots. In: Proc. IEEE Int. Conf. Control Applications. Kohala Coast,
HI USA. pp. 502–507.
Rincon, D. M. and J. Sotelo (2003). Ver-Vite: Dynamic and experimental
analysis for inchwormlike biomimetic robots. IEEE Robot. Autom. Mag.
10(4), 53–57.
Rockafellar, R. T. (1970). Convex Analysis. Princeton Landmarks in Mathematics. Princeton University Press. Princeton, New Jersey.
Saito, M., M. Fukaya and T. Iwasaki (2002). Serpentine locomotion with
robotic snakes. IEEE Contr. Syst. Mag. 22(1), 64–81.
Shan, Y. and Y. Koren (1993). Design and motion planning of a mechanical
snake. IEEE Trans. Syst. Man Cyb. 23(4), 1091–1100.
Spong, M. W. (1996). Energy based control of a class of underactuated mechanical systems. In: Proc. 13th IFAC World Congress. Vol. F. pp. 431–
436.
Spong, M. W. and M. Vidyasagar (1989). Robot Dynamics and Control.
Wiley & Sons Inc.
Tanev, I., T. Ray and A. Buller (2005). Automated evolutionary design, robustness, and adaptation of sidewinding locomotion of a simulated snakelike robot. IEEE Trans. Robot. 21(4), 632–645.
Togawa, K., M. Mori and S. Hirose (2000). Study on three-dimensional
active cord mechanism: Development of ACM-R2. In: Proc. IEEE/RSJ
Int. Conf. Intelligent Robots and Systems. Vol. 3. pp. 2242–2247.
Transeth, A. A. and K. Y. Pettersen (2006). Developments in snake robot
modeling and locomotion. In: Proc. IEEE Int. Conf. Control, Automation, Robotics and Vision. Singapore. pp. 1393–1400.
Transeth, A. A. and K. Y. Pettersen (2008). A survey on snake robot modeling and locomotion. Robotica. Submitted.
Bibliography
175
Transeth, A. A., N. van de Wouw, A. Pavlov, J. P. Hespanha and K. Y.
Pettersen (2007a). Tracking control for snake robot joints. In: Proc.
IEEE/RSJ Int. Conf. Intelligent Robots and Systems. San Diego, CA,
USA. pp. 3539–3546.
Transeth, A. A., P. Liljebäck and K. Y. Pettersen (2007b). Snake robot
obstacle aided locomotion: An experimental validation of a non-smooth
modeling approach. In: Proc. IEEE/RSJ Int. Conf. Intelligent Robots
and Systems. San Diego, CA, USA. pp. 2582–2589.
Transeth, A. A., R. I. Leine, Ch. Glocker and K. Y. Pettersen (2006a). Nonsmooth 3D modeling of a snake robot with external obstacles. In: Proc.
IEEE Int. Conf. Robotics and Biomimetics. Kunming, China. pp. 1189–
1196.
Transeth, A. A., R. I. Leine, Ch. Glocker and K. Y. Pettersen (2006b).
Non-smooth 3D modeling of a snake robot with frictional unilateral constraints. In: Proc. IEEE Int. Conf. Robotics and Biomimetics. Kunming,
China. pp. 1181–1188.
Transeth, A. A., R. I. Leine, Ch. Glocker and K. Y. Pettersen (2008a).
3D snake robot motion: Non-smooth modeling, simulations, and experiments. IEEE Trans. Robot. Accepted.
Transeth, A. A., R. I. Leine, Ch. Glocker, K. Y. Pettersen and P. Liljebäck
(2008b). Snake robot obstacle aided locomotion: Modeling, simulations,
and experiments. IEEE Trans. Robot. Accepted.
Wiriyacharoensunthorn, P. and S. Laowattana (2002). Analysis and design
of a multi-link mobile robot (serpentine). In: Proc. IEEE Int. Conf.
Robotics, Intelligent Systems and Signal Processing. Vol. 2. pp. 694–699.
Worst, R. and R. Linnemann (1996). Construction and operation of a snakelike robot. In: Proc. IEEE Int. Joint Symp. Intelligence and Systems.
Rockville, MD USA. pp. 164–169.
Xinyu, L. and F. Matsuno (2003). Control of snake-like robot based on
kinematic model with image sensor. In: Proc. IEEE Int. Conf. Robotics,
Intelligent Systems and Signal Processing. Vol. 1. pp. 347–352.
Yamada, H. and S. Hirose (2006a). Development of practical 3-dimensional
active cord mechanism ACM-R4. Journal of Robotics and Mechatronics
18(3), 1–7.
176
Bibliography
Yamada, H. and S. Hirose (2006b). Study on the 3D shape of active
cord mechanism. In: Proc. IEEE Int. Conf. Robotics and Automation.
pp. 2890–2895.
Yamada, H., S. Chigisaki, M. Mori, K.Takita, K. Ogami and S. Hirose
(2005). Development of amphibious snake-like robot ACM-R5. In: Proc.
36th Int. Symp. Robotics.
Ye, C., S. Ma, B. Li and Y. Wang (2004a). Turning and side motion of
snake-like robot. In: Proc. IEEE Int. Conf. Robotics and Automation.
Vol. 5. pp. 5075–5080.
Ye, C., S. Ma, B. Li and Y. Wang (2004b). Twist-related locomotion of a 3D
snake-like robot. In: Proc. IEEE Int. Conf. Robotics and Biomimetics.
pp. 589–594.
Appendix A
An additional Proof and a
Theorem for Chapter 6
This appendix includes a theorem and and a proof referred to in Chapter 6.
First, we include a theorem from Khalil (2002). Then we prove that MV
in (6.102) on page 127 is positive de…nite.
A.1
Theorem on Boundedness
Theorem 4.18 on page 172 in Khalil (2002) is employed in Chapter 6 to
show boundedness of the variable w. The remainder of this section is an
excerpt from Khalil (2002) and describes the theorem. We include it in this
thesis for the convenience of the reader. Consider the system
x_ = f (t; x) ;
(A.1)
where f : [0; 1) D ! Rm is piecewise continuous in t and locally Lipschitz
in x on [0; 1) D, and D Rm is a domain that contains the origin.
Theorem A.1 (Theorem 4.18 in Khalil (2002)) Let D Rm be a domain that contains the origin and V : [0; 1) D ! R be a continuously
di¤ erentiable function such that
1 (kxk)
@V
@V
+
f (t; x)
@t
@x
V (t; x)
W3 (x) ;
2 (kxk)
8 kxk
(A.2)
>0
(A.3)
8t 0 and 8x 2 D, where 1 and 2 are class K functions and W3 (x) is
a continuous positive de…nite function. Take r > 0 such that Br D and
178
An additional Proof and a Theorem for Chapter 6
suppose that
1
<
2
(
1 (r)) :
(A.4)
Then there exists a class KL function and for every initial state x (t0 ),
1
satisfying kx (t0 )k
0 (dependent on x (t0 ) and
2 ( 1 (r)), there is T
) such that the solution of (A.1) satis…es
kx (t)k
(kx (t0 )k ; t
1
kx (t)k
2
(
1(
t0 ) ;
)) ;
t
t0
t
t0 + T
t0 + T:
(A.5)
(A.6)
Moreover, if D = Rm and 1 belongs to class K1 , then (A.5) and (A.6)
hold for any initial state x (t0 ), with no restrictions on how large is.
Note that (A.5) and (A.6) show that x (t) is uniformly bounded for
all t
t0 and uniformly ultimately bounded with the ultimate bound
1
(
(
2 )).
1
A.2
Positive De…niteness of MV
In this section we will show that MV ( ) in (6.102) on page 127 is positive
de…nite. We see from (6.101)-(6.102) in Chapter 6 that
MV ( ) =
n
X
MVi ( ) ;
(A.7)
i=3
where
MVi ( ) =
X
Jv
T
i
3
RB
Bi
=R;F
FN i
D
2
3
RB
Bi
V
T
Jv
i
;
(A.8)
and
D
V
=
Vx
0
0
Vy
:
(A.9)
Consider the chain of massless rods with body-…xed frames
Bi
i
Bi = Oi ; eB
attached to the rear part of each rod. The chain
x ; ey
may move freely in the horizontal plane. Assume that two point masses
(i.e. without moment of inertia) are …xed to rod i such that they do not
coincide. A structure with two such rods is depicted in Figure A.1 where
we notice that the …rst rod is number 3 due to the inertial position and
orientation of the chained structure. Denote the centre of the inertial frame
A.2 Positive De…niteness of MV
179
Figure A.1: Chain of two massless rods with four point masses …xed to the
rods.
I = OI ; eIx ; eIy as OI . Assume that the mass matrix of each point mass
connected to rod i is
FN i
Mi =
D V:
(A.10)
2
Hence, each point mass has more kinetic energy when moving longitudinal
to the rod than transversal to it. An analogy to such a property is the added
mass matrix for a completely submerged rigid body with a particular shape
(see Fossen (2002) for more on added mass). Now we will proceed to develop
the complete mass matrix for the chain of rods and its kinetic energy.
De…ne the generalized coordinates for the system of connected masses
as
T
2 Rn ;
(A.11)
q = xO2 yO2
1
n 3
where O2 = (xO2 ; yO2 ) is the position of the rearmost part of rod 3 which
coincides the origin of frame B2 ; is the anti-clockwise rotation of frame B3
with respect to the inertial frame, and j , j = 1; : : : ; n 3; are the relative
angles between the adjacent rods. Let B3 v O2 be the translational velocity
of O2 given in frame B3 . Let the translational velocities of the front and
rear mass of rod i expressed in frame B3 be
B3 v Fi
where
= J vF i ;
B 3 v Ri
2
B 3 v O2
=4
_
_
3
5:
= JvRi ;
(A.12)
(A.13)
Moreover, JvF i 2 R2 n and JvRi 2 R2 n are equal to the Jacobians (6.32)(6.33) by de…ning the rods to be of the same length as the links of the snake
180
An additional Proof and a Theorem for Chapter 6
robot presented in Chapter 6 and that the point masses in this section are
placed at the same points as the contact points with the ground de…ned
for the links in Chapter 6. The velocities of the point masses are expressed
from (A.12) in their corresponding frames as
Bi v Fi
3
= RB
Bi
T
B3 v Fi ;
B i v Ri
3
= RB
Bi
T
B 3 v Ri ;
(A.14)
3
where RB
Bi is the rotation matrix that transforms a velocity vector from
being described in frame Bi to being described in frame B3 by the trans3
formation B3 v Fi = RB
Bi Bi v Fi . The total kinetic energy K of the chained
structure of connected rods can now be found as
n
X
K=
i=3
1
1
T
T
Mi Bi v Ri ;
Bi v Fi Mi Bi v Fi +
B v
2
2 i Ri
(A.15)
and by inserting (A.10), (A.12) and (A.14) into (A.15), we …nd that
1
K=
2
T
X
n
X
i =Ri ;Fi i=3
|
FN i
D
2
{z
3
(JvF i )T RB
Bi
MK
V
3
RB
Bi
T
:
J vF i
}
(A.16)
Since the kinetic energy of the system is always positive for k k 6= 0 we
have that MK is positive de…nite. Moreover, by comparing MV given in
(A.7) to MK indicated in (A.16) we see that they are equal. Hence, MV
is also positive de…nite, which concludes the proof.
Was this manual useful for you? yes no
Thank you for your participation!

* Your assessment is very important for improving the work of artificial intelligence, which forms the content of this project

Download PDF

advertisement