Embedded Motion Control 2015 Group 3/Mapping
This page is part of the EMC03 CST-wiki.
Mapping block
The mapping block contains a very high-level model of the world. The mapping has been created in such a way that only essential information is stored, in order to create a very flexible and modular world model.
For solving the maze, a variant of the Tremaux algorithm is used. The Tremaux algorithm is an implementation of DFS (Depth First Search), which proves to be an efficient way of solving a maze with minimum backtracking.
Because the Localization block did not work in the end, and as such loops could not be resolved, a Random Walk strategy was used, so that at least the maze would (probably) be solved, and the capabilities of the other blocks could be shown.
World model structure
The maze will consist of nodes and edges; i.e., an undirected graph. A node is either a dead end, or any place in the maze where the robot can go in more than one direction. An edge is the connection between one node and another. An edge may also lead to the same node. In the latter case, this edge is a loop. The algorithm is called by the general decision maker whenever the robot encounters a node (junction or a dead end). The input of the algorithm is the possible routes the robot can go (left, straight ahead, right, turn around) and the output is the direction that is advised, based on the Tremaux algorithm.
Since the maze is axis-aligned, a simplified coordinate system can be used, which only has four principal directions (in simple terms, 'Up', 'Down', 'Left', and 'Right'). Although the node/edge structure can in principle work for a non-axis-aligned maze, the current implementation has some methods specific to an axis-aligned maze, to reduce the complexity of the implementation. This is expressed in two ways: it is assumed that Pico can only drive in any of the principal directions, and that the junctions can only be of specific formats, namely (from Pico's perspective): T-junction ╦, left junction ╣, right junction ╠ four-way intersection ╬ and dead end ╥ .
For each node, the following information is stored:
- Position. The position is used to identify and close loops within the maze, by matching a new node with a previous node. The position is defined in global coordinates. To obtain the global coordinates, we will use the localisation class.
- Adjacent corridors. Since the maze is axis-aligned, there can be anything between one (dead end) and four (cross-intersection) corridors/edges leading to a node. Because of this, the corridors are stored in an array with each element corresponding to a (global) direction.
For each corridor/edge, the following information is stored:
- Number of times Pico has traversed a corridor. This is important for Tremaux algorithm, which will be explained later.
- Travel time for a corridor. This can be used to give priorities in case multiple options are present.
In the implementation, the graph is stored in a BGL (Boost Graphing Library) Graph object. This means that most of the overhead of maintaining an undirected graph is done by an existing library. The library used is also extremely extendable by means of Bundled Properties, which facilitate an arbitrary number of properties for nodes and edges. The main disadvantage is the syntactic overhead generated especially because BGL is so extendable, although the overhead is mostly contained into making the basic graph objects, which only has to be done once.
Schedule
The schedule looks like this:
- Updating the map:
- Robot tries to find where he is, located in global coordinates. Now it can decide if it is on a new node or on an old node.
//Where is the node located?
cv::Point2d nodePosition = getNodePos();
//Check if we're going to an already known node (in which case we won't do any matching)
if(nextNode == noNode){
// We do not know anything abbout this node yet, so let's see if we can match it with another one.
//Iterate over all nodes, and pick the closest.
double minDistance = HUGE_VAL; //By definition, more than the largest double you'll ever get.
Node match = noNode; //Initialize to noNode, so we can check if there was no match.
NodeIt node, lastNode; //Set up for the for loop iterator magic.
for(tie(node,lastNode) = vertices(maze); node!=lastNode; ++node){ //Iterator magic.
//Skip checking the distance if the node we're checking is an undiscovered node.
if(*node!=this->noNode)
{
double distance = cv::norm(nodePosition-maze[*node].position); //Calculate distance (2-norm)
if(distance<SAME_NODE_TOLERANCE && distance < minDistance){
match = *node; //Set the match to the close node we found.
minDistance = distance; //Set the new minimum for the closest node.
}
}
}
if(match==noNode){
// Previous loop did not result in a match, so: new node encountered! Let's create it.
match = add_vertex(NodeInfo(nodePosition),maze);
this->nextNode = match; //Now we know where we were going to (was noNode because we didn't know before)
//Initialize each corridor to default (we don't know yet where they lead)
Corridor defaultCorr = add_edge(nextNode,noNode,maze).first;
for(int i=0; i<4; ++i){ //Iterate through all four possible directions for a node and set them to default.
maze[nextNode].corridor[i]=defaultCorr;
}
// But we do know where we came from, so set the edge from whence we came.
this->currentCorridor = add_edge(this->prevNode,this->nextNode,maze).first; //Make a new edge
maze[prevNode].corridor[this->prevNodeExitedAt] = this->currentCorridor; //Update edge of previous node
maze[nextNode].corridor[flipD(this->currentDirection)] = this->currentCorridor; //Idem for next node. (D+2)%4 = flip direction.
} else {
// Revisted old node.
this->nextNode = match;
maze[nextNode].corridor[flipD(this->currentDirection)] = this->currentCorridor;
}
- The robot figures out from which node it came. Now it can define what edge it has been traversing. It marks the edge as 'visited once more'.
- All sorts of other properties may be associated with the edge. Energy consumption, traveling time, shape of the edge... This is not necessary for the algorithm, but it may help formulating more advanced weighting functions for optimizations.
- The robot will also have to realize if the current node is connected to a dead end. In that case, it will request the possible door to open.
//Above process updated nextNode to be a proper node in our system. Let's update the corridor. this->maze[this->currentCorridor].timesVisited += 1; this->maze[this->currentCorridor].travelTime = scanvars.timeStamp - this->lastTimeStamp; this->lastTimeStamp = scanvars.timeStamp;
- Choosing a new direction:
- Check if the door opened for me. In that case: Go straight ahead and mark the edge that lead up to the door as Visited 2 times (not currently implemented due to previous concerns in the door detection robustness.). If not, choose the edge where you came from
- Are there any unvisited edges connected to the current node? In that case, follow any unvisited edge.
- Are there any edges visited once? Do not go there if there are any unvisited edges. If there are only edges that are visited once, any edge that is visited only once.
- Are there any edges visited twice? Do not go there. According to the Tremaux algorithm, there must be an edge left to explore (visited once or not yet), or you are back at the starting point and the maze has no solution.
- Above points can be summarized as 'pick the least visited edge'. If formulated this way, the algorithm will also allow for 'accidents', e.g., an edge visited more than two times, which should definitely be avoided, and will never stop exploring the maze even if it thinks it is back at the starting point, which improves robustness.
- In case there is more than one edge an option according to Tremaux, a cost function will determine the best option to follow. This cost function penalizes turning over going straight ahead, severely penalizes U-turns, and incorporates the travel time of known edges (shorter being better, since that will explore as much options as possible in a given time). The cost function could be extended by analyzing the entire maze as a whole, to determine which direction will explore as much of the maze as possible in the shortest time. Although currently not implemented, this again shows a huge advantage of using an existing graphing library, since graph traversal is built-in in the library.
//Done with the mapping process. Now, let's pick a node to go to!
int minTimesVisited = HUGE_VAL;
double minCost = HUGE_VAL;
int decidedDirection; //GLOBAL COORDINATES
for(int direction = 0; direction<4; ++direction){
//check if this direction is actually possible
if(isPossibleDirection(type,direction)){
int thisCorridorTimesVisited = maze[maze[nextNode].corridor[direction]].timesVisited;//Boost can only access properties of edges and vertices as maze[edge] or maze[vertex]
if(thisCorridorTimesVisited <= minTimesVisited){
if(thisCorridorTimesVisited<minTimesVisited) { minCost = HUGE_VAL;}
minTimesVisited = thisCorridorTimesVisited;
double cost=getCost(direction);
if(cost<minCost){
decidedDirection = direction;
}
}
}
- Translation from chosen edge to turn command:
- The nodes are stored in a global coordinate system. The edges have a global direction pointing from the node to the direction of the edge. The robot must receive a command that will guide it through the maze in local coordinates. Globally, we can assume that there are only four possible directions because of the axis-aligned maze: North, west, south and east. These directions are represented by the integers 0, 1, 2 and 3 respectively in our code.
- The actual command is formulated
int decision = (decidedDirection - this->currentDirection + 4)%4; //From global to local coordinates return static_cast<WhereToNow>(decision); //Cast from int to enum.
- A set-up is made for the next node
- e.g., the current node is saved as 'prevNode', so the next time the algorithm is called, it knows where it came from and start figuring out the next step.
Tremaux Algorithm
Tremaux' algorithm is an implementation of Depth-First Search. It is best visualized as walking through the maze with a big bucket of paint and a paint brush. Continuously, the maze solver will drag the brush behind him, creating a line on the floor. When an intersection is encountered, the solver will see if any corridors do not have a line of paint on the floor. He will take one of those corridors, since he hasn't explored that bit of maze yet. At some point, the solver will have no unpainted corridors to go to, so he will have to backtrack. At that point, a corridor will have two lines on the floor, which indicates that not only has the solver been there, but there was also nothing left for him to explore there. As such, corridors with two lines on the floor should be avoided, and corridors with no lines on the floor should be preferred.
A simple video of how this should be visualized can be found here.
In our implementation, we chose not to equip Pico with a bucket of paint, but instead use the world model we created. Pico will update an integer for each corridor which indicates the number of times he visited a certain corridor. Then, Pico simply prefers the corridor with the least amount of visits. In theory, this should be always 0, 1 or 2 times (with 0 times preferred over 1 time, and 2 times should be generally avoided). However, we decided to also allow for integers greater than 2, in case the world model has miscounted something - in this case, 2 times is preferred over 3 times, because the latter means that a certain area is definitely explored more than necessary.
Furthermore, we extended Tremaux with a priority model. In principle, Tremaux does not describe to do when there are multiple possibilities - it does not matter for solving a maze if time is not an issue. We would however like to quickly explore as many nodes as possible, so instead of just picking a random direction, we pick the direction which minimizes the travel time to a next node. For example, turning around is generally not preferred, and the shortest corridor to the next node is chosen if at all possible.
This can in theory be extended with a more elaborate searches. For example, to try not to go to a node that will lead to only twice-visited edges, which should be perfectly possible by using some built-in algorithms from Boost. However, this is not implemented yet.
Random Walk
To detect and close loops (which were likely to be present) in the maze, the Mapping block needed information from Localization, which was not functional in time. Wall-following is not a viable option if the robot starts in a loop (which was in fact the case in the A-Maze-ing challenge as it turned out), so an alternative strategy must be employed. The strategy chosen is random walk. This strategy will solve any (static) maze with probability [math]\displaystyle{ 1 }[/math] as [math]\displaystyle{ t\to\infty }[/math]. Of course, we do not have infinite time, but trying to optimize this strategy in any way (e.g., try and avoid going in one direction all the time) could mean that it will never solve the maze, since aforementioned property only hold for a truly random walk. As a bonus, there is also a certain probability that it will optimally solve a maze regardless of its properties, which was in fact the case for our maze challenge run.
Since the software for the mapping was almost completely decoupled from the rest, the random walk could be implemented easily. It will still get an intersection type from the Decision Maker, and will still return a decision. It was decided not to have U-turns in the random walk (except for dead ends, of course), since it's trivially shown that a U-turn can always be achieved with a combination of other decisions, and U-turns would in general severely impede progress through the maze.
Throughout the course, it was always a balance act between decoupling and ease of implementation. In this particular case, ease of implementation was favoured, since the random walk block merely tries to solve the maze and highlight (successfully so) the capabilities of all other blocks. Furthermore, this block was focused on robustness; as such, even though in the below example, case 2: could be used, default was chosen to eliminate the risk of accidental bugs.
srand (time(NULL)); //Initialize random number generator
int direction =0; //Initialize direction
switch(type){
case FourWay:
direction = rand()%3;
switch(direction){
case 0:
return Left;
break;
case 1:
return Forward;
break;
default:
return Right;
break;
}
break;
case LeftJunction: //etc