2009 Localization

R OBO C UP 2009
T EAM C HAOS D OCUMENTATION
Humberto Martnez Barbera

Juan Jose Alcaraz Jimenez
Pedro Cavestany Olivares
David Herrero Perez
University of Murcia
Carlos III University
Department of Information and

Communications Engineering
30100 Murcia, Spain
Department of Systems Engineering

and Automation
28911 Leganes, Spain
Acknowledgements
For the elaboration of this document we have used material from past team members,
be them from the original TeamSweden or its follow-up TeamChaos, all participating in
the Four-Legged League with the different AIBOs. The list is rather large, so we strongly
thank all of them for their contributions and effort.
In particular, this document is based on previous team description papers and some
conference papers, from which we have borrowed an important amount of information.
Although they appear in the references, we want to explicitly cite the following papers
and give extra thanks to their authors:
A. Saffiotti and K. LeBlanc. Active Perceptual Anchoring of Robot Behavior in a
Dynamic Environment. Int. Conf. on Robotics and Automation (San Francisco,
CA, 2000) pp. 3796-3802.
P. Buschka, A. Saffiotti, and Z. Wasik. Fuzzy Landmark-Based Localization for a
Legged Robot. Int. Conf. on Intelligent Robotic Systems (IROS) Takamatsu, Japan,
2000.
Z. Wasik and A. Saffiotti. Robust Color Segmentation for the RoboCup Domain.
Int. Conf. on Pattern Recognition (ICPR), Quebec City, CA, 2002.
A. Saffiotti and Z. Wasik. Using Hierarchical Fuzzy Behaviors in the RoboCup Domain . In: C. Zhou, D. Maravall and D. Ruan, eds, Autonomous Robotic Systems,
Springer, DE, 2003.
J.P Canovas, K. LeBlanc and A. Saffiotti. Multi-Robot Object Localization by Fuzzy
Logic. Proc. of the Int. RoboCup Symposium, Lisbon, Portugal, 2004.
TeamChaos Documentation
Contents
Contents
1 Architecture
1.1 Introduction . . . . . . . . . . . . . . . .
1.2 ThinkingCap Architecture . . . . . . . .
1.3 Naoqi . . . . . . . . . . . . . . . . . . .
1.3.1 Modules y brokers . . . . . . . .
1.3.2 Main broker modules . . . . . . .
1.3.3 Adaptation . . . . . . . . . . . .
1.4 Timing of periodic functions in OpenNao
1.5 Communications . . . . . . . . . . . . .
1.5.1 CommsModule . . . . . . . . . .
1.5.2 CommsManager . . . . . . . . . .
1.6 Message . . . . . . . . . . . . . . . . . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
2 Locomotion
2.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . .
2.1.1 Information received by the locomotion module
2.1.2 Information provided by the locomotion module
2.1.3 Class diagram . . . . . . . . . . . . . . . . . . .
2.2 Description of the robot Nao . . . . . . . . . . . . . . .
2.3 Navigation . . . . . . . . . . . . . . . . . . . . . . . . .
2.3.1 Foot orientation . . . . . . . . . . . . . . . . . .
2.3.2 Foot position . . . . . . . . . . . . . . . . . . .
2.4 Kinematics . . . . . . . . . . . . . . . . . . . . . . . .
2.4.1 State-machine of kinematics . . . . . . . . . . .
2.4.2 Feet motion . . . . . . . . . . . . . . . . . . . .
2.4.3 Hip motion . . . . . . . . . . . . . . . . . . . .
2.5 Direct kinematic problem . . . . . . . . . . . . . . . .
2.6 Inverse kinematics . . . . . . . . . . . . . . . . . . . .
2.6.1 Telescopic leg . . . . . . . . . . . . . . . . . . .
2.6.2 Leg length . . . . . . . . . . . . . . . . . . . . .
2.6.3 Foot orientation . . . . . . . . . . . . . . . . . .
2.6.4 Combining previous results . . . . . . . . . . . .
i
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
1
1
2
3
4
5
6
8
9
10
11
11
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
15
15
15
16
16
18
21
21
22
24
25
26
26
36
39
40
40
42
46
Contents
2.7
Future works . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 46
3 Perception
3.1 PAM: Perceptual Anchoring Module
3.1.1 Standard vision pipeline . . .
3.1.2 Experimental vision pipeline .
3.2 Color Segmentation . . . . . . . . . .
3.2.1 Seed Region Growing . . . . .
3.2.2 Experiments . . . . . . . . . .
3.3 Corner Detection and Classification .
3.4 Goal recognition . . . . . . . . . . . .
3.4.1 Sighting the goal . . . . . . .
3.4.2 Case discrimination . . . . . .
3.4.3 Goal location . . . . . . . . .
3.4.4 Filters . . . . . . . . . . . . .
3.5 Active Perception . . . . . . . . . . .
3.5.1 Perception-based Behaviour .
3.5.2 Active Perceptual Anchoring .
3.5.3 Implementation . . . . . . . .
3.5.4 Experiments . . . . . . . . . .
4 Self-Localization
4.1 Uncertainty Representation . . . . .
4.1.1 Fuzzy locations . . . . . . . .
4.1.2 Representing the robots pose
4.1.3 Representing the observations
4.2 Fuzzy Self-Localization . . . . . . . .
4.3 Experimental results . . . . . . . . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
5 Behaviours
5.1 Low-level Behaviours . . . . . . . . . . . .
5.1.1 Basic Behaviours . . . . . . . . . .
5.1.2 Fuzzy Arbitration . . . . . . . . . .
5.1.3 The Behaviour Hierarchy . . . . . .
5.2 High-level Behaviours . . . . . . . . . . . .
5.2.1 Hierarchical Finite State Machines
5.3 Team Coordination . . . . . . . . . . . . .
5.3.1 Ball Booking . . . . . . . . . . . .
5.3.2 Implementation . . . . . . . . . . .
5.4 The Players . . . . . . . . . . . . . . . . .
5.4.1 GoalKeeper . . . . . . . . . . . . .
5.4.2 Soccer Player . . . . . . . . . . . .
ii
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
47
47
47
48
49
50
51
53
58
58
59
60
63
64
64
66
68
70
.
.
.
.
.
.
73
74
74
74
75
76
79
.
.
.
.
.
.
.
.
.
.
.
.
81
81
81
84
85
86
87
87
88
88
91
91
94
Contents
A ChaosManager Tools
A.1 Introduction . . . . . . . . . .
A.2 Vision . . . . . . . . . . . . .
A.2.1 Color Calibration . . .
A.2.2 General Configuration
A.3 Game Monitor . . . . . . . . .
A.4 Game Controller . . . . . . .
B HFSM Builder
B.1 Using the Application . . . .
B.1.1 Buttons and Menus . .
B.1.2 Code Generation . . .
B.2 Building a Sample Automata
B.3 Implementation . . . . . . . .
B.3.1 File Format . . . . . .
B.3.2 Class Diagrams . . . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
iii
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
99
99
99
100
101
101
102
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
105
. 105
. 106
. 108
. 109
. 110
. 110
. 111
Contents
iv
Architecture
Chapter 1
Architecture
1.1
Introduction
The problem of creating a robotic football team is a very difficult and challenging problem.
There are several fields involved (low level locomotion, perception, location, behaviour
development, communications, etc.), which should be developed for building a fully functional team. In addition debugging and monitoring tools are also needed. In practical
terms, this means that the software project can be very large. In fact, if we execute the
command sloccount[45] to our current robots code (that is, excluding the off-board tools),
we get:
[...]
Total Physical Source Lines of Code (SLOC)
[...]
= 60,987
There are more than 60.000 lines of code. This fact shows that if we dont organise
and structure the project, it could be very difficult to manage. We use the Eclipse IDE
for programming and debugging and SVN for sharing code and documentation.
TeamChaos development is organised in three Eclipse and SVN projects: TeamChaos,
ChaosDocs, and ChaosManager. TeamChaos contains all code related to the robot, ChaosManager is a suite of tools for calibrating, preparing memory sticks and monitoring different aspects of the robots and the games, and ChaosDocs contains all the available
documentation, including team reports, team description papers, and RoboCup applications.
Communication is an important aspect because we can use external tools for achieving laborious tasks easily. Using the ChaosManager we receive images from the robots
and we can refine the camera parameters and reconfigure the robots camera while the
robot is running. We can teleoperate the robot for testing kicks or locomotion using
communication between robots and ChaosManager.
1
Architecture
We will study the special characteristics of the robot Nao and how has TeamChaos
proceeded to adapt our AIBO-based existing system to them. The operative system is
OpenNao, a Linux based distribution and over this operative system there is a middleware layer called Naoqi, developed by the enterprise which builds the robot: Aldebaran
Robotics.
1.2
ThinkingCap Architecture
Each robot uses the layered architecture shown in Figure. 1.1. This is a variant of the
ThinkingCap architecture, which is a framework for building autonomous robots jointly
developed by Orebro
University and the University of Murcia [25]. We outline below the
main elements of this architecture:
Other
Robot
Other
Robot
Team
Communication
Module
Global
Map
LocalState
GlobalState
Hierarchical
Finite
StateMachine
LocalState
Communication
Layer
HigherLayer
Behaviours
Perceptual
Anchoring
Module
Sensor
Data
Messages
Hierarchical
Behaviour
Module
Motor
Commands
Commander
MiddleLayer
LowerLayer
Figure 1.1: The variant of the ThinkingCap architecture.
The lower layer (commander module, or CMD) provides an abstract interface to the
sensori-motor functionalities of the robot. The CMD accepts abstract commands
from the upper layer, and implements them in terms of actual motion of the robot
effectors. In particular, CMD receives set-points for the desired displacement velocity < vx , vy , v >, where vx , vy are the forward and lateral velocities and v is the
angular velocity, and translates them to an appropriate walking style by controlling
the individual leg joints.
The middle layer maintains a consistent representation of the space around the robot
(Perceptual Anchoring Module, or PAM), and implements a set of robust tactical
behaviours (Hierarchical Behaviour Module, or HBM). The PAM acts as a shortterm memory of the location of the objects around the robot: at every moment, the
PAM contains an estimate of the position of these objects based on a combination of
current and past observations with self-motion information. For reference, objects
are named Ball, Net1 (own net), Net2 (opponent net), and LM1-LM6 (the six
possible landmarks, although only four of them are currently used). The PAM
is also in charge of camera control, by selecting the fixation point according to the
2
Architecture
current perceptual needs [34]. The HBM realizes a set of navigation and ball control
behaviours, and it is described in greater detail in the following sections.
The higher layer maintains a global map of the field (GM) and makes real-time
strategic decisions based on the current situation (situation assessment and role
selection is performed in the HFSM, or Hierarchical Finite State Machine). Selflocalization in the GM is based on fuzzy logic [10] [17]. The HFSM implements a
behaviour selection scheme based on finite state machines [20].
Radio communication is used to exchange position and coordination information
with other robots (via the TCM, or Team Communication Module).
This architecture provides effective modularisation as well as clean interfaces, making
it easy to develop different parts of it. Furthermore, its distributed implementation allows
the execution of each module in a computer or robot indifferently. For instance, the low
level modules can be executed onboard a robot and the high level modules can be executed
offboard, where some debugging tools are available. However, a distributed implementation generates serious synchronisation problems. This causes delays in decisions and
robots cannot react fast enough to dynamic changes in the environment. For this reason
we have favoured the implementation of a mixed mode architecture: at compilation time
it is decided whether it will be a distributed version (each module is a thread, Figure 1.1)
or a monolithic one (the whole architecture is a thread and the communication module is
another, Figure. 1.2).
COMMUNICATIONIMPLENTATION
Other
Robot
Team
Communication
Module
Other
Robot
Messages
CONTROLIMPLENTATION
Global
Map
LocalState
GlobalState
Hierarchical
Finite
StateMachine
LocalState
Behaviours
Perceptual
Anchoring
Module
Sensor
Data
Hierarchical
Behaviour
Module
Commander
Motor
Commands
Figure 1.2: Implementation of the ThinkingCap architecture.
1.3
Naoqi
Naoqi is a distributed environment that can manage as many binary executables as the
user desires, depending on the chosen architecture. However, the particularities of its
designs make us consider carefully the way our system is going to lay over this middleware.
Some features such as SOAP communications between modules could be interesting to
3
Architecture
BIOS
GruB
kboot
linux
NaoQi(Main Module)
ChaosModule
RobotManagaer
Figure 1.3: The stages of the booting process

make more comfortable the task of a programmer in a general environment, but for the
teams participating in RoboCup this is not the reality, since the target is to achieve the
maximum performance and this feature adds a fatal delay to our system.
Before launching Naoqi (with our module ChaosModule), the system goes through
several phases in the booting process. In figure 1.3 is shown the booting process of NoaQi.
First, the motherboard initializing process is carried out, then GruB is launched to load
kboot, which is a minimum image of Linux which used as a workaround for the lack of
support of the BIOS to the NAND controller. Finally, the operative system is loaded and
over it the middleware NaoQi, which automatically launches our modules ChaosModule.
NaoQi proposes an structure of modules and brokers to organize the different pieces
of software running on the robot. The standard implementation of NaoQi only includes
a main broker and several modules to manage the different sensors and actuators of the
robot. Next, we will describe this architectural units of NaoQi.
1.3.1
Modules y brokers
The brokers are executables with a server listening to an IP address. All brokers are
connected to other brokers except the main one.
The modules are classes derived from ALModule that can be loaded by a broker.
Both brokers and modules can be linked locally or remotely. When we link them
remotely, they communicate with the rest of the system by SOAP, which makes the
communication tasks pretty heavies. The main advantage of this approach is that we will
be able to connect to the system from any other machine different from the one running
NaoQi if both machines are connected.
The advantage of linking the modules or brokers in a local way is the high efficiency,
since we get rid of SOAP.
4
Architecture
In the same way, if we use brokers instead of modules, we will get an higher computational cost. The positive part is that if the code belonging to a broker has an error, that
broker will fall but the rest of the brokers will continue running. On the other side, if our
code is associated to the main broker, all the NaoQi system will collapse and the robot
could suffer some damage. After this failure, a reboot of NaoQi is required.
1.3.2
Main broker modules
The standard version of Naoqi includes some default modules to interact with the robot.
Three of these modules are necessary to make the system work (ALLogger, ALMemory
and ALPreferences) while the rest of them are intended to make easier the use of the
robot. Next, we will describe briefly these modules.
ALLogger: manages the use of the logger to store information, warnings and errors.
ALMemory: can be used to share variables within a module or between different
modules. Any modules can add or request variables from ALMemory. This module
is thread-save, the modules which use ALMemory dont have to care about mutex
of semaphores. The data registered in ALMemory are of type ALValue, which is a
generic type able to store bool, int, float, double, string and ALValue vectors. In
order to request the value of a variable registered in ALMemory one can consult
the value or ask ALMemory to notify when the value of the variable changes (a
new thread is created). Among the values stored in ALMemory one can find the
sensor meassures, actuator values and error codes of the boards distributed along
the robot.
ALPreferences: checks the values assigned to some configuration parameters of the
system. These values are taken from the configuration files in the directory Naoqi
Preferences
when the system boots.
ALAudioPlayer: plays audio tracks in mp3 or wav.
ALLeds: turns on, turns off or sets the level of intensity of several leds. One of
the options variables let you access the leds by groups. Some of these groups are
predefined (all the leds of the right foot), but you can create new groups if you want
to.
ALMotion: is the module which manages the locomotion tasks. Its main function is
to set the values of the opening angles of the joints and to request the values of that
angles. In order to set the value of an angle, it can be indicated the speed of the joint
to reach that value or the time to get to that position. There are some functions
more complex that can be expressed through this modules, such as the modification
of the torso orientation or the position of the Center Of Mass. The more powerful
functions are, however, those that make the robot walk forward, laterally or turn
some angle over itself. These functions have some parameters like the distance to
walk, the high and length of the step or the maximum lateral movement of the hip.
5
Architecture
ALTextToSpeech: reads a text and plays the sentences. It sends commands to the
voice synthesizer engine and configures the voice. It can be used to change the
language or to generate a double voice effect (usually employed by cinema robots).
ALVision: communicates with the camera placed in the head of the robot. It can be
used to change several parameters of the camera such as the gain, the white balance
or to correct some of the aberrations of the lens.
ALUltraSound: modifies the pooling frequency of the ultrasound sensors. This
parameter must be above 240ms. This modules cant be used to read the values of
the sensors, for that purpose ALMemory module can be consulted.
ALWatchDog:
Monitors the level of charge of the battery, changes the colour of the LED
placed on the chest according to it.
If the battery level is low, disables locomotion and makes the robot adopt a
safe pose.
Plays Heat if the body of the robot overheats.
Manages the pressing of the chest button in the following way:
One pressing: Nao says hello, identifies itself and announces it IP.
Two pressings: Disables the stiffness of all motors smoothly.
Three pressings: Turns off the system in a proper way.
DCM: This module is architecturally under the rest. That is, the rest of the modules
use it. Provides a way to communicate with the robot at a lower level. For instance,
if one wants to move a joint, instead of using the locomotion module, it can be sent
a command to the DCM module with an order to set a value for the actuator that
wants to be moved. The timing information must be added to the order. Therefore,
the DCM can be used to have a deeper control over the hardware, but the interface
is far more complex.
1.3.3
Adaptation
In our project, we have adopted a design based in two different configurations for the
system. Both take a main class RobotManager that has instances of all the proxies
of the modules that are going to be used (for instance: DCM, ALMemory, ALMotion,
ALWatchdog...). The first of these configurations is designed for the daily development
and for the competition versions. The whole system is compiled as a dynamic library
that represents a module associated to the main broker of NaoQi and that is loaded
automatically in the booting stage. In this way, when NaoQi boots it loads our modules
and calls a function StartPlaying that sets the robot in a ready to play state (robot
standing and head turning and looking for the ball). When a signal is received (either
telematically or by pressing the chest button), the robot starts playing.
6
Architecture
Nao
Naoqi
MainModule
...
ChaosModule
ALproxie
Figure 1.4: Maximum performance configuration.
Figure 1.5: Remote debugging configuration.
Architecture
DCM
NaoCam
ALMemory
ALWatchDog
RobotManager
pam
comms
gm
ctrl
cmd
Figure 1.6: RobotManager adapts ThinkingCap architecture to NaoQi modules.

The second of these configurations is thought to be used in deep debugging activities.
The system is compiled as an executable that instantiates the class RobotManager and this
class connects to the modules that it needs remotely (through SOAP). This configuration
is not adequate to competitions, because SOAP communications are very heavies and
makes impossible the fluid communication between modules. However, this configuration
can be used to execute our code in another machine, making easier the debugging tasks.
In the same way, if our program goes wrong it doesnt takes with it the other NaoQi
modules, which can be important.
1.4
Timing of periodic functions in OpenNao
Our target will be to process as much data as possible from the sensors (specially from the
camera) and to generate frequent decisions of as much quality as possible. However, the
capacity of the processor that is employed is limited, and the code written by TeamChaos
must share it with other processes related to NaoQi.
Therefore, it is important to keep the load of the processor at a controlled level, in
order to avoid its saturation. The consequences of this processor saturation could be
dramatic. One of the consequences is the shaking of the robot when it moves. Since the
locomotion module is designed to generate sequences of movements periodically, if this
period is not precise, the movements of the joints of the robot do not match up the ones
calculated. As a result, the robot is very likely to fall while moving.
Moreover, if the robot shakes, the battery consumption rises. Every time that an
engine starts a movement there is an high peak of power consumption, and in shaking
8
Architecture
situations, the engines are starting and stopping in every cycle, instead of rotating in a
fluid way, approximately constant. As a result, the life of the battery decreases drastically.
Finally, since all the joints receive their movement commands at the same time, all the
engines generate a consumption peak at the same instant, which can possible destabilize
the electric system of the robot. If the battery is not very loaded or if the consumption
peak is very high, the amount of intensity supplied to the processor can be less than the
one needed by the processor. Then, the operative system will reboot suddenly, causing
an instant blackout in the robot.
One of the meassures taken by TeamChaos in order to avoid this problem has been to
reduce as much as possible the load of the processor in every cycle. To achieve this target,
the modules loaded by NaoQi have been reduced to its minimum expression. Some works
are still been doing in this field, and the final goal would be to use only those modules
necessaries for NaoQi execution (ALMemory, ALLogger and ALPreferences) and the DCM
module, which is used to interact with the robot at a lower level.
However, there is another cause for deterioration of the period of the locomotion
activities: the concurrency. Since OpenNao is not a real time operative system, it does
not ensure an accurate execution of the timing orders sent to the system. Nevertheless,
this leak of accuracy is not very important in mono-thread processes. The problem then
is to fit ThinkingCap architecture in a mono-thread process. One solution could be to
employ a long cycle period that allows all the modules of the ThinkingCap architecture
to execute once per cycle.
The problem with this approach is that the duration of the cycle would be too long
to generate smooth movements of the robot. For instance, if we only execute five cycles
per second, the walking style of the robot would have to be defined by five values of the
body joints per second, and this is not enough. The other solution is to decrease the
execution period of the main cycle, and to execute only some of the modules together
with the locomotion module in every cycle. After a few cycles all the modules will have
been executed at least once, and then the process continues.
1.5
Communications
The communication module, or COMMS, is the module responsible of the message exchange between a robot and its environment. One of its tasks is to define the format of the
messages that the robot are going to exchange between them and with the debugging and
monitoring tool (ChaosManager). Another task of this module is to manage the sending
and receiving process of that messages.
Since the system is based in a Linux platform, the communications will employ sockets.
This sockets can use either TCP or UDP protocols. We will choose one or the other
according to the type of message.
In general, it will be used UDP for all the messages that the robot exchange between
themselves, while TCP will be the protocol employed for the communications between
9
Architecture
cmd->updateSensors
pam->updateOdometry
pam->UpdateImage
cmd->updateMotion
cmd->updateSensors
pam->updateOdometry
gm->process
cmd->updateMotion
cmd->updateSensors
pam->updateOdometry
pam->UpdateImage
cmd->updateMotion
cmd->updateSensors
pam->updateOdometry
ctrl->process
pam->setNeeds
cmd->updateMotion
Figure 1.7: Supercycle of global execution. Every row is a locomotion cycle.
robots and ChaosManager.

