Embedded Motion Control 2017 Group 5
This page contains the progress of the development of the PICO maze controller for the course 4SC020 Embedded Motion Control. The objective of the project is to design a controller for PICO, a small robot equipped with a holonomic base, wheel encoders and a laser range finder (LRF). This controller shall enable PICO to autonomously navigate through a random maze without bumping into walls. The designed system recieves data from the wheel encoders and the LRF and converts these to movement commands for the actuation of the base.
Members Group 5
| Name | Student ID | |
| Torben Beernaert | 0778690 | t.f.beernaert@student.tue.nl |
| Rodrigo Estrella Treviño | 1035228 | r.estrella.trevino@student.tue.nl |
| Kagan Incetan | 1037760 | k.incetan@student.tue.nl |
| Sjoerd Knippenberg | 0738104 | s.c.m.knippenberg@student.tue.nl |
| Angel Molina Acosta | 1036456 | a.molina.acosta@student.tue.nl |
| Bart Vercoulen | 0747283 | b.c.g.vercoulen@student.tue.nl |
Requirements
The requirements for the system are deducted from the course page and established from a brainstorm session. The FURPS model is used to group the requirements together in the following aspects:
- Functionality
- Usability
- Reliability
- Performance
- Supportability
A complete list of all requirements can be found in the requirements overview.
System Architecture
This section describes the development of the system architecture. First, the translation from the established requirements to functions the system should perform is covered. These functions are assigned to separate subsystems and the information flow between these subsystems is specified.
Functionality
From the requirements list several functionalities can be extracted, which are arranged using the Task-Skill-Motion hierarchy. In this hierarchy one main task, the most abstract and all-embracing functionality, is specified. In this case the main task is to exit the maze. To perform this task a combination of several subordinate functionalities, the skills, is necessary. Finally, the motions are one level deeper than the skills and contain basic functionalities.
Task
- Exit maze
Skills
- Observe environment
- Determine action
- Manoeuvre PICO
Motions
- Process sensor data
- Maintain world map
- Request open door
- Turn base
- Drive forward
The relationships between functionalities can be classified as "include" or "extend" types, which complete the hierarchy. An include relationship indicates that the functionality at the arrow tail is a necessary part of the arrow head function, while an extend relationship means that the arrow tail functionality can be a follow-up action of the one on the arrow head.
Subsystems
The previously introduced functionalities are grouped together to create the subsystems WorldMap, ActionManager and MovementController. Every subsystem will be responsible for executing any of these functionalities. The figure below shows that the main system is composed of these three subsystems and the functionalities they are mapped to:
Internal flows
Finally, the information flows between the subsystems are specified. The incoming sensor data is gathered in the WorldMap module, where it is converted to relevant information about the current situation and position of the robot. The situational information is then sent to the ActionManager, which will decide the upcoming action based on the situation (straight corridor / corner / T-junction etc.). The MovementController will then receive this command from the ActionManager and perform the manoeuvre using raw and processed position data provided by the WorldMap. The diagram below shows this flow of data through the system:
Initial Design
The Initial Design document can be found here: Initial_Design_Group5.
Corridor Challenge
Plan & Expectations
The strategy for the corridor challenge was to drive forward as fast as possible with potential field as controller to keep PICO in the middle of the corridor. While driving through the corridor the sides are continuously checked for gaps using ranges of laser data to the left and right of PICO. These ranges are tuned during experiment sessions. When a gap is detected PICO stops for a moment and has to turn 90 degrees to the side of the gap.
The initial idea was to use the odometry data for this purpose, but from testing results it seemed that this data was not reliable enough to let PICO turn. For this reason it is chosen to implement turning that is based on time delay using a for loop (see below). The amount of iterations is also tuned during testing sessions. When the turn is completed PICO will drive forward again as we did before the turn.
Pico start turning
For x iterations (where x is around 25000)
Keep turning
Pico stop turning
Results & Discussion
During the corridor challenge driving forward controlled with the potential field worked as we expected. There were some minor side movements which could be tuned for the future, but overall the potential field worked fine. The gap was detected and also at the right time, that is when PICO was standing in front of the gap. PICO stops and started turning, but it never stopped turning during the corridor challenge.
This failure was caused by the for-loop delay, the time PICO had to turn. This delay was tuned in the simulator, but on PICO it did not work as simulated. We learned that PICO does not behave exactly as in the simulator, and that a for loop delay is based on CPU time, which is depending on the computer or state of the computer. Also turning based on a delay is not as robust as using the sensor data. For the maze challenge we have to improve the turning.
Maze-solving Algorithms
Wall follower
This algorithm is very simple to implement since only one rule applies: Follow the wall on either your left or right side (depending on the setting). This wall will be tracked until eventually, an exit is found. The algorithm is guaranteed to work when the robot starts at the entrance of a maze. However, if PICO starts at an arbitrary position it might get stuck in a loop, keeping track of the walls of an ‘island’. Since the case description states that the robot will be start from an arbitrary position and that the maze will contain a loop, this algorithm is risky to use and might not be able to guide the robot to the exit.
Pledge algorithm
The Pledge algorithm is very similar to the wall follower, but contains an upgrade to enable the robot to break free from isolated islands. The robot is given a preference direction (north, south, east or west) and follows this heading until it bumps into a wall. Then, an arbitrary (left or right) wall follower starts and keeps track of the orientation of the robot. This wall follower stops when the robot is heading towards the preference direction again and the orientation is zero degrees. Note that for this case, zero degrees is not equal to 360 degrees. Special care has to be taken when the maze is not made up of straight-angled corners, which is luckily not the case in this specific application. The Pledge algorithm is still fairly simple and guaranteed to find the exit of the maze.
Trémaux algorithm
The Trémaux algorithm is a more efficient, but also more complex algorithm. This algorithm keeps track of where PICO has already been by marking paths between junctions. When a junction is detected, a decision is made based on the markings that are already present on the other paths of the junction. The increased efficiency of the algorithm imposes more dependency on the detection of maze elements and maintenance of a world map.
World Skills
Filtering
To be able to process the laser data for detecting gaps, aligning or detecting doors, it is filtered first. Consecutive points have to be within a certain distance from each other, otherwise the points will be ignored. Furthermore, this filtering is used to look at a certain bundle of beams between two specified angles as shown in Figure WS1. Also the maximum length of a beam can be specified. When a point is within these boundaries it is considered as a valid point. This valid point is than stored as a coordinate.
Struct coordinate{
x double;
y double;
orig_r double;
orig_angle double;
}
Alignment
Guiding PICO to the exit of a maze needs a proper alignment according to the walls. Since the obtained odometry data are not as precise as in the simulations, the robot is aligned after each 90 degrees turn that is made. Important to mention is that the first thing that PICO does when the controller software is started is aligning itself. This is done because the exact position and orientation of the robot in the maze is not known beforehand.
For determining the alignment with respect to walls of the maze, the laser beams of both the left- and right side of PICO are used. The ranges that are used for both sides are from XX to XX degrees on the left and from XX to XX degrees on the right, as shown in Figure filtering.
Using the Pythagorean theorem, the corresponding distance ‘d’ is calculated for each of the laser beams in the range, using its angle and length. When these distances are determined, the difference between the mean distance of the beams in front of PICO and the ones behind PICO are subtracted. The sign of the value that comes out of this comparison indicates the direction in which PICO has to turn for proper aligning with respect to the walls of the maze. For example, when the compared value of the laser beams on the right side of PICO has a positive sign, the robot has to turn in …tive direction. As clarification, Figure WS2 is added in which the example as explained before is pictured.
AFBEELDING XPYTHdX WS2
It can be the case that the robot is in an open space and only notices a wall on one side of the robot. Since PICO is able to align using only one wall, this is not a problem at all. In this case, the range of laser beams on the side where no wall is detected is neglected. The part of the designed controller that is responsible for the alignment of PICO can be found here.
The amount of alignment that is needed for driving through the maze safely, without bumping, is set to a fixed value that is tuned during the testing sessions. Below in Figure WS3 it is shown how PICO aligns between two walls of the maze.
Side Free Left/Right
One of the basic skills that is implemented in the code of the robot controller is detecting if a side is free or not. When a side is free, the algorithm as used in the action manager decides if PICO drives into the free direction or continues its path. For the detection if one or both of the sides of PICO are free, the laser ranges from XX to XX degrees on the left and from XX to XX degrees on the right or the robot are used, as indicated in Figure WS1. For the detection of the free sides of the robot, a range of XX meters is used. Laser beams with an angle or length outside the specified ranges will not be taken into account by PICO.
Door detection
Essential for finding the exit of the maze, a door located in a dead end of a corridor or in a wall has to be opened. In Figure WS4, a graphical representation of both these situations is shown. When PICO has detected a dead end, a bell will be rung and the door in the maze will be opened within a time of five seconds. Worth mentioning is that when PICO has opened a door in the maze, it will not ring a bell again when it reaches a dead end or detects a fake door in the side of the walls of the maze.
Front
When PICO has detected a door in the front and the bell is rung, the timestamp of the laser data corresponding to the moment where the dead end is detected is saved. This timestamp will be checked continuously until the before mentioned five seconds have expired. After these seconds, PICO will check if the front is free, indicating the door is open and the maze solving algorithm will continue. When PICO detects that the ‘door’ has not been opened and that the dead end is still a dead end, the algorithm will continue looking for a door.
Sides
Next to a door that is located in a dead end, a door will be detected in the walls of the maze. This detection is implemented using laser data ranges on both sides of PICO. These ranges are [XX,XX] on the left of the robot and [XX,XX] on the right side. Using the laser beams and the Pythagorean theorem as described in the Alignment section, distance ‘d’ is calculated for recognizing so-called vertical walls of the maze. A vertical wall is defined as the set of consecutive laser data points that have a value for ‘d’ within a certain uncertainty limits. When a distance ‘d’ is calculated that is not within these limits, PICO is recognizing a horizontal wall. For these horizontal walls, the same idea is applied as for the vertical walls, only now for determining distance ‘k’ using the laser beams and the Pythagorean theorem. In Figure WS5, a schematic overview of both the horizontal and vertical walls and distanced ‘d’ and ‘k’ is shown.
AFBEELDING DOORSd&k WS5
When PICO detects a horizontal wall after it is detecting a vertical wall, a corner is identified. This also counts when PICO detects a vertical wall after a horizontal wall. When four corners are detected at the same time, as shown in the figure above, a possible door is detected. For this detection of the four corners, a range of the length of the laser beams is set.
After detecting the four corners of a possible door at the side of PICO, it will continue driving forward until it recognizes two corners only. This means that corners X and X are out of range of the laser beams used for door detection and that PICO is positioned in front of the door.
After PICO has positioned itself in front of a possible door, a bell is rung and the same steps will be performed as for the front door detection. When PICO after five seconds recognizes the door has not been opened, and therefore is not a door, it will continue the maze solving algorithm. In the case where the door has been opened, PICO will notice the door no longer as a door but as a gap. Next, the maze solving algorithm will turn the robot towards the opened door, commands it to drive through it and continues solving the maze.
Unused functions
The functions as described below are not used in the final code for the maze challenge.
Wall detection
The first approach to process the laser data was to extract walls from it. This was done by looking at consecutive points who are close enough to each other to be the same wall. With this function we could detect separate walls when the distance between points were large enough. FIGURE XX This way of detecting walls neglects the corners. Therefore we tried to calculate the direction between two consecutive points to determine if it is one straight wall. The implementation of this part has never worked as it should. A reason could be that the noise between two consecutive points are to much at some point.
FIGURE XX.
Corner detection
To identify the corners which could not be detected by the initial wall detection we implemented the following algorithm. Take a range of a certain amount of consecutive points. Draw an imaginary line between the first and the last point of the range. Calculate for each point in between the lateral distance to the imaginary line. When the distances are below a threshold there is no corner in this piece of wall. When there is at least one distance larger than the threshold it is a wall. If there are more than one larger distances the furthest point is the corner point. When there is no corner detected in the current range the range is shifted one point further. When an actual corner was detected the next range begins with the corner point.
This approach seemed to work for some corners, but the corner points were to unstable. Sometimes it did not see corners when PICO was really close to the corner. We tried to tune the algorithm but in the end we stopped using it.
Meetings Overview
Brief summaries on the contents of team meetings are available here: [1]