A Parametric MPC Approach to Balancing the Cost of Abstraction for
Differential-Drive Mobile Robots

arXiv:1802.07199v1 [cs.RO] 20 Feb 2018

Paul Glotfelter and Magnus Egerstedt
Abstract— When designing control strategies for differentialdrive mobile robots, one standard tool is the consideration of a
point at a fixed distance along a line orthogonal to the wheel axis
instead of the full pose of the vehicle. This abstraction supports
replacing the non-holonomic, three-state unicycle model with a
much simpler two-state single-integrator model (i.e., a velocitycontrolled point). Yet this transformation comes at a performance cost, through the robot’s precision and maneuverability.
This work contains derivations for expressions of these precision
and maneuverability costs in terms of the transformation’s
parameters. Furthermore, these costs show that only selecting
the parameter once over the course of an application may
cause an undue loss of precision. Model Predictive Control
(MPC) represents one such method to ameliorate this condition.
However, MPC typically realizes a control signal, rather than
a parameter, so this work also proposes a Parametric Model
Predictive Control (PMPC) method for parameter and sampling
horizon optimization. Experimental results are presented that
demonstrate the effects of the parameterization on the deployment of algorithms developed for the single-integrator model
on actual differential-drive mobile robots.

I. I NTRODUCTION
Models are always abstractions in that they capture some
pertinent aspects of the system under consideration whereas
they neglect others. But models only have value inasmuch
as they allow for valid predictions or as generators of design
strategies. For example, in a significant portion of the many
recent, multi-agent robotics algorithms for achieving coordinated objectives, single-integrator models are employed
(e.g., [1], [2], [3], [4]). Arguably, such simple models have
enabled complex control strategies to be developed, yet, at
the end of the day, they have to be deployed on actual
physical robots. This paper formally investigates how to
strike a balance between performance and maneuverability
when mapping single-integrator controllers onto differentialdrive mobile robots.
Due to the single-integrator model’s prevalence as a
design tool, a number of methods have been developed
for mapping from single-integrator models to more complex, non-holonomic models. For example, the authors of
[5] achieve a map from single integrator to unicycle by
leveraging a control structure introduced in [6]. However,
this map does not come with formal guarantees about the
degree to which the unicycle system approximates the singleintegrator system. One effective solution to this problem is
to utilize a so-called Near-Identity Diffeomorphism (NID)
This research was sponsored by Grants No. 1531195 from the U.S.
National Science Foundation.
The authors are with the Institute for Robotics and Intelligent Machines, Georgia Institute of Technology, Atlanta, GA 30332, USA,
{paul.glotfelter,magnus}@gatech.edu.

between single-integrator and unicycle systems, as in [7],
[8], where the basic idea is to perturb the original system
ever-so-slightly (the near-identity part) and then show that
there exists a diffeomorphism between a lower-dimensional
version of the perturbed system’s dynamics and the singleintegrator dynamics. As the size of the perturbation is given
as a design parameter, a bound on how far the original
system may deviate from the single-integrator system follows
automatically.
A concept similar to NIDs from single-integrator to unicycle dynamics appears in the literature in different formats.
For example, [9] utilizes this technique from a kinematics
viewpoint to stabilize a differential-drive-like system. This
"look-ahead" technique also arises in feedback linearization
methods as a mathematical tool to ensure that the differentialdrive system is feedback linearizable (e.g., [10], [11]).
This paper utilizes the ideas in [7], [8] to show that
the NID incurs an abstraction cost, in terms of precision
and maneuverability, that is based on the physical geometry
of the differential-drive robots; in particular, the precision
cost focuses on increasing the degree to which the singleintegrator system matches the unicycle-modeled system, and
the maneuverability cost utilizes physical properties of the
differential-drive systems to limit the maneuverability requirements imposed by the transformation. By striking a
balance between these two costs, a one-parameter family of
abstractions arises. However, the maneuverability cost shows
that only selecting the parameter once over the course of an
experiment may cause a loss of precision.
A potential solution to this issue is to repeatedly optimize
the parameter based on the system’s model and a suitable
cost metric. Model Predictive Control (MPC) represents one
such method. In particular, MPC approaches solve an optimal
control problem over a time interval, utilize a portion of the
controller, and re-solve the problem over the next time interval, effectively producing a state- and time-based controller.
The authors of [12], [13] produce such a Parametric Model
Predictive Control (PMPC) formulation. However, this formulation does not permit the cost metric to influence the
time interval, which has practical performance implications.
Using the formulated precision and maneuverability costs,
this work formulates an appropriate PMPC cost metric and
extends the work in [12], [13] to integrate a sampling horizon
cost directly into the PMPC program.
This paper is organized as follows: Sec. II presents the
system of interest and introduces the inherent trade-off contained in the NID. Sec. III discusses the PMPC formulation.
Sec. IV formulates the cost functions that allow a balanced

