Category: Video

Home / Blog

The Hyper Demo

  2020-12-14  |     |    2 Comments

This autumn when we had our quarterly planing meeting, it was obvious that there would not be any conferences this year like other years. This meant we would not meet you, our users and hear about your interesting projects, but also that we would not be forced to create a demo. Sometimes we joke that we are working with Demo Driven Development and that is what is pushing us forward, even-though it is not completely true it is a strong driver. We decided to create a demo in our office and share it online instead, we hope you enjoy it!

The wish list for the demo was long but we decided that we wanted to use multiple positioning technologies, multiple platforms and multiple drones in a swarm. The idea was also to let the needs of the demo drive development of other technologies as well as stabilize existing functionality by “eating our own dogfood”. As a result of the work we have for instance:

  • improved the app layer in the Crazyflie
  • Lighthouse V2 support, including basic support for 2+ base stations
  • better support for mixed positioning systems

First of all, let’s check out the video

We are using our office for the demo and the Crazyflies are essentially flying a fixed trajectory from our meeting room, through the office and kitchen to finally land in the Arena. The Crazyflies are autonomous from the moment they take off and there is no communication with any external computer after that, all positioning is done on-board.

Implementation

The demo is mainly implemented in the Crazyflie as an app with a simple python script on an external machine to start it all. The app is identical in all the Crazyflies so the script tells them where to land and checks that all Crazyflies has found their position before they are started. Finally it tells them to take off one by one with a fixed delay in-between.

The Crazyflie app

When the Crazyflie boots up, the app is started and the first thing it does is to prepare by defining a trajectory in the High Level Commander as well as setting data for the Lighthouse base stations in the system. The app uses a couple of parameters for communication and at this point it is waiting for one of the parameters to be set by the python script.

When the parameter is set, the app uses the High Level Commander to take off and fly to the start point of the trajectory. At the starting point, it kicks off the trajectory and while the High Level Commander handles the flying, the app goes to sleep. When reaching the end of the trajectory, the app once more goes into action and directs the Crazyflie to land at a position set through parameters during the initialization phase.

We used a feature of the High Level Commander that is maybe not that well known but can be very useful to make the motion fluid. When the High Level Commander does a go_to for instance, it plans a trajectory from its current position/velocity/acceleration to the target position in one smooth motion. This can be used when transitioning from a go_to into a trajectory (or from go_to to go_to) by starting the trajectory a little bit too early and thus never stop at the end of the go_to, but “slide” directly into the trajectory. The same technique is used at the end of the trajectory to get out of the way faster to avoid being hit by the next Crazyflie in the swarm.

The trajectory

The main part of the flight is one trajectory handled by the High Level Commander. It is generated using the uav_trajectories project from whoenig. We defined a number of points we wanted the trajectory to pass through and the software generates a list of polynomials that can be used by the High Level Commander. The generated trajectory is passing through the points but as a part of the optimization process it also chooses some (unexpected) curves, but that could be fixed with some tweaking.

The trajectory is defined using absolute positions in a global coordinate system that spans the office.

Positioning

We used three different positioning systems for the demo: the Lighthouse (V2), the Loco Positioning system (TDoA3) and the Flow deck. Different areas of the flight space is covered by different system, either individually or overlapping. All decks are active all the time and pick up data when it is available, pushing it into the extended Kalman estimator.

In the meeting room, where we started, we used two Lighthouse V2 base stations which gave us a very precise position estimate (including yaw) and a good start. When the Crazyflies moved out into the office, they only relied on the Flowdeck and that worked fine even-though the errors potentially builds up over time.

When the Crazyflies turned around the corner into the hallway towards the kitchen, we saw that the errors some times were too large, either the position or yaw was off which caused the Crazyflies to hit a wall. To fix that, we added 4 LPS nodes in the hallway and this solved the problem. Note that all the 4 anchors are on the ground and that it is not enough to give the Crazyflie a good 3D position, but the distance sensor on the Flow deck provides Z-information and the overall result is good.

The corner when going from the kitchen into the Arena is pretty tight and again the build up of errors made it problematic to rely on the Flow deck only, so we added a lighthouse base station for extra help.

Finally, in the first part of the Arena, the LPS system has full 3D coverage and together with the Flow deck it is smooth sailing. About half way the Crazyflies started to pick up the Lighthouse system as well and we are now using data from all three systems at the same time.

Obviously we were using more than 2 basestations with the Lighthouse system and even though it is not officially supported, it worked with some care and manual labor. The geometry data was for instance manually tweaked to fit the global coordinate system.

The wall between the kitchen and the Arena is very thick and it is unlikely that UWB can go through it, but we still got LPS data from the Arena anchors occasionally. Our interpretation is that it must have been packets bouncing on the walls into the kitchen. The stray packets were picked up by the Crazyflies but since the Lighthouse base station provided a strong information source, the LPS packets did not cause any problems.

Firmware modifications

The firmware is essentially the stock crazyflie-firmware from Github, however we did make a few alterations though:

  • The maximum velocity of the PID controller was increased to make it possible to fly a bit faster and create a nicer demo.
  • The number of lighthouse base stations was increased
  • The PID controller was tweaked for the Bolt

You can find the source code for the demo on github. The important stuff is in examples/demos/hyperdemo

The hardware

In the demo we used 5 x Crazyflie 2.1 and 1 x Bolt very similar to the Li-Ion Bolt we built recently. The difference is that this version used a 2-cell Li-Po and lower KV motors but the Li-Ion Bolt would have worked just as well.

Hyperdemo drones and they configurations

To make all positioning to work at the same time we needed to add 3 decks, Lighthouse, Flow v2 and Loco-deck. On the Crazyflie 2.1 this fits if the extra long pin-headers are used and the Lighthouse is mounted on top and the Loco-deck underneath the Crazyflie 2.1 with the Flow v2 on the bottom. The same goes for the Bolt, but here we had to solder the extra long pin-header and the long pin-header together to make them long enough.

There is one catch though… the pin resources for the decks collide. With some patching of the loco-deck this can be mitigated by moving its IRQ to IO_2 using the solder-jumper. The RST needs to be moved to IO_4 which requires a small patch wire.

Also some FW configuration is needed which is added to the hyperdemo makefile:

CFLAGS += -DLOCODECK_USE_ALT_PINS
CFLAGS += -DLOCODECK_ALT_PIN_RESET=DECK_GPIO_IO4

The final weight for the Crazyflie 2.1 is on the heavy side and we quickly discover that fully charged batteries should be used or else the crash probability is increased a lot.

Conclusions

We’re happy we were able to set this demo up and that it was fairly straight forward. The whole setup of it was done in one or two days. The App layer is quite useful and we tend to use it quite often when trying out ideas, which we interpret as a good sign :-)

We are satisfied with the results and hope it will inspire some of you out there to push the limits even further!

An Efficient Real-Time NMPC for Quadrotor Position Control under Communication Time-Delay

  2020-11-16  |     |    Leave a comment

This week we have a guest blog post from Bárbara Barros Carlos, PhD candidate at DIAG Robotics Lab. Enjoy!

