Category: Software

Home / Blog

Sniffy Bug: A Fully Autonomous Swarm of Gas-Seeking Nano Quadcopters in Cluttered Environments

  2021-07-14  |     |    1 Comment

This week we have a guest blog post from Bart Duisterhof and Prof. Guido de Croon from the MAVlab, Faculty of Aerospace Engineering from the Delft University of Technology. Enjoy!

Tiny drones are ideal candidates for fully autonomous jobs that are too dangerous or time-consuming for humans. A commonly shared dream would be to have swarms of such drones help in search-and-rescue scenarios, for instance to localize gas leaks without endangering human lives. Drones like the CrazyFlie are ideal for such tasks, since they are small enough to navigate in narrow spaces, safe, agile, and very inexpensive. However, their small footprint also makes the design of an autonomous swarm extremely challenging, both from a software and hardware perspective.

From a software perspective, it is really challenging to come up with an algorithm capable of autonomous and collaborative navigation within such tight resource constraints. State-of-the-art solutions like SLAM require too much memory and processing power. A promising line of work is to use bug algorithms [1], which can be implemented as computationally efficient finite state machines (FSMs), and can navigate around obstacles without requiring a map.

A downside of using FSMs is that the resulting behavior can be very sensitive to their hyperparameters, and therefore may not generalize outside of the tested environments. This is especially true for the problem of gas source localization (GSL), as wind conditions and obstacle configurations drastically change the problem. In this blog post, we show how we tackled the complex problem of swarm GSL in cluttered environments by using a simple bug algorithm with evolved parameters, and then tested it onboard a fully autonomous swarm of CrazyFlies. We will focus on the problems that were encountered along the way, and the design choices we made as a result. At the end of this post, we will also add a short discussion about the future of nano drones.

Why gas source localization?

Overall we are interested in finding novel ways to enable autonomy on constrained devices, like CrazyFlies. Two years ago, we showed that a swarm of CrazyFlie drones was able to explore unknown, cluttered environments and come back to the base station. Since then, we have been working on an even more complex task: using such a swarm for Gas Source Localization (GSL). 

There has been a lot of research focussing on autonomous GSL in robotics, since it is an important but very hard problem. The difficulty of the task comes from the complexity of how odor can spread in an environment. In an empty room without wind, a gas will slowly diffuse from the source. This can allow a robot to find it by moving up gradient, just like small bacteria like E. Coli do. However, if the environment becomes larger with many obstacles and walls, and wind comes into play, the spreading of gas is much less regular. Large parts of the environment may have no gas or wind at all, while at the same time there may be pockets of gas away from the source. Moreover, chemical sensors for robots are much less capable than the smelling organs of animals. Available chemical sensors for robots are typically less sensitive, noisier, and much slower.  

Due to these difficulties, most work in the GSL field has focused on a single robot that has to find a gas source in environments that are relatively small and without obstacles. Relatively recently, there have been studies in which groups of robots solve this task in a collaborative fashion, for example with Particle Swarm Optimization (PSO). This allows robots to find the source and escape local maxima when present. Until now this concept has been shown in simulation [2] and on large outdoor drones equipped with LiDAR and GPS [3], but never before on tiny drones in complex, GPS-denied, indoor environments.

Required Infrastructure

In our project, we introduce a new bug algorithm, Sniffy Bug, which uses PSO for gas source localization. In order to tune the FSM of Sniffy Bug, we used an artificial evolution. For time reasons, evolution typically takes place in simulation. However, early in the project, we realized that this would be a challenge, as no end-to-end gas modeling pipeline existed yet. It is important to have an easy-to-use pipeline that does not require any aerodynamics domain knowledge, such that as many researchers as possible can generate environments to test their algorithms. It would also make it easier to compare contributions and to better understand in which conditions certain algorithms work or don’t work. The GADEN ROS package [4] is a great open-source tool for modeling gas distribution when you have an environment and flow field, but for our objective, we needed a fully automated tool that could generate a great variety of random environments on-demand with just a few parameters. Below is an overview of our simulation pipeline: AutoGDM.

AutoGDM, a fully automated gas dispersion modeling (GDM) simulation pipeline.

First, we use a procedural environment generator proposed in [5] to generate random walls and obstacles inside of the environment. An important next step is to generate a 3D flowfield by means of computational fluid dynamics (CFD). A hard requirement for us was that AutoGDM needed to be free to use, so we chose to use the open-source CFD tool OpenFOAM. It’s used for cutting-edge aerodynamics research, and also the tool suggested by the authors of GADEN. Usually, using OpenFOAM isn’t trivial, as a large number of parameters need to be selected that require field expertise, resulting in a complicated process. Next, we integrate GADEN into our pipeline, to go from environment definition (CAD files) and a flow field to a gas concentration field. Other parts that needed to be automated were the random selection of boundary conditions, which has a large impact on the actual flow field, and source placement, which has an equally large impact on the concentration field.

After we built this pipeline, we started looking for a robot simulator to couple it to. Since we weren’t planning on using a camera, our main requirement was for the simulator to be efficient (preferably in 2D) so that evolutions would take relatively little time. We decided to use Swarmulator [6], a lightweight C++ robot simulator designed for swarming and we plugged in our gas data.

Algorithm Design

Roughly speaking, we considered two categories of algorithms for controlling the drones: 1) a neural network, and 2) an FSM that included PSO, with evolved parameters. Since we used a tiny neural network for light seeking with a CrazyFlie in our previous work, we first evolved neural networks in simulation. One of the first experiments is shown below.

A single agent in simulation seeking a light source using a tiny neural network.

While it worked pretty well in simple environments with few obstacles, it seemed challenging to make this work in real life with complex obstacles and multiple agents that need to collaborate. Given the time constraints of the project, we have opted for evolving the FSM. This also facilitated crossing the reality gap, as the simulated evolution could build on basic behaviors that we developed and validated on the real platform, including obstacle avoidance with four tiny laser rangers, while communicating with and avoiding other drones. An additional advantage of PSO with respect to the reality gap is that it only needs gas concentration and no gradient of the gas concentration or wind direction (which many algorithms in literature use). On a real robot at this scale, estimating the gas concentration gradient or the direction of a light breeze is hard if not impossible.

Hardware