selection of the NID’s parameters, with respect to the generated cost functions. To demonstrate and verify the main
results of this work, Sec. V shows data from simulations and
physical experiments, with Sec. VI concluding the paper.
II. F ROM U NICYCLES TO S INGLE I NTEGRATORS
This article uses the following mathematical notation. The
expression k · k is the usual Euclidean norm. The symbol
∂x f (x) represents the partial derivative of the function f :
Rn → Rm with respect to the variable x, assuming the
convention that ∂x f (x) ∈ Rm×n . The symbol R≥0 refers
to the real numbers that are greater than or equal to zero.
As the focus of the paper is effective abstractions for
controlling differential-drive robots, this section establishes
the Near-Identity Diffeomorphism (NID) that provides a
relationship between single-integrator and unicycle models.
That is, systems whose pose is given by planar positions
T
x̄ = [x1 x2 ] and orientations θ, with the full state given
 T T
T
= [x1 x2 θ] . The associated unicycle
by x = x̄ θ
dynamics are given by (dropping the dependence on time t)

 
R(θ)e1 0 v
ẋ =
,
(1)
0
1 ω
where the control inputs v, ω ∈ R are the linear and
rotational velocities, respectively, 0 is a zero-vector of the
appropriate dimension, and



T
cos(θ) − sin(θ)
e1 = 1 0 , R(θ) =
.
sin(θ) cos(θ)
Letting

ux = v

ω

T

be the collective control input to the unicycle-modeled agent,
the objective becomes to turn this model into a singleintegrator model. To this end, we here recall the developments in [7]. Let xsi ∈ R2 be given by
xsi = Φ(x, l) = x̄ + lR(θ)e1 ,

(2)

where l ∈ (0, ∞) is a constant. The map Φ(x, l) is, in fact,
the NID, as defined in [7]. Geometrically, the point xsi is
simply given by a point at a distance l directly in front of
the unicycle with pose x.
Now, assume that the dynamics of xsi are given by a
controller
ẋsi = usi ,
where usi ∈ R2 is continuously differentiable, and compare
this system to the time-derivative of (2), which yields


cos(θ) −l sin(θ)
ẋsi = usi =
ux = Rl (θ)ux .
(3)
sin(θ) l cos(θ)
Note that the NID maps from three degrees of freedom to
two degrees of freedom. As a consequence, the resulting
unicycle controller cannot explicitly affect the orientation θ
of the unicycle model.
By [7], Rl (θ) is invertible, yielding a relationship between
usi and ux . Consequently, (3) allows the transformation of
linear, single-integrator algorithms into algorithms in terms

of the non-linear, unicycle dynamics. Note that in this paper,
which is different from [7], we let l˙ = 0 over the PMPC
time intervals (i.e., l is a constant value).
The unicycle model in (1) is not directly realizable on
a differential-drive mobile robot. However, the relationship
between the control inputs to the unicycle model and the
differential-drive model is given by
rw
rw
(ωr + ωl ), ω =
(ωr − ωl ),
(4)
v=
2
lw
where ωr and ωl are the right and left wheel velocities,
respectively. The wheel radius rw and base length lw encode
the geometric properties of the robot.
In the discussion above, the parameter l (i.e., the distance
off the wheel axis to the new point) is not canonical.
Moreover, it plays an important role since
kx̄ − xsi k = l.

(5)

