The Wake Up Robot problem
We define the wake up robot problem as follows:
Given:
(i) the robot is provided with the hierarchical atlas for its current environment and
(ii) the robot is located at an unspecified meetpoint in the environment
The robot must correctly localize itself to a node frame in the atlas.
The solution overview
In order to solve this problem, the robot creates a forest of search trees that represent possible meetpoint locations and edge traversals. The robot then traverses any edge to its destination meetpoint. This destination meetpoint is used to eliminate those possible meetpoint locations in the search forest that do not match. Similarly, the traversed edge is used to eliminate non-matching edge possibilities. The remaining possible locations are expanded upon to make an additional row of the search forest.
The robot traverses its second edge, making either a "right" or "left" turn relative to its arrival edge and driving to a second destination meetpoint. The second traversed edge and destination meetpoint are used to eliminate non-matching possibilities in the search forest. In addition, the robot can make use of the turn direction information to eliminate impossible paths in the search forest. The process of path traversing and search forest pruning and expansion is continued in a similar fashion to this second edge.
If at any time the list of possible locations is reduced to one, and the robot has enough information to determine its environment frame's relation to the solution meetpoint's frame, then the localization is completed successfully.
The obvious failing of this procedure is that its completeness relies entirely on the existence of some uniqueness in the environment such that the search forest pruning phase will eventually leave only one possible location. Therefore, it is important to detail explicitly that information which provides the uniqueness in the environment.
Uniqueness in the Hierarchical Atlas
The data encoded in the atlas can be categorized as feature map data and topological data.
Topology
The topological data stored at a meetpoint consists of a single value: the average of equidistant minima distances. Edge angle information is also available, and we plan to make use of that data in the future; for now, the edge angle information is not used. (See discussion here) The topological information for a given edge consists of the straight line coordinate and accumulated travel distances, both of which are derived from dead-reckoning.Feature Maps
The feature map information is stored as two directional edge maps for every GVG edge.
Definitions and datastructures
In order to describe the details of the process, some definitions are required:
Current Search Depth (d) - The matching process phases can be labelled in time by this measure.
Current Match Node(Md) - When the robot is not certain of its location, the node is stored as a Match Node.
The Match Node corresponding to the robot's current location at search depth d is the current match.
Current Match Edge(Ed) - The edge traversed out from the Match Node Md is the Match Edge Ed.
Match Subgraph - The collection of Match Nodes.
Search Node - The datastructure that stores candidate node matches in the search forest.
Search Edge - The datastructure that stores possible paths between the search nodes.
Current Candidate List (dCi) - The row of Search Nodes at depth d corresponding to Md.
Candidate Search Edge (dSEi) - The Search Edge that leads to Search Node d+1Ci.
Search Forest - The collection of rows of Search Nodes connected by Search Edges.
The Algorithm
Building the search forest
We begin at search depth 0.
For every node Ni in the Atlas,
if Ni matches M0 a new Search Node 0Ci is added to the Current Candidate List.
For every edge of each 0Ci of degree j, new 0SEj and 1Ck are created.
Traversing the match edge
At depth 0 the match edge selected to depart E0 is arbitrary.
At depth d > 0 the match edge Ed is selected according to a vote taken from the Current Candidate List.
Note that for d > 0, Ed has a relative turn direction (R or L).
The robot traverses the edge and arrives at the destination meetpoint Md+1.
Md+1 becomes the Current Match.
The Current Candidate List is updated to the row of the Search Forest at depth d+1.
The Current Search Depth d is set to d+1.
Updating the Search Forest
For every dCi,
if dCi does not match Md OR
if d-1SEi is mismatched with Ed-1 OR
if d-1SEi has a mismatched turn direction with Ed-1
then dCi is marked as inactive.
If the Current Candidate List contains a single active element dCi,
then Md is actually dCi and Ed-1 is actually d-1SEi, and the localization is completed successfully.
If the Current Candidate List contains more than one active element,
For every active dCi,
For every edge of dCi, new dSEj and d+1Ck are created.
Proceed to "Traverse the match edge" step.
A Simple Illustrative Example
Search Depth 0:
The robot is placed in the environment represented by the atlas above, with nodes {0,1,2,3}. Nodes 0 and 1 are indistinguishable, as are nodes 2 and 3. The robot drives to node 0, is given the atlas, and is instructed to localize. The environment and initial state of the Match Subgraph and Search Forest are shown below. The robot drives out on edge 0 of node 0.
Search Depth 1:
The robot arrives at edge 0 of node 1. Node matching is used to eliminate Search Nodes for candidates 2 and 3. Edge matching is used to eliminate Search Nodes for candidates 1 and 0, shown below with the mismatched distances marked.
The remaining active candidates at depth 1 are expanded upon, creating candidates at depth 2. The votes for turning left or right are collected from the active candidates at depth 1. For this example, let us say the result is to turn right (R). The robot departs on edge 1 of node 1.
Search Depth 2:
The robot arrives on edge 2 of node 0. The node and edge matching eliminate candidates 3 and 2. The edge turn direction eliminates candidate 1. Since only one active candidate remains at depth 2, the robot is located at that candidate node (in this case 0). The robot now has the knowledge of the true nodes and edges that it has traversed from the beginning of the procedure. This knowledge can be used to update the Atlas.
