Robot localization using beacons

Lately I was playing around with Estimote beacons. We bought them a few months back with an idea to use them in the robotics lab, but no student picked them up for any project. I thought that this could be a nice addition to my PhD thesis, so I started working on the code to see, if it can be in any way useful. Beacons are widely used for advertising in shopping malls, museums, and places where it makes sense, sending highly contextual, hyper-local messages to smartphones of people that are located near them. This is done thanks to Bluetooth Low Energy (BLE) installed on devices. With much less power consumption, comes much longer battery lifetime, meaning such beacons can last up to 3 years on a single battery cell.

For the presented code you need the following Python packages:

Moreover, this code runs on Ubuntu, I’m not quite sure how it would work on other operating systems (due to Bluetooth drivers). You might want to check the pyBluez documentation on requirements here: https://pybluez.github.io/.

Connecting to beacons

Our first step is to import used packages. We will import pandas to store our beacon information in a clear way, and a couple of things from the beacontools package. We will be using the scanner and specific Estimote Telemetry frames and filters.

import pandas as pd
from beacontools import (
    BeaconScanner,
    EstimoteTelemetryFrameA,
    EstimoteTelemetryFrameB,
    EstimoteFilter,
)

To make things easier for me, I have created the EstimoteScanner class, which will manage the beacontools methods as well as store the information in a handy way. Whenever you see the self keyword, you can deduct that it comes from that class. The initialization of the class is done in the __init__ function without any additional parameters - it reads the beacon metainfo from a csv file, creates Estimote filters and creates the BeaconScanner.

To read the information we will use the read_csv function of pandas as seen in the code below. Note that I am setting the index_col to 'identifier', to use the beacon id as our DataFrame id. Moreover I create two new columns, that will hold the rssi (signal strength) and distance (calculated distance to the beacon) variables.

# Create DataFrame to store beacon info
self.devices = pd.read_csv('beacons.csv', index_col='identifier')
self.devices['rssi'] = 0
self.devices['distance'] = None

When we print out the devices DataFrame, we get the following output:

                    name  x_pos  y_pos  rssi distance
identifier                                           
a7bfafb716c4a815   vader      0      2     0     None
c20770c80bd9f59c  tarkin     10      0     0     None
ec6247cf5570b993    leia     10     10     0     None

Now that we have the data storing handled, we can move on to beacons. First we need to create a list of beacons, that the software should look for. This is handled by passing the identifier of the beacon as a parameter to the EstimoteFilter constructor. We will iterate through our DataFrame using the iterrows() function and add filters for each beacon to the device_filters variable.

# Create scanning filters
self.device_filters = []
for index, _ in self.devices.iterrows():
    self.device_filters.append(
        EstimoteFilter(identifier=index)
    )

Once that is done, we can create the scanner by passing selected telemetry frames and our filters.

# Create the scanner
self.scanner = BeaconScanner(self._callback,
    packet_filter=[
        EstimoteTelemetryFrameA,
        EstimoteTelemetryFrameB
    ],
    device_filter=self.device_filters,
)

But wait! As you can see, a function called _callback is referenced in the BeaconScanner. We should implement a callback function that will update our DataFrame, each time the bacon info is received. I am selecting the row based on the identifier received from the beacon and the columns for rssi and distance. We will use that variable for calibartion.

def _callback(self, bt_addr, rssi, packet, info):
    self.devices.loc[info['identifier'],'rssi'] = rssi
    self.devices.loc[info['identifier'],'distance'] = self._calc_dist(rssi)

The distance is calculated using the _calc_dist() function. It uses the standard distance calculations formula. What is interesting is the MEASURED_POWER variable - this is the power that the beacon should have at 1 meter at 4dBm.

Note that, if you change the broadcasting power of your Estimote beacon, you will have to update the MEASURED_POWER variable to get good distance calibrations. Check the estimote forums for more info.

# Calibration power at 1 meter for 4dBm for Estimote Beacons
MEASURED_POWER = -66

def _calc_dist(self, rssi):
    return pow(10, (MEASURED_POWER - rssi) / 20)

Now to start scanning you just need to run self.scanner.start() and self.scanner.stop() when you are finished with gathering data (usually when the robots gets to the goal).

Robot localization

Although robot localization using beacons isn’t the most precise way to do that, it gives some advantages to use such systems. This could be a tool of improving robot localization in rooms that look the same (eg. corridors). The main problem with beacons is that the signal power which is used to calculate the distance can fluctuate highly depending on the number of metal objects, electronical equipment or even the building construction.

Nevertheless, we can use the data we have gathered from the scanner to calculate the position of the robot, based on the readings. We will use a technique called Trilateration, which is used for calculating eg. earthquake epicenters. Having static points for beacons and the distances to them, we can use this technique to calculate the robot position.

The input parameters for the fucntion are a,b,c for beacon positions represented by a tuple (x,y) and the distances in meters gathered from our DataFrame - da, db, dc. The below code just inputs those variables into the math equations of Trilateration, so feel free to copy that one out.

def trilateration(a, b, c, da, db, dc):
    """Function responsible for Trilateration
    Parameters
    ----------
    a, b, c: tuple
        Contains the position of the beacon (x, y)
    da, db, dc: float
        The distance to the beacon in meters.
    """
    if da is None or db is None or dc is None:
        return None

    W = pow(da, 2) - pow(db, 2) - pow(a[0], 2) - pow(a[1], 2) + pow(b[0], 2) + pow(b[1], 2)
    Z = pow(db, 2) - pow(dc, 2) - pow(b[0], 2) - pow(b[1], 2) + pow(c[0], 2) + pow(c[1], 2)

    x = (W * (c[1] - b[1]) - Z * (b[1] - a[1])) / (2 * ((b[0] - a[0]) * (c[1] - b[1]) - (c[0] - b[0]) * (b[1] - a[1])))
    y = (W - 2 * x * (b[0] - a[0])) / (2 * (b[1] - a[1]))
    y2 = (Z - 2 * x * (c[0] - b[0])) / (2 * (c[1] - b[1]))
    y = (y + y2) / 2

    return x, y

Summary

Using BLE Beacons for robot localization is a fun project. The results you get aren’t ultra precise compared to other techniques, but give you a quick overview on the surroundings. The method can greately enhance your other algorithms, especially when there are many beacons around (think shopping malls). The code can be easily transferred to any Ubuntu running device, so possibly can be used with RoS.

Later next month, we will be installing the system for student use at our robotics lab. If you are interested in the full code of the project, visit the UWM Robotics Club repository on GitHub: https://github.com/nkr-uwm/robot-ble-localization (stil work in progress).