Localization and Mapping
The robot does not have direct observation of all the resources at any given time and for this reason it is necessary to equip the robot with the ability to map the locations of previously observed blocks. This map will help the robot to effectively navigate the workspace and aid it in the decision making process.
SLAM (simultaneous localization and mapping) allows robots to build a map of the environment from a sequence of landmark measurements (in our case the resources). Standard SLAM also allows to localize the robot in the workspace, however since the ARMLab is equipped with a Motion Capture system we are only going to focus on the mapping aspect of SLAM.
The original EKF (Extended Kalman Filter) SLAM [1] incrementally estimates the posterior distribution of landmark's positions based on the trajectory of the robot in the workspace. The key limitation of EKF-SLAM is the computational complexity: with each sensor update all landmark locations are updated even if just a single landmark is observed. FastSLAM [2] is an alternative to EKF-SLAM that decomposes the SLAM problem into a robot localization problem (not needed in this case), and a collection of independent EKF estimation problems only for the landmarks that are observed.
SLAM (simultaneous localization and mapping) allows robots to build a map of the environment from a sequence of landmark measurements (in our case the resources). Standard SLAM also allows to localize the robot in the workspace, however since the ARMLab is equipped with a Motion Capture system we are only going to focus on the mapping aspect of SLAM.
The original EKF (Extended Kalman Filter) SLAM [1] incrementally estimates the posterior distribution of landmark's positions based on the trajectory of the robot in the workspace. The key limitation of EKF-SLAM is the computational complexity: with each sensor update all landmark locations are updated even if just a single landmark is observed. FastSLAM [2] is an alternative to EKF-SLAM that decomposes the SLAM problem into a robot localization problem (not needed in this case), and a collection of independent EKF estimation problems only for the landmarks that are observed.
Data association is particularly challenging in SLAM, that is determining which measurement corresponds to which landmark. To deal with this we:
- For each measurement we check the Euclidean distance between the measurement and the current set of landmark position estimates. If it is less than than a threshold δ and the color matches, we consider that that measurement is associated to one of the already seen landmarks. Otherwise we create a new position estimate that corresponds to a new landmark.
- If we find that the measurement is associated to one of the already seen landmarks, we pinpoint to which landmark it corresponds to by finding the position estimate that minimizes the Mahalanobis distance.
References
- Montemerlo, M., FastSLAM: A Factored Solution to the Simultaneous Localization and Mapping Problem with Unknown Data Association, Carnegie Mellon University Pittsburgh, PA, CMU-RI-TR-03-28, 2003.
- Smith, R., Self, M. & Cheeseman, P., Estimating Uncertain Spatial Relationships in Robotics, http://arxiv.org/abs/1304.3111, 2013.