Tracking, Motion Generation and

Active Sensing of Nonholonomic

Wheeled Mobile Robots
Lyudmila Mihaylova
Katholieke Universiteit Leuven

Content
Holonomic and nonholonomic robots
Basic methods for tracking tasks
Motion generation
Active sensing
 What Distinguishes a
Nonholonomic System From a
Holonomic System?

■ With a holonomic system, return to the

original joint configuration means return
to the original position. For a
nonholonomic system, return to the
original wheel configuration does not
guarantee return to the original system
position.
■ The motion of a nonholonomic robot is
path-dependent.
Lyudmila Mihaylova, Katholieke Universiteit Leuven, Division PMA
 Tracking the WMR motion
■ Tracking: a wheeled mobile robot (WMR) is
moving in an environment (known, or
unknown, with or without obstacles) and its
motion is tracked using the sensor data;
■ Basic methods: Kalman filtering techniques
(linear and EKFs, multiple-model estimators),
Monte Carlo methods (bootstrap, Gibbs
samplers, particle filters, unscented particle
filters), derivative-free algorithms, Daum
filters, etc.
■ New advanced methods are needed to cope
with the 3N: nonlinearity , nonstationarity
Lyudmila Mihaylova, Katholieke , non
Universiteit Leuven, Division PMA
 Motion generation
■ Main motion tasks : control problem U
=?
* Point-to-point motion : the robot must reach a
desired goal configuration starting from a
given initial configuration.
* Trajectory tracking: a motion should be
generated so that the WMR reaches and
follows a trajectory in the Cartesian space (i.e.
a geometric path with an associated timing
law) starting from a given initial configuration
(on or off the trajectory).
http://www.laas.fr/~jpl/book.html : J.-P.
Lyudmila Mihaylova, Katholieke Laumond,
Universiteit Robot
Leuven, Division PMA
Motion Planning, Springer-Verlag, 1998.
 Main Motion Generation
Tasks
I. Point-to-point motion

Goal
II. Trajectory tracking

Start

Lyudmila Mihaylova, Katholieke Universiteit Leuven, Division PMA

Open area of research due to
...
■ The nonholonomic character of the systems: the
dimension of the controls is less than the dimension of
the configurable variables (states). The main
consequence of a nonholonomic constraint is that not
each path from the admissible configura- tion space
corresponds to a feasible trajectory for the robot.
■ The task solution is dependent on the optimality
criterion. It should be such that maximum information is
extracted from the sensor data and at the same time
processed in a computational- ly efficient way.
■ It is related to the computational load (time, number of
operations). The generated motions are needed to be
executed on line.
Lyudmila Mihaylova, Katholieke Universiteit Leuven, Division PMA
 Open area of research due
to...
■ The nonlinear character of the problem poses questions
about the system controllability. The main methods deal
with special classes of nonholonomic systems. The
nonlinear model is reduced to a linear, easier to deal with
(chained forms, Goursat normal forms or other linear
representations);
■ Obstacles avoidance adds additional level of difficulty.
Often solutions in this case rely on a combination between
geometric techniques for obstacles avoidance with
advanced control techniques. Steering methods rely on
topological properties of the environment map or other
learning techniques.

■ Other uncertaintiesLyudmila
(in the Mihaylova, Katholieke Universiteit Leuven, Division PMA
model, sensor data, etc.)
 Active Sensing
The main question to answer:”Where to move
next?”
Given a current knowledge about the robot state and
the environment, how to select the next sensing action
or sequence of actions. A vehicle is moving
autonomously through an environment gathering
information from sensors. The sensor data are used to
generate the robot actions.
Beginning from a starting configuration (xs,ys,φs) to a
goal configuration (xg,yg,φg) in the presence of a
reference trajectory and without it; With and without
obstacles; Taking into account the constraints on the
Lyudmila Mihaylova, Katholieke Universiteit Leuven, Division PMA
velocity, steering angle, the obstacles, and other
 Active Sensing of a WMR
Robot model
y  
y  xk +1   xk + vk ∆T cos(φk + ψ k )  η x ,k 
yB Beacon      
ψk  yk +1  =  yk + vk ∆T sin(φk + ψ k )  + η y ,k 
φ   vk ∆T  η 
L  k +1   φk + sinψ k   φ ,k 
yk φk  L 

