Experimental Comparison of Trajectory Tracking

Algorithms for Nonholonomic Mobile Robots

Misel Brezak, Ivan Petrovic and Nedjeljko Peric
University of Zagreb
Faculty of Electrical Engineering and Computing
Zagreb, Croatia
Email: {misel.brezak, ivan.petrovic, nedjeljko.peric}@fer.hr
AbstractThis paper presents an experimental overview of
common trajectory tracking methods for nonholonomic mobile
robots: linear control, nonlinear control and model predictive
control. All methods are compared experimentally on a two-wheel
mobile robot with differential drive. The goal was to determine
which control method is the best with respect to robustness and
low trajectory tracking error. Thereby, a special emphasis is given
on a fast real-time trajectory tracking and behavior in conditions
near robot velocity and acceleration limits.

I. I NTRODUCTION
The field of mobile robotics is an active research area with
promising new application domains in industrial as well as
in service robotics. Mobile robots are especially appropriate
in applications where flexible motion planning is required.
There are many robot navigation strategies, and in cases where
operating environment map is known, approach with trajectory
planning algorithms is commonly used. Here the term trajectory denotes the path that robot should traverse as a function
of time. Trajectory planner generates the appropriate trajectory
with goal of arriving at a particular location, patrolling trough
specified area etc, and at the same time avoiding collisions
with different kinds of obstacles. To obtain feasible trajectory,
that is, the trajectory that is robot actually able to track, the
planner also needs to consider various robot physical and
dynamic limitations such as its velocity and acceleration limits.
A trajectory can be generated in real-time on the basis of
current sensor readings or generated in advance on the basis
of operating environment map.
Once the trajectory is planned, the robot must actually track
it. This means that reference point on the robot must follow
the planned trajectory. In other words, the distance between
the reference point on the robot and current path reference
point must be kept as small as possible. Ideally, this could be
achieved using only feedforward commands. But of course,
measurement errors and other real world issues force us to use
feedback control and combine it with feedforward commands.
An experimental overview of trajectory tracking controllers
based on linear, nonlinear and dynamic feedback linearization
methods is given in [1]. However, advanced control methods,
such as model predictive control, are not considered. Some
examples of using model predictive controller for trajectory
tracking of nonholonomic systems can be found in [2] and [3],

and a newer example is in [4]. As experimental comparison

of denoted control approaches is not covered in literature, the
main contribution of this paper is to give experimental testing
and comparison of denoted control approaches, especially in
conditions near robot velocity and acceleration limits when
high-speed real-time control is required.
This paper is organized as follows. In section 2 a robot
model is described. In section 3 the compared trajectory
tracking controllers are presented. In section 4 experimental
results are presented. The paper ends with conclusions and
ideas for future work.
II. ROBOT M ODEL
y
vr

(xr , yr)
e1

e2

vL

vR

(x , y)
b

x
Fig. 1.

Robot model

Kinematics of a mobile robot with differential drive is

defined by a Jacobian matrix J() that transforms tangential
and angular velocities expressed in coordinates of mobile robot
base to its velocities expressed in global Cartesian coordinates
(Fig. 1):




x
cos 0 
y = J v = sin 0 v .
(1)

0
1

Angle is robot heading direction that is taken counterclockwise from the x-axis. The mapping between tangential
(driving) and angular (steering) velocities and circumferential

velocities of the wheels (see Fig. 1), which are actual commands to the system, is given by:
vR = v + b/2,

vL = v b/2,

(2)

where b is the distance along the axle between the centers of

the drive wheels, and vR and vL are circumferential velocities
of the right and left wheel, respectively.
Also, the nonholonomic constraint that the driving wheels
purely roll and do not slip must be fulfilled, which is:
x sin + y cos = 0.

(3)

It is important to mention that dynamics of the robot is not

