Embedded Motion Control 2017 Group 10

Group Members

0773865	Tim Coerver
0953808	Pieter de Groot
0970955	Jos Terlouw
0973811	Bas Vermij
0972391	Roel Vromans
0975718	Corné van Haren

Initial Design

Link to the PDF version of the design document: PDF

Requirements

• Software easy to setup
  • Updated with one easy command, e.g. 'git pull'.
  • Software can be compiled using 'cmake' and 'make'.
  • One executable to start software.

• Autonomously solving the corridor challenge
  • Solving the corridor challenge has to be done within 5 minutes.

• Autonomously solving the maze challenge
  • Solving the maze challenge has to be done within 7 minutes.

• Avoiding collision
  • Do not bump into walls or doors.

• Recognize and open a door in the maze
  • There might be multiple dead ends in the maze, one of which is a door. The robot has to be autonomously open the door (by ringing a bell) to solve the maze.

• Detect corridors, corners, T-junctions and intersections
  • The maze and corridor challenge consists of several, corridors, corners, T-junctions and intersections. Various algorithms created in order to detect these.

• Navigate through corridors, corners, T-junctions and intersections
  • When corridors, corners, T-junctions and intersections are detected, the robot has to drive trough these. In order to do this, strategies have to be created for each of these.

• Detecting dead ends
  • The maze can contain dead ends, these have to be distinguished from doors.

• Detecting maze exit
  • The exit of the maze has to be detected in order to know when the maze is finished.

• Navigate open spaces
  • The maze can contain open spaced, when these are detected these has to be explored by navigating trough them.

Specifications

The specifications are related to the requirements specified above.

• The time to reach the end of the maze is 7 minutes.
• The time to accomplish the corridor challenge is 5 minutes
• The robot may not be idle for more than 30 seconds.
• When the robot find a door, it rings a bell and the door will be opened manually.
• Two attempts to solve the maze are allowed. Both attempts have to be finished within 7 minutes total.
• Two attempts are allowed to solve the corridor challenge. Both attempts have to be finished within 5 minutes.
• When the robot bumps into a wall the attempt is over.

Specifications of the maze

• All walls are positioned approximately perpendicular to one another.
• Open spaces might occur (as depicted in Figure 1 ).
• There may be loops in the maze, which means that some walls may not be connected to other walls.
• Walls are approximately parallel.

Doors and open spaces are specified as described by Figure 1.

Figure 1: Maze open spaces and door specifications

Functions

The functions are categorized in three different levels of complexity: Low, Mid and High level. An overview of the functions is given in Figure 2.

Figure 2: Function overview. The functions are grouped based on complexity as well as input/output type.

Components

The following components are accessible on the pico:

• Sensors
  • Laser Range Finder (LFR)
  • Wheel encoders (odometry)

• Actuators
  • Holonomic Base (Omni-wheels)

• Computer
  • CPU Intel i7
  • OS Ubuntu 14.04

Interface

The interfaces between functions are shown in Figure 3. Here the functions are devided into: Tasks, Skills, Motion and World Model. The world model contains the functions that are used to keep track of Pico's surrounding. Tasks contains the functions on the highest level where the robot decides what it is going to do, like solve the maze. The skill block consists of all functions that make the robot perform lower level tasks such that the high level objective can be achieved. The motion block does the low level operations that make the skill functions work.

Figure 3: Interface overview of all functions needed by Pico to solve the maze and corridor challenge.

The Corridor Challenge

Moving and cornering

A couple of things are used in order for Pico to move through the corridor and make a corner. The first thing is a potential field. This creates an artificial bubble around Pico which repels it from objects that get too close. The second thing that allows Pico to make the corner in the corridor is its corner detection. This corner detection analyses the data from Pico's laser sensor and finds the corners of the corridor. When Pico approaches a corner on either side of the corridor, a reference point is placed beyond this corner in the middle of the hallway Pico should turn into. An attracting force is created that pulls Pico straight towards this reference point. Of course Pico can't go straight to this reference point since part of the wall is in the way and Pico is pushed from this by the potential field (see figure below ...). This combination of attraction and repulsion creates a curved path around the corner and into the exit of the corridor. As shown in the video (above below?), Pico makes a beautiful turn to exit the corridor. In the two following sections the inner workings of the potential field and the corner detection are explained in greater detail.

...

Insert picture if corridor with pico and reference point. Add vector towards reference point through the wall. Add potential field vectors and create dotted line which highlights the resulting path of pico when pulling vector and potential field are combined.

...

...

Insert math to show how reference point vector and potential field are balanced.

...

Potential field

TO DO: - Images - Math - Code - Explanation

