The C4 processor receives the current position of the vehicle and its destination. C4 has access to a map of all the roads, and it must find the best route from origin to destination. It uses the A* algorithm.
Latitude and longitude defines position on the surface of a sphere. At distances relevant to urban land transportation, the spherical surface is very closely approximated by a flat map. As in spreadsheet GPS_UBC.xls, we make the conversion by the following equations:
NorthPositionMeters = EarthRadiusMeters * (Latitude – LatOrigin) * π /180
EastPositionMeters = EarthRadiusMeters * cos(LatOrigin) * (Longitude – LonOrigin) * π /180
This approximation is more accurate when close to the origin.
The region in which the vehicle operates can normally be covered by one map. For a wider area, we would have a collection of maps, and select the one with origin closest to the vehicle position.
The name of the map is formed from “Map”, LatOrigin, and LonOrigin. For example:
Map47_7589488_-122_190746.csv
Latitude and longitude of the origin are in decimal degrees, with an underscore replacing the decimal point.
The map collection is stored on an SD card. The card may be associated with the C4 processor, the C6 processor, or the processor handling the user interface. Communication is over a serial line at start-up. Messages are defined in SerialCmd.html. For the present, we will assume that the map file is local to the processor. There are two example maps:
Map47_748949_-122_190746.csv: University of Washington, Bothell
Map47_621300_-122_350900.csv: Seattle Center
A data line in each map consists of:
node,latitude,longitude,PositionEast,PositionNorth,Link1,Link2,Link3,Link4,Dist1,Dist2,Dist3,Dist4
Example data:
8, 47.760342, -122.189784, 71.903, 154.917, 2, 7,15, , 268.441, 142.961, 269.358,
15, 47.757929, -122.189467, 95.599,-113.397, 8,16,18, , 269.358, 26.369, 182.216,
The sample maps give the euclidean distance between junctions, which works on straight paths. More refined data would give the true path length. A future enhancement would be to provide a file Path47_748949_-122_190746.csv which would give the width, speed limit, lane count, and lane marker type of each link between nodes.
File UWB_ODpath.jpg is a graphic of Map47_748949_-122_190746.csv. Nodes 8 and 15 in the example above are on the North Creek Trail at the right edge of the map. The graphic shows the road junctions as black dots. It shows the origin and destination as red dots. Neither the origin nor destination need be on a road. If they are not, the algorithm will compute the shortest distance to a road, and route the intermediate journey on roads.
The Seattle Center data was obtained from Open Street Maps. These maps give latitude and longitude of various features (such as roads and buildings), and assign an 11 digit reference number and a two digit waypoint ID. Processed data is presented in spreadsheet junctions47_6213_-122_3509.xls. This data was used to produce Map47_621300_-122_350900.csv.
Open Street Maps also gives the coordinates of building outlines. It would be possible to construct maps of landmarks, and take distances with a sonar or other ranging sensor. This could be combined with other information to improve localization. See particle filters in S. Thrun, W. Burgard and D. Fox, Probabilistic Robotics, MIT Press, 2006.
There are several popular search algorithms. In simple form, they are often presented on a grid, where a path from the start to the finish must be found that avoids blocked off areas. The optimal path is the shortest. Breadth-first search expands all nodes from the starting node, then expands all successor nodes until reaching the goal. Dijkstra’s algorithm is a variant of breadth first in which successors are selected on contours of equal cost, instead of equal depth. Neither algorithm is optimal computationally, nor guaranteed to find the shortest path.
Most video game search uses the A* algorithm. It is guaranteed to find the shortest path if a solution exists, and will determine if a goal is unreachable. Any other algorithm guaranteed to find the shortest paths will require at least as many node expansions as A*. The A* algorithm differs from the Dijkstra algorithm by not only counting the cost to reach an intermediate node, but also providing an estimate of the cost from the intermediate node to the goal. The estimate (or heuristic) is often the Euclidean distance to the goal.
While A* is an optimal search algorithm, search often eats up major computational resources for games. The trick to achieving faster results is to limit the search space. One way is to mix finer and coarser grids. For paths that must follow roads or sidewalks, the only decision points are at junctions, which reduces the search space considerably.
Source code C4_Planner implements the A* algorithm to find the best path between origin and destinations. It is based on the 2007 DARPA Urban Challenge, which presented vehicles with two files. The Route Network Definition File (RNDF) was a text-based visual map, which was known in advance. The Mission Definition File (MDF) was an ordered list of points that the vehicle was required to visit, and was revealed shortly before the contest. The present software is hard-coded to use Map47_621300_-122_350900 as the RNDF. The MDF is contained in the arrays goal_lat and goal_lon, which are meant to be modified when a new mission is begun. The present code is designed for the Seattle Robotics Society’s RoboMagellan event, which is held at Seattle Center. For that contest, cones are positioned at several outdoor locations. These coordinates are not known until the start of the contest. Coordinates are approximate, and the robot won’t be able to touch the cones without visual localization.
No special hardware required.
Code was lightly tested on Seattle Center data and appears to work.