Robotics and sensor data fusion

Mobile Robotics
Matteo Ragni [161822] July 14, 2013

A.A. 2012/2013

Contents
1 Mobile Robot Design 2 Nonholomic systems
Constraint types . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Integrability and involutivity Global and local accessibility

1 3
3 4 5

3 Accessibility space
Unicycle . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Car Like . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Car Like FD Car Like RD . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

6
6 8 9 10

4 Matlab Project
Theoretic approach . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Simulink models . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Feedback linearization

11
11 17 17 22

Input/output linearization . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

Mobile Robot Design

task operational environment chosen kinematic model admissible path/trajectory planning low level control high level control
Denition of

Introduce mobile robot designing with considerations about the interrelation existing within:

task, during analysis and synthesis of a mobile robot, represents the rst necessary
1

step that will dene a great number of specications, both mechanical and electrical, of the nal robot. Actually, we can identify two dierent approaches in the denition of the task, as testied in "Learning to understand task for mobile robots ": an approach that is completely "a priori", that will be the one that we will consider in this little essay, and an hybrid approach, that is the one deepened in Cognitive Robotics of this course.

1 Hegen

- Krose, IEEE International conference on System, Man and Cybernetics, 2004

In the

hybrid approach, the task is completely dened only after the design and production

of the mobile robot, and this constrains the designer to give some redundant and maybe not completely useful characteristics to the robot, to make it capable to work in a particular context (we can also consider it as a framework approach). Sometimes, the procedure to assign a task to the robot is put beside a control architecture that is able to learn. In the

a priori approach, before robot initial design, task is analyzed deeply, and main aspects type of environment in which the robot will operate.
The environment

of a task will be used to dene, in a rigorous way, specications of the robot. This approach is the only one that can be used to achieve an optimal design. Specication and design process should take into account the may be:

structured : environment's map is completely known, so it is possible to elaborate a path or

a trajectory during robot design;

semi-structured :

environment's map is partially known, so it is possible to elaborate an

initial path or trajectory, but robot must be elastic to perturbation (obstacle avoidance, map variations, symmetric spaces, etc.);

unstructured : there is no knowledge about enviroment and its map, and also may be no
knowledge about eventually perturbation to system recognition/environment mapping.

Try to think about a mobile robot that will operate in a semi-structured environment, in which designer may dene a

trajectory

q(t)

for

t [ts , te ]

that robot will follow.

The problem of

planning a trajectory can be broken down in nding a path Mobile robots have some

kinematic constraints called nonholonomic (for example, model of a

=0 AT (q) q

q(s)

and dening a time law

s(t).

centered wheel without skeet generates a nonholonomic constraint on contact point between wheel and ground) and dened as non-integrable, or in Pfaan form:

The kinematic constraints can be devised from the chosen

kinematic model:

unicycle (that can be used also for dierential drive and synchro drive robots); bicycle rear driven or front driven (that can be used also with all car-like robots).

This equation, if it is not integrable, costraints generalized speeds of mobile robots in a subspace of all possible speed congurations. We will see integrability conditions in the next exercise. In addition to complying with the bonduary conditions, a possible path planning must also satisfy any nonholonomic constraint at all points. a time-dependent scalar function space, so given: If we can express our path as a function of

s(t),

the space of possible trajectory is constrained in velocity

= q
that may be used to rewrite constraints:

dq dq = s =q s dt ds

AT (q(s)) q s = A (q(s)) q
for

0 0 q.
There are

s = 0.

The kinematic constraint induces the space of geometrical admissible path

dierent way to dene this path, the most used are: planning via cartesian polynomials on degree space or in chained form space; planning via parametrized input (given the parametric input, one evaluate parameters that generate desired output);

l 3,

that can be done in kinematic model

Robotics and sensor data fusion | 2

From this last equation is clear the strict relation between kinematic model (that denes constraints) and feasible trajectory or path (that must be a possible solution of kinematic constraints). It is important to notice that even if space of possible trajectories is reduced, space of conguration for the system is not reduced: even if a conguration

q(t ) = q

cannot be reached locally (or

with a minimal trajectory), globally this conguration could be reached with a concatenation of minimal trajectories (if it belongs to congurational space). The last step is to dene a timing law that could be obtained through:

time scaling; optimal trajectories (Pontryagin minimum principle); minimum-time trajectory:

min (J ) = min

tf
ti

dt

Solution of this problem grants a way to dene a series of

low level routines like path following

Besides this, it

(as key point or geometrical function), trajectory following (dened as a geometrical function parametrized with respect to time, expressed in joint-space or in work-space). from the sensors. Those low level routines shall be used to dene a series of must be implemented a big set of any simple commands, such as the response to events derived

high level routines

that allow

our robot to response in an appropriate way to environment perturbation, for example, to avoid collision with object (devising a new path on the imposed trajectory). This architecture in robotic cognition science is also called

subsumption architecture,
2 3

one of the new robotics paradigm