Measurement model
xB xk x  ( x − x )2 + ( y − y )2 
 rk   B k B k   ξ r ,k 
  =   y B − yk   +  
θ k   a tan  ξ
 −ψ k   θ ,k 
  x B − xk  
Lyudmila Mihaylova, Katholieke Universiteit Leuven, Division PMA
Highly nonlinear models !
 Trajectory optimization
■ Between two points there are an infinite number of
possible trajectories. But not each trajectory from the
configuration space represents a feasible trajectory for
the robot.
■ How to move in the best way according to a criterion
from the starting to a goal configuration?
■ The key idea is to use some parameterized family of
possible trajectories and thus to reduce the infinite-
dimensional problem to a finitely parametrized
optimization problem. To characterize the robot motion
and to process the sensor information in efficient way,
an appropriate criterion is need. So, active sensing is a
decision making, global optimization problem sub- ject
to constraints. Lyudmila Mihaylova, Katholieke Universiteit Leuven, Division PMA
Trajectory Optimization
■ Let Q is a class of smooth functions. The problem of
determining the ‘best’ trajectory q with respect to a
criterion J can be then formulated as
q = argmin(J)
where the optimization criterion is chosen of the form
J = min{ c1I + c2C}
Ai ,k
information part losses (time, traveled
distance)
subject to constraints
l y ,k ≤ l y ,max , vk ≤ vmax , ψ k ≤ ψ max , d o,k ≥ d o,min , 

Lyudmila Mihaylova, Katholieke Universiteit Leuven, Division PMA

l: lateral deviation, v: WMR velocity;      : steering angle;  d  : distance to 
Trajectory Optimization
The class Q of harmonic functions is chosen,
Q = Q(p),

p: vector of parameters obeying to preset constraints;

Given N number of harmonic functions, the new modified
robot trajectory is generated on the basis of the
reference trajectory by a lateral deviation as a linear
superposition
N  sr , k 
l y ,k = ∑ Ai ,k sin iπ
 
 s 
i =1  r , fin 

Lyudmila Mihaylova, Katholieke Universiteit Leuven, Division PMA

Why harmonic functions?
■ They are smooth periodic functions;
■ Gives the possibility to move easily the robot to the
desired final point;
■ Easy to implement;
■ Multisinusoidal signals are reach excitation signals and
often used in the experimental identification. They have
proved advantages for control generation of
nonholonomic WMR (assure smooth stabilization). For
canonical chained systems Brockett (1981) showed that
optimal inputs are sinusoids at integrally related
frequencies, namely 2π, 2. 2π, …, m/2. 2π.

Lyudmila Mihaylova, Katholieke Universiteit Leuven, Division PMA

 Optimality Criterion
J = min{ c1I + c2C}
Ai ,k

I = trace(WP), or averaged I = 1 kb
∑ trace(WPk )
k a − kb k = k a
I is computed at the goal configuration or on the
the whole trajectory (part of it, e.g. in an[kinterval a , kb ]

)
where W = MN; {
N = diag σ x− 2 , σ −y 2 ,σ φ− 2 }
M: scaling matrix; N: normalizing matrix:
P: estimation error covariance matrix (information
Lyudmila Mihaylova, Katholieke Universiteit Leuven, Division PMA
matrix or entropy) from a filter (EKF);
Trajectory (N=2 sinusoids)
19
B eaco n

18
N=1
y   p o s it io n ,   [ m ]

N=2
17

16

N=0
15

14
0 2 4 6 8 10 12

x   p o s it io n ,   [ m ]

Lyudmila Mihaylova, Katholieke Universiteit Leuven, Division PMA

Trajectory (N=3 sinusoids)
19
B eaco n

18 N=3
y   p o s it io n ,   [ m ]

17

16

N=0
15

14
0 2 4 6 8 10 12

x   p o s it io n ,   [ m ]

Lyudmila Mihaylova, Katholieke Universiteit Leuven, Division PMA

Trajectory (N=5 sinusoids)
19
B eaco n

18 N=5
y   p o s it io n ,   [ m ]

17

16

N=0
15

14
0 2 4 6 8 10 12

t im e ,   [ s e c ]

Lyudmila Mihaylova, Katholieke Universiteit Leuven, Division PMA

Evolution of trace(WP) in
time

