Embedded Motion Control 2014 Group 9

Team members

Name	E-mail	Student #	Remark
Koos Elferink	k.elferink@student.tue.nl	0716494
Joost Potma	j.potma@student.tue.nl	0864146
Jasper Spanjaards	j.j.a.spanjaards@student.tue.nl	0746039
Jordi van Dijk	j.v.dijk@student.tue.nl	0805981
René Bruintjes	r.f.bruintjes@student.tue.nl	0877139

---

Planning

Week 1

• Installing Ubuntu
• Installing ROS
• Installing SVN
• Follow tutorial C++

Week 2

• Install Qt-creator (IDE)
• Group meeting to make a start on the project
• Test robot with standard script
• Test robot with added self made script parts

Week 3

• Group meeting (goal: finish corridor script)
• Second test on robot (test script)
• Corridor contest (16 May)

Week 4

• Group meeting (goal: maze solving strategy)
• Function to make Pico drive straight through the maze. (Koos)
• Make proper software architecture with nodes/topics (Joost/René)
• Start writing arrow detection algorithm (Jordi/Jasper)
• Measure laser distances during pico test

---

Corridor competition

The code written for the corridor competition was written and tested. Even though the code functioned very well in the simulator, it did not function on the real pico. Corridors on the left were taken without problems but right corners were ignored. Pico detected the right corridors well, and also took the decision to turn right. However the right turn command was not executed properly. After reordering the code, the right turns were taken fine.

Simple parallel wall driving

A simple way is implemented to drive parallel next to the wall.

The distance to the wall is measured in at an angle of 45deg left (a) and right (b). If the distance become below a threshold than pico will rotate in opposite direction as long as there is the wall (or other obstacle) within it's range.

Simple corner (corridor) detect

A simple corner (corridor) detect is impmlemented in a seperate function. On the left and right side side corridors are detected by scanning an angle of -95 -> -85 and 85 -> 95 of pico. If 95% or more laserdata points within a range is higher than a treashold, than a corridor is detected. A detected corridor, left and/or right, is send a boolean the the navigation script as a return value.

Script overview for corridor competition

The strategy for the corridor competition is quite simple. Pico will start drive trough the corridor keeping straight using the parallel wall driving. Once a corridor is detected pico will start rotating open loop wile keeping a forward velocity for a fixed amount of time. In this way a approximately 90 degree turn with a small radius is made. After the turn is complete pico will resume driving straight until it detects that is has no more walls around him and stop, assuming that it is finished.

---

Maze competition

This week we started with designing the proper framework for the different Ros Nodes and Ros Messages. Each node has a specific function and a predefined interface.

As shown as ellipses in the figure above, the framework consists of several Ros nodes. Some of these nodes are already implemented in Pico such as the laser node and the camera node. The other nodes are described below.

Dead end

• 
• 
• 

To determine if there is a dead end, the field of view of pico is first divided in to three sections (left, right and front). If there is opening in one of these sections, so no dead end, this will show up as a step in the distance of the laser data. In the above example there is a step only in the right section so left and straight are a dead end.

Corridor

• 
• 
• 

When pico is approximately in the middle of a corridor to the left of right, than the there edge at 45 deg behind pico. To detect this edge the first or last section of the laser data is divided in too three sections of nine points. These nine points are than added to get a mean value for this section. If there is an edge, there will be a local minimum in the data. So if the first and third section have a larger mean value than the second, than there is an edge and there could be a corridor. To make sure that there is indeed a corridor, pico also checks the distance besides him to see if there is really a opening in the wall. This avoids detecting a corridor when there is only an edge due to for example an irregularity in the wall.

Arrow

The Arrow node processes the camera date to look for an arrow in the image, and if there is an arrow, in which direction it is turning. This information is also send to the navigation node.

The arrow detection first crops the RGB image so that possible disturbances outside the maze are not looked at.

RGB image with the red rectangle indicating the cropped area.

The RGB image will be converted to an HSV image, where the hue, saturation and variation play an important roll to make sure only the red colour is selected.
The HSV image will then be converted into a binary image, which is then blurred to decrease disturbances.

Non-blurred binary image

In this blurred image it is looked for how long white pixels are connected with each other. When they have a minimal length of and x-amount of pixels and are not interrupted by more than a x-amount of black pixels, a line is created. Lines with an angle between 20 and 60 degree are filtered, since these lines are created at the head of the arrow and indicate if the arrow is pointing left or right. By checking the slope of each line (negative or positive) and check whether lines with a positive slope are below or above the lines with a negative slope, it can be seen if the arrow is pointing right or left. If the lines with a positive slope are above the lines with a negative slope, then the arrow is pointing to the left, visa versa, the arrow is pointing to the right.