Our CrazyFlie needs to be able to avoid obstacles, execute velocity commands, sense gas, and estimate the other agent’s position in its own frame. For navigation, we added the flow deck and laser rangers, whereas for gas sensing we used a TGS8100 gas sensor that was used on a CrazyFlie before in previous work [7]. The sensor is lightweight and inexpensive, but accurately estimating gas concentrations can be difficult because of its size. It tends to drift and needs time to recover after a spike in concentration is observed. Another thing we noticed is that it is possible to break them, a crash can definitely destroy the sensor.

To estimate the relative position between agents, we use a Decawave Ultra-Wideband (UWB) module and communicate states, as proposed in [8]. We also use the UWB module to communicate gas information between agents and collaboratively seek the source. The complete configuration is visible below.

A 37.5 g nano quadcopter, capable of fully autonomous waypoint tracking, obstacle avoidance, relative localization, communication and gas sensing.

Evaluation in Simulation

After we optimized the parameters of our model using Swarmulator and AutoGDM, and of course trying many different versions of our algorithm, we ended up with the final Sniffy Bug algorithm. Below is a video that shows evolved Sniffy Bug evaluated in six different environments. The red dots are an agent’s personal target waypoint, whereas the yellow dot is the best-known position for the swarm.

Sniffy Bug evaluated in Swarmulator environments.

Simulation showed that Sniffy Bug is effective at locating the gas source in randomly generated environments. The drones successfully collaborate by means of PSO.

Real Flight Testing

After observing Sniffy Bug in simulation we were optimistic, but unsure about performance in real life. First, inspired by previous works, we disperse alcohol through the air by placing liquid alcohol into a can which is then dispersed using a computer fan.

Dispersion of liquid alcohol in flight tests.

We test Sniffy Bug in our flight arena of size 10 x 10 meters with large obstacles that are shaped like walls and orange poles. The image below shows four flight tests of Sniffy Bug in cluttered environments, flying fully autonomously, i.e., without the help from any external infrastructure.

Time-lapse images of real-world experiments in our flight arena. Sniffy was evaluated on four distinct environments, 10 x 10 meters in size, seeking a real isopropyl alcohol source. The trajectories of the nano quadcopters are clearly visible due to their blue lights.

In the total of 24 runs we executed, we compared Sniffy Bug with manually selected and evolved parameters. The figure below shows that the evolved parameters are more efficient in locating the source as compared to the manual parameters.

Maximum recorded gas reading by the swarm, for each time step for each run.

This does not only show that our system can successfully locate a gas source in challenging environments, but it also demonstrates the usefulness of the simulation pipeline. The parameters that were learned in simulation yield a high-performance model, validating the environment generation, randomization, and gas modeling parts of our pipeline.

Conclusion and Discussion

With this work, we believe we have made an important step towards swarms of gas-seeking drones. The proposed solution is shown to work in real flight tests with obstacles, and without any external systems to help in localization or communication. We believe this methodology can be extended to larger environments or even to 3 dimensions, since PSO is a robust, multi-dimensional heuristic search method. Moreover, we hope that AutoGDM will help the community to better compare gas seeking algorithms, and to more easily learn parameters or models in simulation, and deploy them in the real world.

To improve Sniffy Bug’s performance, adding more laser rangers will definitely help. When working with only four laser rangers you realize how little information it actually provides. If one of the rangers senses a low value it is unclear if a slim pole or a massive wall is detected, adding inefficiency to the algorithm. Adding more laser rangers or using other sensor modalities like vision will help to avoid also more complex obstacles than walls and poles in a reliable manner.

Another interesting discussion can be held on the hardware required for real deployment. When working with 40 grams of maximum take-off weight, the sensors and actuators that can be selected are limited. For example, the low-power and lightweight flow deck works great but fails in low-light scenarios or with smoke. Future work exploring novel sensors for highly constrained nano robots could really help increase the Technological Readiness Level (TRL) of these systems.

Finally, this has been a really fun project to work on for us and we can’t wait to hear your thoughts on Sniffy Bug!

References

[1] K. N. McGuire, C. De Wagter, K. Tuyls, H. J. Kappen, and G. C. H. E.de Croon, “Minimal navigation solution for a swarm of tiny flying robotsto explore an unknown environment,”Science Robotics, vol. 4, no. 35,2019.

[2] W. Jatmiko, K. Sekiyama and T. Fukuda, “A pso-based mobile robot for odor source localization in dynamic advection-diffusion with obstacles environment: theory, simulation and measurement,” in IEEE Computational Intelligence Magazine, vol. 2, no. 2, pp. 37-51, May 2007, doi: 10.1109/MCI.2007.353419.

[3] Steiner, JA, Bourne, JR, He, X, Cropek, DM, & Leang, KK. “Chemical-Source Localization Using a Swarm of Decentralized Unmanned Aerial Vehicles for Urban/Suburban Environments.” Proceedings of the ASME 2019 Dynamic Systems and Control Conference. Volume 3, Park City, Utah, USA. October 8–11, 2019. V003T21A006. ASME. https://doi.org/10.1115/DSCC2019-9099

[4] . Monroy, V. Hernandez-Bennetts, H. Fan, A. Lilienthal, andJ. Gonzalez-Jimenez, “Gaden: A 3d gas dispersion simulator for mobilerobot olfaction in realistic environments,”MDPI Sensors, vol. 17, no.7: 1479, pp. 1–16, 2017.

[5] K. McGuire, G. de Croon, and K. Tuyls, “A comparative study of bug algorithms for robot navigation,”Robotics and Autonomous Systems, vol.121, p. 103261, 2019.

[6] https://github.com/coppolam/swarmulator

[7] J. Burgues, V. Hern ́andez, A. J. Lilienthal, and S. Marco, “Smellingnano aerial vehicle for gas source localization and mapping,”Sensors(Switzerland), vol. 19, no. 3, 2019.[8] S. Li, M. Coppola, C. D. Wagter, and G. C. H. E. de Croon, “An autonomous swarm of micro flying robots with range-based relative localization,” Arxiv, 2020.

[8] S. Li, M. Coppola, C. D. Wagter, and G. C. H. E. de Croon, “An autonomous swarm of micro flying robots with range-based relative localization,” Arxiv, 2020.

Links

ArXiv: https://arxiv.org/abs/2107.05490

Code: https://github.com/tudelft/sniffy-bug

Video:

Please reach out if you have any questions or ideas, you can reach us at: b.p.duisterhof@gmail.com or g.c.h.e.decroon@tudelft.nl

Release 2021.06

  2021-07-05  |     |    Leave a comment