introduced by Brooks in "Intelligence without representation ", based upon the previous work "A robust layered control for a mobile robot ". This high level routines act as a general strategies to accomplish the dened task. The circle is closed whereas we consider that the nature of the task aects directly the complexity of the strategy that must be implemented. Sometimes some high level structures are not necessary, so we can relax some control hypothesis (for example, a symmetric robot may not be interested in attitude control), simplifying our problem.

Nonholomic systems

Explain the interrelations within:

(non)holonomicity (non)integrability (non)involutivity instantaneus motion directions (local) global accessibility (with manoeuvring !?)

Constraint types
Consider a mechanical system. The space of possible congurations

C Rn

is described with

a vector of generalized coordinates

q C. = 0)

This vector describes time evolution of the system.

Mechanical systems may have several type of constraints:

) bilateral constraint (h(t, q, q

) and unilateral constraint (h(t, q, q = 0);

0);
or rheonomic (or

) scleronomous (or xed) constraints if not time dependent (h(q, q ) mobile) constraints if time dependent (h(t, q, q

= 0)

smooth constraint (if reaction is parallel to constraint direction) or scabrous constraint (if reaction isn't parallel to constraint direction).

2 Brooks, 3 Brooks,

Articial Intelligence, 1991 A.I. Memo, 1985

Robotics and sensor data fusion | 3

Consider a mechanical system subject only to If all constraint may be rewritten in form:

) = 0. k < n scleronomous bi-later constraints: h(q, q

hi (q) = 0
those constraints are dened as functions

i = 1, ..., k

h() : C R are suciently smooth. This type of constraint limits the space of accessible nk congurations to a subspace G R and can be used to dene a map to subspace G . Considering qA G the set of degree of freedom that are accessible, and qB C\G the degrees of freedom that are not accessible, it is possible to dene, inverting the set of holonomous equations h(): qB, i (qA ) : C\G C
of the space

holonomic

or

integrable.

In this case it is assumed that the

i = 1, ..., k

Possible congurations that can be reached by mechanical system coincide with all congurations

G , while congurations C\G

are not accessible by mechanical system. Typical example

of this constraints are spherical and prismatic joints used in robot manipulators. Constraints that involve both the generalized coordinates that their derivatives with respect to time and can not be traced (either by integration) to holonomous form are said or

not integrable:

non-holonomic

) = 0 ai (q, q

i = 1, ..., k

In the next section we will see what characterized this constraints, but before that we have to explore the concept of integrability of this type of equations.

Integrability and involutivity

Most mechanical systems, appears to be characterized by a system of second order dierential equations. The equations are usually reduced in order to refer to a system of rst order equations solvable by a simpler Cauchy problem. Direct result of this situation is the possibility of expressing in a linear form with respect to the rst derivatives of general coordinates the scleronomous bilateral constraint equations:

) = 0 AT (q) q =0 a(q, q
This particular system of equations is said Pfaan system of equations, if matrix characterized by column vectors linearly independent. Consider for the moment, in order to understand, a mechanical system with constraint equations turn out to be only one:

A()

is

ai () : C Rn

with functions that are regular, smooth and

k = 1.

The

aT (q) q =0
If this constraint equation is of type holonomic, this can be integrated and dened as a function of only the generalized coordinates:

h(q) = c;

this is true because of the relation:

h =0 (q) = q h(q) q t
and

c is only an integration constant. h().

The equation clearly shows that the mechanical conguration

of the system were conned with respect to a particular contour line of the hyper-surface dened by the function In fact, given an initial conguration for the model in a space of congurations dened by the contour line (the contour line itself ) of

q0 , the system is constrained h(q0 ) = c, which uniquely denes a subspace

n1

degrees of freedom.

Consider instead that the constraint is non-holonomous. We said that this constraint is also called non-integrable, eg. can not be traced back to the primitive form expressed above for holonomic constraints. Considering the constraint equation leads us to infer that:

the derivatives of generalized coordinates belong to a subspace

n 1;

generalized coordinates are bound by a kinematic point of view, but there is no restriction regarding access to the conguration space

C.

Robotics and sensor data fusion | 4

This implies that a mechanical system subject to non-holonolomic constraints can access all space even if its speed must belong to a subspace Take a set of equations of motion of the type:

n k.
4

That a system is of type holonomic or

non-holonomic depends on the condition of integrability .

aT i (q) q
i=1

j = 1, ..., p k

= span

hp (q) h1 (q) , ..., q q

T span aT 1 (q), ..., ak (q)

In the case that the distribution is singular (det() the Frobenius theorem: this theorem, it is evident that if the distribution

a non-singular distribution is integrable if involutive5 .

= 0))

is called involutive.

According to From

results to be integrable, is representative of

holonomic constraints. In this case there are equations:

h1 (q) = c1 , ..., hp (q) = cp

that constrain the conguration space

C Rn

to the subspace

particular case of system completely holonomic. and constrains are said to be non-holonomic.

G Rnp . When p = k we have a In case of det() = 0 distribution is not involutive

Global and local accessibility

To summarize what has been said so far: the presence of constraints non-holonolomous implies a tying of the conguration space of the speed, but not the positional conguration of the mechanical system. not. A typical example of this situation is the lateral displacement of the mobile robot, which is locally constrained to move only in a conguration space which coincides with the direction of translation that is perpendicular to the wheel hub. The local motion space is delimited by the two semicircles centered in the centers of instantaneous rotation and tangent to vertical displacement. Globally, it can access congurations locally constrained by making use of a sequence of local motions (maneuvering). Given a matrix of constraint Bringing these notions to a mechanical system such as a mobile robot globally implies that all congurations remain accessible to the system, while locally part of the congurations are

AT (q),

the conguration space of generalized speed can be rep-

resented by the null-space (kernel) of the matrix, a distribution of linearly independent vectors:

N (AT (q)) = [g1 (q), ..., gnk (q)] = G(q)

from which it can characterize the admissible trajectories of the mechanical system dened in state space:

= f (q) + G(q) u q
where the function

f (q)

represents the internal dynamics of the system. The accessibility degree

may be evaluated using the system accessibility matrix :

RA=m =

n1 G adf G ad2 adf G f G ... (1)

which coincides locally with the accessibility matrix of a linear system. Globally, the accessibility may be reduced only by holonomic constraints. In fact system completely non-holonolomous has accessibility space which is equal to full conguration space.

4 Even if a constraint taken alone can be non-holonomic, a multiplicity of constraints can be completely integrable. 5 A very accurate description, with proof of this theorem is in the book: A. Isidori, Nonlinear Control Systems:
An Introduction, Chapter 1: Local Decomposition of Control Systems, 1985

6 adk g = [f, adk1 g ] f f

Robotics and sensor data fusion | 5

Accessibility space

Compute the accessibility space for the following representative kinematic models after having re-obtained them from the original kinematic equations:

Unicycle (ideal, single wheel):

sin() cos() 0
Car like front driven (FD):

T 0 0 1

x y q=

T cos() sin() 0 sin() cos() 0 sin()/l 0 0 1

Car like rear driven (RD):

v1F

v2

x y q=

cos() sin() tan()/l 0

T 0 0 0 1

v1R

v2

x y q=

Introduce briey the advantages in adopting Pfaan constraints.

Unicycle
The model of the wheel that will use is the following: In particular, it is:

vn=0

. vt=R

Figure 1: Wheel model - no slip

vt vn
model:

= R = 0

Taking the model of unicycle: Dened feed speed, you get one of the kinematic constraints of the

x = v cos() y cos() x sin() = 0 y = v cos(/2 ) = v sin()

Robotics and sensor data fusion | 6

. y=v sin

/2

. x=v cos

x
Figure 2: Unicycle model and feed speed projections

from which is obtained:

= AT (q ) q
The kernel of the matrix

sin()

cos()

=0 q

AT ()

represents the distribution of functions of the kynematic model

(there are dierent vector that one could choose to represent this kernel, we use one that is confortable for us, as we will see in the next exercise):

N (AT (q)) =

g1

g2

cos() = sin() 0

0 0 1

cos() x 0 y sin() 0 = v+ 0 1
The accessibility may be calculated using Lie algebra:

[g1 , g2 ]

= =

g2 g1 g1 g2 = q q 0 0 cos() sin() [0] 0 0 0 0 0 sin() cos() 0 cos() = sin() 0

sin() 0 cos() 0 = 0 1

Accessibility matrix is:

RA=2
and the determinant is:

0 sin() 0 cos() 1 0

det (RA=2 ) = ( cos2 () sin2 ()) = 1

from which it results that the degree of controllability of the system is equal to non-holonomicity egual to

3,

with a degree of

2.

Robotics and sensor data fusion | 7

yf

x
Figure 3: Car Like model

xf

Car Like
From the picture we can draw the following conclusions, applying the constraints of pure rolling exploited in the case of the unicycle to the wheels of this model:

constraint on the rear wheel:

x sin() y cos() = 0
constraint on the front wheel:

x f sin( + ) y f cos( + ) = 0
position of the front wheels, as rigid body constraint:

xf = x + l cos() yf = y + l sin()
that dierentiating becomes:

sin() x f = x l y f = y + l cos()
The location constraint dierentiated can be replaced in the equation of the front axle as follows:

x f sin( + ) y f cos( + ) = 0 (x l sin()) sin( + ) (y + l cos()) cos( + ) = 0 (sin() sin( + ) + cos() cos( + ) = 0 x sin( + ) y cos( + ) l
analyzing the trigonometric element in parentheses and exploiting the expansion of the sum between angles:

sin() cos() + cos() sin() cos() cos() sin() sin() sin() sin( + ) + cos() cos( + ) (cos2 () + sin2 ()) cos() + (sin() cos() sin() cos()) sin()
that will take us to a new constraint expression for the front wheels:

= = = =

sin( + ) cos( + ) cos()

cos() = 0 x sin( + ) y cos( + ) l

Robotics and sensor data fusion | 8

At this point, the equations can be reported in the usual Pfaan form:

= AT (q) q

sin() cos() 0 sin( + ) cos( + ) l cos()

0 0

x y =0

which has rank 2. So

AT 's null space will be of dimension n k = dim(q) rk(AT ) = 2 represented cos() cos() 0 sin() cos() 0 = sin()/l 0 0 1

by means of two linearly independent vectors (bases of null space):

N (AT ) =

g1

g2

from which we obtain the model in state space:

= q x y =
where

g1 (q) u1 + g2 (q) u2 cos() cos() sin() cos() u1 + sin()/l 0 ,

while

0 0 0 1

u2

(1)

u2

corresponds to the steering angular velocity

u1

corresponds to the speed of the

mobile robot and depends on the axis to which it is applied, front (FD) or rear (RD).

Car Like FD
In FD case:

u1 = v
and then the model is quite equal to that of equation 1:

x cos() cos() y sin() cos() = sin()/l 0

0 0 = g1 v + g2 v+ 0 1

Using the Lie algebra we can estimate the accessibility space:

RA=3 =
where:

g1

g2

[g1 , g2 ]

[g1 , [g1 , g2 ]]

g1 g2 g1 g2 = [g1 , g2 ] = q q cos() cos() 0 0 sin() cos() 0 0 = [0] 0 0 sin( ) /l 0 0 0 cos( ) sin( ) sin() sin() = cos()/l 0

sin() cos() cos() sin() 0 cos() cos() sin() cos() 0 0 cos()/l 0 0 0 1

g1 [g1 , g2 ] g1 [g1 , g2 ] = [g1 , [g1 , g2 ]] = q q 0 0 sin() sin() cos() cos() cos() cos() 0 0 cos() cos() sin() sin() sin() cos() = 0 0 0 sin()/l sin()/l 0 0 0 0 0

Robotics and sensor data fusion | 9

 0 0 sin() cos() cos() sin() cos() sin() 0 0 cos() cos() sin() cos() sin() sin() 0 0 0 cos()/l cos()/l 0 0 0 0 0 2 2 (sin ( ) + cos ( )) sin( ) /l sin( ) /l cos()/l (sin2 () + cos2 ()) cos()/l = = 0 0 0 0
Is obtained:

RA=3

cos() cos() 0 cos() sin() sin()/l sin() cos() 0 sin() sin() cos()/l = sin()/l 0 cos()/l 0 0 1 0 0 det(RA ) = 1 =0 l2 3.

whose determinant is:

therefore the system is controllable with a degree of non-holonomicity equal to

Car Like RD
In RD case:

u1 =
and the model in state space becomes:

v cos()

x cos() y sin() = tan()/l 0

building the accessibility matrix:

0 0 = g1 v + g2 v+ 0 1

g2 g1 [g1 , g2 ] = g1 g2 = q q 0 0 sin() 0 0 0 cos() 0 = 0 0 0 1/(l cos2 ()) 0 0 0 0

cos() 0 sin() 0 0 [0] tan()/l 0 1

0 0 = 2 1 / ( l cos ()) 0

[g1 , g2 ] g1 g1 [g1 , g2 ] = [g1 , [g1 , g2 ]] = q q 0 0 0 0 cos() 0 0 0 0 sin() 0 0 0 2 tan()/ cos2 () tan()/l + 0 0 0 0 0 0 sin() 0 0 0 0 cos() 0 0 2 0 0 0 1/(l cos2 ()) 1 / ( l cos ()) 0 0 0 0 0
the matrix:

sin()/(l cos2 ()) cos()/(l cos2 ()) = 0 0 sin()/(l cos2 ()) cos()/(l cos2 ()) 0 0

RA=3

cos() sin() = tan()/l 0

0 0 0 1/(l 1

0 0 cos2 ()) 0

which determinant is:

det(RA=3 ) =

1 =0 l cos3 ()

Robotics and sensor data fusion | 10

for

non-holonomicity

+ k 2

k N7 . equal to 3.
for

Even in this case, the system is controllable with a degree of

Matlab Project

Assume the unicycle kinematic model with dierential drive (no uncertainties about the mechanical system size), and a  suciently smooth path adopting the chained-form trajectory planning method  powered by constant inputs. The coordinate change in chained-form have to be obtained via Lie algebra. Therefore, invert the equations and compute the kinematic model inputs for allowing the unicycle to follow the assigned path in the assigned time interval (normalize/rescale/parametrize the time-law between 0 and 1). Moreover, dene an output function for this MIMO nonholonomic system and motivate the choice. Then, apply the linearizing feedback (in the new coordinates). Compute the linearizing feedback via Lie algebra from the new coordinates where

(the new set of generalized coordinates) and,

only for didactic purposes, rewrite it in the old set of generalized coordinates Finally model a perturbation (time-limited and not constant).

For compensating the trajectory  space-time oset apply a control (trajectory tracker) and choose the inputs required for obtaining  zero dynamic in order to reduce the quantity:

e = yd (t) y(t)
Explain why, in this case, the  strictly normal form for the generalized coordinate change does not exist in the set of possible solutions (be concise: one short sentence, now you already should have the answer...).

Why the trajectory has to be coordinates?

(or suciently smooth) in respect to the generalized

Why do we need to apply the  zero dynamic approach?

Theoretic approach
There are two main theoretic approach that we will use to control our system:

feedback linearization in chained form; input/output linearization.

The model of dierential unicycle is simplied by transforming the angular velocities of the two wheels in speed and angular velocity of the system:

r/2 r/2 r/d r/d

r l

Feedback linearization
8

Theroy of exact feedback linearization is quite complex and requires In this work, is supposed that the reader already know this

dierent concept to be explained.

nonlinear control theory . The problem is to identify an output function dene a coordinate transformation from this typical nonlinear system:

h(x)

that allows us to

x y
7 Condition 8 Refer

= f (x) + g(x) u = h(x)

] /2, /2[, = /2
front wheels become a pivot for the system.

that appears to be always satised, since the steering angle turns out to be always:

for a RD mobile robot. It is easy to immagine that for

to: A. Isidori, M.D. di Benedetto Feedback linearization of nonlinear systems and Non linear Zero

dynamics in chapter 57. Design Methods of AA.VV. The control handbook, 1996, CRC Press IEEE

Robotics and sensor data fusion | 11

to this linear system:

z i z r zj

= zi+1 = v = q (z)

i = 1, ..., r 1 j = r + 1, ..., n

y = z1
that is linear in the controllable part (identied by the rst refer to the the form:

relative degree of the system. This subsystem is called also chained form). The trasformation is a dieomorphism that must be dened at least locally and a control action in
h(x) Lf h(x)
. . .

component of

z,

where also

= (x) =

1 Lr h(x) f (x) r +1 . . . n (x)

+ v)

u =
There exists a function are satised:

1 (Ln f h(x) n1 Lg Lf h(x)

h(x)

that allows to dene this map, if and only if the following conditions

the matrix the set

n1 [g(x), adf g(x), ..., adf g(x)]

has rank

n;

2 span{g(x), adf g(x), ..., adn g(x)} f

is involutive.

Now we expand this notion from SISO system to a MIMO system. For a we dene a new type of generalized vector's dimension:

chained form for system in which relative degree r is equal to n that is

= 1 (z) v1 + 2 (z) v2 z
T
and,

(2 input, n output)-system

with

1 (z) = [1, 0, z2 , z3 , ..., zn ]

2 (z) = [0, 1, 0, ..., 0]T . 0

. . .

If we evaluate the accessibility

space of this system:

k 1 adk 1 2 (z) = [1 , ad1 2 (z)] =

(1)k
. . .

@ position k + 2

2 A = span{1 , 2 , ad1 2 , ..., adn 1 2 }

we get a distribution of full rank (n), and a system that is always controllable with a degree of non-holomicity for a driftless

k = n 1. There are some conditions to prove the existence (2 input, 3 output)-system, like our unicycle; from our system: = g1 (q) u1 + g2 (q) u2 q

of a output function

we denes three distributions:

0 1 2

= = =

span{g1 , g2 , adg1 g2 } span{g2 , adg1 g2 } span{g2 }

Robotics and sensor data fusion | 12

if

ential

dim(0 ) = n = 3 dh1 satises

and

{1 , 2 }

is involutive, there exists a scalar function

h1 (q)

whose dier-

dh1 1, i dh1 g1
for which the system could transformed in:

= =

0 1

i = {1, 2}

z1 z2 z3
where

= h1 = Lg1 h2 = h2 h1 (q)
and such that:

h2 (q)

must be chosen independent of

h2 2 = 0
And the input transformation is:

v1 v2

= u1 = L2 g1 h2 u1 + Lg2 Lg1 h2 u2

Now we are ready to apply this input transformation to the unicycle model:

cos() 0 x y = 0 + sin() v 0 1
now we make all computations to create the three distributions

{0 , 1 , 2 }:

adg1 g2

g1 g2 g1 g2 = [g1 , g2 ] = q q 0 0 sin() 0 cos() 0 sin() = 0 0 cos() [0] = 0 0 0 1 0 sin() cos() = 0 =

2
where:

0 cos() sin() 0 sin() cos() = span 1 0 0 cos() sin() sin() cos() = span 0 0 cos() sin() = span 0

0 dim(0 ) : det 0 1
that conrm also that

cos() sin() sin() cos() = (cos2 () + sin2 ()) = 1 = 0 dim(0 ) = 3 0 0

For a sake of semplicity we choose as rst function:

{1 , 2 } are involutive.

h1 (q) =

Robotics and sensor data fusion | 13

and we use conditions of transformation:

dh1 =
the system:

cos() sin() dh1 1, 1 = 0 0 1 =0 0 sin() cos() dh1 1, 2 = 0 0 1 =0 0 0 0 dh1 g1 = 0 0 1 =1 1

The conditions on

True

True

True

h1

are respected. As

h2 (q)

we can use the function:

h2 (q) = x sin() y cos() dh2 =

and the condition:

sin() cos() x cos() + y sin() cos() sin() = 0

dh2 2

= =

sin() cos() x cos() + y sin() sin() cos() sin() cos() = 0 True

Because all conditions are respected, those functions could be used for a transformation, after a Lie derivative:

Lg1 h2

= =

h2 g1 = q sin() cos() x cos() + y sin() 0 0 = 1

= x cos() + y sin()
and the trasformation:

x cos() + y sin() z = T(q) = x sin() y cos() 0 0 = y cos() 1

the input tasformation is based upon Lie derivatives of the second order:

L2 g1 h2 =

(Lg1 h2 ) g1 = q

cos()

sin() y cos() x sin()

x sin() = z3 Lg2 Lg1 h2 = sin2 () = 1

and the input transformation:

(Lg1 h2 )g2 = q

cos()

cos() sin() y cos() x sin() sin() = cos2 ()+ 0

v1 v2

= u1 = = L2 g1 h2 u1 + Lg2 Lg1 h2 u2 = = v + (y cos() x sin()) = = v z3

Robotics and sensor data fusion | 14

we dene also the inverse trasformation, for both states and inputs:

z2 cos(z1 ) + z3 sin(z1 ) z2 sin(z1 ) z3 cos(z1 ) q = T1 (z) = z1 v = v1 = v2 + z3 v1

Now we make a dierentiation, that will give us the chained system:

dz1 = = v1 dt dz2 dt = x cos() x sin() + y sin() + y cos() = = = ( x

v cos( ) 2

cos() +

y
v sin( )

sin()) + (y cos() x sin()) =

v cos () + v sin2 () + (y cos() x sin()) =

= v + (y cos() x sin()) = = v + z3 = = v2 dz3 dt = x sin() + x cos() y cos() + y sin() = = (x sin() y cos()) + (x cos() + y sin()) =
=0 (constraint)

= z2 = = z2 v1
summing up the new model is:

z 1 z 2 z 3
We have chosen those two functions

= v1 = v2 = z2 v1
because they linearize the system with a new set of

{h1 , h2 },

coordinates that has a physical interpretation:

z1 z2 z3

represents angle

represents direction parallel to sagittal axis of the unicycle; represents direction orthogonal to

z2 .

In the next section is shown how to control a simple cubic path, that is sucently smooth to respect nonholonomic constraints, with an external linear error compensation:

x = ax t3 + bx t2 + cx t y = ay t3 + by t2 + cy t = atan2(y, x )
but we have to said that it is really dicult to nd in litterature trajectory follow problem with this kind of decomposition because of the stiness that could be introduced during trajectory planning, that is also why in this model there is no external error simulation. In fact, this control loop is used mainly in posture regulation, in combination with a polar input, due to the fact that be considered as a radius, and litterature.

z2

could

z1

as an orientation.

The next decomposition tends to be more

robust due to a redenition of the output, and in fact is the one that we nd as the more used in

Robotics and sensor data fusion | 15

Input/output linearization

If we use

input/output linearization, than we can directly use

0 0 1

the model of unicycle, choosing as output a slightly dierent point from the center of the axle shaft, in such a way as to obtain the complete observability:

x y = y1 y2 =

cos() sin() 0

v =

= [g1 g2 ] u h1 h2

x + b cos() y + b sin()

from which we can derive the feedback:

h1 g1 = Lg1 h1 = q h1 g2 = q h2 g1 = q h2 g2 = q

Lg2 h1 =

Lg1 h2 =

Lg2 h2 =

cos() b sin() sin() = cos() 0 0 b sin() 0 = b sin() 1 cos() b cos() sin() = sin() 0 0 b cos() 0 = b cos() 1 A(q) = cos() b sin() sin() b sin()

(det(A)

= b > 0)

and its inversion:

A1 (q) =
from which we can evaluate:

cos() sin()/b

sin() cos()/b

u v

= A 1 v v1 cos() + v2 sin() = 1 1 b sin( ) v1 + b cos( ) v2

and using the following coordinate transformation:

z1 x + b cos() x z1 b cos(z3 ) z2 y + b sin() y z2 b sin(z3 ) = = z3 z3

and chained form:

z1 z2 = z3 y1 y2 =

v1 v2 1 b (v2 cos(z3 ) v1 sin(z3 )) z1 z2

to which to apply the error of the trajectory, drawn as a at output. For the trajectory

x = 5 sin(10 t) y = 5 sin(20 t) yd = = atan2(y, x )

t [0, 1]

Robotics and sensor data fusion | 16

we consider the error

e = yd y
At this point, it can be concluded by exploiting the zero dynamics as linear control law:

v=
where e

z 1d k1 (z1d z1 ) z 2d k2 (z2d z2 )
exponentially stable for

k1 , k2 are two control parameters. Non linear control result k2 > 0. The anti-transformation is x z1 b cos() y z2 b sin() = z3

k1 > 0

and we can go back to the control action by the following relationships:

r l

1/r 1/r

d d

cos(z3 ) b sin(z3 ) sin(z3 ) b cos(z3 )

v1 v2

Here are some results of the simulation and implementation of the Matlab/Simulink model. Both model are based upon the concept of at output, that derives directly from the zerodynamics .

Simulink models
with respect to

Feedback linearization
The results of simulation can be seen in the next few images. As already said, the system converge

z1 ,

that represents orientation, and tends to mantain a costant error (if we not

consider derive due to integration) in position. The control scheme could be improved using a PI controller, instead the only P used. However, the control action is to be contained.

Unicycle model 16 14 12 10 8 6 4 2 0 2 trajectory desired trajectory followed 0 2 4 6 x [m] 8 10 12

y [m]

Figure 4: Trajectory

9 Refer

to: A. Isidori, M.D. di Benedetto Feedback linearization of nonlinear systems and Non linear Zero

dynamics in chapter 57. Design Methods of AA.VV. The control handbook, 1996, CRC Press IEEE

Robotics and sensor data fusion | 17

Error 1

0.5

0 error 0.5 1

eZ1 e e
Z2 Z3

1.5

4 time [s]

10

Figure 5: Errors

Control Action 6 4 2 0 Control Action 2 4 6 8 10 12 14 0 2 4 time [s] 6 8 V1 V2 10

Figure 6: Control Actions

[v1 , v2 ]

This is the complete Symulink Model and the startup script. For blocks implementation, see the compressed le that can be downloaded from here.
1

% % Simulation for a unicycle % The system is linearized with a feedback linerization , and thus obtains % the form : %

Robotics and sensor data fusion | 18

% $$ \ dot { z_ {1}} = v_ {1} $$ % % $$ \ dot { z_ {2}} = v_ {2} $$ % % $$ \ dot { z_ {3}} = v_ {1} z_ {2} $$ clear all ; close all ; clc ; %% % Initial conditions X0 = 1; Y0 = 1; TH0 = pi /4;

11

16

21

%% % Controller Gain . It is a $$2 \ times3 $$ matrix . $$dim ( e ) = 3 $$ and we % have a correction of dimension $$2 $$ Gain = [ 10 0 0; 0 10 10];
26

% % Simulation % % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%% if 1 == 1 sim ( ' Model ') ; end % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % % Results figure ( ' color ' , 'w ') % Desired trajectory plot ( Trajectory (: ,1) , Trajectory (: ,2) , 'g - - ') , hold on , ... % Robot motion plot ( Results (: ,1) , Results (: ,2) , 'r ') , hold on , ... legend ( ' trajectory desired ' , ' trajectory followed ' , ' Location ' , ' SouthEast ') , ... % Robot model for i =1:150: size ( Results ,1) DrawUnicycle ( Results (i ,1) , Results (i ,2) , Results (i ,3) ) , ... hold on , ... end grid on , ... xlabel ( 'x [ m ] ') , ylabel ( 'y [ m ] ') , ... title ( ' Unicycle model ') figure ( ' color ' , 'w ') % Error on z1 plot ( time , Error (: ,1) , 'r ') , hold on , ... % Error on z2 plot ( time , Error (: ,2) , 'g ') , hold on ... % Error on z3 plot ( time , Error (: ,3) , 'b ') , ... grid on , xlabel ( ' time [ s ] ') , ylabel ( ' error ') , ... legend ( ' e_ { Z1 } ' , ' e_ { Z2 } ' , ' e_ { Z3 } ' ,' Location ' , ' SouthEast ') , ... title ( ' Error ')

31

36

41

46

51

56

61

Robotics and sensor data fusion | 19

66

71

figure ( ' color ' , 'w ') % Control Action V1 plot ( time , ControlAction (: ,1) , 'r ') , hold on , ... % Control Action V2 plot ( time , ControlAction (: ,2) , 'b ') , ... grid on , xlabel ( ' time [ s ] ') , ylabel ( ' Control Action ') , ... legend ( ' V_ {1} ' , ' V_ {2} ' , ' Location ' ,' SouthEast ') , ... title ( ' Control Action ') %% % Trajectory function , contained in symulink model % % % % % % % % % % % % % % % % function [X , Y , Xdot , Ydot , TH ] = trajectory ( t ) %# codegen A = 0.003; B = 0.05; C = 0.2; D = 0.001; E = 0.07; F = 0.8; X = A * t ^3 + B * t ^2 + C * t ; Y = D * t ^3 + E * t ^2 + F * t ; Xdot = 3* A * t ^2 + 2* B* t + C ; Ydot = 3* D * t ^2 + 2* E* t + F ; TH = atan2 ( Ydot , Xdot ) ;

76

81

86

91

% This document could be published with Matlab publishing function

Robotics and sensor data fusion | 20

Input/output linearization

The results of this simulation are, as aspected, much better, due to the dention of the chain system. The main dierence with respect to the previous model can be seen in the fast convergence of the error, characteristic that make this model the most used in trajectory following problems. We can see in

e1 vs e2

plot, that even with an external error source (5 rand([1, ..., 1],

= 0, 2 = 1)),

the system could mantain the error very limited to an ellipsys with very small radius. The control action is provided as wheels rotational speed, it is quite contained but is evident the eect of random white noise.

Unicycle model 3 trajectory desired trajectory followed 2

0 y [m] 1 2 3 4 4

0 x [m]

Figure 7: Trajectory

Robotics and sensor data fusion | 22

Error 0.05 0 0.05 0.1 0.15 error [m] 0.2 0.25 0.3 0.35 0.4 0.45 0 2 4 time [s] 6 8 e [m]
x y

e [m] 10

Figure 8: Errrors

Differential drive control action 80 r 70 60 Angular speed [rad/s] 50 40 30 20 10 0 10 l

4 time [s]

Figure 9: Control Action

Robotics and sensor data fusion | 23

Error 1 vs 2 0.05 0 0.05 0.1 0.15 eZ2 0.2 0.25 0.3 0.35 0.4 0.45 0.45 0.4 0.35 0.3 0.25 0.2 0.15 eZ1 0.1 0.05 0

Figure 10:

e1 vs e2

Position errors for system without random errors on input 0.05 0 0.05 0.1 0.15 error [m] 0.2 0.25 0.3 0.35 0.4 0.45 0 2 4 time [s] 6 8 eZ1 [m] eZ2 [m] 10

Figure 11: Errors for a model without white noise

This is the complete Symulink Model and the startup script. For blocks implementation, see the compressed le that can be downloaded from here.

% % Simulation for a unicicle in input / output linearization % The system is linearized with a input output , and thus obtains % the form : %

Robotics and sensor data fusion | 24

% $$ \ dot { z_ {1}} = v_ {1} $$ % % $$ \ dot { z_ {2}} = v_ {2} $$ % % $$ \ dot { z_ {3}} = \ frac {1}{ b } ( v_ {2} cos ( z_ {3}) - v_ {1} sin ( z_ {3}) ) $$ clear all close all clc

10

15

%% % Sampling time Ts = 10^ -3;

20

%% % Path control tmX tmY RadX RadY = = = = 2; 1; 3; 3;

25

%% % Model parameters
30

d = 0.135; r = 0.11; b = r; PS = 5; % Perturbation control

35

40

%% % Initial Condition x0 = 0.5; y0 = 0.5; th0 = pi /3; %% % Controller Gain k1 = 100; k2 = 100; % % Simulation %

45

50

% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%% if 1 == 1 sim ( ' Unicycle3 ') ; end % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % % Results figure ( ' color ' , 'w ') % Desired trajectory plot ( Xd (: ,1) ,Xd (: ,2) , 'g -- ') , hold on , ... % Robot motion plot ( Qout (: ,1) , Qout (: ,2) , 'r ') , hold on , ... legend ( ' trajectory desired ' , ' trajectory followed ') , ... % Robot model for i =1:150: size ( Qout ,1)

55

60

65

Robotics and sensor data fusion | 25

70

end grid on , ... xlabel ( 'x [ m ] ') , ylabel ( 'y [ m ] ') , ... title ( ' Unicycle model ')

DrawUnicycle ( Qout (i ,1) , Qout (i ,2) , Qout (i ,3) ) , ... hold on , ...

75

80

figure ( ' color ' , 'w ') % Error on y1 plot ( time , error (: ,1) , 'r ') , hold on , ... % Error on y2 plot ( time , error (: ,2) , 'b ') , ... grid on , xlabel ( ' time [ s ] ') , ylabel ( ' error [ m ] ') , ... legend ( ' e_ { x } [ m ] ' , ' e_ {y } [ m ] ' , ' Location ' , ' SouthEast ') , ... title ( ' Error ') figure ( ' color ' , 'w ') % Control action plot ( time (100: end ,1) ,U (100: end ,1) , 'r ') , hold on , ... plot ( time (100: end ,1) ,U (100: end ,2) ) , ... grid on , ... xlim ([1 ,6.5]) , ... legend ( ' {\ omega } _ { r } ' , ' {\ omega } _ { l } ') , ... xlabel ( ' time [ s ] ') , ylabel ( ' Angular speed [ rad / s ] ') , ... title ( ' Differential drive control action ') figure ( ' color ' , 'w ') % Error action e1 on e2 plot ( error (: ,1) , error (: ,2) ) , ... grid on , ... xlabel ( ' e_ { Z1 } ') , ylabel ( ' e_ { Z2 } ') , ... title ( ' Error 1 vs 2 ') figure ( ' color ' , 'w ') % Error on y1 plot ( time , error_2 (: ,1) , 'r ') , hold on , ... % Error on y2 plot ( time , error_2 (: ,2) , 'b ') , ... grid on , xlabel ( ' time [ s ] ') , ylabel ( ' error [ m ] ') , ... legend ( ' e_ { Z1 } [ m ] ' , ' e_ { Z2 } [ m ] ' , ' Location ' , ' SouthEast ') , ... title ( ' Position errors for system without random errors on input ') % This document could be published with Matlab publishing function

85

90

95

100

105

110

Robotics and sensor data fusion | 26