Sensor fusion in humanoid robots

(1)

Universidade de Aveiro Departamento deElectrónica, Telecomunica¸cões e Informática, 2012

Carlos Eduardo dos

Reis Rodrigues

Sobrinho

Fus˜

ao Sensorial em Robots Human´

oides

Sensor Fusion in Humanoid Robots

(2)

(3)

Universidade de Aveiro Departamento deElectrónica, Telecomunica¸cões e Informática, 2012

Carlos Eduardo dos

Reis Rodrigues

Sobrinho

Sensor Fusion in Humanoid Robots

Disserta¸cão apresentada à Universidade de Aveiro para cumprimento dos req-uisitos necessários à obten¸cão do grau de Mestre em Engenharia Electrónica e Telecomunica¸cões, realizada sob a orienta¸cão cient´ıfica de Professor Doutor João Manuel de Oliveira e Silva Rodrigues e do Professor Doutor José Nuno Panelas Nunes Lau, professores auxiliares do Departamento de Electrónica, Telecomunica¸cões e Informática da Universidade de Aveiro

(4)

(5)

o j´uri / the jury

presidente / president Professor Doutor Lu´ıs Filipe de Seabra Lopes

Professor Associado da Universidade de Aveiro

vogais / examiners committee Professor Doutor Lu´ıs Paulo Gon¸calves dos Reis

Professor Associado da Escola de Engenharia da Universidade do Minho

Professor Doutor Jo˜ao Manuel de Oliveira e Silva Rodrigues

Professor Auxiliar da Universidade de Aveiro (orientador)

Professor Doutor Jos´e Nuno Panelas Nunes Lau

(6)

(7)

agradecimentos / acknowledgements

AGRADECIMENTOS