We are happy to announce the availability of the 2021.06 release of the Crazyflie firmware and client! This release includes bugfixes related to flashing the Crazyflie and the Lighthouse deck and also the new concept of core parameters and logging variables, that we talked about in the blog post: Crazyflie logging and parameters interface.

There are new binaries for the Crazyflie (2021.06), the Crazyflie client (2021.6) as well as a new release of the Crazyflie python library (0.1.16). The firmware package can be downloaded from the Crazyflie release repository (2021.06) or can be flashed directly using the Crazyflie client.


Noteworthy features and fixes

Crazyflie STM firmware

* Added an app layer application of the wall following demo
* Fixed a bug where the initial position of the Kalman estimator was set to (1.5, 0, 0)
* Enable an estimator to be added by out of tree build
* Add and document core parameters and log variables
* Fix deck info mem implementation can return uninitialized buffer
  - Caused: Flashing errors with Flowdeck
* Rechecked INDI controller for mistakes/consistency

Crazyflie NRF firmware

* pm: Increase delay before powering system
  - Caused: Issues flashing Lighthouse deck

Crazyflie Python library (0.1.16)

* Add support for crazyflie-link-cpp
* Add get_value to parameter class
* Make tutorial more user friendly
* Improve reset_to[firmware|bootloader]

Crazyflie Client

* Do not allow upgrading firmware when connected to USB
* Console tab: new messages should always appear at the bottom
* Update install doc

Documentation

* Core logging groups and variables
* Core parameter groups and variables
* Improved motion commander tutorial

Help us out!

We always strive to release quality software and firmware, but we are not perfect! Please help us out by installing the new library, client and firmware and make sure that your applications, tools and algorithms still work as you expect! And if not please file an issue with us or contact us via the forum.

Happy hacking!

Ultra-Wideband Swarm Ranging

  2021-06-21  |     |    Leave a comment

This week we have a guest blog post from Dr Feng Shan at School of Computer Science and Engineering
Southeast University, China. Enjoy!

It is possible to utilize tens and thousands of Crazyflies to form a swarm to complete complicated cooperative tasks, such as searching and mapping. These Crazyflies are in short distance to each other and may move dynamically, so we study the dynamic and dense swarms. The ultra-wideband (UWB) technology is proposed to serve as the fundamental technique for both networking and localization, because UWB is so time sensitive that an accurate distance can be calculated using the transmission and receive timestamps of data packets. We have therefore designed a UWB Swarm Ranging Protocol with key features: simple yet efficient, adaptive and robust, scalable and supportive. It is implemented on Crazyflie 2.1 with onboard UWB wireless transceiver chips DW1000.

Fig.1. Nine Crazyflies are in a compact space ranging the distance with each other.

The Basic Idea

The basic idea of the swarm ranging protocol was inspired by Double Sided-Two Way Ranging (DS-TWR), as shown below.

Fig.2. The exsiting Double Sided-Two Way Ranging (DS-TWR) protocol.

There are four types of message in DS-TWR, i.e., poll, response, final and report message, exchanging between the two sides, A and B. We define their transmission and receive timestamps are Tp, Rp, Tr, Rr, Tf, and Rf, respectively. We define the reply and round time duration for the two sides as follows.

Let tp be the time of flight (ToF), namely radio signal propagation time. ToF can be calculated as Eq. (2).

Then, the distance can be estimated by the ToF.

In our proposed Swarm Ranging Protocol, instead of four types of messages, we use only one type of message, which we call the ranging message.

Fig.3. The basic idea of the proposed Swarm Ranging Protocol.

Three sides A, B and C take turns to transmit six messages, namely A1, B1, C1, A2, B2, and C2. Each message can be received by the other two sides because of the broadcast nature of wireless communication. Then every message generates three timestamps, i.e., one transmission and two receive timestamps, as shown in Fig.3(a). We can see that each pair has two rounds of message exchange as shown in Fig.3(b). Hence, there are sufficient timestamps to calculate the ToF for each pair, that means all three pairs can be ranged with each side transmitting only two messages. This observation inspires us to design our ranging protocol.

Protocol Design

The formal definition of the i-th ranging message that broadcasted by Crazyflie X is as follows.

Xi is the message identification, e.g., sender and sequence number; Txi-1 is the transmission timestamp of Xi-1, i.e., the last sent message; RxM is the set of receive timestamps and their message identification, e.g., RxM = {(A2, RA2), (B2, RB2)}; v is the velocity of X when it generates message Xi.

As mentioned above, six timestamps (Tp, Rp, Tr, Rr, Tf, Rf,) are needed to calculate the ToF. Therefore, for each neighbor, an additional data structure is designed to store these timestamps which we named it the ranging table, as shown in Fig.4. Each device maintains one ranging table for each known neighbor to store the timestamps required for ranging.

Fig.4. The ranging table, one for each neighbor.

Let’s focus on a simple scenario where there are a number of Crazyflies, A, B, C, etc, in a short distance. Each one of them transmit a message that can be heard by all others, and they broadcast ranging messages at the same pace. As a result, between any two consecutive message transmission, a Crazyflie can hear messages from all others. The message exchange between A and Y is as follows.

Fig.5. Message exchange between A and Y.

The following steps show how the ranging messages are generated and the ranging tables are updated to correctly compute the distance between A and Y.

Fig.6. How the ranging message and ranging table works to compute distance.

The message exchange between A and Y could be also A and B, A and C, etc, because they are equal, that’s means A could perform the ranging process above with all of it’s neighbors at the same time.

To handle dense and dynamic swarm, we improved the data structure of ranging table.

Fig.7. The improved ranging table for dense and dynamic swarm.

There are three new notations P, tn, ts, denoting the newest ranging period, the next (expected) delivery time and the expiration time, respectively.

For any Crazyflie, we allow it to have different ranging period for different neighbors, instead of setting a constant period for all neighbors. So, not all neighbors’ timestamps are required to be carried in every ranging message, e.g., the receive timestamp to a far apart and motionless neighbor is required less often. tn is used to measure the priority of neighbors. Also, when a neighbor is not heard for a certain duration, we set it as expired and will remove its ranging table.

If you are interested in our protocol, you can find much more details in our paper, that has just been published on IEEE International Conference on Computer Communications (INFOCOM) 2021. Please refer the links at the bottom of this article for our paper.

Implementation

