265 lines
12 KiB
TeX
265 lines
12 KiB
TeX
\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}.
|