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.