There are three main classes in the communications module which define its functionality. These classes are CommsModule, CommsManager and Message.
1.5.1
CommsModule
This class defines the interface that must implement all the classes which can send and
receive messages. Basically, this interface includes an identification of the class which
will use the interface, a function to assign the main module (CommsManager) and the
definition of the prototype for the messages reception function.
In order to be qualified for receiving messages, a class must inherit from CommsModule
and register in commsManager.
10
Architecture
CommsManager
CommsModule
pam
ctrl
gm
cmd
Figure 1.8: Associations in the communication system.
1.5.2
CommsManager
This is the main class in the communication process. Once instantiated, it creates two
threads, one for listening to the UDP messages and the other for the TCP ones. In the
same way, it attends the sending requests from the classes which want to send messages.
In order to establish the TCP communication, first is necessary that a client (ChaosManager) tries to connect, and then the messages are transmitted as a data flow. That
is, none of the two parts knows when does a message starts or finishes. To figure it out,
the CommsManager class consults the field in the header of the messages that indicates
the total size of the message and takes into account the TCP property of not altering the
order of the bytes.
Therefore, if the messages are big or there is a saturation situation, it is likely to face
a message fragment assembling problem. It is the Message class the one which will put
together the fragments and count the remaining bits for getting the whole message. We
will talk about it in the following section.
1.6
Message
The Message class defines the way to store the message data and provides with methods
for message creation and fragment assembling. All the types of messages are defined in a
different class and they all must inherit from Message.
There are two different cases of creation of a Message object. During the reception
process, a Message is created from the data flow and during the sending one, the message
will be created out of a set of fields.
In this way, when a datagram is received, the class CommsManager employs the
method parseMessage of the Message class, which is static, to get an object of type
Message which matches up that data flow. In order to interpret the flow of bytes, the
class Messages analyzes the first bytes of the datagram, which will be the header ones,
and depending on them, the type and size of the message is set. Once known the type of
message, the bytes of the data flow are assigned to the fields of the corresponding message
11
Architecture
Type
MsgBallBooking
MsgBehaviour
MsgDebugCtrl
Module
ctrl
ctrl
ctrl
MsgDebugGm
gm
MsgFile
MsgGetFile
MsgImage
MsgRunKick
MsgTeleoperation
ctrl y pam
pam
pam
ctrl
cmd
MsgTeleoperationEnable
MsgVideoConfig
cmd
pam
MsgGetImage
MsgGrid
pam
gm
Content
Intention to catch de ball.
Odometric and perceptive estimations.
Debugging state application for the behaviours module.
Debugging state application for the localization module.
File.
File application.
Formatted image from the camera.
Name of the behaviour to execute.
Movement orders for the head or the
body of the robot.
Teleoperation enabling or disabling.
Application for video (formatted images sequence).
Application for a formatted image.
Location algorithm details.
Table 1.1: Types of messages.
type object. Thanks to the knowledge of the message size, it can be figured out if there
are any remaining fragments of the message left.
Size (Bytes)
4
4
4
4
4
4
4
Variable
Field
MSGID
DESTINYMODULE
SOURCEID
DESTINYID
TEAMCOLOUR
COMPRESSED
LENGTHPAYLOAD
PAYLOAD
Description.
Message type ID.
Message subtype
Origin robot ID
Destiny robot ID
Origin and destiny robots team ID.
Compression data flag
Payload size
Payload
Table 1.2: Messages format.
If, after the reception of a datagram, the Message class that is created indicates that
there are some bytes left, the next datagram is assigned again to that class and the cycle
will be repeated. Once the complete message is received, the commsManager class will
forward the message to the class which is registered as receiver of this type of messages.
Therefore, if can happen that a data flow can contain both the end and the beginning
of two different messages. This situation is detected by the Message class during the
assembling fragments process. In this case, a new Message class is created to manage the
extra bits.
12
Architecture
Figure 1.9: TCP messages reception
13
Architecture
The message sending process from the robot is not as complex as the receiving one.
A Message derived class is chosen according to the type of information that is going to
be sent. Then, the constructor of that class takes care of setting all the message fields
by using the information provided as parameters. Lately, the sendMessage method is
employed in order to send this message to the network.
14
Locomotion
Chapter 2
Locomotion
2.1
Introduction
In this chapter we will describe the locomotion system, which is the trajectories generation
process that makes the robot move.
The biped walk style is a research area in which some development have been made in
the lats three decades. The principles of this investigation were established by Vibratoc
in 1969, who analyzed the walking style and set the criterion to make a walking stable.
The problem with making a robot walking like a human is the fact that a technology
approximation must be done from functions and characteristics typical of human beings.
This is a rather complex task and simplifications must be used. The locomotion system
is based in a position control system and integrated in a thinking-Cap architecture [?].
First the perception module analizes the data from the sensors of the robot, secondly this
data are integrated in a global map scenario, and finally this scenario is analyzed by the
behavior module that will generate the target position, that is, the place were the robot
must go.
The goal of the locomotion system that we are going to describe is to take the target
position for the robot calculated by the behaviour module and generate the position for
the joints that will move the robot to that position.
In order to avoid the falling of the robot, it is employed the dynamically stable criterium. This criterium stablishes that the ZMP must fall within the support polygon of
the robot. The position of the ZMP is calculated by computing both inertial and gravity
forces.
2.1.1
Information received by the locomotion module
The module that generates the information that the locomotion module is going to employ
is the behaviour module. This modules specifies the target position for the robot, which
contains the following information: the relative position of the robot in the horizontal
15
Locomotion
plane, the orientation of the robot and the layout of its feet. This information can be
modified at any time. In the meantime, the target position and orientation are updated
with the odometry information.
The position in the plane is specified in cartessians coordinates and the orientation
with an angle.
The feet layout specifies the position of the feet relative to the destiny point of the
robot. For example, in order to kick the ball with the right leg it is necessary to put the
left leg next to the ball on the side and the right leg behind it. Every kick type will have
its own feet layout.
2.1.2
Information provided by the locomotion module
While the information received from the behavior system can be updated at any time,
the exit of the locomotion module must be ready at a fixed frequency. This exit is an
array with the values that we want to set for the 21 joints of the robot. In our system,
we are delivering the values of the joints every 20 ms to the low level commander.
2.1.3
Class diagram
Several classes are involved in the locomotion process, they are Navigator, Locomotion, ZMP, COM, KinematicTransform, MovementSequences, Inertial Unit, SwingingFoot, FootTargetFitting.
There are two main classes which rise above the rest. They are Navigator and Locomotion.
Navigator is the one that takes the orders from the control module and performs the
following actions:
1. Calculates as well which one is going to be the next position of the hip LEFT,
CENTER, RIGHT
2. Generates the next position of a foot in order to reach the final target position
3. Detects when has the robot reached a position where kicking the ball is possible.
Locomotion performs several tasks:
1. Checks if the robot has fall and orders the getUpAction.
2. Coordinates the moving hip and the moving foot actions.
3. Orders the kicking action when informed that the ball is in the scope.
Both Locomotion and Navigator make use of the other classes to make its actions.
Next a brief information about the other classes:
16
Locomotion
Standard Layout
Frontal right leg kick
Lateral right leg kick

Figure 2.1: Different feet layout are used to specify the target position of the feet relative
to the destiny point. We will usually identify the destiny point with the ball.
17
Locomotion
CTRL
MODULE
Navigator
FootTarget
Fitting
Locomotion
COM
Swinging
Foot
ZMP
InertialUnit
Movement
Sequences
Figure 2.2: Relations between classes