Quadrotors are characterized by their underactuation, nonlinearities, bounded inputs, and, in some cases, communication time-delays. The development of their maneuvering capability poses some challenges that cover dynamics modeling, state estimation, trajectory generation, and control. The latter, in particular, must be able to exploit the system’s nonlinear dynamics to generate complex motions. However, the presence of communication time-delay is known to highly degrade control performance.

A composite image showing our real-time NMPC with time-delay
compensation being used on the Crazyflie during the tracking of
a helical trajectory.

In our recent work, we present an efficient position control architecture based on real-time nonlinear model predictive control (NMPC) with time-delay compensation for quadrotors. Given the current measurement, the state is predicted over the delay time interval using an integrator and then passed to the NMPC, which takes into account the input bounds. We demonstrate the capabilities of our architecture using the Crazyflie 2.1 nano-quadrotor.

Time-Delay Compensation

In our aerial system, because of the radio communication latency, we have delays both in receiving measurements and sending control inputs. Likewise, since we intend to use NMPC, the potentially high computational burden associated with its solution becomes an element that must also be taken into account to minimize the error in the state prediction.

Crazyflie NMPC response without
considering the time-delay compensation.

To tackle this issue, we use a state predictor based on the round-trip time (RTT) associated with the sum of network latencies as a delay compensator. The prediction is computed by performing forward iterations of the system dynamic model, starting from the current measured state and over the RTT, through an explicit Runge Kutta 4th order (ERK4) integrator. Due to the independent nature of this operation, perfect delay compensation can be achieved by adjusting the integration step to be equal to the RTT. Thus, it is assumed that there is a fixed RTT, defined by τr, to be compensated.

Nonlinear Model Predictive Control

The NMPC controller is defined as the following constrained nonlinear program (NLP):

Therein, p denotes the inertial position, q the attitude in unit quaternions, vb the linear velocity expressed in the body frame, ω the angular rate, and Ωi the rotational speed of the ith propeller. The NLP is tailored to the Crazyflie 2.1 and is implemented using the high-performance software package acados, which solves optimal control problems and implements a real-time iteration (RTI) variant of a sequential quadratic programming (SQP) scheme with Gauss-Newton Hessian approximation. The quadratic subproblems (QP) arising in the SQP scheme are solved with HPIPM, an interior-point method solver, built on top of the linear algebra library BLASFEO, finely tuned for multiple CPU architectures. We use a recently proposed Hessian condensing algorithm particularly suitable for partial condensing to further speed-up solution times.

When designing an NMPC, choosing the horizon length has profound implications for computational burden and tracking performance. For the former, the longer the horizon, the higher the computational burden. As for the latter, in principle, a long prediction horizon tends to improve the overall performance of the controller. In order to select this parameter and achieve a trade-off between performance and computational burden, we implemented the NLP in acados considering: five horizon lengths (N = {10,20,30,40,50}), input bounds on the rotational speed of the propellers (lower bound = 0, upper bound = 22 krpm), discretizing the dynamics using an ERK4 integration scheme. Likewise, we compare the condensing approach with the state-of-the-art solver qpOASES against the partial condensing approach with HPIPM, concerning the set of horizons regarded.

Left: closed-loop trajectories comparing different horizon lengths.
Right: average runtimes per SQP-iteration
for different horizon lengths considering two distinct QP solvers.

As qpOASES is a solver based on active-set method, it requires condensing to be computationally efficient. In line with the observations found in the literature that condensing is effective for short to medium horizon lengths, we note that qpOASES is competitive for horizons up to approximately N = 30 when compared to HPIPM. The break-even point moves higher on the scale for longer horizons, mainly due to efficient software implementations that cover: (a) Hessian condensing procedure tailored for partial condensing, (b) structure-exploiting QP solver based on novel Riccati recursion, (c) hardware-tailored linear algebra library. Therefore, we chose horizon N = 50 as it offers a reasonable trade-off between deviation from the reference trajectory and computational burden.

Onboard Controller Considerations

How the onboard controllers (PIDs) use the setpoints of the offboard controller (NMPC) in our architecture is not entirely conventional and, thereby, deserves some considerations. First, the reference signals that the PID loops track do not fully correspond to the control inputs considered in the NMPC formulation. Instead, part of the state solution is used in conjunction with the control inputs to reconstruct the actual input commands passed as a setpoint to the Crazyflie. Second, a part of the reconstructed input commands is sent as a setpoint to the outer loop (attitude controller), and the other part is sent to the inner loop (rate controller). Furthermore, as the NMPC model does not include the PID loops, it does not truly represent the real system, even in the case of perfect knowledge of the physical parameters. As a consequence, the optimal feedback policy is distorted in the real system by the PIDs. 

Closed-loop Position Control Performance

Our control architecture hinges upon a ROS Kinetic framework and runs at 66.67 Hz. The Crazy RealTime Protocol (CRTP) is used in combination with our crazyflie_nmpc stack to stream in runtime custom packages containing the required data to reconstruct the part of the measurement vector that depends on the IMU data. Likewise, the cortex_ros bridge streams the 3D global position of the Crazyflie, which is then passed through a second-order, discrete-time Butterworth filter to estimate the linear velocities.

To validate the effectiveness of our control architecture, we ran two experiments. For each experiment, we generate a reference trajectory on a base computer and pass it to our NMPC ROS node every τs = 15 ms. When generating the trajectories, we explicitly address the feasibility issue in the design process, creating two references: one feasible and one infeasible. In addressing this issue, we prove through experiments that the performance of the proposed NMPC is not degraded even when the nano-quadrotor attempts to track an infeasible trajectory, which could, in principle, make it deviate significantly or even crash.

Overall, we observe that the most challenging setpoints to be tracked are the positions in which, given a change in the motion, the Crazyflie has to pitch/roll in the opposite direction quickly. These are the setpoints where the distortion has the greatest influence on the system, causing small overshoots in position. The average solution time of the tailored RTI scheme using acados was obtained on an Intel Core i5-8250U @ 3.4 GHz running Ubuntu and is about 7.4 ms. This result shows the efficiency of the proposed scheme.

Outlook

In this work, we presented the design and implementation of a novel position controller based on nonlinear model predictive control for quadrotors. The control architecture incorporates a predictor as a delay compensator for granting a delay-free model in the NMPC formulation, which in turn enforces bounds on the actuators. To validate our architecture, we implemented it on the Crazyflie 2.1 nano-quadrotor. The experiments demonstrate that the efficient RTI-based scheme, exploiting the full nonlinear model, achieves a high-accuracy tracking performance and is fast enough for real-time deployment.

Related Links

This research project was developed by:

Bárbara Barros Carlos1, Tommaso Sartor2, Andrea Zanelli3, and Gianluca Frison3, under the supervision of professors Wolfram Burgard4, Moritz Diehl3 and Giuseppe Oriolo1.

1 B. B. Carlos and G. Oriolo are with the DIAG Robotics Lab, Sapienza University of Rome, Italy.
2 T. Sartor is with the MECO Group, KU Leuven, Belgium.
3 A. Zanelli, G. Frison, and M. Diehl are with the syscop Lab, University of Freiburg, Germany.
4 W. Burgard is with the AIS Lab, University of Freiburg, Germany.