N=0
4
tra c e (W P )

2 N=3
N=2 N=1

1
N=5

0
20 40 60 80 100

t im e Katholieke
Lyudmila Mihaylova, ,   [ s e c ] Universiteit Leuven, Division PMA
 Criterion value at the goal
configuration
J = min{ c1I + c2C}
Ai ,k
I = trace(WP), C = t fin / tr , fin
N 0 1 2 3 5
I 3.00 2.88 2.76 2.42 2.10
C 1 1.14 1.15 1.20 1.21
J 3.1 2.99 2.88 2.54 2.29
c1 = 1, c2 = 0.1
Conclusion:
the bigger N it is, better the accuracy is, but theKatholieke
Lyudmila Mihaylova, computational complexity
Universiteit also increases
Leuven, Division PMA !
Results with two beacons
19
B e a c o n  1
18
N=3 N=5
17
N=2
y   p o s it io n ,   [ m ]

16
N=0
15

14

13

12
B e a c o n  2
11

0 2 4 6 8 10 12

x   p o s it
Lyudmila Mihaylova, io n ,   [ m
Katholieke ]
Universiteit Leuven, Division PMA
With an obstacle
19
B eaco n

18
N=3
y   p o s it io n ,   [ m ]

O b sta cle
17

16

N=0
15

14
0 2 4 6 8 10 12

x   p o s it io n ,   [ m ]

Lyudmila Mihaylova, Katholieke Universiteit Leuven, Division PMA

Trajectories with
constraint on the
orientation angle
19
B eaco n

18
N=3
y   p o s it io n ,   [ m ]

N=5
17
N=2

16

N=0
15

14
0 2 4 6 8 10 12

x   p o s it io n ,   [ m ]

Lyudmila Mihaylova, Katholieke Universiteit Leuven, Division PMA

The information criterion
I1

N=0
4
tra c e (W P )

2
N=2
N=3
1
N=5

0
20 40 60 80 100

t im e ,   [ s e c ]

Lyudmila Mihaylova, Katholieke Universiteit Leuven, Division PMA

Trajectories with constraint
on the orientation angle and
I2
19
B eaco n
N=3
18

N=5
y   p o s it io n ,   [ m ]

17 N=2

16

N=0
15

14
0 2 4 6 8 10 12

x   p o s it io n ,   [ m ]

Lyudmila Mihaylova, Katholieke Universiteit Leuven, Division PMA

The information criterion
I2

N=0
4
tra c e (W P )

3
N=2

2
N=3
1
N=5

0
20 40 60 80 100

t im e ,   [ s e c ]

Lyudmila Mihaylova, Katholieke Universiteit Leuven, Division PMA

Point-to-point optimization
21

20 G oal
N=3
19
y   p o s it io n ,   [ m ]

18

17
B eaco n
16 S ta rt
15
O b sta c le
14

13

12

0 2 4 6 8 10 12

x   p o s it io n ,   [ m ]

Lyudmila Mihaylova, Katholieke Universiteit Leuven, Division PMA

 Implementation
■ Using Optimization Toolbox of MATLAB,
■ fmincon finds the constrained minimum of a
function of several variables
■ With small number of sinusoids (N<5) the
computational complexity is such that it is
easily implemented on-line. With more
sinusoidal terms (N>10), the complexity (time,
number of computations) is growing up and a
powerful computer is required or off-line
computation. All the performed experiments
prove that the trajectories generated even
with N=3 sinusoidal terms respond to the
imposed requirements.
Lyudmila Mihaylova, Katholieke Universiteit Leuven, Division PMA
 Conclusions
An effective approach for trajectories
optimization has been considered :
■ with and without a reference trajectory;
■ with one and more beacons;
■ Appropriate optimality criteria are
defined. The influence of the different
factors is decoupled;
■ The approach is applicable in the
presence of and without obstacles.
Lyudmila Mihaylova, Katholieke Universiteit Leuven, Division PMA
 Further research
■ Active sensing without a reference
trajectory, when taking into account the
topology of the obstacles, with unknown
environment;
■ With other performance criteria;
■ Trajectories searched within other
classes of functions with appealing
properties;
■ For other robotic systems
(nonholonomic and holonomic);
Lyudmila Mihaylova, Katholieke Universiteit Leuven, Division PMA
■ With real data from robots.