principles of robot autonomy iasl.stanford.edu/aa274/pdfs/lecture/lecture_5.pdf · consistency of...
TRANSCRIPT
![Page 1: Principles of Robot Autonomy Iasl.stanford.edu/aa274/pdfs/lecture/lecture_5.pdf · Consistency of Time Discretization Forward Euler time discretization Is this approximation consistent](https://reader030.vdocuments.us/reader030/viewer/2022040906/5e7afb2197442a6d705521a9/html5/thumbnails/1.jpg)
Principles of Robot Autonomy IAdvanced methods for trajectory optimization
![Page 2: Principles of Robot Autonomy Iasl.stanford.edu/aa274/pdfs/lecture/lecture_5.pdf · Consistency of Time Discretization Forward Euler time discretization Is this approximation consistent](https://reader030.vdocuments.us/reader030/viewer/2022040906/5e7afb2197442a6d705521a9/html5/thumbnails/2.jpg)
Motion control• Given a nonholonomic system, how to control its motion from an
initial configuration to a final, desired configuration
• Aim• Revisit trajectory planning as optimal control problem• Learn key ideas underpinning indirect methods for optimal control • Establish link between direct and indirect methods
• Readings• D. K. Kirk. Optimal Control Theory: An introduction. 2004.
AA 203 | Lecture 510/3/19 2
![Page 3: Principles of Robot Autonomy Iasl.stanford.edu/aa274/pdfs/lecture/lecture_5.pdf · Consistency of Time Discretization Forward Euler time discretization Is this approximation consistent](https://reader030.vdocuments.us/reader030/viewer/2022040906/5e7afb2197442a6d705521a9/html5/thumbnails/3.jpg)
Optimal control problemThe problem:
where 𝒙 𝑡 ∈ 𝑅%, 𝒖 𝑡 ∈ 𝑅(, and 𝒙 𝑡, = 𝒙,
• In trajectory optimization, we typically consider the case
AA 203 | Lecture 510/3/19 3
![Page 4: Principles of Robot Autonomy Iasl.stanford.edu/aa274/pdfs/lecture/lecture_5.pdf · Consistency of Time Discretization Forward Euler time discretization Is this approximation consistent](https://reader030.vdocuments.us/reader030/viewer/2022040906/5e7afb2197442a6d705521a9/html5/thumbnails/4.jpg)
Open-loop control
• We want to find
• In general, two broad classes of methods:
1. Indirect methods: attempt to find a minimum point “indirectly,” by solving the necessary conditions of optimality ⇒ “First optimize, then discretize”
2. Direct methods: transcribe infinite problem into finite dimensional, nonlinear programming (NLP) problem, and solve NLP ⇒ “First discretize, then optimize”
10/3/19 AA 203 | Lecture 5 4
![Page 5: Principles of Robot Autonomy Iasl.stanford.edu/aa274/pdfs/lecture/lecture_5.pdf · Consistency of Time Discretization Forward Euler time discretization Is this approximation consistent](https://reader030.vdocuments.us/reader030/viewer/2022040906/5e7afb2197442a6d705521a9/html5/thumbnails/5.jpg)
Preliminaries: constrained optimization
• Form Lagrangian function 𝐿: 𝑅%1( → 𝑅
• If 𝒙∗a is a local minimum which is regular, the NOC conditions are
• First order condition represents a system of n + m equations with n + m unknowns
10/3/19 AA 203 | Lecture 5 5
![Page 6: Principles of Robot Autonomy Iasl.stanford.edu/aa274/pdfs/lecture/lecture_5.pdf · Consistency of Time Discretization Forward Euler time discretization Is this approximation consistent](https://reader030.vdocuments.us/reader030/viewer/2022040906/5e7afb2197442a6d705521a9/html5/thumbnails/6.jpg)
Indirect methods: NOCAssume no state/control constraints• Form Hamiltonian• Hamiltonian equations
• Boundary conditions: , and
10/3/19 AA 203 | Lecture 5 6
![Page 7: Principles of Robot Autonomy Iasl.stanford.edu/aa274/pdfs/lecture/lecture_5.pdf · Consistency of Time Discretization Forward Euler time discretization Is this approximation consistent](https://reader030.vdocuments.us/reader030/viewer/2022040906/5e7afb2197442a6d705521a9/html5/thumbnails/7.jpg)
Indirect methods: NOCAssume control inequality constraints: e.g., |𝑢6| ≤ 8𝑢6 for all i• Form Hamiltonian• Hamiltonian equations
• Boundary conditions: , and
10/4/19 AA 203 | Lecture 5 7
Pontryagin’s minimum principle
![Page 8: Principles of Robot Autonomy Iasl.stanford.edu/aa274/pdfs/lecture/lecture_5.pdf · Consistency of Time Discretization Forward Euler time discretization Is this approximation consistent](https://reader030.vdocuments.us/reader030/viewer/2022040906/5e7afb2197442a6d705521a9/html5/thumbnails/8.jpg)
Substitutions for boundary conditions
10/3/19 AA 203 | Lecture 5
Problem Substitution
BC
Problem Substitution
BC
Problem Substitution
BC
Problem Substitution
BC
8
![Page 9: Principles of Robot Autonomy Iasl.stanford.edu/aa274/pdfs/lecture/lecture_5.pdf · Consistency of Time Discretization Forward Euler time discretization Is this approximation consistent](https://reader030.vdocuments.us/reader030/viewer/2022040906/5e7afb2197442a6d705521a9/html5/thumbnails/9.jpg)
Indirect methods: practical aspects
Reference for NOC: D. K. Kirk. Optimal Control Theory: An introduction. Dover Publications, 2004.In practice: To obtain solution to the necessary conditions for optimality, one needs to solve two-point boundary value problems• For example, in Python:
https://pythonhosted.org/scikits.bvp_solver/• Allows to solve problem of the form
• Syntax: solve(bvp_problem, solution_guess)• In Matlab: bvp4c10/3/19 AA 203 | Lecture 5 9
![Page 10: Principles of Robot Autonomy Iasl.stanford.edu/aa274/pdfs/lecture/lecture_5.pdf · Consistency of Time Discretization Forward Euler time discretization Is this approximation consistent](https://reader030.vdocuments.us/reader030/viewer/2022040906/5e7afb2197442a6d705521a9/html5/thumbnails/10.jpg)
Example
10/3/19 AA 203 | Lecture 5 10
![Page 11: Principles of Robot Autonomy Iasl.stanford.edu/aa274/pdfs/lecture/lecture_5.pdf · Consistency of Time Discretization Forward Euler time discretization Is this approximation consistent](https://reader030.vdocuments.us/reader030/viewer/2022040906/5e7afb2197442a6d705521a9/html5/thumbnails/11.jpg)
Extensions• What about problems whose necessary conditions do not fit directly the
“standard” form (e.g., free end time problems)?• Handy tricks exist to convert problems into standard form: Ascher, U., &
Russell, R. D. (1981). Reformulation of boundary value problems into “standard” form. SIAM review, 23(2), 238-254.
Important case: free final time (Problem 4 in pset)1. Rescale time so that 𝜏 = 𝑡/𝑡;, then 𝜏 ∈ [0,1]
2. Change derivatives @@A≔ 𝑡;
@@C
3. Introduce dummy state r that corresponds to 𝑡; with dynamics �̇� = 04. Replace all instances of 𝑡; with r10/4/19 AA 203 | Lecture 5 11
![Page 12: Principles of Robot Autonomy Iasl.stanford.edu/aa274/pdfs/lecture/lecture_5.pdf · Consistency of Time Discretization Forward Euler time discretization Is this approximation consistent](https://reader030.vdocuments.us/reader030/viewer/2022040906/5e7afb2197442a6d705521a9/html5/thumbnails/12.jpg)
Example
• Dynamics:
• Cost:
• Analytical solution gives:
10/3/19 AA 203 | Lecture 5 12
![Page 13: Principles of Robot Autonomy Iasl.stanford.edu/aa274/pdfs/lecture/lecture_5.pdf · Consistency of Time Discretization Forward Euler time discretization Is this approximation consistent](https://reader030.vdocuments.us/reader030/viewer/2022040906/5e7afb2197442a6d705521a9/html5/thumbnails/13.jpg)
Example (solution)• Define state as 𝒛 = [𝒙, 𝒑, 𝑟]• BC are:
• BVP becomes
• BC become
10/3/19 AA 203 | Lecture 5 13
![Page 14: Principles of Robot Autonomy Iasl.stanford.edu/aa274/pdfs/lecture/lecture_5.pdf · Consistency of Time Discretization Forward Euler time discretization Is this approximation consistent](https://reader030.vdocuments.us/reader030/viewer/2022040906/5e7afb2197442a6d705521a9/html5/thumbnails/14.jpg)
Direct methods - nonlinear programming transcription
10/3/19 AA 203 | Lecture 5 14
min JCK
CL𝑔 𝐱 𝑡 , 𝐮 𝑡 , 𝑡 𝑑𝑡
�̇�(𝑡) = 𝐚 𝐱 𝑡 , 𝐮 𝑡 , 𝑡 , 𝑡 ∈ [𝑡,, 𝑡;]
𝐱 0 = 𝐱,, 𝐱 𝑡; ∈ 𝑀;
(OCP)
𝐮 𝑡 ∈ 𝑈 ⊆ ℝ(, 𝑡 ∈ [𝑡,, 𝑡;]
Forward Euler time discretization
1. Select a discretization 0 = 𝑡, < 𝑡Y < ⋯ < 𝑡[ = 𝑡; for the interval [𝑡,, 𝑡;] and, for every 𝑖 = 0, … , 𝑁 − 1, define𝐱6~𝐱 𝑡 , 𝐮6 ~ 𝐮 𝑡 , 𝑡 ∈ (𝑡6, 𝑡61Y] and 𝐱,~𝐱 0
2. By denoting ℎ6 = 𝑡61Y − 𝑡6, (OCP) is transcribed into the following nonlinear, constrained optimization problem
min(𝐱b,𝐮𝐢) d6e,
[fY
ℎ6𝑔(𝐱6, 𝐮6, 𝑡6)
𝐱61Y = 𝐱6 + ℎ6𝐚 𝐱6, 𝐮6, 𝑡6 , 𝑖 = 0,… , 𝑁 − 1(NLOP)
𝐮6 ∈ 𝑈 , 𝑖 = 0,… , 𝑁 − 1 , 𝐹 𝐱[ = 0
![Page 15: Principles of Robot Autonomy Iasl.stanford.edu/aa274/pdfs/lecture/lecture_5.pdf · Consistency of Time Discretization Forward Euler time discretization Is this approximation consistent](https://reader030.vdocuments.us/reader030/viewer/2022040906/5e7afb2197442a6d705521a9/html5/thumbnails/15.jpg)
Direct methods - nonlinear programming transcription
10/3/19 AA 203 | Lecture 5 15
Forward Euler time discretizationConsistency of Time Discretization
Is this approximation consistent with the original formulation?
Yes!Indeed, the KKT conditions for (NLOP) converge to the necessary optimality conditions for (OCP),
that are given by the Pontryagin’s Minimum Principle, when ℎ6 ⟶ 0
1. Select a discretization 0 = 𝑡, < 𝑡Y < ⋯ < 𝑡[ = 𝑡; for the interval [𝑡,, 𝑡;] and, for every 𝑖 = 0, … , 𝑁 − 1, define𝐱6~𝐱 𝑡 , 𝐮6 ~ 𝐮 𝑡 , 𝑡 ∈ (𝑡6, 𝑡61Y] and 𝐱,~𝐱 0
2. By denoting ℎ6 = 𝑡61Y − 𝑡6, (OCP) is transcribed into the following nonlinear, constrained optimization problem
min(𝐱b,𝐮𝐢) d6e,
[fY
ℎ6𝑔(𝐱6, 𝐮6, 𝑡6)
𝐱61Y = 𝐱6 + ℎ6𝐚 𝐱6, 𝐮6, 𝑡6 , 𝑖 = 0,… , 𝑁 − 1(NLOP)
𝐮6 ∈ 𝑈 , 𝑖 = 0,… , 𝑁 − 1 , 𝐹 𝐱[ = 0
![Page 16: Principles of Robot Autonomy Iasl.stanford.edu/aa274/pdfs/lecture/lecture_5.pdf · Consistency of Time Discretization Forward Euler time discretization Is this approximation consistent](https://reader030.vdocuments.us/reader030/viewer/2022040906/5e7afb2197442a6d705521a9/html5/thumbnails/16.jpg)
Consistency of time discretization
10/3/19 AA 203 | Lecture 5 16
Pontryagin’s Minimum Principle (PMP)
Recall that the necessary optimality conditions for (OCP) are given by the following expressions
Simplified Formulation
min J,
CL𝑔 𝐱 𝑡 , 𝐮 𝑡 𝑑𝑡
�̇�(𝑡) = 𝐚 𝐱 𝑡 , 𝐮 𝑡 , 𝑡 ∈ [0, 𝑡;]
𝐱 0 = 𝐱,
(OCP)
• Co-state equation:
�̇� 𝑡 = −𝜕𝐚𝜕𝐱 𝐱 𝑡 , 𝐮 𝑡 l𝐩 𝑡 −
𝜕𝑔𝜕𝐱 (𝐱 𝑡 , 𝐮 𝑡 )
• Control equation:𝜕𝐚𝜕𝐮 𝐱 𝑡 , 𝐮 𝑡 l𝐩 𝑡 +
𝜕𝑔𝜕𝐮 𝐱 𝑡 , 𝐮 𝑡 = 𝟎
![Page 17: Principles of Robot Autonomy Iasl.stanford.edu/aa274/pdfs/lecture/lecture_5.pdf · Consistency of Time Discretization Forward Euler time discretization Is this approximation consistent](https://reader030.vdocuments.us/reader030/viewer/2022040906/5e7afb2197442a6d705521a9/html5/thumbnails/17.jpg)
Consistency of time discretization
10/3/19 AA 203 | Lecture 5 17
Related non-linear program (NLOP)
After discretization in time:
Simplified Formulation
min(𝐱b,𝐮b) d6e,
[fY
ℎ6𝑔(𝐱6, 𝐮6)
𝐱6 + ℎ6𝐚 𝐱6, 𝐮6 − 𝐱61Y = 𝟎, 𝑖 = 0,… , 𝑁 − 1
min J,
CL𝑔 𝐱 𝑡 , 𝐮 𝑡 𝑑𝑡
�̇�(𝑡) = 𝐚 𝐱 𝑡 , 𝐮 𝑡 , 𝑡 ∈ [0, 𝑡;]
𝐱 0 = 𝐱,
(OCP)
(NLOP)
![Page 18: Principles of Robot Autonomy Iasl.stanford.edu/aa274/pdfs/lecture/lecture_5.pdf · Consistency of Time Discretization Forward Euler time discretization Is this approximation consistent](https://reader030.vdocuments.us/reader030/viewer/2022040906/5e7afb2197442a6d705521a9/html5/thumbnails/18.jpg)
Consistency of time discretization
10/3/19 AA 203 | Lecture 5 18
Related non-linear program (NLOP)
After discretization in time:
KKT Related to (NLOP)
min(𝐱b,𝐮b) d6e,
[fY
ℎ6𝑔(𝐱6, 𝐮6)
𝐱6 + ℎ6𝐚 𝐱6, 𝐮6 − 𝐱61Y = 𝟎, 𝑖 = 0,… , 𝑁 − 1
(NLOP)
Denote the Lagrangian related to (NLOP) as
Then, the KKT conditions related to (NLOP) read as:
ℒ = d6e,
[fY
ℎ6𝑔 𝐱6, 𝐮6 + d6e,
[fY
𝛌6l 𝐱6 + ℎ6𝐚 𝐱6, 𝐮6 − 𝐱61Y
• Derivative w.r.t. 𝐱6 :
ℎ6𝜕𝑔𝜕𝐱6
𝐱6, 𝐮6 + 𝛌6 − 𝛌6fY + ℎ6𝜕𝐚𝜕𝐱6
𝐱6, 𝐮6 l𝛌6 = 𝟎
• Derivative w.r.t. 𝐮6 :
ℎ6𝜕𝑔𝜕𝐮6
𝐱6, 𝐮6 + ℎ6𝜕𝐚𝜕𝐮6
𝐱6, 𝐮6 l𝛌6 = 𝟎
![Page 19: Principles of Robot Autonomy Iasl.stanford.edu/aa274/pdfs/lecture/lecture_5.pdf · Consistency of Time Discretization Forward Euler time discretization Is this approximation consistent](https://reader030.vdocuments.us/reader030/viewer/2022040906/5e7afb2197442a6d705521a9/html5/thumbnails/19.jpg)
Consistency of time discretization
10/3/19 AA 203 | Lecture 5 19
Consistency with the PMPKKT Related to (NLOP)
Denote the Lagrangian related to (NLOP) as
Then, the KKT conditions related to (NLOP) read as:
ℒ = d6e,
[fY
ℎ6𝑔 𝐱6, 𝐮6 + d6e,
[fY
𝛌6l 𝐱6 + ℎ6𝐚 𝐱6, 𝐮6 − 𝐱61Y
• Derivative w.r.t. 𝐱6 :
ℎ6𝜕𝑔𝜕𝐱6
𝐱6, 𝐮6 + 𝛌6 − 𝛌6fY + ℎ6𝜕𝐚𝜕𝐱6
𝐱6, 𝐮6 l𝛌6 = 𝟎
• Derivative w.r.t. 𝐮6 :
ℎ6𝜕𝑔𝜕𝐮6
𝐱6, 𝐮6 + ℎ6𝜕𝐚𝜕𝐮6
𝐱6, 𝐮6 l𝛌6 = 𝟎
We finally obtain:
𝛌6 − 𝛌6fYℎ6
= −𝜕𝐚𝜕𝐱6
𝐱6, 𝐮6 l𝛌6 −𝜕𝑔𝜕𝐱6
𝐱6, 𝐮6𝜕𝐚𝜕𝐮6
𝐱6, 𝐮6 l𝛌6 +𝜕𝑔𝜕𝐮6
𝐱6, 𝐮6 = 𝟎
Let 𝐩 𝑡 = 𝛌6 for 𝑡 ∈ [𝑡6, 𝑡61Y] , 𝑖 = 0, … , 𝑁 − 1 and 𝐩 0 = 𝛌,. Then, the equations above are the discretized version of the necessary conditions for (OCP):
�̇� 𝑡 = −𝜕𝐚𝜕𝐱 𝐱 𝑡 , 𝐮 𝑡 l𝐩 𝑡 −
𝜕𝑔𝜕𝐱 (𝐱 𝑡 , 𝐮 𝑡 )
𝜕𝐚𝜕𝐮 𝐱 𝑡 , 𝐮 𝑡 l𝐩 𝑡 +
𝜕𝑔𝜕𝐮 𝐱 𝑡 , 𝐮 𝑡 = 𝟎
![Page 20: Principles of Robot Autonomy Iasl.stanford.edu/aa274/pdfs/lecture/lecture_5.pdf · Consistency of Time Discretization Forward Euler time discretization Is this approximation consistent](https://reader030.vdocuments.us/reader030/viewer/2022040906/5e7afb2197442a6d705521a9/html5/thumbnails/20.jpg)
Direct methods – software packagesSome software packages:• DIDO: http://www.elissarglobal.com/academic/products/
• PROPT: http://tomopt.com/tomlab/products/propt/• GPOPS: http://www.gpops2.com/
• CasADi: https://github.com/casadi/casadi/wiki• ACADO: http://acado.github.io/
For an in-depth study of direct and indirect methods, see AA203 “Optimal and Learning-based Control” (Spring 2020)
10/3/19 AA 203 | Lecture 5 20
![Page 21: Principles of Robot Autonomy Iasl.stanford.edu/aa274/pdfs/lecture/lecture_5.pdf · Consistency of Time Discretization Forward Euler time discretization Is this approximation consistent](https://reader030.vdocuments.us/reader030/viewer/2022040906/5e7afb2197442a6d705521a9/html5/thumbnails/21.jpg)
Next time: graph search methods for motion planning
10/3/19 AA 203 | Lecture 5 21