AR Drone 2/Mapping
Mapping has not been integrated with paparazzi due to lack of time. Yet, all components have been tested and are working properly.
One of the functions that the AR.Drone2 should have is to be able to map a certain area (starting off with a room). In order to do this the AR.Drone2 needs to be able to: measure distance to objects, keep track of it's own position and heading, store a map efficiëntly and avoid crashing into objects. The first two things are needed to create a map from measurements during flight, the third is to use a map and still be able to go through all the control loops of paparazzi, the fourth is to maintain flight. It would also be usefull to make an algorithm for the AR.Drone2 to explore the area and store the map in the most efficient way.
Storing the map
For storing the map, at first we believed Octomap (paper) to be a good solution. It's a datastructure using an OcTree to store a 3D map that is updateable, flexible and compact. However, since we want to fly in a 2D manner, it's not very usefull. You'd have to retreive a 2D slice of the map at a certain hight (for which there is no build-in function) every iteration which would take a lot of unnecessary time.
Since we liked the flexibility of the Octomap, we created our own 2D version of it: a quadmap so to speak. Instead of an octree, it uses a quadtree to store information about the map. This way we represent the map as a square instead of a cube, like the Octomap does. The rest is pretty much the same: we keep a record of all the occupied, free and unknown cells and we can update the map by casting rays from a startingpoint towards an endpoint (with endpoint being occupied or not).
Exploring the area
Since the AR.Drone2 will only have point measurement sensors to measure distance to objects, it is pretty much impossible to create a 3D map. Flying on the same hight all the time, will make a 2D map sufficient for the job. We're using Frontier-Based Exploration for this purpose. With this approach you start with a map filled with unknown cells. Then, by scanning the surroundings of the robot, you fill in the unknown cells: occupied or unoccupied. Next, you search all the frontiers (the border between unoccupied and unknown cells) and pick the frontier with the optimum of being the largest and with the least travel distance. Subsequently you take that frontier as your next destination and upon arival you scan again. There are also variations for 3D (paper). But this is based on a wide range sensor (kinect for example) and therefore not suited for our AR.Drone2.
As explained, Frontier-Based Exploration consists out of a few steps that will be repeated until the whole area is explored:
- Scan
- Search for frontiers
- Pick best frontier
- Travel
We can use these steps to create a statemachine. Since paparazzi needs to go through its control loops to keep the drone in the air (and because it's a single threaded system), we need to execute the mapping algorithm in smaller steps and thus keep track of what state we are.
As you can see, some of the steps of Frontier-Based Exploration have been split up into smaller substates. This way we can see more easily what the program is supposed to do while keeping the operation time per iteration to a minimum.
Init
Inside the mapping algorithm, we not only keep track of the state, we also keep track of other things like indexes, iterator positions etc. In the init state, all these fields are set to their respective starting values, after which the algorithm enters the next state.
Scan
Some robots can scan entire areas while moving with wide range sensors. Our Ar.Drone2 is limited to point measurement sensors and therefore has to have an explicit step "Scan". Seeing as our drone will have 4 horizontally placed sensors in a + sign, a 360 degrees scan can be made by making a 45 degrees turn. By keeping track of our rotation and position, with the use of internal sensors, we can deduct where the scanned points are in our selfmade map.
Search for frontiers
Find paths
Pick best frontier
Travel
Avoiding objects
To be able to avoid object, you need to know your surroundings. That's why the object avoidance function is closely related to mapping. Therefore it will probably be developed simultaneously.