in the main script a counter is added to see how many frames in a row an arrow is detected. If it is detected more than an x-amount of times in the last x-frames, the boolean is stated true and the arrow is detected. This is done to add robustness to the arrow-detection.

Navigation

The Navigation node determine the direction in which to drive to. To make a decision based on the data from the Dead end, Corridor, and Arrow node. It is a rule based navigation based on the principal to always go left. In this case the exit of the maze is always found. If there is a possibility to go left it always will go left unless it is overruled by a right pointing arrow. If it can go straight and right it will always go straight. If the only option is right it will go right. And if all the three directions are a dead end if will turn 180 degrees. These rules are shown in the flowchart below.

Drive

The Drive node will execute the commands given by the Navigation node. The default option is to drive straight, other options are to make a turn to either left of right, or to turn around in case of a dead end.

For the functionalities of the drive node a few other functions are used, which are explained below.

Line and middle detection

The functions are specified in three different functions:

Scanxy (brown lines)

This function reviews the laserdata and tranlates them to a structure with their x and y distance with respect to pico.

linedetect

input: (Sxy xy,double x1, double x2, double y1, double y2, double angle, double Tol, int conf) Here the square where the function searched within is defined bij x1<x<x2 and y1<y<y2, by the user, and so is the desired search angle with certain tolerance angle and confidence. This program can be used to find angles of certain lines in a domain. But it can also be used to determine if there is any.

For the detection of the world and the desired direction, two scripts have been implemented. These two are visualised in the figure.

OLD version: Straightdetect; (other lines) This function uses the result of scanxy, a struct of type Sxy of x and y coordinates. It analyzes the direction of the corridor with respect to the orientation of pico. It does this by looking at 2 laser points and comparing the dx and dy to calculate an angle. For robustness this process is repeated for multiple pairs of dots, the results are filtered and averaged. This results in a good and robust measurement of the angle which pico could rotate to be located straight in the corridor.

Middledetect

Middledetect uses the structure Sxy xy to determine the middle of the hallway. It returns the distance in which it could drive to find the middle of the corridor, but does not steer of the wall for more than 0.75m Method: determine average of left and right points. (with maximal distance of 0.75m) Then subtract to find the difference between the two. Note that the box with the points and the maximal value 0.75 are also input of the function.

Straight driving

For straight driving the forward speed is set to the maximum. Pico should then rotate towards the straight, at the same time pico should correct to find the center of the corridor.

Y-axis; The centre is tracked by analyzing the distance towards the middle using middledetect. A simple and weak P controller is already enough to follow the middle of the corridor.

Rotation; At the same time pico will try to find the angle of the corridor (using linedetect) and compensate for that, also with a simple weak P controller. In this case pico has two states, a high precision state where little tolerance is allowed, using lines at the straight or at a 90 degree rotation. If this high precision is not met, than a higher tolerance (-/+ 30 degrees) is allowed to find only the straight. In this case pico can also follow curved/.. walls and other strange situations. But then it is more sensitive to disturbances.

High precision straight driving versus low precision straight driving

• 
• 

Cornering

For cornering the linedetection is used again. As soon as drive receives the order it assumes the corner can be taken. The methods are nearly similar in a right or left rotation.

Cornering is separated in two parts; the first 45 degrees, then the next 45-90 degrees rotation. Only after both parts are done, the cornering is finished and straight driving is continued.

Cornering; first 45 degrees

• 
• 
• 
• 

Pico starts rotating and determines appropriately when the first 45 degrees of the rotation is done. Different (blue) areas are chosen for robustness.

Cornering; Next 45 to 90 degrees rotation

• 
• 

The last part of the rotation is done as soon as a straight line is found again by linedetect, after which straigth driving is continued

dead end, turn around

In the case of a dead end pico turns in the opposite direction of the tactic, so clockwise. Pico turns for as long as dead end detection is true, and finds a straight afterwards.

Safety

Next to all the other nodes, Pico could still run into problems. In this case pico searches for the most open direction and rotates towards it. For this is searches at a 45 degree angle. Should the safedrive go off, pico will rotate clockwise

Messages and Topics

All the nodes use different Topics and messages to communicate with each other as shown in the framework picture below. Below is an explanation of all the topics and messages.

pico/dead_end

The Dead end node will publish it results on the pico/dead_end topic. To do this it uses a dead_end message with the following construction:

bool left

bool right

bool straight

pico/corridor

The Corridor node will publish it results on the pico/corridor topic. To do this it uses a corridor message with the following construction:

bool left

bool right

bool straight

pico/arrow

The Arrow node will publish it results on the pico/arrow topic. To do this it uses a arrow message with the following construction:

bool left

bool right

pico/direction

The Navigation node will publish it results on the pico/Direction topic. To do this it uses a direction message with the following construction:

int direction

The value of this integer determines the direction as follows:

1 = straight

2 = left turn

3 = right turn

4 = 180 deg turn