considered in kinematic model. Therefore, it is assumed that
robot exactly realizes velocity commands v and . Of course,
due to robot and actuator dynamics, and also nonidealities such
as friction, gear backslash, wheel slippage, actuator deadzone
and saturation, the robot cannot exactly realize velocity commands. Nevertheless, in case when reference trajectory has
continuous profile with velocities and accelerations that do
not exceed robot limitations, robot can approximatively track
the reference velocity commands, i.e. the trajectory is feasible
for the robot.
But, in case when trajectory tracking controller temporarily
generates command velocities higher than actual robot limitations, a velocity saturation will occur. During this time
it is important that velocity commands are saturated so that
robot retains its steering direction, i.e. path curvature. This
means that ratio /v (which is equal to path curvature) has
to be preserved. Therefore, if vmax and max are maximum
robots tangential and angular velocity commands, respectively, bounded command velocities vc and c that preserve
path curvature are obtained by first defining
= max {|v|/vmax ,

||/max ,

1} .

Then the following algorithm is used to determine bounded

velocities [5]:
vc = v,
vc = sign(v)vmax ,
vc = v/,

c = ,
= /,

if = 1;
if = |v|/vmax ;

= sign()max ,

(4)

else.

III. T RAJECTORY T RACKING C ONTROLLERS

Trajectory tracking problem is actually a problem of computing appropriate robot commands so that robot (i.e. reference
point on the robot) tracks a reference point of the trajectory.
Here the trajectory is given as a desired robot state as a
function of time:
T

[xr (t), yr (t), r (t), vr (t), r (t)] ,

(5)

where (xr (t), yr (t)) is reference position in cartesian space,

r (t) is reference robot orientation, and vr (t) and r (t) are
reference tangential and angular velocities, respectively. Of
course, trajectory must be planned so that the nonholonomic
r
constraint (3) is fulfilled. If the time derivatives x r , y r , x
and yr exist, i.e. if desired Cartesian motion (xr (t), yr (t)) is

twice differentiable, state variables (t), vr (t) and r (t) in

trajectory (5) are not independent, but can be calculated as
r (t) = arctan2(y r (t), x r (t))

vr (t) = x 2r (t) + y r2 (t)
yr2 (t) y r2 (t)
xr (t)
x r (t)
r (t) =
.
x 2r (t) + y r2 (t)

(6)
(7)
(8)

But if for some time t the reference tangential velocity vr (t)

is zero, neither the reference angular velocity r (t) nor the
reference angle r (t) are defined by eq. (6) and eq. (8), and
must be given explicitly.
Commonly, the trajectory tracking controller consists of
feedforward and feedback part. The values of feedforward
commands are computed based on reference tangential and
angular velocity (vr and r ) in trajectory (5). The output
of feedback part is then computed based on current tracking
error, and is superposed on generated feedforward commands.
T
Therefore robot velocity command vector u = [v ] can be
written as
(9)
u = uF + uB ,
where uF is feedforward, and uB is feedback part. Feedforward component is obtained using nonlinear transformation of
reference velocities
uF = [vr cos(e3 )

r ] ,

(10)

and is equal for all presented controller schemes, while the

feedback part is unique for each particular controller.f The
structure of general trajectory tracking controller, which consists of feedforward and feedback part, is shown in Fig. 2.
A. Linear Controller
The simplest design of trajectory tracking controller is
obtained using tangent linearization along the reference trajectory. Here it is required that robot states track given
reference trajectory, so that dynamics of tracking error has
to be stabilized. The tracking error e(t) is defined as follows

cos
sin 0
xr x
e1
e(t) = e2 = sin cos 0 yr y .
r
e3
0
0
1
(11)
Note that errors e1 and e2 are actually position error components expressed in robot local coordinate system, as shown
in Fig. 1. By deriving equation (11) and taking into account
robot kinematics (1) and (3) the error dynamics becomes




cos e3 0 
1 e2
e 1
v
v
r

e 2 = sin e3 0
0 e1
,
+