We have implemented our swarm ranging protocol for Crazyflie and it is now open-source. Note that we have also implemented the Optimized Link State Routing (OLSR) protocol, and the ranging messages are one of the OLSR messages type. So the “Timestamp Message” in the source file is the ranging message introduced in this article.

The procedure that handles the ranging messages is triggered by the hardware interruption of DW1000. During such procedure, timestamps in ranging tables are updated accordingly. Once a neighbor’s ranging table is complete, the distance is calculated and then the ranging table is rearranged.

All our codes are stored in the folder crazyflie-firmware/src/deck/drivers/src/swarming.

The following figure is a ranging performance comparison between our ranging protocol and token-ring based TWR protocol. It’s clear that our protocol handles the large number of drones smoothly.

Fig.8. performance comparison.

We also conduct a collision avoidance experiment to test the real time ranging accuracy. In this experiment, 8 Crazyflie drones hover at the height 70cm in a compact area less than 3m by 3m. While a ninth Crazyflie drone is manually controlled to fly into this area. Thanks to the swarm ranging protocol, a drone detects the coming drone by ranging distance, and lower its height to avoid collision once the distance is small than a threshold, 30cm.

Build & Run

Clone our repository

git clone --recursive https://github.com/SEU-NetSI/crazyflie-firmware.git

Go to the swarming folder.

cd crazyflie-firmware/src/deck/drivers/src/swarming

Then build the firmware.

make clean
make

Flash the cf2.bin.

cfloader flash path/to/cf2.bin stm32-fw

Open the client, connect to one of the drones and add log variables. (We use radio channel as the address of the drone) Our swarm ranging protocol allows the drones to ranging with multiple targets at the same time. The following shows that our swarm ranging protocol works very efficiently.

Summary

We designed a ranging protocol specially for dense and dynamic swarms. Only a single type of message is used in our protocol which is broadcasted periodically. Timestamps are carried by this message so that the distance can be calculated. Also, we implemented our proposed ranging protocol on Crazyflie drones. Experiment shows that our protocol works very efficiently.

Related Links

Code: https://github.com/SEU-NetSI/crazyflie-firmware

Paper: http://twinhorse.net/papers/SZLLW-INFOCOM21p.pdf

Our research group website: https://seu-netsi.net

Feng Shan, Jiaxin Zeng, Zengbao Li, Junzhou Luo and Weiwei Wu, “Ultra-Wideband Swarm Ranging,” IEEE INFOCOM 2021, Virtual Conference, May 10-13, 2021.

Rust, Wasm and the Crazyflie

  2021-06-14  |     |    Leave a comment

For quite a while now, I have been very interested in the Rust programming language and since Jonas joined us we are two rust-enthusiast at Bitcraze. Rust is a relatively recent programming language that aims at being safe, performant and productive. It is a system programming language in the sense that it compiles to machine code with minimal runtime. It prevents a lot of bugs at compile time and it provides great mechanisms for abstraction that makes it sometime feels as high level as languages like Python.

I have been interested in applying my love for Rust at Bitcraze, mostly during fun Fridays. There is two area that I have mainly explored so far: Putting Rust in embedded systems to replace pieces of C, having such a high-level-looking language in embedded is refreshing, and re-writing the Crazyflie lib on PC in rust to make it more performant and more portable. In this blog post I will talk about the later, I keep embedded rust for a future blog post :).

Re-implementing Crazyflie lib

To re-implement the Crazyflie lib, the easiest it to follow the way the communication stack is currently setup, more information can be found on Crtp in a pevious blog post about the Crazyflie radio communication and the communication reliability.

Since I am currently focusing on implementing communication using the Crazyradio dongle, I have separated the implementation in the following modules (A crate is the Rust version of a library):

This organization is very similar to the layering that we have in the python crazyflie-lib, the difference being that in the Crazyflie lib all the layers are distributed in the same Python package.

At the time this blog post is written, the Crazyradio crate is full featured. The link is in a good shape and even has a python binding. The Crazyflie lib however is still very much work in progress. I started by implementing the ‘hard’ parts like log and param but more directly useful part like set-points (what is needed to actually fly the Crazyflie) are not implemented yet.

Compiling to the web: Wasm

One of the nice property of Rust is that compiling to different platform is generally easy and seemless. For instance, all the crates talked about previously will compile and run on Windows/Mac/Linux without any modification including the Python binding using only the standard Rust install. One of the Rust supported platform is a bit more special and interesting compared to the other though: WebAssembly.

WebAssembly is a virtual machine that is designed to be targeted by system programming language like C/C++ and Rust. It can be used in standalone (a bit like the Java VM) as well as in a web browses. All modern web browser supports and can run WebAssembly code. WebAssembly can be called from JavaScript.

The WebAssembly in the web is unfortunately not as easy to target as the native Windows/Mac/Linux: WebAssembly does not support threading yet, USB access needs to be handled via WebUSB and since we run in a web browser from JavaScript we have to follow some rules inherited from it. The most important being that the program can never block (ie. std::sync::Mutex shall not be used, I have tried ….).

I made two major modification to my existing code in order to make it possible to run in a web browser:

  • crazyflie-link and crazyflie-lib have been re-implemented using Rust async/await. This means that there is no thread needed and Rust async/await interfaces almost seamlessly with Javascript’s promises. The link and lib still compile and work well on native platforms.
  • I have created a new crate named crazyradio-webusb (not uploaded yet at the release of this post) that exposes the same API as the crazyradio crates but using WebUSB to communicate with the Crazyradio.

To support the web, the relationship between the crates becomes as follow:

The main goal is to keep the crazyflie-lib and crazyflie-link unmodified. Support for the Crazyradio in native and on the web is handled by two crates that exposes the same async API. The crate used is chosen by a compile flag (called Features in the rust world). This architecture could easily be expanded to other platform like Android or iOS.

Status, demo and future work

I have started getting something working end-to-end in the browser. The lib currently only implements Crazyflie Param and the Log TOC so the current demo scans for Crazyflie, connects the first found Crazyflie and prints the list of parameters with the parameters type and values. It can be found on Crazyflie web client test server. This doesn’t do anything useful now, but I am going to update this server when I make progress, so feel free to visit it in the future :).

Note that WebUSB is currently only implemented by Chromium-based browser so Chrome, Chromium and recent Edge. On Windows you need to install the WinUSB driver for the Crazyradio using Zadig. On Linux/Mac/Android it should work out of the box.

