3

Closed-Loop Motion Planning & Control

The previous chapter introduced the concepts of open-loop and closed-loop con-
trol laws, and then dove into techniques for designing open-loop control laws
for robots based on optimal control and differential flatness. These techniques
are useful for determining control inputs that accomplish different objectives,
such as “move from point A to point B in a minimal amount of time while sat-
isfying some constraints”. Additionally, computing open-loop control laws is
often computationally less challenging that computing closed-loop control laws.
However in practice open-loop control is not very robust since observations
are not leveraged to update the control input. One solution to this robustness
problem is to convert the open-loop control law into a closed-loop control law,
typically referred to as trajectory tracking controllers. Another solution is to not
use any open-loop techniques but rather to directly synthesize a closed-loop
control law, for example by performing a Lyapunov stability analysis. This chap-
ter will introduce techniques for synthesizing closed-loop controllers in both of
these ways.

Closed-loop Motion Planning & Control

Recall from the previous chapter that open-loop control laws are defined as a
function of time for a given initial condition. In contrast, closed-loop control
laws are a function of the current state, and therefore are reactive.

Definition 3.0.1 (Closed-loop Control). If the control law is a function of the state
and time, i.e.,
u ( t ) = π ( x ( t ), t ) (3.1)

then the control is said to be in closed-loop form.

Closed-loop controllers (also sometimes referred to as feedback controllers

or policies), are much more robust than open-loop controllers. For example,
suppose a controller needs to be designed to make a wheeled robot move from
point to point. If the model used for open-loop controller design wasn’t perfect,
if the initial state was not perfectly known, or if external disturbances affected
2 closed-loop motion planning & control

the system (e.g. wheel slipping), then the robot would not exactly reach its
desired destination. Alternatively, a closed-loop control law can continuously
correct for these errors since it is always taking in new information.

3.1 Trajectory Tracking

One common approach to closed-loop control is to simply extend the open-loop

control techniques from the previous chapter to include a feedback component.
Such an approach consists of two steps:

1. Use open-loop control techniques to design a desired trajectory xd (t) and

corresponding control ud (t).

2. Design a closed-loop control law that is designed to make sure the system
stays close to the desired trajectory.

These controllers are referred to as trajectory tracking controllers, and their con-
trol law is defined as

u ( t ) = u d ( t ) + π ( x ( t ) − x d ( t ), t ). (3.2)

This type of control law is also said to be a “feedforward plus feedback” con-
troller. This is because the term ud (t) is an open-loop “feedforward” term that
attempts to generally make the system follow the desired trajectory, while the
term π ( x(t) − xd (t), t) is a “feedback” term that attempts to correct for any
errors.
The previous chapter discussed techniques for solving open-loop control
problems to define the desired trajectory, and additionally there are several
approaches for designing the feedback component π ( x(t) − xd (t), t):

• Geometric approaches generally leverage some sort of insight about the sys-
tem and are therefore hard to discuss in general settings. They are also typi-
cally difficult to derive theoretical guarantees for.

• Linearization based approaches typically linearize nonlinear dynamics models

about points along the desired trajectory. These linearized models are then
used to design linear controllers (e.g. linear quadratic regulators). For some
nonlinear systems, instead of linearizing about specific points it possible to
feedback linearize the system. This essentially means that the non-linearities
can be exactly “canceled” out such that the system can be considered lin-
ear. Linear control theory can then be applied to design a feedback control
scheme.

• Non-linear control techniques also exist which do not rely on linearization.

These approaches are also heavily system dependent, but one common tool
for non-linear control is based on Lyapunov theory.
 principles of robot autonomy 3

• Optimization-based feedback control laws can also be designed. These ap-

proaches often leverage optimal control theory, some of which was pre-
sented in the previous chapter. One common optimization-based approach
for closed-loop control is known as model predictive control (MPC).

3.1.1 Trajectory Tracking for Differentially Flat Systems

For differentially flat systems linearization based approaches to designing tra-
jectory tracking controllers are particularly useful1 . In fact, every flat system can 1
J. Levine. Analysis and Control of
be linearized via dynamic feedback and a coordinate change to yield a dynami- Nonlinear Systems: A Flatness-based
Approach. Springer, 2009
cal system of the form
z(q+1) = w, (3.3)

where z(q+1) is the q + 1-th order derivative of the flat outputs z and q is the
degree of the flat output space (i.e. the highest order of derivatives of the flat
output that are needed to describe system dynamics), and w is a modified “vir-
tual” input term.
The set of ODEs (3.3) are linear, which means that techniques from linear
control theory can be applied to design a control law for w. In particular, for
trajectory tracking problems suppose a reference flat output trajectory zd (t)
has been defined which corresponds to the virtual input wd (t). Let the error
between the actual flat output and desired flat output be defined as e(t) =
z(t) − zd (t) and consider a closed-loop control law of the form
q
∑ ki,j ei
( j)
wi (t) = wi,d (t) − ( t ), (3.4)
j =0

( j)
where (·)i denotes the i-th component of the vector, e( j) = z( j) − zd is the j-th
order derivative of the error, and k i,j are called controller gains. The application
of this control law to the system (3.3) will result in closed-loop dynamics of the
form
q
z ( q +1 ) = wd − ∑ K j e( j) ,
j =0

( q +1)
where K j is a diagonal matrix with i-th diagonal element k i,j . Since zd =
wd (t) this can be simplified to give the closed-loop error dynamics:
q
e ( q +1) + ∑ K j e( j) = 0. (3.5)
j =0

This set of linear ODEs describes the dynamics of the error, and many classical
techniques from linear control theory can be used to choose the gains k i,j that
will guarantee this system is stable. Having stable error dynamics means that
the error will decay to zero, which in this case means the system will track the
desired trajectory.
4 closed-loop motion planning & control

Example 3.1.1 (Extended Unicycle Trajectory Tracking). Consider the dynami-

cally extended unicycle model

ẋ (t) = v cos(θ (t)),

ẏ(t) = v sin(θ (t)),
(3.6)
v̇(t) = a(t),
θ̇ (t) = ω (t),

where the two inputs are the acceleration a(t) and the rotation rate ω (t). This
system is differentially flat with flat outputs x and y and order q = 1. It can
therefore be expressed as:
" # " #" # " #
ẍ cos(θ ) −V sin(θ ) a w1
z̈ = = := ,
ÿ sin(θ ) V cos(θ ) ω w2
| {z }
:= J

and a trajectory tracking controller can be defined as

w1 = ẍd − k px ( x − xd ) − k dx ( ẋ − ẋd ),
w2 = ÿd − k py (y − yd ) − k dy (ẏ − ẏd ),

where (·)d represents a term associated with the desired trajectory. The control
inputs a(t) and ω (t) can then be computed by solving the linear system
" # " #
a w1
J = ,
ω w2

assuming that J is full rank.

3.2 Closed-loop Control

Trajectory tracking is just one example of closed-loop control, which assumes

the existence of a desired trajectory for which to track. As previously discussed,
one way of computing the desired trajectory is by solving an open-loop opti-
mal control problem. However, in the context of optimal control, modifying
an open-loop optimal control with feedback is not always the most desirable
option. Instead, it may be preferred to just directly solve a closed-loop optimal
control problem to obtain an optimal policy u∗ = π ( x(t), t). Techniques for
solving closed-loop optimal control problems typically are based on either the
Hamilton-Jacobi-Bellman equation or dynamic programming.
Another common closed-loop control problem is to drive to or stabilize the
system about a particular state (often called regulation). For systems with linear
dynamics models the most controller for regulation problems is called the lin-
ear quadratic regulator. However, for nonlinear systems, stabilizing closed-loop
controllers are commonly designed through Lyapunov analysis2 . 2
J.-J. E. Slotine and W. Li. Applied
Nonlinear Control. Pearson, 1991
 principles of robot autonomy 5

3.2.1 Lyapunov-based Control

A Lyapunov stability analysis is a common tool for analyzing the stability of
nonlinear systems. This analysis is based on the definition of a Lyapunov func-
tion, which can be thought of as a measure of the “energy” of the system. Sim-
ilar to mechanical systems, if the energy does not increase in time then the
system is considered stable3 . 3
Note there are more technical defini-
The most challenging part of a Lyapunov stability analysis is finding a suit- tions of stability, but for simplicity these
will not be discussed here
able Lyapunov function, and for many complex systems this may be extremely
difficult. However, one of the advantages of the method is that it provides nice
theoretical guarantees regarding the stability of the system, and is applicable to
any system of interest.
Example 3.2.1 (Pose Stabilization). 4 Consider a robot that is modeled by the 4
M. Aicardi et al. “Closed loop steering
unicycle robot model (differential drive robot model) represented graphically in of unicycle like vehicles via Lyapunov
techniques”. In: IEEE Robotics & Au-
Figure 3.1 tomation Magazine 2.1 (1995), pp. 27–
35
ẋ (t) = v(t) cos θ (t),
ẏ(t) = v(t) sin θ (t), (3.7)
θ̇ (t) = ω (t),
where the control inputs are the robot speed v and the rotational rate ω. The
objective is to design a closed-loop controller that will drive the robot the origin
(i.e. x = 0, y = 0, θ = 0).
To make the controller design easier the dynamics will be alternatively ex-
pressed in polar coordinates. This can be accomplished by defining Figure 3.1: Pose stabilization
q of a unicycle robot in Cartesian
ρ = x 2 + y2 , coordinates.
α = atan2(y, x ) − θ + π, (3.8)
δ = α + θ,
where ρ is the Euclidean distance to the origin, α is the heading angle with
respect to the line from the robot to the origin, and δ is the angle between the x-
axis and the line from the robot to the origin. These coordinates are graphically
shown in Figure 3.2.
Using the newly defined polar coordinates, the dynamics equations (3.7) can
be equivalently expressed as
Figure 3.2: Pose stabilization
ρ̇(t) = −v(t) cos α(t), of a unicycle robot using polar
v(t) sin α(t) coordinates.
α̇(t) = − ω ( t ), (3.9)
ρ(t)
v(t) sin α(t)
δ̇(t) = .
ρ(t)
By expressing the dynamics in polar form, a Lyapunov stability analysis can
now be easily performed. Consider the following candidate Lyapunov function:
1 2 1 2
V (ρ, α, θ ) = ρ + ( α + k 3 δ2 ), (3.10)
2 2
6 closed-loop motion planning & control

and consider the following closed-loop control law:

v = k1 ρ cos α,
sin α cos α (3.11)
ω = k2 α + k1 ( α + k 3 δ ),
α
where k1 , k2 , k3 > 0.
The candidate Lyapunov function is quadratic and therefore is positive ev-
erywhere, V ≥ 0, and is equal to zero only at the origin with ρ = 0, α = 0,
δ = 0. Therefore, if it is possible to show that along all closed-loop system tra-
jectories the Lyapunov function is decreasing (V̇ < 0), then it can be guaranteed
that the system will converge to the origin! To show that the Lyapunov function
decreases along trajectories of the system, begin by taking the derivative of V:

V̇ = ρρ̇ + αα̇ + k3 δδ̇.

This quantity can now be shown to decrease along all closed-loop trajectories by
substituting in the dynamics equations (3.8) with the closed-loop control law as
defined by (3.11):

V̇ = ρρ̇ + αα̇ + k3 δδ̇,

v sin α
k δv sin α
= −ρv cos α + α −ω + 3 ,
ρ ρ
= −k1 ρ2 cos2 α − k2 α2 ,

where in the last line the control laws were substituted in for v and ω and al-
gebraically simplified. Note that since k1 and k2 have been chosen to be strictly
positive, this function must be strictly negative for all values of ρ and α! There-
fore this Lyapunov stability analysis has theoretically proven that the system
under the closed-loop control law (3.11) will converge to the origin.

3.3 Exercises

Both exercises for this chapter can be found in the online repository:

https://github.com/PrinciplesofRobotAutonomy/AA274A_HW1.

3.3.1 Pose Stabilization

Complete Problem 2: Pose Stabilization, where you will implement the Lyapunov-
based pose controller for the unicycle robot described in Example 3.2.1.

3.3.2 Trajectory Tracking

Complete Problem 3: Trajectory Tracking, where you will implement the differen-
tial flatness-based trajectory tracking controller for the extended unicycle robot
described in Example 3.1.1.
Bibliography

[1] M. Aicardi et al. “Closed loop steering of unicycle like vehicles via Lya-
punov techniques”. In: IEEE Robotics & Automation Magazine 2.1 (1995),
pp. 27–35.
[2] J. Levine. Analysis and Control of Nonlinear Systems: A Flatness-based Ap-
proach. Springer, 2009.
[3] J.-J. E. Slotine and W. Li. Applied Nonlinear Control. Pearson, 1991.