r
0
1
e 3
0
1
(12)
Rewriting eq. (12) and using equations (9) and (10), the
following error dynamics model is obtained

0 0
0
1 0
0 uB .
e = 0 0 e + sin e3 vr + 0
0
0 0
0
0 1
(13)

Feedforward
part

uF
Reference
trajectory
generator

uB
qr

Transformation to
robot coordinates

Fig. 2.

It can be shown that this system can be locally stabilized

around reference trajectory which consists of linear or circular
paths with constant velocity [1]. Furthermore, it is shown
that by using linear design methods it is possible to locally
stabilize given system even for arbitrary feasible trajectories
under condition that they do not come to a stop.
Now, following [1], the design procedure of the linear
controller will be shortly described. First, the linear feedback
law is defined as
(15)
uB (t) = Ks e(t),

(16)

The structure of gain matrix (16) can be intuitively explained by looking at Fig. 1. The tangential error e1 is reduced
by manipulating the command tangential velocity vB via gain
k1 . The orientation error e3 is reduced similarly by controlling
command angular velocity B of the robot via gain factor k3 .
Finally, the orthogonal error e2 is also reduced by changing
the angular velocity, but here the driving direction (forward or
backward) should also be taken into account.
Now the gains k1 , k2 , k3 are determined by using desired
closed-loop characteristic equation
( + 2n )(2 + 2n + n2 )

(17)

with one negative eigenvalue at 2n and a complex pair

with natural angular frequency n > 0 and damping coefficient (0, 1). The closed loop characteristic (17) is obtained
by choosing the gains
n2 r (t)2
.
(18)
|vr (t)|
The problem with this choice of gains is that k2 goes
to infinity as reference linear velocity vr goes to zero. The
possible gain scheduling scheme
 that solves the problem is
r2 (t) + gvr2 (t) [1], which
obtained by letting n (t) =
gives:

(19)
k1 = k3 = 2 r2 (t) + gvr2 (t), k2 = g|vr (t)|,
k1 = k3 = 2n , k2 =

Mobile
robot

Trajectory tracking controller structure

Linearizing eq. (13) around the reference trajectory (e =

0, uB = 0, v = vr , = r ) gives a linear model

0
r 0
1 0
e = r 0 vr e + 0
(14)
0 uB .
0
0 0
0 1

where the gain matrix Ks is defined as



k1
0
0
Ks =
.
0 sign(vr )k2 k3

Controller gain
K

where parameter g > 0 gives an additional degree of freedom.

It can be noted that with this choice of gains linear controller
actually becomes nonlinear, but we still call it linear as it is
obtained using linear design methods. This is also a continuous
controller, but under assumption that sampling period is small,
it can also be used as discrete controller.
B. Nonlinear Controller
The main importance of nonlinear controller design is that
its global asymptotic stability can be proven. First of all, the
feedback control is defined with following gain matrix [6]