1. InertialUnit: It processes the accelerometer data and detects forward and backward
falls.
2. Movement sequences: prepares sequences of movement to be played in the next
locomotion cycles. This sequences can be either read from a file (get up) or generated
on-line (kick).
3. COM: calculates the position of the center of mass or calculates the position of the
support leg in order to place the COM.
4. ZMP: generates trajectories of the COM to move it from one support leg to the
other.
5. SwinginLeg: generates the trajectory of a foot, from an origin point to a destiny
one.
`
6. FootTargetFitting: translates a target position for a foot into an Ofeasible,
as near
as possible oneO.
7. KinematicTransformation: translates Cartesian coordinates of the ankles or the hip
into positions of the robot joints.
2.2
Description of the robot Nao
NAO is light, compact, humanoid robot, fully programmable and easy to operate. Its
biped architecture is based in 21 degrees of freedom, which enables it to develop a great
mobility. It is 57cm tall and weights 4.5Kg.
Concerning the computational architecture, the robot is equipped with a x86 AMID
Geode 500MHz processor, 256 MB of SD-RAM and 1GB of flash memory. There are two
communications interfaces in the robot: Ethernet and wife 802.11g. The operative system
is Open, an open Linux based distribution.
18
Locomotion
The mechanic disposition of the links in the robot is shown in the figure 2.3. Putting
together all the links, you can find the joints, all of them of a rotation type. In table 2.1,
you can see the names and rotation limits of that joints.
Figure 2.3: Dimensions of the robot.

Another interesting data of the robot is the mass of the links, which will be useful in
order to obtain the center of mass of the robot. The masses of the parts of the robot can
be consulted in Table 2.2. In the same way, the length of the links can be consulted in
2.3.
Concerning the sensory equipment, you can find in Nao:
Four Force sensors in each of the feet, which can be used to get the COP.
One inertial unit in the torso of the robot. It contains a processor, two gyrometers
and three accelerometers.
19
Locomotion
Name of kinematic chain

Head
Left arm
Left leg
Right leg
Right arm
Name of joint
HeadYaw
HeadPitch
Childish
Shouldered
Elbows
Elbowroom
LHipYawPitch*
Prole
Lippi
Keypunch
Lankly
Bankroll
Repaying*
Ripely
Pitch
Keypunch
Rankle
Bankroll
Childish
Shouldered
Elbows
Elbowroom
Movement
Head joint twist
Head joint front back
Left shoulder joint front back
Left shoulder joint right left
Left shoulder joint twist
Left elbow joint
Left hip joint twist
Left hip joint right left
Left hip joint front and back
Left knee joint
Left ankle joint front back
Left ankle joint right left
Right hip joint twist
Right hip joint right left
Right hip joint front and back
Right knee joint
Right ankle joint front back
Right ankle right left
Right shoulder joint front back
Right shoulder joint right left
Right shoulder joint twist
Right elbow joint
Table 2.1: Name and range of joints.
Part of the body

Chest
Head
Upper Arm
Lower Arm
Thigh
Tibia
Foot
Total
Mass [g]
1217,1
401
163
87
533
423
158
4346,1
Table 2.2: Mass of the parts of the robot.
20
Range[]
-120 to 120
-45 to 45
-120 to 120
0 to 95
-120 to 120
-90 to 0
-90 to 0
-25 to 45
-100 to 25
0 to 130
-75 to 45
-45 to 25
-90 to 0
-45 to 25
-100 to 25
0 to 130
-75 to 45
-25 to 45
-120 to 120
-95 to 0
-120 to 120
0 to 90
Locomotion
Link name
Songfests
Shouldest
UpperArmLength
LowerArmLength
Vladivostoks
Gravesides
Length [mm]
126.5
98
90
135
100
71.7
Link name
Hipsters
Hippest
Thigh Length
Tibia Length
Foot Height
Comrades
Length [mm]
85
50
100
100
46
49.1
Table 2.3: Dimensions of the links of the robot.
Two ultrasound sensors, which can be used to estimate the distance yo obstacles in
the robots environment. The detection ranges from 0 to 70cm, with a dead band
under 15cm.
A 30 frames per second video camera placed on the head of the robot. The resolution
of the camera is 640x480.
Apart from this sensors, the robot has as well, several LEDs from different colours in
the eyes, ears, torso and feet. Moreover, you can find two microphones and two speakers
at the sides of the head.
2.3
Navigation
With the information obtained from the behavior module, you can find the final destiny
position of the feet. However, some steps are usually necessaries to reach that position.
The task of the navigation stage is to find the position of a foot in the next step.
First, all the information of the behavior module must be combined, that is, the
relative target position and orientation of the robot and the feet layout must be taken
into account in order to find the goal position of the feet. Once this is accomplished, the
position and orientation of the foot which is going to perform the next step is calculated.
2.3.1
Foot orientation
The first parameter to calculate is the turning angle of the foot. Since we know the target
relative position of the foot, we know its target relative angle, and we try to set it in the
next step, so that the foot will be aligned in the next step and the robot movement is as
straight as possible in the future.
However, it is desirable to have smooth turnings of the robot, and that is the reason
to introduce a maximum turning angle, which limits the value of this turn.
21
Locomotion
2.3.2
Foot position
At this point, the target position of the foot and its orientation is available. Then, a
feasible step is calculated to make the moving foot reach a position as near to the target
one as possible but respecting the kinematics constraints of the robot.
In order to get a feasible foot destiny, three main restriction have been considered:
1. The feet cannot collide, a minimum separation between both feet must be kept.
2. There must exist a valid configuration of the joints of the robot that makes the foot
reach that destiny.
3. Given a destiny position for a foot, a worst case movement of the hip to set the CoP
on that foot is simulated, and the resulting joints configuration must be valid.
The CPU usage to calculate this restrictions and find a proper foot position is quite
heavy, therefore it was decided to store this calculus in a Look-up table.
LUT generation
The LUT generated has the following entries:
1. Feet angle, that is, orientation of the floating foot relative to the support foot.
2. Angle of the destiny point relative to the support foot.
For every entry of such a table the following data are stored:
1. A boolean that indicates if there is any valid destiny point in the direction of that
angle.
2. The minimum and maximum valid distances from the support foot of the destiny
point.
With the previous information a similar table is generated without the entry of the
angle of the destiny point relative to the support foot. This table is employed to store
the minimum and maximum angles of the destiny point.
Fitting process
In the following lines, it will be described the process to transform a final foot position
into a feasible intermediate step one.
The fitting algorithm is illustrated in figure 2.3.2.
22
Locomotion
Figure 2.4: Fitting algorithm
23
Locomotion
Figure 2.5: Fitting schema
In our model, the feet are represented by the projection of the ankle on the floor
plane. Then, the collision boundaries are calculated, that is, the lines that define the area
in which the floating foot would collide with the support foot. If the candidate point is
within the collision area, it is moved to the nearest point out of the collision area. After
this correction, the polar coordinates of the resulting point respect to the support foot
are calculated.
The LUT is consulted to obtain a maximum and minimum angle of this point. If
the angle exceeds a limit, it is set to that limit. Once the angle is within the limits, the
maximum and minimum rho are consulted in the LUT, and the candidate point is moved
once more to adjust to that limits. The resulting point represents a feasible foot position,
with the requiered orientation, and as near to the target position as possible.
2.4
Kinematics
Once we know where are we going to set the foot in the next step, it is necessary to find
a proper trajectory for the foot in order to reach that point. However, we cannot just
move the foot, a complementary hip movement must be added in order to free that foot
of the robot support.
24
Locomotion
2.4.1
State-machine of kinematics
The walking process is just one part of the locomotion of the robot. Other features of the
locomotion include kicking the ball or getting up from the floor after falling down. The
state maching displayed next shows how these actions are linked.
Sequence finished
STILL
Target
position
reached
Go to
target
position
Kick
PLAY
SEQUENCE
DOUBLE
SUPPORT
Robot fallen
SINGLE
SUPPORT
Figure 2.6: Locomotion state machine
At the begining, the robot starts in the still state. When a target position is received,
next state is double support one. In this state, the robot moves its hip to prepares the
COM to enter in the single support one. When that moment arrives, the foot which does
not receive the support of the robot is free to move to its target position. The time spent
in the single support state must be enough to let swinging foot reach its destiny. Then
the double support state moves the COM of the robot in order to prepare it for the next
single support state.
Every time that the a single support stage reaches its end, it is calculated the trajectory
of the foot that has been supporting the robot since then. When the trajectory of the
foot is calculated, it is available the time that the foot will take to accomplish it. With
this information we can generate the trajectory of the hip that will move the support of
the robot to the other foot and stay there for the necessary time.
When the robot has reached its destiny position it stops. If the goal of this position
is to kick the ball, the active state becomes the reproduce movement one. In this state,
the joints of the robot take the values of a pre-recorded sequence. There are different
sequences for every kick type.
The othe type of sequences are the getting up ones. The difference with the previous
ones are that they are not necessarily launched when the robot is still, but at any time
the robot falls, regardless the state.
25
Locomotion
2.4.2
Feet motion
The feet trajectories should have two main properties: fastness and smoothness. The
algorithm employed to generate the trajectories has been designed to be very flexible. In
this way, the trajectory can be calibrated depending on the floor conditions so that it can
be as fast and smooth as possible.
These are the parameters employed to configure the feet trajectory:
1. STEP-AMPLITUDE: maximum height of the step.
2. SAFETY-FLOOR-DISTANCE: minimum vertical distance to the floor before starting horizontal and turning movments.
3. STEP-TIME-EXPANSION: if a trajectory takes a determined time to move the
foot, a percentage of that time can be added to the trajectory and employed to keep
the foot on the floor at the begining and at the end of the movement.
4. Maximum and minimum accelerations and decelerations for the rising, landing,
turning and horizontal movement of the foot.
The first calculus to be done are the rising and landing trajectories, they take the
foot from the floor to the maximum height and back to the floor. With this information,
we can get the minimum time spent from the point that the rising trajectory exceeds
the safety floor distance to the point when the landing trajectory goes beyond it. Then,
the horizontal and turning trajectory times are calculated. The maximum of these three
amounts of time is the one that will be employed both for the horizontal and the turning
movements.
New horizontal and landing accelerations and decelerations are calculated to employ
the definied amount of time.
2.4.3
Hip motion
The goal of moving the hip is to make one of the foot support the robot for enough time
so that it can perform an step with the other foot.
In order to design a trajectory for the hip that moves the support of the robot to one
of the feet, we will calculate the trajectory of the COM that places the ZMP in that foot.
Then, we will calculate the positions of the joints of the support leg that set the COM in
the required positions.
Trajectory of the COM
First we will design the single support stage that places the ZMP on the desired foot
for exactly the time that takes the other foot to complete its trajectory. Once the single
26
Locomotion
Foot trajectory in sagital plane

25
20
x
15
10
5
0
50
40
30
20
10
0
z
10
20
30
40
50
Figure 2.7: In a straight frontal walking, this is the aspect of the trajectory of the foot.
Every point is separated 20ms from the next one. It should be noticed that when the foot
is rising or landing, the points are closer, which indicates slower motion.
27
Locomotion
Vertical foot trajectory

25
20
15
10
5
0
0.1
0.2
time
0.3
0.4
0.3
0.4
Frontal foot trajectory

60
40
20
0
20
40
60
0.1
0.2
time
Figure 2.8: Vertical and horizontal trajectories.
28
Locomotion
support trajectory is calculated, we will proceed with the double support one, whose
goal is to place the COM in the initial position and speed required by the single support
trajectory.
Lets suppose that we are going to move the right foot. That means that the single
support stage of the right foot has just finished and that we are entering in the double
support stage right now. We will first calculate the single support trajectory that will
place the ZMP on the left foot for the time necessary to move the right foot (tr).
As we said before, in order to calculate the position of the ZMP we are going to take
into account both the inertial and the gravity forces.
We will employ the left ankle projection on the floor as the origin of our reference
system. The place where we want to set the ZMP is not necessarily in the middle of the
foot, so we will define a parameter called zof f to set the point where the ZMP will lay.
Given an height of the ZMP (H) and a distance from the current position of the COM
to the origin (d), we are interested in finding the acceleration that we have to apply to
the COM to place the ZMP at the point zof f .
If we did not take into consideration the inertial forces, the gravity force would place
the ZMP right below the current position of the COM. This point is called COG (Center
of Gravity). However, the existance of inertial forces modify the position of the ZMP. We
are interested in finding the inertial force that added to the gravity force place the ZMP
in zof f .
As we know, the inertial force experimented by the COM is I = ma, and the gravity
force is G = mg. If we want to place the sum of the forces over the zof f point, the
following relation must be kept:
a
(d zof f )
I
= =
G
g
H
(2.1)
By developing this equation and combining some design parameters we can obtain the
COM trajectory that will move the COP from one foot to the other.
Moving joints to set the COM
In the previous section we saw how to get the sagital and the frontal expressions for the
movement of the COM. Since we will fix the height of it, we have the three cartessian
coordinates that we need to place it in the space.
On the other hand, in section ?? we described the way to get the three cartessians
coordinates that give us the expression of one ankle refered to the other one.
In order to place the COM we need some more information about the configuration
of the joints, we will employ the following simplifications:
The torso of the robot will be always straight
29
Locomotion
footHeight
15
10
0.2
0.4
0.6
0.8
1
time
1.2
1.4
1.6
1.8
0.2
0.4
0.6
0.8
1
time
1.2
1.4
1.6
1.8
Sagital plane
150
100
50
Figure 2.9: Forward walking. Yellow for the left leg and red for the right one. In blue the
forward evolution of the COM, in green the ZMP. During single support stages, the ZMP
lies on one of the feet, while in the double support ones, it moves between the two feet.
30
Locomotion
footHeight
15
10
0.2
0.4
0.6
0.8
1
time
1.2
1.4
1.6
1.8
0.2
0.4
0.6
0.8
1
time
1.2
1.4
1.6
1.8
Frontal plane
100
80
60
40
20
0
Figure 2.10: Forward walking. Yellow for the left leg and red for the right one. In blue
the lateral evolution of the COM, in green the ZMP.
31
Locomotion
250
Sagital plane
200
150
100
50
10
20
30
40
50
60
Frontal plane
70
80
90
100
Figure 2.11: Forward walking. Trajectory of the COM in the horizontal plane. The height
of the COM is fixed.
32
Locomotion
footHeight
15
10
0.5
1.5
2
time
2.5
3.5
0.5
1.5
2
time
2.5
3.5
Frontal plane
300
200
100
Figure 2.12: Lateral walking towards right side. Yellow for the left leg and red for the
right one.In blue the lateral evolution of the COM, in green the ZMP.
33
Locomotion
Transversal plane
150
100
50
50
100
150
250
200
150
100
50
50
100
Figure 2.13: Arc walking. Trajectory of the COM in the horizontal plane.
34
Locomotion
We will not move the arms of the robot

We will consider the head as still and straight
We will neglect the efect of knees and elbows in the distribution of arms and legs:
they will be considered straights.
With the previous assumptions, we can model the upper part of the body of the robot
as a block. Our goal will be to get the position of the hip given the position of the COM
and a foot.
A first step will be to perform the inverse operation. That is, to find the position of
the COM given the position of the joints.
First, we will define the blocks in that we are going to group the mass segments of the
robot:
1. massLeg = MASS-TIBIA + MASS-THIGH;
2. massFoot = MASS-FOOT;
3. massChest= MASS-CHEST + MASS-HEAD + MASS-UPPER-ARM + MASSLOWER-ARM;
4. massTotal = massLeg *2 + massChest + 2*massFoot;
Now, we will suppose that the left leg of the robot is on the floor and supporting the
robot. In the next lines s will state for support leg, f floating leg, h is the hip end of a leg
while a is the ankle end. l means left and r right. We can now calculate the position of
the mass blocks defined previously with the data provided by the cartessian coordiantes
of the positions of the support leg and the relative position of the floating ankle related
to the support one.
sLegX = xhal/2;
sLegY = yhal/2;
sLegZ = zhal/2;
f LegX = (xaf s + xhal)/2;
f LegY = (yaf s + cos(hipRot/2) LH + yhal)/2;
f LegZ = (zaf s sin(hipRot/2) LH + zhal)/2;
f F ootX = xaf s;
f F ootY = yaf s;
f F ootZ = zaf s;
chestX = xhal + lengthChest/2;
chestY = yhal + cos(hipRot/2) LH/2;
chestZ = zhal sin(hipRot/2) LH/2;
35
Locomotion
We can combine the position of the mass blocks with the mass of them to find the
position of the COM:
xCOM =
sLegX M L + f LegX M L + f F ootX M F + chestX M C

M T + LF
(2.2)
yCOM =
sLegY M L + f LegY M L + f F ootY M F + chestY M C

MT
(2.3)
zCOM =
sLegZ M L + f LegZ M L + f F ootZ M F + chestZ M C

MT
(2.4)
The last step is to solve the previous equation system to find the value of sLegX,
sLegY and sLegZ given the position of xCOM , yCOM and zCOM .
xhas =
xCOM M T
xaf sM L
2
xaf s M F LC
MC
2
LF M T
(2.5)
ML + MC
yhas =
yCOM M T (yaf s + cos( hipRot

) LH)
2
zhas =
zCOM M T (zaf s sin( hipRot

) LH)
2
ML
2
yaf s M F cos( hipRot

)
2
LH
2
MC
LH
2
MC
ML + MC
ML
2
zaf s M F + sin( hipRot

)
2
ML + MC
(2.6)
(2.7)
Where ML, MF, MC and MT are respectively the masses of a leg, a foot, the chest
and the whole body. In the other hand, LC is the vertical length of the chest, LH the
horizontal length of the hip, and LF the height of the foot.
2.5
Direct kinematic problem
Direct kinematic problem has to be solved in order to find the position and orientation
of a final effector based on the position of the angles of the joints and the geometric
parameters of them. Homogeneous transformations are used in order to find the position
and orientation of a final effector referenced to a base coordinates system. Next, the
equations of the direct kinematic problem.
x = fx (q1 , q2 , q3 , . . . , qn )
y = fy (q1 , q2 , q3 , . . . , qn )
z = fz (q1 , q2 , q3 , . . . , qn )
= f (q1 , q2 , q3 , . . . , qn )
= f (q1 , q2 , q3 , . . . , qn )
= f (q1 , q2 , q3 , . . . , qn )
(2.8)
(2.9)
(2.10)
(2.11)
(2.12)
(2.13)
The Davit Gutenberg convention has been used to solve the problem. This convention
relates the homogeneous transformation both of rotation and movement in a kinematic
36
Locomotion
chain of i links. In order to find a basic coordinates system we will use one of the legs
of the robot. When the robot moves one leg we use the support leg as a reference and
the other way round. These are the kinematic chains of the robot: left leg, right leg, left
arm, right arm and head. The establishment of the coordinate systems of all this chains
can be appreciated in figure 2.14.
In table 2.4 has been realized the transformation from the basis coordinate system to
any other by using the transformation matrix shown in (2.14).
cos q cos sin q sin sin q a cos q

sin q cos cos q1 sin cos q a sin q
i1
Ai =
0
sin
cos
d
0
0
0
1
Link
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
Joint
Left leg chain

LeftAnkleRoll
q1ll
LeftAnklePitch
q2ll
Lifetaking
q3ll
Liverpool
q4ll
Foppish
q5ll+90
LeftHipYawPitch
q6ll
Right leg chain
RightHipYawPitch
q6rel
Hotchpotch
q5rel+90
Thrill
q4rel
Retrench
q3rel
Radionuclide
q2rel
Triangularly
q1rel
Left arm chain
LeftShoulderPitch
q1la
LeftShoulderRoll
q2la
Loftily
q3la
Lavatorial
q4la
Right arm chain
RightShoulderPitch
q1Ra
Mitochondrial
q2Ra
Rightly
q3Ra
Rattlebrain
q4Ra
Head chain
HeadYaw
q1h
HeadPitch
q2h
0
0
0
0
0
0
0
l2
l1
0
0
0
90
0
-90
-45
-45
0
0
0
0
0
0
0
0
0
l1
l2
0
l3
-225
90
-90
0
90
0
0
0
l4
0
0
0
0
l5
90
90
-90
0
0
0
l4
0
0
0
0
l5
90
90
-90
0
0
0
0
0
-90
0
(2.14)
Table 2.4: DH Parameters of the robot.

The values of q, d, a and are the parameters of the Davit-Gutenberg convention.
37
Locomotion
Figure 2.14: Placement of the coordinate systems in the robot.
38
Locomotion
The transformation from the reference to any other kinematic chain requires the transformation for every chain in order to align every reference system to the main one. These
transformations are listed in table 2.5 for the left leg. Similar procedure can be performed
for the other kinematic chains.
The definition of the direct kinematic can be used as well to perform the calculation
of the position of the COM.
Coordinate system adaptation transformations
From
To
Transformation
Reference - Left leg start
Left leg end
Rot(x)
Rot(z)
Rot(x)
Left leg end
Right leg start
Move(y)
Rot(y)
Rot(x)
Left leg end
Left arm start
Move(x)
Move(y)
Rot(x)
Rot(z)
Left leg end
Right arm start
Move(x)
Move(y)
Rot(x)
Rot(z)
Left leg end
Head start
Move(x)
Move(y)
Rot(y)
Rot(z)
Left leg start
Head end (Camera)
Rot(z)
Rot(x)
Move(z)
Move(x)
Amount
45
-90
-90
10cm
-90
-45
18.5cm
14.8cm
90
90
18,5cm
-4.8cm
90
90
20.65cm
5cm
90
180
-90
-90
4.91cm
7.17cm
Table 2.5: Chains transformations.
2.6
Inverse kinematics
Inverse kinematics is the process of determining the parameters of a jointed flexible object
(a kinematic chain) in order to achieve a desired pose. In this section, it will be explained
how to solve the inverse kinematics problem for the legs and the hip of a robot Nao. In
our case, the pose is specified by the relative position of the hip to the ankles and by the
angle made by the feet.
In order to solve this problem more comfortably, it will be divided in three stages.
First, it is suposed that the leg is a telescopic extension of the hip, that is, the leg is
39
Locomotion
Figure 2.15: Names and senses of the joints

straight between the hip and the foot. With this model, we will find the hipPitch, hipRoll,
anklePitch and ankleRoll contributions to the position of the foot. In the second stage,
we will find out the value of the joints hipPitch, kneePitch and anklePitch necesaries to
make the leg as log as it is needed in the previous stage. Finally, in the third stage, the
hipYawPitch joints are set and the hipPitch and hipRoll values are calculated in order to
keep the torso straight. The final position of the joints is obtained by the combination of
the three stages.
2.6.1
Telescopic leg
We will call legPitch to the angle between the leg and the hip in the sagital plane, and
legRoll to the angle in the frontal plane.
xha
)
xha
yha
legRoll = arctan(
)
xha
legP itch = arctan(
2.6.2
(2.15)
(2.16)
Leg length
The telescopic leg of the previous model and the real leg segments that we are going to
employ to configure it are disposed in a triangular shape, as it is illustrated in figure 2.6.2.
We will employ the Law of cosines in order to get the three angles of the triangle.
40
Locomotion
Figure 2.16: Relative position between the left ankle and the hip.
41
Locomotion
HIP
hipAngle
Thigh
haDist
KNEE
kneePitch
Tibia
tibiaAngle
ANKLE
Figure 2.17: Joints configuration of a leg in order to get the leg length required.
haDist = xha2 + yha2 + zha2

T ibia2 + haDist2 T high2
tibiaAngle = arccos(
)
2 haDist T ibia
T high2 + haDist2 T ibia2
hipAngle = arccos(
)
2 haDist T high
2.6.3
(2.17)
(2.18)
(2.19)
Foot orientation
In the last stage to solve the inverse kinematics problem, the target is to get an angle
between the feet. In this stage, the relative position between feet and hip is not taken
into account, only the angle between the feet, which we will call feetAngle.
In order to make an angle between the feet, the position of the joints hipYawPitch,
hipPitch and hipRoll must be changed. First we will set the hipYawPitch joint to a some
angle. Once changed the value of this joint, we will set the torso in a vertical position
again with the help of hipRoll and hipPitch joints. When this process will be completed,
42
Locomotion
Figure 2.18: Position of the left hip joints. Two different reference systems will be employed: R1 and R2.
we will evaluate the actual angle between the feet. In this way, we will generate a mapping
between hipYawPitch and the feet angle.
We will complete a table by repeating this process for all the range of hipYawPitch,
and writting the matching values for hipRoll, hipPitch and feetAngle.
However, we are interested in finding the values of hipYawPitch, hipRoll and hipPitch
known feetAngle. For this reason we will create a new table having as entry the range
of values of feetAngle, and as exits the matching values of hipYawPitch, hipRoll and
hipPitch. For every value of feetAngle, we will find the two nearest values in the previous
table and interpolate the angles of the joints between this two values.
In the next lines, we will detail the process to find the values of the joints hipPitch and
hipRoll for the left leg. First, hipYawPitch is set to the desired value, then the resulting
position of the torso is analyzed. The torso will have some orientation in the sagital and
frontal plane. The orientation in the frontal angle is corrected with a value of LHipRoll,
and the new angle in the sagital plane is obtained. Finally, the angle in the sagital plane is
corrected with a LHipPitch movement. The orientation of the torso is initially represented
as an unitary vector in the direction of x1 . The process can be divided in the following
steps:
1. Calculate the coordinates of (1, 0, 0)1 in R2.

1
0
a1 =
0
43
(2.20)
Locomotion
R12
cos 45 sin 45 0
= sin 45 cos 45 0
0
0
1
(2.21)
In order to simplify:
k = sin 45 = cos 45
(2.22)
k k 0
1
k
a2 = R12 a1 = k k 0 0 = k
0 0 1
0
0
(2.23)
2. Rotate a2 around x2 an angle .
1
0
0
k
b2 = 0 cos sin a2 = k cos
0 sin cos
k sin
(2.24)
3. Calculate the coordinates of b2 in R1.
k 2 + k 2 cos
b1 = R21 b2 = k 2 k 2 cos
k sin
(2.25)
4. Rotate b1 around the z1 axis an angle .
cos sin 0
k + k 2 cos
c1 = sin cos 0 k 2 k 2 cos =
0
0
1
k sin
2
k (cos (1 + cos ) sin (1 cos ))

k 2 (sin (1 + cos ) + cos (1 cos ))
k sin
(2.26)
5. Find the value of that anulates the y component of c1.

k 2 (sin (1 + cos ) + cos (1 cos ) = 0
= arctan(
6. Rotate c1 around
cos
0
d1 =
sin
cos 1
)
cos + 1
(2.27)
(2.28)
the y2 axis an angle .

2
0 sin
k (cos (1 + cos ) sin (1 cos )
1
0 k 2 (sin (1 + cos ) + cos (1 cos ) = (2.29)
0 cos
k sin
...
...
2
sin (k (cos (1 + cos ) sin (1 cos ))) k cos sin
44
Locomotion
7. Find the value of that anulates the z component of d1.

sin (k 2 (cos (1 + cos ) sin (1 cos ))) = 0
(2.30)
sin
)
k(cos (1 + cos ) sin (1 cos ))
(2.31)
= arctan(
Now, we have the corresponding values of LHipRoll and LHipRot for a value of
LHipYawPitch. The last step is to calculate the angle between the feet that is generated for that values. In order to get that value, we will repeat the previous process but
this time with the unitary vector around the z axis instead around the x axis.
1. Calculate the coordinates of a1 = (0, 0, 1)1
k k
k k
a2 = R12 a1 =
0 0
in R2.

0
0
0
0
0
0
=
1
1
1
(2.32)
2. Rotate a2 around x2 an angle .
1
0
0
0
b2 = 0 cos sin a2 = sin
0 sin cos
cos
(2.33)
3. Calculate the coordinates of b2 in R1.
k sin
b1 = R21 b2 = k sin
cos
(2.34)
4. Rotate b1 around the z1 axis an angle .
cos sin 0
k sin
c1 = sin cos 0 k sin =
0
0
1
cos
k sin (cos + sin )

k sin (sin cos )
cos
5. Rotate c1 around the y2 axis an angle .
cos 0 sin
k sin (cos + sin )
0
1
0 k sin (sin cos ) =
d1 =
sin 0 cos
cos
...
k sin (sin cos )

k sin sin (cos + sin )) + cos cos
45
(2.35)
(2.36)
Locomotion
For the right leg the process is similar.

We have now enough information to get the corresponding value of feetAngle.
f eetAngle = 2atan(
k sin (sin cos )

)
k sin sin (cos + sin )) + cos cos
(2.37)
As we said before, the values of , and will be stored in a table so that it can be
searched the corresponding values to a given feetAngle.
2.6.4
Combining previous results
In order to get the joint angles of the robot, the three previous models are combined in
the following way:
LHipYawPitch = -
LHipRoll = lLegRoll -
LHipPitch = lLegPitch -lThighAngle +
LKneePitch = lTibiaAngle + lThighAngle
LAnklePitch = -(lLegPitch + lTibiaAngle)
LAnkleRoll = -legRoll
Where , and are specific for every feetAngle.
2.7
Future works
The locomotion module developed shows an efficient way to generate parametric trajectories for a real-time, omnidirectional walking.
The parameters of the trajectories could be tuned in real time to optimize performance. Some artificial intelligence approach could be used to get optimum values for the
parameters.However, it is a main target to rise these speeds, and the dynamic behavior
should be revised.
46
Perception
Chapter 3
Perception
3.1
PAM: Perceptual Anchoring Module
The locus of perception is the PAM, which acts as a short term memory of the location
of the objects around the robot. At every moment, the PAM contains the best available
estimates of the positions of these objects. Estimates are updated by a combination of
three mechanisms: by perceptual anchoring, whenever the object is detected by vision;
by odometry, whenever the robot moves; and by global information, whenever the robot
re-localizes. Global information can incorporate information received from other robots
(e.g. the ball position).
The PAM also takes care of selective gaze control, by moving the camera according
to the current perceptual needs, which are communicated by the HBM in the form of a
degree of importance attached to each object in the environment. The PAM uses these
degrees to guarantee that all currently needed objects are perceptually anchored as often
as possible (see [34] for details).
Object recognition in the PAM relies on three techniques: color segmentation based
on a fast region-growing algorithm; model-based region fusion to combine color blobs into
features; and knowledge-based filters to eliminate false positives. For instance, a yellow
blob over a pink one are fused into a landmark; however, this landmark may be rejected
if it is, for example, too high or too low relative to the field.
3.1.1
Standard vision pipeline
Significant improvements have been made to the PAM over the last two years. Previously,
seeds for the growing algorithm were obtained by hardware color segmentation on YUV
images, taken directly from the camera. Seeds are now chosen by performing software
thresholding on the images, which are first converted to HSV. This allows for a very
portable and robust color segmentation, which works even in the face of changing lighting
conditions.
47
Perception
Figure 3.1: Standard game-playing vision pipeline
The vision pipeline works as follows (see Fig. 3.1). When a new YUV image is captured, it is first converted to HSV. Then it is segmented using software thresholding
to obtain a labelled image whose pixels are used as seeds for the seed region growing
method. Then, for each region type all the blobs are processed and merged when appropriate conditions apply (overlapped blobs, adjacent blobs, etc). The resulting blobs are
then processed to verify constraints (size, number of pixels, aspect ratio, etc) in order to
identify which type of objects they represent. Finally, those blobs that are classified as
given objects are further processed to estimate the distance from the robot to the object.
All the information is then aggregated into a robot centered data structure called Local
Perceptual Space (LPS), which represents a local state of the robot. This data structure
is used by the HBM to decide which action the robot should perform.
3.1.2
Experimental vision pipeline
In addition to the normal objects in the RoboCup domain, we also detect natural features,
such as field lines and corners. Currently, we use corners comprised of the (white) field
lines on the (green) carpet. Corners provide useful information, since they can be classified
by their type, and they are relatively easy to track (given the small field of view of the
camera). Natural feature detection is performed using three techniques: corner detection
based on changes in the direction of the brightness gradient; color-based filtering for
filtering out corners that arent on the carpet; and corner grouping for associating the
detected corners with the natural features for which we are looking.
While the corner based processing is still experimental (it is still a bit slow to be used
for actual game playing), it is fully operational. It works as follows (see Fig. 3.2). In
addition to the standard image processing, the Y channel of the captured YUV image is
used to detect grey-level image corners. As we are only interested in field lines, the greylevel corners are filtered out based on the segmented HSV image. Only corners produced
by white blobs are considered. Then, the resulting corners are classified depending on
whether they are convex or concave. Then these corners are grouped and classified into
different feature categories, corresponding to different field lines intersection types (C,
T, etc). Finally, for each detected feature a distance to the robot is computed and the
feature is projected into the LPS.
48
Perception
Figure 3.2: Corner-based experimental vision pipeline
3.2
Color Segmentation
Color segmentation is a fundamental part of object recognition in the RoboCup domain.

The segmentation algorithm has to be robust, which means that it has to give the right
results under nominal conditions and, without returning, give acceptable results under
slightly different conditions, such as blurred images or different lighting. The main methods for color segmentation can be classified into the following approaches, sometimes used
in combination [41].
Threshold techniques rely on the assumption that pixels whose color value lies
within a certain range belong to the same class or object [38]. Many teams in
RoboCup use threshold techniques [16] [8] with slight modifications to increase its
performance, e.g., for images that blur at object boundaries. Threshold techniques
need a careful tuning of thresholds and are extremely sensitive to light conditions.
Edge-based methods rely on the assumption that pixel values change rapidly at
the edge between two regions [27]. Edge detectors such as Sobel, Canny and Susan
are typically used in conjunction with some post-procedure in order to obtain the
closed region boundaries. This tends to make these methods very computationally
expensive.
Region-based methods rely on the assumption that adjacent pixels in the same
region have similar color value; the similarity depends on the selected homogeneity
criterion, usually based on some threshold value. Seeded region growing (SRG) is
a region-based technique that takes inspiration from watershed segmentation [43]
and is controlled by choosing a (usually small) number of pixels, known as seeds [1].
Unfortunately, the automatic selection of the initial seeds is a rather difficult task
[21]. For instance, Fan and Yau [14] use color-edge extraction to obtain these seeds.
By doing so, however, they inherit the problems of edge-based methods, including
sensitivity to noise and to the blurring of edges, and high computational complexity.
In our current implementation we use a hybrid method for color segmentation that integrates the thresholding and the SRG methods. We use a threshold technique to generate
49
Perception
Figure 3.3: Problems of SRG: homogeneity criterion too strict (center) or too liberal
(right)
an initial set of seeds for each color of interest, and then use SRG to grow color regions
from these seeds. Special provisions are included in our SRG to account for the fact that
we can have many seeds inside the same region. The integration of the two methods allows
us to use a conservative tuning for both of them, thus improving robustness: robustness
with respect to changing lighting conditions is improved because we use a conservative
thresholding, and robustness with respect to blurred edges is improved because we use
a conservative homogeneity criterion in SRG. The technique, with minor variations, has
been used in the 4-legged league since 2001.
3.2.1
Seed Region Growing
In a nutshell, our color segmentation algorithm works as follows. We fix a set of colors
to be recognized. We use thresholds to produce a labelled image where only pixels that
belong to one of these colors with high certainty are marked as such. Then, we use each
labelled pixel as an initial seed for a region of that color. From each one of these seeds,
we grow a blob by including adjacent pixels until we find a significant discontinuity. The
so called homogeneity criterion embodies the notion of a significant discontinuity: in our
case, this is a change in one of the Y, Cr or Cb values beyond a given threshold.
The choice of the homogeneity criterion is critical in SRG techniques. If this criterion
is too strict, the blob growing can stop prematurely because of local variations, e.g., as
produced by shadows or noise. If it is too liberal, the blob can flood to an adjacent
region though a weak edge, e.g., as caused by blurring. These two cases are illustrated in
Fig. 3.3, where the single initial seed is indicated by a black dot (the ball outline is drawn
for clarity). The thresholds used in the homogeneity criterion are 2 and 3, respectively. In
our technique, we address this problem by using many seeds for each color region. When
growing a blob from a given seed, we use a strict homogeneity criterion, thus reducing the
risk of flooding. We then reconstruct the full region by merging all the local blobs that
belong to the same region, that is, all the adjacent blobs grown from seeds of the same
color. The following procedure constitutes the core of the segmentation algorithm. (N is
the size of the image.)
procedure FindBlobs (seeds)
reset label[0:N], color[0:N], conn table
blob id = 1
50
Perception
for each color id

for each pixel p in seeds
if (seeds[p] = color id) and (label[p] = null)
queue = p
while queue is not empty
q = pop(queue)
label[q] = blob id
color[q] = color id
for s in 4-neighbors(q)
if (label[s] = null)
if CheckHomegeneity(s, q)
push(s, queue)
else if (label[s] != blob id)
if color[s] = color id
add <p,q> to conn table
blob id = blob id + 1
return (label, color, conn table)
The FindBlob procedure operates on the following data structures: label[N], an array that
associates each pixel to a region ID, or null; color[N], an array that associates each pixel
to a color ID, or null; and conn table, a list of connected regions of the same color. The
procedure takes the labelled image, and returns a set of blobs plus a connection table. For
each desired color, the procedure scans the seeds image to identify labeled pixels (seeds)
of that color that have not yet been incorporated in a blob. For each such seed, it starts
a standard growing loop based on 4-connectivity. If during the growing we hit a pixel
that has already been labeled as belonging to a different blob, then we check its color. If
its color is the same as the one of the current blob, we mark the two blobs as connected
in the connection table. When we add entries to the connection table, we also check it
for cycles and remove them. The growing stops at the blob border, as identified by the
homogeneity criterium discussed above. After all the blobs in the original image have
been individuated, a post-processing phase goes through the connection table and merges
all the connected blobs of the same color as discussed above. In our experiments, 5 to 10
adjacent blobs were often generated for the same connected region. A larger number of
blobs (up to 30) were occasionally generated for very large regions. We have incorporated
in the above algorithm the computation of blob parameters like the number of pixels,
center, width and height. These are updated incrementally as the blobs are grown during
the main phase and fused during the post-processing phase. These parameters are needed,
e.g., to estimate the position of the objects.
3.2.2
Experiments
We have performed several test to assure the quality of the vision processing and measure
the performance of the different algorithms. These algorithms depend on lighting conditions, number of objects, distance to objects, etc. In this section we show a series of test
realized in an office environment with a couple of RoboCup objects: the ball and a fake
51
Perception
yellow net. The main idea is to compare the processing time in the real robot using the
different algorithms implemented. Thus, the ball has been placed at different distances
from the robot: (100 cm, 50 cm, and 15 cm) and very close to the camera (5 cm). The
next table shows the abbreviation of the segmentation and blob methods used.
Abrev
SRG
LUT-T
LUT-SRG
BG
BGO
Method Name
Seed Region Growing
LUT Thresholding
LUT Region Growing
Blob Growing
Blob Growing Optimised
The following table resumes the timing (in microseconds) of the different method
combinations tested so far. The conditions have been quite similar, although calibration
parameters are not exactly the same (color cubes and region growing constant). It can
be noticed that the best performance is achieved by the LUT-T and BGO combination,
being the blob extraction the bottleneck at short distances.
Distance
NEAR
MEDIUM
FAR
SRG+BG
19799
24029
46005
LUT-SRG+BGO
8492
9284
40279
LUT-T+BGO
4938
5470
14189
Figure 3.4 shows the SRG+BG combination. It includes three segmented images of
the ball at different distances. The total processing time for each image is also shown,
decomposed in the three main stages of the vision process: segmentation, blob extraction,
and object recognition. The times correspond to average values of a series of six frames.
This was the combination used during the RoboCup 2005 competition. Unfortunately,
these tests have been performed after, mainly because lack of time. This resulted in a
very poor ball control, because at short distances the CPU time needed to process a frame
was in excess of 30 ms.
Figures 3.5, 3.6 shows the LUT-T+BGO and LUT-SRG+BGO combinations. They
include four segmented images of the ball at different distances and the corresponding
blobs. The total processing time for each image is also shown, decomposed in the three
main stages of the vision process: segmentation, blob extraction, and object recognition.
The times correspond to average values of a series of six frames.
Clearly, these combinations outperform the SRG-BG combination. The main reason
is the YUV to HSV conversion. Using a very optimized online computation it takes about
13 ms, while the look-up-table methods takes about 4 ms.
52
Perception
Segment=18264
Blobs=1073
Recognise=461
Total=19799
Segment=21591
Blobs=2120
Recognise=318
Total=24029
Segment=36429
Blobs=9386
Recognise=190
Total=46005
Figure 3.4: SRG + BG
Segment=3793
Blobs=892
Recognise=253
Total=4938
Segment=3924
Blobs=1300
Recognise=247
Total=5470
Segment=4584
Blobs=6176
Recognise=361
Total=11120
Segment=4008
Blobs=9611
Recognise=570
Total=14189
Figure 3.5: LUT-T (7bits) + BGO
3.3
Corner Detection and Classification
The robot localizes relying on the extracted features, both the amount of features detected
and their quality will clearly affect the process. Currently the robots rely only on colored
landmarks for localization, and thus the perception process is based on color segmentation
for detecting the different landmarks. Soon artificial landmarks will be removed, as it was
done in other leagues. For this reason, our short term goal is to augment the current
landmark based localization with information obtained from the field lines, and our long
term goal is to rely only on the field lines for localisation [17].
Because of the League rules all the processing must be done on board and for practical
reasons it has to be performed in real time, which prevents us from using time consuming
algorithms. A typical approach for detecting straight lines in digital images is the Hough
Transform and its numerous variants. The various variants have been developed to try to
overcome the major drawbacks of the standard method, namely, its high time complexity
53
Perception
Segment=5624
Blobs=2468
Recognise=400
Total=8492
Segment=7113
Blobs=1931
Recognise=240
Total=9284
Segment=18807
Blobs=7887
Recognise=144
Total=26838
Segment=29076
Blobs=11055
Recognise=148
Total=40279
Figure 3.6: LUT-SRG (7bits) + BGO
and large memory requirements. Common to all these methods is that either they may
yield erroneous solutions or they have a high computational load.
Instead of using the field lines as references for the self-localization, we decided to use
the corners produced by the intersection of the field lines (which are white). The two
main reasons for using corners is that they can be labelled (depending on the type of
intersection) and they can be tracked more appropriately given the small field of view of
the camera. In addition, detecting corners can be more computationally efficient than
detecting lines. There are several approaches, as reported in the literature, for detecting
corners. They can be broadly divided into two groups:
Extracting edges and then searching for the corners. An edge extraction algorithm is
pipelined with a curvature calculator. The points with maximum curvature (partial
derivative over the image points) are selected as corners.
Working directly on a gray-level image. A matrix of cornerness of the points
is computed (product of gradient magnitude and the rate of change of gradient
direction), and then points with a value over a given threshold are selected as corners.
We have evaluated two algorithms that work on gray-level images and detect corners depending on the brightness of the pixels, either by minimizing regions of similar
brightness (SUSAN) [39] or by measuring the variance of the directions of the gradient
of brightness [40]. These two methods produce corners, without taking into account the
color of the regions. As we are interested in detecting field lines corners, the detected
corners are filtered depending on whether they come from a white line segment or not.
In the first method (SUSAN) [39], non-linear filtering is used to define which parts of
the image are closely related to each individual pixel; each pixel has associated with
54
Perception
it a local image region which is of similar brightness to that pixel. The detector is
based on the minimization of this local image region. The local area contains much
information about the structure of the image. From the size, centroid and second
moments of this local area corners and edges can be detected. This approach has
many differences to the well-known methods, the most obvious being that no image
derivatives are used and that no noise reduction is needed. The integrating effect
of the principle, together with its non-linear response, give strong noise rejection.
The local area is at a maximum when the nucleus lies in a flat region of the image
surface, it falls to half of this maximum very near straight edge, and falls even further
when inside a corner. It is this property which is used as the main determinant of
the presence of edge and corner features.The output of this method is shown in
Fig.3.7(a).
The second method [40] is based on measuring the variance of the directions of the
gradient of brightness. The probability of the event that a point belongs to the
approximation of a straight segment of the isoline of brightness passing through
the point being tested is computed using the technique of Bayesian estimations and
used as a weight. Along every isoline, the size of the gradient of brightness remains
constant. The two half-lines that form the isoline along which the size of the gradient
of brightness is maximal are edges. Along each half-line that is a part of an isoline,
the direction of the gradient of brightness remains constant. The directions of the
gradient are different at the points lying to the left and to the right of the corner
axis. The image points at which the size of the gradient of brightness is greater than
a predefined threshold are considered to be candidates for corners. The candidates
are then examined, the candidate at which the value of corner measure exhibits its
local maximum and at which the values of angle of break, and corner appearance
are greater than chosen thresholds is a corner. The output of this method is shown
in Fig.3.7(b).
(a)
(b)
Figure 3.7: (a) Brightness similarity based detector (SUSAN). (b) Brightness gradient
based detector
The gradient based method [40] is more parametric, produces more candidate points
and requires similar processing capabilities than SUSAN [39], and thus it is the one that
we have selected to implement corner detection in our robots.
Because of the type of camera used [42], there are many problems associated to resolution and noise. The gradient based method detects false corners in straight lines due
55
Perception
to pixelation. Also, false corners are detected over the field due to the high level of noise.
These effects are shown in Fig. 3.8 (a). To cope with the noise problem, the image is
shows the reduction in noise level, and
filtered with a smoothing algorithm. Fig. 3.8(b)
how it eliminates false detections produced by this noise (both for straight lines and the
field).
(a)
(b)
Figure 3.8: Brightness gradient-based detector. (a) From raw channel. (b) From smoothed
channel. The detected corners are indicated by an orange small square.
The detected corners are then filtered so that we keep only the relevant ones, those
produced by the white lines over the field. For applying this color filter, we first segment
the raw image. Fig. 3.9 (b) and Fig. 3.10 (b) show the results obtained using a threshold
technique for a sample image. We can observe that this technique is not robust enough for
labelling the pixels surrounding the required features. Thus, we have integrated thresholding with a region-based method, namely the Seed Region Growing (SRG), by using
the pixels classified by the a conservative thresholding as seeds from which we grow color
regions. (See [44] for details.) The resulting regions for our image are shown in Fig. 3.9 (c)
and Fig. 3.10 (c). Finally, we apply a color filter for all corners obtained with the gradientbased method. We show in Fig. 3.9 (d) and Fig. 3.10 (d) the corners obtained with the
gradient-based method (white) and the corners that comply with the color conditions
(black).
(a)
(b)
(c)
(d)
Figure 3.9: Feature detection. (a) RGB image. (b) Segmented image by threshold. (c)
Segmented image by seeded region growing. (d) Gradient-based detected corners (white)
and color-based filtered corners (black). The two detected corner-features are classified
as a C
Once we have obtained the desired corner pixels, these are labelled by looking at the
amount of field pixels (carpet-color) and line or band pixels (white) in a small window
around the corner pixel. Corners are labelled according to the following categories.
56
Perception
(a)
(b)
(c)
(d)
Figure 3.10: Feature detection. (a) RGB image. (b) Segmented image by thresholding.
(c) Segmented image by seeded region growing. (d) Gradient-based detected corners
(white) and color-based filtered corners (black). The two detected corner-features are
classified as a C and a T, respectively.
Open Corner A corner pixel surrounded by many carpet pixels, and by a number of
white pixels above a threshold. As shown in Fig. 3.11(a).
Closed Corner A corner pixel surrounded by many white pixels, and by a number of
carpet pixels above a threshold.As shown in Fig. 3.11(b).
Net Closed Corner A corner surrounded by many white pixels, and by a number of
carpet and net (blue or yellow) pixels above a threshold. As shown in Fig. 3.11(c).
(a)
(b)
(c)
Figure 3.11: Corner labelling. (a) Open corner (b) Closed Corner (c) Net Closed Corner
Note that in order to classify a corner pixel, we only need to explore the pixels in its
neighborhood. From these labelled corners, the following features are extracted.
Type C An Open corner nearby of a closed corner. This feature can be detected in
the goal keeper area of field. As shown in Fig. 3.16(c).
Type T-field Two closed corner. Produced by the intersection of the walls and the
inner field lines. As shown in Fig. 3.16(c).
Type T-net A closed corner nearby of a net closed corner. Produced by the intersection of goal field lines and the net. As shown in Fig. 3.16(c).
57
Perception
(a)
(b)
(c)
Figure 3.12: Feature types. (a) Type C (b) Type T-field(c) Type T-net
3.4
Goal recognition
Goal recognition is a method of perception processed after the creation of blobs. The
aim is to characterize the goals seen in the image. This function must also estimate the
distance, angle and orientation of the goal in relation to the robot, so as to kick the ball
into it, if the goal belongs to the other team, or to clear it otherwise.
3.4.1
Sighting the goal
It is important to take into account that the Goal class has to recognize the goal in all
the possible cases that it can be seen. This point makes its recognition really difficult.
The possibilities are:
Case 1.- The whole goal is seen.
Case 2.- Part of the goalpost is seen.
-Subcase 2c.- Part of the crossbar is also seen.
-Subcase 2w.- The whole goal is seen, except the bottom of the nearest goalpost.
Case 3.- Goalmouth is seen.
-Subcase 3w.- The whole goal is seen, but the nearest goalpost is partially hidden
at one side and the bottom of the picture.
Case 4.- The whole crossbar is seen.
-Subcase 4w.-The whole goal is seen, except the bottom of the nearest goalpost.
Case 5.- The basements of the two goalposts are seen.
-Subcase 5w.- The crossbar is also seen, but the goal is split at the top of the picture.
Case 6.- The whole goalpost and part of the crossbar are seen.
-Subcase 6w.- The whole goal is seen, but the nearest goalpost is partially hidden
at a side of the picture.
58
Perception
Case 7.- The basement of a goalpost is seen.

-Subcase 7c.- Part of the crossbar is also seen.
-Subcase 7w.- The whole goal is seen, except a part of the nearest goalpost.
Case 8.- Part of the crossbar is seen.
-Subcase 8c.- Part of the crossbar and of a goalpost are seen.
-Subcase 8w.- Part of the goalposts is seen, as well.
The difference between Case 2c and 3, (and between Case 7c and 6, as well) lies in
that in the Cases 2c and 7c the crossbar ends at the top of the picture, and in the cases
3 and 6 at a side of the picture. The difference between Case 2w and 3w (and between
7w and 6w, as well) are similar to this. Most of times the goal is seen as in the Case 1.
Subcases are very unlikely to happen.
The robots sight depends on the distance between the robot and the goal and on the
orientation of the robot relatively to the goal (the goal is in front of the robot or in its
sideways). We can draw a table that shows each case (Fig. 3.16), depending on the two
factors we have mentioned above.
The goal is..
In front of the robot
Diagonally from the robot
Sideways of the robot
Far away
1
1, 6
6
Middle distance
4, 5
6
2c, 7c
Near
2, 7, 8
2c, 7c, 8
3
Figure 3.13: Different Cases according to the distance and orientation of the goal.
On the other hand, the program treats the cases depending on the characteristic sight
of each one. The following cases are treated similarly:
Cases 1, 5 and all the subcases w (the robot sees both goalposts).
Cases 2 and 7 (the robot sees only one goalpost).
Cases 3, 6 and all the subcases c (the robot sees the goalmouth).
Cases 4 and 8 (the robot sees the crossbar).
3.4.2
Case discrimination
First of all, we obtain the position of all the color pixels of a goal (blue or yellow) clustered
in blobs. We must notice that this method does not work with the blobs processed, but
with the previous state of the blobs, where the blobs are created but not discarded yet.
So, we have to filter out the blobs which do not belong to the goal. To begin with, this
filter rules out the blobs which do not exceed ten per cent of the pixels of the biggest
59
Perception
blob, as well as those which are made up of less than 20 pixels. Through this filter, we
eliminate small blobs (belonging to the blue players, onlookers T-shirts, etc). Now we
can suppose that the survivor blobs belong to the goal. After that, the blobs are merged
and the blob center is computed.
The next step is to distinguish the case we are dealing with. This problem is sorted
out by studying the point where the blob reaches each edge of the image.
A careful study of the possibilities points to us that no case reaches the same edges
(except Case 5 and 7, which are differenciated later.) In fact, all possible situations are
covered by the 8 cases.
After this first selection of the blobs, we roughly know the case we are studying. We
need to go through the blob again, in order to discriminate the case accurately. In Case 2
(and 7) we do not know if the crossbar is seen, or even if we are in Subcase 2w (or 7w). In
Case 3 (and 6), we do not know what goalpost we are seeing, or even if we are in Subcase
3w (or 6w). In the same way, Case 5 can be mistaken for Case 7.
In order to get an accurate discrimination of the case, we study a square of 3 pixels
wide located on the center of the blob. If we find a pixel inside this square which belongs
to the blob, it will mean that:
-Case 2 and 7: we are seeing only part of a goalpost.
-Case 8: we are seeing only part of the crossbar.
If we find no pixel inside the square, we must look for the pixels at that height of the
image. Depending on the cases, the location of the pixels relatively to the center tells us
that:
-Case 3 and 6: We know what goalpost we are seeing according to the side on which
we find pixels of the blob.
-Case 2, 7, 8: We are in Case 2c, 7c or 8c. Like in Case 3 and 6, we know what
goalpost we are seeing according to the side on which we find pixels of the blob.
-Case 2, 3, 4, 6, 8: If we find pixels on both sides, we are in Case 2w, 3w, 4, 6w, or
8w, respectively. Then, all these cases must be treated as in Case 1.
-Case 7: If we find pixels on both sides, it will mean that we are in one of following
cases: 5, 5w, 7w. Then, all these cases must be treated as in Case 1.
3.4.3
Goal location
Now we are able to reach the purpose of this Class: to calculate the distance from the
robot to the middle point between both goalposts (rho), and the angle of the robot to
this point (theta). We also will estimate, as far as possible, the orientation of the goal to
the robot (phi). These three measurements are showed in Fig. 3.16.
First of all, the program asks for the values of rho and theta given by the Odometry,
which in turn has received the values of rho and theta obtained from previous images,
60
Perception
Figure 3.14: The three measurements related to the goal.
and has updated them through the movement of the robot. Afterwards, we estimate the
position of the goalposts predicted by Odometry. If the goal has not been seen for a long
time or we are in Case 2 or 3, this information will be useful.
We must now find the pixels which belong to the bottom of goalposts. To achieve it,
we use several variants of the scan method.
Before aproaching the scan method, we must take into account that the blob consists
in arunsa. A run is a row of pixels which belong to the blob. All the information of a run
is given by three data: the row within the image where the run is (y), the column of the
far left pixel of the run (x), and the width of the run, given by the number of pixels that
belong to the run (the length of the run), (width). Obviously, the width of a run cannot
exceed the width of the image.
For the most common case, Case 1, the scan method works as follows: First of all, we
must find the runs that make the goalposts up. In order to do this, the method uses the
data of each run. If x plus the width is more leftwards than the x-coordinate of the center
of the blob, the run belongs to the left-part of the goal. If its y is below the y-coordinate
of the center of the blob, then the run belongs to the left goalpost. The bottom of each
goalpost will be determined by the pixel in the middle of the run whose y is the lowest
of that goalpost. The same processure applies to the right-part: if the x of a run is more
rightwards than the x-coordinate of the center, the run belongs to the right-part of the
goal (we ignore now the width).
In the rest of cases, we already know what part of the goal we are seeing, or we
already know we are seeing only a goalpost. This point modifies slightly the scan method
regarding Case 1.
The next step is to estimate the distance from the robot to the goal. For this purpose,
we must estimate before the location of goalposts. In order to do this, we have located
the bottom of goalposts within the image.
By other hand, we know that the bottom of goalposts belongs to floor as well. Besides,
we know the height and tilt of the camera. Therefore, we can sort out the distance between
the robot and a goalpost by means of trigonometry, as Fig. 3.16 shows.
61
Perception
Figure 3.15: Right-angled triangle created between the robot and the goalpost.
Thus, we obtain the distance d through the following equation:

d = tan(alf a) h
(3.1)
We already know the view angle of the image. So, pixels from the bottom of the
goalpost to the middle of the image can be transformed into degrees. With these data,
we work out the value of alfa, taking into account the tilt of the camera. In the same way
we can determine the horizontal angle from the robot to the goalpost. We do this task
for both goalposts.
Thus, we must sort out the distance to rho and theta. To get this purpose, we transform our data of the goalpost, which are in polar coordinates, to cartesian coordinates.
Then we locate the center of the goal by means of a sample average. The back transformation to polar transformation will give us rho and theta.
There is phi left to be calculated. This angle is given by the perpendicular vector to the
line that connects both goalposts. It is very easy to do with an orthogonal transformation
of this line.
In Cases 2w, 3w and 4w the bottom point of the nearest goalpost is not seen. We
can sort out the distance to the hidden goalpost by using, in polar coordinates, an equation which relates the horizontal angle to that goalpost and the distance between both
goalposts, that we know to be 1400 mm.
In Fig. 3.16, robot is placed in Origin. We can obtain the horizontal angle to the
nearest goalpost (A). We also know the location of the furthest goalpost (B). The point
A will be located in the intersection between a circle with a radius of 1400 mm centred
on point B and a line that crosses the origin with an angle theta (A) relatively to x axis.
In Cases 6, 7c and 8c, we are able to compute the location of a goalpost. On the other
hand, we already know if that goalpost is the right or the left one. We try to estimate
the location of the other goalpost by means of the orientation of the goal relatively to the
62
Perception
Figure 3.16: Squematic representation of cases 2w, 3w and 4w.
robot, which is given by Odometry, as it is said. In Case 7c, if in the previous frame we
saw the other goalpost, now we update the orientation. Cases 6 and 8c are almost the
same case than 7c.
In Cases 2c and 3, we work in the same way than Cases 6, 7c and 8c. The only
difference is the distance to the goalpost which is seen. This value is obtained from the
information given by Odometry, as well.
In Cases 4 and 8 we assume that the goalpost is in front of the robot, and we suppose
that goal is 1400 mm away for Case 4, and 1220 mm away for Case 8.
Case 7 only gives us the location of one goalpost, but we do not know which goalpost
it is. From previous estimations, we can try to decide on what side the goalpost is. If the
inmediately preceding frame was the goalpost from the other side, we can put together
the information of both and estimate the orientation and the location of the center.
Case 2 provides the least information of all. As in Case 7, we try to decide on what
side the goalpost is, and work out the distance to the goalpost seen by using Odometry.
Finally, we estimate the location of the other goalpost by means of the orientation of the
goal relatively to the robot, as in Cases 6, 7c and 8c.
The last step of the method is to provide the values of rho and theta to LPS.
3.4.4
Filters
In adittion to the filter which rules out blobs that belong to stuff which is not a goalpost,
there are also filters to discard wrong images, due to the closeness of the robot to the goal.
In that case, sometimes the robot sees a leaning goalpost or, because of its brightness, it
mistakes it for two goalposts.
63
Perception
3.5
Active Perception
In order to perform reliably in a dynamic environment, an autonomous robot needs to

link, or anchor, its actions to the appropriate objects in the environment via perception.
The environment being dynamic, the properties of these objects may change over time,
and thus require constant perceptual attention. However, a robot typically has limited
perceptual resources, both in terms of sensing and of processing power. The design of
effective allocation strategies for these resources is therefore of paramount importance in
dynamic and partially unpredictable environments.
The literature in the field of autonomous robotics has reported many solutions to
the problem of controlling perceptual effort. One common approach is to pack both
the perceptual and action processes into one module (or behavior), in order to focus the
perceptual effort exclusively on those features in the environment which are relevant to
the current task. Early examples of this approach can be found in Arkins concept of
affordances [2], in Malcom and Smithers concept of encapsulation [24], and in Brooks
subsumption architecture [7]. Another common approach is to use information about
the current task to perform active control of the sensors, in order to search for specific
features. Active control may imply selecting a specific algorithm, or physically pointing a
sensor in a specific direction. This approach has been pioneered by Bajcsy [3] and Ballard
[4], and is an active research area in computer vision (e.g., [13, 18]).
We use a technique for active perceptual anchoring in dynamic environments which
combines the two approaches above. Following [33], we introduce perception-based behaviors as basic units of control which use anchors to connect behavior to the environment.
An anchor is a data structure which stores information about a physical object, which is
needed by the behavior, e.g., the position of a specific door to cross. We then extend the
notion of an anchor, in order to use it to focus the perceptual effort. In particular, we
include in each anchor a measure of how much the corresponding object is actually needed
for the execution of the current task. This measure is used to selectively direct the gaze
by deciding which anchor we should try to acquire at any given moment.
3.5.1
Perception-based Behaviour
In autonomous robotics, one often distinguishes between so-called reactive, or sensordriven behavior, and proactive, or goal-driven behavior. One way to draw this distinction
is to say that these two types of behavior use different types of input data. Proactive
behavior typically uses as input a combination of prior information and proprioceptive
data, i.e., data from sensors that observe the state of the robots body. Reactive behavior,
by contrast, typically takes as input exteroceptive data, i.e., data from sensors that
observe the environment.
This distinction can be seen in the example shown in Fig. 3.17. On the left, the
robot is engaged in a proactive GoToNet behavior: the expected position of the net in
some reference frame is given as prior information, and the robot relies on its odometry
to estimate and reduce its distance from this position. On the right, the robot is using
64
Perception
Figure 3.17: Proactive (left) vs. reactive (right) behavior.
visual-feedback to reactively move in the direction in which the net is observed. Although
the two behaviors may produce the same trajectory, there are important differences. The
proactive behavior ignores any real-time data about the net: if this is moved, or if the
odometry is wrong, the robot will reach a position which is not related to the actual goal.
The reactive behavior does not suffer from this limitation, but it will be lost if the net is
not perceived, e.g., owing to sensor noise or to occlusion.
As it appears, proactive and reactive strategies have merits and drawbacks which are
somehow complementary; it is therefore reasonable to combine them into one type of
behavior which integrates both goal-based and sensor-based information. To this aim, we
define a perception-based behavior as a unit of control organized as shown in Fig. 3.18.
The behavior takes exteroceptive data, proprioceptive data, and prior information, and
integrates them into an internal state S. In general, S integrates information about the
part of the physical world which is relevant to the behavior, and which is to be used by
the control law . The pivot of this integration is the notion of an anchor.
Figure 3.18: A perception-based behavior.
Anchors were originally introduced in [29] as object descriptors, and have been further investigated in [33, 12]. An anchor is a data structure in S which collects information
about a specific physical object, such as the net in the above example. This information is
used by the control law to generate a control value ~u. Perceptual anchoring is responsible
for initializing and updating the anchors in S as follows.
Creation: build an anchor in S for each object in the environment used by the behavior,
and initialize it from prior information; set the criteria against which future percepts
should be matched (e.g., the color of the net).
Prediction: predict the properties of the anchor after some time has elapsed since it was
65
Perception
last observed; this phase takes proprioceptive data, and possibly a dynamic model
of the object into account.
Anchoring: match newly acquired percepts (exteroceptive data) against the anchor, and
update it accordingly; a consistency check with the prior information may also be
performed in this phase.
This prediction-update cycle is typical of recursive estimation [15]. Anchoring, however, is peculiar in its use of prior, abstract information for initialization and verification
[12].
It is easy to see that proactive and reactive behaviors are special cases of perceptionbased behaviors. The difference between these types of behavior does not reside in the
control law, but in the different way in which the anchors relate to the environment. For
example, the two go-to-net behaviors above can be produced by a simple control law that
minimizes the distance to an anchor associated with the net. In the proactive behavior,
this anchor is initialized from prior information and updated by odometry; in the reactive
one, it is updated by visual data.
The most interesting case, however, is when we use all three types of information to
update the anchor. In this case, the anchor acts as a micro-model of one object in the
environment, which is relevant to the behavior.
Through the anchor, the behavior operates in a closed loop with the environment
whenever we have perceptual data, and relies on plausible assumptions when these are
not available. This is particularly important for the robots used in this work, since looking
at one object usually results in losing sight of others.
It is useful to know how much an anchor is supported by recent perceptual data,
rather than being based on prior information or predictions. Therefore we associate each
anchor with a number in the [0, 1] interval, called anchored, which measures the level of
current perceptual support. This number is set to 1 each time the anchor is updated from
perceptual data, and it exponentially decreases when the anchor is updated by prediction
only. The amount of the decrease depends on the reliability of the prediction. This in
turns depends on the type of object (e.g., fixed or movable) and on the displacement of
the robot. For the legged robots considered in this work, the decrease was rather dramatic
since odometry is extremely unreliable and since most objects are movable.
3.5.2
Active Perceptual Anchoring
Anchors provide the key to making the perceptual processes more relevant to the needs of
the behavior. Since the anchors in S contain all the variables used by the control law,
the perceptual processes only need to extract the values of these variables. Moreover, since
anchors are models of the needed objects, the perceptual routines can use the information
in the anchors to narrow the search space; for example, in order to anchor the ball, we
only need to check the camera image for orange blobs close to the ground.
66
Perception
Focusing the perceptual processes on the anchors in S greatly simplifies perception.

Execution of complex tasks, however, may require information about many objects, and
therefore S may contain many anchors. The key observation here is that the control law
typically needs to access different anchors at different stages of execution. For example,
in the RoboCup domain the robot only needs the position of the ball and of the opponent
net when it must kick the ball into the net, and it only needs the position of the landmarks
when it must return to its home position in the field.
To exploit this fact, we use the organization summarized in Fig. 3.19. Anchors are
extended to include a value that measures, on a [0, 1] scale, how much that specific anchor
is currently needed by the control law . This value is fed by back into S. The perceptual
processes use the needed values to decide for which anchors they should try to extract
information.
Figure 3.19: Feedback of perceptual needs.
As marked in the figure, the anchoring module may also generate sensor controls, e.g.,
a pan-tilt control to change the fixation point of a camera. In this respect, the needed
measure can be effectively used to address an important problem in active gaze control:
how to decide where to move the fixation point. This problem arises whenever we have
one single sensor with only a limited field of view, as it is the case with the camera in the
Sony robots.
For each anchor a in S, we measure the importance of a as follows:
important(a) = needed(a) (1 anchored(a)).
(3.2)
The anchor f with the highest importance is selected as the current focus of attention;
the fixation point is then determined by the expected position of the object which is
associated to f . It is easy to see that if there are n anchors which are needed, this
selection mechanism will select and anchor all of them in a round-robin fashion. Once an
object has been perceived, its anchored value will raise, and a new object will then be
selected whose anchored level had decayed in the meanwhile.
Equation (3.2) has a logical reading. The anchored and needed measures can be
interpreted as fuzzy predicates. Then, (3.2) gives the truth value, according to the rules
and connectives of fuzzy logic [28], of the fuzzy predicate important defined as
important(a) needed(a) anchored(a).
In other words, re-anchoring a becomes important if a is needed, and if it has not been
anchored recently.
67
Perception
3.5.3
Implementation
At an abstract level, the control architecture that we have used in each robot is similar to
the one sketched in Fig. 3.19. The S state contains an anchor for each object of interest.
Each anchor contains the type of object, its (, ) position in robots polar coordinates,
and the value of the anchored and needed fuzzy predicates. The updating of the
position and anchored fields is done as discussed above. The needed field is set by
the control law (in the HBM module, as described in section 5.1).
Perceptual anchoring is implemented as follows. Embedded in the last step of the
vision pipeline (object recognition), we check blobs against the following three objectdependent criteria, illustrated in Fig. 3.20.
Figure 3.20: A ball blob in the astray zone (left), tracking zone (center), and anchoring
zone (right).
First, we reject any blob which is astray for its type: in the leftmost picture in
Fig. 3.20, the pink band of a landmark has been incorrectly classified as a ball (orange)
blob, but rejected because the ball is assumed to lay on the ground. Second, we select
blobs which are in the anchoring zone (AZ) for their type. This is the area in the image
where we can reliably measure the objects position. For instance, a landmark blob is
in its AZ if it is fully inside the image (i.e., it does not touch the image border) since
we use the blobs size to estimate distance. On the other hand, the AZ for the ball is a
small fovea at the center of the image. Finally, we consider the remaining blobs to be in
tracking zone (TZ); these should be brought into the anchoring zone using gaze control.
Gaze control is described by the finite state automaton sketched in Fig. 3.21, where
rectangles denote head motion states whose transitions are activated by events, and ovals
denote purely computational states whose transitions depend on the result of the computation. The meaning of each state is as follows.
Select: choose an anchor f on which to focus using (3.2); if the anchored level of f is
greater than a given threshold, then set the fixation point to this position and exit
via the Good Estimate transition; otherwise exit via the not-GE transition.
Scan: perform a visual scan to explore the part of the space where f could be located,
e.g., the field ground for the ball; exit when one of the following events occurs:
a blob that matches f appears in the TZ; set the fixation point to that blobs
position and exit via the f -in-TZ transition;
68
Perception
Figure 3.21: State transitions for perceptual anchoring.
a blob that matches an anchor a 6= f appears in the AZ for a; exit via the
nf-in-AZ transition;
the physical scan is completed; exit via the Scan Complete transition.
LookTo: move the camera to the current fixation point; when:
a blob that matches f appears in the AZ, exit via the f -in-AZ transition;
a blob that matches f appears in the TZ, set the new fixation point to that
blobs position and stay in this state;
a blob that matches an anchor a 6= f appears in the AZ for a, exit via the
nf-in-AZ transition;
the fixation point has been achieved, and no blob that matches f is in the
image, exit via the Position Achieved transition.
Anchor: measure the position of the object and update the f anchor; select a new focus.
Serendipitous anchor: an object other than f is in the AZ: measure its position and
update the corresponding anchor; continue to search for f .
Lost: either Scan or LookTo have been completed without the current f being found;
mark f as lost and go select a new focus.1
There is a subtlety in the a blob that matches conditions above. In general, how
to associate regions to objects is a complex issue, which may require the use of extraperceptual information. For example, the three opponent robots all have the same perceptual signature, and can only be distinguished by using information about their past
positions. In our case, we simply associate a blob to an anchor based on its type, i.e., its
color. (See [12] for a more elaborated solution to the association problem.)
1
A lost object will not be selected as focus as long as there are other objects which are important
according to (3.2).
69
Perception
3.5.4
Experiments
We present three simple experiments. In the first experiment, the robot simply had to
stand still and track the ball, which was moved by hand. This was achieved by the
following rule (see section 5.1 for information on rules), which sets the needed value for
the ball to 1:
ALWAYS
NEED (Ball)
Fig 3.22 shows the result of ball tracking. The upper part plots the temporal evolution
of the estimate of in the anchor for the ball. The circles mark the LookTo commands
sent to the head whenever a new fixation point was determined; the dotted vertical lines
mark Scan commands. The lower part of the figure shows the corresponding evolution
of the anchored value. From the beginning till time 66 the ball was moved between
60 , at a fixed distance in front of the robot. The robot was able to track and anchor
the ball at all times. At time 66 the ball was removed from the field; the robot then
started a visual scan, while maintaining the previous value and decreasing the degree of
anchoring. At time 86 the ball was laid down again, and the robot could rapidly identify
and re-anchor it. The rest of the plot shows another sequence of moving the ball, stealing
it, and putting it down again. The robot consistently kept the ball anchored when it was
in view, and searched for it when it was out of view.
Figure 3.22: Tracking a moving ball.
In the second experiment the robot was placed in the middle of the field and heading
at 80 from a net (Net1), and it had to track both the moving ball and the net. This was
achieved by the following two rules:
ALWAYS
ALWAYS
NEED (Ball)
NEED (Net1)
70
Perception
The plot in Fig. 3.23 shows the temporal evolution of the estimate of for the ball
and net anchors, and the corresponding values for the anchored predicates. The ball
was first moved between 60 in front of the robot, then taken out of the field at time 43.
At time 57, it was put back down and moved from left to right in front of the robot. The
robot alternatively looked at the ball and at the net using the Select function discussed
above, and kept both of them constantly anchored when the ball was in view. When the
ball was removed from the field, the robot alternatively made a scan for the ball, and
re-anchored the net.
Figure 3.23: Tracking a moving ball and the net..
The third and final experiment illustrates how active perceptual anchoring is guided
by the current perceptual needs expressed by the behaviors during execution. The robot
first has to approach the ball, then align with the net and finally kick the ball. This
behavior uses the following two perceptual rules:
ALWAYS
NEED (Ball)
IF (ballClose) THEN NEED (Net1)
The plot in Fig. 3.24 shows the evolution of (, ) for the ball anchor, and of for
the net anchor, together with the two values for anchored. Until time 118 the robot
was approaching the ball (as testified by the decreasing ) while tracking it. Since the
ballClose predicate was false, the needed value for the net was kept at 0, and the net
was never selected as the focus of attention; as a consequence, the camera was never
pointed at the net, which remained fully unanchored, with at its default value of 0.
At time 118, the truth value of ballClose, and hence the nets needed value, was high
enough for the net to become the focus of attention. The robot then started a visual
scan to search for the net, momentarily losing sight of the ball. The net was soon seen
and anchored, and the robot then pointed its camera to the expected position of the ball
and re-anchored it. The robot then got close to the ball, and at time 127 it executed the
alignment maneuver while using the camera to alternatively anchor the ball and the net.
71
Perception
The alignment maneuver brought the of both the ball and the net close to zero. Finally,
at time 137, the robot kicked the ball, which headed away from the robot and toward the
net.
Figure 3.24: Switching between tracking only the ball, and tracking both ball and net.
The above behaviors are extremely simple, but they illustrate the main ingredients
of active perceptual anchoring. A set of significantly more complex behaviors were implemented for the RoboCup competition using the mechanisms described in this paper.
During the games, the robots successfully tracked all and only the objects which were
relevant to the task being performed.
72
Self-Localization
Chapter 4
Self-Localization
Self-localization in the 4-legged robot league is a challenging task: odometric information
is extremely inaccurate; landmarks can only be observed sporadically since a single camera
is needed for many tasks; and visual recognition is subject to unpredictable errors (e.g.,
mislabeling). To meet these challenges, we have developed a self-localization technique
based on fuzzy logic, reported in [10]. This technique needs only qualitative motion
and sensor models, can accommodate sporadic observations during normal motion, can
recover from arbitrarily large errors, and has a low computational cost. The result of
self-localization is used to make strategic decisions inside the HFSM, and to exchange
information between robots in field coordinates.
(a)
(b)
Figure 4.1: Self localization grids.
This technique, implemented in the GM module, relies on the integration of approximate position information, derived from observations of landmarks and nets, into a fuzzy
position grid see Fig. 4.1. To include own motion information, we dilate the grid
using a fuzzy mathematical morphology operator. Using this technique, our robots can
maintain a position estimate with an average error of approximately 20 cm and 10
during normal game situations. Localization is done continuously during normal action.
Stopping the robot to re-localize is only needed occasionally (e.g. in case of major errors
due to an undetected collision).
The extension to our method which includes natural features as landmarks is described
in [17]. The extended technique allows the robot to localize using only the nets and natural
73
Self-Localization
features of the environment (i.e. corners).
4.1
4.1.1
Uncertainty Representation
Fuzzy locations
Location information may be affected by different types of uncertainty, including vagueness, imprecision, ambiguity, unreliability, and random noise. An uncertainty representation formalism to represent locational information, then, should be able to represent all
of these types of uncertainty and to account for the differences between them. Fuzzy logic
techniques are attractive in this respect [31]. We can represent information about the
location of an object by a fuzzy subset of the set X of all possible positions [46, 47]. For
instance, X can be a 6-D space encoding the (x, y, z) position coordinates of an object
and its (, , ) orientation angles. For any x X, we read the value of (x) as the degree
of possibility that the object is located at x given the available information.
Fig. 4.2 shows an example of a fuzzy location, taken in one dimension for graphical
clarity. This can be read as the object is believed to be approximately at , but this belief
might be wrong. Note that the unreliability in belief is represented by a uniform bias
b in the distribution, indicating that the object might be located at any other location.
Total ignorance in particular can be represented by the fuzzy location (x) = 1 for all
x X.
4.1.2
Representing the robots pose
Following [10], we represent fuzzy locations in a discretized format in a position grid: a

tessellation of the space in which each cell is associated with a number in [0, 1] representing the degree of possibility that the object is in that cell. In our case, we use a 3D
grid to represent the robots belief about its own pose, that is, its (x, y) position plus
its orientation . A similar approach, based on probabilities instead of fuzzy sets, was
proposed in [9].
This 3D representation has the problem of having a high computation complexity,
both in time and space. To reduce complexity, we adopt the approach proposed by [10].
Instead of representing all possible orientations in the grid, we use a 2D grid to represent
the (x, y) position, and associate each cell with a trapezoidal fuzzy set x,y = (, , , h, b)
that represents the uncertainty in the robots orientation. Fig. 4.2 shows this fuzzy set.
The parameter is the center, is the width of the core, is the slope, h is the height
and b is the bias. The latter parameter is used to encode the unreliability of our belief as
mentioned before.
For any given cell (x, y), x,y can be seen as a compact representation of a possibility
distribution over the cells {(x, y, ) | [, ]} of a full 3D grid. The reduction in
complexity is about two orders of magnitude with respect to a full 3D representation
(assuming a angular resolution of one degree). The price to pay is the inability to handle
74
Self-Localization
Figure 4.2: Fuzzy set representation of an angle measurement .
multiple orientation hypotheses on the same (x, y) position but we can still represent
multiple hypotheses about different positions. In our domain, this restriction is acceptable.
4.1.3
Representing the observations
An important aspect of our approach is the way to represent the uncertainty of observations. Suppose that the robot observes a given feature at time t. The observed range and
bearing to the feature is represented by a vector

r . Knowing the position of the feature
in the map, this observation induces in the robot a belief about its own position in the
environment. This belief will be affected by uncertainty, since there is uncertainty in the
observation.
In our domain, we consider three main facets of uncertainty. First, imprecision in the
measurement, i.e., the dispersion of the estimated values inside an interval that includes
the true value. Imprecision cannot be avoided since we start from discretized data (the
camera image) with limited resolution. Second, unreliability, that is, the possibility of
outliers. False measurements can originate from a false identification of the feature, or
from a mislabelling. Third, ambiguity, that is, the inability to assign a unique identity to
the observed feature since features (e.g., corners) are not unique. Ambiguity in observation
leads to a multi-modal distribution for the robots position.
All these facets of uncertainty can be represented using fuzzy locations. For every type
of feature, we represent the belief induced a time t by an observation

r by a possibility
distribution St (x, y, | r ) that gives, for any pose (x, y, ), the degree of possibility that
the robot is at (x, y, ) given the observation

r . This distribution constitutes our sensor
model for that specific feature.
The shape of the St (x, y, |

r ) distribution depends on the type of feature. In the case
of net observations, this distribution is a circle of radius |

r | in the (x, y) plane, blurred
according to the amount of uncertainty in the range estimate. Fig. 4.3 show an example
of this case. In the figure, darker cells indicate higher levels of possibility. We only show
the (x, y) projection of the possibility distributions for graphical clarity.
Note that the circle has a roughly trapezoidal section. The top of trapezoid (core)
75
Self-Localization
(a)
(b)
Figure 4.3: Belief induced by the observation of a blue net (a) and a yellow net (b). The
triangle marks the center of gravity of the grid map, indicating the most likely robot
localization.
identifies those values which are fully possible. Any one of these values could equally be
the real one given the inherent imprecision of the observation. The base of the trapezoid
(support) identifies the area where we could still possibly have meaningful values, i.e.,
values outside this area are impossible given the observation. In order to account for
unreliability, then, we include a small uniform bias, representing the degree of possibility
that the robot is somewhere else with respect to the measurement.
The S (x, y, |
r ) distribution induced by a corner-feature observation is the union
t
of several circles, each centered around a feature in the map, since our simple feature
detector does not give us a unique ID for corners. Fig. 4.4 shows an example of this.
It should be noted that the ability to handle ambiguity in a simple way is a distinct
advantage of our representation. This means that we do not need to deal separately
with the data association problem, but this is automatically incorporated in the fusion
process (see below). Data association is one of the unsolved problems in most current
self-localization techniques, and one of the most current reasons for failures.
4.2
Fuzzy Self-Localization
Our approach to feature-based self-localization extends the one proposed by Buschka et

al in [10], who relied on unique artificial landmarks. Buschkas approach combines ideas
from the Markov localization approach proposed by Burgard in [9] with ideas from the
fuzzy landmark-based approach technique proposed by Saffiotti and Wesley in [37].
The robots belief about its own pose is represented by a distribution Gt on a 2 12 D
possibility grid as described in the previous section. This representation allows us to
represent, and track, multiple possible positions where the robot might be. When the
robot is first placed on the field, G0 is set to 1 everywhere to represent total ignorance. This
belief is then updated according to the typical predict-observe-update cycle of recursive
76
Self-Localization
(a)
(b)
Figure 4.4: Belief induced by the observation of a feature of type C (a) and T (b). Due
to symmetry of the field, the center of gravity is close to the middle of the field.
state estimators as follows.

Predict When the robot moves, the belief state Gt1 is updated to Gt using a model
of the robots motion. This model performs a translation and rotation of the Gt1
distribution according to the amount of motion, followed by a uniform blurring
to account for uncertainty in the estimate of the actual motion. Fig. 4.5 show an
example of this step, in which motion (translation) and uncertainty (blurring) model
are applied.
Observe The observation of a feature at time t is converted to a possibility distribution
St on the 2 12 grid using the sensor model discussed above. For each pose (x, y, ),
this distribution measures the possibility of the robot being at that pose given the
observation. Fig. 4.3 and 4.4 show these possibility distribution.
Update The possibility distribution St generated by each observation at time t is used
to update the belief state Gt by performing a fuzzy intersection with the current
distribution in the grid at time t. The resulting distribution is then normalized.
In the Fig. 4.6 is shown the localization pipeline process, the recursive system is
continously applied to mantain the robots pose estimation.
If the robot needs to know the most likely position estimate at time t, it does so by
computing the center of gravity of the distribution Gt . A reliability value for this estimate
is also computed, based on the area of the region of Gt with highest possibility and on
the minimum bias in the grid cells. This reliability value is used, for instance, to decide
to engage in an active re-localization behavior.
In practice, the predict phase is performed using tools from fuzzy image processing, like
fuzzy mathematical morphology, to translate, rotate and blur the possibility distribution
in the grid [5, 6]. The intuition behind this is to see the fuzzy position grid as a gray-scale
image.
77
Self-Localization
(a)
(b)
(c)
Figure 4.5: Robots belief posisition is represented by Gt distribution (a), when robot
moves a translation is applied to Gt using the robots motion model (b) and a blurring to
account for uncertainty of motion(c).
Figure 4.6: Flowchart localization process. First, the belief state Gt is updated with
the possibility distribution St (x, y|r) of an observation, then the robot moves and the
odometry model B is applied to Gt , obtaining Gt+1 . Then, a new perception is updated,
0
outcoming the last belief state Gt .
78
Self-Localization
For the update phase, we update the position grid by performing pointwise intersection of the current state Gt with the observation possibility distribution St (|r) at each
cell (x, y) of the position grid. For each cell, this intersection is performed by intersecting
the trapezoid in that cell with the corresponding trapezoid generated for that cell by the
observation. This process is repeated for all available observations. Intersection between
trapezoids, however, is not necessarily a trapezoid. For this reason, in our implementation we actually compute the outer trapezoidal envelope of the intersection. This is a
conservative approximation, in that it may over-estimate the uncertainty but it does not
incur the risk of ruling out true possibilities.
There are several choices for the intersection operator used in the update phase, depending on the independence assumptions that we can make about the items being combined. In our case, since the observations are independent, we use the product operator
which reinforces the effect of consonant observations.
Our self-localization technique has nice computational properties. Updating, translating, blurring, and computing the center of gravity (CoG) of the fuzzy grid are all linear
in the number of cells. In the RoboCup domain we use a grid of size 36 54, corresponding to a resolution of 10 cm (angular resolution is unlimited since the angle is not
discretized). All computations can be done in real time using the limited computational
resources available on-board the AIBO robot.
4.3
Experimental results
To show how the self-localisation process works, an example generated from the goal
keeper position is presented. Lets suppose that the robot starts in its own area, more or
less facing the opposite net (yellow). At this moment the robot has a belief distributed
along the full field it does not know its own location. Then the robot starts scanning
its surroundings by moving its head from left to right. As soon as a feature (natural or
not) is perceived, it is incorporated into the localization process.
When scanning, the robot first detects a net and two features of type C (Fig.??).
The localization information is shown in (Fig.4.7), where the beliefs associated to the
detection are fused. The filled triangle represents the current estimate.
In order to cope with the natural symmetry of the field, we use unique features, like
the nets are. When the robot happens to detect the opposite net (yellow), it helps to
identify in which part of the field the robot is currently in, and the fusion with the previous
feature based location gives a fairly good estimate of the robot position.
Since our feature detector doesnt provide unique identifiers for corners, the possible
robot positions induced by a corner observation is the union of several circles, each centered around a corner in the map. This can be seen in Fig. 4.4. It should be noted that the
ability to handle this ambiguity efficiently is a primary advantage of this representation.
The data association problem is automatically addressed in the fusion process (Fig. 4.7).
Data association is a difficult problem, and many existing localization techniques are
unable to adequately address it.
79
Self-Localization
(a)
(b)
(c)
Figure 4.7: Belief induced by the observation of (a) a net, (b) the first feature, and (c)
the second feature. Initially the position of the robot is unknown (belief distributed along
the full field).
80
Behaviours
Chapter 5
Behaviours
5.1
Low-level Behaviours
Behavior-based systems are increasingly used in many robotic applications, including mobile units, manipulators, entertainment robots and humanoids. Behavior-based systems
were initially developed on mobile robots, where complex tasks were achieved by combining several elementary control modules, or behaviors [7]. In most of these systems,
however, arbitration between behaviors was crisp, meaning that only one behavior was
executed at a time, resulting in jerky motion. In other systems, several behaviors are
executed cuncurrently and their outputs are fused together, resulting in smoother motion
during switching of behaviors [11, 35, 30].
The use of behavior-based system for more complex plants than a wheeled unit needs
a framework which is able to handle several DOF. [22] uses fuzzy rules to control a 6 DOF
arm, and [23] describes an automated mining excavator that uses concurrent behaviors.
However, the complexity of these systems makes the design process very demanding: [22]
uses 120 rules to perform a PickUp task, and [23] uses a neural network for behavior
arbitration, thus giving up the readability of the arbitration rules.
We follow an approach to building behavior-based systems that can be applied to
control plants with many DOF. Complexity is addressed by a hierarchical decomposition
[36] of the overall task into simple behaviors. These behaviors are encoded by small sets
of fuzzy rules. In addition, fuzzy meta-rules are used to encode the way behaviors are
combined together: this makes it very easy to re-configure the system for different tasks.
5.1.1
Basic Behaviours
We define a set of basic behaviours, that is, behaviours that perform elementary types
of actions, most of them common to all players independently of their role in the field.
In our domain, these include turning toward the ball, going to the ball, or moving to a
given position. These behaviours constitute the basic building blocks from which more
complex types of actions are obtained by hierarchical composition. The behaviours, which
81
Behaviours
are packaged in the HBM module, are defined by way of fuzzy rules [33] [32]. The input
space used by all behaviours is the local state provided by the PAM, which contains the
current estimates of the position of all the objects in the field. The output space consists
of the velocity set-points < vx , vy , v > which are transmitted to the CMD module. An
additional control variable k[] is used to indicate which kicks are applicable in the given
situation.
Thus behaviours are coded by using fuzzy rules of the form:
IF <predicate> THEN <action>
where predicate is a formula in fuzzy logic that evaluates a series of properties of the
local state. This can be done without the need of an analytical model of the system or an
interaction matrix, which may be difficult to obtain for complex plants and tasks, as the
RoboCup case is. It is also worth noting that the uncertainty in fuzzy system is taken in
account as well.
In order to give a more concrete impression of how behaviours are implemented, we
show here the CutBall behaviour. For sake of clarity, all the rules shown in this section
are given in a slightly simplified form with respect to the actual rules implemented in the
robot. The CutBall behaviour uses three rules to control the lateral motion of the robot
(action STRAFE ) to locate it on the trajectory from the ball to the centre of the net.
It also uses three rules to control the orientation of the robot to point the head toward
the ball (action TURN ). This behaviour does not use the forward motion, and thus it is
always set to zero (action GO). Finally, it controls the type of kick to be applied (action
KICK ). If the ball is close enough and the robot is pointing to a safe area it tries to kick
it using its both arms and the chest.
IF (posLeft)
IF (posAhead)
IF (posRight)
IF (headedLeft)
IF (headedAhead)
IF (headedRight )
IF AND (freeToKick, ballClose)
ALWAYS
THEN STRAFE (RIGHT)

THEN STRAFE (NONE)
THEN STRAFE (LEFT)
THEN TURN (RIGHT)
THEN TURN (NONE)
THEN TURN (LEFT)
THEN KICK (FrontKick)
GO (STAY)
The aim of this behavior is to keep the goalkeeper on the trajectory of the ball to
the net. The rule conditions are evaluated from features of the LPS like Ball < , >
(distance and orientation to the ball), and produce a truth value between 0 and 1. Fig. 5.1
shows the truth value, or membership function, of the first three conditions as a function
of the x position of the robot with respect to the ball-to-net trajectory. These functions
are defined by the designer, and depend on how the robot should try to cut the ball. The
consequent of the rules indicate which control variable should be affected, and how: the
first three rules involve lateral motion of the robot, while the three following rules involve
rotational motion. Each rule affects the corresponding control variable by an amount
82
Behaviours
Figure 5.1: Membership functions for the posLeft, posAhead and posRight conditions
that depends on the truth value of its condition: smaller or larger adjustments to the
robot motion will be generated depending on how much the robot is close to the ball-tonet trajectory. Rules are evaluated and combined according to the standard Mamdani
approach.
Behaviours also may incorporate perceptual rules used to communicate the perceptual
needs of active behaviors to the PAM (see section 3.5.2), by using fuzzy rules of the form:
IF <predicate> THEN NEED <object>
whose effect is to assert the need for an object at a degree that depends on the truth
value of condition.
In order to give a more concrete impression of how perceptual behaviours are implemented, we show here the TrackBallAndNet1 behaviour. This behaviour is useful when
the robot tries to store a goal in the opposites net. In this situation it will always concentrate attention on the ball, but when it is close to the ball, it might want to also get
information on the relative position of the net. in order to fine motion to head to the net.
This can be accomplished with only the following two rules:
ALWAYS
NEED (Ball)
IF (ballClose) THEN NEED (Net1)
where ALWAYS is a condition which is always true. In a situation where the ball is at
400 mm from the robot, the truth value of ballClose is 0.7, and these rules assert a value
of needed of 1.0 for the anchor Ball and of 0.7 for Net1. Behaviors are dynamically activated and deactivated according to the current task and situation, and several behaviors
can be active at the same time. The needed values stored in the S state are those asserted
by the active behaviors, combined by the max operator. This guarantees that perceptual
anchoring only depends on the currently active behaviors, hence on the current task.
83
Behaviours
5.1.2
Fuzzy Arbitration
We build complex behaviours by combining simpler ones using fuzzy meta-rules, which
activate concurrent sub-behaviors. This procedure can be iterated to build a full hierarchy of increasingly complex behaviours. The mechanism used to perform behaviour
composition is called Context-Dependent Blending [31]. Thus, under the CDB paradigm
flexible arbitration policies can be obtained using fuzzy meta-rules of the form:
IF <condition> THEN USE <behaviour>
For each such rule, the controller evaluates the truth value, in range [0,1], of <condition>
and activates <behaviour> at a level that corresponds to this value. Several behaviors
may be active at the same time: in this case, their outputs are fused by a weighted
combination according to the respective activation levels. Fusion is performed on each
control variable independently. The use of fuzzy logic to fuse the outputs of concurrent
behaviors provides a smooth transition during switching between behaviors [30], which
would be difficult to achieve in architectures based on crisp arbitration like subsumption
architecture [7].
As an example, the following meta-rules implement the ClearBall behaviour, which
uses four rules to decide what action to take: turning until the robot faces the ball, moving
the robot to approach the ball location, kicking the ball or moving the robot between the
ball and the opponents net. The behaviour also controls the type of kick to be applied,
depending on the orientation of the robot it uses both arms or the head.
IF
IF
IF
IF
IF
IF
NOT (ballSeen)
AND (ballSeen, ballClose, freeToKick)
AND (ballSeen, ballClose, NOT (freeToKick))
AND (freeToKick, freeLeftKick)
AND (freeToKick, freeFrontKick)
AND (freeToKick, freeRightKick)
THEN
THEN
THEN
THEN
THEN
THEN
USE Face (Ball)

USE KickForward
USE AlignBallAndNet1
KICK (LeftHeadKick)
KICK (FrontKick)
KICK (RightHeadKick)
One key characteristic of our behaviors combination technique is that there are well
established techniques to perform fuzzy fusion of the output of behaviours [30].
The rule-based approach and hierarchical organization allows us to design, implement
and test very complex behaviours, like the goalkeeper behaviour, with a limited amount
of effort. The goalkeeper behaviour involves the use of navigation, manipulation, and
perceptual actions in a highly dynamic and unpredictable environment. The full GoalKeeper behaviour has been decomposed into 12 behaviours, which involve more than 70
fuzzy rules (including those in the basic behaviours) plus 12 perceptual rules. The development of these rules required about 4 weeks of work by one person. This behaviour was
successfully used in the RoboCup 2002 competition in Fukuoka, Japan [26].
84
Behaviours
Figure 5.2: Behaviour hierarchy for the goalkeeper
5.1.3
The Behaviour Hierarchy
We can use complex behaviors to form even more complex ones, thus creating a behavior
hierarchy. A simplification of the goalkeepers strategy is exemplified in Fig. 5.2. These
behaviours were used in the RoboCup 2002 competition [26]. The squared boxes indicate
basic behaviours, that is, behaviours that directly control motion and do not call any
sub-behaviours.
There is a set of behaviours that are common to all players and are also used by the
goalkeeper.
Face. Turns the robot until it directly sees a given object.
GoTo. Moves the robot to a given location.
KickForward. Moves the robot towards the ball and applies a kick.
AlignBallAndNet1. Moves the robot until it is aligned with both the ball and the
opponents net.
Moreover, there are some basic behaviours which are specific for the goalkeeper:
TrackLandmarks. Select the least recently seen landmark as a desired perceptual
goal.
KeepOutNet2. Turns the robot slowly moving until it is outside its net.
KeepInArea. Put the robot facing forward and then moves it to the goalkeeper area.
85
Behaviours
KeepBackInArea. Put the robot facing backwards and then moves it below the
penalty kick line.
CutBall. Turn and move sideways in order to intercept the ball trajectory.
The other behaviours in the hierarchy are complex behaviours intended to perform the
following goalkeeper tasks:
GoalKeeper. Top-level goalkeeper behaviour.
ScanForObj. Scan the field on place until a given object is found. Look at Net1,
LM6, Net1, LM1 cyclically.
Localize. Get a better position estimation through looking for the closest landmarks.
ClearBall. Turn and move forward in order to kick the ball.
Some of the above behaviours express specific perceptual needs by way of perceptual rules.
For instance, most behaviours express a need for the ball position. The KeepOutNet2
and KeepInArea behaviours both need to have accurate information about the robots
own location in the field, and hence they express a need for the most probably visible
landmarks, including the opponents net. Moreover, CutBall and ClearBall behaviours
both need to have accurate information about the ball position in addition to the robots
own accurate location in the field (to avoid self scoring). In general, the overall perceptual
needs of the GoalKeeper behaviour depend on which sub-behaviours are activated at every
moment, and are used to direct perception.
5.2
High-level Behaviours
For the high level layer, we implement a hierarchical finite state machine (HFSM) [20],
which is derived from the tool created by the Universite de Versailles [19]. We have
reimplemented this tool, adding some new features: monitoring the current state, reusing
states from different HFSMs, etc. The HFSM tool is described in Appendix B. An
HFSM consists on a set of states, metastates, which are state machines as well, and
transitions between states and/or metastates. When the robot is in a state, it executes
the corresponding states code. This is standard C++ code accessing both local and
global perception (ball, nets, landmarks, robot positions, etc.), and shared information
from other robots. We have a mechanism to call low level behaviours, which allows us to
call any specific behaviour (for example, GoToBall) and/or select any specific kind of kick
(KickForward). This kick is active and it will be performed if low level conditions (ball
position) are correct. Thus, several kick can be actived and a low level selection process
chooses which of them will be performed. Transitions in the HFSM are conditions to
change between states (ball position, robot position, etc.).
The HFSM mechanism can be used for role assignment and execution, so that field
players can play different roles in different game conditions. For example, if a defender
86
Behaviours
goes to the ball it can change its role to attacker and another robot should change to its
own role to defender. This can be easily achieved defining several metastates and sharing
information between the robots in order to know when to change.
5.2.1
Hierarchical Finite State Machines
An automata can be formed by states and transitions between states. Some essential
concepts:
State In every moment, a robot will be executing a state. A state is a set of actions
(normally a call to a low level behavior) which are executed by the robot. In our
implementation, it contains C code (or OPENR).
Metastate It is a state which contains an automata. It can be formed by states, transitions or metastates. We can use extern metastates. An extern metastates is a
normal automata. An automata can contain several external metastates (but not
itself) and these metastate can be referenced in different places in our automata or
from other automata, without duplicating code. It can be thought like a subrutina.
Transitions They allows changes between states. They always have a initial and final
state. Here, when we talk about states we are refereing to state or metastate,
indistinctly. Transitions have two parts. First, is the test code. When the robot is
in a state, it checks the test conditions from all transitions from this state. If some
of this tests is true, the new state for the robot is the final state of that transition. If
any test is true, the robot continues executing the current state. The test condition
of the automata has to return a boolean value, returning true when the condition to
change the state happens. Transitions have a code associated. This code is executed
every time the test code is executed. If several transitions come out from the same
state and we want that a concrete transition is checked before other, we have to use
the priority associated to the transition. The less priority it has a transition the
earlier it is checked.
A metastate is an automata in itself and must carry out all the preconditions fro an
automata. In fact, our main automata is a mestate. An automata must always have
a initial state, which is the one which will be executed first. If we have a metastate
in our automata and the execution arrives to this metastate, the initial state of this
metastate is executed. When the transitions are checked, not only the transitions from
the state (inside the metastate) are checked but also the transitions from the metastate.
Furthermore, transitions from the metastate are first checked.
5.3
Team Coordination
A problem arises when a team play a match without coordination between the players: all
the robots go to the ball. This situation is not desirable because the players can become
87
Behaviours
in a encumbrance for the mates, or even they can be penalised for pushing to mates or to
adversaries.
During RoboCup 2005 a mechanism was developed for avoiding all players going to
the ball. This mechanism basically consists in booking the ball by the player that believes
it is the nearest player to the ball. The idea is to make the farest robot from the ball to
go to a defender position, the nearest robot to go to the ball, and the other one to go to
the ball up to a given distance from ball.
5.3.1
Ball Booking
For the ball booking strategy to correctly work, a series of requisites must be satisfied:
The nearest player to the ball must go to the ball. The others mates should get an
optimum position in the field, looking to the ball, but without going to it.
At least one player must go to the ball. A situation such as no players going to the
ball is not allowed.
Two or more players with almost the same distance to the ball should not get locked
because they are booking the ball alternatively.
A player can book the ball and then be penalised. The others players must be aware
of this situation so that this penalised player releases the ball. The same situation
is possible when the player who booked the ball crashes down.
The mechanism consists in maintaining a local booking state of the ball. The ball
can be requested by a robot, which announces this booking to other robots using the
communication mechanism explained in ??. Other robots update this information, and
locally decides if they can book the ball or not.
The robot who has booked the ball must renew periodically the booking sending
messages to the other mates. The information sent to the others player is the distance
to the ball. A player who receives a booking message updates its local state with the
received distance, the robot who has booked the ball and the local time when the ball was
booked.
If a player does not receive a renewal of the book in a time ttimeout , it locally unbook the
ball. In this way, if a player who has booked the ball crashed, the ball is not permanently
booked.
If a player is penalised or decides it does not want the ball, it can release the ball and
to inform to its mates.
5.3.2
Implementation
In order to implement this mechanism a simple Booking Protocol was implemented. This
protocol basically consist of the following methods:
88
Behaviours
bookBall: this method books the ball if it has not been previously booked, or if the robot
distance to the ball was smaller (considering a configurable tolerance for avoiding
alternatively ball booking by two or more players).
releaseBall: if the robot has booked the ball but it is no longer having it, it can release
it.
checkBookedBall: this method checks if the timestamp of the booked ball has expired.
If so, the state is set to free. This method is call in the Hbm::NextBehavior method
each time the behavior is evaluated.
isBookedBall and haveBookedBall: returns if the ball has been booked by any robot
or by the robot calling itself. The last one has been implemented using the first one.
These methods are based in a new information that is transmitted among robots. This
class is BallBooking and has been added in the source/utils/udp directory. This class
inherits from ISerializable to be transmitted:
class BallBooking : public ISerializable
{
public:
// Standard OpenR methods
BallBooking();
~BallBooking() {}
void
void
void
void
setBallBooked(bool val) {ballBooked = val;}

setRobotBooker(int val) {robotBooker = val;}
setDistance(int val) {distance = val;}
setTimeStamp(int val) {timeStamp = val;}
bool getBallBooked() {return ballBooked;}

int getRobotBooker() {return robotBooker;}
int getDistance() {return distance;}
int getTimeStamp() {return timeStamp;}
//Virtual methods to implement, inherited from ExternalMesage
void serialize(ISerializable *object, byte **stream, size_t *len);
void unserialize(byte **stream, ISerializable *object, size_t len);
private:
bool ballBooked;
int robotBooker; // ID of the robot that have booked the ball
int distance; // Distance of the robot which has booked the ball
unsigned int timeStamp;
};
In order the behavior to work, the following changes has been made:
89
Behaviours
In the nextBehavior method of the Hbm class, the booked ball state has to be
checked, and if the robot is penalised it should release the ball:
if(gameControllerState->teams[*myteamis].players[*dog_num -1].penalty != 0)
{
// If I have booked the ball and I am penalised, I must release the ball
if ( !(dog_num==NULL || lpscpy->Ball_l == NULL) &&
( ballBookingInfo.getRobotBooker() == *dog_num )
)
releaseBookedBall();
RunBehavior(BehaviorStill);
return;
};
// Check the state of the booked ball
checkBookedBall();
Methods to implement the booking mechanism have been added to the Hbm class,
as well as information to be transmitted:
TcmClient *tcmClient;
void
void
bool
bool
void
void
checkBookedBall ( );
bookBall ();
isBookedBall();
haveBookedBall();
releaseBookedBall();
printBookedBall();
[ ... ]
static BallBooking ballBookingInfo;
int *dog_num;
// Which dog this is
The only changes made was to modify the previous attacker in the following way:
void BallControl::whatToDo_in_state_GoToBall()
{
#ifdef TracePlayerField
printf("STATE: GoToBall\n");
#endif
my_hbm->bookBall();
if(my_hbm->haveBookedBall())
{
90
Behaviours
my_hbm->RunBehavior(BehaviorGoToBall);
}
}
5.4
The Players
5.4.1
GoalKeeper
The GoalKeeper HFSM implements a high level goalkeeper behavior. It has to do several
things: go to its net, keep inside its area and, face the ball and positioning the robot in the
ball-net trajectory. Figure 5.3 shows the current automata for the goalkeeper behavior.
Figure 5.3: Goalkeeper automata.

The initial state of the automata is the GoToArea metastate (Figure 5.4). We assume
that the robot is outside of its small area so it has to go there. Also the robot has to be
able to go to its small area in every moment is the match. A good localization is assumed.
The sequence in this metastate is: first a GoToPos low level behavior is used to go to
a predefined position (initial position) and when the distance to this position is below
a fixed threshold (100) then it turns searching the opposite net (SearchNet1 behavior).
There are several transitions from this metastate:
If the quality of the localization is below a fixed threshold, the robot changes its
state to Localize, in which an active localization is done.
If the ball is close to the robot, the Anchored of the ball is enough high and the
quality of the localization is good, go to the Defender metastate.
91
Behaviours
If the distance to the initial position is below a threshold and the quality of my
localization is good, the robot is in its area and the KeepInArea metastate is invoked.
If the robot is inside its own net, go to EscapeFromNet2. There are a variable in
the LPS call astray which indicates this situation.
Figure 5.4: GoToArea Metastate.

The KeepInArea metastate (Figure 5.5) tries to keep the robot in its area. First, it
uses a low level behavior called BehaviorGKKeepInArea which uses the global position of
the robot in order to get the robot centered in its net. If the opposite net is not seen, the
automata executes the SearchNet1 state, which turns until that net is seen. From this
metastate there are the following transitions:
If the quality of my localization is below a threshold, the Localize state is invoked.
If the ball is close to the robot, the Anchored of the ball is enough high and the
quality of the localization is good, go to the Defender metastate.
If the distance to the initial position is over a threshold and the quality of my
localization is good, the robot is out of its area and the GoToArea metastate is
invoked.
If the robot is inside its own net, go to EscapeFromNet2.
Defender metastate (Figure 5.6) faces the ball positioning the robot between the ball
and the net. When the ball is close (less than 500 millimeters) a GKClearBall behavior
is executed. Depending of the angle between the robot and the net, different kicks are
selected (if my net is at my left, RightHeadKick is selected, if at my right LeftHeadKick,
and FrontHeadKick if the opposite net is in front of me). While Im seeing the ball in my
area I must continue defending. Only in the case of not seeing the ball (its anchored is
below a threshold or the ball is not in our area) the robot considers (through a transition)
to do other thing:
92
Behaviours
Figure 5.5: KeepInArea Metastate.

Executes KeepInArea metastate if the position is close to its initial position, or
Executes GoToArea metastate if is far from this position.
Figure 5.6: Defender Metastate.

EscapeFromNet2 metastate (Figure 5.7) detects when the robot is inside its own net.
This is a undesirable situation and it has to be able to keep out of it quickly. To do that
a GKEscapeFromNet2 behavior is invoked, which turns and strafes to get the robot out.
Once the robot is out of its net, it turns until the opposite net is seen. There is only one
transition from this metastate. If the robot is seeing the opposite net, it is not in its net,
and changes to KeepInArea metastate, because if the robot was seeing its net it should
not be so far from its net.
Several private variables are defined:
93
Behaviours
Figure 5.7: EscapeFromNet2 Metastate.

BALL CLOSE Distance to consider too close the ball.
QUALITY Two thresholds: QUALITY LOW and QUALITY HIGH. They are used to
know if the quality of the localization is good, applying a hysteresis.
ANCHORED Two thresholds: ANCHORED LOW and QUALITY HIGH. The same
as before for the anchored of an object.
BALL IN AREA Distance to consider the ball in my area (half field).
IN POS OUT POS Thresholds for considering the robot in its initial position.
GLOBALXPOS GLOBALYPOS Initial position of the goalkeeper.
ANG Two thresholds: ANG LOW and ANG HIGH. Used for angles.
5.4.2
Soccer Player
The Soccer Player HFSM implements a high level player behavior. This behavior has
as main target the team coordination for playing soccer using the ball booking method
described in section 5.3.1. The top level state is showed in Figure 5.8. The robot starts
at LookForBall state, in which the robot performs the necesary movements and scans
for finding the ball. When the ball is found, the BallFound transition changes the state
to PlaySoccer metastate. If the ball is lost, the BallLost transition changes the state to
initial LookForBall state.
The PlaySoccer metastate (Figure 5.9) starts at BookBall state, in which the robot
tries to book the ball. Depending on the distance from the ball and if the robot has
actually booked the ball, a rol for the player will be selected.
If the robot has actually booked the ball, the HaveBookedBall transition will change
to Attacker metastate, which implements the attacker role. If the robot hasnt booked the
94
Behaviours
Figure 5.8: Top level state for player

ball, but it is the second nearest robot from the ball, the NotBookedButNext transition
will change to Supporter metastate, which implements the supporter role. On the other
hand, the robot is the farder from the ball, the NotBookedAndFar transition will change to
Defender metastate, which implements the defender role. If any of the previous conditions
changes (a change in booking or a change in the relative position from ball), the robot
changes to the BookBall.
The Attacker metastate (Figure 5.10) starts at BookBall state. In this state the robot
continues going to the ball, until a kick condition is satisfied. The transitions defined
in this state (NetIsFront, NetIsLeft,...) decide change to which kick state (KickFront,
KickLeft,...) depending on the relative position from ball and the angle with respect the
nets. In the kick states the adequate kicks are activated. If the ball or net condition
changes, the robot changes to BookBall state.
The Supporter metastate (Figure 5.11) starts at GoNextBall state. In this state the
robot goes to the ball until a predefined distance (typically 40 cm). When this distace
is reached, the BallAtDist transition changes to FaceBallAtDist state. In this state,
the robot mantains the distance from the ball and faces it. If the distance changes the
DistChanged transition changes to GoNextBall state.
The Defender metastate (Figure 5.12) starts at GoToDefPos state. In this state the
robot calculates a defensive position having in mind the global ball position, and try to
reach that position. When the right position is reached, the AtDefPos transition changes
to FaceBallAtDef state. In this state, the robot mantains the position and faces the
ball. If the conditions change or the right defensive position depending on the global ball
position changes, the DefPosChanged transition change the state to GoToDefPos state.
95
Behaviours
Figure 5.9: PlaySoccer metastate
Figure 5.10: Attacker metastate
96
Behaviours
Figure 5.11: Supporter metastate
Figure 5.12: Defender metastate
97
Behaviours
98
ChaosManager Tools
Appendix A
ChaosManager Tools
A.1
Introduction
ChaosManager is the ultimate tool developed by Team Chaos as a generic way for interacting with robots. This tool is used to calibrate, test and develop different parts
regarding the NAO robots and its application frame of playing football within the Standard Platform League. The user inferface, although intuitive, is not too easy to operate
initially. This document tries to simplify the first approach to the ChaosManager project,
not just for users, but for programmers also. Next sections will show the main features
of the application from the user and the programmer point of view.
Next sections will guide the user through the application.
A.2
Vision
This part is used for all tasks regarding vision. The screen is divided into three different
frames:
Video
Control and Monitoring
Calibration
There are two different ways of operation: online and offline. To operate online, there
must be a connection with a dog (select the dog to connect to and press the Disconected
button). When operating online, the application can display what the robot sees by activating the radio button Get Video. Pictures can be taken and saved. When operating
offline there is no connection to any dog and single pictures or a whole directory can be
loaded into the application from the hard drive.
The calibration section can be divided into four different parts:
99
ChaosManager Tools
A.2.1
Color Calibration
Color calibration is where the segmentation process (thresholding) of the artificial vision
module of the robot can be configured. Next figure shows this screen.
Figure A.1: Color Calibration

The color table shows the 9 different colors and its properties. This table can be
loaded from a file, modified and saved (to the same file or to a different one, in case it
is not desired to overwrite it). Activating the Segment check box in a color, indicates
that that color will be used in the segmentation process. By pressing the new seeds to a
specific color, erases all the seeds associated to that color, whereas by pressing the Reset
seeds button, all seeds are eliminated from all colors.
The segmentation process performed by the vision module is based on color discrimination, so the way to tune this process consists in identifying the color of every object
(channel) that must be recognized later. To identify the color of a channel, that channel
must be selected and new seeds can be added by placing the mouse on the pixels of the
image that corresponds to that desired color and clicking on them. The zoom window
at the bottom of the screen facilitates the precess of identifying which pixels have to be
included. The Undo and Redo buttons are also useful to go back or forward a step.
To activate this seed determination process, the View seeds checkbox must be activated
and depending whether the Add seed box or Remove seed box is activated, new seeds
are added or eliminated to the channel. Selected seeds are showed in the image in black
color. If a threshold collision between more than one channel occurs, a warning window
will appear containing a list of all colliding channels (press Undo to eliminate the collision). The RG Threshold text area of the channel list is used to set the region growing
factor to be applied to each channel. Update button must be pressed to update each
change of the channel list. Once the segmentation configuration is well enough, it can
be sent to the robot by pressing the Send button of the Action panel. To make this
configuration permanent on the robot, Save button is to be pressed. The application
allows to retrieve the current segmentation configuration of the reobot by pressing the
100
ChaosManager Tools
Receive button. Figure A.2 shows two pictures before and after some seeds have been
added to the orange channel.
Figure A.2: Segmentation progress on orange channel
A.2.2
General Configuration
The image update delay can be chosen. This screen allows also the teleoperation of the
robot. If the Teleoperate button is activated, the dog can be moved (or just the head,
depending on whether the Control Head is activated or not) by using the pad of the
joystick part. Buttons from 1 to 8 are used for kicks.
Figure A.3: General configuration
A.3
Game Monitor
This part is used to monitor the position of the dog while playing. The tool catches the
broadcast messages sent by the dogs with their own location and the position of the ball.
The application just show this information on the field. By activating any of the check
boxes on the top of the screen, shome information about the received packages is shown,
101
ChaosManager Tools
and activating the Debug button of any player, an individual field for that specific
player showing its position. Next figure shows a screeshot of this tool.
Figure A.4: Game Monitor
A.4
Game Controller
A Robocup match consists of two game parts, each one of ten minutes. During the game
there is some situations that the robots must be aware of. At beginning of each part and
after goals, the robot must go to an initial position. This position depends on which team
has the kick-off. A penalized robot must stop for some time and play again when that
penalized period is finished.
If wireless is available, the game information is set by an application called Game
Controller. This application sends UDP packets with the information needed by the
robot. If availability of the wireless cannot be guaranteed, the robot must receive the
game information manually, pressing in the robot sensors.
Robots can be in six different states A.5. The Game Controller (or a referee by pressing
the head sensor) must inform to the robots about the game state. These states are:
Initial After booting, the robots are in their initial state. In this state the team color
and whether the team has kick-off is configured. The button interface to manually
set these values is active. The robots are not allowed to move in this state.
Ready In this state, the robots walk to their legal kick-off position. This state finished
when all the robot are in their legal positions or when the referee decides that some
robots cannot reach their position.
Set In this state the robot which are not reached the legal position are manually allocated.
The robots are stopped in his state and they are waiting for the kick-off.
102
ChaosManager Tools
Figure A.5: Robot states during the match

Play The robots play soccer.
Penalized If a robot is penalized by a referee, the Game Controller informs to the
penalized robot of this situation, or if the wireless is not available, the referee will
touch the back sensor of the robot for more than one second. Then, the robot stops
until it is unpenalized.
Finished The half is finished.
The robots must have implemented a module for accepting the orders sent by the
Game Controller and the button interface. In this chapter, we will explain how the
TeamChaos architecture is able to receive the orders sent by the Game Controller or
the button interface, and how this information is used by the others components of the
architecture.
103
ChaosManager Tools
104
HFSM Builder
Appendix B
HFSM Builder
B.1
Using the Application
To execute the application, just execute ChaosManager, go to the tab Player Tools and
at the right top hand you have FSMBuilder button, which executes the application. The
main window is shown in Figure B.1. First, you can choose between open an existing
project or create a new one. If the new one option is selected, a place to save it is
required. Important: as norm, always begin the project name with capital letter. If you
choose to open a existing project, you have to enter in the corresponding directory and
open the XML file with .xas extension. It always stores the automata in ./conf/hfsm.
Figure B.1: Main window of the HFSM.

There are still some missing or unfinished things:
105
HFSM Builder
The state tracer to show graphical information about the current state of the robot.
A contextual menu for the private variables (showing the current variables defined).
May be interesting to attach some help (a phrase) about the variable.
A contextual menu with the available low-level behaviours.
B.1.1
Buttons and Menus
Several options are available in the application (see Figure B.2). The different menus are:
File Opens, renames, saves, saves as and exits the application.
Generation Verifies if the automata is correct and generates the code for the TeamChaos.
Add Adds new states, metastates, transitions, and manages private variables.
View Shows or hides the navigation tree and the tool bar.
Help Help and About.
Figure B.2: Buttons and menus of the application.

In the tool bar (just below menus) we have several buttons. From left to right:
Opens a project.
Saves the current project.
Adds a new state. Just clicking in any place in the canvas, it puts the state.
Adds a new metastate. Same as state.
Adds a extern metastate. Just click in a place in the canvas and a file chooser is
open, which allow us to select (just select, not included) a extern metastate (from
another project).
Adds a new transition. First, click in the initial state, then the final state and finally
the point where to place the transition.
106
HFSM Builder
Expands all the elements in the canvas.

Minimizes all the elements in the canvas (see Figure B.3).
Figure B.3: Minimization of all the elements in the canvas.

Every metastate can be opened in a new tab. All these tabs can be open and close,
except the main automata, the first level one.
Important: the names of the states (metastates and so on) and transitions must be
a valid name in LUA. When we are changing the name to a state (for example) we are
not able (they dont appear) to introduce invalid characters. To change the name to a
element, we have to write the name and click in the OK button. Every element has several
buttons at the right bottom hand. Here you have an explanation of each one:
Last one, X, is used to delete the element. When an element is deleted all the
transitions from or to this element are deleted too. When an element is deleted,
just the element is deleted.
The first button in a state changes the state to metastate. The second one opens an
editor to introduce the code to execute when the robot is in this estate. The code
is just the code: do not write a LUA function (see Figure B.4).
In the transition case, the first button is used to modify the test code of the transition. This code is executed each time the robot is placed in a state which is the
initial state of the transition. The test code always must return a boolean value.
The second button is the code of the transition. This code is executed when the
test is applied.
In the metastate case, we have a button to open it, to create or modify the content
of the metastate. The extern metastate does not allow to modify it. To do it just
open it. Very important: cycles are not checked.
The last thing is the private variables. Select the Private variables option in the Add
menu and a new window will appear like the one shown in Figure B.5. We can add new
variables or modify existing ones. A private variable can be used in every place in the
automata: any state or transition can use it inside its code. When a new variable is
created, a new row appears with the following elements::
107
HFSM Builder
Figure B.4: Example of code in a state. Editor used when the code button is clicked.
Name: Name of the variable. As before, it must be a LUA name.
Type: Type of the variable (int, float, etc.). Not checking is done.
NumberElem: Number of elements in the variable. 1 if it is a variable, more if it is
an array.
InitialValue: Initial value (not required). In case of an array, this field is not used.
Figure B.5: Private variables.
B.1.2
Code Generation
The automata must be well defined in order to generate code LUA. In the Generation
menu we have several options related with code generation. The first options (Verify and
Verify all ) check if the automata is correct. An automata is not correct if:
It has an isolated state (no transition from or to this state).
There is no initial state.
Some element names are repeated.
108
HFSM Builder
B.2
Building a Sample Automata
We are now going to describe, step by step, the process to build an automata. Itll
consist of a state, a metastate (with just one state) and transitions from the state and the
metastate. Start the application and select the new project option. Give a name (e.g.,
MyRol) to the automata and save it in any place.
Figure B.6: Creation of a new automata.
Now, we are going to create the automata. Select New State option (from the tool
bar or Add menu) and click where you want to place the state. Do the same for the
metastate. For the transition, select New Transition, click in the state (itll be the initial
state of the transition), then click in the metastate (the final state) and click in a place
for the transition. Add a new transition from the metastate to the state. Change the
name to all the elements. The automata will be something similar to the one shown at
Figure B.7.
Modify the code of each state and transition, clicking in the corresponding button of
each element. Now we are going to define the content of the metastate. Click in the
button of the metastate and a new tab will be shown. Insert a new state and change its
name. Finally, generate code and you will be able to compile at TeamChaos.
109
HFSM Builder
Figure B.7: Definition of an automata.
B.3
B.3.1
Implementation
File Format
A project is stored in a directory, with the name of the project. Inside this directory
we have a file with .xas extension. This file is a XML file, which contains the automata
definition. There are several .cc files with the code from the states and transitions. This
is an example of a file containing an automata:
<strategy stateCount=29 transitionCount=20 metaStateCountName=6
stateCountName=3 transitionCountName=8 >
<metastate name=Center id=1 x=675 y=262 metaStateCountName=1
stateCountName=3 transitionCountName=3 initial=1 extern=0 >
<state name=KeepInArea id=24 x=77 y=129 initial=1 > </state>
<state name=Face id=28 x=594 y=241 initial=0 > </state>
<transition name=BallCloseAndInArea id=18 x=260 y=374 priority=1
to=28 from=24 ></transition>
</metastate> <metastate name=Localized id=3 x=104 y=33 metaStateCountName=4
stateCountName=3 transitionCountName=1 initial=0 extern=0 >
<state name=Localize id=13 x=143 y=89 initial=1 > </state>
</metastate>
<transition name=BallVisMyArea id=7 x=420 y=342 priority=1
to=2 from=1></transition>
<transition name=NotLocDL id=4 x=206 y=236 priority=1 to=3 from=2>
</transition>
<transition name=LocBallNotVisOtherAreaDC id=5 x=440 y=503 priority=2
to=1 from=2></transition>
</strategy>
110
HFSM Builder
In the file, information about states, metastates and transitions are stored. Metastates can
contain more states, transitions and metastates. For each element, its name, identifier (not
duplicated and used to identify the .cc file) and its position in the canvas are stored. Several
aditional information is stored: from which to which state is defined a transition, a states
indicates if it is initial state, a metastate if it is extern.
B.3.2
Class Diagrams
The HFSM application is divided in several packages:

classes Contains the main classes of the application (see Figure B.8). There are three classes:
State, MetaState and Transition. State implements a state and MetaState inherits from
State. All three implement the basic operations for managing the elements.
editor This package implements an editor for the code in the elements.
help Open a navigator window with the help.
xml Class for read and write XML files.
generator Classes used for generating code (see Figure B.9).
gui Graphical User Interface. The most complex package (see Figure B.10). It contains all the
classes for the graphical interface.
111
HFSM Builder
Figure B.8: Class diagram of Classes package.
112
HFSM Builder
Figure B.9: Class diagram of Generator package.
113
HFSM Builder
Figure B.10: Class diagram of Gui package.
114
Bibliography
Bibliography
[1] R. Adams and L. Bischof. Seeded region growing. IEEE Trans. on Pattern Analysis and
Machine Intelligence, 16:641647, 1994.
[2] R.C. Arkin. The impact of cybernetics on the design of a mobile robot system: a case
study. IEEE T. on Sys., Man, and Cybernetics, 20(6):12451257, 1990.
[3] R. Bajcsy. Active perception. Proceedings of the IEEE, 76(8):9661005, 1988.
[4] D.H. Ballard. Animate vision. Artificial Intelligence, 48:5787, 1991.
[5] I. Bloch and H. Matre. Fuzzy mathematical morphologies: a comparative study. Pattern
Recognition, 28(9):13411387, 1995.
[6] I. Bloch and A. Saffiotti. Why robots should use fuzzy mathematical morphology. In Proc.
of the 1st Int. ICSC-NAISO Congress on Neuro-Fuzzy Technologies, La Havana, Cuba,
2002.
[7] R.A. Brooks. A layered intelligent control system for a mobile robot. IEEE T. Robotics
and Automation, 2:1423, 1986.
[8] J. Bruce, T. Balch, and M. Veloso. Fast and inexpensve color image segmentation for
interactive robots. In Proc. of the IEEE/RSJ Int. Conf. on Intelligent Robots and Systems
(IROS), volume 3, pages 20612066, 2000.
[9] W. Burgard, D. Fox, D. Hennig, and T. Schmidt. Estimating the absolute position of
a mobile robot using position probability grids. In Proc. of the National Conference on
Artificial Intelligence, 1996.
[10] P. Buschka, A. Saffiotti, and Z. Wasik. Fuzzy landmark-based localization for a legged
robot. In Proc. of the IEEE/RSJ Int. Conf. on Intelligent Robots and Systems (IROS),
pages 12051210, Takamatsu, Japan, 2000.
[11] J.M. Cameron, D.C. MacKenzie, K.R. Ward, R.C. Arkin, and W.J. Book. Reactive control
for mobile manipulation. In Proc. IEEE Int. Conf. on Robotics and Automation, pages
228235, 1993.
[12] S. Coradeschi and A. Saffiotti. Anchoring symbols to vision data by fuzzy logic. In A. Hunter
and S. Parsons, editors, Proc. of the ECSQARU99 Conf. LNCS 1638, pages 104115,
Berlin, 1999. Springer-Verlag.
[13] J.L. Crowley and H.I. Christensen, editors. Vision as Process. Springer-Verlag, 1995.
115
Bibliography
[14] J. Fan, D. Yau, A. Elmagarmid, and W. Aref. Automatic image segmentation by integrating
color-edge extraction and seeded region growing. IEEE Trans. on Image Processing, 10(10),
2001.
[15] A. Gelb, J.F. Kasper, R.A. Nash, C.F. Price, and A.A. Sutherland. Applied Optimal
Estimation. MIT Press, 1974.
[16] B. Henst, D. Ibbotson, S. Pham, and C. Sammut. The UNSW united 2000 sony legged robot
software system. In RoboCup 2000: Robot Soccer World Cup IV, pages 6475. SpringerVerlag, 2001.
[17] D. Herrero-Perez, H. Martnez-Barbera, and A. Saffiotti. Fuzzy self-localization using natural features in the four-legged league. In Proc. of the Int. RoboCup Symposium, Lisbon,
Portugal, 2004.
[18] R. Howarth. Interpreting a dynamic and uncertain world: task-based control. Artificial
Intelligence, 100:585, 1998.
[19] V. Hugel. Les Trois Mousquetaires. http://www.lrv.uvsq.fr/research/legged/robocup.php.
[20] V. Hugel, G. Amouroux, T. Costis, P. Bonnin, and P. Blazevic. Specifications and design
of graphical interface for hierarchical finite state machines. In Proc. of the Int. RoboCup
Symposium, Osaka, Japan, 2005.
[21] N. Ikonomakis, K. Plataniotis, and A. Venetsanopoulos. Unsupervised seed determination
for a region-based color image segmentation scheme. In IEEE Int. Conf. Image Processing,
pages 537540, 2000.
[22] C.S. Kim, W.H. Seo, S.H. Han, and O. Khatib. Fuzzy logic control of a robot manipulator based on visual servoing. In Proc. IEEE Int. Symp. on Industrial Electronics, pages
15971602, 2001.
[23] P.J.A. Lever, F-Y. Wang, and D. Chen. A fuzzy control system for an automated mining
excavator. In Proc. IEEE Int. Conf. on Robotics and Automation, pages 32843289, 1994.
[24] C. Malcom and T. Smithers. Symbol grounding via a hybrid architecture in an autonomous
assembly system. In P. Maes, editor, Designing autonomous agents, pages 123144. MIT
Press, 1990.
[25] H.
Martnez-Barber
a
and
A.
Saffiotti.
http://ants.dif.um.es/humberto/robots/tc2/.
ThinkingCap-II
Architecture.
[26] H. Martnez-Barber
a and A. Saffiotti. On the strategies of a 4-legged goal-keeper for
RoboCup. In Proc. of the 6th Int. Conf. on Climbing and Walking Robots (WLAWAR03), pages 745752, Catania, Italy, 2003.
[27] P.L. Palmer, H. Dabis, and J. Kittler. Performance measure for boundary detection algorithms. Computer Vision and Image Understanding, 63(3):476494, 1996.
[28] E. Ruspini, P. Bonissone, and W. Pedrycz, editors. IEEE Handbook of Fuzzy Computation.
Oxford Univ. and IOP Press, 1998.
[29] A. Saffiotti. Pick-up what? In C. Backstrom and E. Sandewall, editors, Current Trends in
AI Planning, pages 166177. IOS Press, 1994.
116
Bibliography
[30] A. Saffiotti. Fuzzy logic in autonomous robots: behavior coordination. In Proc. of the 6th
IEEE Int. Conf. on Fuzzy Systems, pages 573578, 1997.
[31] A. Saffiotti. Using fuzzy logic in autonomous robot navigation. Soft Computing, 1(4):180
197, 1997.
[32] A. Saffiotti, K. Konolige, and E.H. Ruspini. Blending reactivity and goal-directedness in
a fuzzy controller. In Proc. of the 2nd IEEE Int. Conf. on Fuzzy Systems, pages 134139,
San Francisco, USA, 1993.
[33] A. Saffiotti, K. Konolige, and E.H. Ruspini. A multivalued-logic approach to integrating
planning and control. Artificial Intelligence, 76(1-2):481526, 1995.
[34] A. Saffiotti and K. LeBlanc. Active perceptual anchoring of robot behavior in a dynamic
environment. In Proc. of the IEEE Int. Conf. on Robotics and Automation (ICRA), pages
37963802, San Francisco, USA, 2000.
[35] A. Saffiotti, E.H. Ruspini, and K. Konolige. Blending reactivity and goal-directedness in
a fuzzy controller. In Proc. of the 2nd IEEE Int. Conf. on Fuzzy Systems, pages 134139,
1993.
[36] A. Saffiotti and Z. Wasik. Using hierarchical fuzzy behaviors in the robocup domain. In
Zhou, Maravall, and Ruan, editors, Autonomous Robotic Systems, pages 235262. SpringerVerlag, 2002.
[37] A. Saffiotti and L.P. Wesley. Perception-based self-localization using fuzzy locations. In
Proc. of the 1st Workshop on Reasoning with Uncertainty in Robotics, pages 368385,
Amsterdam, NL, 1996.
[38] P.K. Sahoo, S. Soltani, A.K.C. Wong, and Y.C. Chen. A survey of thresholding techniques.
Computer Vision, Graphics, and Image Processing, 41(2):233260, 1988.
[39] S.M. Smith and J.M. Brady. SUSAN - a new approach to low level image processing.
International Journal of Computer Vision, 1(23):4578, 1997.
[40] E. Sojka. A new and efficient algorithm for detecting the corners in digital images. In Proc.
24th DAGM Symposium, pages 125132, Berlin, 2002. Springer-Verlag.
[41] M. Sonka, V. Hlavac, and R. Boyle. Image Processing Analysis and Machine Vision. International Thomson Computer Press, 1996.
[42] Sony. Sony AIBO robots. http://www.aibo.com.
[43] L. Vincent and P. Soille. Watersheds in digital spaces: an efficient algorithm based on
immersion simulations. IEEE T. Pattern Analysis and Machine Intelligence, 13(6):583
598, 1991.
[44] Z. Wasik and A. Saffiotti. Robust color segmentation for the robocup domain. In Proc. of
the Int. Conf. on Pattern Recognition (ICPR), Quebec, Canada, 2002.
[45] D.A. Wheeler. Counting source lines of code (SLOC). http://www.dwheeler.com/sloccount.
[46] L.A. Zadeh. Fuzzy sets. Information and Control, 8:338353, 1965.
[47] L.A. Zadeh. Fuzzy sets as a basis for a theory of possibility. Fuzzy Sets and Systems, 1:328,
1978.
117

2009 Localization

Uploaded by

Document Information

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

2009 Localization

Uploaded by

Copyright:

Available Formats

R OBO C UP 2009

T EAM C HAOS D OCUMENTATION

Humberto Martnez Barbera

David Herrero Perez

Carlos III University

Department of Information and

Department of Systems Engineering

Figure 1.1: The variant of the ThinkingCap architecture.

Figure 1.2: Implementation of the ThinkingCap architecture.

Figure 1.3: The stages of the booting process

Main broker modules

Figure 1.4: Maximum performance configuration.

Figure 1.5: Remote debugging configuration.

Figure 1.6: RobotManager adapts ThinkingCap architecture to NaoQi modules.

Timing of periodic functions in OpenNao

Figure 1.7: Supercycle of global execution. Every row is a locomotion cycle.

robots and ChaosManager.

Figure 1.8: Associations in the communication system.

Table 1.1: Types of messages.

Table 1.2: Messages format.

Figure 1.9: TCP messages reception

Information received by the locomotion module

Information provided by the locomotion module

Frontal right leg kick

Lateral right leg kick

Figure 2.2: Relations between classes

Description of the robot Nao

Figure 2.3: Dimensions of the robot.

Name of kinematic chain

Table 2.1: Name and range of joints.

Part of the body

Table 2.2: Mass of the parts of the robot.

Table 2.3: Dimensions of the links of the robot.

Figure 2.4: Fitting algorithm

Figure 2.5: Fitting schema

Figure 2.6: Locomotion state machine

Foot trajectory in sagital plane

Vertical foot trajectory

Frontal foot trajectory

Figure 2.8: Vertical and horizontal trajectories.

We will not move the arms of the robot

sLegX M L + f LegX M L + f F ootX M F + chestX M C

sLegY M L + f LegY M L + f F ootY M F + chestY M C

sLegZ M L + f LegZ M L + f F ootZ M F + chestZ M C

yCOM M T (yaf s + cos( hipRot

zCOM M T (zaf s sin( hipRot

yaf s M F cos( hipRot

zaf s M F + sin( hipRot

Direct kinematic problem

cos q cos sin q sin sin q a cos q

Left leg chain

Table 2.4: DH Parameters of the robot.

Figure 2.14: Placement of the coordinate systems in the robot.

Table 2.5: Chains transformations.

Figure 2.15: Names and senses of the joints

legP itch = arctan(

haDist = xha2 + yha2 + zha2

2. Rotate a2 around x2 an angle .

3. Calculate the coordinates of b2 in R1.

4. Rotate b1 around the z1 axis an angle .

k (cos (1 + cos ) sin (1 cos ))

5. Find the value of that anulates the y component of c1.

the y2 axis an angle .

7. Find the value of that anulates the z component of d1.