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:
pybluezfor bluetooth functions,
beacontoolsfor managing beacons,
pandasfor storing our data.
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
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
'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
# 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
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)
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.stop() when you are finished with gathering data (usually when the robots gets to the goal).
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, 2) - pow(a, 2) + pow(b, 2) + pow(b, 2) Z = pow(db, 2) - pow(dc, 2) - pow(b, 2) - pow(b, 2) + pow(c, 2) + pow(c, 2) x = (W * (c - b) - Z * (b - a)) / (2 * ((b - a) * (c - b) - (c - b) * (b - a))) y = (W - 2 * x * (b - a)) / (2 * (b - a)) y2 = (Z - 2 * x * (c - b)) / (2 * (c - b)) y = (y + y2) / 2 return x, y
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).