\section{Turning to Ball}
\label{j sec turning to ball}

The task which we try to accomplish here is to bring the robot in a position,
so that it is looking straight at the ball. The robot should be able to find
the ball anywhere on the field and rotate itself so that it will focus on the
ball. The algorithm which we implemented to solve this problem can be found
in figure \ref{j figure turn to ball}.

\begin{figure}[ht]
  \includegraphics[width=\textwidth]{\fig turn-to-ball}
  \caption{Turn to Ball algorithm}
  \label{j figure turn to ball}
\end{figure}

The Turn to Ball algorithm consists of four main parts.

The first part is the \textbf{Ball Detection} part, the main task of which is
to find out, if the ball is visible in one of the cameras. At first frames from
both cameras of the robot are read in. It is then checked, if a ball is visible
in the picture using the ball detection algorithm. Depending on if the ball is
visible or not, different strategies are considered. If no ball was found in
the pictures, the algorithm proceeds with \textbf{Can't See the Ball}; if the
ball was found in the pictures, the ball yaw angle in the camera frame is
calculated and forwarded to \textbf{Head Adjustment}.

The next part of the Turn to Ball algorithm is the \textbf{Can't See the Ball}
part. This part of the algorithm is executed, if the ball was not visible in
any of the camera frames. Different succeeding strategies are run to cover
different situations. If a strategy is successful, the algorithm continues with
the \textbf{Head Adjustment}. If a strategy is not successful, the next
strategy in the chain is run. The possible strategies will now be presented.

As the ball detection algorithm can sometimes fail, it is first ensured, that
the ball detection algorithm was used on at least three consecutive frames. If
the ball was not found in three succeeding frames, the next possible solution
is letting the robot stop its current movement and step back for a specified
distance, for the case if the ball was directly in front of the robot and could
not therefore be detected by both cameras. If this strategy was not successful,
the robot starts to move its head to some defined angles and tries to find the
ball in both camera frames. If this strategy than also fails, the robot starts
to rotate around its z-axis until it is able to detect the ball in one of its
camera frames.

In the \textbf{Head Adjustment} part all necessary head movements are covered.
In this part of the algorithm the head is rotated by a calculated angle, which
depends on the ball yaw angle, which was provided by the \textbf{Ball
  Detection} part. Therefore, the ball should now be aligned in the center of
the robots' camera frames. If the angle between the head and the rest of the
body is now below a specified threshold, the ball is locked and the algorithm
stops, otherwise the algorithm continues with \textbf{Body Adjustment}.

The \textbf{Body Adjustment} part covers all necessary parts regarding the
rotation of the body. In this part of the algorithm, the robot first stops its
current movement. Then the robot starts to rotate around its z-axis depending
on the current head yaw angle. To ensure that the head and body of the robot
are aligned, like in the beginning of the whole algorithm, the head is rotated
back into zero yaw. The algorithm continues then with another \textbf{Ball
  Detection}, to ensure that the robot is properly centered at the ball.

The proposed algorithm provided decent results during many test runs. It allows
the robot to align itself to the ball fast, while some strategies are in place
to let the robot find the ball in situations, when the ball is not in the
field of view of the robot. However, there are still some things to improve. If
the ball is far away from the robot and can therefore not be detected by the
\textbf{Ball Detection algorithm}, the robot will continue to rotate and try to
find the ball. To solve this, some self-localization algorithms could be
implemented to let the robot move to predefined locations on the field and then
start the \textbf{Turn to Ball algorithm} again.

%TODO
%Follow the ball always -> problem: movement while walking
%Describe in more Detail??? Are all steps in can not see the ball executed every time?
%Mention stand up
\newpage
\section{Distance Measurement}
\label{j sec distance measurement}

The task which we try to accomplish here is to measure the distance to the ball
using the images from the cameras.

The proposed solution to measure the distance to the ball is shown in figure
\ref{j figure distance measurement}. In the right upper corner of the picture
the camera frame is shown, which belongs to the top camera of the robot.

\begin{figure}[ht]
  \includegraphics[width=\textwidth]{\fig distance-meassurement}
  \caption{Distance measurement}
  \label{j figure distance measurement}
\end{figure}

As this algorithm is executed after \textbf{Turning to Ball} (Section \ref{j
  sec turning to ball}) it is assumed, that the robot is already turned to the
ball and has the ball centered in its top camera view.

The distance measurement will now be described. At first,
the robot is brought to a defined stand-up posture, to ensure that the
distance calculations are accurate. The current camera frame is then
used to estimate the angle $\Phi_{\mathrm{meas}}$ between the position of the
ball and the center of the camera frame. In the stand-up position, the top
camera of the robot is not aligned with the parallel to the floor. There is
therefore an offset angle for the center of the camera frame, which has to be
considered in the calculations. As seen in figure \ref{j figure distance
  measurement} $ \Phi_{\mathrm{ball}} $ and $
\Phi_{\mathrm{meas}}+\Phi_{\mathrm{cam}} $ are alternate interior angles.
Therefore, the following equations holds:

\begin{equation}
  \Phi_{\mathrm{ball}} = \Phi_{\mathrm{meas}}+\Phi_{\mathrm{cam}} \; .
\end{equation}

Using the estimated angle $ \Phi_{\mathrm{ball}} $ and the height of the robot
the distance to the ball can now be calculated using simple trigonometry:

\begin{equation}
  d = \frac{\mathrm{height}}{\Phi_{\mathrm{ball}}} = \frac{\mathrm{height}}{\Phi_{\mathrm{meas}}+\Phi_{\mathrm{cam}}} \; .
\end{equation}

Even so the proposed equation for distance measurement is rather simple, it
provided sufficient precision during the test runs.

%TODO
%Mention Stand up to ensure, that robot is always in the same position
%Explain how angles are derived from the camera frames?
%Value of phi cam?
% \newpage

\section{Approach Planning}
\label{j sec approach planning}

An important part of the approach strategy is to determine, in which direction
the approach should start, so that the robot is later in a good position for
the consecutive approach steps. The task is therefore to choose an appropriate
approach path.

%TODO
%It is assumed, that ball and goal are aligned in such a way, that the robot ....

\begin{figure}[ht]
  \includegraphics[width=\textwidth]{\fig choose-approach-start}
  \caption{Starting condition of approach planning}
  \label{j figure starting condition choose-approach}
\end{figure}

The task is solved as following. At the beginning the robot is in the standing
position and the ball is in the center of the camera view. As the position of
the ball is therefore known, it is important to find out, where the goal is to
determine an appropriate approach path. The robot will therefore rotate its
head, until it is able to recognize the goal in the view of its top camera
(figure \ref{j figure choose-approach}).

\begin{figure}[ht]
  \includegraphics[width=\textwidth]{\fig choose-approach}
  \caption{Choose approach}
  \label{j figure choose-approach}
\end{figure}

Using the position of the center of the goal, the angle between the ball and
the goal is estimated. Depending on the value of the angle, different approach
directions are chosen. In figure \ref{j figure choose-approach}, the goal
is on the right side of the ball. It therefore makes sense to approach the ball
somewhere from the left side. In the current implementation there are three
possible approach directions. The robot could approach the ball either from the
left or the right side; or if the angle between the goal and the ball is
sufficiently small or the distance between the ball and the robot is sufficiently small, the robot could also do a straight approach to the ball. As
the exact approach angle to the ball is calculated in the next part of the
approach planning, it's enough for now to decide between those three possible
approach directions.

The proposed algorithm worked fine under the consideration of the
possible scenarios. As the goal detection algorithm works quite reliable, the
appropriate approach direction was found quickly most of the time.


As the approach direction is now known, the approach angle and the walking
distance of the robot have to be estimated. The task is to find an approach
angle and walking distance in such a way, that the robot is in a good position
for a later kick.

\begin{figure}[ht]
  \includegraphics[width=\textwidth]{\fig walking-distance}
  \caption{Approach angle and walking distance}
  \label{j figure approach angle and walking distance}
\end{figure}

%TODO
%bdist is hypo and walking distance is hypo

The task is solved as following. Again the robot is in the standing position
and the ball is centered in the camera view of the top camera. The ball
distance has already been estimated as described in section \ref{j sec distance
  measurement}. To estimate the approach angle and the walking distance, a
desired distance is set which defines the distance between the robot and
the ball after the walk. Approach angle and walking distance can then be
computed. Thereby we considered three different approaches depending on the
distance between the ball and the robot. If the distance between the robot and
the ball is below or equal to a specified threshold the triangle looks as shown
in figure \ref{j figure rdist hypo}.

\begin{figure}[ht]
  \centering
  \begin{tikzpicture}[scale=0.75]
    %\draw [help lines, dashed] (0,0) grid(8,4);
    \draw (8,4) coordinate (a) node[anchor=west]{Robot}
    -- (0,4) coordinate (b) node[anchor=east]{Ball}
    -- (0,0) coordinate (c) node[anchor=north]{Position after walk}
    -- cycle pic[draw=black, <->, angle eccentricity=1.2, angle radius=2cm]
    {angle=b--a--c};
    \node at (6,3.5) {$\Theta_\mathrm{appr}$};
    \node at (4,4.5) {ball distance};
    \node[align=left] at (-1.5,2) {desired\\ distance};
  \end{tikzpicture}	
  \caption{Approach for short ball distance}
  \label{j figure rdist hypo}
\end{figure}

% \newpage

During our tests this approach seemed more suitable for short ball distances.
For long ball distances however we choose a different approach. If the distance
between ball and the robot is larger than a specified threshold, the triangle
looks like in figure \ref{j figure bdist hypo}.

\begin{figure}[ht]
  \centering
  \begin{tikzpicture}[scale=0.75]
    %\draw [help lines, dashed] (0,0) grid(8,4);
    \draw (8,4) coordinate (a)  node[anchor=west]{Robot}
    -- (0,4) coordinate (b)  node[anchor=east]{Ball}
    -- (2,1)coordinate (c)  node[anchor=north]{Position after walk}
    -- cycle pic[draw=black, <->, angle eccentricity=1.2, angle radius=2cm]
    {angle=b--a--c};
    \node at (6,3.5) {$\Theta_\mathrm{appr}$};
    \node at (4,4.5) {ball distance};
    \node[align=left] at (-0.25,2) {desired\\ distance};
  \end{tikzpicture}
  \caption{Approach for long ball distance}
  \label{j figure bdist hypo}
\end{figure}

To calculate the appropriate walking distance, the following formulas estimate
the approaching angle and calculate the walking distance, depending on the distance to the ball. 

\begin{equation}
\Theta_\mathrm{appr} =
\begin{cases}
\arctan\left(\frac{\mathrm{Desired\ distance}}{\mathrm{ball\ distance}} \right) & \text{for short distances}\\
\arcsin\left(\frac{\mathrm{Desired\ distance}}{\mathrm{ball\ distance}}\right) & \text{for long distances}
\end{cases}
\end{equation}

\begin{equation}
\mathrm{walking\ distance} =
\begin{cases}
\frac{\mathrm{ball\ distance}}{\cos(\Theta_\mathrm{appr})} & \text{for short distances}\\
\cos(\Theta_\mathrm{appr}) \cdot \mathrm{ball\ distance} & \text{for long distances}
\end{cases}
\end{equation}

As already mentioned, the robot starts a direct approach to the ball regardless of the position of the goal if the distance between the robot and the ball is really small.
This makes more sense for sufficiently short distances, than the two approaches stated
above. In this case the necessary actions for goal alignment will happen in a
dedicated goal alignment stage, described in the section \ref{p sec goal
  align}.