Automated anchor position estimation

2017-10-16 | Kristoffer Richardsson | 4 Comments

One of the pain points when setting up the Loco Positioning system is to measure the anchor positions and enter them into the system. I wanted to see if I could automate this task and let the system calculate the positions, and if so understand what kind of precision to expect. I have spent a few Fun Fridays playing with this problem and this is what I have found so far.

The problem can be broken down into two parts:
1. How to calculate the anchor positions. What data is required?
2. How to define the coordinate system. To make it useful the user must to be able to define the coordinate system in a simple way.

How to calculate the anchor positions

The general idea of how to calculate the anchor positions is to set up a system of equations describing the distances between the anchors and/or the Crayzflie and solve for the anchor positions. The equations will be non linear and the (possibly naive) plan is to use the Gauss Newton method to solve the system.

To understand how to calculate the anchor positions we must first take a look at the data that is available. The Loco Positioning system can be run in two different modes: Two Way ranging (default mode) and TDoA.

Two way ranging

In the Two Way ranging mode we measure the distance between each anchor and the Crazyflie and to get enough data we must record ranging data for multiple positions. The anchor positions are unknown, and for each new Crazyflie position we add yet a new unknown position, on the other hand we measure the ranges to the anchors so these are knowns.

The equations used are simply to calculate the distance between the assumed position of each anchor and the Crazyflie and then subtracting it from the measured distance.

TDoA

In TDoA we measure the Time Difference of Arrival, that is the difference in distance to two anchors from the Crazyflie’s position. It is probably possible to use this information, but I was looking for a different solution here. In our new TDoA implementation that we have been playing with a bit, we get the distance between all anchors (calculated in the anchors) as a side effect.

In this case the Crazflie is not really needed and the equations describe the distance between assumed anchor positions versus measured distances.

How to define the coordinate system

To get a useable positioning system, the coordinate system must be well defined and oriented in a practical direction. For example when writing a script you probably want (0, 0, 0) to be at some specific spot, the X-axis pointing in a certain direction, the Z-axis to point up and so on. My initial idea was to use the anchors to define the coordinate system, use anchor 0 as (0, 0, 0), let the X-axis pass through anchor 1 and so on. Just by looking at our flight lab I realised that this would be too limiting and decided that the coordinate system should be completely disconnected from the anchor positions, but still easy to define. I also realised that a really good way to tell the system about the desired coordinate system would be to move the Crazyflie around in space to show what you want. The solution is to place the Crazyflie at certain positions and click a button to record data at these positions. The steps I have chosen are:

Place the Crazyflie at (0, 0, 0)
Place the Crazyflie on the X-axis, X > 0
Place the Crazyflie in the XY-plane, Y > 0
Move the Crazyflie around in the space with continuous recording of data

In this scheme the XY-plane is typically the floor.

Results

I have written basic implementations for both the Two Way ranging and TDoA modes and they seem to work reasonably well in simulations. I have also tested the Two Way ranging algorithm in our flight lab with mixed results. The solution converged in most cases but not always. When converging the estimated anchor positions ended up in the right region but some were off by up to a meter. Finally I did run the algorithm and fed the result into the system and managed to fly using the estimated positions which I find encouraging.

I will continue to work on this as a Friday Fun project and maybe it will make its way into the client code base at some point in the future? There are probably better ways to estimate the anchor positions and more clever algorithms, feel free to share them in the comments.

Improved swarm support in Loco Positioning with TDoA 2.0

2017-08-28 | Kristoffer Richardsson | Leave a comment

We have had an implementation of a Time Difference of Arrival algorithm (TDoA) in the Crazyflie 2.o and the Loco Positioning System for quite a long time. The coolest feature of the TDoA algorithm is that it can be used to position virtually unlimited number of Crazyflies concurrently as opposed to the standard Two Way Ranging algorithm that is limited to one (or very few Crazyflies). The original implementation is working pretty well but contains some flaws that we are not completely happy with, hence we have not released it officially and are still calling it experimental. Since support for tracking multiple objects is a requirement for flying swarms and we like swarms, we have started to iron out the problems.

A small swarm using the old TDoA algorithm, from February 2017.

