Embedded Motion Control 2014 Group 8

This is the WIKI of group #8.

Group members

Name	Student ID	Email
Harm van Deursen	0707393	h.j.b.v.deursen@student.tue.nl
Lex Hoefsloot	0718252	a.v.hoefsloot@student.tue.nl
Teun Melief	0656886	t.j.melief@student.tue.nl
Robin Smit	0756271	r.smit@student.tue.nl
Paul Verhoeckx	0663727	p.b.verhoeckx@student.tue.nl

PICO information

1. 1080 laser points for distance measurement
2. approximately 250 degrees of measurement direction
3. The datapoint at the left (930), front (540) and right (150)
4. PICO's input for the velocity and angular velocity are in respectively m/s and rad/s.

The figure on the right shows the location of the laser (distance) measurement points

Project structure

Below, the structure of the program is explained.

Master

The Master file is the file which is taking control over everything that happens. Within the master file, there are the options that should be checked and actions that should be performed. The checking action "Detection" is constantly sending (with a frequency of 20 Hz) information to the master. The information which is sent is whether there is; nothing special, a corner, a corridor, a T-junction, a plus-crossing or a dead end.
The decision action "Strategy" is receiving information from the master. This information is about whether something is detected or not and what is detected (by "Detection"). Then within "Strategy" it is determined whether there is an arrow or not. With this information, a desired direction is replied to the master.
Next to the checking sub files there are two executers. Note that either one of both can be executed. The drive block is called when "Strategy" does not give any input to the master. When "Strategy" does however, "Prefered turn" is the action to be executed.

The figure below gives a visual representation of the interaction within the system.

For extra clearity, the movies below shows the nodes in use while PICO is running.

Driving straight

For the PICO to be able to drive straight to the corridors a “drive straight” component is implemented. This is used when no walls are critically close and no corners are detected yet. To do so first of all the ranges of N odd points, which are equally distributed over the frontal view of the robot, are evaluated. When any of these ranges is less than a certain treshold, the angle with the nearest wall is calculated. Therefor the smallest range of the set is selected, and it is assumed this represents the distance perpendicular to the wall. This is depicted in the left situation in the picture below where N=7, the use of the dashed lines is explained later. Now the angle θ is calculated with the use of the smallest range and the range straight ahead which are presented in red. When this angle is known the angular velocity is controlled with a PD controller which forces θ to zero. This basic controller equals: ω = P*θ + D*θ’. To calculate the time derivative of θ, a first order linear differential equation is used.

However if the nearest wall is not the wall PICO is directed to this approach would fail. This situation is shown in the right picture below, where the far left (orange) range is the smallest of the set. Now this range is compared with the dashed (green) range which is pointing slightly backwards. When this dashed range is smaller it is clear PICO is directed to the other wall. Now the smallest range on the other side is used to calculate θ, as shown with the red arrows.

Drive safely

To drive safe is the main task of this project. For this reason it is always checked (if just driving forward, in a turn, etc.) whether or not a wall is too close. There are four possible cases of too close:

At the front, left (laserpoints 540 till 680) a wall is detected
At the front, right (laserpoints 400 till 540) a wall is detected
At the left side (laserpoints 680 till 1080) a wall is detected
At the right side (laserpoints 1 till 400) a wall is detected

In the first two cases, the robot drives backwards and turns to the right (in the first case) or to the left (in the second). In the second two cases the robot keeps driving but steers to the right (in the third case) or to the left (fourth case).

Corner detection

The differences between several laser points are used to detect openings. On both sides five laser points diagonally to the front and diagonally to the rear are used, as well as five laser points straight ahead. When the difference between all the askew frontal and rear points are larger than some threshold, an opening is detected. Note that five laser points are used for robustness. If one or two laser points would go through a hole in the wall, no opening should be detected. The difference between the two points is taken in order to use relative distances instead of absolute ones. Otherwise a new corner could be detected whenever a distance is large, for example in a large open field.

If an opening is detected there are four possible situations:

There is a crossing;
There is a T-junction;
There is an exit into a new corridor;
There is a single corner.

Ideally a crossing is detected as in the left figure below: at both sides an opening is detected as well as the opening straight ahead. However, if PICO arrives in a direction not exactly parallel to the wall one side opening is detected first, as will be the case in practical situations. Therefore, if a side opening is detected, a new set of laser point (the green area in the right figure below) checks for an opening at the other side too. If so, a crossing is found.

The same is done for the T-junction, with the only difference there is not an opening straight ahead. Again the ideal situation (left figure below) will never occur, so an extra set of laser points is needed. See the figures below.

