EN530.

678 Nonlinear Control and Planning in Robotics

Lecture 2: System Models
January 29, 2020

Prof: Marin Kobilarov

0.1 Constraints
The configuration space of a mechanical sysetm is denoted by Q and is assumed to be an n-
dimensional manifold, locally isomorphic to Rn (we’ll say exactly what this means in a future
lecture). A configuration is denoted by q ∈ Q.
We first introduce the notion of constraints:
• holonomic (or geometric):
hi (q) = 0, i = 1, . . . , k
restrict possible motions to a n − k dimensional sub-manifold (think hypersurface embedded
in a larger ambient space)
• linear (Pfaffian) nonholonomic (or kinematic):

aTi (q)q̇ = 0, i = 1, . . . , k, or AT (q)q̇ = 0 in matrix form

linear in the velocities

Nonholonomic constraints are not integrable, i.e. it is not possible to find k functions hi such
that
∇q hi (q) = ai (q), i = 1, . . . , k
If one can find such functions then the constraint is holonomic, i.e.
Z Z
ai (q(t))q̇(t)dt = ∇hi (q(t))T q̇(t)dt = hi (q) + c,
T

where c is a constant of integration.

Holonomic constraints are inherently different than nonholonomic. If a(q)T q̇ = 0 can be in-
tegrated to obtain h(q) = c, then the motion is restricted to lie on a level surface (a leaf) of h
corresponding to the constant c obtained by the initial condition c = h(q0 ). Practically speaking,
once the system is on the surface, it cannot escape.
Consider a single constraint a(q)T q̇ = 0. When the constraint is nonholonomic the instantaneous
motion (velocity) is allowed in all directions except for a(q) (i.e. to an n − 1-dimensional space).
But it could still be possible to reach any configuration in Q. So the system will leave the surface.
Example 1. The unicycle. The canonical example of a nonholonomic system is the unicycle (a.k.a.
the rolling disk). The configuration is q = (x, y, θ) denoting position (x, y) and orientation θ. There
is one constraint, i.e. the unicycle must move in the direction in which it is pointing:
ẏ
ẋ sin θ − ẏ cos θ = 0, or = tan θ,
ẋ

1
We have  
sin θ
a(q) =  − cos θ  .
0
The feasible velocities are then contained in the null space of A(q) = a(q), i.e.
   
 cos θ 0 
null(aT (q)) = span  sin θ  ,  0 
0 1
 

This system starts at configuration q0 = (x0 , y0 , θ0 ) and can reach any desired final configuration
qf = (xf , yf , θf ). The simplest strategy is first to rotate so that the disk points to (xf , yf ), then
move forward until (xf , yf ) is reached, then turn in place until the orientation reaches θf .
Draw a picture of the motion in the the configuration space.

More generally, let us denote the allowed directions of motion by vectors gj , i.e.

ai (q)T gj (q) = 0, i = 1, . . . , k, j = 1, . . . , n − k

or in matrix form
AT (q)G(q) = 0.
The feasible trajectories of the mechanical system are the solutions of
m
X
q̇ = gj (q)vj = G(q)v,
j=1

where v(t) ∈ Rm , m = n − k, are called reduced velocities or pseudovelocities

We will be concerned with two classes of models. Kinematic models assume that v can be
directly controlled. Dynamic models require the derviation of another differential equation deter-
mining the evolution of v.
For kinematic systems the question of controllability is equivalent to nonholonomy.

0.2 Dynamics
How do we obtain ẋ = f (t, x, u) for dynamical systems? We will focus on mechanical systems
with equations of motion derived through a Lagrangian approach, which is often sufficient for most
systems of interest in robotics.

0.2.1 Holonomic Underactuated Systems

Let q ∈ Rn denote generalized coordinates. Assume that the system has a Lagrangian
1
L(q, q̇) = q̇ T M (q)q̇ − V (q),
2
with inertia matrix M (q) > 0 and potential energy V (q). The system is subject to external forces
fext (q, q̇) ∈ Rn and control inputs τ ∈ Rm .

2
 The equations of motion in terms of the Lagrangian (i.e.the Euler-Lagrange equations) are given
by
d
∇q̇ L − ∇q L = fext (q, q̇) + B(q)τ,
dt
where B(q) ∈ Rn×m is a matrix mapping from m control inputs to the forces/torques acting on the
generalized coordinates q.
This equation is obtained from Lagrange-d’Alembert variational principle
Z tf Z tf
δ L(q, q̇)dt + [fext (q, q̇) + B(q)τ )]T δq(t) = 0.
t0 t0

The actual equations take the form

M (q)q̈ + b(q, q̇) = B(q)τ, (1)

where
1
b(q, q̇) = Ṁ (q)q̇ − ∇q (q̇ T M (q)q̇) + ∇q V (q) − fext (q, q̇).
2
The system is written in control form in terms of the state x = (q, q̇) as
   
q̇ 0
ẋ = f (x) + g(x)u = + u
−M (q)−1 b(q, q̇) M (q)−1 B(q)

Example 2. 2-dof manipulator. Consider a 2 dof-manipulator subject to gravity with the following
parameters:
Description Notation
Length of link #1 l1
Length of link #2 l2
Distance to COM of link #1 lc1
Distance to COM of link #2 lc2
link #1 mass m1
link #2 mass m2
link #1 inertia I1
link #2 inertia I2
gravity acceleration g
The mass matrix is
m1 `2c1 + m2 [l12 + l2c
2 + 2l l cos q ] + I + I 2
 
1 c2 2 1 2 m2 (lc2 + l1 lc2 cos q2 ) + I2
M (q) = 2 + l l cos q ) + I 2 +I ,
m2 (lc2 1 c2 2 2 m2 lc2 2

while the bias term is

   
−m2 l1 lc2 sin(q2 )q̇2 −m2 l1 lc2 sin(q2 )[q̇1 + q̇2 ] [m1 lc1 + m2 l1 ]g sin(q1 ) + m2 lc2 g sin(q1 + q2 )
b(q, q̇) = q̇+ ,
m2 l1 lc2 sin(q2 )q̇1 0 m2 lc2 g sin(q1 + q2 )

For fully actuated manipulator we have B(q) = I. For actuation only at the first joint we have
 
1 0
B(q) = .
0 0

3
Example 3. Simplified model of a boat in 2D, with two rear propellers. The configuration is
denoted by q = (x, y, θ). The mass matrix is given by
 
m 0 0
M (q) =  0 m 0 .
0 0 J

while the bias is

b(q, q̇) = R(θ)D(q̇)R(θ)T q̇,
where tha matrix D(q̇) ≥ 0 denotes drag terms and R(θ) is the rotation matrix
 
cos θ − sin θ 0
R(θ) =  sin θ cos θ 0 
0 0 1

which transforms forces from body-fixed to spatial frame. The control martix is
 
1 1
B(q) = R(θ)  0 0  ,
−r r

where the constant r > 0 denotes the distance between each thruster and central axis.

0.2.2 Nonholonomic Systems

Assume that the system has a Lagrangian
1
L(q, q̇) = q̇ T K(q)q̇ − V (q),
2
with inertia matrix K(q) > 0 and potential energy V (q). The system is subject to external forces
fext (q, q̇) and control inputs τ ∈ Rm .
The Euler-Lagrange equations take the form
d
∇q̇ L − ∇q L = A(q)λ + fext (q, q̇) + S(q)τ,
dt
where S(q) ∈ Rn×m is a matrix mapping from m control inputs to the forces/torques acting on the
generalized coordinates q and where λ ∈ Rk is a vector of Lagrange multipliers. The term A(q)λ
should be understood as a force which counters any motion in directions spanned by A(q).
This equation is obtained from the Lagrange-d’Alembert variational principle
Z tf Z tf
δ L(q, q̇)dt + [fext (q, q̇) + S(q)τ )]T δq(t) = 0,
t0 t0

subject to both A(q)T q̇ = 0 and A(q)T δq(t) = 0, i.e. the variations are restricted as well.
The actual equations take the form

K(q)q̈ + n(q, q̇) = A(q)λ + S(q)τ, (2)

T
A (q)q̇ = 0, (3)

4
where
1
n(q, q̇) = K̇(q)q̇ − ∇q (q̇ T K(q)q̇) + ∇q V (q)
2
The Lagrange multipliers can be eliminated by first noting that

AT (q)G(q) = 0

and multiplying (2) by GT (q) to obtain a reduced set of m = n − k differential equations

GT (q)(K(q)q̈ + n(q, q̇)) = GT S(q)τ.

A standard assumption will be that det(G(q)T S(q)) 6= 0 or that all feasible directions are control-
lable. The final equations are then expressed as

q̇ = G(q)v, (4)
M (q)v̇ + b(q, v) = B(q)τ, (5)

where

M (q) = GT (q)K(q)G(q) > 0

b(q, v) = GT K(q)Ġ(q)v + GT (q)n(q, G(q)v)
B(q) = GT (q)S(q)

using the notation

m
X
Ġ(q)v = (∇gi (q)T vi )G(q)v.
i=1

For nonholonomic systems, we would normally assume an isomorphism between pseudo-accelerations

a = v̇ and control inputs τ , i.e. any acceleration a can be achieved by setting

τ = B(q)−1 (M (q)a + b(q, v)).

That is why often in nonholonomic control we take a as the (virtual) control input, i.e. u ≡ a and
express the control system in terms of the state x = (q, v)
   
G(q)v 0
ẋ = f (x) + g(x)u = + u.
0 Im

Example 4. Unicycle. The configuration is q = (x, y, θ) with mas m, moment of inertia J, driving
force τ1 , steering force τ2 . The general dynamic model

K(q)q̈ + n(q, q̇) = A(q)λ + S(q)τ,

takes the form

      
m 0 0 ẍ sin θ cos θ 0  
 0 m 0   ÿ  =  − cos θ  λ +  sin θ 0  τ1 ,
τ2
0 0 J θ̈ 0 0 1

5
We have G(q) = S(q), GT (q)S(q) = I2 , and GT (q)B Ġ(q) = 0, from which we obtain the reduced
mass matrix and bias  
m 0
M (q) = , b(q, q̇) = 0.
0 J
The complete equations of motion are

ẋ = cos θv1
ẏ = sin θv1
θ̇ = v2
mv̇1 = τ1
J v̇2 = τ2 ,

which can be put in a standard form, for x = (x, y, θ, v1 , v2 )

ẋ = f (x) + g(x)τ.

Example 5. Simple car models. A common way to model a car for control purposes is to employ
the bycycle model, i.e. collapse each pair of wheels to a single wheel at the center of their axle.
The generalized coordinates are
q = (x, y, θ, φ),
where φ is the steering angle. We have the constraints

ẋ sin θ − ẏ cos θ = 0 front wheel (6)

ẋ sin(θ + φ) − ẏ cos(θ + φ) − θ̇` cos φ = 0 rear wheel (7)

For the real-wheel drive we have  

cos θ 0
 sin θ 0 
G(q) = 
 1 tan φ

` 0 
0 1
while for the front-wheel drive we have
 
cos θ cos φ 0
 sin θ cos φ 0 
G(q) = 
 1 sin φ

` 0 
0 1

A kinematic model is given by

q̇ = G(q)u,
where the inputs are u1 – rear drive velocity, u2 - steering. A dynamic model includes the dynamics
of v̇ which is the same as the unicycle.