# Motion Planning with Crazyflie Drone

Motion Planning essentially answers the question `How to get from point A to point B?` given the map of the environment. In this project on Udacity, the drone had to navigate through an urban environment with tall buildings to reach the destination specified as GPS coordinates. You can checkout the Udacity project along with my solution to the project here. In this blog post, we will port the Udacity project to Crazyflie, a micro-UAV that can be safely used indoors. We use local coordinates indoors derived from the flow deck for lateral position (x and y coordinate) and laser range sensor for drone altitude (z coordinate).

We assume that the map of the environment is given to us — this is an assumption to simplify the problem and focus on understanding motion planning ideas. In reality, the map of environment may not be available or even if it’s available, it may not be precise. Some of the obstacles such as furniture, people, or other clutter in the indoor space may not be captured in the map. However, our assumption lets us deal with the problem in a manageable way and learn the basics of motion planning.

# Graphs

There may be too many waypoints regardless of your representation (grid or graph), i.e., for a UAV too many waypoints means too many stop and starts and this results in a jerky motion. Instead, we need smooth flying of the UAV — this is possible if we can create a high-level “summary” of the too many waypoints we have. We use Bresenham algorithm to condense waypoints to minimal points enabling UAV to fly without unnecessary stopping at extraneous waypoints.

Probabilistic Road Map (PRM) scales well for large spaces compared to approaches such as Voronoi graphs or grid based approaches. However, with the FCND simulator, I had difficulty using PRM so ended up using Voronoi graphs and Bresenham for refinements of paths to come up with the final flight paths for the UAV. When we port this code to Crazyflie, we will use PRM instead of Voronoi graphs as it’s a more practical and scalable approach.

# Simulator to Crazyflie

• Implement Probabilistic Road Map (PRM) to find best course of waypoints to reach the goal location from a start location. Here is an example implementation for your reference. Once implemented, we initialize all waypoints for the Crazyflie as shown here:

`self.all_waypoints = self.plan_path_graph()`

• Initialize the grid by setting the following variables before constructing the PRM. These are the settings I had to use for the indoor space I used for flying Crazyflie. Please ensure you plugin the values that are appropriate for your case. `TARGET_ALTITUDE` and `SAFETY_DISTANCE` are your choice expressed in inches (since my measuring tape only had inches). If you have a measuring tape that can measure in centimeters, you can as well use meters for all measurements. `GRID_NORTH_SIZE` and `GRID_EAST_SIZE` are the total size of the indoor space used to fly the UAV. `NUMBER_OF_NODES` is the number of nodes to be sampled and `OUTDEGREE` is the number of edges that are going out from a node.
`# convert meters to inchesTARGET_ALTITUDE = 0.3 / 0.0254SAFETY_DISTANCE = 0.25 / 0.0254GRID_NORTH_SIZE = 130GRID_EAST_SIZE = 86NUMBER_OF_NODES = 200OUTDEGREE = 4grid, G = construct_road_map_crazyflie(data, GRID_NORTH_SIZE, GRID_EAST_SIZE, TARGET_ALTITUDE, SAFETY_DISTANCE, NUMBER_OF_NODES, OUTDEGREE)`
• Initialize the start and end location using a measuring tape — the current scale is in inches. Note that the coordinates are in (NORTH, EAST, ALTITUDE) since this the format used by Crazyflie.
`# set start and goal locationsgrid_start = (32, 40, 0)grid_goal = (110, 40, 0)`

Here is the complete code for motion planning with Crazyflie and the utility methods for planning. Here are examples of paths generated from PRM for an obstacle course you will see in the next section. You can run PRM module separately even if you don’t have the Crazylie using the following command

`python planning_utils.py`

You will see PRMs generate a path first and when you close the visualization, the waypoints from PRM are reduced using Bresenham to generate realistic waypoints that the Crazyflie can follow.

# Crazyflie in action

`python crazyflie_motion_planning.py`

# Conclusion

Building such a map manually is not practical for unknown cluttered environments. Even if we have such a map, it’s probably going to get stale pretty quickly in such a dynamic environment. Further, there may be non-static obstacles such as people moving around which are never captured in a static map. All these challenges motivates us to pursue approaches to create maps of unknown cluttered environments using various sensors on the UAV. Such a dynamic map creation would enable UAVs to truly explore an unknown environment.

# References

1. Python sample codes for robotics algorithms
2. The Open Motion Planning Library

I’m a Researcher and Engineer working on Machine Learning and Computer Vision related topics. I enjoy working on fun projects in my spare time.

## More from Pramod Anantharam

I’m a Researcher and Engineer working on Machine Learning and Computer Vision related topics. I enjoy working on fun projects in my spare time.

## The Layer Cake

Get the Medium app