.. _WallFollow:

Wall Following
----------------

The :ref:`hybrid`
algorithm alone can not solve :ref:`cul-de-sac`. We need a behavior that 
will cause the robot to move parallel to the obstacle and even move away
from the goal if needed to move around a non-convex obstacle.

As with :ref:`hybrid`, the robot heading (:math:`\alpha`) is calculated as
the weighted sum of :math:`\alpha_{ao}` to avoid obstacles and a value
called :math:`\alpha_{wall}` which will be designed to follow the turns of a
wall at a fixed distance :math:`D_{wall}`. Since the robot is expected to
follow somewhat near a wall, :math:`\alpha_{ao}` will be larger than it was
for the Hybrid algorithm, so we will use a larger threshold value to
calculate the weighting variable :math:`h`.

.. math:: 
   :label: blended-hwall

   h = \left\{ \begin{array}{ll}
       \frac{|\alpha_{ao}|}{Max_{\alpha\_{wall}}} & 
                \mbox{if} \; |\alpha_{ao}| < Max_{\alpha\_{wall}} \\
       1    &   \mbox{otherwise}
            \end{array}
       \right.

.. math:: \alpha = h \cdot \alpha_{ao} + (1 - h) \cdot  \alpha_{wall}

Thus, for :math:`|\alpha_{ao}| \geq Max_{\alpha\_{wall}}`, we have pure
collision avoidance:

.. math:: \alpha = \alpha_{ao}

Note that when the wall ahead of the robot turns in towards the robot, the
:math:`\alpha_{ao}` term will dominate the equation. When the wall is
straight or turns away from the robot, the value of :math:`\alpha_{wall}`
becomes critical to the heading angle calculation.

Perpendicular Vector Wall Follower
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^

This algorithm uses a bit of linear algebra of vectors to find a point to
drive to that keeps such that a wall is followed. It requires the robot to
have five distance sensors (one in front and two to each side of the robot
pointing at about 45 and 90 degrees to the robot).

Let the end points of the first two distance sensors on the side of the
robot towards the wall be points :math:`p1` and :math:`p2` in the world
coordinate frame. Given the pose of the robot, we can find a vector from the
robot to point :math:`p1`, :math:`\bm{A} = p1 - [x\: y]^T`. We can also define
a vector between points :math:`p1` and :math:`p2`, :math:`\bm{B} = p2 - p1`.
Vector :math:`\bm{B}` generally defines the direction that the robot should
travel; but more specifically, we wish to drive towards a point
:math:`D_{wall}` distance from the closest point along vector :math:`\bm{B}`
to the robot.  The closest point is :math:`x\bm{\hat{B}}` from point
:math:`p1`, where :math:`x` is a scalar value that we need to find and
:math:`\bm{\hat{B}}` is a normalized (unit vector) of :math:`\bm{B}`.  We
will use a vector, :math:`\bm{C}`, from the robot to :math:`x\bm{\hat{B}}`
to find the target driving point. 


.. figure:: wall_followPvector1.png
   :align: center
   :width: 50%

.. figure:: wall_followPvector2.png
   :align: center
   :width: 20%

.. math:: \bm{C} = \bm{A} + x\bm{\hat{B}}

.. math:: \bm{\hat{B}} = \frac{\bm{B}}{\|\bm{B}\|}

Since vectors :math:`\bm{B}` and :math:`\bm{C}` are to be perpendicular,
the dot product between the vectors should have a value of zero.

.. math:: \bm{\hat{B}^T}\left( \bm{A} + x\,\bm{\hat{B}} \right) = 0

.. math:: \bm{\hat{B}^T}\,\bm{\hat{B}}\,x = - \bm{\hat{B}^T\,A}

Since :math:`\bm{\hat{B}}` is a unit vector, 
:math:`\bm{\hat{B}^T}\bm{\hat{B}} = 1`. Vectors of dot products may
also be reversed if desired.

.. math:: x = - \bm{A^T\,\hat{B}}

.. math:: \bm{C} = \bm{A} - \bm{A^T\,\hat{B}\,\hat{B}} 

Now the vector, :math:`\bm{u_{wall}}` defining the robot driving direction is
given by

.. math:: \bm{u_{wall}} = \bm{p2} - \spalignvector{x; y} 
            - D_{wall}\,\frac{\bm{C}}{\|\bm{C}\|}

.. math:: \alpha_{wall} = atan2(u_{wall}(2), u_{wall}(1))

The robot's current position is subtracted because :math:`p2` is in the
global coordinate frame.
As mentioned before, this driving vector should be blended with the vector
for avoiding obstacles.

Virtual Triangle Wall Follower
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^

This algorithm is one that I developed in 2015. It is based on trigonometry
functions to give a smooth turning behavior. The algorithm forms a model of
a right triangle where sonar measurements determine the lengths of the sides
opposite and adjacent to the angle :math:`\alpha_{wall}`.

The calculation uses the :math:`x, y` coordinates from the first two sonar
measurements on the side of the wall.

.. figure:: wall_follow.png
   :align: center

   Right Triangle Model for Wall Following 


Use the `atan2` function to calculate :math:`\alpha_{wall}`:

.. math:: \alpha_{wall} = atan2((y_1 - D_{wall}), (x_1 + Wall\_lead - y_0))

The length of the second sonar measurement is an indicator of the near
future path provided by the wall. Thus the right triangle model begins with
:math:`(x_1, y_1)` on the adjacent and opposite sides of the triangle. The
vector to :math:`(x_1, y_1)` always spans the same angle, but the length of
the measurement relates to the distance from the wall and turns in the wall
ahead.

The value of :math:`D_{wall}`, which is a tunable constant, indicates how
far from the wall we want to be. Thus :math:`y_1 - D_{wall}` is the length
of the opposite side of the triangle.  If :math:`y_1 - D_{wall} = 0`, then
the robot is following the wall as desired. 

The adjacent side length is calculated from three variables. 

* Like :math:`y_1`, :math:`x_1` has valuable information about the contour
  of the wall. It will be shorter if the wall turns in towards the robot and
  longer if it turns away. When the robot nears the end of a wall that turns
  away from the robot, both :math:`y_1` and :math:`x_1` will become much
  larger.
  
* The second variable, :math:`Wall\_lead` is a tunable constant to
  effectively adjust the gain of the controller. The initial value was set
  at 2 meters. It should be increased if the robot is turning too
  aggressively and reduced if the turning is too sluggish.

  The minimum value of :math:`Wall\_lead` can be calculated by considering
  the maximum value that we want :math:`\alpha_{wall}`, :math:`\pi/4` seems
  reasonable. Taking the maximum values for the two sonar measurements and
  their angles, when :math:`D_{wall} = 0.4`, 
  :math:`Min\{ Wall\_lead \} = 1.772`, so our value of 2.0 seems reasonable.

* The value of :math:`y_0` tells us how far the robot currently is from the
  wall. When the robot reaches the end a point where the wall turns away
  from the robot, :math:`y_0` will suddenly become larger. Since this value
  is subtracted from length of the adjacent side of the model right
  triangle, this increased size of :math:`y_0` will cause the robot to turn. 
  Thus, when the wall turns away from the robot, :math:`y_0` can be
  considered a gating factor to keep the robot from turning until robot goes
  past the end of the wall.
  Correspondingly, when the robot is close to the wall, the small value of
  :math:`y_0` will cause the robot to turn away from the wall.
  
.. figure:: track_end_of_wall.png
   :align: center
   :width: 60%

.. figure:: wall_ahead.png
   :align: center
   :width: 25%