k1 (vr (t), r (t))
0
0
,
Kn =
3)
0
k2 vr (t) sin(e
k3 (vr (t), r (t))
e3
(20)
where k2 > 0 is constant positive gain, and k1 (, ) and
k3 (, ) are positive continuous gain functions. Assuming that
vr and r are bounded and with bounded derivatives, and that
vr (t)  0 and r (t)  0 when t , using Lyapunov
analysis one can prove that the control law (20) globally
asymptotically stabilizes the origin e = 0.
Following the design of previous linear controller, the gain
functions k1 and k3 , and the constant gain k 2 can be chosen
as

k1 (vr (t), r (t)) = k3 (vr (t), r (t)) = 2 r2 (t) + gvr2 (t),
k 2 = g,

(21)

where g > 0 and (0, 1).

C. Model Predictive Controller
The main idea of model predictive controller (MPC) approach is to minimize the difference between reference trajectory tracking error and predicted trajectory tracking error, as
well as control effort. Accordingly, a quadratic cost function
can be written as
J(uB , k) =

h

i=1

T (k, i) Q (k, i) + uTB (k, i) R uB (k, i),

(22)
where (k, i) = er (k + i) e(k + i|k) is difference between
reference (desired) trajectory tracking error er (k + i) and
predicted trajectory tracking error e(k + i|k), h is prediction
horizon interval, and Q and R are weighting matrices, where
Q 0, Q Rn Rn and R 0, R Rm Rm , n

is the number of state variables and m is the number of input

variables.
For the design purpose, error dynamics (14) is discretized
as
(23)
e(k + 1) = Ae(k) + BuB (k),
where A Rn Rn and B Rn Rm . Discrete model
matrices A and B for short sampling time Ts can be well
approximated with
A = I + Ac Ts
B = Bc Ts .

(24)

Following [4], error prediction at the time instant h can be

written as

h1
h
h1

e(k + h|k) =
A(k + j|k)e(k) +
A(k + j|k)
j=1

i=1

j=i

B(k + i 1|k)uB (k + i 1)
+B(k + h 1|k)uB (k + h 1).

(25)
Now the vector of trajectory tracking error predictions is
defined as
T

E (k) = e(k + 1|k)T e(k + 2|k)T . . . e(k + h|k)T
(26)
where E Rnh . If the control vector is defined as
T

UB (k) = uB (k)T uB (k + 1)T . . . uB (k + h 1)T ,
(27)
where UB Rmh , and
(k, i) =

h1

A(k + j|k),

(28)

j=i

the vector of trajectory tracking error predictions can be

written in matrix form as
E (k) = F (k)e(k) + G(k)UB (k),

(29)

where
F (k) = [A(k|k)
and
G(k) =

B(k|k)

A(k + 1|k)B(k|k)
.
.
.
(k, 1)B(k|k)

A(k + 1|k)A(k|k)

B(k + 1|k)
.
.
.
(k, 2)B(k + 1|k)

..
.

...

(k, 0)] ,
(30)

0
.
.
.
.
.
.
B(k + h 1|k)

(31)
where F (k) Rnh Rn , G(k) Rnh Rmh .
The reference trajectory tracking error can be defined as
er (k + 1) = Air e(k),

i = 1, . . . , h.

(32)

Matrix Ar is defined so that reference trajectory tracking

error decreases with desired dynamics over time. Now the
vector of reference trajectory tracking errors is defined as
T

Er (k) = er (k + 1|k)T er (k + 2|k)T . . . er (k + h|k)T ,
(33)

and it can be computed as

Er (k) = Fr e(k),
where


Fr = Ar
nh

A2r

...

Ahr

(34)
T

(35)

and Fr R R .
The cost function (22) can now be expressed as
J(UB ) = (Er E )T Q(Er E ) + UBT RUB .

(36)

In order to obtain optimal control law, the cost function is

minimized by setting its derivative to zero. In this way the
control law becomes

1 T
G Q(Fr F )e(k),
(37)
UB (k) = GT QG + R
where

Q=

0
..
.

Q
..
.

..
.

0
..
.
..
.
Q

,R =

0
..
.

R
.. . .
.
.
0

0
..
.
..
.
R

(38)
where Q Rnh Rnh and R Rmh Rmh . The feedback
control law of the MPC controller now becomes
uB (k) = Kmpc e(k)

(39)

where Kmpc is defined as first m rows of the matrix

 T
1 T
G QG + R
G Q(Fr F ), so that Kmpc Rm Rn .
IV. E XPERIMENTAL RESULTS
The developed algorithms were tested on robot soccer platform that is ideal for testing mobile robot related algorithms
[7]. It consists of a team of five wheeled radio-controlled
microrobots of size 7.5 cm cubed, with maximum velocity
2.4 m/s and maximum acceleration 2.5 m/s2 . The playground
is of size 2.21.8 m. Above the center of the playground
(2.5 m height), IEEE-1394 digital color camera with resolution of 656494 pixels with maximal framerate of 80 fps
was mounted (model Basler a301fc). Camera is connected
to central computer, which executes vision algorithm, and
also makes decisions and controls robots by transmitting
referent linear and angular velocities to robots via wireless
communication.
For experiments 8-shaped trajectory was used that is defined
by


1.5t
xr (t) = 1.1 + 0.7sin
 2.45
(40)
2 1.5t
yr (t) = 0.9 + 0.7sin
.
2.45
This type of trajectory is appropriate for experiments, because it has sharp turns, as well as parts with high acceleration
and velocity, but its still feasible for the robot because there
are no step changes of the tangential, nor angular velocity,
and maximum acceleration of the robot is never exceeded.

1.6

1.4

1.4

1.2

1.2

y [m]

y [m]

1.6

0.8

0.8

0.6

0.6

0.4

0.4

0.4

0.6

0.8

1.2
x [m]

1.4

1.6

1.8

0.2

Fig. 3. Trajectory tracking experiment with linear controller: robot path (),
reference path (- -)

The maximum tangential velocity of this trajectory is 1.5 m/s,

and maximum tangential acceleration is 1.9 m/s2 , which is
close to robot limits. In experiments robot was first accelerated to achieve position, orientation and velocity that are
approximately equal to trajectory initial condition.
The sampling period for all controllers was 12.5 ms. The
parameters for linear controller were = 0.7 and g = 60,
and the same parameters were used for nonlinear controller.
For the MPC controller, after many experiments the prediction
horizon h = 12 was chosen as best, the reference matrix was
Ar = I33 0.85, and weighting matrices were

4 0
0
Q = 0 10 0 , R = I22 103 .
0 0 0.1
The obtained robot paths for experiments with linear, nonlinear and MPC controller are given in Figures 3, 4 and 5,
respectively. The correspondent sum of squared errors (SSE)
for each component of error vector e are
SSElinear = (0.0177 0.0397 0.4395)
SSEnonlinear = (0.0144 0.0354 0.3555)
SSEMP C = (0.0442 0.0416 0.4010).
It can be seen that nonlinear controller has the lowest SSE
for all three error components, although this difference is not
significant which can also be confirmed visually by comparing
obtained paths. The error of linear controller is only slightly
higher compared to nonlinear. But although in simulations
MPC gives lower tracking error than linear and nonlinear
controllers, in real world it is no more the case as it results
with somewhat higher longitudinal error e1 while other two
error components are comparable with other controllers.
The produced command robot velocities of linear, nonlinear and MPC controller experiments, together with ideal
commands obtained from reference trajectory, are given in

0.4

0.6

0.8

1.2
x [m]

1.4

1.6

1.8

Fig. 4. Trajectory tracking experiment with nonlinear controller: robot path

(), reference path (- -)

1.6
1.4
1.2
1
y [m]

0.2

0.8
0.6
0.4
0.2

0.4

0.6

0.8

1.2
x [m]

1.4

1.6

1.8

Fig. 5. Trajectory tracking experiment with MPC controller: robot path (),
reference path (- -)

Figures 6, 7 and 8, respectively It can be observed that

all controllers produce similar command values but MPC
controllers command values u1 and u2 are closest to ideal
command tangential vr and angular r velocities obtained
from reference trajectory.
Regarding the parameters choice, for linear and nonlinear
controllers it was very easy to choose controller parameters, but MPC controller was very sensitive to parameters
choice and control easy became unstable with wrong choice.
Regarding the robustness, at high velocities the linear and
nonlinear controller showed acceptable robustness in presence
of measurement noise and small initial robot position error,
as opposed to MPC controller which often showed unstable
behavior in presence of noise and initial error. This was also
proved by the fact that the maximum velocity that could be
obtained was about 1.5 m/s for MPC, 1.6 m/s for linear and

vr [m/s], u1 [m/s], r [rad/s], u2 [rad/s]

vr [m/s], u1 [m/s], r [rad/s], u2 [rad/s]

2
u1
vr

u2

2
u1
vr

u2

r
6

r
7

t [s]

t [s]

Fig. 6. Experiment with linear controller: reference tangential (vr ) and

angular (r ) velocity, command tangential (u1 ) and angular (u2 ) velocity

vr [m/s], u1 [m/s], r [rad/s], u2 [rad/s]

Fig. 8.
Experiment with MPC controller: reference tangential (vr ) and
angular (r ) velocity, command tangential (u1 ) and angular (u2 ) velocity

has low computational requirements which make it especially

appropriate for fast, real-time trajectory tracking.
Future work will include developing and comparison of
more advanced controller schemes, like dynamic feedback
linearization based controller and nonlinear model predictive
controller. Also, the procedure of determining optimal controller parameters will be developed.

ACKNOWLEDGMENT
2

This research was supported by the Ministry of Science,

Education and Sports of the Republic of Croatia (grant No.
036-0363078-3018).

u1
vr

u2
r

t [s]

Fig. 7. Experiment with nonlinear controller: reference tangential (vr ) and

angular (r ) velocity, command tangential (u1 ) and angular (u2 ) velocity

1.7 m/s for nonlinear controller.

V. C ONCLUSION
An experimental comparison of three trajectory tracking
controllers for nonholonomic mobile robot is presented: linear
controller, nonlinear controller and model predictive controller.
The goal was to obtain highest possible trajectory velocity, and
all controllers were tested on a real mobile robot. Experiments
have shown that all three controllers produce comparable
quality of trajectory tracking, but MPC controller has high
sensitivity to initial robot state error, and it is very difficult to
determine optimal parameters of the MPC algorithm. Moreover, the drawback of the MPC controller is its complexity,
especially if longer prediction intervals are used. Therefore,
at high velocities the nonlinear controller is recommended
as it provides global asymptotic stability, it is not difficult
to choose its parameters and is simple to implement and

R EFERENCES
[1] A. D. Luca, G. Oriolo, and M. Vendittelli, Control of wheeled mobile
robots: An experimental overview, in:S. Nicosia, B. Siciliano. A. Bicchi,
p: Valigi, (Eds.) RAMSETE - Articulated and Mobile Robotics for Services
and Technologies, 3rd ed. Springer-Verlag, 2001.
[2] A. Ollero and O. Amidi, Predictive path tracking of mobile robots,
in Proceedings of 5th International Conference on Advanced Robotics,
Robots in Unstructured Environments (ICAR 91), vol. 2, June 1991, pp.
1081 1086.
[3] J. E. Normey-Rico, J. Gmez-Ortega, and E. F. Camacho, A smithpredictor-based generalised predictive controller for mobile robot pathtracking, Control Engineering Practice, vol. 7, no. 6, pp. 729 740,
1999.
[4] G. Klancar and I. Skrjanc, Tracking-error model-based predictive control
for mobile robots in real time, Robotics and Autonomous Systems,
vol. 55, pp. 460469, 2007.
[5] G. Oriolo, A. D. Luca, and M. Vendittelli, WMR control via dynamic
feedback linearization: Design, implementation, and experimental validation, IEEE Tranasactions on Control Systems Technology, vol. 10, no. 6,
pp. 835 852, 2002.
[6] C. Samson, Time-varying feedback stabilization of car-like wheeled
mobile robots, International Journal of Robotics Research, vol. 12, no. 1,
pp. 55 64, 1993.
[7] H. Kitano, M. Asada, Y. Kuniyoshi, I. Noda, and E. Osawa, RoboCup:
The robot world cup initiative, in Proceedings of the First International
Conference on Autonomous Agents (Agents97). New York: ACM Press,
1997, pp. 340347.