Testing Crazyflie Bolt and 1 cell Li-Ion

  2020-10-19  |     |    15 Comments

Li-Ion batteries have packed more energy per gram for a long time compared to Li-Po batteries. The problem for UAV applications has been that Li-Ion can’t deliver enough current, something that is starting to change. Now there are cells that are supposed to be able to deliver 30-35A continuously in the 18650 series, at least according to the specs. Therefor we thought it was time to do some testing and decided to build a 1 cell Li-Ion drone using the Crazyflie Bolt as base.

Since a 18650 battery is 18mm in diameter and 65mm long, the size would affect the design but we still wanted to keep the drone small and lightweight. The battery is below 20mm wide which means we can run the deck connectors around it, that is nice. We chose to use our 3D printer to build the frame and use off the shelf ESCs, motors and props. After a couple of hours of research we selected 3″ propellers, 1202.5 11500kv motors and tiny 1-2s single ESCs for our first prototype.

Parts list:

  • 1 x Custom designed 130mm 3D printed frame
  • 1 x Crazyflie Bolt flight controller
  • 4 x Eachine 3020 propeller (2xCW + 2xCCW)
  • 4 x Flywoo ROBO RB 1202.5 11500 Kv motors
  • 4 x Flash hobby 7A 1-2S ESC
  • 1 x Li-Ion Sony 18650 VTC6 3000mAh 30A
  • Screws, anti vib. spacers, zipties, etc.

The custom designed frame was developed in iterations, and can still be much improved, but at this stage it is small, lightweight and rigid enough. We wanted the battery to be as central as possible while keeping it all compact.

Prototype frame designed in FreeCAD.

Assembly and tuning

The 3D printed frame came out quite well and weighed in at 13g. After soldering the bolt connectors to the ESCs, attaching motors and props, adjusting battery cable and soldering a XT30 to the Li-Ion battery it all weighed ~103g and then the battery is 45g of these. It feels quite heavy compared to the Crazyflie 2.1 and we had a lot of respect when we test flew it the first time. Before we took off we reduced the pitch and roll PID gains to roughly half and luckily it flew without problems and quite nicely. Well it sounds a lot but that is kind of expected. After increasing the gains a bit we felt quite pleased with:

#define PID_ROLL_RATE_KP  70.0
#define PID_ROLL_RATE_KI  200.0
#define PID_ROLL_RATE_KD  2
#define PID_ROLL_RATE_INTEGRATION_LIMIT    33.3

#define PID_PITCH_RATE_KP  70.0
#define PID_PITCH_RATE_KI  200.0
#define PID_PITCH_RATE_KD  2
#define PID_PITCH_RATE_INTEGRATION_LIMIT   33.3

#define PID_ROLL_KP  7.0
#define PID_ROLL_KI  3.0
#define PID_ROLL_KD  0.0
#define PID_ROLL_INTEGRATION_LIMIT    20.0

#define PID_PITCH_KP  7.0
#define PID_PITCH_KI  3.0
#define PID_PITCH_KD  0.0
#define PID_PITCH_INTEGRATION_LIMIT   20.0

This would be good enough for what we really wanted to try, the endurance with a Li-Ion battery. A quick measurement of the current consumption at hover, 5.8A, we estimated up to ~30 min flight time on a 3000mAh Li-Ion battery, wow, but first a real test…

Hover test

For the hover test we used lighthouse 2 which is starting to work quite well. We had to change the weight and thrust constants in estimator_kalman.c for the autonomous flight to work:

#define CRAZYFLIE_WEIGHT_grams (100.0f)

//thrust is thrust mapped for 65536 <==> 250 GRAMS!
#define CONTROL_TO_ACC (GRAVITY_MAGNITUDE*250.0f/CRAZYFLIE_WEIGHT_grams/65536.0f)

After doing that and creating a hover script that hovers at 0.5m height and was set to land when the voltage reached 3.0V. We leaned back with excitement, behind a safety net, and started the script… after 19 min it landed… good but not what we hoped for and quite far from the calculated 30 min. Maybe Li-Ion isn’t that good when it needs to provide more current…? A quick internet search and we could find that Li-Ion can run all the way down to 2.5V, but we have to stop at 3.0V because of electronics and loosing thrust, so we are missing quite a bit of energy… Further investigations are needed.

Lighthouse 2 flight test

As a final test we launched some flight scripts to fly in a square and in a spiral so we would get a feel for Lighthouse 2 + Bolt with PID controller combination. We think it turned out quite nicely, and this with almost no optimization effort:

Summary

Li-Ion felt like it could be a game changer when it comes to flight time but was not as promising as we hoped for. It doesn’t mean we can’t get there though. More research and development is required.

New Compilation Video

  2020-09-21  |     |    Leave a comment

It has almost been 2.5 years since we last made a compilation video about what has been done with the crazyflie 2.0 (see this blogpost). During this time we released several new products and many of you were able to achieve very cool and awesome applications so we thought it was time for a new research compilation video!

We have seen quite a lot of projects with swarms of Crazyflies, ranging from close proximity flight to autonomous exploration in a building. Some research groups have also been experimenting with controlling the Crazyflie with our own hands, either to control its position or to reach another state of mind entirely. Others have created their own deck in order to add their own sensors or cameras necessary for their application. One of those even led to the new AI-deck that we introduced in early release before the summer! Last but not least: we were shown that the low level control can be further improved and multiple crazyflies can be linked together and still fly!

We were overwhelmed by all the awesome things that the community showed us of what possible with the Crazyflie and this will inspire others to think of new things to do as well. We hope that we can continue with helping you to make your ideas fly, so that we are soon to be forced to make another completion video ;)

Here is a list of all the research that has been included in the movie:

  • Close proximity flight of sixteen quadrotor drones, CalTech: B. Rivière, W. Hoenig, Y. Yue, and S.-J. Chung (video, paper)
  • Pointing gestures, IDSIA: B. Gromov, J, Guzzi, L. M. Gambardella, A. Giusti (video, project)
  • Yaw actuation, Modlab UPenn: B. Gabrich G. LiM. Yim (video, paper)
  • Learning to seek, Harvard University: B. P. Duisterhof , S. Krishnan, J. J. Cruz, C. R. Banbury, W. Fu, A. Faust, G. C. H. E. de Croon, V. Janapa Reddi (video, paper)
  • Drone Chi, Exertion Labs and RMIT: J. La Delfa, M. Baytas, E. Luke, B. Koder, F. Mueller (video, project)
  • Generalization through simulation, Berkeley AI Research (BAIR): K. Kang, S. Belkhale, G. Kahn, P. Abbeel, S. Levine (video, paper)
  • Swarm exploration, TU Delft: K.N. McGuire, C. De Wagter, K. Tuyls, H. Kappen, G.C.H.E. de Croon (video, paper)
  • PULP-based autonomous drone, ETH Zürich: D. Palossi, F. Conti, and L. Benini (video, paper)
  • Networked Autonomous Aerial Vehicles, University of Klagenfurt, Austria: Research by Karl Popper Kolleg (video)
  • GLAS for Multi-Robot Motion Planning with End-to-End Learning, CalTech: B. Rivière, W. Hoenig, Y. Yue, and S.-J. Chung (video, paper)
  • Resilience by reconfiguration, USC: R. K. Ramachandran, J. A. Preiss, G. S. Sukhatme (video, paper)