The source code for the Web Client is not pushed on Github yet, once it is, it will be named crazyflie-client-web. It is currently mostly implemented in Rust and it will likely mostly be Rust since it is much easier to stick with one (great!) language. One of the plan is to make a javascript API and to push it on NPM, this will then become a Crazyflie lib usable by anyone on the web from JavaScript (or a bit better, TypeScript …).

My goal for now is to implement a clone of the Crazyflie Client flight control tab on the web. This would provide a nice way to get started with the Crazyflie without having to install anything.

Robust Kalman Filter for Ultra-wideband Localization

  2021-06-07  |     |    6 Comments

This week we have a guest blog post from Wenda Zhao, Ph.D. candidate at the Dynamic System Lab (with Prof. Angela Schoellig), University of Toronto Institute for Aerospace Studies (UTIAS). Enjoy!

Accurate indoor localization is a crucial enabling capability for indoor robotics. Small and computationally-constrained indoor mobile robots have led researchers to pursue localization methods leveraging low-power and lightweight sensors. Ultra-wideband (UWB) technology, in particular, has been shown to provide sub-meter accurate, high-frequency, obstacle-penetrating ranging measurements that are robust to radio-frequency interference, using tiny integrated circuits. UWB chips have already been included in the latest generations of smartphones (iPhone 12, Samsung Galaxy S21, etc.) with the expectation that they will support faster data transfer and accurate indoor positioning, even in cluttered environments.

A Crazyflie with an IMU and UWB tag flies through a cardboard tunnel. A vision-based  motion capture system would not be able to achieve this due to the occlusion.

