Embedded Motion Control 2018 Group 7

Group Members

TU/e Number	Name	E-mail
0833049	Sam Roovers	s dot roovers at student dot tue dot nl
0886654	Siebrand Keja	s dot c dot keja at student dot tue dot nl
1022624	Milan Haverlag	m dot a dot haverlag at student dot tue dot nl
1279637	Piyush George Alexander	p dot g dot alexander at student dot tue dot nl

Initial Design

The Initial Design document can be found here : File:Emc-designrequirements.pdf

Escape room competition

To escape the room, PICO will first scan its surroundings. By the use of a corner detection program the exit is determined from two laser sets (from the front and back). PICO aligns with the angle at which the exit is detected and will drive forward. A potential field must make sure that PICO does not hit any walls.

Simulation for the Escape room challenge is shown below.

Figure 1 Animation of the Escape room challenge

Exit detection

For detecting the exit, the range from the laser data was considered. PICO does a 360 degree scan and detects an exit point based on distance criteria, i.e. if the distance between consecutive sections is within the exit door tolerance ( 0.5-1 m). After detecting the two points, it searches for the point right in the middle of the detected points, which would be a waypoint for PICO to drive to, followed by which it would drive till the exit is reached.

Figure 2 Exit detection

Potential field method

In order to prevent PICO from crashing into the walls, obstacle avoidance is needed. For this it is chosen to implement potential field method. The potential field method works by combining a attractive force towards a desired set-point with repulsive forces generated from obstacles.

The Attractive force is calculated from the distance between the robot and the set-point. [math]\displaystyle{ r_{att} = \sqrt{(x_{set}-x_{PICO})^2+(y_{set}-y_{PICO})^2} }[/math] and the angle: [math]\displaystyle{ \alpha_{att} = tan(\frac{x_{set}-x_{PICO}}{y_{set}-y_{PICO}}) }[/math]

The attractive force is calculated using a proportional controller:

[math]\displaystyle{ F_{att} = r_{att}*K_{att} }[/math]

The repulsive forces are calculated by taking the inverse of the distance to the wall measured with the laser range finder with the following formula: [math]\displaystyle{ r_{rep}(i) = \sqrt{(x_{wall}(i)-x_{PICO})^2+(y_{wall}(i)-y_{PICO})^2} }[/math] [math]\displaystyle{ F_{rep}(i) = \frac{K_{rep}}{r_{rep}(i)^3} }[/math]

The attractive and repulsive forces are then split in their x and y parts. Their respective x an y parts are then added together:

[math]\displaystyle{ F_{tot}(x) = F_{att}(x) + \sum{F_{rep}(i)(x)} }[/math] [math]\displaystyle{ F_{tot}(y) = F_{att}(y) + \sum{F_{rep}(i)(y)} }[/math]

These Forces are then used to determine where PICO should drive to.

Shortcommings

Due to time constraints the following features are not implemented which would have made the escape room routine a lot better.

• the positioning of a waypoint in front of the exit with enough clearing from the walls to drive to.
• alignment with the corridor after the waypoint has been reached.

Hospital challenge

Main Loop

Scanning the hospital map

The first part of the challenge consists of mapping the hospital. The program to do this consists of the following steps

Step 1: Pico rotates 360 degrees at its starting position to start creating a map of the hallway. The program calculates the end of the hallway using the furthest set of corners that it can find. A way-point is place half a meter from the end and Pico drives to that way-point. When Pico has arrived at the way-point, it starts rotating 360 degrees again completing the map of the hallway.

Step 2: The program tries to find the doors by looking at corner points found while scanning the hallway. Way-points are then placed in front and behind the center point of the found exits. The way-points that are placed behind the exit are each given a new room number. The created way-points are considered connected with each other, meaning that the robot can use them to drive to another room.

Step 3: The program checks which way-point is closest. If no way-point can be found that has not already been used, Pico will check which room it is in at that moment. If Pico is in the hallway, Pico will go back to its initial location and go to the parking program. If Pico is not in the hallway, it will check from which way-point it entered the room and go back through that way-point. If a unused way-point is found Pico drives towards that way-point. When Pico arrives at the closest way-point,it then drives to the way-point thats connected to it. The program starts rotating 360 degrees again, adding the room to the exiting map of the hallway.

Step: 4 The program again tries to find the doors by looking at corner points found while scanning the new room. Way-points are then placed in front and behind the center point of the found exits. The way-points that are placed behind the exits are each given a new room number. Steps 2-4 are repeated until no more unused way-points are found in any of the rooms.

Using the hint

After Parking, Pico has to start searching the object. The following hint is given on the location of the object, The object is located in the room that's connected with the hallway through the highest number of doors. A program has been written that determines the room that's furthest away from the hallway. The function find_longest_path, starts by looking at the first way-point in the hallway and finds its connected way-point. the program then looks at which room the connected way-point is located and looks if there any other way-points, if there are other way-points then for each of those way-points, the function finds the connected way-points and again looks for other way-points. this is repeated until no more unused way-points can be found. The program then saves the longest path it could find. The function then repeats this for all way-points in the hallway. When all way-points in the hallway are used, the program saves the longest path and the room number that the path leads to. This path consist of a array of consecutive way-points that lead to the room. This information is then fed to the function drive_to_room.

Driving to a room

When Pico has determined where the object is expected to be using the given hint, the function drive_to_room is given the path derived by the find_longest_path function and starts driving to the first way-point in the input array. When Pico has reached the point, it starts driving the second way-point. this is repeated until the last way-point in the array. when the last way-point is reached, the object detection function is called.

Feature Detection