New Step-by-Step Guides

  2020-08-03  |     |    Leave a comment

As you probably already know we have been wondering how to best handle our documentation and how to provide information to new Crazyflie starters as easy as possible, as you can read in our blogpost of two weeks ago. In the mean time, we also had a chance to think about the results of a poll we had when we discussed about new ways on how to meet our users. We had about 30 responses, but it became clear that many of you are in the need of getting some more knowledge about working with the Crazyflie. The majority voted for online tutorials, and although it might be difficult to do those during the summer holidays, we already started to make step-by-step guides of various parts of the Crazyflie Eco-system.

Poll result of alternative events.

Python Library Tutorials

Currently we have started with step-by-step tutorials of the CFLIB (the python library of the crazyflie). Usually we refer to the example pages of the CFLIB, however we feel that many users copy paste parts of these scripts for their own purposes, without understanding what is actually going on. Therefore, in order to move Crazyflie beginners to the starting developer phase, we have made these guides in order to teach exactly what is going on in each module, step-by-step.

These tutorials can be found in the python library documentation. The first tutorial focuses on connecting, logging and parameters, which guides you through the process of connecting the Crazyflie through a python script, starting up logging configurations in two ways (asynchronous and synchronous) and how to read and set parameters.

The second tutorial is about the motion commander and is a logical continuation of the first. A nice thing is that we also show how to build in some protection in your script as well. If the Flowdeck is not attached, you can check that and prevent the Crazyflie from taking off altogether if it does not detect the Flowdeck. This will be a life saver in your future endeavors, and trust me I know from experience ;). Afterwards it will go into how to take off – fly forward and go back to the initial position. The application of the end of the motion commander tutorial, is where we also use the logging functionality to get the actual estimated position of the Crazyflie. With this information, we show how to write an application that create a virtual bounding box where the Crazyflie can bounce around in (like the old windows screensaver).

The results of the motion commander step by step guide.

We are planning to finish this step-by-step guide by adding the multi-ranger to the mix, continuing on the bouncing ball example. After that we will probably start some tutorials on how to use the swarming functionality before moving on to the firmware or the client.

Work in Progress

The tutorials are still work in progress. So let us know on the forum, python library github repository or as a comment on this blogpost if you see anything wrong or if something is not very clear. This will improve the quality further so that other users can benefit as well. Also once the these step-by-step tutorials are finished we can start working on video based tutorials as well.

Remember, it is possible to contribute your own fixes (or tutorials) to our repositories if you want to. It’s an open source project after all ;)

Scaling Flying Modular Structures Based on Quadrotors

  2020-07-27  |     |    Leave a comment

Modular robotics implies in general flexibility and versatility to robots. In theory, you could design a modular robot basically on the way you would want it to be, by simply adding or removing modules from the already existing robot. Changing the robot configuration by adding more individuals, generally increases the system redundancy, meaning that now, there are probably many different ways to achieve a specific goal. From a naive standard point of view, more modules could imply in practice more robustness due to this redundancy. In fact, it does get more robust by the cost of becoming more complex, and probably harder to control. Added to that, other issues may arise when you take into account that your modular robot is flying, and how physical properties and actuation scales as the number of modules grow.

In the GRASP Laboratory at the University of Pennsylvania, one of our focuses is to allow robots to achieve a specific task. In this work, we present ModQuad-DoF, which is a modular flying platform that enlarges the configuration space of modular flying structures based on quadrotors (crazyflies), by applying a new yaw actuation method that relies on the desired roll angles of each flying vehicle. This research project is coordinated by Professor Mark Yim , and led by Bruno Gabrich (PhD candidate).

Scaling Modular Robots

Scaling modular robots is a very challenging problem that usually limits the benefits of modularity. The sum of the performance metrics (speed, torque, precision etc.) from each module usually does not scale at the same rate as the conglomerate physical properties. In particular, for ModQuad, saturation from individual motors would increase as the structures became larger leading to failure and instability. When conglomerate systems scale up in the number of modules, the moment of inertia of the conglomerate often grows faster than the increase in thrust capability for each module. For example, the increase in the moment of inertia for a fifth module added to four modules in a line can be approximated by the mass of the module times half the distance to the center squared. This quadratic increase gives us the intuition that the required yaw actuation grows faster than the actuation authority.

Yaw Actuation

An inherit characteristic of quadrotors is to have their yaw controlled by the drag moments from each propeller. For ModQuad as more modules are docked together, a decreased controllability in yaw is noticed as the structure becomes larger. In a line configuration the structure’s inertia grows quadratically with the distance of each module to the structure center of mass. On the other hand the drag moments produced scales linearly with the number of modules.

The new yaw actuation method relies on the fact that each quadrotor is capable to generate an individual roll enabled by our new cage design. By working in coordinated manner, each crazyflie can then generate structure moments from moment arms provided by the propellers given its roll and its distance from the structure’s center of mass.

Cage Design

The Crazyflie 2.0 is the chosen platform to enable thrust and attitude to the individual modules. The flying vehicle measures 92×92×29 mm and weights 27 g while its battery lasts around 4 minutes for the novel design proposed. In this work the cage performs as pendulum relative to the flying vehicle. The quadrotor is joined to the cage through a one DOF joint. The cages are made of light-weight materials: ABS for the 3-D printed connectors and joints, and carbon fiber for the rods.

Although the flying vehicle does not necessarily share same orientation as the cage, the multiple connected cages do preserve same orientation relative to each other. With the purpose of allowing such behavior, we used Neodymium Iron Boron (NdFeB) magnets as passive actuators to enable rigid cage connections. Docking is only allowed at the back and front face of the modules, and each one of these faces contains four magnets. Those passive actuators have dimensions of 6.35 × 6.35 × 0.79 mm with a bonding force of 1 kg.

Structure Flying Performance

Conclusions

ModQuad-DoF is a flying modular robotic structure whose yaw actuation scales with increased numbers of modules. ModQuad-DoF has a one DOF jointed cage design and a novel control method for the flying structure. Our new yaw actuation method was validated conducting experiments for hovering conditions. We were able to perform two, four and, six modules cooperatively flying in a line with yaw controllability and reduced loss in thrust. In future work we aim to explore the structure controllability with more robots in a line configuration, and exploring different solutions for the desired roll angles. Possibly, with more modules in the structure, only a few would be required to roll in order to maintain a desired structure yaw. Given that, we could explore the control allocation for each  module in a specific structure configuration, and dependent on its desired behavior. Further, structures that are not constrained to a line will also be tested using the basis of the controller proposed in this work.

Detailed Video Explanation (ICRA 2020)

This work was developed by:

Bruno Gabrich, Guanrui li, and Mark Yim

