Stanford OnlineStanford AA274A Principles of Robotic Autonomy | Autumn 2019 | Advanced Trajectory Optimization
Stanford Online
Problem Setup 0:04
Today’s topic is advanced trajectory optimization, framed as optimal control. A path is not just supposed to work; it may also need to minimize time, control effort, or both. The standard setup has system dynamics, input limits such as speed or acceleration bounds, a cost with running and final terms, and usually no state constraints here because obstacles are postponed to the next week.
Indirect Methods 6:01
Optimal control is treated as an infinite-dimensional problem because the control is a function of time. Indirect methods start by writing necessary optimality conditions and then solving them. The lecture compares this with equality-constrained optimization in finite dimensions, where Lagrange multipliers turn constraints into an augmented cost and the stationary point conditions involve both the state variables and the multipliers.
Lagrange conditions 26:00
Set the partial derivatives of the Lagrangian to zero. For the example here, that gives two equations in x1 and x2, plus the equality constraint from the multiplier. Solving them yields two candidate points, (-1, -1) and (1, 1).
Hamiltonian setup 33:01
For optimal control, build the Hamiltonian in the same way. It adds the running cost to a multiplier term, and the multipliers are called costates because they are functions of time. Then solve the necessary conditions, discard the costates, and keep the state and control values as candidates.
Boundary value problems 43:00
Those conditions form a two-point boundary value problem. You usually get some boundary data at the start time and some at the end time, so you need the right terminal conditions to determine all the integration constants. A free final time adds one more unknown and one more condition, which gives the equation that selects the best final time.
Using BVP solvers 50:00
The practical tool here is a BVP solver, such as MATLAB BVP4C. You define the differential equation and then define the residuals for the boundary conditions, which measure how far your current guess is from satisfying them. The simple example with z1 and z2 shows how the solver is set up before you apply the same idea to optimal control.
Boundary residuals 54:00
The solver treats the boundary conditions as residuals. It checks whether the left and right endpoint values are zero, and if they are not, it penalizes them until the two-point boundary value problem is satisfied. The setup asks for the number of differential equations, the number of left boundary conditions, the ordinary differential equation callback, the residual form of the boundary conditions, and an initial guess. A constant guess is usually enough to get started.
Free final time 57:01
A standard boundary value solver usually assumes the final time is known. In optimal control, the final time is often free, so you rescale time with tau equal to t divided by TF. That moves the interval to 0 to 1. Then every derivative with respect to t is rewritten in tau, and TF is added as a state with trivial dynamics, R dot equals 0. The solver then chooses the right TF through the resulting boundary condition.
Double integrator example 1:01:32
The example is a particle on a line with x double dot equal to u, x at 0 equal to 10, x dot at 0 equal to 0, and both x and x dot equal to 0 at final time. The goal is to minimize time plus control effort, with u squared in the cost and TF free. The state is rewritten as x1 and x2, with x1 dot equal to x2 and x2 dot equal to u. The Hamiltonian gives u equal to minus p2 over b, p1 dot equal to 0, and p2 dot equal to minus p1.
Boundary conditions and code 1:07:03
After rescaling time, the system is written on tau in the interval from 0 to 1, with R standing for TF and added as a constant state. The boundary conditions fix x at the start, x and x dot at the end, and the transversality condition comes from the Hamiltonian. The code then packages x1, x2, p1, p2, and R into a single vector, computes u from p2, and multiplies the dynamics by the time-scaling factor. The numerical results match the analytic solution closely, and larger alpha puts more weight on finishing sooner.
Direct and indirect 1:14:31
The last topic is the link between direct and indirect methods. Direct methods turn the problem into a finite nonlinear program by discretizing the dynamics and the cost. If you write the necessary conditions for that discretized problem and make the time step smaller and smaller, they converge to the necessary conditions of the original optimal control problem. The two approaches are therefore closely related, even if they look different. The next topic will be path planning with constraints and obstacle avoidance.
AI-generated summary. It can be wrong or incomplete - check anything that matters against the original.