For the detecting the features first it is essential to find out the number of sections in the surrounding. A section comprises a continuous set of data points. A section ends when the distance between consecutive data points exceeds a threshold value. In this case, it is 0.5. In the function that is made, the section details are verified by returning the indexes of the beginning and end section details into separate vectors. The index details are further used to determine the number of sections. The beginning and end details of each section is used in the split and merge algorithm to find the corners and ultimately return a 2 dimensional vector in which each row corresponds to each section and each column has elements which are of a structure type that has the x and y coordinate details, whether itś an intermediate point/ corner and if itś an exit corner or not.

In order to generate the Map, it is essential to detect the features. This is attained using a split and merge algorithm. The basic idea of the split and merge algorithm is as follows if the endpoint and the starting point of the section are known, it fits a line between these two points and evaluates the distance of each point from this line. The point that is at a maximum distance from this line, greater than the threshold set for it being part of a wall, would be marked as corner point1, and a boolean would mark it as false for a corner point( true for intermediate point). If not, it would be identified as a wall. Now the program searches for any more corners between the starting point and the corner point1 and in between the corner point 1 and the exit point. If it finds more corner points it returns the details of these corner points and repeats until only wall segments are identified. For a section, the function ultimately returns the initial point, corner1, corner2 ... corner#n, the endpoint of the section as each row.

Algorithm

The Modified split and merge algorithm works in the following sequence:

• The starting node = Section beginning point, ending node is the section ending point.
• Line is fitted between these points and distance to each point in between is found.
• distance of each point inbetween from the fitted line is determined with the equation given below.

dist = abs( a*x+ b*y + c )/ sqrt( a²+ b² )

a = (yi - ye)/(xi - xe)

b = -1 

c= (xi*ye - xe*yi)/(xi - xe)

• Check is all distances are within the threshhold.( here threshhold was tuned to 0.1 considering the fact that the wall thickness is 20 cm.

 if (distance < threshold )

Return the initial and end point with flag as true indicating that itś a wall. 

else fit lines line between the initial point - cornerpoint found and the exit - corner point and search for more corners. 

If all wall segments are identified with no more corners, return for the current section, the initial point, corner 1, corner 2 ... etc end point in sequence.

goto the next section, follow the same steps until the last section. 

For illustration of the modified split and merge algorithm refer the sequence generated below. The threshold length mentioned here is the length for the point separated from the wall to be categorized as not a wall.

Figure 2.1 Animation of Modified split and merge

The feature depends on laser data as input, the data on reading the CSV file showed several data points as Nan( not a number). These are eliminated from the section by filtering the laser data.

• Separate structure to characterize the points(Marker Allocation)

Figure 3 Identifying the points

Every point that has been detected is a structure of the following form.

• x coordinate
• y coordinate
• bool, true if itś an intermediate point, false if a corner
• bool, true if itś an exit corner

Characterizing the corners found

Figure 4 : Characterising Corners

• Distinguishing between exterior corners and interior corners.

In order to distinguish between interior corner points and exterior points the following characterisation is made using the MARKERALLOCATION function. A tuning parameter(INDEX) is considered and datapoints with indices before and after the detected corner point are taken and the average of both the indices are considered. If the length of this point to PICO is more than that of the length of the corner point till PICO then itś an exterior corner point, else its an interior corner point.

• Identifying the exit points

The main criteria that was considered for an exit point to be a door is to identify that the distance between the exit point and the next immediate point in the laser range to be within the range of 0.5-1m, except for the last point in the section. If the criteria is satisfied, then the flag for the point is returned with an index of true. This can also be seen in the debugger that has been attached alongside. Only one for the points is identified with an exit corner index.The next immediate point which is at the shortest distance is found.

Figure 5: Exit detection

Test results

show the generated image

Debugging interface

Figure 6 debugging interface

The above interface was used to debug the modified split and merge algorithm. It gives real time values that are detected as corner/ intermediate points and on whether they are exit or not( from the bool flag). As PICO moves through the map it dynamically gives out the point which can be verified from the map used for in the PICO simulator.

The interface gives real time details of each section under CORNER MATRIX DETAILS. The details are mentioned for each section identified in the anti-clockwise direction with respect to PICO. A span angle of 180 degree is considered for each scan. Note that all end points of a section are identified as intermediate points.

Limitations and solutions

• Missing corners case .

As shown in the above figure, for a general split and merge algorithm if the index of the first corner that is detected is after another corner point, it would update the initial point to the corner found and miss it. Also, to resolve this it would be required to run the algorithm with the end point to the initial point to identify the missed corners. Followed by which a seperate function would have been needed in order to arrange them in sequence. These requirements are implicitly carried out in the modified split and merge algorithm.

• Fitted line being parallel case

Another major drawback faced was if the line fitted between the starting and end node of a section is searching for a corner point in a line thatś formed by two corner points, which is parallel to the initial fitted line.

Localization

The odometry localization present on PICO is only accurate for a short time due to the wheels slipping. In order to improve the accuracy of the localization, laser rangefinder data is combined with the odometry data.

When the robot starts, it scans its surroundings using the laser rangefinder and stores the found wall positions to a world map with its initial position being the center point. At the start of the main loop, the localization algorithm uses the odometry data to determine its approximate position. it the scans its surroundings using the rangefinder. in an area around the approximated position, the algorithm searches for a position where the wall locations found with the laser rangefinder can be best fitted on the world map. The fitting is done by comparing the wall locations with the world map for multiple locations. A Gaussian filter with a 5x5 kernel is used to create a smoother world map to compare to. The location where the best fit can be made is taken as the current position of the robot. The fitted laser rangefinder data is then added to the world model to expand the map.