Bearings-only simultaneous localization and mapping (SLAM) is a particularly challenging problem for autonomous mobile robots. The typical approach is to apply the extended Kalman Filter (EKF) to linearize the relationship between robot pose and landmark observations, send the robot off on its merry way and sit back and pray that the filter doesn't diverge due to the highly non-linear observation function.
This work approaches the problem of bearings-only SLAM by taking exploratory trajectories that optimize the stability of the measurement update while maximizing coverage of the unexplored space. These movies demonstrate that the robot can successfully map an unknown environment using an appropriate trajectory, even in the presence of significant observation noise (up to 10 degrees standard-deviation, normally distributed noise).
The exploratory trajectories are as follows:
- Straight: The robot takes a straight-line path to successive landmarks. A very bad strategy.
- Random: The robot follows a random trajectory. The map converges, but the robot fails to explore unknown space.
- Spiral: The robot follows a spiral-like path to each successive landmark, attempting to encircle it and thereby localizing it with high accuracy. When every landmark has been visited, the robot returns to the start and refines the map by continuing to visit each landmark successively.
In all of these runs, the robot has a field of view of 500m. The width of the environment is 2000m. The ground-truth landmarks and robot pose are in blue, the estimated landmarks are in red and the robot's estimated trajectory is yellow. Ignore the wide sweeping red arcs, which correspond to uncertainties associated with undiscovered landmarks (these are distributed at random throughout the environment to drive exploration). Check back here in the future for images that have better contrast.
Low observation noise (normally distributed 1 degree std-deviation)
Click each image for an mpeg of the exploration. |
||
High observation noise (norm. dist 10 degree std-deviation)
One more movie, with 5 degree std-dev error and 40 landmarks.
Discussion
The jaggedness of the trajectories, particularly in the high-noise case reflects the highly unstable localization estimate. Despite this fact, in the spiral case, the robot is able to successfully navigate to each landmark in the environment. It should be noted that mapping accuracy can only be measured relative to a global translation and rotation of the estimated map-- hence the reason why the spiral map in the noisy case is 'correct' but offset from the true landmark positions. Notice that the 'Straight' strategy diverges almost immediately.Note that not only does the spiral-based approach successfully converge, but it also successfully closes the loop, a notoriously difficult problem in SLAM.
This results have important implications for a variety of exploratory applications, and in particular vision-based SLAM, where range cannot be directly observed. Clearly, in real-world applications there are a slew of other issues: imperfect data association, the presence of obstacles, etc. However, these results demonstrate that it is valuable (and important) for an exploring robot to collect observations in a principled manner.
Robert Sim, Last modified: 14 May 2005