Additional resources at:

https://www.modlabupenn.org/
https://www.grasp.upenn.edu/

Intro to Autonomous Robotics with the Crazyflie

  2020-07-06  |     |    Leave a comment

Autonomous Robotics at UW Seattle

Our team’s Crazyflie quadcopter was equipped with Bitcraze’s Optic Flow deck and Multi-ranger deck. A BH-1750 light intensity sensor was soldered to a Bitcraze Prototype deck to complete our hardware.

The Crazyflie 2.1 was the perfect robotics platform for an introduction to autonomous robotics at the University of Washington winter quarter 2020. Our Bio-inspired Robotics graduate course completed a series of Crazyflie projects throughout the 10 weeks that built our skills in:

  • Python
  • Robot Operating System (ROS)
  • assembling custom sensors
  • writing new drivers
  • designing and testing control algorithms
  • trouble shooting and independent learning

The course was offered by UW Mechanical Engineering’s Autonomous Insect Robotics Laboratory, headed by Dr. Sawyer B. Fuller. The course was supported by PhD candidate Melanie Anderson, who has done fantastic research with her Crazyflie-based Smellicopter. The final project was an opportunity to turn a Crazyflie quadcopter into a bio-inspired autonomous robot. Our three person team of UW robotics grad students included Nishant Elkunchwar, Krishna Balasubramanian, and Jessica Noe.

Light Seeking Run-and-Tumble Algorithm Inspired by Bacterial Chemotaxis

The goal for our team’s Crazyflie was to seek and identify a light source. We chose a run-and-tumble algorithm inspired by bacterial chemotaxis. For a quick explanation of bacterial chemotaxis, please see Andrea Schmidt’s explanation of chemotaxis on Dr. Mehran Kardar’s MIT teaching page. She provides a helpful animation here.

In both bacterial chemotaxis and our run-and-tumble algorithm, there is a body (the bacteria or the robot) that can:

  • move under its own power.
  • detect the magnitude of something in the environment (e.g. chemical put off by a food source or light intensity).
  • determine whether the magnitude is greater or less than it was a short time before.

This method works best if the environment contains a strong gradient from low concentration to high concentration that the bacteria or robot can follow towards a high concentration source.

The details of the run-and-tumble algorithm are shown in a finite state machine diagram below. The simple summary is that the Crazyflie takes off, begins moving forward, and if the light intensity is getting larger it continues to “Run” in the same direction. If the light intensity is getting smaller, it will “Tumble” to a random direction. Additional layers of decision making are included to determine if the Crazyflie must “Avoid Obstacle”, or if the source has been reached and the Crazyflie quadcopter should “Stop”.

Run & Tumble Algorithm
The run-and-tumble algorithm represented as a finite state machine.

Crazyflie Hardware

To implement the run-and-tumble algorithm autonomously on the Crazyflie, we needed a Crazyflie quadcopter and these additional sensors:

The Optic Flow deck was a key sensor in achieving autonomous flight. This sensor package determines the Crazyflie’s height above the surface and tracks its horizontal motion from the starting position along the x-direction and y-direction coordinates. With the Optic Flow installed, the Crazyflie is capable of autonomously maintaining a constant height above the surface. It can also move forward, back, left, and right a set distance or at a set speed. Several other pre-programmed movement behaviors can also be chosen. This Bitcraze blog post has more information on how the Flow deck works and this post by Chuan-en Lin on Nanonets.com provides more in-depth information if you would like to read more.

The Bitcraze Multi-ranger deck provided the sensor data for obstacle avoidance. The Multi-ranger detects the distance from the Crazyflie to the nearest object in five directions: forward, backward, right, left, and above. Our threshold to trigger the “Avoid Obstacle” behavior is detecting an obstacle within 0.5 meters of the Crazyflie quadcopter.

The Prototype deck was a quick, simple way to connect the BH-1750 light intensity sensor to the pins of the Crazyflie to physically integrate the sensor with the quadcopter hardware. This diagram shows how the header positions connect to the rows of pads in the center of the deck. We soldered a header into the center of the deck, then soldered connections between the pads to form continuous connections from our header pin to the correct Crazyflie header pin on the left or right edges of the Prototype deck. The Bitcraze Wiki provides a pin map for the Crazyflie quadcopter and information about the power supply pins. A nice overview of the BH-1750 sensor is found on Components101.com, this shows the pin map and the 4.7 kOhm pull-up resistor that needs to be placed on the I2C line.

It was easy to connect the decks to the Crazyflie because Bitcraze clearly marks “Front”, “Up” and “Down” to help you orient each deck relative to the Crazyflie. See the Bitcraze documentation on expansion decks for more details. Once the decks are properly attached, the Crazyflie can automatically detect that the Flow and Multi-Ranger decks are installed, and all of the built-in functions related to these decks are immediately available for use without reflashing the Crazyflie with updated firmware. (We appreciated this awesome feature!)

Crazyflie Firmware and ROS Control Software

Bitcraze provides a downloadable virtual machine (VM) to help users quickly start developing their own code for the Crazyflie. Our team used a VM that was modified by UW graduate students Melanie Anderson and Joseph Sullivan to make it easier to write ROS control code in the Python coding language to control one or more Crazyflie quadcopters. This was helpful to our team because we were all familiar with Python from previous work. The standard Bitcraze VM is available on Bitcraze’s Github page. The Modified VM constructed by Joseph and Melanie is available through Melanie’s Github page. Available on Joseph’s Github page is the “rospy_crazyflie” code that can be combined with existing installs of ROS and Bitcraze’s Python API if users do not want to use the VM options.

  • “crazyflie-firmware” – a set of files written in C that can be uploaded to the Crazyflie quadcopter to overwrite the default firmware
    • In the Bitcraze VM, this folder is located at “/home/bitcraze/projects/crazyflie-firmware”
    • In the Modified VM, this folder is located at “Home/crazyflie-firmware”
  • “crazyflie-lib-python” (in the Bitcraze VM) or “rospy_crazyflie” (in the Modified VM) – a set of ROS files that allows high-level control of the quadcopter’s actions
    • In the Bitcraze VM, “crazyflie-lib-python” is located at “/home/bitcraze/projects/crazyflie-lib-python”
    • In the Modified VM, navigate to “Home/catkin_ws/src” which contains two main sets of files:
      • “Home/catkin_ws/src/crazyflie-lib-python” – a copy of the Bitcraze “crazyflie-lib-python”
      • “Home/catkin_ws/src/rospy_crazyflie” – the modified version of “crazyflie-lib-python” that includes additional ROS and Python functionality, and example scripts created by Joseph and Melanie

In the Modified VM, we edited the “crazyflie-firmware” files to include code for our light intensity sensor, and we edited “rospy-crazyflie” to add functions to the ROS software that runs on the Crazyflie. Having the VM environment saved our team a huge amount of time and frustration – we did not have to download a basic virtual machine, then update software versions, find libraries, and track down fixes for incompatible software. We could just start writing new code for the Crazyflie.

