Adaptive Monte Carlo Localization is a probabilistic localization system for robots moving in two dimensions, developed by Thun et al. [39]. The system implements a set of probabilistic localization algorithms in order to solve the localization problem.
This section aim to describe how the algorithm works.
Monte Carlo Localization (MCL), also known as particle filter localization, is an algorithm commonly used for robots to localize using a particle filter. The algorithm uses a given map of the environment to estimate the position and orientation of a robot as it moves and senses its environment. A particle filter is used to represent the distribution of likely states, where each particle is representing a possible state, i.e., a hypothesis of where the robot is. The algorithm can either be initialized with an initial position estimate defined by the operator, or with a uniform random distribution of particles over the whole configuration space, meaning the robot have no information about its initial position, and assumes it is equally likely to be at any point in the environment. Whenever the robot moves, the algorithm shifts the particles to predict its new state after the movement. When the sensors on the robot senses something, the particles are resampled based on recursive Bayesian estimation, i.e., how well the predicted state correlate with the actual sensed data. Ultimately, the particles converge towards the actual position of the robot.
The state of a robot depends on the application and design. For example, the state of a two dimensional robot typically consists of a tuple(x, y, θ) for position x, y and orientation θ. The estimate of the robot’s current state is a probability density function distributed over the state space. In the MCL algorithm, the estimate at time t is represented by the setXt={x1t, x2t, . . . , xMt }. Each particle contains an estimate of the robots state, and the regions in the state space with many particles correspond to a greater possibility that the robot will be there.
Additionally, the MCL algorithm assumes theMarkov property, that is, the current state’s probability distribution only depends on the previous state and not states
before that, i.e. Xt only depends on Xt−1. This means that the algorithm only works if the environment is static and does not change over time.
Given a map of the environment, the goal of the algorithm is to determine the robots pose within the environment. At each timet, it takes as input the previous estimateXt−1={x1t−1, x2t−1, . . . , xMt−1}, an actuation commandut, and sensor data zt, and outputs the updated estimateXt(see algorithm 2).
Algorithm 2 M CL(Xt−1, ut, zt)
Monte Carlo localization can be improved by adaptively sampling the particles based on an error estimate using theKullback-Leibler Divergence (KLD). Initially, the algorithm requires a large sample sizeM in order to cover the entire map with a uniformly random distribution of particles. However, maintaining such a large sample size when the particles converges is computationally inefficient. AMCL (also known as KLD-sampling) is a variant of Monte Carlo Localization where the sample size, Mx, at each iteration is calculated such that, with probability1−δ, the error between true posterior and the sample-based approximation is less then the variable . The idea in AMCL is to create a histogram overlaid on the state space. The bins are initially empty, and at each iteration, a new particle is drawn from the previous weighted particle set with a probability that is is proportional to its weight. Instead of the resampling done in MCL, the adaptive MCL algorithm draws particles from the previous weighted set and then applies sensor and motion updates before placing the particles into their bins. The algorithm tracks the number of non-empty bins,k, and if a particle is placed in a previously empty bin, the value ofMxis recalculated. This process is repeated until the sampleM is the same asMx.
Since AMCL removes redundant particles from the particle set by only increasing Mx when a new bin has been filled, it consistently outperforms and converges faster the classic MCL.
Navigation Stack Development
7 | Specifications and Require-ments
7.1 Specifications
Specifications for the navigation stack have been selected based on the problem description on page i.
1. The navigation stack must be implemented in ROS as a stack of ROS nodes.
2. Communication with other ROS nodes must use ROS protocols.
3. The navigation stack must provide the navigation module presented in section 3.3.1 with location updates on a ROS topic with message type geometry_msgs/PoseWithCovarianceStamped.
4. The navigation stack must provide an implementation of an action interface that interfaces the navigation module. Specifically:
(a) The ActionServer must take in goals containing geometry_msgs/PoseStampedmessages.
(b) The ActionServer must provide feedback containing the current position of the Cyborg in the environment.
(c) The ActionServer must provide status information on the goals that are sent to navigation stack.
(d) The action interface should enable the navigation module to cancel goals.
5. The navigation stack must provide localization functionality as a ROS node.
6. The navigation stack should utilize a map of the environment, provided by a ROS node.
7. The navigation stack must include a ROS node interfacing the peripherals on the Cyborg.
8. The navigation stack must include a navigation controller performing path planning and obstacle avoidance. Specifically:
(a) The navigation controller must output velocity commands on a ROS topic.
(b) The navigation controller should include recovery functionality for when the Cyborg gets stuck.
(c) The navigation controller should utilize local and global costmaps to perform local and global path planning.
9. The navigation stack must include a ROS node interfacing the robot controller on the Pioneer LX base.