If a side opening is detected and the extra set of laser points (the green ones) don’t detect another side opening, there are two possibilities left: an exit into a new corridor or just a corner, dependent of whether or not an opening is detected straight ahead. This is depicted in the two figures below.

Finally it is checked whether a dead end is encountered. This is the case when no side opening is found and the set of lasers straight ahead return a value smaller than a threshold. This is also send back to the master file, so that a u-turn can be made instead of letting the drive safe modus inefficiently turn the PICO around.

Strategy

If a single corner to the left or to the right is found without any other options, these corners should obviously be taken. But if, in case of a corridor, T-junction or crossing,where multiple options are available, a choice has to be made whether a corner should be taken or not. If an arrow is detected which points to a direction which is not an option for the pico (if there is no corner found in that direction), the arrow is neglected. This is for driving more robustly if the corner detection misses any corners. If an arrow is found which points to a direction in which a corner is found, the pico always follows the arrow, regardless of any previous choices made or any other strategic considerations.
If no corner if found, the pico always prefers taking corners to the right and avoids taking corners to the left as much as possible. This essentially makes the pico a wall follower, with a preference to the right. Previous corners taken are not saved of stored anywhere since the have no influence on the decions in the future.

Take the corner

Once there is decided that a corner or u-turn should be taken, a subfile is called from the master file. The input for this subfile is wheter the corner is to the right, to the left or a u-turn. In case of a u-turn, the pico simply spins approximately 180 degrees and continues driving in the opposite direction.
In case of a corner to the left or right, the subfile performes 2 actions. First, the forward and angular velocities are set to a fixed value and the pico makes the corner for a fixed amount of time. Next, the forward velocity is set to a value without any angular or side velocity and the pico drives forward for a fixed amount of time, without checking for any new corners. This second action is performed to prevent the detection to detect a new corner immediately after finished the first corner, in case it is taken to short or incorrect. While performing these actions, the lasers keep updating their values. Unlike in the previous section of driving safe, the lasers are now divided into two subsections: left and right. If something is spotted on the left while taking the corner, a sideways velocity is added to the right. The other way around applies when a wall is found to close to the right. The forward and angular velocity are not altered when something is detected. Since makeing the corner is performed for a fixed amount of time, altering the forward or side velocity when something is detected whould result in a corner which is to short. For the driving forward after making the corner, the same safety applies as for the normal safe driving.

Arrow detection

These are the steps taken by 'arrowDetection':

Converting ROS image to an OpenCV image.
Converting the RGB OpenCV image to an HSV image.
Thresholding the HSV image in order to create a binary image of environment in which only bright right objects are 'white' and the rest is 'black'.
Dilate the image to remove any black spots in white surfaces.
Erode the image to create sharper edges
Preform template matching: matching images of left and right arrows to check wether they are present in the picture. Only if the similarity between the template and the camera footage is above a certain threshold, the template is considered 'found' in the image.
Matching of the two types of arrows is compared. The one with most similarity to the arrow in the camera feed is chosen.
Depending on the result, either 0 (no arrow), 1 (left arrow) or 2 (right arrow) is being sent to the strategy of pico.

In the tests there was a problem. Arrows were detected at a too large distance from the arrow (see "Fourth test" on this wiki). This problem is solved by implementing two different sizes of arrows. The bigger template of the arrow is recognized if Pico is close to the arrow, the smaller if Pico is further away. Now at a way larger range an arrow can be detected. Also a history buffer that saves observed arrows for several iterations is added. This history buffer makes the algorithm more robust against "false positives". This was needed because false positives were detected. If in the last, for example, 6 iterations 5 arrows to the left, and 1 to the right are detected, it is possible the arrow to the right (the wrong arrow) is sent to the master node. After implementing the buffer, this problem was solved.

Algorithm to solve the maze

In order to complete the maze as fast as possible the program uses a fixed strategy. When the robot detects a situation where it can continue in multiple directions it chooses a preferable one. This decision is made based on two parameters, namely which options it detects and whether or not an arrow is observed.
When an arrow is detected in a direction which is a subset of the detected options this will be the preferred new direction. In the case the robot observes an arrow in a direction which is not part of the found options, the arrow is ignored and a direction is chosen based on the rules explained below.
When a crossing is observed without the detection of an arrow, the program chooses the new direction according to a fixed sequence. If possible it chooses to go right, otherwise to go straight. This ensures that every corridor of the maze is evaluated before returning to the same crossing.

Tests

During each test session, several things are tested seperately and observations are done. This chapter is about the logging of all the information.

First test [15/05/2014]

The first goal was to survive the corridor test. This means any corner, left or right, should be detected and taken without any collosion.
There were two main tactics to be tested;

