Run and tumble + obstacle avoidance on the Crazyflie palm-sized drone

This repository contains the source code for a biology-inspired “run and tumble” source seeking behaviour coupled with obstacle avoidance on a palm-sized drone, Crazyflie 2.1.

Major highlights

Deck drivers

• BH1750 light sensor (if you are not using the continuous high resolution mode on the BH1750 sensor, please refer to the sensor datasheet and change the conv_factor, CONT_HI_RES_MODE in i2cdevWriteByte() and delay time in vTaskDelayUntil() accordingly)
• HDC2010 temperature sensor
• nRF52840 dongle configured as an TWI device to measure bluetooth RSSI (note: this is possible using the on-board nRF chip too, but this method was used to quickly filter out bluetooth advertising packets from a particular MAC address by flashing the nRF dongle)

A simulation environment for designing and testing the algorithm

See scripts/simulation.py.

simulation.webm.mov

Might also be of interest: unicycle-navigation, a repository that simulates go-to-goal (with known robot and goal positions) and obstacle avoidance behaviours in a finite state machine. That repository is more educational in purpose, but this simulation builds upon that one and adds simulated sensors, etc

Light source seeking

See scripts/run-and-tumble-alg_light.py.

light-algo_compressed.webm.mov

Temperature source seeking

See scripts/run-and-tumble-alg_temp.py.

fire.webm.mov

fire_thermal-cam_compressed.webm.mov

Keyboard-based crazyflie control, live plotting and logging

See scripts/cflibController.py. This script can be invoked to manually control the crazyflie. Also contains functionality to live plot drone position estimate, log various measurements (eg. position, temperature, light intensity, multiranger ranges) to a csv file. Please use the -h flag for usage

Using this repository

• Patch a recently cloned copy of the crazyflie-firmware with the contents from ./crazyflie-firmware/src/deck/drivers/src
• Modify the crazyflie-firmware/Makefile to include any of the new drivers from this repository being used (eg. bh1750deck.c in the example below):

# Decks
PROJ_OBJ += bh1750deck.o
# PROJ_OBJ += hdc2010deck.o
# PROJ_OBJ += nrfI2Cdeck.o

• Copy crazyflie-firmware/tools/make/config.mk.example to crazyflie-firmware/tools/make/config.mk and force loading of any new drivers (eg. bh1750deck driver in the example below):

# bh1750 deck
CFLAGS += -DDECK_FORCE=bh1750Deck
DEBUG=1

# hdc2010 deck
# CFLAGS += -DDECK_FORCE=hdc2010Deck

# nrf I2C deck
# CFLAGS += -DDECK_FORCE=nrfI2CDeck

• Compile and flash the firmware to a crazyflie. Instructions here.
• Use the relevant script from scripts.

Description of repository contents

• crazyflie-firmware/src/deck/drivers/src: the deck drivers for the three devices used for signal strength detection
• data: data recorded from all experiments. See the Results section in this readme for details
• docs: relevant datasheets etc
• resources: resources used in this README
• scripts: all scripts
  • scripts/plots: scripts used for generating all plots from data. See the Results section in the readme for details
  • scripts/cflibController.py: manual control and live plotting of the crazyflie along with logging measurements to a csv file
  • scripts/run-and-tumble-alg_light.py and scripts/run-and-tumble-alg_temp.py: the algorithms used for each source seeking method. The only major difference between the two at the high level is that the temperature seeking algorithm has a stop command for a few seconds to let the temperature sensor stabilize at a measurement. See the respective files for more details
  • scripts/simulation.py: runs a simulation of a robot seeking a source with an inverse square potential in the presence of obstacles. Use the -h flag for details

Requirements

• The following python packages (obtain using pip, conda, your OS distribution package manager or any other preferred means):
  • numpy
  • pygame
  • cflib
  • argparse
  • matplotlib
• crazyflie-firmware (just clone the repository)

Method

Source characterization

