In this last video of Chapter 10, we consider
a very different approach to motion planning,
based on nonlinear optimization.
The goal is to design a control history u
of t, a trajectory q of t, and a trajectory
duration capital T minimizing some cost functional
J, such as the total energy consumed or the
duration of the motion, such that the dynamic
equations are satisfied at all times, the
controls are feasible, the motion is collision
free, and the trajectory takes the start state
to the goal state.
We refer to J as the cost, and the last 5
lines are the constraints.
Nonlinear optimization requires an initial
guess at the solution.
Let's say that this control history is our
initial guess.
To turn the motion planning problem into a
nonlinear optimization, we need a finite parameter
representation of the control.
There are many ways to do this; we could use
a set of knot points interpolated by polynomials
or splines, or piecewise constant controls,
or piecewise linear controls.
For this example, let's assume piecewise linear
controls.
Integrating the equations of motion, we get
the robot's trajectory.
This trajectory intersects obstacles and does
not end at the goal configuration, so our
initial guess is not a solution to the problem.
To evaluate the constraints on the motion
due to obstacles, we can choose a finite set
of test points along the trajectory and evaluate
whether those points are collision free.
The collision constraints can be expressed
as constraints on the distances between the
test points and the obstacles, where a positive
distance means that there is no collision
and a negative distance implies a collision.
To update our guess at the control history,
we need to calculate the gradients of the
constraints with respect to the trajectory.
These gradients provide information on how
the test points should move to satisfy the
constraints.
These gradients map through the gradient of
the trajectory with respect to the controls
to suggest a direction in which to modify
our guess at the control history.
We also use information on the gradient of
the cost with respect to the controls in calculating
the direction to update our control history.
We then update the control history and integrate
the new control to get an updated trajectory.
Since our new trajectory still does not satisfy
the constraints, we repeat the process of
calculating a deformation direction for the
trajectory then map this through the sensitivity
of the trajectory with respect to the controls
to get a direction to deform the control history.
After taking a step in this direction in the
control space, we integrate the equations
of motion again to get the new trajectory.
This process repeats until we find a trajectory
that satisfies the constraints and locally
minimizes the cost.
Returning to the problem formulation, and
focusing on the first three lines, the method
I just described is called shooting.
With shooting, the design variables are the
total time duration capital T and the parameters
describing the control history.
The trajectory is found by simulation of the
equations of motion, ensuring that the dynamic
constraints are satisfied.
This method is called shooting, because designing
the controls is like aiming a cannon.
You see what happens when you fire and update
your aim so that the goal is more closely
achieved on the next try.
Another popular approach is called collocation,
in which you simultaneously design the control
history and the trajectory.
Since you design both, you have to enforce
that the controls and trajectory are consistent
according to the dynamics.
This is commonly done by ensuring that the
equations of motion are satisfied at a finite
set of test points.
The process of turning the problem statement
into a standard finite parameter nonlinear
optimization, which can be solved by techniques
such as sequential quadratic programming,
is called transcription.
There are many ways you could choose to represent
the controls, trajectory, cost, and constraints,
and your choice will affect the performance
of the optimization.
One thing that is critical, though, is that
you are able to calculate the gradients of
the cost and the constraints with respect
to your design variables, as these gradients
guide the search through the design variable
space.
Ideally you would be able to calculate these
gradients analytically, but failing that,
you should be able to numerically evaluate
them both efficiently and accurately.
Even with good gradient calculation, gradient-based
nonlinear optimization is inherently a local
method, and the optimization is prone to getting
stuck in local minima, where the solution
either does not globally minimize the cost
or does not satisfy the constraints.
Nonlinear optimization is a good choice when
other methods can be used to provide a reasonable
initial guess.
For example, nonlinear optimization could
be used to smooth a jerky motion found by
an RRT.
The RRT would handle the global search among
clutter, while the nonlinear optimization
would deform the RRT's solution to locally
minimize the cost.
So this concludes Chapter 10.
Motion planning is one of the most active
subfields of robotics, but you should now
have an understanding of the key concepts
of some of the most popular approaches.
In Chapter 11, on robot control, we study
the problem of designing feedback controllers
to drive the robots along the trajectories produced
by motion planners.