In the current implementation each anchor transmits the time of the transmission and the times of the latest reception of transmissions from all other anchors about every 16 ms. From this information it is possible to calculate the difference in time of flight for the radio waves from two anchors to the Crazyflie. When we know the difference in time of arrival, by multiplying with the speed of light we get the difference in distance and can calculate the position of the Crazyflie. This all sounds fine and dandy but the set up has some problems, the biggest one being error handling. If one or more packets are lost, either from anchor to Crazyflie or anchor to anchor, there is no deterministic way to detect it in some cases. The current algorithm relies on sanity checking the calculated result and discarding data that looks suspicious, which is usually easy as the distances quickly gets unrealistic (several thousands of meters!). We suspect that some erroneous values slip through the check though and we would like to be able to really understand when data is valid or not.

TDoA 2.0

What we are working on now is to add a sequence numbering scheme to enable the receiving party to understand when a packet has been lost. With this information it will be possible to discard bad data as well as use the available information better. While we are re-writing the code we are also moving a part of the algorithm from the Crazyflie to the anchors, after all there is a CPU in the anchors that is not fully utilized. The idea is to let each anchor continuously calculate the distance to all other anchors and add this information to the messages it transmits, which will reduce the work in the Crazyflie.

This is work in progress and we are not completely sure where we will end up, but we are aiming at making the TDoA mode part of the official release at some point.

Sensor fusion

Related to the Loco Positioning system is our line of other positioning sensors; the Z-ranger deck and the Flow deck. The Flow deck has really good precision at low altitudes but can not provide absolute positioning while the Loco Positioning system does not have the same precision but absolute position capabilities. So what if we fuse the information from the Flow deck with the Loco Positioning system? We have tried it out and it works pretty well, we can get the best of two worlds! The Z-ranger can also be used in the same way to improve the Z component of the estimated position when flying bellow ~1m.

Even though it works using multiple positioning sensors at the same time, there is room for improvements and some tweaking will be required to make it rock solid.

It’s summer again!

2017-07-17 | Arnaud | Leave a comment

It is summer again in Sweden and things are now starting to slow down and people are going to vacation. The last couple of years we have used the summer to look back and clean-up the technical dept accumulated during the year: when trying to get things done we have to prioritize which means that some things have to be left on the side (at least until we invent a way to add more hours to each day). This year is no exception, the last couple of weeks we have been working very hard to get the Flow products out and now the production is hopefully on rails so the cleanup can begin.

There is a lot of things we could do but here is a sneak peek of what we are currently looking at:

Crazyflie Client gamepad handling and configuration: The current input device handling is complicated and the architecture is hard to work with. There is a lot that can be done both in the front-end and the back end to make it easier to use and to work with.
Loco positioning system support for multiple Crazyflie, we have two mode implemented for that, TWR-TDMA and TDoA, both are very experimental and need some more work.
Cleanup of the webpage, information and documentation: we already have done a lot of work to make better documentation but there is always margin for improvement.
Cleaning up and improving the Crazyradio firmware: the Crazyradio starts to show its limits when flying swarms of Crazyflie. There is some improvement that could be done in the Firmware to make it more efficient. The first step is clean up the current implementation.

If you have any ideas of areas you feel we should focus on, even better if you want to help with some things and fix it together with us, just tell us in the comment.

On a side note, the manufacturing of the Flow products is still on progress and it should soon be on the Bitcraze shop and the Seeedstudio bazaar, stay tuned.

ICRA 2017

2017-06-12 | Kristoffer Richardsson | Leave a comment

We exhibited at the IEEE International Conference on Robotics and Automation in Singapore a couple of weeks ago.

We had a booth where we demoed autonomous flight with the Crazyflie 2.0 and the Loco Positioning system, without any external computer in the loop. The core of the demo was that the Crazyflie had an onboard trajectory sequencer that enabled it to fly autonomously along a path, based on the position from the Loco Positioning system.

We had a pre programmed path that we used most of the time, since it enabled us to start the demo and the leave the Crazyflie without any further manual interference from our side (except changing battery). The other option was to manually record a path for the Crayzflie to retrace by moving it around in the flying space. When we dropped it (detecting zero gravity) the onboard sequencer and controller took over to replay the recorded path. This mode was very useful when showing the accuracy and performance of the system by recording a short sequence of one point and just leaving the Crazyflie to hover. We had mounted a deck with two buttons on the Crazyflie that we used to chose which mode to use.

