Embedded Motion Control 2019 Group 9
Group members
Nicole Huizing | 0859610
Janick Janssen | 0756364
Merijn Veerbeek | 0865670
Design Documents
The Initial Design document can be found here: File:Initial Design Group9.pdf
The powerpoint presentation about the design is in this file: File:Presentation initial design.pdf
The powerpoint presentation about the final design can be found here: File:Presentation final design group9.pdf
Logbook
The composition of group 9 has changed twice. At first, George Maleas decided to quit the course at the 13th of May, two days before the escape room competition. At second, Merijn Floren also quit the course on the 3rd of June. The motive for both students was that they lacked the c++ programming skills to meaningfully contribute to the project. Participating in the course would take a too large investment of their time. The code for the final challenge was written by Janick Janssen, Merijn Veerbeek and Nicole Huizing.
Initial design
<<Mostly based on hospital challenge. Some remarks for escape room challenge can be made.>>
Requirements
Janick
Functions
Janick
Components
Janick
Specifications
Janick
Interfaces
Merijn
Escape room challenge
For this assignment, we decided to work out two different strategies. The first one is the wall following strategy and the second one is the direct exit detection strategy. The idea was that the detection, drive control and world model code could be shared between both strategies and that only the planning module would be different. Pico should first try to use the direct exit detection strategy. If he fails to find the exit, Pico should change to wall following strategy. Since the direct exit detection code was not yet robust enough during the escape room competition, the code that was used in the challenge uses only the wall following module.
The table below presents an overview of the most important functionalities that Pico should have to compete in the Escape room competition.
| Wall following | Direct exit detection | Both |
|---|---|---|
| Rotate until aligned to right wall | Detect convex corners when seeing both walls | Walk through the hallway |
| Walk while staying aligned to right wall | Detect convex corner when seeing only one wall | Cope with wall misalignment |
| Detect exit from laser data on right side | Rotate and move to exit entrance point | Stop after reaching exit |
| Rotate to exit entrance | Prevent bumps at all times | |
| Cope with odometry deviations caused by slip |
Code architecture
In the figure below you can find a schematic representation of the code architecture that we used:
World Model
The class 'WorldModel' functions as main storage place for important variables that can be used by all other modules. To enhance separation of tasks among the different modules, all values stored in the World Model are private. The class also has some public functions by which other modules can obtain and process data.
The most important variables in the World Model will be explained here. The first one is the minimum distance to a wall, accompanied by the angle where it is found, relative to Pico. These values are used to allign Pico to the right wall and turn left in a corner. When the shortest distance to a wall is found on the right of Pico, he can walk straight ahead. However, if the angle is outside the margins, Pico will rotate either clockwise or anticlockwise until he is aligned to the wall again. If the shortest distance is found in front of pico, he will turn left until the closest wall is found on the right again.
The other important data structure in the World Model is a matrix, in which the spatial derivatives are saved. By spatial derivative, we mean the difference between the measured laser distance of one array and the next evaluated array. Between these two arrays, 4 arrays are skipped.
Detection
The laser data is called in the World Model and sent to the class 'Perception'. In this class, the laser data is converted to important variables as the minimal distance to a wall, the accompanying angle relative to Pico and the spatial derivative matrix. These values are then updated in the World Model.
In the class 'Monitoring', the output of the class 'Perception' is converted to useful information about the exit. There are two ways of detecting the exit. It depends on Pico's initial position which way should be used. If Pico is standing within the area extended from the two corners of the exit, Pico will see both walls of each convex corner. If Pico is located outside this area, it will only conceive one wall belonging to the closest convex corner and both walls of the furthest convex corner. For both situations, the matrix with spatial derivatives can be used.
First, there will be sought for a "gap" in the spatial difference data. If the difference between the laser distance of one array and the array 4 steps further is above a certain threshold value, the angle of the first array is saved. This evaluation will be done for all arrays. By leaving 4 arrays in between, a real convex corner will have a higher occurrence than a small slit in the wall. A cluster of laser arrays with a high occurrence is expected to be the closest convex corner. Next, the spatial derivative matrix will be scanned for transitions from negative derivatives to positive derivatives. In this way, the furthest convex corner can be found (or both convex corners if Pico is between the corners).
The sideways, forward and rotational motions Pico needs to make to get to the entrance of the hallway are calculated. These values are stored in the World Model.
State manager
Nicole
Controller
Nicole
Wall follower
explanation & movie of simulation & movie of real challenge & discussion on approach - Nicole
Exitfinder
explanation & movie of simulation (do we have something useful?) & discussion on approach - Janick
Hospital challenge
World model
Merijn
Perception
Janicks
Also simulation movie of updating map possible?
Localization
Merijn
Path planning
Nicole
Global path planning
The hospital is divided in structs 'room'. This is done both to preserve the semantics and for practical purposes: it reduces computation time for local path planning and it makes it easy to call information about corners and cabinets in the proximity of Pico. A 'door' is a separate struct. The connections between all rooms and doors are stored in a graph. A vector 'globalPath' is filled by repeatedly choosing the connected door/room with the highest ID-number (if there are more possibilities). ID's that are already in the path are ignored to prevent Pico from walking in a loop. When a doorway is blocked, the connection will be removed from the graph. The route calculated in this way is not per definition the fastest route, but this is not needed to succeed in the challenge.
Each cabinet and each door have a unique enter-gridpoint. These points are used for local path planning. As the "large door" connecting the two parts of the hallway could be partly obstructed, three enterpoints are assigned to this door. If no obstruction is present, the middle node will be used. The enterpoints can be viewed as white circles in the image left below.
Local path planning
To plan Pico's path between enter-gridpoints, use is made of an A* pathplanning algorithm. This method was chosen over Dijkstra's method because it is faster. We will choose a heuristic function that is admissible, so it will be guarenteed that A* will give the shortest path from start to end.
All grid points on the map come with a boolean "accessible". Points that are close to a wall or an object receive the value 'false'. Mapping new objects is also based on this principle: whenever a new static object is detected, the gridpoints it covers will become unaccessible.
After the A* function is called with a certain start-gridpoint and end-gridpoint, it first goes through some checks. These checks assess whether the startposition and endposition are accessible and whether the startposition is the destination itself. If the startposition is unaccessible, the gridpoint itself and the points directly around it are temporarily made accessible (this is needed when Pico accidentally got too close to a wall or object).
Next, the start-node is put in the open list. At every cycle of the loop, the node in open list with the smallest f-value is evaluated. The f-value of node n is the sum of the cost of the path from start to node n and the heuristic cost of that node. This current node is put in the closed list. Next, the eight nodes around it are evaluated. If one of the nodes is the destination, the path from start-node to destination-node is reconstructed. If not, the cost-functions of these nodes are updated and if they are not yet in the closed list, they will be added to the open list from which the node with the lowest f-value will be evaluated in the next loop.
Finite state machine
Merijn
Results
Nicole
The video below shows the performance of Pico in the final challenge. We have to say that this is not exactly the result we hoped for. As can be seen, Pico has difficulties finding the entrance of the first room. This problem is due to an unfixed bug in Pico's localization. Pico manages to determine it's global route to cabinet 0. He starts it's local route to it's first destination: the doorway of the first room. However, he turns a bit too far and heads to the corner of the doorway. Pico is programmed to step back when he is too close to a wall. After reaching a certain backward distance, Pico relocalizes itself and recalculates a path to it's destination. However, due to the problem in localization, he thinks he is outside the zone where he expects himself to be. He now re-takes the old route until he gets too close to the wall again and stays in this loop forever.
Discussion
Janick