The above equation seems to indicate that one should simply
choose l ∈ (0, ∞) to be as small as possible. However, the
following sections show that small values of l induce high
maneuverability costs.
In order to strike a balance between precision and maneuverability, we will, for the remainder of this paper, assume
that the control input to the unicycle model is given by
ux = Rl (θ)−1 usi ,
where usi is the control input supplied by a single-integrator
algorithm. Sec. IV contains the further investigation of the
effects of the parameter l on the precision and maneuverability implications of the transformation in (2).
III. A PARAMETRIC MPC F ORMULATION
Having introduced the system of interest, this section
contains a derivation of a Parametric Model Predictive Control (PMPC) method with a variable sampling interval for
general, nonlinear systems. Later, Sec. V utilizes a specific
case of these results. In general, MPC methods solve an
optimal control problem over a time interval and use only
a portion of the obtained controller (for a small amount
of time) before resolving the problem, producing a timeand state-based controller. In this case, PMPC optimizes
the parameters of a system. That is, this method finds the
optimal, constant parameters of a system, rather than a timevarying control input, over a time interval. For clarity, this
section specifies dependencies on time t. Let
ẋ(t) = f (x(t), p, t), xt0 = x(t0 ),
where x(t) ∈ Rn , p ∈ Rm , and f (·) is continuously
differentiable in x, measurable in t. The program
t0Z+∆t

arg min

J(p, ∆t) =

p∈Rm ,∆t∈R≥0

s.t. ẋ(t) = f (x(t), p, t)
x(t0 ) = xt0 ,

L(x(s), p, s)ds + C(∆t)
t0