`

A minha m˜ae e fam´ılia mais chegada pelo apoio que me deram durante estes longos anos.

Aos meus amigos mais chegados que fizeram “trinta por uma linha” para que eu acabasse o curso contribuindo com ideas ou simplesmente uma dose diária de boa disposi¸cão e motiva¸cão.

Aos meus orientadores por acreditarem em mim e me ajudarem a perceber que o pr´ıncipio do fim era “agora” e n˜ao num “dia futuro”.

Agora ´e andar para a frente com um sentimento de dever cumprido. Um bem haja para todos!

(8)

(9)

Resumo A fusão sensorial combina pe¸cas de informa¸cão proveniente de diferentes fontes/sensores de modo a obter informa¸cão global mais precisa quando com-parada com sistemas que apenas dependem de fontes/sensores. Diferentes métodos de fusão sensorial têm sido desenvolvidos de forma a optimizar a resposta geral dos sistemas. Resultados finais, como a unidade inercial que funde duas fam´ılias diferentes de sensores para dar uma estimativa mais precisa/melhor dos dados sensoriais ou a auto-localiza¸cão do robot que deve ser capaz de avaliar a sua própria posi¸cão e consequentemente a posi¸cão dos membros da sua equipa são exemplos da fusão sensorial. Esta tese irá descrever detalhadamente, desde a fase de algoritmo até à implementa¸cão juntamente com algumas bases matemáticas necessárias para a compreensão dos conceitos introduzidos, todo o trabalho desenvolvido para a equipa por-tuguesa que serviu para tornar o objectivo proposto em realidade: participar pela primeira vez na categoria Standard Platform League no RoboCup 2012.

(10)

(11)

Abstract The technology of sensor fusion combines pieces of information coming from different sources/sensors, resulting in an enhanced overall information accuracy when compared with systems that rely only on sources/sensors. Different sensor fusion methods have been developed in order to optimize the overall system output. End results like the inertial unit that fuses two different sensor families to give a more accurate/better estimate of the sensory data or the self-localization of the robot that should be able to evaluate its position and consequently its team members position. A walk-through, from the algorithm phase to the implementation, will be given in this thesis along with some mathematical background necessary to comprehend the concepts introduced and description of the auxiliary tools that were built for the Portuguese Team to help accomplish the objective: First presence in the Standard Platform League in the RoboCup 2012.

(12)

(13)

List of Figures

2.1 Different body types of the NAO robot sold by Aldebaran Robotics . . . 6 2.2 NAO T21 links and its corresponding lengths. . . 8 2.3 Positioning the rotated coordinate frame(x′, y′, z′) using Euler angles (α, β, γ). 12 2.4 Frame convention and Denavit and Hartenberg (DH) parameters. Source: [Sah08]. 13 2.5 NAO T21 head joints and operational range. . . 16 2.6 NAO T21 arm joints and operational range. . . 17 2.7 NAO T21 pelvis, left and right leg joints and operational range. . . 18 2.8 Various definitions of sensor terminology. Source (a)-(d): J.J. Carr, Sensors

and Circuits Prentice Hall. . . 23 2.9 Continuation of definitions of sensor terminology. Hysteresis curve. Source: J.J.

Carr, Sensors and Circuits Prentice Hall. . . 24 2.10 Some examples of sensor readings. (a) is a linear sensor, (b) a linear sensor

with two different linear regions and (c) a non-continuous and non-linear sensor. 24 2.11 Using a simple mathematical approximation to find the transfer function of (a)

an almost linear and (b) an exponential transfer function sensor. . . 25 2.12 Sigmoid - Example of a non-linear transfer function . . . 26 2.13 Example of the principle and actual implementation of a 3D MEMS gyroscope. 26 2.14 Example of the principle of the accelerometer mass-spring system and the

mass-sprig deflection when under acceleration and rest. . . 28 2.15 Calculated angle (bottom panel) and angular velocity (middle panel) obtained

by numerical integration of the measured acceleration of the segment (top panel). The dashed line should remain approximately constant however, due to the integration drift, this line has a parabolic shape. Retrieved from [DJJP11]. 29 2.16 Simplified geometry and mathematics of the pinhole camera . . . 31 2.17 Camera snapshots from the NAO top camera. . . 33 2.18 NAO T21 v3.x and v4.0 camera position and orientation. . . 33 2.19 Hall Effect illustration. The forces on the charge carriers in a conductor inside

(18)

2.20 (a) Ultra-sound sensor echo representation and (b) ultra-sound sensor from Parallax. . . 37 2.21 Three typical measurement scenarios. (a) will generate a precise measurement

since the sensor is opposite and perpendicular to the obstacle. (b) will also give a precise measurement but the sensor will not be able to detect the difference between the wall and the obstacle. (c) will generate an inaccurate measurement since the left part of the sensor will be the one to detect the obstacle. Source: http://www.generationrobots.com. . . 38 2.22 Some limitations of ultrasound sensors. It is easy to understand why the shape

of the object to detect plays an essential role and might lead to wrong measures. Source: http://www.generationrobots.com. . . 38 2.23 Typical shape for an ultrasonic beam. It largely depends of the characteristics

of the sensor used. Source: http://www.generationrobots.com. . . 38 2.24 (a) Use of a acrylic reflector to improve the distance and sensitivity of ultra-sound

sensor. (b) The cylindrical parabolic curve works by focusing the transmitted acoustic beam in one dimension while letting it continue to diverge in the other and thus improving the distance and sensitivity [LLC11]. . . 39 2.25 Plot of force vs (a) resistance and (b) conductance for a typical Force-Sensing

Resistor (FSR) sensor. In (b) it is easy to see that after the break-point the output is approximately linear with force. Source: [SP96]. . . 40 2.26 (a) Schematic view of a detail over the lens, CCD and focus point from the

perspective system being (a) the left side view and (b) the right top-down view. Source (a)-(b): [JRNMP09]. . . 45 2.27 (a) robot and its perspective system schematic side view, (b) schematic top-down

view of the robot and perspective system. Source (a)-(b): [JRNMP09]. . . 47 3.1 (a) Illustration of Kalman filters: (a) initial belief, (b) a measurement (in bold)

with the associated uncertainty, (c) belief after integrating the measurement into the belief using the Kalman filter algorithm, (d) belief after motion to the right (which introduces uncertainty), (e) a new measurement with associated uncertainty, and (f) the resulting belief. Source (a)-(f): [TBF05]. . . 53 3.2 Illustration of a particle filter. This example shows the ability of particle filters

to represent the ambiguities occurring during global robot localization. The images show a series of sample sets (projected into 2D) generated during global localization using a robot’s ring of 24 sonar sensors. The samples are shown in red and the sensor readings are plotted in blue. Notice that the robot is drawn at the estimated position, which is not the correct one in the beginning of the experiment. Source (a)-(i): [?]. . . 54 3.3 Illustration of a LUT map used by the algorithm. Darker areas indicate

positions where the distance to the nearest line is large and brighter areas indicate positions with smaller distances. The white lines represent the lines of the RoboCup field that are defined in the rules and know a priori. . . 55 4.1 Time vs Total Cost when developing with and without using unit tests . . . . 58

(19)

4.2 UML diagram of aFooBar class that has an external dependency of a database connection. Italic methods represent virtual methods that can be overwritten. 61 4.3 Test-driven development life-cycle flowchart . . . 66 4.4 Pre and Post development life-cycle with and without unit testing . . . 67 4.5 UML diagram of classesDcmController, AgentModel, Location with their

cor-responding test and real relationships. Italic methods represent virtual methods that can be overwritten. . . 68 5.1 Initial screen of the NaoSensor application. The simulated NAO robot on the

right of (a) and (b) was updated in real-time with the sensor values from a physical robot connected to IP address 10.10.10.1, in this case NAO1. . . 78 5.2 Camera tab showing the bottom camera preview in VGA, RGB colorspace and

5 frames per second from a physical robot connected to IP address 10.10.10.1, in this case NAO1. . . 80 A.1 Screenshots from the bottom camera with different ball positions. Used in the

DistanceMapConverter unit test . . . 90 A.2 Screenshots from the top camera with different ball positions. Used in the

(20)

(21)

List of Tables

2.1 Detailed specifications of the NAO T21 v3.3 robot . . . 7 2.2 NAO T21 Masses of link/joints along with its corresponding Center of Mass. . 19 2.3 Specifications of the camera model OV7670 . . . 34 2.4 Specifications of the joint position sensors used in the NAO robot. . . 36 2.5 Main advantages and disadvantages of the various types of range finding sensors. 37 2.6 NAO ultrasound specifications . . . 39 4.1 List of all the tests for the added or modified classes. . . 72

(22)

(23)

Listings

2.1 List of ALMemory command key names for the Head joints actuators (in radian) 16 2.2 List of ALMemory value key names for the Head joints sensors (in radian) . . 16 2.3 List of ALMemory command key names for the Left and Right arms joints

actuators (in radian) . . . 17 2.4 List of ALMemory value key names for the Left and Right arms joints sensors

(in radian) . . . 17 2.5 List of ALMemory command key names for the Left and Right legs joints

actuators (in radian) . . . 18 2.6 List of ALMemory value key names for the Left and Right legs joints sensors

(in radian) . . . 19 2.7 List of ALMemory value key names for the Gyroscope sensors (in rad/s) . . . 27 2.8 List of ALMemory value key names for the Filtered Inertial Unit sensors (in rad) 30 2.9 List of ALMemory value key names for the Accelerometer sensors (in m/s2) . 30 2.10 Torso to Vertical transformation from the AgentModel class . . . 41 2.11 Head to Torso transformation from the AgentModel class . . . 42 2.12 Head to Vertical transformation from the AgentModel class . . . 42 2.13 Camera to Vertical transformation from the AgentModel class . . . 43 2.14 Estimated Height from the head to the ground function from the AgentModel

class . . . 43 2.15 Pixel position relative to the middle of the CCD from the DistanceMapConverter

class . . . 45 2.16 relative position on the ground from a camera pixel from the

DistanceMapCon-verter class . . . 48 4.1 Example of a factorial and prime numerical test in googletest . . . 62 4.2 Example of a simple test in boost test . . . 63 4.3 Example of a simple mock in googlemock . . . 64 4.4 Command examples to run all tests . . . 66 5.1 Example of the logging file output with all the joints selected. . . 81 B.1 Execution log of the unit tests with the pass/fail and execution time. . . 92 B.2 nao/fcpagent/tests/physics/AgentModelTest.cpp . . . 94 B.3 nao/config/nao.xml . . . 98 B.4 nao/fcpagent/tests/world/WorldStateTest.cpp . . . 103 B.5 nao/fcpagent/tests/world/drivers/camera/OV7670Test.cpp . . . 105 B.6 nao/fcpagent/tests/world/DistanceMapTest.cpp . . . 106 B.7 nao/config/XYZcam.conf . . . 109

(24)

B.9 nao/fcpagent/tests/world/localizers/tribots/FieldLUTTest.cpp . . . 111 B.10 nao/fcpagent/tests/world/localizers/tribots/ReferentialTest.cpp . . . 111 B.11 nao/fcpagent/tests/world/localizers/tribots/TribotsLocalizerTest.cpp . . . 112 B.12 nao/config/field.xml . . . 114 B.13 nao/fcpagent/tests/utils/SingletonTest.cpp . . . 115

(25)

Chapter 1

Introduction

Robotics is the science of perceiving and manipulating the physical world through computer-controlled mechanical devices. Examples of successful robotic systems include mobile platforms for planetary exploration, robotics arms in assembly lines, cars that travel autonomously on highways, actuated arms that assist surgeons, and the list goes on. Robotics systems have in common that they are are situated in the physical world, perceive their environments through sensors, and manipulate their environment through things that move.

This thesis will focus on four key elements. Two in the area of robotics: Sensor Fusion and Uncertainty and two in the area of software development: Best Practices and Tools.

Sensor Fusion is the combination of sensor data or data derived from the sensor data from different sources in a way that the resulting information could be considered more accurate, complete or even more dependable than when this sources are used individually. A data source can come from identical sensors or from totally different sensors [Elm02]. Sensor Fusion primary concern is to address the problem of integrating information across space and time. Sensor data normally is contaminated by different types of noise that may need to be dealt with. Another common requirement is the integration of data from multiple sensors or across time to provide robust and accurate estimates.

One can divide sensor fusion into three different categories: direct fusion, indirect fusion or a combination of the both. Direct fusion is the fusion of sensor data from a set of similar or different sensors families, soft sensors that aggregate different sensor values into one or from a history backlog of sensor data values.

Sensor fusion involves a wide spectrum of areas, ranging from hardware for sensors and data acquisition, through analogue and digital processing of the data, up to the symbolic analysis all within a theoretical framework that solves some class of problem [HDKG98].

Multiple sensors in a control system can be used to provide:

∎ More Information - the sensors are used to monitor wider aspects of a system; this may mean over a wider geographical area or diverse aspects of the system.

(26)

sensor exists.

∎ Complementary Information - allows a number of benefits (high degree of accuracy, extended coverage, systematic errors estimation and correction, etc), and brings new challenges for the fusion process in order to guarantee an improvement with respect to any of those sensor techniques used alone as well as to increase the reliability of interpretation.

Important applications exist in logistics, advanced driver assistance systems, medical care, public security, defence, aerospace, robotics, industrial production, precision agriculture, traffic monitoring.

Uncertainty can happen from five different factors [TBF05]:

∎ Environments - Physical world is inherently unpredictable. While the degree of uncertainty in well-structured environments such as assembly lines is small, environments such as highways, private homes or even a RoboCup field are highly dynamic and unpredictable.

∎ Sensors - Sensors are inherently limited in what they can perceive. Limitations arise from two primary factors. First, range and resolution of a sensor is subject to physical laws. Second, sensors are subject to noise, which perturbs sensor measurements in unpredictable ways and hence limits the information that can be extracted from sensor measurements.

∎ Robots - Robot actuation involves motors that are, at least to some extent, unpre-dictable, due to effects like control noise and wear-and-tear.

∎ Models - Models are inherently inaccurate. Models are abstractions of the real world. As such, they only partially model the underlying physical processes of the robot and its environment.

∎ Computation - Robots are real-time systems, which limits the amount of computation that can be carried out.

All of these factors give rise to uncertainty. Traditionally, such uncertainty has mostly been ignored in robotics. However, as robots are moving away from factory floors into increasingly unstructured environments, the ability to cope with uncertainty is critical for building successful robots.

Development Tools turned out to be more a necessity than a requirement from this thesis initial objective. Being able to code and test ideas and algorithms offline (without physical access to the test devices) in a quick iteration cycle with some flexibility and ease, quickly become a subject that should be studied and discussed. Only when the right tools are present can a work flow consistently.

Best Practices Finally, when building software to run in highly specialized hardwares (like the NAO platform) that have several CPU, memory and time constraints or even platforms that could have borrowed a big part of its code from its simulation counterpart, best practices for software development can and should be a subject to take into consideration.

(27)

1.1 Motivation

The main motivation for this thesis, besides the fact that it is a requirement to graduate, has to do with a deep nurture about the robotics subject in general. The second part of the thesis that deal mainly with software coding has to do with the fact the author is a software developer professionally and wanted to contribute and share as much as possible about software best practices that he learned during his professional live.

After living personally the pressure of the RoboCup competition one can always trust Murphy’s law ”If anything can go wrong, it will” to not let you down! Only a flexible coding that is able to adapt to the current situation can give good results.

1.2 Objectives

This thesis aims to use recent techniques and research works in the area of sensor fusion to resolve new problems in the Humanoid Robots always using what is considered best practices in software development. This thesis intends to provide a complete description of all the solutions developed by the Portuguese Team1 in sensor fusion for the RoboCup SPL and also all the tools that were developed in between. It could also be of interest to researchers, who need to process and interpret the sensor data in scientific and engineering fields.

1.3 Thesis outline

Chapter 2, Robot Model starts with a mathematical background about affine transformations then goes into detailed information about Forward Kinematics, continues with a deep look into the sensors available in the NAO platform and finishes with the implementation and results obtained.

Chapter 3, Localization starts with a quick introspection about he lack of localization in the RoboCup context, then gives a quick description about possible localization filters and finishes with the implementation and results obtained.

Chapter 4, Development, Testing and Logging starts with the definition of what is a unit test, introduces the Test Driven Development and and finishes with the benefits in having a testable code in the NAO framework.

Chapter 5, NaoSensor Application describes in detail the development tool that was built to help understand and use the NAO framework.

(28)

Chapter 2

Robot Model

2.1 Robot Kinematics

2.1.1 NAO Robot Specifications

The Aldebaran NAO robot is a integrated, medium-sized humanoid robot made by Aldebaran Robotics in France. Since August 2007 that it is the official robot in the RoboCup SPL. Since the project started in 2004, it has evolved in several designs and versions. The robot is available with one to five kinematic chains (head, arms and legs) that depend on the body type. All the body types are presented in figure 2.1.

The body type used in the Robocup competition is the T21. This version is very similar to the T25 and mainly lacks prehensile hands and wrist joints. Within T21 and T25 there are three hardware versions (v3.2, v3.3, v4). This thesis will focus mainly on the T21 v3.3 because those were the robots used by the Portuguese Team.

The full specifications of the v3.3 version can be checked in table 2.1 and [Rob10]. The robot has 21 Degree of Freedom (DOF). Two on the Head, four on each arm, five on each leg and one in the pelvis. Even though the legs are independent from the pelvis down, they are coupled to the same servo (joint) and cannot be moved independently. The five kinematic chains and their respective joints are:

Head HeadYaw, HeadPitch Figure 2.5

Left Arm LShoulderPitch, LShoulderRoll, LElbowYaw, LElbowRoll→ figure 2.6 Right Arm RShoulderPitch, RShoulderRoll, RElbowYaw, RElbowRoll→ figure 2.6

Left Leg LHipYawPitch, LHipRoll, LHipPitch, LKneePitch, LAnklePitch, LAnkleRoll→ figure 2.7

Right Leg RHipYawPitch, RHipRoll, RHipPitch, RKneePitch, RAnklePitch, RAnkleRoll → figure 2.7

A complete specification of the available joints with their corresponding length and operational range is described in the following sections.

(29)

(a) NAO H25 (b) NAO H21

(c) NAO T14 (d) NAO T2

Figure 2.1: Different body types of the NAO robot sold by Aldebaran Robotics

Kinematic chains and joints

A kinematic chain refers to an assembly of rigid body segments that are connected by joints and in which each segments operate together to provide a wide range of motion for a limb (chain). The joints rotate and control the relative angular position of the links of the manipulator.

Only the combinations that don’t lead to collisions between the links and/or some fixed items in the environment can be considered valid and all those form was is commonly called as the joint space.

The term DOF defines the count of joints in a kinematic chain. More DOF lead to more freedom of the kinematic chain and more flexibility on how to achieve a point in space.

Robot kinematics applies geometry to the study of the movement of multi-degree of freedom kinematic chains that form the structure of robotic systems [Pau81]. The links in the robot kinematic chain are modelled as rigid bodies and are assumed to have a pure rotation or translation. More specifically, robot kinematics provides the transformation from joint space (starting reference space) to the Cartesian space (final reference space). The opposite is also true since the transformation matrices are Affine Transformations (a detailed description is available in 2.2).

(30)

Table 2.1: Detailed specifications of the NAO T21 v3.3 robot Degrees of Freedom

Head 2 DOF

Arm 4 DOF in each arm

Pelvis 1 DOF

Leg 5 DOF in each leg Hand Non prehensile

Hardware

CPU x86 AMD Geode 500 MHz

Memory 256MB SDRAM / 2GB Flash Network Type WI-FI (IEE 802.11 b/g)

Ethernet

Height ∼ 58cm

Weight ∼ 4.8kg Battery Capacity ∼ 90min.

Sensors 32x Hall effect sensors

2x Gyrometer 1 axis 1x Accelerometer 3 axis - Bumpers 2x Channel sonar 2x Infrared Tactile sensor 4x FSR in each leg Multimedia 2x Speakers 4x Microphones

2x CMOS Digital Cameras

2.1.2 Forward Kinematics

The joint space information is very reduced in terms of the position and orientation of the end effector of the kinematic chain. The forward kinematics defines a mapping from the joint space→ three-dimensional Cartesian space. Given a kinematic chain with m joints and a set of joint values(θ1, θ2, . . . , θm) the forward kinematics can find the position (px, py, pz) and the orientation(ax, ay, az) of the end effector of the kinematic chain in the three-dimensional (x, y, z) space.

Forward kinematics will result in what is called a closed-form, analytical solution since it will always be expressed analytically in terms of a finite number of certain “well-known” functions.

2.1.3 Inverse Kinematics

A robot manipulator will often need to do the opposite operation of the previously described mapping that is a three-dimensional Cartesian space→ joint space conversion in order to reach a target point with a certain orientation or follow a trajectory in the three-dimensional space. This is called inverse kinematics. In particular, the inverse kinematics defines a relation between points in the Cartesian space (let it be the position (px, py, pz) and orientation (ax, ay, az)) with the joint values (θ1, θ2, . . . , θm) in the joint space of a kinematic chain with m joints.

To be able to achieve this the inverse kinematics will need to find the correct values for each and every joint in the chain that will allow the manipulator to reach the intended position

(31)

(a) Front (b) Top

Figure 2.2: NAO T21 links and its corresponding lengths.

and orientation. Unlike the forward kinematics, the inverse kinematics yields various solutions. It is easy to imagine how many different paths (ways) a human arm could take to reach a certain point in front of it’s chest. This has to do with the amount of DOF and, like the forward kinematic, more DOF will allow more solutions. This multiplicity of solutions set the inverse kinematics more as a relation than a mapping since a point in the Cartesian space may have more than one matching point in the joint space.

2.2 Affine Transformations

In geometry, an affine transformation or affine map or affinity is a mapping that transforms points and vectors from one space to another in such a way that the ratios of distances are kept. It does not necessarily preserve angles or lengths, but does have the property that sets of parallel lines will remain parallel to each other after an affine transformation. The source and target spaces can be n-dimensional with n≥ 2.

A point in the three-dimensional space can be represented as a column vector:

p= ⎛ ⎜⎜ ⎜ ⎝ px py pz ⎞ ⎟⎟ ⎟ ⎠ = (px, py, pz)⊺ (2.1)

(32)

contraction, expansion, reflection, rotation. It is also equivalent to a linear transformation followed by a translation. Any possible combination of the above will also produce an affine transformation. The flexibility of affine transformations makes them very useful in computer graphics.

This thesis will only describe the rotation and translation affine translations since they were heavily used in the forward kinematics and referential transformations described in section 2.4.

2.2.1 Affine Transformation Matrix

An affine transformation f∶ A → B between two affine spaces is a map that acts linearly on the vectors. The affine transformation is a composition of two functions: a translation and a linear map. Let the linear map be a multiplication of A and the translation of the vector ⃗b, the affine map f acting on vector ⃗x can be expressed as:

⃗y = f(⃗x) = A⃗x + ⃗b (2.2)

In order to represent both the rotation and translation in a single matrix multiplication the matrix can be rewritten using the augmented matrix (homogeneous coordinates).

T=⎡⎢⎢⎢ ⎢⎣ ⃗y 1 ⎤⎥ ⎥⎥ ⎥⎦= ⎡⎢ ⎢⎢ ⎢⎣ A ⃗b [0⋯0] 1 ⎤⎥ ⎥⎥ ⎥⎦ ⎡⎢ ⎢⎢ ⎢⎣ ⃗x 1 ⎤⎥ ⎥⎥ ⎥⎦ and is equivalent to 2.2 (2.3) where A is a (n × n) matrix, ⃗b is a (n × 1) vector and the last line of T contains n − 1 zeros followed by a 1. The augmented matrix (2.4) is called affine transformation matrix, or projective transformation matrix.

As an example, applying the transformation to a given point p = (p1, p2, . . . , pn) in a n-dimensional space would result in the multiplication of the affine transformation matrix with a column vector v= (p1, p2, . . . , pn, 1)⊺:

v′= ⎡⎢ ⎢⎢ ⎢⎢ ⎢⎢ ⎢⎢ ⎢⎣ p′₁ ⋮ p′_n 1 ⎤⎥ ⎥⎥ ⎥⎥ ⎥⎥ ⎥⎥ ⎥⎦ = Tv =⎡⎢⎢⎢ ⎢⎣ A ⃗b [0⋯0] 1 ⎤⎥ ⎥⎥ ⎥⎦ ⎡⎢ ⎢⎢ ⎢⎢ ⎢⎢ ⎢⎢ ⎢⎣ p1 ⋮ pn 1 ⎤⎥ ⎥⎥ ⎥⎥ ⎥⎥ ⎥⎥ ⎥⎦ (2.4)

For a point p= (px, py, pz) in the three-dimensional space the result will be:

v′= ⎡⎢ ⎢⎢ ⎢⎢ ⎢⎢ ⎢⎢ ⎢⎣ p′_x p′_y p′_z 1 ⎤⎥ ⎥⎥ ⎥⎥ ⎥⎥ ⎥⎥ ⎥⎦ = Tv = ⎡⎢ ⎢⎢ ⎢⎢ ⎢⎢ ⎢⎢ ⎢⎣ Axx Axy Axz bx Ayx Ayy Ayz by Azx Azy Azz bz 0 0 0 1 ⎤⎥ ⎥⎥ ⎥⎥ ⎥⎥ ⎥⎥ ⎥⎦ ⎡⎢ ⎢⎢ ⎢⎢ ⎢⎢ ⎢⎢ ⎢⎣ px py pz 1 ⎤⎥ ⎥⎥ ⎥⎥ ⎥⎥ ⎥⎥ ⎥⎦ (2.5)

(33)

As discussed earlier, the multiplication of two affine transformation matrices T1 and T2 is also a affine transformation:

T= T1T2= ⎡⎢ ⎢⎢ ⎢⎣ A1 b⃗1 [0⋯0] 1 ⎤⎥ ⎥⎥ ⎥⎦ ⎡⎢ ⎢⎢ ⎢⎣ A2 b⃗2 [0⋯0] 1 ⎤⎥ ⎥⎥ ⎥⎦= ⎡⎢ ⎢⎢ ⎢⎣ A1A2 A1b⃗2+ A2b⃗2 [0⋯0] 1 ⎤⎥ ⎥⎥ ⎥⎦ (2.6)

This property can be generalized for the product of any number of affine transformation matrices: ˆ T= T1T2. . . Tk= ⎡⎢ ⎢⎢ ⎢⎣ ˆ A ˆb [0⋯0] 1 ⎤⎥ ⎥⎥ ⎥⎦ (2.7)

An affine transformation is invertible, if and only if ˆA is invertible and takes the form:

T−1=⎡⎢⎢⎢ ⎢⎣ A−1 −A−1⃗b [0⋯0] 1 ⎤⎥ ⎥⎥ ⎥⎦ and if A is unitary (2.8) =⎡⎢⎢⎢ ⎢⎣ A⊺ −A⊺⃗b [0⋯0] 1 ⎤⎥ ⎥⎥ ⎥⎦ (2.9) 2.2.2 Translation Matrix

In a Cartesian space, translation is a function that moves every point preserving the distances and directions between them by a fixed distance in a specified direction. Let d= (dx, dy, dz) be the offset to translate and dx, dy and dz the distance of translation along the x, y and z axis respectively the translation matrix Z would be:

Z= ⎡⎢ ⎢⎢ ⎢⎢ ⎢⎢ ⎢⎢ ⎢⎣ 1 0 0 dx 0 1 0 dy 0 0 1 dz 0 0 0 1 ⎤⎥ ⎥⎥ ⎥⎥ ⎥⎥ ⎥⎥ ⎥⎦ (2.10)

The translation matrix is an affine transformation matrix with A= I. To move a point p= (px, py, pz)⊺ in the three-dimensional space by distance(dx, dy, dz)⊺ the transformation to apply will be:

v′= ⎡⎢ ⎢⎢ ⎢⎢ ⎢⎢ ⎢⎢ ⎢⎣ p′_x p′_y p′_z 1 ⎤⎥ ⎥⎥ ⎥⎥ ⎥⎥ ⎥⎥ ⎥⎦ = Zv = ⎡⎢ ⎢⎢ ⎢⎢ ⎢⎢ ⎢⎢ ⎢⎣ 1 0 0 dx 0 1 0 dy 0 0 1 dz 0 0 0 1 ⎤⎥ ⎥⎥ ⎥⎥ ⎥⎥ ⎥⎥ ⎥⎦ ⎡⎢ ⎢⎢ ⎢⎢ ⎢⎢ ⎢⎢ ⎢⎣ px py pz 1 ⎤⎥ ⎥⎥ ⎥⎥ ⎥⎥ ⎥⎥ ⎥⎦ (2.11)

(34)

2.2.3 Rotation Matrix

In a Cartesian space, rotation is a function that rotates vectors by a fixed angle in a specified direction preserving the distances and directions between. A rotation in the n-dimensional space is described as an(n × n) orthogonal matrix R with determinant 1. This allows the following properties:

R⊺= R−1 RR⊺= R⊺R= I det(R) = 1 (2.12) Three rotation matrices can be defined θx, θy and θz, one for each rotation along the x, y and z axis respectively:

Rθx= ⎡⎢ ⎢⎢ ⎢⎢ ⎢⎢ ⎢⎢ ⎢⎣ 1 0 0 0 0 cos θx − sin θx 0 0 sin θx cos θx 0 0 0 0 1 ⎤⎥ ⎥⎥ ⎥⎥ ⎥⎥ ⎥⎥ ⎥⎦ Rθy = ⎡⎢ ⎢⎢ ⎢⎢ ⎢⎢ ⎢⎢ ⎢⎣ cos θy 0 sin θy 0 0 1 0 0 − sin θy 0 cos θy 0 0 0 0 1 ⎤⎥ ⎥⎥ ⎥⎥ ⎥⎥ ⎥⎥ ⎥⎦ (2.13) Rθz = ⎡⎢ ⎢⎢ ⎢⎢ ⎢⎢ ⎢⎢ ⎢⎣ cos θz − sin θz 0 0 sin θz cos θz 0 0 0 0 1 0 0 0 0 1 ⎤⎥ ⎥⎥ ⎥⎥ ⎥⎥ ⎥⎥ ⎥⎦

To rotate a vector ⃗v = (vx, vy, vz) with a specific orientation θ = (θx, θy, θz) one solution would be to decompose the orientation using Euler angles in it’s θx, θy and θz. Each angle would then produce a rotation transformation matrix and the multiplication in the inverted order of all the rotation transformation matrix would create the final rotation transformation matrix.

Info Mathematical profs of the concepts introduced in this chapter are outside of the scope of this thesis but can be verified in any Analytical Geometry book and/or in [GC09].

Rotation convention using Euler angles

Any arbitrary orientation of a rotated coordinate system can be rotated to any position using Euler rotations and can be translated so the origin of the rotated coordinate system sits on an arbitrary point in the main coordinate system. Three rotations can be performed to give any arbitrary orientation of a rotated coordinate system. By convention, the following rotations are chosen:

∎ Rotate by angle γ about the world z axis,

∎ Rotate by angle β about the (unrotated) world y axis, and ∎ Rotate by angle α about the (unrotated) world z axis.

(35)

x y z x′ y′ z′ γ _x y z x′ y′ z′ _β x y z x′ y′ z′ α

Figure 2.3: Positioning the rotated coordinate frame (x′, y′, z′) using Euler angles (α, β, γ).

These rotations are shown in figure 2.3. This rotation matrix R(α, β, γ) is given by: R(α, β, γ) = Rθz(α)Rθy(β)Rθz(γ) = ⎡⎢ ⎢⎢ ⎢⎢ ⎢⎢ ⎢⎢ ⎢⎣ cos α − sin α 0 0 sin α cos α 0 0 0 0 1 0 0 0 0 1 ⎤⎥ ⎥⎥ ⎥⎥ ⎥⎥ ⎥⎥ ⎥⎦ ⎡⎢ ⎢⎢ ⎢⎢ ⎢⎢ ⎢⎢ ⎢⎣ cos β 0 sin β 0 0 1 0 0 − sin β 0 cos β 0 0 0 0 1 ⎤⎥ ⎥⎥ ⎥⎥ ⎥⎥ ⎥⎥ ⎥⎦ ⎡⎢ ⎢⎢ ⎢⎢ ⎢⎢ ⎢⎢ ⎢⎣ cos γ − sin γ 0 0 sin γ cos γ 0 0 0 0 1 0 0 0 0 1 ⎤⎥ ⎥⎥ ⎥⎥ ⎥⎥ ⎥⎥ ⎥⎦ = ⎡⎢ ⎢⎢ ⎢⎢ ⎢⎢ ⎢⎢ ⎢⎣

cos α cos β cos γ− sin α sin γ − cos α cos β sin γ − sin α cos γ cos α sin β 0 sin α cos β cos γ+ cos α sin γ − sin α cos β sin γ + cos α cos γ sin α sin β 0

− sin β cos γ sin β sin γ cos β 0

0 0 0 1 ⎤⎥ ⎥⎥ ⎥⎥ ⎥⎥ ⎥⎥ ⎥⎦ (2.14)

2.2.4 Affine Transformation Matrices and Kinematics

In the kinematics context all rotation and translation matrices from this point forward will be defined as a single affine transformation matrix that combine rotation and translation like:

T= ⎡⎢ ⎢⎢ ⎢⎢ ⎢⎢ ⎢⎢ ⎢⎣ ˆ R ⎡⎢ ⎢⎢ ⎢⎢ ⎢⎢ ⎣ dx dy dz ⎤⎥ ⎥⎥ ⎥⎥ ⎥⎥ ⎦ [0 0 0] 1 ⎤⎥ ⎥⎥ ⎥⎥ ⎥⎥ ⎥⎥ ⎥⎦ (2.15)

(36)

2.3 Denavit-Hartenberg (DH) Parameters

The Denavit–Hartenberg parameters (also called DH parameters) is a convention introduced in 1955 by Jacques Denavit and Richard Hartenberg [DH55] [HD64] that defines a minimal representation for attaching reference frames to the links of a spatial kinematic chain, or robot manipulator using only four parameters. Although other conventions already existed at the time they are still considered as the standard approach when dealing with spatial linkages and forward kinematics. αi bi θi ai ai-1 Joint i+1 Link i

Joint i-1 Link i-1

Xi X’i

X’’i, X’’’i, Xi+1 Zi, Z’i, Z’’i

Joint i Z’’’i, Zi+1 Zi-1 O’i Oi Oi+1

Figure 2.4: Frame convention and Denavit and Hartenberg (DH) parameters. Source: [Sah08]. Using figure 2.4 and [Sah08] as reference the following “rules” should be defined:

∎ Axis i → The axis of the join connection link i − 1 to link i.

∎ X_i,Y_i,Z_i → A coordinate system attached to the end of the link i − 1.

∎ Axis Z_i → Should be chosen along the axis of the join i. Any direction towards the direction of the axis will do.

∎ O_i → Origin is located at the intersection of the common normal to Zi−1 and Z_i. ∎ O′

i → Origin is located at the intersection of the common normal to Zi and Zi+1. ∎ Axis X_i → Should be chosen along the common normal to axes Zi−1 and Z_i. The

direction should be from Zi−1 and Zi.

(37)

Once the link frames have been established for every joint, the position and orientation of frame i relative to frame i− 1 are fully specified by the four Denavit and Hartenberg (DH) parameters.

This frames are also referred to as DH frames. The definitions of the four DH parameters are:

∎ b_i (Joint offset)→ Length of the intersections of the common normals on the joint axis Zi i.e. the measured distance between Xi and Xi+1 along Zi.

∎ θ_i (Joint angle)→ Angle between the orthogonal projections of the common normals about the Xi and Xi−1. If the rotation is made counter clockwise, the angle is positive. ∎ a_i (Link length)→ Length between O′

i and Oi+1 i.e. the measured distance between the common normals to axis Zi and Zi+1 along Xi+1.

∎ α_i (Twist angle) → Angle about the common normal from Z_i and Zi+1 onto the plane normal to the common normal. If the rotation is made counter clockwise, the angle is positive.

Info This description is valid for common cases of revolute joints. For a full description please consult the original articles [DH55], [HD64] or [Sah08]. For more multimedia material please check the Kinematics section of the Tekkotsu framework [TTT10]. This convention defines two affine transformations that combined together allow the transformation of frame reference of link n to a frame reference of link n− 1. Entire kinematic chains can the defined this way.

Let T be the transformation locating the end-link, Zi a transformation associated with the joint i and Xi a transformation associated with the link i.

A combined transformation along a kinematic chain consisting of n links form the kinematics equation:

T= Z1X1Z2X2. . . Xn−1Zn (2.16) With Zi being defined as the movement of the links around a common joint axis:

Zi= ⎡⎢ ⎢⎢ ⎢⎢ ⎢⎢ ⎢⎢ ⎢⎣ cos αi − sin αi 0 0 sin αi cos αi 0 0 0 0 1 bi 0 0 0 1 ⎤⎥ ⎥⎥ ⎥⎥ ⎥⎥ ⎥⎥ ⎥⎦ (2.17)

(38)

And Xi being defined as the dimensions of each link in the serial chain: Xi= ⎡⎢ ⎢⎢ ⎢⎢ ⎢⎢ ⎢⎢ ⎢⎣ 1 0 0 ai,i+1

0 cos θi,i+1 − sin θi,i+1 0 0 sin θi,i+1 cos θi,i+1 0

0 0 0 1 ⎤⎥ ⎥⎥ ⎥⎥ ⎥⎥ ⎥⎥ ⎥⎦ (2.18)

The combined DH affine transformation would result in:

n−1_T n= ⎡⎢ ⎢⎢ ⎢⎢ ⎢⎢ ⎢⎢ ⎢⎣

cos θn − sin θncos αn sin θnsin αn ancos θn sin θn cos θncos αn − cos θnsin αn ansin θn

0 sin αn cos αn bn 0 0 0 1 ⎤⎥ ⎥⎥ ⎥⎥ ⎥⎥ ⎥⎥ ⎥⎦ = ⎡⎢ ⎢⎢ ⎢⎢ ⎢⎢ ⎢⎢ ⎢⎣ R T 0 0 0 1 ⎤⎥ ⎥⎥ ⎥⎥ ⎥⎥ ⎥⎥ ⎥⎦ (2.19)

where R and T correspond once again to the orientation (3× 3) and translation (3 × 1).

2.3.1 Base frame to End Frame ↔ End frame to Base frame

One important aspect of the Denavit and Hartenberg transformations and other affine transformations is that the inverse transformation is:

n−1 Tn −1 =n_T n−1= ⎡⎢ ⎢⎢ ⎢⎢ ⎢⎢ ⎢⎢ ⎢⎣ R⊺ −R⊺T 0 0 0 1 ⎤⎥ ⎥⎥ ⎥⎥ ⎥⎥ ⎥⎥ ⎥⎦ (2.20)

since R⊺ is a orthogonal matrix its inverse and transpose are the same so R⊺= R−1 and, in terms of numeric calculations, is a much cheaper operation to execute.

This nice to have property of affine transformations allows the conversion of frame links from the end effector to the base frame instead of the normal behaviour of base frame to end effector.

2.4 Joints description and Forward Kinematics

The robot has 21 DOF so it can execute various simple or complex moves like for example, walking, kicking, getting up, falling to defend a ball, rest, etc. Kinematics can be used in the planing and execution phase of these movements. With forward kinematics, one can easily find the position of the ball in the floor or the position and orientation of the head and correlate that with the horizon.

(39)

2.4.1 Head joints

(a) Head

Joint Name Range in ○ HeadYaw -119.5↔ 119.5 HeadPitch -38.5↔ 29.5

(b) Operational range

Figure 2.5: NAO T21 head joints and operational range.

1 Device / SubDeviceList / HeadPitch / Position / Actuator / Value 2 Device / SubDeviceList / HeadYaw / Position / Actuator / Value

Listing 2.1: List of ALMemory command key names for the Head joints actuators (in radian)

1 Device / SubDeviceList / HeadPitch / Position / Sensor / Value 2 Device / SubDeviceList / HeadYaw / Position / Sensor / Value

(40)

2.4.2 Arm joints

(a) Right and left arm

Joint Name Range in ○ LShoulderPitch -119.5 ↔ 119.5 LShoulderRoll -18 ↔ 76 LElbowYaw -119.5 ↔ 119.5 LElbowRoll -88.5↔ -2 RShoulderPitch -119.5 ↔ 119.5 RShoulderRoll -76 ↔ 18 RElbowYaw -119.5 ↔ 119.5 RElbowRoll -2↔ 88.5 L(R)WristYaw disabled (b) Operational range

Figure 2.6: NAO T21 arm joints and operational range.

1 Device / SubDeviceList / LShoulderPitch / Position / Actuator / Value 2 Device / SubDeviceList / LShoulderRoll / Position / Actuator / Value 3 Device / SubDeviceList / LElbowYaw / Position / Actuator / Value 4 Device / SubDeviceList / RShoulderPitch / Position / Actuator / Value 5 Device / SubDeviceList / RShoulderRoll / Position / Actuator / Value 6 Device / SubDeviceList / RElbowYaw / Position / Actuator / Value

Listing 2.3: List of ALMemory command key names for the Left and Right arms joints actuators (in radian)

1 Device / SubDeviceList / LShoulderPitch / Position / Sensor / Value 2 Device / SubDeviceList / LShoulderRoll / Position / Sensor / Value 3 Device / SubDeviceList / LElbowYaw / Position / Sensor / Value 4 Device / SubDeviceList / RShoulderPitch / Position / Sensor / Value 5 Device / SubDeviceList / RShoulderRoll / Position / Sensor / Value 6 Device / SubDeviceList / RElbowYaw / Position / Sensor / Value

Listing 2.4: List of ALMemory value key names for the Left and Right arms joints sensors (in radian)

(41)

2.4.3 Pelvis and legs joints

(a) Pelvis, left and right leg

Joint Name Range in○ LHipYawPitch -65.62↔ 42.44 RHipYawPitch -65.62↔ 42.44 LHipRoll -21.74↔ 45.29 LHipPitch -101.63 ↔ 27.73 LKneePitch -5.29↔ 121.04 LAnklePitch -68.15↔ 52.86 LAnkleRoll -22.79↔ 44.06 RHipRoll -42.30↔ 23.76 RHipPitch -101.54 ↔ 27.82 RKneePitch -5.90↔ 121.47 RAnklePitch -67.97↔ 53.40 RAnkleRoll -45.03↔ 22.27 (b) Operational range

Figure 2.7: NAO T21 pelvis, left and right leg joints and operational range.

1 Device / SubDeviceList / LHipRoll / Position / Actuator / Value 2 Device / SubDeviceList / LHipPitch / Position / Actuator / Value 3 Device / SubDeviceList / LKneePitch / Position / Actuator / Value 4 Device / SubDeviceList / LAnklePitch / Position / Actuator / Value 5 Device / SubDeviceList / LAnkleRoll / Position / Actuator / Value 6 Device / SubDeviceList / RHipRoll / Position / Actuator / Value

(42)

9 Device / SubDeviceList / RAnklePitch / Position / Actuator / Value 10 Device / SubDeviceList / RAnkleRoll / Position / Actuator / Value

Listing 2.5: List of ALMemory command key names for the Left and Right legs joints actuators (in radian)

1 Device / SubDeviceList / LHipRoll / Position / Sensor / Value 2 Device / SubDeviceList / LHipPitch / Position / Sensor / Value 3 Device / SubDeviceList / LKneePitch / Position / Sensor / Value 4 Device / SubDeviceList / LAnklePitch / Position / Sensor / Value 5 Device / SubDeviceList / LAnkleRoll / Position / Sensor / Value 6 Device / SubDeviceList / RHipRoll / Position / Sensor / Value 7 Device / SubDeviceList / RHipPitch / Position / Sensor / Value 8 Device / SubDeviceList / RKneePitch / Position / Sensor / Value 9 Device / SubDeviceList / RAnklePitch / Position / Sensor / Value 10 Device / SubDeviceList / RAnkleRoll / Position / Sensor / Value

Listing 2.6: List of ALMemory value key names for the Left and Right legs joints sensors (in radian)

2.4.4 Masses of link/joints (body parts)

Table 2.2: NAO T21 Masses of link/joints along with its corresponding Center of Mass.

NAO T21 Masses of link/joints

Frame Name Mass (Kg) CoMx (mm) CoMy (mm) CoMz (mm)

Torso 1.03948 −4.15 0.07 42.58 HeadYaw 0.05930 −0.02 0.17 25.56 HeadPitch 0.52065 1.2 −0.84 53.53 RShoulderPitch 0.06996 −1.78 24.96 0.18 RShoulderRoll 0.12309 18.85 −5.77 0.65 RElbowYaw 0.05971 −25.6 0.01 −0.19 RElbowRoll 0.185 65.36 −0.34 −0.02 LShoulderPitch 0.06996 −1.78 −24.96 0.18 LShoulderRoll 0.12309 18.85 5.77 0.65 LElbowYaw 0.05971 −25.6 −0.01 −0.19 LElbowRoll 0.185 65.36 0.34 −0.02 RHipYawPitch 0.07117 −7.66 12 27.17 RHipRoll 0.1353 −16.49 −0.29 −4.75 RHipPitch 0.39421 1.32 −2.35 −53.52

(43)

Table 2.2 – continued from previous page

Frame Name Mass (Kg) CoMx (mm) CoMy (mm) CoMz (mm)

RKneePitch 0.29159 4.22 −2.52 −48.68 RAnklePitch 0.13892 1.42 −0.28 6.38 RAnkleRoll 0.16175 25.4 −3.32 −32.41 LHipYawPitch 0.07117 −7.66 −12 27.17 LHipRoll 0.1353 −16.49 0.29 −4.75 LHipPitch 0.39421 1.32 2.35 −53.52 LKneePitch 0.29159 4.22 2.52 −48.68 LAnklePitch 0.13892 1.42 0.28 6.38 LAnkleRoll 0.16175 25.4 3.32 −32.41 Total Mass 4.88083 2.4.5 Implementation

In the AgentModel class, the base class of the Forward Kinematics is a interface named iBodyPart. This class contains a transformation matrix that forms the actual Forward Kinematics n−1Tn conversion and a calculated three-dimensional vector with the relative position from the Head referential to thisiBodyPart that simply is the translation (Z) part from the transformation.

TheAgentModel uses a slightly different convention for the Forward Kinematics than the Denavit-Hartenberg parameters. First theHead body part is processed. This serves as the base reference for all the otheriBodyPart so the transformation in the Left Foot will transform a point in theLeft Foot referential to the Head referential.

The two classes (BodyPart and the Joint) that implement this interface have different ways of calculating the transformation matrix.

TheBodyPart class uses the joint anchor to do the initial translation and then uses the parent’s transformation to convert the referentials. Each joint will be processed after this so all the joints in the same level from the same parent are processed one at the time and only after that the algorithm processes the next level.

Algorithm 1: Body Part transformation creation. begin

initialization;

for joint ∈ bodyPart.joints do

joint.transform ←Ð joint.transform × Z(joint.anchor1); joint.relPosition←Ð joint.transform.toVector3D;

(44)

The Joint on the other hand will translate the body part attached to with based on the second anchor and them rotate the same body part with the joint angle and axis:

Algorithm 2: Joint Part transformation creation. begin

initialization;

joint.body.transform ←Ð joint.transform × R(joint.axis,joint.angle) × Z(joint.anchor2);

joint.body.relPosition←Ð joint.transform.toVector3D; end

The filenao.xml was updated (B.2) from the v3.2 simulation version to the real device v3.3. Besides some small changes in the dimensions and max/min angles, the head joint anchor point was changed. In the simulation the head joint is centred in the head and in the real device the head is connected to the neckBodyPart.

Sensing and estimation are essential aspects of the design of any robotic system.

A simple measurement of the state of the robot can be useful for feedback control. On the other hand, a more advanced perception were integration of the sensor information (across space and time), allows a better interpretation of sensor data and must be used to better estimate and control the robot.

(45)

2.5 Sensors

A sensor is a device that converts a physical quantity or stimulus (as heat or pressure or light or motion etc.) into a signal which can be read by an observer.

Taking into consideration that the amount of sensor types is to vast, this chapter will focus mainly in sensor types mostly used in robotic platforms and in more detail the ones used by the NAO robot.

A good sensor should follow this basic principles: ∎ Be sensitive to the measured property.

∎ Not be affected by other properties in the system. ∎ Not influence the system.

The most common sensor types can be classified into two main categories. The linear or linear to a mathematical function and the non-linear.

2.5.1 Common terminology

Range Every sensor is made to work in a specified range. These ranges are normally fixed, and if exceeded, may result in permanent damage or even destruction of the sensor. It is usual to only use the sensor in parts of its range where it provide predictable performance and enhanced linearity.

Zero When making a measurement a known value is usually set as the starting point or zero and it is often convenient to adjust this value to zero more than once if needed.

Zero Drift The signal level may vary from its zero value while the sensor is on. This introduces a systematic error equal to the amount of variation, or drift. This zero drift may result from changes in the operating temperature, humidity, electronic stability or aging.

Sensitivity The slope of the output characteristic curve. This factor may be constant over the range of the sensor (linear), or it may vary (nonlinear).

Resolution The smallest detectable incremental change of input parameter that can be detected in the output signal.

Response The time the sensor takes to reach its true output when subjected to a step input. Commonly called response time.

Linearity An expression to which the actual measured curve of the sensor output relates to the the ideal curve.

Hysteresis The characteristic that a sensor has when it is not able to repeat faithfully the same value in the same situation as the previous measured value.

(46)

Calibration To get a meaningful measurement it may be necessary to compare the measured values with an accurately known input. The devices that produce this reference values are described as calibration standards.

Full Scale The algebraic difference in the output between the maximum and the minimum input signal.

Accuracy Most commonly treated as inaccuracy since it is measured as the ratio of the highest difference of a value read with the ideal value. Also called ∆.

(a) Ideal curve and sensitivity error. (b) Ideal versus measured curve showing lin-earity error.

(c) Rise-time definition. (d) Fall-time definition.

Figure 2.8: Various definitions of sensor terminology. Source (a)-(d): J.J. Carr, Sensors and Circuits Prentice Hall.

(47)

Figure 2.9: Continuation of definitions of sensor terminology. Hysteresis curve. Source: J.J. Carr, Sensors and Circuits Prentice Hall.

2.5.2 Curve Fitting

The most convenient sensor to use is one with a linear transfer function. An output variation that is directly proportional to a input variation over its entire range, so that the slope of a graph of output vs input describes a straight line.

y(x) = mx + b (2.21)

For some sensors, the transfer function is not linear but can be defined using simple mathematical functions like:

y(x) = ex _or _{y(x) = log(x)} _(2.22)

5 10 15 20 20 40 x f(x) (a) 5 10 15 20 20 40 x f(x) (b) 5 10 15 20 20 40 x f(x) (c)

Figure 2.10: Some examples of sensor readings. (a) is a linear sensor, (b) a linear sensor with two different linear regions and (c) a non-continuous and non-linear sensor.

(48)

x f(x) f(x) =1 2x+ 1 0 1 2 3 4 5 1 2 3 4

(a) Almost linear transfer function.

x f(x) f(x) = 1 40exp(x) 0 1 2 3 4 5 1 2 3 4

(b) Exponential transfer function.

Figure 2.11: Using a simple mathematical approximation to find the transfer function of (a) an almost linear and (b) an exponential transfer function sensor.

As a last resort, if none of the previous methods could be applied, for sensors that don’t have a linear transfer function for their full measurement interval, it might still be possible to only consider one small portion of the full measurement scale as linear as long as the value to measure lies in the considered interval.

In most scientific and engineering models the dependent variable depends on the parameters in a non-linear way this mean that the relationship between measured values and measurement variables is non-linear.

The curve-fitting normally needs to find parameters. In non-linear models, however, it is usually not possible to solve the equation for the parameters. Various iterative procedures are used instead but they depend on the user knowing, to some extent, the underlying physics, chemistry, electronics and other properties of a problem to choose the right model.

Nonlinear fitting always starts with an initial guess of the parameters values. One usually looks at the general shape of the model curve and compares it with the shape of the data points. The logistic regression is one of the most used non-linear curve-fitting methods and is based on the sigmoid function (figure 2.12).

Alternative options

In some cases the transfer function could be defined in branches with one simple mathemat-ical function per branch or a lookup table could be used to map an input to an output value. This might require a very good understanding of the sensor internals and peculiarities and could also be a very time consuming calibration. In some other cases this calibration and/or lookup table might even only be valid for that specific sensor and another sensor of the same family or even the same model number could need an entirely different calibration/lookup table.

(49)

x f(x) f(x) = 1 1+ e−x −5 −4 −3 −2 −1 0 1 2 3 4 5 0.25 0.5 0.75 1.0

Figure 2.12: Sigmoid - Example of a non-linear transfer function

Info More information about Curve Fitting and Linear Regression can be consulted in [Led][Wei][Bev03].

2.5.3 Inertial Sensors

Gyroscope

The main goal of a gyroscopic systems is to measure rotation changes (velocity of rotation) in a certain frame or axis. This device uses known physical laws that produce predictable effects under rotation. “A rotating frame is not an inertial frame, and thus many physical systems will appear to behave in an apparently non-Newtonian manner” [SK08].

(a) Simple gyroscopic principle. (b) Die structure of a 3D MEMS gyroscope. Credits to SensorDynamics.

(50)

the same type used in the device inside the NAO platform.

“In a MEMS gyroscopic systems the gyroscope’s proof-masses are electrostatically oscillated at resonance . An internal Automatic Gain Control (AGC) circuit precisely sets the oscillation of the proof masses. When the sensor is rotated in the X− or Y − axis, the Coriolis effect [GK] causes a vibration that can be detected by a capacitive pickoff. The resulting signal is then amplified, demodulated and filtered to produce an analog voltage that is proportional to the angular rate.” [Inv06].

A common problem to all gyroscopes is the bias drift. Each of the relative motion measurements is corrupted by an error, and these errors accumulate over time. This, coupled with specific measurement errors associated with the individual gyroscope technologies, means that unless the error is corrected through reference to some alternate (external) measurement, the drift will eventually exceed the required accuracy of the measurement [SK08].

By using signal processing techniques (Kalman and 1st Order Markov filters) and combining the outputs of an array of MEMS gyroscopes is it possible to improve the angular rate accuracy and it’s drift [JXC+11]. Another possibility to improve the drift is using a input-output controller to compensate for the non-ideal interdependencies of drive and sense axes [IDF08].

Info For more information please check the device datasheet [Inv06], the physical phe-nomenon Coriolis effect [GK] and the Aldebaran Hardware Documentation [Ald12a] and [Ald12b].

1 Device / SubDeviceList / InertialSensor / GyrRef / Sensor / Value 2 Device / SubDeviceList / InertialSensor /GyrX/ Sensor / Value 3 Device / SubDeviceList / InertialSensor /GyrY/ Sensor / Value

Listing 2.7: List of ALMemory value key names for the Gyroscope sensors (in rad/s)

Accelerometer

Just as gyroscopes can be used to measure changes in orientation of a robot, other inertial sensors – known as accelerometers – can be used to measure external forces acting on the vehicle. One important factor concerning accelerometers is that they are sensitive to all external forces acting upon them – including gravity [SK08].

A accelerometer is easy to visualize as a mass on a spring. They’re very simple and can pick up almost every kind of movement. They use various different ways to transduce external forces into a computer-readable signal.

(51)

(a) Mass-Spring system used for measuring acceler-ation. Source: Stanford University.

(b) Rest and under acceleration position of the mass-spring system. Source: Stanford University.

Figure 2.14: Example of the principle of the accelerometer mass-spring system and the mass-sprig deflection when under acceleration and rest.

If the accelerometer is falling freely through space the read value will be 0. That is called free-fall and the returned value of 0 could seem strange at first but the reason why is because gravity pulls both on the mass in the middle and on the frame of the accelerometer. In that situation, and only in that situation, the system can be considered a inertial referential and so there won’t be any relative movement between the mass and the frame. There is also and additional force holding the structure up and the mass will droop under gravity.

Different types of accelerometers with different manufacturing processes exist. What differentiates the types is the sensing element and the principles of their operation such as Capacitive, Piezoelectric, Piezoresistive, Hall Effect, Magnetoresistive, Heat transfer and MEMS do exist. This thesis will briefly describe the MEMS type since it is the same type used in the device inside the NAO platform.

MEMS accelerometers are actually the simplest type, since they consist of little more than a seismic mass, also known as a proof mass, as well as a cantilever beam. Often times residual gas becomes sealed inside the device, which can cause damping over time but is usually not too severe unless the Q-factor is too low. This Q-Factor (also called quality factor) is a dimensionless parameter that describes how under-damped an oscillator or resonator is. In this case the damping process can cause a loss of sensitivity.

MEMS accelerometers use nanotechnology in order to enhance the natural abilities common between all accelerators; hence, these devices are extremely fine-tuned and accurate. They specifically possess a mass-displacer that can translate external forces such as gravity into kinetic motion energy. This part of the accelerometer usually contains some type of spring force in order to balance the external pressure and displace its mass, thus leading to the motion that produces acceleration.

The mechanism works like this: external force is applied and shifts the position of the proof mass from a neutral position to an active position; typically the amount of this deflection is measured by analog or digital readouts. These forces may be static, like the constant force of gravity pulling at our feet, or they could be dynamic - caused by moving or vibrating the accelerometer.

(52)

manufacture.

Most MEMS accelerometers operate in-plane, that is, they are designed to be sensitive only to a direction in the plane of the die. By integrating two devices perpendicularly on a single die a two-axis accelerometer can be made. By adding a third out-of-plane device three axes can be measured. Such a combination may have much lower misalignment error than three discrete models combined after packaging.

One of the main problems with accelerometers is significant drift of the position value [DJJP11] after integration, whether the integration is performed numerically or by means of analog integrators. A characteristic example of the drift, which resulted even with carefully calibrated accelerometers, is presented in figure 2.15.

Figure 2.15: Calculated angle (bottom panel) and angular velocity (middle panel) obtained by numerical integration of the measured acceleration of the segment (top panel). The dashed line should remain approximately constant however, due to the integration drift, this line has a parabolic shape. Retrieved from [DJJP11].

Raw vs filtered data

Dealing with raw sensor values can sometimes be cumbersome. Certain sensors have a better response when they are static like the accelerometer that is described in 2.5.3. Others are the complete opposite like the gyroscope described in 2.5.3, that give better results when they are moving. So what do to when both devices have important contributions to the value that is to be measured? The answer might be the fusion of the sensors readings when they are in their best behaviour.

For instance, a smart algorithm that returns inertial data from the gyroscope if it detects the robot is in motion and from the accelerometer if the robot is considered static (gavitational acceleration). This smart algorithm could even do pre-filtering of the data using a low pass filter to smooth the output result (but and also increases the response time) or a more advanced Wavelet-based pre-filtering [ESMS11][AHA10] that would de-noise the output signal by effectively band-limiting it and thus improving its Signal to Noise Ratio (SNR).

The NAO SDK allows two types of inertial unit data. The raw data that comes directly from the sensors or a filtered data that incorporated a similar algorithm like the one described

(53)

before.

In the Portuguese Team and NAO context the important data to extract from the Inertial Unit is the orientation of the robot with the ground, namely its torso. This orientation allows the controlling software to know if the robot is falling, if it’s upside down or to serve as a vital information to find the height from the ground and the orientation of the camera for location purposes (full description in 2.6.6).

After doing some tests with the filtered data it was trivial to conclude that the results obtained from proprietary algorithm implemented by Aldebaran Robotics served the simple purposes that were established. It wasn’t a 100% match especially when the robot was in motion but it was satisfactory and new SDK versions also brought some improvements in that area.

No additional development was done either in the raw or the filtered data values and all references from this point forwards to Inertial Unit orientation or angles are related to the filtered data from the Inertial Unit.

1 Device / SubDeviceList / InertialSensor / AngleX / Sensor / Value 2 Device / SubDeviceList / InertialSensor / AngleY / Sensor / Value

Listing 2.8: List of ALMemory value key names for the Filtered Inertial Unit sensors (in rad)

Implementation

The accelerometer model in the NAO platform is the LIS302DL from ST [Mic08]. It is a 3-axis digital output linear MEMS accelerometer.

1 Device / SubDeviceList / InertialSensor /AccX/ Sensor / Value 2 Device / SubDeviceList / InertialSensor /AccY/ Sensor / Value 3 Device / SubDeviceList / InertialSensor /AccZ/ Sensor / Value

Listing 2.9: List of ALMemory value key names for the Accelerometer sensors (in m/s2) The gyroscope model in the NAO platform is the IDG-300G ([Inv06]) from InvenSense. It is a dual-axis gyroscope consisting of two independent vibratory MEMS gyroscopes. One detects rotation about the X− axis; the other detects rotation about the Y − axis. These internal reference axis are referenced to the device. Each structure is fabricated using proprietary bulk silicon technology.

The device is centered (with a small offset) in the robot’s body center (torso) and gives the rotation speed around the robot X axis and around the robot Y axis (robot reference).

When the robot does not move, ”no rotation” value is returned, that is not 0 and needs a calibration, usually around−2000 (raw sensor value). The main difficulty here is to calibrate this 0. After calibration, the change of the value is around 2.7 increment/(○/s).

For reference, no further calibration and drift correction was needed since the values of the Filtered Data (2.5.3) were found to be enough for the needs of the Agent.

(54)

2.5.4 Video Camera

A video camera is a sensor that converts an optical image to an electric signal. Inside it normally has some sort of image sensor like a charge-coupled devices (CCD) or CMOS sensors and one or more lenses.

Pinhole Camera

A pinhole camera is a simple camera without any lenses and with a single small aperture (pinhole). Light passes through this single point and projects a inverted image on the opposite of the camera sensor. Since no lenses are used to focus light, it’s cost and the size are reduced while still getting a good representation of the image and a easier to use mathematical model.

X2 X1 Y 1 1 2 P x1 Q −y1 O f x3

Figure 2.16: The 2D simplified geometry and mathematics of a pinhole camera were a ray passes through the point P and reaches the sensor plane (Y 1).

The pinhole aperture of the camera, in which all projection lines must pass, is assumed to be very small (a point). This point is commonly referenced to as the optical (or lens or camera) center.

The related geometry and mapping of a pinhole camera is illustrated in the figure 2.16. The following was considered:

∎ Camera aperture is located in the origin O.

∎ Two orthogonal coordinate systems were defined in the figure: X1⊥ X2 and Y 1 ⊥ X2. ∎ X1⊥ X2 corresponds to the camera aperture (or principal plane).

∎ Y 1⊥ X2 corresponds to the sensor plane (or image plane).

∎ Point P is somewhere in the world at coordinate (x₁, x₂, x₃) relative to the axes X1, X2. ∎ The projection (green) line of point P into the camera which passes through point P

(55)

∎ The projection of point P onto the image plane, denoted Q at coordinate (y₁, y₂, y₃). ∎ x₂= y₂= y₃= 0.

With this simplified model it is easy to notice that two similar triangles, left(1)and right(2), both have their hypotenuses in the green projection line.

With the catheti of the left triangle (−y1, f) and the catheti of the right triangle (x1, x3) it follows that: −y1 f = x1 x3 or y1= − f x1 x3 or f = −y1x3 x1 (2.23) A similar investigation using the negative direction of X1 gives:

−y2 f = x2 x3 or y2= − f x2 x3 (2.24) This can also be merged and written into:

(y1 y2) = − f x3( x1 x2) (2.25) After applying 2.23, the focal distance obtained experimentally was: 2.8123 mm. This procedure was repeated for the bottom camera of the same robot and also for another robot to check if these values were correlated or independent.

All the obtained results were very similar so the same value was used without any specific calibration for all the cameras of all the robots.

!! Since the image in the image plane is inverted and some of the distances used in the formula are also negative this leads to a positive focal length.

Info A deeper description of the camera parameters like geometric properties, projection matrix, orthogonal rectification or calibration can be consulted in [IFT05][Fus].

Implementation

In the NAO robot two identical pinhole video cameras (model name OV7670) are located in the forehead. Each camera is able to provide a 640x480 resolution at 30 frames per second (fps) (internally) in the YUV422 color space although not at the same time.

For remote access and/or debugging the fps lower considerately going as low as 12 fps for ethernet access or 2.5 fps for Wireless G and in some cases like calibration, turning this process a costly task.

Sensor fusion in humanoid robots

Carlos Eduardo dos

Reis Rodrigues

Sobrinho

Fus˜

ao Sensorial em Robots Human´

oides

Sensor Fusion in Humanoid Robots

Carlos Eduardo dos

Reis Rodrigues

Sobrinho

Sensor Fusion in Humanoid Robots

Contents

List of Figures

List of Tables

Listings

Chapter 1

Introduction

1.1

Motivation

1.2

Objectives

1.3

Thesis outline

Chapter 2

Robot Model

2.1

Robot Kinematics

2.2

Affine Transformations

2.3

Denavit-Hartenberg (DH) Parameters

2.4

Joints description and Forward Kinematics

2.5

Sensors