Embedded Motion Control 2015 Group 3/Mapping

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.

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. This may later be used to define weighting functions to choose a next corridor.

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

Map&solve algorithm (update?)

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 from. 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. If not, choose the edge where you came from
  • Are there any unvisited edges connected to the current node? In that case, follow the edge straight in front of you if that one is unvisited. Otherwise, follow the unvisited edge that is on your left. Otherwise, follow the unvisited edge on your right.
  • 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, follow the one straight ahead. Otherwise left, otherwise right.
  • 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.

These questions are collected in a cost function. The new direction is chosen with:

   //Done with the mapping process. Now, let's pick a node to go to!
   int minTimesVisited = HUGE_VAL; //future: shortest distance, turn time etc
   double minCost = HUGE_VAL;
   int decidedDirection;   //GLOBAL COORDINATES
   cout << "Current direction: " << currentDirection;
   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 1, 2, 3 and 4 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 a 'nodeWhereICameFrom', 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.

C++ implementation

this->shoud==code; HIER KOMT INTERESSANTE CODE TE STAAN...