Blog

Project Story One

Mapping and Adaptive Monte-Carlo Localization

Enabling the identification and navigation components to work successfully requires an accurate map of the environment around the robot. To do that, our team is using Adaptive Monte-Carlo Localization implemented through the neato_robot ROS package built by Mike Ferguson. This ROS package was ultimately chosen for its ease of implementation on the Neato robots as opposed to rolling our own AMCL implementation.

Essentially, Adaptive Monte-Carlo Localization is a type of particle filter localization wherein a robot uses an internal map of its environment to help it figure out where is is within the map based on moving around. Determining its location and rotation (more generally, the pose) by using its sensor observations is known as robot localization.

In the process of localization, the robot randomly generates guesses of where it is going to be next. These guesses are known as particles and each of these particles contains a description of one possible future state for the robot. As it observes the environment around it, the robot discards any particles that might be inconsistent with the new observation and then generates more particles closer to those that appear consistent. In the end, the particles should converge to the actual position of the robot as in the diagram below.

Adaptive sampling is used to improve the sampling of particles for Monte-Carlo localization. In this case, neato_2dnav uses a KLD-sampling method based on an error estimate made using Kluber-Leibler divergence. Using a KLD approach creates a grid (a histogram) overlaid on the state space where each bin in the histogram is initially empty. At each iteration, a new particle is drawn from the previous (weighted) particle set with probability proportional to its weight. Instead of the resampling done in classic Monte-Carlo localization, the KLD–sampling algorithm draws particles from the previous, weighted, particle set and applies the motion and sensor updates before placing the particle into its bin. This helps to improve the sampling of the algorithm and, by extension, the speed and accuracy with which the robot is able to localize itself. Implementing Adaptive Monte-Carlo Localization on the PINbot enables us to focus our efforts on unique and interesting identification and navigation implementations while ensuring that our perception is fast and reliable.

Project Story Two

An Adventure into Point Clouds

An important part of our project is the identification of specific items in the room for our path planning. By locating object positions in our map, we could send the required data needed for our path planning node to function. The goals of this were then to receive a .yaml map file, convert the map to a point cloud, and run a processing algorithm.

To achieve these goals we worked on three different processes in parallel, that will be the focus of this project story.

Maps!

From the mapping node we received .yaml and .pgm files. We decided to build our processing architecture in the context of point clouds instead of pixel maps, so we wrote a short script that used the .yaml file properties to convert our map to an (x,y) coordinate list shown in Figure 1.

Clustering

In the interest of accuracy and efficiency of our map processing we decided to implement a point-clustering algorithm. The goal was to be able to locate the center of a cluster and target our template-matching to a small area around the center. This improves the efficiency of the algorithm, since we don’t need to search through a mesh of the entire map area. It also improves accuracy since focusing around cluster-centers helps avoid false positives.

The clustering algorithm is pretty straight forwards. The processed map is sent as an array of (x,y) points and a clustering algorithm is applied onto the points. We implemented DBSCAN (Density Based Spatial Clustering of applications with noise). Figure 2 shows all the different clustering algorithms applied to a set of points. DBSCAN was used because of its ability to identify separate objects within close quarters.

Map Clustering

The map is then split up into separate clusters, and sent over to an object identification algorithm. Figure 3 shows the raw map comparison to the clustering algorithm. The DBSCAN effectively highlights the circle, sofa, and walls and filters out noisy particles. The node sends the array to the object identification algorithm to be processed further. One decision that was made was to keep all marker visualization on one node rather than having the particle clustering node and object identification node processing the different arrays and publishing markers. This was to make sure we were keeping track of all markers within one node for easier debugging later.

Object Identification

Given a point cloud of the room we wanted to be able to identify an object if we could match against a point-cloud of the desired shape. This story will detail the first-pass implementation of this template-matching algorithm. It’s functionality is described in the corresponding diagram.

The preparation listened to the point clustering node and received the center points of all clustered points as well as the associated point cloud. We generated a micro-grid of points in the map around each center that we would search. Then, in our first implementation, we generate an equation-driven template of a circle of the same radius as our goal object.

The template matching was very reminiscent of the particle filter project. We wrote our own point to point minimization function this time that interfaced with the (x, y) point cloud instead of the map directly. We weighted and normalized every scanned location, and sorted the locations to determine which point in the map had the highest probability of being our goal. The final location and associated points were then sent to our visualization node for confirmation.

Project Story Three

Bonus Edition: A*

One of Isaac's learning goals was to write a blog post about A* including a brief introduction and explanation of our implementation. That is why we have three project stories rather than the standard two.

Illustration of A* search for finding path from a start node to a goal node in a robot motion planning problem. The empty circles represent unexplored nodes and the filled ones have been checked. Color on each closed node indicates the distance from the start: the greener, the farther. Courtesy of Wikipedia Page on A*

A*: An Introduction

A* is a pathfinding algorithm that traverses graphs in order to find a path between multiple nodes. As a best-first search algorithm, A* considers a potential path from the starting node to the given goal node in terms of smallest cost calculation. The smallest cost is usually shortest time or least distance travelled, but can sometimes be other metrics depending on the application. A* determines the smallest cost by storing a tree of paths from the start node and extending those paths one edge at a time until its found the goal.

A* is one of a class of path-finding algorithms that run on graphs and, as such, is closely related to Breadth First Search and Dijkstra's Algorithm. In fact, A* is an optimized modification of Dijkstra's for a single destination node. The optimization comes in the sense that Dijkstra's can be used to find paths to all locations whereas A* prioritizes paths that lead closer to the goal node.

The real key to the success of A* is in combining parts from Dijkstra's Algorithm (like favoring vertices closer to the starting node) and other graph searching methods (i.e. favoring vertices closer to the goal, etc.)

By combining a lot of the useful information from other graph search mechanisms with a heuristic function that describes an estimate of the minimum cost from any node to the goal node, A* allows for some variability and modification while remaining a really robust way to do path planning.

Our A* Implementation

Astar.py

Given a map with start and end nodes, our A* algorithm checks all adjacent pixels to the start node and then uses the cost function heuristics to pick the node bringing it closest to the goal node. It then checks the adjacent pixels to that node and repeats this process until it either finds an obstacle and plans around it or reaches the destination node. It then outputs these "safe" nodes a s a list of coordinates

Navstar.py

Given the map and the list of coordinates that make up a path from the starting node to the destination node, we convert this list into a vector approximation of the path including the distance and direction of each vector command. This list of vector commands is then converted into velocities and iterated through by the robot such that it travels from the starting node to the destination node.

Google Sites

Report abuse