In our lab, we show that a Crazyflie nano-quadcopter can stably fly through a cardboard tunnel with only an IMU and UWB tag, from Bitcraze’s Loco Positioning System (LPS), for state estimation. However, it is challenging to achieve a reliable localization performance as we show above. Many factors can reduce the accuracy and reliability of UWB localization, for either two-way ranging (TWR) or time-difference-of-arrival (TDOA) measurements. Non-line-of-sight (NLOS) and multi-path radio propagation can lead to erroneous, spurious measurements (so-called outliers). Even line-of-sight (LOS) UWB measurements exhibit error patterns (i.e., bias), which are typically caused by the UWB antenna’s radiation characteristics. In our recent work, we present an M-estimation-based robust Kalman filter to reduce the influence of outliers and achieve robust UWB localization. We contributed an implementation of the robust Kalman filter for both TWR and TDOA (PR #707 and #745) to Bitcraze’s crazyflie-firmware open-source project.

Methodology

The conventional Kalman filter, a primary sensor fusion mechanism, is sensitive to measurement outliers due to its minimum mean-square-error (MMSE) criterion. To achieve robust estimation, it is critical to properly handle measurement outliers. We implement a robust M-estimation method to address this problem. Instead of using a least-squares, maximum-likelihood cost function, we use a robust cost function to downweigh the influence of outlier measurements [1]. Compared to Random Sample Consensus (RANSAC) approaches, our method can handle sparse UWB measurements, which are often a challenge for RANSAC.

From the Bayesian maximum-a-posteriori perspective, the Kalman filter state estimation framework can be derived by solving the following minimization problem:

Therein, xk and yk are the system state and measurements at timestep k. Pk and Rk denote the prior covariance and measurement covariance, respectively.  The prior and posteriori estimates are denoted as xk check and xk hat and the measurement function without noise is indicated as g(xk,0). Through Cholesky factorization of Pk and Rk, the original optimization problem is equivalent to

where ex,k,i and ey,k,j are the elements of ex,k and ey,k. To reduce the influence of outliers, we incorporate a robust cost function into the Kalman filter framework as follows:

where rho() could be any robust function (G-M, SC-DCS, Huber, Cauchy, etc.[2]).

By introducing a weight function for the process and measurement uncertainties—with e as input—we can translate the optimization problem into an Iteratively Reweighted Least Squares (IRLS) problem. Then, the optimal posteriori estimate can be computed through iteratively solving the least-squares problem using the robust weights computed from the previous solution. In our implementation, we use the G-M robust cost function and the maximum iteration is set to be two for computational reasons. For further details about the robust Kalman filter, readers are referred to our ICRA/RA-L paper and the onboard firmware (mm_tdoa_robust.c and mm_distance_robust.c).

Performance

We demonstrate the effectiveness of the robust Kalman filter on-board a Crazyflie 2.1. The Crazyflie is equipped with an IMU and an LPS UWB tag (in TDOA2 mode). With the conventional onboard extended Kalman filter, the drone is affected by measurement outliers and jumps around significantly while trying to hover. In contrast, with the robust Kalman filter, the drone shows a more reliable localization performance.

  • Conventional extended Kalman filter
  • M-estimation-based robust Kalman filter
Quadcopter is commanded to hover. UWB TDOA-based localization performance of standard method on-board a Crazyflie 2.1 quadcopter (left) and the proposed robust Kalman filter (right).

The robust Kalman filter implementations for UWB TWR and TDOA localization have been included in the crazyflie-firmware master branch as of March 2021 (2021.03 release). This functionality can be turned on by setting a parameter (robustTwr or robustTdoa) in estimator_kalman.c. We encourage LPS users to check out this new functionality.

As we mentioned above, off-the-shelf, low-cost UWB modules also exhibit distinctive and reproducible bias patterns. In our recent work, we devised experiments using the LPS UWB modules and showed that the systematic biases have a strong relationship with the pose of the tag and the anchors as they result from the UWB radio doughnut-shaped antenna pattern. A pre-trained neural network is used to approximate the systematic biases. By combining bias compensation with the robust Kalman filter, we obtain a lightweight, learning-enhanced localization framework that achieves accurate and reliable UWB indoor positioning. We show that our approach runs in real-time and in closed-loop on-board a Crazyflie nano-quadcopter yielding enhanced localization performance for autonomous trajectory tracking. The dataset for the systematic biases in UWB TDOA measurements is available on our Open-source Code & Dataset webpage. We are also currently working on a more comprehensive dataset with IMU, UWB, and optical flow measurements and again based on the Crazyflie platform. So stay tuned!

Reference

[1] L. Chang, K. Li, and B. Hu, “Huber’s M-estimation-based process uncertainty robust filter for integrated INS/GPS,” IEEE Sensors Journal, 2015, vol. 15, no. 6, pp. 3367–3374.

[2] K. MacTavish and T. D. Barfoot, “At all costs: A comparison of robust cost functions for camera correspondence outliers,” in IEEE Conference on Computer and Robot Vision (CRV). 2015, pp. 62–69.

Links

The authors are with the Dynamic Systems Lab, Institute for Aerospace Studies, University of Toronto, Canada, and affiliated with the Vector Institute for Artificial intelligence in Toronto.

Feel free to contact us if you have any questions or suggestions: wenda.zhao@robotics.utias.utoronto.ca.

Please cite this as:

<code>@ARTICLE{Zhao2021Learningbased,
author={W. {Zhao} and J. {Panerati} and A. P. {Schoellig}},
title={Learning-based Bias Correction for Time Difference of Arrival Ultra-wideband Localization of Resource-constrained Mobile Robots},
journal={IEEE Robotics and Automation Letters},
volume={6},
number={2},
pages={3639-3646},
year={2021},
publisher={IEEE}
doi={10.1109/LRA.2021.3064199}}
</code>

Crazyflie logging and parameters interface

  2021-05-31  |     |    Leave a comment

Background

In the past couple of weeks we have been busy trying to improve the development interface of the Crazyflie. We want to make developing with and for the platform a more pleasant experience.

We have started looking at the logging- and parameter framework and how to improve it for our users. The aim of this framework is to easily be able to log data from the Crazyflie and to set variables during runtime. Your application can use them to control the behavior of the platform or to receive data about what it is currently up to. As of today, in the firmware there are 227 parameters and 467 logging variables defined.

View from the cfclient of the different logging variables one could subscribe to

These logging variables and parameters have been added to the Crazyflie firmware over the course of years. Some are critical infrastructure, needed to be able to write proper applications that interface with the platform. Some are duplicates or were added as debug years ago. Others have in some way outlived their usefulness as the firmware and functionality has moved on. The problem is that we have no way of conveying this information to our users and this is what we are trying to rectify.

An attempt of stability

We are currently reviewing all of our logging variables and parameters in an attempt to make the situation clearer for our users … and ourselves. We are adding documentation to make the purpose of each individual parameters and logging variables more clear. And we are also dividing them up into two categories: core and non-core.

If a parameter or logging variable is marked as core in the firmware that constitutes a promise that we will try very hard to not remove, rename or in any other way change the behavior of it. The idea is that this variable or parameter can be used in applications without any fear or doubt about it going away.

If a variable or parameter is non-core it does not mean that it is marked for removal. But, it could mean that we need more time to make sure that it is the proper interface for the platform. It means that it could change in some way or in some cases be removed in later firmware releases.

The reason for doing this is twofold: we want to make the Crazyflie interface clearer for our users and we want something that we feel we can maintain and keep an up-to-date documentation of.

What is the result?

We have introduced a pair of new macros to the firmware, LOG_ADD_CORE and PARAM_ADD_CORE which can be used to mark a parameter or variable as core. When using these we also mandate that there should be a Doxygen comment attached to the macro.

Below is an example from the barometer log group, showing the style of documentation expected and how to mark a logging variable as core. Parameters gets treated in the same way.

/**
 * Log group for the barometer
 */
LOG_GROUP_START(baro)

/**
 * @brief Altitude above Sea Level [m]
 */
LOG_ADD_CORE(LOG_FLOAT, asl, &sensorData.baro.asl)

/**
 * @brief Temperature [degrees Celsius]
 */
LOG_ADD(LOG_FLOAT, temp, &sensorData.baro.temperature)

/**
 * @brief Air pressure [mbar]
 */
LOG_ADD_CORE(LOG_FLOAT, pressure, &sensorData.baro.pressure)

LOG_GROUP_STOP(baro)

We have also added a script In the firmware repository: elf_sanity.py. The script will return data about parameters and logging variables that it is included in a firmware elf. This can be used to count the number of core parameters. If we point it to a newly built Crazyflie elf, after we’ve done our initial review pass of the parameters and variables, we get the result below.

$ python3 tools/build/elf_sanity.py --core cf2.elf 
101 parameters and 78 log vars in elf

To produce a list of the parameters and variables you can add the --list-params and --list-logs options to the script.

What is the next step?

Once we have finished our review of the parameters and logging variables we will explore different ways of making the documentation of them available in a clear and accessible way. And we will come up with a scheme for making changes to the set of parameters and variables. Once this is all finished you can expect an update from us.

The end goal of our efforts is making developing for the Crazyflie a smoother process, and we would love to hear from you. What is confusing? What are your pain points? Let us know! So we can do better.

Positioning System Overview

  2021-05-24  |     |    Leave a comment

As you have noticed, we talk about the lighthouse positioning a lot these last couple of months ever since we got it out of early release. However, it is good to realize that it is not the only option out there for positioning your Crazyflie! That is why in this blog-post we will lay out possible options and explain how they are different/similar to one another.

The four possible ways to position the crazyflie

Absolute Positioning / Off-board Pose Estimation

Absolute Positioning and External Pose Estimation with the MoCap System

The first we will handle are the use of motion capture systems (MoCap), which resolves around the use of InfraRed cameras and Markers. We use the Qualysis camera ourselves but there are also labs out there that use Vicon or Optitrack. The general idea is that the cameras have an IR-light-emitting LED ring, which are bounced back by reflective markers that are supposed to be on the Crazyflie. These markers can therefore be detected by the same cameras, which pass through the marker positions to an external computer. This computer will have a MoCap program running which will turn these detected markers into a Pose estimate, which will in turn be communicated to the Crazyflie by a Crazyradio PA.

Since that the positioning is estimated by an external computer instead of onboard of the crazyflie, a MoCap positioning system is categorized as an off-board pose estimation using an absolute positioning system. For more information, please check the Motion Capture positioning documentation.

Absolute Positioning / On-board Pose Estimation

Absolute Positioning and Internal Pose Estimation with the Lighthouse and Loco Positioning System

The next category is a bit different and it consists of both the Loco positioning system and the Lighthouse positioning system. Even though these both use beacons/sensors that are placed externally of the Crazyflie, the pose estimation is done all on-board in the firmware of the Crazyflie. So there is no computer that is necessary to communicate the position back to the Crazyflie. Remember that you do need to communicate the reference set-points or high level commands if you are not using the App layer.

Of course there are clear differences in the measurement type. A Crazyflie with the Locodeck attached takes the distance to the externally placed nodes as measured by ultra wide band (UWB) and the Lighthouse deck detects the light plane angles emitted by the Lighthouse Base Stations. However the principle is the same that those raw measurements are used as input to the Extended Kalman filter onboard of the Crazyflie, and outputs the estimated pose after fusing with the IMU measurements.

Therefore these systems can be classified as absolute positioning systems with on-board pose estimation. To learn more please read the Loco and Lighthouse positioning system documentation!

Relative Positioning / On-board Pose Estimation

Relative Positioning and Internal Pose Estimation with the Flowdeck V2.

It is not necessary to have to setup an external positioning system in your room in order to achieve a form of positioning on the Crazyflie. With the Flowdeck attached, the Crazyflie can measure flows per frame with an optical flow sensor and the height in millimetres with a time of flight sensor. These measurements are then fused together with the IMU within the Extended Kalman filter (see the Flow deck measurement model), which results in a on-board pose estimation.

The most important difference here to note is that positioning estimated by only the Flowdeck, will not result in a absolute positioning estimate but a relative one. Instead of using an external placed system (like MoCap, Lighthouse and Loco) which dictate where the zero position is in XYZ, the start-up position the Crazyflie determines where the origin of the coordinate system is. That is why the Flowdeck is classified as a Relative Positioning System with On-board Pose Estimation.

IMU-only On-board Pose Estimation ?

Oh boy… that is a different story. Theoretically it could be possible by using the onboard accelerometers of the crazyflie and fusing those in some short of estimator, however practice has shown that the Crazyflie’s accelerometers are too noisy to result in any good pose estimation… We haven’t seen any work that has been successfully to achieve any stable hover on only the IMU of the Crazyflie, but if you have done/see research that has, please let us know!

And if you would like to give a go yourself and build an estimator that is able to do this, please check out the new out of tree build functionality for estimators. This is still work in progress so it might have some bugs, but it should enable you to plugin in your own estimator separate from the Crazyflie firmware ;)