expresses the PMPC problem of interest, where L(·) is
continuously differentiable in x and p. Note that, in this
case, both ∆t and p are decision variables determined by
the PMPC program.
A. Optimality Conditions
This section contains the derivation of the necessary,
first-order optimality conditions for the PMPC formulation,
realizing gradients for the proposed cost. In particular, the
derivation proceeds by calculus of variations.
˜ ∆t),
Proposition 1. The augmented cost derivatives ∂p J(p,
˜
∂∆t J(p, ∆t) are

∆t) = 0. Applying the mean value theorem and taking the
limit as  → 0 shows that
˜
˜ ∆t)
J(p+γ,
∆t+τ ) − J(p,
=
lim
→0


 t +∆t
0Z

∂p L(x(s), p, s)+λ(s)T ∂p f (x(s), p, s)ds γ
t0

+[∂∆t C(∆t)+L(x(t0 +∆t), p, t0 +∆t)] τ,
which is linear in τ and γ, and provides the final expressions
t0Z+∆t

˜ ∆t) =
∂p J(p,

t0Z+∆t

˜ ∆t) =
∂p J(p,

∂p L(x(s), p, s) + λ(s)T ∂p f (x(s), p, s)ds

t0

˜ ∆t) = L(x(t0 + ∆t), p, t0 + ∆t) + ∂∆t C(∆t),
∂∆t J(p,
˜ ∆t) (i.e., J(p, ∆t) augmented
where the augmented cost J(p,
with the dynamics constraint) is given by
˜ ∆t) =
J(p,
t0Z+∆t
L(x(s), p, s)+λ(s)T (f (x(s), p, s)− ẋ(s))ds+C(∆t).
t0

Proof. The proof proceeds by calculus of variations. Perturb
p and ∆t as p 7→ p + γ and ∆t 7→ ∆t + τ , where γ ∈ Rm ,
τ ∈ R. The perturbed augmented cost is
˜
J(p+γ,
∆t+τ ) =
t0 +∆t+τ
Z
L(x(s)+η(s), p+γ, s)
t0
T

λ(s) (f (x(s)+η(s), p+γ, s)− ẋ(s)−η̇(s))ds+
C(∆t+τ )+o().
Performing a Taylor expansion yields that
˜
J(p+γ,
∆t+τ ) =
t0 +∆t+τ
Z
L(x(s), p, s)+∂x L(x(s), p, s)η(s)+∂p L(x(s), p, s)γ
t0

+λT (f (x(s), p, s)+∂x f (x(s), p, s)η(s)
+∂p f (x(s), p, s)γ − ẋ(s)−η̇(s))ds
+C(∆t) + ∂∆t C(∆t)τ +o().
The proof now proceeds with multiple steps. First, the
application of integration by parts to the quantity λ(t)T η̇(t).
˜ + γ, ∆t + τ ) −
Second, the subtraction of the costs J(p
˜ ∆t). Note that, to subtract the costs properly, the inteJ(p,
˜
gral in J(p+γ,
t+τ ) must be broken up into two intervals:
[t, t + ∆t] and [t + ∆t, t + ∆t + τ ]. Furthermore, the costate
assumes the usual definition: λ̇(t) = −∂x L(x(t), p, t)T −
∂x f (x(t), p, t)T λ(t) with the boundary condition λ(t0 +

∂p L(x(s), p, s)+λ(t)T ∂p f (x(s), p, s)ds

t0

˜ ∆t) = ∂∆t C(∆t)+L(x(t0 + ∆t), p, t0 +∆t),
∂∆t J(p,
completing the proof.
Interestingly, both of the usual conditions for free parameters and final time still hold, and the first-order, necessary
optimality conditions for candidate solutions p∗ and ∆t∗ are
that
˜ ∗ , ∆t∗ ) = 0, ∂∆t J(p
˜ ∗ , ∆t∗ ) = 0.
∂p J(p
Furthermore, this formulation becomes amenable to solution by numerical methods for the optimal parameters p∗ and
˜ ∆t) can also
∆t∗ . In such cases, the expression for ∂p J(p,
be expressed as a costate-like variable ξ : [t0 , t0 +∆t] → Rm
with dynamics
˙ = −∂p L(x(t), p, t)T − ∂p f (x(t), p, t)T λ(t)
ξ(t)
ξ(t0 + ∆t) = 0,
where ξ(·) is defined as
t0Z+∆t

∂p L(x(s), p, s)T +∂p f (x(s), p, s)T λ(s)ds.

ξ(t) =
t

In this case, the necessary optimality condition is that
ξ(t0 ) = 0.
B. Numerical Methods
The above expressions allow for applications of typical
gradient descent methods. Many such methods could apply,
and this article presents one simple method in Alg. 1.
Note that this algorithm procures the decision variables
over one sampling interval [t0 , t0 + ∆t]. In practice, one
typically applies this algorithm repeatedly. For example, the
experiments in Sec. V-C consecutively apply this algorithm
to solve the PMPC problem.
IV. P RECISION VS . M ANEUVERABILITY
As already noted in Sec. II, the parameter l is a design parameter. This section discusses the importance and effects of
selecting l and proposes precision and maneuverability costs
that elucidate the selection of this parameter and its impact
on the differential-drive system. These derivations influence
the PMPC cost metric in Sec. V and, for comparison, an
optimal, static parameterization.

Algorithm 1 Gradient Descent Algorithm for PMPC
1:
2:
3:
4:
5:
6:
7:
8:
9:
10:

k←0
pk ← initial guess
∆tk ← initial guess
˜ k , ∆t) >  do
˜ k , ∆t) + ∂∆t J(p
while ∂p J(p
Solve forward for x(·) from xt using pk and ∆tk
Solve backward for λ(·), ξ(·) using x(·)
˜ k , ∆t) and ∂∆t J(p
˜ k , ∆t)
Compute gradients ∂p J(p
˜ k , ∆t)T
pk+1 ← pk − γ1 ∂p J(p
˜ k , ∆t)
∆tk+1 ← ∆tk − γ2 ∂∆t J(p
k ←k+1

and let θsi be the angle of the vector usi . From (3),(4) we can
retrieve the magnitude of the difference in angular velocities,
|ωr − ωl |, as
|ωr − ωl | =
=
=
=
=

A. Precision Cost
Seeking to select l, we initially present a cost that incorporates the degree to which the transformed system in
(2) represents the original system xsi over an arbitrary time
duration T ≥ 0. As such, we model the precision cost by
the averaged tracking error
Z
1 T
D1 (x̄, xsi ) =
kx̄ − xsi k dt.
(6)
T 0

=
=
≤

lw
ω
rw

l T
e Rl (θ)−1 usi
r 2
lw T −1
e T R(−θ)R(θsi )ūsi
rw 2
lw T −1
e T R(θsi − θ)ūsi
rw 2




lw
1 cos(θsi − θ) − sin(θsi − θ) kusi k
0
sin(θsi − θ) cos(θsi − θ)
0
rw
l



lw sin(θsi − θ) cos(θsi − θ) kusi k
0
rw
l
l
lw kusi k
sin(θsi − θ)
rw l
lw v̄
.
rw l

It immediately follows from (5) that D1 (x̄, p) can be directly
written as a function of l, given by
Z
1 T
D1 (x̄, xsi ) =
kx̄ − xsi k dt
T 0
Z
1 T
=
l dt = l.
(7)
T 0

Thus, Prop. 2 yields an upper bound on the magnitude of
the wheel-velocity difference

This immediate result states that the smaller l is, the better
the unicycle model tracks the single-integrator model.

Proposition 3. Given that the control-input magnitude kusi k
is upper-bounded by v̄, the magnitude of the forward velocity,
|ωr + ωl |, is upper bounded by

B. Maneuverability Cost
In this section, we derive a geometrically-influenced maneuverability cost that models the degree to which the
selection of l influences the maneuverability requirements of
the unicycle-modeled system, with respect to the map defined
in (2). That is, we wish to elucidate how the parameter
l affects the expressions for the differential-drive agent’s
forward velocity, wheel difference, and exerted control effort.
To this end, we utilize the differential-drive model in
(4). Initially, note that the magnitude of the wheel-velocity
difference |ωr − ωl | represents a measure of the complexity
of a maneuver that the differential-drive system performs.
Using this definition as guidance, we state the following
proposition.
Proposition 2. Given that the control-input magnitude kusi k
is upper-bounded by v̄, the magnitude of the wheel-velocity
difference, |ωr − ωl |, is upper bounded by
|ωr − ωl | ≤

|ωr + ωl | ≤

Proof. Let

e1 = 1

"
1
T
−1
1 , T =
0

#
0

1 , ūsi = kusi k
l

T
0

2v̄
.
rw

T
0

and
T −1 , ūsi , θsi
be defined as in the proof of Prop. 2. Then, we have, through
(4), that
|ωr + ωl | =
=

=

Proof. Let

e2 = 0

Prop. 3 shows a similar result for the forward velocity of the
differential-drive agent.

=

lw v̄
.
rw λ

lw v̄
.
rw l

|ωr − ωl | ≤

=

2
v
rw
2 T
e Rl (θ)−1 usi
rw 1
2 T −1
e T R(−θ)R(θsi )ūsi
rw 1
2
[1 0] R(θsi − θ)ūsi
rw


2
kusi k
[cos(θsi − θ) − sin(θsi − θ)]
0
rw

2
kusi k cos(θsi − θ)
rw
2v̄
≤
.
rw

where α ∈ (0, 1). Now, we seek the optimal l such that (10)
is minimized. That is,

=

l∗ = arg min D(l).

(11)

l

So Prop. 3 reveals that the forward velocity of the
differential-drive system remains independent of the selection of the parameter l.
To elucidate an appropriate maneuverability cost in terms
of l, define the average control effort exerted by the
differential-drive system over an arbitrary time duration
T >= 0 as
Z
1 T
|ωr − ωl | + |ωr + ωl | dt.
T 0
Directly applying Props. 2,3 reveals that the above expression
is bounded above by
lw v̄ 2v̄
+ .
(8)
rw l
r
The expression in (8) demonstrates an interesting quality of the system. As l grows large, the forward velocity
dominates the control effort exerted by the differential-drive
system. However, if l becomes small, then the choice of l
affects the potentially exerted control effort.
Thus, (8) reveals how l affects the maneuverability requirements imposed by the abstraction. The fact that we
always pay the forward-velocity price, regardless of the selection of l, naturally excludes the forward velocity from the
soon-to-be-formulated cost, because any selection of l results
in the same cost bound; but the choice of l directly affects
the cost associated with the wheel difference. Accordingly,
the wheel difference must play a role in the final PMPC cost
metric. With this conclusion in mind, we define the static
maneuverability cost as
lw v̄
D2 (l) =
,
(9)
rw l
which the static parameterization in the following section
utilizes.
C. An Optimal, One-Time Selection
Sec. V utilizes the results in Sec. IV to formulate an
appropriate cost metric for a PMPC program. To have a baseline comparison, this section formulates an optimal, one-time
selection for the parameter l. That is, the selection occurs
once over the experiment’s duration. This selection should
strike a balance between precision and maneuverability.
Eqns. (7) and (9) represent each of these facets, respectively,
and introduce an inherent trade-off in selecting l. Making l
smaller directly reduces the cost in (7). However, consider
the relationship in (9); as l decreases, the differential-drive
system accumulates a higher maneuverability cost.
As such, the convex combination of (6) and (9) yields a
precision and maneuverability cost in terms of l as
D(l) = αD1 (l) + (1 − α)D2 (l)
lw v̄
,
= αl + (1 − α)
rw l

(10)

(11) leads to Prop. 4.
Proposition 4. The optimal l∗ is given by
r
1 − α lw v̄
.
l∗ =
α rw
Proof. We have that
∂
lw v̄
D(l) = α − (1 − α)
∂l
rw l2
lw v̄
= α − (1 − α)
.
rw l2
Setting this equation equal to zero directly yields the minimizer
r
1 − α lw v̄
∗
l =
.
(12)
α rw
Note that the above result utilizes (9), which is an upper
bound on the wheel velocity difference. Thus, the PMPC
method should outperform this static selection, a suspicion
that Sec. V investigates.
D. PMPC Cost
This section formulates a PMPC cost based on the analysis
in Sec. IV. To increase precision, the parameter l must be
minimized. However, (9) in Sec. IV-B indicates that the
wheel velocity difference must be managed. Thus, precision
and maneuverability are balanced with the cost
L(x, l, t) = (1 − β)(ωr − ωl )2 + βl2
= (1 − β)((lw /rw )e2 Rl (θ)−1 (usi ))2 + βl2 ,
where β ∈ (0, 1).
With this cost metric, the PMPC program becomes
t0Z+∆t

(1 − β)((lw /rw )e2 Rl (θ)−1 (usi ))2 + βl2

arg min
l∈R,∆t∈R≥0

t0

+ C(∆t)


R(θ)e1 0
s.t. ẋ =
R (θ)−1 usi
0
1 l

(13)

x(t0 ) = xt0 ,
Note that the sampling cost C(∆t) and single-integrator
control input usi have yet to be specified.

Angular Velocity ( )

0.1
0
-0.1
-0.2

Static
PMPC

-0.3
0

20

40

60

80

100

Time (s)

Fig. 3: Angular velocity (ω) during the simulation. The
simulation shows that the static selection (solid line) and
PMPC method (dashed line) both generate similar angular
velocity values.
Fig. 1: The GRITSbot, which is a small, differential-drive
mobile robot used in the Robotarium. This figure displays
the base length and wheel radius of the GRITSbots.

V. N UMERICAL R ESULTS
To demonstrate the findings in Sec. IV, we conduct two
separate tests: in simulation and on real hardware. The
simulation portion shows the effects of a one-time parameter
selection on the angular velocity versus the PMPC method.
The experimental section contains the same implementation on a real, physical system: the Robotarium (www.
robotarium.org). In particular, the experimental results
highlight the practical differences between using a PMPC
approach and a one-time selection.
A. Experiment Setup
This section proposes cost functions based on the results
in Sec. IV and expresses the PMPC problem to be solved
in simulation and on the Robotarium. Furthermore, this
section also statically parameterizes the NID to provide a
baseline comparison to the PMPC strategy. In this case,
the particular setup involves a mobile robot tracking an
ellipsoidal reference signal

Parameter (l)

0.06

0.04

r(t) =



0.4 cos((1/10)t)
.
0.2 sin((1/10)t)

(14)

For a single-integrator system, the controller
usi = xsi − r + ṙ
drives the single-integrator system to the reference exponentially quickly. Utilizing the transformation in Sec. IV yields
the controller
ux = Rl (θ)−1 (r − xsi + ṙ)
= Rl (θ)−1 (r − (x̄ + lR(θ)e1 ) + ṙ).
The GRITSbots of the Robotarium (shown in Fig. 1) have a
wheel radius and base length of
rw = 0.005 m, lw = 0.03 m.
Furthermore, their maximum forward velocity is
v̄ = 0.1 m/s.
For this problem, we also consider the sampling cost
1
,
∆t
which prevents the time horizon from becoming too small
(i.e., the cost penalizes small time horizons).
Substituting these values into (13), the particular PMPC
problem to be solved is
C(∆t) =

0.02
0

20

40

60

80

t0Z+∆t

100

Time (s)

Fig. 2: Parameter (left) and sampling horizon (right) from
PMPC simulation, which oscillate because of the ellipsoidal
reference trajectory in (14). Due to the sharp maneuvers required, the time horizon shortens and the parameter increases
on the left and right sides of the ellipse. On flatter regions,
the PMPC reduces the parameter and increases the sampling
time. The zoomed portion displays the discrete nature of the
PMPC solution.

(β − 1)((lw /rw )e2 Rl (θ)−1 (r − xsi +ṙ))2

arg min
l∈R,∆t∈R≥0

t0

+ βl2 ds + (1/∆t)

R(θ)e1 0
s.t. ẋ =
R (θ)−1 (r − xsi + ṙ)
0
1 l


x(t0 ) = xt0 ,
where l and ∆t are the decision variables and β = 0.01.
Both simulation and experimental results utilize Alg. 1 to

l =0.030937
t =3.3119

l =0.027048
t =3.4262

l =0.043572
t =3.2757

Angular Velocity ( )

Fig. 4: Robot during the PMPC experiment. This figure shows that the parameter grows and sampling horizon shrinks when
the robot must perform more complex maneuvers (i.e., on the left and right sides of the ellipse). Over the flatter portions
of the ellipse, the parameter increases and sampling horizon (solid line) reduces, allowing the robot to track the reference
(solid circle) more closely.

with α = 0.99. This assignment to α in (12) implies that

1

l∗ = 0.078.

0.5

Note that this value of l∗ is only for the one-time selection.
The PMPC method induces different parameter values every
0.033 s.

0
-0.5

Static
PMPC

-1
0

20

40

60

80

100

Time (s)

Fig. 5: Angular velocity of robots for PMPC method (dashed
line) versus static parameterization (solid line). In this case,
both methods generate similar angular velocities, but the
PMPC method produces better tracking.

solve for the optimal parameters and time horizon online
with the step-size values
γ1 = 0.001, γ2 = 0.01.
Each experiment initially executes Alg. 1 to termination;
then, steps are performed each iteration to ensure that the
current values stays close to the locally optimal solution
realized by Alg. 1. In particular, each iteration takes 0.033 s,
which is the Robotarium’s sampling interval.
For comparison, the one-time selection method stems
directly from the abstraction cost formulated in Sec. IV-C

Parameter (l)

0.06
0.05
0.04
0.03
0.02
0

20

40

60

80

100

Time (s)

Fig. 6: Parameter (left) and sampling horizon (right) from
PMPC experiment on the Robotarium. The PMPC program
reduces the sampling horizon and increases the parameter to
cope with the sharp maneuvers required at the left and right
sides of the ellipse. On flatter regions, the PMPC decreases
the parameter and increases the time horizon, providing
better reference tracking. The zoomed portion illustrates the
discrete nature of the PMPC solution.

B. Simulation Results
This section contains the simulation results for the method
described in Sec. V-A. In particular, the simulation compares
the proposed PMPC method to the one-time selection process
in Sec. IV-C, showing that the PMPC method can outperform
the one-time selection. Fig. 3 shows the simulated angular
velocities, and Fig. 2 shows the parameter and sampling
horizon evolution. Both methods generate similar control
inputs. However, Fig. 2 demonstrates that the PMPC method
selects smaller parameter values, implying that this method
provides better reference tracking.
Additionally, Fig. 2 also shows that the sampling horizon
shortens and the parameter increases around the left and
right portions of the ellipse, because these regions require
sharper maneuvers and incur a higher maneuverability cost.
Furthermore, the ellipsoidal reference trajectory induces the
oscillations in Fig. 2. Overall, these simulated results show
that the PMPC method can outperform a static parameterization.
C. Experimental Comparison
This section contains the experimental results of the implementation described in Sec. V-A. The physical experiments
for this paper were deployed on the Robotarium and serve
to highlight the efficacy and validity of applying the PMPC
approach on a real system. Additionally, the experiments
display the propriety of the maneuverability cost outlined
in Sec. IV-B.
Figs. 5-6 display the angular velocity of the mobile robot,
the sampling horizon, and the parameter selection, respectively. As in the simulated results, Fig. 5 shows that the static
parameterization and PMPC method produce similar angular
velocities, and Fig. 6 shows that the PMPC method is able
to adaptively adjust the parameter and sampling horizon to
handle variations in the reference signal.

Moreover, on a physical system, the PMPC method still
adjusts the time horizon and parameter to account for maneuverability requirements. For example, on the left and
right sides of the ellipse, the maneuverability cost rises,
because the reference turns sharply. Thus, the parameter
increases and the sampling horizon decreases. Over flat
portions of the ellipse, the maneuverability cost decreases,
permitting the extension of the time horizon and reduction
of the parameter (i.e., better tracking). That is, reductions
of the maneuverability cost permit decreasing the parameter
l, allowing the PMPC strategy to outperform the static
parameterization. Furthermore, the decrease of the sampling
horizon during high-maneuverability regions accelerates the
execution of Alg. 1, which is useful in a practical sense.
VI. C ONCLUSION
This work presented a variable-sampling-horizon Parametric Model Predictive Control (PMPC) method that allows for
optimal parameter and sampling horizon selection with the
application of controlling differential-drive mobile robots. To
formulate an appropriate cost for the PMPC strategy, this
article discussed a class of Near-Identity Diffeomorphisms
(NIDs) that allow the transformation of single-integrator
algorithms to unicycle-modeled systems. Additionally, this
work showed an inherent trade-off induced by the NID and
formulated precision and maneuverability costs that allow
for the optimal parameterization of the NID via a PMPC
program. Furthermore, simulation and experimental results
were produced that illustrated the validity of the proposed
costs and the efficacy of the PMPC method.
R EFERENCES

[1] M. Ji and M. Egerstedt, “Distributed formation control while preserving connectedness,” in Proceedings of the IEEE CDC, pp. 5962–5967,
Dec 2006.
[2] A. Y. Yazicioğlu and M. Egerstedt, “Leader selection and network
assembly for controllability of leader-follower networks,” in 2013
ACC, pp. 3802–3807, June 2013.
[3] Y. Hong, J. Hu, and L. Gao, “Tracking control for multi-agent
consensus with an active leader and variable topology,” Automatica,
vol. 42, no. 7, pp. 1177 – 1182, 2006.
[4] W. Ni and D. Cheng, “Leader-following consensus of multi-agent
systems under fixed and switching topologies,” Systems & Control
Letters, vol. 59, no. 3–4, pp. 209 – 217, 2010.
[5] J. Cortes et al., “Coverage control for mobile sensing networks:
Variations on a theme,” in Proceedings of the MCCA, July 2002.
[6] A. Astolfi, “Exponential stabilization of a wheeled mobile robot via
discontinuous control.,” Journal of dynamic systems, measurement,
and control, vol. 121, no. 1, pp. 121–126, 1999.
[7] R. Olfati-Saber, “Near-identity diffeomorphisms and exponential tracking and -stabilization of first-order nonholonomic se(2) vehicles,” in Proceeding of the 2002 ACC, May 2002.
[8] R. Olfati-Saber, “Exponential -tracking and -stabilization of secondorder nonholonomic se(2) vehicles using dynamic state feedback,”
in Proceedings of the 2002 ACC (IEEE Cat. No.CH37301), vol. 5,
pp. 3961–3967 vol.5, May 2002.
[9] P. Ogren, M. Egerstedt, and X. Hu, “A control Lyapunov function
approach to multi-agent coordination,” in Decision and Control, 2001.
Proceedings of the IEEE Conference on, vol. 2, 2001.
[10] G. Oriolo, A. D. Luca, and M. Vendittelli, “Wmr control via dynamic
feedback linearization: design, implementation, and experimental validation,” IEEE Transactions on Control Systems Technology, vol. 10,
Nov 2002.
[11] E. Yang, D. Gu, and H. Hu, “Nonsingular formation control of cooperative mobile robots via feedback linearization,” in 2005 IEEE/RSJ
IROS, pp. 826–831, Aug 2005.
[12] G. Droge and M. Egerstedt, “Adaptive look-ahead for robotic navigation in unknown environments,” in 2011 IEEE/RSJ International
Conference on Intelligent Robots and Systems, pp. 1134–1139, Sept
2011.
[13] G. Droge and M. Egerstedt, “Adaptive time horizon optimization
in model predictive control,” in Proceedings of the 2011 American
Control Conference, pp. 1843–1848, June 2011.