(left) Light intensity distribution of an indoor light source using drone’s position estimate. [top] with 2D position [bottom] along a straight line path towards the source, (middle) Temperature distribution for a fire using ground truth distance [top], and drone position estimate [bottom], (right) Bluetooth source packet detection rate distribution with ground truth distance

We can see that for the light source the intensity distribution qualitatively follows an inverse-squared distribution as expected from a point light source. Hence this choice in the simulation was reasonable. For the fire we see from the top plot that it takes a while for the temperature sensor to reach a steady state (since it is not an ideal blackbody) hence we add a stop command for a few seconds after every run to ensure the sensor measurements have stabilized. Future work will focus on extending this to Radio Frequency (RF) sources as it can be observed that bluetooth packet rate also follows a roughly monotonic trend.

Simulation

We used PyGame to create the simulation environment to for rapid testing of the algorithm before deploying it on the actual drone. The code in simulation.py is pretty self-explanatory to understand what is going on.

Algorithm

The algorithm is implemented as the following finite state machine:

The parameters for the algorithm are:

Parameter	Simulation	Light seeking	Heat seeking
stop_threshold	10^4	800 lux	13 Celcius
fwd_velocity	90 px/sec	0.1 m/s	0.2 m/s
ao_time	100 ms	2 s	0.5 s
ao_angle	0.1 deg	20 deg	20 deg
run_time	10 ms	1 s	1 s
obst_threshold	40 px	0.5 m	0.35 m

The three behaviours are:

• run: move forward with a constant velocity
• tumble: turn left or right to a random angle
• avoid-obstacle: move directly away from the closest obstacle along the direction of the distance sensor, and change heading away slightly

The exact implementation details can be seen in scripts/run-and-tumble-alg_light.py and scripts/run-and-tumble-alg_temp.py.

Note: The last intensity is taken to be the average of last 10 recorded intensity values to minimize errors due to sensor noise.

Results

Distance to source always decreases

For example, various runs from the simulation:

(left) The blue circles are randomly placed obstacles. Multiple trajectories (different colors) with different starting points (red dots) at the same distance from the source (black diamond) (right) Variation of distance of the robot from the light source for the trajectories on the left

Increasing obstacle avoidance distance threshold increases source seeking time

For example, various runs from the simulation:

obst_threshold	average seek time for 10 runs
20 px	6.54 s
40 px	11.10 s
80 px	13.74 s
100 px	22.88 s

Light source seeking experiments

Heat source seeking experiments

Authors

In case more details are needed, please contact one of the authors listed below:

• Nishant Elkunchwar
• Vikram Iyer
• Melanie Anderson
• Krishna Balasubramanian
• Jessica Noe
• Yash Talwekar
• Sawyer B. Fuller

Acknowledgements

The authors would like to thank Yogesh Chukewad for insightful suggestions as well as Johannes James for assistance with the fire seeking experiments. The authors would also like to thank The Air Force Office of Scientific Research (AFOSR), grant no. FA9550-14-1-0398 by The Air Force Center of Excellence on Nature-Inspired Flight Technologies and Ideas (NIFTI) for funding the equipment used in this research.

Note: This project started as a course project for the course biology-inspired robotics taken by Prof. Sawyer B. Fuller at the University of Washington during Winter 2020. The original goal was light source seeking (video here). This blog post on the Bitcraze blog provides a good introduction. However the information contained in that blog post (and previous versions of this README) is outdated considering the current contents of this repository because the project has been in continued development after the course completion (at AIRlab) and it has increased in scope and some implementation details have been changed. The original project poster, proposal etc can be accessed from this commit. The initial approach was based on using the ROS ecosystem and the rospy_crazyflie library (a wrapper around cflib) used in the smellicopter paper. We currently directly use cflib to be Python3 compliant. The ros-based scripts and other deprecated files can be accessed from this commit. The current version also does not use a virtual machine.

Todos

• [ ] The crazyflie now has an app layer. Implement this algorithm on-board and remove the dependency on an external computer