1. Dynamic Planning / Optimal Control
ECE 383 / ME 442

2. Context: Model-based control
Model-free control: choose u(x,t), avoid modeling dynamics (ex: PID control)
Good for simple problems
Low computational complexity
Requires parameter tuning
Difficult to choose suitable policy for complex systems
Has no performance guarantees
Model-based control: model dynamics and choose u to yield a “good” future trajectory. (ex: gravity compensation, optimal control)
Able to achieve better performance (e.g., optimality)
Dynamics may be hard to model
Must usually be combined with model-free control or model adaptation to compensate for un-modeled errors (implementation difficulty)

3. Context: Myopic vs Predictive
A model-based controller is myopic (or greedy) if the decisions 𝑢(𝑥,𝑡) only depend on predicting 𝑥 =𝑓(𝑥,𝑢) at the current state 𝑥.
Ex: gravity compensation tells you how to compensate for gravity now
Ex: potential fields
Ex: operational space control
A model-based controller is predictive if the decisions 𝑢(𝑥,𝑡) depend on both the current state and predictions of the future trajectory taken under the time-evolution of dynamics, under the controls
Ex: motion planning
Ex: optimal control
For most non-trivial problems, good myopic strategies are hard to devise

4. Agenda (2 day) Today Next time Principles Ch 7.2
Concept of model predictive control
Controllability
Kinematic reductions & maneuvers in motion planning
Sampling-based planning with dynamics
Next time
Principles Ch 8
Optimal control
Trajectory optimization
Bellman equation

5. Model predictive control (MPC)
Idea: repeatedly compute feedforward control using model of the dynamics, prediction
In other words, rapid replanning
Feedforward calculation
u=uff
Plant
xdes
x(t)

6. Kinematic Planning
So far, all constraints on motion have been represented as C-space obstacles
Dynamic constraints: omnidirectional motion in C-space not permitted

7. Kinematic reductions
Can we plan kinematically-feasible paths and then convert them to dynamically-feasible ones?
Sometimes
Kinematic reduction

8. Holonomic systems
If m>=n, and for all x, all dimensions of 𝑥 =𝑓(𝑥,𝑢) can be spanned by a suitably chosen control u, then the system is holonomic
I.e., 𝜕𝑓 𝜕𝑢 (𝑥,𝑢) is full rank at all x
E.g., letting 𝑥 = 𝑓 0 𝑥 + 𝑓 1 𝑥 𝑢 1 +…+ 𝑓 𝑚 𝑥 𝑢 𝑚 need [ 𝑓 1 𝑥 |…| 𝑓 𝑚 𝑥 ] to be full rank for all x
Otherwise the system is non-holonomic
Omnidirectional mobile base: holonomic
Differential drive: non-holonomic

9. Planning for Holonomic Systems = Kinematic planning
Given any kinematic path x(t), can choose 𝑢(𝑡) at 𝑥(𝑡) to derive path velocity 𝑥 (𝑡)
Given bounds on control 𝑢∈𝑈, can find the control in 𝑈 that maximizes forward progression along path
Most interesting dynamic systems are nonholonomic… but maybe we can do this approximately

10. Sea horses By: Saya h.

11. Path Deformation
Holonomic path
Nonholonomic path 11

12. Example: 1D Point Mass x v dx/dt = dv dv/dt = f / M Solution:
v(t) = v(0)+t f /M
x(t) = x(0)+t v(0) + ½ t2 f / M
|f|<=fmax x

13. Example: Car-Like Roboty x L q f
dx/dt = v cosq
dy/dt = v sinq
dq/dt = (v/L) tan f
|f| < F
dx sinq – dy cosq = 0
Configuration space is 3-dimensional: q = (x, y, q)
But control space is 2-dimensional: (v, f) with |v| = sqrt[(dx/dt)2+(dy/dt)2] 13

14. Example: Car-Like Roboty x L q f
dx/dt = v cosq
dy/dt = v sinq
dq/dt = (v/L) tan f
|f| < F
dx sinq – dy cosq = 0
q = (x,y,q)
q’= dq/dt = (dx/dt,dy/dt,dq/dt) dx sinq – dy cosq = is a particular form of f(q,q’)=0
A robot is nonholonomic if its motion is constrained by a non-integrable equation of the form f(q,q’) = 0 14

15. Example: Car-Like Roboty x L q f
dx/dt = v cosq
dy/dt = v sinq
dq/dt = (v/L) tan f
|f| < F
dx sinq – dy cosq = 0
Lower-bounded turning radius 15

16. How Can This Work? Tangent Space/Velocity Spacex L q f x y q
(x,y,q)
(dx,dy,dq)
(dx,dy)
dx/dt = v cosq
dy/dt = v sinq
dq/dt = (v/L) tan f
|f| < F q 16

17. How Can This Work? Tangent Space/Velocity Spacex L q f x y q
(x,y,q)
(dx,dy,dq)
(dx,dy)
dx/dt = v cosq
dy/dt = v sinq
dq/dt = (v/L) tan f
|f| < F q 17

18. Kinematic Reduction of 1D Point Mass
If:
Constraints are time-invariant
Starts and stops at rest
Then we only need to consider position x1
x1+e
1. Speed up
2. Slow down

19. Dynamic Execution of Kinematic Paths
For fully actuated N-D robots, a collision-free path can be executed “slowly enough” v x

20. Optimal Straight-line Following
Given a particle with velocity and acceleration bounds, what is the optimal motion between two points, starting and stopping at rest?

21. Optimal Straight-line Following
Given a particle with velocity and acceleration bounds, what is the optimal motion between two points, starting and stopping at rest?
Max acceleration, (optional) max velocity, max deceleration
Trapezoidal velocity profile x a- v+ a+ t v
vmax t