The Modified VM for the Crazyflie takes advantage of the Robot Operating System (ROS) architecture. The example script provided within the Modified VM helped us quickly become familiar with basic ROS concepts like nodes, topics, message types, publishing, and subscribing. We were able to understand and write our own nodes that published information to different topics and write nodes that subscribed to the topics to receive and use the information to control the Crazyflie.

For more information, see the Bitcraze Development overview.

Updating the crazyflie-firmware

A major challenge of our project was writing a new driver that could be added to the Crazyflie firmware to tell the Crazyflie system that we had connected an additional sensor to the Crazyflie’s I2C bus. Our team referenced open-source Arduino drivers to understand how the BH-1750 connects to an Arduino I2C bus. We also looked at the open-source drivers written by Bitcraze for the Multi-ranger deck to see how it connects to the Crazyflie I2C bus. By looking at all of these open-source examples and studying how to use I2C communication protocols, our team member Nishant Elkunchwar was able to write a driver that allowed the Crazyflie to recognize the BH-1750 signal and convert it to a sensor value to be used within the Crazyflie’s ROS-based operating system. That driver is available on Nishant’s Github. The driver needed to be placed into the appropriate folder: “…\crazyflie-firmware\src\deck\drivers\src”.

The second change to the crazyflie-firmware is to add a “config.mk” file in the folder “…\crazyflie-firmware\tools\make”. Information about the “config.mk” file is available in the Bitcraze documentation on configuring the build.

The final change to the crazyflie-firmware is to update the make file “MakeFile” in the location “…\crazyflie-firmware”. The “MakeFile” changes include adding one line to the section “# Deck API” and two lines to the section “# Decks”. Information about compiling the MakeFile is available in the Bitcraze documentation about flashing the quadcopter.

Making additions to the ROS control architecture

The ROS control architecture includes messages. We needed to define 3 new types of messages for our new ROS control files. In the folder “…\catkin_ws\src\rospy_crazyflie\msg\msg” we added one file for each new message type. We also updated “CMakeLists.txt” to add the name of our message files in the section “add_message_files( )”.

The second part of our ROS control was a set of scripts written in Python. These included our run-and-tumble algorithm control code, publisher scripts, and a plotter script. These are all available in the project’s Github.

Characterizing the Light Sensor

At this point, the light intensity sensor was successfully integrated into the Crazyflie quadcopter. The new code was written and the Crazyflie quadcopter was reflashed with new firmware. We had completed our initial trouble shooting and the next step was to characterize the light intensity in our experimental setup.

Experimental setup for light intensity characterization.

This characterization was done by flying the Crazyflie at a fixed distance above the floor in tightly spaced rows along the x and y horizontal directions. The resulting plot (below) shows that the light intensity increases exponentially as the Crazyflie moves towards the light source.

The light characterization allowed us to determine an intensity threshold that will only happen near the light source. If this threshold is met, the algorithm’s “Stop” action is triggered, and the Crazyflie lands.

Light intensity (units of lux) was experimentally characterized by piloting the Crazyflie in a linear pattern at a constant height above the ground. The resulting plot shows that light intensity is characterized by an exponential roll off in both the x and y directions.

Testing the Run-and-Tumble Algorithm

With the light intensity characterization complete, we were able to test and revise our run-and-tumble algorithm. At each loop of the algorithm, one of the four actions is chosen: “Run”, “Tumble”, “Avoid Obstacle”, or “Stop”. The plot below shows a typical path with the action that was taken at each loop iteration.

Flight Tests of the Run-and-Tumble Algorithm

In final testing, we performed 4 trial runs with 100% success locating the light source. Our test area was approximately 100 square feet, included 1 light source, and 2 obstacles. The average search time was 1:41 seconds.

The “Avoid Obstacle” and “Run” behavior are demonstrated in the above video clip (1.5x actual speed).


The “Run” and “Tumble” actions are demonstrated in the above video clip (2x actual speed). At the end, the “Stop” action is demonstrated when the light intensity reaches the threshold value of 800 lux, indicating that the Crazyflie has found the light source and should land.

Lessons Learned

This was one of the best courses I’ve taken at the University of Washington. It was one of the first classes where a robot could be incorporated, and playing with the Crazyflie was pure fun. Another positive aspect was that the course had the feel of a boot camp for learning how to build, control, test, and improve autonomous robots. This was only possible because Bitcraze’s small, indoor quadcopter with optic flow capability made it possible to safely operate several quadcopters simultaneously in our small classroom as we learned.

This development project was really interesting (aka difficult…) and we went down a few rabbit holes as we tried to level up our knowledge and skills. Our prior experience with Python helped us read the custom example scripts provided in our course for the ROS control program, but we had quite a bit to learn about the ROS architecture before we could write our own control scripts.

Nishant made an extensive study of I2C protocols as he wrote the new driver for the BH-1750 sensor. One of the biggest lessons I learned in this project was that writing drivers to integrate a sensor to a microcontroller is hard. By contrast, using the Bitcraze decks was so easy it almost felt like cheating. (In the nicest way!)

On the hardware side, the one big problem we encountered during development was accidentally breaking the 0.5 mm headers on the Crazyflie quadcopter and the decks. The male headers were not long enough to extend from the Flow deck all the way up through the Prototype deck at the top, so we tried to solder extensions onto the pins. Unfortunately, I did not check the Bitcraze pin width and I just soldered on the pins we all had in our tool kits: the 0.1 inch (2.54 mm) wide pins that we use with our Arduinos and BeagleBones. These too-large-pins damaged the female headers on the decks, and we lost connectivity on those pins. Fortunately, we were able to repair our decks by soldering on replacement female headers from the Bitcraze store. I wish now that the long pin headers were available back then.

In summary, this course was an inspiring experience and helped our team learn a lot in a very short time. After ten weeks working with the Crazyflie, I can strongly recommend the Crazyflie for robotics classes and boot camps.

Links to Project Files

Team’s Research Poster: https://github.com/thecountoftuscany/crazyflie-run-and-tumble/blob/master/documents/Project-Final_Poster.pdf

Github Link courtesy of Nishant Elkunchwar: Crazyflie-Run-and-Tumble

YouTube Links courtesy of Nishant Elkunchwar: Crazyflie locates Light, Simulation of Run and Tumble Algorithm in PyGame

Latest Update on the AI deck

  2020-05-25  |     |    2 Comments

It has been a while since we have updated you all on the AI deck. The last full blogpost was in October, with some small updates here and there. It is not that we have not focused on it at all; on the contrary… this has been a high priority project for a while now. It is just quite a complex board with a lot of bells and whistles, which can be challenging to work with sometimes so early in development, something that our previous intern can definitely agree on. So therefore we rather wanted to wait until we were able to make sufficient progress before we gave you an update… and so we have!

A Crazyflie 2.1 with the AI deck

Together with Greenwaves technologies we have been trying to get the SDK of the GAP8 chip on the AI deck stable enough for an early release. The latest release of the SDK (version 3.4) has proved itself to work with relative ease on the AI deck after extensive testing. Currently it is possible to use OpenOCD for flashing and debugging, and it supports most commonly available debuggers with a jtag connector. In the upcoming weeks both of Bitcraze and Greenwaves will test and try out all examples of the SDK on the AI deck to make sure that everything is still compatible. Also the documentation will be extended as well. As there is so much to document, it might be difficult to catch all of it. However, if you notify us and Greenwaves on anything that is missing once the AIdeck is out, that will help us out to catch the knowledge gaps.