Global tactic (inserted in both programs):
- The corner can be detected by either laser point #150 or #930.
- Safety (not hitting the wall) is inserted for the measurement points 150 - 250, 400 - 540, 540 - 680 and 820 - 970.

first tactic:
1. The distance in front is measured and saved when a corner is detected.
2. Than PICO starts rotating until the distance on the side where the corner has not been measured is equal to the distance that was previously in front
3. PICO drives sidewards until the distance is 30 centimeters less
4. pico drives straight ahead again. The corner is taken

Second tactic:
1. The corner is measured but PICO keeps on driving straight ahead and the timer starts
2. The end of the corner is detected and the timer stops
3. PICO drives backwards for half of the time the timer has measured
4. PICO drives sidewards through the corner
5. PICO turns to get back in its neutral position

Observations;
- All measurement points should be used for safety to ensure safety
- The big mess should be cleaned and divided into seperate files which will be included into the master file
- One point to measure a corner is not robust. This should be done by the use of more points.

However, the second tactic seems to work quite okay and will thus be used for the corridor competition

Corridor test [16/05/2014]

Unfortunately there was not enough time to implement the observations in the code. The second method is used and the corridor was passed in 32 seconds.

second test [21/05/2014]

In the second test the new safety driving, the new corner detection and the straight driving have been tested. The method to detect a corner was adjusted. Where earlier corners were detected with only one laser point, the new corner detection got a lift. They are detected via the subtraction of five laser points at the side and five laser points at the side a bit more backwards. Parameters such as the difference and the position of these laser points which are used are tuned within this meeting.
To get the exactly desired position where a corner should be detected, PICO is placed at the place where the corner should be detected. Two points that differ half a meter in distance are used as the points for measuring a corner. For robustness this is extended to the five closest measurement points.
The safe driving is used in any of the testing cases. It is working very properly. The only thing is that the safety margin sometimes seems to be less than .3 meters.
The straight driving has also been tested. Whereas it works perfectly in the simulations, it worked properly one out of ten times in the tests. This is given the highest priority to get right for the next test session.

Third test [05/06/2014]

After a new strategy for driving straight was proposed, this was tested on the setup. The new PD controller worked properly after being tuned. Also the new method for taking corners and ensuring safety while doing so was tested with satisfying results. Thereafter the u-turn was tested and tuned however this yielded a problem. After turning 180 degrees the PICO can stand under an angle with respect to the wall. When there is a short distance to the next (T-)crossing there may not be enough time to counteract this. When PICO is still not nearly parallel to the wall at this time an opening can be missed, this could send PICO in the wrong direction.

Fourth test [12/06/2014]

Here we started to test the arrow detection in real life. We ran into some problems regarding its robustness, the arrows were not always recognized. Thereby, if the were recognized, it was at a too large distance. By the time the Master node requested the arrow direction it was not detected any more.

Fifth test [19/06/2014]

Here we fine tuned some thresholds for the arrow detection. In the previous test we only looked for the color red on one side of the spectrum. When this was altered the arrow detection was robust.

Sixth test [25/06/2014]

In the final test we fine tuned the thresholds for the situation recognition. Resulting in a robuster detection of corners, crossings, dead ends, etc.

Maze challenge

Finally, the real maze challenge. The goal; drive as fast as possible through the maze.
Expectations before seeing the maze;

Due to the controller PICO should be able to drive nicely straight.
The template matching worked very good during the last test sessions, hence we are expected to see all arrows.
We take corners by cutting them instead of 90 degrees corners. This should give us some time advantage against other groups.
We can solve any type of maze but due to the 'always turn right' algorithm, pico will most likely not take the most efficient way.

When we arrived at the challenge, we saw the maze. Yes, we will solve it. But unfortunately the first crossing will not be taken efficiently. The corridors are too large to detect dead ends before driving in there.
And indeed, all of our expectations were met. The maze was solved as shown in the figure below, in a time of 1'23.

Review on this challenge;

Positives
1. No other group was able to drive Pico as straight through the corridors as we did.
2. Our turns were very fast and efficient.
3. All arrows have been detected properly.
4. U-turn went perfect when the dead end was detected, and Pico came out of it straight due to the controller.
5. Every corner, T-junction and crossing have been detected.
6. Pico did not hit any wall.

Shortcomings;
1. We do not fit lines through the points. This also meant that we were not able to detect the dead end when the distance to the wall was larger than our treshold. Therefore we went into a wrong corner two times (algorithm of always turning right).

We would definately have been at the top 3, and have a very good chance on winning, if we would have implemented this.

On behalf of group 08;