22. Optimization of Dynamic Paths for Robot Arms
Hauser and Ng Thow Hing (2010)

23. Small-Time Local Controllability
A system is small-time locally controllable if at a given configuration q, it can reach an open neighborhood of q with “small” motions e q
SLTC
Not SLTC
Not SLTC

24. Small-Time Local Controllability
A system is small-time locally controllable if at a given configuration q, it can reach an open neighborhood of q with “small” motions
Surprise: simple car model is STLC e q
SLTC
Not SLTC
Not SLTC

25. Lie Bracket Maneuver made of 4 motions -X -Y Y X (dt)
Assuming this is just a repetition of the first presentation, otherwise, need more details… 25

26. Lie Bracket Maneuver made of 4 motions For example: dx/dt = v cosq
dy/dt = v sinq
dq/dt = (v/L) tan f
|f| < F
X: Going straight
Y: Turning, angle f T
Assuming this is just a repetition of the first presentation, otherwise, need more details… 26

27. Lie Bracket Maneuver made of 4 motions For example: -X
X (dt) Y -X -Y
[X,Y] (dt2 )
X: Going straight
Y: Turning, angle f T
Assuming this is just a repetition of the first presentation, otherwise, need more details…
Lie bracket 27

28. Lie Bracket [X,Y] = dY.X – dX.Y X1/x X1/y X1/q
dX = X2/x X2/y X2/q
X3/x X3/y X3/q
X (dt) Y -X -Y
[X,Y] (dt2 )
Assuming this is just a repetition of the first presentation, otherwise, need more details…
[X,Y]  Lin(X,Y)  the motion constraint is nonholonomic
Lie bracket 28

29. Lie Bracket -X -Y Y X (dt) [X,Y] (dt2 ) [X,Y] = dY.X – dX.Y =
v sin θ
v cos θ
v sin θ
v cos θ
dX =
dY =
X (dt) Y -X -Y
[X,Y] (dt2 )
X: Going straight
Y: Turning, angle f T
Assuming this is just a repetition of the first presentation, otherwise, need more details…
v2/L sin θ tan φ
-v2/L cos θ tan φ
[X,Y] = dY.X – dX.Y = 29

30. Sufficient condition for STLC
Let S = {Xi}, i=1,…,k be control vector fields over n dimensional manifold
For each pair Xi,Xj in S:
Compute lie bracket U = [Xi,Xj]
If U  Lin(S), then add U to S and repeat step 2
If |S|=n, then STLC

31. Tractor-Trailer Example
4-D configuration space
2-D control/velocity space
 two independent velocity vectors X and Y
U = [X,Y]  Lin(X,Y)
V = [X,U]  Lin(X,Y,U) 31

32. Lie Bracket Maneuvers
Problem: To move distance d along vector field U=[X,Y] using Lie bracket, may need to move much farther in C-space
Solution: Use better maneuvers
Assuming this is just a repetition of the first presentation, otherwise, need more details… 32

33. Type 1 Maneuver CYL(x,y,dq,h) h = 2r tandq d = 2r(1/cosdq - 1) > 0
(x,y,q) dq h h
(x,y) q r dq dq r
When dq  0, so does d and the cylinder becomes arbitrarily small
 Allows sideways motion 33

34. Type 2 Maneuver
 Allows pure rotation 34

35. Combination 35

36. Coverage of a Path by Cylindersq + q q’ y x 36

37. Path Examples 37

38. Drawbacks of Two-phase Planning
Final path can be far from optimal
Not applicable to robots that are not locally controllable (e.g., car that can only move forward) 38

39. Reeds and Shepp Paths 39

40. Reeds and Shepp Paths
CC|C0
CC|C
C|CS0C|C
Given any two configurations, the shortest RS paths between them is also the shortest path 40

41. Example of Generated Path
Holonomic
Nonholonomic 41

42. Path Optimization 42

43. Perspective
If one can determine kinematic reductions or steering functions, usually the best method is to reduce to kinematic planning + postprocessing
If these cannot be found, must resort to the approach of control-based planning in state space

44. Control-Based Sampling
PRM sampling technique: Pick each milestone in some region
Control-based sampling:
Pick control vector (at random or not)
Integrate equation of motion over short duration (picked at random or not)
If the motion is collision-free, then the endpoint is the new milestone
Tree-structured roadmaps
Need for endgame regions 44

45. Example dx/dt = v cosq dy/dt = v sinq dq/dt = (v/L) tan f |f| < F
1. Select a milestone m
2. Pick v, f, and dt
3. Integrate motion from m
 new milestone m’ 45

46. Example
Indexing array: A 3-D grid is placed over the configuration space. Each milestone falls into one cell of the grid. A maximum number of milestones is allowed in each cell (e.g., 2 or 3).
Asymptotic completeness: If a path exists, the planner is guaranteed to find one if the resolution of the grid is fine enough. 46

47. Computed Paths Tractor-trailer Car That Can Only Turn Left
jmax=45o, jmin=22.5o
jmax=45o 47

48. Control-Sampling RRT Configuration generator f(x,u)
Build a tree T of configurations
Extend:
Sample a configuration xrand from X at random
Find the node xnear in T that is closest to xrand
Pick a control u that brings f(xnear,u) close to xrand
Add f(xnear,u) as a child of xnear in T

50. Weaknesses of RRT’s strategy
Depends on the domain from which xrand is sampled (sampling strategy)
Depends on the notion of “closest” (metric sensitivity)
A tree that is grown “badly” by accident can greatly slow convergence (history dependence)

51. Next time
Considering optimality
Principles Ch 8