The AI deck also contains the ESP-based NINA module for establishing a WiFi connection. This enables the users to stream the video stream of the AI deck onto their computers, which will be quite an essential tool if they would like to generate their own image database for training the CNNs for the GAP8 (and it happens to also be quite practical for debugging by the way!). Currently it is required to set credentials of your local WiFi network and reflash the AI-deck to be able to connect and streaming the images, but we are working on turning the Nina into an access-point instead so no reflashing would be required. We hope that we will be able to implement this before the release.

Top view of the AI deck

We are also trying out to adjust applications to make suitable of the AI deck. For instance, we have adapted Greenwaves’ face-detector example to use the image streamer instead of the display available on the GAPuino boards. You can see a video of the result here underneath. Beware that this face-detector is not based on a CNN but on HOG descriptors, so it only works in good conditions where the face is well lit. However, it is possible to train a CNN to detect faces in Tensorflow and flash this on the AI deck with the GAPflow framework as developed by Greenwaves. At Bitcraze we haven’t managed to try that out ourselves ( we are close to that though!) but at least this example is a nice demonstration of the AI deck’s abilities together with the WiFi-streamer. This example and more testing code can be found in our experimental repo here. For examples of GAPflow, please check out the examples/NNtool section of the GAP8 SDK.

For some reason WordPress has difficulty embedding the video that was supposed to be here, so please check https://youtu.be/0sHh2V6Cq-Q

Seeing how the development has been progressing, we will be comfortable to say that the AI deck could be ready for early release somewhere in the next month, so please keep an eye out on our website! We will continue to test the GAP SDK’s stability and we are very thankful for Greenwaves Technologies with their help so far. We will also work on getting-started guides in order to get acquainted with the AI deck, supplementing the already existing documentation about the GAP8 chip.

Even-though the AI deck will soon be ready for early release, this piece of hardware is not for the faint-hearted and embedded programming experience is a must. But keep in mind that the possibilities with the AI deck are huge, as it will be mean that super-edge-computing on a 30 gram flying platform will be available for anyone. It will all be worth it when you have your Crazyflie flying autonomously while being able to recognize its surroundings :)

Crazyflies in the Robotarium

  2020-05-18  |     |    Leave a comment

We have a guest blog post this week from Christopher Banks at Georgia Tech, where he tells us about their work with the Robotarium. Enjoy!

Multi-Agent Aerial Robotics

In the GRITS Lab  we focus on autonomous control and coordination of multi-robot systems with applications in – but not limited to – optimal control, constraint-based control, and hardware development. We are home to the Robotarium [1], a remotely accessible swarm robotics testbed that is free for anyone around the world to use for academic and educational purposes. We have integrated Crazyflies into the Robotarium as the main vehicle for aerial robot swarms due to their small size, quiet operation, and high maneuverability . Also, due to their low inertia, they pose minimal harm to their surroundings if system failures occur. Their small size and robust nature are well suited for flying in an indoor testbed like the Robotarium. As we work towards extending the operation of the Crazyflies in the Robotarium to external users, we encountered some important research questions: How do we guarantee the quadcopters remain “safe” (undamaged) while minimizing modifications to user inputs? How do we develop an easy to use interface for external users, with experience ranging from novice to expert? What commands can be used by external users to control a swarm of robots?  This post will briefly describe the ongoing research aimed at solving these questions.

Safety Guarantees

To ensure hardware safety while flying experimental algorithms we have developed Control Barrier Functions (CBFs) for quadcopters, allowing users to give nominal control inputs while obeying some safety constraints for the system (e.g. collision-free trajectory following). In the video below, we give four Crazyflies the commands to fly in a circle. A fifth Crazyflie is then told to fly to waypoints that will intersect the circle and attempt to collide with the circling quadcopters. Using CBFs a central controller can modify the inputs given to Crazyflies near collision to ensure safe velocity commands that are close as possible to the user intended control [2] . These CBFs can also be designed to ensure safety by bounding the quadcopters to a designated region of the testbed, giving additional safety constraints by protecting areas outside of the motion capture system during flights.

Quadcopters execute pre-planned flight trajectories designed to collide and use CBFs to avoid collisions.

User Interfaces

We have also used the Crazyflies to understand how remote users can best interact with the Robotarium both at the interface level and in planning. One project involved studying the effectiveness of graphical user interfaces (GUIs) on swarm robotic control. Two GUIs were developed with different interaction modalities. The GUIs were designed to map user inputs to a set of hoops placed in the Robotarium. One GUI (shown in Fig. 1) provided users the ability to draw paths through a touchscreen interface on a two-dimensional map and then map those inputs to trajectories for a team of robots. The other GUI (illustrated in Fig. 2) allowed users to input a sequence of desired hoops for a team of robots and execute trajectories based on the input.

Figure 1: A GUI that maps hand-drawn paths to inputs for a group of Crazyflies
Figure 2: A GUI that maps the string of indexed hoops as inputs for a group of Crazyflies.

Multi-Agent Planning

In planning, we looked at how multi-agent planning can be approached using high-level specifications. These high-level specifications allow users to develop plans requiring groups of robots to visit regions of interest (see Fig. 3) and trajectories are generated automatically. To represent these specifications, we use a logic formalism known as temporal logic to encode a preferred sequence of plan execution. As an additional step, users could include constraints on the trajectory by minimizing a cost using stochastic sampling. For more details, see the attached video demonstrating task allocation in a fire-fighting scenario.

Figure 3: Using the multi-agent planning framework, users give high-level specifications that plan trajectories for quadcopters to visit regions of interest (hoops) in the Robotarium.
A optimizing task allocation framework that assigns quadcopters a set of tasks based on user specifications.

Future Directions

As we continue to expand the capabilities of the Robotarium we are looking into how to develop long term autonomy for the Crazyflies. This includes autonomous charging as well as remote access for the lab and other users. We hope to use the Lighthouse system as a method for long term tracking since the Crazyflie will know its position instead of relying on passive tracking from a Vicon system. Our plans also include a lab-based simulator for in house projects related to the Crazyflies as well as updating our system to incorporate Crazyswarm to make control of the Crazyflies easier in implementation. In addition to this, in order to accommodate unknown users, we will have to figure out a control scheme that encourages use from a wide variety of users ranging from novices in quadcopter control to experts. We’ll keep Bitcraze updated on the Robotarium’s progression towards fully autonomous aerial swarms!

Links

  1. Robotarium Article: https://ieeexplore.ieee.org/document/8960572
  2. CBFs for Quadcopters: https://ieeexplore.ieee.org/document/7989375

Learning-based Bias Correction for Accurate Ultra-wideband Localization of a Crazyflie

  2020-04-27  |     |    Leave a comment

