Embedded Motion Control 2012 Group 7
|Group Members -||email (at student.tue.nl)|
Rob Janssen - R dot J dot M dot Janssen at tue dot nl
|General||Weekly meetings with group on Tuesday, Wednesday and Thursday. |
At least a weekly update of the wiki.
Tasks are divided weekly.
|Week 3 (7 may)||Write code to create a 2D map from the laser data. |
Write code for position control. Read chapter 9 from the book.
|Navigation team||Image processing team|
|Week 4 (14 may)||Recognize the direction of walls|
Recognize opening in walls
|Install webcam in ROS gscam.|
Get familiar with openCV for image processing.
|Week 5 (21 may)||Write code for the navigation for the corridor competition. |
Make sure the Jazz recognizes exits.
Prepare the lecture.
|Make algorithm in MATLAB on example images.|
|Week 6 (28 may)||Give the lecture. |
Finish and test the navigation for the corridor competition.
Make sure Jazz takes the first exit.
|Implement MATLAB algorithm in ROS with openCV.|
|Week 7 (4 jun)||Corridor competition.|
Write code for image recognition.
Make sure arrows and their pointing direction are recognized.
Discuss and choose a strategy for solving the maze.
Start implementing the strategy into the navigation code.
|Test algorithm with the Jazz Robot.|
Make images with Jazz Robot and test in MATLAB.
Improve algorithm and test on Jazz Robot.
|Week 8 (11 jun)||Continue implementing the strategy into the navigation code.|
|Week 9 (18 jun)||Finish implementing the strategy into the navigation code.|
|Week 10/11 (25 jun / 2 jul)||Final competition.|
Our main goal is to be able to solve the maze Jazz we will be faced with during the final competition. As the philosophy in the world of robotics and also behind the ROS platform is to share knowledge and to use knowledge of others in order to speed up the design process. Of course, we take care of not just plugging in some code designed by others without really understanding what is going on and on what principles the process is based on. So our focus will be on designing a robust and fast navigation algorithm, rather than designing all kinds of new implementations of really nice functions that are already available for ROS.
The scheme above shows the first proposal of the components of which the final program will consist. A more detailed overview will be published in the coming days or week(s). The components in grey are considered to be not very important in order to successfully complete the corridor competition. However, these components are necessary for a successful completion of the final competition.
The final structure of the program is slightly different to the one shown above which was basically the outcome of our first brainstorm. Our current structure is shown below, where the oval shapes reperesent the node and the ones with a thick, coloured (but not blue) border are services. The navigation node actually does the navigation task and sends the velocity commands to Jazz in order to let it drive forward. Furthermore it recognizes exits and does the normal driving between two parallel walls. The node called "create dump map" constructs a map that counts the number of visits of each corridor in order to be able to perform the Tremaux's algorithm. The SLAM node is actually the gmapping node which is used to do simultaneous self-localization and mapping. Arrow detection is done by the service "Detect Arrows" and the node Hough Linedetection is used to detect walls and also an opening in a wall. Looking up the number of visits from the dumpmap is done by the service "Read dump map".
Maze solving Algorithm's
There are many ways to solve a maze. These ways of finding the exit are translated into algorithm's. An overview of these algorithm's is given here: .
Based on this website and on the results shown in the paper "Simulated Maze Solving Algorithms through Unknown Mazes", presented in this pdf-file: , we probably will tackle the problem of solving the maze by means of the so-called Tremaux's Algorithm. As a backup plan the Wall follower algorithm is chosen.
The Tremaux's Algorithm solves a maze by marking how many time a passage has been passed, when a junction is encountered that is not visited yet a random passage is chosen. If the junction is visited before the passage with the lowest counts of visits is chosen (if there are multiple options a random choice between these options is made). Exception to this is if the passage which we are arriving from is only visited once, then we will go back through the same passage, this is to avoid going around in circles.
Some pictures that clarify how the decision making is done by Tremaux's Algorithm are shown below.
An example of why there has to be turned around at a junction at certain conditions is shown below. If at this point there is not decided to turn back, there is a change that the direction back to the beginpoint is chosen.
An example of a maze solved by the Tremaux's Algorithm is shown below, the green markers indicate a decision not made random but based on the number of visits of all the possible passages.
Wall follower algorithm
The Wall follower algorithm is a very simple algorithm to solve a maze. In the beginning a wall is chosen and this wall is followed, basicly this means start moving through a passage and if a junction is encountered alway turn right (or always turn left) if possible. Disadvantage of this algorithm is that it does not always find a solution, but the great advantage is that it is very simple to implement because it does not require a memory of which passages have been visited.
An example of a maze solved by the Wall follower algorithm is shown below.
To help Jazz driving straight through corridors and detecting junctions a line detection algorithm is implemented. The hough transform algorithm is the algorithm we have chosen for this, how this algorithm works and is implemented is explained here.
For every point that is detected by the laserscanner lines are drawn through that point with different angles, the distance perpendicular to the line is then measured. If for a certain angle the distances for two points are the same, these points are on the same line. This is further explained with some pictures.
From this example can be concluded that the points or on a line with angle 45° at a distance of 7. Of course in reality more lines per point are calculated depending on the required accuracy. Also the distance does never exactly match because of measurement noise and the fact that real walls are never perfectly straight. Therefore the distance does not have to be exactly the same but has to be within a certain tolerance.
How this works for the real laserdata is explained with some pictures. First for every point the distance is plotted as a function of the angle. This angle goes from 0 degrees to 180 degrees in steps of the desired accuracy. This range can describe all possible lines like shown in the pictures below.
The implementation of this algorithm is now further explained with a captured frame of the laserdata shown below.
First all the angles and distances for each point from the laserdata are calculated, then a raster is created with all possible lines. Each element of this raster represents a line with a certain angle and a certain distance to Jazz (measured perpendicular on the line). The size of the elements of this raster depends on the tolerances for the distance and angle that have been set. The number of points inside each element is counted. When the number of points is more than a certain threshold this is considered to be a line.
The first plot shows a graphical interpretation of all the lines that are calculated for each point. The second plot shows the raster and its elements, the points within each of these elements are counted. The elements which hold at least a certain amount of points represent a line, these are the peaks shown in the last figure.
Extraction of start and endpoints of the lines
The start and endpoints of a line are examined by appointing all the points from the laserscan to the lines the belong to. After this is done, two neighboring points are compared to see if there mutual distance is smaller than some tolerance in order to make sure that they lay on the same line section. For all the points belonging to one section, the first and last point are considered to be the start and endpoint of that particular section. The obtained information is sent to the navigation node which decides about the actions to take. The messages, defined by a new structure, that are sent by the line recognition node contain the number of lines, and for each line, the number of sections, and per section the coordinates of the begin and endpoint together with the number of points on the section.
Driving through corridor
To drive safeley through a corridor without colliding into walls a corridor is virtually divided into 3 different zones.
This zone is in the middle of the corridor and the desired velocity here will be at maximum allowed velocity, the desired driving direction of Jazz will be straight ahead.
These zones are a bit closer to the walls, therefore the desired velocity here will be half of the maximum allowed velocity, the desired driving direction will be slightly to the middle of the corridor to get into the safe zone again.
These zones are so close to the wall that there is a severe risk of colliding with the walls. The desired velocity here will be set to zero if Jazz' driving direction is still directing to the wall. If the driving direction is directing from the wall the desired velocity will be half of the maximum allowed velocity. The desired driving direction will be pretty sharp to the middle of the corridor to leave the critical zone.
The zones are illustrated in the picture below.
In order to succesfully navigate through the maze, an appropriate junction handler has to be designed. In our opinion, the junction handling described below will do the job: There are basically 5 types of junctions Jazz will encounter during its drive through the maze as shown in the figure below. The red arrow indicates the direction from which Jazz approaches the junction. Note that a dead end is also considered to be and handeled is if it is a junction.
The recognition of the different situations is planned to do in the following manner:
- A junction is detected when a line at an angle of approximately 90 degrees is found, this line is referred to as an 'exitline'.
- A line parallel to the side walls is drawn at the right end of this exitline, then the distance between this line (red dashed line in the image below) and the right wall is calculated. If this distance is larger than a certain tresshold, a right exit exists.
- This is also done on the left side.
- To check for front exits a line is drawn parallel to the exitline at the end of the lines at the side walls. The distance between this line and the exitline is calculated and if this distance is larger than a certain tresshold, a front exit exists.
- Based on which exits are found the situation is determined.
Based on the junction type detected, the appropriate action should be taken, such as drive forward, turn left/right, check for an arrow and check each passage for the number of visits.
In order to make the junction recognition robust and also working in case of minor deviations from the ideal situation we have taken some measures. The most important measures are explained below. Note that these are additions to the original proposed method, as this method worked fine in ideal cases but not for a whole maze with a seris of junctions connected to each other.
- Paralell lines which are used by Jazz to recognize the walls between which it drives are limited to a distance of 1 meter, in order to make the right distinction between for example a corner and a dead end in case of coupled junctions.
- If only one side wall is detected, the previously measured hallwidth is used to approximate the position of the other side wall.
- Decisions are made only after one type of exit is recognized 2 times, to prevent Jazz from taking a wrong decision if a line is once not recognized properly.
- Finally, the line at an angle of 90 degrees is always the nearest line at 90 degrees, which may otherwise also cause trouble in case of connected junctions.
Implementation of Tremaux's Algorithm
In order to use Tremaux's Algorithm to solve the maze a map is made to remember the number of visits per corridor. The navigation node gives order to the 'create dump map' node whenever it leaves or enters a corridor to increase the value that indicates the number of visits. The 'create dump map' node then checks the current value and increases that value. The 'read dump map' service is called by the navigation node whenever a junction is encountered, this service then returns the number of visits of all corridors at the encountered junction. Pictures and a video are shown below to illustrate how this is implemented.
The detection of the arrows will be done by detecting the corners of the arrow. First, we did a simple test in Matlab with a photo of an arrow, using the Image Processing toolbox of Matlab. And now we are trying to write a corner detection algorithm for ROS. Probably using the "cornerSubpix" function of the OpenCV library. We will use a simple webcam to test our algorithm. To be able to use a webcam in ROS the following tutorial is followed: .
Update* We will only check for arrows in the neighborhood of a junction, to avoid unnecessary image processing. Only a the time that a t-junction is detected by Jazz the Arrow algorithm will be called. Then the Arrow algorithm should detect the possible arrow between 1.2m (when the T-junction is detected) and 0.5m (when Jazz is commanded to take a turn) before the wall. How the arrow will be detected is described below, in this section a brief summary of the Arrow algorithm is given.
Step 1.) Compensate for distortion fish-eye lens
The provided images taken with the camera of Jazz have not a really high quality partially due to the so-called fish-eye effect which causes a distortion. Distorted arrows may be hard to recognize and as the camera properties will most likely not change, we have used to solve this issue by correcting the distortion by a calibration as is described on this webpage: Camera Calibration example a other webpage is Information on distortion Correction. The calibration with the images below is not perfect, because of there were no image captured on the side of the camera.
Step 2.) Resize image 2x bigger
After lots of testing with our arrow detection algorithm we were still not satisfied with the result. Not because our arrow detection did not work, but because our detection was limited to a small range. Only in less then 1 meter from the arrow we were able to detect the direction of the arrow. The main reason for this is that the resolution of the camera is not so high. Therefore we decided to use linear interpolation to increase the resolution of our input image. We used the function Resize to increase the resolution in both directions with a factor 2.
We do not get more information with this function, but with a higher resolution we were able to detect more corners. And therefore we got a better determination of the direction of the arrow.
Step 3.) Convert image to HSV image
HSV varies the hue component of the hue-saturation-value color model. The colors begin with red, pass through yellow, green, cyan, blue, magenta, and return to red. The advance of using HSV is that you can use a color for different illuminations by using a hue and saturation value. We used is so we are sure that we are looking for a red object. With the saturation and value it is possible to be less sensitive to light.
Step 4.) Convert red pixels to black and white image
From the HSV image we create a black and white image (binary image). In the tables above you can see which values to use if you only want to find red arrows. We implemented a track-bar for each parameter to find easily the ideal parameters settings for a variety of conditions. This resulted in the following boundaries for each parameter:
(H \leq 16 \vee H \geq 343) \land S \geq 0.50 \land V \geq 0.50
Step 5.) Find biggest contour
Afther the conversion of the HSV image to a binary image there was still some noise in our image. Probably as a result of reflection of the light and the presence of other red objects near the red arrow. Therefore we search in the binary image for closed contours, i.e. for object pixels that are connected with each other, using the FindContours function of OpenCV. For each contours we calculate the area and store the biggest contour. We use a threshold value (a minimum area) the biggest object must have to proceed with the algorithm. Since from a far distance the shape of an arrow is not clearly visible. This is also prevented by only calling the Arrow algorithm at a short distance from a t-junction. Since only the biggest contour remains the residual noise is filtered out and only a possible arrow is left.
Step 6.) Region of interest
Region of Interest is a rectangular area in an image, to segment object for further processing. The ilustration is shown in Figure 1 below. For speeding up the algorithm we take 50 pixel up, down, left and right bigger as the boundaries of the contour. So that the watershed segmentation cannot segment something what is not red or not interested in.
Step 7.) Watershed segmentation
We have used the watershed segmentation so we get a all the information of the arrow possible out of the image at a bigger distance.
The are different approaches may be employed to use the watershed principle for image segmentation.
- Local minima of the gradient of the image may be chosen as markers, in this case an over-segmentation is produced and a second step involves region merging.
- Marker based watershed transformation make use of specific marker positions which have been either explicitly defined by the user or determined automatically with morphological operators or other ways.
We have used the Marker based watershed transformation.
Step 8.) Match the shape of the contour with a template arrow
If a contour is found does this not directly mean that it is also a arrow. Therefore the function MatchShapes is used to compare the shape of the contour with a predefined template arrow. This function calculates the rotation invariant moments (Hu moments) of each possible arrow that we get. Then the function compares the Hu moments with the Hu moments of a template arrow which we recorded earlier. And compares these two sets of Hu moments and gives a result in the form of a certain value. The lower the value (ideally is 0) the more similarities between the two Hu moments and thus the more simularities between the two shapes. A lot of testing is done to find an reliable maximum value.
The advantage of the Hu moments is that they are invariant under translation, changes in scale, and also rotation. So this MatchShapes function will also work properly with slightly rotated arrows or when the detected arrow is of a different size than the template arrow. Or even when the arrow points to the other direction then our template arrow.
Step 9.) Corner detection
To detect the direction of the arrow we use the function Cornersubpix to calculate the corners of the arrow. Based on this corners we determine the direction of the arrow, by looking at which side the most corners are found. Because the resolution of the arrow is still not perfect, the function Cornersubpix sometimes detects corners along the edge in the middle of the arrow. Which also can be seen in the picture below. Therefore we implemented a filter which filters out the corners which are detected within a certain area around the centre point of the arrow. With the result that only the actual corners of the arrow are left, resulting in a correct detection of the direction of the arrow.
Video of testing arrow algorithm
We have recorded two video's while running the arrow detection algorithm. The first one was the first test with a bag file. Not al the functions we discussed above were implemented yet during this test. The second one is our final test. With a driving Jazz robot controlled by the remote control.
Initialisation fase This fase only runs in the beginning and in some failsafe cases (see below). During this fase Jazz finds the line that is closest and turns untill its heading direction is parallel to this line. The initialisation fase then ends and the parallel driving fase starts.
Parallel driving fase In this fase Jazz tries to move parallel to the closest parallel line. This is achieved by controlling the radspeed dependent of the difference in angle between the driving direction and the parallel line (shown below). If Jazz is not in the safe zone an offset is given to this angle to move towards the safe zone. The forward velocity is also dependent of the zone where Jazz is in.
Turning fase In this fase Jazz turns left or right. The turning fase has two subfases, in the first fase Jazz does not start turning yet but moves towards the exitline (shown in the picture below) untill the distance to this exitline is half the hallwidth. Then the forward velocity is set to zero and Jazz begins turning, this is done by controlling the radspeed bependent of the difference in angle between the driving direction and the exitline. The angle with this exitline is monitored and if it is close to parallel the turning fase ends and the parallel driving fase starts.
Dead end fase In this fase Jazz turns 180 degrees drives back into the corridor where it came from. The forward velocity is set to zero and Jazz starts turning untill it is parallel in the corridor again. How this works is shown in the picture below.
Simulation versus real world
Temporarily solution for the laserdata points
Because we encountered some unexpected problems during the first test on the real robot we have made some modification to the simulator so it becomes a better representation of the real world. Our line detection uses the laserdata as an input and was optimized for the laserscanner of the simulator which returned 180 points. The real laserscanner returns 1080 points, which our program could not handle. To prevent this problem in the future an extra program is written that takes in the 180 points laserdata from the simulator and transforms this to 1080 points laserdata. This is done by lineair interpolation between the points of the simulated laserdata.
The visualisation of the new laserdata is shown in de picture below, the red dots represent the points from the new simulated laserdata, the white dots are those from the old simulated laserdata.
We also made a corridor in the simulator that does not have perfectly straight walls, because in reality this is also not the case. For testing the robustness of our line detection algorithm this is very useful. The difference between the old and new corridor world is shown below.
Modified corridor world with walls that are not perfectly straight.
To make sure our algorithm worked properly some messages had to be synchronized, this all went well in the simulator. In the real world test the messages would not synchronize in some cases, this was caused by the fact that the program ran on an external workstation. Both the workstation and Jazz have their own system time and some messages received their time stamp from Jazz' system time and others from the workstation's system time. These messages can not by synchronized properly. We decided to take out the synchronization and during the tests this went well, but this is no guarantee that it will never cause problems.
Presentation Final competition June 29th 2012
In this section I will publish all kinds of information on the things I am currently learning, I have learned so far and my further ambitions.
29th of June 2012 – I won the final contest!
It was an awesome day today. I was able to complete the final contest within the fastest time. Not everything went perfect because some parameters of my program were not optimized due to limited test time, nevertheless I have made me and my bosses very proud. Look at the video below to see me driving through the maze.
27th of June 2012 – Final tests
Today, I had my last oppertunity to test in a real maze, and as last Thursday my communication suffered again from delays up to a 4 seconds, even by calling just "pico ping". But after reporting this situation someone passed by who fixed this issue of netwrok overload, and afterwards, my tests went pretty well as I can drive through the maze completely autonomously while making a map and looking up how many times an exit is visited at a junction. The arrow recognition is not robust enough yet, but I have some ideas on how to improve robustness for the final competition! Will be continued...
26th of June 2012 – Achieved one of my ultimate goals!
It was a great day today, as I succeeded to solve several maze's in simulation using Tremaux's algorithm, with different types of junction handling in case of an equal number of visits of several directions at a junction. A video is shown below and I hope to achieve this also in the experiments. So, hope to have more good news later...
21th of June 2012 – Real world test 3
Today, I had my third test event, but that was not a big succes. The communication between me and the computer suffered from HUGE delays up to 10 seconds and more. Sometimes the communcation broke down completely. However, once the communcation was up and running properly the test were a succes, except for a missing transform needed to test the mapping algorithm implemented to perform the Tremaux's Algorithm. This issue is solved yet so I hope to show you some evidence in the coming few hours or days.
18th of June 2012 – First step towards Tremaux's Algorithm
In order to use Tremaux's Algorithm, I successfully can draw points on a map at the position I currently visit and store it in my memory. I will extend this tomorrow to coloring the full corridor and finally detecting the number of visits.
14th of June 2012 – Real world test 2
Today I had my second day of real life testing and I recognized all types of junction separately. However, some connected junctions gave some trouble but in the meanwhile I have figured out some measures to overcome the problem. There is also quite some progress made in the arrow recognition, as I can detect the arrows on almost all of the provided pictures, including their direction. I am still studying to improve the robustness and to see how I can make a map in my memory in order to apply Tremaux's algorithm. Will be continued...
12th of June 2012 – Travelled through the maze
Finally, I have learned how to drive through the maze without any intervention from outside. I travelled through the maze by means of the wall follower algorithm, following the right wall. This strategy is also known as the right-hand-rule. I will post a video soon to give you some evidence...
Here my evidence is:
As in all cases, I can improve myself and I definetely will do during the coming days. Moreover I have planned to see if I also can learn how to apply to Tremaux algorithm to prevent endless searching in bigger mazes with more junctions and exits.
5th of June 2012 – Recognition of the junction types
Today, I have learned to recognize the different types of junctions described in the section junction handling. I have only to pay some more attention to see at which side the corner (junction type A) is directed and at which side the exit is in case of junction type E. Will be continued soon...
In the meanwhile, I have also overcome the challenge of detecting the side of the exit for junction type A and E. Now, I only have to recognize arrows and look at the map to take appropriate actions according to Tremaux's Algorithm. See the video below of how I am detecting the junction types.
4th of June 2012 – I passed the corridor challenge!!
A small step for men, a huge step for me: Today I passed the corridor competition in 1 minute and 40 seconds. This was not the fastest time, but that was due to the fact that I had no time to practice due to an error. See the video below of me going through the corridor.
1st of June 2012 – Drove autonomously through the corridor and exit
I drove autonomously through the corridor and turned towards the exit. I also kept aligning myself during my drive in the corridor, such that I drove at the center of the corridor. See this video for a prove:
During the real world tests, I had a memory problem so this test failed for me but I have solved this yet. So hope for better luck on Monday. To be continued...
31th of May 2012 – Driving centered through a corridor
I made again a leap forward in solving the maze problem. I succeeded in driving at the center of the corridor and staying at the center while driving on. Hope to learn also to make a turn at a junction later today...
30th of May 2012 – Wall detection 4
Yesterday, I finally managed to find the begin and endpoints of the walls in a proper way. To see how I can detect lines now watch the video below:
Currently I am trying to figure out how I can ride through the maze without bumping into the walls while making nice corners at a junction. In the meanwhile I am also still studying how to process my camera images to detect the pointing direction of the arrows. To be continued soon...
24th of May 2012 – Wall detection 3
Today, I have detected the direction of and distance to walls, seen from processing the laser scan data. I now only have to figure out where the walls end in order to recognize an exit and pass the corridor compition succesfully!! Recognition of the end of a wall I will do by selecting the laser data points which belong to a certain line and looking at the first and last point of the selection. Therefore I also have to check whether a line is splitted in to more sections, or wheter it is just one solid line. Hope to get this done by the weekend!!
22th of May 2012 – Wall detection 2
I am still struggling with detecting all the walls from the laser scan data. I changed my plan and I will probably use a homemade piece of code to detect the lines, based on the socalled Houghtransform. I prefer this method above the ransac method, as I mentioned last week, since it is capable of detecting more lines at one evaluation. Besides, I am still trying to make the arrow recognition more robust. However, I can recognize an arrow with some images stored last week, but it does not work with all new images.
I hope to have some progress in the coming days, as the corridor competition comes closer and closer! To be continued...
18th of May 2012 – Detecting the walls
Today I managed to find the position of the walls of the corridor while I was walking through. Even without hitting the walls, which is of course a big leap forward on the way to my ultimate goal: escaping from the maze. I employed the point cloud library of ROS and especially the RANSAC method in order to detect the position of the walls.
16th of May 2012 – Thoughts about maze solving algorithms
Yesterday I watched the fairy tale ‘Hans and Gretel’ and I was intrigued by their idea to mark the path they had walked to find their way back. However they were not solving a maze, I can use this path marking to help me solve the maze. Of course, because I am a robot without arms I cannot drop pebbles like Hans did, but because I already learned how to create and remember a map, I can use this map to mark the passages I have already visited.
Because I also have to recognize walls and junction to solve the maze I am now trying to recognize lines in the laserdata which I receive from my sensors. Once I have learned all this I can use the Tremaux’s Algorithm to solve the maze. If I am not able to learn all these things in time I can use the Wall follower algorithm as a backup plan. These algorithms are further explained in the ‘Maze solving Algorithm's’ section.
11th of May 2012 - Created my first map
Today, I created my first map while being controlled by some keyboard inputs to steer forward, left and right. Here is a movie of this mapping process:
Currently, I am busy with learning how to process the map to enable navigating trough the maze. Moreover I am learning how to process images captured by my camera, such that I can recognize arrows and their pointing direction.
Next week, I'll update you about my progress...
8th of May 2012 - Learning how to make a map
Today, I have been busy with learning how I can produce a 2D map of the data I receive from all my sensors. I hope to succeed in making a map before the end of this week. Besides, I will try to succeed in driving a predefined distance or angle. This should eventually help me following the instructions of my navigation program.
I hope to have more exciting news soon...