The motion planners we have seen so far are based on offline search.
In this video we introduce a reactive real-time controller based on virtual potential fields
defined on the robot's C-space.
One potential field pulls the robot's configuration toward the goal configuration, while obstacle
potential fields repel the robot from C-space obstacles.
An attractive potential field on the C-space can be described by this quadratic in the
difference between the actual configuration and the goal configuration, where the K matrix
is positive definite.
From physics, or from the Lagrangian approach to dynamics from Chapter 8, we know that the
force due to a potential field is the negative of the gradient of the potential with respect
to the configuration q.
Taking the gradient of our artificial potential field, the force on the robot is proportional
to the vector from the current configuration to the goal configuration, as you would get
from a spring.
We can plot the potential on a two-dimensional C-space as a quadratic bowl, where the z-coordinate
is the potential.
The left figure shows the 3D plot of the quadratic bowl, while the right plot shows the equipotential
contour plot on the 2D C-space.
The goal configuration is at the bottom of the bowl and the initial configuration is
labeled q.
The basic control law says that we apply a force to the robot that is equal to the gradient
of the potential, and the robot's motion evolves according to its dynamics.
If we assume that the robot's mass matrix is the identity matrix, the robot moves as
a ball rolling in the bowl, as shown in this simulation, where the robot begins with a
nonzero initial velocity.
Since the dynamics are essentially those of a mass pulled by a spring, the robot's total
energy, potential plus kinetic, remains constant, and it never comes to rest at the goal configuration.
To fix this, we can add damping to the control law, where B is a positive-definite matrix.
This damping subtracts energy from the robot, allowing it to settle at the goal configuration.
Finally, we could use an even simpler control law, where we directly control the robot's
velocity to be equal to the force calculated from the potential.
Under this control law, the robot moves directly to the goal.
To allow for obstacles in the environment, we define a repulsive obstacle potential.
This repulsive potential requires a distance function between the C-space obstacle B and
the configuration q.
This distance is zero when the robot is in contact with the obstacle and positive when
the robot is not in contact with the obstacle.
The potential is proportional to one over the distance squared, so the potential is
large when the robot is near the obstacle.
The force due to the obstacle is the negative gradient, and it points in the direction in
which the distance between q and the C-space obstacle grows the fastest.
The total force acting on the robot is the sum of the force attracting the robot to the
goal and the forces repelling the robot from the obstacles.
This 2-dimensional C-space has three obstacles and the goal configuration is at the center.
The sum of the attractive and repulsive potentials, capped at a maximum potential value, is shown
in this figure.
The equipotential contour plot is shown in this figure, with the goal configuration indicated
by a plus sign.
The potential field has a unique global minimum at the yellow X.
Ideally this global minimum would be at the goal configuration, but the obstacles have
pushed the minimum away from the bottom of the attractive quadratic bowl.
The potential field also has saddle points, where the field is at a minimum in one direction
and a maximum in another direction.
There is also one local minimum, and this is a problem, as the local minimum attracts
all points in this basis of attraction.
If we simulate the robot's motion from two different initial configurations, q_1 and
q_2, we see that one motion gets stuck at the local minimum while the other finds its
way to the global minimum.
Artificial potential fields are appropriate for real-time control as they can be evaluated
relatively quickly, but local minima are a significant problem.
A particular class of potential functions, called navigation functions, are guaranteed
to have no local minima, but we only know how to compute navigation functions for a
limited class of systems.
We have not addressed the details of how to calculate the repulsive force due to an obstacle,
since it may not be easy to explicitly calculate a distance between a configuration and a C-space
obstacle.
One option is to attach a finite set of control points to the robot, and for each of these
control points, to calculate the closest point on each obstacle, and therefore the obstacle's
linear repulsive force.
Each repulsive force can be transformed to joint torques according to the Jacobian transpose
corresponding to the location of the point on the robot.