As mentioned in a previous blog post, we have a both at ICRA in London this week. If you are there too, come and visit us in booth H10 and tell us what you are working on!
We are showing our live autonomous demo and our products in the booth, including the flapping drone Flapper Nimble, don’t miss it!
The autonomous demo
The decentralized autonomous demo that we are showing is based on technologies in the Crazyflie ecosystem. The general outline is that Crazyflies are autonomously flying in randomized patterns without colliding. The main features are:
Positioning using the Lighthouse positioning system, all positioning estimation is done in the drone. The Lighthouse positioning system provides high accuracy and ease of use.
Communication is all peer-to-peer, no centralized functionality. Each Crazyflie is transmitting information about its state and position to the other peers, to enable them to act properly.
Collision avoidance using the on-board system without central planing. Based on the position of the other peers, each Crazyflie avoids collisions by modifying its current trajectory.
Wireless charging using the Qi-deck. When running out of battery, the Crazyflies go back to their charging pads for an automatic re-fill.
The App framework is used to implement the demo. The app framework provides an easy way of writing and maintaining user code that runs in the Crazyflie.
We are happy to answer any questions on how the technology works and implementation details. You can also read more about the demo in the original blog post by Marios.
The next developer meeting is next week, Wed June 7 15:00 CEST and the topic will be the demo and how it is implemented. If you want to know about any specific technologies we used, how it is implemented or if you are just curious about the demo in general, please join the developer meeting. We will start with a presentation of the different parts of the demo, and after that a Q&A. As always we will end up with a section where you can ask any question you like related to our ecosystem. Checkout this announcement on our discussion platform for information on how to join.
This week’s guest blogpost is from Matěj Karásek from Flapper Drones, about flying the Nimble + with a positioning system. Enjoy!
Flapper Drones are bioinspired robots flying by flapping their wings, similar to insects and hummingbirds. If you haven’t heard of Flappers yet, you can read more about their origins at TU Delft and about how they function in an earlier post and on our company website.
In this blogpost, I will write about how to fly the Flappers (namely the Flapper Nimble+) autonomously within a positioning system such as the Lighthouse, and will of course include some nice videos as well.
The Flapper Nimble+ is the first hover-capable flapping-wing drone on the market. It is a development platform powered by the Crazyflie Bolt and so it can enjoy most of the perks of the Crazyflie ecosystem, including the positioning systems as well as other sensors (check this overview). If you would like to get a Flapper yourself, just head to the Bitcraze webstore, where there are some units ready to be shipped! (At the time of writing at least…)
The minimal setup for flying in a positioning system is nearly identical as with a standard Crazyflie. Next to a Flapper with a recent firmware, a Crazyradio dongle, a positioning system (in this post we will use the Lighthouse), and a compatible positioning deck (Lighthouse deck) you will also need: 1) a mount, such that the deck can be attached on top of the Flapper, and 2) a set of extension cables. You can 3D print the mounts yourself (models here), the extension cable prototypes can either be inquired from Flapper Drones, or can be soldered by yourself (in that case, the battery holder deck, standard Crazyflie pin headers and some wires come handy). Just pay attention to connect the cables in the correct way, as if the deck was mounted right on top of the Bolt. The complete setup with the Lighthouse deck will look like this:
For the Lighthouse, as with regular Crazyflies, the minimum number of base stations (with some redundancy) is 2, but you will get larger tracking volume with more base stations. 4 base stations mounted at 3 m height will give you about 5 meters time 5 meters coverage, which is recommended especially if you want to fly more than 1 Flapper at a time (they are a bit larger than the Crazyflies, after all…). From now on, it is exactly the same as with standard Crazyflies. After you calibrate the Lighthouse system using the standard wizard procedure via the Cfclient, you can just go to the Flight Control Tab and use the “Command Based Flight Control” buttons to take-off, command steps in xyz directions and land. It is this easy!
Assisted flight demo
We used this setup in February for the demos we were giving at the Highlight Delft festival in the Netherlands. This allowed people with no drone piloting skills (from 3-year-olds, to grandmas – true story) fly and control the Flapper in a safe way (safe for the Flapper, as the Flapper itself is a very safe platform thanks to its soft wings and low weight). To make it more fun, and even safer for the Flapper, we used a gamepad instead of on screen buttons, and we modified the cfclient slightly such that the flight space can be geofenced to stay within the tracking volume.
If you would like to try it yourself (it works also with standard crazyflies), the source code is here (just keep in mind it is experimental and has some known bugs…). To fly in the position-assisted mode, you need to press (and keep pressing) the Alt 1 button, and use the joysticks to move around (velocity commands, headless mode). Releasing the Alt 1 button will make the Flapper autoland. Autoland will also get triggered when the battery is low. You can still fly the Flapper in a direct way when pressing Alt 2 instead.
Flying more Flappers at a time
Again, this is something that works pretty much out of the box. As with a regular crazyflie, you just need to assign a unique address to each of the Flappers and then use e.g. this example python script to run a preprogrammed sequence.
With a few extra lines of code, we pulled this quick demo at the end of the Highlight Delft festival, when we had 30 minutes left before packing everything (one of the Flappers decided to drop its landing gear, probably too tired after 3 evenings of almost continuous flying…):
Other positioning systems
Using other positioning systems is equally easy. In fact, for the Loco Positioning system, the deck can even be installed directly on the Flapper’s Bolt board (no extension cables or mounts are needed). As for optical motion tracking, we do not have experience with Qualisys and the active marker deck, but flying with retro-reflective markers within OptiTrack system can be setup easily with just a few hacks.
When choosing and setting up the positioning system, just keep in mind that due to its wings, the Flapper needs to tilt much more to fly forward or sideways, compared to a quadcopter. This is not an issue with the Loco Positioning system (but there can be challenges with position estimation, as described further), but it can be a limitation for systems requiring direct line of sight, such as the Lighthouse or optical motion tracking.
In terms of control and flight dynamics, the Flapper is very different from the Crazyflie. Thus, for autonomous flight, there remains room for improvement on the firmware side. We managed to include the “flapper” platform into the standard Crazyflie firmware (in master branch since November 2022, and in all releases since then), such that RC flying and other basic functionality works out of the box. However, as many things in the firmware were originally written only for a (specific) quadcopter platform, the Crazyflie 2.x, further contributions are needed to unlock the full potential of the Flapper.
With the introduction of “platforms” last year, many things can be defined per platform (e.g. the PID controller gains, sensor alignment, filter settings, etc.), but e.g. the Extended Kalman filter, and specifically the motion model inside, has been derived and tuned for the Crazyflie 2.x, and is thus no representative of the Flapper with very different flight dynamics. This is what directly affects (and currently limits) the autonomous flight within positioning systems – it works well enough at hover and slow flight, but the agility and speed achievable in RC flight cannot be reached yet. We are planning to improve this in the future (hopefully with the help of the community). The recently introduced out of tree controllers and estimators might be the way to go… To be continued :)
Thanks Matej ! And for those of you at home, don’t forget that we have our dev meeting next Wednesday (the 5th), where we’ll discuss about the Loco positioning system, but also will take some time for general discussions. We hope to see you there!
This week’s guest blogpost is from Florian Goralsky from Bok o Bok about their dance piece with multiple Crazyflies. Enjoy!
Flying bodies across the fields is a contemporary dance piece for four performers and a swarm of drones, exploring the phenomenon of the disappearance of bees and the use of pollinating drones to compensate for this loss. The piece attempts to answer this crucial question in a poetical way: can the machine create life and save us from ecological disaster?
We’re super excited to talk about a performance that we’ve been working on for the past two years in collaboration with Bitcraze. It premiered at the Environmental Forum, Centre Pompidou Paris, in 2021, and we’ve had the opportunity to showcase it at different venues since then. We are happy to share our thoughts about it!
Beyond symbolizing current attempts to use drones to pollinate fields, the presence of the Crazyflie drones, supports the back and forth between nature and technology. We integrate a swarm, performing complex choreographies, which refer to the functioning of a beehive, including the famous “bee dance”, discovered by Karl von Frisch, which is used to transmit information on the food sources. Far from having a spectacular performance as its only goal, the synchronization of autonomous drones highlights bio-inspired computer techniques, focused on collective intelligence.
Challenges within a dance performance
Making a dance performance with drones needs a high accuracy and adaptability, both before and during the show. Usually, we only have a few hours, sometimes even a few minutes, to setup the system according to the space. We quickly realized we needed pre-recorded choreographies, and hybrid choreographies where the pilot could have a few degrees of freedom on pre-defined behaviors.
GUI Editor + Python Server
Taking this into account, we developed a web GUI editor, that is able to send choreographies created with any device to a Websocket Python server. The system supports any absolute positioning system (We use the Lighthouse), and then converts all the setpoints and actions to the Crazyflie API HighLevelCommander class. This system allows us to create, update, and test complex choreographies in a few minutes on various devices.
What is next?
We are looking forward to developing more dancers-drones interactions in the future. It will imply, in addition to the Lighthouse system, other sensors, in order to open up new possibilities: realtime path-finding, obstacle avoidance even during a recorded choreography (to allow improvisation), etc.
The demo has similarities with our previous demo (see IROS 2019) but has been upgraded to be a fully autonomous and decentralized swarm with 9 Crazyflies buzzing around in a cage, going back to charging pads for wireless charging when the battery is running out. The demo supports multiple Crazyflies flying at the same time, avoiding collisions without a central authority, all decision making is done in each Crazyflie, that is fully decentralized.
The hardware is off-the-shelf products available in our store (links here). The software is obviously written specifically for the demo, but we wanted to use the building blocks already available in the system so the demo code is mainly “glue” to connect them together.
The cage/flying space
The flying space was box shaped, 3×2 meters in foot print and 2.5 meters high. We enclosed it in our lightweight travel cage made from aluminium pipes and a light net. It is a pretty small space to fly multiple Crazyflies in at the same time but it worked! The main problem with such a small space is down-wash from other Crazyflies and having enough room to avoid collisions. 3 Crazyflies worked pretty well, but had the space been larger it would have been possible to fly all nine.
Localization was handled by the Lighthouse positioning system. We used two base stations and the lighthouse deck on each Crazyflie which provides the Crazyflies with their current position with high accuracy.
Since the position is computed in the Crazyflie, using only data from on-board sensors, no external communication is needed in relation to the localization system. The only exception was that we uploaded the physical geometry of the system when setting up the cage.
When a Crazyflie is flying in the demo, the standard mode of operation is to fly a randomized pattern of straight lines. From time to time (randomized) the Crazyflie can also chose to fly the spiral that we have used in earlier demos (see the IROS 2019 demo for instance).
When the battery is running out, the Crazyflie goes back to the charging pad for charging. The position is sampled before taking off and this coordinate is used as the landing point to find the charging pad. When landed the Crazyflie verifies that the battery is being charged. If the battery is not charging the Crazyflie assumes it missed the charging pad and it takes off again to adjust the position.
The Crazyflies were equiped with the Qi-charging deck for wireless charging. The charging pads are 3D-printed pads with a slope to make the Crazyflie slide into position also if the landing is not perfect. In the center of the pads there are standard Qi-chargers from IKEA mounted to provide power.
To fly continuously, the system charging rate must be higher than what is consumed by the flying Crazyflies. With a system of nine Crazyflies that are charging through Qi-chargers it is possible to keep one Crazyflie flying, just. To get some margin we increased the charging speed a bit, the down side being that the Crazyflies get warm and the batteries ware out faster.
There is no planing ahead, but each Crazyflie must know where the other Crazyflies are located. Based on this information they avoid each other and chose a new path to reach their target position. For this to work each Crazyflie is continuously broadcasting its position to the other Crazyflies using the peer-to-peer framework.
Swarm control and collaboration
As mentioned earlier there is no central authority that decides which Crazyflie that should take off or go to a specific position, instead this functionality is handled in each Crazyflie. To make it possible for each Crazyflie to have a rough idea of the system state, each Crazyflie is broadcasting its position and state (landed, flying etc) to the other Crazyflies. If a Crazyflie realizes that too few drones are flying, it will simply take off to fix the problem, if it sees that too many are flying it will go back to the charging pad. To avoid that all Crazyflies takes off or lands at the same time, a randomized hold-back time is used before the actions is executed. This does not fully prevent two individuals from taking off at the same time, but makes it less likely, and eventually the correct number of drones will fly.
The number of drones that should fly at the same time is a system wide parameter that can be set from one of the peers in the system. To make sure they all agree on the value, a simple mechanism is used based on the age of the data. The value and the age of the value is included in the broadcast data. When another Crazyflie receives the data it compares the age of the received data with the age of the data it already has and replaces it only if it is younger.
A tenth Crazyflie is used in the demo as a sniffer. It is essentially a non-flying member of the swarm that listens to the broadcast traffic and it is used to feed data to a GUI that displays the state of the system. It can also be used to inject a new value for the desired number of flying Crazyflies.
Implementation and how to run it
The code is mainly implemented as an app in the Crazyflie firmware, using the app layer. The main part is a state machine that keeps track of what to do next with some other modules handling communication and trajectories.
Bolt DSHOT support for ESCs configurable via Kconfig
ESC pass-though configuration via USB (virtual COM port)
For more information (and files for the release) see the release notes and files on GitHub, the releases of the different projects are listed below. As always, we’re really eager to get feedback, so let us know if you try it out and how it works!
There are some nice and exciting improvement in the CF-client that we worked on during the summer months! First of all we worked on a toolbox structure, where every tab can be reconfigured as a toolbox as well, allowing it to be docked to the sides of the window. Secondly we have added a new geometry estimation wizard for Lighthouse systems to support multi base-station estimation. Finally we have added a new tab for PID controller tuning, mainly intended for the Bolt.
Toolboxes in the CFclient
Everyone who used the CFclient has experienced the tabs before. Anytime you want to configure the lighthouse system, setup plotting or look at the parameter states, you switch to the appropriate tab to perform your desired action. This is all fine, but sometimes it can be useful to see the contents of two tabs at the same time, maybe you want to watch the graphing of a log variable at the same time as you change a parameter. This is what the combined tab/toolbox feature adds! Any tab can now be converted into a toolbox that can be docked to the side of the window.
In the example above the plotter is displaying the estimated position of a Crazyflie with a Flow deck, while the parameter window is opened as a toolbox. The “motion.disable” parameter was just set to true and we can see that the kalman estimator gets into trouble when it no longer gets data from the flow deck.
To switch from tab to toolbox mode, go to the View/Toolboxes menu and select the window that you want to show as a toolbox. In a similar way, use the View/Tabs menu to turn it back to a tab.
Even though all tabs can be turned into toolboxes, some of them might still look better as tabs due to their design. We hope to be able to improve the design over time and make them more toolbox friendly, contributions are welcome!
Lighthouse Geometry Estimation Wizard
In a blogpost of almost a half year ago, we presented a new multi base station geometry estimation method that enabled the user to include more than 2 base station for flying a Crazyflie. This heavily increases the flight area covered by the base station V2s, as technically it should be able to handle up to 16!
However, up until this summer it has been in experimental mode as we weren’t so sure as how stable this new estimation method is, so the only way to use it was via a script in the Crazyflie python library directly, and not from the CFclient. Since we haven’t heard of anybody having problems with this new experimental feature, we decided to go ahead to make a nice multi base station geometry estimation wizard in the CFclient’s Lighthouse tab.
This wizard can be accessed if you go to the lighthouse tab-> ‘manage geometry’ and press ‘Estimate Geometry’. We had to make it a wizard as this new method requires some extra intermediate steps compared to the previous, to ensure proper scaling, ground plane setting and sweep angle recording. If you are only using 2 base stations this seems like extra effort, where you only had to put the Crazyflie on the ground and push a button, but if you compare flight performance of the two methods, you will see an immediate difference in positioning quality, especially around the edges of your flight area. So it is definitely worth it!
We will still provide the “simple” option for those that want to use it, or want to geometry estimate only one base-station, as we don’t have support for that for this new estimator (see this issue). In that case, you will have to install the headless version of opencv separately like ‘pip3 install opencv-python-headless’. We will remove this requirement from the cflib itself for the next release as there are conflicts for users who has installed the non-headless opencv on their system, like for the opencv-viewer of the AI-deck’s wifi streamer for instance.
PID tuning tab
And last but not least, we introduced an PID tuning tab in a PR in the CFClient! And of course… also available as a toolbox :) This is maybe not super necessary for the Crazyflie itself, but for anyone working with a custom frame with the Bolt or BigQuad deck this is quite useful. Tuning is much handier with a slider than to adjust each parameter numerically with the parameter tab. Also if you are just interested of what would happen if you would increase the proportional gain of the z-position controller of the crazyflie, this would be fun to try as well… but of course at your own risk!
If you are happy with your tuned PID values, there is the “Persist Values” button which will store the parameters in the EEPROM memory of the Crazyflie/Bolt, which means that these values will persist even after restarting the platform. This can be cleared with the ‘Clear persisted values’ button and you can retrieve the original firmware-hardcoded default values with ‘Default Values’ button. Please check out this blogpost to learn more about persistent parameters.
There is a new fresh release of both the firmware and the python library and client! The last release (2022.01) was from 2 months ago but we already added quite some extra functionality so we wanted to make a snapshot of this before continuing on other priorities.
Kbuild on CF firmware
One of the biggest changes that you will notice, is that there is now a new way to configure your Crazyflie firmware before building it. The old config.mk is gone and you will now need to either automatically generate a config file or generate one with the menuconfig, of which kbuild is most known for. For more information, please read the blogpost about this latest change, for the exception that we do prefer the users to use ‘make cf2_config’ as instructed in the 2022.03 version of the repo documentation.
Platform support for Bolt
We now defined the Bolt as a different platform. That means that for each release, there should now also be a bolt flavor zip file, next to the cf2 and tag zips, as you can see in the release page. Moreover, if you want to build the firmware to be Bolt compatible, you would first need to do ‘make bolt_defconfig’ to generate the needed configs with kbuild. For more information of how to add your own custom platform, please check out these instructions.
2+ Lighthouse base stations (experimental)
For those that feel constrained by the max 2 lighthouse base station support in the firmware and client, this functionality is now part of the release. This blogpost will explain more about this, and it is still experimental in nature, as you would need to reconfigure the firmware with… you guessed it: Kbuild! Also the geometry estimation needs to be done as a separate python script as well all from the Crazyflie python library. No worries, if you still prefer using the cfclient, it still uses the old way of estimating if you click the button, but just remember that you would need to do something extra in order to get 2+ base station support.
New VM release
We were also made aware of a pretty big error in the bitcraze VM, namely that we still used the old git:// type url for github repositories. IN the new release of the bitcraze VM this should be fixed, so please download the new one, or fix it yourself in your current VM by changing the remote URLs of the github repos you are working on to https://.
There has been some background work going on related to the Lighthouse system, as mentioned in a previous blogpost. The solution has been improved since that blog post and we believe the functionality is now on a level where it works pretty well and can add value to most Lighthouse users.
How to use it?
We have added a brief documentation to get you started. Though the solution has been stabilized, it is still a bit experimental and it has not been fully integrated into the client yet. The base station geometry estimator still has to be run as a python script from the command line, and a reconfigured version of the Crazyflie firmware has to be built and flashed.
We have added some improvements to the client thought to enable it to display base station status for 2+ base stations. This was the final part of the client UI that did not support 2+ base stations, and now remains only the possibility to run the new geometry estimation from the client.
What kind of improvements does it bring?
First of all, the functionality to use more than 2 base stations and the possibility to cover a larger flight space. It also makes it possible to set up multi-room systems to support flight from one room to another.
Secondly an improved estimation of the base station geometry (also when using 2 base stations) that generally reduces the errors and improves the position estimation of the Crazyflie when flying. “Jumping” of the estimated position when one base station is occluded should be reduced. When following a trajectory that is straight line through space, the Crazyflie should now actually fly on a fairly straight line, previously the flown path might be a bit curved.
The new solution has a better match to the physical world and hopefully the estimated Z will be closer to zero when the Crazyflie is on the floor, with the “old” method, the solution sometimes is slightly tilted with a Z != 0 in some areas.
Most of the Lighthouse system works just like before, the new functionality is related to base station geometry estimation. The “standard” geometry estimation is still available in the client and if you continue to use this nothing is changed, the following list is for the new estimation method.
The new geometry estimation is a bit clunky to use and the user still has to rebuild the firmware and run a python script.
Lighthouse 1 is not fully supported
The new geometry estimation does not work with one base station.
We hope to address the above problems in future releases.
Talking about releases, we are working on a new official release. If no unforeseen obstacles are found, we plan to make a new release within a week or two.
The functionality discussed in this blog post is still only in source code, on master or possibly in some pull requests. If you wait for the release all repositories should be syncronized and make it a bit easier to try out.
As the environment of the system has an impact on this type of functionality, we would love to get feedback from you if you try it out. We’d love to hear how it works for you!
Keeping things in stock has not been easy the last couple of years due to the general problems with availability of components. We have been mitigating this by increasing stock volumes when it has been possible, but we have also looked at redesigns of some products to be able to switch to other components. A positive side effect has been that it also enabled us to do some small changes we wanted to do for a long time.
The decks we have updated are the Lighthouse, SD-card and BigQuad decks. There are no big functionality changes so the decks have not gotten any updated version only a new board revision.
Lighthouse (Rev.D -> Rev.D1) The outline of the PCB has changed a bit in the hope of protecting the photo-diode sensors a bit better during hard crashes.
SD-card (Rev.C -> Rev.D) Some solder bridges were added to the bottom of the PCB to make it easier to utilize the “hidden” SPI port. This can be useful if wanting to log a lot of values to the SD-card in combination with decks using the SPI port as well, such as the Loco or Flow decks. See the datasheet for more details.
Biq-Quad (Rev.C -> Rev.C1) The capacitor C1 was removed. This was used to filter the analog current measurement reading but also caused problem for the SPI bus on the deck port. The SPI bus turned out to be a more used functionality and therefore capacitor C1 was removed. If the analog filtering functionality is wanted, a 100nF 0603 capacitor can be soldered to C1.
From now on we ship the updated revisions if you order in our store.
Base station geometry estimation is a function in the python client (in the lighthouse tab), where the system estimates the position and orientation of the base stations. The user places the Crazyflie on the floor (in the desired origin) and clicks a button to measure the angles to the base stations, which are used to estimate the geometry. The current implementation is fairly basic and has some issues associated with it:
All base stations must be received from the point where the Crazyflie is located
Only 2 base stations are supported
The coordinate system is not properly aligned with the room
The generated geometry is not as good as it could be, that is the position/orientation is sub-optimal
The code has a dependency to OpenCV which causes problems for ROS users
I have been working on a solution for these problems as my fun Friday project and in this blog post I will tell you a bit more about the problems and a possible solution.
What are the problems to be solved?
In the current implementation, the user places the Crazyflie in the origin, with the front of the Crazyflie pointing in the direction of the positive X-axis. When the user hits the “Estimate Geometry” button, the angles to the visible base stations are recorded and the solvePnP() function in OpenCV is used to estimate their poses (position and orientation). This is all fine but it also has its limitations and in the following section we will outline what the limitations are and how to solve them.
All base stations must be received in the origin and only 2 base stations are supported
Currently the Crazyflie ecosystem supports up to 2 base stations and this works fine for a flight space of around 4×4 meters. With more base stations it would be possible to cover larger areas or multiple rooms, which is a feature that many users have been asking for. In these scenarios it will not be possible to receive all base stations from one position any more though, and it will require a new method for geometry estimation using multiple measurements. Suppose base station 1 and 2 are received in one position and 2 and 3 in another, then we can map the measurements together since we know base station 2 must have the same pose in both measurements. This way it is possible to relate all base station poses to each other, provided there are measurements that link them together.
The coordinate system is not properly aligned with the room
When generating the geometry in the current implementation, the orientation of the Lighthouse deck is used to define the coordinate system: forward of the deck defines the X-axis, left defines the Y-axis and up the Z-axis. The problem is that the deck might not be perfectly aligned with the Crazyflie, the floor might not be completely flat or the Crazyflie might not point exactly in the desired direction. A pretty small misalignment will result in fairly large errors a couple of meters away, resulting in unexpected behavior, for instance not flying at constant height. Expanding to more base stations and larger systems, the problem will become even bigger and a better solution is clearly needed.
If we used the position of the Crazyflie when placed at multiple positions, we could use this information to rotate the coordinate system to be better aligned. For instance, suppose we measured some points on the floor of the flight space, then we could make sure the XY-plane of the coordinate system goes through those points, or at least as close as possible. Similarly one or more measurements along the X-axis would help to define the rotation around the Z-axis.
The generated geometry is not as good as it could be
The lighthouse positioning system is based on measuring the angles between the sensors on the Lighthouse deck and the base stations. One can think of it as four beams or rays, going from each base station to the sensors on the deck, for which we measure the direction very precisely seen from the base station’s point of view. The purpose of the geometry estimation is to figure out the position and orientation of the base stations so that we can calculate how the beams are oriented in the flight space instead. By looking at where the beams from two base stations intersect we know where the sensors are located and can calculate the position of the Crazyflie. This is a somewhat simplified picture of how it works but it is sufficient for the following discussion.
So what happens if the geometry is not completely correct? If the estimated positions or orientations of the base stations are slightly off, the beams will not intersect and we have to use some method to find the point closest to the two beams instead to use as the sensor position. In a real world system there will always be errors and the implementation must be able to handle them, but we want to keep them as small as possible. Further more we want to make sure the errors are uniformly distributed in the flight space so that we get equally good results everywhere.
In the current estimation process, where we take a measurement in one position, we are able to generate a geometry that is good at that point, but due to noise in the measurements and other subtleties the error at the edges of the flight space might be several centimeters.
The solution to this problem is to measure the angles in multiple positions and try to find a geometry where the error is equally small for all of them. It does not guarantee that the error will be equal everywhere, but if we make measurements in the volume we plan to fly in we know it will be OK where we need it to be. It should also be a much better geometry, for the full covered volume, than what we can be achieved by measuring in one point only.
One bonus problem that hopefully will be solved by this approach is the moving back and forth that sometimes can be seen in a Lighthouse 2 system. What happens is that the base stations interfere with each other from time to time (by design) and most of the time the Crazyflie gets positioning information from both base stations, but every couple of seconds only from one of them. When both are available the “average” position is used, but when only one is received, the Crazyflie will “jump” to the position indicated by that base station (the simplified model from above with crossing beams does not hold in this case, sorry!). If the difference between the suggested positions of the two base stations (the error in the geometry) is large there will be a noticeable motion in the Crazyflie.
The code has a dependency to Open CV
In the current solution we use the solvePnP() function in Open CV to estimate the geometry. Open CV is an awesome library but unfortunately it has turned out that this dependency interferes with ROS, and since a fair amount of our users also use ROS, we would like to get rid of it if possible.
Luckily I found an open source implementation of IPPE, an algorithm that finds the pose of an object based on points seen by a camera, that we can use instead. There is actually an option to use Ippe in OpenCV’s solvePnP(), but we used another flavor.
The core idea is to first collect measurements of beams in many positions in the flight space by moving a Crazyflie around and record the lighthouse angles. Secondly an equation system is created that takes the poses of the base stations and all the recorded Crazyflie poses as input and as output calculates the lighthouse angles those poses would correspond to for all the sensors. Finally the output is compared to the recorded values and poses are adjusted using the least-square solver in scipy to find the poses that minimizes the difference between the measurements and the output from the equation system.
Before we can solve the equation system we have to record the angles from the base stations. There is a handy function in the Crazyflie that pushes measured lighthouse angles to the PC via the radio, and by letting the user move the Crazyflie around in space we get the angles along that path. What we are looking for though are angles collected in discrete positions and as an approximation I group measurements together based on time. The assumption is that if two angle measurements are closer than 10 ms in time, the Crazyflie did not move very far and they can be considered to be taken in the same position. The output of this process is a list of samples where each sample contains the measured lighthouse angles of one or more base stations for one specific Crazyflie pose. After this has been done, the list is filtered to only contain samples with two or more base stations.
We also need an initial guess of the base station and Crazyflie poses for the least-square solver to make the solution converge. I use IPPE for this and use the first sample as the reference to define a temporary global reference frame. Suppose the first sample contains angles for base stations 2 and 3, we can then use IPPE to calculate an estimate of the pose of the two base stations, in the Crazyflie reference frame of this sample. Since we use the first sample as the reference for the global reference frame (that is the pose of the Crazyflie in this sample is the origin by definition), those poses are also equal to the base station poses in the global reference frame.
Suppose the next sample contains lighthouse angles for base stations 1 and 2, using IPPE we can estimate the base station poses for base stations 1 and 2 in the reference frame of the Crazyflie in this sample. Since the relative positions of the base stations is the same, regardless of reference frame, we can rotate/translate the poses of the base stations so that base station 2 pose matches the pose of base stations 2 in the first sample. We now have an estimate of the poses of base stations 1, 2 and 3, further more the transformation used represents the pose of the Crazyflie in sample 2. Repeating the process for all samples gives us a pretty good idea of where all the base stations are located as well as the pose of the Crazyflie in all the samples.
We can now feed the initial guess and the equation system into scipy and hopefully get a refined solution back. From the estimated poses of the base stations and Crazyflie samples it is possible to calculate the distance between sensors and beams which gives us an approximation of how good the solution is.
The final step is to align the coordinate system with the room, as mentioned earlier the solution we have this far is based on the pose of the Crazyflie in the first sample. The way it is done in the suggested implementation is to ask the user to place the Crazyflie at some points in the desired origin, on the positive X-axis and in the XY-plane and measure the angles in these positions. The measurements are included as samples in the above process which means we will get the estimated positions as a part of the over all solution, in the temporary global reference frame. The task at hand is then to find the rotation/translation from the temporary global reference frame to the one indicated by the positions sampled by the user. Again we do a least-square optimization to find the transformation that minimizes the error in the sampled points. We can now calculate the final solution by applying the transformation to the base station poses we got earlier.
Does it work?
Yes, it seems to work pretty well, I have not had the time to do extensive testing yet but the results looks promising. In our flight arena with 4 base stations, the solution seems to generally be acceptable. We don’t know the exact poses of our base stations since it is very hard to measure, but they are mounted in the same truss and should be at similar heights.
The above snippet is part of the output from one run and as can be seen the estimated height is between 2.96 and 3.02 m. You can also see that the estimated average error for sensor positions is in the order of 2-3 mm while the maximum error is 1.5 cm.
Below is graph of the recorded Crazyflie positions in the final solutions. Note the three single points at the bottom that are from the origin, the X-axis and XY-plane.
I did some testing on larger systems with 6-8 base stations this Friday and it seemed to be harder to get a solution that converges which indicates that there might be something to look into here.
Try it out
This is still work in progress, but if you want to try it out, you can find the code in this pull request. Run the examples/lighthouse/bs_geometry_estimation.py script, you will get instructions on the screen as you go.
Officially the firmware supports 2 base stations , but most of the code is designed to handle up to 16 and if you want to test the functionality with more than two base stations you have to update PULSE_PROCESSOR_N_BASE_STATIONS and re-flash your Crazyflie.