In order for Pico to safely manoeuvre through the corridor, a potential field is implemented. This potential field will push Pico away from walls with a force that gets bigger the closer Pico gets to a wall. The size and direction of this force is calculated based on the data gained from the laser sensor. First, the length of all 1000 laser lines is inverted and the angle of each line is shifted with pi. This creates vectors that point away from obstacles with a force that increases the closer an obstacle is to Pico. After that, all converted vectors are split into x and y segments which are summed up and recombined to create a single vector pointing away from any obstacles. The size of the resulting vector equals the force, and thus speed, with which Pico is pushed away from the obstacles. In order to make the behaviour of Pico more predictable the size of the resulting vector is normalized and rescaled based on the closest object at a later point. Rescaling all vectors that influence Picos movements based on the distance to the nearest object creates a clear overview of how these different forces interact and make Pico move.

...

insert image of pico and walls with potential field

insert (image?) math latex

...

Corner detection

To detect corners and walls the Split and merge algorithm is used on the laser data. This algorithm contains the following procedure;

• 1. Laser data is transformed in x and y coordinates
• 2. The laser data points will split in segments
• 3. Within each segment the points between the first and last point will be check for the distance perpendicular to the line between the first and last point
• 4. If the maximum perpendicular point is above a set threshold (0.2 m), this point will set as a vertex within this segment.
• 5. Step 3 and 4 will be repeated until there are no new vertices are found.
• 6. Within each segment a line will be defined using the least squire method.
• 7. If two lines are connected with each other, the intersection will be used to define the vertex.
• 8. If there is not a connection with another line, the end point will be fitted on the line.

These steps are visualized in the figure below:

Using the Least Squire Method, the standard algorithm has to be expended. As can be seen in the fist situation in the figure below, there is a wall left of Pico. Using the standard Least Squire Method, a line (yellow) is constructed where the error (purple) between the points and the line is minimized over the y axis. However, if there is a wall in front of pico, as shown in the second situation, the least squire method found a line that is nog equal to the orientation of the wall. This is because the standard method minimizes the error over the y axis. If the same procedure method is used over the y axis, as shown in the third situation, the found line has the same orientation as the wall. Therefore, the Least Squire Method is extend as shown in #THE CODE BELOW#, where the Least Squire Solution over x or y axis is picked for which one the error is the smallest.

Figure #: Least squire method for x and y axis.

//=====================Extended Leas Squire Method =========================
// Define variables
   sum_x = 0;
   sum_y = 0;
   sum_xy = 0;
   sum_x2 = 0;
   sum_y2 = 0;

// Length section
   n = sections[i][1] - sections[i][0];
   for(int j = 0; j < n1 ;j++){
       // Get x,y,xy,x^2 and y^2 from all coordinates
       sum_x += Coordinates[sections[i][0]+j][0];
       sum_y += Coordinates[sections[i][0]+j][1];
       sum_xy += Coordinates[sections[i][0]+j][0]*Coordinates[sections[i][0]+j][1];
       sum_x2 += pow(Coordinates[sections[i][0]+j][0],2);
       sum_y2 += pow(Coordinates[sections[i][0]+j][1],2);
   }
// Devide by number of coordinates
   sum_x /= n;
   sum_y /= n;
   sum_xy /= n;
   sum_x2 /= n;
   sum_y2 /= n;

   A = -(sum_xy -sum_x*sum_y);
   Bx = sum_x2-sum_x*sum_x;
   By = sum_y2-sum_y*sum_y;

// Define best solution, over x or y axis
   if(fabs(Bx)<fabs(By)){
       // Least squire method over y axis
       B = By;
       std::swap(A,B);
   }else{
       // Least squire method over x axis
       B=Bx;
   }

   C = - ( A*sum_x + B*sum_y);

// Solution of extended least squire method where line if defined as
// y = b*x+a
   a = (-C/B);
   b = (-A/B)

Challenge Result

Pico managed to finish the corridor challenge in 24 seconds, this got us the third place. As can be seen in die video on the straight the Pico wobbles a lot this is due to the over reactive implementation of the potential field. Without this wobbling we would possible be in the first place due to the very smooth corner.

Corridor challenge result showing a drunk pico making a perfect corner.

The Maze Challenge

Final design

To solve the maze challenge there are designed individual parts, which will be explained in the next part. To manage these individual parts there is needed a state machine which is called the supervisor in Figure 4. This state machine executes the individual part based on their outputs. The start state of the state machine is 'Go Straight', this is because the counter of the Plegde algorithm start at zero because Pico is not following a wall at that time. Pico stays within this state until a wall comes in his path, therefor the function detect wall is used. When this function sees a wall, the output of this function will be one, then the state machine will go from 'Go straight' to the 'Follow wall' state. Besides contain the 'Go straight' state a function to move the robot. When this wall is detected, the state machine of Pico goes to the 'Follow wall' state. This state machine is designed to always come in the 'Follow wall' state because then there are no confusing loops, this keeps the design simple and robust.