Documentation

We try to keep keep all the information of all our positioning systems on our website. So check out the positioning system overview page to be referred to more details if you would be interested in a particular system that fits your requirements!

Switching to Crazyflie-link-Cpp by default

  2021-05-10  |     |    Leave a comment

A little while ago we made a blog post talking about the communication reliability, one part of this work did include an alternative CRTP link implementation for the Crazyflie lib. This week we will make one of the native CRTP link implementations, the Crazyflie-link-Cpp, the default link for the Crazyflie python lib.

In the Crazyflie communication stack, the CRTP link is the piece of software that handles reliable packet communication between a computer and the Crazyflie. In the Crazyflie it is mainly implemented in the nRF51 radio chip (for the radio link), in the computer it is part of the Crazyflie python lib + Crazyflie client and for Ros and Crazyswarm it is implemented in crazyflie-cpp. Other clients like Lamouchefolle also have re-implemented their own versions of the CRTP link.

The CRTP link is the most critical part in the communication with Crazyflie, if it is not working properly, nothing can work properly, all communication with the Crazyflie passes though the CRTP link. This code duplication in multiple projects means that we have many places for diverging behaviour and creative bugs which makes it hard to develop new clients for the Crazyflie.

One way we are trying to solve this problem is to unify the links: let’s try to use the same link in all the clients. In order to do so we need a link that can act as a minimum common denominator: it needs to work with Python in order to accommodate the Crazyflie Python lib, work with C++ for Crazyswarm and LaMoucheFolle and be high-performance to be able to handle swarms of Crazyflies. The C++ link implementation can full-fill all these requirements using a python binding, and it will give higher performance to the Python lib.

