Principles of Robot Autonomy I

Trajectory tracking and closed-loop control

Motion control
• Given a nonholonomic system, how to control its motion from an
initial configuration to a final, desired configuration

• Aim
• Learn how to handle bound constraints via space-time separation
• Learn about trajectory tracking
• Learn about closed-loop control

• Readings
• B. Siciliano, L. Sciavicco, L. Villani, and G. Oriolo. Robotics: modelling, planning and
control. 2010. Chapter 11.
9/30/21 AA 274A | Lecture 4 2
Summary of previous lecture
• A nonlinear system 𝐱̇ = 𝐚(𝐱, 𝐮) is differentially flat if there exists a
set of outputs 𝐳 = α(𝐱, 𝐮, . . . , 𝐮(") ) such that

• One can then use any interpolation scheme (e.g., piecewise

polynomial (spline)) to plan the trajectory of 𝐳 in such a way as to
satisfy the appropriate boundary conditions
• The evolution of the state variables 𝐱, together with the associated
control inputs 𝐮, can then be computed algebraically from 𝐳
9/30/21 AA 274A | Lecture 4 3
Summary of previous lecture
• Constraints on the system can be transformed into the flat output
space and (typically) become limits on the curvature or higher order
derivative properties of the curve

• An important class of constraints is represented by bounds on some

of the system variables, and in particular the inputs, for example:
|𝑣 𝑡 | ≤ 𝑣!"# and |𝜔(𝑡)| ≤ 𝜔!"#

• Bound constraints can be effectively addressed via time scaling

9/30/21 AA 274A | Lecture 4 4

Path and time scaling law
• The problem of planning a trajectory can be divided into two steps:
1. computing a path, that is, a purely geometric description of the sequence
of configurations achieved by the robot, and
2. devising a time scaling law, which specifies the times when those
configurations are reached
• Mathematically, a trajectory 𝐱(𝑡) can be broken down into a
geometric path 𝐱(𝑠) and a timing law 𝑠 = 𝑠(𝑡), with the parameter
𝑠 varying between 𝑠 𝑡$ = 𝑠$ and 𝑠 𝑡% = 𝑠% in a monotonic
fashion, i.e., with 𝑠̇ 𝑡 > 0
• A possible choice for 𝑠 is the arc length along the path (in this case,
𝑠$ = 0, and 𝑠% = 𝐿, the length of the path)
9/30/21 AA 274A | Lecture 4 5
Enforcing bound constraints
• Such a space-time separation implies that
𝑑𝐱(𝑡) 𝑑𝐱(𝑠(𝑡))
𝐱̇ 𝑡 = = 𝑠̇ 𝑡
𝑑𝑡 𝑑𝑠
• Thus, once the geometric path is determined, the choice of a timing
law 𝑠 = 𝑠(𝑡) will identify a particular trajectory along this path, with
a corresponding set of time-scaled inputs (Problem 1 in pset)
• Example, for unicycle model
$|𝐱(()| $|𝐱(*(())|
•𝑣 𝑡 = $(
= $*
𝑠̇ 𝑡 = 𝑣(𝑠)
- 𝑠̇ 𝑡
$+(() $+(*(()) ,(()
•𝜔 𝑡 = $( = $* 𝑠̇ 𝑡 =𝜔
. 𝑠 𝑠̇ 𝑡 = 𝜔
. 𝑠 -
,(*)

• Simplest choice, with 𝑠 being arc length: 𝑠 𝑡 = 𝑡 𝐿/𝑇

9/30/21 AA 274A | Lecture 4 6
The see-think-act cycle
Knowledge Mission goals
Localiza(on posi(on Decision making
Map Building global map Mo(on planning

environmental model trajectory

local map

Informa(on Trajectory
extrac(on execu(on

actuator
raw data commands
See-think-act
Sensing Actua(on Closed-loop
control/tracking
Real world
environment

9/30/21 AA 274A | Lecture 4 7

Trajectory tracking
• Back to two-step design strategy Tracking control law

• Reference trajectory and control history (i.e., 𝐱 & 𝑡 and 𝐮& 𝑡 ) are
computed via open-loop techniques (e.g., differential flatness)
• For reference tracking (Problem 3 in pset)
• Geometric (e.g., pursuit) strategies
• Linearization (either approximate or exact) + linear structure
• Non-linear control
• Optimization-based techniques (e.g., MPC)
9/30/21 AA 274A | Lecture 4 8
Trajectory tracking for differentially flat systems
• Key fact (see, e.g,. Levine 2009): a differentially flat system can be
linearized by (dynamic) feedback and coordinate change, that is it
can be equivalently transformed into the system
𝐳 ('()) = 𝐰
• One can then design a tracking controller for the linearized system
by using linear control techniques; in particular, for a given
reference flat output 𝐳& , define the component-wise error
('())
𝑒* : = 𝑧* − 𝑧*,& , which implies 𝑒* = 𝑤* − 𝑤*,&
• For guaranteed convergence to zero of tracking error, one can set
' (,)
𝑤* = 𝑤*,& − ∑,-$ 𝑘*,, 𝑒* ,
with the gains {𝑘*,, } chosen so as to enforce stability
9/30/21 AA 274A | Lecture 4 9
Trajectory tracking for differentially flat systems
• Example: dynamically extended unicycle model
ẋ(t) = V cos(✓(t))
ẏ(t) = V sin(✓(t))
V̇ (t) = a(t)
˙ = !(t)
✓(t)

• The system is differentially flat with flat outputs (𝑥, 𝑦), in particular
   
ẍ(t) cos(✓) V sin(✓) a w1
= :=
ÿ(t) sin(✓) V cos(✓) ! w2
| {z }
:=J
9/30/21 AA 274A | Lecture 4 10
Trajectory tracking for differentially flat systems
• Then one can use the following virtual control law for trajectory
tracking:
w1 = ẍd + kpx (xd x) + kdx (ẋd ẋ)
w2 = ÿd + kpy (yd y) + kdy (ẏd ẏ)

where 𝑘". , 𝑘&. , 𝑘"/ , 𝑘&/ > 0 are control gains

• Such a law guarantees exponential convergence to zero of the

Cartesian tracking error

9/30/21 AA 274A | Lecture 4 11

Closed-loop control
• General closed-loop control: we want to find

• Main techniques:
• Hamilton–Jacobi–Bellman equation, dynamic programming
• Lyapunov analysis

For an in-depth study of this topic, see AA203 “Optimal and Learning-
based Control” (Spring 2020)

9/30/21 AA 274A | Lecture 4 12

Closed-loop control: posture regulation
• Consider a differential drive mobile robot

✓
y

x
• Inputs: V (linear velocity of the wheel) and 𝜔 (angular velocity
around the vertical axis)
• Goal: drive the robot to the origin [0, 0, 0]
9/30/21 AA 274A | Lecture 4 13
Control based on polar coordinates
• Polar coordinates
• 𝜌: distance of the reference point of the unicycle from the goal
• 𝛼: angle of the pointing vector to the goal w.r.t. the unicycle main axis
• 𝛿: angle of the same pointing vector w.r.t. the 𝑋. axis

• Coordinate transformation
• 𝜌 = 𝑥/ + 𝑦/
• 𝛼 = atan2 𝑦, 𝑥 − 𝜃 + 𝜋
• 𝛿 =𝛼+𝜃

9/30/21 AA 274A | Lecture 4 14

Equations in polar coordinates
• In polar coordinates, the unicycle equations become

• In order to achieve the goal posture, variables (𝜌, 𝛼, 𝛿) should all

converge to zero

9/30/21 AA 274A | Lecture 4 15

Control law
• Closed-loop control law (Problem 2 in pset):

• If 𝑘! , 𝑘" , 𝑘# > 0, then closed-loop system is globally asymptotically

driven to the posture (0,0,0)!

• For more details, see M. Aicardi, G. Casalino, A. Bicchi, and A. Balestrino

(1995). Closed loop steering of unicycle like vehicles via Lyapunov
techniques. IEEE Robotics & Automation Magazine.
9/30/21 AA 274A | Lecture 4 16
Summary
• We covered closed-loop control along two main dimensions
1. Trajectory tracking (useful to infuse robustness of point-to-point motion)
2. Posture regulation (useful for final phase of motion)

• We’ll see in Pset 2 how the topics of differential flatness, trajectory

tracking, posture regulation, and motion planning will lead to an
end-to-end trajectory optimization module

9/30/21 AA 274A | Lecture 4 17

Next time: graph search methods for motion planning

9/30/21 AA 274A | Lecture 4 18