The main function of the 'Follow wall' state is as can be imagined the follow wall function. This function uses a wall to drive straight in combination with potential field. When there is a corner, the pledge algorithm and corner detection will play a role. As specified the corners will have a angle of around 90 degrees, based on this the pledge algorithm keep track of the number of corners that is taken. When this counter of the pledge algorithm is back at zero, number of right corners equal to number of left corners, the state machine will go to state 'Go straight'. To detect if there is a corner the 'Follow wall' state has the function corner detection which is explained in the corridor challenge. Another way to leave the 'Follow wall' state is by detecting a dead end. When a dead end is detected, the state machine goes to the state 'Dead end' because this needs a couple of features. The last function in the state 'Follow wall' is the move function to drive the robot.

Part of the maze challenge is to open a door which can be seen as a dead end because of the specifications of a door in the assignment. This comes part of the state 'Dead end'. In the 'Follow wall' state, a function dead end detection is implemented and through this state the state machine can come in the state 'Dead end'. When the state machine comes in this state,their will be two options, first it is could really be a dead end or there could be a door. This is why in the state 'Dead end', first the bell will be ringed and waited for a couple of seconds. Namely, when this is done, the door will open or not and can be made a conclusion of the two options. This is why after ringing the bell and waiting for a couple of seconds, again the dead end detection function is executed. When again the dead end is detected, so there is no door, the robot has to turn around and the state machine will go to state 'Exit dead end'. But when there was a door, the door is opened and the robot can continue to follow the wall and the state machine will go to the state 'Follow wall'.

To exit a dead end when is concluded that it is not a door, a few steps has to be taken, and this is done in the state 'Exit dead end'. Basically, Pico follows the wall again as in state 'Follow all' by using the functions pledge, corner detection, follow wall and move. The pledge function is used because otherwise the number of corners taken is not accurate anymore. By detecting if the dead end is left, the state machine can change to the state 'Follow wall' again.

Figure 4: State machine

With these four states and the individual functions, Pico should be capable of solving the maze challenge.

Individual state machine functions

Wall following

To implement the pledge based strategy it is necessary for Pico to be able to follow a wall in a robust manner. For this goal a wall following function has been implemented. This function makes use of the

Pledge

The Pledge algorithm is used to make a very simplistic global world map to solve the maze. The pledge algorithm uses relative angle movements to calculate the total rotation angle of the robot. Rotation clockwise has negative angle and rotation clockwise has positive angle. After the robot rotates pi rad, the OdometryData switches to a negative angle of -pi rad. The relative angle is in that case 2 pi rad. To compensate, 2 pi is subtracted leaving with only the relative angle. With this rotated angle, the taken corners can be calculated.

To make the pledge algorithm robust, a method is used where a corner is counted after the robot moved pi/2-delta rad and will hold this count till the robot moved another pi/2-delta rad as shown in Figure 5 (left circle). In between the robot can move freely without corners being added. The same holds for the robot rotating back where the corner counter is reduced only after the robot moved -pi/2+delta rad Figure 5 (right circle). Because the rotated angle is calculated with underlying code, this can go on until infinity and so many corners will not be a problem. For the straight forward case a second parameter, delta 2 is added so the robot will get out the follow wall as straight as possible.

Figure 5: Pledge counter

   //corner counter
   if ((rotationAngle < DELTA_ZERO_ANGLE && corners_counted == 1) || (rotationAngle > -DELTA_ZERO_ANGLE && corners_counted == -1)) //go straight
   {
       corners_counted = 0;
   }
   else if (rotationAngle >= 0+DELTA_CORNER_TAKEN) // first turn is counterclockwise
   {
       if ((rotationAngle+DELTA_CORNER_TAKEN)/(M_PI/2*(corners_counted+1)) > 1) //rotate counterclockwise
       {
           corners_counted = corners_counted +1;
       }
       else if (((M_PI/2*(corners_counted-1))/(rotationAngle-DELTA_CORNER_TAKEN)) > 1) //rotate clockwise
       {
           corners_counted = corners_counted -1;
       }
   }
   else if (rotationAngle <= 0-DELTA_CORNER_TAKEN) // first turn is clockwise
   {
       if ((rotationAngle-DELTA_CORNER_TAKEN)/(-M_PI/2*(corners_counted-1)) < -1) //rotate clockwise
       {
            corners_counted = corners_counted -1;
       }
       else if (((-M_PI/2*(corners_counted+1))/(rotationAngle+DELTA_CORNER_TAKEN)) < -1) //rotate counterclockwise
       {
            corners_counted = corners_counted +1;
       }
   }