The code used for the demo is available at github for anyone to play with.

Optical flow

We also showed our brand new Flow deck that we will release soon. It is a deck that is mounted underneath the Crazyflie with a downwards facing optical flow sensor. The sensor is in essence what is used in an optical mouse but with a different lens that enables it to track motion further away. The output from the deck is delta X and Y for the motion of the Crazyflie and can be used by the onboard controller to control the position. We will publish more information in this blog soon.

We had a great time talking to all you interesting, bright and awesome people. Thanks for all feedback, sharing ideas and telling us about your projects!

Loco positioning out of early access!

2017-05-29 | Björn Mauritz | 5 Comments

We have been working with our indoor positioning system for over a year now and during that time the interest for the Loco positioning system have continuously been growing. Universities all over the world are already using our system and the attention we have received have been very encouraging. But the system has been a bit hard to set up and lacking in documentation. So we decided that this had to be fixed before we could leave early access.

So now we’re happy to announce that the Loco positioning system has reached a state where it’s out of “Early access” and we’re excited to see even more customers starting to use the system. We would like to thank everybody in the community that has been contributing to the project, this is what makes open source great!

Below are some of the steps we’ve taken to make it easier to use.

Getting started guide

We have created a brand new “Getting started with Loco positioning” tutorial that will replace the old “Getting started” video that we made. The tutorial contains a step-by-step guide for flying one Crazyflie 2.0 autonomously using two way ranging. The more advanced features such as TDoA is still considered as experimental and won’t be covered in this tutorial. We are very happy with the outcome of this tutorial but please help us out and try the tutorial and give us feedback.

LPS tool

In order to easily configure and upgrade the firmware for the Loco positioning nodes we’ve created the LPS tool. With it you can set the address of the node, the mode and also upgrade the firmware easily.

LPS tab

Up until lately there hasn’t been any support for the Loco positioning system in our Crazyflie Python client, instead we’ve been using ROS. It’s a great tool and very powerful tool, but ROS can be a bit complicated with specific requirements on the environment in which it runs. So we wanted to be able to perform basic use-cases directly in our client, like setting up the system, debugging it and flying with it. In order to achieve this we created the Loco positioning tab and a new flight mode called Position hold.

The Loco positioning tab is used to set-up the system and view it’s status. You can configure the position of the anchors, easily see which anchors you’re close to and if all the anchors are responding as they should. It’s also possible to see the layout of the system and the Crazyflies position in it.

For flying the Crazyflie we’ve added the Position hold mode. When enabled this will translate gamepad input into velocity control set-points that are followed using the positioning.

Wiki documentation

We’ve added technical documentation for the Loco positioning system to the wiki and we’ve also created product pages with schematics and details on the hardware for the Loco positioning node and deck. In the documentation you can find useful things like protocol documentation and details on our reference set-up that we use in our lab. Sine the system is still continuously evolving this is a working document that will continue to be improved over time and we hope to push the boundaries further for swarm flying and robotics in general.

ICRA

Finally a reminder, if your are attending 2017 IEEE International Conference on Robotics and Automation in Singapore you can meet us in booth C08 from 30/5 – 01/6. We will show autonomous flight with the Crazyflie 2.0 enabled by the the Loco Positioning System. Come by have a chat with us and see the Loco positioning in action!

You will also be able to attend the presentation of the paper “Crazyswarm: A Large Nano-Quadcopter Swarm” by researchers from USC during Wednesday 31/5 at 11:15.

Releases

To support the features mentioned above we’ve released version 2017.05 of the Crazyflie Python client and Crazyflie firmware.

Demo for ICRA 2017

2017-05-22 | Arnaud | 2 Comments

A couple of weeks ago we played with recording and retracing trajectory directly from the Crazyflie using Loco Positioning System. The result was quite nice and resulted, a first for us, in a fully autonomous Crazyflie, no computer or controller required:

We decided to expand on this experiment for our demo at ICRA. We have modified the retracing code to accepts multiple modes, including running pre-programmed sequence. The plan for the demo is to have Crazyflies that can:

Record and retrace a manual trajectory
Record and replay in a loop a manual trajectory
Play a pre-defined trajectory in a loop
Land automatically when the battery level is low

With this we should be able to demonstrate quite well the capabilities of both the Crazyflie and the Loco Positioning system, and since we do not require a computer in the loop it simplifies a lot running the demo. Of course we keep the possibility to connect the Crazyflie with the Crazyflie client and with ROS while the crazyflie is flying.

Having a completly autonomous Crazyflie is also new to us and it brings its share of problems: how to we choose the working mode and how to we stop the flight if something happens (things tends to happen …).

To solve the former we have made a button deck that adds 2 push-button to the Crazyflie. One means “Start autonomous sequence”. The second means “Record trajectory”. If the recorded trajectory is a loop (if the end point is close to the start point) then the loop is played back as soon as the crazyflie is dropped, otherwise Crazyflie retraces the trajectory and stop.

We solved the later problem by making an autonomous emergency stop button that sends a radio watchdog signal. If the signal stops to be sent or if an emergency stop signal is sent (ie. by pressing the button), the Crazyflie will stop all motors and drop. The button is implemented using a Raspberry pi, a Crazyradio and an Arduino to interface the button:

If you are curious about code, we have created a github repos where we push all code we are making for this demo. As usual, this conference is an opportunity for us to hack new functionalities, though not everything can be done in the master branch. Later some things can be merged, others (like the retrace trajectory recorder/player that looks more like a user app.) will need much more though if we want to merge it in the Crazyflie firmware.

Meet us in Singapore!

2017-05-15 | Kristoffer Richardsson | 1 Comment

We are going to the IEEE International Conference on Robotics and Automation in Singapore. The exhibition is open Tuesday May 30 to Thursday June 1 and we will have a booth, number C08, where we will show demos and discuss our work, positioning technologies and quadcopters.

We have not finalized our demos yet but they will include autonomous flight with the Crazyflie and the Loco Positioning system. We also hope to show our brand new optical flow expansion deck that will enable positioning and autonomous flight when on a tight budget. We also plan to show integration with external computers running ROS or our own python library. If we are lucky there might even be a small swarm, even though the space is very limited.

We love to talk to people that are using our products or just interested in our technology, if you are at the conference please drop by and say hi and tell us what you are working on. We will arrive in Singapore on Saturday morning May 27, and if you want to hook up and say hi and have a coffee during the weekend, drop us an email.

See you in Singapore!

Loco positioning tag

2017-05-08 | Marcus | 6 Comments

Ever since we released the Alpha round of the Loco positioning system we’ve been talking about designing a more generic tag that could be used together with other robotics platforms for local positioning. We did a quick design of a prototype that we tested, but with the workload involved in bringing the LPS out of Early Access, finishing the Z-ranger and lots of other stuff , it’s remained on the shelf. But recently we’ve been getting more and more requests for this kind of hardware, so we thought it might be time to dust off the prototype and try to release it. One of the blockers (except workload) has been that we’re not sure how the tag should look mechanically and how to interface it electrically for it to be as useful as possible for our community. This post is for detailing the current status of the hardware/firmware and to see if we can get some feedback on what our community would like the finished product to look like.

The hardware

To make use of the firmware that’s been developed so far for the Crazyflie and the Loco positioning we aimed at making something similar to what we already have but with another form factor and slightly different requirements. As you might know the Loco positioning node can be configured as a tag, but there’s two drawbacks that we wanted to fix. First of all the Loco positioning node might be a bit big to put on smaller robots. Secondly the Loco positioning node can only measure the distances to the anchors, it doesn’t have an IMU to get attitude of the board and doesn’t have the processing power to run the same algorithms we have on the Crazyflie 2.0.

So for our Loco positioning tag prototype we decided to fix these. The prototype has the same sensors as the Crazyflie 2.0: Gyro, accelerometer, magnetometer and pressure sensor. It also has the same MCU as the Crazyflie 2.0: STM32F405. In addition to this it has the DWM1000 module for the ultra wide-band radio (used for positioning). We’ve also added the interfaces we have on the Crazyflie 2.0: SWD debugging, micro-USB for communication and power as well as a button. Looking at the pictures below you might also notice that we’ve added the Crazyflie 2.0 deck connector. So does this mean you can connect it to the Crazyflie 2.0? No, well not this prototype at least. The reason for adding it was we wanted to be able to use the same expansion decks as for the Crazyflie 2.0. So it’s possible to add the breakout deck for breadboard prototyping or the LED-ring for visual feedback.

So what’s the status of the hardware? Even though it’s the first prototype it’s fully functional and will give you positioning and attitude. What’s left is defining the electrical interfaces and the form-factor of the board so it can easily be attached to what ever you might want to track. The images below shows a side-by-side comparison with the current Loco positioning deck.

Loco positioning tag (on the right) compared to Loco positioning deck (on the left) (FRONT)

Loco positioning tag (on the right) compared to Loco positioning deck (on the left) (BACK)

The firmware/software

Like I wrote above we wanted to reuse as much of the firmware and software as possible. So the firmware running on the prototype is just a scaled down version of the Crazyflie 2.0 firmware. As you might have noticed the prototype looks a lot like the Crazyflie 2.0, except that it’s not a quadcopter and doesn’t have the nRF51 radio. So by “scaled down” I mean we’ve removed the motor and radio drivers, that’s about it. So how do you communicate with it? Well you can use one of the protocol available on the deck connector: SPI, I2C or UART. But the currently implemented way is using USB. Since it’s basically a Crazyflie you can use our client and python libraries to set parameters and log data values from it.

Conclusion

The current prototype is basically a USB dongle where you get position and attitude. It could easily be connected via USB to a Raspberry Pi, Beaglebone or any other SoC based platform or a computer. You can also interface it from an Arduino using the peripherals on the deck connector. The firmware is working and using the python library (or any other of our community supported libraries) you can easily get the position and attitude of the board. But to be able to take the next step and make something our community could make the most of we would love some feedback on the prototype. What kind of electrical interfaces and form-factor would you like?

Crazyflie firmware and client release 2017.4 with Z-Ranger support

2017-04-24 | Arnaud | Leave a comment

We are happy to announce that we have released new versions of the Crazyflie Firmware and the Crazyflie client, both are now in version 2017.4. The main feature of the release is support for the new Z-Ranger deck.

To support the Z-Ranger, a new flight mode has been added to both the firmware and the client: the Height-hold mode. This mode allows to fly with the Z-ranger deck at a fixed height above the floor.

There are also a number of other improvements and bug fixes in the releases, mainly related to the Loco Positioning system and autonomous flight.

On the client side, this new release is also a come-back of the windows build. It means that it is now easier to get started and fly your Crazyflie directly from Windows as you can install it as a native app. We really want to build the client for Mac OS too but have met some problems. If anyone has experience in building pyqt apps for mac OS, with your help we might be able to have a mac build for the next client version ;-).

For instructions on how to upgrade see the getting started guide.

Have fun!

Updated autonomous flight using OpenCV and Kalman filter

2017-04-03 | Arnaud | Leave a comment

At Bitcraze we have some history with trying to fly our Crazyflie autonomously. The most recent step is the Loco Positioning System that allows us, and you, to fly in a full room. The Loco Positioning system has boosted development of advanced algorithms for onboard position estimation and control.

Our earlier attempts where mostly based on different kinds of cameras, either a 3D camera like Kinect or regular webcams. Though, at that point, we only had the camera for position estimation and where doing the position control on the PC and not onboard the Crazyflie. This has the disadvantage to be brittle and requires a very high quality positioning from the camera: any frame where we loose the Crazyflie has a huge impact on the control behavior since the position controller relies exclusively on the camera detection.

With the Kalman filter and onboard position controller, the Crazyflie can now handle lost position information for at least a couple of seconds without big problems. This has the potential of making webcam-based position detector much more robust!

To test this theory we have grabbed the 2 years old crazyflie-ar-detector from ~~the dawer~~ github, updated it to OpenCV 3.2.0, and fed the position output to the Crazyflie 2.0 external position port. The crazyflie-ar-detector program is using ZeroMQ to communicate position and so we made a simple external position tab for the Crazyflie Client that receives position from ZeroMQ and sends it to the connected Crazyflie.

Using the new position-hold mode recently introduced in the client we can test and fly the Crazyflie under the webcam. We have taken a short video to show the performance. The result is promising and we will continue to play with ways to fly the Crazyflie autonomously.