Switching to the native C++ link will give us a lot of benefits:

  • It is higher performance, it has about 20% higher packet throughput than the pure python implementation
  • It will unify the Python lib and Crazyswarm a bit more. Crazyswarm will be able to use the same link as soon as broadcast packet support is added.
  • The Cpp link supports dynamic allocation of radios, no need to choose what radio to use for each connection, the link will distribute connections over all available radios by using a star instead of the radio number (radio://*/80/2M) in the URI.

The biggest drawback so far is that the CPP link needs to be compiled for each platform. We are compiling a python Wheel for it in Github actions for Windows/Mac/Linux on X86_64, any other architecture (including raspberry pi) will use the existing python CRTP link for the time being.

We are in the process of enabling the new link as the default in the lib. There are still some outstanding bugs related to boot loading that needs to be ironed out, but we expect this change to be included in the next release.

Transitioning back to Windows Development

  2021-04-05  |     |    Leave a comment

When I was started my Robotics Master back in 2012, I remembered how frustrated I was at the time to setup my development environment in Windows for the C++ beginners course. My memory is a bit fuzzy of course but I remembered it took me days to get all the right drivers, g++ libraries right, and to setup all in the path environmental in Eclipse at the time. Once I started working on Ubuntu for my Master thesis, forced to due to ROS, I was hooked and swore I will never go back to Windows for robotics development again… until now!

I always used Windows on my personal machine on the side for gaming and have a dual-boot on the work computer for some occasional video editing, but especially I had begun to learn game development for Fun Fridays, I started to be drawn to the windows side of the dual boot more and more. But if I needed to try something out on the Crazyflie or needed to debug a problem on the forum, I needed to restart the computer to switch operating systems and that was starting to become a pain! Slowly but steadily I tried out several aspects of the crazyflie ecosystem for development on Windows 10 and actually…. it is not so traumatic as it was almost 10 years ago.

Python Library and Client

It went quite smooth when I first try to install python on windows again. Adding it to the PATH environment variable is still very important but luckily the new install manager provides that as an option. Moreover, Visual Studio Code also provides the possibility to switch between python environments so that you try different versions of python, but for now version 3.8 was plenty for me.

With the newest versions of the Windows install of Python, pip is by definition already installed, but I experienced that it would still be necessary to upgrade pip by typing the following in either a Command Prompt or (my favorite) Powershell:

python -m pip install --upgrade pip

After this, install the cflib from release was quite an ease (‘pip install cflib’) but even installing it from source with Git configured on Windows was no problem at all and very similar to a native Ubuntu install.

Until recently the cfclient was a bit more challenging to install through pip from due the SDL2 windows library had to be downloaded separately, so the only options would have been installing from source or the .exe application release. The later has not been updated since the 2020.09 release due to building errors. Luckily, with the latest release, this has now been fixed as a SLD2 python library was found. Now the cfclient can be installed with a simple ‘pip install cfclient’.

Firmware Building with WSL

The firmware development was the next thing that I tried to get up and running, which managed to be slightly trickier. About a year ago I tried to get Cygwin to work on Windows, but my bad memories of the past came back due to the clunkiness of it all and I abandoned it again. Also there are some reported issues with the out-of-tree build (aka the App layer). Some colleagues at Bitcraze already mentioned the Windows Subsystem for Linux (WSL) but I never really looked at it until the need came to move back to Windows for development. And I must say, I wish I had tried it out a while ago.

With some repositories downloaded already on my Windows system with Git, I installed Ubuntu 20.04 WSL, got the appropriate gcc libraries and accessed the crazyflie-firmware by ‘cd /mnt/c/my/repos‘. Building the firmware with ‘make all’ went pretty okay… although it took about a minute which is a little long compared to the 20 secs on the native Ubuntu install. The big problem was that I could not use Docker and the handy bitcraze toolbelt due to the WSL version still being 1. These functionalities were only available for version 2 so I went ahead and upgraded the WSL and linked it to docker desktop. But after upgrading, building the firmware from that same repository on the C:/ drive took insanely long (almost 10 minutes). So I switched back the WSL ubuntu 20.04 to version 1, installed a second WSL (this time Ubuntu 18.04), updated that one to WSL2 and used solely for docker and toolbelt purposes. Not ideal quite yet… but luckily with visual studio code it is very easy to switch the WSL .

But there is more though! Recently I timed how long it took to build the crazyflie-firmware with ‘make all -j8’ from both WSL version in a repository that is on the C:/ drive on Windows (accessible by /mnt/c from the WLS), or from a repository on the local file system:

  • WSL 2 (ubuntu 18.04)
    • C:/ = 11m06s
    • WSL local = 00m19s
  • WSL 1 (ubuntu 20.04)
    • C:/ = 01m04s
    • WSL local = 00m59s

This is done on an Windows laptop with an i7-6700HQ with 32 Gb RAM. The differences with WLS2 between build firmware on the windows file system or the local WSL file system is huge! So that means that the right way is to have WSL2 with the repo on the WSL file system, which is similar to build time as a native install of Ubuntu.

Flashing the Crazyflie

The main issue still with WSL is that it does not allow USB access… So even if the crazyradio driver is installed on windows with Zadig, you will not be able to see if you type ‘lsusb‘ in WSL for both version 1 and 2. So when I still had the repository on the C:/ drive and build the crazyflie-firmware from there I could flash the Crazyflie through the Cfclient or Cflib (with cfloader) through Powershell, but building it from the local subsystem, which is way faster for WSL2, would require to first copy the cf2.bin file to my C drive before doing that.

Another option, although still in Alpha phase, is to use the experimental Crazyradio server for WSL made by Arnaud, for which the user instructions can be found in an issue thread only for now. The important thing is that the Zadig installed driver has to be switched to WinUSB and switched back again to LibUSB if you want to use the Cfclient on windows. It would still needs some work to improve the user experience but gives promise of better integration of WSL development for the Crazyflie.

To Conclude

Soon I’m planning to soon reinstall the Windows part on the dual boot laptop but there are already some things that I will integrate on my freshly installed Windows based on what I experienced so far:

  • Keep using Python on windows and install the Cfclient and Cflib by pip only.
  • Only use Ubunu 20.04 as WSL2 and install the Crazyflie-firmware on the WSL local file system.
  • Use Visual Studio Code as the editor for both C:/ based and WSL based repos.
  • Use the Crazyradio server or copy the bin file to C:/ whenever I need to flash the crazyflie with development firmware.

For any AIdeck development, I would still need to use the native Ubuntu part or the bitcraze-VM since there is not a USB access or server yet for the programmer. However, if Windows would support USB devices and a graphical interface for WSL, that will make all our Windows-based Crazyflie development dreams come true!

Release 2021.03: Lighthouse is stable!

  2021-03-29  |     |    4 Comments

We are happy to anounce the availability of the 2021.03 release of the Crazyflie firmware and client! This release includes new binaries for the Crazyflie (2021.03), the Crazyflie client 2021.03 as well as the Crazyflie python library 0.1.13.2. The firmware package can be downloaded from the Crazyflie release repository (2021.03) or can be flashed directly using the client bootloader window. The firmware package contains the STM firmware (2021.03), the NRF firmware (2021.03) and the Lighthouse deck FPGA binary (V6).

The main feature in this release is the stabilization of the Lighthouse positioning system. The main work done has been on the system setup and management, it has taken a lot of work spawning all the projects and a brunch of documentation, but we think we have reached a stage where the lighthouse positioning system is working very well and is very easy to setup and get working. We have now published the new Lighthouse getting started guide and will be working this week at updating all required materials to mark Lighthouse as released!

When the Lighthouse positioning system was released in early access, it required to install SteamVR, run some custom scripts and flash a modified firmware to get up and running. This has been improved slightly over time with scripts that allows to setup the system without using SteamVR and some way to store the required system data in the Crazyflie configuration memory rather than hard-coded in the firmware. With this release, everything is coming together and it is now possible to go from zero to an autonomous Crazyflie flying in a lighthouse system in minutes by just using the Crazyflie client.

  • Lighthouse positioning setup and management tab in the Crazyflie client

Another major improvement made to support the lighthouse is the modification of the bootloader Crazyflie update sequence in the client as well as in command line. The new sequence will restart the Crazyflie a couple of times while upgrading the Crazyflie, this allows for an upgrade of the firmware in the installed decks if required. The lighthouse deck firmware has been added to the Crazyflie .zip release file and will be flashed into the deck while flashing the release to a Crazyflie that has the deck installed.

An alternative, robust TDoA implementation has been added for the Loco Positioning System. This change has been contributed by williamwenda on Github and can optionally be enabled at runtime.

An event subsystem has also been added to the firmware. It allows to log events onto the SD-Card which can be very useful when acquiring positioning data from the various positioning system supported by the Crazyflie. We have described this subsystem in an earlier blog post.

There has also been a lot of smaller improvement and bugfixes in this release. See the individual project releases not for more information.

We hope you are going to enjoy this new Crazyflie and lighthouse release. Do not hesitate to drop a comment here, questions on the forum if you have any or bug reports of github in the (very unlikely ;-) event that there are bugs left.