From bd375373ad5ef9fd48b174b818cf53c574953c04 Mon Sep 17 00:00:00 2001 From: Pavel Lutskov Date: Wed, 8 Aug 2018 22:15:19 +0200 Subject: [PATCH] First round of corrections --- documentation/align.tex | 1 + documentation/approach.tex | 4 +- documentation/conclusion.tex | 8 +- documentation/jonas.tex | 274 +++++++++++++++++++++---------- documentation/kick.tex | 2 +- documentation/robotum_report.tex | 2 +- 6 files changed, 199 insertions(+), 92 deletions(-) diff --git a/documentation/align.tex b/documentation/align.tex index 7ff8bac..b1804ed 100644 --- a/documentation/align.tex +++ b/documentation/align.tex @@ -1,4 +1,5 @@ \section{Goal Alignment} +\label{p sec goal align} After the approach, described in the section \ref{p sec approach}, is finished, the robot is facing the ball, and the ball is at a short distance. In the stage diff --git a/documentation/approach.tex b/documentation/approach.tex index 21275d1..19549ac 100644 --- a/documentation/approach.tex +++ b/documentation/approach.tex @@ -3,8 +3,8 @@ \subsection*{Approach from the Side} -The first possibility is that in the approach planing stage, described in the -section \ref{j sec approach planing}, the decision was taken to approach the +The first possibility is that in the approach planning stage, described in the +section \ref{j sec approach planning}, the decision was taken to approach the ball from the side. In this case the robot will walk the calculated distance in the calculated direction. Normally, after the movement the robot should lose the sight of the ball. However, the approximate angle, where the ball should be diff --git a/documentation/conclusion.tex b/documentation/conclusion.tex index b988f34..d77275a 100644 --- a/documentation/conclusion.tex +++ b/documentation/conclusion.tex @@ -16,18 +16,18 @@ goal in the camera image. Another important achievement is the overall system robustness. In our tests the robot could successfully reach the ball, do the necessary alignments and -kick the ball. When the robot decided that he should kick the ball, in the +kick the ball. When the robot decided that it should kick the ball, in the majority of cases the kick was successful and the ball reached the target. We performed these tests from many starting positions and assuming a variety of different relative positions of the ball and the goal. Furthermore, we managed not only to make the whole approach robust, but also -worked on making the procedure fast, and the approach planing was a crucial +worked on making the procedure fast, and the approach planning was a crucial element of this. In the project's early stages, the robot couldn't approach the ball from the side, depending on the goal position, and instead always walked towards the ball directly and aligned to the goal afterwards. The tests have shown, that in such configuration the goal alignment was actually the longest -phase and could take over a minute. Then we introduced the approach planing, +phase and could take over a minute. Then we introduced the approach planning, and as a result the goal alignment stage could in many scenarios be completely eliminated, which was greatly beneficial for the execution times. Finally, thanks to the strong kick, the goal can be scored from a large range of @@ -41,7 +41,7 @@ for improvement. Some of the most interesting topics for future work will now presented. The first important topic is \textit{self-localization}. Currently our robot is -completely unaware of his position on the field, but if such information could +completely unaware of its position on the field, but if such information could be obtained, then it could be leveraged to make path planning more effective and precise. diff --git a/documentation/jonas.tex b/documentation/jonas.tex index 76ec221..7cb49fb 100644 --- a/documentation/jonas.tex +++ b/documentation/jonas.tex @@ -1,152 +1,258 @@ \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 he 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 the ball. \\ -The algorithm which we implemented to solve this problem can be found in figure \ref{j figure turn 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} + \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 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 first part is the \textbf{Ball Detection} part \label{j sec ball detection}, which main task 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. -Another 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 one of the camera frames. Different succeeding strategies are run to cover different situations. If the strategies are successful the algorithm continues with \textbf{Ball Detection} and \textbf{Head Adjustment}. If the strategies are not successful, the next strategy in the chain is run. \\ -At the ball detection algorithm can sometimes fail, it is first insured, that -the ball detection algorithm was used on at least 3 succeeding frames. If the ball was not found in 3 succeeding frames, the next possible solution is letting the robot stop his current movement and step back for a specified distance, in case if the ball was directly in front of the robot and could therefore not be detected by both cameras. If this strategy was not successful, the robot starts to move his head to some defined angles and tries to find the ball in both camera frames. If this strategy than also failes, the robot starts to rotate around his z-axis until he is able to detect the ball in one of his camera frames. +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 centre of the robots' camera frames. -If the angle between the head an 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}. +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 his current movement. Then the robot starts to rotate around his z-axis depending on the current head yaw angle. To ensure that the head and body of the robot are aligned as in the beginning of the whole algorithm, the head is rotated back into zero yaw. The algorithm continues than with the \textbf{Ball Detection}. +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 himself to the ball fast while some strategies are in place to let the robot find the ball for situations, where the ball is not in the direct 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 localisation algorithms could be implemented to let the robot move to predefined locations the field and then start the \textbf{Turn to ball algorithm} again. +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 + \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 is the camera frame shown, which belongs to the top camera of the robot. + +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} + \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 centred in his top camera view. \\ -The distance measurement is described in the following. At first, it is assured, that the robot is in the defined stand-up posture, to ensure that the distance calculations are accurately enough. The current camera frame is then used to estimate the angle $ \Phi_{\mathrm{meas}} $ between the position of the ball and the centre 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 centre 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: +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}}} \; . + \Phi_{\mathrm{ball}} = \Phi_{\mathrm{meas}}+\Phi_{\mathrm{cam}} \; . \end{equation} -Even so the proposed equation for distance measurement is rather simple it provided sufficient results during the test runs. +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 planing} -An important part of the approaching strategy is to find out, in which direction the robot should start to approach the ball, so that it is later in a good position for the following approach steps. -The task is therefore to choose an appropriate approach path. +\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} + \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 centre of the camera view. As the position of the ball is therefore known, it is important to find out, where the ball is to determine an appropriate approach path. The robot will therefore rotate his head, until he is able to recognize the goal in the view of his top camera (figure \ref{j figure choose-approach}). +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} + \includegraphics[width=\textwidth]{\fig choose-approach} + \caption{Choose approach} + \label{j figure choose-approach} \end{figure} -Using the centre position 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 makes therefore 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. If the angle between the goal and the ball 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 another part of the approach planning, it's enough to decide between those 3 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 fast 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. +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 the 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, 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. + +\newpage + +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} + \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 centred 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 defined 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 to 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}. + +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 defined 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 two 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} + \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}. +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} + \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 distance. +To calculate the appropriate walking distance, the following formulas estimate +the approaching angle and calculate the distance. \begin{equation} \Theta_\mathrm{appr}=\arctan\left(\frac{\mathrm{Desired\ distance}}{\mathrm{ball\ distance}} \right) \; \; \mathrm{or} \; \; \arcsin\left(\frac{\mathrm{Desired\ distance}}{\mathrm{ball\ distance}}\right) \end{equation} \begin{equation} - \mathrm{walking\ distance}=\frac{\mathrm{ball\ distance}}{\cos(\Theta_\mathrm{appr})} \; \; \mathrm{or} \; \; \frac{\cos(\Theta_\mathrm{appr})}{\mathrm{ball\ distance}} + \mathrm{walking\ distance}=\frac{\mathrm{ball\ distance}}{\cos(\Theta_\mathrm{appr})} \; \; \mathrm{or} \; \; \frac{\cos(\Theta_\mathrm{appr})}{\mathrm{ball\ distance}} \end{equation} -If the distance between the robot and the ball is really small, the robot starts a direct approach to the ball regardless of the position of the goal. This makes more sense for short distances, than the two approaches stated above. - +If the distance between the robot and the ball is really small, the robot +starts a direct approach to the ball regardless of the position of the goal. +This makes more sense for short distances, than the two approaches stated +above. In this case the neccessary actions for goal alignment will happen in a +dedicated goal alignment stage, described in the section \ref{p sec goal + align}. diff --git a/documentation/kick.tex b/documentation/kick.tex index a213013..5059945 100644 --- a/documentation/kick.tex +++ b/documentation/kick.tex @@ -9,7 +9,7 @@ goal. Therefore, due to time constraints we implemented the simplest possible kick, that would satisfy those requirements. The procedure is as follows. First the robot will use its ankle joints to shift -its weight to the base leg. After this, the robots will be able to lift the +its weight to the base leg. After this, the robot will be able to lift the kicking leg for the swing. Finally, the robot will perform the swing and return to the standing position. Both raising the leg and doing the swing require precise coordinated joint movements, so we had to conduct experiments to diff --git a/documentation/robotum_report.tex b/documentation/robotum_report.tex index 925deac..6ea248f 100644 --- a/documentation/robotum_report.tex +++ b/documentation/robotum_report.tex @@ -40,7 +40,7 @@ \input{solintro} % Our solution intro \input{perception} % Ball goal and field % \input{Yuankai} -\input{jonas} % Distance, approach planing +\input{jonas} % Distance, approach planning \input{approach} % Ball approach \input{align} % Goal alignment \input{kick}