Dead end detection

The dead end detection method makes use of the corner mapping method as used in the corridor challenge.

The corner mapping returns the vertices's of the line segments it has found, and if the vertices's are connected to each other. This information is used by the corner detection to form vectors between these vertices's.

These vectors are then normalized so they can be used to determine the angle between them. If two vectors are connected, the angle is calculated using the following function

Where [math]\displaystyle{ \theta }[/math] is the angle between the vectors and v_i • v_i+1 is the inner product of vector i with i+1. If this angle is 90±5 degrees, the corner is marked perpendicular. The calculated angle does not give information on the sign. A vector pointing to the left or right result in the same angle (as the inner product between almost perpendicular vectors is approximately 0). In order to determine the direction of the turn, the vector v_i is rotated 90 degrees to the left, using a rotation matrix. The inner product is then calculated again, if the result is positive, then the turn is to the left. If rotating the vector to the right results in a positive inner product, then the turn is to the right. All vectors are checked in this fashion, resulting in a list of left-, right- or no-turns, describing all corners visible to the robot.

Using the list of turns, possible dead ends can be identified. An important aspect of the list of vertices's is that it is generated in an anti clockwise direction. Therefore it is known that when two left turns follow after each other indicate a dead end candidate. Since the walls of the dead end candidate can be long, and therefore can just be a wall in the maze, further identification is done. The specifications of a dead end are given in the specifications above. There it is noted that the width of the dead end has to be within (0.5 < width < 1.5) [m]. So when a possible dead end is found, the width of that dead end candidate is checked. If it fails, the dead end is dropped. If it passes the minimum depth of 0.3 [m] is checked. If that also passes the dead end will be marked.

As all turns in the turn vector have been evaluated, the dead end closest to pico is returned from the dead end detection method for furher use in the state machine.

Matrix multiplication, matrix norms and inner products where used in this function to make the implementation easier. Here fore the Eigen library was used, this made working with matrices a lot more convenient (almost matlab like). The implementation of the function is found on git dead end detection

Door detection

The function for the door detection is simple and uses the information from the dead end detection function. The function works as follows:

1. Dead end function detects an dead end.
2. Ring bell
3. Wait 5 seconds, the wait time for the door to be centainly open if there is an door.
4. Continue with wall following. If the door is open Pico will move trough it, if not it will exit the dead end.

Final challenge result

After the mild succes at the corridor challenge, where we finished 3th. As we had to use code that was not tested in practice, the speed was not good enough for first place.

After the corridor challenge, where we relied heavily on potential field, a complete overhaul of our approach was made. We decided to use potential field only for obstacle avoidance, when all our other strategies failed. So for the primary motion skill, the follow wall procedure was created. This makes use of the laser data in a smarter way such that speed could be increased (as no obstacles had to be avoided, only walls had to be followed). This combined with the robust implementation of pledge ment that solving a maze was not the problem.

As we had code that was able to detect walls, that could than be converted to vectors, used to identify angles between walls. We where able to detect wall sections that form dead ends. After matching these to a templates, a robust and reliable procedure to detect dead ends was added to the general maze solving algorithm. The only task left was the ringing of the bell to open the door.

When we first tested this maze solving program, the day before the challange, we where amazed by the robustness of our code. All scenarios made up by us where solved by the robot. Walls where followed, loops in the maze where navigated correctly, dead ends where found and doors where opened. All good.

The day of the challenge

As we where the last group to have a go at the challenge, the time to beat was set. 2:43 minutes. In the first go good progress was made by pico, the door was found and opened. The only task left was finding the exit. This was also looking good. We had enough time on the clock to become first. Pico found the exit, 2:05 on the clock. With the finish line in sight (20 cm). Pico decided to stall and trow a segmentation error.

So now knowing that our pico was the fastest of the day we still could win due to our second try. In the second try pico is looking steady as it did in the first go, it managed to find the door. In the first corner after the door pico stalls again, segementation error? No this time it give us an matrix multiplication error. Resulting in a 3th place, since two groups manged to get over the finish line.

It is unclear to us where the segementation error came from. A possible cause might be that the net hanging close to the exit of the maze gave some strange environment we did not take into account when we where testing. But we do not know what part of our code was unable to deal with this situation. The matrix multiplication error might be caused by the eigen library used in the dead end detection. Since it allows for the use of matrix multiplications and only is being used by the dead end detection.

Maze challenge result first try ( speed 4x).

Maze challenge result first try ( speed 4x).

Code Snippets

These are the code snippets of various functions used in the maze challenge.

• Corner detection: test
• Dead end detection:
• Pledge:
• Wall following:
• Potential field: