Lecture 1 Intro and Dynamics Review
1. Continuous-Time Dynamics
(1) Basic form (most general/generic) for smooth systems:
x ˙ = f ( x , u ) \dot{\mathbf{x}} = \mathbf{f}(\mathbf{x}, \mathbf{u}) x˙=f(x,u)
x ∈ R n \mathbf{x}\in \mathbb{R}^n x∈Rn is the state, u ∈ R m \mathbf{u}\in \mathbb{R}^m u∈Rm is the control input, f : R n × R m → R n \mathbf{f}:\mathbb{R}^n\times \mathbb{R}^m\rightarrow \mathbb{R}^n f:Rn×Rm→Rn is the dynamics function.
The state can be written as x = [ q v ] \mathbf{x} = \begin{bmatrix}\mathbf{q} \\ \mathbf{v}\end{bmatrix} x=[qv], where q ∈ R n \mathbf{q}\in \mathbb{R}^n q∈Rn is called configuration (not always a vector), v ∈ R n \mathbf{v}\in \mathbb{R}^n v∈Rn is the velocity.
Attention: v \mathbf{v} v is not always equal to q ˙ \dot{\mathbf{q}} q˙.
(2) Example: pendulum
Dynamics:
m
l
2
θ
¨
+
m
g
sin
θ
=
τ
ml^2\ddot{\theta}+mg\sin\theta = \tau
ml2θ¨+mgsinθ=τ
where
θ
\theta
θ is the angle,
m
m
m is the mass,
l
l
l is the length,
g
g
g is the gravity,
τ
\tau
τ is the torque (and also the control input
u
\mathbf{u}
u).
Define
x
=
[
θ
θ
˙
]
\mathrm{x}= \begin{bmatrix}\theta \\ \dot{\theta}\end{bmatrix}
x=[θθ˙],
q
=
θ
\mathbf{q} = \theta
q=θ,
v
=
θ
˙
\mathbf{v} = \dot{\theta}
v=θ˙, then the dynamics can be written as
x
˙
=
[
θ
˙
θ
¨
]
=
[
θ
˙
−
g
l
sin
θ
+
1
m
l
2
τ
]
=
[
θ
˙
−
g
l
sin
θ
]
+
[
0
1
m
l
2
]
u
\dot{\mathbf{x}} = \begin{bmatrix}\dot{\theta}\\ \ddot{\theta} \end{bmatrix} = \begin{bmatrix}\dot{\theta} \\ -\frac{g}{l}\sin\theta+\frac{1}{ml^2}\tau\end{bmatrix} = \begin{bmatrix}\dot{\theta} \\ -\frac{g}{l}\sin\theta\end{bmatrix} + \begin{bmatrix}0 \\ \frac{1}{ml^2}\end{bmatrix}\mathbf{u}
x˙=[θ˙θ¨]=[θ˙−lgsinθ+ml21τ]=[θ˙−lgsinθ]+[0ml21]u
q ∈ S 1 q\in S^1 q∈S1(circle), not a vector space.
velocity v ∈ R v\in \mathbb{R} v∈R, which is a vector space.
state x ∈ S 1 × R x\in S^1\times \mathbb{R} x∈S1×R (cyllinder).
configuration space of a double pendulum: torus.
2. Control-Affine Systems
(1) Basic form
x ˙ = f 0 ( x ) + B ( x ) u \dot{\mathbf{x}} = \mathbf{f}_0(\mathbf{x}) + \mathbf{B}(\mathbf{x})\mathbf{u} x˙=f0(x)+B(x)u
f 0 ( x ) \mathbf{f}_0(\mathbf{x}) f0(x) is called “drift” and B ( x ) \mathbf{B}(\mathbf{x}) B(x) is called “input Jacobian”.
- not linear, but affine in control input u \mathbf{u} u.
- most systems can be written in this form.
(2) Example: pendulum
m
l
2
θ
¨
+
m
g
sin
θ
=
τ
ml^2\ddot{\theta}+mg\sin\theta = \tau
ml2θ¨+mgsinθ=τ
The above equation can be written as
f
0
(
x
)
=
[
θ
˙
−
g
l
sin
θ
]
,
B
(
x
)
=
[
0
1
m
l
2
]
\mathbf{f}_0(\mathbf{x}) = \begin{bmatrix}\dot{\theta} \\ -\frac{g}{l}\sin\theta\end{bmatrix}, \mathbf{B}(\mathbf{x}) = \begin{bmatrix}0 \\ \frac{1}{ml^2}\end{bmatrix}
f0(x)=[θ˙−lgsinθ],B(x)=[0ml21]
3. Manipulator Dynamics
(1) Basic form
M ( q ) v ˙ + C ( q , v ) = B ( q ) u \mathbf{M}(\mathbf{q})\dot{\mathbf{v}}+ \mathbf{C}(\mathbf{q}, \mathbf{v}) = \mathbf{B}(\mathbf{q})\mathbf{u} M(q)v˙+C(q,v)=B(q)u
M ( q ) \mathbf{M}(\mathbf{q}) M(q) is called “mass matrix”, C ( q , v ) \mathbf{C}(\mathbf{q}, \mathbf{v}) C(q,v) is called “dynamic bias” (coriolis + gravity), B ( q ) \mathbf{B}(\mathbf{q}) B(q) is called “input Jacobian”.
The notion B ( q ) u \mathbf{B}(\mathbf{q})\mathbf{u} B(q)u is overloaded, it a function of q \mathbf{q} q instead of x \mathbf{x} x.
The right hand side includes the unconserative forces, so external forces F \mathbf{F} F can be added to the right hand side.
(2) Velocity kinematics
q
˙
=
G
(
q
,
v
)
v
\dot{\mathbf{q}} = \mathbf{G}(\mathbf{q}, \mathbf{v})\mathbf{v}
q˙=G(q,v)v
(since
v
\mathbf{v}
v is not always equal to
q
˙
\dot{\mathbf{q}}
q˙)
Using this equation, we can rewrite the continuous-time dynamics as
x
˙
=
f
(
x
,
u
)
=
[
G
(
q
,
v
)
v
M
−
1
(
q
)
(
B
(
q
)
u
−
C
(
q
,
v
)
)
]
\dot{\mathbf{x}} = \mathbf{f}(\mathbf{x}, \mathbf{u}) = \begin{bmatrix}\mathbf{G}(\mathbf{q}, \mathbf{v})\mathbf{v} \\ \mathbf{M}^{-1}(\mathbf{q})\left(\mathbf{B}(\mathbf{q})\mathbf{u}-\mathbf{C}(\mathbf{q}, \mathbf{v})\right)\end{bmatrix}
x˙=f(x,u)=[G(q,v)vM−1(q)(B(q)u−C(q,v))]
(3) Example: Pendulum
mass matrix term:
M
(
q
)
=
m
l
2
\mathbf{M}(\mathbf{q}) = ml^2
M(q)=ml2
coriolis term:
C
(
q
,
v
)
=
g
l
sin
θ
\mathbf{C}(\mathbf{q}, \mathbf{v}) = gl\sin\theta
C(q,v)=glsinθ
input Jacobian term:
B
(
q
)
=
I
,
\mathbf{B}(\mathbf{q}) = I,
B(q)=I,
velocity kinematics matrix:
G
=
I
\mathbf{G}=I
G=I
All mechanimal systems can be written in this form because this function is a rewriting version of Euler-Lagrange equation.
Euler-Lagrange equation:
L = 1 2 v T M ( q ) v − U ( q ) , L = \frac{1}{2}\mathbf{v}^T\mathbf{M}(\mathbf{q})\mathbf{v}-U(\mathbf{q}), L=21vTM(q)v−U(q), where U ( q ) U(\mathbf{q}) U(q) is the potential energy, 1 2 v T M ( q ) v \frac{1}{2}\mathbf{v}^T\mathbf{M}(\mathbf{q})\mathbf{v} 21vTM(q)v is the kinetic energy.
4. Linear Systems
(1) Basic form
x ˙ = A ( t ) x + B ( t ) u \dot{\mathbf{x}} = \mathbf{A}(t)\mathbf{x}+\mathbf{B}(t)\mathbf{u} x˙=A(t)x+B(t)u
- If
A
(
t
)
=
A
\mathbf{A}(t) = \mathbf{A}
A(t)=A and
B
(
t
)
=
B
\mathbf{B}(t) = \mathbf{B}
B(t)=B, then the system is called “time-invariant”.
Otherwise, the system is called “time-varying”. - Super important in control theory.
- Approximation of nonlinear systems.
if x ˙ = f ( x , u ) \dot{\mathbf{x}} = \mathbf{f}(\mathbf{x}, \mathbf{u}) x˙=f(x,u), then A ( t ) = ∂ f ∂ x ( x , u ) \mathbf{A}(t) = \frac{\partial \mathbf{f}}{\partial \mathbf{x}}(\mathbf{x}, \mathbf{u}) A(t)=∂x∂f(x,u) and B ( t ) = ∂ f ∂ u ( x , u ) \mathbf{B}(t) = \frac{\partial \mathbf{f}}{\partial \mathbf{u}}(\mathbf{x}, \mathbf{u}) B(t)=∂u∂f(x,u).
5. Equilibrium
(1) Definition
It is a state
x
∗
\mathbf{x}^*
x∗ such that
f
(
x
∗
,
u
∗
)
=
0
\mathbf{f}(\mathbf{x}^*, \mathbf{u}^*) = \mathbf{0}
f(x∗,u∗)=0.
Algebraically, it is a solution to
f
(
x
,
u
)
=
0
\mathbf{f}(\mathbf{x}, \mathbf{u}) = \mathbf{0}
f(x,u)=0.
(2) Example: Pendulum
Dynamics:
x
˙
=
[
θ
˙
−
g
l
sin
θ
]
+
[
0
1
m
l
2
]
τ
\dot{\mathbf{x}} = \begin{bmatrix}\dot{\theta} \\ -\frac{g}{l}\sin\theta\end{bmatrix} + \begin{bmatrix}0 \\ \frac{1}{ml^2}\end{bmatrix}\tau
x˙=[θ˙−lgsinθ]+[0ml21]τ
Let x ˙ = 0 \dot{\mathbf{x}} = \mathbf{0} x˙=0, then τ = 0 \tau = 0 τ=0, θ ˙ = 0 \dot{\theta} = 0 θ˙=0, and θ = 0 \theta = 0 θ=0 or π \pi π.
It means the configuration manifold is compact.
6. First Control Problem
- Can we move the equilibrium using control input u \mathbf{u} u? - Yes
- Consider letting the pendulum stay at
θ
=
π
/
2
\theta = \pi/2
θ=π/2.
We can write
Solve the equation, we can get
u
=
m
g
l
\mathbf{u} = mgl
u=mgl
In general, we can make the system stay at any equilibrium by choosing the right control input u \mathbf{u} u.(Not always true, but true for some systems)
7. Stability of Equilibrium
(1) Definition
When will we stay “near” an equilibrium point under perturbation?
Take a 1D system as an example (
x
∈
R
\mathbf{x} \in \mathbb{R}
x∈R)
There are three equilibrium points.
(1) The right one and the left one are unstable equilibrium points.
(2) The origin is a stable equilibrium point.
From the above example, we can know:
If
∂
f
/
∂
x
<
0
\partial \mathbf{f}/\partial \mathbf{x} < 0
∂f/∂x<0, then the equilibrium point is stable.
If
∂
f
/
∂
x
>
0
\partial \mathbf{f}/\partial \mathbf{x} > 0
∂f/∂x>0, then the equilibrium point is unstable.
For higher dimensional systems, the conclusion also holds.
∂
f
/
∂
x
\partial \mathbf{f}/\partial \mathbf{x}
∂f/∂x is called “Jacobian”.
We need to check the real part of the eigenvalues of the Jacobian.
(2) Example: Pendulum
f ( x , u ) = [ θ ˙ − g l sin θ ] \mathrm{f}(\mathbf{x}, \mathbf{u}) = \begin{bmatrix}\dot{\theta} \\ -\frac{g}{l}\sin\theta\end{bmatrix} f(x,u)=[θ˙−lgsinθ]
The Jacobian is
∂
f
∂
x
=
[
0
1
−
g
l
cos
θ
0
]
\frac{\partial \mathbf{f}}{\partial \mathbf{x}} = \begin{bmatrix}0 & 1 \\ -\frac{g}{l}\cos\theta & 0\end{bmatrix}
∂x∂f=[0−lgcosθ10]
(1) When
θ
=
π
\theta = \pi
θ=π, then the Jacobian is
∂
f
∂
x
=
[
0
1
g
l
0
]
\frac{\partial \mathbf{f}}{\partial \mathbf{x}} = \begin{bmatrix}0 & 1 \\ \frac{g}{l} & 0\end{bmatrix}
∂x∂f=[0lg10]
The eigenvalues are ± g l \pm \sqrt{\frac{g}{l}} ±lg. One is positive and the other is negative, so the equilibrium point is unstable.
(2) When
θ
=
0
\theta = 0
θ=0, then the Jacobian is
∂
f
∂
x
=
[
0
1
−
g
l
0
]
\frac{\partial \mathbf{f}}{\partial \mathbf{x}} = \begin{bmatrix}0 & 1 \\ -\frac{g}{l} & 0\end{bmatrix}
∂x∂f=[0−lg10]
The eigenvalues are ± i g l \pm i\sqrt{\frac{g}{l}} ±ilg. Both are imaginary, which means the motion is undamped oscillation. It is called marginally stable. (Remember that the analysis is based on linearization)
We can add damping using controller (e.g. u = − k θ ˙ \mathbf{u} = -k\dot{\theta} u=−kθ˙). Then the system will be stable.