Accurate indoor localization is a crucial enabling technology for many robotic applications, from warehouse management to monitoring tasks. Ultra-wideband (UWB) localization technology, in particular, has been shown to provide robust, high-resolution, and obstacle-penetrating ranging measurements. Nonetheless, UWB measurements are still corrupted by non-line-of-sight (NLOS) communication and spatially-varying biases due to doughnut-shaped antenna radiation pattern. In our recent work, we present a lightweight, two-step measurement correction method to improve the performance of both TWR and TDoA-based UWB localization.  We integrate our method into the Extended Kalman Filter (EKF) onboard a Crazyflie and demonstrate a closed-loop position estimation performance with ~20cm root-mean-square (RMS) error.

A stylized depiction of our UWB indoor localization system and the schematics of the proposed estimation framework.

Methodology

UWB measurement errors can be separated into two groups: (1) systematic bias caused by limitations in the UWB antenna pattern and (2) spurious measurements due to NLOS and multi-path propagation. We propose a two-step UWB bias correction approach exploiting machine learning (to address(1)) and statistical testing (to address (2)). The data-driven nature of our approach makes it agnostic to the origin of the measurement errors it corrects. 

(1) Neural Network Bias Correction

The doughnut-shaped antenna radiation pattern causes the relative poses of anchors and tags to have a noticeable impact on the received signal power, which leads to systematic, predictable biases.  To empirically demonstrate the systematic measurement errors resulting from varying the relative pose between anchors and tags, we placed two DWM1000 UWB anchors at a distance of 4m and collected both TWR and TDoA UWB range measurements for the UWB tag mounted on top of a Crazyflie spinning around its own z-axis.

Left: schematics of the ranges (∆p’s), azimuth (α’s) and elevation angles (β’s) defining the relative poses of tag T and anchors A0, A1 when collecting the systematic bias measurements. Right: the neural network’s inferred bias (in red) with respect to the tag’s varying azimuth angle towards anchor T0, αT0, plotted against the UWB raw measurements.

We choose to leverage the nonlinear representation power of neural networks to learn the systematic bias which only depends on anchor-tag relative poses. Considering the limited onboard computation power, we select a fully connected neural network with 50 neurons in each of two layers with ReLU activation. To represent the relative pose between the UWB tag and anchors, we select the relative distance ∆p and roll, pitch, and yaw angles of the quadcopter as the input features x for the network. As we used fixed anchors, we do not include their poses as inputs (this level of generalization is left for future work). Given sufficient training data, the spatially-varying measurement bias can be described by a nonlinear function b=f(x) captured by the trained neural network.

(2) Outlier (Spurious Measurements) Rejection

Besides our learning-based bias correction, we use a quadcopter’s dynamic model to filter inconsistent UWB range measurements. Given the estimated velocity v and maximum acceleration amax, we can compute the maximum distance dmax a quadcopter can cover during time ∆t. Based on this information, we can reject unattainable measurements before fusing them into the EKF by comparing the measurement innovation with dmax. 

Moreover, we use a statistical hypothesis test to further classify potential outlier measurements. Since the measurement innovation vector is assumed to be distributed according to a multivariate Gaussian distribution, the normalized sum of squares of its values should follow a Chi-square distribution. We use the Chi-square hypothesis test to determine whether a measurement innovation is likely coming from this distribution.

UWB measurement bias f (x) prediction performance of the trained neural network (in red) compared to the actual measurement errors (blue dots) as well as the role of model-based filtering (purple dots) and statistical validation (orange dots) in rejecting outlier measurement innovations (teal dots) during a 60” flight experiment.

Data Collection and Training

We use a Crazyflie 2.0 quadcopter and the Loco Positioning System (LPS)’s UWB DW1000 modules as our research platforms. Our calibration approach runs on the Crazyflie STM32 microcontroller within the FreeRTOS real-time operating system. We equipped a cuboid flying arena with 8 UWB anchors, one for each vertex. The anchor positions were measured using a Leica total station theodolite.

Left: three-dimensional plot of our flight arena showing the positions and poses of the eight UWB DW1000 anchors (each facing towards its own x-axis, i.e., the red versor). Right: two of the training trajectories we flew to collect the samples that we used to train our neural network-based bias estimator

For all experiments, the ground truth position of the Crazyflie was provided by 10 Vicon cameras. The neural network was trained using PyTorch. To perform inference on the Crazyflie’s microcontroller, we re-use PyTorch’s trained weights in a plain C re-implementation. Since the DW1000 modules in the LPS provide UWB measurements every 5ms, the neural network inference runs at 200Hz during flight as well. Our outlier rejection method is also implemented in plain C and merged with the onboard EKF.

Close-loop Position Estimation Performance

We demonstrate the position estimation and close-loop performance of the proposed methods by flying a Crazyflie quadcopter along planar and non-planar circular trajectories (which were not among the trajectories used for training). A comparison between the estimation error of (A) the UWB localization estimate enhanced with outlier rejections and (B) the estimated enhanced with both outlier rejection and neural network bias compensation is conducted in our experiments for both TWR and TDoA2 modes. We repeated all of our experiments 10 times with a target velocity of 0.375m/s. The quadcopter trajectories during these flight tests are displayed in the following plots.  

Flight paths and the tracking performance of our approach with (in blue) and without (in orange) the neural network bias correction for two reference trajectories (planar and non-planar circular orbits) and both UWB modes (TWR and TDoA).

The distributions of the RMS estimation errors are summarized into a box plot. TWR-based ranging results in better localization performance than TDoA. However, we observe that, with our neural network bias compensation, the average RMS error of TDoA localization is around 0.21m, which is comparable to that of TWR-based localization (~0.19m). Thanks to the neural network bias compensation, the average reduction in the RMS error is ~18.5% and 48% for TWR and TDoA, respectively. Most notably, this result suggests that bias compensation might help closing the performance gap between TWR- and TDoA-based localization.

Root mean square error (RMSE) of the quadcopter position estimate before (in orange) and after (in blue) the neural network calibration step for both TWR and TDoA ranging modes. Each pair of box plots refers to a planar reference trajectory (left of each pair) and a reference trajectory with varying z (right of each pair), showing a greater performance enhancement for the latter.

Outlook

In this work, we presented a two-step methodology to improve UWB localization—for both TWR- and TDoA-based measurements. We used a lightweight neural network to model and compensate for pose-dependent and spatially-varying biases and an outlier rejection mechanism to filter spurious measurements. Through several real world flight experiments tracking different trajectories, we showed that we are able to improve localization accuracy for both TWR and TDoA, granting safer indoor flight. In our future work, we will include the anchors’ pose information to allow our method to further generalize to previously unobserved indoor environments, with different anchor configurations.

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 ideas: wenda.zhao@robotics.utias.utoronto.ca. Please cite this as:

<code>@article{wenda2020learning,
  title={Learning-based Bias Correction for Ultra-wideband Localization of Resource-constrained Mobile Robots},
  author={Wenda Zhao and Abhishek Goudar and Jacopo Panerati and Angela P. Schoellig},
  journal={arXiv preprint arXiv:2003.09371},
  year={2020}
}</code>